錢曉明　朱立群　樓佩煌　張滬松

(1.南京航空航天大學機電學院， 南京 210016； 2.江蘇省精密與微細制造技術重點實驗室， 南京 210016)

0　引言

“中國制造2025”戰(zhàn)略的實施以及近年來數字化、無人化工廠、物流業(yè)和倉儲業(yè)的迅速發(fā)展，使自動導航車(Automated guided vehicles,AGV)的需求量急劇上升?，F代制造系統(tǒng)中，各個物流的傳輸時間占整個生產過程的80%～90%，物料傳輸與存儲費用占整個零部件加工費用的30%～40%[1-2]，AGV在縮短物流傳輸時間、降低物料傳輸與存儲費用方面起到至關重要的作用。因此，提高AGV路徑跟蹤的效率和精度對推廣AGV的應用具有至關重要的作用。

針對路徑跟蹤問題[3-9]，文獻[8]提出了一種最優(yōu)控制策略，通過狀態(tài)反饋得到合適的控制律，從而使系統(tǒng)的性能指標函數達到最小值。但隨著差速驅動輪中心速度的改變，其魯棒性也隨之變化。文獻[9]針對全方位移動機器人的延時問題，引入了廣義預測控制法，同時將多輸入多輸出的線性模型分解成單輸入單輸出模型。在最優(yōu)化控制中，加權矩陣Q、R的選擇是一個難以避免的難題，文獻[8-9]也沒能有效地解決這個難題，而目前主要使用試湊法[10-12]，即將加權矩陣R固定為一個確定的或特殊的矩陣，通過反復實驗與仿真從而找到合適的加權矩陣Q。

本文提出一種基于相對位姿狀態(tài)的優(yōu)化控制，將連續(xù)的狀態(tài)轉換成離散的系統(tǒng)，通過有限的糾偏步驟，使角度偏差和距離偏差同步收斂，同時通過建立Mecanum輪AGV的運動學模型，將目標函數中的加權矩陣求解轉換為求解一個有限的控制序列，并且該控制序列是關于控制量的函數，從而根據該有限的控制序列實現對AGV的控制，且控制周期取決于控制步數，同時避開Q、R選擇難的問題。

1　Mecanum輪AGV運動學模型

圖1　全方位AGV運動學模型Fig.1　Omni-directional AGV kinematic model

本文采用的是視覺導航四輪獨立驅動的Mecanum輪AGV小車，其在平面上具有3個自由度，機動性能好[13]。且該形式AGV的加速度性能在各個方向上比較均勻，加速度的大小基本受各個方向的影響[9]。全方位運動AGV運動學模型如圖1所示，AGV系統(tǒng)運動的逆運動學方程為

(1)

式中ω1——第1個輪子角速度，rad/s

ω2——第2個輪子角速度，rad/s

ω3——第3個輪子角速度，rad/s

ω4——第4個輪子角速度，rad/s

r——輪子半徑，mm

vx——AGV沿X方向的速度，mm/s

vy——AGV沿Y方向的速度，mm/s

ωz——AGV旋轉的角速度，rad/s

L1——前后輪距離的一半，mm

L2——左右輪中心距離的一半，mm

對式(1)進行變換可得到系統(tǒng)的正運動學方程為

(2)

式(2)、(1)分別為AGV系統(tǒng)的正、逆運動學方程。由正逆運動學方程可知，當4個Mecanum輪的速度選擇恰當時，就可以實現AGV在運動平面上的任意運動。由Rank(A)為3可知變量vx、vy、ωz相互獨立，從而將原來的4個控制變量轉換為3個變量vx、vy、ωz。其中vx、vy和后續(xù)的路徑跟蹤的距離偏差ed與沿軌跡的切線速度vτ相關，而ωz與路徑跟蹤的角度偏差eθ相關。

2　基于連續(xù)系統(tǒng)離散化的有限步運動預測優(yōu)化控制

最優(yōu)控制問題可以簡單描述為根據被控對象的數學模型，選擇一個容許的控制律，使被控對象按照預定的要求運行，并使給定的性能指標達到最優(yōu)值。文獻[14]通過對控制方程的簡化和歸一化處理使機器人的控制量Ui規(guī)整到一個確定的閉區(qū)間上，從而得到系統(tǒng)的時間Bang-Bang最優(yōu)控制。

