CN115407780A - 一种基于三阶段避障策略的无人船局部路径规划方法 - Google Patents

Abstract

本发明公开的一种基于三阶段避障策略的无人船局部路径规划方法，属于无人船路径规划技术领域。阶段0采用动态避障操纵范围和动态目标点，解决动态窗口法的固有问题；且在动态目标点的选择过程中引入环境的影响，降低无人船在静态避障过程的能耗。阶段1结合速度障碍法和《国际海上避碰规则》，采用速度障碍限制，降低无人船在避障过程中的碰撞风险和避障难度。阶段2结合速度障碍法和《国际海上避碰规则》，采用左舷限制和右舷评分机制，实现无人船基于《国际海上避碰规则》的避障。同时，通过基于碰撞风险值的双阈值方法，有效减小阶段1和阶段2的避障策略切换过程中的抖动问题。本发明适用于海上运输等领域，提高无人船避障过程的安全性。

Description

一种基于三阶段避障策略的无人船局部路径规划方法

技术领域

本发明涉及一种基于三阶段避障策略的无人船局部路径规划方法，属于无人船路径规划技术领域。

背景技术

随着智能船舶的提出，无人船在救援、海上作业、海洋探测等方面的应用广泛。因此，对无人船的自主航行性能要求也更高，而航线规划技术是自主航行的重要部分。无人船的自主航行通常跟随提前规划好的全局路径行驶，但是在实际行驶过程中还存在一些问题和挑战：出现在全局路径规划时未被预知的障碍物；同时出现静态障碍物和动态船舶的复杂形势；动态窗口法存在的固有问题；海上复杂的风、浪、流信息对无人船避障过程的影响；动态船舶之间的避障需符合《国际海上避碰规则》；分阶段避障过程中阶段切换的抖动问题。

无人船的全局路径规划通常根据电子海图获取环境信息，因此，在全局路径规划中的环境信息的准确度受到电子海图精度的限制；在无人船行驶过程中，可能出现没有被提前预知的静态障碍物，比如小型岛屿和礁石等；另外，由于电子海图无法获取动态船舶的具体信息，且无人船行驶过程中其周边船舶信息具有实时性，因此在全局路径规划时无法考虑动态船舶的避障。

现有的局部路径规划方法大多考虑在一个空旷的海域进行动态船舶之间的避碰，但是，在无人船实际行驶过程中，特别是在近海区域，可能会出现静态障碍物和动态船舶同时存在的场景，这增大了无人船局部路径规划的难度。

动态窗口法是局部路径规划方法中的常用技术之一，但是该技术存在不足：动态窗口法需要计算无人船与障碍物之间的距离项，将该项作为评价函数的指标之一，根据评价函数计算得到最佳速度，但是并没有规定障碍物距离项的计算范围，这将导致无人船进行过度避障，即距离障碍物过远，从而带来更高的避障能耗。在无人船实际避障过程中，可能会陷入局部最优位置，无人船在达到目标点之前停在某障碍物附近，因此无法实现有效避障。

在实际海域的避障过程中，无人船将受到海上风、浪、流的干扰，当该干扰信息处于无人船承受能力范围之内时，避障过程的所需能耗将受到影响。如果在避障过程中不考虑风、浪、流的干扰，则避障过程的能耗可能会增加，这不利于无人船高效避障。

无人船与动态船舶之间的避障操作需要遵循《国际海上避碰规则》，以下简称《规则》。《规则》是国际海事组织为了防止海上船舶之间的碰撞所指定的海上交通规则，可以保证船舶安全高效的航行。由于《规则》划分了不同的会遇局势，且对不同会遇局势规定了船舶的避碰操作，这为无人船的局部路径规划带来了挑战。

分阶段的避障策略在不同阶段之间切换时，需要考虑动态船舶的碰撞风险值。若只引入一个风险高阈值作为阶段之间的切换条件，当动态船舶的碰撞风险值在风险高阈值附近摇摆时，避障策略的阶段切换过程将发生抖动，无人船的避障行为也将出现摇摆过程，不利于无人船的安全避障。

发明内容

针对现有的局部路径规划技术没有考虑在无人船航行过程中既要避开静态障碍物，也要避开动态船舶，且动态船舶之间的避障需要遵循《国际海上避碰规则》的复杂情况，本发明的主要目的是提出一种基于三阶段避障策略的无人船局部路径规划方法，通过三阶段避障策略实现该复杂情况的避障，提高无人船避障过程的安全性。

本发明的目的是通过如下技术方案实现的：

本发明公开的一种基于三阶段避障策略的无人船局部路径规划方法，其中三阶段分别对应动态船舶距离较远、动态船舶距离较近但不满足风险条件、动态船舶距离较近且满足风险条件三种情况。阶段0的避障策略采用动态避障操纵范围和动态目标点，解决动态窗口法的固有问题；且在动态目标点的选择过程中引入环境的影响，降低无人船在静态避障过程的能耗。阶段1的避障策略结合速度障碍法和《国际海上避碰规则》，采用速度障碍限制，即VO限制，降低无人船在阶段1避障过程中的碰撞风险和避障难度。阶段2的避障策略结合速度障碍法和《国际海上避碰规则》，采用左舷限制和右舷评分机制，实现无人船基于《国际海上避碰规则》的避障。另外，通过一种基于碰撞风险值的双阈值方法实现阶段1和阶段2的避障策略的切换过程，有效减小了切换过程的抖动问题，提高了无人船避障过程的安全性。

本发明公开的一种基于三阶段避障策略的无人船局部路径规划方法，包含以下步骤：

步骤一：简化无人船运动学模型为三自由度，计算公式如下，

其中，[u v r]^T为无人船在载体坐标系的速度，[x y ψ]^T为无人船在北-东坐标系的位置，J(η)为两坐标系的转换矩阵。

步骤二：根据感知到的静态障碍物构建栅格化地图，根据海上风、浪、流信息作用于无人船的干扰力得到环境矢量场。

步骤三：使用模糊综合评价法计算动态船舶的碰撞风险值。

步骤3.1：计算感知障碍物的相关信息。

