基于三點定位的機器人路徑跟蹤算法

蘇有剛

(萊蕪職業技術學院研發辦公室，山東萊蕪 271100)

1 前言

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

2 機器人的三點定位原理

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

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

圖1 機器人定位原理

3 三點定位算法的實現

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

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

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

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

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

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

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

由此可得CD的斜率

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

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

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

4 機器人路徑跟蹤的實現

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

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

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

圖4 機器人路徑跟蹤實現

5 路徑跟蹤的實驗

實驗采用自制的4輪機器人，由轉速可精確調節的伺服電機驅動，測距傳感器選用相位激光傳感器，測量精度不小于2mm，用PC機完成數據處理和控制，采樣周0.02ms，AB距離設為10m.

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

表1 任意放置測量結果

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

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

圖5 試驗模擬路徑圖

6 結論

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

［1］陳少斌，蔣靜坪.基于神經網絡和粒子群優化算法的移動機器人動態避障路徑規劃［J］.系統仿真技術，2006，2(4):192－197.

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

［3］P.Núńez，R.Vázquez－Martín，J.C.del Toro，A.Bandera，F.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.