[發明專利]一種基于視覺引導激光重定位的移動機器人定位方法

有效

• 申請號: 202010119797.2
• 申請日: 2020-02-26
• 公開（公告）號: CN111337943B
• 公開（公告）日: 2022-04-05
• 發明（設計）人: 劉成菊;陳浩;陳啟軍
• 申請（專利權）人: 同濟大學
• 主分類號: G01S17/88
• 分類號: G01S17/88;G01S17/66;G01S17/06
• 代理公司: 上海科盛知識產權代理有限公司 31225
• 代理人: 陳源源

1.一種基于視覺引導激光重定位的移動機器人定位方法，其特征在于，包括以下步驟：

S1、根據視覺特征地圖對機器人的位置進行初始化定位，并將其映射到激光地圖上；

S2、采用自適應粒子濾波方法，根據激光掃描匹配的結果獲取機器人在激光地圖上的精準定位；自適應粒子濾波的方法具體包括以下步驟：

S21、獲取里程計和激光傳感器的數據；

S22、通過粒子分布設定機器人位置的后驗概率分布，設t時刻的N個粒子為其中i＝1～N，初始化粒子使其滿足機器人在激光地圖中的初始位置；

S23、根據里程計移動模型和激光傳感器模型更新粒子

S24、計算粒子的權重并且按權重進行自適應重采樣；

S25、輸出粒子的加權值，作為機器人的當前位姿；

所述的里程計移動模型的表達式為：

其中，p_rot1為在機器人位置在世界坐標系下的旋轉誤差分布，p_rot2為機器人的自身旋轉誤差分布，p_trans為機器人移動的位移誤差分布，u_t為里程計提供的數據；

所述的激光傳感器模型的表達式為：

其中，z_hit、z_rand、z_max為權重，p_hit為測量誤差，p_rand為觀測隨機噪聲，p_max為最大測量距離似然，z_t為激光傳感器提供的數據；

S3、判斷在自適應粒子濾波方法的定位過程中定位的粒子方差是否超過設定的閾值，若是，則執行步驟S4；若否，則執行步驟S5；

S4、利用視覺特征地圖進行視覺重定位，輸出機器人的定位結果，并對當前的粒子進行重新初始化，即錯誤恢復；

S5、輸出機器人的定位結果。

2.根據權利要求1所述的基于視覺引導激光重定位的移動機器人定位方法，其特征在于，所述的步驟S1中，根據視覺特征地圖對機器人進行定位具體包括以下步驟：

S11、提取圖像ORB特征，使用DBoW2模型計算當前幀的BoW詞袋向量，生成一個表征向量v；

S12、挑選視覺特征地圖上的備選關鍵幀，計算備選關鍵幀與當前幀的BoW相似性得分，相似性標準采用L1范數距離，返回所有滿足設定得分的關鍵幀；

S13、逐個比較當前幀和備選關鍵幀的匹配特征點，得到匹配關系；

S14、根據匹配關系，對每個備選關鍵幀輪流執行EPnP算法，使用RANSAC算法迭代求解當前幀的位姿，即在視覺特征地圖中的位置。

3.根據權利要求2所述的基于視覺引導激光重定位的移動機器人定位方法，其特征在于，所述的步驟S14中，EPnP算法的步驟包括：

A1、選取世界坐標下的四個控制點坐標為：

C_w＝[0 0 0 1]^T，[1 0 0 1]^T，[0 1 0 1]^T，[0 0 1 1]^T；

A2、通過n個3D點在攝像頭平面的投影關系，以及與這四個控制點的權重關系，構建一個12×12方陣，求得其零空間特征向量，得到虛擬控制點的攝像頭平面坐標，然后使用正交投影迭代變換即可求出攝像頭位姿；

A3、通過EPnP算法求得四對點下的封閉解后，將該解作為非線性優化的初值，優化提高精度。

4.根據權利要求1所述的基于視覺引導激光重定位的移動機器人定位方法，其特征在于，所述的步驟S1中，將視覺特征地圖中定位的位置映射到激光地圖上，具體包括：將機器人在激光地圖下的位姿估計為地圖間齊次變換矩陣與使用視覺特征地圖重定位后獲取位姿的乘積。

5.根據權利要求1所述的基于視覺引導激光重定位的移動機器人定位方法，其特征在于，自適應粒子濾波方法的定位過程中定位的粒子方差的計算表達式為：

其中，表示定位的粒子集合。

6.根據權利要求1所述的基于視覺引導激光重定位的移動機器人定位方法，其特征在于，視覺特征地圖根據ORB-SLAM2方法建立，建立的視覺特征地圖中包含特征點和關鍵幀的信息；激光地圖根據cartographer方法建立；建立視覺特征地圖和激光地圖時選擇相同的初始點。