九軸陀螺儀在AGV自主導航中的應用設計

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

0 引言

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

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

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

1 應用場景

本設計為在動車車體裝配車間使用，系統使用兩個AGV小車共同完成動車車體在車間的移動。AGV小車使用麥克納姆輪，正常工作時，從車接受主車發出的指令，兩者協調同步運行。兩個AGV小車導航行走的現場平面如圖1所示(默認為30m×15m)，AGV導航車的平時停車位置點與目標位置點如圖1已標出。

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

圖1 AGV導航控制示意圖

AGV小車裝有麥克納姆輪，即每個輪子在沒有任何轉角的情況下，通過控制每一個輪子的速度和方向實現3自由度的全方位移動。AGV小車車頂安裝反射板，停車板上方安裝激光掃描儀，激光掃描儀通過對反射板的掃描判斷小車是否到達停車板附近。目標位置點TA和TB處有停車板，停車板設計為長方形，中軸線平行于TA和TB連線，正好容小車停下。AGV小車配有遙控器，當小車到達停車板附近時小車會發出警報，此時需要小車的操作人員通過遙控器指揮小車停到停車板上面。

2 坐標系與姿態角

本課題用到車體坐標系、場地坐標系和地理坐標系。因車體坐標系是隨時變動的，故需要依靠不變的地理坐標系確定航向角，因此需要將車體坐標系轉換到地理坐標系，再進行相關角度計算[4]。

2.1 車體坐標系

如圖2所示。取平面圖中小車的實際停放位置中心為坐標原點O，OXb軸的正向指向AGV小車的前方，OYb軸的正向指向AGV小車的右側，OZb軸的正向指向天，建立車體坐標系。

圖2 車體坐標系圖

2.2 地理坐標系

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

圖3 地理坐標系圖

2.3 場地坐標系

以場地的右方作為X軸，下方為Y軸進行定義，用于計算場地的導航角。場地坐標系與地理坐標系之間的夾角為α。

2.4 坐標變換矩陣

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

圖4 車體坐標系與地理坐標系變換圖

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

C=C1·C2·C3=

(1)

3 卡爾曼濾波與數據融合

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

3.1 卡爾曼濾波模型建立

本文認為磁力計、加速度計的角度變化都符合線性關系，故在卡爾曼濾波中建立狀態方程和量測方程。

(2)

(3)

將AGV狀態方程轉換為加速度計相關量的方程，如公式4所示。將AGV的測量方程轉換為磁力計相關量的方程，如公式5所示。

(4)

(5)

3.2 數據融合

卡爾曼濾波是用遞推的形式，對未來某一時刻狀態進行估計。本文以加速度計的航向角作為測量值，加速度計的航向角作為預測值，然后根據前一時刻的預測值和測量值來估計當前時刻系統的最優值。

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

首先根據系統上一次得來的最優值計算出當前的估計值和協方差。

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

(6)

(7)

再根據協方差大小計算出卡爾曼增益的大小。

(8)

最后根據當前估計值和測量值計算出當前最優值和誤差協方差。

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

(9)

(10)

4 PID控制算法

由于AGV使用采樣式控制，不能連續不斷的輸出信號，故不能連續控制，所以本課題使用數字PID控制算法，即在模擬PID算法基礎上，將微分和積分項進行離散化處理。

4.1 PID控制算法

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

圖5 PID控制原理圖

在實際應用中，需要通過大量實驗來確定PID三個參數[12]。其控制規律為：

(11)

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

4.2 數字PID控制算法

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

(12)

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

Aek+Bek-1+Cek-2

(13)

其中：θk代表計算機第k次采樣時的輸出；ek代表第k次采樣時輸入數據的偏差值；KI代表積分系統，KI=KpT/TI；KD代表微分系數，KD=KPTD/T。

卡爾曼濾波輸出的融合后的航向角作為數字PID算法中的輸入值，每隔一個時間周期都要重新計算從小車當前位置到目標位置的航向角，作為小車下一次行駛的輸入航向角，此輸入航向角與融合以后的航向角進行對比，從而進行PID調節。

5 實際應用

5.1 系統編程流程

AGV導航前期，首先使用九軸陀螺儀的磁力計測量出工作場地與地理坐標系的夾角α，車體停放的位置姿態與工作場地平行；根據圖1所示的幾何尺寸，使用場地坐標計算出發地點到目標地點的夾角β，并與α結合形成初始的車體導航角；在出發地點，通過麥克納姆輪轉動小車的車頭，使車頭朝向目標點。

啟動AGV車體向目標點出發。在行駛過程中，通過九軸陀螺儀的磁力計讀取γ角數據，通過九軸陀螺儀的加速度計計算在采樣周期內車體移動的夾角φ，其計算如公式14所示。

(14)

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

將它們作為卡爾曼濾波的輸入，通過卡爾曼濾波進行數據融合輸出融合后的航向角，AGV按照融合后的航向角控制行駛，同時通過PID算法調整AGV的行駛方向，使其更準確的朝著目標點行駛。

當小車到達停車板附近時，AGV小車會發出警報，此時需要小車操作人員用遙控器手動控制小車移動到停車板上。此時小車需要按照初始出發時的角度調整落位。

全方位移動平臺可以通過遙控器選擇單車或者兩車系統。在理想狀態下，當兩移動平臺協同運動時，從車受控地與主車保持相同的姿態。

5.2 場地航向角計算

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

AGV小車的導航圖如圖1所示，以UL為原點建立坐標系，使車體坐標系與地理坐標系原點重合。AGV小車從各出發點到目標點的初始航向角的關系如表1所示。

5.3 系統應用步驟

當AGV小車行駛的場地如圖1所示，而且停車點與目標點的距離是默認距離時，將小車從停車點UL(0，0)通過自主導航的形式到達目標點TB(23,7.5)，行駛過程如下：

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

2)通過麥克納姆輪原地調轉AGV小車的車頭，轉動角度為初始航向角，本課題中為18.06°。

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

4)將三軸磁力計和三軸陀螺儀融合后的角度與三軸加速度計的航向角作為卡爾曼濾波的輸入值，對角度進行卡爾曼濾波處理，將系統中與信號無關的噪聲干擾去除，然后將兩個角度進行融合，得到融合后的航向角。首先進行時間的更新，確定先驗估計值和誤差協方差預估計矩陣，然后再進行測量更新，最后確定小車的卡爾曼濾波增益、最優估計和誤差協方差矩陣，時間更新與測量更新交替進行，直到小車到達目標點為止。

5)小車行駛過程中，實時的計算小車的當前位置與目標位置的航向角，通過數字PID控制算法進行控制，使小車更準確的進行自主導航。

6)當小車行駛至目標點附近時，目標點附近的激光掃描儀會發出警報聲，此時需要操作人員使用遙控器控制小車準確的停到目標點的停車板上面。

5.4 系統仿真

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

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

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

圖6 系統仿真圖

圖7 系統誤差圖

6 結論

AGV小車運行過程中，使用卡爾曼濾波和數字PID控制算法，使小車行駛過程更精準，停車點使用與人工控制相結合的方式，可以使小車停車更精確。本課題對AGV小車在不定長導航圖的初始導航角度進行研究，可以根據研究結果，確定所有類似導航場景中的初始航向角。該設計通過編程實現，很好地完成了系統的導航功能，數據融合效果良好。