一种欠驱动船舶靠泊路径规划方法与流程

1.本发明属于船舶智能航行领域，涉及一种欠驱动船舶靠泊路径规划方法。


背景技术：

2.为推进智能船舶的发展，使船舶更加安全、环保、经济、可靠，作为船舶智能化自主航行“最后一公里”的关键，船舶自动靠泊技术势在必行。但由于船舶靠泊工作环境复杂，靠泊时低速导致的舵效差、岸壁效应等带来的影响，使得船舶靠泊变得更加复杂和困难。靠泊路径规划是船舶自动靠泊的关键与前提，欠驱动船舶不具有跟踪任意路径的能力，因此在船舶自动靠泊过程中，要考虑船舶自身运动与靠泊水域限制的约束，提前规划出一条可行的靠泊路径。


技术实现要素：

3.针对现有技术中存在的问题，本发明的目的在于提供一种考虑几何约束的欠驱动船舶靠泊路径规划方法，至少部分解决上述技术问题为船舶的自动靠泊提供导引。
4.本发明实施例提供了一种欠驱动船舶靠泊路径规划方法，包括：
5.步骤一：获取欠驱动船舶靠泊过程历史数据；对所述历史数据进行分析，得到待规划靠泊路径的终端约束和运动约束；所述历史数据包括：所述欠驱动船舶的运动特性、靠泊起始船位、泊位位置和朝向；
6.步骤二：利用三次b样条插值生成所述待规划靠泊路径曲线；根据所述终端约束设置所述待规划靠泊路径曲线的起始端样板控制点和终止端样板控制点；
7.步骤三：根据所述运动约束利用改进的粒子群算法完善所述待规划靠泊路径曲线，形成最优的靠泊路径曲线。
8.进一步地，所述步骤一中得到所述运动约束的具体过程包括：
9.根据所述运动特性生成所述欠驱动船舶的三自由度操纵性模型；
10.对所述三自由度操纵性模型进行分析，得到所述待规划靠泊路径的定常回转动力学方程、回转半径和曲率；
[0011][0012][0013][0014]
式中y
v
'、v'、m'、y
r
'、r'、y
δ
'、n
v
'、n
r
'、n
δ
'均为水动力无因次量，δ为舵角；r为所述回转半径；l为船长；为所述曲率；ω为路径自变量参数。
[0015]
进一步地，所述步骤二中所述待规划靠泊路径曲线表达式包括：
[0016][0017]
u∈[0,1]，k＝3，i＝0,1,2,
…
,n
[0018][0019]
n
i,k
(u)＝a
k
n
i,k
‑1(u) b
k
n
i 1,k
‑1(u)
[0020][0021]
式中u为所述待规划靠泊路径曲线的参数；n表示控制点总数；i为控制点标号；k为b样条的次数；a
k
与b
k
为b样条插值的两个中间参数；n
i,k
(u)为所述待规划靠泊路径曲线的基函数；n
i,0
(u)为次数为0的b样条基函数；0≤u0≤
…
≤u
n k 1
≤1为所述待规划靠泊路径曲线的节点向量集。
[0022]
进一步地，所述根据所述终端约束设置所述待规划靠泊路径曲线的起始端样板控制点和终止端样板控制点包括：
[0023]
根据所述终端约束得到初始端样板点(x0,y0)；
[0024]
p0＝p1＝p2＝(x0,y0)
[0025]
其中，(x0,y0)为所述欠驱动船舶的初始位置；
[0026]
根据所述初始端样板点，在船舶速度方向延长线上设置所述待规划靠泊路径曲线的起始端样板控制点p3；
[0027]
p3＝(x0 l1cosψ0,y0 l1sinψ0)
[0028]
式中，ψ0为所述欠驱动船舶的初始艏向角，l1为路径参数；
[0029]
设舵效消失的最短距离为l
min
，则所述待规划靠泊路径曲线的终止端样板控制点表达式为：
[0030]
p
n
‑2＝p
n
‑1＝p
n
＝(x
r
,y
r
)
[0031]
p
n
‑3＝(x
r
‑
l2cosψ
r
,y
r
‑
l2sinψ
r
)
[0032]
p
n
‑4＝(x
r
‑
l3cosψ
r
,y
r
‑
l3sinψ
r
)
[0033]
式中(x
r
,y
r
)为目标泊位的位置；ψ
r
为泊位开角；l2和l3为路径参数且l3＞l2≥l
min
。
[0034]
进一步地，所述步骤三具体包括：
[0035]
粒子群算法表达式为：
[0036][0037][0038]
式中分别为在第m次迭代中第j个粒子第d维的速度、位置；w为惯性权重；分别为粒子的个体最优、全局最优；c1、c2为加速因子，r1、r2为[0,1]上的随机数；
[0039]
对粒子群进行编码和初始化，完善所述待规划靠泊路径曲线；粒子群由所述待规划靠泊路径曲线的n
‑
8个控制点组成；
[0040]
根据所述运动约束得到适应度函数；
[0041]
根据所述适应度函数优化所述粒子群算法，形成最优的靠泊路径曲线。
[0042]
进一步地，所述对粒子群进行初始化完善所述待规划靠泊路径曲线具体包括：
[0043]
连接所述控制点p3与p
n
‑4，得到线段；
[0044]
在所述线段上均匀取n
‑
8个路径节点，计算所述路径节点的坐标为：
[0045][0046]
判断所述路径节点是否在障碍物内；若所述路径节有点不在障碍物内，则对所述不在障碍物内的所述路径节点周围随机取值作为粒子编码；所述不在障碍物内的所述路径节点周围随机取值表达式为：
[0047][0048][0049]
若所述路径节点中有点在障碍物内，则求取在障碍物内所述路径节点垂直于线段的直线；求出所述直线与障碍物边界的交点左边(x
′
s 3
,y
′
s 3
)，对所述交点左边(x
′
i
,y
′
i
)周围随机取值作为粒子编码；所述交点左边(x
′
i
,y
′
i
)周围随机取值表达式为：
[0050][0051][0052]
判断所述粒子编码是否在障碍物中；若所述粒子编码有点在障碍物中，则将对应的所述粒子编码作为所述路径节点返回上一步，重新对在障碍物内的所述路径节点取值；若没有则初始化完成，得到完善后的所述待规划靠泊路径曲线；
[0053]
进一步地，所述适应度函数表达式为：
[0054][0055]
式中h
o
为落入障碍物或曲率，不满足所述运动约束的点；t为惩罚因子，表示h
o
连续增大的迭代次数；若h
o
较上次迭代减少，则惩罚因子t清零；h为所述完善后的所述待规划靠泊路径曲线上的h个点；z为曲线上的点的标号。
[0056]
进一步地，根据所述适应度函数优化所述粒子群算法中的参数；所述优化后的所述粒子群算法中的参数表达式为：
[0057][0058][0059]
[0060]
式中w
s
和w
f
分别为w的初始值和终止值，且w
s
＞w
f
；c
1s
和c
1f
分别为c1的初始值和终止值，且c
1s
＞c
1f
；c
2s
和c
2f
分别为c2的初始值和终止值，且c
2s
＜c
2f
；k
max
为最大迭代次数。
[0061]
本发明实施例提供的一种欠驱动船舶靠泊路径规划方法，通过分析欠驱动船舶特性与起始船位与泊位位置与朝向对靠泊任务的约束，确定路径规划的约束条件；设计样板控制点利用b样条插值平滑靠泊路径并解决规划的终端约束，设计粒子群优化算法解决规划的运动约束，最终生成一条平滑的可航靠泊路径。
[0062]
本发明设计非线性调整率根据粒子群算法的迭代次数，进行自适应调整算法参数，以提升粒子群算法的前期探索能力与后期收敛能力，考虑船舶自身运动与靠泊水域限制的约束，提前规划出一条可行的欠驱动船舶靠泊路径，使规划的靠泊路径接近最优。
[0063]
本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
附图说明
[0064]
图1为本发明实施例提供的一种欠驱动船舶靠泊路径规划方法框图；
[0065]
图2为本发明实施例提供的改进的粒子算法流程图；
[0066]
图3为本发明实施例提供的场景1下的靠泊规划路径示意图；
[0067]
图4为本发明实施例提供的场景1下路径曲率变化曲线示意图；
[0068]
图5为本发明实施例提供的场景1下适应度变化曲线示意图；
[0069]
图6为本发明实施例提供的场景2下的靠泊规划路径示意图；
[0070]
图7为本发明实施例提供的场景2下路径曲率变化曲线示意图；
[0071]
图8为本发明实施例提供的场景2下适应度变化曲线示意图。
具体实施方式
[0072]
下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0073]
在本发明的描述中，需要说明的是，术语“上”、“下”、“内”、“外”、“顶/底端”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0074]
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“设置有”、“内接”、“连接”等，应做广义理解，例如“连接”，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
[0075]
本发明实施例提供的一种欠驱动船舶靠泊路径规划方法，如图1所示，包括：步骤一：获取欠驱动船舶靠泊过程历史数据；对历史数据进行分析，得到待规划靠泊路径的终端
约束和运动约束；历史数据包括：欠驱动船舶的运动特性、靠泊起始船位、泊位位置和朝向；
[0076]
步骤二：利用三次b样条插值生成待规划靠泊路径曲线；根据终端约束设置待规划靠泊路径曲线的起始端样板控制点和终止端样板控制点；
[0077]
步骤三：根据运动约束利用改进的粒子群算法完善待规划靠泊路径曲线，形成最优的靠泊路径曲线。
[0078]
本发明通过分析欠驱动船舶特性与起始船位与泊位位置与朝向对靠泊任务的约束，确定路径规划的约束条件；设计样板控制点利用b样条插值平滑靠泊路径并解决规划的终端约束，设计粒子群优化算法解决规划的运动约束，最终生成一条平滑的可航靠泊路径。
[0079]
本发明设计非线性调整率根据粒子群算法的迭代次数，进行自适应调整算法参数，以提升粒子群算法的前期探索能力与后期收敛能力，考虑船舶自身运动与靠泊水域限制的约束，提前规划出一条可行的欠驱动船舶靠泊路径，使规划的靠泊路径接近最优。
[0080]
本发明实施例提供的一种欠驱动船舶靠泊路径规划方法。其整体包括三个部分：欠驱动船舶运动约束与靠泊终端约束分析、三次b样条插值与样板控制点设计以及改进的粒子群优化算法设计。下面将通过三部分对本发明进行详细说明。
[0081]
第一部分：靠泊规划运动约束与终端约束分析。
[0082]
在欠驱动船舶的靠泊过程中，由于欠驱动船舶自身的特性与泊位位置与朝向的限制，在靠泊规划时会存在一些约束，这些约束主要体现为靠泊路径的终端约束与靠泊过程的运动约束。
[0083]
终端约束：靠泊路径连接的始末端必须满足路径连续性要求。在起始端，要求靠泊路径的起点要与船舶当前位置一致，并且起始点的切线方向要与船艏向一致；在终止端，要求靠泊路径的终点要与船舶最终停靠的位置一致，终点处的路径切线方向与泊位朝向呈0
‑
30
°
夹角，且为了使船舶在最后转艏角速度为0，在到达泊位前设置一定长度的直线路径。
[0084]
运动约束：根据欠驱动船舶三自由度操纵性模型分析船舶操纵特性，可以得到船舶定常回转的动力学方程为：
[0085][0086]
式中y
′
v
、v
′
、m
′
、y
′
r
等均为无因次量，δ为舵角，则可求出回转半径为
[0087][0088]
式中，r为所述回转半径；l为船长；为所述曲率；ω为路径自变量；参数m
′
、v
′
与r
′
为船舶质量、横向速度与转艏速度的无因次量；y
′
v
与n
′
v
分别为横向速度对应的水动力与水动力矩的无因次量，y
′
r
与n
′
r
分别为转艏角速度对应的水动力与水动力矩的无因次量，y
′
δ
与n
′
δ
分别为横向与转艏方向的舵力与力矩系数的无因次量，各个无因次量可由以下公式推导得到：
[0089]
[0090][0091][0092]
式中，m、v与r为船舶质量、横向速度与转艏角速度，ρ为水的密度，v为船舶航速；y
v
与n
v
分别为横向速度对应的水动力与水动力矩系数，y
r
与n
r
分别为转艏角速度对应的水动力与水动力矩系数，y
δ
与n
δ
分别为横向与转艏方向的舵力与力矩系数。则靠泊路径的曲率应满足表达式：
[0093][0094]
式中，表示靠泊路径的曲率，ω为路径参数。
[0095]
第二部分：三次b样条插值与样板控制点设计。
[0096]
为了得到一条平滑、曲率连续的靠泊曲线，本发明使用b样条插值来生成靠泊曲线。b样条曲线是一种分段函数曲线，它由一系列控制点pi和次数k决定，三次b样条插值表达式如下：
[0097][0098]
u∈[0,1]，k＝3，i＝0,1,2,
…
,n
[0099]
式中，u为b样条曲线的参数，n为控制点总数，i为第i个控制点，n
i,k
(u)为b样条曲线的基函数，其表达式如下所示：
[0100][0101]
n
i,k
(u)＝a
k
n
i,k
‑1(u) b
k
n
i 1,k
‑1(u)
[0102][0103]
其中0≤u0≤
…
≤u
n k 1
≤1为b样条曲线的节点向量集。
[0104]
由b样条插值的原理可以看出，三次b样条曲线的形状完全由控制点序列pi决定，其中i＝0,1,...,n，k代表b样条的次数，k＝3表示本发明中使用三次b样条插值；a
k
与b
k
为b样条插值的两个中间参数；n
i,k
(u)为所述待规划靠泊路径曲线的基函数，n
i,0
(u)为次数为0的b样条基函数。因此本发明通过设计控制点序列的方式来确定规划的靠泊路径，其中路径的终端约束由通过设计序列两端的样板控制点来解决，具体方法如下：
[0105]
起始端样板控制点：起始端要求路径的起始位置与船舶位置一致，即规划的路径要穿过起始点，并且路径斜率应与船舶速度方向一致，因此可确定初始样板点为：
[0106]
p0＝p1＝p2＝(x0,y0)
[0107]
式中(x0,y0)为船舶初始位置。
[0108]
在船舶速度方向的延长线上设置控制点p3(这里假设船的初始速度方向与船艏向近似相等)，使船速保持连续。
[0109]
p3＝(x0 l1cosψ0,y0 l1sinψ0)
[0110]
式中，ψ0为船舶初始艏向角，l1为路径参数，可根据实际情况调节。
[0111]
终止端样板控制点：对于欠驱动船舶，其位置和航向由尾部的推进器和舵控制。当船舶接近目标泊位时，其速度较低，舵效基本为零，因此在进行路径规划时应保证在目标泊位之外的一定距离就完成对目标艏向的调整。舵效消失的最短距离可由舵效消失的临界航速的躺航距离决定，这里设定舵效消失的最短距离为l
min
，则终止端的样板控制点为：
[0112]
p
n
‑2＝p
n
‑1＝p
n
＝(x
r
,y
r
)
[0113]
p
n
‑3＝(x
r
‑
l2cosψ
r
,y
r
‑
l2sinψ
r
)
[0114]
p
n
‑4＝(x
r
‑
l3cosψ
r
,y
r
‑
l3sinψ
r
)
[0115]
式中(x
r
,y
r
)为目标泊位的位置，ψ
r
为泊位开角，l2和l3为路径参数，满足l3＞l2≥l
min
。p0‑
p
n
‑4这9个点表示在优化控制点之前已经确定的样板控制点，p0‑
p4为起始端控制点，其余为终止端控制点；l1、l2、l3为设置样板控制点用到的路径长度参数，而曲率中的ω为整条参数化路径的自变量参数。
[0116]
第三部分：改进的粒子群优化算法设计。
[0117]
在确定了首尾样板控制点后，接下来要确定中间控制点形成一条完整的靠泊路径，并解决靠泊路径的运动约束问题。本发明设计了一种改进的粒子群优化算法来优化求解b样条插值的n
‑
8个中间控制点。
[0118]
粒子群算法(particle swarm optimization，简称pso)是1995年由eberhart博士和kennedy博士一起提出的一种群体智能优化算法，它源于对鸟类捕食行为的研究。粒子群算法的核心是利用群体中个体对信息的共享从而使整个群体运动在问题求解空间中产生从无序到有序的演化过程，从而获得问题的最优解。粒子群算法的表达式如下所示：
[0119][0120][0121]
式中分别为在第m次迭代中第j个粒子第d维的速度、位置；w为惯性权重；分别为粒子的个体最优、全局最优；c1、c2为加速因子，r1、r2为[0,1]上的随机数。
[0122]
其中，上述改进粒子群算法具体如下：
[0123]
首先，进行粒子编码及初始化。本发明中粒子的编码为n
‑
8个中间控制点的坐标组成，这n
‑
8个中间控制点与样板控制点组成确定b样条曲线的n 1个控制点。
[0124]
在粒子的初始化方面，通常采用随机初始化方法，但这样会降低算法的收敛速度。因此本发明设计了一种初始化策略来解决这一问题，具体如下：
[0125]
1)连接控制点p3与p
n
‑4，记为线段l；
[0126]
2)在线段l上均匀取n
‑
8个路径节点，计算其坐标为：
[0127][0128]
3)判断n
‑
8个点是否在障碍物内，若不在，则在这些点周围随机取值作为粒子编码，路径节点取值方式为：
[0129][0130][0131]
4)若有点在障碍物中，则求取该点垂直于线段l的直线，求出该直线与障碍物边界的交点左边(x
′
s 3
,y
′
s 3
)，此时路径节点取值方式为：
[0132][0133][0134]
5)判断此时是否还有路径点在障碍物中，若有，则取出这些点重新执行步骤4)，若没有则初始化完成。
[0135]
其次，对适应度函数进行确定。粒子需要不断地迭代优化来寻找一条符合船舶运动约束的无碰靠泊路径，而评价路径是否最优的标准是适应度函数。本发明的适应度函数设计标准是b样条曲线的曲率满足运动约束中的曲率公式、无碰且在满足上述条件下最短，为了简化计算，在b样条曲线上取m个点，相邻两个间隔尽可能小，判断点上的曲率是否符合曲率要求，并判断点是否在障碍物内。因此适应度函数表达式为：
[0136][0137]
式中，h
o
为落入障碍物或曲率，不满足所述运动约束的点；t为惩罚因子，表示h
o
连续增大的迭代次数；若h
o
较上次迭代减少，则惩罚因子t清零；h为完善后的待规划靠泊路径曲线上的h个点，z表示曲线上的第z个点，即完善后的待规划靠泊路径曲线上点的标号。
[0138]
最后，确定参数调整率。粒子群算法有三个人为设定的可调参数，分别为惯性权重w，“自身经验”加速因子c1和“社会经验”加速因子c2。其中，惯性权重w值较大时具有较大的“探索”能力，适用于对整个规划空间进行大范围搜索；w值较小时粒子具有较强的“利用”能力，适于对当前局部区域进行小范围探索。c1值较大时有利于粒子的大范围搜索，但收敛速度较慢；c2值较大时有助于粒子学习群体经验从而快速收敛，但搜索规划空间的能力就相对较弱。因此为了提高粒子群算法前期的搜索能力和后期的收敛能力，在算法前期保持较大的w值，较大的c1值和较小的c2值，在算法后期保持较小的w值、c1值和较大的c2值，则三个参数表达式为：
[0139][0140][0141][0142]
式中w
s
和w
f
分别为w的初始值和终止值，且w
s
＞w
f
；c
1s
和c
1f
分别为c1的初始值和终
止值，且c
1s
＞c
1f
；c
2s
和c
2f
分别为c2的初始值和终止值，且c
2s
＜c
2f
；k
max
为最大迭代次数，随着迭代次数k增加，w和c1逐渐减小，c2逐渐增大。
[0143]
上述改进的粒子群算法如图2所示，其具体应用步骤为：
[0144]
1、根据靠泊码头的环境信息确定路径的控制点个数n(n≥9)，确定插值点的个数m，确定种群规模d，初始化粒子群算法中的系数w
s
、w
f
、c
1s
、c
1f
、c
2s
和c
2f
，对所有粒子进行初始化操作；
[0145]
2、使用三次b样条插值求出所有粒子的插值点，并根据适应度函数求取粒子的适应度值；
[0146]
3、为每个粒子选择个体最优与群体最优，根据粒子群算法的表达式更新粒子的速度和位置，并根据适应度函数算出更新后粒子的适应度值，迭代次数加1；
[0147]
4、选择出适应度最差的10％粒子做初始化操作，防止算法陷入局部最优；
[0148]
5、若迭代次数达到设定值，则迭代结束；没有，则转到步骤3继续执行。
[0149]
为了验证本发明实施例提供的欠驱动船舶靠泊路径规划方法的有效性，下面以三个圆形障碍物的场景来进行仿真实验，三个障碍物的圆心分别为(150,450)、(400,300)和(120,150)，半径均为100m。靠泊规划的参数选择为：限制回转半径r＝100，样板点参数l1＝10，l2＝40，l3＝50，控制点参数n＝12，插值点个数m＝100，种群规模d＝100，迭代次数n＝200，粒子群算法参数w
s
＝0.1，w
f
＝1，c
1s
＝c
2f
＝2.5，c
1f
＝c
2s
＝0.5。
[0150]
场景1：设置船舶开始靠泊时位置为(0,0)，艏向为0
°
，泊位位置为(500,500)，朝向为0
°
，则规划仿真结果如图3
‑
图5所示。图3为船舶在障碍物场景中的靠泊规划路径，可以看出规划出的靠泊路径是一条连续、平滑的曲线，并且不与任何障碍物相交；图4为靠泊路径100个采样点上的曲率变化情况，虚线为满足曲率公式的最大曲率，可以看出路径曲线的所有采样点都满足曲率要求，靠泊路径符合船舶的运动约束；图5为算法迭代过程中适应度函数的变化曲线，可以看出适应度值初期变化较快，此阶段算法在调整曲线满足曲率公式约束和无碰约束，中期变化较慢，此时算法在优化路径，使路径在满足约束前提下长度最短，最终在80代左右收敛，此时适应度值为750左右。
[0151]
为了验证算法的适用性，改变靠泊初始艏向为60
°
，记为场景2。规划仿真结果如图6
‑
图8所示。可以看出算法可以很好地避开障碍物完成靠泊规划任务，且曲率满足要求，算法在100代左右收敛。
[0152]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其改进构思加以等同替换或改变，都应涵盖在本发明的保护范围内。