姿态解算
准确快速的姿态解算是无人机姿态控制和位置控制的基础。现在常用的姿态解算方法主要有互补滤波、扩展卡尔曼滤波。相比较两种方法,互补滤波较为简单,容易调试,我这里也是选择Mahony互补滤波方法进行姿态解算。
无人机姿态
无人机姿态角一共有三个,分别是滚转、俯仰、偏航,分别是绕无人机的X轴、Y轴、Z轴旋转的角度。知道无人机的实时姿态情况是进行下一步控制的基础,很难想象不知道无人机的姿态角度还能控制它平稳的飞行。
机体坐标系和大地坐标系
大地坐标系比较容易理解,我们选取正北、正东、正上构成这个坐标系的X、Y、Z轴的正方向。我们用R表示大地坐标系。为了编程和控制的方便,我们在飞机上也构想一个坐标系,称为机体坐标系,分别选取滚转、俯仰、偏航所绕的轴作为x、y、z的正方向,我们用r表示机体坐标系。
通过传感器能够得到飞机相对于r坐标系的姿态,但是飞机在高空中飞来飞去,坐标系是不断变化的,我们很难肉眼观察到它相对于机体坐标系的角度。但是如果能够知道飞机的姿态角相对于大地坐标系的值,我们就可以很直接看出来或者显示出来,方便后面的控制。
姿态表示方法
姿态表示的方法常见的有三种:四元数、欧拉角、旋转矩阵。我们所说的滚转、俯仰、偏航就是用欧拉角来表示的。(这里放图不是很方便,后面再放图来形象表示一下吧)
算法 | 优点 | 缺点 | 其他 |
---|---|---|---|
四元数 | 直接求解四元数微分方程,四个未知参数,计算量小 | 漂移比较严重 | 可用于变换时保存姿态角 |
欧拉角 | 直接求解欧拉微分方程,得到pitch、roll、yaw,直观容易理解 | 求解方程困难,当pitch接近90度时候会出现退化现象 | |
旋转矩阵 | 求解姿态矩阵微分方程,避免退化现象 | 参数量多 | 可用于向量的表示和变换 |
姿态解算
姿态解算是根据IMU数据求解无人机在空中的姿态,也叫做IMU数据融合。得到陀螺仪的角速率后(后面再写一篇博客说说怎么进行IMU传感器数据的获取和校准),按照道理可以用陀螺仪的角速率积分得到角度,因为陀螺仪为三轴传感器,可以得到滚转、俯仰、偏航的角度。但是由于积分的作用,会随着时间产生非常大的积分误差。那如何尽量消除这个误差呢?
IMU模块一般都不会仅仅包含一个陀螺仪传感器,至少还会有一个加速度计。而加速度计通过飞机的受力计算可以得到一个角度。(但加速度计无法感知水平方向的角度,所以必须得加上磁力计才能修正偏航角)那这两个角度更应该相信谁呢?由于飞机在空中飞行时,难免会产生振动,但加速度计对震动比较敏感,陀螺仪却不敏感。所以我们可以说,要长时间相信陀螺仪的数据,偶尔相信加速度计和磁力计的数据,也就是说用加速度计和磁力计的数据来修正陀螺仪带来的积分误差。
四元数和欧拉角在姿态解算中使用
姿态解算的核心在于旋转,旋转的表示方法主要有:四元数、旋转矩阵、欧拉角。其中,欧拉角最为直观,用来表示姿态角具体为多少度。旋转矩阵表示向量最为方便,而四元数参数较少,更为方便表示旋转,用于保存变换中的飞机姿态。
主要代码分析
/* ! Using accelerometer, sense the gravity vector. */
/* ! Using magnetometer, sense yaw. */
static void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz)
{
float initialRoll, initialPitch;
float cosRoll, sinRoll, cosPitch, sinPitch;
float magX, magY;
float initialHdg, cosHeading, sinHeading;
initialRoll = atan2(-ay,-az); //求出Roll,Pitch用加速度表示方法
initialPitch = atan2(ax, -az);
cosRoll = cosf(initialRoll); //求余弦正弦
sinRoll = sinf(initialRoll);
cosPitch = cosf(initialPitch);
sinPitch = sinf(initialPitch);
//其中一种的旋转方法引起Z轴的角度变化情况
magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
magY = my * cosRoll - mz * sinRoll;
initialHdg = atan2f(-magY, magX);
//具体公式参见全权老师的《多旋翼无人机设计与控制》 103页
cosRoll = cosf(initialRoll * 0.5f);
sinRoll = sinf(initialRoll * 0.5f);
cosPitch = cosf(initialPitch * 0.5f);
sinPitch = sinf(initialPitch * 0.5f);
cosHeading = cosf(initialHdg * 0.5f);
sinHeading = sinf(initialHdg * 0.5f);
//四元数q0,q1,q2,q3表示
q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
q0q0=1;
// auxillary variables to reduce number of repeated operations, for 1st pass
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
}
/*
使用的是Mahony互补滤波算法,没有使用Kalman滤波算法
改算法是直接参考pixhawk飞控的算法,可以在Github上看到出处
https://github.com/hsteinhaus/PX4Firmware/blob/master/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
*/
static void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt)
{
float recipNorm;
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
// Make filter converge to initial solution faster
// This function assumes you are in static position.
// WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
if(bFilterInit == 0)
{
NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
bFilterInit = 1;
}
//增加一个条件: 加速度的模量与G相差不远时。 0.75*G < normAcc < 1.25*G
/*Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)*/
/* 如果加速计各轴的数均是0,那么忽略该加速度数据。否则在加速计数据归一化处理的时候,会导致除以0的错误 */
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
{
float halfvx, halfvy, halfvz;
// Normalise accelerometer measurement
//归一化,得到单位加速度
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
/*把加速度计的数据进行归一化处理。其中invSqrt是平方根的倒数,使用平方根的倒数而不是直接使用平方
根的原因是使得下面的ax,ay,az的运算速度更快,通过归一化处理后,ax,ay,az的数值范围变成-1到+1 */
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and magnetic field
//根据当前四元数的姿态值来估算出各重力分量
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += ay * halfvz - az * halfvy;
halfey += az * halfvx - ax * halfvz;
halfez += ax * halfvy - ay * halfvx;
}
! If magnetometer measurement is available, use it.
if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)))
{
float hx, hy, hz, bx, bz;
float halfwx, halfwy, halfwz;
// Normalise magnetometer measurement 归一化处理
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Reference direction of Earth's magnetic field 旋转矩阵中磁力计 这个不是很明白!!
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
bx = sqrt(hx * hx + hy * hy);
bz = hz;
// Estimated direction of magnetic field
//根据当前四元数的姿态值来估算出各地磁分量Wx,Wy,Wz
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += (my * halfwz - mz * halfwy);
halfey += (mz * halfwx - mx * halfwz);
halfez += (mx * halfwy - my * halfwx);
}
// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f)
{
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f)
{
gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki
gyro_bias[1] += twoKi * halfey * dt; //Ki*Ts*X(n)
gyro_bias[2] += twoKi * halfez * dt;
// apply integral feedback
gx += gyro_bias[0];
gy += gyro_bias[1];
gz += gyro_bias[2];
}
else
{
gyro_bias[0] = 0.0f; // prevent integral windup
gyro_bias[1] = 0.0f;
gyro_bias[2] = 0.0f;
}
// Apply proportional feedback
gx += twoKp * halfex; //Kp*X(n)
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// Time derivative of quaternion. q_dot = 0.5*q\otimes omega.
//! q_k = q_{k-1} + dt*\dot{q}
//! \dot{q} = 0.5*q \otimes P(\omega)
dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx);
q0 += dt*dq0;
q1 += dt*dq1;
q2 += dt*dq2;
q3 += dt*dq3;
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
}
#define so3_comp_params_Kp 1.5f
#define so3_comp_params_Ki 0.05f //0.05f
//函数名:IMUSO3Thread(void)
//描述:姿态软件解算融合函数
//该函数对姿态的融合是软件解算
void IMUSO3Thread(void)
{
//! Time constant
float dt = 0.01f; //s
static uint32_t tPrev=0,startTime=0; //us
uint32_t now;
uint8_t i;
static uint8_t t=0;
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f}; //rad
float gyro[3] = {0.0f, 0.0f, 0.0f}; //rad/s
float acc[3] = {0.0f, 0.0f, 0.0f}; //m/s^2
float mag[3] = {0.0f, 0.0f, 0.0f};
float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f };
/**< init: identity matrix */
now=HAL_GetTick();
dt=(tPrev>0)?(now-tPrev)/1000.0f:0;
tPrev=now;
ReadIMUSensorHandle(); //得到IMU数据,注意单位
gyro[0] = imu.gyro[0];
gyro[1] = imu.gyro[1];
gyro[2] = imu.gyro[2];
acc[0] = -imu.accb[0];
acc[1] = -imu.accb[1];
acc[2] = -imu.accb[2];
mag[0]= imu.accg[0];
mag[1]= imu.accg[1];
mag[2]= imu.accg[2];
// NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
-acc[0], -acc[1], -acc[2],
mag[0], mag[1], mag[2],
so3_comp_params_Kp,
so3_comp_params_Ki,
dt);
// Convert q->R, This R converts inertial frame to body frame.
Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12
Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13
Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21
Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3; // 22
Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23
Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31
Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32
Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3; // 33
//1-2-3 Representation.
//Equation (290)
//Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel.
// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll
euler[1] = -asinf(Rot_matrix[2]); //! Pitch
euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]);
//DCM . ground to body
for(i=0; i<9; i++)
{
*(&(imu.DCMgb[0][0]) + i)=Rot_matrix[i];
}
imu.rollRad=euler[0];
imu.pitchRad=euler[1];
imu.yawRad=euler[2];
/* 直接利用陀螺仪的Z轴角速度会有很大的漂移 */
Yaw_Gyro_Earth_Frame = -sin(imu.rollRad) * imu.gyro[0]*180.f/M_PI_F+
cos(imu.rollRad)*sin(imu.pitchRad) * imu.gyro[1]*180.f/M_PI_F+
cos(imu.pitchRad) * cos(imu.rollRad ) * imu.gyro[2]*180.f/M_PI_F; //用于Yaw 角速度反馈
/* 对磁力计的磁偏角进行补偿 https://blog.csdn.net/u013636775/article/details/72675148 */
/* https://blog.csdn.net/xiaoxilang/article/details/80525236 */
/* 计算磁力计数据在水平面 X Y轴上的投影 补偿前提是磁力计与加速度计同轴,方向正反错误需要调整*/
imu.accg[1] = imu.accg[0] * sin(imu.pitchRad) * sin(imu.rollRad)
+ imu.accg[1] * cos(imu.pitchRad)
- imu.accg[2] * cos(imu.rollRad) * sin(imu.pitchRad);
imu.accg[0] = imu.accg[0] * cos(imu.rollRad)+ imu.accg[2] * sin(imu.rollRad);
euler[2]= atan2((double)(imu.accg[0]),(double)(imu.accg[1])); /* 磁力计的 X Y 轴颠倒了,在滤波前调换了一下*/
Mag_Yaw = euler[2] * 180.0f / M_PI_F; /* 磁力计计算出的偏航角 单位 deg */
att_angle[0] += Yaw_Gyro_Earth_Frame * dt; /* 陀螺仪积分计算出的偏航角 单位 deg */
/*---- 偏航角一阶互补 -----*/
if((Mag_Yaw>90 && att_angle[0]<-90)|| (Mag_Yaw<-90 && att_angle[0]>90))
{
att_angle[0] = -att_angle[0] * 0.99f + Mag_Yaw * 0.01f;
}
else
{
att_angle[0] = att_angle[0] * 0.99f + Mag_Yaw * 0.01f;
}
imu.yaw = att_angle[0]; /* 最后计算出的偏航角 单位 deg */
/* 计算欧拉角,单位 deg/s 实际上Pitch 和Roll反了,参考Pixhawk的硬件的结果*/
imu.roll= euler[0] * 180.0f / M_PI_F;
imu.pitch= euler[1] * 180.0f / M_PI_F;
// imu.yaw= euler[2] * 180.0f / M_PI_F;
if(imu.yaw<0)
{
imu.yaw+=360;
}
else if(imu.yaw>360)
{
imu.yaw-=360;
}
}
代码原理分析
对于四元数法的姿态求解,需要求解的就是四元数的值。方向余弦矩阵用来转换时表示向量,本来矩阵的元素都是一些三角函数,这里可以用四元数来表示矩阵的元素。最后变成求解由四元数构成的余弦矩阵。
加速度计仅仅测量的是重力加速,三轴加速度计是重力加速度的三轴分量。重力加速度的方向和大小是固定的,通过这种关系,可以得到机体坐标系和地面坐标系的角度关系。在大地坐标系R中,加速度计a输出为
[
0
,
0
,
1
]
T
[0,0,1]^{T}
[0,0,1]T
经过旋转矩阵的变换到机体坐标系r中,a变成
[
v
x
,
v
y
,
v
z
]
T
[v x, v y, v z]^{T}
[vx,vy,vz]T
在机体坐标系中,加速度计的测量值为
[
a
x
,
a
y
,
a
z
]
T
[a x, a y, a z]^{T}
[ax,ay,az]T
那得到两个加速度计的向量值,我们由此可以做向量积,得到误差向量,再用误差向量来修正旋转矩阵。(这一个思想在光流传感器数据互补融合中也用到了)还有一些数学理论知识,这里打公式不太好搞,等以后慢慢更新。