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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

步驟3：針對(duì)本地?cái)?shù)據(jù)文件，對(duì)其已存儲(chǔ)的數(shù)據(jù)進(jìn)行預(yù)處理，刪除匹配度低于閾值的數(shù)據(jù)幀；

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

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

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

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

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

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

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

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

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

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

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

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

進(jìn)一步的，基于步驟4的具體過程為：

步驟4.1：將步驟3.3中存儲(chǔ)的本地?cái)?shù)據(jù)文件中各數(shù)據(jù)幀以點(diǎn)云圖的形式顯示；

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

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

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

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

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

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

步驟5.4：通過客戶端添加停靠點(diǎn)，選擇已繪制的直線路徑，并在此路徑上添加點(diǎn)作為移動(dòng)機(jī)器人導(dǎo)航時(shí)的?？奎c(diǎn)；

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

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

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

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

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

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

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

附圖說明

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

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

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

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

具體實(shí)施方式

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

如圖1所示，本發(fā)明的移動(dòng)機(jī)器人內(nèi)置工控機(jī)、解算板、激光傳感器和里程計(jì)，工控機(jī)與解算板通過RS232串口進(jìn)行通信。工控機(jī)通過wifi與客戶端進(jìn)行遠(yuǎn)程通信。客戶端可采用PC機(jī)、服務(wù)器等形式實(shí)現(xiàn)。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

尋找對(duì)應(yīng)點(diǎn)：根據(jù)歐式距離最小原則尋找，歐式距離定義如下：

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

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

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

應(yīng)用變換，更新機(jī)器人位姿。用配準(zhǔn)變換矩陣R、T進(jìn)行坐標(biāo)變換，得到新的點(diǎn)集Q₁，即Q₁＝RQ+T，計(jì)算E_k+1。

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

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

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

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

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

步驟5.3：通過客戶端進(jìn)行地圖路徑規(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₃四個(gè)點(diǎn)在平面中定義了三次方貝塞爾曲線，P₀、P₃為曲線起始點(diǎn)，P₁、P₂為曲線控制點(diǎn)，貝塞爾曲線始于P₀走向P₁，并從P₂的方向來到P₃。

步驟5.4：通過客戶端添加?？奎c(diǎn)，選擇已繪制的直線路徑，并在此路徑上添加點(diǎn)作為移動(dòng)機(jī)器人導(dǎo)航時(shí)的?？奎c(diǎn)。

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

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

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

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

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

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