小型无人机多传感器组合导航系统设计与实现

周红坤

(昆明船舶设备研究试验中心 3室，云南 昆明 650051)



小型无人机多传感器组合导航系统设计与实现

周红坤

(昆明船舶设备研究试验中心 3室，云南 昆明 650051)

准确、可靠的运动状态数据对小型无人机运动控制至关重要，在实际飞行控制过程中通常要用到两种甚至更多传感器进行数据融合获取。文中根据小型无人机实际飞行运动控制需求，设计集成卫星导航(GPS)、惯性姿态参考系统(AHRS)和气压高度计的组合导航系统，采用多传感器数据融合算法，对无人机的位置、速度、加速度、姿态角等状态数据进行融合估计。通过实际飞行实验对所设计系统的有效性和实用性进行了验证。

无人飞行器；运动控制；数据融合；状态估计

随着现代导航技术的发展，GPS导航和惯性导航成为两大主流的导航系统。两者各有优缺点：GPS能直接提供无人机的位置信息，尤其经过RTK实时校正后的定位精度能够达到2 cm，但其输出频率较低，典型值为1～5 Hz，而无人机自主控制系统的周期一般均是50～100 Hz，这样就不能满足控制系统的要求，自主性差，在动态系统中可靠性不高。惯性导航恰好相反，其具有良好的动态性能，在短时间内数据准确；缺点是数据会随着时间漂移发散，长时间的稳定性较差。目前大部分的解决方案都是将IMU测量值与GPS测量值进行融合估计，采用的融合算法大多是基于Kalman的最优状态估计[1-4]。

采用这种方法确实可行，也能够取得良好的效果，但最大的弱点是需要得到过程模型的噪声协方差矩阵Q和测量噪声协方差矩阵R[5]，这在工程实践中很难获得。本文从工程实践出发，采用多传感器测量数据融合，利用各传感器器数据间的冗余量来弥补各传感器在特定数据测量的缺点，对小型无人机运动状态参数进行估计。

1　无人机运动参数及基本坐标系描述

1.1空间运动参数描述

无人机在空间中的运动可以等效为一个6自由度刚体运动，其飞行状态由三轴的位置(x,y,z)、线速度(u,v,w)、线加速度(ax,ay,az)、欧拉角(φ,θ,ψ)、角速度(p,q,r)、角加速度(rx,ry,rz)这6组运动参数给出[6]。

本文针对位置、速度、姿态和线加速度这4组飞行状态参数，运用卫星导航(GPS)、惯性姿态参考系统(AHRS)和数字式气压高度计所得数据进行融合以得到满足自主飞行控制的位置和速度状态。

1.2基本坐标系描述

为能方便量化上述各无人机运动参数，定义如下参考坐标系：

(1)地球空间坐标系—XYZ。以地心为坐标原点，X轴在指向0°经线的赤道平面内，Z轴指向北极，Y轴与X、Y轴构成右手直角坐标系。GPS给出的导航数据即为地球空间坐标系中的经纬高坐标(λ,μ,υ);

(2)地面坐标系—XYZ。以无人机的初始位置作为坐标原点，ZE轴垂直于过原点的切平面，XE,YE轴在切平面内，XE轴指向正东方向，YE轴指向正北方向，该坐标系为惯性坐标系，在飞行控制过程中，参考轨迹的位置和速度都是基于此坐标系给出的;

(3)机体坐标系—XYZ。以无人机的重心为坐标原点，XB轴在机体的纵向对称面内指向机头方向并与机头保持水平，ZB轴在纵向对称面内指向上，YB轴与XB,ZB构成右手坐标系。姿态参考系统中给出的线速度、线加速度、欧拉角角速度、角加速度都是基于此坐标系给出的，此坐标系随无人机一起移动，为非惯性坐标系[7]。

机体坐标系XBYBZB和地面坐标系XEYEZE之间转化最频繁，两者可以姿态角(φ,θ,ψ)为中介，通过式(1)进行转化[8]。

(1)

其中，a11=cosθcosψ;a12=sinφsinθcosψ-cosφsinψ;a13=cosφsinθcosψ+sinφsinψ;a21=cosθsinψ;a22=sinφsinθsinψ+cosφcosψ;a23=cosφsinθsinψ-sinφcosψ;a31=-sinθ;a32=sinφcosθ;a33=cosφcosθ。

变换矩阵RB→E是单位正交矩阵，空间坐标系向机体坐标系的变换矩阵为[9]

(2)

(3)

其中

(4)

地球空间坐标系到地面坐标系的转换则通过WGS-84地球模型，地球近似为标准球形，半径R=6 371 km。假设地面坐标系原点的坐标p0=(λ0,μ0,υ0)，则无人机当前GPS坐标p=(λ,μ,υ)在地面惯性坐标系中的位置通过下式计算

(5)

式(1)～式(5)给出了联系位置、速度、姿态及角速度这4组运动参数在各坐标系中的转化。

2　组合导航系统设计及实现

2.1系统总体设计

根据上述分析的无人机运动控制参数，本文采用卫星导航(GPS)、惯性姿态参考系统(AHRS)和数字式气压高度计多传感器组合导航的设计，如图1所示。组合导航微处理器MCU将AHRS、GPS和数字式气压高度计输出的数据进行处理和数据融合后，得到无人机的位置和速度信息。

图1　总体设计框图

AHRS系统由3轴的陀螺仪、加速度计和磁力计组成，通过Kalman算法对上述9轴数据进行融合，实现姿态参数(φ,θ,ψ)的准确估计，对3轴加速度数据进行积分可实现速度和位置状态的估计，但惯性积分在短时间内能保证数据的相对准确，随着时间的推移会产生严重的积偏，尤其在具有高频率振动的环境中，估计值会迅速发散。

为弥补惯性导航积偏的缺陷，在此通过MCU接收GPS模块和数字式气压高度计的数据对AHRS估计的位置和速度两个状态量进行定时修正。GPS在水平方向上具有良好的定位精度，但是在垂直方向上定位精度较差，一般为十几到几十米之间，不能满足小型无人机在垂直面内的定位精度。因此，本文中采用数字式气压高度计代替GPS对垂直方向上的位置和速度状态量的积偏进行修正。

2.2传感器技术参数

(1)GPS定位模块。Ublox-LEA-6H GPS接收机，具有2.5 m的CEP定位精度，5 Hz的数据更新率且支持UART数据接口；

(2)AHRS姿态参考系统。荷兰Xsense公司的MTI姿态参考系统，内部集成了3轴加速度计、3轴角速率陀螺、3轴磁力计和温度传感器，可得到准确的姿态参数估计值，分辨率为0.1°；

(3)数字式气压高度计。MS5803数字式气压高度计，内部集成了压力和温度传感器，带有24位的A/D转换器，压力分辨率为0.012 mbar(10 cm)，数字信号输出。

2.3系统硬件电路设计

系统主控MCU要实现GPS、AHRS和高度计的数据采集与解析，同时也要进行导航数据的融合，这就要求MCU具有强大的运算能力和丰富的外设接口。本文采用ST公司的STM32F4系列MCU芯片开发系统主控板，系统电路实物如图2所示。

图2　系统电路实物图

3个传感器的数据接口分别为：GPS--TTL、MTI姿态参考系统—RS232、气压高度计—SPI，数据的输出方式为固定帧格式的流模式输出。

2.4数据接口软件实现

在STM32F4系列MCU中集成有DMA单元，DMA 构建了一条直接连接外设与内存的通道，无需CPU 的参与就能实现外设接口与系统内存间的数据交换。这样CUP节省了大量资源进行导航数据的融合，大幅提高了系统的效率，增强了系统的稳定性。

本文中所有传感器的数据接收方式采用串口接收中断触发DMA 数据接收完成中断的形式进行，具体流程如图3所示。当接口缓冲区有数据时触发USART 接收中断，检测到数据帧后触发MDA 接收帧数据。

图3　UART/DMA数据接收流程

3　状态估计及实验数据分析

根据上述的传感器特性，在每一个GPS数据更新时刻采用互补滤波算法对速度和位置参数进行估计。GPS数据长时间准确，短期不稳定，通过低通滤波器滤掉高频的波动，而AHRS和气压高度计数据更新较快，在短时间内数据相对准确，但随着时间推移会产生积偏，通过高通滤波器滤掉长周期信号保留下短周期信号来抵消漂移。

互补滤波器一般形式为

y=(a)×(y)+(1-a)(x)

(6)

时间常数τ为

(7)

本文中小型无人机的控制频率为50 Hz，GPS数据更新频率为5 Hz，在此设计时间常数为0.06 s的互补滤波器，即取常数a为0.75。

