一種慣性傳感器與編碼器相結(jié)合的AGV航跡推算系統(tǒng)*

吳 鵬，李東京，贠 超

(北京航空航天大學(xué) 機械工程及自動化學(xué)院，北京 100191)

0 引 言

自動導(dǎo)航車AGV是一種自動化無人駕駛的智能化搬運設(shè)備，是現(xiàn)代工業(yè)自動化物流系統(tǒng)中的關(guān)鍵設(shè)備[1-2]。其定位系統(tǒng)是實現(xiàn)自動導(dǎo)引的關(guān)鍵部分，也是目前的研究熱點。定位是指AGV通過感知自身和周圍環(huán)境信息，經(jīng)過一定的數(shù)據(jù)處理得到自身位姿的過程。目前主要分為兩類：絕對定位和相對定位[3]。

常用的絕對定位方式有全球定位系統(tǒng)(GPS)、超聲波定位系統(tǒng)、紅外網(wǎng)絡(luò)系統(tǒng)、射頻識別系統(tǒng)等等[4]。其中，GPS不能用在室內(nèi)，并且更新頻率較低[5]；超聲波定位系統(tǒng)和紅外網(wǎng)絡(luò)系統(tǒng)具有低成本、小型化和易于連接的特點[6]，然而這兩種方法不能在長距離下使用，同時需要在場景中大面積布置，并且由于信號干擾而難以滿足AGV的定位要求[7]；而射頻識別系統(tǒng)需要額外的設(shè)備和較高的成本[8]。這些絕對定位方法的優(yōu)點在于它們不會積累定位誤差，但整體定位誤差較大[9]。

相對定位主要是指慣性導(dǎo)航和里程計算法(或測距法)[10]。慣性導(dǎo)航使用的主要傳感器為IMU或AHRS，包括三軸的磁強計、三軸的陀螺儀和三軸的加速度計[11]。靜止狀態(tài)下，通過磁強計與加速度計獲取姿態(tài)；運動狀態(tài)下，通過加速度計獲取位置，通過陀螺儀獲取姿態(tài)。里程計算法使用的主要傳感器為輪上編碼器。運動狀態(tài)下，基于主動輪上編碼器的脈沖計數(shù)來估計AGV的位姿[12]。這兩種相對定位方式都由于算法中引入了一次或二次積分而存在較大累積誤差，單獨使用時都很難保證AGV定位的準確性和穩(wěn)定性[13]。

針對該問題，多數(shù)的研究采用將慣性導(dǎo)航與絕對定位相結(jié)合或里程計算法與絕對定位相結(jié)合的方式來消除累積誤差。例如LU M F、LIU B C等人[14]將紅外網(wǎng)絡(luò)系統(tǒng)與里程計算法相結(jié)合；HELLMERS H、NORRDINE A等人[15]將超聲波定位系統(tǒng)與慣性導(dǎo)航相結(jié)合；張永澤、艾長勝等人[16]將慣性導(dǎo)航與視覺相結(jié)合。但是這些研究都主要集中在使用外部信號來消除相對定位的累積誤差上，并沒有實質(zhì)上減少里程計算法或者慣性導(dǎo)航算法本身所引入的累積誤差。

本文將提出將慣性導(dǎo)航與里程計算法這兩種相對定位算法相結(jié)合的方式，并搭建相應(yīng)的實驗平臺對算法的有效性進行驗證。

1 慣性導(dǎo)航

1.1 初始對準算法

慣性導(dǎo)航基于初始位置和初始姿態(tài)來計算AGV的相對位姿[17]。假設(shè)以起始點為原點，AGV的初始的姿態(tài)可以由相互正交的三軸加速度計和磁強計計算得出。

首先求出載體坐標系上的加速度向量：

(1)

根據(jù)幾何關(guān)系，φ，θ可由下面兩式算出：

(2)

(3)

磁強計可以測量地球磁場在該地區(qū)的磁場強度在載體坐標系中的三軸分量，根據(jù)磁場強度可以計算得到偏航角φ。

