[發明專利]系繩約束下的空間三體系統多傳感器精確定位方法

有效

• 申請號: 201910493859.3
• 申請日: 2019-06-07
• 公開（公告）號: CN110196445B
• 公開（公告）日: 2022-08-16
• 發明（設計）人: 黃攀峰;方國濤;張夷齋
• 申請（專利權）人: 西北工業大學
• 主分類號: G01S19/49
• 分類號: G01S19/49
• 代理公司: 西安凱多思知識產權代理事務所(普通合伙) 61290
• 代理人: 高凌君

1.一種系繩約束下的空間三體系統多傳感器精確定位方法，其特征在于：包括以下步驟：

步驟1：構造空間三體系統的動力學模型：

建立的空間三體系統的動力學模型為

其中X＝[r₁ r₂ r₃]^T，r_i＝[x_i y_i z_i]^T(i＝1,2,3)為空間旋轉繩系衛星系統中三顆衛星質點在地心慣性坐標系中的位置矢量，μ為引力常數；所述空間旋轉繩系衛星系統由三根系繩和三顆衛星組成，并且衛星通過系繩連接起來形成閉合的平面三角形狀；

步驟2：基于細繩約束條件建立觀測模型：

根據空間三體系統在運動的過程中系繩長度保持不變，建立觀測模型為

Z＝H(X)

其中H(X)＝[(r₁-r₂)² (r₂-r₃)² (r₁-r₃)²]^T，l₁、l₂、l₃為三根系繩長度；

步驟3：根據步驟1和步驟2得到的空間三體系統的動力學模型和觀測模型，建立空間三體系統的狀態方程和觀測方程如下：

其中W(k)為過程噪聲，其為均值為0，方差為Q的高斯白噪聲；V(k+1)為觀測噪聲，其為均值為0，方差為R的高斯白噪聲；

根據空間三體系統的狀態方程和觀測方程，采用擴展卡爾曼濾波算法對空間三體系統中每個衛星上的星載全球定位系統接收機的數據進行融合，實現對空間三體系統中的三個衛星位置精確定位，具體步驟為：

步驟3.1：初始化初始狀態X(0)、Z(0)、協方差矩陣P(0)；

步驟3.2：確定狀態方程

步驟3.3：確定觀測方程Z(k+1)＝H(X(k+1))+V(k+1)；

步驟3.4：一階線性化狀態方程，將一階線性化處理后，得到X(k+1|k)＝f[k,X(k)]+W(k)，求解狀態轉移矩陣

步驟3.5：一階線性化觀測方程，將Z(k+1)＝H(X(k+1))+V(k+1)一階線性化處理后，得到Z(k+1)＝h[k+1,X(k+1)]+V(k+1)，求解觀測矩陣

步驟3.6：求協方差矩陣P(k+1|k)＝Φ(k+1)P(k|k)Φ^T(k+1)+Q；

步驟3.7：求卡爾曼濾波增益K(k+1)＝P(k+1|k)Θ^T(k)/[Θ(k)P(k+1|k)Θ^T(k)+R]；

步驟3.8：求狀態更新X(k+1)＝X(k+1|k)+K(k+1)[Z(k+1)-Z(k)]；

步驟3.9：協方差更新P(k+1)＝[I_n-K(k+1)Θ(k+1)]P(k+1|k)，其中矩陣I_n為單位矩陣。