根据已知的感知半径R_perception，确定感知范围内的静态障碍物的位置和大小，以及动态船舶的位置、速度等信息。若感知范围内存在动态船舶，则计算船舶之间的碰撞风险值，否则无需计算碰撞风险值。碰撞危险值的影响因素包括最小会遇距离DCPA、最短会遇时间TCPA、两船距离D、障碍船相对方位B和船速比K，计算公式如下，

K＝v_T/v₀ (7)

其中，(x₀,y₀)、v₀和

分别表示无人船在北-东坐标系下的位置、速度和航向，(x_T,y_T)和v_T分别表示动态船舶在北-东坐标系下的位置和速度，v_R和

分别表示动态船舶相对于无人船的速度和航向，θ表示动态船舶相对于无人船的方位角。

步骤3.2：计算各个影响因素的隶属度函数。

计算最小会遇距离、最短会遇时间、两船距离、障碍船相对方位和船速比的隶属度函数，分别表示为U_DCPA、U_TCPA、U_D、U_B和U_K，计算公式如下，

其中，d₁表示无人船与动态船舶在行驶过程中的最小安全范围，d₂表示无人船安全通过距离。t₁表示动态船舶从最晚避让行动采取位置到最近会遇点的航行时间，t₂表示动态船舶从当前位置到最近会遇点的航行时间。D₁和无人船的尺寸相关，D₂表示无人船与动态船舶之间的安全距离值。C表示无人船与动态船舶之间的航向交角。

步骤3.3：使用权值求和方法计算碰撞风险值，计算公式如下，

CRI＝[ω_DCPA,ω_TCPA,ω_D,ω_B,ω_K]*[U_DCPA,U_TCPA,U_D,U_B,U_K]^T (13)

其中，ω_DCPA、ω_TCPA、ω_D、ω_B和ω_K分别对应五种影响因素的计算权值。

步骤四：定义三阶段避障策略中三个阶段分别的对应范围和决定条件，无人船根据对应范围和决定条件选择对应阶段的避障策略。

步骤4.1：定义三阶段避障策略中三个阶段分别的对应范围。

无人船艏向方向分别定义三个半径为R、2R、4R的相对于无人船艏向方向对称的半圆形区域，该区域用于判断动态船舶和无人船之间的距离方位情况，其中R的大小受到无人船自身运动特性和动态避障操纵域的约束，以4R为半径的半圆形区域应该位于无人船感知范围内，即R_perception＞4R，R_perception表示无人船的感知范围的最大半径大小。阶段0的对应范围：动态船舶位于无人船艏向方向的半径为4R的半圆形区域外。阶段1的对应范围：动态船舶位于无人船艏向方向的半径为2R和4R的半圆环区域内。阶段2的对应范围：动态船舶位于无人船艏向方向的半径为2R的半圆形区域内。

步骤4.2：定义三阶段避障策略中三个阶段分别的决定条件，如表1所示。其中distance表示无人船和动态船舶之间的距离，CRI表示动态船舶与无人船之间的碰撞风险值，CRI_high为碰撞风险高阈值，CRI_low为碰撞风险高阈值。当无人船感知范围内无动态船舶时，无人船始终采用阶段0的避障策略。当无人船感知范围内存在动态船舶时，若动态船舶位于阶段0的对应范围内，则采用阶段0的避障策略；若动态船舶位于阶段1的对应范围内，则采用阶段1的避障策略；若动态船舶位于阶段2的对应范围内，需根据动态船舶的碰撞风险值选择避障策略：若碰撞风险值高于风险高阈值，则采用阶段2的避障策略，碰撞风险值在避障过程中逐渐降低，直到碰撞风险值低于风险度低阈值，则采用阶段1的避障策略。

表1三阶段避障策略的决定条件

步骤4.3：由于在阶段2的对应范围内存在阶段1和阶段2的避障策略的切换问题，因此，结合碰撞风险值在阶段1和阶段2的避障策略之间的切换条件中采用双阈值方法，公式如下，

其中，Stage表示阶段编号，Flag_stage2表示当前是否采用阶段2的避障策略，值为1则表示采用阶段2的避障策略，值为0表示当前未采用阶段2的避障策略。当动态船舶位于阶段2的对应范围内，若碰撞风险值始终低于风险低阈值CRI_low，则采用阶段1的避障策略；若碰撞风险值高于风险高阈值CRI_high，则采用阶段2的避障策略，直到碰撞风险值低于风险低阈值，再切换到阶段1的避障策略。

步骤五：规定无人船和动态船舶处于对遇、右交叉、左交叉局面时，均主动转向右舷进行避让；无人船和动态船舶处于追遇局面时，无人船作为后方船从左舷或右舷超越动态船舶，使无人船的避障行为符合《规则》。

步骤六：通过动态避障操纵域和动态目标点改进动态窗口法，实现无人船的感知范围内，只有静态障碍物或无障碍物的情况下的避碰，即阶段0的避碰。

步骤6.1：确定动态窗口法相关参数，包括动态速度窗口的固定时间Δt和评价函数的计算权值α、β和γ。

步骤6.2：设定无人船的动态避障操纵域。

动态避障操纵域为包围无人船的一个椭圆区域，无人船位于椭圆区域偏后方的焦点处，椭圆长轴和无人船的艏向方向一致。当无人船的速度增大时，动态避障操纵域的范围增大，反之其范围减小。动态避障操纵域的椭圆参数计算公式如下，

其中，a和b分别为椭圆的长轴和短轴，R_min表示最小长轴值，R_self和R_obs分别表示无人船和障碍物的膨化半径，R_stop表示当前速度对应的最小制动距离，同时要求R对应范围大于椭圆区域的最大范围。u_max为无人船的最大速度，u(t)为无人船的当前速度。

步骤6.3：定义动态目标点的候选点集、候选点约束和评价函数，将动态目标点作为动态窗口法中的目标点。

在无人船艏向方向以无人船为圆心得到半径为R的半圆弧形，候选点集即位于该半圆弧形的一系列点。候选点约束在阶段0的避障策略中主要为障碍物限制。在选择动态目标点前，首先排除无人船到候选点的方向上一定宽度的范围内存在障碍物的候选点，即障碍物约束。计算评价函数，其最大值所对应的候选点则为动态目标点。动态目标点的评价函数G₀的计算公式如下，

