[發明專利]一種基于偶極勢場的AUV回塢導引系統及導引方法

有效

• 申請號: 201510226276.6
• 申請日: 2015-05-06
• 公開（公告）號: CN104820430B
• 公開（公告）日: 2017-06-30
• 發明（設計）人: 嚴衛生;齊貝貝;高劍;崔榮鑫;王銀濤;李慧平
• 申請（專利權）人: 西北工業大學
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 西北工業大學專利中心61204
• 代理人: 王鮮凱

1.一種利用基于偶極勢場的AUV回塢導引系統導引AUV回塢的方法，所述的基于偶極勢場的AUV回塢導引系統：包括USBL收發器和兩個USBL應答器；USBL收發器設置于AUV的頭部的中心軸線上，兩個USBL應答器設置于回收站的兩側；所述兩個USBL應答器為電偶極子，其特征在于步驟如下：

步驟1：AUV在回收坐標系xoy下從初始點X(0)＝[x(0)y(0)]_T以初始航向角ψ(0)開始進行回塢導引；其中：x、y為AUV在回收坐標系下的位置坐標，(x(0)y(0))在AUV航行之前進行設定；所述回收坐標系xoy是以回收站的兩側安裝的兩個USBL應答器B₁和B₂的中心為原點，兩個USBL應答器B₁和B₂之間的連線為y軸，水平向上垂直于y軸且過原點的為x的正方向軸，B₁位于B₂的左邊，B₁指向B₂的方向為y軸的正方向；

步驟2：AUV上安裝的USBL收發器與回收站兩側的應答器B₁和B₂進行通信，測量出應答器B₁和B₂在AUV機體坐標系x_Bo_By_B下的距離L_i和方位μ_i，其中i＝1,2；

步驟3：計算應答器B₁和B₂在AUV機體坐標系x_Bo_By_B下的位置坐標B_i(x_Ti,y_Ti)，i＝1,2：

$\left[\begin{array}{c}{x}_{Ti}\\ y_{Ti}\end{array}\right]=\left[\begin{array}{c}{\mathrm{cos\μ}}_i\\ {\mathrm{sin\μ}}_i\end{array}\right]\[L_i\]+\left[\begin{array}{c}l\\ 0\end{array}\right]$

步驟4：根據電偶極子電場原理，定義電偶極矩矢量應答器中點的位置坐標為由于回收站的入口矢量與垂直，則o為回收坐標系xoy的原點，l為應答器B₁和B₂之間的距離；

步驟5：計算出AUV機體坐標系x_Bo_By_B和AUV回收坐標系xoy之間的夾角：

$\mathrm{\ψ}=arctan\left(\frac{x_{T2}-x_{T1}}{y_{T2}-y_{T1}}\right)$

B_i(x_Ti,y_Ti)是應答器B₁和B₂在AUV機體坐標系x_Bo_By_B下的位置坐標，i＝1,2；

步驟6：計算AUV在回收坐標系xoy下的位置坐標：

$\left[\begin{array}{c}x\\ y\end{array}\right]=-\left[\begin{array}{cc}cos\mathrm{\ψ}& -\sin \mathrm{\ψ}\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}\end{array}\right]{(\frac{x_{T1}+x_{T2}}{2},\frac{y_{T1}+y_{T2}}{2})}^T$

步驟7：定義偶極勢場F(n)＝λ(p^Tn)n-p(n^Tn),是一個除了原點之外處處非零的二維的矢量場，其中，常量λ≥2，p＝[p_x p_y]^Τ為偶極勢場的電偶極矩，n為偶極勢場的位置矢量，偶極勢場最大的特點是在偶極勢場空間中任意一點的運動都沿著勢場線的切線方向；

兩個應答器在回收坐標系xoy下的位置坐標分別為p₁＝(p_1x,p_1y)和p₂＝(p_2x,p_2y)，則電偶極矩p＝(p_2x-p_1x,p_2y-p_1y)^Τ；

由于AUV在偶極勢場中的位置矢量為n＝[x y]^Τ，則偶極勢場為

F_nx＝(λ-1)(p_2x-p_1x)x²+λ(p_2y-p_1y)xy-(p_2x-p_1x)y²

F_ny＝(λ-1)(p_2y-p_1y)y²+λ(p_2x-p_1x)xy-(p_2y-p_1y)x²；

步驟8：根據步驟7定義的偶極勢場，確定AUV回收的航向角指令為：

${\mathrm{\ψ}}_d=arctan\left(\frac{F_{ny}}{F_{nx}}\right)-arctan\left(\frac{v}{u}\right)$

步驟9：根據步驟8的航向角指令ψ_d，采用自適應非奇異終端滑模控制方法對航向角進行跟蹤控制，選擇舵角為：

${\mathrm{\δ}}_r=-{\mathrm{\α}}_2({\mathrm{\α}}_0+{\mathrm{\α}}_1\mathrm{\ω}+\mathrm{\β}\frac{q}{p^{\′}}{\mathrm{\ω}}^{2-p^{\′}/q}+({\hat{x}}_0+\mathrm{\η}\left)sgn\right(s\left)\right)$

${\mathrm{\δ}}_{r1}=-{\mathrm{\α}}_2({\mathrm{\α}}_0+{\mathrm{\α}}_{1}w+\mathrm{\β}\frac{q}{p^{\′}}{w}^{2-p/q}+({\hat{x}}_0+\mathrm{\η}\left)\mathrm{sgn}\right(s\left)\right)$

其中，u、v、ω分別是機體坐標系下AUV的前向速度、側向速度和航向角速度，m₁₁、m₂₂和m₃₃為AUV包含附加質量的慣性系數，d₁₁、d₂₂和d₃₃為AUV的流體動力阻尼系數，N_σ為AUV偏航力矩系數，β、η都為常量且η>0，β>0，p′>q>0且為奇數，x⁰是建模不確定項上界，是x₀的估計值，q₀>0是自適應增益，s為滑模控制量；

步驟10：如果AUV與回收點的距離小于給定的可接受誤差時，則轉到下一步驟；否則返回到步驟2；

步驟11：AUV已經到達回收點，回塢結束。