[发明专利]一种多AUV动态圆弧编队控制方法

权利要求书

1.一种多AUV动态圆弧编队控制方法，其特征在于：步骤如下：

步骤1：获得领航者和跟随者需跟踪的圆弧路径；

步骤2：建立参考坐标系

建立表达AUV运动的固定参考坐标系，以步骤1中获取的圆弧路径的圆心为固定参考坐标系的原点O；任意选取圆弧的一条直径所在的直线为坐标轴的纵轴，记为X轴；垂直于该直径且经过圆弧圆心的直线为坐标轴的横轴，记为Y轴；AUV重心为G，速度向量为V_t，在固定坐标系下X轴和Y轴上投影为AUV受到的外力F在固定坐标系下X轴和Y轴方向上的分力为F_x,F_y；受到的外力矩为T；

建立表达AUV运动的运动坐标系，原点取在AUV重心G处；纵轴取在AUV纵中剖面内，指向船首，记为x轴；横轴与AUV纵中剖面垂直，指向右舷，记为y轴；速度向量为V_t，在运动坐标系下x轴和y轴上投影为u,v；AUV受到的外力F在固定坐标系下x轴和y轴方向上的分力为F_x,F_y；受到的外力矩为T；因外力矩而获得的角速度为r；

步骤3：建立AUV数学模型

固定坐标系的X轴与运动坐标系x轴的夹角为艏向角，记为ψ；AUV速度V_t与固定坐标系X轴的夹角为φ；假设u恒不为0，定义侧滑角β，则：

β＝φ-ψ＝arctan(v/u)

得到AUV的动力学模型为：

式中：为ψ和φ的导数，V_t为AUV的合速度，且

在运动坐标系中，实际流体有黏性，会出现流体惯性阻力效应，相当于AUV质量增加，针对欠驱动AUV，只会在AUV纵向受到水平推力F_T和垂直水平面方向转艏力矩Γ，AUV运动时会受到其它因素在水平面对AUV的阻力F_w和垂直水平面方向的力矩N_w，其中F_w在x轴和y轴上的分力为F_xw,F_yw：

则AUV在实际情况下的动力学模型为：

式中：m_u；m_vr；m_v；m_ur；m_r分别为实际运动中的质量；

步骤4：由圆弧视线导引法推导AUV期望艏向角

在圆弧路径跟踪中，p_k(x_k,y_k)为AUV到圆心连线与圆弧的交点，p(x_t,y_t)为AUV的实时位置坐标，ψ(t)为AUV的实时艏向角，e为路径跟踪的横向误差，p_los(x_los,y_los)为视线引导法生成的前视点，ψ_d为AUV期望角，Δ为选取的前视向量p_kp_los的长度，α(t)为AUV期望运行方向与有向线段Op的夹角，θ为有向线段Op与北向坐标轴的夹角；

由几何关系得：

θ＝arctan(y_t,x_t)

ψ_d＝θ-α(t)

式中：α(t)＝arctan(|e|/Δ)；

步骤5：构建固定坐标系下的跟随者AUV的误差模型

在固定坐标系中，假定AUV_i为领航者，其它AUV为跟随者，在编队队形中AUV_i将其位姿纵坐标通过水声通信传感器发送给跟随者，跟随者实时采集领航者的纵向坐标信息，AUV_i在固定坐标系下的位姿为(X_i,Y_i,ψ_i)；第j个跟随者的坐标为(X_j,Y_j,ψ_j)；

当多AUV实现编队后，领航者将跟随步骤1所规划的圆弧路径，半径为R_i，即而跟随者将在圆弧路径所在的同心圆弧上运动，半径为R_j，即其中D_j＝R_j-R_i；

则得到以下误差模型：

(1)姿态角角度误差

定义AUV姿态角θ＝arctan(Y/X)，其中X,Y为AUV在步骤2中固定参考坐标系下的坐标，则有：

领航者AUV姿态角θ_i＝arctan(Y_i/X_i)，

跟随者AUV姿态角θ_j＝arctan(Y_j/X_j)

则对于领航者的位姿角度误差为0；

而第j个跟随者的位姿角度误差θ_ej：

θ_ej＝θ_i-θ_j-θ_sj

式中：θ_sj为第j个跟随者AUV与领航者AUV_i的理想姿态角之差；

(2)艏向角误差

领航者与第j个跟随者的距离误差E_j为：

又有：

式中：e_i和e_j可以看作领航者和第j个跟随者AUV路径跟踪中的误差；

将e_i和e_j带入E_j:

E_j＝e_j-e_i

当时，有所以此时领航者和跟随者均做圆弧运动，则将领航者和第j个跟随者AUV路径跟踪中的误差E_j转化为AUV的路径跟踪误差e_j,e_i；

