劉　軍，袁　俊，蔡駿宇，陶昌嶺，王利明，程　偉（江蘇大學(xué)汽車與交通工程學(xué)院，鎮(zhèn)江212013）

?

基于GPS/INS和線控轉(zhuǎn)向的農(nóng)業(yè)機(jī)械自動(dòng)駕駛系統(tǒng)

劉軍，袁俊，蔡駿宇，陶昌嶺，王利明，程偉
（江蘇大學(xué)汽車與交通工程學(xué)院，鎮(zhèn)江212013）

摘要：研究旨在設(shè)計(jì)出一套農(nóng)用車輛自動(dòng)導(dǎo)航控制系統(tǒng)，讓機(jī)器人代替農(nóng)民進(jìn)行田間作業(yè)，實(shí)現(xiàn)農(nóng)用車輛自動(dòng)駕駛，從而可以有效提高農(nóng)業(yè)機(jī)械的作業(yè)精度、生產(chǎn)效率和使用安全性，并且為精細(xì)農(nóng)業(yè)研究提供技術(shù)支持，改善農(nóng)業(yè)生產(chǎn)的方法。該文通過GPS/INS（global positioning system/ inertial navigation system）組合導(dǎo)航技術(shù)實(shí)時(shí)獲得載體的導(dǎo)航信息（位置、速度、航向、姿態(tài)），根據(jù)導(dǎo)航信息與預(yù)設(shè)軌跡參數(shù)計(jì)算出載體的目標(biāo)前輪轉(zhuǎn)向角，并以該目標(biāo)前輪轉(zhuǎn)向角與當(dāng)前前輪轉(zhuǎn)角的差值作為控制輸入，實(shí)現(xiàn)對轉(zhuǎn)向執(zhí)行電機(jī)的精確控制，從而實(shí)現(xiàn)載體的路徑跟蹤控制。同時(shí)對整個(gè)系統(tǒng)的軟硬件進(jìn)行設(shè)計(jì)，并對系統(tǒng)控制策略進(jìn)行仿真和試驗(yàn)驗(yàn)證。最終結(jié)果表明，本文所設(shè)計(jì)的組合導(dǎo)航系統(tǒng)定位精度高，其定位精度可達(dá)到0.1～0.5 m；路徑跟蹤系統(tǒng)誤差小，當(dāng)車速分別為0.5 m/s和1 m/s時(shí)，路徑跟蹤的最大橫向誤差分別為0.16 m和0.27 m；整個(gè)系統(tǒng)響應(yīng)速度快，可達(dá)到0.1s。通過將GPS/INS組合導(dǎo)航技術(shù)與線控轉(zhuǎn)向技術(shù)相結(jié)合，能夠?qū)崿F(xiàn)農(nóng)用車輛的自動(dòng)駕駛。

關(guān)鍵詞：農(nóng)業(yè)機(jī)械；機(jī)器人；控制系統(tǒng)；GPS/INS；組合導(dǎo)航；路徑跟蹤；線控轉(zhuǎn)向

劉軍，袁俊，蔡駿宇，陶昌嶺，王利明，程偉.基于GPS/INS和線控轉(zhuǎn)向的農(nóng)業(yè)機(jī)械自動(dòng)駕駛系統(tǒng)[J].農(nóng)業(yè)工程學(xué)報(bào)，2016，32（01）：46-53.doi：10.11975/j.issn.1002-6819.2016.01.006 http://www.tcsae.org

Liu Jun, Yuan Jun, Cai Junyu, Tao Changling, Wang Liming, Cheng Wei.Autopilot system of agricultural vehicles based on GPS/INS and steer-by-wire[J].Transactions of the Chinese Society of Agricultural Engineering（Transactions of the CSAE）, 2016, 32（01）: 46-53.（in Chinese with English abstract）doi：10.11975/j.issn.1002-6819.2016.01.006 http://www.tcsae.org

0　引言

農(nóng)業(yè)是一個(gè)國家經(jīng)濟(jì)發(fā)展水平的標(biāo)志，我國一直在尋找改善農(nóng)業(yè)生產(chǎn)的方法，促進(jìn)我國國民經(jīng)濟(jì)的發(fā)展[1]。目前,歐洲、美國和日本等一些國家較早地對農(nóng)用車輛路徑跟蹤控制技術(shù)進(jìn)行了研究，尤其是在導(dǎo)航控制、導(dǎo)航定位等方面,但這些研究主要集中在單一的GPS或慣性導(dǎo)航技術(shù)在車輛導(dǎo)航中的應(yīng)用上[2-4]。GPS導(dǎo)航系統(tǒng)有全天候作業(yè)、定位精度高、速度快等優(yōu)點(diǎn)，但也存在其固有的缺點(diǎn)，如復(fù)雜環(huán)境可靠性差，數(shù)據(jù)輸出頻率低，信號抗干擾能力差等；慣性導(dǎo)航的信息連續(xù)性好、噪聲低、不易受外界干擾，短期精度和穩(wěn)定性好，但該系統(tǒng)存在累積誤差，定位精度會(huì)隨時(shí)間增長而降低[5-7]。本文主要針對車載導(dǎo)航控制系統(tǒng)進(jìn)行研究,開發(fā)出了一套基于GPS/INS（global positioning system/ inertial navigation system）組合的車載導(dǎo)航控制系統(tǒng)，可以實(shí)現(xiàn)全天候、高精度、高可靠性和低成本的車載導(dǎo)航。

