基于麥克納姆輪的AGV 定位及糾偏技術的研究

王殿衛(wèi)，郭津津，李維驍

（1.天津理工大學機械工程學院天津市先進機電系統(tǒng)設計與智能控制重點實驗室，天津300384；2.南京郵電大學通信與信息工程學院，南京210023）

自動導航運輸車（AutomatedGuidedVehicle，AGV）是現(xiàn)代生產(chǎn)中的重要裝備，定位精度以及導航精度是制約其發(fā)展的重要因素.現(xiàn)使用的導航方式主要有磁導引、激光導航、慣性導航以及二維碼導航，但是磁導航、電磁導航柔性差[1，2]，激光導航容易受到環(huán)境光線的影響[3]，慣性導航會產(chǎn)生誤差累積[4].于是快速響應（Quick Response，QR）二維碼導航產(chǎn)生，其由慣性導航加上在特定位置糾偏組成[5]，其吸收了慣性導航實時性好的優(yōu)點.又在站點處對誤差進行糾正，所以其效果較好，而站點處的糾偏算法為其難點.

Gupta N、陳寧等使用純追蹤（Pure Pursuit）算法，或其改進算法[6，7]，能很好的跟蹤航向角與轉(zhuǎn)向角，但其誤差較大，適用于農(nóng)機等對誤差要求不高的領域.郭景華等人采用遺傳算法控制[8]，但需要大量訓練且實時性不高.楊前明等人使用PID 算法來糾偏[9]，獲得良好的效果，但是PID 算法快速性與穩(wěn)定性取決于三個參數(shù)的確定，對干擾等不敏感.H.Q.T.NGO[10]等人采用模糊算法控制小車尋跡，但實時性不高.而將模糊控制和PID 算法結合的模糊自適應PID 算法，可以達到更好的效果.其使用模糊算法計算出實時變化的PID 參數(shù)，再使用PID 算法計算出小車四個電機的轉(zhuǎn)速，從而對小車行進方向進行控制.

而轉(zhuǎn)向輪主要有單舵輪式[11]、差動式[12]、以及萬向輪式.萬向輪主要有連續(xù)切換輪[13]以及麥克納姆輪（Mecanum）輪[14].

本AGV 系統(tǒng)采用麥克納姆輪（以下簡稱麥輪）作為驅(qū)動輪，并采用二維碼導航，模糊自適應PID 算法于站點處糾偏.

1 麥輪運動分析

1.1 麥輪簡介

麥輪是一種全向輪，如圖1.其圓周分布一些可以自由轉(zhuǎn)動的梭狀的輥子，輥子與輪截面成45°. 由于四輪轉(zhuǎn)速各不相同，所以地面對四個輪的反作用力方向以及大小都不相同，因此四個力的合力可根據(jù)四輪轉(zhuǎn)速變化而變化，再加上輥子的輔助，使得AGV 可以在平面內(nèi)做前后、左右以及以繞中心旋轉(zhuǎn)三個自由度的運動[15].

圖1 麥輪實物圖Fig.1 Mecanum wheel

1.2 麥輪運動模型

如圖2 所示，中點O 為相對輪兩兩連線的交點，輪子中間兩條斜線代表輪子上的輥子方向. ω→為AGV 角速度矢量分別為小車x軸、y軸運動向量為0 號輪相對于世界坐標系的速度矢量，不代表實際速度.

圖2 AGV 車體速度模型Fig.2 Velocity model of body of AGV

由圖可知：

圖3 單個輥子速度Fig.3 Velocity model of single roller

由圖可知：

車輪轉(zhuǎn)速與輥子的速度有如下關系

其中，ωi為i輪轉(zhuǎn)速；R為麥輪半徑.

綜上所述麥輪逆運動學公式為

2 QR 碼定位

QR 碼有兩個作用.首先，QR 碼用于確定直線誤差與角度誤差.當掃描到QR 碼時，AGV 會通過三個角點找到QR 碼中心點以及QR 碼的方向，并計算出橫向偏差以及角度偏差.其次，QR 用于確定其世界坐標，AGV 會將QR 碼解析成代表坐標的數(shù)字，并根據(jù)其坐標選擇行進方向.解析二維碼使用開源函數(shù)庫Zbar[16]，但是Zbar 需要接收優(yōu)質(zhì)的二進制圖像，所以需要對圖像進行預處理.為提高實時性，圖像處理過程采用了Opencv 和C++進行開發(fā).

