，，，

(浙江工業(yè)大學(xué) 信息工程學(xué)院，浙江 杭州 310023)

移動(dòng)機(jī)器人以其具有靈活性、提高生產(chǎn)效率和改善勞動(dòng)條件等優(yōu)點(diǎn)而得到廣泛應(yīng)用[1]。隨著社會(huì)的不斷發(fā)展，人們對(duì)移動(dòng)機(jī)器人的靈活性及智能提出了更高的要求，要求移動(dòng)機(jī)器人能在一定的范圍內(nèi)安全運(yùn)動(dòng)，完成特定的任務(wù)，增強(qiáng)移動(dòng)機(jī)器人對(duì)環(huán)境的適應(yīng)能力[2]。

因此，近年來(lái)移動(dòng)機(jī)器人成為機(jī)器人研究領(lǐng)域的中心之一[3]。移動(dòng)機(jī)器人的移動(dòng)機(jī)構(gòu)有多種形式，常見的有輪式、腿式和復(fù)合式等，這些移動(dòng)機(jī)器人之中，輪式移動(dòng)機(jī)器人由于設(shè)計(jì)與控制簡(jiǎn)單，被廣泛采用[4]。當(dāng)移動(dòng)機(jī)器人需要在狹窄、擁擠或需要避障的環(huán)境中運(yùn)行時(shí)，機(jī)器人的全向移動(dòng)能力就變得至關(guān)重要[5]。全向移動(dòng)機(jī)器人是一類典型的移動(dòng)機(jī)器人系統(tǒng)，可以在不改變車體的情況下，實(shí)現(xiàn)從當(dāng)前位置向任意方向的運(yùn)動(dòng)，而且可以零半徑轉(zhuǎn)向，且有平面上沿X，Y軸平動(dòng)和繞Z軸轉(zhuǎn)動(dòng)的3個(gè)自由度[6]。目前，許多學(xué)者都對(duì)全向移動(dòng)機(jī)器人做了大量的研究工作，Jamali等[7]以Mecanum輪式的全向移動(dòng)機(jī)器人為對(duì)象，研究它的建模與仿真，并設(shè)計(jì)了三個(gè)獨(dú)立的模糊控制器來(lái)完成機(jī)器人的位置和角度跟蹤。Oftadeh等[8-11]采用四輪全向移動(dòng)平臺(tái)，針對(duì)移動(dòng)機(jī)器人在奇異點(diǎn)附近存在無(wú)界的轉(zhuǎn)向速度問(wèn)題，設(shè)計(jì)了一種新的路徑跟蹤控制器，將機(jī)器人的車體線速度描述為車輪驅(qū)動(dòng)和轉(zhuǎn)向速度的函數(shù)，然后利用這些函數(shù)求出最大瞬時(shí)速度，使車輪速度保持在預(yù)定的界限之下。

當(dāng)移動(dòng)機(jī)器人進(jìn)入奇異點(diǎn)及附近區(qū)域，車輪出現(xiàn)較大的轉(zhuǎn)向速度，導(dǎo)致系統(tǒng)不穩(wěn)定。一些方法是使機(jī)器人遠(yuǎn)離這些區(qū)域，但是這樣會(huì)限制機(jī)器人的靈活性。筆者基于傳統(tǒng)的路徑跟蹤控制器，設(shè)計(jì)了一種全局穩(wěn)定的路徑跟蹤控制器。首先搭建一臺(tái)對(duì)角驅(qū)動(dòng)的四輪全向移動(dòng)機(jī)器人。其次，根據(jù)車體期望位置-航向和實(shí)際位置-航向偏差信號(hào)，構(gòu)建正定的Lyapunov函數(shù)，設(shè)計(jì)適應(yīng)車體線速度的反饋控制律，使閉環(huán)控制系統(tǒng)下的Lyapunov函數(shù)導(dǎo)數(shù)為負(fù)定，車體位置-航向在該控制律作用下跟蹤上期望位置-航向。依據(jù)逆運(yùn)動(dòng)學(xué)約束，建立了車體最大速度與車輪牽引速度、轉(zhuǎn)向速度之間的車體運(yùn)動(dòng)約束關(guān)系；在此基礎(chǔ)上，選取車體最大線速度，規(guī)劃車體線速度，使得車體以時(shí)間最優(yōu)跟蹤期望軌跡。最后，基于CoDeSys上位機(jī)進(jìn)行實(shí)驗(yàn)，驗(yàn)證了算法的有效性和穩(wěn)定性。

1 全向移動(dòng)機(jī)器人結(jié)構(gòu)設(shè)計(jì)

對(duì)于移動(dòng)機(jī)器人的車輪布局，一般常用的布局主要分為三輪配置和四輪配置。四輪布局相對(duì)于三輪布局穩(wěn)定性更好[12]。四輪布局如圖1所示，一般可分為前輪驅(qū)動(dòng)(圖1a)、后輪驅(qū)動(dòng)(圖1b)和左右輪對(duì)角驅(qū)動(dòng)(圖1c)三種。對(duì)角驅(qū)動(dòng)方式可使車輛具有橫向移動(dòng)的能力，提高車輛的靈活性[13]。所以筆者采用左右輪對(duì)角驅(qū)動(dòng)(圖1c)方式設(shè)計(jì)車輛底盤。

