服務(wù)機(jī)器人導(dǎo)航與路徑規(guī)劃技術(shù)研究

何 寧，趙 彬，毛 欣

（1.重慶水利電力職業(yè)技術(shù)學(xué)院，重慶 永川 402160；2.沈陽新松機(jī)器人自動(dòng)化股份有限公司，遼寧 沈陽 110168；3.國(guó)網(wǎng)重慶市電力公司永川供電分公司，重慶 永川 402160）

1 引言

隨著機(jī)器人技術(shù)和信息技術(shù)的結(jié)合，面向家庭應(yīng)用的智能服務(wù)機(jī)器人必將成為未來數(shù)字家庭的主導(dǎo)。移動(dòng)機(jī)器人已經(jīng)逐步走入到我們的日常生活中，其最具典型的應(yīng)用功能包含：服務(wù)、清潔、衛(wèi)生、房屋維修、家庭服務(wù)、病人護(hù)理和商業(yè)宣傳等[1-2]。隨著相關(guān)技術(shù)的進(jìn)一步發(fā)展，為了能夠滿足社會(huì)生活和生產(chǎn)的需求，獨(dú)立決策、移動(dòng)機(jī)器人感知和最優(yōu)控制等功能越來越受到國(guó)內(nèi)外學(xué)者的關(guān)注。動(dòng)態(tài)環(huán)境實(shí)時(shí)建模和導(dǎo)航技術(shù)是家用智能服務(wù)機(jī)器人研究和開發(fā)的關(guān)鍵技術(shù)，是機(jī)器人在家庭環(huán)境安全可靠運(yùn)行、自主完成任務(wù)的基礎(chǔ)和保障。

移動(dòng)機(jī)器人依靠機(jī)器人核心信息處理單元，主要功能提供多個(gè)傳感器信息融合技術(shù)、自主學(xué)習(xí)技術(shù)和最優(yōu)規(guī)劃控制技術(shù)。移動(dòng)機(jī)器人是由多個(gè)傳感器組成的，其中包含本身和其周邊地區(qū)各種各樣的信息，信息經(jīng)過數(shù)據(jù)融合進(jìn)行智能處理。自主移動(dòng)機(jī)器人還可以感知位置環(huán)境信息，從而幫助它的運(yùn)動(dòng)和控制決策等。國(guó)內(nèi)外學(xué)者對(duì)移動(dòng)機(jī)器人也進(jìn)行了深入的研究。例如：研究文獻(xiàn)[3]對(duì)基于網(wǎng)格的移動(dòng)機(jī)器人地圖構(gòu)建進(jìn)行了深入的研究，它闡述了移動(dòng)機(jī)器人地圖構(gòu)建的詳細(xì)過程的。研究文獻(xiàn)[4]介紹了基于衛(wèi)星圖像拓?fù)涞闹悄芤苿?dòng)機(jī)器人地圖構(gòu)建方法。文獻(xiàn)[5]提出了一種基于視覺的方式來移動(dòng)機(jī)器人的導(dǎo)航，并使用激光投影建地圖，重建了三維地圖，可以用于未知環(huán)境的導(dǎo)航控制[5-6]。結(jié)合我國(guó)對(duì)服務(wù)機(jī)器人產(chǎn)業(yè)發(fā)展的重大需求，瞄準(zhǔn)智能服務(wù)機(jī)器人技術(shù)發(fā)展的前沿，探索適合于在家庭動(dòng)態(tài)環(huán)境下應(yīng)用的服務(wù)機(jī)器人導(dǎo)航系統(tǒng)體系結(jié)構(gòu)，以解決目前國(guó)內(nèi)在動(dòng)態(tài)非結(jié)構(gòu)環(huán)境下的家用服務(wù)機(jī)器人的實(shí)時(shí)環(huán)境建模和導(dǎo)航技術(shù)中存在的問題和不足。

2 運(yùn)動(dòng)控制

