李忠利 劉小鋒 陳修魁 高永升 楊淑君

(河南科技大學(xué)車輛與交通工程學(xué)院, 洛陽 471003)

0 引言

農(nóng)業(yè)工程領(lǐng)域?qū)Ш蕉ㄎ幌到y(tǒng)的廣泛應(yīng)用在很大程度上提高了農(nóng)業(yè)生產(chǎn)和作業(yè)效率。由于單一的車輛定位技術(shù)都存在一定的局限性和適用范圍，往往不能滿足拖拉機(jī)田間作業(yè)的精度要求，因此以全球定位系統(tǒng)(Global position system，GPS)和慣性導(dǎo)航聯(lián)合的多傳感器信息融合定位技術(shù)成為農(nóng)業(yè)車輛導(dǎo)航定位系統(tǒng)的關(guān)鍵技術(shù)[1-4]。

華南農(nóng)業(yè)大學(xué)研制的拖拉機(jī)自動(dòng)導(dǎo)航定位控制系統(tǒng)在拖拉機(jī)速度為2.88 km/h時(shí)，拖拉機(jī)的跟蹤誤差保持在0.3 m[5-6]；中國農(nóng)業(yè)大學(xué)研制的拖拉機(jī)自動(dòng)導(dǎo)航控制系統(tǒng)能實(shí)時(shí)進(jìn)行數(shù)據(jù)處理和定位導(dǎo)航，并且能夠較好地進(jìn)行路線追蹤，最大橫向偏差為11 cm[7-8]。這些拖拉機(jī)自動(dòng)導(dǎo)航定位系統(tǒng)雖然具有較高的導(dǎo)航定位精度，也能進(jìn)行直線和轉(zhuǎn)彎跟蹤，但忽略了拖拉機(jī)車身姿態(tài)引起的定位誤差。為了修正這一誤差，須獲得拖拉機(jī)姿態(tài)信息。拖拉機(jī)姿態(tài)信息可通過加速度計(jì)或陀螺儀獲得。文獻(xiàn)[9]基于捷聯(lián)慣導(dǎo)系統(tǒng)結(jié)合四元數(shù)進(jìn)行了姿態(tài)解算，文獻(xiàn)[10]基于RTK-GPS雙天線進(jìn)行了姿態(tài)解算，文獻(xiàn)[11]采用自適應(yīng)擴(kuò)展卡爾曼進(jìn)行姿態(tài)解算。上述研究成果對(duì)于姿態(tài)解算都獲得了很好的測(cè)姿效果，但當(dāng)拖拉機(jī)產(chǎn)生側(cè)向加速度時(shí)，由側(cè)向加速度引起姿態(tài)解算偏差，對(duì)偏差進(jìn)行定位修正的研究較少。因此基于多傳感器數(shù)據(jù)融合的拖拉機(jī)組合導(dǎo)航定位修正研究對(duì)于農(nóng)業(yè)車輛導(dǎo)航具有重要意義。

本文采用載波相位差分全球定位系統(tǒng)(Real time kinematic differential global position system，RTK-GPS)和慣性測(cè)量單元(Inertial measurement unit，IMU)組合定位方式，將多傳感器信息進(jìn)行融合，充分發(fā)揮各個(gè)導(dǎo)航系統(tǒng)的優(yōu)勢(shì)，以修正拖拉機(jī)在田間作業(yè)時(shí)因地形傾斜、土質(zhì)硬度不均或連續(xù)轉(zhuǎn)彎而引起的定位誤差，從而提高系統(tǒng)的定位精度和可靠性。

1 組合導(dǎo)航定位方案

本文設(shè)計(jì)的組合導(dǎo)航系統(tǒng)主要由RTK-GPS和IMU組成。其中RTK-GPS主要包括基站移動(dòng)站接收機(jī)、基站移動(dòng)站無線數(shù)傳電臺(tái)、GPS天線、基站移動(dòng)站電臺(tái)收發(fā)天線和移動(dòng)電源；IMU由3軸的加速度計(jì)和3軸的陀螺儀組成。RTK-GPS[12]主要獲得拖拉機(jī)的位置、速度、航向信息，拖拉機(jī)的側(cè)傾角、俯仰角由加速度計(jì)、陀螺儀融合得到[13]。