1　系統(tǒng)工作原理

本文設(shè)計(jì)的農(nóng)用車自動(dòng)駕駛系統(tǒng)取消了傳統(tǒng)的轉(zhuǎn)向執(zhí)行機(jī)構(gòu)，采用電子控制單元代替方向盤和轉(zhuǎn)向輪之間的機(jī)械連接，擺脫了機(jī)械轉(zhuǎn)向系統(tǒng)的局限性[8]。將捷聯(lián)式慣性導(dǎo)航和GPS導(dǎo)航以松耦合的方式組合在一起，同時(shí)采用卡爾曼濾波技術(shù)對INS和GPS數(shù)據(jù)進(jìn)行融合，從而獲得優(yōu)于單一子系統(tǒng)的位置、速度和姿態(tài)信息[9-11]。上位機(jī)將位置、速度、姿態(tài)等信息通過CAN（controller area network）總線發(fā)送給主控制器。主控制器根據(jù)實(shí)時(shí)接收的各類信號，對車輛的行駛姿態(tài)進(jìn)行識別，并根據(jù)相應(yīng)的路徑跟蹤控制策略計(jì)算出目標(biāo)前輪轉(zhuǎn)角，控制轉(zhuǎn)向執(zhí)行電機(jī)使其快速、準(zhǔn)確地達(dá)到目標(biāo)轉(zhuǎn)角。最終完成車輛的跟蹤定位，實(shí)現(xiàn)了農(nóng)用車自動(dòng)駕駛功能。

2　系統(tǒng)硬件設(shè)計(jì)

本文所設(shè)計(jì)的自動(dòng)駕駛系統(tǒng)主要包括了以下子系統(tǒng)：組合導(dǎo)航系統(tǒng)、路徑跟蹤系統(tǒng)、轉(zhuǎn)向執(zhí)行系統(tǒng)。其中組合導(dǎo)航系統(tǒng)實(shí)時(shí)獲得車輛運(yùn)行狀態(tài)、位置信息；路徑跟蹤系統(tǒng)根據(jù)載體運(yùn)行參數(shù)確定載體目標(biāo)前輪轉(zhuǎn)角；主控制器根據(jù)載體目標(biāo)前輪轉(zhuǎn)角與當(dāng)前前輪轉(zhuǎn)角的差值實(shí)現(xiàn)對轉(zhuǎn)向執(zhí)行電機(jī)的精確控制。整個(gè)系統(tǒng)硬件架構(gòu)如圖1所示：

2.1車載感知單元硬件設(shè)計(jì)

根據(jù)車載感知單元的測量精度、系統(tǒng)體積及功耗等因素要求，最終選擇的傳感器模塊為運(yùn)動(dòng)處理組件MPU-6050，它集成有3軸陀螺儀與加速度計(jì)[12]；選擇的微處理器模塊為STM32F103T8U6。

MPU-6050傳感器模塊采集的信號先經(jīng)過調(diào)理電路進(jìn)行處理，然后通過微處理模塊的A/D轉(zhuǎn)換實(shí)現(xiàn)對傳感器信號的采集?？紤]到信號的抗干擾性及對芯片的限壓保護(hù)，所以在調(diào)理電路的設(shè)計(jì)中加入了RC（resistancecapacitance circuits）濾波、電壓調(diào)節(jié)、限壓保護(hù)等功能。微處理器根據(jù)姿態(tài)解算算法對傳感器模塊采集的數(shù)據(jù)進(jìn)行處理，獲得最優(yōu)姿態(tài)角估計(jì)值，并將其以串口的形式輸出。其實(shí)物如圖2所示：

圖1　系統(tǒng)硬件架構(gòu)Fig.1 Overall system hardware design

圖2　車載狀態(tài)感知單元Fig.2 Vehicle perception unit

2.2GPS模塊

根據(jù)差分GPS定位原理：由基準(zhǔn)站已知精密坐標(biāo)，計(jì)算出基準(zhǔn)站到衛(wèi)星的距離改正數(shù)，并由基準(zhǔn)站實(shí)時(shí)地將這一改正數(shù)發(fā)送給流動(dòng)站。流動(dòng)站不但接收GPS信號，同時(shí)也接收基準(zhǔn)站的改正數(shù)，并對其定位結(jié)果進(jìn)行改正，提高定位精度。所以本系統(tǒng)采用兩組GPS模塊，一組作為基準(zhǔn)站，一組作為流動(dòng)站。鑒于流動(dòng)站需要隨車實(shí)時(shí)采集數(shù)據(jù)，所以兩組GPS模塊之間選擇通過ZigBee實(shí)現(xiàn)無線傳輸。本文所采用GPS模塊為BD-126模塊，GPS模塊和ZigBee模塊均采用5V直流電源進(jìn)行供電。其實(shí)物如下圖3所示：

圖3　GPS模塊Fig.3 GPS Modules

2.3主控制器設(shè)計(jì)

主控制器需要采集傳感器信號來實(shí)現(xiàn)對直流無刷電機(jī)驅(qū)動(dòng)電路的控制，從而控制轉(zhuǎn)向執(zhí)行電機(jī)。根據(jù)系統(tǒng)的實(shí)際需要，本文基于飛思卡爾MC9S12XET256芯片設(shè)計(jì)了轉(zhuǎn)向系統(tǒng)的電控單元[13]，主要包括：單片機(jī)最小系統(tǒng)、直流無刷電機(jī)驅(qū)動(dòng)電路、濾波電路、CAN總線通訊電路、串口通訊模塊等。

