[發明專利]一種單領航多AUV協同定位及軌跡跟蹤控制方法

在審

• 申請號: 202210279687.1
• 申請日: 2022-03-21
• 公開（公告）號: CN114840003A
• 公開（公告）日: 2022-08-02
• 發明（設計）人: 徐博;沈浩;趙玉新;楊澤瓊;王朝陽;吳磊;王權達
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G05D1/06
• 分類號: G05D1/06
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種單領航多AUV協同定位及軌跡跟蹤控制方法，其特征在于，包括：

步驟一：建立跟蹤器運動學模型、領航器運動學模型以及兩者之間距離量測的模型；

步驟二：根據步驟一建立的模型通過擴展卡爾曼濾波算法對領航器狀態進行估計；

步驟三：根據可觀測性進行協同軌跡規劃，確定跟蹤器理想跟蹤軌跡；

步驟四：根據步驟二以及步驟三中的目標狀態以及跟蹤軌跡，設計控制器，完成對目標的跟蹤。

2.根據權利要求1所述的一種單領航多AUV協同定位及軌跡跟蹤控制方法，其特征在于：步驟一具體為：

建立跟蹤器運動學模型：

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

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

當跟蹤器運動為二維運動時：

建立領航器運動學模型：

將領航器看作被跟蹤的目標，則代表目標的軌跡，代表慣性系下目標的速度向量，假設目標在緩慢的改變速度，其運動模型采用準穩態模型的形式描述：

其中代表目標的狀態，其中υ(t)是緩慢變化，w～N(0,Q_t)為零均值高斯噪聲，協方差為Q_t，

代表著k時刻的目標狀態，進行離散化得：

x_k+1＝Fx_k+w_k

其中，當跟蹤器運動為三維運動時，當跟蹤器運動為二維運動時T_s是采樣時間，w～N(0,Q)，

建立距離量測模型：

表示跟蹤器和目標之間在k時刻的真實距離其中為跟蹤器i在k時刻所處位置，跟蹤器與目標之間的量測模型為：

其中，為高斯量測噪聲。

3.根據權利要求2所述的一種單領航多AUV協同定位及軌跡跟蹤控制方法，其特征在于，步驟二具體為：

假設對目標狀態x的估計為：

其中，為對目標真實位置q的估計，為對目標真實速度υ的估計；

令代表在時間k之前跟蹤器到目標的距離的集合，系統估計的量測向量和量測矩陣z,Ω分別為：

根據擴展卡爾曼濾波進行估計，其基本方程為：

在k＝0時初始化可得：

距離量測估計為：

量測向量估計為：

量測向量更新為：

狀態一步預測為：

量測向量，量測矩陣一步預測為：

其中，為k時刻對距離量測的估計，F為系統狀態方程的雅可比矩陣；

進而獲得k時刻對目標狀態估計的先驗和后驗密度，即和其中，x_k為第k時刻目標狀態，為k時刻估計狀態，為根據k-1時刻的狀態估計對k時刻狀態進行預測得到的結果,P_k|k-1為根據k-1時刻的均方誤差陣進行預測得到k時刻的均方誤差矩陣。