一種差動輪式競賽機(jī)器人的路徑跟蹤方法

賀洪江，鐘志賢

(1. 河北工程大學(xué) 裝備制造學(xué)院，邯鄲 056038；2.河北工程大學(xué) 信息與電氣工程學(xué)院，邯鄲 056038)

0 引言

隨著機(jī)械設(shè)備自動化程度的提高，移動機(jī)器人的應(yīng)用領(lǐng)域也越來越廣泛。競賽機(jī)器人也作為機(jī)器人學(xué)的一個重要分支，對其的跟蹤控制也得到了廣泛的研究。在差動輪式自主移動競賽機(jī)器人路徑控制中，主控制器通過連續(xù)采集編碼器脈沖，記錄機(jī)器人走過的真實(shí)里程。要對路徑進(jìn)行跟蹤控制還需要機(jī)器人的實(shí)時角度，可以使用陀螺儀測得，但是由于陀螺儀成本高，易受環(huán)境影響[1]。針對上述問題，本文介紹一種只基于兩個對稱安裝的編碼器作為傳感器的閉環(huán)路徑控制方法，為以后多傳感器信息融合進(jìn)行路徑規(guī)劃提供物理實(shí)現(xiàn)的保證。

1 編碼器安裝和系統(tǒng)組成

兩輪差動驅(qū)動的移動機(jī)器人的底盤結(jié)構(gòu)示意圖如圖1(a)所示，左右兩輪由兩個電機(jī)驅(qū)動，前輪是一個可以自由轉(zhuǎn)動的角輪。兩編碼器軸上裝上輪子，做成隨動碼盤輪,對稱地安裝在底盤上，并有彈簧機(jī)構(gòu)使其在隨動過程中接地良好不被懸空。

在機(jī)器人的路徑控制中，對直流電機(jī)轉(zhuǎn)速和位置的控制有較高的要求，有高精度的電機(jī)控制，才可以知道自己的位置和跟蹤自己要行走的路線。所以系統(tǒng)采用專用電機(jī)控制盒對電機(jī)進(jìn)行轉(zhuǎn)速控制。控制系統(tǒng)的組成如圖1(b)所示。

圖1 系統(tǒng)結(jié)構(gòu)

2 編碼器采集距離和角度

2.1 實(shí)時距離換算

機(jī)器人運(yùn)動過程中編碼器A、B線輸出波形如圖2所示。

圖2 編碼器輸出波形

1）單邊沿采集方法：把A線接到主控制器外部中斷，下降沿觸發(fā)，在中斷服務(wù)程序中判斷B線，若B線為低電平則正轉(zhuǎn)，脈沖計數(shù)加加，若B線為高電平則反轉(zhuǎn)，脈沖計數(shù)減減[2]。

2）雙邊沿采集方法：同樣把A線接到主控制器的外部中斷，雙邊沿觸發(fā)，在中斷服務(wù)程序中先判斷B線，若B線為高，再判讀A，若A也為高則表示正轉(zhuǎn)，脈沖計數(shù)加加，若此時A為低則表示反轉(zhuǎn)，脈沖計數(shù)減減。若進(jìn)中斷后先判讀的B線為低，則返回。

實(shí)際應(yīng)用中，有時機(jī)器人在看似靜止的情況下，由于機(jī)器人的微動編碼器輸出的波形會在某一邊沿來回跳變，此時若采用單邊沿采集則會造成脈沖計數(shù)的錯誤遞增或遞減，而雙邊沿采集方法可以相抵消，避免機(jī)器人停止時，卻有里程值改變的問題。

編碼器采集到的脈沖數(shù)與里程成正比例關(guān)系。因此，可以由脈沖數(shù)轉(zhuǎn)化為里程值。分別對左右兩個編碼器進(jìn)行標(biāo)定，即找到脈沖數(shù)與里程值的比例系數(shù)。采用多次測量取平均的方法減小誤差。實(shí)驗(yàn)中機(jī)器人5次行走2m測得的脈沖數(shù)如表1所示。

