CN112650231B - 一种实现避碰和避障的欠驱船编队控制系统 - Google Patents

Abstract

本发明提供一种实现避碰和避障的欠驱船编队控制系统，导引系统是由虚拟的船来产生期望的轨迹，从而得到船舶编队领导者在各个时刻的期望位姿及期望速度。GPS，陀螺仪等传感器采集各个船舶实际位姿及速度信息。图论描述了编队中的信息流向。各船舶间的信息经过图论传输给避碰和避障系统。避碰和避障系统根据障碍物的位置和船舶之间的位置产生响应的信息。控制器基于避碰和避障系统、期望编队、以及领导船的位姿速度信息设计，辅助动态系统被用来处理控制器的饱和问题，控制器产生的信号传输给船舶的执行机构，调整每艘欠驱船的艏向和速度，到达期望编队。本发明实现了编队避碰和避障设计，同时考虑了欠驱船的推力饱和；这使本专利更容易应用到实践。

Description

一种实现避碰和避障的欠驱船编队控制系统

技术领域

本发明涉及一种实现避碰和避障的欠驱船编队控制系统。

背景技术

在近十年间，由于无人船编队在搜索救援、资源勘探、海洋清污等方面的广泛的用途，无人船编队控制引起了控制领域的关注。

在设计无人船编队控制的方面，已经有许多专利给出了关于无人船的分布式控制器。然而很少有专利考虑避碰和避障的问题。中国专利CN110196599A提出了一种针对无人艇的分布式控制方法，避碰和连接保持这两个任务也得到了实现。然而该专利并未考虑执行机构的物理限制，这就意思着其控制器很难应用于实践。编队避碰和避障的一个难点在于：一般设计的避碰和避障均需要远超执行机构所能提供的力。不同于CN110196599A，本专利提供了另一种编队避碰和避障控制的解决方法。势能函数被应用于避碰和避障函数，这种设计提供了一个相对平缓的避碰和避障轨迹。辅助动态系统被用来处理输入饱和和欠驱问题，并且辅助动态的引入并未影响避碰和避障的实现。

发明内容

本发明的目的是为了提供一种实现避碰和避障的欠驱船编队控制系统。

本发明的目的是这样实现的：包括避碰和避障系统、障碍物、控制器、图论、欠驱船、领导船、执行机构、辅助动态系统、期望编队、干扰观测器，领导船是由虚拟的船来产生轨迹，根据此轨迹，期望编队、避碰和避障系统得到各个船在各个时刻的期望位姿及期望速度；传感器采集到的各个船舶位置和速度信息通过图论得出的第i条船附近的船的信息，避碰和避障系统根据该信息、障碍物信息产生相应的避碰和避障函数，并将其传出控制器，控制器根据领导船的信息、期望编队产生的信号、避碰和避障系统输出的信息、干扰观测器对干扰的估计值产生相应的信号，并将其传输给执行机构，执行机构调整船舶的纵向推力和舵角实现对第i条船的准确控制，实现期望的编队队形。

本发明还包括这样一些结构特征：

1.图论用来描述无人船编队之间的信息流向，有向图G＝{V_G，θ}包含了一组矢量V_G＝{0,1,…,N}和矢量θ＝{(i,j)∈{0,1,…,N}×V_G}，N代表欠驱船的个数；(i,j)∈θ描述了第j条船的信息可以传递给第i条船；

描述了第i条船的信息不能传递给第j条船；当第i条船靠近第j条船时，第j条船维持当前航向和航速，因为第j条船其无法获得第i条船的信息，无法做出避碰和避障机动，第i条船需要做出避让以避免碰撞。

2.避碰和避障系统是根据第i条船附近的船和障碍物信息设计的，当第i条船远离障碍物和其他无人船时，避碰和避障系统不起作用；当第i条船接近障碍物或其他无人船时，避碰和避障成为首要任务，编队跟踪任务暂时中止，以便于绕过障碍物；避碰和避障系统提供了两种避碰和避障轨迹，第一种为第i条船在附近无人船的后面穿越，和障碍物在右舷相遇；第二种为第i条船在附近无人船的前方穿越，和障碍物在左舷相遇。

3.欠驱船有四个，i＝1，2，3，4。

4.控制器的设计过程具体包括：

第一步：定义编队位置误差矢量如下：

式中：如果第i艘船可以获得第j艘船的信息a_ij＝1，否则a_ij＝0；p_ijd是期望的第i艘船相对第j艘船的位置，

