基于路程檢測(cè)的機(jī)器人運(yùn)動(dòng)控制研究

李靜，劉朝福

(桂林電子科技大學(xué)信息科技學(xué)院，廣西桂林541004)

0 前言

機(jī)器人比賽路線日趨復(fù)雜，不僅只限于直線、轉(zhuǎn)彎，而且增加了上坡、下坡等難度。賽道的復(fù)雜性涉及到機(jī)器人驅(qū)動(dòng)電機(jī)要根據(jù)不同路況實(shí)時(shí)進(jìn)行多種狀態(tài)轉(zhuǎn)換，并且要保證機(jī)器人速度。這是機(jī)器人比賽在賽道上行進(jìn)時(shí)的重點(diǎn)和難點(diǎn)。2009年亞廣聯(lián)機(jī)器人大賽賽道具有較強(qiáng)的典型性，包含上坡、下坡、直行、轉(zhuǎn)彎等多種路段，以此賽道為例，基于路程檢測(cè)研究機(jī)器人運(yùn)動(dòng)控制，保證機(jī)器人在不同路況下快速、平穩(wěn)地行進(jìn)完整個(gè)賽道，這對(duì)機(jī)器人比賽來(lái)說(shuō)具有重要意義。比賽賽道運(yùn)動(dòng)路線示意圖如圖1 所示。

圖1 運(yùn)動(dòng)路線示意圖

1 路程檢測(cè)

將機(jī)器人的整個(gè)運(yùn)動(dòng)軌跡分為數(shù)段，每一段由于路段不同，所以機(jī)器人的運(yùn)動(dòng)狀態(tài)也不同，如上下坡和多次轉(zhuǎn)彎，這將涉及到電機(jī)的多次狀態(tài)轉(zhuǎn)換，因此必須通過(guò)路程檢測(cè)系統(tǒng)來(lái)算出機(jī)器人所走的位移，確定機(jī)器人當(dāng)前的位置，以保證準(zhǔn)確控制機(jī)器人的運(yùn)動(dòng)狀態(tài)。

1.1 光電編碼器鑒相方法

光電路程檢測(cè)系統(tǒng)原理簡(jiǎn)單，是一種常用的路程檢測(cè)方法［1］。該方法檢測(cè)機(jī)器人在賽道中某段的位移，通過(guò)該位移決定機(jī)器人下一時(shí)刻的運(yùn)動(dòng)狀態(tài)。選用直流伺服電機(jī)自帶的增量式光電編碼器，它是一款高精度角位移傳感器［2］，與移動(dòng)機(jī)器人的驅(qū)動(dòng)車(chē)輪同軸安裝，該軸的正反轉(zhuǎn)代表了移動(dòng)機(jī)器人前進(jìn)和后退兩種相反的運(yùn)動(dòng)方向。識(shí)別光電編碼器主軸的轉(zhuǎn)向是實(shí)現(xiàn)移動(dòng)機(jī)器人運(yùn)動(dòng)控制的必須要求［3］。主軸每旋轉(zhuǎn)一周分兩路輸出500 個(gè)電壓脈沖信號(hào)Out_A 和Out_B，其中，Out_A 和Out_B 兩路信號(hào)相位差為90°，可通過(guò)Out_A 和Out_B 的相位關(guān)系，判斷主軸的轉(zhuǎn)動(dòng)方向。通過(guò)對(duì)光電編碼器所輸出的相位差90°的兩路電壓脈沖信號(hào)Out_A 和Out_B 進(jìn)行鑒相，就能夠判別轉(zhuǎn)盤(pán)正轉(zhuǎn)或反轉(zhuǎn)。圖2 示意了光電編碼器正轉(zhuǎn)時(shí)，Out_A、Out_B 的信號(hào)波形和鑒相電路的輸出。

圖2 光碼盤(pán)輸出波形

正轉(zhuǎn)時(shí)，Out_B 信號(hào)的相位超前Out_A 信號(hào)90°。反轉(zhuǎn)時(shí)，Out_B 信號(hào)的相位延后Out_A 信號(hào)90°。

1.2 路程檢測(cè)電路硬件設(shè)計(jì)

