CN112799405B - 基于动态障碍物环境下的无人船路径规划方法 - Google Patents

Abstract

本发明公开的是一种基于动态障碍物环境下的无人船路径规划方法，适用于湖泊水质采样以及路径规划领域。首先采用无人机对湖面环境进行图像采集，然后用栅格法进行分割，并在栅格地图上设置起始点与采样点；采用改进的D*Lite算法对起始点与采样点进行路径规划；得到最优采样路径后对其进行平滑处理；最后，无人船按照该最优路径移至采样点后并返航。本发明通过对D*Lite算法中的启发函数改进，以及路径平滑处理，满足无人船在动态未知环境中的路径规划需求，有效解决传统D*Lite算法中扩展节点数目较多的情况，极大缩短了路径规划时间，减少设备消耗成本。

Description

基于动态障碍物环境下的无人船路径规划方法

技术领域

本发明主要属于湖泊水质采样以及动态路径规划领域，具体是一种能够解决动态障碍物环境下的路径规划方法。

背景技术

随着科技文明的发展，人类生活工作对环境的影响问题越来越多，其中对水环境的影响尤为突出，因此对水环境采样监测工作极为重要。随着智能化程度的提高，现今大多采用无人船对水质进行采样。但是无人船的路径规划问题一直是该领域诸多学者关注的重难点，动态环境中的路径选择直接影响了巡航船的采样效率与成本。寻找一种能够适应未知环境并快速规划出采样最优路径的方法能够显著提高无人船的行进采样效率以及降低采样成本。

当前已经有许多传统方法和智能方法来解决无人船的路径规划问题，比如遗传算法、蚁群算法和LPA*算法等。

D*Lite算法是2005年提出的一种基于LPA*算法的改进算法，LPA*采用正向搜索方式，对定起始点和定目标点进行路径规划，但遇到新的障碍物后需要以当前位置为起始点重新规划路径，效率低下，不能应对动态环境。而D*Lite算法采用反向搜索方式，能够解决动态未知环境下的路径规划问题，由于D*Lite的增量规划思想，可以做到较少的重规划次数，因此更适合动态的环境。但环境地图比较大时，传统D*Lite算法反向搜索过程中需要扩展判定的节点数急剧增加，增加了搜索的时间复杂度，从而影响了路径规划的效率。

发明内容

本发明针对无人船的在动态环境中的快速路径规划需求，通过对传统D*Lite算法进行改进，提出一种基于动态障碍物环境下的无人船路径规划方法，充分利用传统D*Lite算法的优势，并对其缺点进行优化改进，缩短路径规划所消耗的时间，减少设备成本，并加入了路径平滑过程，有效地解决了未知动态环境中的无人船路径规划问题。

本发明基于动态障碍物环境下的无人船路径规划方法，具体步骤如下：

步骤1：对湖面的环境进行图像采集，并通过栅格法进行分割，得到湖面二维坐标栅格地图并设置起始点、目标点和多个采样点。

步骤2：计算得到栅格地图中起始点与目标点之间的最优栅格路径。

201：对每个栅格进行初始化，其中将所有栅格的rhs(s)值和g(s)值设置为∞，目标点的rhs(s)值和环境变化时更新变量k_m值设置为0。

202：从栅格地图的目标节点s_goal开始向周围8个相邻节点扩展，通过比较相邻节点的估值函数k(s)，选取k(s)值最小的点作为下一扩展节点；k(s)包含k₁(s)和k₁(s)

k₁(s)＝min(g(s),rhs(s))+h(s_start,s)+k_m

k₂(s)＝min(g(s),rhs(s))

式中，h(s_start,s)为启发函数，表示起始点s_start到当前节点s的路径代价，启发函数的变化直接影响估值函数的变化。

扩展节点时，先比较k(1)值大小并选取k(1)值最小的点作为下一扩展节点，当k(1)值相等时比较并选择k(2)值最小的点作为下一节点。

203：将下一个扩展节点作为当前节点，继续扩展相邻节点，直到到达起始点s_start，得到一条从节点s_goal到节点s_start之间的路径；节点s_start初始值为栅格地图的起始点。