2.1 移動(dòng)機(jī)器人平臺(tái)

為移動(dòng)機(jī)器人平臺(tái)，并且承載激光導(dǎo)航，如圖1所示。移動(dòng)機(jī)器人平臺(tái)采用的是雙輪差動(dòng)方式結(jié)構(gòu)的移動(dòng)底盤平臺(tái)，該底盤平臺(tái)能夠適應(yīng)各種復(fù)雜地形，在小區(qū)域能夠行走自如。從而使得導(dǎo)航控制更為靈活，無運(yùn)動(dòng)控制障礙。

圖1 移動(dòng)機(jī)器人平臺(tái)Fig.1 The Mobile Robot Platform

一般來說移動(dòng)機(jī)器人的運(yùn)動(dòng)模式：目前規(guī)劃三種運(yùn)動(dòng)模式，分別是平移運(yùn)動(dòng)，原地旋轉(zhuǎn)和弧線運(yùn)動(dòng)。平移運(yùn)動(dòng)，角速度W為0，根據(jù)V和V_theta分解出x方向線速度和y方向線速度，進(jìn)行速度，運(yùn)動(dòng)過程中，機(jī)器人的theta值不變（朝向不變）。原地旋轉(zhuǎn)，V為0，根據(jù)W值進(jìn)行旋轉(zhuǎn)?；【€運(yùn)動(dòng)，同時(shí)存在線速度和角速度，進(jìn)行圓弧運(yùn)動(dòng)。

2.2 差動(dòng)控制算法

采用雙輪差動(dòng)方式運(yùn)動(dòng)的移動(dòng)機(jī)器人，其左右輪輪速分別為 Vl，Vr，車體寬度為 d，，整車運(yùn)動(dòng)軌跡的圓半徑為 r，在 Vl＜=Vr的情況下，由左右輪差速所構(gòu)造的運(yùn)動(dòng)軌跡分析如下。

左輪若當(dāng)做質(zhì)點(diǎn)運(yùn)動(dòng)有：

右輪若當(dāng)做質(zhì)點(diǎn)運(yùn)動(dòng)有：

在實(shí)際中，左右輪位于機(jī)器人本體上，運(yùn)動(dòng)的角速度相同，所以有wl=wr，車體質(zhì)點(diǎn)運(yùn)動(dòng)速度為：

當(dāng) Vl＞Vr時(shí)，情況與此相同，則公式變?yōu)橼呄蛘?fù)無窮的時(shí)候，車走直線，r=0時(shí)車原地旋轉(zhuǎn)，r＞0時(shí)車體向左側(cè)旋轉(zhuǎn)，r＜0時(shí)車體向右側(cè)旋轉(zhuǎn)。以上為計(jì)算碼盤位姿公式，根據(jù)讀取的碼盤值，計(jì)算出位姿中各個(gè)成員的增量。每次對(duì)地圖類中的各個(gè)數(shù)據(jù)進(jìn)行更新。

3 移動(dòng)機(jī)器人導(dǎo)航定位

導(dǎo)航系統(tǒng)最為關(guān)鍵的技術(shù)包括定位技術(shù)、環(huán)境建模技術(shù)和路徑規(guī)劃技術(shù)[7-9]。環(huán)境建模與定位是移動(dòng)機(jī)器人導(dǎo)航研究的基礎(chǔ)，也是移動(dòng)機(jī)器人真正實(shí)現(xiàn)自主的最重要的。導(dǎo)航系統(tǒng)的功能是讓機(jī)器人能夠感知自己的位置和周圍的環(huán)境信息，并且能夠按照某一性能指標(biāo)搜索出一條從起始狀態(tài)到目標(biāo)狀態(tài)的最優(yōu)或次最優(yōu)的無碰路徑。

3.1 可靠環(huán)境感知方法