RTK-GPS/IMU組合導(dǎo)航的核心在于多傳感器數(shù)據(jù)的融合，從而得到最優(yōu)估計(jì)的定位信息[14]。多傳感器融合的常用算法是卡爾曼濾波。但卡爾曼濾波需要建立精確的系統(tǒng)模型并且狀態(tài)噪聲和觀測(cè)噪聲應(yīng)該滿足正態(tài)分布才能得到比較準(zhǔn)確的估計(jì)效果。而拖拉機(jī)在田間工作過程中，由于田間環(huán)境復(fù)雜多變，將導(dǎo)致模型的狀態(tài)噪聲和觀測(cè)噪聲特性發(fā)生改變。因此，當(dāng)建立的卡爾曼模型與實(shí)際的拖拉機(jī)工作狀態(tài)存在較大偏差時(shí)，將導(dǎo)致卡爾曼濾波失敗。

遺傳算法反向傳播(Genetic algorithm back propagation，GA-BP)神經(jīng)網(wǎng)絡(luò)可以完美擬合任意非線性系統(tǒng)，可以和卡爾曼濾波算法融合，彌補(bǔ)卡爾曼算法的局限性[15]。因此，采用GA-BP神經(jīng)網(wǎng)絡(luò)卡爾曼濾波算法實(shí)現(xiàn)RTK-GPS/IMU組合導(dǎo)航的信息融合。其組合導(dǎo)航定位方案如圖1 所示。

圖1 組合導(dǎo)航定位方案圖Fig.1 Integrated navigation positioning plan diagram

2 基于GA-BP神經(jīng)網(wǎng)絡(luò)的卡爾曼濾波器設(shè)計(jì)

2.1 GA-BP神經(jīng)網(wǎng)絡(luò)模型

2.1.1BP神經(jīng)網(wǎng)絡(luò)模型

BP神經(jīng)網(wǎng)絡(luò)是一種以信號(hào)前向傳遞，誤差逆向傳播的多層前饋神經(jīng)網(wǎng)絡(luò)[16-17]。在BP神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)訓(xùn)練過程中，輸入信號(hào)經(jīng)過權(quán)值、閾值和激活函數(shù)處理之后依次到達(dá)隱含層和輸出層。如果輸出層的輸出值與期望值之間的均方根誤差沒有達(dá)到設(shè)定的網(wǎng)絡(luò)精度，則誤差將反向傳播，以修改網(wǎng)絡(luò)的權(quán)值和閾值。多次迭代訓(xùn)練，使得BP神經(jīng)網(wǎng)路的預(yù)測(cè)輸出不斷逼近期望值，從而建立輸入輸出的非線性模型。BP神經(jīng)網(wǎng)絡(luò)模型可以采用3層網(wǎng)絡(luò)結(jié)構(gòu)，將JY61型六軸姿態(tài)角度傳感器的X軸加速度Xacc、Y軸加速度Yacc、Z軸加速度Zacc、Z軸角速度w和RTK-GPS速度信息v的乘積wv作為網(wǎng)絡(luò)的輸入變量，由Kolmogorov公式計(jì)算得出隱含層神經(jīng)元的個(gè)數(shù)為9，側(cè)向加速度在X、Y、Z軸的分量Xnoise、Ynosie、Znoise作為BP神經(jīng)網(wǎng)絡(luò)的輸出。

在構(gòu)建的BP神經(jīng)網(wǎng)絡(luò)中，隱含層傳遞函數(shù)為正切S型tansig函數(shù)，輸出層傳遞函數(shù)為線性purelin函數(shù)。權(quán)值學(xué)習(xí)訓(xùn)練函數(shù)為trainlm函數(shù)，此函數(shù)采用Levenberg-Marquardt算法[18]，可以使得權(quán)值快速收斂，達(dá)到網(wǎng)絡(luò)訓(xùn)練目標(biāo)的要求。

2.1.2權(quán)值與閾值優(yōu)化

因?yàn)锽P神經(jīng)網(wǎng)絡(luò)權(quán)值和閾值的隨機(jī)初始化導(dǎo)致網(wǎng)絡(luò)的泛化能力不強(qiáng)，容易陷入局部最優(yōu)解，所以采用遺傳算法優(yōu)化網(wǎng)絡(luò)的初始權(quán)值和閾值以提高神經(jīng)網(wǎng)絡(luò)的訓(xùn)練效果[19]。遺傳算法優(yōu)化BP神經(jīng)網(wǎng)絡(luò)的權(quán)值和閾值流程圖如圖2所示。

