无人机导航系统的多传感融合和姿态解算

在无人机领域，大疆一直都行走在第一队列，各款消费级无人机层出不穷，去年的人气王MavicAir2,今年的FJIFPV和DJIAir2S。大疆无人机在中国市场的占有率为90%，在全球市场的占有率为70%，而在美国市场的占有率接近80%，以至于大家可以频频在各类好莱坞大片中看到大疆无人机的身影。

(大疆无人机，图源：https://www.dji.com/cn/air-2s)

所谓无人机，就是利用无线电遥控设备和自备的程序控制装置操纵的不载人飞机，机上不配备驾驶舱，不需要飞行员驾驶，内部配置有驾驶仪等控制装置。近年来，无人机在民用、军事和科研等领域被广泛应用，例如侦察监视、对地攻击、交通巡逻等。其中，在无人机的飞行控制中，飞行姿态是重要参数之一，惯性导航系统是较为常用的一种导航方式。
 
一、传感器件

对于飞行器而言，姿态角的解算进度和导航精度密切相关，因此姿态解算和实时更新是导航系统的重点。加速度计和陀螺仪是姿态解算的主要器件。

（加速度计，图源：百度百科）

加速度计测量载体所受到的加速度力的大小，当加速度计保持稳定不动时，测量的数据即为重力在三维平面上的投影生成的反作用力的大小。仅仅利用三轴加速度计可以实现姿态角的估计，但是由于三轴加速度计动态响应较慢，若仅使用加速度计获取姿态角，会产生较大的延时，并且在运动过程中，姿态解算精度较低。

(陀螺仪，图源：百度百科)

陀螺仪是测量载体绕转轴转动的角速度，又称角速度计。当载体以某个速率运动时，可以通过测量哥氏加速度获取三维空间转动的角速率。理论上，对角速率进行积分既可获取载体额姿态角信息，但是由于陀螺仪的测量会受到漂移误差的影响，对于有规律的漂移，可以通过建立数学模型进行补偿，但是对于随机漂移误差的补偿尚未出现特别有效的方法。因此，仅利用陀螺仪解算姿态角也是不合适的。
 
二、惯性导航系统

1、工作原理

惯性导航系统通过对总的增量进行积分运算获取位置和姿态信息，主要可分为平台式惯性导航系统和捷联式惯性导航系统。
 
平台惯惯性导航系统中将加速度计和陀螺仪安装在导航平台上，用导航平台模拟导航坐标系，沿着导航坐标系轴向将导航加速度计固定测量轴方向，进而加速度计即可获取飞行器在导航坐标系中的加速度，再利用几何方法，从平台上获取飞行器的姿态和航向信息。
 
捷联惯性导航系统是将加速度计和陀螺仪直接固连在飞行器上，而不是采用机械式的陀螺稳定平台。按照飞行器的滚转轴、俯仰轴和偏航轴的方向确定加速度计和陀螺仪的输入轴，
 
利用姿态矩阵，将三轴加速度计和三轴陀螺仪测量的飞行器沿机体坐标系轴向的加速度和角速度信息，变换至导航坐标系中，进而获取飞行器的速度、位置、姿态信息。
 
两种惯性导航系统相比之下，捷联惯性导航系统具有自主式的导航方法，系统的可靠性高于平台惯性导航系统。
 
2、导航系统融合算法

鉴于加速度计和陀螺仪频域特性互补的特点，可以融合两者的原始数据进行姿态解算。下面主要介绍互补滤波和卡尔曼滤波两种常用融合算法。
 
1.互补滤波：

互补滤波器通常用于将不同传感器测量的物理意义相似或者相同的数据进行融合，对于输入的多个输入量需要进行相应的预处理，即具有高频噪声的输入量，需要使用低通滤波器进行滤波。在飞行器姿态测量中，对于加速度计输入的变量通常需要进行低通滤波，对于陀螺仪输入的变量会进行高通滤波。
 
下图是一种常见的二阶互补滤波算法实现框图。

在二阶互补滤波中，对输出值x与加速度计的测量值y1作差，然后将差值通过PI控制器进行处理后，以负反馈形式对陀螺仪测量值进行校正，简化的姿态角计算公式为

其中，Anglek为第k次滤波后的角度，Anglek-1为第k-1次滤波处理后的角度，angle_rate为陀螺仪测量的角速度值，t 为陀螺仪采样间隔时间。Angle_acc为加速度计的角度测量值，a为高通滤波系数，b为低通滤波系数，两者均为定值，且在系统工作过程中，该权值不会改变。
 
2.卡尔曼滤波

卡尔曼滤波是将最小均方误差作为标准进行最优估计。首先，将离散系统的控制过程用下面的方程进行表达。

k代表的是离散时间，X(K)代表的是k时刻的状态，U(k)代表的是k时刻的控制量，W(k)则代表的是干扰量，其表现形式为高斯白噪声，干扰量的协方差记作Q。M、N 代表系统的固有参数，当系统是多模型系统时，M、N表现为多维向量的形式。Z(k)代表的是k时刻的测量值，V(k)代表的是测量时的干扰信号，其协方差记为R,P是测量系统的固有参数。
 
卡尔曼滤波过程：

首先根据系统在k-1时刻的状态量X(k-1)估算其在下一时刻的状态量x(k|k-1)，根据系统总体的运行机制，有：

然后对x(k|k-1)的协方差进行估算，S(k|k-1)表示X(k|k-1)的协方差，S(k-|k-1)则表示x(k-1|k-1)的协方差。

将测量值和估算值进行融合，

x(k|k-1)表示系统在k时刻计算出的最优状态值。G(k)是卡尔曼增益，其意义是在指导数据融合时，应该更加信任测量值，还是更加信任估算值.
 
那么G(k)如何计算？

不同于互补滤波，卡尔曼滤波中的参数不是固定的，其与系统固有参数和干扰量都有关。进一步的，为保持系统的动态实时更新，需要对x(k|k)的协方差进行更新，即

三、光流导航系统

除了常见捷联惯导系统，光流导航系统也是近年来应用和研究较多的导航系统。光流导航系统是模仿生物的眼睛感受光的变化设计相关算法，将二维图像中的像素通过数学建模与三维载体进行关联，通过光流图像连续帧的变化，计算得到载体的运动信息。美国杨百翰大学的研究团队首次将光流导航技术应用在无人机上，使用了光电鼠标技术和激光测距。
 
在光流导航系统中，利用计算机视觉进行导航时通常是利用两帧连续图片的特征点之间的关系建立变化坐标，由此来判断无人机的运动信息。进行位置判断的关键是解决图像之间的特征点的提取以及匹配，并且由此建立起基于特征点的位置估计方程，设计出相应的光流导航算法。

(光流传感器，图源：https://www.jianshu.com/p/b7499050dc10)

基于特征点算法可以获取光流数据，但只是坐标信息，需要进行相应标定和转化。
 
在光流传感器相机标定中，为获取实际物体与图像中物体的转化关系，需要求出相机的内参矩阵和外参矩阵，即用一定尺寸的相机，在不同的视角下对同一个物体进行拍照，接着通过这些图像求得相机的内外参矩阵。内参矩阵由相机本身决定，外参矩阵由拍照的不同角度决定。
 
通过内外参矩阵，可以求出无人机上的光流传感器与光流场之间的对应关系，将光流信息转换成无人机的速度信息。为了获取无人机的运动信息，需要利用无人机的高度信息和载体坐标和地面坐标的转换矩阵。
 
因为当光线条件不好时，仅利用光流传感器将导致较大的误差。因此，将光流导航系统经常与惯性导航系统组合使用，可以互相弥补。光流/惯性松耦合模型是利用光流与惯导系统中的速度建立联系，利用光流系统对惯导系统进行数据修正，如下图所示。

利用光流导航系统的速度与惯导系统的速度作差，得到速度测量方程

Zp是观察矩阵，即惯导系统和光流导航系统提供的数据，Hp是测量转移矩阵，Vp是测量噪声矩阵，X 是t 时刻的状态矢量。VE-nE，VN-nN 表示惯导系统与光流导航系统之间速度的差值，h-hn表示惯导系统与光流导航系统之间位置的差值。

导航技术以及MEMS技术等理论的进步降低了无人机的生产、使用以及维护成本，使得无人机的应用范围逐渐扩大，与此同时，无人机的安全问题不可忽视，尤其是在复杂情况下更需要提高飞行的稳定性，多传感器融合和姿态解算具有重要的研究和应用意义。