CN112327872A - 面向溢油围捕的双无人艇协同轨迹跟踪方法 - Google Patents

Abstract

一种面向溢油围捕的双无人艇协同轨迹跟踪方法，属于多艇协同轨迹跟踪领域。本发明针对现有双无人艇溢油围捕方法中，轨迹跟踪算法复杂度高造成溢油围捕的工作效率低的问题。包括，在当前时间周期通过两艘无人艇上的位姿传感器分别测量双无人艇的当前位置和当前姿态信息；给合当前位置和当前姿态信息以及当前溢油点位置，采用零空间行为融合方法对双无人艇进行行为融合，得到双无人艇的期望航速及航向；控制双无人艇按照所述期望航速及航向运动，达到新位置；然后进入下一个时间周期，时间周期不断迭代，直到完成协同轨迹跟踪；所述零空间行为融合方法中的行为包括相互避碰、保持队形和趋向目标三种行为。本发明用于溢油围捕中的轨迹跟踪。

Description

面向溢油围捕的双无人艇协同轨迹跟踪方法

技术领域

本发明涉及面向溢油围捕的双无人艇协同轨迹跟踪方法，属于多艇协同轨迹跟踪领域。

背景技术

随着经济发展和科技进步，海上石油运输的需求越来越大。海上石油运输作为航运的重要组成部分，溢油事故的发生却越来越频繁。大量的海洋溢油，不仅造成巨大的经济损失，同时挥发溢油对工作人员的生命安全也造成巨大威胁。因此，溢油处理方法的研究具有重要意义。

无人艇具有成本低廉，灵活性高等特点，可以通过远程遥控以及自主决策执行任务，在应对危险环境等任务时具有独特的优势。因此，利用无人艇进行溢油处理作业有巨大的发挥空间。

现有无人驾驶的溢油自动回收系统及其工作方法中，提到了一种单艇溢油回收方法。它通过无线连接的智能控制终端控制无人艇，再由无人艇拖曳收油船前往溢油区进行溢油自动回收。其属于单无人艇为动力的溢油回收方案，通过收油船进行溢油回收工作，无人艇仅仅作为运送收油船的工具，回收效率不高。现有技术中还提到一种面向溢油围捕的柔性连接式双无人艇自主协同方法，它采用遗传算法规划最优航迹，使用模糊零空间的行为融合方法修正航态，得到双无人艇的期望艏向和航速。该方法设置了四种行为，包括速度动态调整、队形保持、轨迹跟踪以及溢油围捕。该方法在完成溢油围捕任务的过程中，无法实现避障功能；并且由于行为融合算法比较复杂，会使双艇将长时间处于多种行为的判断和队形修正中，导致执行溢油围捕的工作效率低。

发明内容

针对现有双无人艇溢油围捕方法中，轨迹跟踪算法复杂度高造成溢油围捕的工作效率低的问题，本发明提供一种面向溢油围捕的双无人艇协同轨迹跟踪方法。

本发明的一种面向溢油围捕的双无人艇协同轨迹跟踪方法，包括，

在当前时间周期通过两艘无人艇上的位姿传感器分别测量双无人艇的当前位置和当前姿态信息；给合当前位置和当前姿态信息以及当前溢油点位置，采用零空间行为融合方法对双无人艇进行行为融合，得到双无人艇的期望航速及航向；

控制双无人艇按照所述期望航速及航向运动，达到新位置；然后进入下一个时间周期，时间周期不断迭代，直到完成协同轨迹跟踪；

所述零空间行为融合方法中的行为包括相互避碰、保持队形和趋向目标三种行为，三种行为的优先级依次为相互避碰、保持队形和趋向目标；

进行行为融合的结果包括：

在行为为相互避碰时，行为融合结果为相互避碰的期望航速及航向；

在行为为保持队形时，行为融合结果为相互避碰和保持队形的期望航速及航向；

在行为为趋向目标时，行为融合结果为相互避碰、保持队形和趋向目标行为的期望航速及航向。

根据本发明的面向溢油围捕的双无人艇协同轨迹跟踪方法，

在行为为相互避碰时对应的双艇间距小于双艇安全距离；

在行为为保持队形时对应的双艇间距大于或等于双艇安全距离；

