蓋軍雄，雷曉春,2，江澤濤,2

(1.桂林電子科技大學(xué) 計(jì)算機(jī)與信息安全學(xué)院, 桂林 541004;2.桂林電子科技大學(xué) 廣西圖像圖形與智能處理重點(diǎn)實(shí)驗(yàn)室, 桂林 541004)

0 引言

隨著人工智能技術(shù)和芯片性能的發(fā)展，出現(xiàn)了具備各種功能的機(jī)器人，在眾多室內(nèi)移動(dòng)機(jī)器人中，以兩輪差速結(jié)構(gòu)機(jī)器人最為常見。但相較于兩輪差速機(jī)器人，基于阿克曼結(jié)構(gòu)的機(jī)器人室內(nèi)室外場景均適宜，實(shí)際應(yīng)用場景更加廣泛。具有阿克曼結(jié)構(gòu)的移動(dòng)機(jī)器人也叫Car-Like機(jī)器人，即為像車一樣的機(jī)器人，因?yàn)榕c真實(shí)汽車外形相似，運(yùn)動(dòng)結(jié)構(gòu)相似，可以應(yīng)用于物流配送、農(nóng)耕、教育等領(lǐng)域，目前在無人物流配送領(lǐng)域應(yīng)用常見。為探索阿克曼結(jié)構(gòu)機(jī)器人在室內(nèi)的應(yīng)用場景，本文提出了一個(gè)基于ROS[1]系統(tǒng)、具有阿克曼轉(zhuǎn)向結(jié)構(gòu)的室內(nèi)移動(dòng)機(jī)器人(下文簡稱阿克曼機(jī)器人)，利用SLAM技術(shù)、傳感器融合技術(shù)、互補(bǔ)濾波算法、時(shí)間彈性帶(Timed Elastic Band，TEB)算法、局部路徑規(guī)劃算法等技術(shù)實(shí)現(xiàn)了阿克曼機(jī)器人在室內(nèi)環(huán)境的定位和導(dǎo)航。

1 阿克曼機(jī)器人控制系統(tǒng)

1.1 阿克曼機(jī)器人系統(tǒng)架構(gòu)

機(jī)器人搭載有激光雷達(dá)、攝像頭、IMU(慣性測量單元)、帶有編碼器的電機(jī)，嵌入式計(jì)算硬件等設(shè)備，阿克曼機(jī)器人硬件架構(gòu)如圖1所示。嵌入式計(jì)算硬件(RK3399)作為機(jī)器人大腦，運(yùn)行ROS系統(tǒng)。系統(tǒng)中激光雷達(dá)、IMU、帶編碼器的電機(jī)是主要的感知硬件，其中激光雷達(dá)負(fù)責(zé)阿克曼機(jī)器人對環(huán)境的感知，IMU負(fù)責(zé)阿克曼機(jī)器人自身的位姿變化的感知，帶編碼器電機(jī)作為機(jī)器人的里程計(jì)通過編碼器記錄機(jī)器人的里程信息，三者結(jié)合在阿克曼機(jī)器人的建圖、定位和導(dǎo)航上起到重要作用。

圖1 阿克曼室內(nèi)移動(dòng)機(jī)器人硬件架構(gòu)

1.2 阿克曼結(jié)構(gòu)運(yùn)動(dòng)學(xué)原理

對機(jī)器人運(yùn)動(dòng)的精準(zhǔn)控制基于對機(jī)器人建立的數(shù)學(xué)化模型。模型精確越高，對機(jī)器人運(yùn)動(dòng)的控制效果和精度就會(huì)越好。

