四周无人机的姿态解算（2）

接上一篇

4、尽量直观地分析一下程序

直接把小飞机的程序考过来了。。。
说明：

 函数名：姿态更新输入：mpu6050结构体，也就是测得数据+初始偏置 输出：三个姿态角数据函数变量的结构体定义： typedef struct{int16_t accX;int16_t accY;int16_t accZ;int16_t gyroX;int16_t gyroY;int16_t gyroZ;int16_t Offset[6];bool Check;}MPU6050Manager_t;typedef struct{float roll;float pitch;float yaw; }Attitude_t;

接下来就是代码了

void ATT_Update(const MPU6050Manager_t *pMpu,Attitude_t *pAngE, float dt) {
Vector_t Gravity,Acc,Gyro,AccGravity;

vector_t结构体的定义如下，实际上就是三维向量：
typedef struct
{
float x;
float y;
float z;
}Vector_t;
看名字应该知道定义的是哪些向量。

static Vector_t GyroIntegError = {0};
static float KpDef = 0.8f ; //四元数收勉值
static float KiDef = 0.0003f;

以上数据是在静态存储区，下面两个是参数，第一个GyroIntegError可以理解是陀螺仪数据的积分的error，这里的error之前总是认为是误差的意思，实际上这样理解非常不准确，应该是期望值与实际值的偏差。为啥要定义这个就要往后看。

float q0_t,q1_t,q2_t,q3_t;

这四个带_t的数是四元数的增量，实际上看到字母q表示的量还是四个一起出现，那肯定是四元数Quaternions了。

float NormQuat;

加速度计读取出来的向量的模，norm就是模的意思。

float HalfTime = dt * 0.5f;

dt单位时间

Gravity.x = 2 * (NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
Gravity.y = 2 * (NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
Gravity.z = 1 - 2 * (NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);

这里的NumQ.qx,是在函数外部定义的静态结构体，结构体里有四个变量。实际上，这就是表示系统姿态的四元数，最终积分也是积在这上面。
static Quaternion_t NumQ = {1, 0, 0, 0};
那么上面这三行是什么意思？
实际上是利用四元数求取当前重力加速度在物坐标系的三个分量，因为四元数是可以表示物体姿态的，所以也就可以求出这个向量。
当然，如果物理含义理解的比较好还会发现这三个式子就是用四元数表示的姿态变换矩阵的第三行元素。

NormQuat = Q_rsqrt(squa(g_MPUManager.accX)+ squa(g_MPUManager.accY) +squa(g_MPUManager.accZ));Acc.x = pMpu->accX * NormQuat; //归一后可化为单位向量下方向分量
Acc.y = pMpu->accY * NormQuat;
Acc.z = pMpu->accZ * NormQuat;

上面这里就是对加速度计测出的向量归一化

//向量叉乘得出的值，叉乘后可以得到旋转矩阵的重力分量在新的加速度分量上的偏差
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);

这里明显是一个叉积运算，两个参与运算的向量分别为：加速度计测量出的向量和当前四元数推导出来的向量的叉积。他们都表示重力加速度在物坐标系的向量，都是单位向量。

GyroIntegError.x += AccGravity.x * KiDef;
GyroIntegError.y += AccGravity.y * KiDef;
GyroIntegError.z += AccGravity.z * KiDef;

至于为什么求叉积，因为叉积的大小，或者说叉积各个分量的大小可以表现出两个向量的偏差。在都是单位向量的情况下，显然偏差越大，即角度越大，得到的叉积越大。如果两个向量重合，夹角为零了，那叉积也是零了。

将通过叉积运算得到这个偏差乘以一个系数加到积分偏差中，作为姿态变化的一部分。

Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x  +  GyroIntegError.x;//弧度制，此处补偿的是角速度的漂移
Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y  +  GyroIntegError.y;
Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z  +  GyroIntegError.z;
//角速度融合加速度比例补偿值，与上面三句共同形成了PI补偿，得到矫正后的角速度值

这里就很精髓，还用了pid。。。
实质上，Gyro.xyz是角速度，是相对于物坐标系，它等于后面三项相加，其中后面第一项是从陀螺仪中读到的角速度值，第二项是偏差值，第三项是偏差值的积分，实际上第二三项是PI控制。
由此就得到了当前的角速度（物坐标系）

// 一阶龙格库塔法, 更新四元数
//矫正后的角速度值积分，得到两次姿态解算中四元数一个实部Q0，三个虚部Q1~3的值的变化
q0_t = (-NumQ.q1 * Gyro.x - NumQ.q2 * Gyro.y - NumQ.q3 * Gyro.z) * HalfTime;
q1_t = ( NumQ.q0 * Gyro.x - NumQ.q3 * Gyro.y + NumQ.q2 * Gyro.z) * HalfTime;
q2_t = ( NumQ.q3 * Gyro.x + NumQ.q0 * Gyro.y - NumQ.q1 * Gyro.z) * HalfTime;
q3_t = (-NumQ.q2 * Gyro.x + NumQ.q1 * Gyro.y + NumQ.q0 * Gyro.z) * HalfTime;NumQ.q0 += q0_t; //积分后的值累加到上次的四元数中，即新的四元数
NumQ.q1 += q1_t;
NumQ.q2 += q2_t;
NumQ.q3 += q3_t;

利用角速度更新四元数。
什么意思呢，就是角速度（物坐标系）是角度（物坐标系）的微分，因此加到角度上可以得到新的角度，因为是离散情况。
而角度和四元数又存在对应，因此根据角度的增量是可以求出四元数的增量的，加到原来四元数上面，就有了新的四元数，代表了当前的姿态信息。

// 重新四元数归一化，得到单位向量下
NormQuat = Q_rsqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3)); //得到四元数的模长
NumQ.q0 *= NormQuat; //模长更新四元数值
NumQ.q1 *= NormQuat;
NumQ.q2 *= NormQuat;
NumQ.q3 *= NormQuat;

