CN110146087A - 一种基于动态规划思想的船舶路径规划方法 - Google Patents

Abstract

本发明公开了一种基于动态规划思想的船舶路径规划方法，属于船舶路径规划领域。本发明步骤包括：(1)径向栅格地图的建立；(2)栅格地图航行信息的赋予；(3)建立评价函数和状态转移方程；(4)利用动态规划思想在径向栅格寻找最优路径；本发明的有益效果在于依据船舶航线建立径向栅格地图，使规划出来的航线更加符合船舶的运动特性，并且有利于使用动态规划算法进行决策阶段的划分；运用动态规划的思想通过建立的评价函数进行条件约束能够规划耗时最少，符合舵角限制，航线与原航线偏差最小且能避开多条障碍船的航线。该算法计算速度快，并且能够得到符合条件约束的最优航线。

Description

一种基于动态规划思想的船舶路径规划方法

技术领域

本发明属于船舶路径规划领域，具体涉及的是一种基于动态规划思想的船舶路径规划方法。

背景技术

随着国民经济的快速发展，我国海域船舶数量急剧增加，船舶交通流量随之增大，海面船舶交通安全事故频繁发生。海上交通事故的原因错综复杂，大部分是由于船只不全面的观测，在采取避免相撞的行为时，因为时间限制和本船、他船或双方采取的避免碰撞的行为不适合以致碰撞事故的发生。因此研究船舶的路径规划算法避免我船与它船发生碰撞具有很深的现实意义和理论意义。

一种简单的做法是对实际环境进行离散分解，使用栅格对环境进行建模。将每个栅格根据实际环境的真实情况划分为可通过栅格和不可通过栅格。确定起始点所在的栅格，以起始点为中心向外搜索扩展，直到扩展到终点。在扩展的过程中，记录可通过的栅格点，从而得到最优路径。这种栅格图搜索的方法以移动机器人或者是小型无人艇为研究对象时，取得了很好的效果，但是不适用于大型船舶。原因是现有栅格地图得到的路径方向的变化增量只能是π/4的倍数，不符合大型船舶的运动学特性。并且海洋不同于城市道路网，它不存在固定的路线限制问题，对大面积的可行区域根本无法构造图，并不能建立最优航线。

一些方法考虑基于需要避碰的障碍物的个数，在航线的起点和终点之间插入相应数量的节点。采用进化算法在航线的起点和终点之间的一定范围内不停的搜索，淘汰不符合条件的节点，选择优良的节点，从而得到理想的航线。这种算法不必建立栅格地图，并且在算法中能够考虑到航线的平滑性问题。但是算法需要设计的算子及其复杂，运算量极其庞大。并且算法的时间复杂度与空间复杂度的大小与设计的算子的优劣有很大的关系。如果不能设计出合适的算子，该方法难以发挥它应有的效果。

发明内容

本发明的目的在于提供一种基于径向栅格点地图与动态规划思想结合的船舶避碰路径规划方法。本发明的目的是这样实现的：

一种基于动态规划思想的船舶路径规划方法，包括以下步骤：

步骤一：构造基于航线的径向栅格地图；

步骤二：为径向栅格地图的每个点赋予航行信息；

步骤三：建立评价函数和状态转移方程；

步骤四：利用动态规划思想评估栅格点回溯最优路径。

其中：

步骤一所述的基于航线的径向栅格地图具体为：

步骤1：预先设置船舶的航线坐标，运用线性插值法将每段航线N等分，得到N+1个插值点，并计算每个插值点的坐标。坐标值为：

(x₀，y₀)，(x₁，y₁)为航线坐标，(x_i0,y_i0)为插值点的坐标。

步骤2：过这N+1个点做线段与航线相交，在线段上取点，并计算点的坐标。

B₁为航线坐标(x₀，y₀)，(x₁，y₁)组成的线段在笛卡尔坐标系下对应的角度，B₂为航线坐标(x₁，y₁),(x₂，y₂)组成的线段在笛卡尔坐标系下对应的角度，B_v为这两角度对应的平均值，R表示路径上搜索宽度半径，MG表示半条线段上点的个数。(x_0i,j，y_0i,j)为以(x₀，y₀)为起点的折线上第i条线段上的第j个点。带入所有航线坐标，通过计算可得到径向栅格地图的所有点a_ij。当航线坐标为多个点时，同样可以按照上述方法处理。