G₀＝[dist_ob,dist_goal,g_w]ω₀ ^T (18)

其中，ω₀为计算权重。dist_ob表示候选点距离静态障碍物的最小距离。dist_goal和候选点与目标点之间的距离呈负相关。g_w表示无人船沿着候选点直线方向行驶固定距离Δx时环境矢量场对无人船做功大小。

dist_goal和g_w的计算公式如下，

g_w＝g_val·Δx·cos(θ_cg) (20)

其中，(x_c,y_c)，(x_goal,y_goal)和(x_USV,y_USV)分别表示候选点、目标点和无人船的位置坐标，g_val表示环境矢量场的大小，θ_cg表示无人船航向和当前环境矢量方向的夹角。

步骤6.4：动态目标点的动态选择和评价函数的动态变化。

通过动态目标点改进后的动态窗口法存在目标不可达的问题，为解决该问题，在真实目标点进入动态避障操纵域内时，将真实目标点作为动态目标点。同时，调整当前动态窗口法的评价函数的权值参数，增大目标距离项的对应权值，减小速度项和障碍物项的对应权值。

步骤七：基于改进后的动态窗口法，阶段1的避障策略中引入新的候选点约束，即基于速度障碍的VO限制；且优先选择符合《规则》的避碰行为，此优先性由预先定义的右舷候选点的阈值决定。同时实现静态障碍物和动态船舶的避碰。

步骤7.1：阶段1的候选点约束包括障碍物限制和VO限制。障碍物限制和阶段0一致，VO限制由速度障碍法得到。根据速度障碍法的原理，首先生成动态船舶无人船之间的相对碰撞锥，以下简称RCC。当无人船相对与动态船舶的速度位于RCC内部时，则两船在未来时刻将发生碰撞。将RCC沿着障碍船舶的速度方向平移得到速度障碍区域，以下简称VO。当无人船的速度位于VO内部时表示在未来一段时间内两船会发生碰撞。以无人船的当前速度大小为半径得到一个以无人船的位置为圆心的半圆弧形，该圆弧形和VO存在两个交点，分别从无人船的位置点连接两个交点得到两条直线，若以R为半径的半圆弧形边界上的候选点位于这两条直线范围内，则为不安全的候选点，放弃这些候选点。为了保证充足的避障空间，以无人船为圆心得到半径为R的半圆弧形边界，该半圆弧形和VO得到另外两个交点，分别从无人船的位置点连接这两个交点得到另外两条直线。根据这四条直线，选择最左侧和最右侧的直线确定最终的VO限制区域，若以R为半径的半圆弧边界上的候选点位于VO限制区域内，则放弃该候选点。该VO限制区域比原始区域更大，有利于为无人船提供充足的避障操作空间。

步骤7.2：定义阶段1的动态目标点的评价函数。阶段1的避障策略要求无人船的避障行为尽量符合《规则》。对于对遇、左交叉和右交叉三种会遇局势，若右舷的候选点数量大于设定阈值，则从右舷候选点选择动态目标点；若不满足阈值条件，则从无人船左舷的候选点选择动态目标点。阶段1的动态目标点的评价函数G₁的计算公式如下，

G₁＝[dist_ob,dist_goal,g_w]ω₁ ^T (21)

其中，ω₁表示计算权值。

步骤八：基于改进后的动态窗口法，阶段2的避障策略引入了新的候选点约束条件和评价函数项，即基于《规则》的左舷限制和右舷得分机制，同时避碰静态障碍物和动态船舶，且严格遵循《规则》。

步骤8.1：阶段2的候选点约束包括障碍物限制、VO限制和左舷限制。障碍物限制和阶段0一致，VO限制和阶段1一致，左舷限制根据《规则》定义。无人船在阶段2的避障过程中，针对会遇、左交叉和右交叉局势均转向右舷，因此对这三种局势引入左舷限制，排除位于无人船左舷的候选点，结合障碍物限制和VO限制得到可行的候选点集。

步骤8.2：由于阶段2的主要任务是避障操作，其优先级高于无人船能耗目标，因此在评价函数中删除环境矢量场的影响，同时引入右舷得分机制。阶段2的动态目标点的评价函数G₂的计算公式如下，

G₂＝[dist_ob,dist_goal,right_score]ω₂ ^T (22)

其中，ω₂为计算权值，right_score为所有候选点的右舷得分值，θ_left和θ_c分别表示无人船的正左舷方向和从无人船重心指向候选点的方向在北东坐标系的角度。Flag_overtaking表示无人船是否处于追越局面，值为1表示追越局面，值为0表示对遇或左交叉或右交叉局面。

步骤九：将步骤六、七、八的三阶段避障策略应用于无人船局部路径规划，提高无人船避障过程的安全性。

有益效果：

1、本发明公开的一种基于三阶段避障策略的无人船局部路径规划方法，针对无人船在行驶过程中同时出现未知的静态障碍物和动态船舶的复杂形势，通过3阶段避障策略，实现无人船同时对静态障碍物和动态船舶的避碰，且依据《规则》实现动态船舶的避碰。

2、本发明公开的一种基于三阶段避障策略的无人船局部路径规划方法，采用椭圆形动态避障操纵域，确定计算动态窗口法的评价函数中的障碍物距离项的位置范围，且动态避障操纵域的参数随无人船当前的运动参数动态变化，实现自适应调整参数，有效解决动态窗口法的过度避障问题，同时降低无人船在避障过程中的能耗。

3、本发明公开的一种基于三阶段避障策略的无人船局部路径规划方法，以无人船为圆心，半径为R的半圆形预测轨迹区域，从区域边界得到一系列候选点，根据定义的评价函数从候选点中选择最佳的动态目标点，将动态目标点作为动态窗口法中的目标点，实时引导无人船进行避障，有效解决动态窗口法的局部最优问题。

4、本发明公开的一种基于三阶段避障策略的无人船局部路径规划方法，在动态目标点的评价函数中引入环境力对无人船做功的相关项，降低了无人船受到海上风、浪、流干扰时在阶段0的避障过程中的能耗，提高无人船避障过程对外界环境的适应性。