圖2 遺傳算法優(yōu)化神經(jīng)網(wǎng)絡(luò)的權(quán)值和閾值流程圖Fig.2 Genetic algorithm optimization of weight and threshold flow chart of neural network

2.1.3Matlab仿真訓(xùn)練

由于無法從傳感器上得到BP神經(jīng)網(wǎng)絡(luò)的訓(xùn)練數(shù)據(jù)，需要用理論數(shù)據(jù)樣本對(duì)神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練。依據(jù)拖拉機(jī)在田間的工作情況大致可以確定拖拉機(jī)速度、角速度、俯仰角、側(cè)傾角以及Z軸加速度的取值范圍，并以表1中拖拉機(jī)角速度和速度乘積作為拖拉機(jī)的側(cè)向加速度Lacc。已知側(cè)傾角、俯仰角、Z軸加速度即可求出X軸加速度Xacc和Y軸加速度Yacc，將拖拉機(jī)側(cè)向加速度Lacc根據(jù)勾股定理分解在X、Y、Z軸上，從而得到BP神經(jīng)網(wǎng)絡(luò)所需的輸入Xinput、Yinput、Zinput和輸出Xnoise、Ynoise、Znoise。根據(jù)表1中拖拉機(jī)參數(shù)的取值范圍編寫了BP神經(jīng)網(wǎng)絡(luò)樣本生成軟件，該軟件以合適的間隔對(duì)這些特征信息的范圍進(jìn)行細(xì)化，遍歷所有可能的組合，用理論值對(duì)神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練。表2為生成BP神經(jīng)網(wǎng)絡(luò)的部分訓(xùn)練樣本數(shù)據(jù)。Xnoise為側(cè)向加速度在X軸的分量，Ynoise為側(cè)向加速度在Y軸的分量，Znoise為側(cè)向加速度在Z軸的分量。從而可得：Xinput、Yinput、Zinput、Lacc為BP神經(jīng)網(wǎng)絡(luò)的輸入特征，Xnoise、Ynoise、Znoise為BP神經(jīng)網(wǎng)絡(luò)的輸出。

表1 拖拉機(jī)參數(shù)的取值范圍Tab.1 Value range of various tractor parameters

表2 BP神經(jīng)網(wǎng)絡(luò)樣本數(shù)據(jù)Tab.2 BP neural network sample data m/s2

根據(jù)設(shè)置的取值范圍生成滿足條件的80 500組數(shù)據(jù)樣本，其中200組作為測(cè)試樣本，剩余的數(shù)據(jù)作為訓(xùn)練樣本。為了使得神經(jīng)網(wǎng)絡(luò)快速收斂、訓(xùn)練時(shí)間短且激活函數(shù)的區(qū)分度明顯，要對(duì)輸入的樣本數(shù)據(jù)進(jìn)行歸一化處理[20]。歸一化公式為

(1)

式中S——樣本歸一化值

Ii——樣本數(shù)據(jù)

Imax——樣本數(shù)據(jù)最大值

Imin——樣本數(shù)據(jù)最小值

經(jīng)過多次仿真試驗(yàn)，設(shè)學(xué)習(xí)率Lr為 0.3，網(wǎng)絡(luò)精度為 0.000 01[21]，迭代次數(shù)為2 000次時(shí)，網(wǎng)絡(luò)訓(xùn)練較好。

遺傳算法對(duì)于BP神經(jīng)網(wǎng)絡(luò)有很好的優(yōu)化效果，在優(yōu)化過程中需要對(duì)算法的一些參數(shù)進(jìn)行設(shè)置。種群規(guī)模表示初始解集的大小，太少容易丟失種群多樣性，導(dǎo)致無法尋找到最優(yōu)解；太多則會(huì)增加計(jì)算量。種群規(guī)模設(shè)置為50；染色體編碼采用浮點(diǎn)編碼形式；選擇函數(shù)為normGeomSelect，傳遞選擇參數(shù)為0.09；交叉函數(shù)為arithXover，傳遞交叉參數(shù)為2；變異函數(shù)為nonUnifMutation，傳遞變異參數(shù)為[2，100，3]；迭代次數(shù)為100次[22]。

