本發(fā)明涉及移動機(jī)器人的自主導(dǎo)航的技術(shù)領(lǐng)域，更具體地，涉及一種基于激光測距儀的全向移動機(jī)器人的自主導(dǎo)航裝置及其方法。

背景技術(shù)：

智能移動機(jī)器人能在未知或部分未知的環(huán)境中自主運(yùn)動，具備環(huán)境感知、行為規(guī)劃、動態(tài)決策、執(zhí)行決策等功能。機(jī)器人在自主運(yùn)動過程中通常需要解決三個(gè)問題，即機(jī)器人的定位問題、機(jī)器人的任務(wù)規(guī)劃問題以及機(jī)器人的行為規(guī)劃問題。移動機(jī)器人在未知環(huán)境中由于缺乏先驗(yàn)地圖信息，需要通過車載激光測距儀、單目、雙目攝像機(jī)等傳感器獲取周圍的環(huán)境信息，以構(gòu)建環(huán)境地圖并估計(jì)機(jī)器人當(dāng)前的位置，即同步定位與建圖(Simultaneous Localization and Mapping，SLAM)?；谒鶚?gòu)建的地圖信息，根據(jù)目標(biāo)任務(wù)規(guī)劃機(jī)器人的可行路徑；機(jī)器人在沿著所規(guī)劃的路徑行進(jìn)的過程中需要準(zhǔn)確的定位信息，并根據(jù)實(shí)時(shí)位置信息和環(huán)境信息實(shí)現(xiàn)動態(tài)的行為決策，完成目標(biāo)任務(wù)。

SLAM分為定位和建圖兩個(gè)方面，是移動機(jī)器人在未知環(huán)境中實(shí)現(xiàn)自主導(dǎo)航的核心技術(shù)。傳統(tǒng)的航位推測法利用車載里程計(jì)等信息對機(jī)器人進(jìn)行定位，但里程計(jì)存在累積誤差，其定位準(zhǔn)確率隨著機(jī)器人的移動而降低。當(dāng)環(huán)境地圖已知時(shí)，可以將觀測到的環(huán)境信息與已知的地圖信息進(jìn)行對比，從而實(shí)時(shí)修正機(jī)器人的位姿估計(jì)，降低定位的累計(jì)誤差。而SLAM考慮的是環(huán)境信息未知的情況，一方面機(jī)器人的位姿估計(jì)需要先驗(yàn)地圖信息，另一方面，地圖的建立需要準(zhǔn)確的機(jī)器人位姿信息，定位和建圖相輔相承、互相影響，準(zhǔn)確的定位提供準(zhǔn)確的地圖，準(zhǔn)確的地圖提供準(zhǔn)確的定位。目前，SLAM以基于卡爾曼濾波和基于粒子濾波的SLAM方法為主，但隨著機(jī)器人的移動以及地圖的擴(kuò)大，卡爾曼濾波器需要維持龐大的狀態(tài)變量矩陣，而粒子濾波器的粒子數(shù)目也需要增加，因此基于卡爾曼濾波和粒子濾波的SLAM算法的計(jì)算復(fù)雜度隨著機(jī)器人的移動而增加。同時(shí)，當(dāng)機(jī)器人的運(yùn)動學(xué)模型和觀測模型均為非線性時(shí)，尤其當(dāng)無法獲得準(zhǔn)確的觀測模型時(shí)，將影響基于卡爾曼濾波和粒子濾波SLAM算法的準(zhǔn)確性。本發(fā)明考慮基于迭代最近點(diǎn)算法(ICP)的SLAM算法，由于激光測距儀得到的點(diǎn)云數(shù)目有限，使用點(diǎn)匹配迭代計(jì)算方法的計(jì)算開銷是固定的，相比基于卡爾曼濾波和粒子濾波的SLAM算法，ICPSLAM算法的計(jì)算復(fù)雜度降低，具有較好的實(shí)時(shí)性。

全局路徑規(guī)劃算法主要包括模糊規(guī)劃算法、蟻群優(yōu)化算法、粒子群算法、A*算法等方法，這些方法通常需要在一個(gè)確定的空間內(nèi)對障礙物進(jìn)行確切的建模和描述。此外，在環(huán)境趨于復(fù)雜或動態(tài)變化時(shí)，如何使機(jī)器人的路徑全局最優(yōu)或次優(yōu)，如何避免所規(guī)劃路徑的震蕩和死鎖，這些仍是有待解決的問題?？焖贁U(kuò)展隨機(jī)樹(RRT)算法是基于采樣的單查詢運(yùn)動規(guī)劃方法，通過狀態(tài)空間的隨機(jī)采樣點(diǎn)，把搜索導(dǎo)向空白區(qū)域，從而找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑，適合復(fù)雜環(huán)境和變化場景的路徑規(guī)劃。

