基于随机信标的因子图同时定位构图方法

2021-12-10宁云晖邓福建翟国威

舰船科学技术 2021年10期

王凯，宁云晖，邓福建，翟国威，刘猛

(1. 天津航海仪器研究所，天津 300131；2. 海军研究院，北京 100161)

0 引言

随着未来作战系统的信息化程度不断提高，水面无人艇（USV）因具有机动灵活、可替代人开展危险作业等特点，被广泛用于执行战场侦察、情报收集、反水雷等高危险任务作业[1-2]。为了更好实现水面无人艇的作业，精确的导航定位也至关重要。目前，常用的导航方法有惯导、卫导、长基线水声定位、多普勒计程仪等，其中惯导误差随时间呈发散趋势，卫导信号脆弱，易受攻击和压制，信标导航由于无需卫星信号即可实现精确定位而成为导航领域新的研究热点。如文献[3]采用单信标布置在水面船上，并利用卫导数据对信标进行定位，水下无人航行器借助装备的长基线系统测量相对于信标的距离，根据DVL和角度传感器进行航位推算，分别研究了EKF和粒子滤波算法的导航定位性能，验证了粒子滤波具有更高的定位精度；文献[4]研究了如何在给定机器人运动轨迹时最优布置信标位置，提高导航定位精度。基于已知信标位置的导航算法简单，导航误差小，具有良好的收敛性，但需要提前标定信标位置，花费大量时间和人力物力，特别是在战时或执行紧急任务时，无法满足对及时性的要求，限制了信标导航的应用范围。为减少系统启动时间，有专家学者提出基于随机信标的导航定位算法，为在一定区域内执行任务的无人艇通过飞机或母船布放一些信标，而不需要标定信标位置，实现导航定位和环境构图[5]。

目前，关于采用因子图算法进行同时定位构图的研究很多。如文献[6]采用滑动窗因子图基于地图上潜在路标位置进行三维路标地图和全部导航状态的非线性优化估计，提高卫星拒止环境下定位和路标位置估计精度；针对无人作战平台在室内等复杂环境下的高精度定位问题，蒋小强等[7]采用因子图模型对WLAN和单目视觉进行数据融合，从而获得精确的定位和地图信息；文献[8]研究了VO和IMU融合因子图模型，实现自动驾驶系统的即时定位和地图构建；Vadim Indelman等[9]把因子图应用于多源导航信息融合系统，将惯导、GPS和视觉传感器进行组合，实现导航定位和环境地图的构建；文献[10]将因子图模型应用于超宽带/INS组合导航中，实现无人飞行器的室内定位，相比于传统滤波算法提高了定位精度。但如何采用水声信息同时进行导航定位和信标位置估计的研究还较少。

本文提出一种基于随机信标的因子图同时定位构图算法，利用无人艇上装备的DVL和罗经测量无人艇移动速度和航向，通过测量无人艇相对于信标的距离，在无需标定信标位置的前提下，实现无人艇和信标的位置估计。

1 因子图模型架构设计

1.1 融合架构

为简化模型，减小计算量，将水深视为固定值，在母船布放无人艇时测定。在构建基于随机信标的因子图同时定位构图模型中，变量节点包括无人艇的位置和航向、随机信标的位置，量测因子节点包括DVL测量的速度信息、罗经测量的航向信息和无人艇相对于信标的距离信息。设计如图1所示因子图模型架构。

图1 基于随机信标的因子图同时定位构图模型架构Fig. 1 Simultaneous localization and mapping model architectureof factor graph based on random beacon

不同时刻之间的无人艇状态变量节点由DVL和罗经量测信息组合成的因子节点fvφ连接，无人艇状态变量节点和路标位置变量节点经信标因子节点连接。USV状态变量节点包括无人艇的位置和航向，即xk=，定义从初始时刻t0到当前时刻tk的无人艇状态变量节点集合为，路标位置变量节点集合定位为；定义从初始时刻t1到当前时刻tk测量的无人艇和信标距离的集合，式中m表示从时刻t1到时刻tk感知到的路标总数量。

根据图1描述的各个变量间的概率密度函数关系，可知USV和信标位置变量的后验概率密度函数如下：

基于最大后验概率估计，可得无人艇和信标位置估计的表达式如下：

1.2 因子节点设计

USV运动学模型为：

式中：Px，Py表示位置；φ表示航向；vx，vy表示速度；ω表示转向角速度。对式（3）进行离散化，可得根据DVL和罗经量测进行状态更新的离散化形式：

式中：xk为tk时刻真实状态变量；xk−1为tk−1时刻真实状态变量；T为离散时间间隔；vk−1为tk−1时刻真实速度；ωk−1为tk−1时刻真实转向角速度。记tk时刻的状态估计为，状态估计误差为δxk，则，记tk−1时刻的状态估计为，状态估计误差为δxk−1，则为tk−1时刻DVL测量的速度，则，式中wvk−1为DVL量测噪声，通常可视为高斯白噪声，方差为σvk−1；为tk−1时刻罗经测量的航向，则表示航向测量误差；ωk−1为tk−1时刻真实转向角速度，为tk−1时刻罗经测量的转向角速度，则，wωk−1为罗经角速度测量噪声，方差为σωk−1。

