稀疏基站条件下基于迭代卡尔曼滤波的协同定位方法

杨金一,郭 妍,江鹏飞

(国防科技大学智能科学学院,湖南长沙 410072)

1 引言

近年来,随着卫星拒止条件下导航定位[1-2]需求的增加,基于测距传感器的定位方法得到了广泛的应用和发展.利用点与点之间距离的测量值,载体可以获得相对于测距源的几何约束.虽然这种约束有别于对载体姿态、速度、位置等导航状态的直接观测,但可以通过距离与位置的数学关系来间接修正载体的导航状态[3-4].Shalaby等人[5]利用测距、惯性传感器和磁力计对卫星拒止环境中的智能体进行定位.Zheng等人[6]基于图优化模型提出一种将惯性测量单元(inertial measurement unit,IMU)与超宽带(ultra wide band,UWB)测距模块进行融合,来实现卫星拒止环境中载体定位的方法.

上述基于测距传感器的定位场景依托于4个以上测距源或基站来辅助定位,其原理类似于卫星的四点定位法[7].在实际应用环境中,往往没有足够多能提供测距信息且有绝对位置信息的基站[8],例如只能接收到3个基站的测距信息[9].在观测量有限的场景中,单个节点的定位总是会受自身测量的影响而产生不易消除的误差.多节点协同定位[10-12]可以在不添加额外传感器的情况下,通过引入额外的测距观测量来提高各个节点的定位精度.Qin等人[13]基于节点间距离测量的相对轨迹估计,对多节点集群进行分散式协同定位,相较于传统的组合导航具有更高的定位精度和更低的计算压力.Liu[14]提出了基于集群协同观测的逆协方差交叉定位优化方法,实现了基于距离等非线性协同观测的集群定位优化.

协同定位方法将其他节点的相对测量作为约束,通过对多节点运动和观测信息的融合,来输出精确定位信息,可以根据应用场景采用多种优化器和滤波器来实现[15-16].误差状态扩展卡尔曼滤波器(extended Kalman filter,EKF)依据传感器误差模型,使用局部线性化估计误差状态来对导航状态进行修正.相较于直接对状态进行计算,引入误差状态的优点是将状态控制在原点附近,避免了参数奇点和万向节锁等问题[17].Jung和Weiss[18]提出一种基于EKF的分布式状态估计方法,通过协同观测来计算微型飞行器的导航状态互协方差的后验估计,提高了集群的定位精度和计算速度.

在状态估计精度要求较高的场景中,EKF带来的非线性误差不可忽略,但在线性化过程中增加泰勒展开的高阶量会影响滤波器的计算效率.Havlik和Straka[19]对迭代扩展卡尔曼滤波器(iterated EKF,IEKF)进行了推导和性能评估,可以用较少的计算代价进行量测的迭代来降低非线性误差.He等人[20]通过拓扑流形对IEKF的原理和性能进行了分析,并在紧耦合的激光雷达惯导系统上进行了实验,验证了IEKF对非线性问题较好的滤波性能.

本文将基于EKF的协同定位方法中的滤波器替换为IEKF,通过迭代计算提高状态更新精度,以适应观测量较少、非线性误差影响较大的稀疏基站场景.本文首先推导IEKF协同定位方法,而后,基于仿真实验对不同稀疏基站场景下的协同定位精度进行分析,对所述协同定位方法的有效性进行验证.

2 基于IEKF的协同定位方法

本文所述集群包括待定位的平台和位置准确已知的基站,统称节点.各节点均搭载测距传感器输出距离测量值Rdis,平台搭载惯性传感器和测距传感器,惯性传感器输出角速度测量值和比力测量值fb.导航系n系坐标方向定义为“东北天”,载体系b系坐标方向定义为“右前上”.

对于由N个平台构成的集群,每个平台的状态向量可以用捷联惯导系统的九维误差向量来表示,即

其中: [δθ δγ δφ]T为俯仰角、横滚角、偏航角误差;[δvEδvNδvU]T为东向速度、北向速度、天向速度误差;[δL δλ δh]T为纬度、经度、高度误差,均为导航系分量.

每个平台的噪声向量为

误差状态微分方程可以表示为

其中:F为状态转移矩阵;G为系统噪声转移矩阵;w为系统噪声,w～N(0,Q).

将误差状态估计过程进行离散化,在k时刻,误差状态的一步预测方程为

其中Fk,k-1用分块矩阵可以表示为

各分块矩阵的定义见附录.

误差协方差的一步预测方程为

