基于三點(diǎn)定位的機(jī)器人路徑跟蹤算法

蘇有剛

(萊蕪職業(yè)技術(shù)學(xué)院研發(fā)辦公室，山東萊蕪 271100)

1 前言

在機(jī)器人路徑跟蹤方面的研究很多，可視圖法［1］、神經(jīng)網(wǎng)絡(luò)［2］、鼠群算法［3］等已取得很大進(jìn)展，但大部分都局限于一般的研究，有很多不足，特別缺少對(duì)特定環(huán)境的機(jī)器人路徑跟蹤［4］的研究，象港口、機(jī)場(chǎng)、車(chē)站貨物裝卸、高爐上料、農(nóng)業(yè)田間工作、衛(wèi)生清潔、軍事巡邏等.本文針對(duì)這些特定場(chǎng)所的特點(diǎn)，提出了三點(diǎn)定位的機(jī)器人路徑跟蹤的實(shí)現(xiàn)方法.提高機(jī)器人對(duì)環(huán)境的適應(yīng)速度和精度，為區(qū)域相對(duì)［5］固定場(chǎng)所的機(jī)器人應(yīng)用奠定基礎(chǔ).

2 機(jī)器人的三點(diǎn)定位原理

機(jī)器人三點(diǎn)定位原理如圖1，即用2個(gè)激光測(cè)距傳感器發(fā)射器A、B，放在機(jī)器人工作目標(biāo)區(qū)的現(xiàn)場(chǎng)適當(dāng)位置，圖1中放在了左邊A、B處，這樣有利于坐標(biāo)定位和計(jì)算.在工作過(guò)程中激光測(cè)距傳感器的發(fā)射器位置A、B固定，距離已知，確定如圖1一方位固定已知的參考坐標(biāo)軸，為工作路徑提供參照，機(jī)器人在整個(gè)移動(dòng)過(guò)程中，以激光測(cè)距傳感器發(fā)射器A、B為定位參考.

在機(jī)器人車(chē)前和車(chē)后分別裝有激光測(cè)距超聲波接收器C、D，實(shí)時(shí)接收激光測(cè)距傳感器發(fā)射器A、B的信號(hào)，不斷測(cè)距，并計(jì)算出機(jī)器人的相應(yīng)距離和角度，實(shí)現(xiàn)實(shí)時(shí)定位，實(shí)現(xiàn)移動(dòng)機(jī)器人自身運(yùn)動(dòng)方向，包括車(chē)前和車(chē)后的方向和速度的精確定位.再通過(guò)和預(yù)置工作目標(biāo)區(qū)地圖的比較擬合，最終實(shí)現(xiàn)移動(dòng)機(jī)器人的完全自主定位.工作目標(biāo)區(qū)地圖即機(jī)器人將要處理的工作目標(biāo)大小形狀方向的詳細(xì)輪廓圖，是機(jī)器人具體的工作環(huán)境圖，在工作前由操作者輸入機(jī)器，在定位過(guò)程中作為內(nèi)部參照，而傳感器作為外部參照，在工作過(guò)程中實(shí)現(xiàn)內(nèi)外定位結(jié)合.在此過(guò)程中，融合了傳感器和工作目標(biāo)區(qū)地圖的信息，并將它用于環(huán)境信息預(yù)測(cè)跟蹤，最后由匹配的環(huán)境信息反過(guò)來(lái)對(duì)機(jī)器人的位置進(jìn)行修正，得到其位置的較好的定位.這樣可解決其它定位方法帶來(lái)的累積誤差，而依靠每一個(gè)位置所觀察到的不同的信息和已有地圖匹配，可以不斷地修正機(jī)器人自身位置的誤差量，從而可實(shí)現(xiàn)長(zhǎng)時(shí)間的自主移動(dòng).

圖1 機(jī)器人定位原理

3 三點(diǎn)定位算法的實(shí)現(xiàn)

三點(diǎn)定位的推導(dǎo)原理如圖2所示，定位問(wèn)題用數(shù)學(xué)語(yǔ)言可簡(jiǎn)單描述如下:設(shè)傳感器發(fā)射點(diǎn)A，B坐標(biāo)為(XA，YA)，(XB，YB)，并使AB平行于Y軸，垂直于X軸，設(shè)置在目標(biāo)工作區(qū)左邊，作為定位參考，且固定設(shè)置AB距離為l0，機(jī)器人經(jīng)過(guò)的每個(gè)路徑S的坐標(biāo)為(X，Y，θ)，θ是機(jī)器人當(dāng)前所處位置對(duì)坐標(biāo)軸的方向的方位角，與定位的機(jī)器人當(dāng)前的位置為Si(Xi，Yi，θi)(i=1，2，…，n).任取路徑S上一點(diǎn)C(Xci，Yci，θci)，C和圖1提到的D(Xdi，Ydi，θdi)設(shè)置在機(jī)器人車(chē)身中心的對(duì)稱(chēng)位置，前后放置.由此根據(jù)三角原理對(duì)于點(diǎn)C(Xci，Yci，θci)，AC，BC距離可由測(cè)距儀測(cè)定分別設(shè)為laci和lbci，由三角余弦定理可得

