飞控姿态解算算法解析

姿态估计的作用？

姿态估计是飞控算法的一个基础部分，而且十分重要。为了完成飞行器平稳的姿态控制，首先需要精准的姿态数据作为控制器的反馈。

飞控姿态估计的难点？

姿态估计的一个难点主要是一般选用的惯性传感器，都是MEMS器件，精度相对较差；此外，实际工作中很难准确的判定姿态估计的是否准确。

姿态估计的指标？

一般考虑三个性能，收敛性、精确性、准确性。

Ø 收敛性：即估计出的姿态角数据不会轻易发散，在动态变化时，能很快的收敛到对应的角度；

Ø 精确性：比如飞行器放置不动，此时得到的姿态角在0度左右波动，这个波动范围即考虑的精确性；

Ø 准确性：这个比较难以考究，即没办法确定所得到的角度的精确程度。一般用外部参考的方法测量，比如飞机上同时挂载自己的飞控以及高精度的IMU设备（比如xsens、sbg等），飞行完成后，对比自己飞控所解算的角度和外部设备的误差；又或者，在室内装vicon设备来给出外部参考。注意，验证算法的时候，最好还是用实际飞行的数据，否则加速度噪声对算法的影响无法验证。

需要什么基础？即在进行姿态估计前需要做什么？

主要对传感器数据进行校准、滤波，陀螺仪、加速度、地磁的数据预处理以及滤波算法请看专栏或公众号的其他文章。

本文主要从工程角度去实现姿态估计算法，相应的理论知识也可见其他文章。

姿态估计如何做？

一言以蔽之，根据陀螺仪的角度数据高频特性好，而加速度计和地磁计得到的角度数据低频特性好，从而进行互补，得到最优角度。无论什么算法，本质都是陀螺仪积分得到角度，然后根据加速度计和地磁计修正积分的漂移误差。

工程上每个算法的引入都是为了解决问题的，所以下面将从简单的互补开始，逐步解释每个算法的优缺点。

1、 记录实际飞行数据，需要保存陀螺仪、加速度、地磁计数据；

如图所示：

笔者这里采用了现成的模块，所以有参考的角度数据。

2、对好数据坐标系

即调整数据的正负号，初学者往往会忽略这一点，这个在姿态估计算法中非常重要，很多人经常有疑问，为什么自己移植别人的代码，一模一样，但是自己就得不到正确的结果，往往原因都在这里。

这里笔者以前右下（xyz）为机体系，另外如果飞行数据噪音很大，可以在仿真中做滤波处理。

3、分析单独的陀螺仪积分角度与加速度计计算得到的角度数据

方法：

角速度数据直接积分；

加速度数据：

代码：

%对比角速度积分与加速度得到的姿态角之间的区别
%角速度积分得到的角度会随时间漂移
%加速度得到的角度会噪声很大

roll_gyro = zeros(m,1);
roll_acc = zeros(m,1);

for i = 2:m
    
    if i==1
        roll_gyro(1) = gx(1) * dt;
    else
        roll_gyro(i) = roll_gyro(i-1) + gx(i) * dt;
    end
    
    roll_acc(i) = atan2(ay(i),-az(i));
    
end

figure(4)
plot(t1,roll_gyro,t1,roll_acc,'r');title('roll 角速度、加速度推角度对比');xlabel('Time(s)');grid;

pitch_gyro = zeros(m,1);
pitch_acc = zeros(m,1);

for i = 2:m
    
    if i==1
        pitch_gyro(1) = gy(1) * dt;
    else
        pitch_gyro(i) = pitch_gyro(i-1) + gy(i) * dt;
    end
    
    pitch_acc(i) = atan2(-ax(i),-az(i));
    
end

figure(5)
plot(t1,pitch_gyro,t1,pitch_acc,'r');title('pitch 角速度、加速度推角度对比');xlabel('Time(s)');grid;

仿真结果：

如上图所示，蓝色的陀螺积分角度随着时间会有漂移，而加速度计得到的角度则噪声很大，都无法使用（这里的数据不是飞行数据，所以噪声很小）。

所以，既然单独的角度都各自有缺陷，而恰好一个有漂移一个没有漂移，一个噪音大一个噪音小，很自然能联想到用互补的办法，每个周期的最优角度为两者加权得到。

4、线性互补滤波

通过设置一个权重值，让每个周期得到的角度由两个数据源共同作用，还可以通过调节权重值，选择是更相信陀螺仪还是加速度。

代码：

%互补滤波 单轴
complementation_filtered = zeros(m,1);
adaption_complementation_filtered = zeros(m,1);

coeff = 0.75;

for i=2:m
    complementation_filtered(i) = (complementation_filtered(i-1) + gx(i)*dt) * coeff + (1 - coeff) * roll_acc(i);
    if abs(gx(i)) > 0.2
        adaption_complementation_filtered(i) = (adaption_complementation_filtered(i-1) + gx(i)*dt) * coeff + (1 - coeff) * roll_acc(i);
    else
        adaption_complementation_filtered(i) = (adaption_complementation_filtered(i-1) + gx(i)*dt) * 0.005 + (1 - 0.005) * roll_acc(i);
    end
end

figure(6)
subplot(411)
title('滚转');
plot(t1,complementation_filtered,'r',t1,roll_gyro);grid;
legend('互补滤波','角速度积分')
subplot(412)
plot(t1,complementation_filtered,'r',t1,roll_acc);grid;
legend('互补滤波','加速度参考')
subplot(413)
plot(t1,complementation_filtered,'r',t1,roll);grid;
legend('互补滤波','参考角度')
subplot(414)
plot(t1,adaption_complementation_filtered,'r',t1,roll);grid;
legend('自适应互补','参考角度')

仿真结果：