目前避障算法常見的有可視圖法、人工勢場法、向量場直方圖法。本發(fā)明考慮使用基于障礙區(qū)域的避障算法，當(dāng)移動機(jī)器人的期望速度落在建立的障礙區(qū)域時(shí)，對期望速度進(jìn)行修正，在保證朝目標(biāo)點(diǎn)前進(jìn)的同時(shí)通過最小的修正量避開障礙物。

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

本發(fā)明為克服上述現(xiàn)有技術(shù)所述的至少一種缺陷，提供一種基于激光測距儀的全向移動機(jī)器人的自主導(dǎo)航裝置及其方法，利用激光測距儀對移動機(jī)器人進(jìn)行定位，同時(shí)建立環(huán)境的二維平面地圖，并根據(jù)目標(biāo)任務(wù)進(jìn)行自主導(dǎo)航，能在行進(jìn)過程中自主避障的方法。

為解決上述技術(shù)問題，本發(fā)明采用的技術(shù)方案是：一種基于激光測距儀的全向移動機(jī)器人的自主導(dǎo)航裝置，其中，包括測量移動機(jī)器人輪子的移動距離的編碼器、測量移動機(jī)器人的旋轉(zhuǎn)角速度和加速度的慣性測量單元，編碼器和慣性測量單元分別連接控制器，還包括激光測距儀，激光測距儀連接上位機(jī)，上位機(jī)連接控制器，控制器連接執(zhí)行單元。

本發(fā)明以三輪全向移動機(jī)器人為機(jī)器人運(yùn)動平臺，利用車載編碼器和慣性測量單元獲取移動機(jī)器人的位置信息和姿態(tài)信息(簡稱位姿信息)，同時(shí)由車載激光測距儀獲取移動機(jī)器人各方向距離最近的障礙物的點(diǎn)云信息。使用迭代最近點(diǎn)算法(ICP)將機(jī)器人當(dāng)前的觀測數(shù)據(jù)與之前的觀測數(shù)據(jù)進(jìn)行匹配，以修正移動機(jī)器人的位姿估計(jì)，同時(shí)建立環(huán)境的平面地圖。在已知環(huán)境地圖的情況下，設(shè)定目標(biāo)任務(wù)點(diǎn)，使用快速搜索隨機(jī)樹算法(RRT)規(guī)劃可行路徑，對移動機(jī)器人進(jìn)行導(dǎo)航控制。在自主導(dǎo)航的過程中，機(jī)器人根據(jù)實(shí)時(shí)激光數(shù)據(jù)檢測障礙物，進(jìn)行局部路徑規(guī)劃，實(shí)現(xiàn)自主避障。

本發(fā)明中，車載編碼器測量移動機(jī)器人輪子的移動距離，慣性測量單元測量移動機(jī)器人的旋轉(zhuǎn)角速度和加速度，車載嵌入式控制器獲取編碼器和慣性測量單元的測量值，結(jié)合移動機(jī)器人的運(yùn)動學(xué)模型計(jì)算其位姿信息，發(fā)送給上位機(jī)系統(tǒng)；上位機(jī)系統(tǒng)同時(shí)獲取激光測距儀的激光點(diǎn)云信息，經(jīng)過算法計(jì)算得到移動機(jī)器人的運(yùn)動速度的期望值(包括平移速度和旋轉(zhuǎn)速度)，并將運(yùn)動速度期望值發(fā)送給車載嵌入式控制器；嵌入式控制器通過移動機(jī)器人運(yùn)動學(xué)模型反解出車輪的運(yùn)動速度，再應(yīng)用PID算法對車輪的轉(zhuǎn)速進(jìn)行閉環(huán)控制。

進(jìn)一步的，移動機(jī)器人底盤設(shè)有三個(gè)全向輪。優(yōu)選的，所述的三個(gè)全向輪按等邊三角形分布。

利用所述的基于激光測距儀的全向移動機(jī)器人的自主導(dǎo)航裝置的控制方法，其中，包括以下步驟：

