基于IGPS和麥克納姆輪的AGV導航控制系統(tǒng)設(shè)計

王 顏，劉凈瑜，李 光，張加波，劉 星，周欣欣

（北京衛(wèi)星制造廠有限公司，北京100094）

最近四五十年來，工業(yè)自動化技術(shù)發(fā)展迅速，物流業(yè)和制造業(yè)發(fā)生了巨大變化。隨著自動導引車（automated guided vehicle，AGV）技術(shù)的不斷突破，生產(chǎn)成本得以降低，整體工作效率得到提高［1］。隨著導航技術(shù)、傳感器技術(shù)、網(wǎng)絡(luò)通信技術(shù)、計算機仿真技術(shù)的發(fā)展，高端制造業(yè)必將轉(zhuǎn)型升級，以實現(xiàn)人機交互、協(xié)調(diào)合作為目標的智能工廠及智能制造成為今后制造業(yè)發(fā)展的必然趨勢。AGV作為一種全向智能移動平臺，為智能工廠的精準配送、智能物流提供了有效的解決方案。AGV有多種導引方式，主要為磁導航、慣性導航、激光導航、視覺導航、基于傳感器數(shù)據(jù)的導航、全球定位系統(tǒng)（global positioning system，GPS）導航、室內(nèi)GPS（indoor GPS，IGPS）導航、光電編碼器導航和無線射頻識別導航等［2］。

目前，諸多研究人員對AGV導引技術(shù)進行了大量研究。張曉霞等［3］提出了一種GPS/DR組合導航定位算法，實現(xiàn)了對室外AGV的導航，但其定位精度不高且無法滿足室內(nèi)導航的精度要求。朱從民等［4-6］基于多傳感器及多種導航技術(shù)建立了AGV導航控制系統(tǒng)，但是其定位精度也無法滿足輔助裝配的定位精度要求；夏凌楠等［7］提出了一種基于慣性傳感器和視覺里程計的定位算法，實現(xiàn)視覺輔助定位，彌補了慣性導航的不足，但仍然無法實現(xiàn)后期AGV位置的修正。本文提出了一種基于IGPS和麥克納姆輪的AGV精確定位和導航方法：首先，通過IGPS接收器與發(fā)射站的組網(wǎng)幾何模型得出接收器的位置坐標；其次，通過多個接收器與快速矩形擬合算法得到AGV中心實時位姿信息（坐標值和姿態(tài)角），同時根據(jù)路徑信息采用模糊PI（proportion integration，比例積分）算法實現(xiàn)路徑的自動糾偏。

1 基于IGPS的AGV導航控制方法

1.1 AGV模型

本文研究的AGV的模型如圖1所示。該AGV由可全方位移動的4個麥克納姆輪驅(qū)動，其運動方式靈活多變，可應(yīng)用于空間有限和高精度定位的場合；4個IGPS接收器安裝于車體四角，其對角中心與車體中心重合。

該AGV導航控制系統(tǒng)主要由IGPS空間定位系統(tǒng)、導航控制器及驅(qū)動控制器三部分構(gòu)成。IGPS空間定位系統(tǒng)中，發(fā)射站產(chǎn)生的2個扇形激光平面對整個測量空間進行掃描，安裝在車體四周的IGPS接收器接收激光信號來實現(xiàn)車體的全局精確定位，獲得其在指定世界坐標系下的空間位置坐標；導航控制器在獲得車體空間位置坐標值后進行坐標轉(zhuǎn)換，得出車體中心在廠房二維平面坐標系下的坐標值，并進行連續(xù)實時路徑糾偏；驅(qū)動控制器則根據(jù)角速度、偏航角和速度值實時控制AGV在二維平面內(nèi)任意方向的移動。

圖1 AGV模型Fig.1 AGV model

1.2 基于IGPS的待測點位置坐標的確定

