[發明專利]一種基于雷達和IMU的姿態動態估計方法及系統

在審

• 申請號: 202110463086.1
• 申請日: 2021-04-28
• 公開（公告）號: CN113175927A
• 公開（公告）日: 2021-07-27
• 發明（設計）人: 不公告發明人
• 申請（專利權）人: 北京中科深智科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01S13/86;G06N3/04;G06T7/277
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于雷達和IMU的姿態動態估計方法，其特征在于，具體步驟包括：

1)從雷達掃描得到的雷達點云數據中提取地標信息；

2)根據IMU數據估計雷達運載體的相對動作；

3)對所述地標信息和所述雷達運載體的相對動作信息進行信息點掃描匹配，得到所述雷達運載體的運動姿態；

4)對步驟3)匹配得到的所述雷達運載體的運動姿態進行姿態回歸計算，并根據姿態回歸計算結果預測得到所述雷達運載體的六自由度運動姿態。

2.根據權利要求1所述的一種基于雷達和IMU的姿態動態估計方法，其特征在于，步驟3)中，采用NDT正態分布變換算法對所述地標信息和所述移動雷達的相對動作信息進行信息點掃描匹配。

3.根據權利要求1所述的一種基于雷達和IMU的姿態動態估計方法，其特征在于，步驟4)中，采用UKF無跡卡爾曼濾波算法對移動雷達運動姿態進行姿態回歸計算。

4.根據權利要求1所述的一種基于雷達和IMU的姿態動態估計方法，其特征在于，步驟4)中，對移動雷達運動姿態進行姿態回歸計算的過程可通過以下表達式表達：

上式中，p_t-1為所述雷達運載體在當前t時刻的上一時刻的所處位置；

f(X_t-1)為預測所述雷達運載體運動位置的第一轉移方程；

q_t-1為表征所述雷達運載體在當前t時刻的上一時刻的旋轉狀態的旋轉四元數；

Δq_t是所述雷達運載體在t-1時刻到當前t時刻的旋轉變化量；

v_t-1表示所述雷達運載體在當前t時刻的上一時刻的運動速度；

是所述雷達運載體在當前t時刻的上一時刻的角速度的偏置。

5.根據權利要求4所述的一種基于雷達和IMU的姿態動態估計方法，其特征在于，所述第一轉移方程使用RNN神經網絡進行建模。

6.根據權利要求4所述的一種基于雷達和IMU的姿態動態估計方法，其特征在于，步驟4)中，預測所述雷達運載體的六自由度運動姿態的第二轉移方程使用LSTM長短期記憶人工神經網絡進行建模。

7.根據權利要求6所述的一種基于雷達和IMU的姿態動態估計方法，其特征在于，用于對所述第二轉移方程進行建模的LSTM神經網絡的內部預測過程可通過以下公式(1)和公式(2)表達：

C_k＝f_k⊙C_k-1+i_k⊙g_k 公式(1)

公式(1)中，C_k用于表示記憶細胞是通過遺忘門和輸入門來控制信息的流動；

f_k用于表示遺忘門；

C_k-1用于表示上一個時刻的記憶細胞；

i_k用于表示輸入門；

g_k用于表示候選記憶細胞；

h_k＝o_K⊙tanh(C_k) 公式(2)

公式(2)中，h_k表示當前時刻的隱藏狀態；

o_K表示輸出門；

tanh是激活函數，值域在[-1,1]。