依據(jù)搭建的GA-BP神經(jīng)網(wǎng)絡(luò)模型在Matlab 2018工作環(huán)境下進(jìn)行BP神經(jīng)網(wǎng)絡(luò)的訓(xùn)練。真實(shí)值與神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)值對(duì)比如圖3所示；側(cè)向加速度在各個(gè)軸分量真實(shí)值與神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)值誤差如圖4所示。

圖3 真實(shí)值與神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)值對(duì)比Fig.3 Comparison between true value and predicted value of neural network

圖4 側(cè)向加速度在各個(gè)軸分量真實(shí)值與神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)值誤差Fig.4 Error between true value of lateral acceleration component on each axis and predicted value of neural network

由圖3可知，雖在一些樣本點(diǎn)處擬合的效果不理想，但整體預(yù)測(cè)值擬合結(jié)果較好，這也驗(yàn)證了所構(gòu)建的GA-BP神經(jīng)網(wǎng)絡(luò)具有較好的預(yù)測(cè)效果。

從圖4可以看到，側(cè)向加速度在X、Y、Z軸分量的真實(shí)值和預(yù)測(cè)值誤差在±0.04 m/s2的范圍內(nèi)波動(dòng)，主要集中在±0.02 m/s2的區(qū)間范圍內(nèi)。

2.2 卡爾曼濾波器設(shè)計(jì)

拖拉機(jī)姿態(tài)解算中的卡爾曼濾波是建立姿態(tài)角的狀態(tài)方程和觀測(cè)方程，然后通過預(yù)測(cè)更新過程得到較準(zhǔn)確的姿態(tài)角[23-24]。將陀螺儀積分得到的姿態(tài)角A和陀螺儀偏差B作為卡爾曼濾波狀態(tài)方程的狀態(tài)量，加速度計(jì)得到的姿態(tài)角Aacc作為觀測(cè)值。該組合導(dǎo)航系統(tǒng)的狀態(tài)方程和觀測(cè)方程為

(2)

(3)

式中wgyro——陀螺儀角速度輸出

wg——陀螺儀噪聲

vacc——加速度計(jì)的測(cè)量噪聲

設(shè)wg與vacc相互獨(dú)立且為滿足正態(tài)分布的白噪聲。令系統(tǒng)的采樣時(shí)間為dt，則離散系統(tǒng)的狀態(tài)方程和觀測(cè)方程為

(4)

(5)

(6)

(7)

式中θ、φ——陀螺儀得到的俯仰角、側(cè)傾角

wXb——X軸角速度偏差

wYb——Y軸角速度偏差

wX、wY——陀螺儀X、Y軸的噪聲

N(0，Rk)——加速度計(jì)噪聲的正態(tài)分布特征

θacc、φacc——加速度計(jì)得到的仰俯角、側(cè)傾角

k——離散系統(tǒng)時(shí)間，下標(biāo)表示時(shí)刻

Q、R——過程噪聲和觀測(cè)噪聲的協(xié)方差矩陣

qθ、qφ——陀螺儀積分得到俯仰角和側(cè)傾角的噪聲協(xié)方差

qbX、qbY——陀螺儀X、Y軸偏差噪聲的協(xié)方差

rθ、rφ——加速度計(jì)得到的俯仰角和側(cè)傾角噪聲協(xié)方差

Q、R表示卡爾曼濾波器對(duì)其傳感器得到數(shù)據(jù)的信任程度，值越小，表明信任程度越高。通過對(duì)采集到的大量試驗(yàn)數(shù)據(jù)進(jìn)行分析，可得：qθ為0.002、qφ為0.005、qbX為0.01、qbY為0.01、rθ為3、rφ為3，代入即可得到Q和R?？柭鼮V波整個(gè)工作過程分為時(shí)間更新和觀測(cè)更新。時(shí)間更新即根據(jù)前一時(shí)刻的狀態(tài)估計(jì)值推算當(dāng)前時(shí)刻的狀態(tài)變量先驗(yàn)估計(jì)值和誤差協(xié)方差先驗(yàn)估計(jì)值；觀測(cè)更新負(fù)責(zé)將先驗(yàn)估計(jì)和新的測(cè)量值結(jié)合起來構(gòu)造改進(jìn)的后驗(yàn)估計(jì)，以次遞歸找到最優(yōu)結(jié)果。