考虑DVL和罗经存在测量误差，记Pk−1为tk−1时刻状态估计误差的协方差矩阵，则定义DVL和罗经因子节点模型为：

将式(5)在估计位置处作泰勒展开，忽略二次及以上项，可得：

记

DVL和罗经的噪声协方差矩阵为：

则式(6)可整理如下：

tk时刻水声测量的水面舰船与第i个信标的距离为：

式中，wd为水声测量噪声，通常视为高斯白噪声，方差为σd，记Rki为tk时刻测量的第i个信标的误差协方差矩阵，则

定义信标量测因子节点模型为：

将式(9)在估计位置处作泰勒展开，忽略二次及以上项，记第i个信标的估计位置为，信标位置估计误差为δli，可得：

将荸荠用大量清水洗去泥土，用去皮刀除去荸荠表面的褐色表皮，将去皮后的荸荠1 kg放置在5 L的玻璃容器中，加无菌水至没过荸荠，浸泡5 min后滤除水分(浸泡时间过长，鲜切荸荠细胞严重失水)，反复浸泡清洗三次，滤干水分备用，该种去皮后的荸荠即为鲜切荸荠。

2 因子图同时定位构图算法

根据上述分析，将式(2)在状态变量和信标位置估计值处作泰勒展开并保留一次项，记变量节点估计误差集合为δX=[δl1···δx1δx2···]T，并将泰勒展开式作标准化处理，然后构造全局雅克比矩阵J和残差项b，求解如下方程：

式中，各符号定义如下：

因子图同时定位构图算法：

2) 当接收到DVL和罗经测量信息时，创建因子节点fvφ并插入因子图G中，同时创建变量节点xk插入变量节点X中；当接收到信标量测因子节点后，创建因子节点插入图G中，创建变量节点xk和lm插入变量节点X中；

3) 提取受新插入因子节点影响的子图Gnew；

4) 根据量测因子节点计算变量节点的初始估计值；

5) 在变量节点估计值处线性化各因子节点建立雅克比矩阵和残差向量；

6) 计算变量节点估计误差并进行修正；

7) 计算残差向量，若大于设定阈值，则转步骤5)进行迭代，否则，完成当前导航状态估计，转步骤2),等待新量测因子的插入。

3 数值仿真验证与分析

设定仿真区域1 000 m×1 000 m，随机布放12个信标，采用DVL和罗经测量速度和航向，测速误差为0.2 m/s，刻度系数误差3%，航向测量误差10°/h，距离测量误差5 m，位置初始误差1 m，初始航向误差1°。

设置圆形和蛇形2种运动轨迹进行数值仿真验证，将因子图算法与EKF滤波算法进行比较，仿真结果如图2～图7所示。

因子图算法、EKF算法导航定位误差和信标位置估计误差如表1和表2所示。

从图2～图7及表1和表2可以看出，基于随机信标，在圆形轨迹和蛇形轨迹2种情况下因子图定位精度和信标位置估计精度均优于传统EKF滤波算法；圆形轨迹和蛇形轨迹仿真验证中，因子图算法相比EKF滤波算法定位精度分别提高37.6%和14.6%，平均信标位置估计误差分别提高14.1%和13.1%。由理论分析可知，因子图算法通过利用所有时刻量测信息，采用非线性最小二乘优化算法进行导航状态估计，EKF滤波算法只考虑当前时刻量测信息，采用非线性滤波算法进行导航状态和信标位置估计，利用量测信息不充分且具有系统线性化误差，因此，因子图算法估计精度应优于EKF滤波算法，与数值仿真验证结果一致。

图2 圆形运动轨迹对比图Fig. 2 Comparison chart of circular trajectory

图3 圆形运动轨迹下X轴方向估计误差Fig. 3 Estimation error of x-axis direction in circular trajectory

图4 圆形运动轨迹下Y轴方向估计误差Fig. 4 Estimation error of y-axis direction in circular trajectory

图5 蛇形运动轨迹对比图Fig. 5 Comparison chart of serpentine trajectory

图6 蛇形运动轨迹下X轴方向估计误差Fig. 6 Estimation error of x-axis direction in serpentine trajectory

4 结语

本文针对战时或执行紧急任务时的导航定位问题，研究了一种基于随机信标的因子图同时定位构图算法，建立了DVL和罗经因子节点、信标因子节点数学模型，构建了因子图架构，同时对水面无人艇和信标的位置进行估计，并对该算法开展了数值仿真试验验证。仿真结果表明：因子图算法克服传统水声定位必须提前标定信标位置的缺点，且比EKF滤波算法具有更好的定位精度和信标位置估计精度，为快速实现信标导航提供了一种导航方法。但是，由于本文只进行了理论分析与数值仿真，还有待进一步开展实船试验进行验证和完善。