芯片资讯
- 发布日期：2024-01-03 08:06 点击次数：109
姿态估计的作用？ 姿态估计是飞控算法的一个基础部分，而且十分重要。为了完成飞行器平稳的姿态控制，首先需要精准的姿态数据作为控制器的反馈。
飞控姿态估计的难点？ 姿态估计的一个难点主要是一般选用的惯性传感器，都是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，
EEPROM带电可擦可编程存储器芯片大全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)，
电子元器件PDF资料大全-az(i));
end
figure(5)
plot(t1，芯片交易网IC交易网pitch_gyro，
ATMEGA系列ATMEL芯片COMt1，
CMOS图像传感器集成电路芯片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('互补滤波'，电子元器件PDF资料大全'角速度积分')
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('自适应互补'，'参考角度')
仿真结果：
首先我们可以看出互补滤波的结果不再像陀螺仪角度那样，随着时间漂移。实际飞行时，固定的权重值很难找到理想值，要不陀螺仪的权重大了，动态性能还可以，大致能跟上角度，但是不能静态保持；加速度权重大了，噪音大，另外动态性能差，原地来回摆动时，得到的角度幅值会很小，这里的数据很难看出这个问题。 所以最好是当飞行器在动态过程时，我们更相信陀螺仪，反之，飞行器静止时，又更相信加速度计。即参数进行自适应调整。判定角速度数据，大于一定阈值，认为在运动，所以加大陀螺仪权重。 5、卡尔曼滤波 作为状态估计常用的算法，卡尔曼滤波的卡尔曼增益是动态调整的，所以这一点比固定权重的线性互补滤波要好，此外要注意的是卡尔曼的效果好坏与所选用的状态变量，建立的模型有很大关系，不可一概而论卡尔曼就一定很好，具体情况具体分析。 因卡尔曼滤波的使用条件是针对线性模型，且状态分布为高斯分布，所以这里建立两种线性模型，对比仿真结果。 模型一： 状态量为角度和角速度偏移，这里认为角速度偏移为常值，即角度是上一时刻的角度加（减）角速度偏移的角度，再加上角速度积分的增量。 
转换成状态空间形式：
代码：
%单轴kalman 模型1Q_angle = 0.1;Q_bias = 0.01;R_angle = 10; Q = [Q_angle 0; 0 Q_bias];pitchKF1 = zeros(m，1);pitchMeasure1 = zeros(m，1);bias = zeros(m，1);P = [0 00 0];C = [1 0]; for i = 2:m pitchMeasure1(i) = atan2(-ax(i)，-az(i)); A = [1 -dt; 0 1]; %Predicted state estimate x = F.x + B.u pitchKF1(i) = pitchKF1(i-1) + dt * (gy(i) - bias(i-1)); bias(i) = bias(i-1); %Predicted estimate covariance P = F.P.F' + Q P = A*P*A' + Q; %Innovation y = z - H.xy = pitchMeasure1(i) - pitchKF1(i); %Innovation covariance S = H.P.H' + R S = C*P*C' + R_angle; %Optimal kalman gain K = P.H'/S K = P * C' / S; %Updated state estimate x=x + K.y pitchKF1(i) = pitchKF1(i) + K(1)*y; bias(i) = bias(i) + K(2)*y; %Updated estimate covariance P = (I - K.H).P P = ([1 0; 0 1] - [K(1); K(2)] * C) * P;endfigure(7)plot(t1，pitchKF1，t1，pitch，'r');title('单轴卡尔曼模型结果验证');xlabel('Time(s)');grid;legend('单轴卡尔曼'，'参考角度')
仿真结果：
放大图像：
可以看出，此算法基本能得到正确结果，除了在某些地方跟踪不好，具体原因后面再讲。 模型二： 状态量分别是俯仰角、滚转角以及对应的角速度偏移。与上面的模型相比，这里我们将水平方向的两轴姿态合并在一起，这样易于代码实现，否则在软件中需针对三个轴进行三次调用。 
模型如下：
代码：
%两轴kalman 模型2 pitchKF2 = zeros(m，1); rollKF2 = zeros(m，1); bp = zeros(m，1); bq = zeros(m，1); pitchMeasure2 = zeros(m，1); rollMeasure2 = zeros(m，1); P_out = zeros(m，4，4); Q_pitch = 0.001;