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

孙思博; 秦莎莎; 王庸; 明瑞和; 王勇; SUN Sibo; QIN Shasha; WANG Yong; MING Ruihe; WANG Yong

引 en

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

孙思博¹

机构：

1. 哈尔滨工程大学水声工程学院,黑龙江哈尔滨 150001

×
，秦莎莎¹

机构：

1. 哈尔滨工程大学水声工程学院,黑龙江哈尔滨 150001

×
，王庸¹

机构：

1. 哈尔滨工程大学水声工程学院,黑龙江哈尔滨 150001

×
，明瑞和¹

机构：

1. 哈尔滨工程大学水声工程学院,黑龙江哈尔滨 150001

×
，王勇²

机构：

2. 哈尔滨工业大学电子与信息工程学院,黑龙江哈尔滨 150001

×

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

Path Planning for Autonomous Underwater Platform Aiming at Improving the Positioning Accuracy

SUN Sibo¹

Affiliation：

1. College of Underwater Acoustic Engineering,Harbin Engineering University, Harbin 150001 ,China

×
， QIN Shasha¹

Affiliation：

1. College of Underwater Acoustic Engineering,Harbin Engineering University, Harbin 150001 ,China

×
， WANG Yong¹

Affiliation：

1. College of Underwater Acoustic Engineering,Harbin Engineering University, Harbin 150001 ,China

×
， MING Ruihe¹

Affiliation：

1. College of Underwater Acoustic Engineering,Harbin Engineering University, Harbin 150001 ,China

×
， #@@#WANG Yong²

Affiliation：

2. School of Electronic and Information Engineering, Harbin Engineering University,Harbin 150001 ,China

×

1. College of Underwater Acoustic Engineering,Harbin Engineering University, Harbin 150001 ,China；
2. School of Electronic and Information Engineering, Harbin Engineering University,Harbin 150001 ,China；

作者简介:

孙思博(1987-),男,博士后,副教授,主要从事水声定位、水声成像研究。

中图分类号:TP2

文献标识码:A

文章编号:2096-5753(2021)06-0475-10

DOI:10.19838/j.issn.2096-5753.2021.06.007

全文
评论
参考文献
出版信息

参考文献 1

贾宇，艾维.无人艇智能航路规划系统研究[J].舰船科学技术，2021，43（5）：115-119.

查找原文

参考文献 2

臧传斌.未知周期声信标定位方法研究及软件设计[D].哈尔滨：哈尔滨工程大学，2020.

查找原文

参考文献 3

徐英杰，郭福成.基于高斯–牛顿迭代的三星时差定位融合算法[J].航天电子对抗，2020，36（5）：14-17.陈娴静，王刚.修正极坐标系中基于AOA的近似无偏定位方法[J].传感技术学报，2020，33（10）：1467-1474.

查找原文

参考文献 4

MUCIENTES M，MORENO D L，BUGARÍN A，et al.Design of a fuzzy controller in mobile robotics using genetic algorithms[J].Applied Soft Computing Journal，2005，7（2）.

查找原文

参考文献 5

王金龙.无人艇航路规划的算法研究[D].吉林：吉林大学，2019.

查找原文

参考文献 6

裴玉.单平台观测水下目标定位算法设计与实现[D].哈尔滨：哈尔滨工程大学，2019.

查找原文

参考文献 7

黄土顺，李曦，梁国茂.纯角度无源定位及角观测量对精度的影响[J].甘肃科技，2013，29（6）：47-49.

查找原文

参考文献 8

庄晓东，孟庆春，高云，等.复杂环境中基于人工势场优化算法的最优路径规划[J].机器人，2003（6）：531-535.

查找原文

参考文献 9

张煌辉.基于动态人工势场的路径规划研究与应用[D].长沙：长沙理工大学，2010.

查找原文

目录contents

摘要Abstract
关键词Keywords
0 引言
1 水声定位方法与航路规划简介
1.1 水声定位
1.2 航路规划
2 TDOA定位法与精度分析
2.1 TDOA定位方法
2.2 TDOA法的定位精度分析
3 航路规划方法
3.1 传统人工势场法
3.2 结合定位精度势场的人工势场
4 实验
4.1 仿真实验
4.2 湖试实验
参考文献

摘要