在行为为趋向目标时对应的双艇队形合格。

根据本发明的面向溢油围捕的双无人艇协同轨迹跟踪方法，

所述趋向目标的行为控制方程为：

σ_a＝||p_d-p||，

式中σ_a表示轨迹跟踪行为的控制量，为无人艇与双艇最终期望位置的距离；p_d表示双无人艇最终期望位置，p表示双无人艇的当前位置；

其中：

式中p₁为一号无人艇的当前位置向量，(x₁ y₁)为一号无人艇的当前位置坐标；

p₂为二号无人艇的当前位置向量，(x₂ y₂)为二号无人艇的当前位置坐标；

式中p_1d表示一号无人艇的最终期望位置，(x_1d y_1d)为一号无人艇的最终期望位置坐标；

p_2d表示二号无人艇的最终期望位置，(x_2d y_2d)为二号无人艇的最终期望位置坐标；

式中d为预设置长度，x_rd为溢油点的x轴坐标，y_rd为溢油点的y轴坐标，

为双无人艇中心与溢油点连线与x轴的夹角。

根据本发明的面向溢油围捕的双无人艇协同轨迹跟踪方法，

所述相互避碰的行为控制方程为：

σ_b表示双艇间避碰的行为函数。

根据本发明的面向溢油围捕的双无人艇协同轨迹跟踪方法，

所述保持队形的行为控制方程为：

σ_c＝D_x＝(x₂-x₁)cosφ+(y₂-y₁)sinφ，

式中σ_c表示双无人艇保持队形的行为函数；

将大地坐标系保持坐标原点不变，旋转

角度，得到坐标系X'OY'；根据坐标系转换原理，得到双无人艇在坐标系X'OY'下的坐标：

式中p₁'为一号无人艇在坐标系X'OY'下的当前位置向量，(x₁,y₁)'为一号无人艇在坐标系X'OY'下的当前位置坐标；

式中p₂'为二号无人艇在坐标系X'OY'下的当前位置向量，(x₂,y₂)'为二号无人艇在坐标系X'OY'下的当前位置坐标；

得到：

式中i＝1,2，表示无人艇的序号；

φ表示坐标系逆时针旋转的角度：

计算得到双无人艇在坐标系X'OY'下沿X'轴的期望间距D_x为：

D_x＝x₂'-x₁'＝(x₂-x₁)cosφ+(y₂-y₁)sinφ。

本发明的有益效果：本发明方法针对双无人艇协同轨迹跟踪的过程进行分析，将协同轨迹跟踪抽象出双艇的几种行为：相互避碰行为、保持队形行为、趋向目标行为；然后根据双艇协同溢油围捕的机理为三种行为分配优先级，再通过零空间行为融合方法得到双无人艇各自的期望航速及航向。

附图说明

图1是本发明所述面向溢油围捕的双无人艇协同轨迹跟踪方法的流程示意图；

图2是双艇保持队型的状态示意图；图中USV1表示一号无人艇，USV2表示二号无人艇；

图3是由大地坐标系转换为坐标系X'OY'的示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其它实施例，都属于本发明保护的范围。

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面结合附图和具体实施例对本发明作进一步说明，但不作为本发明的限定。

具体实施方式一、结合图1和图2所示，本发明提供了一种面向溢油围捕的双无人艇协同轨迹跟踪方法，包括，

在当前时间周期通过两艘无人艇上的位姿传感器分别测量双无人艇的当前位置和当前姿态信息；给合当前位置和当前姿态信息以及当前溢油点位置，采用零空间行为融合方法对双无人艇进行行为融合，得到双无人艇的期望航速及航向；

控制双无人艇按照所述期望航速及航向运动，达到新位置；然后进入下一个时间周期，时间周期不断迭代，直到完成协同轨迹跟踪；

所述零空间行为融合方法中的行为包括相互避碰、保持队形和趋向目标三种行为，三种行为的优先级依次为相互避碰、保持队形和趋向目标；

进行行为融合的结果包括：

在行为为相互避碰时，行为融合结果为相互避碰的期望航速及航向；

在行为为保持队形时，行为融合结果为相互避碰和保持队形的期望航速及航向；

在行为为趋向目标时，行为融合结果为相互避碰、保持队形和趋向目标行为的期望航速及航向。

