(青島科技大學(xué) 信息與科學(xué)技術(shù)學(xué)院，山東 青島 266061)

0 引言

自動(dòng)引導(dǎo)車輛AGV(automated guided vehicles)是指裝備有電磁或光學(xué)等自動(dòng)引導(dǎo)裝置，能夠沿規(guī)定的導(dǎo)引路徑行駛，具有安全保護(hù)以及各種移栽功能的運(yùn)輸車[1]。

目前比較常見的AGV導(dǎo)航方式有激光導(dǎo)航、電磁導(dǎo)航、坐標(biāo)導(dǎo)航和光學(xué)導(dǎo)航等。從使用效果來(lái)看，激光導(dǎo)航和光學(xué)導(dǎo)航對(duì)環(huán)境要求過高；電磁導(dǎo)航需要對(duì)路面安裝金屬線，并且導(dǎo)航路面難以更改或者擴(kuò)展；坐標(biāo)導(dǎo)航則路面安裝定位塊較復(fù)雜，工作量大而且定位不準(zhǔn)確。本文以九軸陀螺儀進(jìn)行導(dǎo)航，對(duì)周圍環(huán)境要求較低，導(dǎo)航路徑靈活，而且導(dǎo)航精度較高，后期維護(hù)成本也較低，是一種較為理想的導(dǎo)航方式[2]。

本課題為九軸陀螺儀在AGV自主導(dǎo)航中的應(yīng)用設(shè)計(jì)，是一種全方位移動(dòng)平臺(tái)控制系統(tǒng)，通過對(duì)單臺(tái)或兩平臺(tái)同步移動(dòng)柔性連接、通過無(wú)線控制使2臺(tái)AGV移動(dòng)平臺(tái)協(xié)同運(yùn)動(dòng)，使其靈活應(yīng)用在空間有限、機(jī)動(dòng)性要求高的場(chǎng)合。在每臺(tái)AGV小車上面安裝MPU9250九軸陀螺儀(3軸加速度計(jì)、3軸陀螺儀、3軸磁力計(jì))，系統(tǒng)采集九軸陀螺儀數(shù)據(jù)，通過卡爾曼濾波進(jìn)行數(shù)據(jù)融合輸出融合后的航向角，然后AGV行駛過程中應(yīng)用該導(dǎo)航角通過PID控制算法進(jìn)行控制實(shí)際行駛方向，從而引導(dǎo)小車朝著目的地方向行駛[3]。

1 應(yīng)用場(chǎng)景

本設(shè)計(jì)為在動(dòng)車車體裝配車間使用，系統(tǒng)使用兩個(gè)AGV小車共同完成動(dòng)車車體在車間的移動(dòng)。AGV小車使用麥克納姆輪，正常工作時(shí)，從車接受主車發(fā)出的指令，兩者協(xié)調(diào)同步運(yùn)行。兩個(gè)AGV小車導(dǎo)航行走的現(xiàn)場(chǎng)平面如圖1所示(默認(rèn)為30m×15m)，AGV導(dǎo)航車的平時(shí)停車位置點(diǎn)與目標(biāo)位置點(diǎn)如圖1已標(biāo)出。

圖1中以UL為停車點(diǎn)，TB為目標(biāo)點(diǎn)為例進(jìn)行研究。首先需要確定AGV從UL到TB初始航向角，即X、Y軸的偏角或者地理坐標(biāo)的夾角。然后通過卡爾曼濾波對(duì)航向角進(jìn)行融合，行駛過程中再通過PID算法不斷控制運(yùn)動(dòng)角度，直到最終到達(dá)TB。其他停車位置到目的位置的研究過程類似。應(yīng)用場(chǎng)地的坐標(biāo)始終以UL為坐標(biāo)原點(diǎn)，向東為X軸，向北為Y軸，分別以UL、UM、UR、DL、DM、DR為出發(fā)原點(diǎn)，以TA、TB為目標(biāo)點(diǎn)，計(jì)算各種出發(fā)點(diǎn)、目標(biāo)點(diǎn)組合，出發(fā)時(shí)的初始航向角。本設(shè)計(jì)默認(rèn)X=30 m，X1=15 m，X2=7 m，Y1=7.5 m，Y=23 m。

圖1 AGV導(dǎo)航控制示意圖