是实现避碰和避障目标的系统函数，

为了稳定p_ie，虚拟控制律β_i＝[β_i1 β_i2]^T被设计为：

其中，K_1i是正定的增益矩阵，

第二步：定义航向角误差如下

ψ_ie＝ψ_i-ψ_ia，

ψ_ia＝ψ_j-atan(v_i/u_j)-atan(β_i2/Δ)

式中：Δ是表达前向距离的常数，atan是反正切函数；

为了稳定ψ_ie，虚拟控制律β_i3被设计如下

式中：k_i2是正常数；

第三步：定义速度误差如下：

式中：

是β_i1,β_i2,β_i3通过一阶滤波器得到的参数，α_i1,α_i2,α_i3为辅助动态系统的参数，用来处理输入饱和和欠驱的问题，Υ₁是正常数；

第四步：辅助动态系统参数的更新律被设计为：

式中：k_i4 T_u T_r是正常数；

第五步：第i艘船的控制律被设计如下：

式中：k_i3 k_i5是正常数。

与现有技术相比，本发明的有益效果是：本发明涉及一种实现避碰和避障的欠驱船编队控制系统。导引系统是由虚拟的船来产生期望的轨迹，从而得到船舶编队领导者在各个时刻的期望位姿及期望速度。GPS，陀螺仪等传感器采集各个船舶实际位姿及速度信息。图论描述了编队中的信息流向。各个船舶之间的信息经过图论传输给避碰和避障系统。避碰和避障系统根据障碍物的位置和船舶之间的位置产生响应的信息。控制器基于避碰和避障系统、期望编队、以及领导船的位姿速度信息设计，辅助动态系统被用来处理控制器的饱和问题，控制器产生的信号传输给船舶的执行机构，从而调整每艘欠驱船的艏向和速度，到达期望编队。此发明的优点在于实现了编队避碰和避障设计，同时考虑了欠驱船的推力饱和；这使得本专利更容易应用到实践。

附图说明

图1是一种实现避碰和避障的欠驱船编队控制系统。

图2是包含4艘无人船和1艘领导船的编队控制效果图。

图3是各艘船的控制输入图。

图中：1—避碰和避障系统；2—障碍物；3—分布式控制器；4—图论；5—欠驱船1；6—欠驱船2；7—欠驱船3；8—欠驱船4；9—领导船；10—执行机构；11—辅助动态系统；12—期望编队；13—干扰观测器。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

本发明是设计了能实现欠驱船编队避碰和避障的控制系统包括避碰和避障系统1，障碍物2，控制器3，图论4，欠驱船1 5，欠驱船2 6，欠驱船3 7，欠驱船4 8，领导船9，执行机构10，辅助动态系统11，期望编队12。领导船9是由虚拟的船来产生轨迹，根据此轨迹，期望编队12和避碰和避障系统1从而得到各个船在各个时刻的期望位姿及期望速度。传感器采集到的各个船舶位置和速度信息通过图论4得出的第i(i＝1,2,3,4)条船附近的船的信息，避碰和避障系统1根据该信息、障碍物信息产生相应的避碰和避障函数，并将其传出控制器3，控制器3根据领导船9的信息、期望编队12产生的信号、避碰和避障函数1输出的信息，干扰观测器13对干扰的估计值，并将其传输给执行机构10。执行机构10调整船舶的纵向推力和舵角实现对第i(i＝1,2,3,4)条船的准确控制，从而实现期望的编队队形。

1)图论4用来描述无人船编队之间的信息流向。有向图G＝{V_G，θ}包含了一组矢量V_G＝{0,1,…,n}和矢量θ＝{(i,j)∈{0,1,…,n}×V_G}，n代表欠驱船的个数。(i,j)∈θ描述了第j条船的信息可以传递给第i条船。

描述了第i条船的信息不可以传递给第j条船。这就意味者当第i条船靠近第j条船时。第j条船维持当前航向和航速，因为第i条船其无法获得第j条船的信息，无法做出避碰和避障机动。第i条船需要做出避让以避免碰撞。

2)避碰和避障系统1是根据第i(i＝1,2,3,4)条船附近的船和障碍物信息设计的。当第i条船远离障碍物和其他无人船时，避碰和避障系统1不起作用。当第i条船接近障碍物或其他无人船时，此时编队跟踪任务变得不那么重要，而避碰和避障成为首要任务，因为碰撞有可能对无人船造成严重损坏。编队跟踪任务暂时中止，以便于绕过障碍物。避碰和避障系统1提供了两种避碰和避障轨迹。第一种为第i条船在附近无人船的后面穿越，和障碍物在右舷相遇。第二种为第i条船在附近无人船的前方穿越，和障碍物在左舷相遇。