204：针对当前节点s_start，计算节点s_start所有后继节点的rhs(s)值，选取rhs(s)最小的节点作为新起始点s'_start，无人船移动到该点s'_start。

步骤205：扫描栅格地图，更新节点信息，判断周围环境是否改变，若未发生变化进入步骤206；否则进入步骤208。

步骤206：判断当前节点是否为目标节点s_goal；如是，则终止算法，获得最优栅格路径。否则进入步骤207。

步骤207：将无人船移动到新的起始点s′_start并再次返回步骤202,得到从节点s_goal到新的起始点s′_start的路径，并继续移动无人船的位置，直至无人船到达节点s_goal。

步骤208：更新估值函数k(s)中的变量k_m值和节点s'_start，返回步骤202。

步骤3：对获得的最优栅格路径进行平滑处理，来消除部分拐点。

无人船从栅格地图对应的起始点开始巡航，沿着优化后的栅格路径移动至目标采样点并返回起始点完成巡航。

本发明的优点在于：

(1)本发明基于动态障碍物环境下的无人船路径规划方法，将湖泊环境转变成二维坐标栅格图，简化了地图，缩短了水质采样领域中无人船的路径规划时间。

(2)本发明基于动态障碍物环境下的无人船路径规划方法，对传统D*Lite算法进行改进，满足无人船在动态未知环境中的路径规划需求，有效解决传统D*Lite算法中扩展节点数目较多的情况，极大缩短了路径规划时间，减少设备消耗成本。

(3)本发明基于动态障碍物环境下的无人船路径规划方法，对规划后的路径才去了平滑处理，减少了无人船的控制难度与动力消耗。

附图说明

图1为本发明基于动态障碍物环境下的无人船路径规划方法流程图；

图2为传统D*Lite算法的具体过程示意图；

图3为改进D*Lite算法的具体过程示意图。

具体实施方式

下面结合附图对本发明进行详细说明。

步骤1：采用无人机对湖面的环境进行图像采集，然后对采集的湖面图像通过栅格法进行分割，得到湖面二维坐标栅格地图并设置起始点、目标点和多个采样点。

具体为：首先，将湖面图像分割成大小相等的栅格，组成一个栅格地图；并将环境中的障碍物设置为黑色栅格，将可行区域设置为白色栅格，根据实际需求，在栅格地图中运用二维坐标选取和设置起始点、目标点和多个采样点。

步骤2：采用改进后的D*Lite算法计算得到栅格地图中起始点与目标点之间的最优栅格路径。

如图1所示，具体过程为：

步骤201：对每个栅格进行初始化，其中将所有栅格的路径代价函数rhs(s)值和g(s)值设置为∞，目标点的路径代价函数rhs(s)值和环境变化时更新变量k_m值设置为0，当周围环境变化时,k_m改变，m即指环境变化的第m次。同时根据获取的湖面二维坐标栅格地图，设定D*Lite算法的相关参数，其中，每个栅格点分别作为一个节点；

步骤202：从栅格地图的目标节点s_goal开始向周围8个相邻节点扩展，通过比较相邻节点的估值函数k(s)，选取k(s)值最小的点作为下一扩展节点。

所述k(s)包含k₁(s)和k₂(s)，其中：

k₁(s)＝min(g(s),rhs(s))+h(s_start,s)+k_m

k₂(s)＝min(g(s),rhs(s))

在扩展节点时，先比较k(1)值大小并选取k(1)值最小的点作为下一扩展节点，当k(1)值相等时比较并选择k(2)值最小的点作为下一节点。上式中，h(s_start,s)为启发函数，表示起始点s_start到当前节点s的路径代价，启发函数的变化直接影响估值函数的变化。h(s_start,s)与k_m两者的表达式如下：

k_m＝k_m+h(s_last,s_start)

在传统D*Lite算法中，估值函数k(s)决定着节点是否被扩展，在扩展节点时，会优先比较相邻节点的k₁(s)值，而k₁(s)包含着启发函数h(s_start,s)，因此算法的搜索效率会直接受到启发函数的影响。

