多传感器融合在无人机室内三维定位中的应用

陈博 李擎

1.北京信息科技大学自动化学院智能控制研究所，北京 100192；2.北京信息科技大学高动态导航实验室，北京 100192

0 前言

近年来，人们对基于室内定位的无人机检测探查技术有很大的需求，因此，如何提高室内定位精度是一个热门话题。

最开始，研究人员提出了一些局部定位系统（Local Positioning System，LPS），例如：文献[1]将无线保真（Wireless Fidelity，WiFi）定位技术应用到复杂的室内环境中；文献[2]采用射频识别（Radio Frequency Identification，RFID）标签进行室内定位。尽管基于WiFi和RFID的LPS技术已经在室内环境中广泛使用，但是它们只能提供米级的定位精度。

为了获取更高的定位精度，研究人员把目光投向超宽带（Ultra Wide Band，UWB）技术。文献[3]阐述了超宽带室内定位技术的分析和最新进展；文献[4]分析比较了基于信号到达时间差的3种定位算法，最好的泰勒算法二维平面定位精度能到10 cm以内；文献[5]使用了粒子滤波和多路径补偿算法，将三维定位精度提高到了20 cm，但使用了16个基站；文献[6]使用了8个基站，在三维空间中精度仍然大于20 cm。

需要指出的是，单独使用超宽带的定位精度并不能满足室内高精度运行的要求。IMU是一种常用的机器人姿态估计传感器，具有精度高、更新频率快、体积小等优点，然而，IMU是通过积分来估计其位置的，存在累积误差，另外，长时间运行后，位置估计会发生漂移。文献[7]中，使用非线性滤波融合UWB和IMU，得到了厘米级的汽车二维位置估计；文献[8]中，UWB和IMU通过改进的EKF滤波算法融合得到了更准确的行人姿态估计，在周长40 m的封闭轨迹内实现小于4%的全局误差；文献[9]中，利用EKF和神经网络，将UWB和IMU融合后得到的非视距误差0.43 m，视距下为0.12 m；文献[10]中，利用粒子滤波融合IMU和UWB，得到精度10 cm的定位效果，其问题主要是粒子滤波计算量大，所需时间过长；文献[11]中，在组合定位系统中建立了INS / UWB紧耦合模型，实现0.24 m以内的定位精度。

常用的多传感器融合算法包括：扩展卡尔曼滤波（Extended Kalman Filtering，EKF）[12]、无迹卡尔曼滤波（Unscented Kalman Filtering，UKF）[13]、粒子滤波（Particle Filtering，PF）[14]等，以及文献[15]提出的单基站和脚安装的行人轨迹推算系统组合定位关键算法。超宽带定位系统模型和IMU模型都是非线性高斯模型。扩展卡尔曼滤波和无迹卡尔曼滤波虽然可以处理非线性高斯模型，但它们的估计精度不同。本文使用姿态角作为状态量，更加减少了滤波过程中的计算量。

因此，本文考虑了超宽带和IMU的信息融合概念设计。利用IMU对目标进行预测和超宽带定位系统抑制IMU积分产生的累积误差，以获得高精度的位置估计。此外，提出了一种基于无迹卡尔曼滤波的超宽带数据的设计方法来获取四旋翼飞行器的位置信息。实验结果表明，本文提出的融合设计方法在四旋翼无人机路径规划中有较高的定位精度，且计算速度快很多。

1 系统结构

本文所使用的室内无人机定位系统由UWB定位系统、上位PC机、树莓派和无人机组成，通信方式主要使用串口和网络通信，基本结构如图1所示。机载飞控Pixhawk与机载树莓派之间采用有线串口通信，Pixhawk将基本的姿态和传感器数据发送给树莓派，树莓派可以通过串口与UWB系统标签连接，获取无人机定位数据，完成控制计算后将电机控制指令发送给Pixhawk，实现四旋翼的基本控制。本文主要研究其中的控制、定位问题。