地磁示意圖如圖1所示。

圖1 地磁示意圖

磁北角α由幾何關(guān)系算出：

(4)

磁北和真北的差角為磁偏角λ，則偏航角為：

φ=λ+α

(5)

1.2 誤差校準算法

當(dāng)AGV運動時，可由三軸陀螺儀獲取車身角速度在載體坐標系中的三軸分量，進而積分算得車身的姿態(tài)。同時，載體坐標系的速度和位置可分別由加速度的一次、二次積分算得。但是，該積分運算會使兩傳感器的誤差隨時間而累積，進而極大地影響速度和位姿的準確度。因此在將加速度計和陀螺儀用于AGV的定位時，必須通過校準和補償來提高精度。

(6)

在靜止狀態(tài)下，加速度計所測得的加速度都來自于重力加速度，因此其在導(dǎo)航坐標系中的北向和東向的分量都為0，即這兩個方向的測量值都是噪聲：

(7)

(8)

求出加速度計噪聲后，便可通過將其移除來獲取估計值：

(9)

通過旋轉(zhuǎn)矩陣將其表示在在載體坐標系中：

(10)

(11)

三軸陀螺儀提供角速度在載體坐標系中的三軸分量。當(dāng)AGV靜止時，陀螺儀在載體坐標系中的三軸分量應(yīng)當(dāng)都為0，因此此時的讀數(shù)都是噪聲：

(12)

(13)

1.3 位置算法

速度和位置可分別由加速度的一次和二次積分算得：

vk+1=vk+fkΔt，Pk+1=Pk+vkΔt

(14)

代入加速度可得：

(15)

2 里程計算法

里程計算法使用的主要傳感器為輪上編碼器。運動狀態(tài)下，基于主動輪上編碼器的脈沖計數(shù)來估計AGV的位姿。

記編碼器轉(zhuǎn)一周產(chǎn)生的脈沖數(shù)為N，記測得的脈沖計數(shù)為M，則有：

(16)

式中：ηk，l，ηk，r—左右兩輪的轉(zhuǎn)角，以弧度為單位；Ml，Mr—兩輪上編碼器所測得的脈沖數(shù)。

里程計算法示意圖如圖2所示。

圖2 里程計算法示意圖

AGV的速度、位置和偏航角都可以基于兩輪的轉(zhuǎn)角ηk，l和ηk，r來計算。AGV的行駛距離ak可以用驅(qū)動輪的半徑Rwheel，和輪子的轉(zhuǎn)角來表示：

ak，l=Rwheelηk，l，ak，r=Rwheelηk，r

(17)

(18)

AGV的偏航角變化量Δφk，可以由AGV兩驅(qū)動輪的間距和每個輪子的行駛距離來計算：

(19)

進而可以算得AGV車身的轉(zhuǎn)動半徑rk：

(20)

由余弦定理，AGV的位置變化量可以表示為：

(21)

當(dāng)AGV作直線運動時，車身的偏航角變化量為0，此時計算得到的車身轉(zhuǎn)動半徑無窮大，上式中括號項為0，進而AGV的位置變化量為0，顯然是不正確的。故單獨處理這種情況，使用泰勒級數(shù)將式(21)展開：

(22)

進而計算AGV在導(dǎo)航坐標系中的位置變化：

(23)

由于行駛地面不可能絕對水平，筆者考慮AGV輕微的橫滾和俯仰，將位置變化量投影到水平面，則可以進一步表示為：

(24)

AGV在k+1時刻的位置和偏航角可以表示為：

(25)

φk+1=φk+Δφk

(26)

速度則可以通過求微分得到：

(27)

3 卡爾曼濾波器

本文將慣性導(dǎo)航和測距法相結(jié)合。采用KF對編碼器和慣性傳感器的數(shù)據(jù)進行融合。線性隨機差分方程為：

xk=Axk-1+Buk-1+wk-1

