基于ROS的智能割草機(jī)器人導(dǎo)航控制系統(tǒng)設(shè)計

王 全, 陳學(xué)海,2, 干 熊,2, 楊光友,2

(1 湖北工業(yè)大學(xué)農(nóng)業(yè)機(jī)械工程研究設(shè)計院, 湖北 武漢 430068;2 湖北省農(nóng)機(jī)裝備智能化工程技術(shù)研究中心, 湖北 武漢 430068)

田間、果園和灘涂上的雜草會直接影響到農(nóng)業(yè)生產(chǎn),導(dǎo)致糧食減產(chǎn)和經(jīng)濟(jì)損失[1]。目前國內(nèi)外的除草方法主要分為化學(xué)除草和非化學(xué)除草兩類[2],化學(xué)除草的主要弊端在于化學(xué)藥劑會殘留在土壤內(nèi),會對自然環(huán)境和人體健康造成危害。非化學(xué)除草主要是用機(jī)械設(shè)備進(jìn)行除草[3],而傳統(tǒng)除草機(jī)械由于智能化程度不足,除草作業(yè)的全程都需要人工參與,導(dǎo)致其勞動強(qiáng)度大、作業(yè)效率較低。為了提高作業(yè)效率和降低人工成本,需要開發(fā)智能割草機(jī)器人,而導(dǎo)航控制系統(tǒng)是智能割草機(jī)器人的關(guān)鍵環(huán)節(jié)。

在導(dǎo)航控制系統(tǒng)方面,羅錫文等[4-6]在二自由度車輛轉(zhuǎn)向模型的基礎(chǔ)上,建立了基于GPS的導(dǎo)航控制系統(tǒng),可以有效地追蹤目標(biāo)軌跡。熊斌等[7]采用氣動轉(zhuǎn)向系統(tǒng),將果園施藥機(jī)的運(yùn)動學(xué)模型和純追蹤模型相結(jié)合,建立了基于北斗的自主導(dǎo)航系統(tǒng),其作業(yè)精度可以滿足果園作業(yè)精度要求。白曉鴿等[8]針對數(shù)學(xué)模型復(fù)雜的拖拉機(jī)運(yùn)動學(xué)問題,采用基于神經(jīng)網(wǎng)絡(luò)的控制方法,設(shè)計了車輛控制的神經(jīng)網(wǎng)絡(luò)控制器,實驗結(jié)果表明該控制器對拖拉機(jī)的運(yùn)動有較好的控制效果。Peichen Huang等[9]研究了固定前視距離的PI路徑跟蹤算法并開展了實驗,實驗結(jié)果表明可以獲取車輛的位置和航向誤差。王輝等[10]采用預(yù)瞄準(zhǔn)純路徑追蹤模型作為路徑跟蹤方法,可以有效地提高車輛在復(fù)雜路面的追蹤精度。Ahn Joonwoo[11-12]等在預(yù)瞄準(zhǔn)純路徑追蹤模型的基礎(chǔ)上采用動態(tài)前視距離純路徑追蹤模型的方法作為路徑跟蹤算法,可以進(jìn)一步提高車輛在復(fù)雜路面的追蹤精度。王玉亮等[13]為解決水稻插秧機(jī)無人駕駛的問題,采用RTK定位技術(shù),以MPC算法作為路徑追蹤算法,能夠在車速1m/s時,有效跟蹤目標(biāo)路線。Shuo Cheng等[14]針對在自動駕駛過程中車輛參數(shù)的不確定性,設(shè)計了基于MPC的車輛路徑跟蹤控制器,實驗結(jié)果表明該方法可以有效追蹤目標(biāo)路徑。

由于實際割草作業(yè)環(huán)境復(fù)雜且多變,導(dǎo)致上述導(dǎo)航控制系統(tǒng)不能直接遷移到智能割草機(jī)器人上。為了實現(xiàn)割草機(jī)智能化水平的提高,本文設(shè)計了差速驅(qū)動底盤的割草機(jī)器人導(dǎo)航控制系統(tǒng),實現(xiàn)了底盤驅(qū)動、GNSS數(shù)據(jù)采集、IMU數(shù)據(jù)采集等功能。針對系統(tǒng)的定位精度和定位頻率較低的問題,采用擴(kuò)展卡爾曼濾波算法將GNSS和IMU的定位信息進(jìn)行融合,提高了智能割草機(jī)器人的定位精度和定位頻率。同時,采用基于預(yù)瞄準(zhǔn)的純路徑追蹤算法,實現(xiàn)了智能割草機(jī)器人對目標(biāo)軌跡的自主追蹤。

1 導(dǎo)航控制系統(tǒng)設(shè)計

