未知環(huán)境下機(jī)器人導(dǎo)航算法與避障算法研究

包震洲，俞鴻飛，金文德，李偉達(dá)

（1.國網(wǎng)浙江省電力公司緊水灘水發(fā)電廠，浙江 杭州 310000；2.國網(wǎng)浙江省電力公司，浙江 杭州 310000；3.蘇州大學(xué)機(jī)電工程學(xué)院，江蘇 蘇州 215021）

1 引言

針對(duì)機(jī)器人在復(fù)雜未知的環(huán)境下進(jìn)行環(huán)境的混合導(dǎo)航與三維地圖重建等復(fù)雜問題進(jìn)行了深入研究，全文闡述了機(jī)器人的運(yùn)動(dòng)控制、視覺導(dǎo)航、機(jī)器人避障等問題。

文獻(xiàn)[2]提出了許多用于新型救援機(jī)器人導(dǎo)航的方法以提高導(dǎo)航精度，卡爾曼濾波器基于機(jī)器人的動(dòng)作（如速度，加速度等）和傳感器的輸出來估計(jì)下一個(gè)位置。測量由編碼器和卡爾曼濾波器產(chǎn)生的誤差。實(shí)驗(yàn)結(jié)果表明，卡爾曼濾波器提高了精度[2]。文獻(xiàn)[3]討論網(wǎng)絡(luò)延遲對(duì)使用云環(huán)境的低成本簡單機(jī)器人的遠(yuǎn)程導(dǎo)航的影響，使用機(jī)器人模擬器進(jìn)行遠(yuǎn)程導(dǎo)航的實(shí)驗(yàn)。文獻(xiàn)[4]討論了通過自主機(jī)器人視覺系統(tǒng)捕獲的圖像的處理和識(shí)別，自主機(jī)器人的視覺系統(tǒng)可以獲得和分析用于自主決策的必要的環(huán)境信息。

以自主機(jī)器人混合導(dǎo)航與三維地圖重建為研究對(duì)象，將其研發(fā)的功能分為如下：在將機(jī)器人集成到先進(jìn)制造系統(tǒng)的研究中，利用三維仿真和虛擬現(xiàn)實(shí)技術(shù)實(shí)現(xiàn)了實(shí)時(shí)地圖三維重建。同時(shí)，建立了基于自然語言約束性的混合導(dǎo)航算法，將路標(biāo)和大致方位和距離的關(guān)系映射建立導(dǎo)航地圖，提取關(guān)鍵引導(dǎo)路徑實(shí)現(xiàn)機(jī)器人路徑規(guī)劃，在網(wǎng)格環(huán)境中，進(jìn)行環(huán)境定位和地圖導(dǎo)航，路徑規(guī)劃可以實(shí)現(xiàn)避障找到最優(yōu)路徑。實(shí)驗(yàn)結(jié)果證明了該方法的可行性、有效性和魯棒性。

2 機(jī)器人概述

2.1 機(jī)器人機(jī)械結(jié)構(gòu)

所研究的機(jī)器人，如圖1所示。該機(jī)器人在差動(dòng)底盤部署有視覺傳感器。通過該視覺傳感器實(shí)現(xiàn)地圖重建與導(dǎo)航算法[5]。

圖1 機(jī)器人示意圖Fig.1 Humanoid Arm Diagrammatic Sketch

在整個(gè)分析過程中，把機(jī)器人建模成輪子上的一個(gè)剛體，運(yùn)行在水平面上[6-9]。在水平面上，機(jī)器人的運(yùn)動(dòng)的總維度為3，其中兩個(gè)為其在平面坐標(biāo)系中的橫縱坐標(biāo)，即在水平面上的位置；剩余的一個(gè)是機(jī)器人沿豎直方向的旋轉(zhuǎn)方向，其對(duì)應(yīng)的方向與水平面正交。注意，使用下標(biāo)I是為了闡明該姿態(tài)的基是全局參考框架：

根據(jù)分量的變化描述機(jī)器人的運(yùn)動(dòng)，就需要把沿全局參考框架的運(yùn)動(dòng)映射成沿機(jī)器人局部參考框架軸的運(yùn)動(dòng)。通過單位正交的旋轉(zhuǎn)矩陣來完成機(jī)器人當(dāng)前位置的映射：

