[發明專利]基于卡爾曼濾波的無人機定位方法及裝置

有效

• 申請號: 202110943418.6
• 申請日: 2021-08-17
• 公開（公告）號: CN113390421B
• 公開（公告）日: 2021-12-10
• 發明（設計）人: 王安睿;董佳霖;崔迅;石兵
• 申請（專利權）人: 武漢理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06F17/16
• 代理公司: 武漢智嘉聯合知識產權代理事務所(普通合伙) 42231
• 代理人: 黃君軍

1.一種基于卡爾曼濾波的無人機定位方法，其特征在于，包括：

基于預設采樣方法對無人機的當前位置值進行采樣，獲得多個采樣點；

基于所述多個采樣點，確定所述無人機下一時刻的先驗位置值和先驗協方差矩陣；

根據當前時刻的當前滑動窗口長度確定所述下一時刻的下一滑動窗口長度；

根據所述下一滑動窗口長度確定所述下一時刻的估計系統噪聲協方差矩陣和估計量測噪聲協方差矩陣；

獲取下一時刻的觀測位置值，并根據所述觀測位置值、所述估計系統噪聲協方差矩陣、所述估計量測噪聲協方差矩陣、所述先驗位置值以及所述先驗協方差矩陣確定所述下一時刻的后驗位置值和后驗協方差矩陣，以對無人機定位；

所述下一滑動窗口長度為：

式中，N_k+1為所述下一滑動窗口長度；N_k為所述當前滑動窗口長度；L為量測維度；η_k為當前殘差序列；η_k(j)為當前殘差序列中第j個元素；為歷史平均殘差序列中第j個元素；Z_k為當前觀測值；為所述當前位置值；h_k( )為量測位置值與觀測位置值之間的映射關系；||表示取整。

2.根據權利要求1所述的基于卡爾曼濾波的無人機定位方法，其特征在于，所述基于所述多個采樣點，確定所述無人機下一時刻的先驗位置值和先驗協方差矩陣包括：

確定所述多個采樣點的一階統計權系數和二階統計權系數；

確定所述多個采樣點的多個一步預測采樣點；

根據所述一階統計權系數、所述二階統計權系數以及所述多個一步預測采樣點確定所述先驗位置值和所述先驗協方差矩陣。

3.根據權利要求2所述的基于卡爾曼濾波的無人機定位方法，其特征在于，所述根據所述當前時刻的當前滑動窗口長度確定所述下一時刻的下一滑動窗口長度包括：

獲取當前時刻的當前觀測值；

根據所述當前觀測值以及所述當前位置值確定所述當前時刻的當前殘差序列；

確定所有歷史時刻的歷史平均殘差序列；

根據所述當前滑動窗口長度、所述當前殘差序列以及所述歷史平均殘差序列確定所述下一滑動窗口長度。

4.根據權利要求3所述的基于卡爾曼濾波的無人機定位方法，其特征在于，所述根據所述觀測位置值、所述估計系統噪聲協方差矩陣、所述估計量測噪聲協方差矩陣、所述先驗位置值以及所述先驗協方差矩陣確定所述下一時刻的后驗位置值和后驗協方差矩陣包括：

根據所述估計系統噪聲協方差矩陣以及所述估計量測噪聲協方差矩陣確定觀測協方差矩陣和量測協方差矩陣；

根據所述觀測協方差矩陣以及所述量測協方差矩陣確定卡爾曼增益矩陣；

根據所述先驗位置值、所述觀測位置值和所述卡爾曼增益矩陣確定所述后驗位置值；

根據所述先驗協方差矩陣、所述卡爾曼增益矩陣以及所述觀測協方差矩陣確定所述后驗協方差矩陣。

5.根據權利要求4所述的基于卡爾曼濾波的無人機定位方法，其特征在于，所述采樣點為：

式中，ζ_i，k為所述采樣點；為所述先驗位置值；n為狀態方程的維度；κ為所述采樣點與所述當前位置值距離的參量；P_k為第k時刻的協方差矩陣。

6.根據權利要求5所述的基于卡爾曼濾波的無人機定位方法，其特征在于，所述先驗位置值和所述先驗協方差矩陣分別為：

ζ_i，k+1/k＝f(ζ_i，k)，i＝0，1，...，2n

式中，為所述先驗位置值；P_k+1/k為所述先驗協方差矩陣；為所述一階統計權系數；為所述二階統計權系數；ζ_i，k+1/k為所述一步預測采樣點；f(ζ_i，k)為所述采樣點的一步預測函數；W_k為第一白噪聲序列；Q_k為第k時刻的當前量測噪聲協方差矩陣。