割草機(jī)器人平臺采用husky的差速底盤機(jī)器人,該平臺采用4輪驅(qū)動,在其左右兩側(cè)沿對角線各布置一臺電機(jī),同一側(cè)輪子采用同步帶驅(qū)動如圖1所示。相較于三輪模型該平臺穩(wěn)定性更好,且能夠原地轉(zhuǎn)向,適合在狹小空間作業(yè)。

圖1 差速底盤機(jī)器人

1.1 系統(tǒng)總體框架

在機(jī)器人操作系統(tǒng)ROS下,根據(jù)經(jīng)典的模塊化分層思想構(gòu)建割草機(jī)器人導(dǎo)航控制系統(tǒng)。其整體框架如圖2所示。

圖2 系統(tǒng)總體框架

如圖2所示,系統(tǒng)共分為三層:控制層、數(shù)據(jù)處理層以及硬件層。控制層分布在遠(yuǎn)程PC端。PC端通過WiFi與樹莓派連接,具有割草機(jī)器人的位置信息實時顯示,割草機(jī)器人遠(yuǎn)程控制以及作業(yè)軌跡發(fā)布等功能。數(shù)據(jù)處理層分布在上位機(jī)ROS和下位機(jī)RT-Thread上,通過WiFi與控制層通信。數(shù)據(jù)處理層中ROS中具有控制算法和傳感器數(shù)據(jù)處理等節(jié)點,并發(fā)布電機(jī)控制指令。數(shù)據(jù)處理層中的RT-Thread主要負(fù)責(zé)接受遙控器的指令或是來自ROS的控制指令,實現(xiàn)對電機(jī)控制。

1.2 導(dǎo)航控制系統(tǒng)上位機(jī)設(shè)計

導(dǎo)航控制系統(tǒng)以樹莓派4B為上位機(jī),安裝Ubuntu16.04系統(tǒng),運(yùn)行ROS,其總體框架如圖3所示。

圖3 上位機(jī)軟件框架

ROS系統(tǒng)中主要有5個節(jié)點,分別為軌跡追蹤節(jié)點、底盤驅(qū)動節(jié)點、GNSS數(shù)據(jù)采集節(jié)點、IMU數(shù)據(jù)采集節(jié)點、卡爾曼濾波節(jié)點。其中:1)軌跡追蹤節(jié)點:主要通過軌跡追蹤算法計算車輛轉(zhuǎn)向角速度w和車輛行駛速度v,并發(fā)布到話題/cmd_msg上;2)底盤驅(qū)動節(jié)點:通過訂閱話題/cmd_msg中的指令來完成機(jī)器人的行駛和轉(zhuǎn)向控制;3)GNSS數(shù)據(jù)采集節(jié)點:首先將GNSS的原始數(shù)據(jù)轉(zhuǎn)化到地心大地坐標(biāo)系下及轉(zhuǎn)化成經(jīng)緯度,在將經(jīng)緯度轉(zhuǎn)化到地心空間直角坐標(biāo)系下及轉(zhuǎn)化成x,y,z,最后將地心空間直角坐標(biāo)系轉(zhuǎn)化為導(dǎo)航坐標(biāo)系及東北天坐標(biāo)系并將坐標(biāo)信息發(fā)布到話題/GNSS_msg上;4)IMU數(shù)據(jù)采集節(jié)點:將原始數(shù)據(jù)解析為三軸加速度和三軸角速度,并通過DR算法得到基于機(jī)體坐標(biāo)系的坐標(biāo),最后將機(jī)體坐標(biāo)系下的坐標(biāo)轉(zhuǎn)化為導(dǎo)航坐標(biāo)系下的坐標(biāo)及東北天坐標(biāo)系下的坐標(biāo)并發(fā)布到話題/IMU_msg上;5)卡爾曼濾波節(jié)點:訂閱話題/GNSS_msg、/IMU_msg中的坐標(biāo)信息并進(jìn)行融合,以得到最優(yōu)估計坐標(biāo)。

1.3 導(dǎo)航控制系統(tǒng)下位機(jī)設(shè)計

導(dǎo)航控制系統(tǒng)以STM32F103C8T6單片機(jī)為下位機(jī),采用嵌入式實時操作系統(tǒng)RT-Thread作為運(yùn)行環(huán)境,其總體框架如圖4所示。

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

下位機(jī)主要負(fù)責(zé)接收上位機(jī)控制指令以及接收遙控器指令。分為三個線程:分別為遙控線程、電機(jī)線程和ROS通訊線程。遙控線程主要負(fù)責(zé)接收遙控器信號并轉(zhuǎn)換成相應(yīng)的控制指令。電機(jī)線程通過PID算法來完成電機(jī)的控制指令。ROS通訊線程通過rosserial把單片機(jī)做為ROS下的一個節(jié)點,以實現(xiàn)通過話題的方式與上位機(jī)的ROS系統(tǒng)通信,并訂閱話題/cmd_vel中的電機(jī)控制指令。