由于传统D*Lite算法的启发函数h(s_start,s)采用的是棋盘距离，在目标点附近扩展节点时，容易出现多个节点的k(s)值相同的情况。如图2所示，黑色栅格表示障碍物节点，浅灰色栅格表示当前扩展节点，每个节点都包含当前信息。在从目标点扩展节点时，首先是节点信息初始化，Step1中选择E3点作为目标点扩展其相邻节点，由Step1可知，在扩展相邻节点后存在D2、D3、D4三个k(s)值相等且最小的节点，因此需要逐步扩展这三个节点。Step2为D2节点的扩展，Step3为D3节点的扩展，下一步则为D4节点的扩展，可知在D层扩展节点共需要三个步骤。扩展完这三个节点后，又出现了C1、C2、C3三个k(s)值相等且最小的节点。当多个节点相同时，则需要扩展更多节点，增加了计算时间，降低了算法的搜索速率。

为了D*Lite算法扩展更少的节点，提高算法的搜索速率，基于原启发函数h(s_start,s)的基础上提出一个新的启发函数h′，公式如下：

具体改进措施为：将横向或纵向的单位移动代价定义为1，对角线移动代价定义为

在此基础上增加一个权值函数μ，使节点距离起始节点越远，权值函数的值越大，从而降低对距离起始点较远节点扩展的可能性，降低多个节点k(s)值相等的可能性，从而减少扩展节点的数目，提高算法的搜索效率。其中x，y为当前节点的横纵坐标，x_start，y_start为起始节点的横纵坐标，x_goal，y_goal为目标节点的横纵坐标。

如图3所示，改进后的D*Lite算法的具体过程。首先是节点信息初始化，然后扩展目标点E3的相邻节点，计算中采用新的启发函数h′得出的Step1节点信息中，k(s)值最小的节点只有D2一个，不再需对D3、D4两个节点进行扩展，在D层扩展节点仅需要一个步骤。Step2为选择D2节点进行扩展，由扩展结果可知，k(s)值最小的节点为C1，没有多个k(s)值相同的节点。Step3为选择C1节点进行扩展。可以得知，在前三个步骤中,改进启发函数的D*Lite算法减少了两个扩展节点，且Step3的扩展节点距离起始节点更近，因此有着更高的搜索效率。

步骤203：将下一个扩展节点作为当前节点，继续扩展相邻节点，直到到达节点s_start，得到一条从节点s_goal到节点s_start之间的路径；节点s_start初始值为栅格地图的起始点；

步骤204：针对当前节点s_start，计算该节点所有后继节点(步骤202与203扩展过的节点，如附图3中移动路径为A1—B1—C1—D2—E3)的rhs(s)值，选取rhs(s)最小的节点作为新起始点s'_start，无人船移动到该点s'_start。其中路径代价函数rhs(s)表达式如下：

式中，rhs(s)表示当前节点s到目标节点的路径代价，s∈Succ(s)表示后继节点的集合，c(s′,s)表示节点s′到s的实际路径成本，g(s′)表示节点s′到目标节点的路径代价。

步骤205：扫描栅格地图，更新节点信息，判断周围环境是否改变，若未发生变化进入步骤206；否则进入步骤208；

步骤206：判断当前节点是否为目标节点s_goal。如是，则终止算法，获得最优栅格路径。否则进入步骤207。

步骤207：将无人船移动到新的起始点s′_start并再次返回步骤202,得到从节点s_goal到新的起始点s′_start的路径，并继续移动无人船的位置，直至无人船到达节点s_goal。

步骤208：更新估值函数k(s)中的变量k_m值和节点s'_start，返回步骤202。

表达式如下：

