CN113110530A - 一种针对三维环境下的水下机器人路径规划方法 - Google Patents

一种针对三维环境下的水下机器人路径规划方法 Download PDF

Info

Publication number
CN113110530A
CN113110530A CN202110414188.4A CN202110414188A CN113110530A CN 113110530 A CN113110530 A CN 113110530A CN 202110414188 A CN202110414188 A CN 202110414188A CN 113110530 A CN113110530 A CN 113110530A
Authority
CN
China
Prior art keywords
node
algorithm
current
point
path
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.)
Granted
Application number
CN202110414188.4A
Other languages
English (en)
Other versions
CN113110530B (zh
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.)
Dalian Maritime University
Original Assignee
Dalian Maritime 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 Dalian Maritime University filed Critical Dalian Maritime University
Priority to CN202110414188.4A priority Critical patent/CN113110530B/zh
Publication of CN113110530A publication Critical patent/CN113110530A/zh
Application granted granted Critical
Publication of CN113110530B publication Critical patent/CN113110530B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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

Abstract

本发明公开了一种针对三维环境下的水下机器人路径规划方法，包括以下步骤：建立近水底环境的三维模型；定义AUV将要运行路线的起点和终点，定义PSLT算法的搜索方向，基于近水底环境的三维模型，从起点开始，采用PSLT算法进行路径的最优航路点搜索，得到路径最优航路点集合，通过路径航路点最优点集合的最优点形成最优路径；PSLT算法继承了Lazy Theta*减少LOS检查和无角度限制规划的优点，从规划路径的长短来看，PSLT算法规划的路径长度要比Lazy Theta*算法规划的路径长度短，要比Theta*算法规划的路径略长，PSLT由于平滑操作，使得航路点大大减少，进而提升了路径的平滑度；综合多组仿真数据，PSLT算法在保证了路径的长短的同时，提升了AUV航行的路径平滑度和算法运行效率。

Description

