CN113050652A - 一种用于智能船舶自动靠泊的轨迹规划方法 - Google Patents

Abstract

本发明提供一种用于智能船舶自动靠泊的轨迹规划方法，包括：初始化参数及障碍物目标点；根据改进的斥力势场函数求斥力和引力；更新船舶位置，并获取目标点；获取初次人工势场规划的不同航向角靠泊路径；分割路径点；等时间间隔差值n‑2个路径点；将n‑2个路径点代入n阶贝塞尔曲线方程，计算n‑2个控制点获得初始解；建立优化问题的目标函数；实用序列二次规划求解优化问题；人工势场法则规划路径经贝塞尔曲线控制点优化后的不同初始航向角路径，并选择最佳靠泊路径。

Description

一种用于智能船舶自动靠泊的轨迹规划方法

技术领域

本发明涉及船舶靠泊技术改进领域，特别是涉及一种用于智能船舶自动靠泊的轨迹规划方法。

背景技术

随着船舶大型化、自动化、智能化的发展，智能船舶已成为当下船舶领域研究的热点，也是船舶工业向高技术、高性能方向转型升级的重要方向，而自动靠泊技术作为智能船舶实现智能航行的重要组成部分，智能船舶自动靠泊轨迹规划己经成为国内外众多学者研究的重点。传统的路径规划算法有人工势场法、遗传算法、神经网络、模拟退火算法、蚁群算法等，但由于船舶所处的码头环境复杂，仅采用单一以上方法规划的路径并不符合船舶的运动学和动力学特性。人工势场法是将船舶在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对船舶产生“引力”，障碍物对船舶产生“斥力”，通过求“引力”和“斥力”的合力来控制船舶的运动。人工势场法因其简单、计算量小、实时性强，使其在路径规划领域得到了广泛的应用。人工势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题，当目标障碍物和船舶同在一条直线上时，可能会产生未到达目标点，但是机器人所受合力为零的状态，船舶停止运动或在最优点处震荡。此外，当船舶靠近障碍物时，由于斥力增大导致规划路径拐角突然增大。此外A*算法作为静态环境中求解最短路径常用的的搜索方法，它在路径规划时引入了全局环境信息，会对当前节点与终点的距离做出估值计算，并据此判断该节点处于最短路线上的可能性大小，并选择代价函数值最小的方向拓展节点，但当应用场景中为动态障碍物，或者目标点变化频繁时，此算法可能会同时产生多个代价函数值最小的路径，不能保证搜索到的路径为最优路径，而且如果环境中障碍物复杂，A*算法扩展的节点较多，也会导致搜索效率低下。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种用于智能船舶自动靠泊的轨迹规划方法，首先建立了船舶三自由度模型，考虑风、浪、流以及静态障碍物的情况下智能船舶自动靠泊轨迹规划方法研究，然后利用改进的人工势场法和贝塞尔曲线控制点优化的船舶自动靠泊轨迹规划方法。其优点是：可以有效地降低船舶自动靠泊过程的能耗和舵角机械损耗，并且可以保证船舶轨迹跟踪控制的精度。

为实现上述目的及其他相关目的，本发明提供一种用于智能船舶自动靠泊的轨迹规划方法，所述方法包括：

初始化参数及障碍物目标点；

根据改进的斥力势场函数求斥力和引力；

更新船舶位置，并获取目标点；

获取初次人工势场规划的不同航向角靠泊路径；

分割路径点；

等时间间隔差值n-2个路径点；

将n-2个路径点代入n阶贝塞尔曲线方程，计算n-2个控制点获得初始解；

建立优化问题的目标函数，所述目标函数的具体表达为：J_min＝ω₁J₁+ω₂J₂₊ω₃J₃

实用序列二次规划求解优化问题；

人工势场法则规划路径经贝塞尔曲线控制点优化后的不同初始航向角路径，并选择最佳靠泊路径。

一种实现方式中，所述斥力势场函数的具体表达为：

其中，η为斥力场系数，ρ为船到障碍物之间的距离，ρ₀为障碍物的影响半径。

一种实现方式中，所述更新船舶位置的具体表达为：

其中，(x_k，y_k)在k时刻船舶位置，(x_k+1，y_k+1)在k+1时刻船舶位置，θ船舶航向角。

一种实现方式中，

所述建立优化问题模型为：

在人工势场法规划路径基础上，以曲线控制点为优化变量，求解安全、平滑的船舶自动靠泊轨迹；包括步骤：

起点终点约束步骤:

构建靠泊优化的起点和终点与初始的一致性方程

(x₀，y₀)＝(x_l，y_l)，(x_W，y_W)＝(x_F，y_F)\*MERGEFORMAT (3)

