定位精度优化驱动的水下无人平台航路规划方法

孙思博，秦莎莎，王 庸，明瑞和，王 勇

（1.哈尔滨工程大学 水声工程学院，黑龙江 哈尔滨 150001；2.哈尔滨工业大学 电子与信息工程学院，黑龙江 哈尔滨 150001）

0 引言

随着无人平台的发展速度越来越快，相关研究也越来越广泛，航路规划作为无人平台的关键技术之一，对无人平台的自主航行起着至关重要的作用。航路规划是指在具有障碍物的环境中，能够规划出一条从起始位置状态到目标位置状态无碰撞的最优路径或次优路径，并在满足所有约束条件最优状态下完成设定任务[1]。

1 水声定位方法与航路规划简介

1.1 水声定位

基本的声学定位方法分为3类[2]。第1类是基于信号到达时间（Time of Arrival，TOA）的定位方法。TOA法观察信号传播时延，传播时延与当前定位节点与目标间的距离成正比，其需要信号发送方与接收方建立时间同步。因此在非合作信号的水声定位中不可用。

第2类是基于信号到达时间差（Time Difference of Arrival，TDOA）的定位方法。TDOA法不需要获得信号具体发射时间，可以实现对非合作信号的定位。徐英杰[3]等人提出了一种基于牛顿迭代方法的三星时差定位融合算法，运用多组时差和高程信息融合，有效提高了定位精度。

第3类是基于信号到达方向（Direction of Arrival，DOA）的定位方法。DOA法利用目标的角度信息，通过传感器阵列获取目标的角度信息。传统的目标源位置定位通过笛卡尔坐标系确定，陈娴静[4]等人提出了一种基于到达角的近远场统一定位方法，使用修正后的极坐标表示定位目标的坐标，避免了目标位于远场时数值问题的出现。

1.2 航路规划

传统的水下无人平台航路规划主要基于人工势场法、蚁群法、遗传算法等。M.Mucientes等[5]提出了一种采用遗传算法的模糊控制器的自动化设计，该算法基于迭代规则学习（IRL）方法，定义了选择规则数量参数，用于在机器人路径规划中绕过障碍物。该算法主要考虑航路规划的避障问题，并没有充分考虑到载荷任务的任务执行效率。王金龙[6]针对传统人工势场法可能会出现局部最小点和目标不可达问题，在人工势场的引力场中增加了振荡函数，当无人艇出现陷入局部最优解无法前进问题时，可以有选择其他的备选路线。该方法也是基于航路规划中的避障问题所提出的解决办法，也没有考虑提高航路规划的任务执行效率。以上方法都没有针对定位问题进行优化，导致定位精度下降，本文采用定位精度优化驱动的水下无人平台航路规划方法，在进行避障的同时，进一步提高定位精度。通过仿真实验和湖试结果对该方法进行了验证。

本文的其他部分如下：在第2节中，本文选取TDOA法对目标位置坐标进行定位解算，给出了TDOA法的定位模型，分析了 TDOA法的定位精度。第3节讨论了传统人工势场法在执行定位任务时的局限性，提出了定位精度优化驱动的水下无人平台航路规划方法，并进行了理论分析。第4节针对本文提出的方法，进行了仿真分析，并在真实环境中对该方法进行了验证。

2 TDOA定位法与精度分析

2.1 TDOA定位方法

本文使用的全球坐标系如图1所示。X轴、Y轴、Z轴分别表示东、北和垂直向上的方向，图1显示了水下定位的几何结构，所定位目标位于海底，坐标表示为[xs,ys,zs]，目标深度可由海图测量。定位解算节点瞬时位置由GPS获得，表示为[xi,yi,zi]，主要求解目标的水平坐标xs和ys。

图1 水下定位过程中的几何配置Fig.1 Geometrical configuration for the UWAL

令ts为信号发射周期，ta为信号初始发射时刻，第2个信号发射时刻为ta+ts，ta+nts为第n个信号的发送时刻。ti为第i个解算节点的信号接收时刻，ni为接收信号的周期号，c为声速。di表示目标到第i个解算节点的直线距离，di=。TDOA法定位时选取2个解算节点，其时间差为

求解目标位置至少需要3个解算节点，基于TDOA法的数学模型为

式中：距离di是待求量xs和ys的非线性函数，因此 TDOA法的数学模型是非线性方程，可以采用牛顿迭代法进行求解。

2.2 TDOA法的定位精度分析

使用偏微分法分析 TDOA法的定位精度，将各变量的误差看作相互独立的且均值为0的服从高斯分布的随机变量，引入HDOP（水平精度因子）表示定位精度，HDOP的数值越大，定位误差越大，其定位精度越差[7]。式（3）中的E[ ]表示对变量求期望，HDOP可以表示为

式中：[xs,ys]表示目标的真实位置坐标；表示目标的解算估计坐标。对TDOA法定位表达式各变量求偏微分，可以得到

将各个偏导数写成矩阵形式

改写为矩阵形式

矩阵运算表达式为