(28)

zk=Hxk+vk

(29)

隨機變量wk和vk分別表示過程噪聲和測量噪聲。相應(yīng)的時間更新方程和測量更新方程[18]為：

(30)

(31)

(32)

(33)

(34)

由于先驗估計和后驗估計是線性關(guān)系，并假設(shè)速度和姿態(tài)的相關(guān)系數(shù)為0，則A是單位矩陣。

輸入變量(加速度、角速度)和狀態(tài)變量只與測量的時間間隔和轉(zhuǎn)換矩陣有關(guān)，故有：

(35)

B=Δt×I6×6

(36)

測量量和狀態(tài)量是線性關(guān)系，但測量量彼此之間不相關(guān)，則有：

H=I6×6

Hxk

(37)

式中：下標o—由里程計算法計算所得；下標a—由公式(2，3)計算所得；下標c—由磁強計計算所得。

其中，過程噪聲協(xié)方差來自慣性傳感器的測量過程，假設(shè)慣性傳感器彼此之間不相關(guān)，則有：

(38)

式中：Qf—加速度的噪聲協(xié)方差。

姿態(tài)的協(xié)方差是陀螺儀輸出的一次積分：

(39)

式中：Nangle—陀螺儀的白噪聲；Qgyro—陀螺儀的噪聲協(xié)方差。

過程噪聲協(xié)方差為：

(40)

測量的噪聲協(xié)方差來自編碼器和磁強計，同時假設(shè)加速度計和磁強計以及編碼器不相關(guān)，則有：

(41)

式中：RvN，RvE，RΔφ—由編碼器估計的速度、偏航角變化量的誤差協(xié)方差；Rφ，Rθ—來自加速度計的誤差協(xié)方差；Rφ—來自磁強計的誤差協(xié)方差。

算法流程圖如圖3所示。

圖3 算法流程圖

4 實驗

為了驗證算法的有效性，本研究搭建了兩輪驅(qū)動實驗平臺。其中，編碼器安裝在兩驅(qū)動輪上，IMU傳感器選用的是Advanced navigation公司的Orientus產(chǎn)品。

4.1 誤差補償實驗

結(jié)果如圖4所示。

補償后的角速度幾乎為0，并且狀態(tài)穩(wěn)定，達到了非常好的補償效果。

圖4 對加速度計的誤差補償

圖5 對陀螺儀的誤差補償

4.2 位置估計實驗

實驗的預(yù)設(shè)路徑為直徑9 700 mm的圓形，控制AGV跟蹤該路徑運行50次，根據(jù)采集的數(shù)據(jù)繪制圖如圖(6～7)所示。

圖6 里程計

圖7 卡爾曼濾波

由圖(6～7)可見，測距法估計出比真實圓形更小的位置，而卡爾曼濾波的結(jié)果在真實數(shù)據(jù)的上下波動，雖然方差較大，但具有更高的準確性。具體地，兩種方式在4個位置的均值和標準差，位置估計實驗數(shù)據(jù)如表1所示。

表1 位置估計實驗數(shù)據(jù)

Odo—單獨使用里程計算法；kal—本文提出的算法

由此可見，與僅使用里程計算法相比，本文提出的算法具有更高的準確性。

5 結(jié)束語

針對AGV的室內(nèi)定位問題，本文提出了一種相對位置估計算法來實現(xiàn)航跡推算，其結(jié)合了慣性導(dǎo)航系統(tǒng)與里程計算法。為了補償慣性導(dǎo)航與里程計算法中的累積誤差，本研究設(shè)計了卡爾曼濾波器來對傳感器的數(shù)據(jù)進行融合。最后基于兩輪AGV實驗平臺，本研究對誤差補償算法與位置估計算法進行了驗證。

實驗的結(jié)果表明：該算法有效地提升了位姿估計的精度與穩(wěn)定性。為進一步借助外部信號來提升AGV的室內(nèi)組合定位精度提供更好的相對定位基礎(chǔ)，同時將有助于提升AGV定位系統(tǒng)在外部信號干擾與缺失時的穩(wěn)定性。