當(dāng)前，室內(nèi)機(jī)器人以兩輪差速結(jié)構(gòu)機(jī)器人最為常見，這種結(jié)構(gòu)通常具有一個(gè)萬向輪作為前輪，兩個(gè)后輪通過電機(jī)驅(qū)動(dòng)作為動(dòng)力輪，通過控制兩個(gè)后輪不同的轉(zhuǎn)速控制機(jī)器人移動(dòng)。本系統(tǒng)的阿克曼結(jié)構(gòu)和兩輪差速結(jié)構(gòu)的區(qū)別主要在于，兩輪差速結(jié)構(gòu)的前輪為萬向輪或全向輪，而阿克曼結(jié)構(gòu)的前輪與路上常見的汽車結(jié)構(gòu)相同，是兩個(gè)普通的單向輪。其運(yùn)動(dòng)包含兩大核心部件：轉(zhuǎn)動(dòng)結(jié)構(gòu)，控制前輪轉(zhuǎn)向；差速器，驅(qū)動(dòng)后輪以及控制后輪的差速運(yùn)動(dòng)。阿克曼結(jié)構(gòu)在直線行駛時(shí)，四個(gè)車輪的軸線互相平行。轉(zhuǎn)彎時(shí)，前輪轉(zhuǎn)動(dòng)的角度是不同的，要使阿克曼機(jī)器人實(shí)現(xiàn)純滾動(dòng)運(yùn)動(dòng)，必須保證機(jī)器人的四個(gè)輪子運(yùn)動(dòng)方向的法線相交于一點(diǎn)，該點(diǎn)為機(jī)器人的轉(zhuǎn)向中心點(diǎn)，如圖2點(diǎn)O，由于機(jī)器人前輪的轉(zhuǎn)動(dòng)角度是有限的，因此阿克曼模型存在最小轉(zhuǎn)彎半徑這一運(yùn)動(dòng)約束。阿克曼機(jī)器人的運(yùn)動(dòng)模型描述在圖2中用實(shí)線表示。后軸中心C設(shè)為阿克曼機(jī)器人的基坐標(biāo)系，阿克曼機(jī)器人前后軸的軸距為L，兩后輪的輪距為K。O點(diǎn)為阿克曼機(jī)器人的瞬時(shí)旋轉(zhuǎn)中心，R為瞬時(shí)旋轉(zhuǎn)半徑，左前輪的瞬時(shí)轉(zhuǎn)向角設(shè)為α，右前輪的瞬時(shí)轉(zhuǎn)向角設(shè)為β。阿克曼結(jié)構(gòu)的核心公式是：

圖2 阿克曼運(yùn)動(dòng)模型與單車模型

(1)

四輪的阿克曼運(yùn)動(dòng)模型可以等效簡化為圖2中虛線所示的單車模型，這是因?yàn)閮煞N模型在運(yùn)動(dòng)機(jī)理方面可以視為一致，通過分析單車模型也可以更方便地研究對阿克曼機(jī)器人的控制。圖中θ為單車模型前輪的轉(zhuǎn)向角，可以視為整個(gè)阿克曼機(jī)器人的轉(zhuǎn)動(dòng)角度，同樣的，阿克曼機(jī)器人此刻的瞬時(shí)旋轉(zhuǎn)半徑為R，前后輪間的軸距為L，O為阿克曼機(jī)器人的旋轉(zhuǎn)中心，在這個(gè)瞬時(shí)狀態(tài)下轉(zhuǎn)彎半徑與轉(zhuǎn)向角有以下關(guān)系：

(2)

在ROS系統(tǒng)中對阿克曼機(jī)器人進(jìn)行實(shí)際的運(yùn)動(dòng)控制時(shí)，控制消息主要包含機(jī)器人X、Y、Z三軸的角度和線速度，由于機(jī)器人主要運(yùn)動(dòng)在2D平面，因此只需要考慮Z軸方向的轉(zhuǎn)角和X軸方向的線速度，對應(yīng)真實(shí)車輛的轉(zhuǎn)向和油門。

對于阿克曼機(jī)器人的轉(zhuǎn)向控制，接收到系統(tǒng)上層發(fā)送的轉(zhuǎn)向信息是機(jī)器人應(yīng)具有的瞬時(shí)轉(zhuǎn)向角度。機(jī)器人自身軸距L已知。應(yīng)用公式(2)，便可以得到車輛在此刻的轉(zhuǎn)彎半徑R，如圖2。再次應(yīng)用公式(2)，可以得到左前輪的轉(zhuǎn)向角關(guān)系式為：

(3)

右前輪的轉(zhuǎn)向角關(guān)系式為：