可以用該矩陣將全局參考框架｛XI，YI｝中的運(yùn)動(dòng)映射到局部參考框架｛XR，YR｝中的運(yùn)動(dòng)。這個(gè)運(yùn)算由R

IR（θ）ξI標(biāo)記，因?yàn)樵撨\(yùn)算依賴于θ的值：

2.2 柳長安法運(yùn)動(dòng)學(xué)模型

機(jī)器人本體采用雙輪差動(dòng)式驅(qū)動(dòng)結(jié)構(gòu)，其平面幾何模型，如圖2所示。

圖2 機(jī)器人輪系結(jié)構(gòu)的幾何模型Fig.2 Geometric Model of Robot Gear Train Structure

雙輪差動(dòng)模型中主要包含的參數(shù)如下：移動(dòng)平臺(tái)上的關(guān)鍵點(diǎn)用F表示；移動(dòng)平臺(tái)的質(zhì)心用G表示；驅(qū)動(dòng)輪的間距用2l表示；驅(qū)動(dòng)輪的半徑用r表示；質(zhì)心G與關(guān)鍵點(diǎn)F之間的距離用lG表示；平臺(tái)轉(zhuǎn)動(dòng)的角度用φ來表示；左、右驅(qū)動(dòng)輪的轉(zhuǎn)角分別用θl與θr來表示。

對(duì)于移動(dòng)平臺(tái)的質(zhì)心G，其速度為vG，作為一個(gè)矢量，其方向垂直于平臺(tái)的輪軸，因此其在平面上兩個(gè)坐標(biāo)軸上的分量分別為：；消去νG得：

點(diǎn)F與點(diǎn)G有如下位置關(guān)系：

平臺(tái)質(zhì)心的速度與驅(qū)動(dòng)輪轉(zhuǎn)速的關(guān)系式為：

可以得到矩陣表達(dá)式為：

2.3 軟件平臺(tái)架構(gòu)

基于提供更加便利的二次開發(fā)環(huán)境，同時(shí)兼顧軟件的穩(wěn)定性和可擴(kuò)展性的目的，設(shè)計(jì)了軟件平臺(tái)架構(gòu)。為機(jī)器人軟件平臺(tái)架構(gòu)，如圖3所示。這種架構(gòu)模式同時(shí)對(duì)于室內(nèi)和室外兩種工作環(huán)境具有良好的兼容性。軟件平臺(tái)底盤程序支持輪式機(jī)器人、全方位機(jī)器人、蛇形機(jī)器人和雙足機(jī)器人等。軟件平臺(tái)的設(shè)計(jì)流程為在一個(gè)給定的初始位置在運(yùn)動(dòng)的過程中，機(jī)器人使用里程表和陀螺測量環(huán)境，計(jì)算相對(duì)于初始狀態(tài)的變化，確定當(dāng)前位置。

圖3 機(jī)器人平臺(tái)系統(tǒng)Fig.3 Robot Platform System

機(jī)器人運(yùn)動(dòng)學(xué)模型通過測量輪輪機(jī)器人的角速度計(jì)算機(jī)器人的參考中心的移動(dòng)速度和方向。通過傳感器獲得機(jī)器人在太空的位置，方向和環(huán)境信息，然后加工環(huán)境地圖模型建立，找到最優(yōu)路徑，機(jī)器人安全移動(dòng)到目標(biāo)位置機(jī)器人通過自身的傳感器來確定局部信息，通過局部信息與全局位置，推算得到機(jī)器人的軌跡。機(jī)器人根據(jù)全局地圖位置更新并且修正地圖信息進(jìn)行導(dǎo)航控制。最終完成機(jī)器人定位于地圖導(dǎo)航算法。

3 視覺導(dǎo)航

3.1 幾何特征提取

假定測量值符合正態(tài)分布，對(duì)于被測點(diǎn)xi=（ρi，θi），可通過隨機(jī)變量Xi=（Pi，Qi）來逼近測量值，其中：

通常情況下，視覺雷達(dá)掃描到的環(huán)境信息可分割為多段近似直線，經(jīng)由視覺測量數(shù)據(jù)在視覺掃描方向上可確定一條直線為：ρcos（θ-β）-r=0

