CN114326702B - 一种基于模型预测控制的内河无人船避碰方法 - Google Patents

Abstract

本发明公开了一种基于模型预测控制的内河无人船避碰方法。属于无人船路径规划领域，具体步骤：1、对无人船及动态障碍物的安全区域进行建模；2、将无人船避碰的最优控制问题转化为非线性规划问题；3、构建控制目标、速度与速度增量的代价函数；4、构建用于评估无人船避碰操纵规则符合度的代价函数；5、求解得到的下一时刻无人船无碰撞风险的状态作为当前时刻无人船的状态；6、最终实现无人船对动、静态障碍物的自主避碰。本发明利用模型预测控制可以考虑多方面约束的特点，综合考虑内河航行规则、无人船运动学约束、航道条件等因素的影响并不断地对无人船进行滚动优化，从而计算出无碰撞风险的路径，实现无人船对动、静态障碍物的自主避碰。

Description

一种基于模型预测控制的内河无人船避碰方法

技术领域

本发明属于无人船路径规划领域，涉及一种基于模型预测控制的内河无人船避碰方法。

背景技术

无人船(Unmanned surface vehicle，USV)是一种可在水面自主导航的智能机器人，能够执行很多危险且耗时的任务，近年来，随着计算机技术、感知技术、人工智能技术的持续发展，以及来自商业、科研和环保等方面的强烈需求，无人船的相关技术已经成为机器人领域的一个研究热点。

有数据表明，当船舶在水面上航行时，大多数的事故是由于人为失误导致的，因此拥有良好的避碰功能不仅在一定程度上反映了无人船智能化水平的高低，也是实现无人船安全航行的关键技术。

目前很多学者在开阔水域无人船的自主避碰问题上取得了不错的研究成果。专利CN 112363512 A公开了一种基于艏向及航速的智能船路径规划方法，用于解决无人船在海洋开阔水域的路径规划问题。该发明可以提升船舶操纵的安全性，减小船舶避碰事故的发生。专利CN 112965477 A提出了一种基于动态安全区域和避碰规则的障碍物避碰方法。该发明还考虑了国际海事避碰规则的影响，可以使无人船能够沿预先规划的路径航行的同时规避障碍物。

相比于开阔水域，应用于内河航道的无人船与人们日常生活的联系更加密切，具有更加广阔的应用前景。然而，由于内河航道具有弯曲狭窄，跨河桥梁多、通航密度大以及部分河段水流速度大等特点，增加了无人船在内河航道航行时的风险和难度，也导致目前相关的避碰研究成果还很少。专利CN 113436467 A提出了一种无人船在河道航行时的避碰方法，在算法中主要考虑了航道边界的限制，可用于无人船在内河水域的避碰。但是该发明并没有仔细考虑无人船的运动学约束、内河航行规则及其他约束的要求。

发明内容

发明目的：为了克服现有技术中存在的不足，本发明目的是针对无人船在航道狭窄、跨河桥梁多、通航密度大以及部分河段水流速度大的内河航道上航行时的自主避碰问题，提出了一种通用性强且容易实现的基于模型预测控制的避碰方法。

技术方案：本发明所述的一种基于模型预测控制的内河无人船避碰方法，具体操作步骤如下：

(1)、对无人船及动态障碍物的安全区域进行建模；

(2)、将无人船避碰的最优控制问题转化为非线性规划问题，将无人船速度、速度增量、最大转向幅度以及航道边界约束转化为非线性规划问题的不等式约束；

(3)、构建用于对无人船避碰操纵合理性进行评估的控制目标、速度与速度增量的代价函数；

(4)、对无人船的会遇态势进行划分，构建用于评估无人船避碰操纵规则符合度的代价函数；

(5)、获取当前时刻无人船的状态以及障碍物的状态，通过模型预测控制法对所构造的代价函数进行求解，以得到下一时刻无人船无碰撞风险的状态，然后将求解得到的下一时刻无人船无碰撞风险的状态作为当前时刻无人船的状态；

(6)、在下一采样时刻重复步骤(5)，直至避碰完成，从而得到一条无碰撞风险的路径，最终实现无人船对动、静态障碍物的自主避碰。

进一步的，步骤(1)中，所述对无人船及动态障碍物的安全区域进行建模具体是：

