运动参数辅助的无人机单站机会信号定位方法

卞志昂，卢 虎，史浩东

(空军工程大学 信息与导航学院，陕西 西安 710082)

随着无人机在军民领域的应用日益广泛，无人机导航定位和姿态估计严重依赖于卫星导航定位系统(Global Navigation Satellite System，GNSS)的缺陷日益突出。所以，人们尝试扩展导航信息源，试图利用射频机会信号进行导航定位[1-6]。当前，室内多采用超宽带，WiFi、Bluetooth当成机会射频信号；在室外，常采用4G/5G、LEO、FM、电视信号等作为机会射频信号。针对不同的信号类型、应用场景和基站间的时钟是否同步，可选择使用TOA、TDOA、AOA、RSSI等方法进行目标的定位。

上述这些机会信号定位的手段和方法均建立在多基站联合的基础之上[7-9]，虽然广泛分布的多基站机会信号能在一定限度内提高定位精度，但增加了用户终端的复杂度，导致定位的实时性变差。因此，研究利用单站机会射频信号的定位技术日渐获得关注[10]。显然，仅靠单个距离量测信息不能实现目标定位。文献[11]融合高精度的惯性测量单元(Inertial Measurement Unit，IMU)和多普勒传感器解决该问题；文献[12]中提出一种依赖距离信息和目标仰角测量信息的单站定位算法，在基站侧对目标进行极坐标定位；文献[13]中提出一种基于相位差变化率的单站定位算法，虽然提高了定位精度，但仍需要高灵敏度的相位测量。现有的方法并未摆脱“伪单站”定位的传感器冗余问题。

鉴于此，通过充分挖掘无人机动力学参量所隐含的载体运动状态中的位置信息，笔者提出一种仅利用无人机运动参数辅助的单站机会射频信号无人机导航定位新方法，可极大地降低现有的多站机会信号定位系统的复杂度价格成本。

1 无人机动力学模型描述

1.1 无人机运动状态描述及其可观性分析

研究对象为固定翼无人机，为简明起见仅考虑二维运动。设无人机运动状态向量为[x，y，θ，v，w]T，其中x，y为无人机位置坐标，θ为航向角，v、w分别为航路线速度和角速度。用a、b表示无人机运动的线加速度和角加速度，则描述无人机运动的状态向量表示如下：

(1)

又知无人机位置量测输出方程可表示为

(2)

其中，x0，y0为机会射频信号基站坐标。L1是无人机与射频基站距离的测量值，可由无人机机载机会信号传感器量测获得；L2是无人机航向测量值，可由无人机上飞控中的微机电陀螺获得。

考虑无人机运动的角速度和线速度v、w，设机会信号基站坐标x0，y0为原点，线性化式(2)中量测距离(x2+y2)1/2为0.5(x2+y2)，则无人机位置量测输出方程(2)可扩充为

(3)

根据飞行器非线性运动方程的可观测性理论[14]，得到无人机运动状态方程：

(4)

其中，无人机运动状态X=[x，y，θ，v，w]，控制量u=[a，b]，则无人机状态空间向量场可表示为

f0(X)=[vcosθ，vsinθ，ω，0，0]T，f1(X)=[0，0，0，1，0]T，f2(X)=[0，0，0，0，1]T。

(5)

(6)

由文献[15]得到无人机系统运动状态方程的可观性判定矩阵G为

(7)

显然，无人机速度参数估计的准确与否是无人机位置可观测的充分条件，也是所提运动参数辅助无人机位置估计的核心所在，为此有必要深入讨论无人机速度参数准确估计的方法。

1.2 无人机运动速度估计方法及误差抑制

如图1所示，设无人机以速度v通过采样点t1、t2、t3，与射频基站的距离量测值分别为d1、d2、d3，r为基站到运动航迹的垂直距离，x是第1个采样点与基站到无人机航迹垂足的距离，则无人机速度的估计值v满足

图1 速度估计原理图

(8)

其中，t2-t1、t3-t1分别为采样时间T和二倍采样时间2T。依据式(8)可解出速度表达式v：

(9)

根据误差传播定律[16]，可知无人机速度值v的传递估计误差为

(10)

其中，δ为量测噪声的标准差。由式(10)可知，当无人机速度估计时间间隔T固定且很小时，d1≈d2≈d3，分母趋近于零，误差变大。另外，误差与d2正相关，在估计时间间隔不变的情况下，无人机距离基站越远，速度估计误差就越大。

