CN117007051A - 一种基于改进蚁群算法的auv高精度水声定位航路规划方法 - Google Patents

一种基于改进蚁群算法的auv高精度水声定位航路规划方法 Download PDF

Info

Publication number
CN117007051A
CN117007051A CN202310946932.4A CN202310946932A CN117007051A CN 117007051 A CN117007051 A CN 117007051A CN 202310946932 A CN202310946932 A CN 202310946932A CN 117007051 A CN117007051 A CN 117007051A
Authority
CN
China
Prior art keywords
auv
positioning
path
grid
ant
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310946932.4A
Other languages
English (en)
Inventor
刘翰富
商志刚
张佳琨
赵梦扬
刘凇佐
周锋
马璐
聂东虎
孙宗鑫
乔钢
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202310946932.4A priority Critical patent/CN117007051A/zh
Publication of CN117007051A publication Critical patent/CN117007051A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/004—Artificial life, i.e. computing arrangements simulating life
    • G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种基于改进蚁群算法的自主水下航行器高精度水声定位航路规划方法。根据预设终点，使用传统蚁群算法驱动AUV行驶，AUV行驶过程中不断接收水下目标发射的声信号，接收一定周期的声信号后，解算目标位置，预估目标具体所在栅格；根据解算估计的目标位置使用Bresenham画圆算法计算得到以目标为圆心的圆形栅格序列，将预设的AUV终点改为计算的圆形栅格序列；计算AUV路径节点的定位精度值，AUV到达圆形路径的终点后，计算AUV在整条路径中的平均定位精度值，以平均定位精度值更新信息素，将定位精度启发因子引入转移概率中，对AUV进行新一轮的路径规划。本发明能解决使用传统蚁群算法为自主水下航行器AUV规划航路时，该航路的定位精度低，定位误差较大，AUV容易驶过目标，甚至导致目标被遗漏的问题。本发明可用于打捞水下黑匣子、对水下非合作目标进行侦察、监视等。

Description

