组合导航系统在四旋翼无人机上的实现*

黄 建，胡越黎，，杨文荣，冉 峰

（1.上海大学 机电工程与自动化学院，上海200072；2.上海大学 微电子研究与开发中心，上海200072）

组合导航系统在四旋翼无人机上的实现*

黄 建1，胡越黎1，2，杨文荣2，冉 峰2

（1.上海大学 机电工程与自动化学院，上海200072；2.上海大学 微电子研究与开发中心，上海200072）

针对无人飞行器行业的快速发展和导航系统对飞行器的重要性，提出了组合导航系统的融合方案。介绍了捷联惯导系统的原理、姿态算法。通过对惯性传感器进行误差标定和补偿，利用扩展卡尔曼滤波器建立了INS/GPS组合导航系统。仿真实验表明，组合导航系统的工作性能要优于纯惯性导航系统，能够为飞行器提供较高的导航精度。最后，将这种组合导航系统在四旋翼无人飞行器上进行了实现。

无人机；惯性导航系统；组合导航系统；卡尔曼滤波器

0 引言

近年来，随着智能化产业的迅速发展，无人机行业逐渐走进人们的视线。由于四旋翼无人机的结构简单可靠，具有垂直起降、定点悬停、稳定低速飞行和在小空间内飞行的能力[1]，所以受到广泛的关注并在多个领域得到了广泛的应用[2]。

在四旋翼无人机系统发展所面临的关键技术中，导航系统是其重要的组成部分之一，它承担着提供给飞行器位置、速度、姿态等参数状态数据的任务。目前常用的导航方法有：惯性导航、卫星导航、视觉导航以及它们的组合导航等。由于单一导航系统难以满足无人飞行器的发展要求[3]，于是产生了把两种或两种以上的不同导航系统以适当的方式组合在一起的组合导航，比单独使用任一导航系统时具有更高的系统性能[4]。本文是利用扩展卡尔曼滤波器将惯性导航系统和GPS导航系统的组合来完成导航任务，建立了系统模型并将其在四旋翼无人飞行器上应用实现。

1 惯性导航系统

1.1 惯性传感器误差分析

影响导航系统工作精度的主要因素之一是惯性传感器误差的存在，为了减少传感器误差，需要先对传感器进行标定和误差补偿来保证导航系统的高精度和高

性能。

如果 εa（g）、Ma（g）以及与 Gg都可以估计出来，则经过补偿之后的比力和角速度为：

式中，符号^表示估计值。

1.2 惯性导航系统姿态矩阵计算

在导航系统的解算过程中，首先要处理的是坐标系的标定，通常有惯性坐标系（通常表示为i系）、地理坐标系（通常表示为g系）、导航坐标系（通常表示为n系）、载体坐标系（通常表示为b系）等。本文在分析导航系统问题时是将导航坐标系设定为地理坐标系，地理坐标系采用x指东向、y指北向、z指天向的形式，惯导系统的原理方程可以用图1表示。

图1 导航系统方程模块图

在导航系统中，最重要的计算就是姿态矩阵的求解，而常见的姿态矩阵算法主要有四元数法、欧拉角法、方向余弦法3种[5]。欧拉角法计算过程中方程式出现退化现象，方向余弦法通常计算量很大，因此经常采用的是四元数法求解姿态矩阵。但四元数法存在不可交换误差，为减小这种误差，本文采用的是 Bortz在 1971年提出的等效旋转矢量算法[6]。

1.2.1 旋转矢量与姿态四元数的关系

设Q（t+h）和Q（t）分别为飞行器载体在t+h时刻和t时刻的姿态四元数，则：

其中，⊗表示四元数乘法，h是时间间隔，q（h）是由旋转矢量Φ构造的四元数：

其中，Φ0=（ΦT·Φ）1/2。

1.2.2 等效旋转矢量的微分方程

假设旋转矢量表示为：Φ（t）=[ΦxΦyΦz]T，用旋转矢量来描述载体姿态运动时，等效旋转矢量微分方程可以写为：

其中，符号×表示叉乘，Φ为等效旋转矢量，它表征载体从t时刻到t+h时刻的等效旋转。

工程上，通常因为姿态更新周期很短，将Φ的高次项可略去不计，得到常用的近似方程：

1.2.3 旋转矢量的求解

通常，对于旋转矢量采用泰勒级数展开法来求解。若采用直线拟合角速度，等效旋转矢量的二子样算法为：

若采用抛物线拟合角速度，等效旋转矢量的三子样算法为：

2 组合导航系统

虽然惯性导航系统能够连续工作并有效地提供姿态信息、位置信息和速度信息，但由于惯性传感器误差的积累，惯性导航系统的工作精度会随时间下降。而GPS虽然可以提供长时间的误差为米级的高精度位置输出且用户设备成本较低，但由于GPS信号会被遮挡或干扰，因此不能仅依赖GPS提供连续导航参数[8]。

