CN110262492B - 一种无人艇的实时避碰和目标跟踪方法 - Google Patents

Abstract

一种无人艇实时避碰和目标跟踪方法，针对无人艇的运动特性和无人艇路径规划过程中需要遵守国际海上规则公约且会受到风浪流干扰等特点以及无人艇目标跟踪过程对速度平滑性的要求，构建了海事规则和转向避碰模型、自适应跟踪模型、无人艇运动学模型以及无人艇能耗模型。之后，为无人艇设定了航行和目标跟踪两种工作模式。针对两种工作模式设定相应中终点条件，并在算法中加入多种约束条件，再利用最小二乘和G2O图优化算法，计算得到最优路径。本发明可以应用于无人艇的路径规划领域，可以实时得到一条考虑了航行时间、碰撞风险、国际海上规则公约、能耗、无人艇的运动学限制、速度/加速度限制等约束条件的最优平滑轨迹。

Description

一种无人艇的实时避碰和目标跟踪方法

技术领域

本发明涉及无人艇路径规划领域，具体是一种无人艇的实时避碰和目标跟踪方法

背景技术

随着无人驾驶***和人工智能技术的快速发展，继无人机和无人车后，无人艇(Unmanned Surface Vehicle,USV)在军用和民用领域中越来越受欢迎，在海上搜救、海事取证、环境监测和敌方侦察等各种海洋应用中发挥着重要作用。

路径规划技术是水面无人艇领域中的核心技术之一，是从有人走向无人的关键一步，是无人艇智能化的重点发展方向，在一定程度上标志着水面无人艇自主航行能力的高低。因此研究无人艇的路径规划技术，将有助于无人艇的进一步发展。水面无人艇的路径规划是指在有障碍物的工作环境中，如何寻找一条从给定起点到终点适当的运动路径，使水面无人艇在运动过程中能安全、无碰地绕过所有障碍物。而实时避碰和目标跟踪更是其中的难点，是需要重点研究的内容。实时避碰和目标跟踪是以全局路径或者被跟踪目标信息为指导，通过传感器信息确定无人艇的实时位置，及得到局部范围内障碍物的分布情况；寻找一种满足一定评价标准和约束的运动方案，可以调整航向和航速，高度智能化、自适应避开海上各类障碍物。

目前大多数的路径规划方法，如势场法、栅格法、A*算法、遗传算法等，都是通过规划坐标点的方式，来得到最优路径，在规划过程中没有考虑时间序列信息以及无人艇的运动约束，这样会导致规划出来的路径过于理想化，存在路径不可达情况，即控制器无法跟随此路径。

而TEB算法可以实时规划出无人艇的轨迹，通过综合考虑无人艇的运动模型和运动约束，并对无人艇执行任务时的速度与加速度进行规划，以满足光滑性和速度可控性等要求。

发明内容

本发明所要解决的技术问题是：克服现有技术的不足，本发明提供了一种无人艇的实时避碰和目标跟踪方法，针对无人艇航行和目标跟踪场景，能够实时规划出一条包含路径坐标点序列、时间序列以及到达任一路径点的速度、角速度、方向等信息的轨迹，确保路径能够被控制器执行，并且规划出来的路径符合国际海上避碰规则公约，而在目标跟踪过程中，无人艇能够以平滑速度进行跟踪，从而降低控制器的控制难度。

本发明所采用的技术方案是：一种无人艇的实时避碰和目标跟踪方法，包括如下步骤：

步骤S10，根据无人艇航行轨迹的起点和终点位置，根据时间间隔和状态序列个数，初始化带有时间序列的状态B:＝(Q,τ)，

其中：状态集合Q＝{q_i}_i＝1...n,n∈N，状态集合τ＝{ΔT_j}_j＝1...n-1,n∈N，q_i＝(p_i,β_i)^T＝(x_i,y_i,β_i)^T，p_i＝(x_i,y_i)为无人艇当前在地图中的位置坐标，β_i为无人艇当前转向角度，并定义东向为0°，逆时针为正，ΔT_j为时间分辨率；