通过以下方法缓解速度估计误差的影响。针对第1个问题，设置与无人机运动性能相关的阈值，根据距离变化自适应地更新估计时间间隔T。当距离测量差超过给定的阈值时，触发估计器更新速度估计值。具体如下：按照系统原始固定的采样频率对距离进行采样，获得a1，a2，a3，…，al，…，ar，其中al表示按照固定采样频率收集到的第l个距离数据，l∈[1，r]，r是大于1的整数。当al-a1>TH且ar-al>TH时，a1=d1，al=d2，ar=d3，其中TH表示根据无人机性能设定的阈值。当速度大时，阈值设置较大；反之，阈值设置较小。由d1、d2、d3和时间间隔，根据式(9)获得速度估计值。这种方法避免了d1≈d2≈d3的情况，相当于采取自适应的采样频率进行距离量测值的采样，此时速度估计值有效地抑制了由第1个问题带来的误差。

针对第2个问题，由于速度估计误差随着里程的增加而增加，因此将里程与比率阈值相乘，得到自适应窗口大小。按窗口大小获取速度估计值，随后根据窗口内的一组速度估计值，对其取平均值，得到当前速度的准确估计。在后续的仿真实验中，具体如下：将一组速度估计值输入一个大小为O的窗口，也就是要求窗口内元素个数小于等于O，O是大于1的整数。设置窗口大小的变换规则，O=d3×γ，其中γ为设定的比率。窗口大小是受当前时刻的采样距离d3约束的，认为自适应窗口里所有速度估计值的平均值为当前时刻的速度值。随着目标距射频信号站的距离增加，自适应窗口就会变大，而当前时刻的速度估计值就会趋于平滑，有效地减小了第2个问题带来的误差。文中算法在目标精度下的有效距离依赖于比率阈值的设定，即在大速度机动和小速度机动状态下，比率阈值设置不同，定位效果也就不同。在后文的仿真环境中，在1 000 m的里程内，定位精度在2 m左右；而在实际验证中，由于旋翼无人机的局限性导致定位精度降低，但仍能在数百米内保持2 m左右的定位精度。特别注意的是，在实际中固定翼无人机应该定位精度更好。

需要指出的是，虽然有研究表明，采用深度学习等方法可以更精确地预测载体运动状态[17-18]，但考虑到无人机平台嵌入式系统的算力和复杂度约束，无人机常用扩展卡尔曼滤波(Extended Kalman Filter，EKF)进行速度、位置和姿态参数的估计与滤波处理。然而要实现多运动状态下的定位跟踪，交互式多模型更加有效。因此，笔者主要采用交互式多模型[19]方法来实现无人机运动参数的近似精确估计，并验证了所提方法与运动参数估计模型无关的泛化性能，即文中所述速度辅助的定位算法能迁移至任何现有滤波模型中。为典型起见，仅以交互式多模型速度修正为例进行仿真和实验测试。对于常用的EKF、UKF等滤波器，文献[20]表明，算法复杂度与状态维数和量测维数呈三次方关系，即在O(n3) 和O(l3)量级，中文算法的复杂度依托于交互式滤波模型的复杂度。对于工程实现而言，卡尔曼滤波在捷联惯导组合导航系统中已应用成熟，证明了文中算法在工程中实现的可行性。

2 运动状态参数辅助的无人机单站定位方法

文中的算法思想适用于任意定位估计滤波算法，现仅以交互式多模型为例进行算法介绍。交互式多模型算法基于贝叶斯理论对多状态模型自动识别和切换，算法以递推方式进行，分为输入模型交互、滤波器的滤波、概率模型的更新和新的数据融合4个步骤。

假设无人机在飞行过程中的运动状态有n种，对应的运动模型设为n。设模型i到模型j的混合概率马尔可夫转移矩阵为

(11)

则n种模型对应的第j个模型的运动状态和量测方程为

(12)

其中，Xj(k)表示第j个模型在k时刻的状态变量，ψj是模型j的非线性状态转移函数，Zj(k)表示第j个模型在k时刻的量测值，φj代表第j个模型的非线性量测函数，wj(k)、vj(k)分别是第j个模型在k时刻的系统噪声和量测噪声，方差分别是Qj(k)和Rj(k)。

第i个模型到第j个模型在k-1时刻的混合概率为

(13)

(14)

第j个模型的混合状态估计和混合协方差估计分别为

(15)

(16)

(17)

对上述无人机非线性运动模型进行扩展卡尔曼滤波。

