蘇有剛

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

1 前言

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

2 機器人的三點定位原理

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

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

圖1 機器人定位原理

3 三點定位算法的實現(xiàn)

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

則可得點C(Xci，Yci，θci)坐標為

圖2 機器人三點定位示意圖

機器人定位首先利用三點定位法計算完成機器人車身點定位，再利用雙三點定位法計算完成位姿的定位，然后算出機器人位置.位姿的定位精確定位移動機器人自身運動方向，包括車前和車后的方向，速度，相對于地圖的精確位置，這樣可以使機器人自動矯正位姿.

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

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

則可得點D(Xdi，Ydi，θdi)坐標為

由此可得CD的斜率

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

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

圖3 機器人雙三點定位示意圖

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

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

因為Xn+1＜Xn，所以θn=βn－90°.

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

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

5 路徑跟蹤的實驗

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

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

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

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

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

圖5 試驗模擬路徑圖

6 結(jié)論

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

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

［2］劉徐迅，曹陽，陳曉偉.基于移動機器人路徑規(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.