近年來，實時嵌入式控制器[13-14]在控制領域中使用越來越多，而一般實時嵌入式控制器都是按步執(zhí)行的，因此對于控制器而言控制序列即離散系統(tǒng)更能有利于滾動控制[15-16]。為了實現對連續(xù)的過程實現嵌入式控制，需要把連續(xù)的系統(tǒng)離散化。本節(jié)首先通過將連續(xù)的系統(tǒng)離散化，再選擇合適的目標函數，最后求解合適的有限控制序列使系統(tǒng)達到要求的狀態(tài)。

2.1　控制系統(tǒng)性能指標的選取及系統(tǒng)離散化

根據最優(yōu)控制理論，性能指標是指控制系統(tǒng)在容許的控制律作用下，系統(tǒng)所能達到性能程度，性能指標的內容和形式取決于最終的目標[17]。本文選擇積分型性能指標J[17-18]，即

(3)

式中x(t)——系統(tǒng)狀態(tài)量

u(t)——系統(tǒng)控制量

L(x(t),u(t),t)——x(t)、u(t)和t的連續(xù)函數

文獻[10-12,18]利用LQR優(yōu)化方法，將目標函數具體化為

(4)

式中Q、R——加權矩陣

文獻[5]認為Q的選取不宜過大，否則系統(tǒng)控制器的輸出會趨向飽和，同時會使系統(tǒng)對噪聲比較敏感，系統(tǒng)的抗干擾能力下降。文獻[10]認為距離偏差不能過大，否則會產生角度超調，并且前輪的轉角會超過機械機構的極限。本節(jié)考慮約束條件

(5)

式中vxmax——X方向最大速度

vymax——Y方向最大速度

ωzmax——AGV最大旋轉角速度

amax——系統(tǒng)的最大加速度

a——系數的加速度

將當前的狀態(tài)偏差分為有限步進行消除，在最優(yōu)控制中通過求解控制量的一個有限最優(yōu)序列，將Q的試湊難題轉換為求解控制量序列，此時將連續(xù)的系統(tǒng)轉換為一個離散的系統(tǒng)，不僅避免了Q的求解，也有利于嵌入式控制器實時滾動控制。

由上述可知，vx、vy、ωz相互獨立，其中vx、vy和路徑跟蹤的距離偏差ed與沿軌跡的切線速度vτ相關，ωz與路徑跟蹤的角度偏差eθ相關。因此，在系統(tǒng)性能滿足的前提下，在確定的控制步數內，總可以選擇合適的角度控制量Δω使角度偏差同步于距離偏差收斂于零。又當AGV進行路徑跟蹤時，希望AGV沿路徑的切線速度vτ是一定的，可知vx、vy之間存在一定的關系，所以式(4)可以簡化為只包含控制量u的形式，且控制量中只包含一個控制量vx。所以，可以將系統(tǒng)的目標函數離散為

(6)

式中k——系統(tǒng)控制步數

2.2　基于位姿狀態(tài)的控制模型的建立

本文研究的是基于位姿狀態(tài)的視覺導引四輪獨立驅動的Mecanum輪路徑跟蹤問題[19-21]，其結構簡圖如圖2所示。AGV在路徑跟蹤過程中，相對于路徑有一定的位姿狀態(tài)，在某一具體時刻該位姿狀態(tài)主要表現為CCD攝像機實時采集到的角度偏差和距離偏差，而路徑跟蹤的實質是在產生角度偏差和距離偏差時，如何快速、平穩(wěn)的消除偏差。本文通過控制vx和ωz來消除這兩種偏差，調整位姿狀態(tài)。

圖2　Mecanum輪AGV結構簡圖Fig.2　Structure diagram of Mecanum wheel AGV 1.Mecanum輪　2.視野范圍　3.CCD攝像機

假設AGV的控制周期為Ts，當前位姿狀態(tài)為k時的角度偏差eθ(k)，經過一個周期的控制后，到達下一個狀態(tài)k+1時，由前面的分析可知vx、vy、ωz相互獨立，所以在AGV的角度偏差變化中可以單獨控制ωz使角度偏差均勻且同步于距離偏差收斂于零，則k+1時的角度偏差為