3)避碰和避障系统巧妙的设计方法，促使该控制器产生的指令大大降低了饱和的风险。这给我们提供了处理饱和的基础。辅助动态系统11被用来解决欠驱问题和输入饱和问题。辅助动态系统11的引入使得该发明的控制系统易于实践。

如图1所示，本发明是设计了能实现欠驱船编队避碰和避障的控制系统包括避碰和避障系统1，障碍物2，控制器3,图论4，欠驱船1 5，欠驱船2 6，欠驱船3 7，欠驱船4 8，领导船9，执行机构10，辅助动态系统11，期望编队12，干扰观测器13。

首先是对于第i艘无人船的动力学和运动学模型介绍。

式中：(x_i,y_i)为第i艘船的位置,ψ_i是艏向向量，u_i,v_i,r_i分别为船舶的纵向速度、横向速度、转艏角速度，m₁₁,m₂₂,m₃₃为船舶的惯性质量，f_ih(h＝1,2,3)包含了水动力阻尼参数，d_ih(h＝1,2,3)为环境干扰力，τ_iu为前进方向的力，τ_ir为艏向力矩。

1)领导船9是由一个下标为“0”的虚拟船行驶的轨迹来生成：

通过积分可以计算出领导船各个时刻的状态。式种(x₀,y₀)是领导船的位置，ψ₀是领导船的艏摇角。u₀是领导船的纵荡速度，v₀为领导船的横荡速度，r₀是领导船的艏摇角速度。

2)输入无饱和、输入饱和之间的差值函数被定义为ω_iu＝τ_iuc-τ_iu，ω_ir＝τ_irc-τ_ir。τ_iuc和τ_irc是控制器计算出的纵向推力和转艏力矩。

3)障碍物2被假设为一个圆形的物体，其圆心坐标为p_k＝[x_k,y_k]^T。

4)避碰和避障系统1是由势能函数组成的函数，被定义如下

c_ij和c_ik为避碰和避障策略，可选为1或-1，n为无人船的个数，N_o为障碍物的个数，

是用来实现船舶之间避碰的函数，表达式如下

式中，α_c是正常数，||p_ijv||＝||p_i-p_j||，p_i＝[x_i,y_i]^T是第i条船的位置矢量，p_j＝[x_j,y_j]^T是第j条船的位置矢量。R _c是最小安全距离，

是避碰距离。

是用来实现避障的函数，被定义为如下：

式中，α_o是正常数，||p_ikv||＝||p_i-p_k||，R _o是最小安全距离，

是避障距离。

5)干扰观测器13根据无人船的控制输入和速度信息估计出环境干扰，其模型如下：

式中ζ_i是观测器状态，

是正定的观测器增益矩阵，

是干扰ξ_i的估计值，

τ_i＝[τ_iu 0 τ_ir]^T，ξ_i＝[ξ_i1 ξ_i2 ξ_i3]^T＝d_i+f_i，d_i＝[d_i1 d_i2 d_i3]^T，f_i＝[f_i1 f_i2 f_i3]^T，M＝diag{m₁₁ m₂₂ m₃₃}为系统惯性矩阵，υ_i＝[u_i v_i r_i]^T。

6)控制器设计

第一步：定义编队位置误差矢量如下

式中，如果第i艘船可以获得第j艘船的信息a_ij＝1，否则a_ij＝0。p_ijd是期望的第i艘船相对第j艘船的位置，

是实现避碰和避障目标的系统函数，

为了稳定p_ie，虚拟控制律β_i＝[β_i1 β_i2]^T被设计为

其中，K_1i是正定的增益矩阵，

第二步：定义航向角误差如下

ψ_ie＝ψ_i-ψ_ia，

其表达式如下：

ψ_ia＝ψ_j-atan(v_i/u_j)-atan(β_i2/Δ)

Δ是表达前向距离的常数，atan是反正切函数。

为了稳定ψ_ie，虚拟控制律β_i3被设计如下

k_i2是正常数。

第三步：定义速度误差如下

这里由于

是β_i1,β_i2,β_i3通过一阶滤波器得到的参数，α_i1,α_i2,α_i3为辅助动态系统的参数，它们被用来处理输入饱和和欠驱的问题，Υ₁是正常数。