S1.同步定位與地圖構(gòu)建；

S2.路徑規(guī)劃；

S3.位置跟蹤與自主避障。

本裝置包括同步定位與建圖，路徑規(guī)劃與自主避障三個(gè)主要技術(shù)模塊。同步定位與建圖采用ICP-SLAM算法，首先利用移動機(jī)器人的里程計(jì)和慣性傳感器，并結(jié)合機(jī)器人的運(yùn)動學(xué)模型預(yù)估移動機(jī)器人的位姿信息，之后使用ICP算法對激光點(diǎn)云信息和地圖信息進(jìn)行對比，對位姿進(jìn)行修正，并更新地圖信息。全局路徑規(guī)劃模塊是在地圖和初始位置已知的情況下，設(shè)定目標(biāo)位置，使用RRT算法，在保證機(jī)器人不與墻壁等障礙物碰撞的前提下，尋找優(yōu)化的可行路徑。自主避障模塊使用ICP算法得到準(zhǔn)確的定位信息，并根據(jù)規(guī)劃的全局路徑計(jì)算期望的運(yùn)動速度，同時(shí)利用激光點(diǎn)云信息判斷前進(jìn)方向上的障礙物，通過局部路徑規(guī)劃對期望的機(jī)器人運(yùn)動速度進(jìn)行動態(tài)矯正以實(shí)現(xiàn)機(jī)器人在行進(jìn)中的自主避障。

本發(fā)明針對移動機(jī)器人在未知室內(nèi)環(huán)境中基于激光測距儀的自主導(dǎo)航問題提出了較為完整的解決方案，具體包括了同時(shí)定位與建圖、路徑規(guī)劃、位置跟蹤以及避障算法。首先，通過編碼器和慣性測量單元獲取移動機(jī)器人的位姿信息，利用激光測距儀獲取環(huán)境信息，使用ICP-SLAM算法進(jìn)行同步定位與建圖。然后根據(jù)目標(biāo)任務(wù)使用RRT算法規(guī)劃全局路徑，并根據(jù)移動機(jī)器人的實(shí)時(shí)位置和環(huán)境信息設(shè)計(jì)避障策略，作局部路徑規(guī)劃。

與現(xiàn)有技術(shù)相比，有益效果是：本發(fā)明設(shè)計(jì)了將當(dāng)前獲取的激光點(diǎn)云信息與已構(gòu)建地圖進(jìn)行匹配的全局ICP算法，以及將當(dāng)前與上一時(shí)刻的激光點(diǎn)云信息進(jìn)行匹配的局部CP算法，并綜合全局ICP算法與局部ICP算法對移動機(jī)器人的位姿信息進(jìn)行修正，提高了位姿估計(jì)的準(zhǔn)確性，同時(shí)降低了匹配失敗的情況。

設(shè)計(jì)了包括內(nèi)環(huán)和外環(huán)兩個(gè)部分的位置跟蹤算法，其中，內(nèi)環(huán)使用編碼器和慣性測量單元預(yù)估移動機(jī)器人的位姿信息；在外環(huán)部分，利用內(nèi)環(huán)預(yù)估的位姿信息，并結(jié)合激光點(diǎn)云與已知地圖，對機(jī)器人的位姿信息進(jìn)行進(jìn)一步修正?；趦?nèi)環(huán)和外環(huán)的位置跟蹤策略，在確保定位準(zhǔn)確的同時(shí)，提高了系統(tǒng)的實(shí)時(shí)性，避免由于計(jì)算耗時(shí)造成的控制延遲。

針對移動機(jī)器人與障礙物之間的距離的四種不同情況，分別設(shè)計(jì)了相應(yīng)的避障策略，使得移動機(jī)器人在躲避障礙物的同時(shí)能夠盡量沿著最短的路徑前進(jìn)。

附圖說明

圖1為移動機(jī)器人系統(tǒng)框圖。

圖2為三輪全向移動機(jī)器人的運(yùn)動學(xué)模型示意圖。

圖3為ICP-SLAM算法流程圖。

圖4為位置跟蹤與自主避障算法流程圖。

圖5為避障算法示意圖。

具體實(shí)施方式

附圖僅用于示例性說明，不能理解為對本專利的限制；為了更好說明本實(shí)施例，附圖某些部件會有省略、放大或縮小，并不代表實(shí)際產(chǎn)品的尺寸；對于本領(lǐng)域技術(shù)人員來說，附圖中某些公知結(jié)構(gòu)及其說明可能省略是可以理解的。附圖中描述位置關(guān)系僅用于示例性說明，不能理解為對本專利的限制。

