基于因子图的协同定位与误差估计算法

范世伟, 张 亚, 郝 强, 姜 畔, 于 飞

(哈尔滨工业大学仪器科学与工程学院, 黑龙江 哈尔滨 150001)

0 引 言

随着人类对海洋的深入开发,自主水下航行器(autonomous underwater vehicle,AUV)在军事应用、科学研究及海洋探索等方面的作用越来越重要[1-2]。AUV正确感知自身在环境中的位置和的能力,是成功执行任务的决定因素。如今的导航不仅确定载体当前和可能的未来位置所必需的测量和计算过程,还包含了与载体从一个点移动到另一个点有关的所有子问题,如感知、状态估计、避障甚至路径规划等[3-4]。在导航过程中定位技术则是解决状态估计、避障等问题的关键技术,所以研究AUV的高精度定位技术具有重要意义[5-6]。由于电磁信号在水下环境中会被强烈衰减导致全球定位系统(global positioning system, GPS)技术在水下无法正常应用[7]。因此,必须采用其他技术为AUV提供准确的导航能力,通常情况下AUV的位置是根据计程仪配合罗经或者利用惯性导航系统(inertial navigation systems, INS)来确定的,但是当AUV运动时,根据计程仪配合罗经进行航位推算的位置估计误差不断累积,最终可能会导致AUV丢失,而高精度的INS通常价格高昂,不利于AUV的大规模应用[8-10]。由于对海洋的深入开发,单个AUV难以执行复杂的军事及民事任务,导致多AUV协同系统在海洋开发、科学研究及军事应用等方面的作用更加重要[11]。因此,基于水声通信网络的多AUV的协同定位技术受到了更多的关注[12-13]。不同AUV利用水声通信来共享彼此的信息,利用水声测距进行相对距离的测量作为观测量,然后利用数据融合技术来降低定位误差,这种方式被称为“协同定位”。由于多AUV协同定位可以提高系统整体的定位能力,而且具有成本低、系统鲁棒性好的优点,可以极大地增强多AUV系统的任务执行能力,具有重要的理论和应用价值。因此,多AUV的协同定位技术渐渐发展为一个热门的研究课题,研究能够提高多AUV协同定位精度的定位算法具有重要意义[14-15]。

目前,国内外学者对协同定位的研究主要集中在如何利用观测信息直接提高定位精度方面,针对从艇的速度误差和航向误差进行估计及补偿算法还有待研究[16]。例如,Webster研究了基于扩展卡尔曼滤波(extended Kalman filter, EKF)的协同定位算法,研究结果表明EKF算法能够有效实现多AUV系统协同定位[17]。但是该研究具有以下局限:① 面对非线性问题时,EKF实际是将非线性系统取泰勒展开一阶项进行线性化,由于截断引起的高阶截断误差会对系统估计结果产生较大影响,协同定位精度降低[18];② 算法没有对从艇速度误差及航向误差进行估计和补偿,降低了协同定位精度。

为便于算法的实施,本文以高斯噪声为前提,通过在协同定位的因子图模型中计算不同变量的均值及方差来得到从艇的后验位置估计结果。然后,根据位置的先验估计作为观测量,在误差估计的因子图模型中计算从艇的速度误差及航向误差。最后,通过将估计的误差结果补偿到协同定位算法中,来提高从艇的定位精度。基于因子图的协同定位算法充分利用了因子图的特点,将复杂的问题分为若干简单问题,保障算法精度的同时降低了计算量。

本文的主要贡献包括:① 针对多AUV协同定位的数据融合问题,设计了基于距离测量的协同定位系统因子图模型,提出了基于因子图模型的多AUV协同定位算法;② 分析了从艇速度误差及航向误差对其定位的影响,并提出了基于因子图的误差估计算法,大幅提高了从艇的定位能力。

1 协同定位系统分析

1.1 AUV运动学模型

利用AUV搭载的深度计可以准确地测量AUV所处深度,可以将各AUV的位置信息及距离观测信息投影到水平面内,所以本文在二维平面内讨论多AUV协同定位问题。可以将AUV的二维运动模型写成如下形式:

(1)

式中,(x,y)表示AUV的位置坐标;v表示AUV在航行时的速度;θ表示AUV的航向。

在k时刻利用计程仪测量得到的速度表示为vk,罗经测量得到的航向表示为θk,此时AUV的位置表示为(xk,yk),根据式(1)可以得到AUV的离散运动模型为

(2)

式中,Δt表示采样周期。

1.2 基于水声测距的量测模型