IGPS發(fā)射站的幾何模型及其坐標系如圖2所示。當發(fā)射站繞Z軸以ω速度轉(zhuǎn)動時，每轉(zhuǎn)1周便會產(chǎn)生一個基準時間值，作為后續(xù)每周測量的時間原點，其所對應(yīng)的角度為360°。記發(fā)射站旋轉(zhuǎn)第k周時的時間原點為tzk，激光平面1（LP1）、激光平面2（LP2）通過待測點p時，接收器各產(chǎn)生一個脈沖信號，此信號對應(yīng)的時間信號tp1k、tp2k與時間原點tzk的差值記為t1k、t2k，稱為特征時間，如圖3所示。由t1k、t2k以及ω便可確定激光平面通過p點時在發(fā)射站坐標系O-XYZ下的所在平面。由2個平面就可確定待測點在發(fā)射站坐標系O-XYZ下所在的直線，由多個發(fā)射站就可以確定待測點的位置坐標。激光平面與被測點的位置關(guān)系如圖4所示。

如圖4（a）所示，通過單發(fā)射站測量時，n1=[n11n12n13]，n2=[n21n22n23]，分別為激光平面LP1、LP2的單位法向矢量，其中n11、n12、n13，n21、n22、n23分別為LP1和LP2在O-XYZ坐標系下的平面方程系數(shù)；Δd為LP2在Z軸上的截距，LP1在Z軸上的截距始終是0。以上參數(shù)均可提前標定?？傻脝伟l(fā)射站下待測點pi的坐標值，表示為［8-9］：

圖2 IGPS發(fā)射站的幾何模型及其坐標系示意Fig.2 Geometric model of IGPS transmitting station and diagram of its coordinate system

圖3 IGPS接收器特征時間示意Fig.3 Diagram of IGPS receiver characteristic time

圖4 待測點位置坐標的確定Fig.4 Determination of coordinates of point to be measured

式中：θ1為LP1的水平角，θ2為LP2的水平角；(x y z)為待測點pi的坐標。

1.3 車體中心坐標提取算法

用IGPS引導AGV時，將發(fā)射站空間坐標系OXYZ轉(zhuǎn)換成平面坐標系O-XIYI。AGV運行時，選取廠房二維平面坐標系作為車體坐標系O-XwYw，并設(shè)AGV在全局坐標系下的位姿Q=(x' y' θ')。發(fā)射站坐標系O-XIYI與車體坐標系O-XwYw的關(guān)系如圖5所示。通過旋轉(zhuǎn)角度α，可以將接收器J在發(fā)射站坐標系下的坐標(xJyJθJ)轉(zhuǎn)換為車體坐標系下的坐標(xgygθg)。

圖5 發(fā)射站坐標系與車體坐標系的關(guān)系Fig.5 Relationship between transmitting station coordinate system and car body coordinate system

因此接收器J的坐標轉(zhuǎn)換為：

根據(jù)接收器在AGV的安裝位置可以得出，車體中心坐標(x' y' θ')為：

式中：xj為第j個接收器傳輸?shù)挠行У膶嶋HX坐標值，yj為第j個接收器傳輸?shù)挠行У膶嶋HY坐標值；n為有效對接收器數(shù)。

1.4 AGV運動學模型

全向移動AGV在平面上有3個自由度，靈活度及運動精度較高［10］。其運動學模型如圖6所示。

AGV的運動學方程為［10］：

式中：vx、vy分別為AGV在X和Y方向的速度，mm/s；ωc為AGV的旋轉(zhuǎn)角速度，rad/s；ω1、ω2、ω3、ω4分別為AGV4個輪子的角速度，rad/s；R為AGV輪子半徑，mm；L1為AGV前后輪子距離的一半，L2為AGV左右輪子距離的一半，mm。

圖6 全向移動AGV運動學模型Fig.6 Kinematics model of omnidirectional moving AGV

