基于自适应卡尔曼滤波的联合RSS/TOA/INS无人机定位算法

胡 敬，田俊曦，邹世明，晁 涛，杨 明

（哈尔滨工业大学航天学院，哈尔滨 150001）

1 引 言

过去20年里，随着微机电系统和导航技术的发展，无人机技术得到了迅速的发展。由于无人机集群之间的协同具有低成本、隐蔽性强等特点，被认为是未来战争中应用最广泛的方向[1]。无人机通过其自身搭载的惯性设备和全球导航卫星系统（Global Navigation Satellite System, GNSS）接收机，可以在开阔地带精确完成定位，从而完成电力巡检、无人机航拍、农业自动化等任务[2]。然而，在山地、峡谷、隧道或者未来战场的复杂环境下，无人机极有可能面临GNSS 信号拒止的问题，在没有引入外部信息源解决惯导设备由零偏不稳定性带来的位置发散问题时，依靠低成本的惯导设备进行位置解算会导致严重的误差发散问题。因此，研究GNSS 信号拒止环境下的导航问题极具意义[3]。

惯性导航系统（Inertial Navigation System,INS）是应用最广泛也是最基本的导航定位方法，GNSS/INS 组合导航被广泛应用于各类无人智能系统中[4]。然而在GNSS 拒止环境缺少有效观测输入的情况下，单纯的惯导设备由于其零偏、噪声等因素，积分求解位置将累积误差，从而导致定位解算的发散。计算机视觉以及激光雷达可以为拒止环境下的无人机提供局部位置以及速度信息，这也是当前的主流做法[5]。然而两者同样面临很多亟待解决的问题，比如视觉同步定位与地图创建（Simultaneous Localization And Mapping,SLAM）可能会遇到特征点稀疏或障碍物等问题[6]。对于激光雷达SLAM，在某些外部干扰（如降雨和平滑反射表面）下定位也存在鲁棒性问题[7]。

基于无线电的定位可以有效避免以上问题。因此，作为一种视觉SLAM 和激光雷达SLAM 定位的冗余方法，无线电定位技术同样具有极高的应用价值。无线电定位通常使用基于接收信号强度（Received Signal Strength, RSS）的测距设备，如Wi-Fi 和Zigbee；基于到达时间差（Time of Arrival, TOA）的设备，如超宽带（Ultra Wide Band,UWB）；基于TOA 的定位，考虑到成本和无人机负载问题，本文主要关注基于UWB 的TOA 测距和基于Zigbee 设备的RSS 测距。基于UWB 的TOA 测距具有传输速率高、精度高、成本低、功耗低等特点，在不安装功率放大器时测距范围只有20～50m[5]；高功率UWB 设备虽然可以增加测距范围，但会带来成本、功耗和安全性问题[7]。Xu 等[8]提出了在非视距环境中利用到达时间差和到达角度差以及惯导信息进行定位，显著降低了成本。Li 等[9]利用UWB 信息和惯导信息，提出了一种基于扩展卡尔曼滤波（EKF）的融合算法，充分考虑了UWB 高延迟和低带宽的问题。上述两种方法都提高了位置估计精度，但是忽略了低成本测距设备范围约束条件下UWB 测距短的问题。Zigbee 最初应用于工业物联网，可用于无人机集群之间的通信。同时，它也可以作为一种无成本的补充测距方法。在UWB 信号衰弱的远距离测距环境中，UWB 的测距精度甚至不如RSS[10]。Ahmed 等[11]介绍了无人机空对空和空对地环境下Zigbee 通信信道的特点，同时并概述了该领域未来的研究方向。Liu 等[12]利用惯导元件、进程UWB 和远程RSS 信息，通过粒子滤波实现精确且鲁棒的定位系统，但同时也带来了功率限制与计算成本问题。

为了实现无人机集群在GNSS 拒止环境下的精确定位问题，本文首先引入惯导的动力学方程作为滤波的状态方程，基于TOA 测距和RSS 测距的优缺点，通过最小二乘方法解算得到局部坐标系下的无人机位置，并将其作为量测值输入序贯扩展卡尔曼滤波算法与惯导进行融合，最终得到对位置的估计值。同时，考虑到在近程TOA/RSS 信号冗余，以及在长程下TOA 测距失效，RSS 测距易受到环境干扰等问题，提出了自适应因子进一步改进滤波算法。最后，通过户外飞行实验，验证了算法的有效性。

2 模型建立

在城市建筑物遮挡、隧道、峡谷或者是未来战场的极端环境下，GNSS 信号会出现拒止的情况。此时，无人机通过结合自身测距信息并进行适当的路径规划，一部分无人机可以到达开阔的地带，可以降低GNSS 信号拒止对集群的影响,无人机可以通过集群之间的信息来恢复自身的位置估计。本文着重关注对无人机传感器的状态实现精确且具有鲁棒性的估计。

