汪思迪 曹小華 周 勇
武漢理工大學物流工程學院 武漢 430063
摘要：提出一種新型的AGV 定位導航方案，通過RFID 與圖像識別解決慣性導航誤差累計的缺陷，以STM32為主控制器實現對平臺的數據採集和姿態計算，由傳感器得到平臺的加速度、角速度，採用四元數法作為平臺的姿態更新算法，經濾波後得到平臺的姿態角。實驗結果表明，該方法可靠性強，計算簡單，且具有較好的定位精度，可以滿足AGV 的定位系統要求。
關鍵詞：AGV; 慣性導航；圖像識別；姿態解算
中圖分類號：TP391 文獻標識碼：A 文章編號：1001-0785（2018）08-0081-04
0 引言
隨著勞動力成本的上升和技術水平的提高，物流倉儲行業的自動化程度不斷提升。作為倉儲物流的核心設備，自動導引小車（AGV）的應用範圍與導航技術得到迅猛發展。目前AGV 主要有電磁導引、光學導引、雷射導航、慣性導航、視覺導航和全球定位系統導航等方式[1]。多數AGV 採用雷射導航和磁導航方式，雖然這些導航技術已是成熟技術，但其成本高，難以維護和改造，因此，新的導航方式越來越受到人們的關注。其中，慣性導航技術作為一種不依賴外界輔助的獨立導航方式，在適用範圍和布局上具有得天獨厚的優勢。另一方面，伴隨著物聯網的興起，無線射頻識別（RFID）技術被引入，該技術成本低，使用靈活，抗汙染能力強。
針對慣性導航方式中的誤差累積和定位精度問題，本設計引入RFID 技術與圖像識別技術，將位置信息存儲在RFID 標籤中，圖像識別獲取姿態信息，實現AGV的輔助定位，提高自動化效率。
1 整體方案設計
AGV 小車利用慣性導航作為導引技術，由於系統中所用數據都是採樣得到的，而且是由前一狀態量推到下一狀態量，因此，在算法解算的過程中，所有誤差都會隨著時間被算法累積下來，可能造成很大的累積誤差[2]。本設計通過RFID 定位來修正定位誤差, 通過圖像識別來修正角度誤差，在地面每隔一段距離貼一個地標，利用AGV 小車上的攝像頭掃描來確定當前AGV 小車的實時位置信息，然後基於這些信息直接對慣性導航系統進行糾正，重新調整初始參數。融合導航方案如圖1 所示，該技術方案不僅可以保證慣性導航系統的精度及可靠性，同時還可以將RFID 標籤作為站點識別的載體，使AGV 可以隨時上線，避免了人工輸入錯誤的情況[3]。
採用柵格狀RFID 標籤進行導航和站點標識，柵格狀標籤設置示例如圖2 所示( 圖中為了路徑清晰用灰框代替RFID 標籤)。將具有坐標信息的RFID 標籤鋪設在倉庫的相應位置上，每個標籤都代表一個站點，可以用於AGV 定位；如果標籤正上方設置了貨架，則該標籤代表一個貨架站點；如果該標籤上方目前已有小車停靠，則在動態地圖上標記該柵格暫時被佔用；如果標籤上方存在障礙物，則在地圖上標記該柵格不可通行。
圖1 融合導航方案
圖2 柵格狀標籤設置示例
AGV 在上述柵格地圖中行駛時，每隔固定距離就會從標籤上方通過，通過有源標籤的識別來獲取離散的坐標信息。為保證AGV 不脫離柵格軌道，只要保持AGV 小車實現準確的停靠、直行、旋轉90°的控制動作，就可以始終保持在相鄰標籤的軸線上行駛。運行時，伺服器結合AGV 的自定位信息和地圖信息，以標籤為站點，為每輛AGV 進行動態路徑規劃。在該柵格地圖下，每個標籤都可以用於標識站點，任意兩個相鄰標籤連線就形成一條短路徑，站點密集，還可進行動態規劃路徑。
2 傳感器信號採集與處理
為了設計體積小、質量輕、功耗低的姿態導航參考系統，選擇集成度高的嵌入式處理器STM32F103ZET6 作為導航核心處理器，實現姿態和導航解算以及輸出。使用集成加速度計和陀螺儀的MPU9255 測量加速度和機體的角速度。
由於載體的姿態方位角速率較大，對姿態矩陣的實時計算提出了更高的要求。確定載體的姿態矩陣即為研究載體坐標系b 和導航坐標系n 的空間轉換關係，一般用載體坐標系相對導航坐標系的三次轉動角確定，即俯仰角θ 、航向角ψ 、滾轉角γ 。目前主要的研究方法為歐拉法、方向餘弦法和四元數法[4]。
歐拉法求解姿態角時，求解速度緩慢，姿態求解方程中會出現「奇點」，而且不能進行全姿態解算。方向餘弦法在求解過程中計算量過大，不能快速得到計算結果，很難實現實際工作中的目標。四元數法不僅求解方程無奇性，而且線性程度高，方程解算時間大大減少，因此，本文姿態測量系統中選用四元數法作為姿態更新算法[5]。
2.1 四元數法
四元數法姿態矩陣計算步驟如下[6]：初始四元數的確定，輸入四元數方程在求解過程中初始姿態角
更新四元數，需要這個周期經過補償後的角速度gx、gy、gz，該計算過程中採用二階龍格庫塔法，有
3）姿態角計算，實時計算出四元數後，再由式（3）完成姿態矩陣的更新計算，通過式（4）獲得載體的姿態角信息
2.2 卡爾曼濾波
在實際測試中，上位機接收到角姿態的數據仍然存在少許噪聲幹擾，為了濾除噪聲幹擾，引入卡爾曼濾波算法。設姿態角為α ，對其離散化並構造狀態方程和觀測方程
1）通過第k -1 時刻的α 值，預測第k 時刻的α 值：k k -1 k α =α
2）根據第k -1 時刻的系統誤差估計第k 時刻的系統預測誤差：
3）濾波增益：
4）系統最優估算值：
5）誤差更新：
2.3 圖像識別
當AGV 小車從標籤上方通過時，需要用圖像識別的方法對慣性導航的累積誤差進行修正，調整角度，使小車在正確的方向上行駛。具體流程：獲取圖像→顏色空間轉換→閾值分割→邊緣提取→ Hough 變換→得到角度。
其中，Hough 變換是檢測圖像中直線和其角度的關鍵。它通過將圖像坐標空間變換到參數空間來實現直線與曲線的擬合[7]。使用極坐標方程來表示直線，其方程為
Hough 變換的具體實現步驟如下[8]：1）建立一個參數（
）空間的二維數組，該數組相當於一個累加器；
2）順序搜索圖像中所有目標像素，對於每一個目標像素，在參數空間中根據式（5）找到對應位置，然後在累加器的對應位置加1；
3）求出參數空間（累加器）中峰值，其位置為（ρ ′，θ ′）；
4）通過參數空間位置（ρ ′，θ ′），根據式（5）找到圖像空間中相對應的直線參數。
3 實驗與結果分析
為了驗證本文算法的精度，設計了AGV 定位實驗。採用上述十軸慣性導航元件，讓AGV 在柵格地圖中沿直線行走，測得的加速度與角速度經過四元數算法解算出姿態角，經過卡爾曼濾波處理後得到估計值。如圖3 所示，在30 s 中1 200 個數據採樣過程中，由於陀螺儀隨機漂移的影響，姿態角有較大的累計誤差，濾波後明顯收斂，誤差在±2°以內。
（a）俯仰角 （b）航向角 （c）滾轉角
圖3 濾波後的姿態角
當小車停靠在地圖標籤上方時，圖像識別當前行駛角度，見圖4。將小車原地旋轉10°，再次識別當前角度，如圖5。可見，圖像識別測量的精度較高，每當小車經過一個標籤時，都可以通過圖像識別的方法測出小車的偏移量，重新調整角度。
圖4 原始圖像（左）與變換圖像（右）測得ψ =0.9°
圖5 原始圖像（左）與變換圖像（右）測得ψ =-9.1°
4 結論
本文提出的算法結合慣性導航AGV 小車，解決了傳統慣性導航中誤差累積的缺陷，採用圖像識別解決小車定位與糾偏。基於四元數的姿態解算方法運算量小的特點滿足了在實際應用中實時姿態估計的要求。對陀螺儀輸出角速度的隨機漂移誤差，應用濾波器得到了姿態測量模塊穩定準確的姿態信息。實驗結果表明，該方法簡單可行，對室內AGV 的路徑導航有一定的實用價值，為解決機器的定位、測量問題提供了一種新思路。
參考文獻
[1] 張正義．AGV 技術發展綜述[J]．物流技術與應用，2005（7）：67-73.
[2] 劉智平，畢開波．慣性導航與組合導航基礎[M]．北京：國防工業出版社，2013．
[3] 張永澤, 艾長勝, 張尉. 慣導與視覺相結合的AGV 小車控制系統設計[J]. 山東工業技術,2015（20）：236.
[4] 朱榮，周兆英. 基於MEMS 的姿態測量系統[J]. 測控技術，2002（10）.
[5] 李楠, 高雲峰. 基於ARM 處理器AGV 用微小型捷聯慣導系統的研究[J]. 傳感器與微系統,2008.
[6] 秦永元. 慣性導航[M]. 北京：科學出版社, 2006.
[7] 秦麗娟，王挺，劉慶濤. 計算機單目視覺定位[M]. 北京：國防工業出版社，2016.
[8] 馬頌德, 張正友. 計算機視覺: 計算理論與算法基礎[M].北京：科學出版社, 1998.
end