一、基础知识

1. 坐标系：遵循右手定则

1.1 大地坐标系（地球坐标系）：北（x轴）东（y轴）地（z轴），地就是向下指向地
1.2 机体坐标系：前（x轴）右（y轴）下（z轴）
1.3 IMU坐标系：该坐标系就是IMU本身的坐标系，一般无人机的IMU都是在飞控的中间，因此，IMU坐标系和机体坐标系可以看做有相同的原点，只需要简单的改变一下坐标轴的正负号就能和机体坐标系一致了，并不需要复杂的旋转。

2.姿态: 横滚（roll）俯仰(pitch) 偏航(yaw)

如果所示：
横滚(roll): 绕x轴旋转的动作叫横滚，向右横滚为正方向，向左横滚为负方向；
俯仰（pitch）:绕y轴旋转的动作叫做俯仰，向上（抬头）为正方向，向下为负方向；
偏航（yaw）: 绕z轴旋转的动作叫做偏航，向右偏航为正方向，向下偏航为负方向。

3. 旋转矩阵

一般定义旋转矩阵为

然后欧拉角和四元数都可以表示旋转矩阵，欧拉角和四元数可以互换。然后一般在无人机上，都是用四元数来表示欧拉角的。

4. 欧拉角：横滚角（roll angle）俯仰角(pitch angle) 偏航角(yaw angle)

3.1 姿态角也叫欧拉角。
横滚角(roll angle): 绕x轴横滚的角度，向右横滚为正方向，向左横滚为负方向；
俯仰角（pitch angle）:绕y轴俯仰的角度，向上（抬头）为正方向，向下为负方向；
偏航角（yaw angle）: 绕z轴偏航的角度，向右（顺时针）偏航为正方向，向左（逆时针）偏航为负方向。

3.2 欧拉角表示旋转，旋转顺序是Z-Y-X。
下面的矩阵是机体系到地球系的旋转矩阵。

如果将一个机体系转到地球系就是用上面的矩阵。

下面是地球系到机体系的旋转矩阵。

下面的意思就是将地球系vg通过地球系到机体系的旋转矩阵旋转至机体系。

5. 四元数

4.1 四元数：q0,q1,q2,q3，软件中默认初值分别是0，1，1，1。
四元数的更新（即q0,q1,q2,q3怎么来）：根据陀螺仪的数据来更新四元数
△t ：是读取陀螺仪的时间，也是调用姿态解算的时间。
wx,wy,wz：分别是陀螺仪测量到的值，单位是弧度每秒，rad/s. 如果用的是度每秒，那么最终的姿态是：你稍微动一下，姿态就变化很大。

4.2 四元数表示的矩阵

6. 用四元数表示欧拉角

欧拉角是地球系下的表示；传感器的数值是在机体系上表示的，因此，需要将机体系转换到地球系。所以要用到下面的旋转矩阵。

对比红框的内容就可以用四元数表示欧拉角了：

二、Mahony 互补滤波算法

开源代码位置：https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
源文件在《四、代码》 Algorithm/madgwick_algorithm_c的压缩包中。

传入的数据：校准且滤波后的九轴数据。其中，陀螺仪的单位要从度转换成弧度，不然的话，稍微动一动IMU，姿态就变换很大。

 gyro.x = gyro.x * DEG2RAD; /* 度转弧度 */gyro.y = gyro.y * DEG2RAD;gyro.z = gyro.z * DEG2RAD;

三、 PID控制：Mahony互补滤波中只用到了P和I。

代码中的下面三个参数是需要根据实际来进行修改的。

这里我只用到了比例调节。下面是根据横滚角输出的PID图，可以看到姿态到达目标姿态还是很快的。

四、代码

gitee 搜索 stm32_minidrone

如果错误，请指正。

无人机姿态解算_互补滤波（1）相关推荐

stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
无人机姿态解算_扩展卡尔曼滤波（2）
一.扩展卡尔曼滤波 KF和EKF的公式对比(基本没差别) 二.扩展卡尔曼五个公式利用扩展卡尔曼滤波估计四元数. 下图是论文中的截图.可以和前面的卡尔曼滤波估计高度文章的那五个公式对应一下. 观测矩阵 ...
MPU6050加速度、角速度的解算以及互补滤波使用
MPU6050加速度.角速度的解算以及互补滤波使用 MPU6050加速度的解算 MPU6050角速度的解算互补滤波使用(数据融合) 数字低通滤波器数字高通滤波器 MPU6050加速度的解算先根据 ...
基于STM32F407四旋翼无人机 --- 姿态解算讲解（四元数）（叉积法融合陀螺仪数据和加速度数据）（五）
基于STM32F407四旋翼无人机 --- 姿态解算讲解(五) 姿态解算姿态解算定义欧拉角四元数四元数性质方向余弦矩阵四元数方向余弦矩阵叉积法融合陀螺仪数据和加速度数据叉积运算一阶龙 ...
无人机姿态解算：四元数及其与欧拉角的转换
无人机姿态解算:四元数及其与欧拉角的转换引言:获得无人机飞行时的飞行姿态对于无人机稳定控制来说至关重要.无人机主要通过传感器数据融合来进行状态估计,常用于无人机的传感器包括:MPU(包含了三轴加速度 ...
UAV012_V2（二）：无人机姿态解算（深入篇）
写这篇博客,已经是第三次了,花了一个周,一遍遍修改,只为了理解好姿态解算并表述出来. 之前写过一篇姿态解算的博客,UAV021(四):飞控传感器数据融合与姿态估计,在小角度假设条件(俯仰角.滚转角都很 ...
python捷联惯导的姿态解算_自动驾驶中高精地图的大规模生产：视觉惯导技术在高德的应用...
导读:导航.驾驶辅助.自动驾驶等技术的不断发展对地图的精细程度提出了更高的要求.常规的道路级地图对于智能交通系统存在很多不足,针对自动驾驶应用的需求,我们提出了利用视觉惯导技术制作高精地图的方法. 本 ...
基于四元数互补滤波的无人机姿态解算
导航坐标系为东北天(ENU),其与机体坐标系(b)的方向余弦矩阵为CbcC_{b}^{c}Cbc
四旋翼无人机飞控系统设计(姿态解算)
姿态解算姿态传感器读出加速度和角速度,而对一个系统的自动控制往往需要更加上层和贴近应用的的一个属性:角度.所以需要通过加速度和角速度进行数据融合转化得到姿态角度. 以MPU6050为例,姿态 ...
卡尔曼滤波五个公式_基于ROS的卡尔曼滤波姿态解算
前段时间由于项目关系需要实现基于卡尔曼滤波的姿态解算,也就是说融合加速度计.陀螺仪及磁罗盘进行AHRS姿态解算,得到机器人的姿态角. 本文的学习需要有一定的卡尔曼滤波器基础,可以参考白巧克力亦唯心的卡 ...

无人机姿态解算_互补滤波（1）

一、基础知识

1. 坐标系：遵循右手定则

2.姿态: 横滚（roll）俯仰(pitch) 偏航(yaw)

3. 旋转矩阵

4. 欧拉角：横滚角（roll angle）俯仰角(pitch angle) 偏航角(yaw angle)

5. 四元数

6. 用四元数表示欧拉角

二、Mahony 互补滤波算法

三、 PID控制：Mahony互补滤波中只用到了P和I。

四、代码

无人机姿态解算_互补滤波（1）相关推荐

最新文章

热门文章

无人机姿态解算_互补滤波（1）

一、 基础知识

1. 坐标系：遵循右手定则

2.姿态: 横滚（roll）俯仰(pitch) 偏航(yaw)

3. 旋转矩阵

4. 欧拉角：横滚角（roll angle）俯仰角(pitch angle) 偏航角(yaw angle)

5. 四元数

6. 用四元数表示欧拉角

二、Mahony 互补滤波算法

三、 PID控制：Mahony互补滤波中只用到了P和I。

四、代码

无人机姿态解算_互补滤波（1）相关推荐

最新文章

热门文章

一、基础知识