環(huán)境感知是地圖創(chuàng)建的基礎(chǔ)。二維激光雷達(dá)對(duì)環(huán)境離散化程度高，受遮蔽等影響大。面向定位的激光雷達(dá)數(shù)據(jù)通用特征提取方法利用環(huán)境的先驗(yàn)知識(shí)可以很容易的解決這些問題，提高特征提取的精度，但在環(huán)境不符合先驗(yàn)知識(shí)的情況下，將產(chǎn)生大量不穩(wěn)定的特征，嚴(yán)重影響定位系統(tǒng)的魯棒性。為了解決這些問題，我們?cè)趯?duì)數(shù)據(jù)進(jìn)行濾波、分割的基礎(chǔ)上提取這些離散點(diǎn)構(gòu)成的網(wǎng)格的結(jié)構(gòu)張量。這些信息穩(wěn)定且可以廣泛的應(yīng)用于多邊形、弧形及叢狀物體（Blob）的激光雷達(dá)觀測(cè)之中。本系統(tǒng)中特征提取方法的特征重復(fù)度＞90%，位置誤差＜0.05m（平均誤差），不確定度估計(jì)準(zhǔn)確度＞80%（統(tǒng)計(jì)誤差）。

3.2 魯棒數(shù)據(jù)關(guān)聯(lián)方法

經(jīng)典的聯(lián)合相容性分支界定樹數(shù)據(jù)關(guān)聯(lián)方法關(guān)聯(lián)率相對(duì)較低，計(jì)算復(fù)雜度高，要求構(gòu)建全協(xié)方差矩陣[9]。構(gòu)建式（1），基于后驗(yàn)概率的聯(lián)合相容性驗(yàn)證函數(shù)可以有效解決經(jīng)典方法所存在的問題。

式中：IHt，S—狀態(tài)變量估計(jì)位置與關(guān)聯(lián)位置之間的距離（新息）；IHt，O—激光雷達(dá)特征估計(jì)位置與關(guān)聯(lián)位置之間的距離（新息）；Pt—狀態(tài)變量的協(xié)方差矩陣；Put—投影矩陣估計(jì)值的不確定度矩陣；V—激光雷達(dá)特征的協(xié)方差矩陣；J1及J2是投影函數(shù)f（·）對(duì)視覺系統(tǒng)位置和投影距離的偏導(dǎo)數(shù)。

定義：S—投影函數(shù)所構(gòu)建的協(xié)方差矩陣；H—f（·）的Hessian矩陣，K=PHTS-1則為卡爾曼濾波增益矩陣；G—將激光雷達(dá)特征投影到視覺傳感器坐標(biāo)系下的投影矩陣對(duì)激光雷達(dá)坐標(biāo)系的Hessian矩陣；I—單位矩陣，則有：

圖2 錯(cuò)誤數(shù)據(jù)關(guān)聯(lián)對(duì)地圖精度的影響Fig.2 Effect of Incorrect Data Association on Map Accuracy

錯(cuò)誤數(shù)據(jù)關(guān)聯(lián)對(duì)地圖精度的影響。錯(cuò)誤的數(shù)據(jù)關(guān)聯(lián)會(huì)嚴(yán)重影響創(chuàng)建地圖的精度。圖2（a）和圖2（b）分別為對(duì)地圖進(jìn)行優(yōu)化時(shí)，狀態(tài)變量鏈接關(guān)系圖中包含十次和一百次錯(cuò)誤關(guān)聯(lián)所產(chǎn)生的地圖。由此可見，由于卡爾曼濾波極小化了均方誤差，因此，式（1）所定義的成本函數(shù)可以極小化關(guān)聯(lián)誤差。本系統(tǒng)采用的數(shù)據(jù)關(guān)聯(lián)方法正確關(guān)聯(lián)率＞90%，錯(cuò)誤關(guān)聯(lián)率＜0.2%。

3.3 定位與地圖匹配

