李 達(dá)，楊光友，，李 軍，謝真良

（1湖北工業(yè)大學(xué)機(jī)械工程學(xué)院，湖北 武漢430068；2湖北省農(nóng)業(yè)機(jī)械研究設(shè)計院，湖北 武漢430068）

農(nóng)業(yè)大面積、高效率種植離不開智能化機(jī)械，用信息化技術(shù)提升農(nóng)業(yè)生產(chǎn)效率是精準(zhǔn)農(nóng)業(yè)發(fā)展迫切需要研究的課題，而農(nóng)業(yè)機(jī)械導(dǎo)航技術(shù)［1］是精細(xì)農(nóng)業(yè)的基礎(chǔ)，也是當(dāng)前農(nóng)業(yè)工程領(lǐng)域的研究熱點(diǎn)之一。相較于復(fù)雜的道路環(huán)境，農(nóng)業(yè)機(jī)械對安全性要求不那么高，引入無人駕駛智能設(shè)備，能大大提高生產(chǎn)效率并減輕工人勞動強(qiáng)度。而GPS自動導(dǎo)航系統(tǒng)設(shè)備裝卸簡便，作業(yè)精準(zhǔn)，操作簡單，系統(tǒng)無車型限制，作業(yè)率可增加23%。但是，如新疆青河縣阿魏灌區(qū)，雖安裝了GPS自動導(dǎo)航系統(tǒng)的播種機(jī)，但仍需工人駕駛。本文在自動駕駛平臺上引入遠(yuǎn)程監(jiān)控系統(tǒng)，實(shí)現(xiàn)了農(nóng)業(yè)機(jī)械無人駕駛。

1 系統(tǒng)整體設(shè)計方案

本系統(tǒng)融合GPS定位數(shù)據(jù)［2］與傳感器姿態(tài)位置數(shù)據(jù)，通過與電子地圖對比，規(guī)劃出行駛的路徑，并將車輛實(shí)時位置信息通過GPRS傳給上位機(jī)。監(jiān)控中心獲得車輛實(shí)時信息后，遠(yuǎn)程發(fā)送控制命令，就可對偏航車輛進(jìn)行修正。本系統(tǒng)主要包括車載控制器系統(tǒng)和遠(yuǎn)程監(jiān)控系統(tǒng)兩大部分（圖1）。

1.1 車載控制器系統(tǒng)

通過對GPS、GSM（GPRS）、步進(jìn)電機(jī)、超聲波傳感器、舵機(jī)、AHRS（微型姿態(tài)方位參考系統(tǒng)）硬件模塊的研究，以及對模塊功能應(yīng)用程序開發(fā)后發(fā)現(xiàn)，由于單個控制器接口不足，不便于模塊化設(shè)計，可靠性與實(shí)時性差，可采用多個控制器協(xié)同實(shí)現(xiàn)系統(tǒng)功能，各處理器間通過CAN總線［3］發(fā)送數(shù)據(jù)。

1.2 遠(yuǎn)程監(jiān)控系統(tǒng)

在VC＋＋平臺上，通過采用TCP/IP協(xié)議、Access數(shù)據(jù)庫、MSFlexGrid，WebBrowser控件、Google地圖，在監(jiān)控端保存遠(yuǎn)程車輛位置信息，將車輛位置實(shí)時顯示在GIS地圖上，當(dāng)車輛偏航時彈出模態(tài)對話框警示進(jìn)行人為修正。

圖1 車輛導(dǎo)航系統(tǒng)圖

2 下位機(jī)軟件設(shè)計

本文下位機(jī)程序在S3C2440、ARM微處理器WinCE［4］系統(tǒng)平臺下，使用EVC＋＋編程完成相應(yīng)模塊應(yīng)用程序開發(fā)［5］。

2.1 GSM，GPRS通信模塊

車載控制器S3C2440通過串口給GSM/GPRS模塊發(fā)送AT指令，撥號成功后通過TCP/IP協(xié)議建立socket套接字與遠(yuǎn)程具有公網(wǎng)IP或固定IP地址的客戶端連接，進(jìn)行數(shù)據(jù)收發(fā)。

GPRS撥號上網(wǎng)部分代碼:

RASDIALPARMS RasDIALParams；//設(shè)置建立 RAS撥號連接的參數(shù)

RasDial（NULL，NULL，＆RasDialParams，0xFFFFFFFF，NULL，