其中(x₀，y₀)、(x_W，y_W)分别表示优化过程中的起点坐标和终点坐标，(x_l，y_l)、(x_F，y_F)分别表示初始的起点坐标和终点坐标；

航向约束步骤：

ψ_{t min}＜ψ_t＜ψ_{t max}\*MERGEFORMAT (4)

ψ_t表示优化过程中时刻船舶的航向，(ψ_{t min}，ψ_{t max})表示时刻船舶航向的约束范围；

速度约束包括船舶的纵向速度以及横向速度方程

u_{t min}＜u_t＜u_{t max}\*MERGEFORMAT (5)

v_{t min}＜v_t＜v_{t max}\*MERGEFORMAT (6)

u_t，v_t分别表示优化过程中时刻船舶的纵向速度、横向速度，u_{t min}，v_{t min}表示t时刻的船舶最小纵向速度与横向速度，u_{t max}，v_{t max}表示t时刻的船舶最大纵向速度与横向速度；

约束舵角和转速在船舶操纵的范围内的方程；

推进器的最大输出和最小输出分别为τ_imax和τ_imin；

在靠泊轨迹优化过程中，为了保证轨迹符合船舶动力学要求，下一个时刻船舶的状态与当前时刻的状态差值满足运动模型；

把区域外的航道部分以及码头都看作障碍物，对船舶进行靠泊轨迹的优化；

(x_t，y_t)≠s(x_d，y_d)\*MERGEFORMAT (8)

s(x_d，y_d)表示障碍物的坐标函数，障碍物往往是不规则的形状，所以该函数是非线性的；

建立约束条件下贝塞尔曲线控制点优化目标函数，包括以下子步骤：

建立目标函数

目标函数J₁为优化轨迹与参考轨迹误差最小，目标函数J₂为路径平滑度最优，目标函数J₃为能耗最优

J₃＝g_eP_eT\*MERGEFORMAT (11)

其中，根据路径插值计算的第i个点参考轨迹点坐标用(x_ref，i，y_ref，i)表示，w₁，w₂，w₃为权重w₁+w₂+w₃＝1，船舶在第i个点的坐标用(x_i，y_i)表示，船舶在第i-1个点的坐标用(x_i-1，y_i-1)表示，船舶在第i+1个点的坐标用(x_i+1，y_i+1)，g_e表示为航程总油耗量，单位为g，P_e为主机油耗率，g/(KW·h),T为航程航行时间，单位h。

如上所述，本发明实施例提供的一种用于智能船舶自动靠泊的轨迹规划方法，首先建立了船舶三自由度模型，考虑风、浪、流以及静态障碍物的情况下智能船舶自动靠泊轨迹规划方法研究，然后利用改进的人工势场法和贝塞尔曲线控制点优化的船舶自动靠泊轨迹规划方法。其优点是：可以有效地降低船舶自动靠泊过程的能耗和舵角机械损耗，并且可以保证船舶轨迹跟踪控制的精度。

所提出的轨迹规划方法得到的路径更加平滑，可以实现对船舶期望轨迹的准确跟踪。该轨迹规划方法使得船舶舵角不会反复改变，避免舵角的机械磨损；可以有效地降低推进系统的能耗，并且可以保证船舶控制的精度和实时性。

附图说明

图1是本发明实施例的用于智能船舶自动靠泊的轨迹规划方法的一种具体应用示意图。

图2是本发明实施例的用于智能船舶自动靠泊的轨迹规划方法的一种控制结构示意图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。

请参阅图1。需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

本发明实施例的一种用于智能船舶自动靠泊的轨迹规划方法的主体可以为一种用于智能船舶自动靠泊的轨迹规划方法，包括：

步骤1、建立三自由度运动学动力学模型；

垂荡、横摇、纵摇方向上的运动忽略不计，在纵荡、横荡和艏摇方向上的三自由度运动的船舶动力学模型可归结如下：

其中，η为船舶在大地坐标系X_EOY_E下的实际位置(x，y)和艏揺角ψ组成的向量，η＝[x，y，ψ]^T；v为船舶在附体坐标系XOY下前进速度u、横漂速度v及艏揺角速度r组成的向量，v＝[u，v，r]^T；R(ψ)为坐标系转换矩阵，M是船体的惯性矩阵，C(v)代表科氏向心力矩阵，D(v)为线性水动力阻尼参数矩阵；τ为船舶推进器控制输入前进力τ₁、横漂力τ₂及艏揺力矩τ₃组成的控制向量，τ＝[τ₁，τ₂，τ₃]^T；τ_d为船舶在附体坐标系下风、浪、流等引起的横向干扰力τ_d1、纵向干扰力τ_d2及艏向干扰力矩τ_d3组成的外部环境扰动向量，τ_d＝[τ_d1，τ_d2，τ_d3]^T。