为验证上述设计的小型无人机多传感器组合导航系统的有效性，将该系统运用于小型四旋翼飞行器飞行控制系统，手动遥控飞行器进行盘旋飞行，同时在高度方向上来回高低起伏飞行，对位置和速度状态进行数据融合。

3.1速度估计实验测试

图4所示为速度估计曲线图，虚线为通过GPS位置点直接微分得到的数据，由于GPS数据更新速率较低，从图中可看出曲线呈现明显的阶梯状。点画线为通过AHRS积分得到的速度曲线，曲线的大体趋势与GPS得到的数据曲线一致，但随着时间的推移积偏越来越严重。实线为经过数据融合后的曲线，既弥补了GPS数据更新率低的缺点，又能较好地修正加速度积分产生的偏差。

图4　速度估计数据曲线图

3.2位置估计实验测试

图5所示为位置估计数据曲线，与速度估计曲线一致，虚线为通过GPS直接微分得到的位置数据，从图中也可看出曲线呈现明显的阶梯状。

图5　位置估计数据曲线图

图中点画线为通过AHRS数据双积分得到的位置曲线，同样曲线的大体趋势与GPS得到的数据曲线一致，但随着时间的推移积偏越发严重。实线为经过数据融合后的曲线，同样经过数据融合后的位置曲线既弥补了GPS数据更新率低的缺点，又很好地修正了加速度积分产生的偏差，能较好的满足控制系统的需求。

4　结束语

本文从无人机的控制实际需求出发，针对小型无人无人机控制所需的飞行状态参数，选用GPS和姿态参考系统AHRS及气压高度计进行多传感器组合导航系统硬件设计与算法实现，得到满足控制需求状态参数，通过实际飞行实验进行验证。

[1]Gavrilets V,Shterenberg A,Dahleh M A, et al. Avionics system for a small unmanned helicopter performing aggres

sive maneuvers[C].Guangzhou: The 19th Digital Avionics Systems Conference, 2000.

[2]La Civita M, Papageorgiou G, Messner W C, et al. Integrated modeling and robust control for full-envelope flight of robotic helicopters[C].CA,USA: IEEE International Conference on Robotics and Automation, 2003.

[3]Dittrich J S,Johnson E N.Multi-sensor navigation system for an autonomous helicopter[C].HI,USA: The 21st Digital Avionics Systems Conference, 2002.

[4]Roberts J M,Corke P I,Buskey G. Low-cost flight control system for a small autonomous helicopter[C].Australia, Robotics:Australasian Conference on Robotics and Automation, 2002.

[5]Bishop G,Welch G.An introduction to the Kalman filter[J].University of North Carolina at Chapel Hill,1995(7):127-132.

[6]Raptis I A,Valavanis K P.Linear and nonlinear control of small-scale unmanned helicopters[M].Berlin: Springer, 2010.

[7]杨一栋.直升机飞行控制[M].2版.北京: 国防工业出版社,2011.

[8]曾丽兰,王道波,郭才根,等.无人驾驶直升机飞行控制技术综述[J].控制与决策,2006, 21(4):361-366.

[9]Pound P,Mahony R,Corke P.Modelling and control of a quad-rotor robot[C].Sydney,Australia: Australasian Conference on Robotics and Automation,2006.

[10] 岳基隆.四旋翼无人机自适应控制方法研究[D].长沙:国防科学技术大学,2010.

Design and Implementation of the Multi Sensor Integrated Navigation System for Mini-UAV

ZHOU Hongkun

(No.3 Research Section, Kunming Marine Equipment Test Research Center, Kunming 650051, China)

Accurate and reliable motion states data are essential for control of small unmanned aerial vehicle (UAV). In the actual control, two or even more of sensors are typically required to obtain the motion data by the specific fusion algorithm. In this paper, an integrated navigation system is designed by integrating GPS, inertial attitude reference system (AHRS) and barometric altimeter based on the actual flight control requirements of UAV. Then a multi-sensor data fusion algorithm is realized to estimate the states data of position, velocity, acceleration, and Euler angles. Finally, the validity and usefulness of the designed system are verified in actual flight tests.

unmanned aerial vehicle; motion control; data fusion; state estimation

10.16180/j.cnki.issn1007-7820.2016.08.004

2015-11-25

周红坤(1988-)，男，硕士研究生。研究方向：机电系统计算机测控与技术。

TN967.1

A

1007-7820(2016)08-010-04