AGV小車裝有麥克納姆輪，即每個(gè)輪子在沒有任何轉(zhuǎn)角的情況下，通過控制每一個(gè)輪子的速度和方向?qū)崿F(xiàn)3自由度的全方位移動(dòng)。AGV小車車頂安裝反射板，停車板上方安裝激光掃描儀，激光掃描儀通過對(duì)反射板的掃描判斷小車是否到達(dá)停車板附近。目標(biāo)位置點(diǎn)TA和TB處有停車板，停車板設(shè)計(jì)為長(zhǎng)方形，中軸線平行于TA和TB連線，正好容小車停下。AGV小車配有遙控器，當(dāng)小車到達(dá)停車板附近時(shí)小車會(huì)發(fā)出警報(bào)，此時(shí)需要小車的操作人員通過遙控器指揮小車停到停車板上面。

2 坐標(biāo)系與姿態(tài)角

本課題用到車體坐標(biāo)系、場(chǎng)地坐標(biāo)系和地理坐標(biāo)系。因車體坐標(biāo)系是隨時(shí)變動(dòng)的，故需要依靠不變的地理坐標(biāo)系確定航向角，因此需要將車體坐標(biāo)系轉(zhuǎn)換到地理坐標(biāo)系，再進(jìn)行相關(guān)角度計(jì)算[4]。

2.1 車體坐標(biāo)系

如圖2所示。取平面圖中小車的實(shí)際停放位置中心為坐標(biāo)原點(diǎn)O，OXb軸的正向指向AGV小車的前方，OYb軸的正向指向AGV小車的右側(cè)，OZb軸的正向指向天，建立車體坐標(biāo)系。

圖2 車體坐標(biāo)系圖

2.2 地理坐標(biāo)系

如圖3所示。取小車的實(shí)際停放位置為坐標(biāo)原點(diǎn)O，OXn軸的正向指地球的東方，OYn軸的正向指向地球的北方，OZn軸的正向指向天，建立地理坐標(biāo)系。

圖3 地理坐標(biāo)系圖

2.3 場(chǎng)地坐標(biāo)系

以場(chǎng)地的右方作為X軸，下方為Y軸進(jìn)行定義，用于計(jì)算場(chǎng)地的導(dǎo)航角。場(chǎng)地坐標(biāo)系與地理坐標(biāo)系之間的夾角為α。

2.4 坐標(biāo)變換矩陣

本課題是將車體坐標(biāo)系OXbYbZb變換到地理坐標(biāo)系OXnYnZn[5]，再對(duì)九軸數(shù)據(jù)進(jìn)行卡爾曼濾波處理，最后得到融合后的航向角。

圖4 車體坐標(biāo)系與地理坐標(biāo)系變換圖

如圖4所示，車體坐標(biāo)系分別繞Z軸旋轉(zhuǎn)Ψ度、繞Y軸旋轉(zhuǎn)θ度、繞X軸旋轉(zhuǎn)γ度，最后與地理坐標(biāo)系重合。公式1即車體坐標(biāo)系到地理坐標(biāo)系的坐標(biāo)變換矩陣，如下所示：

C=C1·C2·C3=

(1)

3 卡爾曼濾波與數(shù)據(jù)融合

本文首先用卡爾曼濾波對(duì)磁力計(jì)和加速度計(jì)的測(cè)量角度進(jìn)行濾波，然后對(duì)磁力計(jì)和加速度計(jì)的輸出角度進(jìn)行融合得到融合后的航向角[7-9]。

3.1 卡爾曼濾波模型建立

本文認(rèn)為磁力計(jì)、加速度計(jì)的角度變化都符合線性關(guān)系，故在卡爾曼濾波中建立狀態(tài)方程和量測(cè)方程。

(2)

(3)

將AGV狀態(tài)方程轉(zhuǎn)換為加速度計(jì)相關(guān)量的方程，如公式4所示。將AGV的測(cè)量方程轉(zhuǎn)換為磁力計(jì)相關(guān)量的方程，如公式5所示。

(4)

(5)

3.2 數(shù)據(jù)融合

卡爾曼濾波是用遞推的形式，對(duì)未來(lái)某一時(shí)刻狀態(tài)進(jìn)行估計(jì)。本文以加速度計(jì)的航向角作為測(cè)量值，加速度計(jì)的航向角作為預(yù)測(cè)值，然后根據(jù)前一時(shí)刻的預(yù)測(cè)值和測(cè)量值來(lái)估計(jì)當(dāng)前時(shí)刻系統(tǒng)的最優(yōu)值。

本文首先通過計(jì)算得到加速度計(jì)的航向角，然后通過卡爾曼濾波模型公式計(jì)算航向角的測(cè)量值和下一時(shí)刻的預(yù)測(cè)值，把磁力計(jì)測(cè)量的角度數(shù)據(jù)和加速度計(jì)計(jì)算的角度數(shù)據(jù)作為卡爾曼濾波的輸入，通過卡爾曼濾波5個(gè)公式計(jì)算得到融合后的航向角。