2.1 圖像預處理

未經(jīng)處理的圖像為RGB 圖像，處理速度慢，因此首先需要將圖像轉(zhuǎn)化為灰度圖像.由于電子干擾等原因，圖像會出現(xiàn)噪聲，一般為椒鹽噪聲，中值濾波可以很好地將噪音濾除[17].

QR 二維碼為黑白兩種顏色組成，因此可以將圖像轉(zhuǎn)化為二值圖像，如此可以在不損害信息的情況下增加運算速度.圖像二值化通過閾值處理實現(xiàn)，即灰度值大于閾值的置為1，反之則置為0. 閾值的確定有全局閾值算法以及局部二值化算法.全局閾值算法最經(jīng)典的方法為Otsu（大津算法）. 其通過求取圖像灰度直方圖雙峰之間的低谷值來確定閾值[18].因此，適用于前景與背景對明顯的圖像.但由于QR 碼被粘貼在地板上，光源為自然光，加之AGV 車體對光線的遮擋，會出現(xiàn)光照不均的現(xiàn)象，因此Otsu 算法并不適用.如圖4 所示.

圖4 Ostu 算法Fig.4 Ostu algorithm

對于光照不均的圖像，局部二值化算法較為適用，常用的局部二值化算法有Niblack 算法以及Wellner算法.但Niblack 算法計算量較大，不適合實時性要求較高的嵌入式系統(tǒng).圖5 為經(jīng)過Niblack 算法計算結果.

圖5 Niblack 算法Fig.5 Niblack algorithm

Wellner 算法原理[20]為，以“S”型曲線（如圖6）遍歷整個圖像，并將其灰度值存入一維數(shù)組中.

圖6 Wellner 算法遍歷順序Fig.6 Traversal sequence of Wellner algorithm

再取需求閾值的像素點n之前的s個數(shù)（包含n點），求其平均值.經(jīng)驗表明一般取s=w/8.w為一行的像素數(shù).為體現(xiàn)和第n點間距離對結果的影響，則求取其加權平均值，如下所示：

為關聯(lián)豎直方向的像素值，用下式得數(shù)hs（n）代替gs（n）

最終二值化公式為：

其中t為修正系數(shù)，經(jīng)驗得t=0.85 效果最佳，其結果如圖7，其中形態(tài)學操作使用5×5 的矩形模板.

圖7 Wellner 算法Fig.7 Wellner algorithm

由圖7 可知Wellner 算法比Niblack 算法效果要差，圖7（d）中間偏左下位置有瑕疵，但可正常解碼，在嵌入式環(huán)境下Niblack 算法運行時間為138 ms，解碼成功率為98.4%，Wellner 算法運行時間只有16 ms，成功率為95.6%.

2.2 QR 碼中心以及方向定位

本小節(jié)中的坐標均基于像素坐標系.

QR 碼角點（又稱位置探測圖形）位于QR 碼的左上、左下、右上三角上，可根據(jù)其位置計算出QR二維碼的中心.

可尋找到三個角點后并提取其外輪廓，后求其一階空間矩[21]，即求出角點的中心坐標，其公式見式（10），然后根據(jù)三個角點的幾何關系找到二維碼中心及方向，如圖8 所示，其中向量的方向為QR碼的方向.

其中，m為x軸坐標值；n為y軸坐標值；m00為圖像的零階空間矩；m10為x方向的一階空間矩；m01為y方向上的一階空間矩；為中點x坐標值；為中點y坐標值.

圖8 三個角點相對位置Fig.8 The location of three corner areas

3 糾偏算法研究

3.1 模糊PID 算法簡介

PID 算法是按偏差的比例（P）、積分（I）和微分（D）進行運算的算法[22].增量式PID 表達式如下，其原理簡單應用面廣，被廣泛應用. 其難點為對參數(shù)Kd、Ki、Kp的確定.

其中，Δu（k）為k時刻輸出的補償量，Kp、Ki、Kd分別為PID 的比例、積分、微分參數(shù)，e（k）為k時刻實際值與設定值的差值.