步骤S20，根据COLREGS，构建海事规则模型和无人艇的转向避碰模型；

所述海事规则模型为：根据COLREGS、船舶避碰的经验数据以及目标船只在海洋环境中的航行范围，为无人艇定义了追赶OT、相遇HO、右交叉CFR、左交叉CFL四种冲突局面；

所述无人艇的转向避碰模型为：

根据海事规则模型得到冲突局面的航行规则表和关系函数

其中，P为目标船只在海洋环境中的航行范围，Δθ为无人艇与目标船只航向角角度差，并定义北向为0°，顺时针为正，

φ₁,φ₂分别为设定的角度上限值和角度下限值，USV_turn为无人艇的转角方向；

当无人艇处于左交叉CFL局面或处于追赶OT局面且Δθ∈[0,φ₁)时，无人艇向左转；当无人艇处于右交叉CFR局面或处于相遇HO局面或处于追赶OT局面且Δθ∈[φ₂,2π)，无人艇向右转；

步骤S30，构建自适应跟踪模型；

所述自适应跟踪模型为终点跟踪速度

其中，

为无人艇最大速度，L＝d-d_h，d为无人艇与目标船只的距离，d_h为保持距离，v_o为目标船只当前速度，

为无人艇最大加速度，α为调节因子；

步骤S40，构建无人艇运动学模型；

所述无人艇运动学模型为：

其中，(x_i+1,y_i+1)为无人艇下一时刻在地图中的位置，v_i为无人艇的当前速度，ω_i为无人艇的当前角速度，θ_i为无人艇的当前航向角，t为时间；

步骤S50，构建无人艇能耗模型；

所述无人艇能耗模型为：

无人艇能耗

其中，

vⁱ _ground为底速度，vⁱ _current为风浪流速度，vⁱ _usv为无人艇的速度；η为无人艇的风浪流力因子；

间隔距离

步骤S60，确定无人艇航行工作模式；

无人艇航行工作模式包括目标跟踪模式和航行模式；

若无人艇处于目标跟踪模式，根据自适应跟踪模型，自动设定当前终点与到达终点的速度；若无人艇处于航行模式，则手动设置终点坐标，并自动将到达终点的速度设置为0；

步骤S70，根据海事规则模型和转向避碰模型、自适应跟踪模型、无人艇运动学模型、无人艇能耗模型构建吸引点约束、静态障碍物约束、动态障碍物约束、速度与加速度约束、无人艇运动学约束、最小时间约束、最小能耗约束，并建立目标函数：

f(B)＝Υ₁f_a+Υ₂f_s+Υ₃f_d+Υ₄f_v+Υ₅f_ω+Υ₆f_acc+Υ₇f_k+Υ₈f_r+Υ₉f_t+Υ₁₀f_e；

其中Υ为权重因子。

所述吸引点约束为：f_a＝||p_b-p_a||；

其中，p_a为吸引点，p_b为状态序列中离p_a最近的坐标点；

所述静态障碍物约束为：

其中，d_s为无人艇与障碍物的距离，d_safe为安全距离；

所述动态障碍物约束为：

非合作动态障碍物约束：

其中，无人艇与非合作目标的距离

为t时刻状态序列中的第i个坐标点，

为t时刻时预测的动态障碍物所在位置；

合作动态障碍物约束：

其中f_p为转角偏向约束，

β_j为无人艇开始转向避碰时刻的转向角度，β_j+1为无人艇转向避碰下一时刻的转向角度。

所述速度与加速度约束分别为：

速度约束

v_min、v_max分别为无人艇的最小速度和最大速度；

角速度约束

ω_min、ω_max分别为无人艇的最小速度和最大速度；

加速度约束

a_i为无人艇当前加速度，a_min、a_max分别为无人艇的最小加速度和最大加速度；

角加速度约束

b_i为无人艇当前角加速度，b_min、b_max分别为无人艇的最小角加速度和最大角加速度；

其中，

的大小与方向由传感器测量得出，方向为δ_i；

方向为β_i＝arctan(p_i+1-p_i)；

的大小与方向分别为：