k_m＝k_m-1+h(s_last,s'_start)

s_last＝s'_start

上式中，k_m-1为环境变化更新时的第m-1次变量，其中k₀＝0；s_last表示无人船前一个位置节点，h(s_last,s′_start)表示无人船前一个位置点到当前节点s'_start的路径代价。

步骤3：对获得的最优栅格路径进行平滑处理，来消除部分拐点，减小无人船的控制难度和动力消耗。

具体过程为：

步骤301：将计划路径(进行平滑处理前的由上述步骤得到的最优路径)上的每一个节点进行标号，起始点为1，下一个路径点为2，以此类推，目标点为点n。

步骤302：连接1和2，判断连线是否穿过障碍物，确保规划路径(平滑处理后的无人船移动的最终路线)的安全性。

步骤303：检查下一个路径点，直到点1和路径点k(k<n)的连线穿过障碍物。

步骤304：将点1和点k-1作为端点，连接两点，并替换点1到点k-1的原始路径。

步骤305：将点k-1作为新起始点，重复上述步骤，直到到达目标点。

无人船从栅格地图对应的起始点开始巡航，沿着规划路径移动至目标采样点并返回起始点完成巡航。

Claims (2)

1.基于动态障碍物环境下的无人船路径规划方法，其特征在于：通过下述步骤完成：

步骤1：对湖面的环境进行图像采集，并通过栅格法进行分割，得到湖面二维坐标栅格地图并设置起始点、目标点和多个采样点；

步骤2：计算得到栅格地图中起始点与目标点之间的最优栅格路径；

201：对每个栅格进行初始化，其中将所有栅格的rhs(s)值和g(s)值设置为∞，目标点的rhs(s)值和环境变化时更新变量k_m值设置为0；rhs(s)为s到目标节点的路径代价，g(s)值由rhs(s)赋值；

202：从栅格地图的目标节点s_goal开始向周围8个相邻节点扩展，通过比较相邻节点的估值函数k(s)，选取k(s)值最小的点作为下一扩展节点；k(s)包含k₁(s)和k₂(s)

k₁(s)＝min(g(s),rhs(s))+h(s_start,s)+k_m

k₂(s)＝min(g(s),rhs(s))

式中，h(s_start,s)为启发函数，表示起始点s_start到当前节点s的路径代价，启发函数的变化直接影响估值函数的变化；

扩展节点时，先比较k(1)值大小并选取k(1)值最小的点作为下一扩展节点，当k(1)值相等时比较并选择k(2)值最小的点作为下一节点；

基于上述启发函数h(s_start,s)，步骤202中采用新的启发函数h′，公式如下：

将横向或纵向的单位移动代价定义为1；对角线移动代价定义为

在此基础上增加一个权值函数μ，使节点距离起始节点越远，权值函数的值越大，从而降低对距离起始点较远节点扩展的可能性，降低多个节点k(s)值相等的可能性，从而减少扩展节点的数目，提高算法的搜索效率；其中x，y为当前节点的横纵坐标，x_start，y_start为起始节点的横纵坐标，x_goal，y_goal为目标节点的横纵坐标；

203：将下一个扩展节点作为当前节点，继续扩展相邻节点，直到到达起始点s_start，得到一条从节点s_goal到节点s_start之间的路径；节点s_start为栅格地图的起始点；

204：针对当前节点s_start，计算节点s_start所有后继节点的rhs(s)值，选取rhs(s)最小的节点作为新起始点s'_start，无人船移动到该点s'_start；

步骤205：扫描栅格地图，更新节点信息，判断周围环境是否改变，若未发生变化进入步骤206；否则进入步骤208；

步骤206：判断当前节点是否为目标节点s_goal；如是，则终止算法，获得最优栅格路径；否则进入步骤207；

步骤207：将无人船移动到新的起始点s′_start并再次返回步骤202,得到从节点s_goal到新的起始点s′_start的路径，并继续移动无人船的位置，直至无人船到达节点s_goal；

步骤208：更新估值函数k(s)中的变量k_m值和节点s'_start，返回步骤202；

步骤3：对获得的最优栅格路径进行平滑处理，来消除部分拐点。

2.如权利要求1所述基于动态障碍物环境下的无人船路径规划方法，其特征在于：

步骤3的具体过程为：

301：将进行平滑处理前得到的最优路径上的每一个节点进行标号，起始点为1，下一个路径点为2，以此类推，目标点为点n；

302：连接1和2，判断连线是否穿过障碍物，确保平滑处理后的无人船移动的最终路线的安全性；

303：检查下一个路径点，直到点1和路径点k的连线穿过障碍物，k<n；

304：将点1和点k-1作为端点，连接两点，并替换点1到点k-1的原始路径；

305：将点k-1作为新起始点，重复上述步骤，直到到达目标点。

Images