本实施方式中，对双无人艇协同轨迹跟踪状态进行分析包括根据已知数据判断双无人艇之间的距离以及队形是否合格；行为融合得到双无人艇的期望航速及航向。

所述零空间行为融合方法基于所设置的双无人艇行为的优先级顺序进行数据融合。结合图1所示，首先，判断双无人艇是否实现相互避碰行为，若未实现相互避碰任务，双艇进行相互避碰、保持队形和趋向目标的行为融合，否则进行保持队形和趋向目标的行为融合。在实现相互避碰基础上，判断双艇是否实现保持队形任务，若未实现保持队形任务，进行保持队形和趋向目标的行为融合，否则进行趋向目标任务，完成溢油围捕任务。

再进一步，结合图1所示，在行为为相互避碰时对应的双艇间距小于双艇安全距离；

在行为为保持队形时对应的双艇间距大于或等于双艇安全距离；

在行为为趋向目标时对应的双艇队形合格。

所述双艇队形合格指双艇之间既满足相互避障的距离要求，也满足双艇保持队形的状态，并且双艇航向一致。

下面对零空间行为融合方法进行详细地说明：

零空间行为融合控制方法，即Null-Spaced-Based(NSB)，就是将所有行为分成不同的优先级，然后将低优先级的行为向高优先级的行为的零空间上投影，最终可以在完成高优先级行为时，完成一部分低优先级的行为，并且低优先级的行为无法影响到高优先级的行为。下面，介绍零空间行为融合方法的计算方法：

对于第i个行为，首先定义如下的一个函数：

σ_i＝f_i(p) (1)

上式中，σ_i∈R^m表示的是待控制的变量，m表示函数的维度；p＝[p₁ p₂]^T，p₁和p₂分别表示双无人艇的位置，即：

于是，完成第i个行为的速度输出为：

上式中，v_i∈R^2×2，是一个2×2的矩阵，表示双无人艇分别在横、纵方向的期望航速；

是雅克比矩阵的伪逆矩阵；σ_id、

分别表示待控制的变量σ_i的期望值及期望值的导数；Λ_i是一个常数收益正定矩阵；

表示待控制变量的误差，即

当存在多个行为时，最终的输出速度是在考虑各行为的优先级的情况下将各行为的速度进行叠加，具体的方法就是先加低优先级的行为的输出速度投影到高优先级的零空间上，以此来消除低优先级行为输出速度中对高优先级行为有影响的那部分。通过上述的分析，得到最终系统的输出速度v_NSB可以表示为：

v_NSB＝v₁+N₁₁v₂+N₁₂v₃ (6)

上式中，速度下标数字表示该速度所对应行为的优先级，假设下标数字为1的输出速度对应的行为的优先级最高，且下标数字越大，优先级越低。N_1k表示将优先级为k的行为投影到优先级为1的行为的零空间所对应的零空间投影矩阵，且有：

上式中，I表示单位矩阵，J_1k表示将优先级为i的行为投影到优先级为1的行为的零空间所对应的雅克比零空间，定义其计算方法为：

假设已经规划出双无人艇协同溢油围捕的期望轨迹，并且通过两艘无人艇上的位姿传感器实时测得双无人艇的位置和姿态信息。首先，针对双无人艇协同轨迹跟踪的过程进行分析，将协同轨迹跟踪抽象出双艇的几种行为：相互避碰行为、保持队形行为、趋向目标行为。

再进一步，所述趋向目标的行为控制方程为：

σ_a＝||p_d-p||，

式中σ_a表示轨迹跟踪行为的控制量，为无人艇与双艇最终期望位置的距离；p_d表示双无人艇最终期望位置，p表示双无人艇的当前位置；所述最终期望位置为以溢油点为圆心，以0.5d为半径的圆上两点，双无人艇中心和圆上两点构成等腰三角形；所述d为预设置长度；

其中：

式中p₁为一号无人艇的当前位置向量，(x₁ y₁)为一号无人艇的当前位置坐标；

p₂为二号无人艇的当前位置向量，(x₂ y₂)为二号无人艇的当前位置坐标；

式中p_1d表示一号无人艇的最终期望位置，(x_1d y_1d)为一号无人艇的最终期望位置坐标；

