CN108469828A - 一种改进人工势场优化算法的auv航路规划方法 - Google Patents
一种改进人工势场优化算法的auv航路规划方法 Download PDFInfo
- Publication number
- CN108469828A CN108469828A CN201810243310.4A CN201810243310A CN108469828A CN 108469828 A CN108469828 A CN 108469828A CN 201810243310 A CN201810243310 A CN 201810243310A CN 108469828 A CN108469828 A CN 108469828A
- Authority
- CN
- China
- Prior art keywords
- auv
- potential field
- barrier
- repulsion
- target point
- 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
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 26
- 238000005457 optimization Methods 0.000 title claims abstract description 18
- 230000004888 barrier function Effects 0.000 claims abstract description 76
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 19
- 241000251468 Actinopterygii Species 0.000 claims abstract description 6
- 238000000034 method Methods 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 11
- 238000009826 distribution Methods 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000005381 potential energy Methods 0.000 claims description 6
- 239000000654 additive Substances 0.000 claims description 4
- 230000000996 additive effect Effects 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 4
- 239000011159 matrix material Substances 0.000 claims description 4
- 230000003252 repetitive effect Effects 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000012545 processing Methods 0.000 claims description 2
- 230000009286 beneficial effect Effects 0.000 claims 1
- 230000005611 electricity Effects 0.000 claims 1
- 230000006872 improvement Effects 0.000 abstract description 5
- 230000015572 biosynthetic process Effects 0.000 abstract description 3
- 238000003786 synthesis reaction Methods 0.000 abstract description 3
- 238000013459 approach Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 6
- 238000001514 detection method Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000009514 concussion Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000011982 device technology Methods 0.000 description 1
- 238000010230 functional analysis Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000011017 operating method Methods 0.000 description 1
- 231100000572 poisoning Toxicity 0.000 description 1
- 230000000607 poisoning effect Effects 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种改进人工势场优化算法的AUV航路规划方法，基于AUV自身特性，一般是在深海区域执行任务，AUV在航行过程中可能会和暗礁、沉船、鱼群等发生碰撞，未知环境的不安全性会对AUV的安全有很大的威胁，在海洋未知环境下，为了解决AUV在有障碍物的情况下无碰撞的完成任务，本发明引入新的斥力场函数，帮助AUV改进目标不可达的现象，引入任意的(0‑π/2)旋转角度，帮助AUV逃离局部极小值的现象，将这二种方法结合在一起，对AUV的航路规划有很好的寻优性。这二处改进点综合提高了算法的寻优性和鲁棒性，降低算法的复杂性。
Description
技术领域
本发明涉及一种AUV航路规划方法，特别是一种改进人工势场优化算法的AUV航路规划方法。
背景技术
当前，无人系统已在空中、陆地、水面、水下迅速发展，并逐渐地在我们的社会生活中发挥重要作用，例如：地面的无人车、机器人，天上的卫星、无人机，海里的无人船、潜航器，每一项无人系统的出现带动社会的发展，全维度的无人系统正在席卷人类的生产和生活领域。自20世纪90年代以来，随着计算机软件、硬件、电子设备技术和智能算法的迅速发展，水下航行器脱颖而出，脱离了缆绳束缚的AUV，在水下执行任务过程中显得更为灵活。目前，AUV更多的被用来执行预报预警、远程搭载、收集信息、采集海洋数据、科学研究和水下资源勘察等任务。
任何任务的执行，都需要能够在工作环境中安全的航行。因此为使AUV能够在复杂环境下安全地顺利完成任务，需要涉及到自定位与目标识别、任务规划、避碰、路径规划和对信息的处理技术。因此，无论出于理论意义还是现实需求，对航路规划的研究都显得意义重大。航路规划问题就是使AUV在有障碍物的工作空间内无碰撞地到达目标点。AUV想要完成多个不同位置的任务，就需要在多种工作环境中有效避开障碍物并顺利到达目标点。
基于AUV自身特性，一般是在深海区域执行任务，AUV在航行过程中可能会和暗礁、沉船、鱼群等发生碰撞，未知环境的不安全性会对AUV的安全有很大的威胁。探测障碍物需要对AUV建立环境模型。由于障碍物的不确定性，很可能出现目标不可达和局部极小值问题。
发明内容
针对上述现有技术，本发明要解决的技术问题是提供一种能够解决目标不可达和局部极小值问题的改进人工势场优化算法的AUV航路规划方法。
为解决上述技术问题，本发明一种改进人工势场优化算法的AUV航路规划方法，包括以下步骤：
步骤1：对于AUV在海洋环境中，初始化AUV执行任务的个数、执行任务的顺序、任务类型、人工势场的引力势场系数、斥力势场系数、步长和迭代系数；
步骤2：在未知的环境中，AUV在航行过程中，计算AUV与目标点的引力势场函数；
步骤3：计算AUV和障碍物的斥力势场函数；
步骤4：把步骤2和步骤3的相加值作为合力势场函数；
步骤5：在计算当前位置后，再计算下一步位置；
步骤6：判断AUV是否陷入局部最小值，如果AUV陷入局部最小值，则给AUV一个(0～kπ/2)角度偏移量，k∈(0,1)，返回步骤2；如果AUV没有陷入局部最小值，执行步骤7；
步骤7：判断是否有目标不可达的情况，如果AUV有目标不可达的情况，则构建一个新的斥力势场函数，返回步骤2；如果没有目标不可达的情况，执行步骤8；
步骤8：判断AUV是否到达了目标点，如果AUV到达了目标点，则AUV停止航行；否则返回步骤5。
本发明一种改进人工势场优化算法的AUV航路规划方法，还包括：
1.步骤2中引力势场函数为：
||Wv-Wa||＝||v(x1,y1,z1)-a(x2,y2,z2)||
若在三维空间S中，有静止目标点的坐标位置Wa为a(x2,y2,z2)和AUV的坐标位置Wv为v(x1,y1,z1)，vrob为AUV的速度矢量，vtar为目标点的速度矢量，m,n为大于0的常数，α为引力增益系数，m为任意大于零的正数，若在二维空间，把z的值设为零，降低空间的维度，当0＜m≤1时，引力表现为锥形分布，而且每一个位置点都会表现出引力恒定；当m≥2时，环境中产生的势能最低点为目标点。
2.步骤3中的斥力势场模型为：
其中，Ua(Wv)为斥力势场函数，为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离，ρ＝‖Wv-Wo‖，β为斥力增益系数。
3.步骤4中的合力势场函数为：
U(W)＝Ua(W)+Uo(W)
式中：U(W)为总势场，为合力，Ua(W)为目标对机器人的引力场，为引力，下标a表示目标点，Uo(W)为障碍物对机器人的斥力场，为斥力，下标o表示障碍物。
4.步骤7中所述新的斥力势场函数满足：
其中ρa＝‖Wv-Wa‖，ρa表示AUV与目标点的相对距离，n是大于零的整数，求出AUV的所对应的斥力的方向和大小，即：
Uo(Wv)为斥力势场函数，为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离为ρ＝‖Wv-Wo‖，β为斥力增益系数。
5.步骤6中当AUV陷入极小值的时候，给与AUV在(0-π/2)任意的旋转角度，将旋转kπ/2角度后的方向作为AUV下一步的航行方向，其中，k∈(0，1)，在k值不断递增的过程，AUV会逐步搜索，再次计算AUV所受的合力是不是为零，如果为零，则AUV仍然处于局部极小值点，改变k值；如果不为零，AUV继续向前航行，继续改变偏转角，重复操作，直到AUV跳出极小值点为止，此时AUV的航行方向为最后一次给定偏转角方向。
6.通过多波束声呐来获取障碍物的位置信息，过程为：
声呐用发射机制造电信号，再用换能器将电信号转换为声音信号，发出声信号在海洋中进行传递，遇到障碍物、鱼类或者潜艇时被反射回来，反射回来的声波被换能器转换为电信号，经过数据处理，在显示屏上显示或者在耳机中变成声音；
建立声呐波束矩阵index(i,j){(i＝1，2，3),(j＝1，2，…，80)}，假设在大地坐标系下，AUV的质心坐标为(xc,yc,zc)，前视声纳的坐标为(xs,ys,zs)，可得AUV质心与前视声纳的间距为lcs＝xs-xc，则在AUV坐标系下，AUV的质心坐标为(xc,yc,zc)＝(0,0,0)，前视声纳的坐标为(xs,ys,zs)＝(lcs,0,0)，障碍物的坐标为(xbs,ybs,zbs)，则前视声呐的视域范围为：
若AUV在航行过程中，在前视声呐视域范围内的K处，第index(i,j)的波束与障碍物的某一处有交点，在AUV坐标系下，障碍物的位置为(xbs,ybs,zbs)，其计算公式为：
经过坐标变换把在AUV坐标系下的障碍物位置信息转换为在大地坐标系下的障碍物位置信息，在大地坐标系下，障碍物的位置信息为(x'bs,y'bs,z'bs)，(x,y,z)为所得位置转换为大地坐标的位置，转换公式为：
本发明有益效果：
在海洋未知环境下，为了解决AUV在有障碍物的情况下无碰撞的完成任务，本发明引入新的斥力场函数，帮助AUV改进目标不可达的现象。引入任意的(0-π/2)旋转角度，帮助AUV逃离局部极小值的现象。将这二种方法结合在一起，对AUV的航路规划有很好的寻优性。这二处改进点综合提高了算法的寻优性和鲁棒性，降低算法的复杂性。
附图说明
图1为本发明的算法流程图；
图2为多波束声呐探测图；
图3为AUV航路规划流程；
图4为AUV在环境中的受力示意图；
图5(a)为引力势场分布图；
图5(b)为引力势场分布图；
图6(a)为斥力势场分布图；
图6(b)为斥力势场分布图；
图7为目标不可达示意图；
图8为局部极小值示意图；
图9为目标不可达航行轨迹；
图10为目标可达航路轨迹；
图11为陷入极小值点的航行轨迹；
图12为改进极小值后的航路轨迹；
图13(a)为X-Y轴方向AUV航路规划；
图13(b)为Y-Z轴方向AUV航路规划；
图13(c)为X-Z轴方向AUV航路规划；
图13(d)为X-Y-Z轴方向AUV航路规划。
具体实施方式
本发明一种改进人工势场优化算法的AUV航路规划方法，下面将以三维环境下为海洋任务环境，1艘携带侦察传感器模块的AUV、3个侦察任务、4个障碍物为例。说明该改进算法的实现方式。本发明方法流程如图1所示：
S1：需要建立在海洋未知环境下的环境模型，通过多波束声呐来获取障碍物的位置信息。该声呐的水平开角为α＝120°，3层波束，垂直开角为β＝17°，80条波束，故多波束声呐共有240条波束。探测距离为R＝120m，工作频率是2Hz。
S2：初始化工作：主要包括水面工作站初始化、使命任务下载、人工势场法的相关参数设置、海域面积设置。设置参数为引力系数为1，斥力系数为2，步长为0.1，障碍物影响距离为100，迭代次数为200。
S3：通过水面工作台加载AUV及其任务序列的位置信息如表1所示：
表1加载的AUV和任务位置信息
为了便于使障碍物归一化展示，在三维环境下，将障碍物都是归一化为球体的形式进行显示，水面工作台加载的障碍物的信息如下表2所示：
表2加载的障碍物信息
S4：在未知的环境中，AUV在航行过程中，计算AUV与目标点的引力势场函数。
S5：计算AUV和障碍物的斥力势场函数。
S6：把步骤2和步骤3的相加值就是合力势场函数。
S7：在计算当前位置后，再计算下一个位置。
S8：如果AUV陷入局部最小值就给AUV一个(0～kn/2)角度偏移量，返回步骤2。
步骤9：如果AUV没有陷入局部最小值，就执行步骤8；
步骤10：如果AUV有目标不可达的情况，就构建一个新的斥力势场函数，返回步骤2；
步骤11：如果没有目标不可达的情况，就执行步骤10；
步骤12：如果AUV到达了目标点，AUV就停止航行。
步骤13：如果AUV到达目标点，返回步骤5；
由于在三维环境下，通过X-Y、Y-Z、X-Z、X-Y-Z方向来显示AUV的航路。AUV航路规划如下图13(a)-图13(d)。其中，图13(a)图为X-Y轴方向，图13(b)Y-Z轴方向，图13(c)X-Z轴方向，图13(d)X-Y-Z轴方向。
AUV遇到障碍物时可通过图13(a)-图13(d)看出其运动方向了发生改变，且很好的规避障碍物且航路非常光滑。进一步验证了改进人工势场优化算法的有效性和寻优性。
综上所述，在海洋未知环境下，本发明提出的改进人工势场优化算法有效提高了算法运行在航路优化的能力，规划出来的航路是非常平滑，且能安全的避过障碍。
本发明具体实施方式还包括，包括以下步骤：
步骤1：对于AUV在海洋环境中，初始化AUV执行任务的个数、执行任务的顺序、任务类型，人工势场的引力势场系数、斥力势场系数、步长和迭代系数。
步骤2：在未知的环境中，AUV在航行过程中，计算AUV与目标点的引力势场函数。
步骤3：计算AUV和障碍物的斥力势场函数。
步骤4：把步骤2和步骤3的相加值就是合力势场函数。
步骤5：在计算当前位置后，再计算下一个位置。
步骤6：如果AUV陷入局部最小值就给AUV一个(0～kn/2)角度偏移量，返回步骤2；
步骤7：如果AUV没有陷入局部最小值，就执行步骤8；
步骤8：如果AUV有目标不可达的情况，就构建一个新的斥力势场函数，返回步骤2；
步骤9：如果没有目标不可达的情况，就执行步骤10；
步骤10：如果AUV到达了目标点，AUV就停止航行。
步骤11：如果AUV到达目标点，返回步骤5；
步骤2中引力势场函数模型为：
‖Wv-Wa‖＝‖v(x1,y1,z1)-a(x2,y2,z2)||
若在三维空间S中，有静止目标点的坐标位置Wa为a(x2,y2,z2)和AUV的坐标位置Wv为v(x1,y1,z1)。vrob为AUV的速度矢量，vtar为目标点的速度矢量，m,n为大于0的常数。α为引力增益系数，m为任意大于零的正数。若在二维空间，把z的值设为零，降低空间的维度。当0＜m≤1时，引力表现为锥形分布，而且每一个位置点都会表现出引力恒定。同理，当m≥2时，环境中产生的势能最低点就是目标点，有可能导致引力趋于无穷大的现象导致AUV在目标点来回抖动。引力势场分布图如图5(a)和图5(b)所示。
步骤3中的斥力势场模型为：
其中，Ua(Wv)为斥力势场函数，为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离为ρ＝||Wv-Wo||，β为斥力增益系数；斥力势场分布图如图6(a)和图6(b)所示。
步骤4中的合力势场模型为：
在构建势场时，AUV利用斥力与引力的合力共同作用，其合势场的数学表达式为：
U(W)＝Ua(W)+Uo(W)
式中：U(W)为总势场，为合力，Ua(W)为目标对机器人的引力场，为引力，下标a表示目标点。Uo(W)为障碍物对机器人的斥力场，为斥力，下标o表示障碍物。说明AUV是顺着势场的负梯度方向进行航向，最终达到目标点。
步骤8中的目标不可达问题，是指AUV航行离目标点的附近，有一个或者多个障碍物也在目标点的附近，并且AUV和目标点都在障碍物能够影响的范围内，根据斥力势场函数和引力势场函数分析，AUV和目标点的距离越来越近，AUV所受的引力就会越来越小。而AUV和障碍物的距离越来越近，AUV所受的斥力就会越来越大。当斥力大于引力的合力作用，AUV就会远离目标点或者在目标点附近不断徘，导致AUV不能到达目标点的位置。目标不可达示意图如图7、目标不可达航行轨迹图9及其目标可达航路轨迹如图10所示。
针对此问题，进一步的考虑AUV与目标点之间的相对距离。构建一个新的斥力势场函数为
其中ρa＝||Wv-Wa||，ρa表示AUV与目标点的相对距离，n是大于零的整数。可以求出AUV的所对应的斥力的方向和大小，即：
其中，Uo(Wv)为构建一个新的斥力势场函数，为势场力，ρa＝||Wv-Wa||，ρa表示AUV与目标点的相对距离，n是大于零的整数。
步骤7中的局部极小值问题，是AUV在航行的过程中，在还没到达目标点的情况下，AUV受到一个障碍物的斥力或者多个障碍物的受到的合力与AUV受到目标点的引力，二者是大小相等，方向相反。在还没到达目标的时候，航行过程中的合力为零，会出现停滞不前或者来回震荡的现象。AUV停止航行的点就是全局势能最小点。
本发明给AUV引入了在一定范围内的随机偏转角。当AUV陷入极小值的时候，这时给与AUV任意的旋转角度，它的范围在(0-π/2)，将旋转kπ/2角度后的方向作为AUV下一步的航行方向。其中，K∈(0，1)，在K值不断递增的过程，AUV会逐步搜索。再次计算AUV所受的合力是不是为零，如果为零，说明AUV仍然处于局部极小值点，改变K值。如果不为零，AUV仍然向前航行，继续改变偏转角，重复操作，直到AUV能够跳出极小值点为止，此时AUV的航行方向就是最后一次给定偏转角方向。局部极小值示意图如图8、陷入极小值点的航行轨迹如图11及其改进极小值后的航路轨迹如图12所示。
本发明通过多波束声呐来获取障碍物的位置信息。声呐先用发射机制造电信号，再用换能器(电信号转换为声音信号)发出声信号在海洋中进行传递，如果遇到障碍物、鱼类或者潜艇时，就会被反射回来，反射回来的声波会被换能器转换为电信号，经过数据处理，会在显示屏上显示或者在耳机中变成声音。
建立声呐波束矩阵index(i,j){(i＝1，2，3),(j＝1，2，...，80)}。假设在大地坐标系下，AUV的质心坐标为(xc,yc,zc)，前视声纳的坐标为(xs,ys,zs)，可得AUV质心与前视声纳的间距为lcs＝xs-xc，多波束声呐探测图如图2所示。则在AUV坐标系下，AUV的质心坐标为(xc,yc,zc)＝(0,0,0)，前视声纳的坐标为(xs,ys,zs)＝(lcs,0,0)。障碍物的坐标为(xbs,ybs,zbs)，则前视声呐的视域范围为
若AUV在航行过程中，在前视声呐视域范围内的K处，第index(i,j)的波束与障碍物的某一处有交点。此时，在AUV坐标系下，障碍物的位置为(xbs,ybs,zbs)，其计算公式为：
经过坐标变换可以把在AUV坐标系下的障碍物位置信息转换为在大地坐标系下的障碍物位置信息。在大地坐标系下，障碍物的位置信息为(x'bs,y'bs,z'bs)。(x,y,z)为所得位置转换为大地坐标的位置。其计算公式为：
通过建立多波束声呐的数学模型，可以得到AUV在航行过程中获得的障碍物的位置信息。由上所述，AUV在航行过程中是基于前视声纳探测范围的扇形模型来做基础来实现未知环境模型。
本发明具体实施方式还包括：
具体的操作步骤如下：
步骤1：建立在海洋未知环境下的环境模型，通过多波束声呐来获取障碍物的位置信息。对于在海洋环境中，AUV的位置信息是通过自身定位系统来获取，对于AUV在航行过程中，如果前视声呐探测到障碍物的信息可通过坐标转换为大地坐标系下的障碍物信息。
步骤2：将改进人工势场法运用在AUV的航路规划上，建立人工势场算法模型。其中包括的元素有斥力势场函数、引力势场函数、合力势场函数、航向角度。
步骤3：对于AUV在海洋环境中，初始化AUV执行任务的个数、执行任务顺序及其类型。
步骤4：在未知的环境中，AUV的前视声纳在航行过程对航行路径进行探测，探测到没有障碍物，将计算引力势场函数作为合力势场函数作为最终势场函数来引导AUV的航行。
步骤5：如果有障碍物，将判断在AUV航行的安全距离内，有几个障碍物，计算多个障碍物的斥力势场函数，将引力势场函数与斥力势场函数的综合作为合力势场函数来引导AUV的航行。
步骤6：如果AUV在航行中陷入局部极小，将给AUV随机在90度的角度，再更新合力函数，直至不再陷入局部极小值点。
步骤7：如果AUV在航行过程中有目标不可达问题，将构建新的斥力势场函数，再更新合力函数，直至不再有目标不可达问题。
步骤8：当AUV完成所有的任务，即说明AUV到达目标终点，AUV停止航行。
引力势场模型为：
||Wv-Wa||＝||v(x1,y1,z1)-a(x2,y2,z2)
若在三维空间S中，Ua(Wv)为引力势场函数，为斥力。静止目标点的坐标位置Wa为a(x2,y2,z2)，AUV的坐标位置Wv为v(x1,y1,z1)。vrob为AUV的速度矢量，vtar为目标点的速度矢量，m,n为大于0的常数，α为引力增益系数。当0＜m≤1时，引力表现为锥形分布，而且每一个位置点都会表现出引力恒定。同理，当m≥2时，环境中产生的势能最低点就是目标点，有可能导致引力趋于无穷大的现象导致AUV在目标点来回抖动。引力势场分布图如图5(a)和图5(b)所示。
斥力势场模型为
其中，Uo(Wv)为斥力势场函数，为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离为ρ＝||Wv-Wo||，β为斥力增益系数；斥力势场分布图如图6(a)和图6(b)所示。
合力势场模型为
在构建势场时，AUV利用斥力与引力的合力共同作用，其合势场的数学表达式为：
U(W)＝Ua(W)+Uo(W)
式中：U(W)为总势场，为合力，Ua(W)为目标对机器人的引力场，为引力，下标a表示目标点。Uo(W)为障碍物对机器人的斥力场，为斥力，下标o表示障碍物。说明AUV是顺着势场的负梯度方向进行航向，最终达到目标点。
关于目标不可达问题，是指AUV航行离目标点的附近，有一个或者多个障碍物也在目标点的附近，并且AUV和目标点都在障碍物能够影响的范围内，根据斥力势场函数和引力势场函数分析，AUV和目标点的距离越来越近，AUV所受的引力就会越来越小。而AUV和障碍物的距离越来越近，AUV所受的斥力就会越来越大。当斥力大于引力的合力作用，AUV就会远离目标点或者在目标点附近不断徘，导致AUV不能到达目标点的位置。目标不可达示意图如图7、目标不可达航行轨迹图9及其目标可达航路轨迹如图10所示。
针对此问题，进一步的考虑AUV与目标点之间的相对距离。构建一个新的斥力势场模型为
Uo(Wv)为斥力势场函数，为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离为ρ＝||Wv-Wo||，β为斥力增益系数，其中ρa＝||Wv-Wa||，ρa表示AUV与目标点的相对距离，n是大于零的整数。
关于局部极小值问题，是AUV在航行的过程中，在还没到达目标点的情况下，AUV受到一个障碍物的斥力或者多个障碍物的受到的合力与AUV受到目标点的引力，二者是大小相等，方向相反。在还没到达目标的时候，航行过程中的合力为零，会出现停滞不前或者来回震荡的现象。AUV停止航行的点就是全局势能最小点。
本发明给AUV引入了在一定范围内的随机偏转角。当AUV陷入极小值的时候，这时给与AUV任意的旋转角度，它的范围在(0-π/2)，将旋转kπ/2角度后的方向作为AUV下一步的航行方向。其中，K∈(0，1)，在K值不断递增的过程，AUV会逐步搜索。再次计算AUV所受的合力是不是为零，如果为零，说明AUV仍然处于局部极小值点，改变K值。如果不为零，AUV仍然向前航行，继续改变偏转角，重复操作，直到AUV能够跳出极小值点为止，此时AUV的航行方向就是最后一次给定偏转角方向。局部极小值示意图如图8、陷入极小值点的航行轨迹如图11及其改进极小值后的航路轨迹如图12所示。
本发明通过多波束声呐来获取障碍物的位置信息。声呐先用发射机制造电信号，再用换能器(电信号转换为声音信号)发出声信号在海洋中进行传递，如果遇到障碍物、鱼类或者潜艇时，就会被反射回来，反射回来的声波会被换能器转换为电信号，经过数据处理，会在显示屏上显示或者在耳机中变成声音。
建立声呐波束矩阵index(i,j){(i＝1，2，3),(j＝1，2，…，80)}。假设在大地坐标系下，AUV的质心坐标为(xc,yc,zc)，前视声纳的坐标为(xs,ys,zs)，可得AUV质心与前视声纳的间距为lcs＝xs-xc，多波束声呐探测图如图2所示。则在AUV坐标系下，AUV的质心坐标为(xc,yc,zc)＝(0,0,0)，前视声纳的坐标为(xs,ys,zs)＝(lcs,0,0)。障碍物的坐标为(xbs,ybs,zbs)，则前视声呐的视域范围为
若AUV在航行过程中，在前视声呐视域范围内的K处，第index(i,j)的波束与障碍物的某一处有交点。此时，在AUV坐标系下，障碍物的位置为(xbs,ybs,zbs)，其计算公式为：
经过坐标变换可以把在AUV坐标系下的障碍物位置信息转换为在大地坐标系下的障碍物位置信息。在大地坐标系下，障碍物的位置信息为(x'bs,y'bs,z'bs)。(x,y,z)为所得位置转换为大地坐标的位置。其计算公式为：
通过建立多波束声呐的数学模型，可以得到AUV在航行过程中获得的障碍物的位置信息。由上所述，AUV在航行过程中是基于前视声纳探测范围的扇形模型来做基础来实现未知环境模型。
Claims (7)
1.一种改进人工势场优化算法的AUV航路规划方法，其特征在于，包括以下步骤：
步骤1：对于AUV在海洋环境中，初始化AUV执行任务的个数、执行任务的顺序、任务类型、人工势场的引力势场系数、斥力势场系数、步长和迭代系数；
步骤2：在未知的环境中，AUV在航行过程中，计算AUV与目标点的引力势场函数；
步骤3：计算AUV和障碍物的斥力势场函数；
步骤4：把步骤2和步骤3的相加值作为合力势场函数；
步骤5：在计算当前位置后，再计算下一步位置；
步骤6：判断AUV是否陷入局部最小值，如果AUV陷入局部最小值，则给AUV一个(0～kπ/2)角度偏移量，k∈(0,1)，返回步骤2；如果AUV没有陷入局部最小值，执行步骤7；
步骤7：判断是否有目标不可达的情况，如果AUV有目标不可达的情况，则构建一个新的斥力势场函数，返回步骤2；如果没有目标不可达的情况，执行步骤8；
步骤8：判断AUV是否到达了目标点，如果AUV到达了目标点，则AUV停止航行；否则返回步骤5。
2.根据权利要求1所述的一种改进人工势场优化算法的AUV航路规划方法，其特征在于：步骤2中引力势场函数为：
||Wv-Wa||＝||v(x1,y1,z1)-a(x2,y2,z2)||
若在三维空间S中，有静止目标点的坐标位置Wa为a(x2,y2,z2)和AUV的坐标位置Wv为v(x1,y1,z1)，vrob为AUV的速度矢量，vtar为目标点的速度矢量，m,n为大于0的常数，α为引力增益系数，m为任意大于零的正数，若在二维空间，把z的值设为零，降低空间的维度，当0＜m≤1时，引力表现为锥形分布，而且每一个位置点都会表现出引力恒定；当m≥2时，环境中产生的势能最低点为目标点。
3.根据权利要求1所述的一种改进人工势场优化算法的AUV航路规划方法，其特征在于：步骤3中的斥力势场模型为：
其中，Ua(Wv)为斥力势场函数，为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离，ρ＝||Wv-Wo||，β为斥力增益系数。
4.根据权利要求1所述的一种改进人工势场优化算法的AUV航路规划方法，其特征在于：步骤4中的合力势场函数为：
U(W)＝Ua(W)+Uo(W)
式中：U(W)为总势场，为合力，Ua(W)为目标对机器人的引力场，为引力，下标a表示目标点，Uo(W)为障碍物对机器人的斥力场，为斥力，下标o表示障碍物。
5.根据权利要求1所述的一种改进人工势场优化算法的AUV航路规划方法，其特征在于：步骤7中所述新的斥力势场函数满足：
其中ρa＝||Wv-Wa||，ρa表示AUV与目标点的相对距离，n是大于零的整数，求出AUV的所对应的斥力的方向和大小，即：
Uo(Wv)为斥力势场函数，为斥力，ρo为障碍物的影响距离，ρ是障碍物和AUV的最短安全距离为ρ＝||Wv-Wo||，β为斥力增益系数。
6.根据权利要求1所述的一种改进人工势场优化算法的AUV航路规划方法，其特征在于：步骤6中当AUV陷入极小值的时候，给与AUV在(0-π/2)任意的旋转角度，将旋转kπ/2角度后的方向作为AUV下一步的航行方向，其中，k∈(0，1)，在k值不断递增的过程，AUV会逐步搜索，再次计算AUV所受的合力是不是为零，如果为零，则AUV仍然处于局部极小值点，改变k值；如果不为零，AUV继续向前航行，继续改变偏转角，重复操作，直到AUV跳出极小值点为止，此时AUV的航行方向为最后一次给定偏转角方向。
7.根据权利要求1所述的一种改进人工势场优化算法的AUV航路规划方法，其特征在于：通过多波束声呐来获取障碍物的位置信息，过程为：
声呐用发射机制造电信号，再用换能器将电信号转换为声音信号，发出声信号在海洋中进行传递，遇到障碍物、鱼类或者潜艇时被反射回来，反射回来的声波被换能器转换为电信号，经过数据处理，在显示屏上显示或者在耳机中变成声音；
建立声呐波束矩阵index(i,j){(i＝1，2，3),(j＝1，2，…，80)}，假设在大地坐标系下，AUV的质心坐标为(xc,yc,zc)，前视声纳的坐标为(xs,ys,zs)，可得AUV质心与前视声纳的间距为lcs＝xs-xc，则在AUV坐标系下，AUV的质心坐标为(xc,yc,zc)＝(0,0,0)，前视声纳的坐标为(xs,ys,zs)＝(lcs,0,0)，障碍物的坐标为(xbs,ybs,zbs)，则前视声呐的视域范围为：
若AUV在航行过程中，在前视声呐视域范围内的K处，第index(i,j)的波束与障碍物的某一处有交点，在AUV坐标系下，障碍物的位置为(xbs,ybs,zbs)，其计算公式为：
经过坐标变换把在AUV坐标系下的障碍物位置信息转换为在大地坐标系下的障碍物位置信息，在大地坐标系下，障碍物的位置信息为(x'bs,y'bs,z'bs)，(x,y,z)为所得位置转换为大地坐标的位置，转换公式为：
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810243310.4A CN108469828A (zh) | 2018-03-23 | 2018-03-23 | 一种改进人工势场优化算法的auv航路规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810243310.4A CN108469828A (zh) | 2018-03-23 | 2018-03-23 | 一种改进人工势场优化算法的auv航路规划方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108469828A true CN108469828A (zh) | 2018-08-31 |
Family
ID=63264676
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810243310.4A Pending CN108469828A (zh) | 2018-03-23 | 2018-03-23 | 一种改进人工势场优化算法的auv航路规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108469828A (zh) |
Cited By (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109407653A (zh) * | 2018-12-18 | 2019-03-01 | 中国人民解放军陆军装甲兵学院 | 一种室内通用多移动机器人算法验证系统 |
| CN109508016A (zh) * | 2018-12-26 | 2019-03-22 | 北京工商大学 | 水质采样巡航船路径规划最优化方法 |
| CN109839956A (zh) * | 2019-03-04 | 2019-06-04 | 北京邮电大学 | 一种无人机的路径规划方法及装置 |
| CN109850516A (zh) * | 2018-11-30 | 2019-06-07 | 山东师范大学 | 一种智能rgv动态调度方法、装置及系统 |
| CN110471429A (zh) * | 2019-09-19 | 2019-11-19 | 华南农业大学 | 基于改进人工势场法的割草机器人实时避障方法 |
| CN110567478A (zh) * | 2019-09-30 | 2019-12-13 | 广西科技大学 | 一种基于人工势场法的无人驾驶车辆路径规划方法 |
| CN112068560A (zh) * | 2020-08-28 | 2020-12-11 | 的卢技术有限公司 | 一种基于改进人工势场法的机器人路径规划方法 |
| CN112534377A (zh) * | 2019-11-29 | 2021-03-19 | 深圳市大疆创新科技有限公司 | 数据处理方法、无人机和计算机可读存储介质 |
| CN112678208A (zh) * | 2021-01-20 | 2021-04-20 | 哈尔滨工业大学 | 基于包含动态规避和阻尼特性人工势场法的卫星集群控制方法 |
| WO2021082710A1 (zh) * | 2019-10-31 | 2021-05-06 | 五邑大学 | 无人船路径规划方法、装置、设备和存储介质 |
| CN112799385A (zh) * | 2019-10-25 | 2021-05-14 | 中国科学院沈阳自动化研究所 | 一种基于引导域人工势场的智能体路径规划方法 |
| CN113110453A (zh) * | 2021-04-15 | 2021-07-13 | 哈尔滨工业大学 | 一种基于图形变换的人工势场避障方法 |
| CN113190001A (zh) * | 2021-04-30 | 2021-07-30 | 南京理工大学 | 一种送药机器人移动方法及装置 |
| CN113534838A (zh) * | 2021-07-15 | 2021-10-22 | 西北工业大学 | 一种基于人工势场法的改进无人机航迹规划方法 |
| CN114131616A (zh) * | 2021-12-28 | 2022-03-04 | 华中科技大学 | 一种应用于机械臂操控的三维虚拟力场视觉增强方法 |
| CN114564009A (zh) * | 2022-01-21 | 2022-05-31 | 首都医科大学 | 一种手术机器人路径规划方法及系统 |
| CN114879706A (zh) * | 2022-06-17 | 2022-08-09 | 哈尔滨工程大学 | 一种rrt与人工势场法相结合的auv目标搜索方法 |
| CN115328152A (zh) * | 2022-09-05 | 2022-11-11 | 北京理工大学 | 基于改进人工势场法的无人车局部实时避障路径规划方法 |
| CN117270522A (zh) * | 2023-08-04 | 2023-12-22 | 哈尔滨工程大学 | 一种基于人工势场的双艇协同系统避障方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101408772A (zh) * | 2008-11-21 | 2009-04-15 | 哈尔滨工程大学 | Auv智能避碰装置及避碰方法 |
| CN102231082A (zh) * | 2011-04-08 | 2011-11-02 | 中国船舶重工集团公司第七○二研究所 | 基于迷你声纳的水下目标探测与auv自动避碰方法及其系统 |
| JP2013242777A (ja) * | 2012-05-22 | 2013-12-05 | Tokyo Univ Of Agriculture & Technology | 植物栽培方法と植物栽培システム |
| CN104020770A (zh) * | 2014-06-13 | 2014-09-03 | 哈尔滨工程大学 | 基于多项式的uuv空间轨迹规划方法 |
| CN105629974A (zh) * | 2016-02-04 | 2016-06-01 | 重庆大学 | 一种基于改进型人工势场法的机器人路径规划方法及系统 |
-
2018
- 2018-03-23 CN CN201810243310.4A patent/CN108469828A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101408772A (zh) * | 2008-11-21 | 2009-04-15 | 哈尔滨工程大学 | Auv智能避碰装置及避碰方法 |
| CN102231082A (zh) * | 2011-04-08 | 2011-11-02 | 中国船舶重工集团公司第七○二研究所 | 基于迷你声纳的水下目标探测与auv自动避碰方法及其系统 |
| JP2013242777A (ja) * | 2012-05-22 | 2013-12-05 | Tokyo Univ Of Agriculture & Technology | 植物栽培方法と植物栽培システム |
| CN104020770A (zh) * | 2014-06-13 | 2014-09-03 | 哈尔滨工程大学 | 基于多项式的uuv空间轨迹规划方法 |
| CN105629974A (zh) * | 2016-02-04 | 2016-06-01 | 重庆大学 | 一种基于改进型人工势场法的机器人路径规划方法及系统 |
Non-Patent Citations (3)
| Title |
|---|
| 吴正平，等: "基于改进人工势场法的AUV路径规划", 《化工自动化及仪表》 * |
| 张永丽: "小型无人水下航行器群体协同区域覆盖方法研究", 《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》 * |
| 朱大奇: "《无人潜水器水下搜救理论与技术》", 31 May 2016 * |
Cited By (23)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109850516A (zh) * | 2018-11-30 | 2019-06-07 | 山东师范大学 | 一种智能rgv动态调度方法、装置及系统 |
| CN109850516B (zh) * | 2018-11-30 | 2019-09-24 | 山东师范大学 | 一种智能rgv动态调度方法、装置及系统 |
| CN109407653A (zh) * | 2018-12-18 | 2019-03-01 | 中国人民解放军陆军装甲兵学院 | 一种室内通用多移动机器人算法验证系统 |
| CN109508016A (zh) * | 2018-12-26 | 2019-03-22 | 北京工商大学 | 水质采样巡航船路径规划最优化方法 |
| CN109839956A (zh) * | 2019-03-04 | 2019-06-04 | 北京邮电大学 | 一种无人机的路径规划方法及装置 |
| CN110471429A (zh) * | 2019-09-19 | 2019-11-19 | 华南农业大学 | 基于改进人工势场法的割草机器人实时避障方法 |
| CN110567478A (zh) * | 2019-09-30 | 2019-12-13 | 广西科技大学 | 一种基于人工势场法的无人驾驶车辆路径规划方法 |
| CN112799385A (zh) * | 2019-10-25 | 2021-05-14 | 中国科学院沈阳自动化研究所 | 一种基于引导域人工势场的智能体路径规划方法 |
| WO2021082710A1 (zh) * | 2019-10-31 | 2021-05-06 | 五邑大学 | 无人船路径规划方法、装置、设备和存储介质 |
| CN112534377A (zh) * | 2019-11-29 | 2021-03-19 | 深圳市大疆创新科技有限公司 | 数据处理方法、无人机和计算机可读存储介质 |
| WO2021102951A1 (zh) * | 2019-11-29 | 2021-06-03 | 深圳市大疆创新科技有限公司 | 数据处理方法、无人机和计算机可读存储介质 |
| CN112068560A (zh) * | 2020-08-28 | 2020-12-11 | 的卢技术有限公司 | 一种基于改进人工势场法的机器人路径规划方法 |
| CN112678208A (zh) * | 2021-01-20 | 2021-04-20 | 哈尔滨工业大学 | 基于包含动态规避和阻尼特性人工势场法的卫星集群控制方法 |
| CN113110453A (zh) * | 2021-04-15 | 2021-07-13 | 哈尔滨工业大学 | 一种基于图形变换的人工势场避障方法 |
| CN113190001A (zh) * | 2021-04-30 | 2021-07-30 | 南京理工大学 | 一种送药机器人移动方法及装置 |
| CN113534838A (zh) * | 2021-07-15 | 2021-10-22 | 西北工业大学 | 一种基于人工势场法的改进无人机航迹规划方法 |
| CN114131616A (zh) * | 2021-12-28 | 2022-03-04 | 华中科技大学 | 一种应用于机械臂操控的三维虚拟力场视觉增强方法 |
| CN114131616B (zh) * | 2021-12-28 | 2024-02-02 | 华中科技大学 | 一种应用于机械臂操控的三维虚拟力场视觉增强方法 |
| CN114564009A (zh) * | 2022-01-21 | 2022-05-31 | 首都医科大学 | 一种手术机器人路径规划方法及系统 |
| CN114879706A (zh) * | 2022-06-17 | 2022-08-09 | 哈尔滨工程大学 | 一种rrt与人工势场法相结合的auv目标搜索方法 |
| CN115328152A (zh) * | 2022-09-05 | 2022-11-11 | 北京理工大学 | 基于改进人工势场法的无人车局部实时避障路径规划方法 |
| CN117270522A (zh) * | 2023-08-04 | 2023-12-22 | 哈尔滨工程大学 | 一种基于人工势场的双艇协同系统避障方法 |
| CN117270522B (zh) * | 2023-08-04 | 2024-08-23 | 哈尔滨工程大学 | 一种基于人工势场的双艇协同系统避障方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108469828A (zh) | 一种改进人工势场优化算法的auv航路规划方法 | |
| Fallon et al. | Relocating underwater features autonomously using sonar-based SLAM | |
| CN109765929B (zh) | 一种基于改进rnn的uuv实时避障规划方法 | |
| Bellingham | Autonomous underwater vehicle docking | |
| Aulinas et al. | Vision-based underwater SLAM for the SPARUS AUV | |
| Feng et al. | Automatic tracking method for submarine cables and pipelines of AUV based on side scan sonar | |
| Lindsay et al. | Collaboration of heterogeneous marine robots toward multidomain sensing and situational awareness on partially submerged targets | |
| Inzartsev et al. | Underwater pipeline inspection method for AUV based on laser line recognition: Simulation results | |
| Ennong et al. | Design and experiment of a sea-air heterogeneous unmanned collaborative system for rapid inspection tasks at sea | |
| Yakimenko et al. | Real-time optimal guidance and obstacle avoidance for UMVs | |
| Choi et al. | Underwater robots | |
| CN110471425B (zh) | 一种基于声呐的改进型模糊人工势场无人船避障方法 | |
| Caccia et al. | Modular USV and payload design for advanced capabilities in marine security applications | |
| Bykova et al. | Safe navigation algorithm for autonomous underwater vehicles | |
| Kantor et al. | Experiments in navigation and mapping with a hovering AUV | |
| Healey | Obstacle avoidance while bottom following for the REMUS autonomous underwater vehicle | |
| Noguchi et al. | Wide area seafloor imaging by a low-cost AUV | |
| Mousazadeh et al. | Experimental evaluation of a new developed algorithm for an autonomous surface vehicle and comparison with simulink results | |
| Almanza-Medina et al. | Sonar FoV segmentation for motion estimation using DL networks | |
| Guo et al. | Moored underwater docking system for resident UUVs with acoustic guidance: Design and experiment | |
| Ma et al. | An improved adaptive dynamic window algorithm for target tracking of unmanned surface vehicles | |
| Rasheed et al. | Underwater Robotics: Principles, Components, Modeling, and Control | |
| Orłowski | Directions of development of the autonomous unmanned underwater vehicles. A review | |
| Inzartsev et al. | Autonomous Underwater Vehicle Motion Control during Investigation of Bottom Objects and Hard-to-Reach Areas | |
| CN113110458A (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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20180831 |