[發(fā)明專利]一種單領(lǐng)航多AUV協(xié)同定位及軌跡跟蹤控制方法

在審

• 申請?zhí)枺?/td>: 202210279687.1
• 申請日: 2022-03-21
• 公開（公告）號: CN114840003A
• 公開（公告）日: 2022-08-02
• 發(fā)明（設(shè)計）人: 徐博;沈浩;趙玉新;楊澤瓊;王朝陽;吳磊;王權(quán)達(dá)
• 申請（專利權(quán)）人: 哈爾濱工程大學(xué)
• 主分類號: G05D1/06
• 分類號: G05D1/06
• 代理公司: 暫無信息
• 代理人: 暫無信息

本發(fā)明公開了一種單領(lǐng)航多AUV協(xié)同定位及軌跡跟蹤控制方法，步驟一：建立跟蹤器運(yùn)動學(xué)模型、領(lǐng)航器運(yùn)動學(xué)模型以及兩者之間距離量測的模型；步驟二：根據(jù)步驟一建立的模型通過擴(kuò)展卡爾曼濾波算法對領(lǐng)航器狀態(tài)進(jìn)行估計；步驟三：根據(jù)可觀測性進(jìn)行協(xié)同軌跡規(guī)劃，確定跟蹤器理想跟蹤軌跡；步驟四：根據(jù)步驟二以及步驟三中的目標(biāo)狀態(tài)以及跟蹤軌跡，設(shè)計控制器，完成對目標(biāo)的跟蹤。本發(fā)明所提出的方法具有很強(qiáng)的魯棒性和有效性。對不同的多個跟蹤器均有很好的效果。

技術(shù)領(lǐng)域

本發(fā)明屬于水下多航行器協(xié)同定位及軌跡跟蹤控制領(lǐng)域，涉及一種單領(lǐng)航多AUV協(xié)同定位及軌跡跟蹤控制方法，特別是一種基于擴(kuò)展卡爾曼濾波的單領(lǐng)航多AUV協(xié)同定位及軌跡跟蹤控制方法。

背景技術(shù)

基于距離的同步目標(biāo)定位與追蹤由于其在海洋科學(xué)領(lǐng)域的大量應(yīng)用的重要性，近年來受到了廣泛的關(guān)注。在執(zhí)行任務(wù)中，跟蹤器配備了測量目標(biāo)距離的聲學(xué)設(shè)備，并利用這些信息估計目標(biāo)的狀態(tài)(定位任務(wù))，同時在預(yù)定目標(biāo)附近保持所需的相對幾何編隊，以獲取目標(biāo)的最大距離信息用于估計(追蹤任務(wù))。由于低功率聲學(xué)設(shè)備所能測量的最大量程有限制，所以追蹤任務(wù)在海洋環(huán)境中具有極大的相關(guān)性。主要問題與定位任務(wù)有關(guān)。除了設(shè)計一個合適的目標(biāo)狀態(tài)估計器外，關(guān)鍵問題是如何規(guī)劃跟蹤器的運(yùn)動，以便通過適當(dāng)?shù)臋C(jī)動使跟蹤器到目標(biāo)的范圍提供足夠的信息來估計目標(biāo)的狀態(tài)。從技術(shù)上講，這個問題的答案必須提供跟蹤器運(yùn)動的條件，使獲得的距離產(chǎn)生目標(biāo)運(yùn)動的可觀測性。第二個問題與跟蹤任務(wù)有關(guān)，即如何控制水下航行器，使它們收斂到目標(biāo)附近，同時保持一個所需的相對幾何編隊。

發(fā)明內(nèi)容

針對上述現(xiàn)有技術(shù)，本發(fā)明要解決的技術(shù)問題是提供一種基于擴(kuò)展卡爾曼濾波的單領(lǐng)航多AUV協(xié)同定位及軌跡跟蹤控制方法，具有很強(qiáng)的魯棒性和有效性。

為解決上述技術(shù)問題，本發(fā)明一種單領(lǐng)航多AUV協(xié)同定位及軌跡跟蹤控制方法，包括：

步驟一：建立跟蹤器運(yùn)動學(xué)模型、領(lǐng)航器運(yùn)動學(xué)模型以及兩者之間距離量測的模型；

步驟二：根據(jù)步驟一建立的模型通過擴(kuò)展卡爾曼濾波算法對領(lǐng)航器狀態(tài)進(jìn)行估計；

步驟三：根據(jù)可觀測性進(jìn)行協(xié)同軌跡規(guī)劃，確定跟蹤器理想跟蹤軌跡；

步驟四：根據(jù)步驟二以及步驟三中的目標(biāo)狀態(tài)以及跟蹤軌跡，設(shè)計控制器，完成對目標(biāo)的跟蹤。

進(jìn)一步的，步驟一具體為：

建立跟蹤器運(yùn)動學(xué)模型：

假設(shè)有N個跟蹤器(N≥1)，{I}＝{x_I,y_I,z_I}為慣性坐標(biāo)系，為第i個跟蹤器的自身坐標(biāo)系，i∈N；代表慣性坐標(biāo)系中第i個跟蹤器的坐標(biāo)；表示第i個跟蹤器在自身坐標(biāo)系中的速度向量，則跟蹤器的運(yùn)動學(xué)模型是：

其中，R(η^[i])是從自身坐標(biāo)系到慣性系的轉(zhuǎn)換矩陣，其中向量是由滾轉(zhuǎn)角、俯仰角、偏航角組成的向量，轉(zhuǎn)換矩陣的導(dǎo)數(shù)滿足：其中ω^[i]＝[p^[i],q^[i],r^[i]]^T為自身固定的角速度向量，p^[i],q^[i],r^[i]分別為x、y、z方向?qū)?yīng)的角速度，S是一個矩陣，當(dāng)跟蹤器運(yùn)動為三維運(yùn)動時，定義為：

當(dāng)跟蹤器運(yùn)動為二維運(yùn)動時：

建立領(lǐng)航器運(yùn)動學(xué)模型：

將領(lǐng)航器看作被跟蹤的目標(biāo)，則代表目標(biāo)的軌跡，代表慣性系下目標(biāo)的速度向量，假設(shè)目標(biāo)在緩慢的改變速度，其運(yùn)動模型采用準(zhǔn)穩(wěn)態(tài)模型的形式描述：