5、本发明公开的一种基于三阶段避障策略的无人船局部路径规划方法，在阶段1的避障策略中优先选择符合《规则》的候选点集，基于阶段0中改进后的动态窗口法，结合速度障碍法提出了两种候选点约束，包括VO限制和障碍物限制，有效降低无人船在阶段2的避障过程中的碰撞风险和避障难度。

6、本发明公开的一种基于三阶段避障策略的无人船局部路径规划方法，在阶段2的避障策略中，基于改进后的动态窗口法，结合速度障碍法和《规则》提出了三种候选点约束，包括VO限制、障碍物限制和左舷限制。在动态目标点的评价函数中引入了右舷得分机制，实现无人船严格遵循《规则》对动态船舶的避碰。

7、本发明公开的一种基于三阶段避障策略的无人船局部路径规划方法，采用基于碰撞风险值的双阈值方法，根据当前动态船舶位置和碰撞风险值，计算阶段2避障的开始时刻和结束时刻，避免当动态船舶位于阶段2的对应范围内但是其碰撞风险度极低时，无人船对动态船舶的过度避障操作；有效减小无人船在阶段1和阶段2的避障策略中切换过程的抖动，使得避障更加安全可靠。

附图说明

图1是无人船基于三阶段避障策略的局部路径规划的流程图；

图2是北-东坐标系和载体坐标系；

图3是栅格地图和环境矢量场；

图4是三阶段避障策略中各阶段对应范围；

图5是四种会遇局势的判断条件；

图6是动态目标点和动态避障操纵域；

图7是动态目标点的确定方法；

图8是传统动态窗口法的局部最优问题；

图9是静态障碍物的避障实验结果；

其中，图(a)为权值为ω₀＝[0.4,0.5,0]的实验结果，图(b)为权值为ω₀＝[0.4,0.5,0.1]的实验结果；

图10是无人船在对遇局面的实验结果；

其中，图(a)为阶段1的避障过程，图(b)为阶段2的避障过程，图(c)表示避障结束，图(d)为两船之间的距离；

图11是无人船在左交叉局面的实验结果；

其中，图(a)为阶段2的避障过程，图(b)为阶段0的避障过程，图(c)表示避障结束，图(d)为两船之间的距离。

具体实施方式

下面将结合附图和实施例对本发明加以详细说明。同时也叙述了本发明技术方案解决的技术问题及有益效果，需要指出的是，所描述的实施例仅旨在便于对本发明的理解，而对其不起任何限定作用。

本实施例公开本发明提出的基于三阶段避障策略的无人船局部路径规划的流程图如附图1所示。以下实施方式在MATLAB中实现，目的是为了实现无人船基于《国际海上避碰规则》同时避碰静态障碍物和动态障碍物。

步骤一：北-东坐标系和载体坐标系如附图2所示，[u v r]^T为无人船在载体坐标系的速度，[x y ψ]^T为无人船在北-东坐标系的位置。初始化无人船的相关参数。无人船初始的位置矢量为

速度矢量为[0,0,0]^T。

步骤二：在MATLAB中构建大小为40*31且带有静态障碍物的栅格地图，并假设该区域均位于无人船的感知范围内。根据国际海洋数据中心获取海上气象数据，处理数据后计算风、浪、流信息对无人船的干扰力，综合三种干扰力建立环境矢量场。

无人船在水面行驶时，海风作用于无人船三个自由度方向的干扰力分别为X_wind，Y_wind和N_wind，计算公式如下，

其中，ρ_a为空气密度，A_f和A_s分别为无人船水线以上的正投影区域和横向投影区域，L_oa无人船的最大长度，U_R为相对平均风速，α_R为风舷角，C_WX(α_R)，C_WY(α_R)和C_WN(α_R)为风压系数，根据Isherwood公式计算。

无人船在水面行驶时，分析二阶波浪力对无人船运动的影响，海浪作用于无人船三个自由度方向的干扰力分别为X_wave，Y_wave和N_wave，计算公式如下，

其中，ρ为海水密度，L为无人船船长，a为平均波浪幅值，λ为对应波浪的波长，χ为无人船遭遇角，C_DX(λ)、C_DY(λ)、C_DN(λ)为波浪漂移力和力矩系数，可由下式计算

其中，L为无人船船长，λ为对应波浪的波长。

海流会造成无人船的绕轴力矩N_c，使得无人船的位置及速度发生变化，计算公式如下，

其中，V_c为洋流流速，L_s为无人船水线长度，β为漂角，C_Nc(β)为绕oz轴的流作用力系数。考虑到洋流随时间变化缓慢，可认为在一定时间内流速恒定。

对海上风、浪、流作用于无人船的干扰力进行加权计算，得到环境矢量场的大小，计算过程中只考虑海平面方向的干扰力，计算公式如下，

其中，u为环境矢量场在正北方向的大小，v为环境矢量场在正东方向的大小。u_cur，u_wind和u_wave分别为洋流、海风和海浪作用于无人船的干扰力在正北方向的分力大小。v_cur，v_wind和v_wave分别为洋流、海风和海浪作用于无人船的干扰力在正东方向的分力大小。

栅格地图和环境矢量场如附图3所示。图中黑色区域表示静态障碍物，白色区域表示海上无障碍物区域，绿色带方向的箭头表示环境矢量场的方向和大小。

步骤三：根据模糊综合评价法计算动态船舶的碰撞风险值。

步骤3.1：计算静态障碍物和动态船舶的相关信息。假设感知到的静态障碍物的位置和大小如附图3中的黑色区域所示。无人船可实时得到动态船舶的位置和速度。根据技术方案中步骤3.1的公式计算得到最小会遇距离DCPA、最短会遇时间TCPA、两船距离D、障碍船相对方位B和船速比K这5个碰撞风险值的影响因素。

步骤3.2：计算各个影响因素隶属度函数U_DCPA、U_TCPA、U_D、U_B和U_K。计算本实施例中设定最小会遇距离的隶属度函数U_DCPA的相关参数d₁和d₂，该计算过程和动态船舶相对于无人船的位置相关，根据《规则》以无人船为中心划定不同的角度范围，得到四种不同的会遇局势，如附图5所示。d₁和d₂的计算公式如下，

d₂＝2d₁ (30)

其中，B表示障碍船的相对方位，θ_T表示动态船舶的位置相对于无人船的角度，对应于附图5。