如圖1所示，一種基于激光測距儀的全向移動機(jī)器人的自主導(dǎo)航裝置，其中，包括測量移動機(jī)器人輪子的移動距離的編碼器、測量移動機(jī)器人的旋轉(zhuǎn)角速度和加速度的慣性測量單元，編碼器和慣性測量單元分別連接控制器，還包括激光測距儀，激光測距儀連接上位機(jī)，上位機(jī)連接控制器，控制器連接執(zhí)行單元。

本實(shí)施例中，車載編碼器測量移動機(jī)器人輪子的移動距離，慣性測量單元測量移動機(jī)器人的旋轉(zhuǎn)角速度和加速度，車載嵌入式控制器獲取編碼器和慣性測量單元的測量值，結(jié)合移動機(jī)器人的運(yùn)動學(xué)模型計(jì)算其位姿信息，發(fā)送給上位機(jī)系統(tǒng)；上位機(jī)系統(tǒng)同時(shí)獲取激光測距儀的激光點(diǎn)云信息，經(jīng)過算法計(jì)算得到移動機(jī)器人的運(yùn)動速度的期望值(包括平移速度和旋轉(zhuǎn)速度)，并將運(yùn)動速度期望值發(fā)送給車載嵌入式控制器；嵌入式控制器通過移動機(jī)器人運(yùn)動學(xué)模型反解出車輪的運(yùn)動速度，再應(yīng)用PID算法對車輪的轉(zhuǎn)速進(jìn)行閉環(huán)控制。

如圖2所示，移動機(jī)器人底盤由三個(gè)全向輪按等邊三角形分布構(gòu)成，通過控制三個(gè)全向輪的轉(zhuǎn)速可以實(shí)現(xiàn)全向移動機(jī)器人在平面上360°全方位的平移以及旋轉(zhuǎn)。全向移動機(jī)器人的運(yùn)動分為旋轉(zhuǎn)和平移兩部分。令三個(gè)車輪的速度分別為(v₁,v₂,v₃)，移動機(jī)器人體坐標(biāo)系沿x軸和y軸的移動速度為(v_x,v_y)。在只有平移沒有旋轉(zhuǎn)的情況下，全向移動機(jī)器人的運(yùn)動學(xué)方程為：

當(dāng)只存在旋轉(zhuǎn)，沒有平移的時(shí)候，旋轉(zhuǎn)角速度為w，則有：

v₁＝v₂＝v₃＝L*w. (2)

當(dāng)同時(shí)存在旋轉(zhuǎn)和平移時(shí)，各全向輪的速度為平移和旋轉(zhuǎn)運(yùn)動的速度疊加。若已知各全向輪的移動速度，可推導(dǎo)移動機(jī)器人的平移速度和旋轉(zhuǎn)速度。由于平移和旋轉(zhuǎn)運(yùn)動的強(qiáng)耦合性，在控制過程中需盡量讓機(jī)器人不同時(shí)進(jìn)行平移和旋轉(zhuǎn)運(yùn)動，以提高移動機(jī)器人位姿估計(jì)的準(zhǔn)確性。

同步定位與地圖構(gòu)建模塊如圖3所示，根據(jù)機(jī)載里程計(jì)和慣性傳感器獲取移動機(jī)器人的移動信息，結(jié)合運(yùn)動學(xué)模型預(yù)估移動機(jī)器人的位姿信息。在當(dāng)前已有的地圖中獲取預(yù)估位置周圍的點(diǎn)云信息，作為虛擬的模型點(diǎn)集，同時(shí)使用激光測距儀獲取的當(dāng)前環(huán)境真實(shí)的點(diǎn)云信息作為數(shù)據(jù)點(diǎn)集，兩者進(jìn)行ICP算法匹配，得到位姿信息全局矯正值。另一方面使用上一次獲取的激光點(diǎn)云信息和當(dāng)前獲取的點(diǎn)云信息作ICP匹配得到位姿信息的局部矯正值。根據(jù)匹配的誤差值對兩組矯正值做加權(quán)得到最后的矯正位姿信息。同時(shí)更新地圖信息，加入當(dāng)前的激光觀測值。