2 導(dǎo)航控制方法研究

2.1 差速轉(zhuǎn)向模型

假設(shè)前進(jìn)速度不變、忽略車輛離心力和側(cè)滑,建立以割草機(jī)器人中心為控制點的運(yùn)動學(xué)模型;依據(jù)模型可以確定左右兩輪的轉(zhuǎn)速與轉(zhuǎn)向半徑的關(guān)系以及割草機(jī)器人相對于起始坐標(biāo)系運(yùn)動的運(yùn)動學(xué)方程。如圖5所示為差速轉(zhuǎn)向機(jī)器人運(yùn)動學(xué)模型。

圖5 差速轉(zhuǎn)向運(yùn)動學(xué)模型

割草機(jī)器人在笛卡爾坐標(biāo)系下的坐標(biāo)為(x0,y0),航向角為θ。割草機(jī)器人的運(yùn)動學(xué)模型為:

(1)

(2)

(3)

可得兩輪線速度與轉(zhuǎn)彎半徑的關(guān)系為:

(4)

式中,(x0,y0)為割草機(jī)器人質(zhì)心坐標(biāo),m;v1,v2為割草機(jī)器人左右輪線速度,m/s;R為割草機(jī)器人轉(zhuǎn)向半徑,m。

實際作業(yè)中由于輪胎側(cè)滑、地面不平坦等因素會對車輪的實際速度產(chǎn)生較大影響,從而降低轉(zhuǎn)向的準(zhǔn)確度,因此對車輪速度采用PID控制,以確保轉(zhuǎn)向的準(zhǔn)確度。

2.2 預(yù)瞄準(zhǔn)純路徑追蹤模型

建立割草機(jī)器人的預(yù)瞄準(zhǔn)純追蹤模型幾何學(xué)示意圖如圖6所示。

圖6 預(yù)瞄準(zhǔn)純追蹤模型原理

建立割草機(jī)器人相對坐標(biāo)系,割草機(jī)器人中心點p為割草機(jī)器人當(dāng)前位置,O1為割草機(jī)器人轉(zhuǎn)向時的瞬時圓心;R為瞬時轉(zhuǎn)向半徑;選取目標(biāo)軌跡上的預(yù)瞄準(zhǔn)點G(x1,y1)為當(dāng)前目標(biāo)位置,割草機(jī)器人中心點p到預(yù)瞄準(zhǔn)點G的距離ld為前視距離;pG對應(yīng)的弧長為割草機(jī)器人到達(dá)目標(biāo)位置所需要行駛的圓弧,根據(jù)圓弧可以計算出轉(zhuǎn)向半徑,在根據(jù)轉(zhuǎn)向半徑即可計算出左右兩輪的理論轉(zhuǎn)速;在下個采樣周期根據(jù)新的當(dāng)前位置信息和目標(biāo)位置信息計算出新的左右輪轉(zhuǎn)速,如此反復(fù)即可完成軌跡追蹤。

依據(jù)圖中幾何關(guān)系可得:

(5)

e=ldsin(θ-yaw)

(6)

(7)

結(jié)合式(4)-(7)可得:

(8)

式中,(x0,y0)為割草機(jī)器人質(zhì)心坐標(biāo),m;(x1,y1)標(biāo)點坐標(biāo),m;v1,v2為割草機(jī)器人左右輪線速度,m/s;l-人左右輪間距,m;ld為前視距離,m;R為割草機(jī)器人轉(zhuǎn)向半徑,m;θ為pG與x軸夾角,(°);yaw為初始航向角,(°);e為橫向誤差,m。

2.3 擴(kuò)展卡爾曼濾波融合GNSS/IMU定位數(shù)據(jù)

全球?qū)Ш叫l(wèi)星系統(tǒng)GNSS具有較高的定位精度,更新頻率一般為10 Hz,因其每次的定位信息不依賴于上次的值,所以不存在累計誤差。慣性傳感器IMU,對其輸出的角速度和加速度進(jìn)行積分可以得到相較于起點的位置和姿態(tài)信息,但由于每次的定位信息依賴于上次的值,所以存在累計誤差,短時間內(nèi)累計誤差較小定位精度很高,但長時間累計誤差較大導(dǎo)致定位精度很低。IMU更新頻率很快,可達(dá)100 Hz以上,為此可以將IMU和GNSS定位系統(tǒng)結(jié)合起來,利用GNSS較高的定位精度來修正IMU的累計誤差。再結(jié)合IMU短時間內(nèi)精度較高和頻率快的特點即可得到高精度高頻率的導(dǎo)航信息。通過傳感器信息融合技術(shù)卡爾曼濾波即可實現(xiàn)GNSS/IMU的定位信息融合,如圖7所示。采用GNSS定位結(jié)果輔助IMU系統(tǒng)的初始化,給出基于GNSS精度的初始位置。在穩(wěn)定運(yùn)行過程中可以通過GNSS的位置信息動態(tài)修正IMU的累計誤差;另一方面,當(dāng)GNSS無法定位或定位誤差太大時,便切換至IMU系統(tǒng),短時間內(nèi)修正GNSS定位信息,當(dāng)GNSS定位恢復(fù)正常后,再用GNSS定位信息對IMU經(jīng)行校正,如此即可實現(xiàn)高精度高頻率的導(dǎo)航。

