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

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

1 引言

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

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

2 四輪轉(zhuǎn)向AGV運動學(xué)建模

自主設(shè)計的AGV為四輪轉(zhuǎn)向后輪差速驅(qū)動形式，左、右前輪轉(zhuǎn)角采用一個交流伺服電動機(jī)與一個伺服電動推桿實現(xiàn)左、右前輪轉(zhuǎn)角獨立控制，后輪采用兩步進(jìn)電動機(jī)實現(xiàn)兩后輪獨立轉(zhuǎn)向并使用兩交流伺服電動機(jī)作為行駛驅(qū)動。

AGV在全局坐標(biāo)系下位姿定位運動學(xué)模型，如圖1所示。

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

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

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

式中：R=L/2tanφ，由式（2）可以得到任意采樣時刻AGV當(dāng)前位姿，從而為自主導(dǎo)航定位提供數(shù)學(xué)基礎(chǔ)。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5.1 AGV測控系統(tǒng)硬件結(jié)構(gòu)設(shè)計

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

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

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

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

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

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

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

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

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

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

圖8 圓角矩形導(dǎo)引試驗結(jié)果Fig.8 Results of Rounded Rectangle Guidance Test

6 結(jié)論

基于自主設(shè)計的四輪轉(zhuǎn)向AGV進(jìn)行了位姿估計的運動學(xué)模型分析，開展了基于模糊控制器的磁導(dǎo)航路徑跟蹤控制研究和多傳感器信息融合的AGV位姿定位方法，實現(xiàn)四輪轉(zhuǎn)向AGV自主導(dǎo)航路徑跟蹤?？刂圃囼灡砻鳎琔KF卡爾曼濾波器AGV位姿定位算法可實現(xiàn)48mm以內(nèi)的位姿誤差和小于4°的航向誤差，可以為農(nóng)用輪式AGV應(yīng)用提供參考。