[發明專利]基于雙目視覺慣性里程計估計位姿的方法、裝置與處理器

在審

• 申請號: 202010144303.6
• 申請日: 2020-03-04
• 公開（公告）號: CN111220155A
• 公開（公告）日: 2020-06-02
• 發明（設計）人: 袁國斌;陶永康;賈寧
• 申請（專利權）人: 廣東博智林機器人有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/00;G01C1/00;G01C11/04
• 代理公司: 北京康信知識產權代理有限責任公司 11240
• 代理人: 霍文娟

1.一種基于雙目視覺慣性里程計估計位姿的方法，其特征在于，包括：

獲取第一姿態角、第二姿態角和位移，所述第一姿態角為通過慣性測量單元獲取的載體的姿態角，從雙目視覺里程計獲取的對應于所述載體的多幀圖像中獲取所述第二姿態角和所述位移；

獲取所述第一姿態角和所述第二姿態角的加權平均值，所述加權平均值為所述載體的融合姿態角；

根據所述融合姿態角更新所述位移。

2.根據權利要求1所述的方法，其特征在于，所述雙目視覺里程計包括雙目相機，在獲取第一姿態角、第二姿態角和位移之前，所述方法還包括：

初始化所述雙目相機的參數和所述慣性測量單元的參數；

采集所述雙目相機的左圖像和右圖像，采集所述慣性測量單元的原始輸出；

初始化所述雙目相機采集的圖像的特征點。

3.根據權利要求2所述的方法，其特征在于，初始化所述雙目相機采集的圖像的特征點，包括：

獲取所述雙目相機采集的第一幀圖像的特征點和特征點深度值。

4.根據權利要求3所述的方法，其特征在于，獲取所述雙目相機采集的第一幀圖像的特征點和特征點深度值，包括：

提取所述雙目相機采集到的第一幀圖像的所述左圖像的第一特征點并保存所述第一特征點；

在所述第一幀圖像的所述右圖像上跟蹤所述左圖像的所述第一特征點，獲取第二特征點，并保存所述第二特征點，所述第一特征點和所述第二特征點構成特征點對；

對所述特征點對進行畸變矯正；

采用矯正后的所述特征點對，計算所述特征點對的所述特征點深度值。

5.根據權利要求4所述的方法，其特征在于，在初始化所述雙目相機采集的圖像的特征點之后，獲取第一姿態角包括：

獲取第N幀圖像和第N+1幀圖像，N為大于1的整數；

獲取第一時刻和第二時刻，所述第一時刻為獲取所述第N幀圖像的時刻，所述第二時刻為獲取所述第N+1幀圖像的所述時刻；

獲取處于第三時刻的所述第一姿態角，所述第三時刻在所述第一時刻和所述第二時刻之間。

6.根據權利要求5所述的方法，其特征在于，在初始化所述雙目相機采集的圖像的特征點之后，獲取第二姿態角和位移包括：

獲取第三特征點，所述第三特征點為所述第N幀圖像的左圖像上的特征點；

在所述第N+1幀圖像的左圖像上跟蹤所述第三特征點，獲取第四特征點，所述第四特征點為所述第N+1幀圖像的左圖像上的特征點；

在所述第三特征點的數量大于第一預定值的情況下，對所述第三特征點和所述第四特征點進行畸變矯正；

采用矯正后的所述第三特征點和所述第四特征點，獲取所述第二姿態角和所述位移。

7.根據權利要求6所述的方法，其特征在于，位姿包括所述第二姿態角和所述位移，獲取所述第二姿態角和所述位移，還包括：

比較所述第N+1幀圖像的所述位姿與所述第N幀圖像的所述位姿，得到比較結果，N為大于1的整數；

根據所述比較結果檢測是否有異常值；

在有所述異常值的情況下，使用所述第N幀圖像的所述位姿替換所述第N+1幀圖像的所述位姿，且刪除所述第N+1幀圖像的所述位姿，并重新跟蹤第N+M幀圖像，M為大于等于2的整數；

在沒有所述異常值的情況下，根據第N+M幀圖像的所述位姿獲取所述第二姿態角和所述位移。

8.根據權利要求5所述的方法，其特征在于，獲取第一姿態角，還包括：

設置時間閾值；

在采集所述第一姿態角的時間達到所述時間閾值的情況下，采用上一次獲取的所述第一姿態角作為慣性測量單元姿態更新的初值；

根據所述初值更新所述第一姿態角。

9.根據權利要求1所述的方法，其特征在于，根據所述融合姿態角更新所述位移，包括：

根據所述融合姿態角，在固定姿態矩陣的情況下，利用預定數量的2D-3D點對更新所述位移。