式中：Dd，DINS，Dt，Dc和Md，MINS，Mt，Mc分别是深度测量误差、解算节点坐标误差、到达时间误差、声速测量误差的协方差矩阵和其偏导数矩阵。各变量误差协方差矩阵通过对角阵表示为

式中：diag()为对角矩阵，σd2，σINS2，σt2，σc2分别是深度测量、解算节点坐标、到达时间和声速测量的均方误差[8]。HDOP表示为HDOP=。

根据以上公式，对 TDOA法的性能进行仿真分析，仿真参数设置如表1所示。

表1 仿真参数和测量噪声参数Tab.1 Simulation parameters and measurement noise parameters for the simulation

两组定位精度仿真结果如图2所示。

图2 TDOA法的定位精度分布Fig.2 Positioning accuracy distribution for the TDOA algorithm

从图中可以看出，TDOA法在解算节点呈等腰三角形分布时有较好的定位精度，其中1 km×1 km正方形区域内的平均定位精度为4.3 m。当定位节点从弧形轨迹上选取时，定位精度下降明显，定位精度高的区域存在断层，分别位于3个定位解算节点的上方和下方，区域内的平均定位精度为15.04 m。因此，定位节点的选择对于TDOA法在进行定位解算时具有较大影响，选取节点时尽量选取一个圆形轨迹的内切等边三角形。

进行目标位置定位时，选取不同的航迹，对TDOA法的定位精度进行分析。各变量误差与表1相同，其中目标水平位置坐标为[0，0]，分别对圆形航迹和直线航迹进行仿真分析，从周期号为200的节点开始进行定位解算。航行轨迹如图 3（a）所示。

图3 不同航迹下TDOA法的定位精度Fig.3 Positioning accuracy for different navigation trajectories

从图 3（b）中可以看出，直线航迹的定位误差起伏大，定位精度不稳定，鲁棒性差；圆形航迹的定位误差起伏小，定位精度高，鲁棒性好。因此，在定位过程中，航迹对 TDOA法的定位结果影响大，尽量选择围绕目标的圆形航迹。

3 航路规划方法

3.1 传统人工势场法

人工势场法航路规划由 Khatib最先提出，是一种通过虚拟力来规划航迹的算法。基本思想是将运动的机器人放在抽象的人为制造的力场中，目标点产生“引力”，障碍物产生“斥力”，各个力以矢量和形式叠加在机器人上，通过产生的合力来规划航路。一般来说，在虚拟的人工势场中，出发点势能较大，障碍物等不可到达位置相当于高峰，而目标点相当于谷地的最低处，如同在势场中出发点放置一个小球，小球在终点的引力与障碍物的斥力的共同作用下的轨迹就作为无人艇的运动轨迹，能够自如地避开障碍物，到达终点，应用势场法规划出来的路径一般是光顺并确保能避开障碍物比较安全的[9]。

传统人工势场法基于目标点产生“引力”，障碍物产生“斥力”，各个力以矢量和形式叠加，通过产生的合力规划航路。AUV进行航路规划时，令引力来源为解算出的目标位置坐标，斥力来源为海图中的障碍物或航禁区，AUV在引力和斥力的共同叠加下前行[10]。通常将斥力函数设为

式中：η为常数，为斥力尺度因子；c为斥力次数，通常设为-2；ρ为势场中某点到障碍物中心点的距离；0ρ为障碍物的影响半径，以障碍物中心点为原点，0ρ为半径的圆内，其势场值为Φ，Φ为一个极大的值；当势场中某点到障碍物中心点的距离大于ρ0时，该点的势场随着ρ的增大而减小。

势场中的斥力为斥力场的梯度，斥力是矢量，即Frep=(Fx,Fy)。

目标点产生的引力场采用类似重力与重力势能的建模方式，如下式

式中：ξ为引力尺度因子；ρ为势场中某点到目标点的距离。AUV在引力场中受到的引力为引力场的梯度，如下式

进行仿真建模：起点为图4（a）左下角的红点，终点为右上角的红点，4个黑色的点为障碍物。斥力势场的建模如图 4（b）所示，4个不可到达点分别产生势能很高的区域，每个障碍点的作用距离有限。引力势场的建模如图 4（c）所示，相对较为平整，作用于整个区域。引力势场与斥力势场的叠加势场如图4（d）所示，红线为AUV从起点出发到达终点的轨迹，避开了障碍物，顺利平滑地抵达了终点。

图4 传统人工势场法示意图Fig.4 Schematic diagram for the traditional artificial potential field method

传统的人工势场法只考虑了避障问题，并没有充分考虑到定位精度问题，因此在执行定位任务时，往往会导致航行到定位误差大的区域，使得任务执行效率低。因此，在航路规划中，本文引入定位精度势场，将定位精度势场与人工势场结合，在避障的同时，提高定位精度。

3.2 结合定位精度势场的人工势场