式中：r—傳感器坐標(biāo)系原點(diǎn)與該特征直線的距離；β—原點(diǎn)到該

直線的鉛垂線與橫軸的夾角。定義評(píng)價(jià)函數(shù)：

求取直線的問題可轉(zhuǎn)化為求取評(píng)價(jià)函數(shù)G的最小值。通過求解二元函數(shù)G的偏導(dǎo)數(shù)的零點(diǎn)，可得：

則由ρ及r可確定環(huán)境直線特征。

3.2 視覺導(dǎo)航建立

設(shè)定坐標(biāo)系｛B｝為導(dǎo)航圖中的參考坐標(biāo)系，其零點(diǎn)位于界面的左上角，其x軸具有與坐標(biāo)系｛A｝相同的方向，其y軸具有與坐標(biāo)系｛A｝相反的方向。

可按照如下的方法將坐標(biāo)系｛A｝與坐標(biāo)系｛B｝中的點(diǎn)，建立映射：

（1）設(shè)定界面中心位置的坐標(biāo)為（u0，v0），以此來表示機(jī)器人在坐標(biāo)系｛B｝中的坐標(biāo)；

（2）li在坐標(biāo)系｛A｝與坐標(biāo)系｛B｝中的坐標(biāo)分別為（u0，v0）與（ui，vi），二者的關(guān)系為：

在式（4）中，j的作用是調(diào)整路標(biāo)在界面上的像素距離。相鄰路標(biāo)的間距大于固定像素閾值是能夠在導(dǎo)航圖中提取到關(guān)鍵點(diǎn)的必要條件。設(shè)定（ui，vi）為li對(duì)應(yīng)的像素坐標(biāo)，則：

其中路標(biāo)的數(shù)量為n．j的取值自1開始遞增，直至有若干個(gè)量剛好未超出界面邊界的臨界情況。據(jù)上可分析得出，NLRP中的距離量的主要作用是建立路標(biāo)在導(dǎo)航映射圖中的相對(duì)位置，而不直接參與到導(dǎo)航中。由于定位和導(dǎo)航高度依賴于視覺系統(tǒng)進(jìn)行，故作為視覺定位中的重要輔助量，像素坐標(biāo)與世界坐標(biāo)的比例尺需要實(shí)時(shí)地進(jìn)行更新。

通過以上幾個(gè)步驟可以建立機(jī)器人和路標(biāo)在導(dǎo)航映射圖中的相對(duì)位置關(guān)系。直接通過提取路標(biāo)周圍的點(diǎn)來提取關(guān)鍵引導(dǎo)點(diǎn)，方法如下：

（1）設(shè)li和li+1在坐標(biāo)系｛B｝中的坐標(biāo)為（ui，vi）、（ui+1，i+1）、li+1對(duì)應(yīng)的關(guān)鍵點(diǎn)的坐標(biāo)是。通常情況下關(guān)鍵引導(dǎo)點(diǎn)與路標(biāo)一具有一個(gè)固定為d的間距。通過大量嘗試，同時(shí)結(jié)合導(dǎo)航意像圖的大小來分析，d的最佳選取區(qū)間為［10，40］，選取40。

（2）具體的解分類討論：

（a）若ui=ui+1且vi＞vi+1，則uki+1=ui+1

（b）若vi=vi+1，則vk

i+1=vi+1

（c）若vi≠vi+1且ui≠ui+1，則：

綜上，提取機(jī)器人前進(jìn)路徑的工作已經(jīng)完成，據(jù)此形成了機(jī)器人的導(dǎo)航圖。

4 機(jī)器人避障

4.1 超聲避障系統(tǒng)

近幾年，超聲波傳感器的技術(shù)趨于成熟，其測距的基本原理為通過超聲發(fā)出及檢測到回聲的時(shí)間差來計(jì)算距離，這種方法通常被稱為時(shí)間旅行法。超聲波傳感器具有抗干擾能力強(qiáng)、成本低等優(yōu)勢。機(jī)器人避障傳感器的依靠的就是超聲波傳感器，但鑒于單超聲波傳感器對(duì)于障礙的檢測仍然有一些無法克服的缺陷，如障礙物定位不準(zhǔn)確、檢測盲區(qū)、鏡子缺陷等問題。需要設(shè)計(jì)一套完善的避障系統(tǒng)解決方案。用于克服單一超聲傳感器不穩(wěn)定因素的障礙，在機(jī)器人的障礙物信息周圍使用多個(gè)超聲波傳感器，每組超聲波傳感器均有兩個(gè)工作模式分別為單循環(huán)模式和雙周期模式，在微處理器的控制下，可以減少干擾和檢測結(jié)果的畸變。

