任务二 卡尔曼滤波工作原理及排故方法
【任务引入】
基于Pixhawk飞控的无人机采用扩展卡尔曼滤波方法进行组合导航，飞行一段时间后，发现组合导航估计的高度和水平位置逐渐发散，以致无人机失控坠机。
【任务分析】
Pixhawk飞控采用了低价的传感器，单纯采用惯性测量单元测得的航姿和位置数据完全不能用于无人机的导航与控制。所以，Piwhawk飞控融合了多种传感器来获得航姿、角速度、位置、加速度等信息，以保障无人机的正常稳定飞行。Pixhawk用于组合导航的传感器有加速度计、陀螺仪、磁力计、气压计、视觉传感器、测高雷达及GPS接收机等设备，涉及了惯性导航、地磁导航、视觉导航、无线电导航及卫星导航，而将多种导航方式融合在一起的有效方式是采用卡尔曼滤波方法。
学习卡尔曼滤波的工作原理，才能掌握无人机组合导航系统故障检修与排除方法。
【相关知识】
卡尔曼滤波是美国工程师卡尔曼（Kalman）在线性最小方差估计的基础上，提出的最优线性递推滤波方法，具有数学结构简单、计算量小、存储量低、实时性高等优点，非常适合实时处理和在飞控上运行。
卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，是一种在不确定状况下组合多源信息得到所需状态最优估计的一种方法。其基本思想：采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新状态变量的估计，从而求得当前时刻的估计值。其过程可以用“预测未来”和“修正当下”来概括。
为了便于理解卡尔曼滤波，先从一个小例子开始。
（1）假设用两个传感器测量同一个物理量。但是它们每次的读数都不太一样，怎么办？
可能的方法是取平均，但如果两个传感器的测量精度有差别，那取平均就不合适了，这时候一般采用加权平均法，即精度高的传感器的权重高；精度低的传感器，权重也低。
（2）如果只用一个传感器测量某个物理量，但是还有一个数学模型可以推导出这个物理量。
方法是仿照上一个例子，把模型推导出来的值和传感器测出的值取加权平均。
卡尔曼滤波的实质也是对多个信息源进行加权平均，只不过各个信息源的权重是由系统自动计算得到。卡尔曼滤波要做的就是根据贝叶斯估计的相关理论，同时考虑预测模块和纠错模块的协方差，对误差小的项赋予较大的权重，对误差大的项赋予较小的权重，并使预测的误差最小。
另外，卡尔曼滤波用到了迭代法，即当前时刻得到的最优估计值是下一时刻预测模型的输入值。
下面介绍卡尔曼滤波的5个重要公式：
式（7-15）和式（7-16）为预测模块，是根据上一时刻（k-1时刻）的后验估计值来估计当前时刻（k时刻）的状态，得到k时刻的先验估计值。X（k-1|k-1）为k-1时刻的最优估计值，U（k）为k时刻的输入，A为状态转移矩阵，B为将输入转换为状态的矩阵，X（k|k-1）为k时刻的先验状态估计值，是滤波的中间计算结果，即根据上一时刻（k-1时刻）的最优估计预测的k时刻的结果，是预测方程的结果；P（k-1|k-1）为k-1时刻后验估计值的协方差，Q为过程激励噪声，P（k|k-1）为k时刻的先验估计协方差。
式（7-17）和式（7-18）为纠错模块，是使用当前时刻的测量值来更正预测阶段估计值，得到当前时刻的后验估计值。H是状态变量到测量（观测）的转换矩阵，R为测量噪声协方差，Kg（k）为卡尔曼增益，即分配给信息源的权重，Z（k）为k时刻的测量值，X（k|k）为k时刻的后验状态估计值，即更新后的最优估计值。
式（7-19）为k时刻的后验估计协方差及k时刻最优估计值的误差。
从上式可以看出，新的最优估计是之前最优估计的预测加上已知的外界影响的修正；新的不确定度是预测的不确定度加上环境的不确定度。
由于惯性导航能提供无人机的位置、速度、航向和姿态角数据，所以组合导航一般以惯导为主，而惯性导航的实质是基于航位推算法，经过各种运算得到物体的位置信息。这样，卡尔曼滤波方法的预测过程就可以由惯性导航来实现，而磁力计、GPS、高度计、视觉传感器等设备可提供观测值参与到纠错校正过程，最终实现对无人机状态的最优估计。
考虑到无人机运动的复杂性，预测过程的状态方程和纠错模块的测量方程是非线性的，这时就要用到扩展卡尔曼滤波方法（EKF），即将非线性系统线性化。Pixhawk飞控固件就是采用EKF来进行多传感器信息融合，即基于陀螺仪、加速度计、磁罗盘、GPS、空速计和气压计等设备来估计无人机位置、速度和角度方向。其优点在于通过融合所有可用的测量数据，能够更好地过滤掉有明显误差的测量数据，这使无人机不会受单个传感器故障的影响。EKF还可以使用光流和激光测距仪等可选传感器进行测量，以辅助导航。
Pixhawk飞控固件中的状态向量如下所示：
式中，q为导航坐标系（北东地坐标系）到机体坐标系的四元数，v为导航坐标系的无人机速度，p为导航坐标系的无人机位置，mNED为导航坐标系下的磁场向量，vwind为无人机在北向和东向的风速，Δθb为角度增量的偏移误差，Δvb为速度增量的偏移误差，mb为磁力计的偏移误差。
飞控上参与EKF的传感器如下：
（1）IMU：用于采集单位角增量和速度增量数据，最低采样频率为100 Hz，参与EKF预测过程，并提供机体坐标系下的陀螺仪角度增量和加速度计速度增量。
（2）GPS：提供导航坐标系下的无人机速度和位置信息。
（3）高度计：如气压计、测距仪，最低采样频率为5 Hz，提供导航坐标下的高度。
（4）磁力计：最低采样率为5 Hz，磁力计数据以两种方式使用。
一种方式是使用倾斜估计和磁偏角，将磁力计测量值转换为偏航角。该偏航角用作EKF的观测量。这种方法不太准确并且没有考虑机体坐标系的磁场偏移，然而其对于磁异常和大的陀螺仪初始偏移更加鲁棒。它是飞行器刚启动并停留在地面上时使用的默认方法。
另一种方式是磁力计的X、Y、Z三轴读数用作单独测观测值。这种方法更准确，并且允许机体坐标系的偏移。但假定地磁场环境只是缓慢变化，这种方法在存在巨大的外部磁异常时性能较差，是飞行器在空中并且爬升超过1.5 m高度时的默认方法。
（5）光流：提供机体坐标系下的无人机姿态角速度。
（6）视觉传感器：提供机体坐标系下的无人机速度。
EKF仅使用IMU的数据进行状态预测，其他传感器数据则用作纠错校正。根据传感器测量值的不同组合，EKF具有不同的操作模式。在启动时，EKF会检查传感器的最小可行组合，并在初始倾斜、偏航及高度对准完成后进入一个提供旋转、垂直速度、垂直位置、IMU角增量误差和IMU速度增量误差估计的模式。此模式需要有传感器的数据，如偏航数据源（由磁力计或者外部视觉设备提供）和高度数据源。所有的EKF操作模式都需要这个最小的数据集。然后可以使用其他传感器数据来估计附加的状态。
那么如何使Pixhawk飞控使用EKF算法来进行组合导航呢？这就需要进行如下设置：
（1）连接飞控至Mission Planner地面站，单击“配置/调试”按钮，然后选择“全部参数表”，在表里面将EK2_ENABLE设置为1，启动EKF，但EKF并不会参与无人机的控制（图7-13）。
图7-13 设置EK2_ENABLE
（2）将AHRS_EKF_TYPE参数设置为2，使得EKF参与无人机的控制（图7-14）。
图7-14 设置AHRS_EKF_TYPE
（3）如果要使用多个IMU或单个IMU，就要对EK2_IMU_MASK参数进行设置。如果只使用IMU1，则要将EK2_IMU_MASK设为1；只用IMU2，则EK2_IMU_MASK设为2即可；如果飞行控制器有两个（或多个）IMU可用，则两个EKF“内核”（即EKF的两个实例）将并行运行，每个使用不同的IMU，则需要将EK2_IMU_MASK设为3，飞控会选取传感器数据一致性最好、性能最佳的EKF核心作为单个EKF输出使用（图7-15）。
图7-15 设置EK2_IMU_MASK
（4）设置好参数后，需要重新启动飞控。
【任务实施】
1.卡尔曼滤波后的高度数据发散
这种现象最常见的原因是由振动引起的IMU测量数据异常。所以，首先要确保飞控已经进行了有效的振动隔离安装。飞控的振动隔离安装拥有6个自由度，意味着有6个固有频率。一般来说，减振装置的6个固有频率应该大于25 Hz，以避免干扰飞控的动态特性。另外，减振装置固有频率还应小于电动机频率。如果减振装置的固有频率与电动机或者桨叶通过频率相一致，那么减振装置反而会加剧振动。
通过设置相关参数，EKF可减弱由于振动带来的高度数据发散的影响。
（1）将参与EKF的高度传感器新息（测量残差，即传感器实测值与EKF最优估计值的差值）门限值变为原来的两倍。例如EKF使用的是气压计，则将EK2_HGT_I_GATE变为之前的两倍即可（图7-16）。
图7-16 设置EK2_HGT_I_GATE
（2）要将EK2_ACC_P_NSE的值增加0.5，如果增加后高度数据仍然发散，则需要再增加0.1，但EK2_ACC_P_NSE的值不能超过1（图7-17）。
图7-17 设置EK2_ACC_P_NSE
注意：设置这些参数将使得EKF对GPS垂直速度及气压数据方面的偏差更敏感。
2.卡尔曼滤波后的水平位置数据发散
这种现象最常见的原因分析及解决方案如下：
（1）飞控振动严重。飞控如果振动严重，会影响导航坐标系下的位置和速度测量残差（图7-18）。
图7-18 位置和速度测量残差变化曲线
解决方法是改善飞控的振动隔离装置，尽可能减小机身振动对飞控带来的影响；同时可以尝试增加EK2_ACC_P_NSE和EK2_GYRO_P_NSE参数值，但可能会使得EKF更容易受GPS故障的影响。
（2）陀螺仪漂移误差过大。如果陀螺仪角度增量偏移误差变化超过5×10-4，则位置数据发散很有可能是由陀螺仪漂移误差过大引起的（图7-19）。
图7-19 角度增量偏移误差变化曲线
解决方法是重新校准陀螺仪，并检查陀螺仪是否对温度过于敏感，即在陀螺仪冷启动升温期间，查看陀螺仪角度偏移率是否大于3°/s。如果确定对温度敏感，要考虑给陀螺仪加上隔热材料，保持陀螺仪环境温度恒定不变。
（3）航向对准没做好。当无人机开始飞行时，航线对准没做好就会使得飞行方向的速度测试比率迅速增加（图7-20）。
图7-20 速度测试比率变化曲线
解决方法是重新进行磁罗盘校准，并检查磁罗盘上箭头标识是否指向无人机机头方向。
（4）GPS精度过低。GPS精度过低会伴随GPS速度的测量残差增加，这些误差一般是由多路径效应、信号遮挡及信号干扰导致的（图7-21）。
图7-21 GPS速度测量残差变化曲线
检查周围是否有干扰，并加长GPS天线固定支架长度，同时远离GPS信号遮挡和反射严重的地方。
【拓展阅读】
飞行日志的查看
在无人机的日常飞行中，经常会遇到很多问题，如飞机无法起飞、飞行不稳等相关问题，这就需要学习使用飞行日志来分析产生该现象的原因并给出其解决方法。
Pixhawk飞控一般有两种记录飞行日志的方法，分别是“数据闪存日志”和“遥测日志”。“数据闪存日志”被记录在飞控上，需要在飞行后，从飞控上下载至计算机文件夹；“遥测日志”则是在数传模块连接飞控时，地面站在计算机里自动记录得到。
1.数据闪存日志的查看方法
在无人机飞行前，启动地面站软件，将飞控连接至地面站，并进行相关调参工作，待无人机可以正常飞行后，再进行无人机的飞行试验。在无人机飞行结束后，需要打开计算机并连接无人机飞控，将飞行日志从飞控上导出至计算机地面站。在状态窗找到“数据闪存日志”，可单击“通过Mavlink下载闪存日志”，如图7-22所示。
单击“通过Mavlink下载闪存日志”后，会弹出日志下载的相关界面，直接选择“下载全部日志”即可，但若不需要下载全部日志，则需要先在日志明细中通过日期找到所需要分析的日志编号，然后勾选该日志，最后单击“下载这些日志”即可，如 图7-23所示。
图7-22 数据闪存日志的基本显示
图7-23 下载飞行日志
飞行日志下载完成后，单击“回顾日志”，电脑便会自动弹出有关飞行日志的文件夹，单击该文件夹，查看自己所需的飞行日志即可，如图7-24所示。
图7-24 飞行日志相关文件夹
图7-24 飞行日志相关文件夹（续）
2.遥测日志查看方法
遥测日志会在无人机飞行过程中将飞行日志直接导入地面站进行保存，因此，在查看遥测日志时，并不需要额外连接无人机飞控。
在查看遥测日志时，单击“TLog＞Kml或图像”按钮，地面站便会出现其相关弹窗，在弹窗中单击“图表日志”按钮即可查看无人机的飞行日志，如图7-25所示。
图7-25 遥测日志的打开方式
若无人机的飞行时间较长，飞行日志里记录的曲线波动较为密集时，可框选想要查看时间段内的波动曲线，并通过鼠标中键滚动放大该区域。
3.相关参数及含义
为了解无人机在飞行过程中的各种姿态信息，无人机上都保存了记录飞行过程各种数据的日志文件，而无人机飞行日志中的不同参数也分别表示着不同的含义。为了更好地分析飞行日志并对无人机进行排故，就必须熟悉飞行日志相关参数的含义，并会分析飞行日志曲线。飞行日志中的基本参数的含义及作用见表7-2。
表7-2 飞行日志中的基本参数的含义及作用
续表
但无人机飞行过程中的错误信息（ERR）也是无人机调试与维修的重要参数信息，因此我们也需要对其进行学习与了解。飞行日志中的错误信息的含义见表7-3。
表7-3 飞行日志中的错误信息的含义
续表
【巩固提高】
1.简述卡尔曼滤波的思想及工作过程。
2.如何让Pixhawk飞控的EKF参与无人机的控制？
3.扩展卡尔曼滤波相较于互补滤波有什么优点？