表1 左右碼盤標(biāo)定實(shí)驗(yàn)數(shù)據(jù)

程序中左里程值表示為：Dis_L = 左脈沖數(shù)× 2000 / 6725 (mm)；

程序中右里程值表示為：Dis_R = 右脈沖數(shù)× 2000 / 6789(mm)；

車體的總里程為左右里程的平均值，表示為：Dis_M=( Dis_L+ Dis_R)/2。

2.2 實(shí)時角度換算

由于是對稱安裝，左右兩車輪的運(yùn)動軌跡可以用左右兩碼盤輪的運(yùn)動軌跡來等效。根據(jù)左右兩個碼盤采集的里程值，在保證兩碼盤不離地的情況下，根據(jù)弧長公式計算出車體轉(zhuǎn)過的實(shí)時角度，方法如圖3如示。

其中，Dis_R為此時機(jī)器人右碼盤記錄的里程，Dis_L為此時機(jī)器人左碼盤記錄的里程。

具體應(yīng)用時標(biāo)定車體的初始朝向?yàn)?，則向左轉(zhuǎn)過的角度為正值，向右轉(zhuǎn)過的角度值為負(fù)值。

圖3 角度換算示意圖

3 直線、弧線跟蹤控制

3.1 直線跟蹤

控制中所用的到距離和角度均來自雙編碼器的計算。在程序中采用經(jīng)典PID控制，主控制器根據(jù)隨動編碼器得到的信息進(jìn)行PID計算，結(jié)果輸出給電機(jī)控制盒控制電機(jī)。理想情況下，若機(jī)器人嚴(yán)格進(jìn)行直線運(yùn)動則左右兩個編碼器采集的里程值之差因該為0或者為某一常量，所以把兩編碼器采集的里程之差作為跟蹤控制的量，0或某一常量作為設(shè)定值，進(jìn)行PID控制。但是實(shí)際對于直線行走的PID校正，誤差的形式有兩種：角度誤差θ和中心位置誤差d。

圖4 直線跟蹤示意圖

如圖4所示，直線行走時機(jī)器人的當(dāng)前朝向角為α，β為規(guī)劃好的直線在坐標(biāo)系中的角度，由平面幾何可得下式：

θ和d這兩個誤差相對獨(dú)立，可以分別進(jìn)行檢測控制，這里為了減少PID參數(shù)整定，通過加權(quán)將他們合成一個誤差e，只用一套PID參數(shù)進(jìn)行校正，即把e作為檢測值，0作為設(shè)定值。如下所示：e=d+kθ。

加權(quán)系數(shù)k反映了機(jī)器人角度誤差與位置誤差相漲或者相消的程度，實(shí)際上它還包含了一定的微分的意義[3]。

對于綜合后的總誤差e，我們采用增量式PID算法對其進(jìn)行校正,在程序中具體實(shí)現(xiàn)的函數(shù)名為：

INT32S PID_common(INT32S set, INT32S feedback, PID_Uint *p);

其中，入口參數(shù)分別為，設(shè)定值(set)，檢測值(feedback)，和指向PID參數(shù)結(jié)構(gòu)體(PID_Uint)的指針。

根據(jù)開環(huán)實(shí)際測試，可以估算得出角度偏差和位置偏差的加權(quán)系數(shù)為3，在實(shí)際調(diào)試時也驗(yàn)證了這個權(quán)值是合適的。在實(shí)際應(yīng)用時還需給出，起始點(diǎn)坐標(biāo)和目標(biāo)點(diǎn)坐標(biāo)，加減速距離，起始速度，最高速度和末速度，然后根據(jù)增量式PID校正算法，對機(jī)器人的左右輪速度進(jìn)行實(shí)時地校正。實(shí)現(xiàn)沿規(guī)劃好的直線行走。

3.2 弧線跟蹤

