本發(fā)明的一種3D點云地圖更新方法和系統(tǒng)，構建局部地圖，獲取機器人粗位姿，然后對機器人位姿進行優(yōu)化更新；其中更新步驟包括由粗到細的匹配算法、增量式優(yōu)化算法，最后采用局部地圖與全局地圖的匹配更新采用點云配準；本發(fā)明旨在地圖場景變化進而導致導航異常時，對此局部區(qū)域地圖進行更新，而不影響原地圖的其他區(qū)域和實施信息，避免了重復工作量。本發(fā)明自動化程度高，實施簡單，針對特定的巡檢環(huán)境，可以實現(xiàn)智能自主實施，即使是不專業(yè)的施工人員也可以輕松使用機器人進行智能巡檢，降低了施工的難度，也減少了現(xiàn)場施工人員的專業(yè)技能要求，降低人力成本；在特定情況，如機器人轉場時，不需要重復實施。

技術領域

本發(fā)明涉及智能機器人和SLAM技術領域，具體涉及一種3D點云地圖更新方法和系統(tǒng)。

背景技術

隨著現(xiàn)代技術的不斷發(fā)展，電力系統(tǒng)形成了信息化和智能化的發(fā)展趨勢，目前越來越多的配電房引入智能巡檢機器人來代替人工巡檢電力設備是否運行正常，可以減少人工成本，且機器人可以一直循環(huán)巡檢，提高工作效率，減少出問題的風險；但目前主流的SLAM都是基于先創(chuàng)建的地圖進行定位導航，一旦地圖場景出現(xiàn)變化，就會導致定位導航異常，需要對地圖進行重新創(chuàng)建修復，增加實施工作量。

輪式巡檢機器人在巡檢前需要先進行地圖構建，然后根據(jù)創(chuàng)建的地圖進行導航巡檢；但是現(xiàn)場環(huán)境經(jīng)常會出現(xiàn)變化的情況，尤其是在增加了物體的情況下，這樣會導致機器人的定位出現(xiàn)不穩(wěn)定和不準確的情況，進而導致導航異常，影響正常巡檢；

一旦出現(xiàn)這種情況，就只能對地圖進行重新構建，但是這樣會導致之前已經(jīng)實施過的點位信息等出現(xiàn)偏差，增加重復工作量；本文提出的方法就是在不改變原有地圖的實施信息的前提下，對巡檢地圖進行局部更新，解決定位導航異常的問題。

地圖在環(huán)境變化時可用性差，需要重新建圖以滿足實際巡檢需要，或者要盡量避免現(xiàn)場的環(huán)境變化，會增加現(xiàn)場的實施工作量。

術語解釋：

實施：機器人在智能巡檢前，創(chuàng)建巡檢地圖，確認巡檢路徑，設置巡檢設備點位信息等的操作；

SLAM：simultaneous localization and mapping，即時定位與地圖構建；

ICP:Iterative Closest Point,迭代最近點算法；

PointToPlane：點到面匹配模式，ICP匹配算法的一種。

發(fā)明內(nèi)容

本發(fā)明提出的一種3D點云地圖更新方法和系統(tǒng)，可解決上述技術問題。

為實現(xiàn)上述目的，本發(fā)明采用了以下技術方案：

一種3D點云地圖更新方法，包括以下步驟：

構建局部地圖，獲取機器人粗位姿，然后對機器人位姿進行優(yōu)化更新；

其中，優(yōu)化更新步驟包括由粗到細的匹配算法，如下：

Step1.對Q_k采用K-D樹描述，對中的每個激光點尋找在Q_k中的最近點，建立配對；

Step2.建立非線性方程，目標使得所有配對的距離最小；

Step3.利用L-M法求解非線性優(yōu)化問題，求解T_opt＝(R,t)；

Step4.則k+1時刻的機器人位姿為

其中Q_k表示k時刻建立的點云地圖，表示k+1時刻當前幀轉換到世界坐標系下，表示k時刻機器人在世界坐標系下的位姿，表示k+1時刻機器人的位姿增量也即上步粗匹配過程中輸出量。

進一步的，優(yōu)化更新步驟還包括增量式優(yōu)化算法，如下：