飞控算法-姿态解算之互补滤波
姿态解算是通过读取各个传感器的数据进行融合最终得到当前飞行器的姿态，姿态通常用Euler角（Roll-Pitch-Yaw）来表示
姿态解算框架
姿态解算的数据源可来自于陀螺仪或加速度计，他们分别都可以得到当前姿态，但实际上他们都有各自的传感器性能的优缺点，需要取长补短，进行数据融合才能得到真实的姿态，这种方法简称 互补滤波 算法
传感器特性
互补滤波
姿态解算原理
假设我们现在没有加速度计，只有陀螺仪，那么我们该如何求解姿态呢？
陀螺仪
1、陀螺仪传感器输出的是角速度，单位为x°/s，我们取ADI的一款高精度IMU来模拟
这款IMU默认输出为16位的LSB数据，最终得到的角速度为：LSB*Scaler
假如：
X_GYRO_OUT = 22887
Scaler = 0.013108
那么：
g_X = X_GYRO_OUT * Scaler = +300°/sec
也就是说在1秒内旋转了300度，正负根据设备定义的方向，右手定则来决定正负
2、现在得到了角速度，我们可以直接对角速度对时间积分得到当前的角度，这是最简单，最直接的想法，但是通过积分后得到的角度漂移会吓死你，你可以把设备静止在桌面上，观察积分后的角度随直接变化，下面是我直接用角速度积分得到的姿态，用python来绘制的曲线：
代码先送上来：
/*
* 陀螺仪积分求角度
* param1: 原始3轴角速度,单位为x°/s
* param2: 间隔时间,单位为ms
*/
void gyro_int(Vector3f &g, float dt)
{
static Vector3f att(0,0,0);
att.x += g.x * dt * 0.001;
att.y += g.y * dt * 0.001;
att.z += g.z * dt * 0.001;
printf("att is %.3f %.3f %.3f\n", att.x, att.y, att.z);
}
// 主循环代码
void stablize_loop()
{
static uint32_t last_time = 0;
float dt = 0;
if(last_time == 0)
dt = 0;
else
dt = AP_HAL::millis() - last_time;
last_time = AP_HAL::millis();
gyro_int(gyro_data, dt);
}
目前我们只分析X轴的数据：
3、表示姿态有很多种方式，主流的有欧拉角，旋转矩阵，四元数。但都有各自的优缺点，欧拉角最直接但是存在万象锁，旋转矩阵太麻烦耗费资源多，四元数是目前主流的姿态求解的方法。关于四元数的定义和基本概念我这里不做解释了，有兴趣可以自行推导
- 如何得到四元数
一阶龙格库塔法
公式：
- q0(t+dt) = q0(t) + 1/2dt * (-Wxq1 - Wyq2 - Wz*q3)
- q1(t+dt) = q1(t) + 1/2dt * ( Wxq0 - Wyq3 + Wz*q2)
- q2(t+dt) = q2(t) + 1/2dt * ( Wxq3 + Wyq0 - Wz*q1)
- q3(t+dt) = q3(t) + 1/2dt * (-Wxq2 + Wyq1 + Wz*q0)
根据公式，我们需要W(x,y,z)，这不就是角速率吗？我们可以直接从陀螺仪里读取到即可
注意：W(x,y,z)为角速率，陀螺仪数据为角速度，需要把度转成弧度(Degree_2_Radian)
上代码:
/*
* 一阶龙格库塔法求四元数
* 四元数初始值: q0 = 1, q1 = q2 = q3 = 0
* Wx Wy Wz为角速率单位为弧度
* Wx = gx*(PI/180) WY = gy*(PI/180) Wz = gz*(PI/180)
* q0(t+dt) = q0(t) + 1/2dt * (-Wx*q1 - Wy*q2 - Wz*q3)
* q1(t+dt) = q1(t) + 1/2dt * ( Wx*q0 - Wy*q3 + Wz*q2)
* q2(t+dt) = q2(t) + 1/2dt * ( Wx*q3 + Wy*q0 - Wz*q1)
* q3(t+dt) = q3(t) + 1/2dt * (-Wx*q2 + Wy*q1 + Wz*q0)
*/
void gyro_2_quaternion(Vector3f &gyro, float dt)
{
float half_dt = 0.5*dt*0.001; // 单位为秒
float q0_t = 0, q1_t = 0, q2_t = 0, q3_t = 0;
q0_t = half_dt * (-gyro.x * q1 - gyro.y*q2 - gyro.z*q3);
q1_t = half_dt * ( gyro.x * q0 - gyro.y*q3 + gyro.z*q2);
q2_t = half_dt * ( gyro.x * q3 + gyro.y*q0 - gyro.z*q1);
q3_t = half_dt * (-gyro.x * q2 + gyro.y*q1 + gyro.z*q0);
q0 += q0_t;
q1 += q1_t;
q2 += q2_t;
q3 += q3_t;
float normal = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= normal;
q1 /= normal;
q2 /= normal;
q3 /= normal;
//printf("Quaternion is %.3f %.3f %.3f %.3f\n", q0, q1, q2, q3);
}
- 如何得到欧拉角
旋转矩阵
前面说到，可以通过旋转矩阵来表示姿态，只不过计算量太大，而且存在奇异点，所以没用，但是他在四元数求欧拉角确实很有用的。
首先上旋转矩阵：
旋转方式有很多种，不同的旋转方式得到的矩阵也不一样，要注意！
这里采用东北天坐标系下的Z-X-Y旋转
接着上四元数下的旋转矩阵：
罗德里格旋转
接着根据矩阵相等，就可以反解出欧拉角
送上代码：
/*
* 四元数转欧拉角
*
*
*
*/
Vector3f quaternion_2_euler_angle(float q0, float q1, float q2, float q3)
{
#if 0
// NED北东地坐标系下的Z-Y-X旋转
float roll = atan2f(2.0f*(q0*q1 + q2*q3), (q0*q0 - q1*q1 - q2*q2 + q3*q3));
float pitch = safe_asin(2.0f*(q0*q2 - q1*q3));
float yaw = atan2f(2.0f*(q0*q3 + q1*q2) , (q0*q0 + q1*q1 - q2*q2 - q3*q3));
#else
// ENU东北天坐标系下的Z-X-Y旋转
float roll = -atan2f(2.0f*(q1*q3 - q0*q2), (q0*q0 - q1*q1 - q2*q2 + q3*q3));
float pitch = safe_asin(2.0f*(q0*q1 + q2*q3));
float yaw = atan2f(2.0f*(q1*q2 - q0*q3), (q0*q0 - q1*q1 + q2*q2 - q3*q3));
#endif
Vector3f euler_angle = {roll*RAD_TO_DEG, pitch*RAD_TO_DEG, yaw*RAD_TO_DEG};
return euler_angle;
}
4、问题来了，为啥要用这么复杂的方式转来转去，目的就为了一个，更新四元数，来实现互补滤波
现在我们可以通过四元数来求欧拉角了，但是还是存在一个积分漂移的情况，接下来我们就该我们的加速度计登场了
加速度计
我们可以通过加速度计得到3轴加速度把他和重力加速度求反余弦，就可以得到角度，但是这不是我们需要的！我们需要加速度计是为了获取他的静态性能来更新四元数，补偿陀螺仪
1、得到实际加速度a)
我们从加速度计读取得到的数据是实际的加速度值g，这里注意从传感器里读出的原始加速度值mg，这是一个力的单位，我们一般要把他转换为加速度单位m/s/s，只需要将mg * 9.80665f * 0.001，记得数据要归一化处理哦
//归一化
acc_data.normalize();
2、得到理论加速度a’
我们的四元数又该出场了！
/*
* 四元数得出理论三轴加速度
* ax = 2(q1*q3 - q0*q2)
* ax = 2(q0*q1 + q2*q3)
* az = q0*q0 - q1*q1 - q2*q2 + q3*q3
*/
void quaternion_2_acc(Vector3f &acc, float q0, float q1, float q2, float q3)
{
acc.x = 2*(q1*q3 - q0*q2);
acc.y = 2*(q0*q1 + q2*q3);
acc.z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
}
3、得到误差a(e)
向量叉乘
a(e) = a X a’ = |a| * |a’| * sinr = sinr
小角近似
a(e) = r
得到的误差角为r
上代码：
/*
* 向量叉乘得到误差e
* ex = ay * vz - az * vy
* ey = az * vx - ax * vz
* ez = ax * vy - ay * vx
*/
void vector_cross(Vector3f &a, Vector3f &v, Vector3f &e)
{
e.x = a.y * v.z - a.z * v.y;
e.y = a.z * v.x - a.x * v.z;
e.z = a.x * v.y - a.y * v.x;
}
4、误差得到了，现在该上我们的PI补偿控制器了
这个控制器的作用，就是为了把计算出来的误差补偿到陀螺仪上，使之快速收敛到真实的数据上去，最后得到一个补偿后的陀螺仪数据
上代码:
/*
* 互补滤波算法
* PI补偿控制器u=kp*error+ ki*∫error
*
*/
void complementary_filter(Vector3f &_g, Vector3f &g, Vector3f &e)
{
float Kp = 0.8f;
float Ki = 0.001f;
//比例项
Vector3f g_P(e.x * Kp, e.y * Kp, e.z * Kp);
//积分项
static Vector3f int_e(0, 0, 0);
Vector3f g_I;
g_I.x = int_e.x + e.x * Ki;
g_I.y = int_e.y + e.y * Ki;
g_I.z = int_e.z + e.z * Ki;
_g = g + g_P + g_I;
}
5、现在陀螺仪的数据是补偿后的了，我们可以通过转四元数再反解欧拉角得到补偿后的姿态
全部代码如下：
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
/*
* 四元数转欧拉角
*
*
*
*/
Vector3f quaternion_2_euler_angle(float q0, float q1, float q2, float q3)
{
#if 1
// NED北东地坐标系下的Z-Y-X旋转
float roll = atan2f(2.0f*(q0*q1 + q2*q3), (q0*q0 - q1*q1 - q2*q2 + q3*q3));
float pitch = safe_asin(2.0f*(q0*q2 - q1*q3));
float yaw = atan2f(2.0f*(q0*q3 + q1*q2) , (q0*q0 + q1*q1 - q2*q2 - q3*q3));
#else
// ENU东北天坐标系下的Z-X-Y旋转
float roll = -atan2f(2.0f*(q1*q3 - q0*q2), (q0*q0 - q1*q1 - q2*q2 + q3*q3));
float pitch = safe_asin(2.0f*(q0*q1 + q2*q3));
float yaw = atan2f(2.0f*(q1*q2 - q0*q3), (q0*q0 - q1*q1 + q2*q2 - q3*q3));
#endif
Vector3f euler_angle = {roll*RAD_TO_DEG, pitch*RAD_TO_DEG, yaw*RAD_TO_DEG};
return euler_angle;
}
/*
* 一阶龙格库塔法求四元数
* 四元数初始值: q0 = 1, q1 = q2 = q3 = 0
* Wx Wy Wz为角速率单位为弧度
* Wx = gx*(PI/180) WY = gy*(PI/180) Wz = gz*(PI/180)
* q0(t+dt) = q0(t) + 1/2dt * (-Wx*q1 - Wy*q2 - Wz*q3)
* q1(t+dt) = q1(t) + 1/2dt * ( Wx*q0 - Wy*q3 + Wz*q2)
* q2(t+dt) = q2(t) + 1/2dt * ( Wx*q3 + Wy*q0 - Wz*q1)
* q3(t+dt) = q3(t) + 1/2dt * (-Wx*q2 + Wy*q1 + Wz*q0)
*/
void gyro_2_quaternion(Vector3f &gyro, float dt)
{
float half_dt = 0.5*dt*0.001; // 单位为秒
float q0_t = 0, q1_t = 0, q2_t = 0, q3_t = 0;
q0_t = half_dt * (-gyro.x * q1 - gyro.y*q2 - gyro.z*q3);
q1_t = half_dt * ( gyro.x * q0 - gyro.y*q3 + gyro.z*q2);
q2_t = half_dt * ( gyro.x * q3 + gyro.y*q0 - gyro.z*q1);
q3_t = half_dt * (-gyro.x * q2 + gyro.y*q1 + gyro.z*q0);
q0 += q0_t;
q1 += q1_t;
q2 += q2_t;
q3 += q3_t;
float normal = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= normal;
q1 /= normal;
q2 /= normal;
q3 /= normal;
//printf("Quaternion is %.3f %.3f %.3f %.3f\n", q0, q1, q2, q3);
}
/*
* 四元数得出理论三轴加速度
* ax = 2(q1*q3 - q0*q2)
* ax = 2(q0*q1 + q2*q3)
* az = q0*q0 - q1*q1 - q2*q2 + q3*q3
*/
void quaternion_2_acc(Vector3f &acc, float q0, float q1, float q2, float q3)
{
acc.x = 2*(q1*q3 - q0*q2);
acc.y = 2*(q0*q1 + q2*q3);
acc.z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
}
/*
* 向量叉乘得到误差e
* ex = ay * vz - az * vy
* ey = az * vx - ax * vz
* ez = ax * vy - ay * vx
*/
void vector_cross(Vector3f &a, Vector3f &v, Vector3f &e)
{
e.x = a.y * v.z - a.z * v.y;
e.y = a.z * v.x - a.x * v.z;
e.z = a.x * v.y - a.y * v.x;
}
/*
* 互补滤波算法
* PI补偿控制器u=kp*error+ ki*∫error
*
*/
void complementary_filter(Vector3f &_g, Vector3f &g, Vector3f &e)
{
float Kp = 0.8f;
float Ki = 0.001f;
//比例项
Vector3f g_P(e.x * Kp, e.y * Kp, e.z * Kp);
//积分项
static Vector3f int_e(0, 0, 0);
Vector3f g_I;
g_I.x = int_e.x + e.x * Ki;
g_I.y = int_e.y + e.y * Ki;
g_I.z = int_e.z + e.z * Ki;
_g = g + g_P + g_I;
}
void get_gyro(Vector3f &g)
{
uint8_t i, sign;
uint16_t gyro[3] = {0};
float gryo_con[3];
uint8_t reg[2] = {0};
uint8_t res[2] = {0};
uint8_t pTxFinal[2] = {0x0, 0x83};
reg[0] = ADIS16XXX_REG_X_GYRO_OUT;
_dev->transfer_fullduplex(reg, res, 2);
gyro[0] = res[0]<<8|res[1];
reg[0] = ADIS16XXX_REG_Y_GYRO_OUT;
_dev->transfer_fullduplex(reg, res, 2);
gyro[0] = res[0]<<8|res[1];
reg[0] = ADIS16XXX_REG_Z_GYRO_OUT;
_dev->transfer_fullduplex(reg, res, 2);
gyro[1] = res[0]<<8|res[1];
_dev->transfer_fullduplex(pTxFinal, res, 2);
gyro[2] = res[0]<<8|res[1];
for(i = 0; i < 3; i++) {
sign = gyro[i]&0x8000;
if(sign)
gryo_con[i] = (-(~(short )gyro[i]+1)) * _adis_chip_info._gyro_scale * DEG_TO_RAD;
else
gryo_con[i] = ((short )gyro[i]) * _adis_chip_info._gyro_scale * DEG_TO_RAD;
//printf("gryo_con[%d] [%d].\n", i, gryo_con[i]);
}
g.x = gryo_con[0];
g.y = gryo_con[1];
g.z = gryo_con[2];
}
void get_acc(Vector3f &a)
{
uint8_t i, sign;
uint16_t accel[3] = {0};
float accel_con[3];
uint8_t reg[2] = {0};
uint8_t res[2] = {0};
uint8_t pTxFinal[2] = {0x0, 0x83};
reg[0] = ADIS16XXX_REG_X_ACCEL_OUT;
_dev->transfer_fullduplex(reg, res, 2);
accel[0] = res[0]<<8|res[1];
reg[0] = ADIS16XXX_REG_Y_ACCEL_OUT;
_dev->transfer_fullduplex(reg, res, 2);
accel[0] = res[0]<<8|res[1];
reg[0] = ADIS16XXX_REG_Z_ACCEL_OUT;
_dev->transfer_fullduplex(reg, res, 2);
accel[1] = res[0]<<8|res[1];
_dev->transfer_fullduplex(pTxFinal, res, 2);
accel[2] = res[0]<<8|res[1];
for(i = 0; i < 3; i++) {
sign = accel[i]&0x8000;
if(sign) {
accel_con[i] = (-(~(short )accel[i]+1)) * _adis_chip_info._accel_scale *GRAVITY_MSS/1000.0f;
} else {
accel_con[i] = ((short )accel[i]) * _adis_chip_info._accel_scale *GRAVITY_MSS/1000.0f;
}
//printf("accel_con[%d] [%f].\n", i, accel_con[i]);
}
a.x = accel_con[0];
a.y = accel_con[1];
a.z = accel_con[2];
}
/*
* 陀螺仪积分求角度
*
*/
void gyro_int(Vector3f &g, float dt)
{
static Vector3f att(0,0,0);
att.x += g.x * dt * 0.001;
printf("roll is %.3f\n", att.x);
}
/*
* 姿态矩阵更新
*
*/
void attitude_update()
{
Vector3f gyro_data;
Vector3f acc_data;
get_gyro(gyro_data);
get_acc(acc_data);
static uint32_t last_time = 0;
float dt = 0;
if(last_time == 0)
dt = 0;
else
dt = AP_HAL::millis() - last_time;
last_time = AP_HAL::millis();
#if 0
//直接求角度
//gyro_int(gyro_data, dt);
//四元数求角度
//gyro_2_quaternion(gyro_data, dt);
//Vector3f euler_angle = quaternion_2_euler_angle(q0, q1, q2, q3);
//printf("%.3f,%.3f,%.3f\n", euler_angle.x, euler_angle.y, euler_angle.z);
#else
//实际三轴加速度
//归一化
acc_data.normalize();
//printf("normalize acc x[%.3f] y[%.3f] z[%.3f]\n", acc_data.x, acc_data.y, acc_data.z);
//理论三轴加速度
Vector3f _acc;
quaternion_2_acc(_acc, q0, q1, q2, q3);
//printf("_acc x[%.3f] y[%.3f] z[%.3f]\n", _acc.x, _acc.y, _acc.z);
//理论和实际三轴加速度的叉乘得到误差e
Vector3f error_acc;
vector_cross(acc_data, _acc, error_acc);
//printf("error_acc x[%.3f] y[%.3f] z[%.3f]\n", error_acc.x, error_acc.y, error_acc.z);
Vector3f _gyro;
//PI比例积分补偿陀螺仪的数据
complementary_filter(_gyro, gyro_data, error_acc);
//得到新的四元数
gyro_2_quaternion(_gyro, dt);
//新四元数解算出补偿后的欧拉角
Vector3f euler_angle = quaternion_2_euler_angle(q0, q1, q2, q3);
printf("%.3f,%.3f,%.3f\n", euler_angle.x, euler_angle.y, euler_angle.z);
#endif
}
可以对比补偿前后补偿后的角度数据，发现补偿后没有出现积分漂移，而且在动态性能上也能快的收敛。我们可以调整PI控制器的参数K和i来控制更相信谁的数据！
本次就不上传最后的测试数据了，不是没有测试，而是数据搞丢了，懒得重新搞！
1111
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