迭代最近點(diǎn)算法是同步定位與地圖構(gòu)建模塊的核心算法，其通過迭代循環(huán)尋找數(shù)據(jù)點(diǎn)集跟模型點(diǎn)集的最近點(diǎn)，構(gòu)成匹配對，并求解數(shù)據(jù)點(diǎn)集相對于與模型點(diǎn)集的旋轉(zhuǎn)矩陣R_k和平移向量T_k，使得每次迭代中相似性度量S_LTF最小。ICP及其各種改進(jìn)歸類可歸為以下幾個(gè)步驟：1.選擇數(shù)據(jù)點(diǎn)集P和模型點(diǎn)集M的子集。2.尋找匹配點(diǎn)集對。3給每個(gè)匹配對賦予權(quán)重。4.剔除某些匹配對。5尋找?guī)缀巫儞Q最小化相似性度量S_LTF。具體步驟如下:

1：初始化旋轉(zhuǎn)矩陣R₀和平移向量T₀，隨機(jī)選取或者通過其它條件獲得。

2：對數(shù)據(jù)點(diǎn)集P的每個(gè)點(diǎn)都在模型點(diǎn)集中找到最近的一個(gè)點(diǎn)構(gòu)成匹配對，同時(shí)去掉錯(cuò)誤的匹配對，構(gòu)成匹配點(diǎn)集對:

N＝{(i,j)|x_i∈P,y_j∈M,y_j＝argmin(||(R_k-₁x_i+T_k-₁-y_j)||²)}. (3)

匹配距離平方之和為

3：尋找最優(yōu)的幾何變換(即旋轉(zhuǎn)矩陣R_k和平移向量T_k)使得變換后的數(shù)據(jù)點(diǎn)集P’和模型點(diǎn)集M距離平方之和(即相似性度量)最?。?/p>

4：若達(dá)到滿足停止條件或者迭代次數(shù)達(dá)到上限值匹配結(jié)束，否則返回步驟2。ICP算法的停止條件有兩種：(1)標(biāo)準(zhǔn)差e＝S_LTS/|N_p|足夠小；(2)前后兩次的標(biāo)準(zhǔn)差變化|e-e'|/e足夠小。

在步驟2中，尋找最近點(diǎn)的方法常用的有基于k-d樹和基于Delaunary三角劃分的最近點(diǎn)搜索算法。實(shí)驗(yàn)研究結(jié)果表明，k-d樹適合高維點(diǎn)集的點(diǎn)搜索，而基于Delaunay三角化算法更適合于低維點(diǎn)集，故本發(fā)明考慮使用基于Delaunary三角劃分的最近點(diǎn)搜索算法。該算法將散點(diǎn)集合劃分成不均勻的三角形網(wǎng)格，且網(wǎng)格滿足兩個(gè)基本準(zhǔn)則，其一是空圓特性，即在Delaunay三角形網(wǎng)中任一個(gè)三角形網(wǎng)格的外接圓范圍內(nèi)不會有其它點(diǎn)存在。其二是最大化最小角特性，即在散點(diǎn)集可能形成的三角剖分中，Delaunay三角剖分所形成的三角形的最小角最大。在ICP算法中，首先將模型點(diǎn)集M進(jìn)行Delaunary三角劃分，然后采用三角劃分搜索策略找到數(shù)據(jù)點(diǎn)集P在模型點(diǎn)集中的最近點(diǎn)作為對應(yīng)點(diǎn)，構(gòu)成匹配對。

理想情況下，數(shù)據(jù)點(diǎn)集P和模型點(diǎn)集M能夠完全匹配，若數(shù)據(jù)點(diǎn)集中一點(diǎn)x_p在模型點(diǎn)集M中找到對應(yīng)點(diǎn)y_pm，則模型點(diǎn)集中y_pm在數(shù)據(jù)點(diǎn)集中找到的對應(yīng)點(diǎn)也應(yīng)該是x_p。可依此作為匹配對的評判準(zhǔn)則。令數(shù)據(jù)點(diǎn)集中P_x1在模型點(diǎn)集中的對應(yīng)點(diǎn)為M_y1，模型點(diǎn)集中M_y1的在數(shù)據(jù)點(diǎn)集的對應(yīng)點(diǎn)為P_x2，P_x2的對應(yīng)點(diǎn)為M_y2。當(dāng)滿足下列關(guān)系是，我們認(rèn)為對應(yīng)點(diǎn)的尋找是正確的，否則剔除該對應(yīng)關(guān)系。