航路规划指在有障碍物的环境中，为无人平台规划出无碰撞的最优路径。以水下定位为任务的航路规划为水下无人平台规划出高定位精度指标的最优路径，对于提高水下无人平台的工作效率至关重要。传统的水下无人平台航路规划主要基于人工势场法、蚁群法、遗传算法等，在这些算法中主要考虑的是避障问题，而没有充分考虑载荷任务，因此任务执行效率低。特别是对于水下定位任务，由于定位精度受到航路影响较大，因此需要对定位精度进行优化。针对这一问题，结合人工势场法与定位精度，提出了一种定位精度优化驱动的水下无人平台航路规划方法，实现了对避障能力与定位任务能力的兼顾，提高了基于水下无人平台的定位精度。

Abstract

Path planning provides the optimal collision free path for unmanned platform in the environment with obstacles. The path planning for underwater positioning gives the optimal path with the highest positioning accuracy，which is very important to improve the work efficiency of the unmanned underwater platform. The traditional path planning of unmanned underwater platform is mainly based on artificial potential field method，ant colony method，genetic algorithm，etc. In these algorithms，the obstacle avoidance problem is mainly considered， but the load task is not fully considered，so the task execution efficiency is low. Especially for underwater positioning tasks，since the positioning accuracy is greatly affected by the route，it is necessary to optimize the positioning accuracy. To solve this problem，combining the artificial potential field method and the localization precision analysis，this paper proposes an unmanned underwater platform path planning method driven by localization precision optimization，which realizes the consideration of both the obstacle avoidance ability and the localization task ability.

关键词

航路规划；人工势场法；水声定位； TDOA

Keywords

path planning ； artificial potential field ； underwater acoustic positioning ； TDOA

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显示了水下定位的几何结构，所定位目标位于海底，坐标表示为 $[x_{s}, y_{s}, z_{s}]$ ，目标深度可由海图测量。定位解算节点瞬时位置由GPS获得，表示为 $[x_{i}, y_{i}, z_{i}]$ ，主要求解目标的水平坐标 $x_{s}$ 和 $y_{s}$ 。
图1 水下定位过程中的几何配置
Fig.1 Geometrical configuration for the UWAL
令 t_s 为信号发射周期， t_a为信号初始发射时刻，第2个信号发射时刻为 $t_{a} + t_{s}, t_{a} + n t_{s}$ 为第 n 个信号的发送时刻。 t_i 为第 i 个解算节点的信号接收时刻，n_i为接收信号的周期号，c 为声速。d_i表示目标到第 i 个解算节点的直线距离， $d_{i} = \sqrt{{(x_{s} - x_{i})}^{2} + {(y_{s} - y_{i})}^{2} + {(z_{s} - z_{i})}^{2}}$ 。TDOA法定位时选取2个解算节点，其时间差为

t_{i} - t_{i - 1} = (t_{a} + n_{i} t_{s} + \frac{d_{i}}{c}) - (t_{a} + n_{i - 1} t_{s} + \frac{d_{i - 1}}{c})

(1)

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

\{\begin{matrix} f_{(T D O A, 1)} = c [t_{2} - t_{1} - (n_{2} - n_{1}) t_{s}] - (d_{2} - d_{1}) = 0 \\ f_{(T D O A, 2)} = c [t_{3} - t_{2} - (n_{3} - n_{2}) t_{s}] - (d_{3} - d_{2}) = 0 \end{matrix}

(2)

式中：距离 d_i 是待求量 x_s和 y_s的非线性函数，因此TDOA法的数学模型是非线性方程，可以采用牛顿迭代法进行求解。
2.2 TDOA法的定位精度分析
使用偏微分法分析TDOA法的定位精度，将各变量的误差看作相互独立的且均值为0的服从高斯分布的随机变量，引入HDOP（水平精度因子）表示定位精度，HDOP的数值越大，定位误差越大，其定位精度越差^[7]。式（3）中的 E^{[ ]}表示对变量求期望，HDOP可以表示为

H D O P = \sqrt{E {[{(\hat{x_{s}} - x_{s})}^{2} + \hat{y_{s}} - y_{s})}^{2}]}

(3)

式中： $[x_{s}, y_{s}]$ 表示目标的真实位置坐标； $[\hat{x_{s}}, \hat{y_{s}}]$ 表示目标的解算估计坐标。对TDOA法定位表达式各变量求偏微分，可以得到