3 試驗(yàn)準(zhǔn)備及誤差分析

3.1 坐標(biāo)系建立

選用高斯平面坐標(biāo)系，y軸指向坐標(biāo)東為正，x軸指向坐標(biāo)北為正。車體坐標(biāo)系為右手坐標(biāo)系，X′軸指向車輛前進(jìn)的方向，Y′軸垂直于車輛前進(jìn)方向且指向駕駛員右側(cè)，Z′軸為通過汽車質(zhì)心指向上方。由于GPS的定位信息所使用的坐標(biāo)系為WGS84大地坐標(biāo)系統(tǒng)，所以需要對(duì)WGS84大地坐標(biāo)系進(jìn)行高斯投影正解計(jì)算，得到相對(duì)應(yīng)的高斯平面坐標(biāo)[25-27]。坐標(biāo)系示意圖如圖5所示。

圖5 坐標(biāo)系示意圖Fig.5 Schematic of coordinate system

3.2 拖拉機(jī)導(dǎo)航參數(shù)

圖6 拖拉機(jī)導(dǎo)航參數(shù)Fig.6 Tractor navigation parameters

將車體前進(jìn)方向與真北方向的夾角定義為拖拉機(jī)的航向角ψ，規(guī)定正方向?yàn)轫槙r(shí)針方向。將X′軸與水平面之間的夾角定義為拖拉機(jī)的俯仰角θ，規(guī)定拖拉機(jī)前輪下傾為正，上傾為負(fù)。將車體Y′軸與水平面之間的夾角定義為拖拉機(jī)的側(cè)傾角φ，規(guī)定沿車體前進(jìn)方向右傾為正，左傾為負(fù)。拖拉機(jī)的導(dǎo)航參數(shù)如圖6所示。

3.3 RTK-GPS和IMU安裝

試驗(yàn)選用的拖拉機(jī)為東方紅MK904型輪式拖拉機(jī)，安裝設(shè)備主要包括GPS雙天線、移動(dòng)站接收機(jī)、移動(dòng)站無線數(shù)傳電臺(tái)、IMU，GPS天線安裝在拖拉機(jī)頂部中心橫軸線上，兩邊各一個(gè)，距離地面約2 580 mm。移動(dòng)站無線數(shù)傳電臺(tái)安裝在拖拉機(jī)前蓋上，移動(dòng)站接收機(jī)放在駕駛室內(nèi)?；綠PS天線、接收機(jī)、無線數(shù)傳電臺(tái)放在田間試驗(yàn)場(chǎng)，RTK-GPS設(shè)備全程采用移動(dòng)電源供電。IMU選用JY61型六軸姿態(tài)角度傳感器，輸出3軸加速度信息、3軸角速度信息和拖拉機(jī)姿態(tài)信息，安裝在拖拉機(jī)的質(zhì)心位置，X軸與X′軸重合、Y軸與Y′軸重合、垂直于模塊向外為Z軸。本次試驗(yàn)需要使用編寫的上位機(jī)數(shù)據(jù)采集軟件將原始的RTK-GPS和IMU的定位信息進(jìn)行保存，用于后續(xù)在Matlab中使用已經(jīng)訓(xùn)練的GA-BP神經(jīng)網(wǎng)絡(luò)進(jìn)行處理以及對(duì)GA-BP神經(jīng)網(wǎng)絡(luò)卡爾曼濾波算法的驗(yàn)證。GPS和IMU安裝位置圖如圖7所示。

圖7 RTK-GPS和IMU安裝位置圖Fig.7 RTK-GPS and IMU installation diagram1.電臺(tái)天線 2.移動(dòng)站無線數(shù)傳電臺(tái) 3.計(jì)算機(jī) 4.移動(dòng)站接收機(jī) 5.JY61型六軸姿態(tài)角度傳感器 6.GPS天線

3.4 上位機(jī)數(shù)據(jù)采集軟件