步骤二所述为径向栅格地图的每一个点赋予航行信息具体为：创建结构体数组，赋予结构体数组以下信息，X，Y，ArriveTime，ArriveAngle，NodeEnsgn，TotalPGVal，分别表示该点的横坐标，纵坐标，本船从上一节点到达该点时间的累计值，本船到达该点时的航向角，该点是否为有效节点，本船到达该点的经历的所有节点评价值和的最小值。

步骤三所述的建立评价函数具体为：

步骤1：根据该节点的安全性，时间的经济性，舵角的限制和与原航线的偏差建立如下总评价函数：

E(a_ij)＝w_t·Arrivetime(a_ij)+w_d·ColliDanger(a_ij)+w_p·PathErr(a_ij)+w_c·DirPunish(a_ij) (4)

在上式中a_ij表示第i行第j个节点，Arrivetime(a_ij)，ColliDanger(a_ij)，PathErr(a_ij)，DirPunish(a_ij)分别表示主船到达a_ij节点所用的时间评价函数，与他船危险度评价函数，本船偏航评价函数，以及操舵限制评价函数。w_t，w_d，w_p，w_c分别为这四个函数的权重参数，可以根据实际需要进行调整。

时间评价函数如下：

x_m，y_m分别表示我船的具体坐标，v_m表示我船的速度矢量，x_ij，y_ij表示a_ij点的坐标。

危险度评价函数如下：

上式中DA为当我船驶到a_ij时k条目标船与a_ij节点的最短距离，(x_ok，y_ok)表示第k条目标船的初始位置坐标。v_okx为目标船的在x轴的速度矢量，v_okx为目标船在y轴上的速度矢量。在公式中引入Arrivetime(a_ij)，推演主船在不同节点时，目标船运动的位置。SA表示本船的安全距离。SA的取值要根据本船的形状、大小、航速、天气状况、船员心理等主观条件确定。在计算危险度函数时，需要计算船舶在a_i-1,j到a_i,j节点行进时的危险度函数，对a_i-1,j和a_i,j节点的坐标进行线性插值得到M组值，将M组数值代入上式可得到行进间的船舶碰撞危险度。

偏航评价函数如下：

PathErr(a_ij)＝j(a_ij)-j(a_i，60) (8)

j(a_ij)表示a_i,j节点所在第i层的标号j，j(a_(i,60))表示原航线上的点在第i层的标号j。

操舵限制评价函数如下：

DirPunish(aij)＝j(E_aij)-j(E_ai-1,j) (9)

j(E_aij)表示第i层最优栅格点E_aij的标号j，j(E_ai-1,j)表示i层记录的i-1的最优栅格点E_ai-1,j的标号。用相邻栅格层栅格点的位置偏差可以近似估计船舶在转弯时的舵角限度。

步骤2：构造状态转移方程

根据对路径规划问题的分析得到状态转移方程为：

MinSum[i][j]表示从a_(0,60)节点到a_ij初始节点之间各阶段节点评价值和的最小值。E[i][j]表示a_ij节点的评价值。

步骤四所述的利用动态规划思想评估栅格点回溯最优路径具体为：

步骤1：生成栅格地图，初始化MinSum[1][j]＝E[1][j]，并设置船舶初始位置为第1层最优点，根据评价函数计算第1层所有栅格点的评价值。

步骤2：根据评价函数计算第2层所有栅格点的评价值E[2][j]并记录在数组中,根据状态转移方程得到MinSum[2][j]，并记录使本层节点评价和最小的上一层的节点标号，以便回溯路径。

步骤3：根据评价函数计算第三层所有栅格点的评价值E[3][j]并记录在数组中，遍历MinSum[2][j]，根据状态方程得到MinSum[3][j]，记录使本层评价和为最小的上一层节点的标号j。以此类推直到得到MinSum[2N+1][j]。