无人艇的角速度，加速度以及角加速度式分别为：

所述运动学约束为：

最小拐弯半径约束为：

其中，无人艇当前的转弯半径

R_m为无人艇最小转弯半径；

最小时间约束为：

最小能耗约束为：

步骤S80，针对目标函数f(B)，根据步骤S70中构建的约束条件，采用最小二乘法和G2O图优化算法，求得获得最优状态序列

即为最优路径。

本发明与现有技术相比的优点在于：

(1)本发明建立了海事规则模型和转向避碰模型，考虑了国际海上规则公约对动态避碰的影响，从而实现了对于合作目标的实时动态避碰。

(2)本发明建立无人艇能耗模型，考虑了能耗对路径的影响以及风浪流干扰对无人艇速度、角速度的影响，从而能够节省无人艇航行过程中的能耗，增加无人艇的工作时长。

(3)本发明建立了自适应跟踪模型，实现了平滑速度跟踪，解决了跟踪过程中速度骤变的问题，降低了控制器的控制难度也进一步确保了安全。

(4)本发明结合了TEB算法，解决了传统算法没有考虑时间序列信息以及无人艇的运动约束，导致规划出来的路径过于理想化的问题。本发明提出的方法考虑了航行时间、碰撞风险、国际海上规则公约、能耗、无人艇的运动学限制、速度/加速度限制的多种约束条件，能够实时规划出一条带有时间、速度、角速度、位置信息的轨迹，得到了相较于传统方法更优的路径。

附图说明

图1为本发明的实施流程示意图；

图2为本发明中合作目标船只的的位置示意图；

图3为本发明中风浪流速度对无人艇影响示意图；

图4为无人艇运动学约束示意图；

图5为本发明中加入多种约束条件后构建的超图。

图6为本发明中静态避碰示意图。

图7为本发明中动态避碰示意图。

图8为本发明中航行模式的速度变化曲线。

图9为本发明中目标跟踪模式中的速度变化曲线。

具体实施方式

以下将结合附图对本发明的技术方案做进一步详细说明。

本发明的实施例提供一种无人艇的实时避碰和目标跟踪方法，其实施流程如图1所示，包括如下步骤：

准备工作：无人艇的起点以及障碍物的位置、速度、属性信息已知。

步骤S10，根据无人艇航行轨迹的起点和终点位置，根据时间间隔和状态序列个数，初始化带有时间序列的状态B:＝(Q,τ)，

其中：状态集合Q＝{q_i}_i＝1...n,n∈N，状态集合τ＝{ΔT_j}_j＝1...n-1,n∈N，q_i＝(p_i,β_i)^T＝(x_i,y_i,β_i)^T，p_i＝(x_i,y_i)为无人艇当前在地图中的位置坐标，β_i为无人艇当前转向角度，ΔT_j为时间分辨率；

步骤S20，建立海事规则和转向避碰模型。

根据国际海上避碰规则公约(COLREGS)、船舶避碰的经验数据以及实际船只在复杂海洋环境中航行范围，为无人艇为无人艇定义了追赶OT、相遇HO、右交叉CFR、左交叉CFL四种冲突局面；

并可得到冲突局面的航行规则表(见表1)和关系函数：

式中：P为目标船只所在范围，如图2所示，Δθ为无人艇与目标船只航向角角度差，并将图中无人艇的航向定义为0°，

根据COLREGS，建立无人艇在遇到目标船只的避碰转向模型。

φ₁,φ₂分别为设定的角度上限值和角度下限值；

在本实施例中，

其中USV_turn为无人艇的转角方向；

当无人艇处于左交叉CFL局面或处于追赶OT局面且

时，无人艇应向左避让障碍物；当无人艇处于右交叉CFR局面或处于相遇HO局面或处于追赶OT局面且

无人艇应向右避让障碍物；

表1航行规则表

表中空白区域与未列出数据，表明此种情况时，目标船只不会对无人艇产生威胁或目标船只应当主动避让无人艇。表中的“避让”情况是因为目标船此时在无人艇右舷侧，根据COLREGS，USV需要主动避让，且由于目标船只在无人艇的后侧，无人艇只需加速即可避开。