上位機(jī)數(shù)據(jù)采集軟件是基于Visual Studio開發(fā)軟件使用C#語言開發(fā)。上位機(jī)數(shù)據(jù)采集軟件同時(shí)對(duì)多傳感器的信息進(jìn)行采集并同步傳感器數(shù)據(jù)，用于后續(xù)的算法驗(yàn)證和處理。上位機(jī)數(shù)據(jù)采集軟件主要有兩大功能：①對(duì)傳感器信息的顯示，這也更加直觀地了解到傳感器當(dāng)前的狀態(tài)信息。②對(duì)傳感器的信息進(jìn)行保存，方便后續(xù)的算法處理。上位機(jī)數(shù)據(jù)采集軟件界面如圖8所示。從圖8可以看到，姿態(tài)傳感器3軸加速度、3軸角速度、繞3軸轉(zhuǎn)動(dòng)的角度、UTC時(shí)間、定位方式、定位信息、速度信息以及航向信息，還能通過曲線實(shí)時(shí)看到數(shù)據(jù)的動(dòng)態(tài)變化，從而判斷采集到的數(shù)據(jù)是否正確。

圖8 上位機(jī)數(shù)據(jù)采集軟件界面Fig.8 PC data acquisition software

3.5 GPS誤差分析

由于田間情況復(fù)雜、地面起伏不定、土質(zhì)硬度不均，拖拉機(jī)在田間耕作或連續(xù)轉(zhuǎn)彎期間，車身極易受到這些因素影響而發(fā)生傾斜，從而導(dǎo)致GPS接收天線發(fā)生傾斜，產(chǎn)生定位誤差。為了提高拖拉機(jī)自主耕作的導(dǎo)航精度，實(shí)現(xiàn)精細(xì)作業(yè)，必須要對(duì)車體傾斜誤差進(jìn)行修正，從而得到較準(zhǔn)確的GPS定位信息。當(dāng)車體發(fā)生傾斜時(shí)，其在車體坐標(biāo)系上X′、Y′軸在水平面上投影的偏差為

e′x=Hsinθ

(8)

e′y=Hsinφ

(9)

式中H——GPS天線距離水平地面的高度

由于e′x、e′y是居于車體坐標(biāo)系下X′、Y′軸水平面上投影的偏差，而定位所采用的坐標(biāo)系是高斯平面坐標(biāo)系，因此需要進(jìn)行坐標(biāo)變換，將車體坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到高斯平面坐標(biāo)系下的坐標(biāo)[24-27]。設(shè)在高斯平面下坐標(biāo)為(ex,ey)，其對(duì)應(yīng)的車體坐標(biāo)系下的坐標(biāo)為(e′x,e′y)，它們的轉(zhuǎn)換關(guān)系為

(10)

其中，航向角由RTK-GPS雙天線測(cè)得，具有很高的精度。得到了高斯平面坐標(biāo)系下的偏差，就可以用此偏差修正原始的RTK-GPS定位信息，從而提高導(dǎo)航精度。

4 田間試驗(yàn)

