CN109213164A

CN109213164A - 基于水面无人艇船体模型的模型预测控制的高速紧急避障方法

Info

Publication number: CN109213164A
Application number: CN201811042089.2A
Authority: CN
Inventors: 陈晓虎; 刘梅; 解杨敏; 彭艳; 罗均
Original assignee: University of Shanghai for Science and Technology
Current assignee: University of Shanghai for Science and Technology
Priority date: 2018-09-07
Filing date: 2018-09-07
Publication date: 2019-01-15

Abstract

本发明公开了一种基于水面无人艇船体模型的模型预测控制的高速紧急避障策略。该策略分为到达目标点的路径规划导航策略和紧急避障策略。没有障碍物或没到达避障范围且有碰撞风险或结束避障，启用导航策略。到达避障范围且有碰撞风险，启用避障策略。但是传统的局部避障方法很少考虑到船体动力学的影响，且只适合低速不紧急的场景。该策略考虑了船体动力学对无人艇导航避障的影响，使对水面无人艇的控制更接近真实物理场景，有效地解决了导航避障输出的状态是控制器无法达到的情况；使用了模型预测控制，用模型可以精确地预测未来行为，在避障过程中对无人艇和障碍物的信息有一个前瞻性的预判，使避障的效果更优适合高速且紧急的场景。

Description

基于水面无人艇船体模型的模型预测控制的高速紧急避障方法

技术领域

本发明涉及水面无人艇高速紧急避障方法，具体是一种基于水面无人艇船体模型的模型预测控制的高速紧急避障方法。

背景技术

无人水面艇(unmanned surface vehicle,USV)是一种轻型智能水面运载工具。如果要保证无人水面艇能够在海洋中安全地航行,那么无人水面艇必须能够对航行过程中遇到的岛屿、暗礁、灯塔、浮标和航行的船只等其他障碍物进行自主避障。当障碍物为静态时称为静态避障；当障碍物为动态时称为动态避障,动态避障算法也可用于静态避障.动态避障算法比较有难度,因为通过移动机器人自身携带的传感器很难精确地获得动态障碍物的运动信息,无法对障碍物的运动趋势进行准确的预测。而无人艇具有吃水浅、自主航行、自主避障、任务载荷配置灵活的特点，是解决近海岛礁海域海图序列不完整问题、复杂海域环境监测问题的有效科技手段，同时也是提升我国海洋军事装备能力，维护我国海洋权益、处理南海等热点海域局势的重要海洋装备。

随着世界各国对水面无人艇的应用研究越来越广泛，对水面无人艇智能化水平的要求也在不断提高，要求水面无人艇在运动过程中具有与周围环境交互的能力，即要求水面无人艇具有从周围环境中收集有用信息，构造符号化的环境模型，并利用模型来规划并执行上层任务的能力，其中路径规划问题就是核心问题之一。水面无人艇的路径规划是指在静动态障碍物并存的海洋环境中，寻找一条从给定起点到终点的满足一定评价标准的运动路径，使水面无人艇在航行过程中能安全可靠地避开所有障碍物。路径规划的分类方法很多，按层次关系可分为全局路径规划和局部路径规划；按照对环境的了解程度，分为环境已知，环境部分已知，环境未知三种规划；同时还有基于功能、行为的规划等等。全局路径规划和局部路径规划是建立在机器人对其环境了解程度的基础上。水面无人艇的全局规划着眼于规划出一条从起始点到目标点的无碰撞路径，综合考虑上层模块的任务要求和水面无人艇本身的运动性能，确立某种最优性能函数，通过一定的搜索方式，就能在水面无人艇的运动空间中找到相应的优化路径。水面无人艇的局部规划立足于实时性，根据传感器信息确定水面无人艇在地图中的当前位置以及周围局部范围内障碍物的分布情况，规划下一时刻的运动，从而避开动态障碍物。

但是传统的局部避障方法很少考虑到船体动力学的影响，且只适合低速(20节以下)不紧急的场景。如果应用在高速(40节左右)容易出现船体动力学达不到的避障输出，且会产生左右摇摆不稳定等现象。

发明内容

本发明的目的在于针对已有技术的不足，提供一种基于水面无人艇船体模型的模型预测控制的高速紧急避障方法。

1.基于水面无人艇船体模型的模型预测控制的高速紧急避障方法,其特征在于操作步骤如下：

1)启动无人艇，设置航行模式为自主巡航；