本文主要研究主从式AUV协同定位系统,主艇配备着高精度的导航设备,在协同定位过程中,认为主艇的位置准确,利用主艇位置修正配备低精度导航设备的从艇,此时主从艇间相互通信,从艇获取主艇位置信息与观测信息。水下协同定位系统中的观测信息是利用水声设备得到的相对距离,因此协同定位系统的量测方程可以表示为

(3)

1.3 定位误差分析

从式(2)可以看出,影响定位精度的关键因素就是速度及航向误差。根据AUV的运动学模型,在l时刻利用测量得到的速度及航向计算位置如下:

(4)

(5)

(6)

将式(5)和式(6)代入式(4)可得

(7)

将式(7)中的两式分别展开，可得

(8)

(9)

假设速度及航向误差为小量,那么有

(10)

将式(10)代入式(8)中可得

(11)

将式(10)代入式(9)中可得

(12)

2 协同定位与误差估计算法

(13)

l时刻的位置误差可以写成

(14)

从l时刻到k时刻由于没有接收到量测信息,所以位置更新采用先验估计值,对x的先验估计如下:

(15)

同理,对y的先验估计如下:

(16)

将式(13)代入式(15)中可得

(17)

将式(13)代入式(16)中可得

(18)

那么k时刻的位置误差可以表示为

(19)

(20)

式中,

根据式(19)、式(20)以及AUV的运动学模型和量测模型,可以得到协同定位与误差估计的因子图模型,如图1所示。图1中的红色实线框内是算法时间更新部分,紫色虚线框内是速度及航向误差估计的量测更新部分,绿色点划线框内是协同定位的量测更新部分。

图1 协同定位及误差估计算法的因子图模型Fig.1 Factor graph model of cooperative positioning and error estimation algorithm

2.1 协同定位算法

在执行基于因子图模型的协同定位算法时,需要在变量节点与函数节点间传递均值和方差,具体步骤如下。

步骤 1初始化

在协同定位开始时确定初始状态:

(21)

那么,可以得到如下表达式:

(22)

(23)

步骤 3xk和yk的第一次更新

首先引入转换矩阵:

(24)

(25)

(26)

其中,

(27)

(28)

(29)

步骤 5Δxk和Δyk的第一次更新

函数节点Ck和Dk的作用是转换相对位置信息与绝对位置信息。因此,从节点Ck传递到节点Δxk的概率密度函数可以表示为

(30)

对于节点Dk,从节点Dk传递到节点Δyk的概率密度函数可以表示为

(31)

步骤 6Δxk和Δyk的第二次更新

函数节点Ek的作用是将x组坐标系与y组结合起来。根据勾股定理,变量节点Δxk与Δyk之间的约束可以描述为

(32)

从函数节点Ek传递到变量节点Δyk的概率密度函数可以表示为

(33)

从函数节点Ek传递到变量节点Δxk的概率密度函数可以表示为

(34)

(35)

(36)

步骤 8xk和yk的第二次更新

经过计算整理后得到xk和yk在更新后的概率密度函数分别为

(37)

(38)

(39)

其均值和方差的计算与式(28)和式(29)相似,不再赘述。到此完成了图1中绿色点划线内协同定位的部分。

2.2 误差估计算法

各国学者对协同定位的研究主要关注协同定位算法方面,如EKF、无迹卡尔曼滤波等,目前并没有在协同定位系统中对速度及航向误差进行估计的研究,本文为了提高协同定位精度,首次开展了误差估计算法的研究。

将式(19)和式(20)中的误差公式重写为

(40)

(41)

成立，则进一步有

(42)

在协同定位算法中位置的时间更新时,需要利用速度和航向的测量值减掉相应的估计误差后参与计算,具体如下:

(43)

(44)

(45)

(46)

(47)

(48)

(49)

(50)

将式(49)代入到式(48)中,可以得到

(51)

(52)

(53)

(54)

(55)

(56)

(57)

(58)

(59)

将式(49)和式(58)代入到式(57),可以得到

(60)

(61)

在此可以将式(60)的期望和方差表示为

(62)

(63)

(64)

可以将式(63)的期望和方差表示为

(65)

(66)

若k时刻没有接收到观测信息,有

(67)

(68)

在从艇接收到观测信息后,首先利用协同定位算法输出的位置后验估计和先验估计求取δxk和δyk的期望和方差:

(69)

(70)

(71)

(72)

(73)

(74)

(75)

(76)

(77)

(78)

(79)

(80)

(81)

