何 寧，趙 彬，毛 欣

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

1 引言

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

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

2 運動控制

2.1 移動機器人平臺

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

圖1 移動機器人平臺Fig.1 The Mobile Robot Platform

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

2.2 差動控制算法

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

左輪若當(dāng)做質(zhì)點運動有：

右輪若當(dāng)做質(zhì)點運動有：

在實際中，左右輪位于機器人本體上，運動的角速度相同，所以有wl=wr，車體質(zhì)點運動速度為：

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

3 移動機器人導(dǎo)航定位

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

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

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

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

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

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

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

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

錯誤數(shù)據(jù)關(guān)聯(lián)對地圖精度的影響。錯誤的數(shù)據(jù)關(guān)聯(lián)會嚴重影響創(chuàng)建地圖的精度。圖2（a）和圖2（b）分別為對地圖進行優(yōu)化時，狀態(tài)變量鏈接關(guān)系圖中包含十次和一百次錯誤關(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%，錯誤關(guān)聯(lián)率＜0.2%。

3.3 定位與地圖匹配

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

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

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

4 移動機器人路徑規(guī)劃

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

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

4.2 Wavefront法

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

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

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

5 功能測試

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

5.1 激光傳感器測試

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

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

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

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

圖5 實驗平臺創(chuàng)建的B3樓地圖Fig.5 Experimental Platform Create B3 Floor Map

6 結(jié)論

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