eθ(k+1)=eθ(k)+ΔωzTs

(7)

如圖3所示，k時刻距離偏差為lOA，經過Ts到k+1時距離偏差的減少量為lBC，并且此時的偏差為lCE，則

lCE=lOA+lDE-lBC

(8)

圖3　AGV偏差幾何解析Fig.3　Geometric analysis of AGV

當控制周期很短且AGV的速度值為有限值時，圓弧OC可近似地看成直線，可視AGV移動的距離為vTs，即lOC=vTs。由圖4可知vx、vy、vτ和v存在關系

(9)

圖4　AGV運行軌跡模型Fig.4　AGV running trajectory model

在RtΔAED中，距離的增量lDE為

lDE=lDAtaneθ=lOCcosθtaneθ

(10)

當角度偏差很小時，由式(8)～(10)可得距離偏差ed(k+1)近似為

(11)

從而得到系統(tǒng)的運動學模型即狀態(tài)方程為

(12)

引入拉格朗日乘子函數

λ(k+1)=[λ1(k+1)λ2(k+1)]T

構造系統(tǒng)的廣義離散泛函

(13)

又離散泛函取得極小值的條件為

(14)

由式(13)和式(14)可得

(15)

式中c——常數

又當控制步數N確定時，有

(16)

由式(15)和式(16)可得

(17)

將式(15)～(17)代入狀態(tài)方程(12)中可得

(18)

又系統(tǒng)應滿足糾偏目標

(19)

即經過N步控制后，偏差同步收斂于零。

由式(18)和式(19)可得

(20)

顯然，c是一個只與初始偏差狀態(tài)控制步數以及控制周期有關的常數。因此可得到最終速度和角速度控制量的解析表達式為

(21)

2.3　有限步數N的確定

速度控制量vx(k)的大小及變化率必須滿足速度和加速度的約束條件，即式(5)的約束，由式(21)可知

顯然vx(k)是(N-k)2的線性函數，分析可知，當k=0時，vx(k)得最大值且

控制周期Ts一定，當|vx(k+1)-vx(k)|得最大值時，則加速度也最大，而

顯然加速度|a(k)|是(2N-2k-1)的線性函數，所以加速度的最大值|a(k)|max也是在k=0時得到，且

故控制步數N確定公式為

(22)

根據式(22)即可確定2個N值，取大者為最優(yōu)控制有限步數的最小值，則只要控制步數大于該最小值即可，一般控制步數不宜過大，過大會造成系統(tǒng)的延時嚴重。同時控制步數反映了路徑跟蹤過程中兩種偏差消除的快慢程度，即糾偏的效率，顯然步數越小系統(tǒng)的糾偏效率也就越高，實時性也就越好。

2.4　基于連續(xù)系統(tǒng)離散化的有限步數運動預測的路徑跟蹤優(yōu)化控制

文獻[9-11,17]通過試湊加權矩陣Q、R來獲得系統(tǒng)的增益系數，而求得的增益系數是在某一特定速度下獲得的，當AGV運行速度改變時，系統(tǒng)的魯棒性就會變差，并且增益系數的獲得是通過反復迭代求解Raccati方程，計算難度大且不利于實時控制器的控制。

本文將系統(tǒng)及目標函數離散化，在系統(tǒng)的約束條件(式(5))下，通過尋求有限步的速度控制序列和角速度控制序列，從而得到系統(tǒng)的最優(yōu)控制。且該控制序列能保證角度偏差和距離偏差同步快速的收斂。同時通過離散化的控制序列更加有利于實時控制器的滾動控制。由式(21)可知，該控制序列只與偏差初始值、控制步數、控制周期、運行速度vτ有關。一般情況下，系統(tǒng)的控制周期是不變的，在初始偏差一定的情況下，控制步數只取決于運行速度，且角度的控制量與k正相關，vx(k)是(N-k)2的線性函數，其計算量小，控制方便。

3　系統(tǒng)仿真與實驗分析

