本發(fā)明屬于智能機器人導航領(lǐng)域，特別地涉及一種基于激光數(shù)據(jù)的移動機器人點云地圖創(chuàng)建方法。

背景技術(shù)：

地圖創(chuàng)建是移動機器人相關(guān)技術(shù)中的一個基本而且十分重要的問題，在移動機器人導航定位和全局路徑規(guī)劃等相關(guān)領(lǐng)域有著廣泛的應用。為了實現(xiàn)移動機器人的自主導航，必須通過自定位來獲取機器人在環(huán)境中的準確位姿。根據(jù)先驗地圖進行的移動機器人定位和導航技術(shù)得到了廣泛的研究，并取得了很好的應用效果。但是實際環(huán)境往往是未知的，不能提供諸如環(huán)境大小、形狀、障礙物位置等先驗信息，而且實際環(huán)境中也往往不存在路標、燈塔等人為設定的標志物，這給移動機器人的自主導航帶來了很大的問題。所以，在未知環(huán)境中，如何通過傳感器提取出環(huán)境和障礙物信息，并快速地創(chuàng)建環(huán)境地圖是移動機器人自定位、導航和路徑規(guī)劃問題解決的關(guān)鍵。

近年來，由于導航定位以及路徑規(guī)劃等技術(shù)的迫切需要，關(guān)于移動機器人基于激光掃描數(shù)據(jù)而進行地圖創(chuàng)建的研究發(fā)展迅速。匹配是基于激光測距儀環(huán)境地圖創(chuàng)建的一種核心方法。匹配就是利用兩組激光數(shù)據(jù)點集并根據(jù)兩組點集間相關(guān)數(shù)據(jù)的配準來估計兩組點集的位姿變化，使得兩組激光數(shù)據(jù)點集能夠最大程度的重合，從而完成機器人環(huán)境地圖的創(chuàng)建。國際上有許多學者進行了大量的研究，提出了大量的數(shù)據(jù)點集匹配方法?？傮w上分為兩類：一是采用經(jīng)過高精度標定的儀器來獲取不同視點的數(shù)據(jù)以及它們之間的變換關(guān)系進行數(shù)據(jù)間的配準計算法。但是由于受到條件的限制，很難得到這些數(shù)據(jù)采集位置信息，因此應用范圍并不廣泛。二是在沒有任何控制之下，基于數(shù)據(jù)點集信息對相鄰數(shù)據(jù)點集的配準。由于這種方法應用范圍廣泛，得到了大量的研究應用。

基于激光數(shù)據(jù)點集匹配算法大致可分為非迭代匹配算法和迭代匹配算法。

直方圖匹配算法是典型的非迭代匹配算法，該算法利用角度直方圖和位移直方圖進行匹配，其最大優(yōu)點是容易實現(xiàn)，匹配速度快，但是由于與直方圖的分辨率有關(guān)，直方圖的匹配精度十分有限。迭代最近點(iterative closest point)算法是一種典型的迭代匹配算法。該算法是一種通過在迭代過程中不斷降低配準誤差來獲取采樣點相對位置變換的掃描匹配方法?？傮w而言，非迭代匹配算法的匹配速度較快但是匹配精度不高，結(jié)合基于激光掃描數(shù)據(jù)進行地圖創(chuàng)建的要求，迭代匹配算法更適用于移動機器人導航系統(tǒng)。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的在于解決現(xiàn)有技術(shù)中存在的問題，并提供一種基于激光數(shù)據(jù)的移動機器人點云地圖創(chuàng)建方法。本發(fā)明所采用的具體技術(shù)方案如下:

一種基于激光數(shù)據(jù)的移動機器人點云地圖創(chuàng)建方法，包括如下步驟：

步驟1：移動機器人與客戶端之間建立實時通信和數(shù)據(jù)傳輸；

步驟2：激光傳感器和里程計隨移動機器人的運動采集外部環(huán)境數(shù)據(jù)并傳輸至工控機，生成本地數(shù)據(jù)文件；

步驟3：針對本地數(shù)據(jù)文件，對其已存儲的數(shù)據(jù)進行預處理，刪除匹配度低于閾值的數(shù)據(jù)幀；

步驟4：根據(jù)預先采集的激光和里程計信息進行離線計算，形成機器人所在環(huán)境的二維點云地圖。

作為優(yōu)選，基于步驟1的具體過程為：

步驟1.1：移動機器人與客戶端連接于同一個局域網(wǎng)，并基于UDP連接實現(xiàn)實時通信；

步驟1.2：移動機器人上搭載的工控機與解算板通過RS232串口進行通信，工控機與激光傳感器通過TCP/IP連接進行數(shù)據(jù)傳輸。