模糊算法是一種智能控制算法，其以模糊集合論、模糊語言變量和模糊邏輯推理為基礎[23]，適用于難以建立系統(tǒng)模型的情況.

模糊自整定PID 將模糊算法與PID 算法結合，使用模糊算法實時計算出Kp、Ki、Kd參數(shù)，可以較好繼承兩種算法的優(yōu)點.

3.2 控制策略描述

在站點處有橫向偏差e與角度偏差θ 兩種偏差，因此，需要設計兩個不同的模糊自整定PID 控制器，分別來控制兩個誤差以達到精準控制的目的.控制流程如圖9.

圖9 控制流程Fig.9 Control flow

3.3 模糊化處理

模糊控制器需輸入模糊信號，因此首先要對兩個偏差e、θ、偏差變化率ec、θc以及輸出量Kp、Ki、Kd進行模糊化處理.其中e的基本論域與攝像頭的攝像范圍有關，取最大偏差為40 mm，因此e的基本論域范圍為[-40，40]mm.偏差變化率ec與AGV 的橫向移動速度有關，取ec論域為[-10，10]mm/s.取θ 論域為[-π/4，π/4]，θc為[-π/16，π/16].

設定所有的輸入量在模糊集上的論域都均為[-3，3]，即論域{NB，NM，NS，ZO，PS，PM，PB}，其分別表示為負大、負中、負小、零、正小、正中、正大.因此各個量的量化因子[24]為

同理根據(jù)經(jīng)驗可知輸出量的論域為：

量化因子為：

隸屬度函數(shù)均采用三角形函數(shù)如圖10.

圖10 隸屬度函數(shù)Fig.10 Membership function

3.4 模糊推理

模糊推理即是使用模糊輸入量通過和模糊規(guī)則庫進行對比得出最優(yōu)的輸出值，根據(jù)PID 三個參數(shù)的特性[25]得出規(guī)則表如表1，θ、θc——ΔKp、ΔKi、ΔKd的規(guī)則表與表1 相同.

表1 e/ec——ΔKp、ΔKi、ΔKd 規(guī)則表Tab.1 Rule table of e/ec to ΔKp/ΔKi/ΔKd

3.5 解模糊

經(jīng)過模糊推理后，輸出的仍然是模糊值，無法在嵌入式平臺上運行，需要將其解模糊化.解模糊化算法使用加權平均值法[26]，其公式如下：

其中，u為解模糊化之后的精確量；K為其對應的量化因子；ui為第i個元素；μ（ui）為其對應的隸屬度函數(shù)值.經(jīng)過解模糊化后即可將其作用在被控對象上.

4 實驗驗證

為驗證算法可靠性，特設計實驗. 實驗分為兩組，第一組直線行駛實驗，AGV 分別以前進與平移的方式運動，速度設置為0.3 m/s，站點之間距離為3 m，共設有5 個站點，其實驗結果平均值如表2.

表2 前進/橫移實驗結果Tab.2 Result of experiment of forward and lateral motion

第二組實驗為AGV 受到擾動時設計，實驗方法為將AGV 與設定軌跡成一定角度放置，測算其恢復直行時間，其實驗結果平均值如表3.

由上兩組實驗可知，前進與橫移過程中誤差最大不超過20 mm，誤差符合實際工況下精度要求.而抗擾動實驗中，在與實際軌跡成55 度時，AGV 會脫離軌道，較小角度下，調(diào)整速率較快.大多數(shù)情況下，角度誤差不會超過30°，所以可以符合要求.

表3 抗擾動實驗Tab.3 Result of experiment of tamper-proof

5 結 論

本文設計了一款基于麥克納姆輪以及QR 碼定位的AGV，首先分析了麥克納姆輪的運動學模型，而后分析了其定位所需的圖像處理算法以及出現(xiàn)誤差時糾正算法，實驗結果顯示其運轉(zhuǎn)狀態(tài)良好，出現(xiàn)偏差糾正速度較快，但仍有以下不足：

1）未設計轉(zhuǎn)彎算法.

2）受到干擾調(diào)整時間還可以降低.

3）為滿足特定需求，角度調(diào)整仍需擴大.