本文所設(shè)計(jì)的主控制器電源模塊采用SCM6716芯片，其具有電源DC-DC功能，將車載24V電壓轉(zhuǎn)換為5V電壓對元器件供電。同時(shí)該芯片具有I/O口驅(qū)動(dòng)，20號引腳用來控制直流無刷電機(jī)的使能、21號引腳用來控制剎車、19號引腳控制電機(jī)轉(zhuǎn)向。直流無刷電機(jī)的調(diào)速則由主控制器輸出的PWM（pulse width modulation）波經(jīng)過兩路的D/A調(diào)節(jié)電路實(shí)現(xiàn),根據(jù)PWM的占空比控制電壓以達(dá)到直流無刷電機(jī)調(diào)速的目的。

信號輸入濾波電路主要是對轉(zhuǎn)向執(zhí)行電機(jī)的電流傳感器信號進(jìn)行調(diào)理，電流傳感器采集的信號先經(jīng)過濾波電路處理將電壓控制在0～5 V的范圍內(nèi)，然后通過控制芯片A/D轉(zhuǎn)換實(shí)現(xiàn)對傳感器信號的采集。調(diào)理電路的設(shè)計(jì)需要考慮信號的抗干擾及對芯片的限壓保護(hù)，所以在電路中加入了RC濾波、電壓調(diào)節(jié)、限壓保護(hù)等功能。

本文設(shè)計(jì)了兩路CAN總線，車載感知單元采集的車輛運(yùn)行狀態(tài)參數(shù)、位置參數(shù)等經(jīng)過上位機(jī)軟件處理后，通過串口轉(zhuǎn)CAN模塊發(fā)送給線控轉(zhuǎn)向主控制器。CAN控制器集成在主控芯片內(nèi)，CAN收發(fā)器選擇TJA1050，電路中加入了ESD（Electro-Static discharge）保護(hù)，提高了CAN總線的EMC（electro magnetic compatibility）性能。

主控制器增加了串口通訊模塊，用來與上位機(jī)進(jìn)行相關(guān)的數(shù)據(jù)傳輸。主控制器設(shè)計(jì)完成后，對其進(jìn)行相應(yīng)的硬件在環(huán)試驗(yàn)，測試軟件的可行性。需要監(jiān)測的數(shù)據(jù)通過從單片機(jī)的TXD0和RXD0引腳輸出，通過RS232串口通訊方式傳輸給上位機(jī)的LABVIEW檢測程序中，經(jīng)過相應(yīng)的分析和處理，把所需要的信息以數(shù)字和波形曲線實(shí)時(shí)顯示出來。主控制器實(shí)物如圖4所示：

圖4　線控轉(zhuǎn)向控制器實(shí)物圖Fig.4 Controller of SBW

3　系統(tǒng)控制策略及算法

整個(gè)自動(dòng)駕駛系統(tǒng)的控制策略和算法比較復(fù)雜，從控制功能的實(shí)現(xiàn)上分為組合導(dǎo)航算法、路徑跟蹤控制及轉(zhuǎn)向執(zhí)行電機(jī)控制策略。系統(tǒng)整體控制框圖如圖5所示：

根據(jù)車輛當(dāng)前位置坐標(biāo)y與規(guī)劃路徑坐標(biāo)ym計(jì)算出車輛橫向偏差Δy，以Δy和車輛航向角θ為輸入變量，利用控制器決策出車輛目標(biāo)航向角θm；結(jié)合車輛當(dāng)前航向角θ計(jì)算航向角偏差Δθ，以Δθ和載體當(dāng)前輪轉(zhuǎn)角α為輸入變量，控制器決策出電機(jī)的目標(biāo)轉(zhuǎn)角ωm；采用雙閉環(huán)控制方法控制電機(jī)轉(zhuǎn)動(dòng)，使電機(jī)目標(biāo)轉(zhuǎn)角偏差最小化。

圖5　系統(tǒng)控制框圖Fig.5 Control block diagram of system

3.1組合導(dǎo)航算法

本文根據(jù)物理組成和功能將組合系統(tǒng)劃分為三個(gè)模塊：GPS接收機(jī)模塊、INS模塊和組合導(dǎo)航計(jì)算模塊。本文采用松耦合方式，將GPS和INS各自輸出的位置估值和速度估值進(jìn)行比較，得到的差值作為卡爾曼濾波器的測量輸入值，對慣導(dǎo)系統(tǒng)提供測量更新，以提高慣性導(dǎo)航的精度。位置速度信息的組合原理框圖如圖6所示：

圖6　組合導(dǎo)航系統(tǒng)設(shè)計(jì)方案Fig.6 Integrated navigation system design

3.1.1車輛姿態(tài)解算方法設(shè)計(jì)

本文利用慣性傳感器測量汽車的6自由度狀態(tài)參數(shù)，采用四元數(shù)法對車輛的姿態(tài)角進(jìn)行捷聯(lián)解算，再通過卡爾曼濾波器對姿態(tài)角進(jìn)行最優(yōu)估計(jì)[14]，減小誤差，其流程如圖7所示。

圖7　姿態(tài)解算流程圖Fig.7 Vehicle attitude angle measurement flow chart

