[發(fā)明專利]一種多AUV動(dòng)態(tài)圓弧編隊(duì)控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201810243237.0
• 申請(qǐng)日: 2018-03-23
• 公開（公告）號(hào): CN108490961B
• 公開（公告）日: 2020-11-10
• 發(fā)明（設(shè)計(jì)）人: 李娟;袁銳錕;張建新;陳興華;陳濤;杜雪
• 申請(qǐng)（專利權(quán)）人: 哈爾濱工程大學(xué)
• 主分類號(hào): G05D1/06
• 分類號(hào): G05D1/06
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種多AUV動(dòng)態(tài)圓弧編隊(duì)控制方法，其特征在于：步驟如下：

步驟1：獲得領(lǐng)航者和跟隨者需跟蹤的圓弧路徑；

步驟2：建立參考坐標(biāo)系

建立表達(dá)AUV運(yùn)動(dòng)的固定參考坐標(biāo)系，以步驟1中獲取的圓弧路徑的圓心為固定參考坐標(biāo)系的原點(diǎn)O；任意選取圓弧的一條直徑所在的直線為坐標(biāo)軸的縱軸，記為X軸；垂直于該直徑且經(jīng)過圓弧圓心的直線為坐標(biāo)軸的橫軸，記為Y軸；AUV重心為G，速度向量為V_t，在固定坐標(biāo)系下X軸和Y軸上投影為AUV受到的外力F在固定坐標(biāo)系下X軸和Y軸方向上的分力為F_x,F_y；受到的外力矩為T；

建立表達(dá)AUV運(yùn)動(dòng)的運(yùn)動(dòng)坐標(biāo)系，原點(diǎn)取在AUV重心G處；縱軸取在AUV縱中剖面內(nèi)，指向船首，記為x軸；橫軸與AUV縱中剖面垂直，指向右舷，記為y軸；速度向量為V_t，在運(yùn)動(dòng)坐標(biāo)系下x軸和y軸上投影為u,v；AUV受到的外力F在固定坐標(biāo)系下x軸和y軸方向上的分力為F_x,F_y；受到的外力矩為T；因外力矩而獲得的角速度為r；

步驟3：建立AUV數(shù)學(xué)模型

固定坐標(biāo)系的X軸與運(yùn)動(dòng)坐標(biāo)系x軸的夾角為艏向角，記為ψ；AUV速度V_t與固定坐標(biāo)系X軸的夾角為φ；假設(shè)u恒不為0，定義側(cè)滑角β，則：

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

得到AUV的動(dòng)力學(xué)模型為：

式中：為ψ和φ的導(dǎo)數(shù)，V_t為AUV的合速度，且

在運(yùn)動(dòng)坐標(biāo)系中，實(shí)際流體有黏性，會(huì)出現(xiàn)流體慣性阻力效應(yīng)，相當(dāng)于AUV質(zhì)量增加，針對(duì)欠驅(qū)動(dòng)AUV，只會(huì)在AUV縱向受到水平推力F_T和垂直水平面方向轉(zhuǎn)艏力矩Γ，AUV運(yùn)動(dòng)時(shí)會(huì)受到其它因素在水平面對(duì)AUV的阻力F_w和垂直水平面方向的力矩N_w，其中F_w在x軸和y軸上的分力為F_xw,F_yw：

則AUV在實(shí)際情況下的動(dòng)力學(xué)模型為：

式中：m_u；m_vr；m_v；m_ur；m_r分別為實(shí)際運(yùn)動(dòng)中的質(zhì)量；

步驟4：由圓弧視線導(dǎo)引法推導(dǎo)AUV期望艏向角

在圓弧路徑跟蹤中，p_k(x_k,y_k)為AUV到圓心連線與圓弧的交點(diǎn)，p(x_t,y_t)為AUV的實(shí)時(shí)位置坐標(biāo)，ψ(t)為AUV的實(shí)時(shí)艏向角，e為路徑跟蹤的橫向誤差，p_los(x_los,y_los)為視線引導(dǎo)法生成的前視點(diǎn)，ψ_d為AUV期望角，Δ為選取的前視向量p_kp_los的長度，α(t)為AUV期望運(yùn)行方向與有向線段Op的夾角，θ為有向線段Op與北向坐標(biāo)軸的夾角；

由幾何關(guān)系得：

θ＝arctan(y_t,x_t)

ψ_d＝θ-α(t)

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

步驟5：構(gòu)建固定坐標(biāo)系下的跟隨者AUV的誤差模型

在固定坐標(biāo)系中，假定AUV_i為領(lǐng)航者，其它AUV為跟隨者，在編隊(duì)隊(duì)形中AUV_i將其位姿縱坐標(biāo)通過水聲通信傳感器發(fā)送給跟隨者，跟隨者實(shí)時(shí)采集領(lǐng)航者的縱向坐標(biāo)信息，AUV_i在固定坐標(biāo)系下的位姿為(X_i,Y_i,ψ_i)；第j個(gè)跟隨者的坐標(biāo)為(X_j,Y_j,ψ_j)；

當(dāng)多AUV實(shí)現(xiàn)編隊(duì)后，領(lǐng)航者將跟隨步驟1所規(guī)劃的圓弧路徑，半徑為R_i，即而跟隨者將在圓弧路徑所在的同心圓弧上運(yùn)動(dòng)，半徑為R_j，即其中D_j＝R_j-R_i；

則得到以下誤差模型：

(1)姿態(tài)角角度誤差

定義AUV姿態(tài)角θ＝arctan(Y/X)，其中X,Y為AUV在步驟2中固定參考坐標(biāo)系下的坐標(biāo)，則有：

