磁導(dǎo)引與UKF濾波定位的輪式AGV路徑跟蹤研究

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

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

1 引言

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

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

2 四輪轉(zhuǎn)向AGV運(yùn)動(dòng)學(xué)建模

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

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

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

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

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

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

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

將AGV位置偏差與相對(duì)角度偏差控制輸入分別設(shè)置7個(gè)模糊等級(jí)：左大偏（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的論域?yàn)椋?9cm，-9cm］；角度偏差α論域?yàn)椋?36°，36°］；導(dǎo)向角控制量Δφ論域?yàn)椋?π/3，π/3］；行駛速度控制量v論域?yàn)椋?.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)境果實(shí)的采摘作業(yè)時(shí)，作業(yè)環(huán)境復(fù)雜，使用單一傳感器獲取周圍及路徑信息，往往具有片面、局部、抗干擾性差、可信度低等問(wèn)題。因此本系統(tǒng)采用多種傳感器對(duì)AGV 姿態(tài)觀測(cè)并進(jìn)行信息融合得到AGV 實(shí)際運(yùn)行位姿及路徑信息。傳感器包括：AH100B 型航姿參考系統(tǒng)（AHRS）、CA-16D 型磁導(dǎo)航傳感器、后輪交流伺服電機(jī)編碼器等。將AGV 初始位姿及磁導(dǎo)航傳感器、航姿傳感器、后輪交流伺服電機(jī)編碼器等傳感器測(cè)量信息，通過(guò)UKF卡爾曼濾波器進(jìn)行信息融合獲取系統(tǒng)導(dǎo)航參數(shù)的動(dòng)態(tài)過(guò)程，準(zhǔn)確顯示AGV運(yùn)動(dòng)狀態(tài)演變。

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

以AGV在全局坐標(biāo)系中位姿估計(jì)模型推導(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)向量所對(duì)應(yīng)的方差陣為Pk；U（t）—系統(tǒng)控制輸入量，表示AGV兩后輪編碼器得到的里程數(shù)據(jù)及導(dǎo)向角；wk-1—高斯白噪聲。

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

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

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

根據(jù)UKF 卡爾曼濾波器基本模型及推導(dǎo)的AGV 系統(tǒng)狀態(tài)方程與量測(cè)方程，四輪轉(zhuǎn)向AGV組合定位UKF濾波算法流程圖，如圖4所示。主要包括系統(tǒng)初始化、樣本點(diǎn)計(jì)算、狀態(tài)更新、量測(cè)更新、濾波增益計(jì)算、循環(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)試驗(yàn)研究

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

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

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

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

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

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

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

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

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

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

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

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

6 結(jié)論

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