[發明專利]基于多傳感器融合的無人機位姿估計方法

有效

• 申請號: 202110700271.8
• 申請日: 2021-06-23
• 公開（公告）號: CN113432602B
• 公開（公告）日: 2022-12-02
• 發明（設計）人: 羅楠;王義峰;霍振鋒;李晟碩;梁婕;盧波;杜欣蔚
• 申請（專利權）人: 西安電子科技大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01S19/49
• 代理公司: 陜西電子工業專利中心 61205
• 代理人: 陳宏社;王品華

1.一種基于多傳感器融合的無人機位姿估計方法，其特征在于，包括如下步驟：

(1)獲取多傳感器數據：

(1a)獲取無人機攜帶的視覺傳感器以10Hz-30Hz的頻率采集M幅不同的地面灰度圖像X＝{X_m|1≤m≤M}，IMU傳感器以100Hz-200Hz的頻率采集N組不同的無人機加速度數據A＝{A_n|1≤n≤N}和角速度數據W＝{W_n|1≤n≤N}，以及GPS傳感器以20Hz-100Hz的頻率采集不同的Z組無人機位置數據G＝{(O_z,L_z,H_z)|1≤z≤Z},其中M≥20，X_m表示第m幅地面灰度圖像，N≥200，A_n、W_n分別表示t時刻無人機的加速度、角速度，Z≥40，O_z、L_z、H_z分別表示t時刻無人機的經度值、緯度值、高度值；

(2)計算每個特征點F_c的視覺殘差：

對每幅地面灰度圖像X_m進行特征點提取，得到X對應的特征點集合X'＝{X_m'|1≤m≤M}，并計算特征點集合X_m'中每個特征點F_c的視覺殘差E_c，其中X_m'表示X_m對應的包括C個特征點的特征點子集合F＝{F_c|1≤c≤C}，C≥30；

(3)計算每幅圖像與相鄰圖像對應無人機位置之間的IMU殘差；

采用預積分方法，通過t時刻無人機的加速度A_n和角速度W_n計算相鄰地面灰度圖像X_m及相鄰圖像X_m+1之間的位移增量P_(m,m+1)、速度增量V_(m,m+1)和旋轉增量Q_(m,m+1)，并通過P_(m,m+1)、V_(m,m+1)和Q_(m,m+1)，計算X_m與X_m+1對應無人機位置之間的IMU殘差E_(m,m+1)；

(4)計算每幅圖像對應無人機位置的GPS殘差E_z：

將GPS采集的Z組無人機位置數據G轉換為東北天坐標系下的坐標G'＝{(O_z',L_z',H_z')|1≤z≤Z},并計算每幅圖像X_m在G_z'下的對應無人機位置的GPS殘差E_z；

(5)獲取無人機位姿估計結果：

通過每個特征點F_c的視覺殘差E_c、每幅圖像X_m及相鄰圖像X_m+1對應無人機位置之間的IMU殘差E_(m,m+1)和每幅圖像X_m對應無人機位置的GPS殘差E_z，構建多傳感器殘差E_tight，并采用非線性優化方法對E_tight進行優化，得到t時刻無人機的位姿，其中：

E_tight＝E_c+W_(m,m+1)·E_(m,m+1)+W_z·E_z

其中，W_(m,m+1)表示圖像X_m及相鄰圖像X_m+1對應無人機位置之間的IMU殘差E_(m,m+1)的權重，W_z表示每幅圖像X_m對應無人機位置的GPS殘差E_z的權重。