則可得點(diǎn)C(Xci，Yci，θci)坐標(biāo)為

圖2 機(jī)器人三點(diǎn)定位示意圖

機(jī)器人定位首先利用三點(diǎn)定位法計(jì)算完成機(jī)器人車(chē)身點(diǎn)定位，再利用雙三點(diǎn)定位法計(jì)算完成位姿的定位，然后算出機(jī)器人位置.位姿的定位精確定位移動(dòng)機(jī)器人自身運(yùn)動(dòng)方向，包括車(chē)前和車(chē)后的方向，速度，相對(duì)于地圖的精確位置，這樣可以使機(jī)器人自動(dòng)矯正位姿.

所謂雙三點(diǎn)定位法，即由三點(diǎn)定位分別測(cè)定機(jī)器人車(chē)身前后不同位置兩個(gè)點(diǎn)，再通過(guò)兩個(gè)點(diǎn)與參考坐標(biāo)的兩個(gè)點(diǎn)比較位置，確定偏差.參考坐標(biāo)的兩個(gè)點(diǎn)選在工作環(huán)境中，其距離及在地圖中位置確定，分別利用三點(diǎn)定位法分別測(cè)定機(jī)器人車(chē)身前后不同位置兩個(gè)點(diǎn)，兩點(diǎn)確定一線，兩線比較就可得到機(jī)器人的方位，確定位姿，機(jī)器人位置本文是取車(chē)身兩點(diǎn)中點(diǎn).這種定位由機(jī)器人自身自動(dòng)完成，是自主的.利用絕對(duì)定位和相對(duì)定位相結(jié)合的方法，實(shí)現(xiàn)移動(dòng)機(jī)器人定位，這種方法對(duì)車(chē)輛機(jī)器人定位非常適合.這樣機(jī)器人得到的原始數(shù)據(jù)是對(duì)應(yīng)距離，定位結(jié)果是對(duì)應(yīng)距離即(X，Y，θ)的形式.

但利用三點(diǎn)定位只能定位路徑S的坐標(biāo)為(X，Y，θ)，不能確定機(jī)器人車(chē)身的位姿，因此采用四點(diǎn)定位或者說(shuō)雙三點(diǎn)定位，即同時(shí)測(cè)算點(diǎn)C(Xci，Yci，θci)，和點(diǎn)D(Xdi，Ydi，θdi)坐標(biāo)，由此確定機(jī)器人位姿，并完成定位.圖3點(diǎn)C(Xci，Yci，θci)坐標(biāo)以求出，同理設(shè)AD，BD距離可由測(cè)距儀測(cè)定分別設(shè)為ladi和lbdi可得點(diǎn)D(Xdi，Ydi，θdi)坐標(biāo)為

則可得點(diǎn)D(Xdi，Ydi，θdi)坐標(biāo)為

由此可得CD的斜率

此即為取i點(diǎn)時(shí)刻機(jī)器人的位姿角，由于機(jī)器人可認(rèn)為工作在二維平面上，由此即為機(jī)器人的方向角.實(shí)際應(yīng)用中可取CD中點(diǎn)為此時(shí)刻的機(jī)器人位置即

然后再有Xi和Yi求出θi，這樣就確定機(jī)器人位置S(Xi，Yi，θi)(i=1，2，…，n)，位姿角為βi.

圖3 機(jī)器人雙三點(diǎn)定位示意圖

4 機(jī)器人路徑跟蹤的實(shí)現(xiàn)

由于機(jī)器人實(shí)時(shí)定位是不連續(xù)的，機(jī)器人并不能實(shí)時(shí)測(cè)定自身的位置，在一個(gè)周期中只能定位一系列連續(xù)的點(diǎn)S(Xi，Yi，θi)(i=1，2，…，n)，路徑跟蹤就是利用最近的兩個(gè)測(cè)量點(diǎn)來(lái)實(shí)現(xiàn).如圖4上點(diǎn)Sn(Xn，Yn)和Sn+1(Xn+1，Yn+1).最佳軌跡如圖4虛線X0.點(diǎn)S'表示調(diào)整后的位置.關(guān)系如下:

因?yàn)閄n+1＜Xn，所以θn=βn－90°.