步骤4：找到2N+1层使MinSum[2N+1][j]最小的点，并搜索该点记录的上一层节点的标号，以此类推可得到最优路径。

本发明的有益效果在于：

1.本发明根据船舶的运动特性，创造了一种符合船舶航行的径向栅格地图。区别于传统的栅格地图，该地图可以很方便的根据船舶的实际转向能力规划船舶的路径，使得路径更加有效准确；

2.本发明依据船舶的实际航线建立径向栅格地图，不必将电子海图划分为栅格进行搜索，极大的节省了内存空间，提升了算法的运行速度；

3.本发明使用动态规划的思想进行路径点的搜索，可以得到符合评价标准的最优路径。并且在动态规划搜索中计算了每个节点我船到达该节点的航行时间，并根据它计算他船的位置。使算法能够预测他船的运动。但是在本发明中只能预测船舶的直线运动。

附图说明

图1是本发明路径规划效果示意图。

图2是本发明动态规划算法流程图。

具体实施方式

下面结合附图对本发明作进一步的说明：本发明属于船舶路径规划领域，具体涉及的是一种基于动态规划思想的船舶路径规划方法。本发明包括：(1)径向栅格地图的建立；(2)栅格地图航行信息的赋予；(3)建立评价函数和状态转移方程；(4)利用动态规划思想在径向栅格寻找最优路径；本发明的有益效果在于依据船舶航线建立径向栅格地图，使规划出来的航线更加符合船舶的运动特性，并且有利于使用动态规划算法进行决策阶段的划分；运用动态规划的思想通过建立的评价函数进行条件约束能够规划耗时最少，符合舵角限制，航线与原航线偏差最小且能避开多条障碍船的航线。该算法计算速度快，并且能够得到符合条件约束的最优航线。

本发明的步骤如下：

步骤1、生成径向栅格地图。根据我船目前实际航行的航线，生成径向栅格点地图，计算每个栅格点的实际坐标，并且将栅格点与设计的结构体数组进行一一对应。

步骤2、为结构体数组中的变量进行赋值，赋予结构体对应的每个栅格点基本的航行信息，分别为该点的横坐标，纵坐标，本船从上一节点到达该点时间的累计值，本船到达该点时的航向角，该点是否可以为本船路径的最优点，本船到达该点的最优路径的总评价值。

步骤3、为每个栅格点建立评价函数和建立状态转移方程。根据该节点的安全性，时间的经济性，舵角的限制和与原航线的偏差建立总评价函数，并且建立子评价函数。根据实际的需要建立状态转移方程，为决策和状态建立联系。

步骤4、通过动态规划算法寻找最优点。根据评价函数搜索每个阶段的节点的最小评价函数值的和，并记录最优点所在的位置。回溯最优点的位置，依次连接便可以得到最优路径。

实施例：

1.生成径向栅格点地图：

本发明采用笛卡尔坐标系，首先为本船设置一条初始航线，在这条航线的基础上建立径向栅格点地图。假设初始航线是由(x₀，y₀)，(x₁，y₁)，(x₂，y₂)三点依次连接组成的折线。将这两段折线每段都N等分。N可以根据实际效果进行调整。这样可以得到2N+1个点。接着采用线性插值法确定这2N+1个点的坐标。(x₀，y₀)到(x₁，y₁)段插值点坐标公式如下：

(x_i0，y_i0)表示以(x₀，y₀)为起点在折线上第i个插值点的坐标。同理可得(x₁，y₁)到(x₂，y₂)段插值点的坐标。实际航行中选择本船在航线上的位置作为起点。然后分别通过这2N+1个点，做2N+1条线段与折线相交，在线段上均匀取点便得到了需要的径向栅格点地图。线段上各坐标点的求法如下：

首先要确定这2N+1条线段的斜率。若简单的以这2N+1个点为垂足，做线段垂直于已知折线，那么靠近折线中间点的线段就会交叉重叠。因此需要线段的斜率进行处理，规定坐标系为笛卡尔坐标系。处理方式如下：