通過(guò)確定機(jī)器人當(dāng)前的位置來(lái)決定機(jī)器人運(yùn)動(dòng)電機(jī)的狀態(tài)，因此機(jī)器人當(dāng)前位置確定非常重要，它是精確控制機(jī)器人調(diào)整速度及平衡的決定因素［4］。通過(guò)人工推斷機(jī)器人測(cè)量每段路程的脈沖數(shù)，在機(jī)器人運(yùn)動(dòng)過(guò)程中通過(guò)光碼盤(pán)產(chǎn)生的脈沖數(shù)來(lái)確定機(jī)器人的位置。光碼盤(pán)轉(zhuǎn)動(dòng)一周產(chǎn)生200 個(gè)脈沖，經(jīng)過(guò)二倍頻之后變?yōu)?00 個(gè)，通過(guò)控制上升電機(jī)的單片機(jī)接收光碼盤(pán)的脈沖數(shù)，對(duì)其計(jì)數(shù)，并通過(guò)該脈沖數(shù)判斷機(jī)器人當(dāng)前的位置。路程檢測(cè)電路原理圖如圖3 所示。

圖3 路程檢測(cè)電路原理圖

2 運(yùn)動(dòng)控制算法

2.1 直流伺服電機(jī)的數(shù)學(xué)模型

采用直流伺服電機(jī)作為機(jī)器人動(dòng)力。當(dāng)機(jī)器人從直線換到上坡或從上坡到下坡或到轉(zhuǎn)彎時(shí)，即從一個(gè)速度調(diào)整到另一速度時(shí)，建立直流伺服電機(jī)的數(shù)學(xué)模型可以保證機(jī)器人快速響應(yīng)，使速度調(diào)整時(shí)間最短而且穩(wěn)定，從而保證機(jī)器人速度。

建立模型，進(jìn)行自動(dòng)控制系統(tǒng)的分析和設(shè)計(jì)時(shí)，通常對(duì)微分方程進(jìn)行拉普拉斯變換，從而得出各環(huán)節(jié)零初始條件下輸出與輸入的比值即傳遞函數(shù)。對(duì)于直流伺服電機(jī)而言，其輸入量是電樞電壓，輸出量一般是轉(zhuǎn)速［5－6］。根據(jù)調(diào)節(jié)特性可知，電機(jī)微分方程可以寫(xiě)成下式:

對(duì)其進(jìn)行拉普拉斯變換得:

由此可得直流伺服電動(dòng)機(jī)的傳遞函數(shù)為:

顯然，這是一個(gè)二階環(huán)節(jié)，對(duì)應(yīng)二階微分方程。如果忽略電磁時(shí)間常數(shù)，就可以得到簡(jiǎn)化的傳遞函數(shù)表達(dá)式:

這就是常用的直流伺服電動(dòng)機(jī)的傳遞函數(shù)表達(dá)式，各式意義如下:n 為轉(zhuǎn)速;Ua為電壓;ku為電壓時(shí)間系數(shù);Tj為機(jī)電時(shí)間常數(shù);Td為電磁時(shí)間常數(shù)。

2.2 PID 模糊控制算法

在控制系統(tǒng)中最常用的控制規(guī)律是PID 控制，它是一種線性控制器，根據(jù)給定與實(shí)際值構(gòu)成控制偏差，將偏差的比例、積分和微分通過(guò)線性組合構(gòu)成控制器［7］。在設(shè)計(jì)中，PID 算法主要用于控制機(jī)器人的直線行走，即當(dāng)機(jī)器人偏離路徑的中心白線時(shí)，采用位置式PID 算法，它會(huì)根據(jù)偏移量自動(dòng)調(diào)整機(jī)器人的運(yùn)動(dòng)狀態(tài)，使機(jī)器人靠近路徑中心白線［8］。PID 調(diào)節(jié)參數(shù)由模糊自整定控制器根據(jù)偏差e 和偏差變化率ec進(jìn)行自動(dòng)調(diào)整，同時(shí)把模糊自整定控制器的模糊部分按Kp、Ki、Kd分成三部分，分別由相應(yīng)的子推理器來(lái)實(shí)現(xiàn)。模糊PID 控制流程如圖4 所示。

圖4 模糊PID 控制流程圖

經(jīng)過(guò)PID 算法調(diào)整后機(jī)器人的運(yùn)動(dòng)軌跡如圖5所示。

圖5 PID 算法改進(jìn)機(jī)器人運(yùn)動(dòng)軌跡示意圖

