賀洪江，鐘志賢

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

0 引言

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

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

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

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

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

2 編碼器采集距離和角度

2.1 實(shí)時(shí)距離換算

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

圖2 編碼器輸出波形

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

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

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

編碼器采集到的脈沖數(shù)與里程成正比例關(guān)系。因此，可以由脈沖數(shù)轉(zhuǎn)化為里程值。分別對(duì)左右兩個(gè)編碼器進(jìn)行標(biāo)定，即找到脈沖數(shù)與里程值的比例系數(shù)。采用多次測(cè)量取平均的方法減小誤差。實(shí)驗(yàn)中機(jī)器人5次行走2m測(cè)得的脈沖數(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í)時(shí)角度換算

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

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

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

圖3 角度換算示意圖

3 直線、弧線跟蹤控制

3.1 直線跟蹤

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

圖4 直線跟蹤示意圖

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

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

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

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

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

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

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

3.2 弧線跟蹤

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

圖5 弧線跟蹤示意圖

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

其中：半徑R為OM的長(zhǎng)度。

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

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

通過(guò)直線和弧線的組合就可以控制機(jī)器人走復(fù)雜的路徑。最后停車時(shí)由于場(chǎng)地摩擦系數(shù)的不確定性，即使在到達(dá)終點(diǎn)附近就開(kāi)始減速，由于慣性機(jī)器人可能往往會(huì)超過(guò)設(shè)定的終點(diǎn)才停下來(lái)，所以終點(diǎn)準(zhǔn)確定位不像行走過(guò)程中的那樣只把左右輪差速作為被控對(duì)象，而機(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次測(cè)試的實(shí)驗(yàn)結(jié)果）

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

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

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

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

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