飞控算法-姿态解算之互补滤波

姿态解算是通过读取各个传感器的数据进行融合最终得到当前飞行器的姿态,姿态通常用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;elsedt = 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));
#endifVector3f 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));
#endifVector3f 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;elsegryo_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;elsedt = 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);//理论和实际三轴加速度的叉乘得到误差eVector3f 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来控制更相信谁的数据!

本次就不上传最后的测试数据了,不是没有测试,而是数据搞丢了,懒得重新搞!

飞控算法-姿态解算之互补滤波相关推荐

  1. stm32 MPU6050 姿态解算 Mahony互补滤波算法

    文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...

  2. 四旋翼无人机飞控系统设计(姿态解算)

    姿态解算   姿态传感器读出加速度和角速度,而对一个系统的自动控制往往需要更加上层和贴近应用的的一个属性:角度.所以需要通过加速度和角速度进行数据融合转化得到姿态角度.   以MPU6050为例,姿态 ...

  3. MPU6050加速度、角速度的解算以及互补滤波使用

    MPU6050加速度.角速度的解算以及互补滤波使用 MPU6050加速度的解算 MPU6050角速度的解算 互补滤波使用(数据融合) 数字低通滤波器 数字高通滤波器 MPU6050加速度的解算 先根据 ...

  4. 详解几种飞控的姿态解算算法

    姿态解算是飞控的一个基础.重要部分,估计出来的姿态会发布给姿态控制器,控制飞行平稳,是飞行稳定的最重要保障.有关姿态解算的基础知识,这里笔者不会细细描述,有关这方面的资料,网上已经有很多.主要是先掌握 ...

  5. bvp解算器是什么_几种飞控的姿态解算算法

    姿态解算是飞控的一个基础.重要部分,估计出来的姿态会发布给姿态控制器,控制飞行平稳,是飞行稳定的最重要保障.有关姿态解算的基础知识,这里笔者不会细细描述,有关这方面的资料,网上已经有很多.主要是先掌握 ...

  6. 无名飞控姿态解算和控制(一)

    无名飞控的姿态解算和控制 从imu和磁力计,气压计拿到数据后,进入AHRSUpdate_GraDes_Delayback函数,其中X_w_av,Y_w_av,Z_w_av来自陀螺仪,X_g_av,Y_ ...

  7. 卡尔曼滤波五个公式_基于ROS的卡尔曼滤波姿态解算

    前段时间由于项目关系需要实现基于卡尔曼滤波的姿态解算,也就是说融合加速度计.陀螺仪及磁罗盘进行AHRS姿态解算,得到机器人的姿态角. 本文的学习需要有一定的卡尔曼滤波器基础,可以参考白巧克力亦唯心的卡 ...

  8. 飞行器控制笔记(二)——姿态解算之坐标变换与欧拉角更新姿态

    飞行器控制笔记(二)--姿态解算之坐标变换与欧拉角更新姿态 飞行器控制笔记(二)--姿态解算之坐标变换与欧拉角更新姿态 一.基本假定 二.坐标变换矩阵 2.1绕z轴旋转 2.2绕y轴旋转 2.3绕x轴 ...

  9. 四旋翼姿态解算——互补滤波算法及理论推导

    转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...

  10. 一阶高通滤波+二阶Mahony滤波的四元数姿态解算

    此次实验我使用icm20602进行 icm20602输出有以下特点: 3轴陀螺仪可选量程有±250dps,±500dps,±1000dps,±2000dps.(dps:degrees per seco ...

最新文章

  1. mysql让数据库的数据按照id来排序方法
  2. 012 pandas与matplotlib结合制图
  3. 机器学习入门——MINST
  4. 用计算机收传真,使用计算机发送和接收传真.doc
  5. Android获取手机Cpu温度
  6. cisco anyConnect 不用每次输入密码的办法
  7. 渗透测试之通道构建Cheat Sheet
  8. Android 自定义价格日历控件
  9. 前端基础总结--CSS
  10. Java测试工程师技术面试题库【持续补充更新】
  11. 古希腊神话故事:菲勒美拉
  12. 霍普菲尔得神经网络(Hopfield Neural Network)
  13. 为什么要建议用自增列做主键
  14. 深度学习——模型调整
  15. react-native播放视频组件 react-native-video的用法
  16. 最近都在说移民火星,这些黑科技你了解吗?
  17. Faster BiSeNet:A Faster Bilateral SegmentationNetwork for Real-time Semantic Segmentation
  18. linux服务精简优化解析,CentOS系统环境精简优化详解
  19. Java中数组怎么初始化?数组初始化方法
  20. 牛逼,前阿里 P7 技术专家!

热门文章

  1. 广义加性模型(GAM)
  2. ArcGIS Pro 3.0最新消息
  3. IAR for stm8安装破解
  4. x550网卡linux驱动,Intel英特尔X520/X540/X550/82599系列万兆网卡驱动5.10.2版For Linux(2021年2月1日发布)...
  5. ctbs mysql_C/C++/Java
  6. mmap内存映射用法总结
  7. 前端特效 —— 八卦图旋转(纯css)
  8. 勒索病毒:解密工具整理
  9. linux at24测试程序,C51读写AT24C04源代码及AT24C04测试程序
  10. 测试项目实战----RuoYi后台管理系统