步骤2、基于改进的人工势场法的路径规划，包括以下子步骤：2-1)大多数人工势场的斥力势场函数都是以船舶与障碍物的相对位置的倒数为自变量的二次函数，船舶小幅度地移动就能引起势场强度大幅度的变化，即斥力场强度变化较快。而在实际应用中，斥力场强度的数值变化过快会影响对运动路径的判断，因此对斥力常量m的选择要求较为苛刻，因此采用指数函数作为斥力势场函数，其表达式如下：

其中，η为斥力场系数，ρ为船到障碍物之间的距离，ρ₀为障碍物的影响半径。

2-2)为了解决目标不可达的问题，引入目标点与无人船的相对位置，将原来的斥力场函数乘以一个因子(X-X_g)的n次方，使得目标点位置的斥力为零。改进的斥力场函数为：

步骤3、建立优化问题模型，在人工势场法规划路径基础上，以曲线控制点为优化变量，求解安全、平滑的船舶自动靠泊轨迹。优化目标定义包括路径偏差、曲率以及能耗。

3-1)建立约束条件:

1.起点终点约束:

船舶靠泊轨迹的优化是从起点到终点找出一条安全可行的最优靠泊轨迹，首先要保证靠泊优化的起点和终点与初始的一致,即

(x₀，y₀)＝(x_l，y_l)，(x_W，y_W)＝(x_F，y_F)\*MERGEFORMAT (29)

其中(x₀，y₀)、(x_W，y_W)分别表示优化过程中的起点坐标和终点坐标，(x_l，y_l)、(x_F，y_F)分别表示初始的起点坐标和终点坐标。

2.航向约束

ψ_{t min}＜ψ_t＜ψ_{t max}\*MERGEFORMAT (30)

ψ_t表示优化过程中时刻船舶的航向，(ψ_{t min}，ψ_{t max})表示时刻船舶航向的约束范围。

3.速度约束包括船舶的纵向速度以及横向速度，

u_{t min}＜u_t＜u_{t max}\*MERGEFORMAT (31)

v_{t min}＜v_t＜v_{t max}\*MERGEFORMAT (32)

u_t，v_t分别表示优化过程中时刻船舶的纵向速度、横向速度，v_{t min}，v_{t min}表示t时刻的船舶最小纵向速度与横向速度，v_{t max}，v_{t max}表示t时刻的船舶最大纵向速度与横向速度。

4.控制约束：在船的靠泊操纵中，控制参数为控制舵角δ和螺旋桨控制转速n。在不改变算法的情况下，实际舵角δ和螺旋桨转速n与控制向量密切相关，所以必须保证舵角和转速在船舶操纵的约束范围内；

推进器的最大输出和最小输出分别为τ_{i max}和τ_{i min}。

4.动力学约束：在靠泊轨迹优化过程中，为了保证轨迹符合船舶动力学要求，下一个时刻船舶的状态与当前时刻的状态差值满足运动模型；

5.障碍物约束：船舶在港内靠泊时，由于航道水深不同，往往都是按照规范规定的区域内进行操作，所以要进行区域约束，把区域外的航道部分以及码头都看作障碍物，所以船舶的坐标点不能位于障碍物的坐标域；对船舶进行靠泊轨迹的优化，必须保证靠泊过程足够的安全，所以需増加更加严格的障碍物约束，并留有一定的安全余量。

(x_t，y_t)≠s(x_d，y_d)\*MERGEFORMAT (34)

s(x_d，y_d)表示障碍物的坐标函数，障碍物往往是不规则的形状，所以该函数是非线性的。

3-2)建立约束条件下贝塞尔曲线控制点优化目标函数，包括以下子步骤：

1)建立目标函数

目标函数J₁为优化轨迹与参考轨迹误差最小，目标函数J₂为路径平滑度最优，目标函数J₃为能耗最优。

J₃＝g_eP_eT\*MERGEFORMAT (37)

其中，根据路径插值计算的第i个点参考轨迹点坐标用(x_ref，i，y_ref，i)表示，w₁，w₂，w₃为权重w₁+w₂+w₃＝1，船舶在第i个点的坐标用(x_i，y_i)表示，船舶在第i-1个点的坐标用(x_i-1，y_i-1)表示，船舶在第i+1个点的坐标用(x_i+1，y_i+1)，g_e表示为航程总油耗量，单位为g，P_e为主机油耗率，g/(KW·h),T为航程航行时间，单位h。

故优化问题可描述为：J_min＝w₁J₁+w₂J₂+w₃J₃\*MERGEFORMAT (38)