通过采用大小随无人船或障碍物的速度而改变的改进Bumper模型对无人船及动态障碍物的安全区域进行建模。

进一步的，在步骤(2)中，所述将无人船避碰的最优控制问题转化为非线性规划问题的具体方法是：考虑水流因素的影响，采用4阶龙格库塔法来对无人船的运动学方程进行离散化，使用多重射击法将无人船避碰的OCP问题转化为NLP问题。

进一步的，在步骤(3)中，所述用于对无人船避碰操纵合理性进行评估的控制目标、速度与速度增量的代价函数的具体形式为：

其中，K_p，K_U，K_r，表示调节参数，p_k表示在t采样时刻USV的位置，/>表示在t采样时刻USV的参考位置，u_dk表示在t采样时刻USV的参考航行速度，r_dk表示在t采样时刻USV的参考艏摇角速度，u_k表示在t采样时刻USV的航行速度；r_k表示在t采样时刻USV的艏摇角速度。

进一步的，在步骤(4)中，所述用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

φ_COLREGs＝φ_heading∨φ_overtaking∨φ_crossing

式中，φ_heading为在对驶相遇遭遇情形下，用于评估无人船避碰操纵规则符合度的代价函数；φ_overtaking为在追越情形下，用于评估无人船避碰操纵规则符合度的代价函数；φ_crossing为在横越情形下，用于评估无人船避碰操纵规则符合度的代价函数。

进一步的，在对驶相遇遭遇情形下，用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

式中，K_g，K_oi表示调节参数；m，n，和i表示是取值为0或1的参数，取值情况根据规则要求而定；d_{oi_z}(k)表示t采样时刻USV与障碍船在地球固定北东坐标系下船舶横荡方向上的距离；d_{safe_heading}表示USV在对驶相遇情形下需要进行避碰操纵的安全距离。

进一步的，在追越情形下，用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

式中，K_g，K_oi表示调节参数；m，n，和i表示是取值为0或1的参数，取值情况根据规则要求而定；d_oi(k)表示t采样时刻USV与障碍船在地球固定北东坐标系下的欧几里得的距离；d_{safe_overtaking}表示USV在追越情形下需要进行避碰操纵的安全距离。

进一步的，在横越情形下，用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

式中，K_g，K_oi表示调节参数；m，n，和i表示是取值为0或1的参数，取值情况根据规则要求而定；d_oi(k)表示t采样时刻USV与障碍船在地球固定北东坐标系下的欧几里得的距离；d_{safe_crossing}表示USV在横越情形下需要进行避碰操纵的安全距离。

有益效果：本发明与现有技术相比，本发明的特点:本发明利用模型预测控制可以考虑多方面约束的特点，综合考虑内河航行规则、无人船运动学约束、航道环境等因素的影响并不断地对无人船进行滚动优化，从而得出无碰撞风险的路径，实现无人船对动、静态障碍物的自主避碰。本发明采用了动态的改进Bumper模型对无人船及动态障碍物的安全区域进行建模，改进后的模型其大小可根据无人船或障碍物的速度实时调整，提高了无人船的避碰效率和航行安全。本发明在原理和计算上很简单，具有高度的通用性，便于在实际工程中实现。

附图说明

图1是本发明的操作流程图；

图2是本发明中动态安全区域的示意图；

图3是本发明中用于划分无人船会遇态势的主要信息图，其中OS为USV；

图4是本发明中符合规则要求的船舶避碰示意图，其中OS为USV；

图5是本发明中对驶相遇会遇情境下，USV的仿真暂态图；

图6是本发明中追越情境下，USV的仿真暂态图；

图7是本发明中横越情境下，USV的仿真暂态图；

图8是本发明中对动、静态障碍物同时进行避碰时USV的仿真暂态图。

具体实施方式

以下结合附图和具体实施例，对本发明做出进一步说明。

本发明所述的一种基于模型预测控制的内河无人船避碰方法，用于解决无人船在内河复杂航道环境下航行时的自主避碰问题，算法的流程图如图1所示，具体实现过程为：

S1、采用大小随无人船或障碍物的速度而改变的改进Bumper模型对无人船及动态障碍物的安全区域进行建模，具体方法为：

当USV或障碍船的速度为0时，本实例将安全区域的大小选为一半径为B＝0.8L的圆形区域，这里L为船的长度，值为5m；