四元數(shù)是一個(gè)由四個(gè)元構(gòu)成的數(shù)，首先建立系統(tǒng)四元數(shù)的微分方程，通過四階的Runge-Kutta法求解q0，q1，q2，q3，再將其帶入車輛姿態(tài)矩陣（1），方程解如下：

其中，q0，q1，q2，q3均為實(shí)數(shù)標(biāo)量。

（1）式即為車輛四元數(shù)姿態(tài)矩陣，將其與車輛的歐拉角姿態(tài)矩陣進(jìn)行對比，可得到姿態(tài)角與四元數(shù)之間的轉(zhuǎn)換關(guān)系。

式中φ為橫擺角、γ為側(cè)傾角、θ為俯仰角，其單位為rad。

所以汽車姿態(tài)角：橫擺角φ、側(cè)傾角γ、俯仰角θ分別為：

3.1.2捷聯(lián)慣導(dǎo)算法設(shè)計(jì)

導(dǎo)航坐標(biāo)系下車輛速度的微分方程為：

根據(jù)式（7）計(jì)算出地理坐標(biāo)系中的經(jīng)緯度位置

式中RN、RM為地球半徑，L為前緯度，h為高度。

3.1.3差分GPS算法設(shè)計(jì)

根據(jù)GPS動(dòng)態(tài)定位法，選擇單差定位模型。在多臺接收機(jī)同時(shí)觀測的情況下，對于同一顆衛(wèi)星，可以同時(shí)列出多個(gè)觀測方程，取其中一個(gè)觀測方程作為基準(zhǔn)，其他方程與之相減。基準(zhǔn)站的空間坐標(biāo)為己知，由此解得流動(dòng)站的空間坐標(biāo)[16]?，F(xiàn)考慮一個(gè)基站一個(gè)流動(dòng)站的動(dòng)態(tài)定位情況，假設(shè)a點(diǎn)為基準(zhǔn)站，b點(diǎn)為流動(dòng)站，GPS接收機(jī)在兩個(gè)不同的地點(diǎn)a，b的誤差為：

當(dāng)a，b處的接收機(jī)選取同一組星座時(shí)，ΔSa=ΔSa=ΔS，位置差分的定位誤差為：

位置誤差可以分成隨機(jī)誤差V和系統(tǒng)誤差B，其中地面多徑效應(yīng)、GPS通道誤差、接收機(jī)噪聲是隨機(jī)誤差的主要原因，對流層

在a，b兩處分別設(shè)置GPS站點(diǎn)，有：

將（13）式帶入（12）式，可以得到：

可以認(rèn)為，Va、Vb是相互獨(dú)立的，并且E{V1}=E{V2}=0。Ba，Bb有很多相同的元素，令：

由（14）式可算出P，其中定位誤差的RMS值是P的前三個(gè)對角線元素之和開方，即：

由（15）式可以看出，由B引起的定位誤差只剩下了二階小量，因此可以認(rèn)為大部分系統(tǒng)誤差被抵消。由V引起的誤差增大了，但V引起的誤差所占成分較小，綜上可以認(rèn)為差分后定位誤差可以大幅度降低。

3.2路徑跟蹤控制算法設(shè)計(jì)

使車輛貼合既定軌跡行走的反饋控制是路徑跟蹤的重要目的[17]，本文設(shè)計(jì)的控制方法是基于車輛的未來位置和目標(biāo)位置的偏差值，當(dāng)車輛行駛的目標(biāo)路徑已知時(shí)，利用預(yù)測控制的理論得到目標(biāo)前輪轉(zhuǎn)角，從而實(shí)現(xiàn)路徑跟蹤控制功能。

3.2.1預(yù)見點(diǎn)Q以及目標(biāo)點(diǎn)P

路徑跟蹤功能的實(shí)現(xiàn)要求預(yù)見點(diǎn)和給定路徑盡量重合。本文中，將點(diǎn)定義成預(yù)見點(diǎn)，也就是經(jīng)過時(shí)間τ后到達(dá)的位置并且車輛始終保持當(dāng)前的轉(zhuǎn)向角，設(shè)xQ=[xQ，yQ，θQ，αQ]T是預(yù)見點(diǎn)的狀態(tài)向量，可由（16）式求出，其中x，y，θ，α為當(dāng)前車輛的狀態(tài)值。

根據(jù)求出的Q點(diǎn)的坐標(biāo)，利用二分法求得目標(biāo)路徑上距離Q點(diǎn)最近的點(diǎn)P的狀態(tài)量xp=[xp，yp，θp，αp]。

3.2.2P點(diǎn)附近的車輛運(yùn)動(dòng)方程

預(yù)見點(diǎn)以及目標(biāo)點(diǎn)的狀態(tài)向量分別為：

上式中δx=[δx，δy，δθ，δα]T為更新后的狀態(tài)向量，δu=δα為更新后的控制向量。A（t），b分別由下式表示：

3.2.3前輪轉(zhuǎn)角算法設(shè)計(jì)

當(dāng)式（18）中的狀態(tài)量θP=0或時(shí)，系統(tǒng)將不能控，所以將速度變化量δV=V-看成新的控制量引入以避免不能控的情況，擴(kuò)張后的車輛運(yùn)動(dòng)方程式為：=A（t）δx+ B（t）u。式中：

根據(jù)無限時(shí)間調(diào)節(jié)器的概念設(shè)計(jì)導(dǎo)航控制器的時(shí)候，目標(biāo)函數(shù)表達(dá)式見（22）式：