圖1 四輪構(gòu)型Fig.1 Four-wheel configuration

車輪選擇意大利CFR舵輪。CFR舵輪具有體積小巧、操作簡(jiǎn)單和適應(yīng)性強(qiáng)等優(yōu)點(diǎn)。舵輪主要由一個(gè)牽引電機(jī)和一個(gè)轉(zhuǎn)向電機(jī)組成，并配有兩個(gè)增量式編碼器。

小車電源選擇磷酸鐵鋰電池。磷酸鐵鋰電池具有壽命長(zhǎng)、使用安全、充電快速、耐高溫和容量大等優(yōu)點(diǎn)。驅(qū)動(dòng)器選用RoboteQ雙通道驅(qū)動(dòng)器，該驅(qū)動(dòng)器具有平穩(wěn)、高精度的低速控制特性和良好的高速性能?？刂破鞑捎没贑oDeSys的運(yùn)動(dòng)控制器，并通過(guò)一個(gè)EtherCAT轉(zhuǎn)CAN的數(shù)據(jù)網(wǎng)關(guān)轉(zhuǎn)換器連接驅(qū)動(dòng)器，實(shí)現(xiàn)對(duì)舵輪的控制?？刂葡到y(tǒng)結(jié)構(gòu)如圖2所示。

圖2 控制系統(tǒng)結(jié)構(gòu)圖Fig.2 Control system structure diagram

在上位機(jī)CoDeSys軟件上，使用結(jié)構(gòu)化文本(ST)和連續(xù)功能圖(CFC)編程語(yǔ)言，開發(fā)小車控制程序，然后下載到控制器。控制器運(yùn)行程序，通過(guò)EtherCAT總線以線性拓?fù)浣Y(jié)構(gòu)的方式[14]，把數(shù)據(jù)傳給從站網(wǎng)關(guān)轉(zhuǎn)換器，網(wǎng)關(guān)轉(zhuǎn)換器再把數(shù)據(jù)轉(zhuǎn)化成CAN數(shù)據(jù)發(fā)給驅(qū)動(dòng)器，然后驅(qū)動(dòng)器分別給牽引電機(jī)和轉(zhuǎn)向電機(jī)下發(fā)指令，控制電機(jī)的運(yùn)轉(zhuǎn)，從而控制車體的運(yùn)動(dòng)?？刂屏鞒虉D如圖3所示。

圖3 控制流程圖Fig.3 Control flow chart

2 速度受限的移動(dòng)機(jī)器人控制算法設(shè)計(jì)

如圖4所示，坐標(biāo)系{U}為世界坐標(biāo)系，坐標(biāo)系{Bd}和{T}分別為期望運(yùn)動(dòng)軌跡Pd(s)(s為自變量，表示軌跡弧長(zhǎng))上的移動(dòng)車體坐標(biāo)系和速度坐標(biāo)系。{B}為移動(dòng)車體坐標(biāo)系，{Bv}移動(dòng)車體速度坐標(biāo)系，{B1}車輪1坐標(biāo)系。p為車體期望位置，θd為期望航向角，θd(s)為航向輪廓，vt，ψt分別為期望線速度大小和方向。q為移動(dòng)車體實(shí)際位置，θb為實(shí)際航向角，v，ψv分別為線速度大小和方向，ωb為車體角速度。l1，l2為車體中心點(diǎn)到驅(qū)動(dòng)輪的距離，φi，vi分別為車輪的角度和速度。

圖4 路徑跟蹤示意圖及參數(shù)Fig.4 Trajectory tracking schematics and parameters

2.1 運(yùn)動(dòng)學(xué)模型

(1)

2.2 控制律設(shè)計(jì)

圖5 系統(tǒng)示意框圖Fig.5 System schematic block diagram

移動(dòng)車運(yùn)動(dòng)偏差方程為

(2)

θe=θd-θb

(3)

對(duì)式(2，3)求導(dǎo)可得

(4)

(5)

(6)

為系統(tǒng)設(shè)計(jì)相應(yīng)的Lyapunov函數(shù)，然后利用Lyapunov函數(shù)的穩(wěn)定性判定條件，得出移動(dòng)機(jī)器人軌跡跟蹤的控制律[15]。設(shè)運(yùn)動(dòng)學(xué)控制律的Lyapunov函數(shù)為

(7)

對(duì)式(7)求導(dǎo)并將式(4～6)代入得

(8)

假設(shè)

(9)

(10)

(11)

可得反饋控制律為

ψv=ψt+arcsin(k2yecosψt-
k1xesinψt)=ψt+arcsinρ

(12)

(13)

(14)

將反饋控制律式(12～14)代入式(8)，可得

(15)

將式(12～14)代入式(4～6)得

(16)

(17)

(18)

根據(jù)運(yùn)動(dòng)學(xué)約束方程式(1)，并對(duì)其兩邊求導(dǎo)可得