当USV或障碍船航速达到最大值时，安全区域的后半部分保持不变，前半部分取得最大值，即A＝5.2L，B＝0.8L；在以其他速度前航时，安全区域的大小介于这两者之间，即A＝0.8L+4.4L(u/u_max)，B＝0.8L，u_max为USV的最大航行速度，具体如图2所示。

S2、将无人船避碰的最优控制问题转化为非线性规划问题；将无人船速度、速度增量、最大转向幅度以及航道边界约束转化为非线性规划问题的不等式约束，其具体方法是；

考虑水流因素的影响，在无人船的运动学方程中加入水流项；采用4阶龙格库塔法来对无人船的运动学方程进行离散化，使用多重射击法(Multiple Shooting)将无人船避碰的OCP问题转化为NLP问题；

将无人船的速度、速度增量、最大转向幅度以及航道边界约束转化为非线性规划问题的不等式约束，具体方法为：

h(Δu_2:Np)＝[Δu_2:Np-Δu_max]≤0

h(Δr_2:Np)＝[Δr_2:Np-Δr_max]≤0

这里，本实例中，u_min表示USV允许达到的最小航行速度，在对驶相遇的交互情形下选为0m/s，在其他交互情形下选为2m/s；r_max，r_min分别表示USV允许达到的最大、最小艏摇角速度，分别选为40°和-40°；Δu_max表示单位时间内允许USV达到的最大速度增量，其值为1m/s²；Δr_max表示单位时间内允许USV达到的最大艏向角速度增量，其值为(π/30)rad/s²；x_max，x_min与y_max，y_min分别为在地球固定北东坐标系下航道边界的横、纵坐标最大值和最小值，分别设置为160m、0m、120m、0m；

S3、构建用于对无人船避碰操纵合理性进行评估的控制目标、速度与速度增量的代价函数，代价函数的具体形式为：

其中，K_p，K_U，K_r，是调节参数。p_k表示在t采样时刻USV的位置，/>表示在t采样时刻USV的参考位置，u_dk表示在t采样时刻USV的参考航行速度，r_dk表示在t采样时刻USV的参考艏摇角速度，u_k表示在t采样时刻USV的航行速度；r_k表示在t采样时刻USV的艏摇角速度；

S4、对无人船的会遇态势进行划分，构建用于评估无人船避碰操纵规则符合度的代价函数，所述代价函数φ_COLREGs的具体形式为：

φ_COLREGs＝φ_heading∨φ_overtaking∨φ_crossing

这里，φ_heading为在对驶相遇遭遇情形下，用于评估无人船避碰操纵规则符合度的代价函数；φ_overtaking为在追越情形下，用于评估无人船避碰操纵规则符合度的代价函数；φ_crossing为在横越情形下，用于评估无人船避碰操纵规则符合度的代价函数；

具体的：(1)、为了保证USV在行驶的过程中有效地避开障碍物，必须确保在t时刻，USV与障碍物的欧几里得距离L(t)大于等于R_ow(t)与R_obs(t)之和，即L(t)≥R_ow(t)+R_obs(t)。这里R_ow(t)为t时刻USV在相对于障碍物方位的安全区域地值，R_obs(t)为t时刻障碍物在相对于USV方位的安全区域的值。

(2)、对于拥有S个静态障碍物的集合O_s＝{O_s1,O_s2,…O_sS}，为了让USV有效地避开障碍物，对于每一个静态障碍物O_si＝(P_si,R_si)，不等式约束表示为：

这里R_si表示静态障碍物i的安全区域的半径；P_ow(t_1:Np+1)是t_1:Np+1时刻USV的位置矢量；P_si是静态障碍物i的位置矢量；

(3)、对于拥有M个动态障碍物的集合O_m＝{O_m1(t),O_m2(t),…O_mM(t)}，为了让USV有效地避开障碍物，对于每一个动态障碍物O_mi(t)＝(P_mi(t),R_mi)，不等式约束表示为：

这里R_mi(t_1:Np+1)是t_1:Np+1采样时刻，从障碍物o_mi的中心到AB连线与动态障碍物安全区域交点的距离；P_mi(t_1:Np+1)是t_1:Np+1采样时刻动态障碍物i的位置矢量；