本实施例设定最小相遇时间的隶属度函数U_TCPA的相关参数t₁和t₂的计算公式如下，

其中，v_R表示动态船舶相对于无人船的速度。

本实施例设定两船距离的隶属度函数U_D的相关参数D₁和D₂的计算公式如下。

D₁＝2 (33)

步骤3.3：使用权值求和方法计算碰撞风险值。根据经验选择ω_DCPA、ω_TCPA、ω_D、ω_B和ω_K的值，计算公式如下，

CRI＝[0.4,0.367,0.167,0.033,0.033]*[U_DCPA,U_TCPA,U_D,U_B,U_K]^T (35)

步骤四：本实施例基于三阶段避障策略中三个阶段的对应范围和决定条件由步骤4.1和步骤4.2给出。

步骤4.1：三阶段避障策略中三个阶段分别的对应范围如附图4所示。R的值由船舶自身运动特性和动态避障操纵域决定，阶段0和阶段1的边界值4R应当在船舶的感知范围内，即R_perception＞4R，其中R_perception为感知范围半径大小。设定R＝8，感知范围半径R_perception＞48。阶段0的对应范围：动态船舶位于无人船艏向方向的半径为48的半圆形区域外。阶段1的对应范围：动态船舶位于无人船艏向方向的内半径为24和外半径为48的半圆环区域内。阶段2的对应范围：动态船舶位于无人船艏向方向的半径为24的半圆形区域内。

步骤4.2：三阶段避障策略中三个阶段的决定条件主要和风险高阈值CRI_high和风险低阈值CRI_low相关。设定CRI_high＝0.48，CRI_low＝0.05。若动态船舶位于阶段0的对应范围，则无人船采用阶段0的避障策略；若动态船舶位于阶段1的对应范围，则无人船采用阶段1的避障策略；若动态船舶位于阶段2的对应范围，则计算动态船舶和无人船之间的碰撞风险值，结合风险阈值条件判断采取阶段1或者阶段2的避障策略。

步骤4.3：在阶段2的对应范围内，存在阶段1和阶段2的避障策略的切换过程。根据步骤4.2提出的风险高阈值CRI_high和风险低阈值CRI_low构成双阈值判断条件。在阶段2的对应范围内，若动态船舶的碰撞风险值始终低于0.48，则采用阶段1的避障策略；若当动态船舶的碰撞风险值高于0.48，采用阶段2的避障策略，直到碰撞风险值低于0.05，则阶段2的避障过程结束，切换到阶段1的避障策略。

步骤五：规定无人船符合《规则》的避障行为。当无人船和动态船舶处于对遇、右交叉、左交叉局面时，无人船均主动转向右舷进行避让；当无人船和动态船舶处于追遇局面时，无人船作为后方船可从左舷或右舷超越动态船舶。

步骤六：阶段0的避碰算法针对无人船的感知范围内只有静态障碍物或无障碍物的情况，该阶段解决了的动态窗口法的固有问题，且基于改进后的动态窗口法实现阶段0的避碰。步骤6.1说明了传统的动态窗口法，动态窗口法的具体改进内容由步骤6.2、步骤6.3和步骤6.4给出。

步骤6.1：确定传统动态窗口法中的相关参数。

首先计算动态速度窗口，设定无人船的最大速度u_max＝1，最大角速度r_max＝40°，固定时间Δt＝3s，得到计算公式如下，

V_s(u,r)＝{(u,r)|0≤u≤1,0≤r≤40} (36)

V_f(u,r)＝V_s(u,r)∩V_d(u,r) (38)

其中，V_f(u,r)表示无人船的最大航速和最大艏摇角速度的约束，u和v分别表示将动态速度窗口离散化后得到的无人船的速度和角速度，u_max和r_max分别表示无人船的最大速度和最大角速度。V_d(u,r)则表示在固定时间Δt内无人船可达到的有限速度，u(t)和r(t)分别表示无人船在当前时刻t的速度和角速度。V_f(u,r)表示最终的速度窗口。

然后确定评价函数相关参数。设定评价函数的计算权重和障碍物距离项的最大距离的具体数值，计算公式如下，

G(u,r)＝0.55H(u,r)+0.35D(u,r)+0.1v(u,r) (39)

其中，H(u,r)用于评估预测路径末端和目标点之间的角度和无人船航行的夹角差，D(u,r)用于评估预测路径末端和最近障碍物之间的距离，v(u,r)用于评估预测路径末端无人船的速度值。

表示目标点相对于无人船的方位角，

表示无人船的航向角。dist_t表示最大距离内的无人船与障碍物的实际距离。

步骤6.2：设定无人船的动态避障操纵域，如附图6所示。动态避障操纵域呈椭圆形，无人船位于椭圆区域偏后方的焦点处。椭圆的长轴方向和无人船艏向一致，其参数随无人船速度变化而变化。设定无人船最大加速度

计算得到最小长轴值R_min＝1.67，设定无人船膨化半径R_self＝0.2，障碍物的膨化半径R_obs＝0.2。动态避障操纵域的参数计算公式如下，

R_stop＝u(t)²/0.6 (43)

步骤6.3：定义动态目标点的候选点集、候选点约束和评价函数，将动态目标点作为动态窗口法中的目标点。以无人船为圆心，得到无人船艏向方向的半径为R的半圆弧形边界，如附图6所示，候选点即位于该边界上的一系列点。候选点约束在阶段0的避障策略中主要为障碍物限制。在选择动态目标点前，首先排除无人船到候选点的方向上一定宽度的范围内存在障碍物的候选点，即障碍物约束，如附图6中的长矩形所示。计算评价函数，其最大值所对应的候选点则为动态目标点。设定计算权重ω₀＝[0.4,0.5,0.1]，动态目标点的评价函数G₀的计算公式如下，

G₀＝[dist_ob,dist_goal,g_w]·[0.4,0.5,0.1]^T (44)

其中，dist_ob表示候选点距离静态障碍物的最小距离。dist_goal和候选点与目标点之间的距离呈负相关。g_w表示无人船沿着候选点直线方向行驶固定距离Δx时环境矢量场对无人船做功大小。

dist_goal和g_w的计算公式如下，