(4)

對公式(3)、公式(4)進(jìn)行逆解，得到左右前輪的轉(zhuǎn)向角。

阿克曼機(jī)器人的轉(zhuǎn)向結(jié)構(gòu)使用了如圖2所示的梯形機(jī)械結(jié)構(gòu)，這樣便無需考慮對每個(gè)前輪的單獨(dú)控制。本系統(tǒng)使用一個(gè)舵機(jī)對機(jī)器人進(jìn)行轉(zhuǎn)向控制實(shí)驗(yàn)，根據(jù)左右前輪的轉(zhuǎn)向角度、機(jī)器人運(yùn)動(dòng)方向與舵機(jī)的轉(zhuǎn)動(dòng)角度，反復(fù)實(shí)驗(yàn)后得到了舵機(jī)轉(zhuǎn)動(dòng)角度與機(jī)器人運(yùn)動(dòng)方向的映射關(guān)系，這樣使用一個(gè)舵機(jī)控制梯形機(jī)械結(jié)構(gòu)的轉(zhuǎn)動(dòng)便可以實(shí)現(xiàn)阿克曼機(jī)器人的轉(zhuǎn)向控制。由于前輪轉(zhuǎn)向的機(jī)械結(jié)構(gòu)轉(zhuǎn)動(dòng)角度是有限制的，通過將左(右)輪轉(zhuǎn)動(dòng)到最大角度，通過公式(3)(公式(4))便可以得到最小的轉(zhuǎn)彎半徑這一限制參數(shù)，方便后續(xù)控制算法使用。

對于機(jī)器人的速度控制，設(shè)小車的前進(jìn)速度為V，左后輪速度為VL，右后輪速度為VR，由角速度的一致性可得：

(5)

結(jié)合公式(2)和角度信息得到轉(zhuǎn)彎半徑R，各動(dòng)力輪需要提供的速度關(guān)系如下：

(6)

(7)

得到左右后輪速度信息后，結(jié)合小車傳動(dòng)比和車輪半徑便能得到每個(gè)驅(qū)動(dòng)輪具體的轉(zhuǎn)速，這樣就實(shí)現(xiàn)了在轉(zhuǎn)彎時(shí)模擬差速器的設(shè)計(jì)和實(shí)現(xiàn)(當(dāng)然也可通過機(jī)械結(jié)構(gòu)實(shí)現(xiàn)差速器)，在實(shí)際機(jī)器人運(yùn)動(dòng)時(shí)會(huì)有車輪形變和車輪打滑的可能性，因此后輪速度要根據(jù)真實(shí)情況進(jìn)行微調(diào)。

2 基于傳感器融合的位姿估計(jì)、定位與地圖構(gòu)建

2.1 機(jī)器人傳感器坐標(biāo)變換

系統(tǒng)硬件搭建完成后，首先根據(jù)機(jī)器人各個(gè)感知模塊相對于系統(tǒng)質(zhì)心的真實(shí)物理位置對系統(tǒng)進(jìn)行傳感器的坐標(biāo)系變換。

由于感知模塊一旦經(jīng)過安裝便不會(huì)移動(dòng)，因此使用靜態(tài)坐標(biāo)變換便可以滿足系統(tǒng)對坐標(biāo)變換的需求，將影響系統(tǒng)感知的IMU和激光雷達(dá)坐標(biāo)系變換到機(jī)器人的BASE坐標(biāo)系，當(dāng)機(jī)器人移動(dòng)時(shí)便可以通過激光雷達(dá)的點(diǎn)云信息和IMU信息獲得機(jī)器人BASE坐標(biāo)系在世界坐標(biāo)系中的位置和位姿。通過使用RVIZ對實(shí)現(xiàn)的坐標(biāo)變換進(jìn)行可視化，如圖3所示，BASE表示機(jī)器人基坐標(biāo)系，IMU表示IMU的坐標(biāo)系，LIDAR表示激光雷達(dá)坐標(biāo)系。

圖3 系統(tǒng)主要感知模塊坐標(biāo)系

2.2 誤差控制與傳感器融合位姿估計(jì)