\{\begin{matrix} \frac{\partial f (T D O A, 1)}{\partial x_{1}} d x_{1} + \frac{\partial f (T D O A, 1)}{\partial y_{1}} d y_{1} + \frac{\partial f (T D O A, 1)}{\partial z_{1}} d z_{1} + \frac{\partial f (T D O A, 1)}{\partial x_{2}} d x_{2} + \\ \frac{\partial f (T D O A, 1)}{\partial y_{2}} d y_{2} + \frac{\partial f (T D O A, 1)}{\partial z_{2}} d z_{2} + \frac{\partial f (T D O A, 1)}{\partial x_{3}} d x_{3} + \frac{\partial f (T D O A, 1)}{\partial y_{3}} d y_{3} + \\ \frac{\partial f (T D O A, 1)}{\partial z_{3}} d z_{3} + \frac{\partial f (T D O A, 1)}{\partial x_{s}} d x_{s} + \frac{\partial f (T D O A, 1)}{\partial y_{s}} d y_{s} + \frac{\partial f (T D O A, 1)}{\partial z_{s}} d z_{s} + \\ \frac{\partial f (T D O A, 1)}{\partial t_{1}} d t_{1} + \frac{\partial f (T D O A, 1)}{\partial t_{2}} d t_{2} + \frac{\partial f (T D O A, 1)}{\partial t_{3}} d t_{3} + \frac{\partial f (T D O A, 1)}{\partial c} d c = 0 \\ \frac{\partial f (T D O A, 2)}{\partial x_{1}} d x_{1} + \frac{\partial f (T D O A, 2)}{\partial y_{1}} d y_{1} + \frac{\partial f (T D O A, 2)}{\partial z_{1}} d z_{1} + \frac{\partial f (T D O A, 2)}{\partial x_{2}} d x_{2} + \\ \frac{\partial f (T D O A, 2)}{\partial y_{2}} d y_{2} + \frac{\partial f (T D O A, 2)}{\partial z_{2}} d z_{2} + \frac{\partial f (T D O A, 2)}{\partial x_{3}} d x_{3} + \frac{\partial f (T D O A, 2)}{\partial y_{3}} d y_{3} + \\ \frac{\partial f (T D O A, 2)}{\partial z_{3}} d z_{3} + \frac{\partial f (T D O A, 2)}{\partial x_{s}} d x_{s} + \frac{\partial f (T D O A, 2)}{\partial y_{s}} d y_{s} + \frac{\partial f (T D O A, 2)}{\partial z_{s}} d z_{s} + \\ \frac{\partial f (T D O A, 2)}{\partial t_{1}} d t_{1} + \frac{\partial f (T D O A, 2)}{\partial t_{2}} d t_{2} + \frac{\partial f (T D O A, 2)}{\partial t_{3}} d t_{3} + \frac{\partial f (T D O A, 2)}{\partial c} d c = 0 \end{matrix}

(4)

将各个偏导数写成矩阵形式
$\begin{matrix} M_{x} = [\begin{matrix} \frac{\partial f (T D O A, 1)}{\partial x_{s}} & \frac{\partial f (T D O A, 1)}{\partial y_{s}} \\ \frac{\partial f (T D O A, 2)}{\partial x_{s}} & \frac{\partial f (T D O A, 2)}{\partial y_{s}} \end{matrix}], M_{t} = [\begin{matrix} \frac{\partial f (T D O A, 1)}{\partial t_{1}} & \frac{\partial f (T D O A, 1)}{\partial t_{2}} & \frac{\partial f (T D O A, 1)}{\partial t_{3}} \\ \frac{\partial f (T D O A, 2)}{\partial t_{1}} & \frac{\partial f (T D O A, 2)}{\partial t_{2}} & \frac{\partial f (T D O A, 2)}{\partial t_{3}} \end{matrix}] \\ M_{c} = [\begin{matrix} \frac{\partial f (T D O A, 1)}{\partial c} \\ \frac{\partial f (T D O A, 2)}{\partial c} \end{matrix}], M_{d} = [\begin{matrix} \frac{\partial f (T D O A, 1)}{\partial z_{s}} & \frac{\partial f (T D O A, 1)}{\partial z_{1}} & \frac{\partial f (T D O A, 1)}{\partial z_{2}} & \frac{\partial f (T D O A, 1)}{\partial z_{3}} \\ \frac{\partial f (T D O A, 2)}{\partial z_{s}} & \frac{\partial f (T D O A, 2)}{\partial z_{1}} & \frac{\partial f (T D O A, 2)}{\partial z_{2}} & \frac{\partial f (T D O A, 2)}{\partial z_{3}} \end{matrix}] \end{matrix}$
$M_{I N S} = [\begin{matrix} \frac{\partial f (T D O A, 1)}{\partial x_{1}} \frac{\partial f (T D O A, 1)}{\partial y_{1}} \frac{\partial f (T D O A, 1)}{\partial x_{2}} \frac{\partial f (T D O A, 1)}{\partial y_{2}} \frac{\partial f (T D O A, 1)}{\partial x_{3}} \frac{\partial f (T D O A, 1)}{\partial y_{3}} \\ \frac{\partial f (T D O A, 2)}{\partial x_{1}} \frac{\partial f (T D O A, 2)}{\partial y_{1}} \frac{\partial f (T D O A, 2)}{\partial x_{2}} \frac{\partial f (T D O A, 2)}{\partial y_{2}} \frac{\partial f (T D O A, 2)}{\partial x_{3}} \frac{\partial f (T D O A, 2)}{\partial y_{3}} \end{matrix}]$
改写为矩阵形式

M_{d} [\begin{matrix} d z_{s} \\ d z_{1} \\ d z_{2} \\ d z_{3} \end{matrix}] + M_{I N S} [\begin{matrix} d x_{1} \\ d y_{1} \\ d x_{2} \\ d y_{2} \\ d x_{3} \\ d y_{3} \end{matrix}] + M_{x} [\begin{matrix} d x_{s} \\ d y_{s} \end{matrix}] + M_{t} [\begin{matrix} d t_{1} \\ d t_{2} \\ d t_{3} \end{matrix}] + M_{c} d_{c} = 0

(5)

将 $[\begin{matrix} d x_{s} \\ d y_{s} \end{matrix}]$ 放到左边，得到

[\begin{matrix} d x_{s} \\ d y_{s} \end{matrix}] = - M_{x}^{- 1} \{M_{d} [\begin{matrix} d z_{s} \\ d z_{1} \\ d z_{2} \\ d z_{3} \end{matrix}] + M_{I N S} [\begin{matrix} d x_{1} \\ d y_{1} \\ d x_{2} \\ d y_{2} \\ d x_{3} \\ d y_{3} \end{matrix}] + M_{t} [\begin{matrix} d t_{1} \\ d t_{2} \\ d t_{3} \end{matrix}] + M_{c} d_{c}\}

(6)

矩阵运算表达式为

\begin{matrix} D_{x} = M_{x}^{- 1} \cdot (M_{d} \cdot D_{d} \cdot M_{d}^{T} + M_{I N S} \cdot D_{I N S} \cdot M_{I N S}^{T} + \\ M_{t} \cdot D_{t} \cdot M_{t}^{T} + M_{c} \cdot D_{c} \cdot M_{c}^{T}) \cdot {(M_{x}^{- 1})}^{T} \end{matrix}

(7)

式中： $D_{d}, D_{I N S}, D_{t}, D_{c}$ 和 $M_{d}, M_{I N S}, M_{t}, M_{c}$ 分别是深度测量误差、解算节点坐标误差、到达时间误差、声速测量误差的协方差矩阵和其偏导数矩阵。各变量误差协方差矩阵通过对角阵表示为
$\begin{matrix} D_{d} = d i a g (σ_{d}^{2}, σ_{d}^{2}, σ_{d}^{2}, σ_{d}^{2}) \\ D_{I N S} = d i a g (σ_{I N S}^{2}, σ_{I N S}^{2}, σ_{I N S}^{2}, σ_{I N S}^{2}, σ_{I N S}^{2}, σ_{I N S}^{2}) \\ D_{t} = d i a g (σ_{t}^{2}, σ_{t}^{2}, σ_{t}^{2}) \\ D_{c} = d i a g (σ_{c}^{2}) \end{matrix}$
式中：diag() 为对角矩阵， $σ_{d}^{2}, σ_{I N S}^{2}, σ_{t}^{2}, σ_{c}^{2}$ 分别是深度测量、解算节点坐标、到达时间和声速测量的均方误差^[8]。HDOP表示为 $HDOP = \sqrt{D_{x} (1,1) + D_{x} (2,2)} 。$
根据以上公式，对TDOA法的性能进行仿真分析，仿真参数设置如表1所示。
表1 仿真参数和测量噪声参数
Table1 Simulation parameters and measurement noise parameters for the simulation
两组定位精度仿真结果如图2所示。
从图中可以看出，TDOA法在解算节点呈等腰三角形分布时有较好的定位精度，其中1km×1km正方形区域内的平均定位精度为4.3m。当定位节点从弧形轨迹上选取时，定位精度下降明显，定位精度高的区域存在断层，分别位于3个定位解算节点的上方和下方，区域内的平均定位精度为15.04m。因此，定位节点的选择对于TDOA法在进行定位解算时具有较大影响，选取节点时尽量选取一个圆形轨迹的内切等边三角形。
图2 TDOA法的定位精度分布
Fig.2 Positioning accuracy distribution for the TDOA algorithm
进行目标位置定位时，选取不同的航迹，对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]。通常将斥力函数设为