g_w＝0.5g_val·u(t)·cos(θ_cg) (46)

其中，(x_c,y_c)，(x_goal,y_goal)和(x_USV,y_USV)分别表示候选点、目标点和无人船的位置坐标，g_val表示环境矢量场的大小，θ_cg表示无人船航向和当前环境矢量方向的夹角，设定Δx＝0.5u(t)。

步骤七：基于《规则》的阶段1的避碰算法同时实现静态障碍物和动态船舶的避碰。阶段1的避障策略优先选择符合《规则》的避碰行为，此优先性由预先定义的右舷候选点的阈值决定。设定右舷候选点的阈值为10。阶段1的动态目标点的约束条件和评价函数由步骤7.1和步骤7.2详细说明。

步骤7.1：阶段1的候选点约束包括障碍物限制和VO限制。障碍物限制和阶段0一致，VO限制由速度障碍法得到，如附图7所示。根据速度障碍法的原理，首先生成动态船舶无人船之间的相对碰撞锥，以下简称RCC。当无人船相对与动态船舶的速度位于RCC内部时，则两船在未来时刻将发生碰撞。将RCC沿着障碍船舶的速度方向平移即可得到速度障碍区域，以下简称VO。若无人船的速度位于VO内部，则在未来一段时间内两船会发生碰撞。以无人船的当前速度大小为半径得到一个以无人船的位置为圆心的半圆弧形，该圆弧形和VO存在两个交点，分别从无人船的位置点连接两个交点得到两条直线，若以R为半径的半圆弧形边界上的候选点位于这两条直线范围内，则为不安全的候选点，放弃这些候选点。为了保证充足的避障空间，以无人船为圆心得到半径为R的半圆弧形边界，该半圆弧形和VO得到另外两个交点，分别从无人船的位置点连接这两个交点得到另外两条直线。根据这四条直线，选择最左侧和最右侧的直线确定最终的VO限制区域，若以R为半径的半圆弧边界上的候选点位于VO限制区域内，则放弃该候选点。该VO限制区域比原始区域更大，有利于为无人船提供充足的避障操作空间。

步骤7.2：确定阶段1的动态目标点的评价函数。阶段1的避障策略要求无人船的避障行为尽量符合《规则》。对于对遇、左交叉和右交叉三种会遇局势，若右舷的候选点数量大于10，则从右舷候选点选择动态目标点；若不满足阈值条件，则从无人船左舷的候选点选择动态目标点。设定ω₁＝[0.45,0.5,0.05]，阶段1的动态目标点的评价函数G₁的计算公式如下，

G₁＝[dist_ob,dist_goal,g_w]·[0.45,0.5,0.05]^T (47)

步骤八：阶段2的避碰算法同时避碰静态障碍物和动态船舶，且严格遵循《规则》。基于改进后的动态窗口法，阶段2的避障策略引入了新的候选点约束条件和评价函数项，即基于《规则》的左舷限制和右舷得分机制。阶段2的候选点约束条件和评价函数由步骤8.1和步骤8.2详细说明。

步骤8.1：阶段2的候选点约束包括障碍物限制、VO限制和左舷限制。障碍物限制和阶段0一致，VO限制和阶段1一致，左舷限制根据《规则》定义。无人船在阶段2的避障过程中，针对会遇、左交叉和右交叉局势均转向右舷，因此对这三种局势引入左舷限制，排除位于无人船左舷的候选点，结合障碍物限制和VO限制得到可行的候选点集。

步骤8.2：确定阶段2的动态目标点的评价函数G₂。由于阶段2的主要任务是避障操作，其优先级高于无人船能耗目标，因此在评价函数中删除环境矢量场的影响，同时引入右舷得分机制。设定ω₂＝[0.5,0.3,0.2]，阶段2的动态目标点的评价函数G₂的计算公式如下，

G₂＝[dist_ob,dist_goal,right_score]·[0.5,0.3,0.2]^T (48)

其中，right_score为所有候选点的右舷得分值，θ_left和θ_c分别表示无人船的正左舷方向和从无人船重心指向候选点的方向在北东坐标系的角度。Flag_overtaking表示无人船是否处于追越局面，值为1表示追越局面，值为0表示对遇或左交叉或右交叉局面。

步骤九：根据步骤一至步骤八，本实施例在MATLAB中得到的实验结果分别由步骤9.1、步骤9.2和步骤9.3给出。其中，步骤9.1验证了阶段0避障算法和改进后的动态窗口法的有效性；步骤9.2和步骤9.3加入动态船舶，验证了本发明提出的完整避障策略和算法的有效性，在实验过程中设定动态船舶的安全半径为2。

步骤9.1：传统的动态窗口法容易陷入局部最优，如附图8所示，无人船无法完成有效避碰且未能到达目标点。本发明针对只存在静态障碍物的情况进行实验，如附图9所示。附图9(a)、(b)均表明无人船可有效避开局部最优位置并实现避障效果，安全到达目标点。附图9(a)不考虑环境的影响，附图9(b)引入了环境的影响，因此，附图9(b)中的避障路径更契合环境矢量场的方向。设定无人船的净能耗＝无人船在静水状态所需能耗-环境矢量场对无人船做功，计算得到无人船在附图9(b)中的净能耗为110.2203，无人船在附图9(a)中的净能耗为112.6559，结果表明引入环境影响后能够有效降低无人船的避障能耗。

步骤9.2：附图10为无人船采用本发明提出的三阶段避障策略在对遇局面的实验结果。动态船舶从(5，0)出发。附图10(a)采用阶段1的避障策略，优先选择无人船右舷方向的候选点作为动态目标点。附图10(b)采用阶段2的避障策略，无人船在对遇局面下转向右舷。附图10(c)表明无人船结束避障并安全达到目标点。附图10(d)表明避障过程中两船之间的最小距离为2.2518，大于动态船舶的安全半径，可以实现安全避障。

步骤9.3：附图11为无人船采用本发明提出的三阶段避障策略在左交叉局面的实验结果。动态船舶从(39,17)出发。附图11(a)采用阶段2的避障策略，无人船在左交叉局面转向右舷进行避障。附图11(b)中动态船舶的位于阶段0的对应范围内，采用阶段0的避障策略。附图11(c)表明无人船结束避障并安全达到目标点。附图11(d)表明避障过程中两船之间的最小距离为3.0446，大于动态船舶的安全半径，可以实现安全避障。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于三阶段避障策略的无人船局部路径规划方法，其特征在于：包括如下步骤，

