[發明專利]一種同步定位與地圖構建方法及設備

有效

• 申請號: 201711134311.7
• 申請日: 2017-11-16
• 公開（公告）號: CN107917712B
• 公開（公告）日: 2020-07-28
• 發明（設計）人: 劉勝明;司秀芬;姜志英
• 申請（專利權）人: 蘇州艾吉威機器人有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06F30/20;G06F16/29
• 代理公司: 蘇州創元專利商標事務所有限公司 32103
• 代理人: 丁浩秋

1.一種同步定位與地圖構建方法，用于機器人，其特征在于，包括以下步驟：

S01：加載預先建立的機器人運動學模型；

S02：通過環境感測傳感器掃描環境在預設坐標系中建立初始全局地圖，初始全局地圖的建立包括以下步驟：

A)通過環境感測傳感器在第一位置掃描機器人所在工作環境，采集工作環境中環境感測傳感器與所在工作環境中各障礙物的距離數據幀，生成點云數據，對生成的點云數據，根據最小二乘法擬合出所在工作環境中各障礙物的幾何原形，并根據幾何位置關系按同一順序標注存儲序號，生成第一局部地圖；

B)移動至第二位置時，通過環境感測傳感器掃描并獲得第二局部地圖，根據所述第二局部地圖和第一局部地圖融合得到第一局部融合地圖；

C)依次多次重復步驟A)、B)，由若干局部地圖和前一局部融合地圖融合得到所在工作環境的初始全局地圖；

所述局部地圖的融合包括以下步驟：

從距離數據幀中提取幾何原形區塊，并用幾何原形區塊對所在工作環境中各障礙物點云數據進行連續的分隔融合處理，擬合生成包含若干特征點和/或特征線段的特征幾何原形，將所述特征幾何原形在環境地圖中按照同一方向順序存儲，并標注存儲序號；

S03：在移動至當前位置時，通過環境感測傳感器掃描獲得當前局部地圖，當前局部地圖采用若干幾何原形表示，所述幾何原形按與初始全局地圖一致的同一順序存儲，并標注存儲序號，根據所述當前局部地圖和初始全局地圖幾何原形存儲序號的比對進行融合得到更新全局地圖；

S04：根據所述當前局部地圖、初始全局地圖和更新全局地圖對所述機器人的當前估計位姿。

2.根據權利要求1所述的同步定位與地圖構建方法，其特征在于：所述幾何原形包括線段、橢圓、圓、斷點和角點，在初始全局地圖中按照幾何位置關系逆時針順序存儲，并標注存儲序號，所述存儲序號為數字、字母或數字與字母的組合。

3.根據權利要求2所述的同步定位與地圖構建方法，其特征在于：所述局部地圖融合還包括以下步驟：

如果第二局部地圖沒有與第一局部地圖相同的存儲序號，則按照順序把第二局部地圖的第二幾何原形插入到第一局部地圖中的適當位置形成第一融合局部地圖；

如果第二局部地圖有與第一局部地圖相同的存儲序號，則將具有存儲序號相同的第二局部地圖的第二幾何原形與第一局部地圖的第一幾何原形重疊，融合第二局部地圖和第一局部地圖形成第一融合局部地圖；

如果第一局部地圖為逆時針順序地圖，則第一融合局部地圖也為逆時針順序地圖。

4.根據權利要求3所述的同步定位與地圖構建方法，其特征在于，在移動至當前位置時，通過環境感測傳感器預估所述機器人的當前估計位姿包括：

在移動至當前位置時，根據所述機器人運動距離和運動方向，計算獲得所述機器人當前位姿，并且將所述機器人當前位姿作為所述機器人的當前估計位姿。

5.根據權利要求1所述的同步定位與地圖構建方法，其特征在于，當環境感測傳感器掃描到環境中出現新障礙物時：

當新障礙物為靜態，則將更新全局地圖存儲；

當新障礙物為動態，則不將更新全局地圖存儲；

根據所述當前局部地圖、初始全局地圖和更新全局地圖對所述機器人的當前估計位姿。

6.根據權利要求1所述的同步定位與地圖構建方法，其特征在于，還包括設置在下部的障礙物識別模塊，所述環境感測傳感器為激光掃描儀，所述障礙物識別模塊為激光掃描儀、聲納環境感測傳感器或超聲波環境感測傳感器。