首先根據(jù)系統(tǒng)上一次得來(lái)的最優(yōu)值計(jì)算出當(dāng)前的估計(jì)值和協(xié)方差。

γk=Aγk-1+Bφk-1

(6)

(7)

再根據(jù)協(xié)方差大小計(jì)算出卡爾曼增益的大小。

(8)

最后根據(jù)當(dāng)前估計(jì)值和測(cè)量值計(jì)算出當(dāng)前最優(yōu)值和誤差協(xié)方差。

ω=γk+Kk(yk-Hγk)

(9)

(10)

4 PID控制算法

由于AGV使用采樣式控制，不能連續(xù)不斷的輸出信號(hào)，故不能連續(xù)控制，所以本課題使用數(shù)字PID控制算法，即在模擬PID算法基礎(chǔ)上，將微分和積分項(xiàng)進(jìn)行離散化處理。

4.1 PID控制算法

PID控制器為一階控制器，設(shè)定值與輸出值之間的差值組成系統(tǒng)偏差，將系統(tǒng)偏差與比例系數(shù)P、積分系數(shù)I和微分系數(shù)D通過一階線性關(guān)系構(gòu)成控制量，控制被控對(duì)象[10]。將輸入角度與輸出行駛角度做差得到e(k)作為PID的輸入，小車的輸出為實(shí)際運(yùn)行角度θ(k)。

圖5 PID控制原理圖

在實(shí)際應(yīng)用中，需要通過大量實(shí)驗(yàn)來(lái)確定PID三個(gè)參數(shù)[12]。其控制規(guī)律為：

(11)

其中:θ(k)是k時(shí)刻的航向角；Kp為比例系數(shù)；TI為積分常數(shù)；TD為微分常數(shù)；θ0為初始角度。

4.2 數(shù)字PID控制算法

數(shù)字PID算法有兩種離散化的方法，分別是位置式和增量式，由于增量式的計(jì)算較簡(jiǎn)單，故本課題選用增量式PID算法[11-12]。也就是把采樣周期設(shè)為T，采樣序號(hào)設(shè)為k，那么離散化的kT就可以近似替代連續(xù)采樣T。用和的形式替代積分形式，用增量式替代微分式，就可以獲得離散化的數(shù)字PID公式(12)和公式(13)。

(12)

Δθk=θk-θk-1=

Aek+Bek-1+Cek-2

(13)

其中：θk代表計(jì)算機(jī)第k次采樣時(shí)的輸出；ek代表第k次采樣時(shí)輸入數(shù)據(jù)的偏差值；KI代表積分系統(tǒng)，KI=KpT/TI；KD代表微分系數(shù)，KD=KPTD/T。

卡爾曼濾波輸出的融合后的航向角作為數(shù)字PID算法中的輸入值，每隔一個(gè)時(shí)間周期都要重新計(jì)算從小車當(dāng)前位置到目標(biāo)位置的航向角，作為小車下一次行駛的輸入航向角，此輸入航向角與融合以后的航向角進(jìn)行對(duì)比，從而進(jìn)行PID調(diào)節(jié)。

5 實(shí)際應(yīng)用

5.1 系統(tǒng)編程流程

AGV導(dǎo)航前期，首先使用九軸陀螺儀的磁力計(jì)測(cè)量出工作場(chǎng)地與地理坐標(biāo)系的夾角α，車體停放的位置姿態(tài)與工作場(chǎng)地平行；根據(jù)圖1所示的幾何尺寸，使用場(chǎng)地坐標(biāo)計(jì)算出發(fā)地點(diǎn)到目標(biāo)地點(diǎn)的夾角β，并與α結(jié)合形成初始的車體導(dǎo)航角；在出發(fā)地點(diǎn)，通過麥克納姆輪轉(zhuǎn)動(dòng)小車的車頭，使車頭朝向目標(biāo)點(diǎn)。

啟動(dòng)AGV車體向目標(biāo)點(diǎn)出發(fā)。在行駛過程中，通過九軸陀螺儀的磁力計(jì)讀取γ角數(shù)據(jù)，通過九軸陀螺儀的加速度計(jì)計(jì)算在采樣周期內(nèi)車體移動(dòng)的夾角φ，其計(jì)算如公式14所示。

(14)

其中：k-1時(shí)刻的坐標(biāo)是(X1,Y1),k時(shí)刻的坐標(biāo)值為(X2,Y2)。