本文的第1節(jié)推導出來了AGV小車的運動學模型，第2節(jié)在第1節(jié)的基礎上推導了離散系統(tǒng)的最優(yōu)控制模型，得到了狀態(tài)方程的有限步控制序列式(18)以及速度和角速度控制量解析表達式式(21)。為了驗證該控制方法的有效性和可行性，本節(jié)在前面模型的基礎上對該模型進行數字仿真和實驗分析。

由于在式(11)的推導過程中，作了近似代替，而此處近似代替的前提條件是角度偏差較小，故本文討論的角度偏差的范圍為|eθ(k)|≤8°。這里將對不同的角度和距離偏差以及速度進行數字仿真，其參數選擇如表1所示，為了滿足系統(tǒng)的運動約束，給定了系統(tǒng)的最大容許速度和加速度。

表1　低速初始位姿狀態(tài)Tab.1　Initial pose state of low-velocity

表1是低速下各個參數的取值，表2是高速下各個參數的取值。

圖5是vτ=0.3 m/s時，初始偏差eθ(0)=5°,ed(0)=0.01 m下的Matlab仿真結果。

表2　高速初始位姿狀態(tài)Tab.2　Initial pose state of high-velocity

圖5　低速糾偏仿真結果Fig.5　Simulation process of rectifying deviation with low-velocity

圖6是vτ=0.5 m/s時，初始偏差eθ(0)=5°,ed(0)=0.01 m下的仿真結果。由圖5和圖6可知，在滿足約束條件的前提下，控制步數越大，距離偏差在開始控制的前幾步內越容易產生超調，因此不宜選擇過大的控制步數。由圖5a、6a或圖5b、6b可知，在控制步數等其他條件相同的條件下，過大的線速度會造成一定距離偏差的超調，但經過幾步后，距離偏差很快收斂。由圖知，無論何種情況下，距離偏差和角度偏差均同步的收斂于目標值，且通過有限的步數，均能將偏差降低至零，控制過程比較平穩(wěn)、快速、協(xié)調。

圖6　高速糾偏仿真結果Fig.6　Simulation process of rectifying deviation with high-velocity

為驗證本算法的有效性和可行性以及在全向運動型AGV路徑跟蹤中的實際效果，在仿真的基礎上，采用如圖7所示自主研發(fā)的視覺導引全向運動型AGV進行了路徑跟蹤實驗。運動控制器是基于ARM并移植了uC/OSII的實時操作控制系統(tǒng)，系統(tǒng)的控制框圖如圖8所示。

圖7　全向運動型AGV路徑跟蹤實驗Fig.7　Experiment of omni-directional motion AGV path tracking

圖8　系統(tǒng)控制框圖Fig.8　System control block diagram

實驗時將位姿狀態(tài)以周期Ts=0.1 s通過串口不斷地反饋給計算機，根據計算機實時采集到的距離偏差和角度偏差可得到全方位運動AGV的實際路徑跟蹤過程，如圖9所示，圖中從第10步到第20步為本算法的糾偏過程，同步快速地消除了角度偏差和距離偏差，第20步到第30步為AGV穩(wěn)定運行階段，該階段角度偏差和距離偏差有一定的小波動，但距離偏差控制在2 mm以內，角度偏差在2°以內。實驗表明了該優(yōu)化算法在全方位運動型AGV路徑跟蹤的有效性和可行性。

圖9　路徑跟蹤實驗結果Fig.9　Experimental results of path tracking

對比前面的仿真結果可知，實際實驗的位姿狀態(tài)的糾偏趨勢和仿真結果是相同的，但實際的過程中存在一些小的波動，造成這些波動的可能原因是系統(tǒng)存在一定的滯后，這些滯后包括DSP圖像的采集以及處理的滯后，同時伺服電機驅動器的響應也具有一定的滯后性，另外，運動控制器的任務調度和管理是通過每個任務調用延時函數自動實現的，此時延時函數的延時時間不合理也會造成整個系統(tǒng)的滯后。