作為優(yōu)選，基于步驟2的具體過程為：

步驟2.1：激光傳感器以一定的頻率和角度范圍平面掃描外部環(huán)境，獲取激光傳感器所在位置與周邊環(huán)境間的距離數(shù)據(jù)，并實時傳輸至工控機；

步驟2.2：里程計隨著機器人的平面運動實時記錄機器人的位置和姿態(tài)信息，同時傳輸至工控機；位置信息包括X、Y軸坐標值，姿態(tài)信息包括旋轉(zhuǎn)角度值；

步驟2.3：工控機將接收到的激光掃描數(shù)據(jù)和位置姿態(tài)信息存儲于本地，生成本地數(shù)據(jù)文件。

作為優(yōu)選，基于步驟3的具體過程為：

步驟3.1：針對本地數(shù)據(jù)文件，從數(shù)據(jù)幀首幀開始，采用迭代最近點算法分別匹配相鄰兩幀數(shù)據(jù)，并獲得相鄰兩數(shù)據(jù)幀的匹配度值；

步驟3.2：設定匹配度閾值，刪除匹配度低于閾值的數(shù)據(jù)幀，保留匹配度不低于閾值的數(shù)據(jù)幀；

步驟3.3：將所保留的數(shù)據(jù)幀重新輸出至本地文件進行存儲。

進一步的，基于步驟4的具體過程為：

步驟4.1：將步驟3.3中存儲的本地數(shù)據(jù)文件中各數(shù)據(jù)幀以點云圖的形式顯示；

步驟4.2：選擇該本地數(shù)據(jù)文件中待拼接的數(shù)據(jù)幀的范圍為第p幀至第q幀，其中q-p>＝0；

步驟4.3：采用點云庫中迭代最近點算法從第p幀至第q幀數(shù)據(jù)進行逐幀自動點云匹配和數(shù)據(jù)融合；待匹配完成后，更新顯示匹配后的數(shù)據(jù)集點云地圖，將點云地圖以*.map的形式存儲于本地文件。

更進一步的，基于步驟4.3已創(chuàng)建的點云地圖進行路徑規(guī)劃和機器人手動控制，具體步驟為：

步驟5.1：客戶端加載*.map點云地圖，并顯示該地圖文件所表示的點云；

步驟5.2：通過UDP連接，客戶端從移動機器人實時獲取里程計記錄的機器人位置和姿態(tài)信息，并實時模擬顯示移動機器人在地圖的相應位置。

步驟5.3：通過客戶端進行地圖路徑規(guī)劃，添加直線路徑和曲線路徑，其中曲線形式為三次貝塞爾曲線；

步驟5.4：通過客戶端添加停靠點，選擇已繪制的直線路徑，并在此路徑上添加點作為移動機器人導航時的?？奎c；

步驟5.5：通過客戶端傳輸運動控制命令至移動機器人上的工控機，實現(xiàn)機器人前后左右手動移動控制、原地旋轉(zhuǎn)控制以及運動速度設置。

步驟5.6：將路徑的起始點、長度、方向以及貝塞爾曲線的控制點信息存儲至數(shù)據(jù)庫中，同時也將?？奎c坐標信息等存儲至數(shù)據(jù)庫中。

再進一步的，步驟5.4中，客戶端接收到用戶所選的點P(x₀,y₀)后，計算該點到各直線路徑的距離值，取最小距離值，若該距離值d₁小于一定的閾值β₁，則將該距離值對應的直線路徑作為所選路徑；點到直線的距離計算公式如下：

其中，公式中的直線路徑的方程為Ax+By+C＝0；

選定路徑后，進行添加?？奎c的操作，客戶端接收到用戶所選的?？奎cQ(x₁,y₁)后，通過上述距離公式計算Q點到所選路徑的距離d₂，若d₂小于一定的閾值β2，則將Q點作為?？奎c。

所述的數(shù)據(jù)庫中創(chuàng)建有路徑信息數(shù)據(jù)表和?？奎c信息數(shù)據(jù)表，路徑信息數(shù)據(jù)表中包含路徑起止點、直線路徑長度、直線路徑方向、曲線路徑控制點信息；停靠點信息數(shù)據(jù)表中包含?？奎c坐標、所?？康穆窂叫畔ⅰ?/p>

本發(fā)明數(shù)據(jù)采集操作簡單，且無需預先了解環(huán)境結(jié)構(gòu)空間，也無需額外鋪設地標，成本低廉、地圖創(chuàng)建速度快精度高，可實現(xiàn)移動機器人在未知環(huán)境下地圖的多次創(chuàng)建和快速更新。

附圖說明

圖1為本發(fā)明的移動機器人的結(jié)構(gòu)框架圖；