將它們作為卡爾曼濾波的輸入，通過卡爾曼濾波進(jìn)行數(shù)據(jù)融合輸出融合后的航向角，AGV按照融合后的航向角控制行駛，同時(shí)通過PID算法調(diào)整AGV的行駛方向，使其更準(zhǔn)確的朝著目標(biāo)點(diǎn)行駛。

當(dāng)小車到達(dá)停車板附近時(shí)，AGV小車會(huì)發(fā)出警報(bào)，此時(shí)需要小車操作人員用遙控器手動(dòng)控制小車移動(dòng)到停車板上。此時(shí)小車需要按照初始出發(fā)時(shí)的角度調(diào)整落位。

全方位移動(dòng)平臺(tái)可以通過遙控器選擇單車或者兩車系統(tǒng)。在理想狀態(tài)下，當(dāng)兩移動(dòng)平臺(tái)協(xié)同運(yùn)動(dòng)時(shí)，從車受控地與主車保持相同的姿態(tài)。

5.2 場(chǎng)地航向角計(jì)算

實(shí)際應(yīng)用中，只考慮X軸和Y軸，不考慮Z軸。場(chǎng)地如圖1所示，當(dāng)停車點(diǎn)和目的地距離默認(rèn)時(shí)，如果AGV小車從停車點(diǎn)UL(0,0)到目標(biāo)點(diǎn)TB(23,7.5)移動(dòng)，那么需要先把車體坐標(biāo)與場(chǎng)地坐標(biāo)結(jié)合轉(zhuǎn)換到地理坐標(biāo)系中，場(chǎng)地坐標(biāo)的出發(fā)航向角為18.06°。小車出發(fā)后通過卡爾曼濾波把九軸陀螺儀磁力計(jì)測(cè)量的角度數(shù)據(jù)和根據(jù)加速度計(jì)計(jì)算的角度數(shù)據(jù)濾波融合后的輸出控制航向角，然后通過PID積分控制算法不斷地修正AGV小車運(yùn)動(dòng)過程中的運(yùn)行角度，直到到達(dá)TB為止。其他停車點(diǎn)到目標(biāo)點(diǎn)的移動(dòng)過程類似。

AGV小車的導(dǎo)航圖如圖1所示，以UL為原點(diǎn)建立坐標(biāo)系，使車體坐標(biāo)系與地理坐標(biāo)系原點(diǎn)重合。AGV小車從各出發(fā)點(diǎn)到目標(biāo)點(diǎn)的初始航向角的關(guān)系如表1所示。

5.3 系統(tǒng)應(yīng)用步驟

當(dāng)AGV小車行駛的場(chǎng)地如圖1所示，而且停車點(diǎn)與目標(biāo)點(diǎn)的距離是默認(rèn)距離時(shí)，將小車從停車點(diǎn)UL(0，0)通過自主導(dǎo)航的形式到達(dá)目標(biāo)點(diǎn)TB(23,7.5)，行駛過程如下：

1)AGV小車出發(fā)之前，將車體坐標(biāo)系轉(zhuǎn)換到地理坐標(biāo)系使兩者重合，計(jì)算出小車從出發(fā)點(diǎn)到目標(biāo)點(diǎn)的角度，本課題中的該角度為18.06°，計(jì)算場(chǎng)地坐標(biāo)系與地理坐標(biāo)系之間的夾角，本課題中假設(shè)場(chǎng)地坐標(biāo)系與地理坐標(biāo)系完全重合，即夾角為0°。將二者進(jìn)行相加，得到的角度為小車的初始航向角。

2)通過麥克納姆輪原地調(diào)轉(zhuǎn)AGV小車的車頭，轉(zhuǎn)動(dòng)角度為初始航向角，本課題中為18.06°。

3)小車出發(fā)后，通過四元數(shù)法進(jìn)行三軸陀螺儀和三軸磁力計(jì)角度的融合(此時(shí)沒有去除噪聲)。

4)將三軸磁力計(jì)和三軸陀螺儀融合后的角度與三軸加速度計(jì)的航向角作為卡爾曼濾波的輸入值，對(duì)角度進(jìn)行卡爾曼濾波處理，將系統(tǒng)中與信號(hào)無(wú)關(guān)的噪聲干擾去除，然后將兩個(gè)角度進(jìn)行融合，得到融合后的航向角。首先進(jìn)行時(shí)間的更新，確定先驗(yàn)估計(jì)值和誤差協(xié)方差預(yù)估計(jì)矩陣，然后再進(jìn)行測(cè)量更新，最后確定小車的卡爾曼濾波增益、最優(yōu)估計(jì)和誤差協(xié)方差矩陣，時(shí)間更新與測(cè)量更新交替進(jìn)行，直到小車到達(dá)目標(biāo)點(diǎn)為止。