其中ε某個(gè)跟標(biāo)準(zhǔn)差相關(guān)的常數(shù)。

步驟3中的尋找最佳的幾何變換常用方法有SVD奇異值分解，四元組，正交矩陣，雙四元組方法等，本方案采用的是SVD分解的方法。

在同步定位與建圖過程中，將預(yù)估的移動機(jī)器人的位姿信息作為ICP算法的初始估計(jì)，估計(jì)值與真值只有微小誤差，可大大減小ICP算法的迭代次數(shù)，提高收斂速度。同時(shí)使用已有的地圖上預(yù)估的移動機(jī)器人位置周圍的點(diǎn)云信息作為模型點(diǎn)集，進(jìn)一步減少模型點(diǎn)集的大小，提高ICP算法計(jì)算速度。

在已知環(huán)境地圖和初始位置P_s的情況下，設(shè)定目標(biāo)位置P_e，使用快速擴(kuò)展隨機(jī)樹(RRT)算法進(jìn)行全局路徑規(guī)劃。RRT算法是以狀態(tài)空間中的一個(gè)初始點(diǎn)作為根節(jié)點(diǎn)，通過隨機(jī)采樣擴(kuò)展的方式，逐漸增加葉節(jié)點(diǎn)，最終生成一棵隨機(jī)擴(kuò)展樹。當(dāng)隨機(jī)樹的葉節(jié)點(diǎn)中包含了目標(biāo)點(diǎn)或目標(biāo)區(qū)域中的點(diǎn)時(shí)，從初始點(diǎn)到目標(biāo)點(diǎn)之間的一條以隨機(jī)樹的葉節(jié)點(diǎn)組成的路徑就是規(guī)劃路徑。規(guī)劃路徑以一系列的離散節(jié)點(diǎn)形式給出，相鄰節(jié)點(diǎn)間不存在障礙物，可根據(jù)規(guī)劃的路徑直接到達(dá)目標(biāo)位置點(diǎn)。考慮到在實(shí)際環(huán)境中，由于移動機(jī)器人運(yùn)動慣性以及未知障礙物的存在，移動機(jī)器人不能嚴(yán)格按照規(guī)劃的路徑運(yùn)動。在以節(jié)點(diǎn)形式給出的規(guī)劃路徑中，移動機(jī)器人可將一個(gè)個(gè)節(jié)點(diǎn)作為子運(yùn)動目標(biāo)，根據(jù)實(shí)際的情況進(jìn)行局部路徑規(guī)劃，減少由于偏離軌跡或未知障礙物等原因而需要重新規(guī)劃全局路徑的情況，提高了導(dǎo)航過程的魯棒性。

考慮到移動機(jī)器人的體積大小，使用RRT算法尋找路徑前先對環(huán)境地圖進(jìn)行膨脹處理，膨脹的區(qū)域稱為碰撞區(qū)域S，從而將移動機(jī)器人的體積轉(zhuǎn)移到障礙物中。在RRT算法過程中，將移動機(jī)器人看為質(zhì)點(diǎn)處理，當(dāng)代表移動機(jī)器人的質(zhì)點(diǎn)位于碰撞區(qū)域時(shí)，在實(shí)際環(huán)境中，移動機(jī)器人已經(jīng)與障礙物發(fā)生碰撞。

RRT算法假設(shè)搜索樹為T，當(dāng)前節(jié)點(diǎn)數(shù)目為N，其節(jié)點(diǎn)為ti_j(t_j)，j為當(dāng)前節(jié)點(diǎn)編號，i為其父節(jié)點(diǎn)編號。以初始位置作為搜索樹的根節(jié)點(diǎn)t₀₁，地圖上兩點(diǎn)構(gòu)成的線段為L(p₁,p₂)。在地圖空白區(qū)域產(chǎn)生隨機(jī)點(diǎn)p(即當(dāng)移動機(jī)器人位于隨機(jī)點(diǎn)時(shí)不碰撞障礙物)。在目前已有的隨機(jī)樹中找到跟隨機(jī)點(diǎn)最近的可直接相通的節(jié)點(diǎn)p_k，即節(jié)點(diǎn)和隨機(jī)點(diǎn)之間沒有障礙物遮擋且不存在碰撞區(qū)域。

將該節(jié)點(diǎn)作為父節(jié)點(diǎn)，沿父節(jié)點(diǎn)和隨機(jī)點(diǎn)的連線方向擴(kuò)展搜索樹，產(chǎn)生新的子節(jié)點(diǎn)t_k(N+1)，且子節(jié)點(diǎn)和父節(jié)點(diǎn)的距離為步長s。

