[發(fā)明專利]一種全局無漂移的自主機器人同時定位與地圖構建方法

在審

• 申請?zhí)枺?/td>: 202110057910.3
• 申請日: 2021-01-15
• 公開（公告）號: CN112945233A
• 公開（公告）日: 2021-06-11
• 發(fā)明（設計）人: 趙良玉;朱葉青;金瑞
• 申請（專利權）人: 北京理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 北京康思博達知識產(chǎn)權代理事務所(普通合伙) 11426
• 代理人: 劉冬梅;范國鋒

1.一種全局無漂移的自主機器人同時定位與地圖構建方法，其特征在于，

將局部定位和全局感知定位融合，實現(xiàn)精確的全局感知定位，

所述局部定位包括采用相機、慣性測量單元、激光雷達進行狀態(tài)估計；

所述全局感知定位包括全球導航衛(wèi)星系統(tǒng)、磁力計、氣壓計進行狀態(tài)估計。

2.根據(jù)權利要求1所述的全局無漂移的自主機器人同時定位與地圖構建方法，其特征在于，

將視覺慣性與全球導航衛(wèi)星系統(tǒng)進行多源融合，所述視覺慣性包括慣性測量單元和相機。

3.根據(jù)權利要求1所述的全局無漂移的自主機器人同時定位與地圖構建方法，其特征在于，

該方法包括以下步驟：

S1、前端點線特征的跟蹤；

S2、后端視覺慣性SLAM位姿態(tài)聯(lián)合優(yōu)化；

S3、與GNSS位姿融合獲得全局位姿。

4.根據(jù)權利要求1所述的全局無漂移的自主機器人同時定位與地圖構建方法，其特征在于，

在步驟S1中，采用線性誤差將線性特征表示為直線端點上的線性約束，從而將線特征自然地整合到基于特征點算法的線性表示中。

5.根據(jù)權利要求1所述的全局無漂移的自主機器人同時定位與地圖構建方法，其特征在于，

步驟S1包含以下子步驟：

S11、建立點線特征表示觀測模型；

S12、建立IMU預積分觀測模型。

6.根據(jù)權利要求1所述的全局無漂移的自主機器人同時定位與地圖構建方法，其特征在于，

在步驟S1中，由相機的光心和p_i,k及q_i,k構成的直線可以確定一個平面，其單位法向量l_i,k表示為：

其中，l₁、l₂、l₃為參數(shù)系數(shù)，為待求解值，i表示相機不同幀圖像，在一幀圖像中通常具有多個點特征，觀測到的第j個特征點的3D坐標表示為X_w,j∈R³，第k條線段端點為P_w,k、Q_w,k∈R³，其在圖像平面的投影坐標為對應的齊次坐標為R³表示三維坐標、R²表示二維坐標；

相機的投影模型表示為π，齊次坐標表示為π^h：

其中，R_w以及t_w分別表示T_iw的旋轉和平移，T_iw表示相機第i幀的位姿；w表示世界坐標系；P_w∈R³表示世界坐標系下的點坐標，O是其對應的相機坐標系下的點坐標，f_x、f_y和c_x、c_y分別為相機的焦距和主點；

所述點線特征表示觀測模型通過點特征的重投影誤差和線特征的重投影誤差表示，分別為：

其中，上標p表示點，上標l表示線，x_i，j，y_i，j表示第i幀匹配得到的第j個特征點的像素坐標，和分別為p_i,k和q_i,k到投影直線的距離。

7.根據(jù)權利要求1所述的全局無漂移的自主機器人同時定位與地圖構建方法，其特征在于，

在步驟S12中，離散時間為k和k+1之間的IMU預積分觀測模型為：

其中，b_m表示當前幀，b_m+1表示下一幀；

表示離散形式的第m個IMU時刻到第m+1個IMU時刻積分得到的的位置、速度和旋轉的增量；表示上一時刻前后兩幀之間經(jīng)過中值積分得到的IMU增量信息；表示加速度，表示角速度。