領(lǐng)航者AUV姿態(tài)角θ_i＝arctan(Y_i/X_i)，

跟隨者AUV姿態(tài)角θ_j＝arctan(Y_j/X_j)

則對(duì)于領(lǐng)航者的位姿角度誤差為0；

而第j個(gè)跟隨者的位姿角度誤差θ_ej：

θ_ej＝θ_i-θ_j-θ_sj

式中：θ_sj為第j個(gè)跟隨者AUV與領(lǐng)航者AUV_i的理想姿態(tài)角之差；

(2)艏向角誤差

領(lǐng)航者與第j個(gè)跟隨者的距離誤差E_j為：

又有：

式中：e_i和e_j可以看作領(lǐng)航者和第j個(gè)跟隨者AUV路徑跟蹤中的誤差；

將e_i和e_j帶入E_j:

E_j＝e_j-e_i

當(dāng)時(shí)，有所以此時(shí)領(lǐng)航者和跟隨者均做圓弧運(yùn)動(dòng)，則將領(lǐng)航者和第j個(gè)跟隨者AUV路徑跟蹤中的誤差E_j轉(zhuǎn)化為AUV的路徑跟蹤誤差e_j,e_i；

而步驟4中圓弧視線導(dǎo)引法又通過將路徑跟蹤誤差得到領(lǐng)航者和第j個(gè)跟隨者期望艏向角ψ_di,ψ_dj，所以領(lǐng)航者和第j個(gè)跟隨者的艏向角誤差分別為ψ_ei,ψ_ej，且有：

ψ_ei＝ψ_di-ψ_i,

ψ_ej＝ψ_dj-ψ_j

其中ψ_i,ψ_j分別為領(lǐng)航者和第j個(gè)跟隨者的當(dāng)前艏向角：

(3)速度誤差

領(lǐng)航者AUV_i的速度誤差u_ei為：

u_ei＝u_i-u_d

式中：u_d為領(lǐng)航者期望速度，u_i為AUV當(dāng)前速度；

對(duì)于跟隨者AUV_j：

u_ej＝u_j-u_cj

式中：u_cj為為第j個(gè)跟隨者AUV期望速度，u_j為第j個(gè)跟隨者AUV當(dāng)前速度；

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

在這里函數(shù)g(·)在定義域[-π,π]內(nèi)滿足如下條件：

設(shè)定：

其中a滿足

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

步驟6：AUV運(yùn)動(dòng)控制器的設(shè)計(jì)

使步驟5中的3個(gè)誤差趨于0，則實(shí)現(xiàn)多AUV之間的圓弧編隊(duì)；定義K_p1、K_i1、K_d1分別為艏向角控制的比例、積分、微分參數(shù)，K_p、K_i、K_d分別為速度控制比例、積分，微分參數(shù)；

以k時(shí)刻設(shè)計(jì)領(lǐng)航者和跟隨者的運(yùn)動(dòng)控制器：

ψ_ei(k)、ψ_ei(k-1)、ψ_ei(k-2)分別為領(lǐng)航者AUV在k時(shí)刻的艏向角誤差、(k-1)時(shí)刻艏向角誤差、(k-2)時(shí)刻艏向角誤差；u_ei(k)、u_ei(k-1)、u_ei(k-2)分別為領(lǐng)航者AUV在k時(shí)刻速度誤差、(k-1)時(shí)刻的速度誤差、(k-2)時(shí)刻的速度誤差；ψ_ej(k)、ψ_ej(k-1)、ψ_ej(k-2)分別為第j個(gè)跟隨者AUV在k時(shí)刻的艏向角誤差、(k-1)時(shí)刻艏向角誤差、(k-2)時(shí)刻艏向角誤差；u_ej(k)、u_ej(k-1)、u_ej(k-2)分別為第j個(gè)跟隨者AUV在k時(shí)刻速度誤差、(k-1)時(shí)刻的速度誤差、(k-2)時(shí)刻的速度誤差；Γ_i(k-1)、Γ_i(k)分別為領(lǐng)航者AUV在(k-1)時(shí)刻、k時(shí)刻的轉(zhuǎn)艏力矩；Γ_j(k-1)、Γ_j(k)為第j個(gè)跟隨者AUV(k-1)時(shí)刻、k時(shí)刻的轉(zhuǎn)艏力矩；F_Ti(k-1)、F_Ti(k)為領(lǐng)航者AUV在(k-1)時(shí)刻、k時(shí)刻的水平推力；F_Tj(k-1)、F_Tj(k)為第j個(gè)跟隨者AUV(k-1)時(shí)刻、k時(shí)刻的水平推力，則有：

(1)艏向角誤差

距離誤差可由步驟4的圓弧視線導(dǎo)引法，通過尋找前向距離，使AUV的距離誤差變?yōu)轸枷蚪钦`差，而艏向角可以通過改變領(lǐng)航者AUV的轉(zhuǎn)艏力矩Γ_i(k)和第j個(gè)跟隨者AUV的轉(zhuǎn)艏力矩Γ_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))

其中當(dāng)k≤0時(shí)Γ_i(k)、ψ_ei(k)、Γ_i(k)、ψ_ej(k)都為0，

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

角度誤差和速度誤差均可以通過AUV速度控制來減小，通常會(huì)給AUV一個(gè)期望速度u_d；通過控制領(lǐng)航者AUV縱向受到水平推力F_Ti(k)和第j個(gè)跟隨者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))

其中當(dāng)k≤0時(shí)，F(xiàn)_Ti(k)、u_ei(k)、F_Ti(k)、u_ej(k)都為0。