5)小車行駛過程中，實(shí)時(shí)的計(jì)算小車的當(dāng)前位置與目標(biāo)位置的航向角，通過數(shù)字PID控制算法進(jìn)行控制，使小車更準(zhǔn)確的進(jìn)行自主導(dǎo)航。

6)當(dāng)小車行駛至目標(biāo)點(diǎn)附近時(shí)，目標(biāo)點(diǎn)附近的激光掃描儀會(huì)發(fā)出警報(bào)聲，此時(shí)需要操作人員使用遙控器控制小車準(zhǔn)確的停到目標(biāo)點(diǎn)的停車板上面。

5.4 系統(tǒng)仿真

用Matlab將AGV從停車點(diǎn)UL到目標(biāo)點(diǎn)TB點(diǎn)進(jìn)行仿真，其中停車點(diǎn)與目標(biāo)點(diǎn)的長(zhǎng)度使用默認(rèn)距離的十倍，真實(shí)軌跡代表從停車點(diǎn)到目標(biāo)點(diǎn)直線行駛，路徑使用黑色標(biāo)記；觀測(cè)軌跡為小車沒有進(jìn)行卡爾曼濾波之前的行駛軌跡，路徑用藍(lán)色標(biāo)記；濾波軌跡為小車進(jìn)行卡爾曼濾波之后的行駛軌跡，路徑用紅色標(biāo)記。從圖6中可以看出AGV小車的理想軌跡是從停車點(diǎn)到目標(biāo)點(diǎn)直線行駛，但是實(shí)際行駛過程中小車并不能如此理想的從停車點(diǎn)到達(dá)目標(biāo)點(diǎn)，從圖中可以看出小車在沒有濾波的情況下行駛過程毫無(wú)章法可言，忽上忽下而且偏離理想軌道的距離較遠(yuǎn)，最高達(dá)到40米，行駛路徑過長(zhǎng)而且復(fù)雜，而通過卡爾曼濾波之后的小車行駛過程中都是緊緊圍繞理想軌跡周圍運(yùn)動(dòng)，偏離理想軌跡的距離基本控制在10米以內(nèi)，由此可以看出AGV通過卡爾曼濾波之后小車運(yùn)行軌跡的精確度有了明顯的提高，行駛距離更短，導(dǎo)航過程更加精確，由此可以證明卡爾曼濾波在AGV小車的自主導(dǎo)航過程中是起到一定作用的。

AGV小車在圖7的行駛過程中，對(duì)其誤差進(jìn)行仿真，圖中理想軌跡與普通軌跡之間的誤差叫做濾波前誤差，用綠色點(diǎn)表示，而濾波后誤差指濾波軌跡與理想軌跡之間的誤差，用紅色點(diǎn)表示，從圖中可以看出，濾波前小車從停車點(diǎn)到目標(biāo)點(diǎn)行駛過程中的誤差較大，而且誤差大小很不穩(wěn)定，波動(dòng)幅度達(dá)到30左右，而濾波后的誤差相對(duì)濾波前而言比較平穩(wěn)，除了小車剛啟動(dòng)時(shí)誤差較大，波動(dòng)幅度達(dá)到13左右，后續(xù)行駛過程中誤差相比較穩(wěn)定，波動(dòng)幅度控制在5以下，由圖中兩個(gè)顏色的折線圖可以看出濾波前后小車行駛過程中的誤差有了明顯的提升，小車行駛過程更加穩(wěn)定，導(dǎo)航過程更加精準(zhǔn)，更加驗(yàn)證了卡爾曼濾波對(duì)小車自主導(dǎo)航過程中起到了重要的作用。

表1 單變量下AGV小車的航向角

圖6 系統(tǒng)仿真圖

圖7 系統(tǒng)誤差圖

6 結(jié)論

AGV小車運(yùn)行過程中，使用卡爾曼濾波和數(shù)字PID控制算法，使小車行駛過程更精準(zhǔn)，停車點(diǎn)使用與人工控制相結(jié)合的方式，可以使小車停車更精確。本課題對(duì)AGV小車在不定長(zhǎng)導(dǎo)航圖的初始導(dǎo)航角度進(jìn)行研究，可以根據(jù)研究結(jié)果，確定所有類似導(dǎo)航場(chǎng)景中的初始航向角。該設(shè)計(jì)通過編程實(shí)現(xiàn)，很好地完成了系統(tǒng)的導(dǎo)航功能，數(shù)據(jù)融合效果良好。