磁導引與UKF濾波定位的輪式AGV路徑跟蹤研究

尹建軍，余承超，賀 坤，劉繼展

（江蘇大學現代農業(yè)裝備與技術教育部重點實驗室，江蘇 鎮(zhèn)江 212013）

1 引言

輪式自動導引車（Automated Guided Vehicle，AGV）適應于現代農業(yè)發(fā)展要求，逐漸應用于對靶噴藥、物資搬運、果實采摘套袋、農作物信息收集等方面[1-2]。AGV作為果實采摘機器人的載體，其自主導航行走系統(tǒng)利用攜帶的傳感器得到自身在作業(yè)環(huán)境中的位姿并實現導航運行，系統(tǒng)的穩(wěn)定性將直接影響整個果實采摘環(huán)節(jié)。然而，目前國內有關采摘機器人自主導航行走系統(tǒng)的設計研究，無法滿足實際采收作業(yè)環(huán)境的要求。因此，穩(wěn)定可靠的自主導航行走系統(tǒng)具有重要的科學研究意義。

移動機器人作為典型的非線性系統(tǒng)，難以建立精準的數學模型，以單磁導引橫向偏差和兩次采樣周期計算得到的AGV與磁條相對角度偏差作為模糊控制器輸入，結合人類駕駛車輛經驗制定規(guī)則庫，得到AGV速度與導向角控制輸出。自動導航系統(tǒng)采用的核心導航算法，根據機器人應用的傳感器種類，主要包括[3-6]：圖像處理、航位推算、模糊邏輯、神經網絡、與遺傳算法以及卡爾曼濾波器。考慮自主設計農業(yè)輪式AGV 采用航姿參考系統(tǒng)、磁導航傳感器、后輪交流伺服電機編碼器等傳感器探測方法，結合常用數據融合方法優(yōu)缺點，選用無跡卡爾曼濾波器（UKF）進行多傳感器信息融合的AGV位姿定位方法。并通過試驗驗證算法的有效性。

2 四輪轉向AGV運動學建模

自主設計的AGV為四輪轉向后輪差速驅動形式，左、右前輪轉角采用一個交流伺服電動機與一個伺服電動推桿實現左、右前輪轉角獨立控制，后輪采用兩步進電動機實現兩后輪獨立轉向并使用兩交流伺服電動機作為行駛驅動。

AGV在全局坐標系下位姿定位運動學模型，如圖1所示。

圖1 AGV位姿估計模型示意圖Fig.1 AGV Pose Estimation Model

圖中：P—AGV 速度瞬心；M—AGV 質心；φ—前輪導向角；Δθ—AGV 旋轉角；R—轉向半徑；ω—AGV 航向角速度；θ—車體姿態(tài)角，即全局坐標系∑xIOyI的x軸到AGV局部坐標系∑xROyR的xR軸的夾角，左右輪輪距W=550mm，前后輪軸距L=870mm。

AGV 離散化運動控制，在不同的采樣時間點，根據AGV 當前位姿、目標位姿及系統(tǒng)輸入信號進行姿態(tài)控制。輸入信號為導向角φ及兩后輪轉速，設ti時刻AGV位姿為ξIi=(xi，yi，θi)，經過采樣時間間隔Δt，AGV 圍繞其瞬時旋轉中心運動到當前位姿ξIi+1=(xi+1，yi+1，θi+1)，在此過程中AGV位姿變化量ΔξI=(Δx，Δy，Δθ)，對于圖1所示的運動學模型，可以得到[7]：

式中：R=L/2tanφ，由式（2）可以得到任意采樣時刻AGV當前位姿，從而為自主導航定位提供數學基礎。

3 基于模糊控制器的四輪轉向AGV路徑跟蹤研究

將磁導航傳感器探測得到AGV與導航路徑相對位置偏差e與通過磁導航傳感器計算得到AGV與導航路徑相對角度偏差α，作為模糊控制器的輸入。針對單一磁導航傳感器僅能直接得到位置偏差e，提出一種采用兩次采樣周期得到的位置偏差計算磁條相對角度偏差α的方法，原理示意圖，如圖2所示。AGV單周期瞬時運動看做直線運動，ti時刻對應磁條相對偏差ei，ti+1時刻為ei+1，AGV直線行進距離S。

