基于多传感器融合的无人机三维状态估计

王琦 王伟 杨溢 南京信息工程大学 自动化学院

引言

2020年爆发的新冠肺炎可以算的上是整个人类面临的挑战，在这场“战疫”中涌现出许多具有奉献精神逆行者。同样在这此挑战面前，无人机也更进一步的进入了大众的视野。从正常存在于百米高空的空旷区域飞行至熙熙攘攘闹市街头，无人机空投喊话已经成为现实，为这一特殊时期的治安管理等方面做出了不小的贡献。同样的，对于公共场所的消毒，使用植保类无人机装载消毒液即可完成任务，在这样的高危环境下，使用无人机替代人工作业很大程度上保护了工作人员的安全。对于无人机行业来说，这次天灾给无人机带来的不仅仅是机遇，其实也更是一次挑战。

无人机从空旷、无人的高空环境到拥挤复杂的街头，除了要考虑环境对各个传感器干扰外，还需要考虑无人机飞行时的安全问题。这就要求无人机系统具有较强的稳定性，能应对传感器如GPS之类的信号丢失等功能。目前在无GPS环境下实现位置估计以及导航一般使用激光雷达与IMU融合或者视觉相机与IMU融合的方案。但在大多数情况下，在无GPS环境下光线不是很充足，而视觉导航对环境的可见度是有要求的，在室内特别是光线比较昏暗的条件下使用视觉相机可能无法取得较理想的效果，甚至会导致无法进行定位以及导航，具有较大的安全隐患。

针对这一情况，本文旨在采用低成本的二维激光雷达传感器完成可以满足上述要求的状态估计系统，为后续的二次功能开发打好基础。

1 简介

本系统使用自主研发的飞控，结合IMU，GPS，地磁计，光流，定高雷达以及激光雷达传感器，实现对在室内环境下无人机的位置，速度以及姿态进行估计。整套系统搭载在四旋翼无人机上，各模块之间采用Can通信的形式。图1即为本文的实验平台。

图1 四旋翼无人机平台

对于室内环境下，采用基于图优化的激光slam算法完成对无人机的定位以及地图的构建，结合IMU与光流模块，使用扩展卡尔曼滤波器融合出无人机机体坐标系下的xy方向的速度信息，采用简易融合的方式，结合气压计以及定高雷达传感器获取高度信息；在室外环境下，则使用GPS传感器，结合IMU模块与气压计推算出无人机实时的经纬度，速度以及高度信息。

2 姿态估计框架

结合上文所述，本文所提出的多传感器融合的状态估计结构如图2所示：

图2 状态估计框架

由图2可知，IMU作为核心传感器经过坐标系转换后经过积分后为EKF的预测部分；光流以及GPS可以提供速度信息，GPS与激光雷达可以提供位置信息，定高雷达与气压计可以提供高度信息，这些传感器的数据经过卡方检测以保证数据的有效性，经过坐标转换后作为EKF的观测部分，经过EKF融合之后，生成的结果即为所求。具体的，在室内条件下使用的融合框架为：通过机体坐标系下的IMU的加速度数据积分后与光流的速度数据进行融合，高度方向由IMU的加速度数据的二次积分与气压计和定高雷达数据进行融合，判断无人机处于移动或者静止状态来切换气压计或者定高雷达作为观测值；在室外条件下，水平方向上，使用IMU的xy轴加速度数据与GPS的速度数据进行融合，具体为将机体坐标系下的xy轴的加速度转换到NED坐标系下，作为卡尔曼滤波器的输入，GPS测量的数据信息则为观测值，经过卡尔曼滤波器后输出的结果即为无人机控制系统所使用的无人机当前的速度值。

3 融合算法

从创建到现在的半个世纪之中，卡尔曼滤波(KF)理论目前已经广泛应用于无人机的状态估计之中。到目前为止仍然是最有效的估计算法之一。但是在实际使用的当中，大多数系统都属于非线性系统。如本文中使用的无人机系统就是一个典型的非线性系统。很多研究人员提出了各种的近似估计方法，考虑到所使用的硬件条件的限制，本文选择使用EKF来进行传感器数据的融合。

系统的预测方程如下：

在本文当中，状态量 x=[α,β,γ,p,v,bgyro,bacc]；其中α,β,γ表示无人机的滚转角，俯仰角以及偏航角，p=[px,py,pz]表示无人机的在NED坐标系下的位置信息，v=[vx,vy,vz]表示无人机在NED坐标系下的速度信息，bgyro,bacc分别表示陀螺仪以及加速度计的偏移量；w_k为高斯白噪声。

系统的矫正方程如下：

在这项研究中，在室内环境下的估计是基于激光雷达完成的，其测量值为相对距离。在室外环境下的估计是基于GPS传感器完成的，其测量值为绝对距离。

4 实验数据及分析

本论文采用的实验方案为：无人机从室外环境起飞后悬停，之后通过飞手操作直线飞进室内环境，观察此时无人机是否能在无人工干预的情况下保持定点悬停，观察这期间无人机的状态变化，主要观察无人机姿态角的变化以及由GPS速度到光流传感器的速度切换是否会出现跳变。

实验数据表明，在整个飞行过程中，滚转角以及偏航角方向基本没有变化，飞机机体坐标系y轴方向的速度也在±0.1范围之内变动，基本保持不变，经过融合后的速度信息与实际飞行的速度信息基本一致。在实际飞行过程中，GPS信号断开后，位置融合使用的数据源由GPS传感器切换至光流传感器，此时y轴方向的融合出的速度与实际测量值的误差较GPS条件下的误差要稍微大一点。导致这种现象的原因主要为光流传感器的测量值不够精确导致，但在GPS有效时的速度信息切换至室内光流速度信息时，整个估计系统并未发生较明显的数据跳变，整体上满足使用要求，因此，通过实验可知本文所述的状态估计方法为实际可行的。

5 结束语

本文以四旋翼无人机系统为例，通过搭载各传感器进行数据融合实现在室内室外条件下无人机实时状态的估计。首先介绍了姿态估计的框架，其次介绍融合所使用的算法，最后通过室内以及室外实际飞行试验验证算法的有效性。本系统目前已经经过无人机多个架次的实际飞行测试，对于室内无人机的定位与导航具有重要意义，更为无人机系统的二次开发打好基础。