t_k(N+1)＝{t|t∈L(t_k,p),||t-t_k||＝s}. (9)

不斷產(chǎn)生隨機(jī)點(diǎn)擴(kuò)展隨機(jī)樹，直到隨機(jī)樹的子節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)，從該節(jié)點(diǎn)及其父節(jié)點(diǎn)回溯即為找到的路徑path＝(p₁,p₂,...,p_n),n為路徑的節(jié)點(diǎn)數(shù)目。

通過RRT算法得到以一系列節(jié)點(diǎn)構(gòu)成的路徑，但往往不是最優(yōu)路徑，不相鄰的節(jié)點(diǎn)存在直接到達(dá)的可能，可對其進(jìn)一步優(yōu)化。優(yōu)化步驟如下：

從初始節(jié)點(diǎn)開始，判斷節(jié)點(diǎn)i和節(jié)點(diǎn)i+2是否相通，即移動機(jī)器人可否直接從節(jié)點(diǎn)i直線運(yùn)動到節(jié)點(diǎn)i+2；若可以則去掉節(jié)點(diǎn)i+1，下一步判斷節(jié)點(diǎn)i和節(jié)點(diǎn)i+3；若不能直接相通，下一步判斷節(jié)點(diǎn)i+1和節(jié)點(diǎn)i+3。以此類推。類似的也可同時(shí)從目標(biāo)點(diǎn)位置開始判斷。

位置跟蹤與自主避障，該模塊包括位置跟蹤和避障兩部分。位置跟蹤得到準(zhǔn)確的位姿信息作為避障算法的輸入，避障算法依據(jù)規(guī)劃路徑和當(dāng)前的位姿信息計(jì)算期望的移動速度，并結(jié)合激光傳感器信息對期望速度進(jìn)行修正，最后將修正的運(yùn)動速度向移動機(jī)器人發(fā)送。如圖4所示。

跟蹤定位過程與定位建圖過程類似，使用里程計(jì)和慣性傳感器預(yù)估移動機(jī)器人的位姿，使用ICP算法對位姿信息進(jìn)行修正。由于激光信息的獲取頻率最高只有10HZ，ICP算法在最差的情況計(jì)算頻率為3HZ。為了保證對機(jī)器人的實(shí)時(shí)控制，設(shè)計(jì)了內(nèi)環(huán)和外環(huán)兩部分的跟蹤算法。內(nèi)環(huán)部分將航位推測法預(yù)估的位姿信息作為避障算法部分的輸入，控制頻率為15Hz，外環(huán)使用ICP算法對位姿進(jìn)行修正，將修正的位姿信息作為避障算法部分的輸入，最低控制頻率為3HZ。如圖3所示，在t時(shí)刻得到了移動機(jī)器人的預(yù)估位姿，包括世界坐標(biāo)下x軸和y軸的位置以及移動機(jī)器人的偏航角X_t＝(x_t,y_t,α_t)，同時(shí)將獲取的激光信息與已知地圖的信息進(jìn)行ICP算法匹配，此時(shí)將預(yù)估位姿X_t作為避障算法的輸入。在t+1,t+2,…,t+k-1時(shí)刻，將當(dāng)前時(shí)刻預(yù)估的位姿信息X_t+1,X_t+2,...,X_t+k-1作為避障算法的輸入。在t+k時(shí)刻預(yù)估得到的移動機(jī)器人的位姿為X_t+k，此時(shí)t時(shí)刻開始進(jìn)行運(yùn)算的ICP算法完成匹配，得到t時(shí)刻的修正位姿考慮激光測距儀的采樣頻率，我們設(shè)定t時(shí)刻和t+k時(shí)刻的時(shí)間間隔需大于0.1s。由此移動機(jī)器人的修正位姿為：

我們將修正的位姿作為避障算法的輸入，同時(shí)獲取t+k時(shí)刻的激光信息，按照上述步驟再次進(jìn)行ICP算法的匹配以修正機(jī)器人的位姿信息。