為了驗(yàn)證所設(shè)計(jì)的基于GA-BP神經(jīng)網(wǎng)絡(luò)卡爾曼濾波算法的有效性，利用搭建的組合導(dǎo)航系統(tǒng)試驗(yàn)平臺(tái)，在洛陽孟津一拖產(chǎn)品試驗(yàn)基地選取一塊未被耕種的試驗(yàn)田進(jìn)行試驗(yàn)。主要包括拖拉機(jī)的直線行駛試驗(yàn)和拖拉機(jī)連續(xù)轉(zhuǎn)彎試驗(yàn)。在拖拉機(jī)直線行駛試驗(yàn)中對(duì)拖拉機(jī)進(jìn)行了4次的數(shù)據(jù)采集，在拖拉機(jī)連續(xù)轉(zhuǎn)彎試驗(yàn)中對(duì)拖拉機(jī)進(jìn)行了2次數(shù)據(jù)的采集。將GA-BP神經(jīng)網(wǎng)絡(luò)卡爾曼濾波算法的組合導(dǎo)航方案對(duì)拖拉機(jī)的位置信息進(jìn)行實(shí)時(shí)估計(jì)，同時(shí)，記錄RTK-GPS和IMU的原始定位信息。RTK-GPS具有雙天線，且橫向安裝只能得到拖拉機(jī)的側(cè)傾角，其動(dòng)態(tài)精度為0.08°。由于RTK-GPS側(cè)傾角的更新頻率為1 Hz，將RTK-GPS測(cè)得的側(cè)傾角進(jìn)行插值處理后作為參考角度來驗(yàn)證算法的效果。圖9為拖拉機(jī)直線行駛過程中側(cè)傾角在不同算法下的對(duì)比。從圖9可以得到，在拖拉機(jī)直線行駛情況下，GA-BP卡爾曼和卡爾曼2種算法獲得的側(cè)傾角基本跟隨GPS參考角度變換，且GA-BP卡爾曼和卡爾曼2種算法得到的側(cè)傾角曲線變化趨勢(shì)大致相同。圖10為拖拉機(jī)直線行駛過程中不同算法下的側(cè)傾角與GPS參考角度的誤差圖。從圖10可以得到，GA-BP卡爾曼算法得到的側(cè)傾角與GPS參考角度的最大誤差為0.07 rad，最小值為-0.04 rad，大部分誤差的范圍為-0.01～0.04 rad；卡爾曼算法得到的側(cè)傾角與GPS參考角度的最大誤差為0.09 rad，最小誤差為-0.05 rad，大部分誤差的范圍為-0.02～0.05 rad。由于GA-BP神經(jīng)網(wǎng)絡(luò)已經(jīng)訓(xùn)練完畢且數(shù)據(jù)處理設(shè)備為計(jì)算機(jī)CPU，所以GA-BP卡爾曼算法和傳統(tǒng)卡爾曼算法的響應(yīng)時(shí)間差距為納秒級(jí)，可忽略不計(jì)，拖拉機(jī)轉(zhuǎn)彎行駛也如此。由此可得，拖拉機(jī)在直線行駛情況下，GA-BP卡爾曼算法獲得的側(cè)傾角比卡爾曼得到的側(cè)傾角精度提高了0.01 rad。

圖9 拖拉機(jī)直線行駛過程中在不同算法下 側(cè)傾角對(duì)比Fig.9 Angle comparison chart of tractor’s roll angle by different algorithms during straight driving

圖10 拖拉機(jī)直線行駛過程中不同算法下的 側(cè)傾角與GPS參考角度的誤差曲線Fig.10 Error graph of roll angle and GPS reference angle by different algorithms during tractor straight driving

圖11 拖拉機(jī)連續(xù)轉(zhuǎn)彎過程中在不同算法下 側(cè)傾角對(duì)比Fig.11 Comparison of roll angle of tractor during continuous turning by different algorithms

圖11為拖拉機(jī)連續(xù)轉(zhuǎn)彎過程中側(cè)傾角在不同算法下的對(duì)比。從圖11可以得到，在拖拉機(jī)連續(xù)轉(zhuǎn)彎過程中，2種算法獲得的側(cè)傾角的動(dòng)態(tài)變換趨勢(shì)同GPS參考角度一致，但卡爾曼算法得到的側(cè)傾角的變換幅度相對(duì)于GA-BP卡爾曼算法變化幅度較劇烈。圖12為拖拉機(jī)連續(xù)轉(zhuǎn)彎過程中不同算法下的側(cè)傾角與GPS參考角的誤差。從圖12可得，GA-BP卡爾曼算法得到的側(cè)傾角與GPS參考角度的最大誤差為0.08 rad，最小值為-0.04 rad，大部分誤差的范圍為-0.02～0.04 rad；卡爾曼算法得到的側(cè)傾角與GPS參考角度的最大誤差為0.11 rad，最小誤差為-0.06 rad，大部分誤差的范圍為-0.04～0.08 rad。由此可得，拖拉機(jī)在連續(xù)轉(zhuǎn)彎過程中，GA-BP卡爾曼算法獲得的側(cè)傾角比卡爾曼得到的側(cè)傾角在左傾時(shí)精度提高了0.02 rad，右傾時(shí)精度提高了0.04 rad。即在轉(zhuǎn)彎過程中GA-BP卡爾曼算法比卡爾曼算法得到的側(cè)傾角具有更好的精度。

圖13 拖拉機(jī)直線行駛軌跡及局部放大圖Fig.13 Straight track of tractor and partial enlarged view

圖12 拖拉機(jī)連續(xù)轉(zhuǎn)彎過程中不同算法下的 側(cè)傾角與GPS參考角度的誤差曲線Fig.12 Error graph of roll angle and GPS reference angle by different algorithms during tractor continuous turning