在传统人工势场的基础上，叠加根据定位误差所建立的定位精度势场，避免AUV驶入定位误差突然增大的区域。定位精度势场的建立，首先对定位精度的分布进行分析。基于TDOA法，根据2.2节所提到的偏微分法求 HDOP的方法，某解算点的HDOP值的求解需要在误差已知的情况下输入 4个坐标，即黑匣子的位置坐标、2个历史定位解算点坐标以及当前定位解算点的坐标。历史解算节点的选取采取以下措施：目前航路中定位解算点的个数不足1 000时，选取出发点作为历史解算点，以增大解算点之间的距离，有利于拉开解算尺度，减小误差；当解算点的个数大于 1 000时，选取当前位置的前1 000个位置作为第1个历史定位解算点，第500个位置作为第2个历史定位解算节点，在每个解算点之间的距离得到保证的前提下，选取较为接近黑匣子的解算点，有利于减小由距离产生的解算误差。

仿真的变量误差与表1相同，分析定位精度势场时，当前位置节点遍历整片待计算的区域，所得定位精度分布如图5（a）所示，第1解算点和第2解算点为历史解算点，图中可以看出当前解算节点不能位于图中的黄色低定位精度区域。

图5 定位精度势场示意图Fig.5 Schematic diagram for positioning accuracy potential field

以定位精度为基础，将 HDOP看作势能值，如图 5（b）所示，黄色区域势能较高，蓝色区域势能低，在定位精度势场作用下产生的模拟力将会促使AUV驶向使目标定位精度更高的点。定位精度势场产生的斥力表示为

式中：FHDOP,X(x,y)为误差梯度力在X方向上的分量；FHDOP,Y(x,y)为误差梯度力在Y方向上的分量；HDOP(x,y)为在位置(x,y)处的误差值。

将引力、斥力、误差梯度力做矢量叠加作为合力，引导AUV前进。引力场和斥力场是恒定的，如图6（a）所示，定位精度势能场随着AUV的位置变动而发生变化。选取一个特定点作为第1解算点的叠加势能场如图 6（b）所示，特定点的位置即图中标示出的“当前位置”，图中红线为模拟行驶的路径。

图6 叠加定位精度势场后的势场示意图Fig.6 Schematic diagram for the total potential field after adding positioning accuracy potential field

由于实际AUV定位载荷（通常是安装在腹鳍的水听器阵列）有一定的指向性，因此，精确定位时期，加入核力势场。精确定位阶段的目的是精确测量目标位置，在精确定位阶段指引AUV前进时，尽可能保证轨迹为一个圆，同时还要避开可能出现的障碍物，因此引入类似核力的力场建模。将核力函数设为

式中：η为核力尺度因子；ρ为势场中某点到障碍物中心点的距离；0ρ为核力的稳定半径，核力的分布如下

核力势场与引力势场的叠加势场如图 7（a）所示，在叠加了核力后的航路如图7（b）所示。

图7 叠加核力势场后的总势场示意图Fig.7 Schematic diagram for the total potential field after adding nuclear force potential field

核力的建模非常适用于AUV航路规划的精确定位阶段，在设定好圆形航路半径的情形下，AUV距目标的距离大于该半径，指引AUV靠近目标；反之，指引AUV适当远离目标，始终保持在圆形轨迹上。结合其他力场，在精确定位阶段也能实现避障的基本操作。

4 实验

4.1 仿真实验

仿真过程中，对AUV 进行航路规划时，在引力势场和斥力势场中加入定位精度势场，以及在精确定位阶段加入核力势场，各个势场力的矢量指引AUV行进。从图8（a）中可以看出，规划航迹后的AUV避开了所有障碍物。仿真实验中，令目标实际坐标为[0，0，50] m，根据图 8（a）中的航迹，对目标位置坐标进行定位解算，测量噪声参数与表 1相同。为了有一定的对比性，同时根据只具备避障能力的AUV航迹对目标位置坐标进行定位解算。

图8 仿真结果Fig.8 Simulation results

图8（b）可以看出，规划后的航迹比只具备避障能力的航迹定位结果更加精确。从图 8（c）中可以看出，规划后的航迹定位误差小于只具备避障能力的航迹定位误差，虽然规划后的航迹因为受到实时航路的影响，前期定位结果误差较大，但是在后期的精确定位阶段，规划后的航迹有很高的定位精度，可以更加精确的实现对目标位置坐标的定位。在精确定位阶段，规划后的航迹平均定位误差为2.26 m，只具备避障能力的航迹定位误差为4.29 m。

4.2 湖试实验

为了进一步验证该方法的有效性，在松花湖进行了实际实验。在湖试中，用水面船代替AUV。目标固定在湖底，通过压力传感器测量目标的深度为62 m。水听器阵列固定在船上，其位置可由GPS测量，声速为1 486 m/s。图9中绿色的点为障碍物，黑色的点为解算出的黑匣子位置坐标。

图9 航路规划及定位结果图Fig.9 Result for path planning and underwater positioning

从船的航迹来看，在障碍物斥力的作用下，其避开了所有障碍点，并逐步定位到黑匣子所在位置，完成了对船进行航路规划的要求。虽然在船航行前期定位误差较大，但在精确定位阶段，定位精度高，平均定位误差在2.45 m，满足水下定位任务对高定位精度的要求。实验结果与理论分析吻合，证明了该方法的有效性。