步骤S30，建立自适应跟踪模型。

为了能够让无人艇以平滑速度跟踪目标船只，且能尽快跟踪上目标，需要对无人艇的终点跟踪速度加以约束，以离目标船只距离d_h的位置为本次规划的终点

d_h为保持距离。那么终点跟踪速度

为：

其中

为无人艇最大速度，

为无人艇最大加速度，v_o为目标船只当前速度，L＝d-d_h，d为无人艇与目标船只的距离，α为调节因子。

步骤S40，建立无人艇运动模型：

其中，(x_i+1,y_i+1)为无人艇下一时刻在地图中的位置，v_i为无人艇的当前速度，ω_i为无人艇的当前角速度，θ_i为无人艇的当前航向角，t为时间；

步骤S50，建立无人艇能耗模型。

其中：vⁱ _ground为底速度，vⁱ _current为风浪流速度，vⁱ _usv为无人艇的速度，如图3所示

其中F_current为风浪流对无人艇的力，η为无人艇的风浪流力因子。

可以得到无人艇能耗消耗模型：

其中：间隔距离

步骤S60，确定工作模式，

无人艇航行工作模式包括目标跟踪模式和航行模式；

若无人艇处于目标跟踪模式，根据自适应跟踪模型，自动设定当前终点与到达终点的速度；若无人艇处于航行模式，则手动设置终点坐标，并自动将到达终点的速度设置为0；

步骤S70，根据起点、终点以及当前速度、方向，终点速度、方向，初始化状态序列B。

步骤S80，加入吸引点约束，以确保无人艇在避障后能够返回全局路径点。

f_a＝||p_b-p_a||

其中，p_a为吸引点，p_b为状态序列中离p_a最近的坐标点；

步骤S90，加入静态障碍物约束，使无人艇能够对静态障碍物进行避障。

其中，d_s为无人艇与障碍物的距离，d_safe为安全距离；

步骤S100，加入动态障碍物约束。

当检测到动态障碍物为非合作目标时，在整个时间序列中加入动态障碍物约束，结合时间序列对动态障碍物的位置进行预测，确保不会与动态障碍物相碰。

其中，无人艇与非合作目标的距离

为t时刻状态序列中的第i个坐标点，

为t时刻时预测的动态障碍物所在位置；

当检测到动态障碍物为合作目标时，即为同样遵循COLREGS的目标船只，加入转角偏向约束f_p，确保在安全避碰的同时以符合COLREGS的情况下进行转向。

其中，β_j为无人艇开始转向避碰时刻的转向角度，β_j+1为无人艇转向避碰下一时刻的转向角度。

得到合作目标的动态障碍物约束如下，此时无人艇将在遵循COLREGS的情况下进行动态避碰。

步骤S110，加入速度与加速度约束，确保规划出来的路径具备速度可控性。

速度约束

其中，v_min、v_max分别为无人艇的最小速度和最大速度；

角速度约束

其中，ω_min、ω_max分别为无人艇的最小速度和最大速度；

加速度约束

其中，a_i为无人艇当前加速度，a_min、a_max分别为无人艇的最小加速度和最大加速度；

角加速度约束

其中，b_i为无人艇当前角加速度，b_min、b_max分别为无人艇的最小角加速度和最大角加速度；

其中，

的大小与方向由传感器测量得出，方向为δ_i；

方向为β_i＝arctan(p_i+1-p_i)；

可以计算出

的大小与方向分别为：

由此可以得到无人艇的角速度，加速度以及角加速度式分别为：

步骤S120，加入运动学约束，确保规划出来的路径平滑且能够被控制器执行。

假设连续的两个状态点的转角相同，如图4所示，则

可以得到如下运动学约束：

此外，实际航行中，当无人艇以稳定速度转弯时，为了防止无人艇翻船，存在一个最小转弯半径R_m。

无人艇当前的转弯半径可通过如下方式计算得到：

得到最小拐弯半径约束：

步骤S130，加入最小时间约束。