圖2 角度偏差檢測示意圖Fig.2 Angle Deviation Detection Diagram

將AGV位置偏差與相對角度偏差控制輸入分別設置7個模糊等級：左大偏（NB）、左中偏（NM）、左小偏（NS）、不偏（ZE）、右小偏（PS）、右中偏（PM）、右大偏（PB）。類似可以得到，導向角控制輸出量：左大轉（NB）、左中轉（NM）、左小轉（NS）、不轉（ZE）、右小轉（PS）、右中轉（PM）、右大轉（PB）；速度控制輸出：L（1km/h）、S（1.5km/h）、M（2km/h）、B（2.5km/h）、V（3km/h）。

其中，橫向偏差e的論域為［-9cm，-9cm］；角度偏差α論域為［-36°，36°］；導向角控制量Δφ論域為［-π/3，π/3］；行駛速度控制量v論域為［0.1m/s，0.3m/s］。依據以上輸入輸出關系制定98條模糊控制規(guī)則，在MATLAB模糊控制箱中生成AGV控制曲面，如圖3所示。

圖3 MATLAB模糊控制箱Fig.3 MATLAB Fuzzy Control Box

4 基于多傳感器信息融合的AGV位姿定位方法

輪式AGV 主要面向溫室、果園等環(huán)境果實的采摘作業(yè)時，作業(yè)環(huán)境復雜，使用單一傳感器獲取周圍及路徑信息，往往具有片面、局部、抗干擾性差、可信度低等問題。因此本系統(tǒng)采用多種傳感器對AGV 姿態(tài)觀測并進行信息融合得到AGV 實際運行位姿及路徑信息。傳感器包括：AH100B 型航姿參考系統(tǒng)（AHRS）、CA-16D 型磁導航傳感器、后輪交流伺服電機編碼器等。將AGV 初始位姿及磁導航傳感器、航姿傳感器、后輪交流伺服電機編碼器等傳感器測量信息，通過UKF卡爾曼濾波器進行信息融合獲取系統(tǒng)導航參數的動態(tài)過程，準確顯示AGV運動狀態(tài)演變。

無跡卡爾曼濾波器（UKF）是一種典型的非線性估計方法，該方法通過一種非線性變換-U變換并結合標準卡爾曼濾波來進行非線性模型的狀態(tài)與誤差協(xié)方差更新，由于UKF沒有線性化過程，因此對于非線性模型其估計結果擁有更高的估計精度[8-9]。

以AGV在全局坐標系中位姿估計模型推導得到的坐標（x，y）與航姿參考系統(tǒng)的航向角θ作為UKF卡爾曼濾波器的狀態(tài)向量，以兩后輪編碼器得到AGV里程數據及AGV導向角φ作為系統(tǒng)控制輸入量，AGV系統(tǒng)的狀態(tài)方程可以表示為：

式中：X（t）—AGV的狀態(tài)向量，狀態(tài)向量所對應的方差陣為Pk；U（t）—系統(tǒng)控制輸入量，表示AGV兩后輪編碼器得到的里程數據及導向角；wk-1—高斯白噪聲。

根據四輪轉向AGV位姿估計模型，則AGV系統(tǒng)狀態(tài)方程可以表示為：

設k時刻AGV所在全局坐標系下位置為[xk，yk，θk]T，根據如圖2所示磁導航傳感器輸出計算得到的橫向偏差e及航姿參考系統(tǒng)輸出的航向角θ轉化為AGV 系統(tǒng)位姿，選取Zk=[x y θ]T作為觀測量，則量測方程Zk可以表示為：

式中：vk—服從高斯分布的零均值白噪聲序列。

根據UKF 卡爾曼濾波器基本模型及推導的AGV 系統(tǒng)狀態(tài)方程與量測方程，四輪轉向AGV組合定位UKF濾波算法流程圖，如圖4所示。主要包括系統(tǒng)初始化、樣本點計算、狀態(tài)更新、量測更新、濾波增益計算、循環(huán)轉跳等步驟。