在只有4个定位基站的前提下，三维定位的精度（主要是高度上的精度）较差，且UWB本身定位效果受到非视距误差的影响非常大，因此，本文所用无人机上还安装了光流传感器和惯性测量单元IMU。系统控制流程框图如图2所示。首先，利用光流传感器输出的高度值修正UWB测得的高度值，得到较好的三维位置信息，在视距场景时，使用三维位姿信息修正IMU的积累误差，在非视距场景时，使用IMU惯性测量单元短时间内所得精确位姿信息弥补UWB测得的位置信息，通过多传感器数据的融合互补，得到任何场景下精确的三维位置信息，提供给无人机进行后续一系列任务。

2 无人机数学模型

具体建模方法可参考文献[16]，本章只简单介绍建模过程中使用的公式和实验系统的实际参数。

本实验中，无人机机体坐标系定义X为飞机中心水平向前，Y为垂直X轴水平向右，Z轴为垂直XY平面竖直向下。电机定义与旋转方向如图3所示，1号和2号电机逆时针旋转，3号和4号电机顺时针旋转。

四旋翼无人机的动力学系统模型为：

其中，——电机的转速；

σ——油门信号（0＜σ＜1）；

Tm、CR、——常参数（具体数值见表1）。

电机控制效率模型为：

其中，f——4个螺旋桨拉力的合力；

cT——拉力系数，为常参数（具体数值见表1）。

其中，τx、τy——X和Y轴的力矩；

τz——Z轴的反扭矩；

d——无人机中心到任一电机旋转轴的距离，为常参数；

cM——扭矩系数，为常参数（具体数值见表1）。

机体姿态与位置的运动和动力学模型为：

其中，ep——惯性系下的位置；

eυ——惯性系下的速度。

其中，qeb——四元数；

bω——机体角速度。

其中，bv——无人机在机体系下的速度；

bω——无人机在机体系下的角速度；

fb——无人机在机体系下的合力；

m——无人机质量，为常参数；

g——重力加速度，为常参数；

R——从惯性系到机体系的旋转矩阵；

e3——惯性系的z轴矢量[0 0 1]T；

f——4个电机的拉力；

Cd——无人机的阻力系数，为常参数（具体数值见表1）。

其中，J——无人机的转动惯量矩阵，为常参数；

Ga——陀螺力矩；

τ——螺旋桨在机体轴上的力矩；

JRP——螺旋桨的转动惯量，为常参数；

Ki——螺旋桨的旋转在机体系下的方向，与电机旋转方向和机体坐标系定义有关，在本系统中，K1=K2=-1，K3=K4=1。

其余参数数值见表1。

表1 常参数值

3 UWB定位模型

UWB定位系统本质上是一种通信过程，由基站向带有标签的目标物体发送信号，记录下信号飞行时间，根据ToF测距原理，测得基站到目标（标签）的距离，再根据空间中已知的基站坐标和测得的各个基站到目标的距离，通过最小二乘法计算出目标相对于定位系统的坐标信息，从而实现定位效果。

在实验室环境下建立如图4所示的UWB定位系统。其中，A0、A1、A2、A3位于同一水平高度的矩形平面上，且分别代表定位基站，T代表标签位置，标签放置在无人机上。d0、d1、d2、d3分别代表标签距各个基站的距离。

因此可以建立坐标系，并以A0为原点，为x轴，为y轴，过A0且垂直xoy平面为z轴，且向上为正方向。为方便与IMU数据融合，根据右手定则，将坐标系排列成东-北-天（E-N-U）。x轴正方向为东方向，y轴正方向为北方向，z轴正方向为天空方向。

设T的坐标为（x,y,z），A0坐标为（x0,y0,z0），A1坐标为（x1,y1,z1），A2坐标为（x2,y2,z2），A3坐标为(x3,y3,z3)，A0、A1、A2、A3坐标已知，则根据图4的定位模型可以得到：

将（8）展开可得：

用第一行分别减去后面三行可得：

其中：

此时，可以把问题转化为矩阵相乘，将式（10）写成矩阵形式，得：

根据矩阵除法，求得：

4 IMU姿态解算和误差分析

本文选取东-北-天（E-N-U）为导航坐标系，记为n系；X-Y-Z为载体坐标系，记为b系。