通過電機(jī)編碼器進(jìn)行里程計(jì)計(jì)算[2]的流程為：已知控制參數(shù)轉(zhuǎn)向角、速度，結(jié)合公式(2)(8)逆推可以獲得機(jī)器人的轉(zhuǎn)向角速度。

(8)

結(jié)合機(jī)器人運(yùn)動(dòng)的速度信息，根據(jù)公式(9)對機(jī)器人短時(shí)間的運(yùn)動(dòng)迭代積分進(jìn)行位姿估計(jì)。公式(8)(9)中，θt為機(jī)器人此刻的轉(zhuǎn)向角，ω為機(jī)器人的角速度，V表示機(jī)器人的速度，dt表示很短的時(shí)間段。由于機(jī)器人運(yùn)動(dòng)過程中不僅可能存在車輪打滑等物理情況產(chǎn)生誤差，而且在迭代積分計(jì)算里程計(jì)信息的過程中存在累計(jì)誤差，因此實(shí)驗(yàn)時(shí)機(jī)器人使用傳統(tǒng)方法并不能很好地進(jìn)行位姿的估計(jì)。

(9)

由于車載的IMU可以提供角度、角速度等信息，針對以上問題，考慮將里程計(jì)信息和IMU數(shù)據(jù)進(jìn)行融合處理以獲得更加精確的位姿估計(jì)。

數(shù)據(jù)融合前的準(zhǔn)備過程中，在單獨(dú)測試IMU數(shù)據(jù)時(shí)，發(fā)現(xiàn)所使用的IMU存在零點(diǎn)漂移的現(xiàn)象，而且通過串口獲得的角度數(shù)據(jù)為X、Y、Z軸的歐拉角，而需要提供給ROS的IMU角度數(shù)據(jù)為四元數(shù)。同樣為了位姿估計(jì)的精度，首先對IMU的歐拉角數(shù)據(jù)進(jìn)行四元數(shù)的轉(zhuǎn)換，之后使用互補(bǔ)濾波器對IMU的四元數(shù)數(shù)據(jù)進(jìn)行濾波處理，最后通過ROS話題將IMU數(shù)據(jù)發(fā)送到話題池。濾波效果如圖4所示。

圖4 互補(bǔ)濾波前后偏航角對比

使用開源的robot_pose_ekf[3]功能包，本系統(tǒng)使用此功能包利用擴(kuò)展卡爾曼濾波器將IMU數(shù)據(jù)與里程計(jì)數(shù)據(jù)進(jìn)行融合。首先，通過訂閱不同話題的方式獲取傳感器信號(hào)，在初始階段檢查帶有時(shí)間戳的傳感器信息是否有效，若有效則與初始狀態(tài)下(靜止)的參考基準(zhǔn)坐標(biāo)系進(jìn)行轉(zhuǎn)換并存儲(chǔ)，直到所有傳感器的信息可用。之后，針對每個(gè)可用的數(shù)據(jù)更新擴(kuò)展卡爾曼濾波器。由于兩組數(shù)據(jù)的參考坐標(biāo)系是不同的，且隨著時(shí)間的漂移都可能有不同程度的漂移現(xiàn)象，所以此功能包使用兩組數(shù)據(jù)的相對位姿差異更新濾波器。位姿估計(jì)流程如圖5所示。

圖5 優(yōu)化后的位姿估計(jì)過程

2.3 基于Gmapping[4]算法的地圖構(gòu)建

使用Gmapping算法進(jìn)行建圖需要提供機(jī)器人基坐標(biāo)與傳感器之間的坐標(biāo)變換(TF)信息、里程計(jì)信息和激光雷達(dá)的點(diǎn)云信息，上文敘述的準(zhǔn)備使機(jī)器人已經(jīng)具有了地圖構(gòu)建的條件，ROS系統(tǒng)導(dǎo)出的機(jī)器人TF樹結(jié)構(gòu)如圖6所示。

圖6 本系統(tǒng)實(shí)現(xiàn)的阿克曼機(jī)器人的TF樹