表3列舉了不同控制方法的路徑跟蹤精度和達到穩(wěn)定的時間，全方位運動AGV的路徑跟蹤誤差為：ed≤2 mm，eθ≤2°，達到穩(wěn)定的時間為0.8～1.2 s,顯然，本文提出的路徑跟蹤算法具有較高的路徑跟蹤精度，且由仿真和實驗結果可知，兩種偏差的消除具有同步性和平穩(wěn)性。

表3　不同控制方法的精度與效率Tab.3　Accuracy and efficiency of different control methods

4　結束語

基于以上的應用背景，本文首先對四輪獨立驅動的Mecanum輪建立運動學模型，推導出四輪獨立驅動的Mecanum輪AGV的正、逆學運動方程。基于AGV運動軌跡的分析，建立了AGV運動學模型，在最優(yōu)控制理論的基礎上，對目標函數進行簡化，為了避免傳統(tǒng)加權矩陣Q、R的選擇難題，本文將系統(tǒng)離散化，通過有限的控制步數，在糾偏目標和運動約束的條件下，尋求最優(yōu)的速度和角速度控制序列，將角度偏差和距離偏差同步消除，經過理論的推導，得到最終的角速度和速度的解析表達式，該算法計算量較小，且有限的控制序列有利于實時控制系統(tǒng)的實時滾動控制。通過以上仿真和實驗可以看出，不同的速度和偏差下，該控制算法均有很好的控制效果，且速度偏差和角度偏差均能同步消除，當系統(tǒng)穩(wěn)定時角度偏差|eθ|≤2°，距離偏差|ed|≤2 mm，系統(tǒng)達到穩(wěn)定時的控制時間為0.8～1.2 s，因此本文提出的算法具有較高的路徑跟蹤精確和效率，同時具有較好的協(xié)調性和平穩(wěn)性。

1WU Xing,YU Jun,LOU Peihuang,et al.Agent-oriented embedded control system design and development of a vision-based automated guided vehicle[J]. International Journal of Advanced Robotic Systems, 2012, 9(2): 1-13.

2WU Xing,LOU Peihuang,YU Jun,et al. Intersection recognition andguide-path selection for avision-based AGV in a bidirectional flow network[J]. International Journal of Advanced Robotic Systems,2014,11(3):1-17.

3武星,樓佩煌,唐敦兵.自動導引車路徑跟蹤和伺服控制的混合運動控制[J].機械工程學報, 2011, 47(3): 43-48.

WU Xing,LOU Peihuang,TANG Dunbing. Integrated motion control of path tracking and servo control for an automated guided vehicle [J]. Journal of Mechanical Engineering,2011,47(3):43-48.(in Chinese)

4WU Xing,LOU Peihuang,SHEN Ke,et al.Precise transshipment control of an automated magnetic-guided vehicle using optics positioning[J]. International Journal on Smart Sensing and Intelligent Systems, 2014, 7(1): 48-71.

5喻俊，樓佩煌，錢曉明，等.自動導引車視覺導引路徑的識別及精確測量[J].華南理工大學學報：自然科學版，2012,40(3):143-149.

YU Jun,LOU Peihuang,QIAN Xiaoming,et al.Recognition and accurate measurement of vision-guided path of automatic guided vehicle[J].Journal of South China University of Technology:Natural Science Edition,2012,40(3):143-149.(in Chinese)

6王皖君，張為公，龔宗洋.自動導引車路徑跟蹤控制[J].測控技術，2009,28(9):49-51.

WANG Wanjun, ZHANG Weigong,GONG Zongyang.Path tracking control of automatic guided vehicle[J].Measurement & Control Technology,2009,28(9):49-51.(in Chinese)

7孟文俊,劉忠強.視覺導引AGV的路徑跟蹤控制研究[J].控制工程，2014,21(3):321-325.

MENG Wenjun,LIU Zhongqiang.Research on the path tracking control for vision-guided AGV[J].Control Engineering of China,2014,21(3):321-325.(in Chinese)

8陳無畏，孫海濤，李碧春,等.基于標識線導航的自動導引車跟蹤控制[J].機械工程學報，2006,42(8):164-170.

CHEN Wuwei, SUN Haitao, LI Bichun, et al.Tracking control of automatic guided vehicle based on lane marker navigation[J].Chinese Journal of Mechnical Engineering, 2006,42(8):164-170.(in Chinese)

