基于ROS的智能割草機器人導航控制系統設計

王 全, 陳學海,2, 干 熊,2, 楊光友,2

(1 湖北工業大學農業機械工程研究設計院, 湖北 武漢 430068;2 湖北省農機裝備智能化工程技術研究中心, 湖北 武漢 430068)

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

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

由于實際割草作業環境復雜且多變,導致上述導航控制系統不能直接遷移到智能割草機器人上。為了實現割草機智能化水平的提高,本文設計了差速驅動底盤的割草機器人導航控制系統,實現了底盤驅動、GNSS數據采集、IMU數據采集等功能。針對系統的定位精度和定位頻率較低的問題,采用擴展卡爾曼濾波算法將GNSS和IMU的定位信息進行融合,提高了智能割草機器人的定位精度和定位頻率。同時,采用基于預瞄準的純路徑追蹤算法,實現了智能割草機器人對目標軌跡的自主追蹤。

1 導航控制系統設計

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

圖1 差速底盤機器人

1.1 系統總體框架

在機器人操作系統ROS下,根據經典的模塊化分層思想構建割草機器人導航控制系統。其整體框架如圖2所示。

圖2 系統總體框架

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

1.2 導航控制系統上位機設計

導航控制系統以樹莓派4B為上位機,安裝Ubuntu16.04系統,運行ROS,其總體框架如圖3所示。

圖3 上位機軟件框架

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

1.3 導航控制系統下位機設計

導航控制系統以STM32F103C8T6單片機為下位機,采用嵌入式實時操作系統RT-Thread作為運行環境,其總體框架如圖4所示。

圖4 下位機軟件設計

下位機主要負責接收上位機控制指令以及接收遙控器指令。分為三個線程:分別為遙控線程、電機線程和ROS通訊線程。遙控線程主要負責接收遙控器信號并轉換成相應的控制指令。電機線程通過PID算法來完成電機的控制指令。ROS通訊線程通過rosserial把單片機做為ROS下的一個節點,以實現通過話題的方式與上位機的ROS系統通信,并訂閱話題/cmd_vel中的電機控制指令。

2 導航控制方法研究

2.1 差速轉向模型

假設前進速度不變、忽略車輛離心力和側滑,建立以割草機器人中心為控制點的運動學模型;依據模型可以確定左右兩輪的轉速與轉向半徑的關系以及割草機器人相對于起始坐標系運動的運動學方程。如圖5所示為差速轉向機器人運動學模型。

圖5 差速轉向運動學模型

割草機器人在笛卡爾坐標系下的坐標為(x0,y0),航向角為θ。割草機器人的運動學模型為:

(1)

(2)

(3)

可得兩輪線速度與轉彎半徑的關系為:

(4)

式中,(x0,y0)為割草機器人質心坐標,m;v1,v2為割草機器人左右輪線速度,m/s;R為割草機器人轉向半徑,m。

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

2.2 預瞄準純路徑追蹤模型

建立割草機器人的預瞄準純追蹤模型幾何學示意圖如圖6所示。

圖6 預瞄準純追蹤模型原理

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

依據圖中幾何關系可得:

(5)

e=ldsin(θ-yaw)

(6)

(7)

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

(8)

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

2.3 擴展卡爾曼濾波融合GNSS/IMU定位數據

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

圖7 GNSS/IMU融合流程

2.3.1GNSS/IMU組合系統狀態方程的建立GNSS/IMU組合導航系統狀態變量為X=[xe,ve,ae,xn,vn,an,w],可以得到組合系統的狀態方程:

(9)

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

(10)

(11)

(12)

(13)

(14)

(15)

(16)

(17)

3 機器人導航控制實驗與結果分析

為驗證智能割草機器人導航系統工作效果,以husky為實驗平臺在湖北省農機鑒定站試驗場地對智能割草機器人進行路徑跟蹤實驗,測試現場如圖8所示。

圖8 導航系統實驗

在試驗地塊上選取機器人起點為圖9a中的點T(-0.1 m, 0),終點為圖9a中的點F(4.5 m, 20.0 m)。規劃路徑的起點為圖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條直線作為機器人跟蹤的目標路徑,如圖9a中的黑實線所示。

(a) 軌跡追蹤

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

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

4 結論

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