路徑跟蹤控制的關(guān)鍵在于x，y，θ的精度跟蹤，假設(shè)農(nóng)用車輛為勻速行駛，可以對Q、R做出以下設(shè)定，消除行駛速度的影響。

由黎卡提代數(shù)方程：

可以得到的反饋增益矩陣為：

由于所求得的Kv會(huì)很小，并且本文中的控制變量不包括行駛速度，所以在實(shí)際情況下的控制器就如（25）式所示：

將計(jì)算出的轉(zhuǎn)角修正量與P點(diǎn)的轉(zhuǎn)向角相加，得到最終轉(zhuǎn)向角如下：

4　系統(tǒng)軟件設(shè)計(jì)

本文根據(jù)系統(tǒng)的功能對系統(tǒng)軟件進(jìn)行設(shè)計(jì)，主要包括組合導(dǎo)航系統(tǒng)軟件設(shè)計(jì)、主控制器軟件設(shè)計(jì)。

4.1組合導(dǎo)航系統(tǒng)軟件設(shè)計(jì)

組合導(dǎo)航定位系統(tǒng)主要為車輛導(dǎo)航控制提供準(zhǔn)確的位姿信息，如車輛位置坐標(biāo)、航向角、行駛速度等，本文在目標(biāo)試驗(yàn)車輛上分別安裝GPS傳感器、慣性傳感器，將傳感器檢測到的車輛狀態(tài)信息通過一定的接口輸入到計(jì)算機(jī)內(nèi)，利用計(jì)算機(jī)軟件計(jì)算車輛的位置坐標(biāo)以完成精確導(dǎo)航定位。圖8所示為組合導(dǎo)航系統(tǒng)軟件設(shè)計(jì)的流程：

圖8　組合導(dǎo)航系統(tǒng)軟件設(shè)計(jì)的流程Fig.8 Overall process of software design

本文以Visual Studio2010為工具，根據(jù)組合導(dǎo)航的系統(tǒng)功能自主開發(fā)了組合導(dǎo)航系統(tǒng)軟件，通過該軟件采集車載感知系統(tǒng)數(shù)據(jù)和GPS數(shù)據(jù)，并計(jì)算出載體姿態(tài)解算值[18]。其軟件如圖9所示：

圖9　組合導(dǎo)航系統(tǒng)軟件Fig.9 Vehicle navigation system interface

4.2主控制器軟件設(shè)計(jì)

本文選用直流無刷電機(jī)來實(shí)現(xiàn)車輛的轉(zhuǎn)向控制，對于直流無刷電機(jī)采用雙閉環(huán)控制[19]，其中轉(zhuǎn)角環(huán)為滑模變結(jié)構(gòu)控制，電流環(huán)為PID控制。轉(zhuǎn)向執(zhí)行電機(jī)的控制框圖如圖10所示。

圖10　轉(zhuǎn)向執(zhí)行電機(jī)控制框圖Fig.10 Control block diagram of BLDCM

由主控制器的功能和上述控制框圖可知，主控制器主要實(shí)現(xiàn)路徑跟蹤控制和對轉(zhuǎn)向執(zhí)行電機(jī)的控制，因此需要實(shí)時(shí)接收電流傳感器、轉(zhuǎn)角傳感器的數(shù)據(jù)，同時(shí)需要實(shí)時(shí)接收CAN總線發(fā)送的載體姿態(tài)信息，并根據(jù)接收到的各類數(shù)據(jù)計(jì)算出載體目標(biāo)前輪轉(zhuǎn)角，根據(jù)目標(biāo)前輪轉(zhuǎn)角、當(dāng)前前輪轉(zhuǎn)角及傳動(dòng)比最終確定轉(zhuǎn)向執(zhí)行電機(jī)的轉(zhuǎn)角。其軟件設(shè)計(jì)方案如圖11：

圖11　轉(zhuǎn)向系統(tǒng)軟件設(shè)計(jì)方案圖Fig.11 Designing scheme of the program of ECU

5　試驗(yàn)驗(yàn)證及仿真

本文根據(jù)車輛運(yùn)動(dòng)學(xué)方程對車輛導(dǎo)航控制方法以及所設(shè)計(jì)的路徑跟蹤控制器進(jìn)行有效性驗(yàn)證，在Matlab環(huán)境下對車輛導(dǎo)航控制進(jìn)行了仿真研究。同時(shí)，建立直流無刷電機(jī)的Simulink模型，對其控制策略進(jìn)行仿真研究[20]。5.1路徑跟蹤控制仿真

利用車輛運(yùn)動(dòng)學(xué)模型和所設(shè)計(jì)的導(dǎo)航控制器，在Matlab里面編寫了相應(yīng)的M文件，設(shè)計(jì)一條曲線參考路徑y(tǒng)=2*sin（x/3），車輛速度分別取0.5、1 m/s進(jìn)行仿真，設(shè)置車輛初始位置偏差為0，跟蹤效果如圖11所示。航向角的偏差變化曲線如圖12所示。最大橫向誤差在兩種速度下分別為0.16、0.27 m。

圖11　目標(biāo)路徑及行駛軌跡Fig.11 The target path and trajectory

圖12　航向偏差Fig.12 Lateral deviation

5.2轉(zhuǎn)向執(zhí)行電機(jī)控制策略仿真