類似直線的跟蹤思想，算法的參考目標(biāo)是轉(zhuǎn)彎半徑R和機(jī)器人相對初始朝向的實(shí)時轉(zhuǎn)彎角度θ，在這兩個參照量和實(shí)際測得的機(jī)器人狀態(tài)的偏差之間做加權(quán)求出總誤差e，減少參數(shù)個數(shù)，以方便參數(shù)的調(diào)試。跟蹤示意圖如圖5所示。

圖5 弧線跟蹤示意圖

e = D R + k × Dq ，ΔR為實(shí)時測得的半徑和規(guī)劃給出的半徑之差，Δθ為規(guī)劃的圓弧計算出的應(yīng)轉(zhuǎn)的角度和由雙編碼器計算反饋回來的自身角度之差。如圖3所示，計算實(shí)時車體運(yùn)動半徑公式如下：

其中：半徑R為OM的長度。

具體應(yīng)用時，應(yīng)根據(jù)事先給定的弧線路徑信息，先計算得出左右兩輪的初始基速度，兩者應(yīng)該對應(yīng)成比例，再此基礎(chǔ)上運(yùn)用增量PID算法對跟蹤過程中產(chǎn)生的總誤差e進(jìn)行調(diào)節(jié)。要實(shí)現(xiàn)快速轉(zhuǎn)弧，在高速且轉(zhuǎn)角較大度時，還應(yīng)逐漸增大兩輪的差速，并在距目標(biāo)角度還有一定的誤差時，逐漸減小兩輪差速以實(shí)現(xiàn)行走過程的流線銜接和平穩(wěn)過渡[4]。

4 終點(diǎn)準(zhǔn)確定位

通過直線和弧線的組合就可以控制機(jī)器人走復(fù)雜的路徑。最后停車時由于場地摩擦系數(shù)的不確定性，即使在到達(dá)終點(diǎn)附近就開始減速，由于慣性機(jī)器人可能往往會超過設(shè)定的終點(diǎn)才停下來，所以終點(diǎn)準(zhǔn)確定位不像行走過程中的那樣只把左右輪差速作為被控對象，而機(jī)器人的左右輪差速和左右輪基速度都成了被控量。機(jī)器人的初始基速度是根據(jù)實(shí)際情況事先設(shè)定的。在終點(diǎn)準(zhǔn)確定位控制中，都采用閉環(huán)PID控制?？刂瓶驁D如圖6所示。

圖6 終點(diǎn)定位控制框圖

5 實(shí)驗(yàn)結(jié)果和分析

表2 機(jī)器人跟蹤軌跡誤差比較（10次測試的實(shí)驗(yàn)結(jié)果）

由測試結(jié)果可以看出，基于雙編碼器計算角度和距離控制機(jī)器人跟蹤事先設(shè)定好的路徑的方法可行，平均誤差都在100mm以內(nèi)，基本可以滿足競賽機(jī)器人要求。在測試過程中，由于左右輪電機(jī)控制盒發(fā)送速度需要時間，所以控制程序的控制周期設(shè)定較大，在20ms左右，使控制效果得不到充分實(shí)現(xiàn)。另外左右兩個電機(jī)控制盒的速度不能被同時設(shè)定，所以在控制周期內(nèi)輪流改變對兩個控制盒的發(fā)送順序，可以改善控制效果。

[1] 李世華,田玉平.移動小車的跟蹤控制[J].控制與決策,2000,15(5): 626-628.

[2] 鮮浩,任愛芝,盛仲飚.增量式編碼器數(shù)據(jù)采集[J].華北工學(xué)院學(xué)報,2003,24(2):142-143.

[3] 吳衛(wèi)國,陳輝堂,王月娟.移動機(jī)器人的全局軌道跟蹤控制[J].自動化學(xué)報,2001,27(3):326-331.

[4] 龔建偉,陸際聯(lián),黃文宇.輪式移動機(jī)器人航向跟蹤預(yù)估控制算法[J].機(jī)器人,2001,(3):193-196.