將原始的RTK-GPS定位信息和經(jīng)過誤差修正之后的定位信息進(jìn)行比較從而驗(yàn)證誤差修正的效果。由于試驗(yàn)環(huán)境限制且拖拉機(jī)連續(xù)轉(zhuǎn)彎行駛無法精確規(guī)劃路徑，所以沒有以規(guī)劃的直線路徑和轉(zhuǎn)彎路徑作為參考，進(jìn)而通過分析定位信息的數(shù)字特征來間接驗(yàn)證修正效果。圖13為拖拉機(jī)直線行駛軌跡及局部放大圖。圖14為拖拉機(jī)連續(xù)轉(zhuǎn)彎行駛軌跡及局部放大圖。圖中所示都為高斯平面相對(duì)坐標(biāo)，x軸對(duì)應(yīng)坐標(biāo)北向，y軸對(duì)應(yīng)東向。在圖13a、13b、13c、13d、14a、14b中圓圈對(duì)應(yīng)的放大圖13e、13f、13g、13h、14c、14d中可以看到，拖拉機(jī)實(shí)際軌跡會(huì)出現(xiàn)曲折的波動(dòng)，這些波動(dòng)主要是由于田間地面起伏不定、土質(zhì)硬度不均導(dǎo)致的車身傾斜而造成的定位偏差。經(jīng)過GA-BP神經(jīng)網(wǎng)絡(luò)卡爾曼濾波算法及誤差修正后的拖拉機(jī)行駛軌跡相對(duì)于原始拖拉機(jī)軌跡整體更加平穩(wěn)。表3為拖拉機(jī)在直線和轉(zhuǎn)彎行駛過程中定位信息的協(xié)方差和相關(guān)系數(shù)。從表3可以得到，只有第1次轉(zhuǎn)彎的定位信息為正相關(guān)，其余情況都為負(fù)相關(guān)。從其對(duì)應(yīng)的協(xié)方差和相關(guān)系數(shù)的原始和修正值可以得到修正后的定位信息協(xié)方差和相關(guān)系數(shù)比原始定位信息都有明顯減小。即修正后的定位信息比起原始信息幅值變化小、集中且平穩(wěn)。根據(jù)圖13、14和表3可得GA-BP卡爾曼算法及誤差分析在一定程度上修正了車身姿態(tài)傾斜引起的定位誤差，使拖拉機(jī)在復(fù)雜的地形或連續(xù)轉(zhuǎn)彎情況下都有較好的定位精度。

圖14 拖拉機(jī)連續(xù)轉(zhuǎn)彎行駛軌跡及局部放大圖Fig.14 Continuous turning track of tractor and partial enlarged view

表3 拖拉機(jī)在直線和轉(zhuǎn)彎過程中定位信息的協(xié)方差和相關(guān)系數(shù)Tab.3 Covariance and correlation coefficient of tractor positioning information during straight and turning

5 結(jié)論

(1)針對(duì)拖拉機(jī)在田間作業(yè)時(shí)因車身傾斜導(dǎo)致的定位誤差問題，結(jié)合卡爾曼算法和BP神經(jīng)網(wǎng)絡(luò)算法的優(yōu)點(diǎn)，設(shè)計(jì)了一種基于GA-BP神經(jīng)網(wǎng)絡(luò)算法的多傳感器信息融合導(dǎo)航定位系統(tǒng)。

(2)基于設(shè)計(jì)的信息融合導(dǎo)航定位系統(tǒng)，實(shí)現(xiàn)多傳感器信息融合導(dǎo)航定位，包括GA-BP神經(jīng)網(wǎng)絡(luò)卡爾曼濾波器的設(shè)計(jì)及誤差分析。

(3)利用搭建的組合導(dǎo)航系統(tǒng)試驗(yàn)平臺(tái)對(duì)定位修正進(jìn)行驗(yàn)證，結(jié)果表明：以GA-BP神經(jīng)網(wǎng)絡(luò)卡爾曼濾波算法為核心的RTK-GPS/IMU組合導(dǎo)航系統(tǒng)能在一定程度上修正因田間地面起伏不定、土質(zhì)硬度不均、連續(xù)轉(zhuǎn)彎而導(dǎo)致的GPS定位誤差，使拖拉機(jī)行駛軌跡更加平穩(wěn)。