(19)

(20)

(21)

(22)

當(dāng)v取vmax時(shí)，小車以可行的最大速度運(yùn)動(dòng)，所用時(shí)間最短，實(shí)現(xiàn)時(shí)間最優(yōu)控制。

2.3 仿真實(shí)驗(yàn)

圖6 軌跡跟蹤示意圖Fig.6 Trajectory tracking schematic.

根據(jù)仿真結(jié)果可知：基于Lyapunov穩(wěn)定性原理設(shè)計(jì)的一種適應(yīng)車體線速度的反饋控制律，可使移動(dòng)機(jī)器人跟蹤上期望軌跡，驗(yàn)證了算法的有效性。

3 算法實(shí)現(xiàn)與實(shí)驗(yàn)分析

3.1 算法實(shí)現(xiàn)

在CoDeSys上位機(jī)上使用結(jié)構(gòu)化文本(ST)和連續(xù)功能圖(CFC)編程語(yǔ)言開發(fā)程序。主程序主要由四部分組成。由算法中誤差計(jì)算模塊、速度計(jì)算模塊及控制律模塊組構(gòu)成控制器功能塊(Controller)，用于參考軌跡的規(guī)劃及機(jī)器人運(yùn)行軌跡控制；由逆運(yùn)動(dòng)學(xué)模塊構(gòu)成逆運(yùn)動(dòng)學(xué)功能塊(InverseKinematic)，用于運(yùn)動(dòng)的分解；驅(qū)動(dòng)功能塊(Driver)用于數(shù)據(jù)的下發(fā)及采集，驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)；由正運(yùn)動(dòng)學(xué)模塊構(gòu)成正運(yùn)動(dòng)學(xué)功能塊(ForwardKinematic)，根據(jù)反饋的數(shù)據(jù)計(jì)算出車體的實(shí)際位姿。如圖7所示，由4 個(gè)功能塊構(gòu)成運(yùn)動(dòng)控制系統(tǒng)，實(shí)現(xiàn)對(duì)機(jī)器人的控制。

圖7 主程序框圖Fig.7 Frame of main program

3.2 實(shí)驗(yàn)分析

為驗(yàn)證控制算法的正確性，對(duì)系統(tǒng)進(jìn)行實(shí)驗(yàn)驗(yàn)證。四輪全向移動(dòng)機(jī)器人的參數(shù)如表1所示。

基于CoDeSys上位機(jī)進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)參考軌跡和仿真參考軌跡相同，實(shí)驗(yàn)結(jié)果如圖8～10所示。

表1 機(jī)器人參數(shù)Table 1 Robot parameters

圖8 車體位姿誤差Fig.8 Body position and posture error

圖9 車體和車輪線速度Fig.9 Car body and wheel linear velocity

圖8顯示了在進(jìn)行軌跡跟蹤時(shí)車體的位姿誤差，可知車體基本跟蹤上參考軌跡；圖9顯示了車體和車輪的線速度；圖10顯示了車輪的旋轉(zhuǎn)速度。從圖10中可以看出：在時(shí)間為1.8 s和4.6 s左右時(shí)，根據(jù)車輪最大轉(zhuǎn)向速度，降低車體速度，從而使車輪轉(zhuǎn)向速度保持在預(yù)定的極限以下。

圖10 車輪旋轉(zhuǎn)速度Fig.10 Wheel steering speed

綜合上述的實(shí)驗(yàn)結(jié)果可以得出：在該控制律的作用下，輪式移動(dòng)機(jī)器人能夠跟蹤上設(shè)定的參考軌跡，并且在復(fù)雜運(yùn)動(dòng)過(guò)程中，車輪的驅(qū)動(dòng)和轉(zhuǎn)向速度均保持在預(yù)定值之下，證明了所設(shè)計(jì)的速度受限控制律的有效性和穩(wěn)定性。

4 結(jié) 論

設(shè)計(jì)并搭建一臺(tái)全向輪式移動(dòng)機(jī)器人作為實(shí)驗(yàn)平臺(tái)。根據(jù)Lyapunov函數(shù)穩(wěn)定判據(jù)，設(shè)計(jì)了適應(yīng)車體線速度的反饋控制律?；谶\(yùn)動(dòng)學(xué)約束，對(duì)車體線速度進(jìn)行規(guī)劃，求出車體最大瞬時(shí)速度，使得無(wú)論機(jī)器人距離它的奇異點(diǎn)區(qū)域有多近或者多遠(yuǎn)，車輪的速度都保持在預(yù)定極限之下，解決因?yàn)槠娈慄c(diǎn)狀況造成的系統(tǒng)不穩(wěn)定問(wèn)題，并實(shí)現(xiàn)跟蹤期望軌跡時(shí)間最優(yōu)。在CoDeSys上位機(jī)上編寫運(yùn)動(dòng)控制程序，實(shí)現(xiàn)了移動(dòng)機(jī)器人對(duì)軌跡的跟蹤，驗(yàn)證了控制律的有效性和穩(wěn)定性。