假设在GNSS 拒止的环境下，无人机集群中只有少数无人机可以自主规划路径搜索GNSS 信息，并充当“伪卫星”。同时，大多数无人机只能获得自身惯导估计信息和测距信息来实现位置估计。无人机集群定位示意图如图1所示。假定无人机集群中M架无人机UAVm，m∈ {1,… ,M}可以自主搜索GNSS 信息，它们的局部坐标信息可直接获取，记坐标pm=(x m,ym)∈R2，则可以将其视为定位系统中的锚节点；同时，以集群中一架不能自主搜寻GNSS 信息的无人机UAV0为例，记其真实位置估计为p0=(x0,y0)∈R2，而位置估计值为

图1 GNSS 失锁环境下定位场景Fig.1 Localization under GNSS loss-of-lock environment

2.1 惯导模型

惯性导航是应用最广泛的定位技术，但是在没有GNSS 作为辅助导航的情况下，由于惯导单元（Inertial Measurement Unit, IMU）的不稳定性及噪声等特点，其解算出来的速度、位置误差会随着时间积累而发散。惯导器件中加速度计可测量无人机的加速度信息，陀螺仪可输出角速度信息，磁力计可感知地磁强度。通过对系统动力学模型泰勒展开并忽略高阶项，对其离散化可以得到如式(1)、(5)所示状态方程。其中，无人机的姿态四元数、速度、位置状态更新方程如式(1)所示，加速度计偏置、陀螺仪偏置、磁力计偏置的更新如式(5)所示。

其中，bθ表示陀螺仪偏置项，θn表示三轴测量误差，服从均值为0，方差为的高斯分布。速度v为导航坐标系下的矢量，其单位为m/s。Cbn为机体坐标系到导航坐标系下的旋转矩阵。gN=[0 0 1]T为重力加速度在导航坐标系下投影，其单位为重力加速度g。记加速度计输出为，而其真实值为与陀螺仪表示类似，加速度真值可以表示为：

同理，a b为陀螺仪零偏，a n为陀螺仪三轴测量噪声，服从均值为0，方差为的高斯分布。在位置解算方程中，位置p的迭代可由速度迭代求解。

陀螺仪、加速度计、磁力计的零偏是非静态的，对于传感器的研究一般将其视为一阶马尔可夫过程，在姿态四元数、速度、位置更新时一并考虑，本文为了简化模型，参考PX4 源码，有：

2.2 TOA/RSS 测距模型

无人机所搭载的UWB 模块基于超带宽载波的到达时间差来计算距离，假设集群中每一架无人机所搭载的UWB 模块的时间戳是同步的，则集群中完全处于GNSS 失锁状态的无人机UA0V与那些能自主搜寻GNSS 信息从而位置状态已知的无人机UAmV之间的距离可写成如下形式：

其中，εt,m0为UWB 模块测距误差，包括测量噪声以及时间戳的误差，同样假定其为均值为0，方差为σt2的高斯过程。

无人机所搭载的Zigbee 模块在负责通信的同时也能够依据功率衰减原理估算距离，则无人机UAV0和无人机UAVm接收功率可建模为：

其中，RSSref为单位距离下的功率衰减，其单位为dBm。nr为路径损耗指数，通常与环境有关。同样，εr,m0为RSS 测距的误差，假定其也服从均值为0，方差为σr2的正态分布。

通过实验采集大量数据，可得RSS 功率衰减与距离关系如图2所示，同时可拟合出式(7)中参数：

图2 RSS 功率衰减拟合曲线Fig.2 RSS attenuation model

3 自适应扩展卡尔曼滤波算法

本文虚拟观测量为局部坐标系下各无人机的坐标，而坐标的解算则依靠上节所介绍的测距模块输出的距离，依据最小二乘法解算而来。当无人机同时收到TOA 和RSS 信息时，序贯卡尔曼滤波可以在短距离范围内估计出较为精确的位置。同时，在中远距离进行状态估计时同样具有较高精度和鲁棒性。与仅用UWB/IMU 或Zigbee/IMU 的滤波算法相比，范围进一步拓展。此外，由于基于Zigbee 的RSS 测距精度远不如TOA，因此在滤波过程中加入自适应因子以动态调整量测噪声方差阵。

3.1 状态量更新

由式(1)(5)可知，状态量可写成：

状态量依次为姿态四元数、速度、位置、陀螺仪偏置、加速度计偏置以及磁力计偏置。则离散状态的预测更新过程如下：

