[發明專利]移動機器人的位姿估計方法及計算機可讀存儲介質

在審

• 申請號: 202110003763.1
• 申請日: 2021-01-04
• 公開（公告）號: CN112815939A
• 公開（公告）日: 2021-05-18
• 發明（設計）人: 張凱;黃鑫;董宇涵
• 申請（專利權）人: 清華大學深圳國際研究生院
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 深圳新創友知識產權代理有限公司 44223
• 代理人: 孟學英

1.一種移動機器人的位姿估計方法，其特征在于，包括如下步驟：

S1：獲取多源傳感器數據，所述多源傳感器數據包括：圖像采集設備采集地場景圖像、慣性測量單元采集地數據、輪式編碼器采集地數據；

S2：根據所述場景圖像檢測場景動態將所述移動機器人的位姿估計初始化；

S3：所述慣性測量單元采集地數據、所述輪式編碼器采集地數據通過狀態轉移方程計算所述移動機器人當前時刻狀態向量的先驗估計量及對應的協方差傳遞矩陣；

S4：根據所述場景圖像對所述當前時刻狀態向量的先驗估計量及對應的協方差矩陣迭代至收斂完成濾波更新。

2.如權利要求1所述的移動機器人的位姿估計方法，其特征在于，還包括如下步驟：

S5：通過滑動窗口優化校準得到具有一致性的所述移動機器人的位姿估計。

3.如權利要求1所述的移動機器人的位姿估計方法，其特征在于，圖像采集設備采集地場景圖像包括當前時刻的圖像幀；

慣性測量單元采集地數據包括角速度和加速度；

輪式編碼器采集地數據包括所述移動機器人的左、右兩輪的行進速度。

4.如權利要求2所述的移動機器人的位姿估計方法，其特征在于，根據所述場景圖像檢測場景動態將所述移動機器人的位姿估計初始化包括：

根據所述場景圖像的連續圖像幀進行角點檢測估計匹配點對的運動狀態進而確定所述移動機器人的狀態；

若所述移動機器人處于運動狀態，則通過所述圖像采集設備、所述慣性測量單元、所述輪式編碼器進行狀態估計完成所述移動機器人的位姿估計初始化；

若所述移動機器人處于靜止狀態，則通過所述慣性測量單元完成所述移動機器人的位姿估計初始化。

5.如權利要求4所述的移動機器人的位姿估計方法，其特征在于，所述慣性測量單元包括陀螺儀和加速度計，通過所述慣性測量單元完成所述移動機器人的位姿估計初始化包括如下步驟：

將陀螺儀偏差bias b_gk設定為所述陀螺儀的靜態數據，加速度計偏差bias b_ak、b_ek設為0：

b_gk＝mean(arry_gyro)

b_ak＝0

b_ek＝0

其中，mean(arry_gyro)表示陀螺儀靜態測量數據的均值；

根據加速度計靜態數據和重力向量計算從機體坐標系{B_k}到世界坐標系{W}的旋轉四元數

g_b＝mean(arry_gyro)

g_w＝(0 0 -9.8)^T

其中，mean(arry_gyro)表示加速度計靜態測量數據的均值，g_w表示世界坐標系{W}下的重力向量，運算符⊙表示向量做差后的四元數轉化；

將所述狀態向量中的其余量均設為0，完成初始化。