vc為vx與vy的合速度，可分解為偏航角φ（速度vc與車體中軸線的夾角）和速度值v。通過調(diào)控(φ，ωc，v)可以實現(xiàn)AGV在運動平面內(nèi)的任意運動。其中φ與后續(xù)路徑跟蹤的距離偏差和沿目標路徑的運行角度有關(guān)，ωc與目標路徑的角度偏差有關(guān)。

AGV車體當前位姿與目標路徑的偏差如圖7所示，其中目標路徑起點坐標為(xsys)，目標點坐標為(xfyf)。AGV車體當前位姿與目標路徑的偏差包括AGV車體中心當前坐標(x' y')與目標路徑的偏移距離Δd'、AGV車體中心當前坐標(x' y')與目標坐標點(xfyf)的偏移距離Δl及AGV車體中心當前位姿角θ'與目標路徑傾斜角θ的偏差角度Δθ'。

圖7 AGV車體當前位姿與目標路徑的偏差Fig.7 Deviation between current pose ofAGV and target path

根據(jù)接收器輸出的位置信息，可以算得車體中心位姿。根據(jù)車體中心位姿和目標路徑的偏差，來求取AGV運動的控制量：

式中：A=yf-ys，B=xf-xs，C=xfys-xsyf。

2 AGV模糊PI控制器的原理和設(shè)計

2.1 模糊PI控制器原理

IGPS接收器、導航控制器、驅(qū)動控制器之間采用串口通訊。IGPS接收器將獲得的位置信息發(fā)送給導航控制器，導航控制器利用模糊PI算法計算出(φ，ωc，v)這3個控制量并發(fā)送給驅(qū)動控制器，實現(xiàn)對AGV運動的控制。

模糊PI控制是在傳統(tǒng)PI控制的基礎(chǔ)上引入模糊控制器，通過模糊控制器實現(xiàn)對PI控制器實際控制系數(shù) kp、ki的調(diào)整［8，11-12，18］。模糊 PI控制原理如圖 8所示。

圖8 模糊PI控制的原理Fig.8 Principle of fuzzy PI control

2.2 AGV模糊控制器的設(shè)計

AGV模糊控制器是采用雙輸入雙輸出的二維模糊控制器［13-14］，以車體偏航角φ控制Δd'的大小，旋轉(zhuǎn)角速度ωc控制Δθ'的大小。因此采用位置控制器和旋轉(zhuǎn)控制器對小車的平移和旋轉(zhuǎn)進行獨立控制。位置控制器輸入量是AGV車體中心位姿與目標路徑的距離偏差值Δd'和距離偏差率dt，輸出量為比例系數(shù)Δkdp、Δkdi。旋轉(zhuǎn)控制器輸入量是AGV車體中心位姿與目標路徑的角度偏差值Δθ'和角度偏差率θt，輸出量為比例系數(shù)Δkθp，Δkθi。

首先將控制器輸入量進行模糊化處理。從理論上講，距離偏差值Δd'的取值范圍為[- ∞，∞ ]，但在實際情況中，由于受到客觀條件的限制，當Δd'值過大時，現(xiàn)場環(huán)境無法滿足AGV的運行需求。綜合考慮AGV運行的靈敏性和延遲性，位置偏差Δd'的取值范圍為［-300，+300］mm，角度偏差值Δθ'的取值范圍為［-45°，45°］。將模糊論域分為7個等級，分別為NB（負大）、NM（負中）、NS（負?。O（零）、PS（正?。M（正中）和PB（正大）［15-18］。

AGV模糊控制器的位置論域定義表如表1所示。

表1 AGV模糊控制器的位置論域定義表Table 1 Location domain definition table of AGV fuzzy controller

AGV模糊控制器的角度論域定義表如表2所示。

表2 AGV模糊控制器的角度論域定義表Table 2 Angle domain definition table of AGV fuzzy controller

獲得論域后，需要選擇合適的隸屬度函數(shù)。本文采用三角型隸屬度函數(shù)。與高斯型和正太分布型隸屬度函數(shù)相比，三角型隸屬度函數(shù)結(jié)構(gòu)簡單，不占用過多內(nèi)存。根據(jù)AGV和路況信息，得到△kp和△ki的模糊推理規(guī)則表，如表3和表4所示。