过程噪声方差阵如下：

其中，P-(k)为状态预测的协方差矩阵，Q(k)=为过程噪声协方差矩阵。Φ(k)为状态转移矩阵，其子块部分通过偏导数求出[13]。

3.2 观测量更新

序贯卡尔曼滤波本质上为一种解耦卡尔曼滤波，要求每个量测值不相关。本文中，UWB 和Zigbee 的测距值同样是独立的。同时，利用M个锚节点依据最小二乘法对定位点进行位置解算，公式如下：

在有了位置的虚拟观测量后，可以通过相邻两点的位置状态和时间间隔得到速度的观测量，则可得到量测量：

量测矩阵可以写成如下形式：

自适应扩展卡尔曼滤波的量测更新如下：

由于TOA 和RSS 的数据频率不同，解算出来的虚拟观测量会不同时地进入滤波器中，为此，本文采用自适应序贯扩展卡尔曼滤波算法，则公式可重写如下：

其中，为卡尔曼滤波增益，α k为自适应因子，可以写成如下形式：

这里，c为阈值，依靠经验通常设置在1.0～2.0[14]，Vk=Z k-Xk-+1为量测和状态预测的差，由此，式(9)～(20)给出了自适应序贯卡尔曼滤波算法。

4 实验结果及分析

本实验中，测距模块通过自行设计的PCB 板与飞控系统进行通信，TOA 和RSS 传输频率分别为5Hz、2Hz。实验用无人机整机图如图3所示。

图3 实验所用无人机Fig.3 UAV used in the experiment

首先验证TOA 和RSS 两种测距方式的特性，综合比较二者丢包率，实验结果如图4所示。

图4 两种测距方式丢包率对比Fig.4 Packet Loss Rate

可以明显看出，RSS 不存在明显的丢包现象。受发射功率以及周围环境的影响，UWB 在超出一定范围后由于功率限制逐渐无法接收到测距信号。本文中经过实测，对于低成本UWB 而言，大约在40～50 m 之间，同时受到客观条件差异影响，每个设备极限测距距离可能并不一致。同时，根据图4中红圈可以看到，在到达极限测距距离后，在较短范围内，受电能波动影响，同样可能存在断续的接收信号，该现象并不长久存在。增加大功率的UWB 可以达到更远的距离，但是本文为了方便实验，仅使用了低成本低功耗的设备来验证TOA/RSS 联合定位的算法。

以飞行任务起点位置为原点，建立东北天导航坐标系，假定有4 架无人机自主规划路径到达特定位置充当“伪卫星”，实验中，将集成UWB和Zigbee 设备的电路板放在体育场内，坐标依次为(-7,-23)，(9,-19)，(-22,37)，(-2,42)，高度均为2 m，以模拟这些“伪卫星”。同时，一架无人机以0.5 m/s 的速度在2 m 的高度上飞行，通过采集UWB、Zigbee 解算出的位置信息作为状态观测输入EKF 以及ASEKF 与INS 进行融合，从而获得对位置的估计。作为参考，以实际飞行中PX4内部的EKF 融合INS/GNSS，得到对参考位置的估计，可绘制不同信息源得出的飞行轨迹。

在图5中，绿色曲线代表PX4 内部提供的GNSS/INS 融合定位点，可作为参考轨迹。红色曲线为TOA 解算的位置坐标，当距离增大时，可以看到局部放大图中红色的TOA 解算点不再密集，说明UWB 出现丢包现象以致无法进行定位，此时下方两个锚点(-7,-23)(9,-19)与当前解算点均约为50 m。为了验证RSS/TOA 无论在短程还是长程定位下均可作为观测信息源，为惯导提供位置信息，本实验设置上方两个锚点(-22,37)(-2,42)与初始解算点距离均约为42 m，使得无人机在飞行尾程可以接收 RSS 信息而无法接收UWB 信息。同时，由RSS 信息解算出的位置如蓝色散点所示，分布在参考轨线周围。通过图6，可以更清晰地观察观测值分布。

图5 不同信息源飞行轨线解算Fig.5 Positioning results of GNSS,TOA and RSS

图6 飞行速度0.5 m/s 时TOA/RSS 定位结果Fig.6 Positioning results of TOA/RSS at speed of 0.5 m/s

可以看到，对于UWB 定位而言，定位精度较高，随着无人机的飞行移动定位结果具有很强的趋势，RSS 定位越分散且定位精度相较于UWB定位波动更大。可以看见，在0～50 s 内，UWB定位值较为平均且并未出现丢包的状况，RSS 定位值波动较大，但是始终保持一定的接受频率。在50～60 s 内，根据趋势，可以看出UWB 定位值仍存在部分值，但相较于50 s 之前，其观测数量已大为减少。最后，在60 s 之后，基本已经看不见UWB 定位值，此时UWB 设备受到功率限制，已经达到测距极限。