U_{rep} = \{\begin{matrix} \frac{1}{2} η {(\frac{1}{ρ} - \frac{1}{ρ_{0}})}^{c}, 若 ρ > ρ_{0} \\ Φ, 若 ρ \leq ρ_{0} \end{matrix}

(8)

式中： $η$ 为常数，为斥力尺度因子；c 为斥力次数，通常设为-2； $ρ$ 为势场中某点到障碍物中心点的距离； $ρ_{0}$ 为障碍物的影响半径，以障碍物中心点为原点， $ρ_{0}$ 为半径的圆内，其势场值为 $Φ$ ， $Φ$ 为一个极大的值；当势场中某点到障碍物中心点的距离大于 $ρ_{0}$ 时，该点的势场随着 $ρ$ 的增大而减小。
势场中的斥力为斥力场的梯度，斥力是矢量，即 $F_{тер} = (F_{x}, F_{y})$

F_{rep} = - \nabla U_{rep} = \{\begin{matrix} - η {(\frac{1}{ρ} - \frac{1}{ρ_{0}})}^{- 3} \frac{1}{ρ^{2}}, 若 ρ > ρ_{0} \\ 0, 若 ρ \leq ρ_{0} \end{matrix}

(9)

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

U_{a t t} = ξ ρ

(10)

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

F_{att} = ξ

(11)

进行仿真建模：起点为图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驶向使目标定位精度更高的点。定位精度势场产生的斥力表示为
$\begin{matrix} F_{H D O P, X (x, y)} = \frac{\partial H D O P}{\partial x} \\ = \underset{Δ x \to 0}{l i m} \frac{H D O P (x + Δ x, y) - H D O P (x, y)}{Δ x} \end{matrix}$

\begin{matrix} F_{H D O P, Y (x, y)} = \frac{\partial H D O P}{\partial y} \\ = \underset{Δ y \to 0}{l i m} \frac{H D O P (x, y + Δ y) - H D O P (x, y)}{Δ y} \end{matrix}

(12)

式中： $F_{H D O P, X (x, y)}$ 为误差梯度力在 X 方向上的分量； $F_{H D O P, Y (x, y)}$ 为误差梯度力在Y方向上的分量； $H D O P (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前进时，尽可能保证轨迹为一个圆，同时还要避开可能出现的障碍物，因此引入类似核力的力场建模。将核力函数设为

U_{ato} = \{\begin{matrix} η l n (ρ - ρ_{0}), 若 ρ > ρ_{0} \\ η l n (ρ_{0} - ρ), 若 ρ < ρ_{0} \end{matrix}

(13)

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

F_{ato} = \{\begin{matrix} η (\frac{1}{ρ - ρ_{0}}), 若 ρ > ρ_{0} \\ η (\frac{1}{ρ_{0} - ρ}), 若 ρ < ρ_{0} \\ 0, 若 ρ = ρ_{0} \end{matrix}

(14)

核力势场与引力势场的叠加势场如图7（a）所示，在叠加了核力后的航路如图7（b）所示。
核力的建模非常适用于AUV航路规划的精确定位阶段，在设定好圆形航路半径的情形下，AUV距目标的距离大于该半径，指引AUV靠近目标；反之，指引AUV适当远离目标，始终保持在圆形轨迹上。结合其他力场，在精确定位阶段也能实现避障的基本操作。
图7 叠加核力势场后的总势场示意图
Fig.7 Schematic diagram for the total potential field after adding nuclear force potential field
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.26m，只具备避障能力的航迹定位误差为4.29m。
4.2 湖试实验
为了进一步验证该方法的有效性，在松花湖进行了实际实验。在湖试中，用水面船代替AUV。目标固定在湖底，通过压力传感器测量目标的深度为62m。水听器阵列固定在船上，其位置可由GPS测量，声速为1 486m/s。图9中绿色的点为障碍物，黑色的点为解算出的黑匣子位置坐标。
从船的航迹来看，在障碍物斥力的作用下，其避开了所有障碍点，并逐步定位到黑匣子所在位置，完成了对船进行航路规划的要求。虽然在船航行前期定位误差较大，但在精确定位阶段，定位精度高，平均定位误差在2.45m，满足水下定位任务对高定位精度的要求。实验结果与理论分析吻合，证明了该方法的有效性。
图9 航路规划及定位结果图
Fig.9 Result for path planning and underwater positioning
参考文献
- [1] 贾宇，艾维.无人艇智能航路规划系统研究[J].舰船科学技术，2021，43（5）：115-119.
- [2] 臧传斌.未知周期声信标定位方法研究及软件设计[D].哈尔滨：哈尔滨工程大学，2020.
- [3] 徐英杰，郭福成.基于高斯–牛顿迭代的三星时差定位融合算法[J].航天电子对抗，2020，36（5）：14-17.陈娴静，王刚.修正极坐标系中基于AOA的近似无偏定位方法[J].传感技术学报，2020，33（10）：1467-1474.
- [4] MUCIENTES M，MORENO D L，BUGARÍN A，et al.Design of a fuzzy controller in mobile robotics using genetic algorithms[J].Applied Soft Computing Journal，2005，7（2）.
- [5] 王金龙.无人艇航路规划的算法研究[D].吉林：吉林大学，2019.
- [6] 裴玉.单平台观测水下目标定位算法设计与实现[D].哈尔滨：哈尔滨工程大学，2019.
- [7] 黄土顺，李曦，梁国茂.纯角度无源定位及角观测量对精度的影响[J].甘肃科技，2013，29（6）：47-49.
- [8] 庄晓东，孟庆春，高云，等.复杂环境中基于人工势场优化算法的最优路径规划[J].机器人，2003（6）：531-535.
- [9] 张煌辉.基于动态人工势场的路径规划研究与应用[D].长沙：长沙理工大学，2010.

基本信息

中图分类号: TP2
文献标识码: A
DOI: 10.19838/j.issn.2096-5753.2021.06.007
文章编号: 2096-5753(2021)06-0475-10

基金信息

引用信息

稿件历史

收稿日期: 2021-10-15

参考文献

[1] 贾宇，艾维.无人艇智能航路规划系统研究[J].舰船科学技术，2021，43（5）：115-119.
[2] 臧传斌.未知周期声信标定位方法研究及软件设计[D].哈尔滨：哈尔滨工程大学，2020.
[3] 徐英杰，郭福成.基于高斯–牛顿迭代的三星时差定位融合算法[J].航天电子对抗，2020，36（5）：14-17.陈娴静，王刚.修正极坐标系中基于AOA的近似无偏定位方法[J].传感技术学报，2020，33（10）：1467-1474.
[4] MUCIENTES M，MORENO D L，BUGARÍN A，et al.Design of a fuzzy controller in mobile robotics using genetic algorithms[J].Applied Soft Computing Journal，2005，7（2）.
[5] 王金龙.无人艇航路规划的算法研究[D].吉林：吉林大学，2019.
[6] 裴玉.单平台观测水下目标定位算法设计与实现[D].哈尔滨：哈尔滨工程大学，2019.
[7] 黄土顺，李曦，梁国茂.纯角度无源定位及角观测量对精度的影响[J].甘肃科技，2013，29（6）：47-49.
[8] 庄晓东，孟庆春，高云，等.复杂环境中基于人工势场优化算法的最优路径规划[J].机器人，2003（6）：531-535.
[9] 张煌辉.基于动态人工势场的路径规划研究与应用[D].长沙：长沙理工大学，2010.

首页

本刊简介

编委会

收录情况

期刊订阅

征稿简则

联系我们

分享给微信好友或者朋友圈

使用微信“扫一扫”功能。

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

Path Planning for Autonomous Underwater Platform Aiming at Improving the Positioning Accuracy

摘要

Abstract

关键词

Keywords

0 引言

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

1.1 水声定位

1.2 航路规划

2 TDOA定位法与精度分析

2.1 TDOA定位方法

(1)

(2)

2.2 TDOA法的定位精度分析

(3)

(4)

(5)

(6)

(7)

3 航路规划方法

3.1 传统人工势场法

(8)

(9)

(10)

(11)

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

(12)

(13)

(14)

4 实验

4.1 仿真实验

4.2 湖试实验

参考文献

基本信息

基金信息

引用信息

稿件历史

参考文献