步骤一：简化无人船运动学模型为三自由度；

步骤二：根据感知到的静态障碍物构建栅格化地图，根据海上风、浪、流信息作用于无人船的干扰力得到环境矢量场；

步骤三：使用模糊综合评价法计算动态船舶的碰撞风险值；

步骤四：定义三阶段避障策略中三个阶段分别的对应范围和决定条件，无人船根据对应范围和决定条件选择对应阶段的避障策略；

步骤五：规定无人船和动态船舶处于对遇、右交叉、左交叉局面时，均主动转向右舷进行避让；无人船和动态船舶处于追遇局面时，无人船作为后方船从左舷或右舷超越动态船舶，使无人船的避障行为符合《规则》；

步骤六：通过动态避障操纵域和动态目标点改进动态窗口法，实现无人船的感知范围内，只有静态障碍物或无障碍物的情况下的避碰，即阶段0的避碰；

步骤七：基于改进后的动态窗口法，阶段1的避障策略中引入新的候选点约束，即基于速度障碍的VO限制；且优先选择符合《规则》的避碰行为，此优先性由预先定义的右舷候选点的阈值决定；同时实现静态障碍物和动态船舶的避碰；

步骤八：基于改进后的动态窗口法，阶段2的避障策略引入了新的候选点约束条件和评价函数项，即基于《规则》的左舷限制和右舷得分机制，同时避碰静态障碍物和动态船舶，且严格遵循《规则》；

步骤九：将步骤六、七、八的三阶段避障策略应用于无人船局部路径规划，提高无人船避障过程的安全性。

2.如权利要求1所述的一种基于三阶段避障策略的无人船局部路径规划方法，其特征在于：步骤一的实现方法为，

其中，[u v r]^T为无人船在载体坐标系的速度，[x y ψ]^T为无人船在北-东坐标系的位置，J(η)为两坐标系的转换矩阵。

3.如权利要求1所述的一种基于三阶段避障策略的无人船局部路径规划方法，其特征在于：步骤三的实现方法为，

步骤3.1：计算感知障碍物的相关信息；

根据已知的感知半径R_perception，确定感知范围内的静态障碍物的位置和大小，以及动态船舶的位置、速度等信息；若感知范围内存在动态船舶，则计算船舶之间的碰撞风险值，否则无需计算碰撞风险值；碰撞危险值的影响因素包括最小会遇距离DCPA、最短会遇时间TCPA、两船距离D、障碍船相对方位B和船速比K，计算公式如下，

K＝v_T/v₀ (7)

其中，(x₀,y₀)、v₀和

分别表示无人船在北-东坐标系下的位置、速度和航向，(x_T,y_T)和v_T分别表示动态船舶在北-东坐标系下的位置和速度，v_R和

分别表示动态船舶相对于无人船的速度和航向，θ表示动态船舶相对于无人船的方位角；

步骤3.2：计算各个影响因素的隶属度函数；

计算最小会遇距离、最短会遇时间、两船距离、障碍船相对方位和船速比的隶属度函数，分别表示为U_DCPA、U_TCPA、U_D、U_B和U_K，计算公式如下，

其中，d₁表示无人船与动态船舶在行驶过程中的最小安全范围，d₂表示无人船安全通过距离；t₁表示动态船舶从最晚避让行动采取位置到最近会遇点的航行时间，t₂表示动态船舶从当前位置到最近会遇点的航行时间；D₁和无人船的尺寸相关，D₂表示无人船与动态船舶之间的安全距离值；C表示无人船与动态船舶之间的航向交角；

步骤3.3：使用权值求和方法计算碰撞风险值，计算公式如下，

CRI＝[ω_DCPA,ω_TCPA,ω_D,ω_B,ω_K]*[U_DCPA,U_TCPA,U_D,U_B,U_K]^T (13)

其中，ω_DCPA、ω_TCPA、ω_D、ω_B和ω_K分别对应五种影响因素的计算权值。

4.如权利要求1所述的一种基于三阶段避障策略的无人船局部路径规划方法，其特征在于：步骤四的实现方法为，

步骤4.1：定义三阶段避障策略中三个阶段分别的对应范围；

无人船艏向方向分别定义三个半径为R、2R、4R的相对于无人船艏向方向对称的半圆形区域，该区域用于判断动态船舶和无人船之间的距离方位情况，其中R的大小受到无人船自身运动特性和动态避障操纵域的约束，以4R为半径的半圆形区域应该位于无人船感知范围内，即R_perception＞4R，R_perception表示无人船的感知范围的最大半径大小；阶段0的对应范围：动态船舶位于无人船艏向方向的半径为4R的半圆形区域外；阶段1的对应范围：动态船舶位于无人船艏向方向的半径为2R和4R的半圆环区域内；阶段2的对应范围：动态船舶位于无人船艏向方向的半径为2R的半圆形区域内；

步骤4.2：定义三阶段避障策略中三个阶段分别的决定条件；

当无人船感知范围内无动态船舶时，无人船始终采用阶段0的避障策略；当无人船感知范围内存在动态船舶时，若动态船舶位于阶段0的对应范围内，则采用阶段0的避障策略；若动态船舶位于阶段1的对应范围内，则采用阶段1的避障策略；若动态船舶位于阶段2的对应范围内，需根据动态船舶的碰撞风险值选择避障策略：若碰撞风险值高于风险高阈值，则采用阶段2的避障策略，碰撞风险值在避障过程中逐渐降低，直到碰撞风险值低于风险度低阈值，则采用阶段1的避障策略；

步骤4.3：由于在阶段2的对应范围内存在阶段1和阶段2的避障策略的切换问题，结合碰撞风险值在阶段1和阶段2的避障策略之间的切换条件中采用双阈值方法，公式如下，

