[發明專利]一種基于智能體間相對幾何測量信息的編隊組合導航方法

在審

• 申請號: 202210198191.1
• 申請日: 2022-03-02
• 公開（公告）號: CN114578852A
• 公開（公告）日: 2022-06-03
• 發明（設計）人: 蘇文山;陳磊;白顯宗;孟志鵬
• 申請（專利權）人: 中國人民解放軍軍事科學院國防科技創新研究院
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 中國兵器工業集團公司專利中心 11011
• 代理人: 王智紅

1.一種基于智能體間相對幾何測量信息的編隊組合導航方法，其特征在于，包括如下步驟：

步驟1、結合編隊任務作業環境和類型，確定各智能體信息感知裝置和信息交互裝置；

步驟2、根據編隊剛性和編隊狀態可觀性要求，確定編隊相對幾何量需滿足的幾何約束；

假定編隊由n個智能體組成，各智能體按順序依次編號1到n，p_i記為編隊中i號智能體位置的慣導測量值(1≤i≤n)，定義p為編隊狀態矢量，由各智能體的位置矢量疊加而成；

p＝[p₁,p₂,...,p_n]^T (1)

2.1距離約束型編隊

記l_ij表示i號智能體到j號智能體間的相對位置矢量，l_ij對矢量l_ij的模，即兩智能體間的距離；

定義以各智能體間相對距離為元素，疊加形成的矢量為剛性函數F_l：

F_l＝[...,l_ij,l_ik,l_kl,....]^T (3)

為高效利用智能體間的相對距離矯正編隊慣導信息，對于距離約束型編隊應使得所設計的距離約束量集合滿足：該組合距離約束下的編隊期望構型唯一，即編隊距離剛性；若剛性函數F_l所包含距離約束對應的編隊期望構型已經唯一，則繼續增加智能體間的距離測量信息，將不會明顯改進編隊導航性能；

根據距離約束型編隊剛性知，對于平面編隊若有

則該組距離約束下的編隊構型唯一，其中rank表示矩陣的秩；

對于三維編隊若有：

則該組距離約束下的編隊構型唯一，其中rank表示矩陣的秩；

2.2方位約束型編隊

記e_ij為矢量l_ij的單位方向矢量：

e_ij＝l_ij/||l_ij|| (6)

對于方位約束型平面編隊，則該方向矢量與智能體間方位角θ_ij一一對應：

e_ij＝[cosθ_ij,sinθ_ij]^T (7)

若是三維編隊，則該方向矢量與智能體間的高低角σ_ij和方位角θ_ij一一對應：

e_ij＝[cosσ_ijcosθ_ij,cosσ_ijsinθ_ij,sinσ_ij]^T (8)

利用單位方向矢量表征智能體間的相對方位信息，定義剛性函數F_e為距離約束型編隊各智能體測得相鄰智能體距離形成的向量；

F_e＝[...,e_ij,e_ik,e_kl,....]^T (9)

同樣，方位約束型編隊應使得所設計的距離約束量集合滿足：該組合距離約束下的編隊期望構型唯一，即編隊距離剛性；若剛性函數F_e所包含方位約束對應的編隊期望構型已經唯一，則繼續增加智能體間的方位測量信息，將不會明顯改進編隊導航性能；

根據方位約束型編隊剛性知，若有

則該組方位約束下的編隊構型唯一；

步驟3、根據編隊剛性函數和測量信息，確定編隊狀態觀測方程

3.1對于距離約束型編隊

編隊狀態測量包含各智能體的慣導信息和智能體間的相對距離信息，定義編隊狀態觀測量Y：

Y＝[F_l^T,p^T]^T (11)

依據剛性函數定義知，編隊狀態觀測量Y為關于編隊狀態量的非線性函數，記p_o為編隊智能體位置信息p鄰域的狀態，則利用泰勒展開式，忽略二階以上高階項，將編隊狀態觀測量Y近似為：

根據上式，可將編隊狀態觀測矩陣H定義為：

式中I_nd表示單位對角方陣，方陣的維數為編隊智能體個數n和空間維數d的乘積；

3.2對于方位約束型編隊

若僅有剛性函數中各邊的方向以及各智能體的慣導位置信息，即測量量

Y＝[F_e^T,p^T]^T (14)

對應觀測矩陣為：

步驟4、編隊狀態測量信息和測量偏差，基于加權最小二乘法確定最優導航狀態，計算定位矯正量

假定編隊幾何測量量和各智能體慣導測得位置誤差特性均服從高斯分布，由各智能體位置、智能體間距離組合形成的測量量F_l、F_e、p、e和l對應偏差距離將均為對角方陣，依次記為R_Fl、R_Fe、R_p、R_e和R_l；

考慮到不同測量信息組合形成的對于距離約束型編隊，其包括相對距離和慣導位置測量偏差，則定義R

R＝diag(R_Fl,R_p) (16)

式中R_Fl由距離測量裝置的精度確定，R_p由各智能體慣導定位精度確定；對于方位約束型編隊，其包括相對方位和慣導位置測量偏差，則定義R

R＝diag(R_Fe,R_p) (17)

式中R_Fe由視線角測量裝置的精度確定；考慮到智能體間方位測量的直接信息為視線角，記平面編隊i號智能體到j號智能體的視線方位角測量偏差為ε_σij，三維編隊i號智能體到j號智能體的視線方位角和高低角偏差分別為ε_σij和ε_θij，則根據視線單位方向向量定義，對于平面編隊視線單位方向向量e_ij的偏差矩陣R_eij應為：

式中符號·表示矢量的點乘，diag表示以矢量元素為對對角元素的矩陣，對于三維編隊視線單位方向向量e_ij的偏差方差矩陣R_eij應為：

其中s_ij＝[σ_ij,θ_ij]^T，ε_sij＝[ε_σij,ε_θij]^T，F_e下各單位方向向量對應的R_eij構成了R_Fe對角小方塊矩陣；

根據加權最小二乘法原理知，其以實現

J＝(Y-Y(p^*))^TR(Y-Y(p^*)) (20)

最小為目的確定編隊狀態的最優估計，其中Y^*為測量值真值，p^*為狀態最優估計值；

當上式最小時應有：

即：

(Y^*-Y(p^*))^TRH(p^*)＝(Y^*-Y(p₀)-H|_p0(p^*-p₀))^TRH|_p0＝0 (22)

相應有：

p^*＝p₀+(H^T|_p0RH|_p0)^TH^T|_p0R[Y-Y(p₀)] (23)

上式即給出了迭代求解最優編隊狀態的表達式；

初始迭代時，可取編隊狀態變量的測量值作為p₀，即令p₀(1)＝p計算H^T|_p0,代入上次計算得到p^*(1)；然后將該p^*賦值給p₀，記為p₀(2)，重新計算H^T|_p0，再次代入上式得到新的p^*,記為p^*(2)；依次反復，不斷迭代更新p₀和p^*的取值，直至有

||p₀-p^*||＜ε (24)

其中ε為大于零的小量，可根據編隊任務需要的控制精度選定；

將最優估計值作為組合導航確定的編隊狀態，記Δp為組合導航對慣導信息的矯正量，即有：

Δp＝p^*-p (25)

步驟5：周期性返回步驟4，不斷矯正慣導測量信息至任務結束。