其中Gk用姿态变换矩阵可以表示为

设观测量为z,即用惯导解算的位置计算的距离Rins与测量的距离Rdis的差值.对于相互测距的两个平台1和2,设平台1在导航系惯导解算的位置为(L1,λ1,h1),平台2在导航系惯导解算的位置为(L2,λ2,h2),则平台1,2间惯导传感器观测的距离在较小的运动范围内可以近似为

其中Re为地球平均半径,取值6371000 m.

平台1的观测矩阵H12,1用雅克比矩阵表示为

其中下标“12,1”表示平台1,2间的距离对平台1的误差状态求偏导,且

同理,平台2的观测矩阵H12,2表示为

其中:

由于测距信号往返过程中引入的误差量不同,所以平台1对平台2的测距Rdis,12与平台2对平台1的测距Rdis,21不同.在基于EKF 的协同定位过程中,观测矩阵可写作

对应的观测方程为

其中νk为系统噪声,νk～N(0,Rk).

在系统有距离观测的时刻,滤波增益矩阵为

误差状态的更新过程为

误差状态协方差矩阵的更新过程为

整个基于EKF的协同定位经预测过程(4)(6)和更新过程(19)-(21),将更新后误差状态反馈到惯导解算的位置中,即可得到修正的定位结果.

与EKF不同的是,IEKF通过对更新过程进行i次迭代来降低非线性误差.当i=0时,根据前一次迭代过程估计的状态参照式(9)(13)推导新的雅克比矩阵,对第i次迭代过程中的观测矩阵H、滤波增益矩阵K进行计算,并得到第i次迭代的状态估计,即将式(17)-(20)改写为

一般而言,迭代次数i是根据状态的更新精度来确定的,状态随i的增大而趋于收敛[19].相较于直接在线性化过程中引入高阶项,IEKF无需进行协方差矩阵P的更新,只对量测过程进行迭代,从而一定程度上减少了计算量.

3 仿真平台设计

仿真对基于IEKF的协同定位方法进行有效性验证,本文建立了四平台四基站测距模型,其运动轨迹的二维水平投影和三维场景如图1-2所示.

图1 仿真轨迹二维图Fig.1 Simulation trajectory in 2D

图中坐标系是以轨迹中心点为坐标原点建立的当地坐标系,各点位置以m为单位.3个节点作逆时针环绕运动,其中节点2有竖直起降过程.各节点在水平运动过程中的速度为2 m/s,在初始和结束阶段有加速和减速过程,初速度和末速度均为零.各节点的惯导数据由轨迹发生器反演得到,测距数据真值为位置之差的二范数,误差服从均值为零的高斯分布.传感器参数如表1所示.

表1 传感器参数Table 1 Sensor parameters

4 仿真实验验证

本节根据上述仿真模型,设计不同观测场景来评估本文所述协同定位方法.由于滤波过程均选用误差状态(error state,ES),因此对比过程中称上述两种方法为EKF和IEKF协同定位方法.

4.1 场景1: 单平台

平台1对基站1,2,3测距,共有3个距离观测,观测矩阵为

观测方程为

其中:“N”表示位置未知的运动节点,“B”表示绝对位置已知的固定基站.

以随迭代次数增加而趋向收敛的位置误差状态大小为判据,设阈值为测距传感器误差1 m,可取迭代次数i=20,此时按照EKF和IEKF两种滤波方法计算的平台1定位误差如图3所示.

图3 场景1平台1定位误差对比Fig.3 Comparison of node 1 positioning errors in Scenario 1

对均方根误差(root mean squared error,RMSE)进行计算以进行定量分析,n个时刻的RMSE计算公式为

表2 场景1平台1定位RMSETable 2 RMSE of node 1 positioning in Scenario 1

可以看出:1)对于三基站测距条件下的单平台定位场景,相较于EKF,IEKF可以有效减小各个方向的定位误差,并在运动过程中使定位误差保持稳定,减少环绕运动中转弯变向带来的周期性误差影响.2)同时也注意到,对IEKF方法,高度方向的定位误差并不能收敛到0 m附近,而是与IEKF相似,在一个突变后收敛到10 m附近.由此可见,虽然IEKF可以大幅降低惯导高度通道发散带来的高度误差,但是单节点定位受自身测量的限制,在没有足够约束的情况下,一个突变的测量值会带来不易修正的定位误差.

单平台的场景验证了IEKF定位方法的有效性,接下来增加平台数,检验IEKF协同定位方法在多平台协同的场景中的效果,比较与单平台场景的异同.