2.4 使用自適應(yīng)蒙特卡洛定位(AMCL)[5]方法進(jìn)行定位

基于蒙特卡洛思想，將粒子濾波應(yīng)用于機(jī)器人的定位問題上，稱為蒙特卡洛定位(MCL)，該方法在初始位置對機(jī)器人進(jìn)行初始化，之后根據(jù)地圖結(jié)合激光雷達(dá)的數(shù)據(jù)進(jìn)行若干次迭代粒子權(quán)重計(jì)算—重采樣—狀態(tài)轉(zhuǎn)移這三個(gè)步驟后，粒子高度分布在可靠性高的區(qū)域，這樣就完成了機(jī)器人定位。自適應(yīng)蒙特卡洛定位(AMCL)在蒙特卡洛定位方法的基礎(chǔ)上進(jìn)行了優(yōu)化：一是在機(jī)器人定位基本完成后，減少了粒子數(shù)量；二是機(jī)器人綁架問題，即當(dāng)粒子的平均分?jǐn)?shù)突然降低時(shí)(這意味著正確的粒子在某次迭代中被拋棄了)再重新撒一些粒子。AMCL在較大范圍中的局部定位工作良好，且這一2D概率定位系統(tǒng)是ROS導(dǎo)航模塊中唯一指定的定位系統(tǒng)，因此本系統(tǒng)使用了自適應(yīng)蒙特卡洛定位(AMCL)方法進(jìn)行阿克曼機(jī)器人的定位算法。算法需要輸入激光雷達(dá)數(shù)據(jù)，里程計(jì)數(shù)據(jù)以及經(jīng)過融合后機(jī)器人在地圖中估計(jì)的位姿。系統(tǒng)使用經(jīng)過融合后的里程計(jì)信息能進(jìn)行更加精確的定位。本系統(tǒng)定位功能包的工作流程如圖7。

圖7 本系統(tǒng)中AMCL功能包的工作流程

3 基于TEB(Timed Elastic Band)[6]算法的局部路徑規(guī)劃與移動(dòng)機(jī)器人導(dǎo)航

3.1 使用TEB算法進(jìn)行局部路徑規(guī)劃[7]

ROS的導(dǎo)航功能包集[8]默認(rèn)使用的局部路徑規(guī)劃器使用的是DWA(dynamic window approach)算法，此算法采用動(dòng)態(tài)窗口法，在速度空間中采樣多組速度，推算其運(yùn)動(dòng)軌跡，然后通過評價(jià)函數(shù)對這些軌跡進(jìn)行評價(jià)，選擇最優(yōu)解。由于阿克曼轉(zhuǎn)向結(jié)構(gòu)轉(zhuǎn)向角度較小，且存在最小轉(zhuǎn)向半徑的限制，對于DWA算法規(guī)劃的局部路徑阿克曼機(jī)器人可能無法進(jìn)行有效的軌跡跟蹤，因此本系統(tǒng)使用TEB算法作為阿克曼機(jī)器人的局部路徑規(guī)劃器。

“TEB”算法的全稱是Time Elastic Band(時(shí)間彈性帶)[9]，該方法首先規(guī)劃了一條連接起始點(diǎn)、目標(biāo)點(diǎn)的全局路徑作為初始軌跡，在其中插入N個(gè)橡皮筋形狀的控制點(diǎn)，所以“TEB”被表述為一個(gè)多目標(biāo)優(yōu)化問題，另外為了顯示機(jī)器人運(yùn)動(dòng)軌跡的運(yùn)動(dòng)學(xué)信息，點(diǎn)與點(diǎn)之間定義了運(yùn)動(dòng)時(shí)間，所以這些控制點(diǎn)就是一些規(guī)劃出的帶有時(shí)間信息的離散位姿，算法通過使用“外力”使局部的路徑發(fā)生“形變”，“形變”的“外力”就是所有約束的條件，這些約束主要有以下幾點(diǎn)。

(1)機(jī)器人本身的運(yùn)動(dòng)學(xué)約束[10]：這些約束條件包括局限的控制量、速度、轉(zhuǎn)向角、最小轉(zhuǎn)彎半徑等，依據(jù)這些約束算法將若干弧組成平滑的運(yùn)動(dòng)軌跡。

