[發明專利]拒止環境下的多無人機自身定位與目標定位方法

在審

• 申請號: 202110903143.3
• 申請日: 2021-08-06
• 公開（公告）號: CN113721188A
• 公開（公告）日: 2021-11-30
• 發明（設計）人: 張福彪;楊希雯;林德福;王亞凱;陳祺;周天澤
• 申請（專利權）人: 北京理工大學
• 主分類號: G01S5/02
• 分類號: G01S5/02;G01S5/00
• 代理公司: 北京康思博達知識產權代理事務所(普通合伙) 11426
• 代理人: 劉冬梅;范國鋒

1.一種拒止環境下的多無人機自身定位與目標定位方法，其特征在于，該方法中：

設置至少兩個觀測無人機、至少一個中轉無人機和一個已知位置的基站，

所述觀測無人機用于探測獲得：每個觀測無人機和目標之間的距離、任意兩個觀測無人機彼此之間的距離；

所述中轉無人機用于探測獲得：中轉無人機和每個觀測無人機之間的距離；

所述基站用于探測獲得：基站與中轉無人機之間的距離、基站與每個觀測無人機之間的距離、基站與中轉無人機之間的視線角；

該方法包括如下步驟：

步驟1，通過觀測無人機、中轉無人機和基站探測得到觀測向量；

步驟2，用卡爾曼濾波算法對觀測向量做融合處理，得到狀態向量

步驟3，獲得觀測無人機和中轉無人機的航向角，并根據航向角分別控制觀測無人機和中轉無人機到達新的航點；

步驟4，重復上述步驟1、步驟2和步驟3，直至狀態估計的誤差方差矩陣的對角線元素之和減小到設定閾值。

2.根據權利要求1所述的拒止環境下的多無人機自身定位與目標定位方法，其特征在于，

在步驟3中，在k時刻預測第k+n時刻的狀態估計誤差方差矩陣P_k+n的逆矩陣，即信息矩陣J_k+n，求解使得信息矩陣J_k+n最大化的觀測無人機和中轉無人機的航向角。

3.根據權利要求2所述的拒止環境下的多無人機自身定位與目標定位方法，其特征在于，

在步驟3中，通過下式(一)獲得觀測無人機和中轉無人機的航向角；

其中.表示包含三個航向角的向量；

J_k+n表示信息矩陣，通過下式(二)獲得：

其中，所述P₀表示卡爾曼濾波算法中狀態估計誤差方差矩陣的初值；

所述I_i表示i時刻的觀測向量中與待估計的狀態向量相關的信息量，通過下式(三)獲得：

其中，H_i為觀測向量對狀態向量求偏導得到的雅可比矩陣，通過下式(四)獲得：

所述觀測向量包括每個觀測無人機和目標之間的距離、任意兩個觀測無人機彼此之間的距離、中轉無人機和每個觀測無人機之間的距離、基站與中轉無人機之間的距離、基站與每個觀測無人機之間的距離、基站與中轉無人機之間的視線角；

所述狀態向量包括目標的位置、中轉無人機的位置和每個觀測無人機的位置；

所述X_i表示第i時刻的狀態向量，包含無人機和目標的位置信息；

所述X_b表示基站位置；

所述R表示觀測傳感器的噪聲方差矩陣。

4.根據權利要求1所述的拒止環境下的多無人機自身定位與目標定位方法，其特征在于，

在步驟4中，所述狀態估計的誤差方差P通過下式(五)獲得：

其中，P_k表示k時刻的狀態估計的誤差方差，P^-_k表示預測的k時刻狀態誤差方差矩陣；

A表示系統狀態轉移矩陣，Q表示狀態誤差方差矩陣，表示預測的k-1時刻狀態誤差方差矩陣；

K_k表示卡爾曼增益：

5.根據權利要求1所述的拒止環境下的多無人機自身定位與目標定位方法，其特征在于，

在步驟4中，所述設定閾值為50～80。