p_2d表示二号无人艇的最终期望位置，(x_2d y_2d)为二号无人艇的最终期望位置坐标；

本实施方式中，一号无人艇的最终期望位置p_1d和二号无人艇的最终期望位置p_2d是随着时间周期的不同，根据当前的行为融合结果而变化的。即所述一号无人艇的最终期望位置p_1d和二号无人艇的最终期望位置p_2d不是固定值，随着不同时间周期内溢油点的变化而变化。

式中x_rd为溢油点的x轴坐标，y_rd为溢油点的y轴坐标，

为双无人艇中心与溢油点连线与x轴的夹角。

则：

再进一步，对双艇相互避碰行为进行说明：

双无人艇在进行协同轨迹跟踪时，可能受到干扰使得无人艇的轨迹跟踪发生偏差，导致双无人艇之间的间距偏小，存在相互碰撞的风险。因此，需要根据双艇的性能，设置一个安全距离d_min，该值的大小需要综合考虑双无人艇的回转性及舵响应时间等。艇间避碰的思想就是双艇将彼此当作障碍物，然后实时计算彼此间的距离，当艇间距离小于安全距离d_min时，执行避碰行为。

艇间避碰行为的控制量为双艇彼此之间的距离，所述相互避碰的行为控制方程为：

σ_b表示双艇间避碰的行为函数，||p₁-p₂||表示双艇间距。

再进一步，对双艇保持队型行为进行说明：

在双无人艇相互避碰的行为中，得到的输出速度只能让双艇之间保持一个安全的距离，最终在以彼此为圆心，以安全距离d_min为半径的圆周上运动，而双无人艇在进行协同溢油围捕时，不仅需要保持双艇间距，还需要双艇保持齐头并进的队形，如图2所示。因此，在双艇间距满足要求时，需要进一步控制双艇的队形。

结合图2和图3所示，所述保持队形的行为控制方程为：

σ_c＝D_x＝(x₂-x₁)cosφ+(y₂-y₁)sinφ，

式中σ_c表示双无人艇保持队形的行为函数；

如图3所示，将大地坐标系保持坐标原点不变，旋转

角度，得到坐标系X'OY'；根据坐标系转换原理，得到双无人艇在坐标系X'OY'下的坐标：

式中p₁'为一号无人艇在坐标系X'OY'下的当前位置向量，(x₁,y₁)'为一号无人艇在坐标系X'OY'下的当前位置坐标；

式中p₂'为二号无人艇在坐标系X'OY'下的当前位置向量，(x₂,y₂)'为二号无人艇在坐标系X'OY'下的当前位置坐标；

得到：

式中i＝1,2，表示无人艇的序号；

φ表示坐标系逆时针旋转的角度：

所述

表示两个坐标系间的变换角度，φ表示将

的范围归一到0°到180°范围内；

计算得到双无人艇在坐标系X'OY'下沿X'轴的期望间距D_x为：

D_x＝x₂'-x₁'＝(x₂-x₁)cosφ+(y₂-y₁)sinφ。

双无人艇在坐标系X'OY'下沿Y'轴的期望间距D_y的值也可以根据坐标轴旋转原理求解：

D_y＝y₂'-y₁'＝(y₂-y₁)cosφ+(x₁-x₂)sinφ

因为在双无人艇相互避碰的行为中，已经将双无人艇的间距D₁₂控制在合适范围，且

因此只需要将D_x和D_y中的一个控制在期望值附近，另一个就会收敛到合适的值。因为无人艇的欠驱动性，所以控制D_x比D_y更加方便，只需要控制两艇的相对速度，便可以调节D_x的值。

得到三种行为的行为函数之后，根据公式(3)-(5)可求出各行为的期望速度向量。然后，根据双艇协同溢油围捕的机理给这些行为分配不同的优先级，首先要保证双艇的间距，以免发生碰撞，因此将双无人艇相互避碰行为定为最高优先级行为。在双无人艇系统完成相互避碰行为后，双艇会保持一定的距离，然后进行保持队形行为，使双无人艇形成一个齐头并进的队形，使保持队形行为处于第二优先级。在双无人艇系统完成前面两个行为之后，双艇会在保持相对安全的距离下齐头并进，此时便可以进行双艇轨迹跟踪行为，即将双艇轨迹跟踪行为的优先级定为最低。所以，优先级由高到低分别为：相互避碰行为>保持队形行为>趋向目标行为。最后，根据通过公式(6)-(8)得到双无人艇各自的期望航速及航向，对双无人艇进行航向与航速控制，使双无人艇朝着期望位置运动，最终完成协同轨迹跟踪。

虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其它所述实施例中。

Claims (5)

1.一种面向溢油围捕的双无人艇协同轨迹跟踪方法，其特征在于包括，

在当前时间周期通过两艘无人艇上的位姿传感器分别测量双无人艇的当前位置和当前姿态信息；给合当前位置和当前姿态信息以及当前溢油点位置，采用零空间行为融合方法对双无人艇进行行为融合，得到双无人艇的期望航速及航向；

控制双无人艇按照所述期望航速及航向运动，达到新位置；然后进入下一个时间周期，时间周期不断迭代，直到完成协同轨迹跟踪；

所述零空间行为融合方法中的行为包括相互避碰、保持队形和趋向目标三种行为，三种行为的优先级依次为相互避碰、保持队形和趋向目标；

进行行为融合的结果包括：

在行为为相互避碰时，行为融合结果为相互避碰的期望航速及航向；

在行为为保持队形时，行为融合结果为相互避碰和保持队形的期望航速及航向；

在行为为趋向目标时，行为融合结果为相互避碰、保持队形和趋向目标行为的期望航速及航向。

2.根据权利要求1所述的面向溢油围捕的双无人艇协同轨迹跟踪方法，其特征在于，

在行为为相互避碰时对应的双艇间距小于双艇安全距离；

在行为为保持队形时对应的双艇间距大于或等于双艇安全距离；

在行为为趋向目标时对应的双艇队形合格。

3.根据权利要求2所述的面向溢油围捕的双无人艇协同轨迹跟踪方法，其特征在于，

所述趋向目标的行为控制方程为：

σ_a＝||p_d-p||，

式中σ_a表示轨迹跟踪行为的控制量，为无人艇与双艇最终期望位置的距离；p_d表示双无人艇最终期望位置，p表示双无人艇的当前位置；

其中：

式中p₁为一号无人艇的当前位置向量，(x₁ y₁)为一号无人艇的当前位置坐标；

p₂为二号无人艇的当前位置向量，(x₂ y₂)为二号无人艇的当前位置坐标；

式中p_1d表示一号无人艇的最终期望位置，(x_1d y_1d)为一号无人艇的最终期望位置坐标；

p_2d表示二号无人艇的最终期望位置，(x_2d y_2d)为二号无人艇的最终期望位置坐标；

式中d为预设置长度，x_rd为溢油点的x轴坐标，y_rd为溢油点的y轴坐标，

为双无人艇中心与溢油点连线与x轴的夹角。

4.根据权利要求3所述的面向溢油围捕的双无人艇协同轨迹跟踪方法，其特征在于，

所述相互避碰的行为控制方程为：

σ_b表示双艇间避碰的行为函数。

5.根据权利要求4所述的面向溢油围捕的双无人艇协同轨迹跟踪方法，其特征在于，

所述保持队形的行为控制方程为：

σ_c＝D_x＝(x₂-x₁)cosφ+(y₂-y₁)sinφ，

式中σ_c表示双无人艇保持队形的行为函数；

将大地坐标系保持坐标原点不变，旋转

角度，得到坐标系X'OY'；根据坐标系转换原理，得到双无人艇在坐标系X'OY'下的坐标：

式中p₁'为一号无人艇在坐标系X'OY'下的当前位置向量，(x₁,y₁)'为一号无人艇在坐标系X'OY'下的当前位置坐标；

式中p₂'为二号无人艇在坐标系X'OY'下的当前位置向量，(x₂,y₂)'为二号无人艇在坐标系X'OY'下的当前位置坐标；

得到：

式中i＝1,2，表示无人艇的序号；

φ表示坐标系逆时针旋转的角度：

计算得到双无人艇在坐标系X'OY'下沿X'轴的期望间距D_x为：

D_x＝x₂'-x₁'＝(x₂-x₁)cosφ+(y₂-y₁)sinφ。

Images