加完之后可能不满足归一化条件，重新归一化

/*机体坐标系下的Z方向向量*/
float NormAccz = -pMpu->accX * vecxZ+ pMpu->accY * vecyZ + pMpu->accZ * veczZ;  /*Z轴垂直方向上的加速度，此值涵盖了倾斜时在Z轴角速度的向量和，不是单纯重力感应得出的值*/
wz_acc_tmp[0] = (NormAccz - 8192) * 0.1196f;// cm/ss //0.1196f;厘米每二次方秒
wz_acc_tmp[1] += 0.1f *(wz_acc_tmp[0] - wz_acc_tmp[1]);//LPF
HeightInfo.Z_Acc = wz_acc_tmp[1];

最后这几句是跟Z方向的加速度有关的，虽然前面说过不考虑除重力加速度之外的加速度，但这个跟高度有关，所以可能用得到。。。虽然我还没看到那里。。。

然后下面这个GetAngle就很容易理解了：

void GetAngle(Attitude_t *pAngE)
{vecxZ = 2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3 ;/*矩阵(3,1)项*///地理坐标系下的X轴的重力分量vecyZ = 2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1;/*矩阵(3,2)项*///地理坐标系下的Y轴的重力分量veczZ = NumQ.q0 * NumQ.q0 - NumQ.q1 * NumQ.q1 - NumQ.q2 * NumQ.q2 + NumQ.q3 * NumQ.q3;  /*矩阵(3,3)项*///地理坐标系下的Z轴的重力分量 pAngE->yaw = atan2f((2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3),(1 - 2 * (NumQ.q2 * NumQ.q2 + NumQ.q3 * NumQ.q3))) * RtA;pAngE->pitch = asin(vecxZ)* RtA;   //俯仰角          pAngE->roll  = atan2f(vecyZ,veczZ) * RtA;  //横滚角
}

四元数换算成欧拉角。

至于根据欧拉角如何控制输出信号，还没看懂。。。下次写吧。估计要看pid和遥控器才能搞清楚。。。

然后还剩下高度控制，然后应该就没啥了。。。把代码看完再做个总结，一个比较典型的工程就算解析完成了，接下来还有一个匿名科创的无人机代码，复杂一些而且还没注释（orz），估计大同小异，花不了太多时间（但愿）。。。

四周无人机的姿态解算（2）相关推荐

无人机姿态解算：四元数及其与欧拉角的转换
无人机姿态解算:四元数及其与欧拉角的转换引言:获得无人机飞行时的飞行姿态对于无人机稳定控制来说至关重要.无人机主要通过传感器数据融合来进行状态估计,常用于无人机的传感器包括:MPU(包含了三轴加速度 ...
四旋翼无人机飞控系统设计(姿态解算)
姿态解算姿态传感器读出加速度和角速度,而对一个系统的自动控制往往需要更加上层和贴近应用的的一个属性:角度.所以需要通过加速度和角速度进行数据融合转化得到姿态角度. 以MPU6050为例,姿态 ...
基于STM32的四旋翼无人机项目（二）：MPU6050姿态解算（含上位机3D姿态显示教学）
前言:本文为手把手教学飞控核心知识点之一的姿态解算--MPU6050 姿态解算(飞控专栏第2篇).项目中飞行器使用 MPU6050 传感器对飞行器的姿态进行解算(四元数方法),搭配设计的卡尔曼滤波器与 ...
基于STM32F407四旋翼无人机 --- 姿态解算讲解（四元数）（叉积法融合陀螺仪数据和加速度数据）（五）
基于STM32F407四旋翼无人机 --- 姿态解算讲解(五) 姿态解算姿态解算定义欧拉角四元数四元数性质方向余弦矩阵四元数方向余弦矩阵叉积法融合陀螺仪数据和加速度数据叉积运算一阶龙 ...
UAV012_V2（二）：无人机姿态解算（深入篇）
写这篇博客,已经是第三次了,花了一个周,一遍遍修改,只为了理解好姿态解算并表述出来. 之前写过一篇姿态解算的博客,UAV021(四):飞控传感器数据融合与姿态估计,在小角度假设条件(俯仰角.滚转角都很 ...
无人机姿态解算_扩展卡尔曼滤波（2）
一.扩展卡尔曼滤波 KF和EKF的公式对比(基本没差别) 二.扩展卡尔曼五个公式利用扩展卡尔曼滤波估计四元数. 下图是论文中的截图.可以和前面的卡尔曼滤波估计高度文章的那五个公式对应一下. 观测矩阵 ...
Pixhawk代码分析-姿态解算篇D
姿态解算篇D 基础知识研究多旋翼无人机前期主要需要了解其气动布局和复杂的动力学模型,然后就是姿态解算和控制器的设计.为了实现精确四旋翼飞行器的姿态估计,首先就是需要了解各传感器采集的数据和误差存在的 ...
Pixhawk代码分析-姿态解算篇C
姿态解算篇C 前言终于到ardupilot源代码的姿态解算了,有了前期关于mahony姿态解算算法的基础以后,理解源代码的姿态解算算法就快多了,所有的东西都在脑海中初步有了一个框架:首先要做什么,然 ...
Pixhawk代码分析-姿态解算篇B
姿态解算篇B 前言本篇博文主要是以mahony的算法为基础理解姿态解算的过程,主要参考的论文就是William Premerlani and Paul Bizard的关于DCM的一篇经典论文< ...

四周无人机的姿态解算（2）

四周无人机的姿态解算（2）相关推荐

最新文章

热门文章