第四步：辅助动态系统参数的更新律被设计为：

式子中，k_i4 T_u T_r是正常数。

第五步：第i艘船的控制律被设计如下：

式子中，k_i3 k_i5是正常数。

仿真分析：图二证明了控制算法的有效性。在图二中优先级顺序为USV0大于USV1大于USV2大于USV3大于USV4。船舶形状符号按照相同时间间隔被放到航行轨迹上。绿色的圆形代表障碍物。可以看出所有的无人船都避开了障碍物。对于USV4航行出的轨迹来说，可以看到USV4成功避开了所有的USV。图三体现了各个船输入信号的变化，可以看出该控制系统能够很好的处理输入饱和。

Claims (4)

1.一种实现避碰和避障的欠驱船编队控制系统，其特征在于：包括避碰和避障系统、障碍物、控制器、图论、欠驱船、领导船、执行机构、辅助动态系统、期望编队、干扰观测器，领导船是由虚拟的船来产生轨迹，根据此轨迹，期望编队、避碰和避障系统得到各个船在各个时刻的期望位姿及期望速度；传感器采集到的各个船舶位置和速度信息通过图论得出的第i条船附近的船的信息，避碰和避障系统根据该信息、障碍物信息产生相应的避碰和避障函数，并将其传出控制器，控制器根据领导船的信息、期望编队产生的信号、避碰和避障系统输出的信息、干扰观测器对干扰的估计值产生相应的信号，并将其传输给执行机构，执行机构调整船舶的纵向推力和舵角实现对第i条船的准确控制，实现期望的编队队形；

控制器的设计过程具体包括：

第一步：定义编队位置误差矢量如下：

式中：如果第i艘船可以获得第j艘船的信息a_ij＝1，否则a_ij＝0；p_ijd是期望的第i艘船相对第j艘船的位置，

是实现避碰和避障目标的系统函数，

为了稳定p_ie，虚拟控制律β_i＝[β_i1 β_i2]^T被设计为：

其中，K_1i是正定的增益矩阵，

第二步：定义航向角误差如下

ψ_ie＝ψ_i-ψ_ia

ψ_ia＝ψ_j-atan(v_i/u_j)-atan(β_i2/Δ)

式中：Δ是表达前向距离的常数，atan是反正切函数；

为了稳定ψ_ie，虚拟控制律β_i3被设计如下

式中：k_i2是正常数；

第三步：定义速度误差如下：

式中：

是β_i1,β_i2,β_i3通过一阶滤波器得到的参数，α_i1,α_i2,α_i3为辅助动态系统的参数，用来处理输入饱和和欠驱的问题，Υ₁是正常数；

第四步：辅助动态系统参数的更新律被设计为：

式中：k_i4 T_u T_r是正常数；

第五步：第i艘船的控制律被设计如下：

式中：k_i3 k_i5是正常数。

2.根据权利要求1所述的一种实现避碰和避障的欠驱船编队控制系统，其特征在于：图论用来描述无人船编队之间的信息流向，有向图G＝{V_G，θ}包含了一组矢量V_G＝{0,1,L,N}和矢量θ＝{(i,j)∈{0,1,L,N}×V_G}，N代表欠驱船的个数；(i,j)∈θ描述了第j条船的信息可以传递给第i条船；

描述了第i条船的信息不能传递给第j条船；当第i条船靠近第j条船时，第j条船维持当前航向和航速，因为第j条船其无法获得第i条船的信息，无法做出避碰和避障机动，第i条船需要做出避让以避免碰撞。

3.根据权利要求2所述的一种实现避碰和避障的欠驱船编队控制系统，其特征在于：避碰和避障系统是根据第i条船附近的船和障碍物信息设计的，当第i条船远离障碍物和其他无人船时，避碰和避障系统不起作用；当第i条船接近障碍物或其他无人船时，避碰和避障成为首要任务，编队跟踪任务暂时中止，以便于绕过障碍物；避碰和避障系统提供了两种避碰和避障轨迹，第一种为第i条船在附近无人船的后面穿越，和障碍物在右舷相遇；第二种为第i条船在附近无人船的前方穿越，和障碍物在左舷相遇。

4.根据权利要求1-3任意一所述的一种实现避碰和避障的欠驱船编队控制系统，其特征在于：欠驱船有四个，i＝1，2，3，4。

Images