一种基于改进蚁群算法的AUV高精度水声定位航路规划方法
技术领域
本发明属于技术领域，具体涉及一种基于改进蚁群算法的AUV高精度水声定位航路规划方法。
背景技术
蚁群算法作为一种全局航路规划方法，在使用栅格法对自主水下航行器AUV所处环境信息进行建模后，可以为自主水下航行器AUV规划一条无碰撞的路径。当使用自主水下航行器AUV对水下目标进行定位时，传统的蚁群算法以规划路径无碰撞和路径长度最小化为目的，没有充分考虑自主水下航行器AUV所执行任务的特殊性，若自主水下航行器AUV行驶到误差较大的区域，会造成解算估计的目标位置误差过大，致使自主水下航行器AUV驶过目标，甚至会导致目标被遗漏。因此传统的蚁群算法不适用于作为自主水下航行器AUV高精度水声定位的航路规划方法。
发明内容
本发明提供了一种基于改进蚁群算法的AUV高精度水声定位航路规划方法，针对执行水下目标定位的自主水下航行器AUV，提高了自主水下航行器AUV行驶途中的定位精度，能够为自主水下航行器AUV提供一条既能满足避障要求，又具有高定位精度的航路，提高了自主水下航行器AUV的载荷效率。
一种基于改进蚁群算法的AUV高精度水声定位航路规划方法，所述航路规划方法包括以下步骤：
步骤一：根据预设终点，使用传统蚁群算法驱动自主水下航行器AUV行驶，自主水下航行器AUV行驶过程中不断接收水下目标发射的声信号，接收一定周期的声信号后，解算目标位置，预估目标具体所在栅格；
步骤二：根据解算估计的目标位置使用Bresenham画圆算法计算得到以目标为圆心的圆形栅格序列，将预设的自主水下航行器AUV终点改为计算的圆形栅格序列；
步骤三：计算自主水下航行器AUV路径节点的定位精度值，自主水下航行器AUV到达圆形路径的终点后，计算自主水下航行器AUV在整条路径中的平均定位精度值，以平均定位精度值更新信息素，将定位精度启发因子引入转移概率中，对自主水下航行器AUV进行新一轮的路径规划。
进一步的，所述步骤一的具体过程为：
令预设的自主水下航行器AUV终点位置为Xe，Xe＝[xe,ye,ze]，自主水下航行器AUV航行过程中自身位置表示为X＝[x,y,z]，水下目标真实位置为Xt＝[xt,yt,zt]，令自主水下航行器AUV深度固定不变，将周围环境分割成网格后，用坐标法或编号法对栅格地图进行表示；编号与坐标点一一对应，令第x行、第y列栅格坐标表示为[x,y]m，对应的栅格编号表示为s，每个栅格的边长表示为a；坐标[x,y]与栅格编号s的关系表示为：
式中，mod为取余运算，col为栅格地图的列数，表示向上取整。
进一步的，假设蚂蚁的数量为m，蚂蚁k根据各个相邻栅格中的信息素含量决定选择前进的路径方向；表示t时刻蚂蚁k从路径节点i转移到路径节点j的概率：
式中，τij(t)为t时刻路径节点i到路径节点j的信息素含量，初始时刻τij(0)＝c(c为常数)，α为信息素启发因子；ηij(t)为启发信息，ηij(t)＝1/dij，
其中：dij表示路径节点i到路径节点j的距离，β为期望度启发因子；allowedk表示蚂蚁k能够到达的下一个路径节点的集合；
蚂蚁结束一轮觅食后，对所有路径点上的信息素进行更新；更新信息素的公式表示如下：
τij(t+n)＝(1-ρ)τij(t)+Δτij(t) (3)
式中：Δτij(t)为一轮觅食结束后，蚁群在路径节点i到路径节点j分泌的总的信息素增量；为蚂蚁k在路径节点i到路径节点j上分泌的信息素含量；ρ为时间段(t,t+n)间的信息素挥发系数，ρ的取值范围为(0,1)；
信息素更新模型选用蚁周模型，蚁周模型中，蚂蚁k在路径点节点i到路径节点j上分泌的信息素含量表示为：
式中，Q为蚂蚁k在本轮觅食结束后分泌的信息素总量，Lk为蚂蚁k所经过路径的总长度；
每轮蚁群出动觅食，信息素增量和转移概率不断更新，多次迭代后，得到一条从起点到终点的最短或次短路径；自主水下航行器AUV沿着此条路径航行，航行过程中不断接收目标发射的声信号，令声信号到达时间为ti，自主水下航行器AUV接收声信号时所处位置为定位节点，令定位节点的位置表示为Xi，Xi＝[xi,yi,zi]，选取两个定位节点X1、X2，此时时延差信息表示为t2-t1，基于信号到达时延差的定位方法的定位模型表示为c为水中声速；
使用TDOA定位方法建立如下定位解算方程：
自主水下航行器AUV接收一定周期声信号后，根据获得的时延差信息建立定位解算方程组，使用牛顿迭代法求解该方程组，估计得出目标位置，解算得到的目标位置表示为
进一步的，所述步骤二的具体过程为：
令圆形序列的圆心位于原点，半径为R，令函数F(x,y)＝x2+y2-R2，使用中点Bresenham画圆算法确定从点(0，R)到点的八分之一圆型序列，若当前栅格为P(xi,yi)，则下一个栅格为正右方的栅格Pr(xi+1,yi)或者正下方的栅格Pd(xi+1,yi-1)；使用中点Bresenham画圆算法确定具体选哪一个栅格；令M为Pr和Pd的中点，即有M(xi+1,yi-0.5)，令vi为F(xM,yM)的值，则有：
vi＝F(xM,yM)＝F(xi+1,yi-0.5)＝(xi+1)2+(yi-0.5)2-R2 (7)
若vi＜0，取Pr(xi+1,yi)为下一个栅格，继续选择下一个的栅格，沿正右方向，递推式为：
若vi≥0，取Pd(xi+1,yi-1)为下一个栅格，继续选择下一个栅格，沿右下方向，递推式为：
第一象限上顺时针八分之一圆的第一个栅格为P0(0,R)，则判别式的初始值为：
为了避免小数运算，用di-0.25代替di，此时d0＝1-R,判别式为di＜-0.25；
进一步的，采用中点Bresenham画圆算法在栅格地图中得到八分之一圆，根据圆关于x＝0，y＝0，x＝y和x＝-y四条线轴对称，进而得到整个圆的栅格序列；当圆心不位于原点时，首先假设圆心位于原点，使用中点Bresenham画圆算法在栅格地图中得到八分之一圆，后续的坐标变换公式根据圆心位置发生变化；令圆心坐标为[xc,yc]，令dc＝yc-xc，则坐标变换公式变为：
使用中点Bresenham画圆算法得到以目标为圆心的栅格序列后，若圆形序列的栅格中包含障碍物，则将包含障碍物的栅格从圆形栅格中去除；将预设的自主水下航行器AUV终点改为步骤二中得到的圆形栅格序列。
进一步的，所述步骤三的具体过程为：
将自主水下航行器AUV终点改为圆形栅格序列后，使用蚁群算法规划自主水下航行器AUV航路时，计算自主水下航行器AUV路径节点的定位精度值；HDOP的定义如下式：
使用偏微分法分析TDOA定位方法的定位精度，令各变量的误差是随机的，均值为0的且各变量之间相互独立；对使用TDOA定位方法建立的定位解算方程求偏导，可得：
将上式写成矩阵形式，有
变换上式矩阵形式，可以得到
式中Dd＝diag(σd 2,σd 2,σd 2,σd 2)，Di＝diag(σi 2,σi 2)(i∈[1,2,3])，Dt＝diag(σt 2,σt 2,σt 2)，Dc＝diag(σc 2)，diag()为对角矩阵,σd 2，σi 2(i∈[1,2,3])，σt 2，σc 2为深度、定位节点坐标、声信号到达时间和声速的均方误差；Md，Mi，Mt(i∈[1,2,3])，Mc为各个变量的偏导数矩阵；HDOP为矩阵Dx的迹，即
将预设的自主水下航行器AUV终点改为计算的圆形栅格序列，计算自主水下航行器AUV路径节点的HDOP值，自主水下航行器AUV到达圆形路径的终点后，计算自主水下航行器AUV在整条路径中的平均定位精度值，以平均定位精度值更新信息素，将定位精度启发因子引入转移概率中，对自主水下航行器AUV进行新一轮的路径规划。
进一步的，令自主水下航行器AUV当前位置为第三定位节点，并在自主水下航行器AUV历史路径中选取两个定位节点，根据选取的三个定位节点，计算由步骤二估计得到的目标位置处的定位精度值；
将定位精度作为评价因子引入转移概率中，并以定位精度值为衡量标准改变信息素增量；计算方法如式(4-8)所示，引入定位精度后的转移概率为：
式中：hij(t)为t时刻路径节点i到路径节点j的定位精度信息，hij(t)＝1/hdopij；其中：hdopij为蚂蚁从路径节点i转移到路径节点j的定位精度值，hdopij的值为以路径节点j为最后一个解算节点，结合自主水下航行器AUV的历史路径，选择两个历史定位节点后得出的定位精度值，r为定位精度启发因子，r越大，蚂蚁越趋向于选择定位精度大的路径；
进一步的，更新信息素增量Δτij(t)时，以第k只蚂蚁整条路径的平均定位精度值为衡量标准，令HPk为第k只蚂蚁所经过路径的平均定位精度值；Totalk为第k只蚂蚁在整条路径中进行定位解算的总次数；平均定位精度值表示为：
式中：E为路径终点，hdopij为第k只蚂蚁从路径节点i转移到路径节点j的定位精度值；采用每一轮每一只蚂蚁觅食途经的整条路径平均定位精度更新信息素增量；信息素增量表示为：
将定位精度分别引入蚁群算法的转移概率和信息素增量中，使蚁群算法在每次迭代寻找最优路径时，趋向于选择路径中定位精度较高的区域，经过多次迭代后，为自主水下航行器AUV规划既满足避障要求，又具有高定位精度的航路。
发明效果
当使用自主水下航行器AUV对水下目标进行定位时，采用本发明对自主水下航行器AUV进行航路规划，在满足避障要求的情况下，可以解决由于基于传统蚁群算法规划的自主水下航行器AUV航路定位精度较低，导致自主水下航行器AUV驶过目标，甚至遗漏目标的问题。使用本发明对自主水下航行器AUV进行航路规划，提高了水下目标的定位精度，定位后期自主水下航行器AUV围绕水下目标行驶，得到较为稳定的误差较低的定位结果，避免目标被遗漏，同时提高了自主水下航行器AUV的载荷效率。
附图说明
图1为自主水下航行器AUV对水下目标进行定位的场景态势图。
图2为本发明的流程图。
图3为本发明实施算例规划的路径图。
图4为本发明实施算例的定位误差图。
具体实施方式
自主水下航行器AUV对水下目标进行定位的场景态势图如图1所示，使用航路规划方法规划自主水下航行器AUV的航行路径，自主水下航行器AUV在行驶过程中不断接收水下目标发射的声信号。
具体实施方式一：一种基于改进蚁群算法的自主水下航行器AUV高精度水声定位航路规划包括以下步骤：
步骤一：首先使用栅格法对自主水下航行器AUV所处环境进行建模，根据预设终点，在自主水下航行器AUV入水后，使用传统蚁群算法驱动自主水下航行器AUV行驶，自主水下航行器AUV行驶过程中不断接收水下目标发射的声信号，接收一定周期的声信号后，使用牛顿迭代法解算目标位置，预估目标具体所在栅格；
步骤二：根据解算估计的目标位置使用Bresenham画圆算法计算得到以目标为圆心的圆形栅格序列，将预设的自主水下航行器AUV终点改为计算得到的圆形栅格序列；
步骤三：计算自主水下航行器AUV路径节点的定位精度值，自主水下航行器AUV到达圆形路径的终点后，计算自主水下航行器AUV在整条路径中的平均定位精度值，以平均定位精度值更新信息素，将定位精度启发因子引入转移概率中。改进的蚁群算法在传统蚁群算法的基础上，自主水下航行器AUV路径终点为计算得到的圆形栅格序列，并以新的转移概率和信息素增量公式，进行每一轮的路径规划。
具体实施方式二：所述步骤一的具体过程为：
令预设的自主水下航行器AUV终点位置为Xe(Xe＝[xe,ye,ze])，自主水下航行器AUV航行过程中自身位置表示为X＝[x,y,z]，水下目标真实位置为Xt＝[xt,yt,zt]，令自主水下航行器AUV深度固定不变，本发明研究二维空间自主水下航行器AUV的路径规划。蚁群算法流程进行前，使用栅格法对自主水下航行器AUV所处环境进行环境建模，将周围环境分割成网格后，用坐标法或编号法对栅格地图进行表示。编号法中，左上角第一个栅格编号为1，编号沿着从左到右，从上到下的顺序依次递增。坐标法中，栅格地图左下角为原点，水平向右为x轴正方向，竖直向上为y轴正方向。编号与坐标点一一对应，令第x行、第y列栅格坐标表示为[x,y]m，对应的栅格编号表示为s，每个栅格的边长表示为a。坐标[x,y]与栅格编号s的关系表示为：
式中，mod为取余运算，col为栅格地图的列数，表示向上取整。
假设蚂蚁的数量为m，蚂蚁k根据各个相邻栅格中的信息素含量决定选择前进的路径方向。表示t时刻蚂蚁k从路径节点i转移到路径节点j的概率：
式中，τij(t)为t时刻路径节点i到路径节点j的信息素含量，初始时刻τij(0)＝c(c为常数)，α为信息素启发因子。ηij(t)为启发信息，ηij(t)＝1/dij，其中：dij表示路径节点i到路径节点j的距离，β为期望度启发因子。allowedk表示蚂蚁k能够到达的下一个路径节点的集合。
蚂蚁结束一轮觅食后，对所有路径点上的信息素进行更新。更新信息素的公式表示如下：
τij(t+n)＝(1-ρ)τij(t)+Δτij(t) (3)
式中：Δτij(t)为一轮觅食结束后，蚁群在路径节点i到路径节点j分泌的总的信息素增量。为蚂蚁k在路径节点i到路径节点j上分泌的信息素含量。ρ为时间段(t,t+n)间的信息素挥发系数，ρ的取值范围为(0,1)。
信息素更新模型选用蚁周模型，蚁周模型中，蚂蚁k在路径点节点i到路径节点j上分泌的信息素含量表示为：
式中，Q为蚂蚁k在本轮觅食结束后分泌的信息素总量，Lk为蚂蚁k所经过路径的总长度。
每轮蚁群出动觅食，信息素增量和转移概率不断更新，多次迭代后，可以得到一条从起点到终点的最短或次短路径。自主水下航行器AUV沿着此条路径航行，航行过程中不断接收目标发射的声信号，令声信号到达时间为ti，自主水下航行器AUV接收声信号时所处位置为定位节点，令定位节点的位置表示为Xi，(Xi＝[xi,yi,zi])，选取两个定位节点X1，X2，此时时延差信息表示为t2-t1，基于信号到达时延差的定位方法的定位模型表示为c为水中声速。
使用基于信号到达时延差的定位方法(TDOA定位方法)建立如下定位解算方程：
自主水下航行器AUV接收一定周期声信号后，根据获得的时延差信息建立定位解算方程组，使用牛顿迭代法求解该方程组，估计得出目标位置，解算得到的目标位置表示为
具体实施方案三：所述步骤二的具体过程为：
本发明所阐述的中点Bresenham画圆算法，令圆形序列的圆心位于原点，半径为R，令函数F(x,y)＝x2+y2-R2，使用中点Bresenham画圆算法确定从点(0，R)到点的八分之一圆型序列，若当前栅格为P(xi,yi)，则下一个栅格为正右方的栅格Pr(xi+1,yi)或者正下方的栅格Pd(xi+1,yi-1)。使用中点Bresenham画圆算法确定具体选哪一个栅格。令M为Pr和Pd的中点，即有M(xi+1,yi-0.5)，令vi为F(xM,yM)的值，则有：
vi＝F(xM,yM)＝F(xi+1,yi-0.5)＝(xi+1)2+(yi-0.5)2-R2 (7)
若vi＜0，取Pr(xi+1,yi)为下一个栅格，继续选择下一个的栅格，沿正右方向，递推式为：
若vi≥0，取Pd(xi+1,yi-1)为下一个栅格，继续选择下一个栅格，沿右下方向，递推式为：
第一象限上顺时针八分之一圆的第一个栅格为P0(0,R)，则判别式的初始值为：
为了避免小数运算，用di-0.25代替di，此时d0＝1-R,判别式为di＜-0.25。
采用中点Bresenham画圆算法在栅格地图中得到八分之一圆，根据圆关于x＝0，y＝0，x＝y和x＝-y四条线轴对称，进而得到整个圆的栅格序列。实际场景中，圆形序列的圆心可处于任意位置，一般不会位于原点。因此当圆心不位于原点时，首先假设圆心位于原点，使用中点Bresenham画圆算法在栅格地图中得到八分之一圆，后续的坐标变换公式根据圆心位置发生变化。令圆心坐标为[xc,yc]，令dc＝yc-xc，则坐标变换公式变为：
使用中点Bresenham画圆算法得到以目标为圆心的栅格序列后，若圆形序列的栅格中包含障碍物，则将包含障碍物的栅格从圆形栅格中去除。将预设的自主水下航行器AUV终点改为步骤二中得到的圆形栅格序列。
具体实施方式四：所述步骤三的具体过程为：
在步骤二中，将自主水下航行器AUV终点改为圆形栅格序列后，使用蚁群算法规划自主水下航行器AUV航路时，计算自主水下航行器AUV路径节点的定位精度值。一般用水平精度因子(Horizontal Dilution ofPrecision，简记为HDOP)衡量定位精度。HDOP的定义如下式：
使用偏微分法分析TDOA定位方法的定位精度，令各变量的误差是随机的，均值为0的且各变量之间相互独立。对使用TDOA定位方法建立的定位解算方程求偏导，可得：
将上式写成矩阵形式，有
变换上式矩阵形式，可以得到
式中Dd＝diag(σd 2,σd 2,σd 2,σd 2)，Di＝diag(σi 2,σi 2)(i∈[1,2,3])，Dt＝diag(σt 2,σt 2,σt 2)，Dc＝diag(σc 2)，diag()为对角矩阵,σd 2，σi 2(i∈[1,2,3])，σt 2，σc 2为深度、定位节点坐标、声信号到达时间和声速的均方误差。Md，Mi，Mt(i∈[1,2,3])，Mc为各个变量的偏导数矩阵。HDOP为矩阵Dx的迹，即
将预设的自主水下航行器AUV终点改为计算的圆形栅格序列，计算自主水下航行器AUV路径节点的HDOP值，自主水下航行器AUV到达圆形路径的终点后，计算自主水下航行器AUV在整条路径中的平均定位精度值，以平均定位精度值更新信息素，将定位精度启发因子引入转移概率中，对自主水下航行器AUV进行新一轮的路径规划。
令自主水下航行器AUV当前位置为第三定位节点，并在自主水下航行器AUV历史路径中选取两个定位节点，根据选取的三个定位节点，计算由步骤二估计得到的目标位置处的定位精度值。
在传统蚁群算法基础上将定位精度作为评价因子引入转移概率中，并以定位精度值为衡量标准改变信息素增量。计算方法如式(4-8)所示，引入定位精度后的转移概率为：
式中：hij(t)为t时刻路径节点i到路径节点j的定位精度信息，hij(t)＝1/hdopij。其中：hdopij为蚂蚁从路径节点i转移到路径节点j的定位精度值，hdopij的值为以路径节点j为最后一个解算节点，结合自主水下航行器AUV的历史路径，选择两个历史定位节点后得出的定位精度值，r为定位精度启发因子，r越大，蚂蚁越趋向于选择定位精度大的路径。
更新信息素增量Δτij(t)时，以第k只蚂蚁整条路径的平均定位精度值为衡量标准，令HPk为第k只蚂蚁所经过路径的平均定位精度值。Totalk为第k只蚂蚁在整条路径中进行定位解算的总次数。平均定位精度值表示为：
式中：E为路径终点，hdopij为第k只蚂蚁从路径节点i转移到路径节点j的定位精度值。采用每一轮每一只蚂蚁觅食途经的整条路径平均定位精度更新信息素增量。信息素增量表示为：
将定位精度分别引入蚁群算法的转移概率和信息素增量中，使蚁群算法在每次迭代寻找最优路径时，趋向于选择路径中定位精度较高的区域，经过多次迭代后，本发明所提出的改进蚁群算法，可以为自主水下航行器AUV规划既满足避障要求，又具有高定位精度的航路。
具体实施方式六：
环境参数如下：自主水下航行器AUV起始位置为第一个栅格，预设自主水下航行器AUV终点位置为最后一个栅格，水下目标位置为[150,150]m，目标深度为100m，自主水下航行器AUV深度为50m，每块小栅格边长为10m，蚂蚁个数为50个，迭代次数为70次，α为1，β为7，信息素挥发系数为0.3，蚂蚁在一次迭代中分泌的信息素总量Q为1，信息素更新选用蚁周模型，初始时刻所有路径上的信息素浓度τij(0)＝6，声速为1500m/s，自主水下航行器AUV坐标均方根误差为2m，时延测量均方根误差为1ms，声速测量均方根误差为1.5m/s，深度测量均方根误差为1m，声信标信号周期为1s。
采用本发明设计的改进蚁群算法对目标进行位置估计。实施算例中自主水下航行器AUV的航行路径如图3所示，实施算例的定位误差如图4所示，平均定位误差为3.57m，且后期定位误差稳定在1m左右，有较高的定位精度。可见，本发明所涉及的航路规划方法解算估计的目标位置，有较高的定位精度，且定位后期自主水下航行器AUV围绕水下目标行驶，有效解决了使用传统蚁群算法为自主水下航行器AUV规划航迹时，只考虑避障问题，没有充分考虑执行水下目标定位任务的自主水下航行器AUV的载荷效率，使得定位精度较低，甚至导致遗漏目标的问题。
具体实施方式七：一种基于改进蚁群算法的AUV高精度水声定位航路规划系统，包括，
目标预估模块，用于根据预设终点，使用传统蚁群算法驱动自主水下航行器AUV行驶，自主水下航行器AUV行驶过程中不断接收水下目标发射的声信号，接收一定周期的声信号后，解算目标位置，预估目标具体所在栅格；
圆形栅格序列模块，用于根据解算估计的目标位置使用Bresenham画圆算法计算得到以目标为圆心的圆形栅格序列，将预设的自主水下航行器AUV终点改为计算的圆形栅格序列；
路径规划模块，计算自主水下航行器AUV路径节点的定位精度值，自主水下航行器AUV到达圆形路径的终点后，计算自主水下航行器AUV在整条路径中的平均定位精度值，以平均定位精度值更新信息素，将定位精度启发因子引入转移概率中，对自主水下航行器AUV进行新一轮的路径规划。