如圖4所示，在獲取了移動機(jī)器人的位姿信息后，避障算法依據(jù)實(shí)時(shí)環(huán)境信息作局部路徑規(guī)劃。首先確定當(dāng)前的運(yùn)動目標(biāo)，由于規(guī)劃路徑以節(jié)點(diǎn)的形式給出，每一個(gè)節(jié)點(diǎn)即為一個(gè)子運(yùn)動目標(biāo)，當(dāng)?shù)竭_(dá)當(dāng)前目標(biāo)節(jié)點(diǎn)附近或判斷移動機(jī)器人位置可越過當(dāng)前目標(biāo)節(jié)點(diǎn)沿直線運(yùn)動到下一節(jié)點(diǎn)時(shí)，則作目標(biāo)節(jié)點(diǎn)切換，也即將下一個(gè)路徑節(jié)點(diǎn)作為目標(biāo)節(jié)點(diǎn)。之后根據(jù)當(dāng)前位置和目標(biāo)節(jié)點(diǎn)位置計(jì)算移動機(jī)器人的期望運(yùn)動方向，利用避障算法對運(yùn)動方向進(jìn)行修正，并將計(jì)算的平移與旋轉(zhuǎn)速度發(fā)送給移動機(jī)器人控制其運(yùn)動。

RRT算法規(guī)劃了一條可行路徑，但在實(shí)際控制過程中，由于移動機(jī)器人的運(yùn)動學(xué)特性以及環(huán)境中存在未知障礙物等原因，移動機(jī)器人并不會完全按照預(yù)期的路徑運(yùn)動，需要根據(jù)當(dāng)前的實(shí)時(shí)位姿信息、移動速度以及環(huán)境信息進(jìn)行局部路徑規(guī)劃，以避開障礙物同時(shí)向目標(biāo)靠近。

局部路徑規(guī)劃算法如圖5所示。安全距離設(shè)定為d。令當(dāng)前位置為S，目標(biāo)位置為E，理想的運(yùn)動方向?yàn)閂_SE。利用上述信息預(yù)測下一個(gè)控制周期內(nèi)的預(yù)測位置P。將避障過程分為以下四種情況：

(1)圖5(a)所示，障礙物中與當(dāng)前位置S距離最近的點(diǎn)為O1，如果兩者距離SO1小于d，則表明移動機(jī)器人即將發(fā)生碰撞，此時(shí)不考慮目標(biāo)點(diǎn)的位置，移動機(jī)器人向遠(yuǎn)離點(diǎn)O1的方向運(yùn)動。

(2)若SO1大于d，則找到距離預(yù)測位置P最近的障礙物點(diǎn)O，以點(diǎn)O為圓心，d為半徑建立障礙區(qū)域VO，即圖5(b)中的陰影部分。若期望的運(yùn)動方向V_SE沒有落在該區(qū)域內(nèi)，則表明機(jī)器人將不會跟該點(diǎn)碰撞，可以保持期望速度不變。

(3)如圖5(c)所示，若V_SE落在障礙區(qū)域VO區(qū)間內(nèi)，在有限時(shí)間內(nèi)，移動機(jī)器人將發(fā)生碰撞，此時(shí)需要對速度方向進(jìn)行修正。此時(shí)若點(diǎn)P位于障礙區(qū)域之外，尋找點(diǎn)S與圓VO的切點(diǎn)T，則最終的運(yùn)動方向?qū)榍芯€方向：

(4)如圖5(d)所示，若點(diǎn)P位于障礙區(qū)域VO之內(nèi)，此時(shí)速度V_SE的修正量為方向，修正后的速度為：

由于使用的激光測距儀的測量范圍為240°，當(dāng)機(jī)器人最終前進(jìn)方向與當(dāng)前方向相差較大時(shí)，令其做旋轉(zhuǎn)運(yùn)動，以使移動機(jī)器人能夠充分測量到前進(jìn)方向的環(huán)境信息，避免因檢測不到障礙物而發(fā)生碰撞。在其余的情況下，控制移動機(jī)器人僅做平移運(yùn)動，避免因同時(shí)發(fā)生旋轉(zhuǎn)和平移運(yùn)動而導(dǎo)致定位誤差增加。

顯然，本發(fā)明的上述實(shí)施例僅僅是為清楚地說明本發(fā)明所作的舉例，而并非是對本發(fā)明的實(shí)施方式的限定。對于所屬領(lǐng)域的普通技術(shù)人員來說，在上述說明的基礎(chǔ)上還可以做出其它不同形式的變化或變動。這里無需也無法對所有的實(shí)施方式予以窮舉。凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明權(quán)利要求的保護(hù)范圍之內(nèi)。