文建立直流無刷電機(jī)的Simulink模型，對其控制策略進(jìn)行仿真研究。分別對直流無刷電機(jī)以普通PID控制和滑模變結(jié)構(gòu)控制，將兩種控制策略結(jié)果進(jìn)行對比。給直流無刷電機(jī)以48°角輸入，觀察仿真時(shí)電機(jī)的響應(yīng)曲線。在0.5 s時(shí)給直流無刷電機(jī)一定常值干擾下，觀察PID控制和滑模變結(jié)構(gòu)控制的響應(yīng)曲線。仿真結(jié)果如圖13所示：

圖13　值擾動(dòng)下電機(jī)轉(zhuǎn)角響應(yīng)曲線圖Fig.13 Changing curve of motor angle under constant disturbance

由圖13可知滑模變結(jié)構(gòu)控制能夠使電機(jī)快速到達(dá)目標(biāo)穩(wěn)定值，而PID控制存在著一定的超調(diào)，所以滑模變結(jié)構(gòu)控制相對于PID控制響應(yīng)速度快、超調(diào)量小。當(dāng)在0.5 s時(shí)給電機(jī)加入一定的定值干擾時(shí)，滑?？刂茖Ω蓴_免疫力強(qiáng)，幾乎不受干擾影響，而PID控制對干擾比較敏感。所以可以看出滑模變結(jié)構(gòu)控制較PID控制抗干擾能力強(qiáng)。

5.3GPS/INS導(dǎo)航驗(yàn)證試驗(yàn)

基準(zhǔn)站與流動(dòng)站安裝完成后，通過車載電源轉(zhuǎn)換裝置為GPS接收機(jī)與慣性感知單元進(jìn)行供電。通過RS232串口通信將慣性感知單元和GPS接收機(jī)輸出的數(shù)據(jù)同時(shí)輸入計(jì)算機(jī)，利用自主設(shè)計(jì)的組合導(dǎo)航軟件將實(shí)驗(yàn)數(shù)據(jù)進(jìn)行處理并存儲(chǔ)。所有設(shè)備安裝好后，預(yù)熱10 min。開始進(jìn)行實(shí)驗(yàn)，在進(jìn)行初始對準(zhǔn)結(jié)束后，車輛開始運(yùn)動(dòng)，利用編寫好的軟件采集組合導(dǎo)航系統(tǒng)輸出的數(shù)據(jù)和GPS的輸出數(shù)據(jù)，并對其進(jìn)行處理。

圖14　位置信息對比Fig.14 Location information comparison

圖15　試驗(yàn)車輛平臺的搭建圖Fig.15 Software program flow chart

由于存在連續(xù)的GPS速度和位置信息的輸出，慣性傳感器的誤差以及INS的導(dǎo)航參數(shù)都被5 Hz的GPS輸出進(jìn)行了校正，從而阻止了INS的定位誤差隨時(shí)間長期積累。從圖14可以看出，組合導(dǎo)航系統(tǒng)解算得到的載體運(yùn)動(dòng)軌跡可以很好的跟蹤上載體真實(shí)的運(yùn)動(dòng)軌跡。組合導(dǎo)航系統(tǒng)最后的導(dǎo)航定位精度在0.1～0.5 m左右。

5.4轉(zhuǎn)向控制試驗(yàn)

由于條件限制，轉(zhuǎn)向?qū)嶒?yàn)不能在實(shí)車上完成。所以設(shè)計(jì)了轉(zhuǎn)向?qū)嶒?yàn)臺，完成相應(yīng)的硬件在環(huán)實(shí)驗(yàn)。本文將路徑跟蹤系統(tǒng)所得到的目標(biāo)前輪轉(zhuǎn)角以數(shù)據(jù)文件的格式通過串口輸入到轉(zhuǎn)向系統(tǒng)ECU中。通過上位機(jī)監(jiān)控程序進(jìn)行監(jiān)測，主要為前輪轉(zhuǎn)角監(jiān)測模塊。實(shí)驗(yàn)平臺硬件搭建如圖16：

圖16　線控轉(zhuǎn)向?qū)嶒?yàn)圖Fig.16 SBW test bench test

試驗(yàn)過程中觀察結(jié)果，電機(jī)響應(yīng)目標(biāo)前輪轉(zhuǎn)角輸入如圖17所示，結(jié)果表明試驗(yàn)臺架轉(zhuǎn)向跟隨效果較好，證明該轉(zhuǎn)向系統(tǒng)性能較好，未來改裝到車輛上進(jìn)行轉(zhuǎn)向控制是可行的。

圖17　目標(biāo)前輪轉(zhuǎn)角與臺架電機(jī)的前輪轉(zhuǎn)角Fig.17 The front wheel steering angle and front wheel corner bench machine

6　結(jié)論

本文自主設(shè)計(jì)了農(nóng)業(yè)機(jī)械自動(dòng)駕駛系統(tǒng)，包括GPS/ INS（global positioning system/inertial navigation system）組合導(dǎo)航系統(tǒng)、路徑跟蹤系統(tǒng)、轉(zhuǎn)向執(zhí)行系統(tǒng)。所設(shè)計(jì)的組合導(dǎo)航系統(tǒng)能獲得較精確的農(nóng)用車輛導(dǎo)航信息；主控制器實(shí)時(shí)精確控制轉(zhuǎn)向執(zhí)行電機(jī)，實(shí)現(xiàn)車輛的自動(dòng)駕駛功能。