9郭旭，熊蓉，胡協(xié)和.全方位移動機器人的運動預測控制[J].電機與控制，2007,11(1)：79-87.

GUO Xu,XIONG Rong,HU Xiehe.Motion predictive control of omni-directional mobile robot[J].Electric Machines and Control,2007,11(1):79-87.(in Chinese)

10王榮本，李兵，徐友春，等.基于視覺導航的智能車輛自主導航最優(yōu)控制器設計[J].汽車工程，2001,21(2)：97-100.

WANG Rongben, LI Bing,XU Youchun,et al.Optimal controller design for an intelligent vehicle based on vision navigation[J].Automotive Engineering,2001,21(2):97-100.(in Chinese)

11陳無畏，李碧春，孫海濤,等.基于視覺導航的AGV模糊-最優(yōu)控制研究[J].中國機械工程，2006,17(24)：2546-2550.

CHEN Wuwei,LI Bichun,SUN Haitao,et al. Study on fuzzy-optimal control of vision navigation for an AGV[J].China Mechanical Engineering,2006,17(24):2546-2550.(in Chinese)

12周俊，姬長英.基于視覺導航的輪式移動機器人橫向最優(yōu)控制[J].機器人，2002,24(3)：209-212.

ZHOU Jun,JI Changying.Lateral optimal control for wheeled mobile robot navigated by machine vision[J].Robot,2002,24(3):209-212.(in Chinese)

13劉洲，吳洪濤.Mecanum四輪全方位移動機構運動分析與仿真[J].中國制造業(yè)信息化，2011,40(5):43-46.

14熊蓉, 張翮, 褚健, 等. 四輪全方位移動機器人的建模和最優(yōu)控制[J]. 控制理論與應用, 2006, 23(1): 93-98.

XIONG Rong,ZHANG He,CHU Jian,et al.Modeling and optimal control of omni-ditectional mobile robots[J].Control Theory & Applications,2006,23(1):93-98.(in Chinese)

15LOZOVA C, MARTI P, VELASCO M, et al. Simulation study of a remote wireless path tracking control with delay estimation for an autonomous guided vehicle[J]. The International Journal of Advanced Manufacturing Technology, 2011, 52(5-8): 751-761.

16SOYSAL B. Real-time control of an automated guided vehicle using a continuous mode of sliding mode control[J]. Turkish Journal of Electrical Engineering and Computer Science, 2014, 22(5): 1298-1306.

17胡壽松，王執(zhí)銓，胡維禮.最優(yōu)控制理論與系統(tǒng)[M].3版.北京：科學出版社，2017.

18張鋒.線性二次型最優(yōu)控制問題的研究[D].天津:天津大學,2009.

19孟慶寬, 張漫, 楊耿煌，等. 自然光照下基于粒子群算法的農業(yè)機械導航路徑識別[J/OL]. 農業(yè)機械學報, 2016, 47(6): 11-20. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20160602&flag=1.DOI: 10.6041 /j.issn.1000-1298.2016.06.002.

MEMG Qingkuan，ZHANG Man，YANG Genghuang, et al. Guidance line recognition of agricultural machinery based on particle swarm optimization under natural illumination[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(6):11-20. (in Chinese)

20武星, 沈偉良, 樓佩煌，等. 視覺導引AGV魯棒特征識別與精確路徑跟蹤研究[J/OL]. 農業(yè)機械學報, 2016, 47(7): 48-56. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20160708&flag=1.DOI:10.6041/j.issn.1000-1298.2016.07.008.

WU Xing, SHEN Weiliang, LOU Peihuang, et al. Robust feature recognition and precise path tracking for vision-guided AGV[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(7): 48-56. (in Chinese)

21楊旭, 樓佩煌, 武星，等. 基于光照約束的AGV視覺導引非均勻光照增強方法[J]. 機械科學與技術, 2013, 32(12): 1752-1757.

YANG Xu, LOU Peihuang, WU Xing, et al. A method for enhancing non-uniform illumination of vision-guided automatic guided vehicle with illumination constraints[J]. Mechanical Science and Technology for Aerospace Engineering, 2013,32(12):1752-1757. (in Chinese)