图5给出了导航系与载体系的关系。设OXYZ为导航坐标系，ObXbYbZb代表载体坐标系，其中Ob为载体质心。假设，IMU和UWB标签分别安装在载体上的P点和Q点，这里R表示IMU与飞机质心之间的距离，r为IMU与UWB标签之间的偏移量，因此，R被认为是一个不确定的杠杆臂，而r是一个确定的杠杆臂（一个偏移量）。

假设在点P处输出的比力写为：

其中，f pb——在点P处输出的比力；

——在飞机质心处输出的比力；

——机身框架相对于惯性框架的转角速率；

下标i——相对于惯性系的导数；

下标b——相对于身体架的导数。

从方程（14）可以得到：

其中：

则式（16）可以写为：

公式（17）右侧的第一项是切线加速度，右侧的第二项是向心加速度。根据公式（17），δfb向北东地坐标系变换如下：

其中，——从载体坐标系到导航坐标系的变换矩阵。

其中，j、θ、V——方位角、俯仰角和滚动角。

输出速度可表示为：

其中，f b——加速度计输出的比力；

ωie——地球自转角速度；

ωen——位置角速度；

g——地球重力加速度。

经度、纬度和高度的变化率分别为

其中，j、w、h——经度、纬度和高度；

vE、vN、vU——东-北-天（E-N-U）方向上的速度；

Rn、Rm——地球横向、纵向曲率半径。

惯性测量单元的误差模型为：

其中，fn——惯性导航系统承受的比力；

——地球自转角速度在导航坐标系的投影；

——系统坐标系相对地球坐标系的转动角速度在导航坐标系下的投影；

——陀螺仪输出的角速度误差。

上述误差模型忽略哥氏项误差和重力矢量误差，可以简化为：

其中，δvn——速度误差；

δpn——位置误差；

Ψ——姿态误差。

具体的IMU解算过程可参考文献[17]。

5 无迹卡尔曼滤波

与扩展卡尔曼滤波（EKF）不同，无迹卡尔曼滤波（UKF）并不对非线性方程f和h在估计点处做线性化逼近，而是利用无迹变换在估计点附近确定采样，用这些样本点表示高斯密度近似状态的概率密度函数。这种近似实质是一种统计近似而且求解，从而不会引入线性化带来的误差，滤波效果更好。因此，本文选用的融合方法为无迹卡尔曼滤波。

5.1 系统建模

设系统的状态方程和观测方程为：

其中，X(k) ——系统k时刻的状态量；

fk-1(X(k-1)) ——上一时刻系统状态到这一时刻系统状态的非线性关系；

w(k-1) ——k-1时刻的系统噪声，对应的协方差矩阵为Qk；

z(k) ——k时刻系统的观测量；

hk(X(k)) ——k时刻状态量被观测的非线性关系；

v(k) ——k时刻的观测噪声，对应的协方差矩阵为Rk。

本文选取的状态量为11维，如下公式所示，其中，ζP、ζv、ζΨ为位置误差、速度误差、姿态误差在导航坐标系 （E-N-U坐标系）上的三维分量（其中，姿态Ψ包括航向角α、姿态角β、滚转角λ）；ba为加速度计的偏置矢量；bg为陀螺仪的偏置矢量。

根据惯导原理，可得状态转换关系为：

对于观测模型，根据图4所示的UWB定位系统的几何关系可以很容易地得到：

其中，(xk,yk,zk) ——标签在k时刻所对应系统坐标系的坐标；

n0(k)、n1(k)、n2(k)、n3(k) ——标签到4个基站距离的测量噪声。

5.2 数据融合

无迹卡尔曼滤波算法如下：

第一步，初始化状态量。对于位置状态量ζP，在静止状态下，取100个UWB输出的标签到基站的距离，取均值算出坐标后作为ζP的初值；静止状态时，ζv为0；静止状态取100组加速度计数据，根据公式计算出航向角α、姿态角β、滚转角λ作为状态量初值：

其中，Ax、Ay、Az——加速度计的x轴、y轴、z轴的加速度值；

Hx、Hy、Hz——磁力计3个轴的测量值。

初始协方差矩阵可以设为单位阵。

第二步，计算σ点的。σ为采样点。依据xk-1｜k-1和pk-1｜k-1生成 2n+1 个σ点，i=0,1,…,2n，n为状态量的维数，本文n为11。无迹变换时，取尺度参数α=0.01，κ=0，β=2。根据公式计算采样点的权重：