其中，Stage表示阶段编号，Flag_stage2表示当前是否采用阶段2的避障策略，值为1则表示采用阶段2的避障策略，值为0表示当前未采用阶段2的避障策略；当动态船舶位于阶段2的对应范围内，若碰撞风险值始终低于风险低阈值CRI_low，则采用阶段1的避障策略；若碰撞风险值高于风险高阈值CRI_high，则采用阶段2的避障策略，直到碰撞风险值低于风险低阈值，再切换到阶段1的避障策略。

5.如权利要求1所述的一种基于三阶段避障策略的无人船局部路径规划方法，其特征在于：步骤六的实现方法为，

步骤6.1：确定动态窗口法相关参数，包括动态速度窗口的固定时间Δt和评价函数的计算权值α、β和γ；

步骤6.2：设定无人船的动态避障操纵域；

动态避障操纵域为包围无人船的一个椭圆区域，无人船位于椭圆区域偏后方的焦点处，椭圆长轴和无人船的艏向方向一致；当无人船的速度增大时，动态避障操纵域的范围增大，反之其范围减小；动态避障操纵域的椭圆参数计算公式如下，

其中，a和b分别为椭圆的长轴和短轴，R_min表示最小长轴值，R_self和R_obs分别表示无人船和障碍物的膨化半径，R_stop表示当前速度对应的最小制动距离，同时要求R对应范围大于椭圆区域的最大范围；u_max为无人船的最大速度，u(t)为无人船的当前速度；

步骤6.3：定义动态目标点的候选点集、候选点约束和评价函数，将动态目标点作为动态窗口法中的目标点；

在无人船艏向方向以无人船为圆心得到半径为R的半圆弧形，候选点集即位于该半圆弧形的一系列点；候选点约束在阶段0的避障策略中主要为障碍物限制；在选择动态目标点前，首先排除无人船到候选点的方向上一定宽度的范围内存在障碍物的候选点，即障碍物约束；计算评价函数，其最大值所对应的候选点则为动态目标点；动态目标点的评价函数G₀的计算公式如下，

G₀＝[dist_ob,dist_goal,g_w]·ω₀ ^T (18)

其中，ω₀为计算权重；dist_ob表示候选点距离静态障碍物的最小距离；dist_goal和候选点与目标点之间的距离呈负相关；g_w表示无人船沿着候选点直线方向行驶固定距离Δx时环境矢量场对无人船做功大小；

dist_goal和g_w的计算公式如下，

g_w＝g_val·Δx·cos(θ_cg) (20)

其中，(x_c,y_c)，(x_goal,y_goal)和(x_USV,y_USV)分别表示候选点、目标点和无人船的位置坐标，g_val表示环境矢量场的大小，θ_cg表示无人船航向和当前环境矢量方向的夹角；

步骤6.4：动态目标点的动态选择和评价函数的动态变化；

通过动态目标点改进后的动态窗口法存在目标不可达的问题，为解决该问题，在真实目标点进入动态避障操纵域内时，将真实目标点作为动态目标点；同时，调整当前动态窗口法的评价函数的权值参数，增大目标距离项的对应权值，减小速度项和障碍物项的对应权值。

6.如权利要求1所述的一种基于三阶段避障策略的无人船局部路径规划方法，其特征在于：步骤七的实现方法为，

步骤7.1：阶段1的候选点约束包括障碍物限制和VO限制；障碍物限制和阶段0一致，VO限制由速度障碍法得到；根据速度障碍法的原理，首先生成动态船舶无人船之间的相对碰撞锥，以下简称RCC；当无人船相对与动态船舶的速度位于RCC内部时，则两船在未来时刻将发生碰撞；将RCC沿着障碍船舶的速度方向平移得到速度障碍区域，以下简称VO；当无人船的速度位于VO内部时表示在未来一段时间内两船会发生碰撞；以无人船的当前速度大小为半径得到一个以无人船的位置为圆心的半圆弧形，该圆弧形和VO存在两个交点，分别从无人船的位置点连接两个交点得到两条直线，若以R为半径的半圆弧形边界上的候选点位于这两条直线范围内，则为不安全的候选点，放弃这些候选点；为了保证充足的避障空间，以无人船为圆心得到半径为R的半圆弧形边界，该半圆弧形和VO得到另外两个交点，分别从无人船的位置点连接这两个交点得到另外两条直线；根据这四条直线，选择最左侧和最右侧的直线确定最终的VO限制区域，若以R为半径的半圆弧边界上的候选点位于VO限制区域内，则放弃该候选点；该VO限制区域比原始区域更大，有利于为无人船提供充足的避障操作空间；

步骤7.2：定义阶段1的动态目标点的评价函数；阶段1的避障策略要求无人船的避障行为尽量符合《规则》；对于对遇、左交叉和右交叉三种会遇局势，若右舷的候选点数量大于设定阈值，则从右舷候选点选择动态目标点；若不满足阈值条件，则从无人船左舷的候选点选择动态目标点；阶段1的动态目标点的评价函数G₁的计算公式如下，

G₁＝[dist_ob,dist_goal,g_w]·ω₁ ^T (21)

其中，ω₁表示计算权值。

7.如权利要求1所述的一种基于三阶段避障策略的无人船局部路径规划方法，其特征在于：步骤八的实现方法为，

步骤8.1：阶段2的候选点约束包括障碍物限制、VO限制和左舷限制；障碍物限制和阶段0一致，VO限制和阶段1一致，左舷限制根据《规则》定义；无人船在阶段2的避障过程中，针对会遇、左交叉和右交叉局势均转向右舷，因此对这三种局势引入左舷限制，排除位于无人船左舷的候选点，结合障碍物限制和VO限制得到可行的候选点集；

步骤8.2：由于阶段2的主要任务是避障操作，其优先级高于无人船能耗目标，因此在评价函数中删除环境矢量场的影响，同时引入右舷得分机制；阶段2的动态目标点的评价函数G₂的计算公式如下，

G₂＝[dist_ob,dist_goal,right_score]·ω₂ ^T (22)

其中，ω₂为计算权值，right_score为所有候选点的右舷得分值，θ_left和θ_c分别表示无人船的正左舷方向和从无人船重心指向候选点的方向在北东坐标系的角度；Flag_overtaking表示无人船是否处于追越局面，值为1表示追越局面，值为0表示对遇或左交叉或右交叉局面。

Images