2)当传感器检测到障碍物时，判断是否有碰撞风险，如有碰撞风险，启动避障策略；如没有检测到障碍物时，启动导航策略；

3)启动避障策略后，判断是否能结束避障，如能结束避障，启动导航策略，否则，继续执行避障策略；

4)巡航过程中，如无人艇到达目标点，则停止航行。

2.步骤1)中，以固定的速度大小，朝着目标点方向运动；

3.步骤2)中，当无人艇与障碍物的实时距离在100m之内且无人艇和障碍物实时位置连线与它们合成速度方向的夹角小于90度时，启动避障策略，否则，保持固定的速度大小，朝着目标点方向运动；

4.步骤2)中，避障策略是基于避障危险度与能耗率构造代价函数，代价函数的自变量是控制力Fx,Fy和力矩Mz。令代价函数取得最小值的Fx,Fy和Mz即为输出的变量，该输出变量直接作用于无人艇本体。该避障策略考虑了船体动力学对无人艇导航避障的影响，使对水面无人艇的控制更接近真实物理场景，有效地解决了导航避障的输出是控制器无法达到的情况，且使用了模型预测控制(Model Predictive Control，简称MPC)，用模型可以精确地预测未来行为，在避障过程中对无人艇和障碍物的信息有一个前瞻性的预判，使避障的效果更优。

5.步骤3)中，若无人艇与障碍物的实时距离在100m之外或无人艇和障碍物实时位置连线与它们合成速度方向的夹角大于90度，则按步骤1)航行；否则，按步骤2)航行。

与现有方法相比，该方法有以下几点显著性优点：

1)考虑了船体动力学对无人艇导航避障的影响，使对水面无人艇的控制更接近真实物理场景，有效地解决了导航避障输出的状态是控制器无法达到的情况；

2)使用模型预测控制(Model Predictive Control，简称MPC)，用模型可以精确地预测未来行为，在避障过程中对无人艇和障碍物的信息有一个前瞻性的预判，使避障的效果更优；

3)构造了基于避障方法和能耗效率的代价函数，通过最值优化可以输出无人艇可行的路径点和速度大小和方向；

4)可以前瞻性预判且能迅速避障，所以不仅适合低速场景，也适合高速且紧急的场景。

为达到上述目的，本发明采用下述技术方案：

(1)船体动力学模型

如说明书附图8所示，无人艇的驱动力都来自于后方两个螺旋浆，但要控制平面运动三个自由度，因此是一个典型的欠驱动***。

刚体动力学方程：

不考虑风力影响和浪影响的运动方程：

***惯性矩阵：M＝M_RB+M_A

科里奥利矩阵：C(v)＝C_RB(v)+C_A(v)

阻尼矩阵：D(v)＝D_L+D_NL(v)

船体控制量：

设左侧螺旋桨的推力为f₁(t),右侧螺旋桨的推力为f₂(t)，两螺旋桨之间的距离为B，则

F_x＝f₁(t)+f₂(t)

F_y＝0

(2)把船体动力学模型转化为状态方程的形式，构造船体状态量与船体控制量(F_x,F_y,M_z)^T的关系

A₁＝[(M·R^-1)·(C(v)+D(v)·R^-1)]^-1

B₁＝(M·R^-1)^-1·L

D₁＝0

D＝0

连续的状态方程为:

y(t)＝C·x(t)+D

(3)对状态方程进行离散化处理，用差分变化法或MATLAB中的c2d命令，得到：

X_k+1＝A_d·X_k+B_d·u_k

(4)基于避障和能耗效率构造代价函数

J＝λ₁·J_vo+λ₂·J_e

其中，避障代价函数为：为无人艇与障碍物在nstep歩位置连线与正北方向的夹角(锐角)，为无人艇与障碍物在nstep歩合成速度方向与正北方向的夹角(锐角)。

能耗代价函数为：u_k为当前时刻的船体控制量。

权重系数为：λ₁＝10⁸,λ₂＝1

预测步长为：nstep＝20

附图说明

图1是基于水面无人艇船体模型的模型预测控制的高速紧急避障方法的流程框图。

图2是单个障碍物对遇场景的仿真结果。

图3是单个障碍物左交叉场景的仿真结果。

图4是多个障碍物右交叉场景的仿真结果。

图5,6,7是多个障碍物场景的仿真结果。

图8是船体模型简图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明优选实施方式作进一步的详细描述。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

