[發明專利]復雜環境下多傳感器的移動機器人slam建圖方法和系統

在審

• 申請號: 201810952109.3
• 申請日: 2018-08-21
• 公開（公告）號: CN109059927A
• 公開（公告）日: 2018-12-21
• 發明（設計）人: 李曉飛;王鵬飛;吳聰;柴磊
• 申請（專利權）人: 南京郵電大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 南京縱橫知識產權代理有限公司 32224
• 代理人: 董建林

1.一種復雜環境下多傳感器的移動機器人slam建圖方法，其特征是，包括以下步驟：

步驟S1，同時獲取同一環境的激光雷達數據、深度數據以及超聲數據；

步驟S2，從激光雷達數據中提取角度和距離構成的數值對，并將深度數據和超聲數據也轉換成角度和距離構成的數值對格式；

步驟S3，將激光雷達、深度和超聲類型所對應的三組數值對進行時間同步，并分別進行卡爾曼濾波；

步驟S4，對三組數值對中對應同一時刻的三個數據，將兩兩不同類型之間的數據差值與預設閾值進行對比，若大于閾值則挑選出最大優先級類型的數值對輸出作為最終的數值對，重復此過程直至完成整組數值對；

步驟S5，利用輸出的最終一組數值對進行建圖。

2.根據權利要求1所述的一種復雜環境下多傳感器的移動機器人slam建圖方法，其特征是，步驟S2中，采用多線程同時處理。

3.根據權利要求1所述的一種復雜環境下多傳感器的移動機器人slam建圖方法，其特征是，以激光雷達數據格式為標準對深度數據進行格式轉換的具體步驟如下：

步驟S2.1，提取深度圖水平中線上下設定值像素區域作為敏感區域；

步驟S2.2，對敏感區域中每列像素點取灰度最小值；

步驟S2.3，對敏感區域中水平范圍內，取整數角度對應列上灰度最小值作為距離數據，加上對應角度值，轉換為激光雷達數據格式的角度和距離數值對。

4.根據權利要求1所述的一種復雜環境下多傳感器的移動機器人slam建圖方法，其特征是，以激光雷達數據格式為標準對超聲數據格式轉換的具體過程如下：

以激光雷達的數值對中的角度，為超聲數組中每一個距離數據添加角度數據，形成激光雷達格式的角度和距離數值對。

5.根據權利要求1所述的一種復雜環境下多傳感器的移動機器人slam建圖方法，其特征是，針對三組數值對進行卡爾曼濾波數據融合，具體計算步驟如下：

步驟S3.1，獲取激光雷達、深度和超聲類型所對應的三組數值對數據；

步驟S3.2，根據數據類型不同選擇卡爾曼參數對卡爾曼濾波器模型初始化；

步驟S3.2.1，由式(1)和式(2)初始化卡爾曼濾波器模型的時間更新公式：

X(K|K-1)＝X(K-1|K-1) (1)

P(K|K-1)＝P(K-1|K-1)+Q (2)

其中式(1)中X(K|K-1)是K狀態的預測值，X(K-1|K-1)是K-1狀態的估計值；式(2)中P(K|K-1)是預測值的協方差，P(K-1|K-1)是K-1狀態的估計值協方差，Q為測量過程的噪聲的協防差可以理解為不確定度；

步驟S3.2.2，由式(3)更新計算卡爾曼權值：

K_g＝P(K|K-1)/(P(K|K-1)+R) (3)

其中K_g為卡爾曼權值，R為測量噪聲的協方差；

步驟S3.2.3，由式(4)和式(5)更新計算當前數據最優估計值及估計值的協方差：

X(K|K)＝X(K|K-1)+K_g(y_k-X(K|K-1)) (4)

其中X(K|K)為K狀態下的估計值，y_k為K狀態下的測量值。

P(K|K)＝(1-K_g)P(K|K-1) (5)

其中P(K|K)為K狀態估計值的協方差；

步驟S3.3，將下一個時刻的數值對，及更新后的最優估計值和不確定度進行步驟S3.2，直到當前批數據處理完成。