[發明專利]一種用輪式里程計-IMU和單目相機實現定位的方法

有效

• 申請號: 202011641098.0
• 申請日: 2020-12-31
• 公開（公告）號: CN112734841B
• 公開（公告）日: 2023-04-28
• 發明（設計）人: 宋高高;哀薇;熊彪
• 申請（專利權）人: 華南理工大學
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T7/80;G06T5/00;G06V10/75;G06V10/762;G06V10/764;G06V10/774;G06F16/29;G01C21/16
• 代理公司: 廣州粵高專利商標代理有限公司 44102
• 代理人: 何淑珍;江裕強

1.一種用輪式里程計-IMU和單目相機實現定位的方法，其特征在于，包括以下步驟：

(1)標定單目相機的內參矩陣、單目相機和車體之間的外參、單目相機和IMU之間的外參；

(2)對單目相機的圖像進行去畸變后，對圖像提取ORB特征點，找到對應的最優匹配特征點后，執行單目SLAM初始化操作，選擇初始化成功時的當前幀的位姿作為世界坐標系的位姿；

(3)根據輪式里程計和IMU測到的方向角預測車體在某一時刻的位姿；

(4)根據位姿將地圖點投影到圖像上，進行特征跟蹤和匹配；

(5)通過三角化得到地圖點的三維坐標，同時插入關鍵幀，從而維護一個局部地圖；

(6)利用相機的圖像特征點構建重投影誤差最小的優化問題，對局部地圖中的地圖點和所有位姿進行光束平差法(bundle?adjustment，BA)優化，得到優化后的位姿，用優化后的位姿去更新車體位姿；

(7)采用回環檢測和回環校正，得到全局一致的車體位姿。

2.根據權利要求1所述的一種用輪式里程計-IMU和單目相機實現定位的方法，其特征在于，所述步驟(1)中，車體坐標系(base?frame)位于車體的正中心，世界坐標系(worldframe)固定在一個位置，相機坐標系(camera?frame)位于相機中心，用表示車體坐標系B到世界坐標系W的旋轉矩陣，表示車體坐標系B到世界坐標系W的平移向量，則T_WB表示從車體坐標系B到世界坐標系W之間的變換矩陣，用r表示車體在地面上的世界坐標(x,y)，Φ表示方向角，則有

其中，p_x、p_y表示取三維向量的x、y部分,表示將矩陣變換為李代數，即旋轉向量，表示取旋轉向量在z軸部分的分量；

路標在世界坐標系中的坐標l轉換為相機坐標系下的坐標l_C的公式如下：

其中，為世界坐標系W到相機坐標系C的旋轉矩陣，為世界坐標系W到相機坐標系C的平移向量，l為路標在世界坐標系W中的坐標，l_C為路標在相機坐標系C下的坐標；

通過外參標定，得到相機坐標系C到車體坐標系B之間的旋轉矩陣和平移向量用于后續的優化過程。