本节根据改进的人工势场法规划的简化路径，预先计算一组合理的初始控制点。首先根据起点终点约束条件可计算第1、2个控制点与最后一个控制点；然后，从改进的人工势场法规划的路径中，等时间间隔插值取出n-2个路径点，不含起点终点，路径点包含时间、位置等信息，将其代入n阶贝塞尔曲线方程可以计算得到剩余的n-2个控制点，以这组控制点作为优化问题初始解。最后使用序列二次规划(SQP)方法求解上述优化问题以优化这组控制点，使最终的无人船轨迹收敛到局部最优解,最终得到一条从起点到终点的最优平滑。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (4)

1.一种用于智能船舶自动靠泊的轨迹规划方法，其特征在于，所述方法包括：

初始化参数及障碍物目标点；

根据改进的斥力势场函数求斥力和引力；

更新船舶位置，并获取目标点；

获取初次人工势场规划的不同航向角靠泊路径；

分割路径点；

等时间间隔差值n-2个路径点；

将n-2个路径点代入n阶贝塞尔曲线方程，计算n-2个控制点获得初始解；

建立优化问题的目标函数，所述目标函数的具体表达为：J_min＝ω₁J₁+ω₂J₂+ω₃J₃

实用序列二次规划求解优化问题；

人工势场法则规划路径经贝塞尔曲线控制点优化后的不同初始航向角路径，并选择最佳靠泊路径。

2.根据权利要求1所述的用于智能船舶自动靠泊的轨迹规划方法，其特征在于，所述斥力势场函数的具体表达为：

其中，η为引力场系数，ρ为船到障碍物之间的距离，ρ₀为障碍物的影响半径。

3.根据权利要求1所述的用于智能船舶自动靠泊的轨迹规划方法，其特征在于，所述更新船舶位置的具体表达为：

x_k+1＝x_k+l·cosθ

y_k+1＝y_k+l·sinθ

其中，(x_k，y_k)在k时刻船舶位置，(x_k+1，y_k+1)在k+1时刻船舶位置，θ船舶航向角。

4.根据权利要求1所述的用于智能船舶自动靠泊的轨迹规划方法，其特征在于，所述建立优化问题模型为：

在人工势场法规划路径基础上，以曲线控制点为优化变量，求解安全、平滑的船舶自动靠泊轨迹；包括步骤：

起点终点约束步骤：

构建靠泊优化的起点和终点与初始的一致性方程

(x₀，y₀)＝(x_l，y_l)，(x_w，y_w)＝(x_F，y_F)

其中(x₀，y₀)、(x_W，y_W)分别表示优化过程中的起点坐标和终点坐标，(x_l，y_l)、(x_F，y_F)分别表示初始的起点坐标和终点坐标；

航向约束步骤：

ψ_{t min}＜ψ_t＜ψ_{t max}

ψ_t表示优化过程中时刻船舶的航向，(ψ_{t min}，ψ_{t max})表示时刻船舶航向的约束范围；

速度约束包括船舶的纵向速度以及横向速度方程

u_{t min}＜u_t＜u_{t max}

v_{t min}＜v_t＜u_{t max}

u_t，v_t分别表示优化过程中时刻船舶的纵向速度、横向速度，u_{t min}，v_{t min}表示t时刻的船舶最小纵向速度与横向速度，u_{t max}，v_{t max}表示t时刻的船舶最大纵向速度与横向速度；

约束舵角和转速在船舶操纵的范围内的方程；

推进器的最大输出和最小输出分别为τ_{i max}和τ_{i min}；

在靠泊轨迹优化过程中，为了保证轨迹符合船舶动力学要求，下一个时刻船舶的状态与当前时刻的状态差值满足运动模型；

把区域外的航道部分以及码头都看作障碍物，对船舶进行靠泊轨迹的优化；

(x_t，y_t)≠s(x_d，y_d)

s(x_d，y_d)表示障碍物的坐标函数，障碍物往往是不规则的形状，所以该函数是非线性的；

建立约束条件下贝塞尔曲线控制点优化目标函数，包括以下子步骤：

建立目标函数

目标函数J₁为优化轨迹与参考轨迹误差最小，目标函数J₂为路径平滑度最优，目标函数J₃为能耗最优

J₃＝g_eP_eT

其中，根据路径插值计算的第i个点参考轨迹点坐标用(x_ref，i，y_ref，i)表示，w₁，w₂，w₃为权重w₁+w₂+w₃＝1，船舶在第i个点的坐标用(x_i，y_i)表示，船舶在第i-1个点的坐标用(x_i-1，y_i-1)表示，船舶在第i+1个点的坐标用(x_i+1，y_i+1)，g_e表示为航程总油耗量，单位为g，P_e为主机油耗率，g/(KW·h)，T为航程航行时间，单位h。

Images