4.2 模糊避障系統(tǒng)

為了提高新機(jī)器人的避障性能，提出了一種具有速度反饋的模糊避障算法。本算法依靠超聲波傳感器對(duì)于未知環(huán)境進(jìn)行感知建立模糊規(guī)則。模擬人類駕駛機(jī)動(dòng)車的經(jīng)驗(yàn)，根據(jù)障礙物的距離對(duì)于當(dāng)前的輸出速度進(jìn)行研判，當(dāng)距離障礙物很遠(yuǎn)的時(shí)候，機(jī)器人加速駛向目標(biāo)；當(dāng)靠近障礙物時(shí)，機(jī)器人降低速度并且根據(jù)障礙物的位置做出合理的運(yùn)動(dòng)決策，以便在避開障礙物的同時(shí)接近目標(biāo)。為了避免避障規(guī)則的建立，在未知環(huán)境中存在8個(gè)分布障礙，如圖4所示。

圖4 實(shí)際傳感器位置Fig.4 Actual Sensor Location

根據(jù)所述模型的特征，以面積重心法作為解模糊的手段。

為避障傳感器的主控邏輯控制表，如表1所示。表中的障礙物區(qū)分圖標(biāo)為：（近處障礙物◎）、（遠(yuǎn)處障礙物●）、（無障礙物○）、（任意狀態(tài)）。避障策略，如表1所示。

表1 傳感器主控邏輯Tab.1 Sensor Master Logic

5 實(shí)驗(yàn)結(jié)論

首先對(duì)視覺導(dǎo)航效果進(jìn)行了實(shí)驗(yàn)分析。機(jī)器人導(dǎo)航位置起始點(diǎn)，如圖5所示。機(jī)器人導(dǎo)航過程位置點(diǎn)，如圖6所示。從圖中可以看到，導(dǎo)航算法的有效性通過成功定位得到驗(yàn)證。從實(shí)驗(yàn)結(jié)果，在路標(biāo)的擺放位置理想的前提下，機(jī)器人可以依靠導(dǎo)航系統(tǒng)通過障礙物，并且運(yùn)動(dòng)到目標(biāo)位置。

圖5 機(jī)器人導(dǎo)航位置起始點(diǎn)Fig.5 Initial Position of Robot Navigation

圖6 機(jī)器人導(dǎo)航路程位置點(diǎn)Fig.6 Robot Navigation Location Points

6 結(jié)論

針對(duì)機(jī)器人導(dǎo)航問題進(jìn)行了深入的研究。首先對(duì)底盤進(jìn)行建模，實(shí)現(xiàn)了差動(dòng)控制?；谛袨橐?guī)劃的設(shè)計(jì)思想，兼顧考慮優(yōu)雅退化能力，機(jī)器人控制軟件的設(shè)計(jì)滿足了適應(yīng)實(shí)際復(fù)雜環(huán)境的初衷。其次在全局環(huán)境未知環(huán)境中進(jìn)行導(dǎo)航，分析了感知周圍環(huán)境狀態(tài)和外部環(huán)境信息，從而實(shí)現(xiàn)路徑規(guī)劃運(yùn)動(dòng)控制決策。鑒于以往方法的優(yōu)缺點(diǎn)，提出了一種依靠超聲波傳感器，多傳感器數(shù)據(jù)融合感知，并結(jié)合速度反饋建立模糊規(guī)則，進(jìn)而優(yōu)化避障效果的先進(jìn)避障算法。本算法，在傳統(tǒng)方法的基礎(chǔ)上，成功克服了超聲波檢測障礙、定位精度等方面缺陷。最后，機(jī)器人依靠路徑規(guī)劃實(shí)現(xiàn)自主避障，完成從起始目標(biāo)到終點(diǎn)目標(biāo)進(jìn)行最優(yōu)路徑。