而步骤4中圆弧视线导引法又通过将路径跟踪误差得到领航者和第j个跟随者期望艏向角ψ_di,ψ_dj，所以领航者和第j个跟随者的艏向角误差分别为ψ_ei,ψ_ej，且有：

ψ_ei＝ψ_di-ψ_i,

ψ_ej＝ψ_dj-ψ_j

其中ψ_i,ψ_j分别为领航者和第j个跟随者的当前艏向角：

(3)速度误差

领航者AUV_i的速度误差u_ei为：

u_ei＝u_i-u_d

式中：u_d为领航者期望速度，u_i为AUV当前速度；

对于跟随者AUV_j：

u_ej＝u_j-u_cj

式中：u_cj为为第j个跟随者AUV期望速度，u_j为第j个跟随者AUV当前速度；

而：u_cj＝(R_j/R_i)*u_d-g(θ_ej)

在这里函数g(·)在定义域[-π,π]内满足如下条件：

设定：

其中a满足

u_min,u_max为AUV的最小速度和最大速度，u为AUV速度；

步骤6：AUV运动控制器的设计

使步骤5中的3个误差趋于0，则实现多AUV之间的圆弧编队；定义K_p1、K_i1、K_d1分别为艏向角控制的比例、积分、微分参数，K_p、K_i、K_d分别为速度控制比例、积分，微分参数；

以k时刻设计领航者和跟随者的运动控制器：

ψ_ei(k)、ψ_ei(k-1)、ψ_ei(k-2)分别为领航者AUV在k时刻的艏向角误差、(k-1)时刻艏向角误差、(k-2)时刻艏向角误差；u_ei(k)、u_ei(k-1)、u_ei(k-2)分别为领航者AUV在k时刻速度误差、(k-1)时刻的速度误差、(k-2)时刻的速度误差；ψ_ej(k)、ψ_ej(k-1)、ψ_ej(k-2)分别为第j个跟随者AUV在k时刻的艏向角误差、(k-1)时刻艏向角误差、(k-2)时刻艏向角误差；u_ej(k)、u_ej(k-1)、u_ej(k-2)分别为第j个跟随者AUV在k时刻速度误差、(k-1)时刻的速度误差、(k-2)时刻的速度误差；Γ_i(k-1)、Γ_i(k)分别为领航者AUV在(k-1)时刻、k时刻的转艏力矩；Γ_j(k-1)、Γ_j(k)为第j个跟随者AUV(k-1)时刻、k时刻的转艏力矩；F_Ti(k-1)、F_Ti(k)为领航者AUV在(k-1)时刻、k时刻的水平推力；F_Tj(k-1)、F_Tj(k)为第j个跟随者AUV(k-1)时刻、k时刻的水平推力，则有：

(1)艏向角误差

距离误差可由步骤4的圆弧视线导引法，通过寻找前向距离，使AUV的距离误差变为艏向角误差，而艏向角可以通过改变领航者AUV的转艏力矩Γ_i(k)和第j个跟随者AUV的转艏力矩Γ_j(k)来控制：

Γ_i(k)＝Γ_i(k-1)+K_p1*(ψ_ei(k)-ψ_ei(k-1))+K_i1*ψ_ei(k)+K_d1*(ψ_ei(k)-2*ψ_ei(k-1)+ψ_ei(k-2))

Γ_j(k)＝Γ_j(k-1)+K_p1*(ψ_ej(k)-ψ_ej(k-1))+K_i1*ψ_ej(k)+K_d1*(ψ_ej(k)-2*ψ_ej(k-1)+ψ_ej(k-2))

其中当k≤0时Γ_i(k)、ψ_ei(k)、Γ_i(k)、ψ_ej(k)都为0，

(2)角度误差和速度误差

角度误差和速度误差均可以通过AUV速度控制来减小，通常会给AUV一个期望速度u_d；通过控制领航者AUV纵向受到水平推力F_Ti(k)和第j个跟随者AUV纵向受到水平推力F_Tj(k)来控制AUV速度，

F_Ti(k)＝F_Ti(k-1)+K_p*(u_ei(k)-u_ei(k-1))+K_i*u_ei(k)+K_d*(u_ei(k)-2*u_ei(k-1)+u_ei(k-2))

F_Tj(k-1)＝F_Tj(k-1)+K_p*(u_ej(k)-u_ej(k-1))+K_i*u_ej(k)+K_d*(u_ej(k)-2*u_ej(k-1)+u_ej(k-2))

其中当k≤0时，F_Ti(k)、u_ei(k)、F_Ti(k)、u_ej(k)都为0。