一種慣性傳感器與編碼器相結合的AGV航跡推算系統*

吳 鵬，李東京，贠 超

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

0 引 言

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

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

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

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

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

1 慣性導航

1.1 初始對準算法

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

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

(1)

根據幾何關系，φ，θ可由下面兩式算出：

(2)

(3)

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

地磁示意圖如圖1所示。

圖1 地磁示意圖

磁北角α由幾何關系算出：

(4)

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

φ=λ+α

(5)

1.2 誤差校準算法

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

(6)

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

(7)

(8)

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

(9)

通過旋轉矩陣將其表示在在載體坐標系中：

(10)

(11)

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

(12)

(13)

1.3 位置算法

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

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

(14)

代入加速度可得：

(15)

2 里程計算法

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

記編碼器轉一周產生的脈沖數為N，記測得的脈沖計數為M，則有：

(16)

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

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

圖2 里程計算法示意圖

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

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

(17)

(18)

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

(19)

進而可以算得AGV車身的轉動半徑rk：

(20)

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

(21)

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

(22)

進而計算AGV在導航坐標系中的位置變化：

(23)

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

(24)

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

(25)

φk+1=φk+Δφk

(26)

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

(27)

3 卡爾曼濾波器

本文將慣性導航和測距法相結合。采用KF對編碼器和慣性傳感器的數據進行融合。線性隨機差分方程為：

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

(28)

zk=Hxk+vk

(29)

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

(30)

(31)

(32)

(33)

(34)

由于先驗估計和后驗估計是線性關系，并假設速度和姿態的相關系數為0，則A是單位矩陣。

輸入變量(加速度、角速度)和狀態變量只與測量的時間間隔和轉換矩陣有關，故有：

(35)

B=Δt×I6×6

(36)

測量量和狀態量是線性關系，但測量量彼此之間不相關，則有：

H=I6×6

Hxk

(37)

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

其中，過程噪聲協方差來自慣性傳感器的測量過程，假設慣性傳感器彼此之間不相關，則有：

(38)

式中：Qf—加速度的噪聲協方差。

姿態的協方差是陀螺儀輸出的一次積分：

(39)

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

過程噪聲協方差為：

(40)

測量的噪聲協方差來自編碼器和磁強計，同時假設加速度計和磁強計以及編碼器不相關，則有：

(41)

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

算法流程圖如圖3所示。

圖3 算法流程圖

4 實驗

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

4.1 誤差補償實驗

結果如圖4所示。

補償后的角速度幾乎為0，并且狀態穩定，達到了非常好的補償效果。

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

圖5 對陀螺儀的誤差補償

4.2 位置估計實驗

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

圖6 里程計

圖7 卡爾曼濾波

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

表1 位置估計實驗數據

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

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

5 結束語

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

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

[1] 倪維晨.自動導引車控制系統的研究與應用[D].天津：天津理工大學機械工程學院，2012.

[2] 劉佳耀，袁佳艷，陶衛軍，等.一種配備智能機械臂的AGV設計與實現[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] 張永澤，艾長勝，張 尉.慣導與視覺相結合的AGV小車控制系統設計[J].山東工業技術，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.