為了獲得較為準確的輸出量，解模糊的清晰化方法采用加權(quán)平均法（即重心法）。該方法在工業(yè)控制中的應(yīng)用效果較好。

式中：xm為隸屬度值；μ(m)為輸出模糊變量；s為加權(quán)平均判決結(jié)果。

表3 Δkp的模糊推理規(guī)則表Table 3 The fuzzy inference rule table ofΔkp

表4 Δki的模糊推理規(guī)則表Table 4 The fuzzy inference rule table ofΔki

由加權(quán)平均法得到的判決結(jié)果，還須乘以一個輸出比例因子，才能滿足控制要求，從而求得最終的輸出量。

2.3 PI控制設(shè)計

PI控制是將設(shè)定值與實際輸出值進行比較構(gòu)成控制偏差，并將偏差的比例和積分通過線性組合構(gòu)成控制量，對被控對象進行控制。

PI控制器采用經(jīng)典PI增量式控制，其表達式如下：

式中：kp'、ki'為PI控制器初始參數(shù)；Δkp、Δki為模糊控制器的輸出參數(shù)。

3 AGV導航控制仿真和試驗

3.1 仿真分析

通過MATLAB的Simulink組件建立AGV導航模糊PI控制仿真模型，如圖9所示。

圖9 AGV導航模糊PI控制仿真模型Fig.9 Simulation model of fuzzy PI control of AGV navigation

在MATLAB的Fuzzy Logic Controller中建立AGV導航模糊控制器，如圖10所示。

圖10 AGV導航模糊控制器Fig.10 Fuzzy controller of AGV navigation

對AGV導航模糊PI控制和傳統(tǒng)PI控制進行仿真。為了檢測在最大偏差下對AGV的控制情況，設(shè)置初始值輸入分別為最大角度偏差45°和-45°以及最大距離偏差300和-300 mm，其仿真結(jié)果對比如圖11所示。

圖11 AGV導航模糊PI控制和傳統(tǒng)PI控制的仿真結(jié)果對比Fig.11 Comparison of simulation results of fuzzy PI control and traditional PI control of AGV navigation

從圖11可知，模糊PI控制和傳統(tǒng)PI控制均可以使AGV在較大的角度偏差和距離偏差下調(diào)整到平衡狀態(tài)，但模糊PI控制的速度和幅度均優(yōu)于傳統(tǒng)PI控制?？梢姡捎帽疚脑O(shè)計的模糊PI控制器可以顯著提高AGV的糾偏能力和反應(yīng)速度。

3.2 試驗驗證

筆者自主研發(fā)的基于IGPS的AGV試驗平臺如圖12所示。

圖12 基于IGPS的AGV試驗平臺Fig.12 AGV test platform based on IGPS

試驗環(huán)境為工廠的地坪漆地面，采用激光跟蹤儀進行位置檢測。定位點的位置坐標為（-2307.877，1179.287，-677.001）mm，AGV連續(xù)在該點停靠200次。實際測得的AGV的定位位置與定位點位置的偏差如圖13所示，可以看出定位距離偏差在-0.2～0.2 mm以內(nèi)。

4 結(jié)論

本文提出了一種由IGPS導引的全向AGV定位方法，通過模糊PI閉環(huán)控制實現(xiàn)AGV定位控制。首先，實時獲取IGPS接收器當前的位置坐標，并計算車體中心位姿和AGV車體中心與目標路徑的偏差量，然后將偏差量作為輸入量，利用模糊PI控制生成控制量，實現(xiàn)AGV的定位和導航。試驗結(jié)果表明，基于IGPS和麥克納姆輪的AGV定位方法，定位精度可達到±0.2mm，實現(xiàn)AGV的高精度定位。

圖13 AGV定位偏差Fig.13 AGV positioning deviation