本文通過對農(nóng)用車輛的路徑跟蹤控制系統(tǒng)進(jìn)行了研究，開發(fā)了一套組合導(dǎo)航跟蹤定位系統(tǒng),可實(shí)時(shí)獲取車輛的精確位置，并實(shí)現(xiàn)車輛對規(guī)定路徑的跟蹤；搭建了線控轉(zhuǎn)向試驗(yàn)臺，完成了主控制器的軟硬件設(shè)計(jì)，并對相關(guān)控制算法進(jìn)行了聯(lián)合仿真和硬件在環(huán)實(shí)驗(yàn)驗(yàn)證，最終驗(yàn)證了整個(gè)系統(tǒng)響應(yīng)速度快，達(dá)到0.1 s；超調(diào)量小、穩(wěn)定性好；組合導(dǎo)航系統(tǒng)定位精度高，定位精度在0.1～0.5 m左右；整個(gè)系統(tǒng)控制策略可行，抗干擾能力強(qiáng)。

[參考文獻(xiàn)]

[1]殷春燕．淺談農(nóng)業(yè)機(jī)械自動(dòng)化[J]．西部大開發(fā)旬刊，2010，（6）：188-188．Yin Chunyan.Introduction of agricultural machinery automation [J].West China Development,2010,（6）:188-188.

[2] Torii T．Research in autonomous agriculture vehicles in Japan [J]．Computers & Electronics in Agriculture, 2000, 25（99）:133-153.

[3] Reid J F，Zhang Q, Noguchi N, et al．Agricultural Automatic Guidance Research In North America[J]．Computers & Electronics in Agriculture，2000，25（1）：155-167．

[4] Keicher R，Seufert H．Automatic guidance for agricultural vehicles in Europe[J]．Computers & Electronics in Agriculture，2000，25（99）：169-194．

[5]黎永鍵，趙祚喜.基于GPS_SINS組合的農(nóng)業(yè)導(dǎo)航定位系統(tǒng)設(shè)計(jì)與研究[D].農(nóng)機(jī)化研究，2014，（3）: 16-22.Li Yongjian, Zhao Zuoxi.The design and research of agricultural navigation and positioning system based on GPS/SINS [D].Agricultural Mechanization Research, 2014,（3）: 16-22.

[6]魏少東.基于GPS和慣性導(dǎo)航的果園機(jī)械導(dǎo)航系統(tǒng)研究[D].西北農(nóng)林科技大學(xué)，2013.Wei Shaodong.Research on orchard mechanical navigation system based on GPS and inertial navigation[D].Northwest Agriculture and Forestry University, 2013.

[7]龔毅．一種無人駕駛車輛路徑跟蹤控制方式研究[D]．南京理工大學(xué)，2014．Gong Yi.An unmanned vehicle path following control method research[D].Nanjing university of science and technology, 2014.

[8]劉軍，穆桂脂，羅石，等.線控轉(zhuǎn)向硬件在環(huán)系統(tǒng)設(shè)計(jì)[J].機(jī)械設(shè)計(jì)與制造, 2009,（3）:232-234.Liu Jun, Mu Guizhi, Luo Shi.Wire control steering hardware in loop system design[J].Machinery Design and Manufacture, 2009, （3）: 232-234.

[9] Hoffman K，Varuso R，F(xiàn)ratta D．The Use of Low-Cost MEMS Accelerometers for the Near-Surface Monitoring of Geotechnical Engineering Systems[J]．American Society of Civil Engineers，2014，（187）．

[10]陳樹人，尹建軍．GPS技術(shù)及其在農(nóng)業(yè)工程中的應(yīng)用[J]．排灌機(jī)械工程學(xué)報(bào)，2003，21（5）：40-42．Chen Shuren, Yi Jianjun.GPS technology and its application in agricultural engineering[J].Journal of Drainage and Irrigation Machinery Engineering, 2003, 21（5）: 40-42.

[11]李富榮，秦永元．松散組合導(dǎo)航系統(tǒng)中慣導(dǎo)和GPS的信號同步[C]．中國航空學(xué)會(huì)控制與應(yīng)用學(xué)術(shù)年會(huì)，1998．Li Furong, Qin Yongyuan.Loosely integrated navigation system of INS and GPS signal synchronization[C].China aviation learn to control and application of academic annual meeting, 1998.

[12]劉軍，石存杰，余節(jié)發(fā)．車載感知與智能終端的硬件設(shè)計(jì)[J]．微型機(jī)與應(yīng)用，2012，（3）：31-33．Liu Jun, Shi Cunjie, Yu Jiefa.The design of intelligent terminal hardware and perception on vehicle[J].Microcomputer & Its Applications, 2012,（3）: 31-33.

[13] MC9S12XET256開發(fā)平臺實(shí)驗(yàn)指導(dǎo)手冊.http://fxfreefly.taobao.com.MC9S12XET256 development platform experiment instruction manual.http://fxfreefly.taobao.com.

[14]羅建軍．組合導(dǎo)航原理與應(yīng)用[M]．西安：西北工業(yè)大學(xué)出版社，2012, 4：115-118.Luo Jianjun.Principle and application of integrated navigation [M].Xi’an: Northwestern Polytechnic University Press, 2012, 4: 115-118.

[15]劉華迪．基于MEMS慣性組件的INS/GPS組合導(dǎo)航定位定姿的研究[D]．沈陽理工大學(xué)，2013．Liu Huadi.Research on INS/GPS integrated navigation and positioning based on MEMS inertial components[D].ShenYang Ligong University, 2013．