因?yàn)榇藭r(shí)機(jī)器人實(shí)際軌跡近似在點(diǎn)Sn和Sn+1的線上，已經(jīng)偏離了最佳軌跡，路徑跟蹤的實(shí)現(xiàn)為調(diào)整機(jī)器人車(chē)位角度，使機(jī)器人位于點(diǎn)Sn+1和S'的線上，使相位角θn+1變?yōu)棣萵+1=θn/2(可根據(jù)實(shí)際數(shù)據(jù)調(diào)整)進(jìn)行調(diào)控.這種路徑跟蹤可以使機(jī)器人無(wú)限接近最佳軌跡，實(shí)現(xiàn)機(jī)器人路徑的最優(yōu)跟蹤，并且計(jì)算量較小，反應(yīng)速度快，并且提高精度可通過(guò)提高采樣頻率實(shí)現(xiàn).當(dāng)然當(dāng)移動(dòng)速度快時(shí)會(huì)降低控制精度.同理，當(dāng)機(jī)器人偏離最佳軌X0時(shí)，需進(jìn)行調(diào)控，反之趨近時(shí)不用調(diào)整.具體位置需具體分析.

圖4 機(jī)器人路徑跟蹤實(shí)現(xiàn)

5 路徑跟蹤的實(shí)驗(yàn)

實(shí)驗(yàn)采用自制的4輪機(jī)器人，由轉(zhuǎn)速可精確調(diào)節(jié)的伺服電機(jī)驅(qū)動(dòng)，測(cè)距傳感器選用相位激光傳感器，測(cè)量精度不小于2mm，用PC機(jī)完成數(shù)據(jù)處理和控制，采樣周0.02ms，AB距離設(shè)為10m.

(1)選取任意固定適當(dāng)坐標(biāo)，以X－軸為機(jī)器人00方向.機(jī)器人隨機(jī)放置目標(biāo)區(qū)，AB距離設(shè)為10m，平行于Y軸進(jìn)行實(shí)驗(yàn).經(jīng)測(cè)量結(jié)果如表1所示.結(jié)果說(shuō)明精度較高.

表1 任意放置測(cè)量結(jié)果

(2)模擬直線路徑20m，地面平坦，設(shè)置2處障礙物，設(shè)定速度為1m/s，模擬路徑結(jié)果如圖5所示，虛線代表設(shè)定路徑，實(shí)線為實(shí)際路徑，圖中說(shuō)明實(shí)際誤差沒(méi)超過(guò)10cm，調(diào)整精度較高，反應(yīng)也較快.

從以上試驗(yàn)結(jié)果可以看到，路徑跟蹤精度很高，測(cè)量距離對(duì)測(cè)量精度影響較大，距離越遠(yuǎn)誤差變小，這與激光測(cè)距絕對(duì)誤差小有關(guān).由于周期性定位，這種路徑跟蹤方法不產(chǎn)生累積誤差.本試驗(yàn)精度也和控制系統(tǒng)的性能相關(guān).

圖5 試驗(yàn)?zāi)M路徑圖

6 結(jié)論

本文給出的利用雙三點(diǎn)定位旋轉(zhuǎn)車(chē)位角度的機(jī)器人路徑跟蹤的方法，計(jì)算方法雖然簡(jiǎn)單，但該路徑跟蹤方法工作量小、精度高、反應(yīng)快、使用便利，適于機(jī)器人工作區(qū)域相對(duì)確定范圍有限的場(chǎng)合，仿真試驗(yàn)證明，該計(jì)算方法具有很高的實(shí)用性.

［1］陳少斌，蔣靜坪.基于神經(jīng)網(wǎng)絡(luò)和粒子群優(yōu)化算法的移動(dòng)機(jī)器人動(dòng)態(tài)避障路徑規(guī)劃［J］.系統(tǒng)仿真技術(shù)，2006，2(4):192－197.

［2］劉徐迅，曹陽(yáng)，陳曉偉.基于移動(dòng)機(jī)器人路徑規(guī)劃的鼠群算法［J］.控制與決策，2008，23(9):1060－1064.

［3］P.Núńez，R.Vázquez－Martín，J.C.del Toro，A.Bandera，F(xiàn).Sandoval.Natural landmark extraction for mobile robot navigation based on an adaptive curvature estimation［J］.Robotics and Autonomous Systems，2008，56:247－264.

［4］Durrant－Whyte，H.F.RYE D，Nebot E.Localization of automous guided vehicles［A］.Proceedings of international Symp on Robotics Reseach［C］.New York:Springer－verlag，1995.

［5］Gordon Wells，Carme Torras.Assessing image features for vision－Based robot positioning［J］.Journal of Intelligent and Robotic Systems，2001，30:95－118.