地圖匹配與合并可以將創(chuàng)建的局部地圖合并入全局地圖中，或?qū)⒍鄠€(gè)局部地圖合并創(chuàng)建全局地圖。地圖匹配與合并的核心是計(jì)算正確的地圖轉(zhuǎn)移量。在二維環(huán)境中，這個(gè)地圖轉(zhuǎn)移量包含在X和Y方向的平移距離以及一個(gè)旋轉(zhuǎn)角。地圖轉(zhuǎn)移量的計(jì)算需要找到地圖之間的相似目標(biāo)，進(jìn)而驗(yàn)證地圖根據(jù)該相似目標(biāo)進(jìn)行合并后的似然函數(shù)。

圖3 特征地圖合并Fig.3 Merge Feature Map

給出了一個(gè)地圖合并的例子，圖 3（a），圖 3（b）包含同樣特征物體的兩個(gè)地圖，圖3（c）為地圖合并后的結(jié)果。地圖合并的核心在于有效特征的提取與數(shù)據(jù)關(guān)聯(lián)。本部分的算法內(nèi)容與地圖創(chuàng)建中的特征提取與數(shù)據(jù)關(guān)聯(lián)基本一致，在此不再贅述。

4 移動(dòng)機(jī)器人路徑規(guī)劃

4.1 動(dòng)態(tài)環(huán)境下的移動(dòng)機(jī)器人路徑規(guī)劃

將全局路徑規(guī)劃方法和局部路徑規(guī)劃方法相結(jié)合、將基于反應(yīng)的行為規(guī)劃和基于慎思的行為規(guī)劃相結(jié)合，解決路徑規(guī)劃算法的合理性、完備性、最優(yōu)性、實(shí)時(shí)性以及對(duì)環(huán)境變化的適應(yīng)性問題，同時(shí)保證機(jī)器人在動(dòng)態(tài)不確定環(huán)境下能夠在滿足自身運(yùn)動(dòng)學(xué)約束的前提下以最短路徑代價(jià)運(yùn)動(dòng)到目標(biāo)點(diǎn)。

4.2 Wavefront法

由于室內(nèi)環(huán)境規(guī)模較小，環(huán)境內(nèi)障礙物相對(duì)較多，一般室內(nèi)環(huán)境都采用柵格地圖或拓?fù)涠攘康貓D標(biāo)示。本系統(tǒng)擬采用柵格地圖表征環(huán)境。在柵格地圖中，采用Wavefront方法進(jìn)行路徑規(guī)劃是一種簡(jiǎn)潔、可靠的選擇。Wavefront方法示意圖，如圖4所示。

圖4 Wavefront路徑規(guī)劃方法示意圖Fig.4 Wavefront Path Planning Method

Wavefront方法將地圖看做一個(gè)可導(dǎo)熱的物體，環(huán)境中的障礙物為熱絕緣體；出發(fā)點(diǎn)被看做熱源，熱量從熱源散布開去。當(dāng)熱量到達(dá)目標(biāo)點(diǎn)時(shí)，熱量停止擴(kuò)散，同時(shí)產(chǎn)生有效路徑。然而，由于計(jì)算復(fù)雜度的原因，Wavefront方法不適用于大規(guī)模環(huán)境之中。利用這種方法，可以確保有效路徑的探測(cè)率達(dá)到100%，同時(shí)，可以根據(jù)應(yīng)用需求，在安全性和能量效率之間進(jìn)行平衡，產(chǎn)生定義的最優(yōu)路徑。

5 功能測(cè)試

功能測(cè)試包含在測(cè)試環(huán)境內(nèi)對(duì)激光傳感器測(cè)試，對(duì)定位精度測(cè)試和路徑規(guī)劃性能測(cè)試。首先指定機(jī)器人起始點(diǎn)，命令機(jī)器人導(dǎo)航運(yùn)動(dòng)至目標(biāo)點(diǎn)。使用尺、量角器進(jìn)行實(shí)際測(cè)量，測(cè)出機(jī)器人實(shí)際的位置與朝向角與起始點(diǎn)的偏差。同時(shí)通過遠(yuǎn)程桌面實(shí)時(shí)監(jiān)控是否丟失位置，是否拾取路徑。