B₁为航线坐标(x₀，y₀)，(x₁，y₁)组成的线段在笛卡尔坐标系下对应的角度，B₂为航线坐标(x₁，y₁)，(x₂，y₂)组成的线段在笛卡尔坐标系下对应的角度，B_v为这两角度对应的平均值，(x_0i,j，y_0i,j)为以(x₀，y₀)为起点的折线上第i条线段上的第j个点。同理，可得到与第2条折线相交线段上所有点的坐标。以此类推可得到与第K条折线相交线段上所有点的坐标。R表示路径上搜索宽度半径，MG表示半条线段上点的个数，在本发明MG为60。通过上述操作将所有栅格点重新编码，把得到的栅格点序列与结构体数组的下标建立对应关系得到数组的形式为E[i][j+MG]。其中i表示折线上第i个点，j+MG表示与i点相交线段上的点。之所以这样表示是因为在C语言中数组的序号不允许为负。为了方便在后文的叙述中直接以E[i][j]代替E[i][j+MG]，相应的j的取值范围变成了0到120。折线上j的标号变为60。

2.为每个栅格点赋予航行信息：

当得到径向栅格点地图后，要想知道所选栅格点的优劣，就要知道栅格点的航行信息。因此需要为已经建立了与径向栅格地图的物理坐标对应的结构体数组赋予航行信息。他们为X，Y，ArriveTime，ArriveAngle，NodeEnsgn，TotalPGVal，分别表示该点的横坐标，纵坐标，本船从上一节点到达该点时间的累计值，本船到达该点时的航向角，该点是否为有效节点，本船到达该点的经历的所有节点评价值和的最小值。

3.建立评价函数：

根据该节点的安全性，时间的经济性，舵角的限制和与原航线的偏差建立如下总评价函数：

E(a_ij)＝w_t·Arrivetime(a_ij)+w_d·ColliDanger(a_ij)+w_p·PathErr(a_ij)+w_c·DirPunish(a_ij) (14)

在上式中a_ij表示第i行第j个节点。

Arrivetime(a_ij)，ColliDanger(a_ij)，PathErr(a_ij)，DirPunish(a_ij)分别表示到达a_ij节点所用的时间评价函数，与他船危险度评价函数，本船偏航评价函数，以及操舵限制评价函数。w_t，w_d，w_p，w_c分别为这四个函数的权重参数，可以根据实际需要进行调整。

(1)时间评价函数

x_m，y_m分别表示我船的具体坐标，v_m表示我船的速度矢量，x_ij，y_ij表示a_ij点的坐标。

(2)危险度评价函数

上式中DA为当我船驶到a_ij时k条目标船与a_ij节点的最短距离，(x_ok，y_ok)表示第k条目标船的初始位置坐标。v_okx为目标船的在x轴的速度矢量，v_oky为目标船在y轴上的速度矢量。在公式中引入Arrivetime(a_ij),就可以推演在不同节点时，目标船运动的位置。SA表示本船的安全距离。SA的取值要根据本船的形状、大小、航速、天气状况、船员心里等主观条件确定。在计算危险度函数时，需要计算船舶在a_i-1,j到a_i,j节点行进时的危险度函数，只需对上述节点的坐标进行线性插值，将参数代入上式可得到。

(3)偏航评价函数

PathErr(a_ij)＝j(a_ij)-j(a_i，60) (18)

j(a_ij)表示a_ij节点所在第i层的标号j，j(a_(i,60))表示原航线上的点在第i层的标号j。

(4)操舵限制评价函数

DirPunish(aij)＝j(E_aij)-j(E_ai-1,j) (19)

j(E_aij)表示第i层最优栅格点E_aij的标号j，j(E_ai-1,j)表示i层记录的i-1的最优栅格点E_ai-1,j的标号。用相邻栅格层栅格点的位置偏差可以近似估计船舶在转弯时的舵角限度。

4.构造状态转移方程：

确定了径向栅格地图和栅格点的评价函数后，需要把从栅格地图中寻找找最优点转化成多阶段决策过程的最优化问题。在构造栅格地图时，将航线划分为2N+1个阶段，依据这2N+1个阶段进行决策。

根据对路径规划问题的分析得到状态转移方程为：