不论时无人艇航行过程中，还是目标跟踪过程中，都希望规划出来的路径最短，在符合无人艇运动学的情况下，路径最短也就就等价于以最小时间航行完此路径，因此加入最小时间约束：

步骤S140，根据无人艇能耗模型，加入最小能耗约束。

步骤S150，此时将路径规划问题转化为如下的多目标优化问题：

f(B)＝Υ₁f_a+Υ₂f_s+Υ₃f_d+Υ₄f_v+Υ₅f_ω+Υ₆f_acc+Υ₇f_k+Υ₈f_r+Υ₉f_t+Υ₁₀f_e；

其中，Υ₁～Υ₁₀均为权重因子。

步骤S160，采用G2O图优化方法来求最优解，根据多目标优化函数建立超图，如图5所示。

步骤S170，采用最小二乘法来进行求解，通过采取LM(Levenberg-Marquardt)方法和稀疏优化器进行迭代计算，不断更新状态序列B。

迭代一定次数后，求得最优解：

最优解由一系列的最优状态组成，包含路径点坐标、方向和时间序列，并且还可以计算出每一路径点时无人艇的速度、角速度等信息。

根据控制器需求，将坐标点或者无人艇速度、角速度信息发送给控制器。

图6显示了静态避碰的路径规划图，黑色圆为障碍物，灰色圆代表无人艇起点。

图7显示了针对合作船只的动态障碍物的示意图，正方形代表动态障碍物，灰色圆代表无人艇起点。从图中可以看出当前处于CFL局面，无人艇向左避让障碍物，符合COLREGS的要求。

图8显示了无人艇在航行模式的速度曲线图，起点速度为0，终点速度为0。

图9显示了无人艇在目标跟踪模式中的速度曲线图，起点速度为0，终点速度为根据自适应跟踪模型中的到的速度。

本发明未详细说明部分属于本领域技术人员公知技术。

Claims (7)

1.一种无人艇的实时避碰和目标跟踪方法，其特征在于，包括如下步骤：

步骤S10，根据无人艇航行轨迹的起点和终点位置，根据时间间隔和状态序列个数，初始化带有时间序列的状态B:＝(Q,τ)，

其中：状态集合Q＝{q_i}_i＝1...n,n∈N，状态集合τ＝{ΔT_j}_j＝1...n-1,n∈N，q_i＝(p_i,β_i)^T＝(x_i,y_i,β_i)^T，p_i＝(x_i,y_i)为无人艇当前在地图中的位置坐标，β_i为无人艇当前转向角度，并定义东向为0°，逆时针为正，ΔT_j为时间分辨率；

步骤S20，根据COLREGS，构建海事规则模型和无人艇的转向避碰模型；

步骤S20中，所述海事规则模型为：根据COLREGS、船舶避碰的经验数据以及目标船只在海洋环境中的航行范围，为无人艇定义了追赶OT、相遇HO、右交叉CFR、左交叉CFL四种冲突局面；

所述无人艇的转向避碰模型为：

根据海事规则模型得到冲突局面的航行规则表和关系函数

其中，P为目标船只在海洋环境中的航行范围，Δθ为无人艇与目标船只航向角角度差，并定义北向为0°，顺时针为正，

φ₁,φ₂分别为设定的角度上限值和角度下限值，USV_turn为无人艇的转角方向；

当无人艇处于左交叉CFL局面或处于追赶OT局面且Δθ∈[0,φ₁)时，无人艇向左转；当无人艇处于右交叉CFR局面或处于相遇HO局面或处于追赶OT局面且Δθ∈[φ₂,2π)，无人艇向右转；

步骤S30，构建自适应跟踪模型；

步骤S30中，所述自适应跟踪模型为终点跟踪速度

其中，

为无人艇最大速度，L＝d-d_h，d为无人艇与目标船只的距离，d_h为保持距离，v_o为目标船只当前速度，

为无人艇最大加速度，α为调节因子；

步骤S40，构建无人艇运动学模型；

步骤S50，构建无人艇能耗模型；

步骤S50中，所述无人艇能耗模型为：