(2)最快路徑約束[11]：目標(biāo)函數(shù)使機(jī)器人路徑上的各個(gè)位姿點(diǎn)在時(shí)間上均勻分撒，而并非一味追求最短路徑。

(3)路徑跟蹤和避障[12]：此約束將全局路徑規(guī)劃器規(guī)劃的全局路徑、地圖發(fā)現(xiàn)的障礙物(靜態(tài)障礙物)、激光雷達(dá)發(fā)現(xiàn)的障礙物(動(dòng)態(tài)障礙物)考慮在內(nèi)。全局路徑施力將局部路徑拉向全局路徑，障礙物施力使局部路徑遠(yuǎn)離障礙物。

經(jīng)過以上目標(biāo)函數(shù)的約束，再使用開源的G2O庫(General Graph Optimization通用圖優(yōu)化法)進(jìn)行優(yōu)化，最終獲得一條到達(dá)時(shí)間最短、距離最短、遠(yuǎn)離障礙物、盡可能跟蹤全局路徑的局部路徑。

由于TEB算法擁有上述特點(diǎn)，因此該算法非常適合阿克曼轉(zhuǎn)向結(jié)構(gòu)。本文實(shí)現(xiàn)的阿克曼機(jī)器人使用開源的teb_local_planner[13]功能包作為局部路徑器。

3.2 機(jī)器人導(dǎo)航功能控制流程示意圖

全局路徑規(guī)劃使用ROS導(dǎo)航功能包集中的默認(rèn)全局路徑規(guī)劃器，ROS導(dǎo)航功能包集中的move_base[8]功能包將全局路徑和局部路徑程序鏈接在一起，在地圖中給定一個(gè)目標(biāo)，move_base節(jié)點(diǎn)將嘗試控制移動(dòng)阿克曼機(jī)器人的基座移動(dòng)到這個(gè)目標(biāo)。結(jié)合以上的所有準(zhǔn)備，配置move_base功能包整體結(jié)構(gòu)如圖8所示。實(shí)現(xiàn)了機(jī)器人的室內(nèi)導(dǎo)航功能。

圖8 機(jī)器人move_base框架實(shí)現(xiàn)

4 真實(shí)環(huán)境系統(tǒng)功能驗(yàn)證

4.1 實(shí)驗(yàn)環(huán)境與定位優(yōu)化

本系統(tǒng)在實(shí)際環(huán)境中進(jìn)行系統(tǒng)功能的驗(yàn)證，驗(yàn)證環(huán)境如圖9所示。

圖9 系統(tǒng)驗(yàn)證環(huán)境

系統(tǒng)完成初期，首先對建圖功能進(jìn)行驗(yàn)證，實(shí)驗(yàn)結(jié)果如圖10 所示，由于IMU的零點(diǎn)漂移與積分里程計(jì)計(jì)算累計(jì)誤差的影響，導(dǎo)致阿克曼機(jī)器人在環(huán)境中的定位與位姿估計(jì)較差，對建圖效果影響巨大。對IMU互補(bǔ)濾波處理、使用擴(kuò)展卡爾曼濾波進(jìn)行傳感器融合定位，定位精度有了大幅度提高，實(shí)現(xiàn)建圖效果如圖11所示。

圖10 未經(jīng)傳感器融合的建圖效果

圖11 經(jīng)傳感器融合后的建圖效果

在獲得更為精確的環(huán)境地圖后，通過切換融合前后的定位算法，通過手動(dòng)控制阿克曼機(jī)器人從相同起點(diǎn)移動(dòng)到相同終點(diǎn)，在其中設(shè)置四個(gè)觀察點(diǎn)，使用RVIZ對阿克曼機(jī)器人的位姿進(jìn)行可視化，進(jìn)一步驗(yàn)證定位算法優(yōu)化對阿克曼機(jī)器人在環(huán)境中的定位效果與對自主導(dǎo)航系統(tǒng)的影響。圖12～圖15為RVIZ顯示的機(jī)器人運(yùn)動(dòng)過程的可視化結(jié)果。其中，黃色軌跡為機(jī)器人里程計(jì)位姿估計(jì)結(jié)果形成的運(yùn)動(dòng)軌跡;紅色點(diǎn)狀輪廓為激光雷達(dá)掃描的環(huán)境輪廓;灰色區(qū)域?yàn)闃?gòu)建的環(huán)境地圖，也為機(jī)器人可運(yùn)動(dòng)區(qū)域;黃色運(yùn)動(dòng)軌跡末端為粒子濾波的置信程度，越密集則置信度越高，越稀疏則置信度越低。