[1] 倪維晨.自動導(dǎo)引車控制系統(tǒng)的研究與應(yīng)用[D].天津：天津理工大學(xué)機械工程學(xué)院，2012.

[2] 劉佳耀，袁佳艷，陶衛(wèi)軍，等.一種配備智能機械臂的AGV設(shè)計與實現(xiàn)[J].兵工自動化，2016，35(10)：38-31.

[3] RONZONI D, OLMI R, SECCHI C, et al. AGV global localization using indistinguishable artificial landmarks[C]. 2011 IEEE International Conference on Robotics and Automation, Shanghai: IEEE，2011.

[4] YOON S W, PARK S B, KIM J S. Kalman filter sensor fusion for Mecanum wheeled automated guided vehicle localization[J].JournalofSensors，2015(8)：1-7.

[5] NAGASAKA Y, TAMAKI K, NISHIWAKI K, et al. A global positioning system guided automated rice transplanter[J].IFACProceedingsVolumes，2013，46(18):41-46.

[6] YAYAN U, YUCEL H, YAZICI A. A low cost ultrasonic based positioning system for the indoor navigation of mobile robots[J].JournalofIntelligent&RoboticSystems，2015，78(3-4):541.

[7] UREA J, GUALDA D, HERNNDEZ, et al. Ultrasonic local positioning system for mobile robot navigation: from low to high level processing[C].2015 IEEE International Conference on Industrial Technology, Seville: IEEE，2015.

[8] ROEHRIG C, HELLER A, HESS D, et al. Global localization and position tracking of automatic guided vehicles using passive RFID technology[C]. Proceeding of 41st International Symposium on Robotics, Munchen: IEEE,2014.

[9] PIERLOT V, DROOGENBROECK M V. A new three object triangulation algorithm for mobile robot positioning[J].IEEETransactionsonRobotics，2014，30(3):566-577.

[10] SILVA O D, MANN G K I, GOSINE R G. An ultrasonic and vision-based relative positioning sensor for multirobot localization[J].IEEESensorsJournal，2015，15(3):1716-1726.

[11] OMARI S, BLOESCH M, GOHL P, et al. Dense visual-inertial navigation system for mobile robots[C]. 2015 IEEE International Conference on Robotics and Automation(ICRA), Washington: IEEE，2015.

[12] MARTNEZ B H, HERRERO P D. Autonomous navigation of an automated guided vehicle in industrial environments[J].RoboticsandComputer-IntegratedManufacturing，2010，26(4):296-311.

[13] AOKI H, ANN R S, TANAKA A, et al. Odometer correction method using disturbed environmental magnetic field[C].2014 IEEE 3rd Global Conference on Consumer Electronics, Tokyo: IEEE，2014.

[14] LU M F, LIU B C, WU J P, et al. The indoor automatic guided vehicle with an IR positioning and low-cost inertial navigation system[J].AppliedMechanicsandMaterials,2013(300-301):494-499.

[15] HELLMERS H, NORRDINE A, BLANKENBACH J, et al. An IMU/magnetometer-based indoor positioning system using kalman filtering[C].2013 International Conference on Indoor Positioning and Indoor Navigation, Guimaraes: IEEE，2013.

[16] 張永澤，艾長勝，張 尉.慣導(dǎo)與視覺相結(jié)合的AGV小車控制系統(tǒng)設(shè)計[J].山東工業(yè)技術(shù)，2015(20):236-236.

[17] NOURELDIN A, KARAMAT T B, GEORGY J. Fundamentals of inertial navigation, satellite-based positioning and their integration[M]. Berlin: Springer Berlin Heidelberg，2013.

[18] BISHOP G, WELCH G. An introduction to the kalman filter[J].UniversityofNorthCarolinaatChapelHill，2013，8(7)：127-132.