[發明專利]基于多超聲傳感器的移動機器人同步定位與地圖構建方法

有效

• 申請號: 201710630892.7
• 申請日: 2017-07-28
• 公開（公告）號: CN107462892B
• 公開（公告）日: 2021-11-30
• 發明（設計）人: 任勇;張哲;劉少山
• 申請（專利權）人: 深圳市遠弗科技有限公司
• 主分類號: G01S15/89
• 分類號: G01S15/89;G01S15/46;G06K9/62;G06F16/29
• 代理公司: 北京知呱呱知識產權代理有限公司 11577
• 代理人: 武媛

1.一種基于多超聲傳感器的移動機器人同步定位與地圖構建方法，其特征在于包括如下步驟：

(1)設置超聲傳感器：

在機器人機身周圍的同一水平面上均勻布置至少16個超聲傳感器，且機身前后兩部分的超聲傳感器對稱分布；

⑵獲取超聲測量數據：

部署在機器人機身的超聲傳感器通過檢測從發送超聲波到接收到回波的時間，獲取超聲傳感器與正前方障礙物之間的距離，即超聲測量數據；在連續時刻,多次獲取超聲測量數據；

⑶組合所獲取到的超聲測量數據：

3.1)設定對所獲取到的超聲測量數據的組合次數為N,且N為大于0的自然數；

3.2)對所獲取的超聲測量數據進行坐標變換,設第一個測量時刻移動機器人在世界坐標系下的位姿為Ta,第一個測量時刻的測量值Za,第二個測量時刻移動機器人的位姿為Tb,第二個測量時刻的測量值為Zb；

通過下式變換得到第二個測量時刻測量值Zb在第一個測量時刻對應坐標系下的測量值Zb2：

Zb2＝Ta^{-1}*Tb*Zb；

將Zb2與Za疊加作為新的第一時刻測量值ZaO；

3.3)根據步驟3.2)的變換方式亦可得到第三個測量時刻測量值Zc在第一個測量時刻對應坐標系下的測量值Zc2：

Zc2＝Ta^{-1}*Tc*Zc；

其中，Tc為第三個測量時刻移動機器人的位姿，Zc為第三個測量時刻的測量值；將Zc2再與ZaO疊加，以此類推，便可得到多個超聲測量數據在同一坐標系下的疊加值，即組合超聲測量數據；

⑷判斷是否達到設定的組合次數：

判斷步驟(3)中測量數據的疊加是否達到設定的組合次數N,得到其全部數據的疊加值；若是則執行步驟⑸，若不是則返回步驟⑵；

⑸預測機器人的當前位置：

設上一時刻的機器人位置已知，根據上一時刻到當前時刻機器人收到的運動指令，結合定位算法，對當前時刻的機器人位置進行預測；

5.1)求解機器人收到運動指令后的當前位置估計：

設上一個時刻機器人的坐標為(xl,yl,thetal)，接收到的運動指令為(dx,dy,dtheta)，得到機器人收到運動指令后的坐標為(x2,y2,theta2)，即當前位置估計；

其中：

x2＝xl+dx；

y2＝yl+dy；

theta2＝thetal+dtheta；

5.2)計算機器人的當前時刻預測位置：

在位置估計的基礎上加上一定噪聲，通過隨機采樣的方式，選擇均值為零的高斯噪聲作為一種擾動(dx’,dy’,dtheta’),設選取三十個粒子,其中每一個粒子表示機器人位置的一種可能性,這三十個粒子可產生三十個擾動，在當前位置估計的基礎上加上這個擾動，得到三十個不同的位置,將這些位置作為機器人的當前時刻預測位置；

⑹確定機器人的當前時刻最終位置：

在根據預測位置已經構建出了部分地圖的情況下，通過假設機器人的當前位置，計算出機器人相對于已知的部分地圖應該有的測量值，即預測的測量值,將這個預測的測量值與實際測量值對比，可得知機器人的當前時刻預測位置是否準確，即預測的測量值和實際測量值是否匹配；根據組合測量數據與已有地圖的匹配情況，對不同粒子進行打分,得分最高的即為最匹配的位置，將該位置確定為機器人的當前時刻最終位置；

⑺根據機器人的當前時刻最終位置更新環境地圖：

以步驟(6)中得到的機器人當前時刻最終位置作為基準，結合當前超聲傳感器的測量數據,更新地圖中未知部分的區域，同時修正部分已知位置的地圖值，從而更新地圖。