圖7 GNSS/IMU融合流程

2.3.1GNSS/IMU組合系統(tǒng)狀態(tài)方程的建立GNSS/IMU組合導(dǎo)航系統(tǒng)狀態(tài)變量為X=[xe,ve,ae,xn,vn,an,w],可以得到組合系統(tǒng)的狀態(tài)方程:

(9)

2.3.2GNSS/IMU組合系統(tǒng)觀測方程的建立系統(tǒng)的觀測量包括GNSS接收機(jī)輸出的車輛東向位置信息xe,北向位置信息xn和慣性傳感器IMU輸出的z角速度w,東向加速度ae和北向加速度an。可得系統(tǒng)離散的觀測方程為:

(10)

(11)

(12)

(13)

(14)

(15)

(16)

(17)

3 機(jī)器人導(dǎo)航控制實驗與結(jié)果分析

為驗證智能割草機(jī)器人導(dǎo)航系統(tǒng)工作效果,以husky為實驗平臺在湖北省農(nóng)機(jī)鑒定站試驗場地對智能割草機(jī)器人進(jìn)行路徑跟蹤實驗,測試現(xiàn)場如圖8所示。

圖8 導(dǎo)航系統(tǒng)實驗

在試驗地塊上選取機(jī)器人起點為圖9a中的點T(-0.1 m, 0),終點為圖9a中的點F(4.5 m, 20.0 m)。規(guī)劃路徑的起點為圖9a中的點A(0.5 m, 0),中間點依次為點B(0.5 m, 20 m)、C(2.5 m, 20 m)、D(2.5 m, 0)、E(4.5 m, 0),終點為F點。將點A、B、C、D、E、F確定的5條直線作為機(jī)器人跟蹤的目標(biāo)路徑,如圖9a中的黑實線所示。

(a) 軌跡追蹤

實驗方法:設(shè)置割草機(jī)器人速度恒定為1 m/s,前視距離恒定為0.4 m,采樣周期為0.01 s。在點T(-0.1 m, 0)啟動機(jī)器人,割草機(jī)器人自動追蹤圖9a中所規(guī)劃的路徑,到達(dá)F點后結(jié)束路徑跟蹤實驗。

割草機(jī)器人軌跡追蹤如圖9a紅色點所示,其橫向誤差如圖9b所示。由圖9b可知,橫向誤差最大為0.35 m,平均橫向誤差為0.11 m。如圖9a所示,在進(jìn)行直線①②③④⑤,5條直線追蹤時誤差較小,在每個直角轉(zhuǎn)彎處則出現(xiàn)最大誤差。在固定前視距離的情況下,割草機(jī)器人進(jìn)行直線追蹤時不會出現(xiàn)較大的超調(diào)。但在進(jìn)行直角轉(zhuǎn)彎時,由于前視距離的設(shè)置為定值,導(dǎo)致控制系統(tǒng)算法不能很好地適應(yīng)當(dāng)前狀態(tài),進(jìn)而對割草機(jī)器人直線追蹤精度產(chǎn)生較大影響。

4 結(jié)論

本文設(shè)計了一種智能割草機(jī)器人的導(dǎo)航控制系統(tǒng)。基于機(jī)器人操作系統(tǒng)ROS,實現(xiàn)了底盤驅(qū)動、GNSS數(shù)據(jù)采集、IMU數(shù)據(jù)采集等功能。通過擴(kuò)展卡爾曼濾波器實現(xiàn)了GNSS/IMU的組合定位,有效地提高了割草機(jī)器人定位精度與定位頻率。建立了基于差速轉(zhuǎn)向模型的預(yù)瞄準(zhǔn)純路徑追蹤運(yùn)動學(xué)模型,基于husky機(jī)器人平臺對算法進(jìn)行了測試實驗,實驗驗證了導(dǎo)航控制系統(tǒng)的有效性。下一步將深入研究提高路徑追蹤精度的算法,同時加入避障功能模塊,進(jìn)一步提高割草機(jī)器人的智能化程度。