MinSum[i][j]表示从a_(0,60)节点到a_ij初始节点之间各阶段节点评价值和的最小值。E[i][j]表示a_ij节点的评价值。

5.用动态规划算法寻找最优路径点，包含以下几个步骤：

(1)生成栅格地图，并初始化MinSum[1][j]，并设置船舶初始位置为第一层最优点。

根据评价函数计算第1层所有栅格点的评价值。

E(a_ij)＝w_t·Arrivetime(a_ij)+w_d·ColliDanger(a_ij)+w_p·PathErr(a_ij)+w_c×DirPunish(a_ij) (21)

在上式中取i＝1，j的范围为0到120，在本发明中取w_t为0.003，w_d为6.0，w_p为0.03，w_c为3.0，SA为800m。将E(a_1j)记录在与栅格点相对应的结构体数组E[1][j]中，令MinSum[1][j]＝E[1][j]；记录第一层船舶所在的位置标号j为最优点。

(2)根据评价函数计算第二层所有栅格点的评价值E(a_2j)并记录在数组中，根据状态转移方程：

计算第二层各个节点的评价值与前一层节点最小评价和遍历相加得到MinSum[2][j]，并记录使本层评价和最小的上一层节点的标号j。

(3)判断i是不是小于2N+1，若i小于2N+1，则继续迭代计算第i层和第i-1层的MinSum[i][j],并记录使本层评价和最小的第i-1层的节点的标号j。若i大于2N+1，则停止迭代。

(4)从第2N+1层寻找本层记录的上一层的标号j，以此类推由后向前将标号j依次连接即可得到本船的最优路径。

本发明考虑改进传统的栅格地图结合动态规划的思想，设计出计算速度快、可靠性高，且能够避开多目标动态船舶的算法。具体方法是在船舶设定航线的基础上，建立径向的栅格点地图，运用动态规划的思想，结合船舶的操纵特性确定合适的评价函数，在栅格点地图中选择出最优的栅格点，并组合成一条航线。该方法实现了船舶与多条动态目标船的避碰，避碰结果可靠。

Claims (7)

1.一种基于动态规划思想的船舶路径规划方法，其特征在于：包括以下步骤：

步骤一：构造基于航线的径向栅格地图；

步骤二：为径向栅格地图的每个点赋予航行信息；

步骤三：建立评价函数和状态转移方程；

步骤四：利用动态规划思想评估栅格点回溯最优路径。

2.根据权利要求1所述的一种基于动态规划思想的船舶路径规划方法，其特征在于：步骤一所述的基于航线的径向栅格地图具体为：

步骤1-1：预先设置船舶的航线坐标，运用线性插值法将每段航线N等分，得到N+1个插值点，并计算每个插值点的坐标，线性插值法计算坐标值如下：

(x₀，y₀)，(x₁，y₁)为航线坐标，(x_i0,y_i0)为插值点的坐标；

步骤1-2：过这N+1个点做线段与航线相交，在线段上取点，并计算点的坐标，公式如下：

B₁为航线坐标(x₀，y₀)，(x₁，y₁)组成的线段在笛卡尔坐标系下对应的角度，B₂为航线坐标(x₁，y₁),(x₂，y₂)组成的线段在笛卡尔坐标系下对应的角度，B_v为这两角度对应的平均值，R表示路径上搜索宽度半径，MG表示半条线段上点的个数，(x_0i,j，y_0i,j)为以(x₀，y₀)为起点的折线上第i条线段上的第j个点，带入所有航线坐标，通过计算可得到径向栅格地图的所有点a_ij，当航线坐标为多个点时，同样按照上述方法处理。

3.根据权利要求1所述的一种基于动态规划思想的船舶路径规划方法，其特征在于：步骤二所述的为径向栅格地图的每一个点赋予航行信息具体为：创建结构体数组，赋予结构体数组以下信息，X，Y，ArriveTime，ArriveAngle，NodeEnsgn，TotalPGVal，分别表示该点的横坐标，纵坐标，本船从上一节点到达该点时间的累计值，本船到达该点时的航向角，该点是否为有效节点，本船到达该点的经历的所有节点评价值和的最小值。

