基于CoDeSys的全向移動機器人控制系統設計

，，，

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

移動機器人以其具有靈活性、提高生產效率和改善勞動條件等優點而得到廣泛應用[1]。隨著社會的不斷發展，人們對移動機器人的靈活性及智能提出了更高的要求，要求移動機器人能在一定的范圍內安全運動，完成特定的任務，增強移動機器人對環境的適應能力[2]。

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

當移動機器人進入奇異點及附近區域，車輪出現較大的轉向速度，導致系統不穩定。一些方法是使機器人遠離這些區域，但是這樣會限制機器人的靈活性。筆者基于傳統的路徑跟蹤控制器，設計了一種全局穩定的路徑跟蹤控制器。首先搭建一臺對角驅動的四輪全向移動機器人。其次，根據車體期望位置-航向和實際位置-航向偏差信號，構建正定的Lyapunov函數，設計適應車體線速度的反饋控制律，使閉環控制系統下的Lyapunov函數導數為負定，車體位置-航向在該控制律作用下跟蹤上期望位置-航向。依據逆運動學約束，建立了車體最大速度與車輪牽引速度、轉向速度之間的車體運動約束關系；在此基礎上，選取車體最大線速度，規劃車體線速度，使得車體以時間最優跟蹤期望軌跡。最后，基于CoDeSys上位機進行實驗，驗證了算法的有效性和穩定性。

1 全向移動機器人結構設計

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

圖1 四輪構型Fig.1 Four-wheel configuration

車輪選擇意大利CFR舵輪。CFR舵輪具有體積小巧、操作簡單和適應性強等優點。舵輪主要由一個牽引電機和一個轉向電機組成，并配有兩個增量式編碼器。

小車電源選擇磷酸鐵鋰電池。磷酸鐵鋰電池具有壽命長、使用安全、充電快速、耐高溫和容量大等優點。驅動器選用RoboteQ雙通道驅動器，該驅動器具有平穩、高精度的低速控制特性和良好的高速性能。控制器采用基于CoDeSys的運動控制器，并通過一個EtherCAT轉CAN的數據網關轉換器連接驅動器，實現對舵輪的控制?？刂葡到y結構如圖2所示。

圖2 控制系統結構圖Fig.2 Control system structure diagram

在上位機CoDeSys軟件上，使用結構化文本(ST)和連續功能圖(CFC)編程語言，開發小車控制程序，然后下載到控制器?？刂破鬟\行程序，通過EtherCAT總線以線性拓撲結構的方式[14]，把數據傳給從站網關轉換器，網關轉換器再把數據轉化成CAN數據發給驅動器，然后驅動器分別給牽引電機和轉向電機下發指令，控制電機的運轉，從而控制車體的運動?？刂屏鞒虉D如圖3所示。

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

2 速度受限的移動機器人控制算法設計

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

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

2.1 運動學模型

(1)

2.2 控制律設計

圖5 系統示意框圖Fig.5 System schematic block diagram

移動車運動偏差方程為

(2)

θe=θd-θb

(3)

對式(2，3)求導可得

(4)

(5)

(6)

為系統設計相應的Lyapunov函數，然后利用Lyapunov函數的穩定性判定條件，得出移動機器人軌跡跟蹤的控制律[15]。設運動學控制律的Lyapunov函數為

(7)

對式(7)求導并將式(4～6)代入得

(8)

假設

(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)

根據運動學約束方程式(1)，并對其兩邊求導可得

(19)

(20)

(21)

(22)

當v取vmax時，小車以可行的最大速度運動，所用時間最短，實現時間最優控制。

2.3 仿真實驗

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

根據仿真結果可知：基于Lyapunov穩定性原理設計的一種適應車體線速度的反饋控制律，可使移動機器人跟蹤上期望軌跡，驗證了算法的有效性。

3 算法實現與實驗分析

3.1 算法實現

在CoDeSys上位機上使用結構化文本(ST)和連續功能圖(CFC)編程語言開發程序。主程序主要由四部分組成。由算法中誤差計算模塊、速度計算模塊及控制律模塊組構成控制器功能塊(Controller)，用于參考軌跡的規劃及機器人運行軌跡控制；由逆運動學模塊構成逆運動學功能塊(InverseKinematic)，用于運動的分解；驅動功能塊(Driver)用于數據的下發及采集，驅動機器人運動；由正運動學模塊構成正運動學功能塊(ForwardKinematic)，根據反饋的數據計算出車體的實際位姿。如圖7所示，由4 個功能塊構成運動控制系統，實現對機器人的控制。

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

3.2 實驗分析

為驗證控制算法的正確性，對系統進行實驗驗證。四輪全向移動機器人的參數如表1所示。

基于CoDeSys上位機進行實驗，實驗參考軌跡和仿真參考軌跡相同，實驗結果如圖8～10所示。

表1 機器人參數Table 1 Robot parameters

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

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

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

圖10 車輪旋轉速度Fig.10 Wheel steering speed

綜合上述的實驗結果可以得出：在該控制律的作用下，輪式移動機器人能夠跟蹤上設定的參考軌跡，并且在復雜運動過程中，車輪的驅動和轉向速度均保持在預定值之下，證明了所設計的速度受限控制律的有效性和穩定性。

4 結 論

設計并搭建一臺全向輪式移動機器人作為實驗平臺。根據Lyapunov函數穩定判據，設計了適應車體線速度的反饋控制律。基于運動學約束，對車體線速度進行規劃，求出車體最大瞬時速度，使得無論機器人距離它的奇異點區域有多近或者多遠，車輪的速度都保持在預定極限之下，解決因為奇異點狀況造成的系統不穩定問題，并實現跟蹤期望軌跡時間最優。在CoDeSys上位機上編寫運動控制程序，實現了移動機器人對軌跡的跟蹤，驗證了控制律的有效性和穩定性。