[16]徐周．GPS差分定位技術(shù)及實(shí)現(xiàn)方法的研究[D]．中國人民解放軍信息工程大學(xué)，2006．Xu Zhou.The research of differential GPS positioning technology and implementation method[D].The PLA InformationEngineering University,2006.

[17] Harada H.Control strategy of active rear wheel steering in consideration of system delay and dead times[J].JSAE Review, 1995, 16（2）:171-177.

[18]隗燕琳，陳進(jìn)明．基于VB與Matlab的混合編程方法[J]．計(jì)算機(jī)與數(shù)字工程，2013，41（8）：1388-1390．Wei Yanlin, Chen Jinming.The mixed programming method based on VB and Matlab[J].Computer & Digital Engineering, 2013, 41（8）: 1388-1390.

[19]羅石.線控轉(zhuǎn)向系統(tǒng)主動(dòng)安全預(yù)測控制策略的研究[D].鎮(zhèn)江：江蘇大學(xué)博士論文，2010.Luo Shi.The control strategy of active safety prediction research of SBW system[D].ZhenJiang: Jiangsu university doctoral thesis, 2010.

[20]李穎，朱伯立，張威．Simulink動(dòng)態(tài)系統(tǒng)建模與仿真基礎(chǔ)[M]．西安：西安電子科技大學(xué)出版社，2004．LiYing, Zhu Boli, ZhangWei.The model of dynamic system and basic simulation[M].Xi′an: xi′an electronic science and technology university press, 2004.

Autopilotsystemofagriculturalvehiclesbasedon GPS/INSandsteer-by-wire

Liu Jun, Yuan Jun, Cai Junyu, Tao Changling, Wang Liming, Cheng Wei
（Jiang Su University School of Automobile and Traffic Engineering, Zhenjiang Jiangsu 212013, China）

Abstract:The application of the path tracking technology on agricultural vehicle makes the robot replace farmers for field operation, and the accuracy, production efficiency and dependability about farming automation are improved effectively.Meanwhile the labor time, labor intensity of drivers and the production cost are saved.The path tracking of agricultural vehicle was studied in an operating condition, and a variety of sensors were installed on the car features of the external environment.Then it generated a four-element in state space by the target path which was given for controling the agricultural vehicles to track the target path automaticly according to the theory of optimal navigation control.The location method of GPS/INS was selected, the navigation system was developed, and the experiment was finished in 2014.According to the needs of automatic walking positioning system, a variety of sensors has been chosen, including the inertial sensor, angle sensor and GPS sensor.Then the serial program was writen to collect the signal from the sensors and calibrate them.In order to meet the requirements of vehicle navigation system, the positioning system with low cost and high precision was developed.The hardware of the system consisted of two GPS modules, two Zigbee wireless transmission modules and an inertial sensor.The data from the sensors are filtered and fused, and finally accurate, reliable vehicle position data was got.The tracking controller based on preview control was designed to obtain the future values and target values of the vehicle.With the target path and its curvature, the feed forward control value was got.There was an error between the current state and the state of vehicle target path that was needed to use LQR for elimination.The performance of the path following controller was simulated by Matlab, then the maximum lateral error was 0.16 m and 0.27 m at the speed of 0.5 m/s and 1 m/s respectively.The results showed that the control method was feasible.The steering control system was designed based on steer-by-wire（SBW）after the study of vehicle navigation control principle.SBW removed out mechanical connection between steering wheel and steering front wheel.It used motors to control front wheel angle and simulated force characteristic.Compared with traditional steering system, SBW had characteristics of ideal steering ratio and active steering control according to vehicle state parameters, and improved safety of driving and handling stability.The strategy of BLDCM was designed in order to make actual front angle follow the desired angle better.PID control and sliding mode variable structure control were applied in strategy of BLDCM and the result of simulation showed that sliding mode variable structure control was better than PID control.This paper designed the electronic control unit of SBW based on chip of MC9S12XET256, mainly including peripheral circuit of MCU, CAN communication circuit, drive circuit of BLDCM, power circuit, signal acquisition and processing circuit, current sampling circuit of motor.Based on the requirement of joint simulation, we designed a bench test for control strategy and hardware, software of ECU in 2015.The results of test bench showed that angle correction was similar with the result of simulation and sliding mode variable structure control was better than PID control in following front angle.Finally, the vehicle steering control test and the vehicle path tracking control test were carried out based on vehicle test platform, which was built personally.The vehicle path tracking system was based on the Windows platform, using Microsoft Visual Studio as the development environment.The integrated navigation system was validated and the test data showed that the integrated navigation system had a high positioning accuracy and the steering system had a reliable tracking performance.The final navigation and positioning accuracy of integrated navigation system was around 0.1 m to 0.5 m and the response speed of the whole system was about 0.1s .The results proved that the system could meet the requirements of agricultural vehicle path tracking control system.

Keywords:agricultural machinery; robots; control systems; GPS/INS; integrated navigation; tracking control; steer-by-wire

作者簡介：劉軍，教授，主要從事汽車電子研究。鎮(zhèn)江江蘇大學(xué)汽車與交通工程學(xué)院，212013。Email：772595769@qq.com

基金項(xiàng)目：國家自然科學(xué)基金資助項(xiàng)目（51275212）

收稿日期：2015-07-08

修訂日期：2015-11-30

中圖分類號：U260.38

文獻(xiàn)標(biāo)志碼：A

文章編號：1002-6819（2016）-01-0046-08

doi：10.11975/j.issn.1002-6819.2016.01.006