4.根据权利要求1所述的一种基于动态规划思想的船舶路径规划方法，其特征在于：步骤三所述的评价函数具体为：根据该节点的安全性，时间的经济性，舵角的限制和与原航线的偏差建立如下总评价函数：

E(a_ij)＝w_t·Arrivetime(a_ij)+w_d·ColliDanger(a_ij)+w_p·PathErr(a_ij)+w_c·DirPunish(a_ij)

在上式中a_ij表示第i行第j个节点，Arrivetime(a_ij)，ColliDanger(a_ij)，PathErr(a_ij)，DirPunish(a_ij)分别表示主船到达a_ij节点所用的时间评价函数，与他船危险度评价函数，本船偏航评价函数，以及操舵限制评价函数；w_t，w_d，w_p，w_c分别为这四个函数的权重参数；

时间评价函数如下：

x_m，y_m分别表示我船的具体坐标，v_m表示我船的速度矢量，x_ij，y_ij表示a_ij点的坐标；

危险度评价函数如下：

上式中DA为当我船驶到a_ij时k条目标船与a_ij节点的最短距离，(x_ok，y_ok)表示第k条目标船的初始位置坐标；v_okx为目标船的在x轴的速度矢量，v_okx为目标船在y轴上的速度矢量；在公式中引入Arrivetime(a_ij)，推演主船在不同节点时，目标船运动的位置；SA表示本船的安全距离；SA的取值要根据本船的形状、大小、航速、天气状况、船员心理等主观条件确定；在计算危险度函数时，需要计算船舶在a_i-1,j到a_i,j节点行进时的危险度函数，对a_i-1,j和a_i,j节点的坐标进行线性插值得到M组值，将M组数值代入上式可得到行进间的船舶碰撞危险度；

偏航评价函数如下：

PathErr(a_ij)＝j(a_ij)-j(a_i，60)

j(a_ij)表示a_i,j节点所在第i层的标号j，j(a_(i,60))表示原航线上的点在第i层的标号j；

操舵限制评价函数如下：

DirPunish(aij)＝j(E_aij)-j(E_ai-1,j)

j(E_aij)表示第i层最优栅格点E_aij的标号j，j(E_ai-1,j)表示i层记录的i-1的最优栅格点E_ai-1,j的标号，用相邻栅格层栅格点的位置偏差可以近似估计船舶在转弯时的舵角限度。

5.根据权利要求4所述的一种基于动态规划思想的船舶路径规划方法，其特征在于：所述的总评价函数中，取i＝1，j的范围为0到120，取w_t为0.003，w_d为6.0，w_p为0.03，w_c为3.0，所述的危险度评价函数中SA为800m。

6.根据权利要求1所述的一种基于动态规划思想的船舶路径规划方法，其特征在于：步骤三所述的状态转移方程具体为：根据对路径规划问题的分析得到状态转移方程为：

MinSum[i][j]表示从a_(0,60)节点到a_ij初始节点之间各阶段节点评价值和的最小值，E[i][j]表示a_ij节点的评价值。

7.根据权利要求1所述的一种基于动态规划思想的船舶路径规划方法，其特征在于：步骤四所述的利用动态规划思想评估栅格点回溯最优路径具体为：

步骤4-1：生成栅格地图，初始化MinSum[1][j]＝E[1][j]，并设置船舶初始位置为第1层最优点，根据评价函数计算第1层所有栅格点的评价值；

步骤4-2：根据评价函数计算第2层所有栅格点的评价值E[2][j]并记录在数组中,根据状态转移方程得到MinSum[2][j]，并记录使本层节点评价和最小的上一层的节点标号，以便回溯路径；

步骤4-3：根据评价函数计算第三层所有栅格点的评价值E[3][j]并记录在数组中，遍历MinSum[2][j]，根据状态方程得到MinSum[3][j]，记录使本层评价和为最小的上一层节点的标号j，以此类推直到得到MinSum[2N+1][j]；

步骤4-4：找到2N+1层使MinSum[2N+1][j]最小的点，并搜索该点记录的上一层节点的标号，以此类推可得到最优路径。