鉴于 INS和 GPS系统的优缺点互补，将二者组合在一起，综合两系统的优势，能提供有效、长时、高精度、完整的导航参数。组合导航的一般结构如图2所示。

2.1 卡尔曼滤波器

Kalman滤波是一种估计算法[9]，是导航系统中大多数状态估计算法的基础，如卫星导航结果的平滑、惯性

导航系统的对准和标定、惯性导航系统与卫星或其他导航传感器间的组合导航等[10]，并已经成为从导航系统各种测量数据中获得最优估计结果的关键技术。

图2 INS/GNSS组合导航一般结构

通常卡尔曼滤波算法可以描述为：若以K-1时刻的最优估计 XK-1为准，预测 K时刻的状态变量XˆK/K-1，同时又对该状态进行观测，得到观测变量ZK，在预测与观测之间进行分析（或者说以观测量对预测量进行修正），可以得到K时刻的最优状态估计XK。

式中，XK是K时刻的系统状态；ZK是K时刻的测量值；ΦK，K-1和 ΓK，K-1是 K-1时刻到 K时刻的状态转移矩阵；HK是测量系统的参数；WK和 VK分别表示过程和测量噪声，它们被假设成高斯白噪声，满足一定条件[9]。

实际工程问题中，多数系统都是非线性的，因此采用扩展卡尔曼滤波器（EKF）。扩展卡尔曼滤波（EKF）为卡尔曼滤波的非线性形式[11]。

EKF的系统动态模型和观测模型分别为：

函数f（·）和h（·）不能直接应用在协方差中，取而代之的是可以计算其雅可比矩阵[12]，这个过程实质上将非线性的函数在当前估计值处线性化了。

2.2 系统模型与状态选择

本文将惯导系统和GPS导航系统进行融合，在当地导航坐标系下建立系统模型。若卡尔曼滤波器估计的是相对于地球并投影到当地导航坐标系的姿态和速度误差，且估计的位置误差以纬度、经度和高度来表示，则状态向量变为：

式中，上标n表示投影到当地导航坐标系。

除了地球旋转角速率和陀螺测量值之外，姿态传播方程还引入了一个转移速率项，当地导航坐标系下的姿态误差为：

惯性导航系统的主要噪声源是由加速度计比力测量噪声导致的速度误差随机游走以及由陀螺角速度率测量噪声带来的姿态误差随机游走。如果单独估计加速度计和陀螺的动态零偏，则加速度计和陀螺零偏在运行时的变化可以近似为白噪声。

在 INS/GPS组合中，使用GPS用户设备的测量输出与根据惯性导航参数预测的测量值之间的差来更新状态向量，采用哪些测量则依赖于组合结构。

3 仿真与实验结果

本次实验选用 Pixhawk飞控单元作为飞行器主控板，MATLAB作为仿真软件，分别对纯惯导系统与组合导航系统进行仿真，四旋翼无人飞行器搭建环境描述如下：

选用材料为尼龙加纤维的机架搭建飞行器，机架的对角轴距为35 cm；选取无刷电机型号为MT2312-960KV用于多旋翼飞行器中，提供动力输出；选取电池容量为5 000 mA，最大放电电流为30 A。该飞行器遥控器型号为乐迪AT9，对应的接收器型号为2.4 G、9通道的R9D；地面站软件采用3DR推荐的专为PX4/PIXHAWK设计的新的 QGroundControl，在该环境下对飞行器进行校准和调试。

3.1 理想状态下的飞行轨迹

为了更好地理解导航系统的工作性能，在四旋翼无人飞行器飞行过程中，设定飞行顺序如图3所示。

图3 仿真轨迹的飞行顺序

具体过程可以描述为悬停时间为15 s；加速过程中，时间为 10 s，加速度大小为 0.5g（g为重力加速度）；爬升过程中，时间为25 s，仰角速度为 2°/s；俯冲过程中，时间为25 s，俯角速度为2°/s；减速过程中，时间为5 s，加速度大小为1g；飞行周期为140 s，循环飞行3次，则无人飞行器理想状态下的飞行姿态参数如图4所示。

3.2 实验结果

（1）纯惯导系统导航系统姿态误差曲线如图5所示。

图4 理想状态下飞行器的姿态角参数

图5 纯惯导系统模式下的姿态误差

（2）组合导航系统的姿态误差曲线如图6所示。

图6 组合导航系统模式下的姿态误差