一种针对三维环境下的水下机器人路径规划方法
技术领域
本发明涉及路径规划技术领域，尤其涉及一种针对三维环境下的水下机器人路径规划方法。
背景技术
随着陆地可开采资源的日渐枯竭以及科学技术的高速发展，人们对于海洋自主探测技术的需求日益强烈，自主式水下航行器(autonomous underwater vehicle,以下简称AUV)一直是水下航行器(unmanned underwater vehicle,UUV)领域的研究热点，在海洋科学研究、海洋资源勘探以及军事方面的应用越来越广泛[1，2]，水下环境不利于人类完成各种高精度工作，为了在水下环境中自主执行各种任务，一种高度自主化的AUV路径规划方法是必要的.而路径规划往往由环境建模和路径搜索两部分组成，根据采集到的环境信息选择合适的方法进行环境建模，环境建模完成后，在建立的环境中选择合适的路径搜索算法以找到最优或者次优的路径[3]。
基于环境模型的路径搜索算法发展很成熟，是一类非常优秀的路径搜索算法[4-17]，HART等最初提出了A*算法[4]，根据已知的环境先验信息执行启发式的搜索，根据搜索时赋予各个顶点的评价，获得一条最优路径，潘思宇[5]等将A*算法的评价函数结合路径长短，安全度以及稳定性进行改进；Eichhorn[6]等将A*算法应用在具有时变洋流的海洋环境中进行路径搜索；A*算法规划的路径可能是不最优且不可行的，这些路径受到搜索方向和栅格边缘的限制，产生的路径航向更改可能非常繁琐；制导技术比如视线制导(line-of-sight，LOS)，误差约束LOS以及双曲线正切LOS等能够引导AUV朝着沿着期望的稀疏的航路点航行，从而降低频繁的航向更改；高云程[7]等使用平滑A*(A*with post-smoothed，A*PS)算法平滑A*算法规划得到的路径，对由A*算法得到的所有航路点进行LOS检查，从而减少航路点个数使路径更加的平滑；Theta*算法由Daniel[8]提出，通过在搜索过程中添加LOS检查，克服了搜索方向对于路径效果的限制，从而使路径更加平滑更短；肖国宝[9]等结合Theta*算法和A*-PS算法提出PS-Theta*，实现对Theta*算法规划路径的优化.Chrpa[10]等考虑了智能体的动态约束，并将其应用在Theta*算法的搜索过程中；Nash[11]考虑到Theta*算法在高维度空间中应用时引起的计算量激增的问题，提出了lazy Theta*算法，用以降低算法搜索过程中LOS检查的次数，从而节约算法运行的时间成本；Nash[12]提出了一种多层级的Lazy Theta*算法结构，将Lazy Theta*算法应用在了实时的路径规划中；Lazy Theta*算法虽然通过LOS检查解决了搜索方向对路径的限制，但是依然存在一些障碍栅格边缘遮挡引起的路径无法局部最优的问题；以上算法没有考虑优化Lazy Theta*算法本身存在的局部不最优的问题，而且没有考虑在三维崎岖海底环境中的应用，因此面向崎岖海底环境的AUV三维安全路径规划是一个热点研究问题。
Lazy Theta*算法虽然通过LOS检查解决了搜索方向对路径的限制，但是依然存在一些障碍栅格边缘遮挡引起的路径无法局部最优的问题.已有的算法没有考虑优化LazyTheta*算法本身存在的局部不最优的问题。
发明内容
根据现有技术存在的问题，本发明公开了一种针对三维环境下的水下机器人路径规划方法，包括以下步骤：
建立近水底环境的三维模型；
定义AUV将要运行路线的起点和终点，定义PSLT算法的搜索方向，基于近水底环境的三维模型，从起点开始，运行PSLT算法进行路径的最优航路点搜索，得到路径最优航路点集合，通过路径航路点最优点集合的最优点形成最优路径。
进一步地，所述PSLT算法的启发函数表达式如下：
f(gi)＝g(gi)+h(gi)+λ1s(gi)+λ2p(gi) (2)
其中：gi是当前待扩展节点，g(gi)是在状态空间R中从初始节点到gi节点的实际消耗代价，h(gi)是从gi路径点到目标顶点的估计代价，s(gi)是待扩展节点的安全性评价，p(gi)是节点的水压评价，λ1与λ2分别是安全性与水压评价的权重系数。
进一步地，所述在状态空间R中从初始节点到gi节点的实际消耗代价g(gi)的表达式如下：
Figure BDA0003025212270000031
其中，(xk,yk,zk)表示表示的是gk航路点在三维状态空间中的坐标，(xk+1,yk+1,zk+1)表示gk+1航路点在三维状态空间中的坐标。
进一步地，从gi路径点到目标顶点的估计代价h(gi)的表达式如下：
Figure BDA0003025212270000032
其中：gt(x,y,z)和now(x,y,z)分别表示目标顶点与当前顶点在状态空间中的坐标。
进一步地，所述待扩展节点的安全性评价s(gi)的表达式如下：
Figure BDA0003025212270000033
其中：d表示当前航路点与障碍物之间的最短距离。
进一步地，所述从起点开始，运行PSLT算法进行最优点搜索的过程如下：
定义集合Sopen和集合Sclose分别用于放置待扩展的节点和已扩展的节点；
对当前节点gi进行扩展，将当前节点从集合Sopen中移除并加入到集合Sclose，即：
Figure BDA0003025212270000034
对于所有的未展开的子节点(g'∈chd(gi)且
Figure BDA0003025212270000035
)，循环判断当前节点gi的父节点的g值加上当前节点gi的父节点到各个子节点的代价是否小于各个子节点自身的g值，即判断g(prt(gi))+c(prt(gi),g')＜g(g')是否为真，其中c(prt(gi),g')表示两点之间的欧式距离，若当前节点gi的父节点的g值加上当前节点gi的父节点到各个子节点的代价小于各个子节点自身的g值，更新此子节点的父节点为其祖父节点，即prt(g')＝prt(gi)；否则将当前节点视为子节点的父节点，即prt(g')＝gi，此时将所有的当前子节点添加进Sopen集合，即：
Sopen←g' (9)
然后对当前节点的下一个子节点进行上述判断过程，直到所有的子节点判断完毕，跳出循环；
在扩展完当前节点后，从Sopen集合中选取最优的待扩展节点(即f值最小的节点)进行扩展，即：
Figure BDA0003025212270000041
选择完将要扩展的节点后，判断gi和prt(gi)之间是否具有LOS，若gi和prt(gi)之间有LOS，则返回S2，若gi和prt(gi)之间没有LOS，则更新当前节点的父节点和当前节点的g值，更新的父节点选自同时处于当前节点搜索范围内与Sclose中的节点，即prt(gi)∈nghbr(gi)∩Sclose，然后选择其中g值与到当前节点代价值之和最小的节点作为当前节点的父节点，同时g值更新为父节点g值与到当前节点的代价值之和：
c:＝||prt(gi)+gi|| (11)
在搜索到终点时，即gi＝gt，得到一个待优化的航路点集合；
Gfir＝{gs,…,gi,…gt} (12)
将航路点集合Gfir＝{gs,…,gi,…gt}作为输入，从第一个航路点开始，将第一个航路点作为当前点gi，判断从目标点依次向前的航路点与当前点之间是否有LOS，即判断LOS(gi,gI)是否为真，若LOS(gi,gI)为真，将当前点添加到Ggp＝{gs,…gi}，并更改当前点为LOS另一端的点，即gi＝gI，依次类推，直到当前点和目标点之间存在LOS为止，最后将目标点gt加入到Ggp集合中，Ggp便是得到的全局路径。
由于采用了上述技术方案，本发明提供的一种针对三维环境下的水下机器人路径规划方法，PSLT算法继承了Lazy Theta*减少LOS检查和无角度限制规划的优点，从规划路径的长短来看，PSLT算法规划的路径长度要比Lazy Theta*算法规划的路径长度短，要比Theta*算法规划的路径略长，从算法消耗时间来看，A*与Theta*算法消耗时间远远大于lazy Theta*算法与PSLT算法的消耗时间，这个和LOS检查次数的大大减少以及遍历的空间变少有很大关系，从路径的平滑度来看，PSLT由于平滑操作，使得航路点大大减少，进而提升了路径的平滑度；综合多组仿真数据，PSLT算法在保证了路径的长短的同时，提升了AUV航行的路径平滑度和算法运行效率。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明的智能体搜索方向示意图；
图2为本发明的LOS示意图；
图3为本发明的Lazy Theta*算法缺陷示意图；
图4为本发明的Post-smoothed算法流程图；
图5(a)为本发明的四种算法仿真对比图；(b)为本发明的四种算法仿真对比三视图。
具体实施方式
为使本发明的技术方案和优点更加清楚，下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚完整的描述：
一种针对三维环境下的水下机器人路径规划方法，包括以下步骤：
S1：建立近水底环境的三维模型；
S2:定义AUV(自主式水下航行器)将要运行路线的起点和终点，定义PSLT算法的搜索方向，基于近水底环境的三维模型，从起点开始，运行PSLT算法进行路径的最优航路点搜索，得到路径最优航路点集合，通过路径航路点最优点集合的最优点形成最优路径。
步骤S1和S2顺序执行；
通常路径规划是在状态空间中进行，在栅格化的三维地形中，定义了可行域空间，水下机器人在进行路径点选择时只需要在可行空间栅格中选择栅格点，将所有选择的栅格点依次连接并分别连接起点终点栅格就形成了一条由起点至终点的路径；即：
Ggp＝{gs,…,gi,…,gt} (1)
其中，gi(0≤i≤n)是算法产生的任一航路点，n是算法产生的航路点总数，其中gs和gt分别表示起始点栅格与目标点栅格，gi表示路径栅格中的任一栅格。
为了简化研究问题，使问题更具针对性，将水下机器人视为一个质点，在栅格地图上表示为一个栅格顶点。启发式算法需要先对地图进行搜索，结合三维栅格地图，定义每一次搜索均有26个搜索方向，26个搜索方向图1所示，图1中中心方块代表当前栅格，其他方块表示在当前方块位置时可以进行搜索的方向，共计26个搜索方向，这些搜索方向根据距离的远近给予不同的代价值。
本文使用的LOS检查方法是用来检测两个直线相连的栅格之间是否有障碍栅格，如图2所示，如果有障碍栅格，则两栅格之间无LOS，反之则两栅格之间有LOS，LOS指的是视线制导。
PSLT算法是一种A*类启发式算法，此算法结合了Lazy theta*算法和Post-smoothed算法，首先要进行对状态空间的搜索，并使用启发函数引导搜索过程的进行，通过给状态空间R中各个节点赋以启发值，并给搜索到的各个节点定义父节点，在搜索到终点时根据父级回溯法，得到一条自终点至起点的路径，然后将得到的路径点集合作为输入，输入给Post-smoothed算法，得到新的一组航路点，便是PSLT算法的最优航路点集合。
PSLT算法的启发函数形式如下：
f(gi)＝g(gi)+h(gi)+λ1s(gi)+λ2p(gi) (2)
其中，gi是当前待扩展节点，prt(gi)是gi的父节点，即gi是prt(gi)其中一个子节点，定义prt(g0)＝gs，g(gi)是在状态空间R中从初始节点到gi节点的实际消耗代价(即从初始路径点到gi路径点的最优路径的代价)，h(gi)是从gi路径点到目标顶点的估计代价，s(gi)是待扩展节点的安全性评价，p(gi)是节点的水压评价，λ1与λ2分别是安全性与水压评价的权重系数。即：
Figure BDA0003025212270000061
其中，(xi,yi,zi)表示的是gi航路点在三维状态空间中的坐标，(xk+1,yk+1,zk+1)表示gk+1航路点在三维状态空间中的坐标，当前航路点的集合为[gs,g0,g1,…,gi-1,gi(nownode)]；
Figure BDA0003025212270000062
其中gt(x,y,z)和now(x,y,z)分别表示目标顶点与当前顶点在状态空间中的坐标。
节点的安全性使用节点与最近障碍物的距离来表示，距离从近到远分为三个等级：危险，警示以及安全。
Figure BDA0003025212270000071
其中，d表示当前航路点与障碍物之间的最短距离，当处于危险距离时，智能体离障碍物越近拿到的负面评价就越大，当处于警示距离时，路径点的评价值随着距离增大而增大，当处于安全距离时，给路径点一个足够大的恒定正面评价。
由于AUV运行在水下环境中，所以水压会对机器人的运行造成一定程度上的影响，因此将水压纳入启发函数的评价项，通常情况下水压大机器人运行损耗大，水压小机器人运行损耗小，假设水压与机器人运行的损耗之间假设是线性相关
p(gi)＝-ρgphi (6)
其中，ρ≈1.02g/cm3为上式中的海水密度，gp≈9.8m/s2为重力加速度，hi为nodei节点处水深，因为水压与水深成正比，但是水压与评价成反比，因此前边取负。
进一步地，所述从起点开始，运行PSLT算法进行最优点搜索的过程如下：
S1:设计两个集合Sopen和Sclose分别放置待扩展的节点和已扩展的节点，具体来讲，Sopen放置已被搜索到但未进行扩展的节点，在进行选择当前最优候选路径点时，便从这个集合中取值。Sclose放置已被扩展过的节点，使其不再被使用。两种集合的存储结构为：
S:＝{g:[prt(g),[f,g,h,s,p]]} (7)
这样便将每个航路点对应的各个值都分离表示出来，便于进行比较排列；
S2：对当前节点gi进行扩展，将当前节点从集合Sopen中移除并加入到集合Sclose，即：
Figure BDA0003025212270000072
对于所有的未展开的子节点(g'∈chd(gi)且
Figure BDA0003025212270000073
)，循环判断当前节点gi的父节点的g值加上当前节点gi的父节点到各个子节点的代价是否小于各个子节点自身的g值，即判断g(prt(gi))+c(prt(gi),g')＜g(g')是否为真，其中c(prt(gi),g')表示两点之间的欧式距离，若当前节点gi的父节点的g值加上当前节点gi的父节点到各个子节点的代价小于各个子节点自身的g值，更新此子节点的父节点为其祖父节点，即prt(g')＝prt(gi)；否则将当前节点视为子节点的父节点，即prt(g')＝gi，此时将所有的当前子节点添加进Sopen集合，即：
Sopen←g' (9)
然后对当前节点的下一个子节点进行上述判断过程，直到所有的子节点判断完毕，跳出循环；
S3:为了递归得到最小的启发值f，在扩展完当前节点后，从Sopen集合中选取最优的待扩展节点(即f值最小的节点)进行扩展，即：
Figure BDA0003025212270000081
S4:在选择完将要扩展的节点后，判断gi和prt(gi)之间是否具有LOS，若有LOS，则继续循环上述过程，若没有LOS，则更新当前节点的父节点和当前节点的g值，更新的父节点选自同时处于当前节点搜索范围内与Sclose中的节点，即prt(gi)∈nghbr(gi)∩Sclose，然后选择其中g值与到当前节点代价值之和最小的节点作为当前节点的父节点，同时g值更新为父节点g值与到当前节点的代价值之和如下：
c:＝||prt(gi)+gi|| (11)
S5:在搜索到终点时，即gi＝gt，得到一个待优化的航路点集合：
Gfir＝{gs,…,gi,…gt} (12)
S6:将上述航路点集合作为Post-smoothed算法的输入,然后进入航路点优化更新循环，首先从第一个航路点开始，将第一个航路点作为当前点gi，判断从目标点依次向前的航路点与当前点之间是否有LOS，即判断LOS(gi,gI)是否为真，若为真，将当前点添加到Ggp＝{gs,…gi}，并更改当前点为LOS另一端的点，即gi＝gI。依次类推，直到当前点和目标点之间存在LOS为止，最后将目标点gt加入到Ggp集合中，Ggp便是算法得到的全局路径。
Lazy Theta*算法结合A*算法的启发式搜索思想以及Basic Theta*使用LOS检查克服搜索方向限制的思想，并大大减少了LOS检查次数，但是lazy Theta*算法由于障碍物边缘遮挡会产生局部路径不能最优的问题，如图3所示；图3定义算法的搜索方向为上下左右前后六个方向，蓝色的点是搜索过程确定的航路点，其箭头指向的顶点表示此箭头尾部顶点的父顶点，Path1是lazy Theta*规划出来的路径，具体的搜索过程如下：从起始点A1b开始依次搜索到点A2b和点B2b作为航路点，此时根据LOS确定了B2b的父顶点为A1b，但从顶点B2b扩展到顶点B3b时，因为顶点B3b与顶点B2b的父顶点A1b之间没有LOS，因此形成了Path1的路径，明显可以发现Path2路径效果更优。
针对以上lazy Theta*在局部路径不最优的问题，本专利提出了一种PSLT算法，结合经典lazy Theta*算法和A*-PS算法的思想，PSLT算法将Lazy Theta*算法搜索的航路点集合作为Post-smoothed算法的输入，算法流程如图4所示。
首先从第一个航路点开始，将第一个航路点作为当前点，判断从目标点依次向前的航路点与当前点之间是否有LOS，若有，则将当前点更改为LOS另一端的点，以此类推，直到当前点为目标点为止，其中所有遍历的当前点便是经过PSLT算法产生的新的航路点集合了。
图5(a)为本发明的四种算法仿真对比图；(b)为本发明的四种算法仿真对比三视图和下表1所示，PSLT算法继承了lazy Theta*减少LOS检查和无角度限制规划的优点；从规划路径的长短来看，PSLT算法规划的路径长度要比Lazy Theta*算法规划的路径长度短，要比A*算法与Theta*算法规划的路径略长。从算法消耗时间来看，A*与Theta*算法消耗时间远远大于lazy Theta*算法与PSLT算法的消耗时间，这个和LOS检查次数的大大减少以及遍历的空间变少有很大关系。从路径的平滑度来看，从图5中可以明显发现，PSLT由于平滑操作，使得航路点大大减少，进而提升了路径的平滑度。综合多组仿真数据，PSLT算法在保证了路径的长短的同时，提升了AUV航行的路径平滑度和算法运行效率。
Figure BDA0003025212270000091
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。
[1]徐玉如，李彭超.水下机器人发展趋势[J].自然杂志，2011，33(003)：125-132.
XU Y R,LI P C.Underwater robot development trend[J].Chinese journalof nature,2011,33(003):125-132.(in Chinese).
[2]曲探宙.我国海洋科技创新发展的回顾与思考[J].海洋开发与管理，2017(10).
QU T Z.Retrospect and reflection on innovation and d-evelopment ofMarine science and technology in Chin-a[J].Marine development and management,2017(10).(in Chinese)
[3]朱大奇，颜明重.移动机器人路径规划技术综述[J].控制与决策,2010,25(007)：961-967.
ZHU D Q,YAN M Z.Survey on technology of mobile robot path planning[J].Control and Decision,2010,25(007):961-967.(in Chinese)
[4]HART P E,NILSSON N J,RAPHAELl B,et al.A Formal Basis for theHeuristic Determination of Minimum Cost Paths[J].IEEE Transactions on SystemsScience&Cybernetics,1972,4(2):28-29.
[5]潘思宇,徐向荣.基于A*算法的机器人二维与三维路径规划实现[J].井冈山大学学报(自然科学版),2015,036(005)：84-88.
PAN S Y，XU X R.2D and 3D robot path planning on the A*algorithm[J],2015,036(005):84-88.(in Chinese)
[6]Eichhorn M.Optimal routing strategies for autonomous underwatervehicles in time-varying environment[J].Robotics&Autonomous Systems,2015,67(May 2015):33-43.
[7]WANG N,GAO Y,ZHENG Z,et al.A Hybrid Path-Planning Scheme for anUnmanned Surface Vehicle[C].2018Eighth International Conference onInformation Science and Technology(ICIST).2018.
[8]DANIEL K,NASH A,KOEING S,et al.Theta*:Any-Angle Path Planning onGrids[J].Journal of Artificial Intelligence Research,2010,39(1):533-579.
[9]肖国宝,严宣辉.一种基于改进Theta*的机器人路径规划算法[J].智能系统学报,2013,8(1)：58-65.
XIAO B G,YAN X H.A path planning algorithm based on improved Theta*for mobile robot[J].CAAI Transaction on Intelligent Systems.2013,8(1):58-65.(in Chinese)
[10]OSBORNE H.Towards a Trajectory Planning Concept:Augmenting PathPlanning Methods by Considering Speed Limit Constraints[M].Kluwer AcademicPublishers,2014.
[11]NASH A,KOENIG S,TOVEEY C A.Lazy Theta*:Any-Angle Path Planningand Path Length Analysis in 3D[C].Proceedings of the Third Annual Symposiumon Combinatorial Search,SOCS 2010,Stone Mountain,Atlanta,Georgia,USA,July 8-10,2010.DBLP,2010.
[12]ZHANG X,HUANG S,LIANG W,et al.HLT*:Real-time and Any angle PathPlanning in 3D Environm-ent[C].IECON 2019-45th Annual Conference of the IEEEIndustrial Electronics Society.IEEE,2019.
[13]WANG N,XU H.Dynamics-Constrained Global-Local Hybrid PathPlanning of an Autonomous Surface Vehicle[J].IEEE Transactions on VehicularTechnology,2020,69(7):6928-6942.
[14]XIE L,XUE S,ZHANG J,et al.A path planning approach based onmulti-direction A*algorithm for ships navigating within wind farm waters[J].Ocean Engineering,2019,184(JUL.15):311-322.
[15]胡蔷,高立娥,刘卫东,等.基于Dubins曲线和改进A*算法的AUV路径规划方法[J].计算机测量与控制，2016,24(8)
HU Q,GAO L E,LIU W D,et al.AUV path planning method based on improvedA*search and dubins cur-ve[J].Computer measurement&control,2016,24(8).(inChinese)
[16]郝启润，吴韵哲，吴浩峻，等.基于改进A～*GBNN算法的AUV路径规划[J].船舶工程,2020,v.42；No.283(09)：148-152+176.
HAO R Z,WU Y Z,WU H J,et al.AUV Path Planning Based on ImprovedGBNNAlgorithm with A*[J].Ship Engineering,2020,v.42；No.283(09):148-152+176.(inChinese)
[17]LI F,ZLATANOVA S,KOOPMAN M,et al.Universal path planning foranindoor drone[J].Automation in Construction,2018,95:275-283.

Claims (6)

1.一种针对三维环境下的水下机器人路径规划方法，其特征在于，包括以下步骤：
建立近水底环境的三维模型；
定义AUV将要运行路线的起点和终点，定义PSLT算法的搜索方向，基于近水底环境的三维模型，从起点开始，采用PSLT算法进行路径的最优航路点搜索，得到路径最优航路点集合，通过路径航路点最优点集合的最优点形成最优路径。
2.根据权利要求1所述的一种针对三维环境下的水下机器人路径规划方法，其特征在于：所述PSLT算法的启发函数表达式如下：
f(gi)＝g(gi)+h(gi)+λ1s(gi)+λ2p(gi) (2)
其中：gi是当前待扩展节点，g(gi)是在状态空间R中从初始节点到gi节点的实际消耗代价，h(gi)是从gi路径点到目标顶点的估计代价，s(gi)是待扩展节点的安全性评价，p(gi)是节点的水压评价，λ1与λ2分别是安全性与水压评价的权重系数。
3.根据权利要求2所述的一种针对三维环境下的水下机器人路径规划方法，其特征在于：所述在状态空间R中从初始节点到gi节点的实际消耗代价g(gi)的表达式如下：
Figure FDA0003025212260000011
其中，(xk,yk,zk)表示表示的是gk航路点在三维状态空间中的坐标，(xk+1,yk+1,zk+1)表示gk+1航路点在三维状态空间中的坐标。
4.根据权利要求2所述的一种针对三维环境下的水下机器人路径规划方法，其特征在于：从gi路径点到目标顶点的估计代价h(gi)的表达式如下：
Figure FDA0003025212260000012
其中：gt(x,y,z)和now(x,y,z)分别表示目标顶点与当前顶点在状态空间中的坐标。
5.根据权利要求2所述的一种针对三维环境下的水下机器人路径规划方法，其特征在于：所述待扩展节点的安全性评价s(gi)的表达式如下：
Figure FDA0003025212260000021
其中：d表示当前航路点与障碍物之间的最短距离。
6.根据权利要求1所述的一种针对三维环境下的水下机器人路径规划方法，其特征在于：所述从起点开始，运行PSLT算法进行最优点搜索的过程如下：
定义集合Sopen和集合Sclose分别用于放置待扩展的节点和已扩展的节点；
对当前节点gi进行扩展，将当前节点从集合Sopen中移除并加入到集合Sclose，即：
Figure FDA0003025212260000024
对于所有的未展开的子节点(g'∈chd(gi)且
Figure FDA0003025212260000022
)，循环判断当前节点gi的父节点的g值加上当前节点gi的父节点到各个子节点的代价是否小于各个子节点自身的g值，即判断g(prt(gi))+c(prt(gi),g')＜g(g')是否为真，其中c(prt(gi),g')表示两点之间的欧式距离，若当前节点gi的父节点的g值加上当前节点gi的父节点到各个子节点的代价小于各个子节点自身的g值，更新此子节点的父节点为其祖父节点，即prt(g')＝prt(gi)；否则将当前节点视为子节点的父节点，即prt(g')＝gi，此时将所有的当前子节点添加进Sopen集合，即：
Sopen←g' (9)
然后对当前节点的下一个子节点进行上述判断过程，直到所有的子节点判断完毕，跳出循环；
在扩展完当前节点后，从Sopen集合中选取最优的待扩展节点(即f值最小的节点)进行扩展，即：
Figure FDA0003025212260000023
选择完将要扩展的节点后，判断gi和prt(gi)之间是否具有LOS，若gi和prt(gi)之间有LOS，则返回S2，若gi和prt(gi)之间没有LOS，则更新当前节点的父节点和当前节点的g值，更新的父节点选自同时处于当前节点搜索范围内与Sclose中的节点，即prt(gi)∈nghbr(gi)∩Sclose，然后选择其中g值与到当前节点代价值之和最小的节点作为当前节点的父节点，同时g值更新为父节点g值与到当前节点的代价值之和：
c:＝||prt(gi)+gi|| (11)
在搜索到终点时，即gi＝gt，得到一个待优化的航路点集合；
Gfir＝{gs,…,gi,…gt} (12)
将航路点集合Gfir＝{gs,…,gi,…gt}作为输入，从第一个航路点开始，将第一个航路点作为当前点gi，判断从目标点依次向前的航路点与当前点之间是否有LOS，即判断LOS(gi,gI)是否为真，若LOS(gi,gI)为真，将当前点添加到Ggp＝{gs,…gi}，并更改当前点为LOS另一端的点，即gi＝gI，依次类推，直到当前点和目标点之间存在LOS为止，最后将目标点gt加入到Ggp集合中，Ggp便是得到的全局路径。
CN202110414188.4A 2021-04-16 2021-04-16 一种针对三维环境下的水下机器人路径规划方法 Active CN113110530B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110414188.4A CN113110530B (zh) 2021-04-16 2021-04-16 一种针对三维环境下的水下机器人路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110414188.4A CN113110530B (zh) 2021-04-16 2021-04-16 一种针对三维环境下的水下机器人路径规划方法

Publications (2)

Publication Number Publication Date
CN113110530A true CN113110530A (zh) 2021-07-13
CN113110530B CN113110530B (zh) 2023-11-21

Family

ID=76718138

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110414188.4A Active CN113110530B (zh) 2021-04-16 2021-04-16 一种针对三维环境下的水下机器人路径规划方法

Country Status (1)

Country Link
CN (1) CN113110530B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117079118A (zh) * 2023-10-16 2023-11-17 广州华夏汇海科技有限公司 基于视觉检测的水下行走检测方法及系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101370649B1 (ko) * 2012-09-04 2014-03-10 주식회사 한화 무인잠수정의 경로 제어방법
CN106444835A (zh) * 2016-10-11 2017-02-22 哈尔滨工程大学 基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法
JP2018177074A (ja) * 2017-04-18 2018-11-15 国立大学法人 東京大学 自律型水中ロボット及びその制御方法
CN109782779A (zh) * 2019-03-19 2019-05-21 电子科技大学 基于种群超启发式算法的洋流环境下auv路径规划方法
CN110209152A (zh) * 2019-06-14 2019-09-06 哈尔滨工程大学 智能水下机器人垂直面路径跟随的深度强化学习控制方法
CN110362089A (zh) * 2019-08-02 2019-10-22 大连海事大学 一种基于深度强化学习和遗传算法的无人船自主导航的方法
CN111831011A (zh) * 2020-08-07 2020-10-27 大连海事大学 一种水下机器人平面轨迹跟踪控制的方法
CN112327885A (zh) * 2020-12-01 2021-02-05 大连海事大学 一种无人船自适应全局-局部混合路径规划的方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101370649B1 (ko) * 2012-09-04 2014-03-10 주식회사 한화 무인잠수정의 경로 제어방법
CN106444835A (zh) * 2016-10-11 2017-02-22 哈尔滨工程大学 基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法
JP2018177074A (ja) * 2017-04-18 2018-11-15 国立大学法人 東京大学 自律型水中ロボット及びその制御方法
CN109782779A (zh) * 2019-03-19 2019-05-21 电子科技大学 基于种群超启发式算法的洋流环境下auv路径规划方法
CN110209152A (zh) * 2019-06-14 2019-09-06 哈尔滨工程大学 智能水下机器人垂直面路径跟随的深度强化学习控制方法
CN110362089A (zh) * 2019-08-02 2019-10-22 大连海事大学 一种基于深度强化学习和遗传算法的无人船自主导航的方法
CN111831011A (zh) * 2020-08-07 2020-10-27 大连海事大学 一种水下机器人平面轨迹跟踪控制的方法
CN112327885A (zh) * 2020-12-01 2021-02-05 大连海事大学 一种无人船自适应全局-局部混合路径规划的方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
"基于蚁群算法和人工势场法的水下机器人路径规划研究" *
ZHU, HUANGCHAO, RESEARCH ON FUZZY CONTROL OF PATH TRACKING FOR UNDERWATER VEHICLE BASED ON GENETIC ALGORITHM OPTIMIZATION *
于飞, 三维海底栅格地形在潜器路径规划中的应用 *
张巧荣, 智能水下机器人分层全局路径规划方法研究_张巧荣 *
张楠楠, 水下机器人路径规划与路径跟踪方法研究 *
赵蕊, 多自主式水下机器人的路径规划和控制技术研究综述 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117079118A (zh) * 2023-10-16 2023-11-17 广州华夏汇海科技有限公司 基于视觉检测的水下行走检测方法及系统
CN117079118B (zh) * 2023-10-16 2024-01-16 广州华夏汇海科技有限公司 基于视觉检测的水下行走检测方法及系统

Also Published As

Publication number Publication date
CN113110530B (zh) 2023-11-21

Similar Documents

Publication Publication Date Title
CN108680163B (zh) 一种基于拓扑地图的无人艇路径搜索系统及方法
Song et al. Online inspection path planning for autonomous 3D modeling using a micro-aerial vehicle
Kularatne et al. Going with the flow: a graph based approach to optimal path planning in general flows
CN106503837B (zh) 一种基于改进水平集算法的时间最优航路规划方法
CN112612290B (zh) 一种考虑洋流的水下航行器三维多任务路径规划方法
Ab Wahab et al. Path planning for mobile robot navigation in unknown indoor environments using hybrid PSOFS algorithm
Respall et al. Fast sampling-based next-best-view exploration algorithm for a MAV
Choi et al. Any-angle path planning on non-uniform costmaps
Li et al. Three-dimensional path planning for AUVs in ocean currents environment based on an improved compression factor particle swarm optimization algorithm
Gu et al. An improved RRT algorithm based on prior AIS information and DP compression for ship path planning
CN114705196B (zh) 一种用于机器人的自适应启发式全局路径规划方法与系统
CN113341998A (zh) 一种改进蚁群算法的三维水下欠驱动auv路径规划方法
Gu et al. Path planning for mobile robot in a 2.5‐dimensional grid‐based map
CN113110530B (zh) 一种针对三维环境下的水下机器人路径规划方法
Rocha et al. A uav global planner to improve path planning in unstructured environments
Doshi et al. Energy–time optimal path planning in dynamic flows: Theory and schemes
Li et al. Dynamic route planning for a USV-UAV multi-robot system in the rendezvous task with obstacles
Teng et al. A dynamic path planning method for terrain-aided navigation of autonomous underwater vehicles
Wang et al. Research on sbmpc algorithm for path planning of rescue and detection robot
Jung et al. Collision‐free local planner for unknown subterranean navigation
El-Hussieny et al. Improved backtracking algorithm for efficient sensor-based random tree exploration
Jung et al. Enabling operational autonomy for unmanned aerial vehicles with scalability
Aguiar et al. Optimizing autonomous underwater vehicle routes with the aid of high resolution ocean models
CN114596360A (zh) 一种基于图拓扑的双阶段主动即时定位与建图算法
Liu et al. An autonomous quadrotor exploration combining frontier and sampling for environments with narrow entrances

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
GR01 Patent grant
GR01 Patent grant