姿态解算也叫姿态分析，姿态估计，姿态融合。姿态解算是根据IMU(惯性测量单元测量)数据（陀螺仪，加速度计，罗盘等）求解出飞行器的空中姿态
IMU 的中文全称是“惯性测量单元”，英文全称是“Inertial Measurement Unit”。
功能和组成
惯性测量单元（IMU）是一种传感器模块，通常包含以下几种传感器：
工作原理
IMU 通过集成的加速度计和陀螺仪实时测量物体的加速度和角速度。这些数据可以用于：
常见 IMU 型号
一些常见的 IMU 型号包括：
- 三轴加速度计：测量物体在三个轴方向上的加速度。
- 三轴陀螺仪：测量物体在三个轴方向上的角速度。
- 三轴磁力计（可选）：测量物体在三个轴方向上的磁场强度，用于辅助姿态估计和航向校正。
- 应用领域
-
IMU 广泛应用于各种领域，包括但不限于：
- 无人机：用于姿态控制和导航。
- 机器人：用于姿态估计和运动控制。
- 航空航天：用于飞行器的姿态和轨迹控制。
- 汽车：用于车辆稳定性和导航系统。
- 运动追踪：用于体育训练和健康监测。
- 虚拟现实和增强现实：用于头戴式显示器和手柄的姿态跟踪。
- 姿态估计：通过融合加速度计和陀螺仪的数据，计算物体的姿态（俯仰角、横滚角和偏航角）。
- 运动跟踪：记录物体的运动轨迹和姿态变化。
- 导航：结合其他传感器（如 GPS）进行精确定位和导航。
- MPU6050：包含三轴加速度计和三轴陀螺仪，广泛用于低成本的惯性测量应用。
- LSM9DS1：包含三轴加速度计、三轴陀螺仪和三轴磁力计，适用于需要更高精度的应用。
- BMI160：高性能的惯性测量单元，适用于高端应用。
姿态解算
参考资料《惯性导航》秦永元，《线性代数》
1.坐标系
1.导航坐标系：在多旋翼中，通常采用北东地(NED)构成坐标系的X,Y,Z轴
2.机体坐标系：固联在多旋翼飞行器上，坐标系原点定位于飞行器中心点（假设中心点与重心点重合），X轴指向机头方向，Y轴指向机头的右方，Z轴垂直于X,Y轴构成平面垂直向下，这个可以用"右手定则"确定坐标轴的正方向
按照右手定则就可以判断飞机横滚或者俯仰或者航向绕轴旋转的正方向
飞机绕X轴旋转就叫横滚角，绕Y轴旋转就叫俯仰角，绕Z轴旋转就叫航向角，这三个角度其实就是姿态，我们求的就是这三个角度
姿态通俗的来说就是指我们站在地面上观察飞行器的俯仰(pitch)/横滚(roll)/航向(yaw)状态
欧拉角:绕
姿态
1. 姿态通俗说就是指我们站在地面观察飞行器的俯仰（pitch）/横滚(roll)/航向(yaw)状态。
飞行器需要知道当前自己的姿态，才能根据需要操控接下来的动作，比如保持平稳，翻滚。
2. 数学模型:姿态是用来描述一个刚体的固体坐标系和参考坐标系之间的角位置关系(姿态角)，有一些数学表示方法。很常见的就是欧拉角，四元数，矩阵，轴角。
（刚体（Rigid Body）并不是物体的另一种描述，而是对一类特殊物体的一种理想化模型。在物理学和工程学中，刚体是指在受力作用下形状和大小都不会发生改变的物体。这种理想化的模型简化了物理分析和计算，使得许多复杂的动力学问题变得更容易处理。）
3.飞行器姿态描述套用上面的数学模型，参考坐标系就对应着导航坐标系，是固定不变的。我们通常用坐标系R表示。固连坐标系就对应着机体坐标系，用坐标系r表示。那么我们就可以用欧拉角，四元数等数学方法来描述r于R的角位置关系。这就是飞行器姿态解算的数学模型
欧拉角，四元数，方向余弦矩阵在姿态解算中的思想
mpu通过AD收集元素数据，通过姿态解算算法得到当前飞行器的当前姿态（姿态使用四元数表示），然后将四元数转换为欧拉角，用于姿态控制算法中（PID控制）
飞行姿态角就是欧拉角
欧拉角是用来描述刚体在三维欧几里得空间的取向，刚体绕Z轴旋转得到a角度，绕X旋转得到白塔角，绕y轴旋转得到伽马角
四元数 是四维的
四元数可以当成三维空间多出来一个实数，i是x轴,j是y轴，k是z轴，q0是多出来的实数
四元数既可以看作一个四维空间的一个向量，又可以看作一个超复数
四元数表达式
1.矢量式，Q=q0+q,一个实数加一个三维空间的向量
三角式，一般推导的时候用
复数式和三角式一般用到的比较多
姿态解算的目的：按照数学定义来说：通过描述两个坐标系之间的角位置关系，来描述一个姿态，所以解算的目的就是来获取这个姿态角（其实就是欧拉角，阿尔法，白塔和伽马）
姿态解算的是获取飞行器机体坐标系在地球坐标系中的姿态角（也就是欧拉角），但是欧拉角不是直接测量的而是通过IMU（惯性测量单元）数据（陀螺仪，加速度传感器，罗盘（就是磁力计））通过Mahony互补滤波算法解算出姿态角，所以IMU（惯性测量单元如mpu6050）是获取姿态角的基础
算法推导
总体过程
姿态角的产生是由机体旋转与参考坐标系产生的
旋转有四种表示方式，方向余弦矩阵（适合坐标变换），欧拉角（最直观），轴角（适合几何推导），四元数（在组合旋转表示最佳），
通过mpu9250将输出的AD值通过(Mahony)姿态解算算法得到飞行器当前的姿态（姿态使用四元数表示），然后将四元数转化为欧拉角，用于姿态控制算法（PID控制）中。
通过惯性测量单元得到角度，姿态控制的时候要用到欧拉角，计算的时候是用四元数来计算姿态，在计算得到结果后要进行姿态控制的时候要将四元数表示方法转换为欧拉角表示，获取了此时的欧拉角（也就是此时的姿态），需要通过遥控器遥控数据控制姿态（经过PID运算），得到一个输出，PWM输出4个电机，控制飞机水平飞行还是俯仰还是横滚，控制四个电机的转速，
欧拉角描述的方向余弦矩阵：
空间中坐标系的依次旋转，等效于依次绕三个坐标轴的定轴旋转
图示，旋转的只是刚体坐标系
绕Z轴旋转，固定向量相对机体坐标系的X和Y轴发生变化，求在机体坐标系下固定向量的X2和Y2
机体绕Z轴旋转， Rx2=Rx1cosa + Ry1sina; Ry2 = Ry1cosa - Rx1sina; Rz2 = Rz1
得到的三个式子写成矩阵形式
蓝色的坐标系是导航坐标系（固定），紫色的是机体坐标系，三个旋转阵，组合成一个旋转矩阵。就是将这三个矩阵进行乘法运算（左乘）。机体坐标系可以旋转到导航坐标系上
导航坐标系左乘方向余弦矩阵得到机体坐标系（n指的是Navigation导航），（b值得是机体坐标系）
姿态解算，欧拉角微分方程包含大量三角运算，这给实时解算带来一定的困难，当俯仰角为90度的时候方程式会出现神奇的“死锁”，而且我们飞行姿态解析欧拉角解析可能相对速度过慢，达不到我们预期想要的解算速度
Cnb矩阵就是方向余弦矩阵，（也称旋转矩阵），这个矩阵描述了b系相对于n系的旋转，如果我们知道任意向量在b系中的坐标，那么就可以利用方向余弦矩阵确定它在n系中的坐标，反之亦然
将方向余弦矩阵简化为上诉，正交矩阵的属性就是一个矩阵是另外一个矩阵的转置矩阵
Cbn是b系转到n系（从机体坐标系转到导航坐标系），Cnb是n系转到b系(也就是从导航坐标系转到机体坐标系) T13是横轴是1，竖轴是3
将上面两个b系到n系的旋转不同的表示方法比对，通过下列计算得到欧拉角，但由于矩阵的元素是未知的，所以下面用其他方法得到矩阵元素
四元数得到方向余弦矩阵
四元数是由4个元构成的数，有四元数构成的方向余弦矩阵，通过反解算得到欧拉角，接下来是如何求解四元数
欧拉角可以描述方向余弦矩阵，四元数也能
如何求解四元数
四元数表示方式三角式
陀螺仪测量得到的是角速度，对四元数的三角表达式对时间求微分，简化后微分方程是下面矩阵形式（我也不知道怎么算），前面的矩阵有三个变量元素，可以通过陀螺仪计算得到，考虑到单片机只能处理离散数据
求解这个微分方程
根据一阶龙库塔法可以求解y的迭代公式
一阶龙格库塔法：可以通过有限次迭代求解出函数的微分方程，在前面这个对四元数三角表达式对时间微分简化
下面一阶龙库塔法没有介绍，不过直接得到了求解y的迭代公式，配合
和四元数的微分方程求解展开得到四元数的微分方程，把四元数的矩阵带到公式里面就可以得到下面这个四元数解算的矩阵公式，利用这个公式，可以利用C语言解算四元数的方法
在矩阵形式中，t+白二塔t是当前四元数，t是上次解算的四元数，白二塔t是微分的时间，就是计算的一个周期， Wx是陀螺仪测的横轴角速度，Wy是测得俯仰角速度，Wz是测得航向角速度
如果陀螺仪的数据是理想状态就不需要校准，我们需要加速度计和磁力计对陀螺仪数据进行校准
计算周期：白二塔t,迭代，上面式子第一项，开始是0
陀螺仪
陀螺仪测试的是角速度值，具有高动态特性，它是一个间接测量角度的器件，它测量的是角速度，要对角速度对时间积分才能得到角度，当陀螺仪固定不动，理想角速度值是0°/s，如果有一个偏置0.1°/s加上面，一分钟就是6°，一小时就是360°，就是转了一圈，所以一定要考虑积分误差的存在
加速度计
测量原理导致容易受高频信号干扰，使得加速度计在震动环境中高频干扰大，用加速度计矫正陀螺仪
例如，重力加速度从导航坐标系转换到机体坐标系，左矩阵1代表的是z
Vbx,Vby和Vbz是机体坐标系下重力加速度的坐标
由于四元数是由角速度数据解算的，由于角速度数据有误差，导致vb和gb不再重合，那么我们此时
由于理论向量和实际向量不重合，通过向量叉积得到误差
理论上认为加速度计测得的数据是绝对准确的，才会用理论机体加速度和重力加速度的叉积补偿到理论重力加速度上
得到的向量的模就是我们想要的误差
误差补偿：
Kp越大越相信加速度计（对误差的校准更大），Kp越小越相信陀螺仪（对误差的校准更小），后面的积分项用于消除静态误差
Ki用于补偿静态误差，Kp可以理解成对传感器的可信度，，不管Ki项，
把PI控制器计算得到的误差补偿值加在角速度上，得到可信的陀螺仪数据，然后代入一阶龙格库塔法公式就可以得到当前的四元数
陀螺仪校准值=把陀螺仪的值+误差值
得到校准的陀螺仪加速度，解算出四元数，然后反解得到欧拉角的时候也会得到一个准确的欧拉角，推导的重力加速度也会得到一个更加准确的值，所以就是这样一个互补融合的过程
软件代码部分，kp=ki=0完全相信陀螺仪
一阶龙格库塔法需要用到微分时间的一半，所以在imu.c的宏定义中有定义到微分时间的一半
q0,q1,q2,q3是四元数
exInt,eyInt,ezInt是对误差进行积分的值
这个函数的输入值是陀螺仪和加速度的数据，用到的指针形式，输出的存储到第三个参数中
为了加快单片机运算速度
理论的是方向余弦矩阵第三列
解算姿态得到的是角度
求解出四元数的四个值，就可以反解出欧拉角了
解算代码及注释如下
/***************************************************************************************
声 明
本项目代码仅供个人学习使用，可以自由移植修改，但必须保留此声明信息。移植过程中出现
其他不可估量的BUG，天际智联不负任何责任。请勿商用！
程序版本：V1.01
程序日期：2018-1-26
程序作者：愤怒的小孩 E-mail：1138550496@qq.com
版权所有：西安天际智联信息技术有限公司
****************************************************************************************/
#include "imu.h"
#include "math.h"
#include "filter.h"
#include "mpu9250.h"
#include "control.h"
#include "ICM20948.h"
#define Kp_New 0.9f //互补滤波当前数据的权重
#define Kp_Old 0.1f //互补滤波历史数据的权重
//#define G 9.80665f // m/s^2
//#define RadtoDeg 57.324841f //弧度到角度 (弧度 * 180/3.1415)
//#define DegtoRad 0.0174533f //角度到弧度 (角度 * 3.1415/180)
#define Acc_Gain 0.0001220f //加速度变成G (初始化加速度满量程-+4g LSBa = 2*4/65535.0)
#define Gyro_Gain 0.0609756f //角速度变成度 (初始化陀螺仪满量程+-2000 LSBg = 2*2000/65535.0)
#define Gyro_Gr 0.0010641f //角速度变成弧度(3.1415/180 * LSBg)
FLOAT_ANGLE Att_Angle; //飞机姿态数据
FLOAT_XYZ Gyr_rad,Gyr_radold; //把陀螺仪的各通道读出的数据，转换成弧度制
FLOAT_XYZ Acc_filt,Gry_filt,Acc_filtold; //滤波后的各通道数据
//FLOAT_XYZ Hmc_filt;
float accb[3],DCMgb[3][3]; //方向余弦阵（将 惯性坐标系 转化为 机体坐标系）
uint8_t AccbUpdate = 0;
/**************************实现函数*********************************************************************
函 数：static float invSqrt(float x)
功 能: 快速计算 1/Sqrt(x)
参 数：要计算的值
返回值：结果
备 注：比普通Sqrt()函数要快四倍See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
*********************************************************************************************************/
static float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
/*********************************************************************************************************
*函 数：void Prepare_Data(void)
*功 能：对陀螺仪去零偏后的数据滤波及赋予物理意义
*参 数：无
*返回值：无
*备 注：此函数对原始数据进行校准（去零偏，滤波处理，赋予物理意义 为姿态解算做准备
**********************************************************************************************************/
void Prepare_Data(void)
{
static uint8_t IIR_mode = 1;
MPU9250_Read(); //触发读取 ，立即返回
// ICM_ReadAccelGyroData();
MPU9250_Offset(); //对MPU6050进行处理，减去零偏。如果没有计算零偏就计算零偏
// Aver_FilterXYZ(&MPU9250_ACC_RAW,&Acc_filt,12);//对加速度原始数据进行滑动窗口滤波
SortAver_FilterXYZ(&MPU9250_ACC_RAW,&Acc_filt,12);//对加速度原始数据进行去极值滑动窗口滤波
//加速度AD值 转换成 米/平方秒
Acc_filt.X = (float)Acc_filt.X * Acc_Gain * G;
Acc_filt.Y = (float)Acc_filt.Y * Acc_Gain * G;
Acc_filt.Z = (float)Acc_filt.Z * Acc_Gain * G;
//printf("ax=%0.2f ay=%0.2f az=%0.2f\r\n",Acc_filt.X,Acc_filt.Y,Acc_filt.Z);
//陀螺仪AD值 转换成 弧度/秒
Gyr_rad.X = (float) MPU9250_GYRO_RAW.X * Gyro_Gr;
Gyr_rad.Y = (float) MPU9250_GYRO_RAW.Y * Gyro_Gr;
Gyr_rad.Z = (float) MPU9250_GYRO_RAW.Z * Gyro_Gr;
if(IIR_mode)
{
Acc_filt.X = Acc_filt.X * Kp_New + Acc_filtold.X * Kp_Old;
Acc_filt.Y = Acc_filt.Y * Kp_New + Acc_filtold.Y * Kp_Old;
Acc_filt.Z = Acc_filt.Z * Kp_New + Acc_filtold.Z * Kp_Old;
// Gyr_rad.X = Gyr_rad.X * Kp_New + Gyr_radold.X * Kp_Old;
// Gyr_rad.Y = Gyr_rad.Y * Kp_New + Gyr_radold.Y * Kp_Old;
// Gyr_rad.Z = Gyr_rad.Z * Kp_New + Gyr_radold.Z * Kp_Old;
Acc_filtold.X = Acc_filt.X;
Acc_filtold.Y = Acc_filt.Y;
Acc_filtold.Z = Acc_filt.Z;
// Gyr_radold.X = Gyr_rad.X;
// Gyr_radold.Y = Gyr_rad.Y;
// Gyr_radold.Z = Gyr_rad.Z;
}
accb[0] = Acc_filt.X;
accb[1] = Acc_filt.Y;
accb[2] = Acc_filt.Z;
if(accb[0]&&accb[1]&&accb[2])
{
AccbUpdate = 1;
}
}
/*********************************************************************************************************
*函 数：void IMUupdate(FLOAT_XYZ *Gyr_rad,FLOAT_XYZ *Acc_filt,FLOAT_ANGLE *Att_Angle)
*功 能：获取姿态角
*参 数：Gyr_rad 指向角速度的指针（注意单位必须是弧度）
* Acc_filt 指向加速度的指针
* Att_Angle 指向姿态角的指针
*返回值：无
*备 注：求解四元数和欧拉角都在此函数中完成
**********************************************************************************************************/
//kp=ki=0 就是完全相信陀螺仪
#define Kp 1.50f // proportional gain governs rate of convergence to accelerometer/magnetometer
//比例增益控制加速度计，磁力计的收敛速率
#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
//积分增益控制陀螺偏差的收敛速度
#define halfT 0.005f // half the sample period 采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
void IMUupdate(FLOAT_XYZ *Gyr_rad,FLOAT_XYZ *Acc_filt,FLOAT_ANGLE *Att_Angle)
{
uint8_t i;
float matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f };//初始化矩阵
float ax = Acc_filt->X,ay = Acc_filt->Y,az = Acc_filt->Z; //存储加速度测量数据
float gx = Gyr_rad->X,gy = Gyr_rad->Y,gz = Gyr_rad->Z; //存储陀螺仪测量数据
float vx, vy, vz; //从四元数推导的理论加速度的变量
float ex, ey, ez; //通过叉积从理论和实际求到的向量的一个误差
float norm; //求模变量
float q0q0 = q0*q0; //把相乘的量初始化成这样
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0) //判断加速度测量值是否有效，都为0是无效的
return;
//加速度计测量的重力向量(机体坐标系) //加速度计实际测出来的单位加速度计的向量
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
// printf("ax=%0.2f ay=%0.2f az=%0.2f\r\n",ax,ay,az);
//陀螺仪积分估计重力向量(机体坐标系) //四元数矩阵第三列就是理论重力加速度
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
// printf("vx=%0.2f vy=%0.2f vz=%0.2f\r\n",vx,vy,vz);
//测量的重力向量与估算的重力向量差积求出向量间的误差 //ex,ey,ez是量化陀螺仪的误差，将实际值和理论值进行叉积
ex = (ay*vz - az*vy); //+ (my*wz - mz*wy);
ey = (az*vx - ax*vz); //+ (mz*wx - mx*wz);
ez = (ax*vy - ay*vx); //+ (mx*wy - my*wx);
//用上面求出误差进行积分 //在单片机中的积分就是一个累加的过程，上一次累加的值加上这次这次误差计算值
exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
//将误差PI后补偿到陀螺仪
gx = gx + Kp*ex + exInt; //gx，gy,gz是陀螺仪测的值
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
//四元素的微分方程
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
//单位化四元数
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
//矩阵R 将惯性坐标系(n)转换到机体坐标系(b)
matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11(前列后行)
matrix[1] = 2.f * (q1q2 + q0q3); // 12
matrix[2] = 2.f * (q1q3 - q0q2); // 13
matrix[3] = 2.f * (q1q2 - q0q3); // 21
matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
matrix[5] = 2.f * (q2q3 + q0q1); // 23
matrix[6] = 2.f * (q1q3 + q0q2); // 31
matrix[7] = 2.f * (q2q3 - q0q1); // 32
matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
//四元数转换成欧拉角(Z->Y->X)
Att_Angle->yaw += Gyr_rad->Z *RadtoDeg*0.01f;
// Att_Angle->yaw = atan2(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3)* 57.3f; // yaw
Att_Angle->pit = -asin(2.f * (q1q3 - q0q2))* 57.3f; // pitch(负号要注意)
Att_Angle->rol = atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3)* 57.3f ; // roll
for(i=0;i<9;i++)
{
*(&(DCMgb[0][0])+i) = matrix[i];
}
//失控保护 (调试时可注释掉)
// Safety_Check();
}
扫一扫
7691
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