5.1 激光傳感器測(cè)試

激光傳感器的參數(shù)比對(duì)，如表1所示。（1）對(duì)激光傳感器的測(cè)量：獲得激光傳感器在離障礙物不同距離時(shí)的測(cè)量值，并與實(shí)際值（50cm間隔，共測(cè)10點(diǎn)）進(jìn)行比對(duì)，從而獲得測(cè)距精度結(jié)果，并確定傳感器的可用量程。（2）不同介質(zhì)測(cè)量：使激光傳感器對(duì)玻璃材質(zhì)（純玻璃、貼膠玻璃）的障礙物與非玻璃材質(zhì)的障礙物進(jìn)行測(cè)距，并比對(duì)結(jié)果。

表1 激光傳感器的參數(shù)比對(duì)Tab.1 Parameters of Laser Sensor

5.2 基于激光傳感器自定位算法的精度測(cè)試

建立好地圖后，遙控機(jī)器人運(yùn)動(dòng)到某處后，運(yùn)行軟件得出機(jī)器人在全局地圖中的位姿。使用尺、量角器進(jìn)行實(shí)際測(cè)量，測(cè)出機(jī)器人實(shí)際的位置與朝向角。計(jì)算偏差。測(cè)試10次，取平均值。著重對(duì)以下方面進(jìn)行測(cè)試：（1）對(duì)于走廊區(qū)域等環(huán)境類似場(chǎng)景的定位能力測(cè)試（2）對(duì)于“綁架”問題的處理能力（3）目前軟件算法能適應(yīng)的最大環(huán)境面積，如果通過降低柵格精度的方法增加導(dǎo)航范圍，則要再測(cè)試新配置下的定位精度。為選取一個(gè)固定位置，通過PAD操作創(chuàng)建地圖。地圖說明：地圖左側(cè)為B3樓3樓大廳正門側(cè)，地圖右側(cè)為電梯側(cè)。左右全長(zhǎng)約136m，上下寬約34m。地圖創(chuàng)建完成后，手動(dòng)遙控實(shí)驗(yàn)平臺(tái)運(yùn)動(dòng)，并使用PC端軟件的地圖顯示功能實(shí)時(shí)監(jiān)控實(shí)驗(yàn)平臺(tái)運(yùn)行，確認(rèn)實(shí)驗(yàn)平臺(tái)實(shí)際運(yùn)行位置和地圖上顯示位置是否一致。如圖5所示。為測(cè)試出的B3樓的地圖，經(jīng)過測(cè)試證明，導(dǎo)航效果較為理想。

圖5 實(shí)驗(yàn)平臺(tái)創(chuàng)建的B3樓地圖Fig.5 Experimental Platform Create B3 Floor Map

6 結(jié)論

首先系統(tǒng)介紹了自主移動(dòng)機(jī)器人雙輪差動(dòng)運(yùn)動(dòng)控制問題。其次利用網(wǎng)格的結(jié)構(gòu)張量解決了環(huán)境不符合先驗(yàn)知識(shí)的情況下，將產(chǎn)生大量不穩(wěn)定的特征，實(shí)時(shí)精準(zhǔn)獲取當(dāng)前的二維地圖信息。再次在柵格地圖表征環(huán)境，采用Wavefront方法進(jìn)行路徑規(guī)劃。完整的解決了移動(dòng)機(jī)器人三個(gè)基本問題。最后進(jìn)行了一系列測(cè)試，測(cè)試機(jī)器人通過狹窄區(qū)域的能力、測(cè)試導(dǎo)航性能和測(cè)試導(dǎo)航效果。實(shí)驗(yàn)結(jié)果表明，該導(dǎo)航算法和路徑規(guī)劃算法具有較好的穩(wěn)定性、實(shí)用性和戰(zhàn)略意義。