圖2為本發(fā)明實施例中加載數(shù)據(jù)幀后的示意圖；

圖3為本發(fā)明實施例中拼接效果示意圖；

圖4為本發(fā)明實施例中添加的直線路徑示意圖。

具體實施方式

下面結(jié)合附圖和具體實施方式對本發(fā)明做進一步闡述，以便更好的理解本發(fā)明。本發(fā)明的技術(shù)特征在沒有沖突的情況下，均可進行相互組合。

如圖1所示，本發(fā)明的移動機器人內(nèi)置工控機、解算板、激光傳感器和里程計，工控機與解算板通過RS232串口進行通信。工控機通過wifi與客戶端進行遠程通信?？蛻舳丝刹捎肞C機、服務器等形式實現(xiàn)。

本實施方式中，基于激光數(shù)據(jù)的移動機器人點云地圖創(chuàng)建方法，包括如下步驟和子步驟：

步驟1：移動機器人與客戶端之間建立實時通信和數(shù)據(jù)傳輸。具體子步驟：

步驟1.1：移動機器人與客戶端連接于同一個局域網(wǎng)，并基于UDP連接實現(xiàn)實時通信；

步驟1.2：移動機器人上搭載的工控機與解算板通過RS232串口進行通信，工控機與激光傳感器通過TCP/IP連接進行數(shù)據(jù)傳輸。

步驟2：激光傳感器和里程計隨移動機器人的運動采集外部環(huán)境數(shù)據(jù)并傳輸至工控機，生成本地數(shù)據(jù)文件。具體子步驟：

步驟2.1：激光傳感器以一定的頻率和角度范圍平面掃描外部環(huán)境，獲取激光傳感器所在位置與周邊環(huán)境間的距離數(shù)據(jù)，并實時傳輸至工控機；

步驟2.2：里程計隨著機器人的平面運動實時記錄機器人的位置(包括X、Y軸坐標值)和姿態(tài)信息(包括旋轉(zhuǎn)角度值)，同時傳輸至工控機；

步驟2.3：工控機將接收到的激光掃描數(shù)據(jù)和位置姿態(tài)信息存儲于本地，生成本地數(shù)據(jù)文件，格式為*.2d。

步驟3：針對本地數(shù)據(jù)文件*.2d，對其已存儲的數(shù)據(jù)進行預處理，刪除匹配度低于閾值的數(shù)據(jù)幀。具體子步驟：

步驟3.1：針對本地數(shù)據(jù)文件，從數(shù)據(jù)幀首幀開始，采用迭代最近點算法分別匹配相鄰兩幀數(shù)據(jù)，并獲得相鄰兩數(shù)據(jù)幀的匹配度值；

步驟3.2：設定匹配度閾值，刪除匹配度低于閾值的數(shù)據(jù)幀，保留匹配度不低于閾值的數(shù)據(jù)幀；

步驟3.3：將所保留的數(shù)據(jù)幀重新輸出至本地文件進行存儲。

步驟4：根據(jù)預先采集的激光和里程計信息進行離線計算，形成機器人所在環(huán)境的二維點云地圖。具體子步驟：

步驟4.1：將步驟3.3中存儲的本地數(shù)據(jù)文件(*.2d)中各數(shù)據(jù)幀以點云圖的形式顯示；

步驟4.2：選擇該本地數(shù)據(jù)文件中待拼接的數(shù)據(jù)幀的范圍為第p幀至第q幀(如圖2所示，其中(q-p)>＝0)，同時在圖像顯示區(qū)用不同顏色分別標注出第p幀數(shù)據(jù)和第q幀數(shù)據(jù)；

步驟4.3：采用點云庫(PCL)中迭代最近點(ICP)算法從第p幀至第q幀數(shù)據(jù)進行逐幀自動點云匹配和數(shù)據(jù)融合，如圖3所示。待匹配完成后，更新顯示匹配后的數(shù)據(jù)集點云地圖，將點云地圖以*.map的形式存儲于本地文件。

ICP算法分為兩大部分，對應點的搜索和變換參數(shù)的求解。整個算法就是在對對應點對的搜素——變換參數(shù)的計算——對應點進行剛性變換——檢驗目標函數(shù)值的迭代過程中不斷降低匹配誤差以獲得兩組數(shù)據(jù)間的相對位姿變化的最優(yōu)變換參數(shù)(R,T)，其中R為旋轉(zhuǎn)矩陣，T為平移矩陣。ICP算法的具體實現(xiàn)過程如下：

給定兩組臨近時刻掃描所得的激光數(shù)據(jù)點集P＝{Pi,i＝0,1,2,…,m}及Q＝{Qi,i＝0,1,2,…,n}，設置最小誤差度量值ε和最大迭代次數(shù)D。