状态预测方程为

(18)

其中，Fj(k-1)是第j个模型k-1时刻的状态转移矩阵。

计算协方差矩阵预测：

(19)

计算卡尔曼增益：

(20)

其中，Hj(k)为第j个模型在k时刻的量测转移矩阵。

计算状态更新矩阵和协方差更新矩阵：

(21)

计算速度估计修正更新值：

(22)

计算残差ej(k)：

ej(k)=Zj(k)-Hj(k)Xj(k|k-1) 。

(23)

计算残差协方差Sj(k)：

(24)

k时刻与模型j最匹配的极大似然函数为

(25)

对模型j的概率进行更新：

(26)

将每个滤波器的滤波输出结果加权，即可得到对多运动状态无人机的精确位置状态估计为[19]

(27)

加权协方差矩阵更新：

(28)

综上，所提出的速度辅助的无人机单站射频信号定位算法伪代码描述如下。

算法1单站运动参数估计定位伪代码描述。

输入：ranget，acct，gyrot，magnt，time。

输出：Xt。

① FunctionVEL_ESTIMATOR (range，time)

② Initialize：Set meas =[range，time]

③ Append：meast，meast+1，meast+2

④ Estimate_velocity (measurt，measurt+1，measurt+2)

⑤ velList ←

⑥ windowLen = range*ratio，vel=average (velList [windowLen(O)])

⑦ Return：v

⑧ While True Do

⑨ headingt=Filter (acct，gyrot，magnt)，ranget= KF (ranget)

⑩Xt.pre=IMM (EKF1.predict，EKF2.predict，…，EKFi.predict)←(range，heading)t

实际中，可首先利用机载飞控中的微机电传感器获得无人机航向角，射频机会信号的测距信息经卡尔曼滤波平滑数据预处理(第⑨行)；随后将距离和航向输入交互式多状态估计模块，得到对无人机位置、航向、线速度和角速度的经典估计(第⑩行)；通过静止和同向运动状态判断，得到无人机速度估计值v(第～行)；为避免速度估计值随距离增加而误差增大，设计可变窗口滤波器平滑速度(第④～⑥行)；最后将距离数据、航向角和速度估计值馈入速度辅助的多状态融合滤波器(SPoKP)中(第行)，得到多模型融合下的无人机位置输出估计值。

另外，在算法复杂度方面，文中算法的复杂度依托于滤波模型的复杂度。不同场景采用的滤波器不同，算法的复杂度也就不同，所以将文中的思想融入低复杂度的滤波模型中，可提高工程实现的可行性。

3 仿真分析和实验验证

本节对所提运动参数辅助的定位方法进行仿真和实验验证，通过均方根误差进行效果的评估。仿真分为两个场景：场景1，产生无人机巡航匀速运动航迹，这是国内外大多数文献所关注的典型运动模型，通过辅助不同的单模型扩展卡尔曼滤波方法，证明所提算法的有效性，进一步验证所提方法的性能边界；场景2，针对多飞行状态的无人机产生多机动状态运动航迹，通过文中算法和交互式多模型进行比较，验证所提算法对任意滤波模型的可迁移性，同时证明卫星导航定位系统拒止环境下所提算法具有对任意运动状态无人机的鲁棒定位能力。

所有仿真的条件均基于以下条件：在卫星导航定位系统拒止环境下，采用文献[21]中的机器人经典运动模型为二维运动模型，依据此运动模型产生目标运动的实时位置、线速度、角速度和加速度，该模型适用于绝大部分无人机；机会信号基站位于坐标原点；航迹分为多段，在每段开始无人机做方向性机动；仿真中距量测量和航向角测量中分别添加了标准差为0.005的高斯白噪声干扰。仿真参数详细说明见表1。

表1 仿真参数说明

3.1 速度参数辅助单站定位方法有效性与精度仿真

场景1 考查一组匀速多阶段无人机飞行航迹，无人机除了在每一段航迹的开始时刻进行方向机动，其余时刻无人机匀速飞行。

图2给出了所提速度辅助算法与单状态滤波模型定位性能对比图。

(a) 航迹对比