无人艇能耗

其中，

vⁱ _ground为底速度，vⁱ _current为风浪流速度，vⁱ _usv为无人艇的速度；η为无人艇的风浪流力因子；

间隔距离

(x_i+1,y_i+1)为无人艇下一时刻在地图中的位置；

步骤S60，确定无人艇航行工作模式；无人艇航行工作模式包括目标跟踪模式和航行模式；

步骤S70，根据海事规则模型和转向避碰模型、自适应跟踪模型、无人艇运动学模型、无人艇能耗模型构建吸引点约束f_a、静态障碍物约束f_s、动态障碍物约束、速度与加速度约束、无人艇运动学约束f_k、最小时间约束f_t、最小能耗约束f_e，并建立目标函数：

其中，C₁～C₁₂均为权重因子；f_r为最小拐弯半径约束；

所述动态障碍物约束包括非合作动态障碍物约束

合作动态障碍物约束

速度与加速度约束包括速度约束f_v，角速度约束f_ω，加速度约束f_vacc，角加速度约束f_ωacc；

步骤S80，针对目标函数f(B)，根据步骤S70中构建的约束条件，采用最小二乘法和G2O图优化算法，求得获得最优状态序列

即为最优路径。

2.根据权利要求1所述的一种无人艇的实时避碰和目标跟踪方法，其特征在于：步骤S40中，所述无人艇运动学模型为：

其中，(x_i+1,y_i+1)为无人艇下一时刻在地图中的位置，v_i为无人艇的当前速度，ω_i为无人艇的当前角速度，θ_i为无人艇的当前航向角，t为时间。

3.根据权利要求1或2所述的一种无人艇的实时避碰和目标跟踪方法，其特征在于：步骤S60中，若无人艇处于目标跟踪模式，根据自适应跟踪模型，自动设定当前终点与到达终点的速度；若无人艇处于航行模式，则手动设置终点坐标，并自动将到达终点的速度设置为0。

4.根据权利要求3所述的一种无人艇的实时避碰和目标跟踪方法，其特征在于：步骤S70中，所述吸引点约束为：f_a＝||p_b-p_a||；

其中，p_a为吸引点，p_b为状态序列中离p_a最近的坐标点；

所述静态障碍物约束为：

其中，d_s为无人艇与障碍物的距离，d_safe为安全距离。

5.根据权利要求4所述的一种无人艇的实时避碰和目标跟踪方法，其特征在于：步骤S70中，所述动态障碍物约束包括非合作动态障碍物约束、合作动态障碍物约束：

非合作动态障碍物约束：

其中，无人艇与非合作目标的距离

为t时刻状态序列中的第i个坐标点，

为t时刻时预测的动态障碍物所在位置；

合作动态障碍物约束：

其中f_p为转角偏向约束，

β_j为无人艇开始转向避碰时刻的转向角度，β_j+1为无人艇转向避碰下一时刻的转向角度。

6.根据权利要求5所述的一种无人艇的实时避碰和目标跟踪方法，其特征在于：步骤S70中，所述速度与加速度约束包括：

速度约束

v_min、v_max分别为无人艇的最小速度和最大速度；

角速度约束

ω_min、ω_max分别为无人艇的最小速度和最大速度；

加速度约束

a_i为无人艇当前加速度，a_min、a_max分别为无人艇的最小加速度和最大加速度；

角加速度约束

b_i为无人艇当前角加速度，b_min、b_max分别为无人艇的最小角加速度和最大角加速度；

其中，

的大小与方向由传感器测量得出，方向为δ_i；

方向为β_i＝arctan(p_i+1-p_i)；

的大小与方向分别为：

无人艇的角速度，加速度以及角加速度式分别为：

角速度

加速度

角加速度

7.根据权利要求6所述的一种无人艇的实时避碰和目标跟踪方法，其特征在于：步骤S70中，所述运动学约束为：

最小拐弯半径约束为：

其中，无人艇当前的转弯半径

R_m为无人艇最小转弯半径；

最小时间约束为：

最小能耗约束为：

Images