至此,完成了基于因子图模型的速度及航向误差估计,结合第2.1节的协同定位算法共同构成基于因子图的协同定位及误差估计(factor graph-based cooperative positioning and error estimation, FGCPAEE)算法,为了清晰描述FGCPAEE算法,其伪代码如下:

算法 1

3 船载试验验证

为了验证本文所提算法在处理实际数据时的有效性,利用船载湖试试验数据进行离线处理与分析。由于条件限制,湖试试验利用水面艇代替AUV进行。而浅水水域的声学通信环境与深水水域相比更加复杂,且其系统噪声特性完全可以表示AUV系统。湖试试验方案中采用两条主艇、一条从艇进行试验,试验过程中两条主艇的位置信息采用高精度的实时GPS位置信息,利用水声通信设备实现主从艇之间信息传递及观测量的测量。从艇自身的定位是利用多普勒计程仪提供的速度信息和磁罗经提供的航向信息计算得到的。为提供参考基准,在相对固定的联合安装基座上还配备位置精度为2～5 cm的高精度组合导航系统。水声通信是实现AUV协同定位的重要环节,湖试中利用S2CR7/17型水声通信设备实现水声通信及测距。在进行试验时两条主艇各自每隔5 s向从艇发送带标识符的GPS位置信息以及主从艇间相对距离信息，其他传感器信息的采样周期为1 s。

设置试验时间为2 000 s,试验数据仅在前1 000 s进行协同定位和误差估计,1 000 s后进行自主定位,与EKF算法和改进的因子图与和积(improved factor graph and sum-product, IFGS)算法进行定位误差的对比[19]。试验中各艇轨迹如图2所示。

图2 试验中各艇航行轨迹Fig.2 Running track of each boat in the experiment

试验中FGCPAEE算法对从艇的速度误差及航向误差的估计结果如图3所示,由于1 000 s后不再进行协同定位,因此1 000 s后速度误差及航向误差不再更新。从图3中可以看出,试验中从艇的速度误差估计值在0.1～0.2 m/s,航向误差的估计值在-1°～0°。

对试验数据分别利用3种算法进行协同定位，定位误差的结果如图4所示。从图4中可以看出,在1 000 s之后不再接收观测信息时,EKF算法和IFGS算法由于没有对速度及航向误差进行估计和补偿，导致定位误差增长很快,而本文所提算法则明显降低了误差增长速度。

图3 FGCPAEE算法的误差估计结果Fig.3 Error estimation results of FGCPAEE algorithm

图4 不同算法的定位误差曲线Fig.4 Positioning error curves of different algorithms

各算法前1 000 s定位误差的均值和均方根如图5所示。从图5可以看出，这段时间内,EKF算法的RMSE为17.13 m,IFGS算法的RMSE为12.79 m,FGCPAEE算法的RSME为12.67 m,FGCPAEE算法的定位误差最小。

图5 不同算法前1 000 s的定位误差比较Fig.5 Comparison of positioning errors of different algorithms before 1 000 s

3种算法的处理时间如表1所示,实验采用的计算机为8 G内存,2.3 G主频,Matlab语言。从表1可以看出,本文提出的FGCPAEE算法在定位误差最小的同时运行时间小于EKF算法,说明此算法可以进行在线的实时计算。

表1 算法运行时间比较

不同算法后1 000 s定位误差的均值和RMSE如图6所示。从图6可以看出,这段时间内,EKF算法的RMSE为99.43 m,IFGS算法的RMSE为96.92 m,FGCPAEE算法的RMSE为35.89 m,此时显然FGCPAEE算法的定位性能大幅优于另外两种算法。

图6 不同算法后1 000 s的定位误差比较Fig.6 Comparison of positioning errors of different algorithms after 1 000 s

4 结 论

本文研究了多AUV协同定位的数据融合问题,提出了速度及航向误差估计算法的因子图模型,并推导了速度和航向误差估计公式。在高斯噪声的前提下,因子图模型利用变量的期望和方差作为因子间传递的信息,推导了FGCPAEE算法的估计过程。船载试验结果验证了本文所提算法的有效性,在实验过程中前1 000 s的协同定位阶段与经典的EKF算法相比均方根误差减少了26.04%;在后1 000 s的自主定位阶段与经典的EKF算法相比均方根误差减少了63.9%。

本文所提算法具有较大的适用性,当主艇无法提供观测信息时,速度误差及航向误差对传统算法影响很大,而对本文所提算法影响较小,有利于应对多AUV执行任务时的突发情况。