将前文解算出的位置坐标作为观测量，自适应因子c设置为1.2，分别进行扩展卡尔曼和序贯卡尔曼滤波得到最终的位置估计，结合参考轨迹，与单独用惯导解算对比，可得如下结论。

惯导系统（INS）、GNSS/INS 基于EKF 提供的参考轨迹（REF-EKF）、基于无线电定位的TOA/RSS/INS 联合扩展卡尔曼滤波（Radio-EKF）、基于无线电定位的TOA/RSS/INS 联合序贯扩展卡尔曼滤波算法（Radio-ASEKF）解算定位结果如图6～7 所示，根据图5可知无人机以0.5 m/s 速度飞行约35 m，行程共计约70 s，飞行期间在X轴方向位移约11 m，Y轴方向约34 m。显然，根据图7～8，仅用惯导系统进行定位估算很快就会发散。在短距离范围内，Radio-EKF 以及Radio-ASEKF 均可提供较为准确的位置估计值，同时Radio-ASEKF 的估计信息更为贴近参考轨迹。随着距离的增大，在60～70 s 内，UWB 模块丢包明显，可用信息只有RSS 信息，其测距方差较大，导致最后10s 内剧烈波动，在自适应因子的调节下，Radio-ASEKF 比Radio-EKF 取得了更好的结果。此外，ASEKF 算法在前60 s 内比EKF算法更接近参考轨迹，且对于后期的估算相对平滑，这也验证了算法的有效性。

图7 x 方向不同算法解算结果Fig.7 Positioning results of x component at speed of 0.5 m/s

同样的，y方向解算结果如8 所示。

图8 y 方向不同算法解算结果Fig.8 Positioning results of y component at speed of 0.5 m/s

在得到上述研究结果后，为了进一步探讨其他因素对本文所提算法的影响，本文在同一布置场景下，将无人机速度提高至1 m/s，TOA 和RSS传输频率分别为5 Hz、2 Hz，自适应因子c设置为1.2。首先本文给出观测值分布如图9所示。

图9 飞行速度1m/s 时TOA/RSS 定位结果Fig.9 Positioning results of TOA/RSS at speed of 1m/s

可以看到，在更高速度飞行时，定位点更少。将定位结果作为观测输入EKF 以及ASEKF 进行对比，可以得到图10～11。

图10 x 方向1 m/s 飞行速度下不同算法解算结果Fig.10 Positioning results of x component at speed of 1m/s

根据图10～11 可知，ASEKF 均取得了比EKF更贴近参考轨迹的结果。同样，纯惯性定位由于受到大量干扰，很快发散，以基于 EKF 的GNSS/INS 组合导航作为参考轨迹，对比EKF 及ASEKF，在图10 中可以看出，ASEKF 取得了更贴近参考轨迹的定位结果，面对25 s 后，TOA 定位结果断续或失效的情况，对于RSS 定位波动大的结果也有一定的自适应效果，在图11 中，前5 s内ASEKF 快速收敛，对于25 s 后RSS 定位波动通用有一定的自适应效果。将图10 与图7对比，图11 与图8对比，可以明显看出图10 中的定位波动更大，图11 中部分时刻波动范围比图8中波动范围更大。这说明飞行速度对最终定位结果影响较大，进一步对比图9以及图6中观测值数量，可知无人机在固定位置下以较大速度飞行时，TOA/RSS 定位的频率将降低，同时根据定位设备的用户手册可知，RSS 测距最高频率约15 Hz，而TOA 定位频率约100 Hz，两者测距频率同时受到传输数据大小影响，与之成反比，即传输数据越多，测距频率越低。同时，低速飞行下高频率测距得到较多的冗余数据可以有效剔除离群点。因此，以TOA/RSS 定位作为观测输入滤波时，飞行速度将会影响观测值数量以及精度，进一步影响滤波结果。

图11 y 方向1 m/s 飞行速度下不同算法解算结果Fig.11 Positioning results of y component at speed of 1m/s

5 结 论

本文针对GNSS 信息拒止环境下定位问题，提出了一种基于TOA/RSS 联合的自适应序贯卡尔曼滤波算法，通过引入自适应因子，结合TOA精度高、测距范围短以及RSS 精度低、测距范围广的特点，扩展了TOA/RSS 联合测距范围，并提高了位置估计的平滑性。最终的实验结果表明，这种自适应序贯卡尔曼滤波算法相对于传统扩展卡尔曼滤波，有效提高了定位精度，对波动更具鲁棒性。