3 機(jī)器人運(yùn)動(dòng)控制的實(shí)現(xiàn)

3.1 路程檢測(cè)的實(shí)現(xiàn)

機(jī)器人在其運(yùn)動(dòng)軌跡的A 段采用串口位置方式控制電機(jī)。因此，在該段不需要用光碼盤(pán)進(jìn)行路程檢測(cè)，直至點(diǎn)B 以后，才開(kāi)始用光碼盤(pán)進(jìn)行路程檢測(cè)。在下坡段，光碼盤(pán)產(chǎn)生的脈沖以不間斷源的形式傳遞給CPU，來(lái)一次脈沖CPU 產(chǎn)生一次中斷，這樣可以防止脈沖的丟失，保證上升電機(jī)的精確控制。在剩余的其他路段，光碼盤(pán)產(chǎn)生的脈沖被CPU 計(jì)數(shù)器計(jì)數(shù)，當(dāng)計(jì)數(shù)值為某路段的路程時(shí)，重新初始化計(jì)數(shù)器準(zhǔn)備下一段路程的計(jì)數(shù)，直至到達(dá)終點(diǎn)。

3.2 從起點(diǎn)到機(jī)器人上坡路段A 的控制方法

當(dāng)差動(dòng)輪式移動(dòng)機(jī)器人左右驅(qū)動(dòng)輪的伺服控制電路參數(shù)和直流電機(jī)型號(hào)參數(shù)相同時(shí)，若給定電機(jī)的控制量大小相等且方向相同時(shí)，移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡應(yīng)為直線。機(jī)器人在A 段時(shí)，機(jī)器人的移動(dòng)電機(jī)采用串口位置控制方式控制。該段機(jī)器人不需轉(zhuǎn)彎，因此機(jī)器人左右輪電機(jī)驅(qū)動(dòng)器通過(guò)max232 連接在機(jī)器人CPU 的串口上，機(jī)器人理論上始終保持走直線。機(jī)器人啟動(dòng)前，CPU 完成機(jī)器人運(yùn)動(dòng)電機(jī)控制方式及參數(shù)的設(shè)置，具體設(shè)置如下:

(1)絕對(duì)位置0，即當(dāng)前位置為絕對(duì)位置;

(2)相對(duì)位置145 000，即電機(jī)正向運(yùn)動(dòng)145 000個(gè)單位(4 000 個(gè)單位電機(jī)轉(zhuǎn)一周);

(3)電機(jī)在該段最大轉(zhuǎn)速為1 000 r/s;

(4)最大加速度為50 r/s2;

若機(jī)器人后輪直徑為10 cm，減速器減速比為4.2∶ 1，故該段機(jī)器人的位移:

若該段內(nèi)，機(jī)器人持續(xù)以最大加速度加減速，則機(jī)器人在該段加減速時(shí)間相等，故電機(jī)轉(zhuǎn)速可做如下計(jì)算:

機(jī)器人加速時(shí)間

因此機(jī)器人全程運(yùn)動(dòng)時(shí)間為T(mén)=2t≈4.626 s

該過(guò)程電機(jī)的實(shí)際最大轉(zhuǎn)速為vmax= at =50 ×2.323≈116 r/s

當(dāng)機(jī)器人到達(dá)A 段末點(diǎn)時(shí)，電機(jī)控制方式轉(zhuǎn)變?yōu)镻WM 控制，上位機(jī)根據(jù)視頻系統(tǒng)采集到的位置偏移處理成為機(jī)器人轉(zhuǎn)動(dòng)量，并將機(jī)器人自動(dòng)調(diào)整到道路中央。

3.3 直行及轉(zhuǎn)彎控制方法

從B 段起點(diǎn)開(kāi)始，機(jī)器人運(yùn)動(dòng)電機(jī)采用PWM 速度控制方式，同時(shí)用光碼盤(pán)來(lái)檢測(cè)機(jī)器人位移，方便控制機(jī)器人狀態(tài)轉(zhuǎn)換的時(shí)間。在PWM 控制方式下，能方便的控制機(jī)器人前進(jìn)的速度，若在比賽過(guò)程中，還可根據(jù)賽況對(duì)機(jī)器人的速度做出適當(dāng)調(diào)節(jié)。在后面的運(yùn)動(dòng)中，機(jī)器人的基本參數(shù)設(shè)置如下:

(1)電機(jī)最大轉(zhuǎn)速5 000 r/s

(2)電機(jī)最大加速度50 r/s2

(3)電機(jī)直行時(shí)PWM 占空比為39.06%

(4)電機(jī)轉(zhuǎn)彎時(shí)PWM 占空比分別為39.06%、44.92%

故有機(jī)器人直行時(shí)，最大速度可做如下計(jì)算:

其中:V 為電機(jī)轉(zhuǎn)速;

v'為機(jī)器人移動(dòng)速度;

φ 為PWM 占空比;

d 為機(jī)器人輪子外徑;

r 為減速器減速比。

機(jī)器人轉(zhuǎn)彎時(shí)，各數(shù)據(jù)量計(jì)算如下:

內(nèi)側(cè)電機(jī)轉(zhuǎn)速

內(nèi)側(cè)輪運(yùn)動(dòng)的速度

同理可得，外側(cè)電機(jī)轉(zhuǎn)速V2=－1 094 r/s，外側(cè)輪運(yùn)動(dòng)速度v2=81.9 m/s。

機(jī)器人轉(zhuǎn)彎運(yùn)動(dòng)軌跡如圖6 所示。

圖6 機(jī)器人轉(zhuǎn)彎運(yùn)動(dòng)軌跡示意圖

機(jī)器人轉(zhuǎn)彎時(shí)，內(nèi)外側(cè)半徑可做如下計(jì)算:

其中:l=48 cm 故有

以上參數(shù)和計(jì)算出的數(shù)據(jù)為機(jī)器人直行和轉(zhuǎn)彎等軌跡控制提供重要的理論依據(jù)，而且可以根據(jù)不同賽道對(duì)機(jī)器人速度做出實(shí)時(shí)調(diào)節(jié)，增強(qiáng)機(jī)器人在賽場(chǎng)上的適應(yīng)性。

4 結(jié)論

通過(guò)在實(shí)驗(yàn)室場(chǎng)地采用上述方法調(diào)試機(jī)器人，從起點(diǎn)到點(diǎn)A 用串口控制方式控制可以保證機(jī)器人準(zhǔn)確到達(dá)點(diǎn)B 停下。然后從點(diǎn)B 出發(fā)時(shí)，根據(jù)計(jì)算結(jié)果控制電機(jī)轉(zhuǎn)速，機(jī)器人可以快速、穩(wěn)定地完成上坡、下坡、轉(zhuǎn)彎等路段。文中提出的基于路程檢測(cè)的機(jī)器人運(yùn)動(dòng)控制方法具有較強(qiáng)的可行性，實(shí)現(xiàn)較為容易，為機(jī)器人適應(yīng)復(fù)雜賽道提供一定的參考意義。

［1］劉玉鵬，海丹，鄭志強(qiáng)．基于測(cè)程法的四輪全向移動(dòng)機(jī)器人自定位方法研究［C］//2005 中國(guó)機(jī)器人大賽論文集，2005:17－23．

［2］高國(guó)富，謝少榮，羅均．機(jī)器人傳感器及其應(yīng)用［M］．北京:化學(xué)工業(yè)出版社，2005．

［3］姜義．光電編碼器的原理與應(yīng)用［J］．機(jī)床電器，2010，37(2):25－28．

［4］李磊，葉濤，譚民，等．移動(dòng)機(jī)器人技術(shù)研究現(xiàn)狀與未來(lái)［J］．機(jī)器人，2002(5):475－480．

［5］TEHRANI Alireza Fadaei，ALI MOHAMMAD Doosthosseini，MOBALLEGH Hamid Reza，et al．A New Odometry System to Reduce Asymmetric Errors for Omnidirectional Mobile Robots［J］．RoboCup，2004，3020:600－610．

［6］ROJAS Raul．Omnidirectional Control［R］，F(xiàn)reie University Berlin，2005．

［7］陶永華．新型PID 控制及其應(yīng)用［M］．2 版．北京:機(jī)械工業(yè)出版社，2003．

［8］馮桂寧，蔡翔?。畣纹瑱C(jī)模糊PID 自整定控制算法的實(shí)現(xiàn)及仿真［J］．電子元器件應(yīng)用，2007，5(9):59－62．