＆WINCE＿GPRS＿NODE.hRasConn）；//實(shí)現(xiàn)撥號

RasHangUp（WINCE＿GPRS＿NODE.hRasConn）；//斷開網(wǎng)絡(luò)

2.2 GPS定位數(shù)據(jù)的采集與發(fā)送

采用GPS和AHRS組合導(dǎo)航定位系統(tǒng)，與卡爾曼濾波組合，獲得高精度定位信息和姿態(tài)信息。GPS模塊通過串口與S3C2440相連，在WinCE系統(tǒng)下配置串口通訊參數(shù)，實(shí)時接收GPS和AHRS信息。通過定時器周期，從NMEA協(xié)議中提取時間、經(jīng)度、緯度、姿態(tài)方位等信息，經(jīng)socket傳到外網(wǎng)服務(wù)器上。

部分實(shí)現(xiàn)代碼:

c＿Gpsinf＝c＿sPositionX＋"N"＋c＿sPositionY＋"E"＋c＿year＋"Y"＋c＿Date＋"D"＋c＿Posture

//GPS和慣性傳感器信息

int sendlen＝c＿Gpsinf.GetLength（）；//取得輸入字符串長度

char＊psendbuff＝new char［sendlen］；

for（int i＝0；i＜sendlen；i＋＋）

psendbuff［i］＝ （char）c＿Gpsinf.GetAt（i）；//轉(zhuǎn)換為單字節(jié)字符

while（cnt＜sendlen）

｛actlen＝ send（m＿Socket，psendbuff＋ cnt，sendlen －

cnt，0）；//發(fā)送數(shù)據(jù)

cnt＝cnt＋actlen；

if（cnt＜sendlen）

｝

GPS獲得定位數(shù)據(jù)是基于世界大地坐標(biāo)系的觀測值，我國地圖采用高斯－克呂格投影［6］，為了便于在平面坐標(biāo)系下直觀分析車輛位置，把GPS定位數(shù)據(jù)換成我國的點(diǎn)位坐標(biāo)值。

高斯－克呂格投影公式

式中:B，l為大地坐標(biāo)經(jīng)緯度；x，y為轉(zhuǎn)換后的高斯坐標(biāo)系坐標(biāo)值。

3 導(dǎo)航控制策略

3.1 車輛運(yùn)動學(xué)模型建立

車輛運(yùn)動學(xué)模型［7］如圖2所示，xoy坐標(biāo)系為世界坐標(biāo) ，L為軸距，O為后軸中心點(diǎn) ，且以這一點(diǎn)的速度v作為智能車輛的速度，O為車輛轉(zhuǎn)彎圓心，R0為O點(diǎn)的轉(zhuǎn)彎半徑。θo為沿x軸逆時針旋轉(zhuǎn)與車輛速度方向所得到的角度，α為前輪轉(zhuǎn)角。設(shè)O點(diǎn)為車輛的中心點(diǎn)，智能車輛在任意時刻的位置狀態(tài)可用變量（x（i），y（i），θ（i））表示，車輛在平面內(nèi)運(yùn)動且忽略側(cè)滑，則有

其中wo為車輪角速度，在Δt時間周期內(nèi)，有

其中:Δθo為Δt時間內(nèi)航偏角變化量；Δy，Δx分別為在Δt內(nèi)車輛位置在x，y軸上的變化量。由v和α可方便地計算出每個Δt時間后車輛的位置狀態(tài)，則可以通過給定目標(biāo)位置推算轉(zhuǎn)彎角度α。

3.2 目標(biāo)點(diǎn)在車體坐標(biāo)系下的坐標(biāo)

x（i），y（i），θ（i）分別為車輛在圖2平面坐標(biāo)系xoy 下的橫軸，縱軸坐標(biāo)和當(dāng)前航向角。xr，yr，θr為目標(biāo)點(diǎn)在平面坐標(biāo)系中的位置坐標(biāo)，設(shè)xe（i），ye（i）為目標(biāo)點(diǎn)在車體坐標(biāo)系（圖3）。

圖2 車輛運(yùn)動學(xué)模型

圖3 車輛坐標(biāo)系

則目標(biāo)點(diǎn)在車體坐標(biāo)系中位置