估計配準：數(shù)據(jù)點集P與U之間的估計配準由同時存儲在2d文件中與這兩組數(shù)據(jù)點對應的里程計數(shù)值給出。沒有估計配準，匹配算法會陷入局部極小值。

尋找對應點：根據(jù)歐式距離最小原則尋找，歐式距離定義如下：

對于集合Q中的各點，在集合P中找出距該點最近的對應點，且設集合P中由這些對應點組成的新點集為P‘＝{P’i,i＝0,1,2,…,n}。作為求解變換的匹配點。

尋找變換(R,T)：使得兩組數(shù)據(jù)點間的誤差度量值最小，計算公式如下：

其中ω為旋轉(zhuǎn)弧度，旋轉(zhuǎn)矩陣平移向量Q_i為參考點集，P’_i為對應的帶匹配的點集。通過最小化E_k，可以得到T_x、T_y和ω的解。

應用變換，更新機器人位姿。用配準變換矩陣R、T進行坐標變換，得到新的點集Q₁，即Q₁＝RQ+T，計算E_k+1。

迭代：當|E_k-E_k+1|>ε，重復步驟(3)-(5)。否則，跳出迭代，計算結(jié)束，匹配成功。

由此，完成移動機器人點云地圖的創(chuàng)建步驟，基于該點云地圖可以進行路徑規(guī)劃和機器人手動控制，具體如下：

步驟5：基于步驟4.3已創(chuàng)建的點云地圖進行路徑規(guī)劃和機器人手動控制，各子步驟為：

步驟5.1：客戶端加載*.map點云地圖，并顯示該地圖文件所表示的點云；

步驟5.2：通過UDP連接，客戶端從移動機器人實時獲取里程計記錄的機器人位置和姿態(tài)信息，并實時模擬顯示移動機器人在地圖的相應位置。

步驟5.3：通過客戶端進行地圖路徑規(guī)劃，添加直線路徑和曲線路徑，其中曲線形式為三次貝塞爾曲線B(t)，公式如下：

B(t)＝P₀(1-t)³+3P₁t(1-t)²+3P₂t²(1-t)+P₃t³,t∈[0,1]

上式P₀、P₁、P₂、P₃四個點在平面中定義了三次方貝塞爾曲線，P₀、P₃為曲線起始點，P₁、P₂為曲線控制點，貝塞爾曲線始于P₀走向P₁，并從P₂的方向來到P₃。

步驟5.4：通過客戶端添加?？奎c，選擇已繪制的直線路徑，并在此路徑上添加點作為移動機器人導航時的停靠點。

該步驟中，具體實現(xiàn)方式為：客戶端接收到用戶所選的點P(x₀,y₀)后，計算該點到各直線路徑的距離值。如通過鼠標點擊選擇一條已繪制的直線路徑。計算鼠標當前點擊的點P(x0,y0)到各直線路徑的距離值。取最小距離值，若該距離值d₁小于一定的閾值β₁，則將該距離值對應的直線路徑作為所選路徑；點到直線的距離計算公式如下：

其中，公式中的直線路徑的方程為Ax+By+C＝0；

選定路徑后，進行添加停靠點的操作，客戶端接收到用戶所選的?？奎cQ(x₁,y₁)后，通過上述距離公式計算Q點到所選路徑的距離d₂，若d₂小于一定的閾值β2，則將Q點作為停靠點。并且可將Q點作為?？奎c并用不同顏色標記出來，以示標記。本實例中，添加的直線路徑如圖4所示。

步驟5.5：通過客戶端傳輸運動控制命令至移動機器人上的工控機，實現(xiàn)機器人前后左右手動移動控制、原地旋轉(zhuǎn)控制以及運動速度設置。

步驟5.6：將路徑的起始點、長度、方向以及貝塞爾曲線的控制點信息存儲至數(shù)據(jù)庫中，同時也將?？奎c坐標信息等存儲至數(shù)據(jù)庫中，供后續(xù)調(diào)用。數(shù)據(jù)庫中創(chuàng)建路徑信息數(shù)據(jù)表，表中包含路徑起止點、直線路徑長度、直線路徑方向、P1、P2曲線路徑控制點等信息。數(shù)據(jù)庫中創(chuàng)建?？奎c信息數(shù)據(jù)表，表中包含?？奎c坐標、所停靠的路徑等信息。

上述實施例僅用于解釋說明本發(fā)明要求保護的內(nèi)容，但并不是用于限制本發(fā)明的要求保護的范圍。本領(lǐng)域技術(shù)人員在本發(fā)明精神內(nèi)所做的改進和替換，均屬于保護范圍內(nèi)。