圖4 四輪轉向AGV組合定位UKF濾波流程圖Fig.4 Four-Wheel Steering AGV Combined Positioning UKF Filter Flow Chart

5 四輪轉向AGV行走系統(tǒng)試驗研究

5.1 AGV測控系統(tǒng)硬件結構設計

AGV控制系統(tǒng)硬件主要包括：傳感器系統(tǒng)、工控機、PLC、電動機驅動系統(tǒng)、無線遙控、供電系統(tǒng)等，如圖5所示。采用主從分布式控制結構，上位機工控機基于LabVIEW 平臺實時進行多傳感器數據信息的獲取、處理、存儲、顯示等，下位機PLC與工控機通過串行通信接收AGV 四輪轉向運動控制信號，完成前進、后退、左轉、右轉、加減速等運動，并反饋驅動電機編碼器信息[10]。

圖5 AGV硬件系統(tǒng)Fig.5 AGV Hardware System

5.2 AGV測控系統(tǒng)軟件設計

四輪轉向AGV位姿估計程序采用LabVIEW軟件設計，其主要完成與PLC程序通訊獲取16位磁導航傳感器以及后輪驅動電機編碼器信息采集，通過串口獲取航姿參考系統(tǒng)（AHRS）航向角信息，最后采用UKF濾波器對采集信息進行融合得到AGV位姿狀態(tài)估計，如圖6所示。

圖6 UKF濾波程序設計流程圖Fig.6 UKF Filtering Program Design Flow Chart

5.3 四輪轉向AGV行駛系統(tǒng)試驗研究

為了驗證四輪轉向AGV位姿定位多傳感器信息融合算法的有效性，設計了圓角矩形磁導引軌跡位姿定位實驗，并分別進行四輪轉向AGV僅使用后輪驅動電機編碼器反饋及使用后輪驅動電機編碼器反饋、航姿參考系統(tǒng)與磁導航傳感器信息融合位姿定位試驗，受實驗條件限制，只進行了室內位姿定位實驗，如圖7所示。實驗設備：四輪轉向AGV、3cm寬磁導航磁條、卷尺等。

圖7 圓角矩形導引軌跡位姿定位試驗Fig.7 Positioning and Orientation Test of Rounded Rectangle Guide Tracking

試驗過程中上位機以100ms采樣間隔實時記錄航姿參考系統(tǒng)航向角、兩后輪編碼器反饋與磁導航傳感器信息，并實時計算2種定位方式的位姿估計結果。

當AGV僅使用后輪驅動電機編碼器進行位姿推斷定位時，隨著AGV向前行進，如圖8（a）、圖8（b）所示。AGV位姿隨著導航時間不斷偏離預定義軌跡，在初始時位姿與理論軌跡比較接近，當在第一個圓角ɑ進行轉彎時AGV位姿誤差開始變大，并且轉彎過程中AGV轉角誤差不斷累積；當轉彎完成后其行進方向已與預定義航向出現10°左右偏差，并且隨著行進位置偏差最大達到425mm，經過多次轉向，AGV運行軌跡相對預定義軌跡出現較大變形；當AGV使用后輪驅動電機編碼器、航姿參考系統(tǒng)與磁導航傳感器信息融合位姿定位，AGV 能夠有效沿著預定義的圓角矩形行進，且在行進過程中AGV位姿誤差小于48mm，航向誤差小于4°，表明采用UKF濾波器進行多傳感器信息融合的方法能夠有效提高AGV位姿定位精度，證明了本AGV位姿定位算法的有效性。

圖8 圓角矩形導引試驗結果Fig.8 Results of Rounded Rectangle Guidance Test

6 結論

基于自主設計的四輪轉向AGV進行了位姿估計的運動學模型分析，開展了基于模糊控制器的磁導航路徑跟蹤控制研究和多傳感器信息融合的AGV位姿定位方法，實現四輪轉向AGV自主導航路徑跟蹤。控制試驗表明，UKF卡爾曼濾波器AGV位姿定位算法可實現48mm以內的位姿誤差和小于4°的航向誤差，可以為農用輪式AGV應用提供參考。