通過車輛坐標(biāo)系，可直觀分析目標(biāo)點(diǎn)在車輛坐標(biāo)系中橫向偏差、縱向偏差及航偏角，便于運(yùn)動學(xué)分析車輛運(yùn)動軌跡，確定車輛相對預(yù)定軌跡的方向和位置關(guān)系，據(jù)此跟蹤目標(biāo)點(diǎn)。

3.3 基于路徑跟蹤控制算法

本文對車輛運(yùn)動模型分析，通過目標(biāo)點(diǎn)和當(dāng)前實(shí)時坐標(biāo)比較，采用積分Backsteppping［7］方法在車體坐標(biāo)下構(gòu)建位置誤差模型，采用非線性系統(tǒng)穩(wěn)定設(shè)計理論，通過目標(biāo)點(diǎn)在車輛坐標(biāo)系中的實(shí)時狀態(tài)坐標(biāo)，以及經(jīng)過周期為t時間后車輛下一運(yùn)動狀態(tài)方程，和預(yù)先構(gòu)造的Lyapunov函數(shù)聯(lián)系起來，在控制率作用下通過逐步推算，修正軌跡，最終使起始位置誤差 ［xg，yg，θg］滿足完成軌跡控制器的設(shè)計。

車輛下一運(yùn)動狀態(tài)方程為:

3.4 仿真分析

通過matlab對預(yù)定義軌跡跟蹤仿真（圖4，圖5）。起始位置誤差 ［xg，yg，θg］＝ ［5，－2，π/2］，參考速度［vr，wr］＝ ［5，0］，期望軌跡為長10m 寬4 m的矩形。

圖4 純追蹤模型的路徑跟蹤

圖5 實(shí)際軌跡與預(yù)計軌跡對比圖

由圖4可知，車輛在起始位置跟蹤目標(biāo)直線，xg，yg，θg隨著時間的變化趨近于0，實(shí)際軌跡與預(yù)期軌跡基本重合，表明在所設(shè)計的控制器作用下，車輛在有限的時間內(nèi)，能快速跟蹤預(yù)先定義的參考軌跡，使系統(tǒng)跟蹤快速收斂，具有良好的局部穩(wěn)定性。說明此算法滿足自動駕駛要求，可作為車輛自動導(dǎo)航的決策方法。

4 上位機(jī)遠(yuǎn)程監(jiān)控系統(tǒng)程序設(shè)計

上位機(jī)在VC＋＋6.0平臺上編寫，通過對TCP/IP的封裝，對Access數(shù)據(jù)庫進(jìn)行數(shù)據(jù)添加、查詢操作。通過MSFlexGrid控件實(shí)時顯示GPS數(shù)據(jù)信息，WebBrowser瀏覽器控件關(guān)聯(lián)Google地圖，將車輛位置實(shí)時顯示在GIS上，當(dāng)車輛偏航報警信息彈出時，可根據(jù)監(jiān)控界面（圖6）顯示小車預(yù)定軌跡與實(shí)時位置關(guān)系，遠(yuǎn)程遙控車輛并進(jìn)行人為修正。

圖6 遠(yuǎn)程監(jiān)控中心

［1］ 賈 全.拖拉機(jī)自動導(dǎo)航系統(tǒng)關(guān)鍵技術(shù)研究［D］.北京:中國農(nóng)業(yè)機(jī)械化科學(xué)研究院，2013.

［2］ 羅錫文，張智剛.東方紅X＿804拖拉機(jī)的DGPS自動導(dǎo)航控系統(tǒng)［J］.農(nóng)業(yè)工程學(xué)報，2009，25（17）:139-145

［3］ 劉 陽.拖拉機(jī)中導(dǎo)航系統(tǒng)CAN總線的設(shè)計與實(shí)現(xiàn)［D］.石河子:石河子大學(xué)，2009.

［4］ Chris Muench .The windows ce technology tutorial:solutions for the developer［M］.北京:中國電力出版社，2001.

［5］ 周立功.ARM＆WinCE實(shí)驗(yàn)與實(shí)踐—基于S3C2410［M］.北京:北京航空航天大學(xué)出版社，2007.

［6］ 王 銳.高斯－克呂格投影的遠(yuǎn)程測向方位角計算方法［J］.火力與指揮控制，2013，38（17）:139-145.

［7］ 游 峰.王榮本.智能車輛換道與超車軌跡跟蹤控制［J］.農(nóng)業(yè)機(jī)械學(xué)報，2008，39（06）:42-45.