(4)、根据无人船与障碍物在地球固定北东坐标系下的相对方位，对无人船的会遇态势进行分类，用于划分无人船会遇态势的主要信息如图3所示，具体描述如下：

其中，矢量表示USV在t时刻的航速，矢量/>表示障碍船在t时刻的航速，矢量/>表示t时刻从USV实时位置指向障碍船实时位置的单位矢量；ψ表示t时刻USV在地球固定北东坐标系下的艏摇角，逆时针方向为正，顺时针方向为负；

在t时刻，当d_oi(t)≤d_close(t)时，障碍船被认为接近于USV，需要进行避让操纵。这里d_oi(t)表示t时刻在地球固定北东坐标系下USV与障碍船之间的欧几里得距离；d_close(t)表示t时刻USV需要做出避让操纵的最小距离；

在t时刻，当d_oi(t)≤d_{close_heading}，且/> 时，USV被认为与障碍船形成对驶相遇的交互情形，本实例中d_{close_heading}值为60m，θ₁值为40°，θ₂值为7.5°；

在t时刻，当d_oi(t)≤d_{close_overtaking}， 且USV落后于障碍船时，USV被认为正在追越障碍船，本实例中d_{close_overtaking}值为30m，θ₃值为68.5°；

在t时刻，当d_oi(t)≤d_{close_crossing}，且USV朝向障碍船时，USV被认为正在横越障碍船，本实例中d_{close_crossing}值为30m，θ₄值为68.5°；

其中，

在对驶相遇遭遇情形下，用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

式中，K_g，K_oi表示调节参数；m，n，和i表示是取值为0或1的参数，取值情况根据规则要求而定；d_{oi_z}(k)表示t采样时刻USV与障碍船在地球固定北东坐标系下船舶横荡方向上的距离；d_{safe_heading}表示USV在对驶相遇情形下需要进行避碰操纵的安全距离；

在追越情形下，用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

式中，K_g，K_oi表示调节参数；m，n，和i表示是取值为0或1的参数，取值情况根据规则要求而定；d_oi(k)表示t采样时刻USV与障碍船在地球固定北东坐标系下的欧几里得的距离；d_{safe_overtaking}表示USV在追越情形下需要进行避碰操纵的安全距离；

在横越情形下，用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

式中，K_g，K_oi表示调节参数；m，n，和i表示是取值为0或1的参数，取值情况根据规则要求而定；d_oi(k)表示t采样时刻USV与障碍船在地球固定北东坐标系下的欧几里得的距离；d_{safe_crossing}表示USV在横越情形下需要进行避碰操纵的安全距离；

S5、获取无人船在采样时刻k的运动状态η_k＝[x_k,y_k,ψ_k]^T和控制v_k＝[u_k,r_k]^T并作为算法的优化变量，通过模型预测控制法求解代价函数得到下一时刻无人船无碰撞风险的运动状态η_k+1和控制v_k+1；然后将求解得到的下一时刻无人船无碰撞风险的状态作为当前时刻无人船的状态。

S6、在下一采样时刻，重复步骤S5，直至避碰完成，从而得到一条无碰撞风险的路径，实现无人船对动、静态障碍物的自主避碰。

本实例用MATLABr2020软件进行仿真实验，仿真中假设水流为理想水流，采样间隔时间设置T为0.1s，步长N设置为20。

图6给出了USV在对驶相遇情况下的仿真暂态图，USV从左向右行驶，障碍船从右向左迎面向USV驶来；图7给出了USV在追越情形下的仿真暂态图，其中USV从左向右行驶并成功完成对障碍船的追越；图8给出了USV在横越过程中的仿真暂态图，USV从左向右行驶并对障碍船进行横越；在左边图中可知，USV从左向右行驶成功避开了障碍船，随后，从右边图中可以看出在避开障碍船后USV又完成了对静态障碍物的避让。

仿真结果表明，本发明所提出的避碰方法能够有效地解决USV在复杂的内河航道条件下航行时的避碰问题，所进行的避碰操纵满足相关规则、航道条件的要求及USV自身运动学的约束。所提算法在原理和计算上都很简单，便于实现，具有高度的通用性。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (8)

1.一种基于模型预测控制的内河无人船避碰方法，其特征是，具体操作步骤如下：