本发明所提出一种基于水面无人艇船体模型的模型预测控制的高速紧急避障方法，其流程图如图1所示，到达目标点的路径规划导航方法和紧急避障方法。如没有障碍物或没到达避障范围且有碰撞风险或结束避障，启用导航方法。如到达避障范围且有碰撞风险，启用避障方法。单个障碍物场景的导航避障效果图如图2,3,4所示，多个障碍物场景的导航避障效果图如图5,6,7所示。

本发明提出的基于水面无人艇船体模型的模型预测控制的高速紧急避障方法，具体实施操作流程包括以下步骤：

1)如图1所示，在没有障碍物或没到达避障范围且有碰撞风险或结束避障时，启用导航方法。在main函数中，用usv表示出无人艇的初始状态(位置、速度、方位角信息)；用obstacle表示出障碍物的初始状态信息(个数，位置，速度，大小)；用NaviInfo表示出导航信息(无人艇的速度大小和方向)，为了方便实现，可假设无人艇导航速度大小为常数，方向始终朝着目标点。

2)如图1所示，首先判断是否进入避障范围，是否有碰撞风险，且是否阻挡回归航迹线，若满足条件，则启动避障方法。基于代价函数J计算出无人艇最优路径和速度时对应的船体控制量τ。如果是在实艇上操作，则船体控制量τ直接控制底层控制器，从而直接控制左右螺旋浆的旋转速度与推力。如果是在仿真中，可以把船体控制量τ输入到船体动力学模型中，从而得出无人艇下一时刻的状态量(位置、速度、方位角信息)。

3)如图2所示，是单个对遇场景无人艇的避障效果图。无人艇从下往上运动，障碍物从上往下运动。避障范围设置为100m，当无人艇和障碍物的范围到达100m内且有碰撞风险(无人艇和障碍物位置连线与合成速度方向的夹角锐角小于90°)时，启动避障方法。

4)如图3所示，是单个障碍物左交叉场景的避障效果图。无人艇从下往上运动，障碍物从无人艇的左侧驶来，当无人艇和障碍物的范围到达100m内且有碰撞风险(无人艇和障碍物位置连线与合成速度方向的夹角锐角小于90°)时，启动避障方法。

5)如图4所示，是单个障碍物右交叉场景的避障效果图。无人艇从下往上运动，障碍物从无人艇的右侧驶来，当无人艇和障碍物的范围到达100m内且有碰撞风险(无人艇和障碍物位置连线与合成速度方向的夹角锐角小于90°)时，启动避障方法。

6)如图5,6,7所示，是多个障碍物对遇、左交叉、右交叉场景的避障效果图。图中有3个障碍物，分别从上往下运动、从无人艇左侧驶来、从无人艇右侧驶来。当无人艇和某个障碍物的范围到达100m内且有碰撞风险(无人艇和障碍物位置连线与合成速度方向的夹角锐角小于90°)时，启动避障方法。当无人艇没检测到障碍物或没到达避障范围且有碰撞风险或结束避障，启用导航方法。

上述具体实施方式用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落在本发明的保护范围。

Claims

1)启动无人艇，设置航行模式为自主巡航；

4)巡航过程中，如无人艇到达目标点，则停止航行。

2.权利要求1步骤1)中，以固定的速度大小，朝着目标点方向运动。

3.权利要求1步骤2)中，当无人艇与障碍物的实时距离在100m之内且无人艇和障碍物实时位置连线与它们合成速度方向的夹角小于90度时，启动避障策略，否则，保持固定的速度大小，朝着目标点方向运动。

4.权利要求1步骤2)中，避障策略是基于避障危险度与能耗率构造代价函数，代价函数的自变量是控制力Fx,Fy和力矩Mz；令代价函数取得最小值的Fx,Fy和Mz即为输出的变量，该输出变量直接作用于无人艇本体；该避障策略考虑了船体动力学对无人艇导航避障的影响，使对水面无人艇的控制更接近真实物理场景，有效地解决了导航避障的输出是控制器无法达到的情况，且使用了模型预测控制(Model Predictive Control，简称MPC)，用模型可以精确地预测未来行为，在避障过程中对无人艇和障碍物的信息有一个前瞻性的预判，使避障的效果更优。

5.权利要求1步骤3)中，若无人艇与障碍物的实时距离在100m之外或无人艇和障碍物实时位置连线与它们合成速度方向的夹角大于90度，则按步骤1)航行；否则，按步骤2)航行。