由实验结果可知，理想飞行状态下，飞行器的横滚角和偏航角数值均为误差值（接近于0），只有俯仰角发生变化，如图4；若采用纯惯性导航，飞行器的姿态误差随着时间而增大，在飞行结束时，东向姿态误差（俯仰角误差）会达到 1.1°左右，北向姿态误差（横滚角误差）达到约1.6°，天向姿态误差（偏航角误差）达到约 1.5°，如图5；若采用INS/GPS组合导航系统，飞行器东向（俯仰）和北向（横滚）的姿态误差均小于0.5°，而且在长时间内可以小于0.2°，而天向（偏航）姿态角误差虽然会在短时间内达到1°左右，但在其他时间段内都能保持在0.5°左右，如图6。因此，在相同的条件下，组合导航系统的性能优于纯惯导系统，误差较小，能够为四旋翼无人飞行器提供较为精确的导航信息。

4 结论

本文主要是分析了导航系统的原理和算法，在对传感器误差进行标定和补偿之后，采用了扩展卡尔曼滤波器将捷联惯性导航系统与GPS导航系统融合，并成功应用于四旋翼无人飞行器上。仿真实验结果表明了系统方案在四旋翼无人飞行器上实现的可行性，且在长时间内可以提供较为准确的导航信息，误差较小，确保了无人飞行器的飞行效果。

[1]马远超.四旋翼飞行器导航及控制技术研究[D].哈尔滨：哈尔滨工程大学，2013.

[2]MILHIM A B，ZHANG Y M，RABBATH C A.Gain scheduling based PID controller for fault tolerant control of a quad-rotor UAV[C].AIAA Infotech@Aerospace，2010：1-13.

[3]SALYCHEV O S.Applied inertial navigation：problems and solutions[M].Moscow，Russia：BMSTU Press，2004.

[4]MERWE R V D，WAN E A.Sigma-point Kalman filters for integrated navigation[C].Proceedings of the 60th Annual Meeting of the Institute of Navigation（ION），2004：641-654.

[5]李晓峰.捷联惯导和组合导航的仿真研究[D].西安：西安电子科技大学，2010.

[6]BORTZ J E.A new mathematical formulation for strapdown inertial navigation[J].Aerospace and Electronic Systems，IEEE Transactions on，1971（1）：61-66.

[7]秦永元，张士邈.捷联惯导姿态更新的四子样旋转矢量优化算法研究[J].中国惯性技术学报，2001，9（4）：1-7.

[8]GROVES P D.Principles of GNSS，inertial，and multisensor integrated navigation systems[M].Artech House，2013.

[9]MOURIKIS A I，ROUMELIOTIS S I.A multi-state constraint Kalman filter for vision-aided inertial navigation[C]. Robotics and Automation，2007 IEEE International Conference on.IEEE，2007：3565-3572.

[10]HELAL A，MOORE S E，RAMACHANDRAN B.Drishti：An integrated navigation system for visually impaired and disabled[C].Wearable Computers，2001.Proceedings.Fifth International Symposium on.IEEE，2001：149-156.

[11]HAYKIN S S，HAYKIN S S.Kalman filtering and neural networks[M].Wiley Interscience，2001.

[12]JULIER S J，UHLMANN J K，DURRANT-WHYTE H F. A new approach for filtering nonlinear systems[C].American Control Conference，Proceedings of the 1995.IEEE，1995，3：1628-1632.

Integrated navigation system implemented on the quad-rotor UAV

Huang Jian1，Hu Yueli1，2，Yang Wenrong2，Ran Feng2
（1.College of Mechatronic Engineering and Automation，Shanghai University，Shanghai 200072，China；2.Microelectronic Research and Development Center，Shanghai University，Shanghai 200072，China）

An information fusion scheme based on the inertial navigation system（INS）/GPS integrated navigation system is presented according to the rapid development of unmanned aerial vehicle（UAV）industry and the importance of navigation system on UAV.The principle of strapdown inertial navigation and the algorithm of attitude matrix are carried out in this paper.Then，the extended Kalman filter of INS/GPS integrated navigation system is built up after calculating and compensating the sensor error.Simulation results show that the working performance of the integrated navigation system is better than that of the pure inertial navigation system and can provide high precision of navigation for UAV.Finally，the integrated navigation system is realized on the quad-rotor UAV.

unmanned aerial vehicle；inertial navigation system；integrated navigation；Kalman filter

V249.32

A

0258-7998（2015）05-0167-04

10.16157/j.issn.0258-7998.2015.05.042

2015-03-16）

黄建（1990-），男，硕士研究生，主要研究方向：嵌入式系统、机器视觉。

国家自然科学基金项目（61376028）

胡越黎（1959-），通信作者，男，博士，教授，博士生导师，主要研究方向：图像处理、机器视觉、片上系统、芯片设计，E-mail：huyueli@shu.edu.cn。