(1)、对无人船及动态障碍物的安全区域进行建模；

(2)、将无人船避碰的最优控制问题转化为非线性规划问题，将无人船速度、速度增量、最大转向幅度以及航道边界约束转化为非线性规划问题的不等式约束；

(3)、构建用于对无人船避碰操纵合理性进行评估的控制目标、速度与速度增量的代价函数；

(4)、对无人船的会遇态势进行划分，构建用于评估无人船避碰操纵规则符合度的代价函数；

(5)、获取当前时刻无人船的状态以及障碍物的状态，通过模型预测控制法对所构造的代价函数进行求解，以得到下一时刻无人船无碰撞风险的状态，然后将求解得到的下一时刻无人船无碰撞风险的状态作为当前时刻无人船的状态；

(6)、在下一采样时刻重复步骤(5)，直至避碰完成，从而得到一条无碰撞风险的路径，最终实现无人船对动、静态障碍物的自主避碰。

2.根据权利要求1所述的一种基于模型预测控制的内河无人船避碰方法，其特征是，在步骤(1)中，所述对无人船及动态障碍物的安全区域进行建模具体是：

通过采用大小随无人船或障碍物的速度而改变的改进Bumper模型对无人船及动态障碍物的安全区域进行建模。

3.根据权利要求1所述的一种基于模型预测控制的内河无人船避碰方法，其特征是，

在步骤(2)中，所述将无人船避碰的最优控制问题转化为非线性规划问题聚具体方法是：考虑水流因素的影响，采用4阶龙格库塔法来对无人船的运动学方程进行离散化，使用多重射击法将无人船避碰的OCP问题转化为NLP问题。

4.根据权利要求1所述的一种基于模型预测控制的内河无人船避碰方法，其特征是，

在步骤(3)中，所述用于对无人船避碰操纵合理性进行评估的控制目标、速度与速度增量的代价函数的具体形式为：

其中，K_p，K_U，K_r， 表示调节参数，p_k表示在t采样时刻USV的位置，/>表示在t采样时刻USV的参考位置，u_dk表示在t采样时刻USV的参考航行速度，r_dk表示在t采样时刻USV的参考艏摇角速度，u_k表示在t采样时刻USV的航行速度；r_k表示在t采样时刻USV的艏摇角速度。

5.根据权利要求1所述的一种基于模型预测控制的内河无人船避碰方法，其特征是，

在步骤(4)中，所述用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

φ_COLREGS＝φ_heading∨φ_overtaking∨φ_crossing

式中，φ_heading为在对驶相遇遭遇情形下，用于评估无人船避碰操纵规则符合度的代价函数；φ_overtaking为在追越情形下，用于评估无人船避碰操纵规则符合度的代价函数；φ_crossing为在横越情形下，用于评估无人船避碰操纵规则符合度的代价函数。

6.根据权利要求5所述的一种基于模型预测控制的内河无人船避碰方法，其特征是，

在对驶相遇遭遇情形下，用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

式中，K_g，K_oi表示调节参数；m，n，和i表示是取值为0或1的参数，取值情况根据规则要求而定；d_{oi_z}(k)表示t采样时刻USV与障碍船在地球固定北东坐标系下船舶横荡方向上的距离；d_{safe_heading}表示USV在对驶相遇情形下需要进行避碰操纵的安全距离。

7.根据权利要求5所述的一种基于模型预测控制的内河无人船避碰方法，其特征是，

在追越情形下，用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

式中，K_g，K_oi表示调节参数；m，n，和i表示是取值为0或1的参数，取值情况根据规则要求而定；d_oi(k)表示t采样时刻USV与障碍船在地球固定北东坐标系下的欧几里得的距离；d_{safe_overtaking}表示USV在追越情形下需要进行避碰操纵的安全距离。

8.根据权利要求5所述的一种基于模型预测控制的内河无人船避碰方法，其特征是，

在横越情形下，用于评估无人船避碰操纵规则符合度的代价函数的具体形式为：

式中，K_g，K_oi表示调节参数；m，n，和i表示是取值为0或1的参数，取值情况根据规则要求而定；d_oi(k)表示t采样时刻USV与障碍船在地球固定北东坐标系下的欧几里得的距离；d_{safe_crossing}表示USV在横越情形下需要进行避碰操纵的安全距离。