其中，λ=α2(n+κ)-n。

第三步，根据公式计算σ点的：

第四步，根据公式计算σ点和Pk｜k-1通过量测方程对xk的传播：

第五步，根据公式计算输出的一步提前预测：

第六步，获得新的量测值后，进行滤波更新：

更新完成后的状态量与协方差矩阵可以直接带入到下一次的迭代过程中，完成无迹卡尔曼滤波过程。

6 实验与分析

6.1 环境搭建

为验证本文所提出的室内三维定位方法在实际中的定位效果与精确性，设计了实验进行验证，实验地点为学校内高动态导航实验室（5 m×4 m×3 m），如图6所示。实验所选设备如图7所示。

其中，UWB定位系统采用Nooploop公司研发的LinkTrack系列产品，基站选用的是LinkTrack S型号，标签选用的是LinkTrack SS型号，厂家手册所标二维精度为20 cm，三维精度在30 cm左右。4个基站固定好后，分别用USB连接到一起，接入PC端即可。无人机选用的是北京灵思创奇科技有限公司的成品无人机，飞控使用的是Pixhawk和树莓派，修改了部分控制器程序，改用内外环控制方法，使其能在实验室场地内平稳飞行。光流传感器选用的是匿名科技下的产品，可输出根据摄像头计算后的高度信息。还有机载飞控中的IMU惯性测量单元，可输出无人机的位置、姿态信息。

6.2 实验设计

实验方案设计：对无人机设定倾斜向上的封闭圆形轨迹，并让无人机沿着设定轨迹飞行3圈再落地。设计了2组实验，分别采用纯UWB定位和普通滤波函数处理后得到的飞行路线，以及采用EKF融合多传感器得到飞行路线，用不同定位方式比较定位效果。

第一组：利用纯UWB进行定位飞行，结果如图8所示。图中黑色轨迹为设定路线，蓝色轨迹为UWB测定的路径，红色轨迹为将UWB测得数据进行普通卡尔曼滤波平滑处理后的轨迹。

从图8中可以明显看出，纯UWB定位的误差较大，且波动较大，不稳定，经过普通滤波处理后，轨迹虽然平滑了许多，但还是与设定轨迹有一定程度上的偏差，这主要是由于UWB信号在角落处发生非视距传播导致的。图9为纯UWB在x、y平面定位效果图。图10为纯UWB在x、y、z轴上的定位效果图。可以看出，在UWB单独定位时，误差在25 cm左右，与商家提供的产品手册中精度一致。

第二组：利用无迹卡尔曼滤波融合UWB和IMU的测量值，在无遮挡、信号好时，利用UWB的精确位置修正IMU由于时间长带来的累积误差，在角落UWB信号不佳时，用IMU的短期准确导航信息来弥补UWB由于非视距带来的误差，并结合光流传感器进行高度修正，结果如图11所示，蓝色为设定路线，红色为实际路线。

可以看出，定位的精度和准确性有明显提高，具有更高的可靠性。图12为UKF后在x、y轴上的定位图。图13为UKF数据融合后在x、y、z轴上的定位效果图。可以看出，在与IMU融合定位后，在角落原本UWB测得数据偏差大的位置处，已经可以达到与设定位置十分精确的位置，总体精度达到了10 cm，导航误差为0.7%。在该定位方法下，室内定位精度得到了显著提升，且具有较强的鲁棒性。

7 结束语

通过实物仿真实验验证，本文提出的基于无迹卡尔曼滤波的多传感器数据融合内外环控制室内定位方法具有较好的定位精度，能使无人机系统快速达到稳定且拥有较强的鲁棒性，定位精度从原先的25 cm提高到了10 cm以内，导航误差为总路程的0.7%，相比纯UWB定位误差12.4%，误差减小了11.7%；相比UWB和普通卡尔曼滤波数据平滑处理定位误差7.1%，误差减小了6.4%。该方法具有良好的导航精度，极大改善了原有设备的定位精度，使其能够满足四旋翼无人机在室内进行相关任务时的定位需求。