圖12 觀察點(diǎn)1運(yùn)動(dòng)軌跡對比

圖13 觀察點(diǎn)2運(yùn)動(dòng)軌跡對比

圖14 觀察點(diǎn)3運(yùn)動(dòng)軌跡對比

圖15 觀察點(diǎn)4運(yùn)動(dòng)軌跡對比

對比實(shí)驗(yàn)結(jié)果可知，在IMU零點(diǎn)漂移與積分累計(jì)誤差的影響下，機(jī)器人逐漸失去在環(huán)境中的定位，通過傳感器融合的方式能減少誤差，有效估計(jì)機(jī)器人位姿。

4.2 TEB算法對阿克曼機(jī)器人的局部路徑規(guī)劃

實(shí)驗(yàn)在已建好地圖的環(huán)境中放置動(dòng)態(tài)障礙物，觀察TEB算法對有最小轉(zhuǎn)彎半徑限制的阿克曼機(jī)器人局部路徑規(guī)劃的效果。實(shí)驗(yàn)效果如圖16～圖18所示。其中，紫黑色為全局代價(jià)地圖;淺藍(lán)色為局部代價(jià)地圖，機(jī)器人將盡可能避免與代價(jià)地圖發(fā)生接觸;紅色為全局路徑規(guī)劃器規(guī)劃出的全局路徑;綠色為TEB算法規(guī)劃出的局部路徑;灰色為環(huán)境地圖。

圖16 全局代價(jià)地圖與局部代價(jià)地圖

圖17 動(dòng)態(tài)障礙下的局部路徑規(guī)劃

圖18 靜態(tài)障礙下的局部路徑規(guī)劃

測試結(jié)果表明,TEB算法能夠考慮到阿克曼機(jī)器人轉(zhuǎn)彎半徑的局限性，規(guī)劃出一條盡可能接近全局路徑、遠(yuǎn)離障礙物，且軌跡半徑不超過阿克曼機(jī)器人轉(zhuǎn)彎半徑的局部路徑。

5 總結(jié)

本文設(shè)計(jì)了一種基于阿克曼轉(zhuǎn)向結(jié)構(gòu)的室內(nèi)導(dǎo)航機(jī)器人，對阿克曼機(jī)器人的運(yùn)動(dòng)學(xué)模型、硬件設(shè)計(jì)、軟件系統(tǒng)結(jié)構(gòu)與功能進(jìn)行了描述。經(jīng)實(shí)驗(yàn)，本系統(tǒng)可以在室內(nèi)未知環(huán)境中進(jìn)行建圖，并能依據(jù)地圖和傳感器實(shí)現(xiàn)阿克曼機(jī)器人在環(huán)境中定位與自主導(dǎo)航等室內(nèi)導(dǎo)航機(jī)器人的基本功能。對于軟硬件精度不足的問題使用濾波和傳感器融合的方法對機(jī)器人的位姿估計(jì)、建圖、定位、導(dǎo)航進(jìn)行了優(yōu)化，通過在真實(shí)環(huán)境中對系統(tǒng)進(jìn)行優(yōu)化前后對比實(shí)驗(yàn)，優(yōu)化效果較為明顯。并根據(jù)阿克曼運(yùn)動(dòng)結(jié)構(gòu)的特點(diǎn)，使用TEB算法作為阿克曼機(jī)器人的局部路徑規(guī)劃器，通過測試，滿足阿克曼機(jī)器人在自主導(dǎo)航狀態(tài)下的運(yùn)動(dòng)要求。