通过对图2(a)中启动阶段、稳定阶段和转弯后阶段的航迹放大图以及对图2(b)中定位误差进行分析，对于匀速运动状态的无人机，匀速扩展卡尔曼滤波效果好于匀变速扩展卡尔曼滤波效果，这符合模型假设。但不管是采用匀变速扩展卡尔曼滤波还是匀速扩展卡尔曼滤波，在所提运动参数辅助的条件下，有速度修正的滤波模型较没有速度修正的滤波模型定位精度高。匀变速扩展卡尔曼滤波速度修正、匀变速扩展卡尔曼滤波模型、匀速扩展卡尔曼滤波速度修正和匀速扩展卡尔曼滤波模型的均方根误差分别为0.573 6 m、0.967 5 m、0.481 1 m和0.648 1 m，可以看出，对于匀速模型来说，有速度辅助定位误差较经典扩展卡尔曼滤波定位误差降低了约25%。由图2(c)可以看出，所提方法基本能对真实速度进行准确估计，其效果明显好于单模型滤波结果。这表明了速度估计的准确度对定位精度的影响，验证了笔者所提算法具有较高的定位性能。

场景2 产生一组匀速、匀变速混合的变向航迹，该航迹除了在每一段航迹的开始时刻进行方向机动，其余每一段为匀速运动或匀变速运动。

图3给出了卫星导航定位系统拒止环境下，笔者提出的方法与现有单滤波器扩展卡尔曼滤波和交互式多模型的效果对比图。

(a) 航迹对比

在图3(a)和图3(b)中，由于无人机运动状态是机动的多状态组合，交互式多模型明显好于单滤波器，但所提运动参数辅助算法好于交互式多模型方法。由图3(c)可看出，虽然所提算法在5～15 s时有速度辅助的绝对误差大于无速度辅助模型的滤波结果，但这是由于仿真产生的航迹方向突变造成的，在现实环境中不会产生此类问题。从15 s之后的稳定结果来看，所提算法在交互式多模型上仍提高了定位精度，定位误差的绝对误差能降低约20%。

由上述两个场景的仿真证明，笔者所提动力学参数辅助算法用于无人机单站定位场景的性能优势，无论无人机实际采用何种运动状态滤波器，都能有效地降低其定位误差，展现了所提算法的优良鲁棒性和可迁移性能。

3.2 SPoKP方法实验验证

笔者提出的算法适用于任意机会信号定位系统。现仅采用芯片DW1000的超宽带模板进行测试，该模块一对一通讯距离为600 m，视距测距精度为5 cm。如图4(a)所示，利用四旋翼无人机仅搭载超宽带测距模块，在操场对算法进行了无人机多次实飞测试。图4(b)为某次实际飞行航迹与笔者所提速度辅助单站定位算法定位航迹的对比图，其中实线为DJI Go 软件飞行纪录数据，虚线为无人机飞行日志航迹，可以看到虚线与实线基本重合；点虚线是笔者所提的单站速度定位曲线；五角星为超宽带机会信号发射基站。

(a) 无人机搭载超宽带测距模块

多次实际飞行测试结果表明，笔者所提方法能完成较为精确的无人机单站定位。虽然在飞行每个阶段的初始精度较差，但能够立即利用速度信息修正位置误差。这是由于笔者提出算法的目标模型为固定翼无人机，而实验采用四旋翼无人机。四旋翼无人机在实验中机动转弯时，线速度为0，只有角速度变化；而固定翼滤波模型设定无人机在变向机动时线速度不能为0。简单来说，因为旋翼无人机变向时线速度为0导致滤波结果暂时发散，但随后算法能对线速度迅速修正，达到精确定位的目的。事实上，在实际应用中，固定翼无人机变向与线速度变化是实时联系的，即机动状态无人机线速度必不为0，所以在每个阶段初期的误差突然发散在实际中不会出现。分析多次飞行数据可知，在70 m×70 m的范围内，算法定位精度的均方根误差为2.709 m，能满足卫星导航定位系统拒止环境下无人机常规巡航、盘旋等任务对位置精度的需求。

机会信号导航定位系统普遍面临着因基站垂直、空间分布多样性差而导致三维定位精度低的问题，在卫星导航定位系统拒止环境下，可采用笔者提出的算法与不易受干扰且成本低廉的气压计或超声波测高仪来提高垂直方向精度。

4 总结与展望

笔者深入挖掘了无人机运动参数，提出一种速度辅助下的仅依靠单站机会测距信号和低成本传感器的无人机定位算法。在实测飞行中，该算法的二维均方根误差达到了约2.7 m，基本满足无人机航路定位和导航的任务需求。相关结论也验证了在卫星导航定位系统拒止环境下，开放空间的射频信号可为无人机提供可选定位手段，证明了机会信号的室外定位潜力，有着良好的应用场景。