4.2 场景2: 两平台协同

平台1,2对基站1,2,3测距,平台间相互测距,共有8个距离观测,观测矩阵为

观测方程为

考虑收敛到1 m的测距精度,并以随迭代次数增加而趋向收敛的位置误差状态大小为判据,可预先取迭代次数i=20,此时按照两种滤波方法EKF和IEKF计算的平台的定位误差如图4-5所示,RMSE计算结果如表3-4所示.

表3 场景2平台1定位RMSETable 3 RMSE of node 1 positioning in Scenario 2

表4 场景2平台2定位RMSETable 4 RMSE of node 2 positioning in Scenario 2

图4 场景2平台1定位误差对比Fig.4 Comparison of node 1 positioning errors in Scenario 2

图5 场景2平台2定位误差对比Fig.5 Comparison of node 2 positioning errors in Scenario 2

可以看出:1)与单平台场景相同的是,相较于EKF,IEKF可以有效减小各个方向的定位误差;2)相较于单平台高度方向的误差并不能收敛到0,两平台协同可以使两个平台的高度误差都趋向于0,即使其中一个平台有高度变化;3)同时也注意到,平台2上升下降过程中的高度误差较大,其原因是天向速度较大,导致一个测距周期内惯导积累的高度误差较大,在没有足够约束的情况下,上升下降过程中的高度误差不易消除,在协同过程中也导致平台1相应时段高度误差较大.

两平台协同的场景验证了IEKF协同定位方法的有效性,接下来继续增加平台数,验证定位精度是否随平台数或观测量的增加而提高.

4.3 场景3: 三平台协同

平台1,2,3对基站1,2,3测距,平台间相互测距,共有15个距离观测,观测矩阵为

观测方程为

取迭代次数i=20,此时按照两种滤波方法EKF和IEKF计算的平台的定位误差如图6-8所示,RMSE计算结果如表5-7所示.

表5 场景3平台1定位RMSETable 5 RMSE of node 1 positioning in Scenario 3

表6 场景3平台2定位RMSETable 6 RMSE of node 2 positioning in Scenario 3

表7 场景3平台3定位RMSETable 7 RMSE of node 3 positioning in Scenario 3

图6 场景3平台1定位误差对比Fig.6 Comparison of node 1 positioning errors in Scenario 3

图7 场景3平台2定位误差对比Fig.7 Comparison of node 2 positioning errors in Scenario 3

图8 场景3平台3定位误差对比Fig.8 Comparison of node 3 positioning errors in Scenario 3

可以看出:1)和场景1,2相同,相较于EKF,IEKF可以有效减小各个方向的定位误差.同时,随着观测量增多,两种滤波方法的定位精度都随之提高;2)相较于两平台协同,三平台协同可以使高度方向的误差进一步收敛,对平台2上升下降过程中引起的高度误差也有更好的修正效果;3)同时也注意到,在平台2的下降过程中,两种滤波方法都还是有较明显的高度误差发散,虽然与图5对比,三平台IEKF协同可以减缓发散的趋势,但是误差的发散难以避免,只能通过改变测距频率或者引入更多平台来解决.

三平台协同的场景在IEKF协同定位方法有效性的基础上,验证了平台定位精度随平台数或观测量的增加而提高.

在主板为AMD Ryzen 9 5900HX,内存32 G的电脑MATLAB中运行仿真程序,3个场景的滤波程序运行时间如表8所示.

表8 各场景滤波程序运行时间Table 8 Simulation program runtime in each Scenario

可以看出,在各个场景中仅靠较少的时间成本就实现了EKF到IEKF的转换,而各场景间时间的显著增加来源于观测量的增多以及观测矩阵维度的增加.因此,相较于增加观测量或增加泰勒展开的高阶项来提高定位精度,用IEKF的量测迭代具有更小的计算代价.

5 结论

本文提出一种基于迭代卡尔曼滤波的惯性/测距协同定位方法,通过对滤波的量测更新过程进行迭代来减小非线性误差,实现稀疏基站下有效的协同定位.本文首先对导航系下基于迭代卡尔曼滤波的协同定位方法进行了推导.其次,建立了三基站测距模型,对3种不同平台参与的场景进行了仿真实验.通过实验结果对比,验证了IEKF协同定位方法的有效性以及较传统EKF的更好的误差修正效果,此方法以较小的计算代价换取了较大的定位精度提升,别切随着参与协同的平台数增多,协同定位精度取得明显提升.

附录

状态转移矩阵F中各分块矩阵定义如下: