CN111123923B - 一种无人船舶局部路径动态优化方法 - Google Patents

Abstract

本发明提供一种无人船舶局部路径动态优化方法，包括：S1、获取目标船舶的静态信息和初始运动状态信息；S2、从初始时刻起，依据目标船舶实时获取的周围碍航物信息，基于静态信息、初始运动状态信息和预先构建的局部路径动态优化模型，采用粒子群算法对局部路径动态优化模型进行求解，确定用于目标船舶实时行驶的最优控制曲线；S3、目标船舶根据确定的最优控制曲线，控制船舶行驶；局部路径动态优化模型是基于船舶运动学模型、几何路径约束条件、边值约束条件和目标函数的动态最优控制模型。可以进行在线规划，解决路径优化实时性问题，使无人船舶在航行过程中时刻保持最优状态。

Description

一种无人船舶局部路径动态优化方法

技术领域

本发明涉及智能无人船舶技术领域，尤其涉及一种基于粒子群加速计算和动态最优控制的无人船舶局部路径优化方法。

背景技术

随着航运业的快速发展，船舶智能化、无人化也随之兴起，局部路径优化问题一直是智能无人船舶领域的一个重要研究课题，在一定程度上反映无人船舶自主导航智能化水平的高低，同时也是保证船舶航行安全性的一个重要前提条件。

目前，关于无人船的路径优化研究可大致概括为：将电子海图或传感器获取的实时图像进行栅格化或自适应二值化阈值处理，然后将处理后的电子海图或图像分为自由航行区域和不可航(障碍物)区域，在自由航行区域中采用A*算法、Dijkstra算法、遗传算法等智能搜索算法、人工势场法和可视图法等搜索最短路径。

现有对局部路径优化的研究主要存在以下不足：1、前人的研究多是将电子海图以及获取的环境图像进行栅格化或二值化阈值处理，然后利用某一智能算法在已处理的地图上静态规划路径，需要对现有可行解不断迭代优化，重复判断是否满足最优条件，最后得到最优解或相对最优解，难以保证无人船在航行过程中时刻保持最优状态，同时还有耗时长以及效率低等缺陷2、无人船在航行过程中位置点是不断变化的，以往研究大多不考虑每一时刻无人船在时间和空间上与动态障碍物之间的相对位置变化，在路径优化算法开始时刻到结束时刻这一时间段内，无人船与动态障碍物相对位置可能已发生变化，即难以解决路径优化实时性问题，无法进行离线或在线规划，影响船舶航行的连续性，无法保证船舶始终航行在最优航线上。3、前人对于动态局部路径优化的研究缺乏对航时和最短路径的同时考量。

局部路径动态优化是指无人船在航行过程中，为降低环境对其影响，根据实时环境信息动态优化路径，使船舶始终沿着最优路径航行。因此，深入研究无人船的局部路径动态优化方法具有很强的现实意义，可为无人船自主航行提供决策依据，提升船舶航行安全性与经济性。

因此，亟需一种无人船舶局部路径动态优化方法。

发明内容

(一)要解决的技术问题

为了解决现有技术的上述问题，本发明提供一种基于粒子群加速计算和动态最优控制的无人船舶局部路径优化方法。该方法可以进行在线规划，解决路径优化实时性问题，使无人船舶在航行过程中时刻保持最优状态。

(二)技术方案

为了达到上述目的，本发明采用的主要技术方案包括：

一种无人船舶局部路径动态优化方法，包括以下步骤：

步骤S1、获取目标船舶的静态信息和初始运动状态信息。

步骤S2、从初始时刻起，依据目标船舶实时获取的周围碍航物信息，基于静态信息、初始运动状态信息和预先构建的局部路径动态优化模型，采用粒子群算法对局部路径动态优化模型进行求解，确定用于目标船舶实时行驶的最优控制曲线。

步骤S3、目标船舶根据确定的最优控制曲线，控制船舶行驶。

局部路径动态优化模型是基于船舶运动学模型、几何路径约束条件、边值约束条件和目标函数的动态最优控制模型。

作为本发明方法的一种改进，船舶运动学模型包括三自由度船舶运动学模型或四自由度船舶运动学模型或六自由度船舶运动学模型。

作为本发明方法的一种改进，局部路径动态优化模型包括，

目标函数：

其中，t₀为初始时刻；t_f为终止时刻；λ₁，λ₂为权重系数，且λ₁,λ₂∈[0,1]，λ₁+λ₂＝1。

约束条件：

ⅰ、船舶运动学模型：

其中，ψ_s为目标船舶s的航向；u_s为目标船舶s的纵向速度；v_s为目标船舶s的横向速度；r_s为目标船舶s绕z轴转动的角速度。

ⅱ、几何路径约束条件：

防撞避碰约束条件

其中，x_s(t)，y_s(t)为目标船舶s在坐标系上的横坐标与纵坐标；x_o(t)，y_o(t)为碍航物o在坐标系上的横坐标与纵坐标；d_os为船舶s与碍航物o之间的安全距离；

状态变量区间限制约束条件

|δ_s(t)|≤δ_max

|v_s(t)|≤v_max

其中，δ_max为航行过程中允许的最大舵角变化量；v_max为航行过程允许的最大安全航速。

iii、边值约束条件：

x_s(t₀)＝x₀,y_s(t₀)＝y₀,v_s(t₀)＝v₀,ψ_s(t₀)＝ψ₀,δ_s(t₀)＝δ₀

ψ_s(t₀)＝ψ_s(t_f)

其中，x₀为初始横坐标；y₀为初始纵坐标；v₀为初始速度；ψ₀为初始航向；δ₀为初始舵角。

作为本发明方法的一种改进，在采用粒子群算法对局部路径动态优化模型进行求解之前，包括：

向局部路径动态优化模型中引入辅助变量w(t)，将复合型性能指标转化为末值型性能指标：

J＝Φ(z(t₀),t₀,z(t_f),t_f)+w(t_f)

w(t₀)＝0

依据末值型性能指标，转化局部路径动态优化模型：

其中，z(t)为被微分后的系统状态变量；u(t)为控制变量；J为末值型性能指标函数；f为常微分方程中的代数函数部分；g为不等式约束；h为等式约束；t₀为起始时刻；t_f为终止时刻。

作为本发明方法的一种改进，采用粒子群算法对局部路径动态优化模型进行求解，确定用于目标船舶实时行驶的最优控制曲线，包括：

步骤A1、对局部路径动态优化模型进行预处理。

步骤A2、初始化粒子群规模、每个粒子的速度和位置。

步骤A3、依据目标船舶实时获取的周围碍航物信息，基于静态信息、初始运动状态信息和预处理后的局部路径动态优化模型，求解目标船舶状态变量。

步骤A4、依据目标船舶状态变量，计算目标函数和适应度函数，更新当代粒子群最优解。

步骤A5、根据粒子群更新公式，更新每一个粒子的位置与速度。

步骤A6、判断是否满足预设条件，若是，输出解向量(状态变量最优解及当前时刻的控制变量)；若否，重复步骤A3。

步骤A7、对解向量进行插值处理，获得最优控制曲线。

作为本发明方法的一种改进，对局部路径动态优化模型进行预处理，包括：控制变量离散化处理和离散化控制变量模型中约束条件的处理；约束条件的处理包括采用欧拉方程处理船舶运动学模型、基于内点障碍函数法处理几何路径约束条件、基于限制解向量范围处理边值约束条件和归一化处理时间约束。

作为本发明方法的一种改进，控制变量离散化处理，包括：

i、控制变量参数化：

其中，[τ_k-1，τ_k)为控制曲线的第k段时间段；τ_k，k＝1，…，n_p为时间节点，ξ_k为第k段时间段上的控制变量值；n_p为时间节点个数。

ii、将参数化的控制变量代入局部路径动态优化模型，获得第一预处理局部路径动态优化模型：

g(z(t)，ξ_k，t)≤0，t∈τ_k-1，τ_k)，k＝1，2，，…，n_p

h(z(t)，ξ_k，t)＝0，t∈τ_k-1，τk)，k＝1，2，，…，n_p

其中，z(t)为被微分后的系统状态变量；z(t_f)为终止时刻系统状态变量；

为拟合后的控制变量；ξ_k为第k段时间段上的控制变量值；f为常微分方程中的代数函数部分；g为不等式约束；h为等式约束；τ_k-1，τ_k)为控制曲线的第k段时间段。

作为本发明方法的一种改进，基于内点障碍函数法处理几何路径约束条件，包括：

i、将第一预处理局部路径动态优化模型表示为：

minJ(Λ)，

s.t.g(Λ)≤0，

h(Λ)＝0.

其中，Λ为决策变量；g(Λ)<0为不等式约束；h(Λ)为等式约束；

向第一预处理局部路径动态优化模型中引入松弛变量s>0，获得第二预处理局部路径动态优化模型：

minJ(Λ)，

s.t.g(Λ)+s＝0，

h(Λ)＝0，

s＞0

ⅱ、将第二预处理局部路径动态优化模型中的不等式约束条件s>0作为障碍项添加到性能函数，获得第三预处理局部路径动态优化模型：

s.t.s＞0

其中，Λ为决策变量；s为松弛变量；

为包含障碍项的目标函数；J为目标函数；r_k>0,r_k→0⁺是障碍因子。

作为本发明方法的一种改进，归一化处理时间约束，包括：

ⅰ、采用归一化映射方法计算时间约束：

ⅱ、定义粒子群中解结构：

其中，τ₀＝t₀；

ξ_k，k＝1,2,…,n_p为第k时间段上具有边值约束的控制变量参数值；θ_k，k＝1,2,…,n_p为用于计算时间节点的无约束“时间变量”。

作为本发明方法的一种改进，适应度函数fit包括：

fit＝α₁fit₁+α₃fit₂+α₃fit₃

其中，fit₁为安全性适应度函数；fit₂为路径长度适应度函数；fit₃为边值约束适应度函数；α₁,α₂,α₃为权重系数，且α₁+α₂+α₂＝1，α₁,α₂,α₂∈[0,1]；(x_o(t),y_o(t))为碍航物在t时刻位置坐标；(x_s(t),y_s(t))为目标船舶s在t时刻位置坐标；

为粒子i代表的路径i；

为粒子i-1代表的路径i-1，每个粒子代表一条路径；(x_i0,y_i0)为第i条路径起点坐标，(x_if,y_if)为第i条路径终点坐标，ψ₀为目标船舶初始航向。

(三)有益效果

本发明的有益效果是：

本专利提出的基于粒子群加速计算和动态最优控制的无人船局部路径优化方法，从船舶系统运动方程入手，综合考虑几何路径约束条件、边值约束条件和性能指标函数等因素，将船舶局部路径动态优化问题看做一个始端自由、非线性、时变、带有控制约束和终端约束的连续Bolza问题，建立连续动态优化性能指标函数；然后基于船舶航行安全性和时效性原则，结合粒子群算法，利用其调整参数少、鲁棒性好、收敛速度相对较快和进化过程连续等优点，对所建模型进行求解。该方法可以进行在线规划，解决路径优化实时性问题，并且可有效缩短局部路径长度，减少航行时间，使无人船舶在航行过程中时刻保持最优状态，为无人船的局部路径动态优化提供决策方法及理论研究基础。

附图说明

本发明借助于以下附图进行描述：

图1为本发明具体实施方式中无人船舶局部路径动态优化方法的流程图；

图2为本发明具体实施方式中采用粒子群算法对局部路径动态优化模型进行求解的流程图；

图3为本发明具体实施方式中t＝25s时，目标船舶的路径图；

图4为本发明具体实施方式中t＝45s时，目标船舶的路径图；

图5为本发明具体实施方式中t＝60s时，目标船舶的路径图；

图6为本发明具体实施方式中t＝70s时，目标船舶的路径图；

图7为本发明具体实施方式中本发明算法和遗传算法分别进行局部路径优化的结果对比图。

具体实施方式

为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。

船舶局部路径优化是指在船舶局部航行的初始时刻状态与终止时刻状态之间找到符合约束条件的航行路径。船舶航行是一个连续过程，并且对于局部路径优化问题，一般要求优化后的路径与原计划航线偏差不大，而船舶在初始时刻的某些参数信息可由船载助航设备获取得知，且不受约束条件限制，所以本发明利用最优控制理论，将船舶局部路径优化问题表述为一个始端自由、非线性、时变、带有控制约束和终端约束的连续Bolza形式最优控制问题，具体如下：

目标泛函：

约束条件：

ⅰ、受控系统状态方程约束

ⅱ、几何路径约束条件

C(z(t)，u(t)，t)≤0，t∈[t₀，t_f] (3)

iii、边值约束条件

其中，z(t₀)为受控系统初始时刻t₀时的状态；z(t_f)为受控系统终止时刻t_f时的状态；z(t)为n维状态变量；u(t)为m维控制变量；Φ(z(t₀),t₀,z(t_f),t_f)为始端与终端指标；

为积分型指标。公式(4)作用于整个船舶运动时域t∈[t₀,t_f]之上，可将受控系统在状态变量z(t)作为坐标轴构建成的状态空间内随时间推移描绘的状态轨线限制在特定范围内。

求解连续Bolza形式最优控制问题，即求取容许控制变量u(t)并确定终止时刻t_f，使得目标泛函沿着相应的状态变量z(t)取得最小值。

本发明提供一种无人船舶局部路径动态优化方法，如图1所示，包括以下步骤：

步骤S1、构建局部路径动态优化模型。

ⅰ、确定船舶运动学模型

由于船舶局部路径优化主要考虑船舶的水面运动，因此本发明选取三自由度(艏摇、纵荡和横荡)模型描述船舶运动。目标船舶s的三自由度运动学方程如下：

其中，X_H、Y_H、N_H分别代表船体在相应的自由度方向上的力和力矩；下标P、R的变量分别代表桨和舵的力和力矩；下标为wind、wave和current的变量分别为风、浪、流的力和力矩；m为船舶质量；m_x和m_y为作用在重心处合力沿x轴和y轴的分量；I_ZZ为绕z轴的转动惯量；J_ZZ为绕z轴的附加转动惯量；u_s、v_s和r_s分别为纵向速度、横向速度和绕z轴转动的角速度；x，y分别为纵向、横向的坐标。

为简化问题，便于计算，将上述三自由方程变换为一阶微分方程：

其中，

为船舶航向；δ_s是舵角变化量。方程(6)和方程(7)能够组成最优控制标准方法处理的标准一阶微分方程组。

将公式(6)、(7)与公式(2)对比发现，

均属于状态变量z(t)，而

属于控制变量u(t)。即如果给定船舶s在初始时刻t₀的运动状态z(t₀)以及运动时域[t₀,t_f]上的控制变量u(t)，可以通过积分计算唯一确定在该时域上的运动状态z(t)，对应船舶s唯一的航行轨迹。

当然，选取三自由度模型描述船舶运动仅仅是优选，可以想见，选取四自由度模型或六自由度模型也可以用来描述船舶运动。

ⅱ、确定几何路径约束条件

几何路径约束条件是指在时域[t₀,t_f]上对船舶航行产生限制作用的约束条件。为保证船舶航行安全性以及经济性，本发明几何路径约束条件主要包括防撞避碰约束和状态变量区间限制约束。

①防撞避碰约束

其中，x_s(t)，y_s(t)为目标船s在坐标系上的横坐标与纵坐标；x_o(t)，y_o(t)为碍航物o在坐标系上的横坐标与纵坐标；d_os为船舶s与碍航物o之间的安全距离。

本发明为简化问题，在防撞避碰约束中仅考虑了距离因素，保证了船舶航行过程中不与动态或静态碍航物发生碰撞，规避潜在的避碰风险，保证船舶在整个运动时域[t₀,t_f]上的航行安全性。

②状态变量区间限制约束

|δ_s(t)|≤δ_max (9)

|v_s(t)|≤v_max (10)

其中，δ_max为航行过程中的最大舵角变化量；v_max为航行过程允许的最大安全航速。

iii、确定边值约束条件

边值条件是目标船s初始时刻状态z_s(t₀)及终止时刻状态z_s(t_f)需要满足的条件。

①船舶的初始时刻状态各个参数可以通过船载雷达及AIS等设备采集得到，是已知的，不受约束条件限制，则有：

x_s(t₀)＝x₀,y_s(t₀)＝y₀,v_s(t₀)＝v₀,ψ_s(t₀)＝ψ₀,δ_s(t₀)＝δ₀ (11)

其中，x₀为初始横坐标；y₀为初始纵坐标；v₀为初始速度；ψ₀为初始航向；δ₀为初始舵角。

②终止时刻的约束条件为：

ψ_s(t₀)＝ψ_s(t_f) (12)

其中，ψ_s(t₀)为目标船s初始时刻航向；ψ_s(t_f)为目标船s终止时刻航向。使终止时刻航向与初始时刻航向相同，保证目标船s局部路径完成后恢复到原有计划航线。

ⅳ、确定船舶目标函数

其中，t₀为初始时刻；t_f为终止时刻；λ₁，λ₂为权重系数，且λ₁,λ₂∈[0,1]，λ₁+λ₂＝1。使局部路径优化过程在最短时间内完成，且规划出的路径应尽可能的短，保证航行的时效性。

综上，在本发明的一具体实施例中，局部路径动态优化模型优选为：

目标函数：公式(13)

约束条件：船舶运动学模型 公式(7)

几何路径约束条件 公式(8)、(9)、(10)

边值约束条件 公式(11)、(12)

结合公式(1)分析，上述目标函数给出的复合型最小化性能指标J由末值型性能指标Φ(z(t₀),t₀,z(t_f),t_f)和积分型性能指标

两部分构成。末值型性能指标用于描述任务对终止时刻状态的优化需求，积分型性能指标用于表征任务对整个时域上某项指标积累程度的优化需求。

步骤S2、获取目标船舶的静态信息和初始运动状态信息。

具体地，目标船舶的静态信息包括船舶总长、型宽、型深、吃水、最高航速、载重量和主推进电机功率；目标船舶的初始运动状态信息包括船舶位置、航向、正横方向速度、首尾方向速度和航向变化率。

步骤S3、从初始时刻起，依据目标船舶实时获取的周围碍航物信息，基于静态信息、初始运动状态信息和预先构建的局部路径动态优化模型，采用粒子群算法对局部路径动态优化模型进行求解，确定用于目标船舶实时行驶的最优控制曲线。

具体地，周围碍航物信息包括碍航物的规模及每个碍航物的位置、大小和速度。

具体地，在采用粒子群算法对局部路径动态优化模型进行求解之前，包括，

向局部路径动态优化模型中引入辅助变量w(t)，将复合型性能指标转化为末值型性能指标：

J＝Φ(z(t₀),t₀,z(t_f),t_f)+w(t_f)

w(t₀)＝0 (14)

依据末值型性能指标，转化局部路径动态优化模型：

其中，z(t)为被微分后的系统状态变量；u(t)为控制变量；J为末值型性能指标函数；f为常微分方程中的代数函数部分；g为不等式约束；h为等式约束；t₀为起始时刻；t_f为终止时刻。此时，上述连续Bolza型最优控制问题可提炼为以下标准格式，简化问题，便于求解。求解最优控制问题即求解符合约束条件的控制变量以及终止时刻，使性能指标泛函最小化。

具体地，采用粒子群算法对局部路径动态优化模型进行求解，确定用于目标船舶实时行驶的最优控制曲线，如图2所示，包括以下步骤：

步骤A1、对局部路径动态优化模型进行预处理。

步骤A2、初始化粒子群规模、每个粒子的速度和位置。

步骤A3、依据目标船舶实时获取的周围碍航物信息，基于静态信息、初始运动状态信息和预处理的局部路径动态优化模型，求解目标船舶状态变量。

步骤A4、依据目标船舶状态变量，计算目标函数和适应度函数，更新当代粒子群最优解。

步骤A5、根据粒子群更新公式，更新每一个粒子的位置与速度。

步骤A6、判断是否满足预设条件，若是，输出解向量(状态变量最优解及当前时刻的控制变量)；若否，重复步骤A3。

步骤A7、对解向量进行插值处理，获得最优控制曲线。

优选地，对局部路径动态优化模型进行预处理，包括控制变量离散化处理和离散化控制变量模型中约束条件的处理；约束条件的处理包括采用欧拉方程处理船舶运动学模型、基于内点障碍函数法处理几何路径约束条件、基于限制解向量范围处理边值约束条件和归一化处理时间约束。

进一步优选地，控制变量离散化处理，包括：

ⅰ、控制变量参数化：

其中，[τ_k-1,τ_k)为控制曲线的第k段时间段；τ_k,k＝1,…,n_p为时间节点，ξ_k为第k段时间段上的控制变量值；n_p为时间节点个数。

采用控制变量参数化法，通过将控制变量离散化结合内点法完成最优控制求解。控制变量参数化将控制变量曲线u(t)在t∈[t₀,t_f]上分成n_p个时间段，在每个时间段上通过插值法选取某个时间点上的控制变量值构造控制变量在该时间段上的曲线，从而将求解的最优曲线无限维参数化优化问题降维成有限维参数化优化问题。

ii、将参数化的控制变量(公式16)代入局部路径动态优化模型，获得第一预处理局部路径动态优化模型：

g(z(t)，ξ_k，t)≤0，t∈τ_k-1，τ_k)，k＝1，2，，…，n_p (21)

h(z(t)，ξ_k，t)＝0，t∈τ_k-1，τ_k)，k＝1，2，，…，n_p (22)

其中，z(t)为被微分后的系统状态变量；z(t_f)为终止时刻系统状态变量；

为拟合后的控制变量；ξ_k为第k段时间段上的控制变量值；f为常微分方程中的代数函数部分；g为不等式约束；h为等式约束；τ_k-1，τ_k)为控制曲线的第k段时间段。

将公式(16)代入公式(15)所示的最优控制问题中，原问题转化为有约束的非线性规划(Nonlinear Programming，NLP)问题。当时间段个数n_p趋于无穷大时，原问题与上述控制变量参数离散化后的非线性规划问题相互等价。

进一步优选地，基于内点障碍函数法处理几何路径约束条件，包括：

i、将第一预处理局部路径动态优化模型表示为：

minJ(Λ)，

s.t.g(Λ)≤0， (23)

h(Λ)＝0.

其中，Λ为决策变量；g(Λ)＜0为不等式约束；h(Λ)为等式约束。

向第一预处理局部路径动态优化模型中引入松弛变量s＞0，获得第二预处理局部路径动态优化模型：

minJ(Λ)，

s.t.g(Λ)+s＝0，

h(Λ)＝0， (24)

s>0

将公式(23)中的不等式约束g转化为等式约束。

ⅱ、将第二预处理局部路径动态优化模型中的不等式约束条件s>0作为障碍项添加到性能函数，获得第三预处理局部路径动态优化模型：

s.t.s>0 (25)

其中，Λ为决策变量；s为松弛变量；

为包含障碍项的目标函数；J为目标函数；r_k>0,r_k→0⁺是障碍因子。

形成包含障碍项的性能函数，也可称为障碍函数，随着r_k趋于0⁺，

的最小值越来越接近原目标函数J的最小值。将原问题转为一个无约束规划问题的近似解。

进一步优选地，归一化处理时间约束，包括：

ⅰ、采用归一化映射方法计算时间约束：

采用归一化映射的方法将时间约束进行简化。

ⅱ、定义粒子群中解结构：

其中，τ₀＝t₀；

ξ_k，k＝1,2,…,n_p为公式(16)第k时间段上具有边值约束的控制变量参数值；θ_k，k＝1,2,…,n_p为用于计算时间节点的无约束“时间变量”。

优选地，适应度函数fit包括：

fit＝α₁fit₁+α₃fit₂+α₃fit₃ (28)

其中，fit₁为安全性适应度函数；fit₂为路径长度适应度函数；fit₃为边值约束适应度函数；α₁,α₂,α₃为权重系数，且α₁+α₂+α₂＝1，α₁,α₂,α₂∈[0,1]；(x_o(t),y_o(t))为碍航物在t时刻位置坐标；(x_s(t),y_s(t))为目标船舶s在t时刻位置坐标；

为粒子i代表的路径i；

为粒子i-1代表的路径i-1，每个粒子代表一条路径；(x_i0,y_i0)为第i条路径起点坐标，(x_if,y_if)为第i条路径终点坐标，ψ₀为目标船舶初始航向。

适应度函数可以区分粒子个体的好坏，是引导粒子群算法向目标函数最优解逼近的关键因素。在局部路径动态优化过程中主要结合最优控制的目标泛函、状态约束条件、几何路径约束条件，寻求约束集的一个最小量化指标。本发明适应度函数的设定主要从以下两方面考虑：从安全性角度，即保证本船不与碍航物发生碰撞；从效率性角度，即优化出的局部路径尽可能短，并且避让结束后，船舶能够恢复到原有计划航线上。

优选地，粒子群更新公式包括：

其中，x_n＝(x_n1,x_n2,…,x_nD)是指粒子n在D维空间中的位置；v_n＝(v_n1,v_n2,…,v_nD)是指粒子的飞行速度；

为粒子n在第k+1代，在d维分量上的速度，d∈[1,D]；ω为惯性权重因子；

为粒子n第k+1代在d维分量上的位置；p_best为粒子n当前位置找到的最优解；g_best为整个粒子群找到的最优解；c₁，c₂为学习因子，也称加速因子，一般情况下c₁,c₂∈[1,2.5]；rand为[0,1]之间的随机数。步骤S4、目标船舶根据确定的最优控制曲线，控制船舶行驶。

仿真实验

为验证本发明所建模型的可行性及准确性，设置如下仿真实验。以实验基地内的“智飞号”集装箱船为目标船舶，其静态信息见表2，初始运动状态信息见表3。

表1粒子群算法参数设置

表2目标船舶静态信息

表3目标船舶初始运动状态信息

注：x、y为大地坐标系下的位置坐标；ψ为航向；V为随船舶坐标系下船舶正横方向的速度；u为随船舶坐标系下船舶首尾方向的速度；r为航向变化率。

设置动态障碍船O₁和O₂，进行局部路径优化仿真，效果图见附3-6。图3中，目标船与动态障碍船O₁相遇，从障碍船船首前方驶过，进行局部避碰；图4中，目标船已完成对动态障碍船O₁的局部避碰，并已恢复到原有计划航线；图5中，目标船与动态障碍船O₂会遇，目标船采取从O₂船尾后方避让来船的措施进行避碰；图6中，目标船完成对动态障碍船O₂的避碰，开始恢复原有计划航线。

为验证本发明所建模型的可靠性及优越性，本发明将本发明算法与基于遗传算法的路径优化方法进行比较。遗传算法参数设置见表4。

表4遗传算法参数设置

对比仿真中，设定三条障碍船的航向、初始位置分别为：120°，(0,200)；335°，(150,0)；65°，(300,300)。障碍船具体参数和本船设置相一致，详见表2。仿真对比结果如图7所示。

图7中虚线为障碍船运动轨迹，可以明显看出，目标船可有效完成局部避障。两种方法的运行结果如表5所示。

表5仿真结果统计表

以上验证从耗时和路径长度两方面进行比较，可以看出，本发明方法规划出的路径较传统遗传算法规划出的路径长度短，耗时减少8.7％。综合考虑，基于粒子群加速计算和动态最优控制的局部路径优化方法具有较高的优越性和可行性。

需要理解的是，以上对本发明的具体实施例进行的描述只是为了说明本发明的技术路线和特点，其目的在于让本领域内的技术人员能够了解本发明的内容并据以实施，但本发明并不限于上述特定实施方式。凡是在本发明权利要求的范围内做出的各种变化或修饰，都应涵盖在本发明的保护范围内。

Claims (8)

1.一种无人船舶局部路径动态优化方法，其特征在于，包括以下步骤：

步骤S1、获取目标船舶的静态信息和初始运动状态信息；

步骤S2、从初始时刻起，依据目标船舶实时获取的周围碍航物信息，基于所述静态信息、所述初始运动状态信息和预先构建的局部路径动态优化模型，采用粒子群算法对所述局部路径动态优化模型进行求解，确定用于目标船舶实时行驶的最优控制曲线；

采用粒子群算法对所述局部路径动态优化模型进行求解，确定用于目标船舶实时行驶的最优控制曲线，包括：

步骤A1、对所述局部路径动态优化模型进行预处理；

对所述局部路径动态优化模型进行预处理，包括：控制变量离散化处理和离散化控制变量模型中约束条件的处理；约束条件的处理包括采用欧拉方程处理船舶运动学模型、基于内点障碍函数法处理几何路径约束条件、基于限制解向量范围处理边值约束条件和归一化处理时间约束；

步骤A2、初始化粒子群规模、每个粒子的速度和位置；

步骤A3、依据目标船舶实时获取的周围碍航物信息，基于所述静态信息、所述初始运动状态信息和预处理后的局部路径动态优化模型，求解目标船舶状态变量；

步骤A4、依据所述目标船舶状态变量，计算目标函数和适应度函数，更新当代粒子群最优解；

步骤A5、根据粒子群更新公式，更新每一个粒子的位置与速度；

步骤A6、判断是否满足预设条件，若是，输出解向量；若否，重复步骤A3；

步骤A7、对所述解向量进行插值处理，获得最优控制曲线；

步骤S3、所述目标船舶根据确定的最优控制曲线，控制船舶行驶；

所述局部路径动态优化模型是基于船舶运动学模型、几何路径约束条件、边值约束条件和目标函数的动态最优控制模型。

2.根据权利要求1所述的方法，其特征在于，所述船舶运动学模型包括三自由度船舶运动学模型或四自由度船舶运动学模型或六自由度船舶运动学模型。

3.根据权利要求1所述的方法，其特征在于，所述局部路径动态优化模型包括，

目标函数：

其中，t₀为初始时刻；t_f为终止时刻；λ₁，λ₂为权重系数，且λ₁,λ₂∈[0,1]，λ₁+λ₂＝1；

约束条件：

ⅰ、船舶运动学模型：

其中，ψ_s为目标船舶s的航向；u_s为目标船舶s的纵向速度；v_s为目标船舶s的横向速度；r_s为目标船舶s绕z轴转动的角速度；

ⅱ、几何路径约束条件：

防撞避碰约束条件

其中，x_s(t)，y_s(t)为目标船舶s在坐标系上的横坐标与纵坐标；x_o(t)，y_o(t)为碍航物o在坐标系上的横坐标与纵坐标；d_os为船舶s与碍航物o之间的安全距离；

状态变量区间限制约束条件

|δ_s(t)|≤δ_max

|v_s(t)|≤v_max

其中，δ_max为航行过程中允许的最大舵角变化量；v_max为航行过程允许的最大安全航速；

iii、边值约束条件：

x_s(t₀)＝x₀，y_s(t₀)＝y₀，v_s(t₀)＝v₀，ψ_s(t₀)＝ψ₀，δ_s(t₀)＝δ₀

ψ_s(t₀)＝ψ_s(t_f)

其中，x₀为初始横坐标；y₀为初始纵坐标；v₀为初始速度；ψ₀为初始航向；δ₀为初始舵角。

4.根据权利要求1所述的方法，其特征在于，在所述采用粒子群算法对所述局部路径动态优化模型进行求解之前，包括：

向局部路径动态优化模型中引入辅助变量w(t)，将复合型性能指标转化为末值型性能指标：

J＝Φ(z(t₀)，t₀，z(t_f)，t_f)+w(t_f)

w(t₀)＝0

依据末值型性能指标，转化局部路径动态优化模型：

minJ＝(z(t)，u(t_f))

其中，z(t)为被微分后的系统状态变量；u(t)为控制变量；J为末值型性能指标函数；f为常微分方程中的代数函数部分；g为不等式约束；h为等式约束；t₀为起始时刻；t_f为终止时刻。

5.根据权利要求1所述的方法，其特征在于，所述控制变量离散化处理，包括：

i、控制变量参数化：

其中，[τ_k-1，τ_k)为控制曲线的第k段时间段；τ_k，k＝1，…，n_p为时间节点，ξ_k为第k段时间段上的控制变量值；n_p为时间节点个数；

ii、将参数化的控制变量代入局部路径动态优化模型，获得第一预处理局部路径动态优化模型：

g(z(t)，ξ_k，t)≤0，t∈[τ_k-1，τ_k)，k＝1，2，…，n_p

h(z(t)，ξ_k，t)＝0，t∈[τ_k-1，τ_k)，k＝1，2，…，n_p

其中，z(t)为被微分后的系统状态变量；z(t_f)为终止时刻系统状态变量；

为拟合后的控制变量；ξ_k为第k段时间段上的控制变量值；f为常微分方程中的代数函数部分；g为不等式约束；h为等式约束；[τ_k-1，τ_k)为控制曲线的第k段时间段。

6.根据权利要求5所述的方法，其特征在于，所述基于内点障碍函数法处理几何路径约束条件，包括：

i、将第一预处理局部路径动态优化模型表示为：

minJ(Λ)，

s.t.g(Λ)≤0，

h(Λ)＝0.

其中，Λ为决策变量；g(Λ)＜0为不等式约束；h(Λ)为等式约束；

向第一预处理局部路径动态优化模型中引入松弛变量s＞0，获得第二预处理局部路径动态优化模型：

minJ(Λ)，

s.t.g(Λ)+s＝0，

h(Λ)＝0，

s＞0

ii、将第二预处理局部路径动态优化模型中的不等式约束条件s＞0作为障碍项添加到性能函数，获得第三预处理局部路径动态优化模型：

s.t.s＞0

其中，Λ为决策变量；s为松弛变量；

为包含障碍项的目标函数；J为目标函数；r_k＞0，r_k→0⁺是障碍因子。

7.根据权利要求5所述的方法，其特征在于，所述归一化处理时间约束，包括：

i、采用归一化映射方法计算时间约束：

ii、定义粒子群中解结构：

其中，τ₀＝t₀；

ξ_k，k＝1，2，…，n_p为第k时间段上具有边值约束的控制变量参数值；θ_k，k＝1，2，…，n_p为用于计算时间节点的无约束“时间变量”，i、j为k区间内任意两个不相等的参数值。

8.根据权利要求1所述的方法，其特征在于，所述适应度函数fit包括：

fit＝α₁fit₁+α₂fit₂+α₃fit₃

其中，fit₁为安全性适应度函数；fit₂为路径长度适应度函数；fit₃为边值约束适应度函数；α₁，α₂，α₃为权重系数，且α₁+α₂+α₃＝1，α₁，α₂，α₃∈[0，1]；(x_o(t)，y_o(t))为碍航物在t时刻位置坐标；(x_s(t)，y_s(t))为目标船舶s在t时刻位置坐标；

为粒子i代表的路径i；

为粒子i-1代表的路径i-1，每个粒子代表一条路径；(x_i0，y_i0)为第i条路径起点坐标，(x_if，y_if)为第i条路径终点坐标，ψ₀为目标船舶初始航向。

Images