Claims (8)

1.一种基于改进蚁群算法的AUV高精度水声定位航路规划方法，其特征在于，所述航路规划方法包括以下步骤：
步骤一：根据预设终点，使用传统蚁群算法驱动AUV行驶，AUV行驶过程中不断接收水下目标发射的声信号，接收一定周期的声信号后，解算目标位置，预估目标具体所在栅格；
步骤二：根据解算估计的目标位置使用Bresenham画圆算法计算得到以目标为圆心的圆形栅格序列，将预设的AUV终点改为计算的圆形栅格序列；
步骤三：计算AUV路径节点的定位精度值，AUV到达圆形路径的终点后，计算AUV在整条路径中的平均定位精度值，以平均定位精度值更新信息素，将定位精度启发因子引入转移概率中，对AUV进行新一轮的路径规划。
2.根据权利要求1所述的一种基于改进蚁群算法的AUV高精度水声定位航路规划方法，其特征在于，所述步骤一的具体过程为：
令预设的AUV终点位置为Xe，Xe＝[xe,ye,ze]，，AUV航行过程中自身位置表示为X＝[x,y,z]，水下目标真实位置为Xt＝[xt,yt,zt]，令AUV深度固定不变，将周围环境分割成网格后，用坐标法或编号法对栅格地图进行表示；编号与坐标点一一对应，令第x行、第y列栅格坐标表示为[x,y]m，对应的栅格编号表示为s，每个栅格的边长表示为a；坐标[x,y]与栅格编号s的关系表示为：
式中，mod为取余运算，col为栅格地图的列数，表示向上取整。
3.根据权利要求2所述的一种基于改进蚁群算法的AUV高精度水声定位航路规划方法，其特征在于，假设蚂蚁的数量为m，蚂蚁k根据各个相邻栅格中的信息素含量决定选择前进的路径方向；表示t时刻蚂蚁k从路径节点i转移到路径节点j的概率：
式中，τij(t)为t时刻路径节点i到路径节点j的信息素含量，初始时刻τij(0)＝c，
α为信息素启发因子；ηij(t)为启发信息，ηij(t)＝1/dij，
其中：dij表示路径节点i到路径节点j的距离，β为期望度启发因子；allowedk表示蚂蚁k能够到达的下一个路径节点的集合；
蚂蚁结束一轮觅食后，对所有路径点上的信息素进行更新；更新信息素的公式表示如下：
τij(t+n)＝(1-ρ)τij(t)+Δτij(t) (3)
式中：Δτij(t)为一轮觅食结束后，蚁群在路径节点i到路径节点j分泌的总的信息素增量；为蚂蚁k在路径节点i到路径节点j上分泌的信息素含量；ρ为时间段(t,t+n)间的信息素挥发系数，ρ的取值范围为(0,1)；
信息素更新模型选用蚁周模型，蚁周模型中，蚂蚁k在路径点节点i到路径节点j上分泌的信息素含量表示为：
式中，Q为蚂蚁k在本轮觅食结束后分泌的信息素总量，Lk为蚂蚁k所经过路径的总长度；
每轮蚁群出动觅食，信息素增量和转移概率不断更新，多次迭代后，得到一条从起点到终点的最短或次短路径；AUV沿着此条路径航行，航行过程中不断接收目标发射的声信号，令声信号到达时间为ti，AUV接收声信号时所处位置为定位节点，令定位节点的位置表示为Xi，Xi＝[xi,yi,zi]，选取两个定位节点X1、X2，此时时延差信息表示为t2-t1，基于信号到达时延差的定位方法的定位模型表示为c为水中声速；
使用TDOA定位方法建立如下定位解算方程：
AUV接收一定周期声信号后，根据获得的时延差信息建立定位解算方程组，
使用牛顿迭代法求解该方程组，估计得出目标位置，解算得到的目标位置表示为
4.根据权利要求2所述的一种基于改进蚁群算法的AUV高精度水声定位航路规划方法，其特征在于，所述步骤二的具体过程为：
令圆形序列的圆心位于原点，半径为R，令函数F(x,y)＝x2+y2-R2，使用中点Bresenham画圆算法确定从点(0，R)到点的八分之一圆型序列，若当前栅格为P(xi,yi)，则下一个栅格为正右方的栅格Pr(xi+1,yi)或者正下方的栅格Pd(xi+1,yi-1)；使用中点Bresenham画圆算法确定具体选哪一个栅格；令M为Pr和Pd的中点，即有M(xi+1,yi-0.5)，令vi为F(xM,yM)的值，则有：
vi＝F(xM,yM)＝F(xi+1,yi-0.5)＝(xi+1)2+(yi-0.5)2-R2 (7)
若vi＜0，取Pr(xi+1,yi)为下一个栅格，继续选择下一个的栅格，沿正右方向，递推式为：
若vi≥0，取Pd(xi+1,yi-1)为下一个栅格，继续选择下一个栅格，沿右下方向，递推式为：
第一象限上顺时针八分之一圆的第一个栅格为P0(0,R)，则判别式的初始值为：
为了避免小数运算，用di-0.25代替di，此时d0＝1-R,判别式为di＜-0.25。
5.根据权利要求4所述的一种基于改进蚁群算法的AUV高精度水声定位航路规划方法，其特征在于，采用中点Bresenham画圆算法在栅格地图中得到八分之一圆，根据圆关于x＝0，y＝0，x＝y和x＝-y四条线轴对称，进而得到整个圆的栅格序列；当圆心不位于原点时，首先假设圆心位于原点，使用中点Bresenham画圆算法在栅格地图中得到八分之一圆，后续的坐标变换公式根据圆心位置发生变化；令圆心坐标为[xc,yc]，令dc＝yc-xc，则坐标变换公式变为：
使用中点Bresenham画圆算法得到以目标为圆心的栅格序列后，若圆形序列的栅格中包含障碍物，则将包含障碍物的栅格从圆形栅格中去除；将预设的AUV终点改为步骤二中得到的圆形栅格序列。
6.根据权利要求4所述的一种基于改进蚁群算法的AUV高精度水声定位航路规划方法，其特征在于，所述步骤三的具体过程为：
将AUV终点改为圆形栅格序列后，使用蚁群算法规划AUV航路时，计算AUV路径节点的定位精度值；HDOP的定义如下式：
使用偏微分法分析TDOA定位方法的定位精度，令各变量的误差是随机的，均值为0的且各变量之间相互独立；对使用TDOA定位方法建立的定位解算方程求偏导，可得：
将上式写成矩阵形式，有
变换上式矩阵形式，可以得到
式中Dd＝diag(σd 2,σd 2,σd 2,σd 2)，Di＝diag(σi 2,σi 2)，i∈[1,2,3]，Dt＝diag(σt 2,σt 2,σt 2)，Dc＝diag(σc 2)，diag()为对角矩阵,σd 2，σi 2，i∈[1,2,3]，σt 2，σc 2为深度、定位节点坐标、声信号到达时间和声速的均方误差；Md，Mi，Mt，i∈[1,2,3]，Mc为各个变量的偏导数矩阵；HDOP为矩阵Dx的迹，即
将预设的AUV终点改为计算的圆形栅格序列，计算AUV路径节点的HDOP值，AUV到达圆形路径的终点后，计算AUV在整条路径中的平均定位精度值，以平均定位精度值更新信息素，将定位精度启发因子引入转移概率中，对AUV进行新一轮的路径规划。
7.根据权利要求6所述的一种基于改进蚁群算法的AUV高精度水声定位航路规划方法，其特征在于，令AUV当前位置为第三定位节点，并在AUV历史路径中选取两个定位节点，根据选取的三个定位节点，计算由步骤二估计得到的目标位置处的定位精度值；
将定位精度作为评价因子引入转移概率中，并以定位精度值为衡量标准改变信息素增量；计算方法如式(4-8)所示，引入定位精度后的转移概率为：
式中：hij(t)为t时刻路径节点i到路径节点j的定位精度信息，hij(t)＝1/hdopij；其中：hdopij为蚂蚁从路径节点i转移到路径节点j的定位精度值，hdopij的值为以路径节点j为最后一个解算节点，结合AUV的历史路径，选择两个历史定位节点后得出的定位精度值，r为定位精度启发因子，r越大，蚂蚁越趋向于选择定位精度大的路径。
8.根据权利要求7所述的一种基于改进蚁群算法的AUV高精度水声定位航路规划方法，其特征在于，更新信息素增量Δτij(t)时，以第k只蚂蚁整条路径的平均定位精度值为衡量标准，令HPk为第k只蚂蚁所经过路径的平均定位精度值；Totalk为第k只蚂蚁在整条路径中进行定位解算的总次数；平均定位精度值表示为：
式中：E为路径终点，hdopij为第k只蚂蚁从路径节点i转移到路径节点j的定位精度值；采用每一轮每一只蚂蚁觅食途经的整条路径平均定位精度更新信息素增量；信息素增量表示为：
将定位精度分别引入蚁群算法的转移概率和信息素增量中，使蚁群算法在每次迭代寻找最优路径时，趋向于选择路径中定位精度较高的区域，经过多次迭代后，为AUV规划既满足避障要求，又具有高定位精度的航路。
CN202310946932.4A 2023-07-31 2023-07-31 一种基于改进蚁群算法的auv高精度水声定位航路规划方法 Pending CN117007051A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310946932.4A CN117007051A (zh) 2023-07-31 2023-07-31 一种基于改进蚁群算法的auv高精度水声定位航路规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310946932.4A CN117007051A (zh) 2023-07-31 2023-07-31 一种基于改进蚁群算法的auv高精度水声定位航路规划方法

Publications (1)

Publication Number Publication Date
CN117007051A true CN117007051A (zh) 2023-11-07

Family

ID=88564966

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310946932.4A Pending CN117007051A (zh) 2023-07-31 2023-07-31 一种基于改进蚁群算法的auv高精度水声定位航路规划方法

Country Status (1)

Country Link
CN (1) CN117007051A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117555341A (zh) * 2024-01-12 2024-02-13 中国石油大学(华东) 基于改进蚁群算法的深海采矿车路径规划方法及系统

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117555341A (zh) * 2024-01-12 2024-02-13 中国石油大学(华东) 基于改进蚁群算法的深海采矿车路径规划方法及系统
CN117555341B (zh) * 2024-01-12 2024-05-24 中国石油大学(华东) 基于改进蚁群算法的深海采矿车路径规划方法及系统

Similar Documents

Publication Publication Date Title
CN111240319B (zh) 室外多机器人协同作业系统及其方法
CN109947120B (zh) 仓储系统中的路径规划方法
CN110703762A (zh) 一种复杂环境下水面无人艇混合路径规划方法
CN117007051A (zh) 一种基于改进蚁群算法的auv高精度水声定位航路规划方法
CN112925315A (zh) 一种基于改进蚁群算法和a*算法的履带车路径规划方法
Schmid et al. A unified approach for autonomous volumetric exploration of large scale environments under severe odometry drift
CN110632932A (zh) 基于膜计算和粒子群优化的局部路径规划算法
CN113587933A (zh) 一种基于分支定界算法的室内移动机器人定位方法
CN113433553B (zh) 一种水下机器人多源声学信息融合精确导航方法
CN106896824A (zh) 一种基于目标函数的多auv编队控制方法
CN114879660B (zh) 一种基于目标驱动的机器人环境感知方法
CN115167398A (zh) 一种基于改进a星算法的无人船路径规划方法
CN112857370A (zh) 一种基于时序信息建模的机器人无地图导航方法
CN114967680A (zh) 基于蚁群算法和卷积神经网络的移动机器人路径规划方法
Choi et al. Improved CNN-based path planning for stairs climbing in autonomous UAV with LiDAR sensor
CN114593739A (zh) 基于视觉检测与参考线匹配的车辆全局定位方法及装置
CN108332756B (zh) 一种基于拓扑信息的水下航行器协同定位方法
CN113064422A (zh) 基于双神经网络强化学习的自主水下航行器路径规划方法
CN113029123A (zh) 一种基于强化学习的多auv协同导航方法
CN116576868A (zh) 一种多传感器融合精确定位及自主导航方法
CN116734860A (zh) 一种基于因子图的多auv自适应协同定位方法及系统
CN108459614B (zh) 一种基于cw-rnn网络的uuv实时避碰规划方法
CN115268461A (zh) 一种融合算法的移动机器人自主导航方法
CN114563006B (zh) 基于参考线匹配的车辆全局定位方法及装置
CN116147636B (zh) 基于地理空间的最佳距离自动寻路计算方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination