最小二乘法與SVM組合的林果行間自主導航方法

劉星星，張 超，張 浩，楊圣慧，江世界，鄭永軍,2※，蘇道畢力格，萬 暢,3

（1. 中國農業大學工學院，北京 100083；2. 現代農業裝備與設施教育部工程研究中心，北京100083； 3. 塔里木大學機械電氣化工程學院，阿拉爾 843300）

0 引言

自動導航是實現農業裝備智能化的重要部分，根據傳感器種類可將導航方式分為激光雷達導航、GPS/GNSS導航、機器視覺導航以及多傳感器融合導航等[1-2]。張雄楚等[3]提出一種棗樹樹行視覺導航路徑檢測算法，對于灰棗棗園與駿棗棗園，該算法的路經檢測準確率平均值分別為94%和93%，單幀圖像處理時間分別為0.042和0.046 s，但該算法受到抖動、人像干擾、逆光以及地膜干擾等情況影響時容易產生檢測錯誤。楊洋等[4]提出了一種基于車輪正前方可行走動態感興趣區域的玉米行導航線實時提取算法，該算法提取的導航線與人工提取結果平均誤差為1.157°，準確率為96%，單幀圖像處理時間為0.097 56 s。聶森等[5]提出一種基于機器視覺的果園環境行間導航算法，該算法能夠提取兩側樹行的中心線作為導航路徑，但是圖像平均處理時間為1.166 s，算法較為復雜，處理時間過長，且也會受到光照與落葉的影響。袁池等[6]針對果樹復雜空間層次造成直線檢測算法失效的問題，提出了一種果樹行中心線檢測算法，單幀圖像處理時間長達0.6 s，圖像處理算法較復雜。所以在實際生產應用中，機器視覺導航技術受光照強度、視角范圍等條件限制較大[7-9]。魏爽等[10]提出一種基于GNSS的農機自動導航路徑搜索方法與基于預瞄點搜索的純追蹤模型，導航均方根誤差最大為25.23 cm。熊斌等[11]采用DBS技術設計了一套施藥機自主導航系統，在果園自動化施藥作業試驗中，導航橫向誤差最大不超過13 cm，橫向誤差平均值不超過3 cm。羅錫文等[12]基于載波相位差分GPS技術設計了一套導航控制系統，并設計了地頭跨行轉向的控制算法，在直線導航速度為0.8 m/s時，平均誤差不足3 cm，最大誤差不足15 cm。但是盡管現代化果園環境相對標準，仍然存在果樹枝葉豐茂冠層郁閉的情況，環境等同于半封閉環境，衛星信號受到樹冠遮擋，容易產生較大誤差，因此在果園作業環境中，基于GNSS定位的導航方法還受到一定的限制。激光雷達受環境影響較小，具有較強的環境適應性，能夠在戶外實時獲取作物或果樹輪廓以及環境、位置信息，近年來在導航、掃描、對靶等領域得到廣泛應用[13-15]。

Oscar等[16]設計了一種基于二維激光雷達的果園自動導航系統，橫向誤差為110 mm，航向角誤差為1.5°。Thanpattranon等[17]利用二維激光雷達，設計了一種適用于果園自動曲線導航行駛系統，與人工駕駛相比，路徑跟蹤平均偏差為275 mm，標準差為9 mm。Bayar等[18]利用廉價激光測距儀實現拖拉機在蘋果園中直線行走和地頭轉彎，在保證系統穩定性情況下，降低了成本。Freitas等[19]利用激光雷達、編碼器等多傳感器感知果園壟行果樹位置信息，并采用派生卡爾曼濾波器判別植保機器人相對壟行的位置以及植保機器人的導航路徑。陳軍等[20]采用SICK LMS291型激光雷達來模擬果園環境導航，平均橫向偏差120 mm，最大橫向偏差400 m。賈士偉等[21]提出一種基于激光雷達的溫室導航定位方法，在走廊環境中平均橫向偏差為12.7 mm，均方誤差為26.8 mm，溫室道路平坦地帶可實現自主行走。艾長勝等[22]利用卡爾曼濾波器處理由激光雷達掃描獲取的果園數據信息，并結合SVM技術實現植保機器人定位，平均橫向偏差為4.22 mm，平均航向偏差為0.72°。毛鵬軍等[23]設計了林下自主導航機器人，采用ROS操作系統，搭載激光雷達實現環境地圖構建，在室內模擬環境中能夠規劃路線、躲避障礙物，實現一定程度的自主導航。周俊等[24]提出一種利用激光雷達前后多幀數據匹配的農業機器人定位方法，在密植果園的果樹行間試驗中定位誤差約為80 mm，精度基本滿足果園作業機器人應用需求。

綜上，近年來國內外基于激光雷達的林間導航技術研究很多，但多針對單一的林間環境，單一掃描樹冠或者掃描樹干。本研究設計一種基于激光雷達的作業平臺導航系統，針對冠層相連的桃園行間、冠層不相連的柑橘園行間以及掃描樹干的松樹林行間3種環境，提出最小二乘法與支持向量機（Support Vector Machine，SVM）融合的導航路徑生成算法，研究作業平臺在林間環境中的導航性能。

1 導航系統與導航路徑計算

1.1 系統組成及原理

本研究基于中國農業大學工學院設計的履帶式噴霧機（如圖1所示），采用激光雷達掃描林間作業環境，獲取導航基礎數據。該履帶式噴霧機主要技術參數如下：外形尺寸1 575 mm×1 190 mm×1 355 mm（長×寬×高），驅動系統由48 V、1.3 kW直流無刷電機與RV50型蝸輪蝸桿減速機組成，動力系統采用電壓為48 V、容量為45 ah鉛蓄電池，配置6 kW增程器提高續航能力。

系統總體方案如圖2所示。激光雷達傳感器獲取兩側樹行點云數據，通過串行接口將數據傳輸至上位機。姿態傳感器獲取作業平臺的姿態信息并通過串行接口傳輸至上位機。上位機對點云數據及姿態位姿數據進行處理、分析并計算出導航路徑后通過串口將控制信號發送至下位機；下位機根據控制信號輸出脈沖寬度調制（PWM）信號至電機驅動器以驅動電動機，實現作業平臺的移動。

所用激光雷達傳感器為上海思嵐科技有限公司的RPLIDAR S1，該激光雷達具有40 m測距半徑，可在室外環境下工作，掃描頻率為8～15 Hz可調。姿態傳感器選用SC-AHRS-100D2型號姿態模塊，可提供動靜態環境下實時的、高精度的橫滾角、俯仰角和航向角。姿態傳感器性能指標如表1所示。

表1 100D2型姿態傳感器性能參數 Table 1 Performance parameters of the attitude sensor of 100D2

1.2 導航路徑計算

當作業平臺在樹林行間行走時，利用姿態傳感器獲取的作業平臺位姿信息，對由激光雷達獲取的行間數據信息做方向上的校正，然后用最小二乘法擬合識別樹行，結合支持向量機，獲得樹林行間中心線，以此中心線作為作業平臺的參考導航線。

1.2.1 激光雷達點云數據校正

作業平臺在樹行行間行走時，激光雷達初步掃描捕獲的樹行行間實況信息如圖3a所示，由于作業平臺行進時有航向偏差，激光雷達掃描的點云信息在行間行進中不能很好分辨出樹林中的行與列，所以用姿態傳感器的航向信息對激光雷達點云進行角度數據校正，排除作業平臺行進時的航向偏差所帶來的激光雷達點云數據起始位置的角度干擾，確保每一幀激光雷達數據起始位置的角度相同，以便于后續數據處理分辨出激光雷達點云數據中的樹行信息。

姿態傳感器讀取的姿態數據使用四元數表示，分別為x、y、z、w，為了方便計算，需要將其轉換為歐拉角。四元數到歐拉角的轉換方程為

式中R、P、Y分別代表翻滾（Roll）角、俯仰（Pitch）角、航向（Yaw）角，（°）。作業平臺在行間初始位置時姿態傳感器獲取的航向角數據記為yaw1，此后每一幀激光雷達數據對應的航向角數據記為yaw2，激光雷達點云數據需要校正的角度β為

激光雷達獲取點云數據格式為Pi(ri,θi)，其中ri表示第i個點到激光雷達的距離，m；θi表示第i個點到初始位置的角度，(°)。分別對當前幀激光雷達每個點的角度減去需要校正的角度，如公式（3），校正后激光雷達點云數據為Pi(ri,φi)

為了方便后續數據處理，將經過姿態傳感器校正后的激光雷達點云數據由極坐標Pi(ri,φi)轉為笛卡爾坐標Pi(xi,yi)，其轉換方程為

式中ir表示一幀激光雷達數據中第i個點到原點的距離，cm；iφ表示第i個點的與原點連線和第1個點與原點連線的夾角，（°）；ix、iy分別表示笛卡爾坐標系中第i個點坐標，cm。

激光雷達點云原始數據由極坐標轉為笛卡爾坐標系如圖3a，校正后如圖3b。當作業平臺在初始位置時，樹行與車身接近平行，在作業平臺行進過程中，航向角發生變化，如圖3a所示，樹行線有一定的傾斜，通過位姿傳感器對激光雷達數據角度校正，如圖3b中，樹行線接近水平，此時激光雷達數據起始位置角度與第一幀數據起始位置角度相同，以便于后續數據處理分辨出激光雷達點云數據中的樹行信息。

1.2.2 最小二乘法擬合樹行

由于樹林行間距約為2.5 m，初步濾掉iy>2.5 m與yi<-2.5 m的數據，大致保留當前樹林行間樹行數據信息，并對Pi(xi,yi)中的數據分為yi>0與yi<0兩類，yi>0的數據集記為一側樹行激光雷達點云數據，記作L1(xi,yi)，yi<0的數據集記為另一側樹行激光雷達點云數據，記作L2(xi,yi)，設L1與L2數據集線性回歸方程為

式中m1，m2分別為L1，L2的斜率；b1，b2分別為L1，L2的截距，cm。

最小二乘法擬合直線求解公式為

式中n為一幀激光雷達數據中點的數量。

將數據集L1(xi,yi)，L2(xi,yi)分別代入公式（6），求出對應的截距 1m，2m與截距b1，b2，將截距與斜率代入公式（5）分別求得最小二乘法擬合的左右兩側樹行位置L1，L2，取L1與L2中心線L3，將L3作為分類線，將Pi(xi,yi)中的數據分為兩類。中心線L3的方程如公式（7）所示，最小二乘法擬合樹行數據如圖4所示。

1.2.3 支持向量機生成導航線

樹行趨向平行是樹林種植的突出特征，而其平行性展現了兩側樹行可以進行二分類的特點。利用支持向量機可確定一條與兩側樹行間隔最大化的最優分類線，以此最優分類線作為作業平臺參考導航線。

由圖4中的L3將激光雷達點云數據分為兩類，對于點Pi(xi,yi)有其對應的標簽zi，對應訓練數據為(Pi,zi)。

設分類面函數為wx+b= 0，即對兩側點云數據進行二分類的最優分類面，為使分類面對所有樣本正確分類并具備分類間隔，其數據需要符合關系：（l為一幀激光雷達數據中點的數量）。由此可得分類間隔為2/||||w，將構造最優分類線的問題轉化為具備約束條件的凸優化問題，且轉化為拉格朗日乘子的目標函數[25]為。其中iα為拉格朗日系數，iα的值決定與其相對應各個激光雷達掃描點是否為支持向量點。同時在約束條件的限定下需要把目標函數的凸優化問題轉變成拉格朗日的對偶求取最優解的問題。其中α= (α1,α2,… ,αi)[22]

通過對α的求解間接獲取w*與b*最優估計

最終求得最優分類線函數，即參考導航線L

通過支持向量機生成理想導航線如圖5所示。

1.2.4 最小二乘法與融合算法對比

為提升基于激光雷達作業平臺的導航算法通用性，分別在北京市平谷區桃園、四川廣安縣柑橘園、北京市海淀區東升八家郊野公園北區松樹林采集激光雷達點云數據。其中海淀區東升八家郊野公園北區松樹林，樹木較為高大，作業平臺在林間行進時，激光雷達只能掃描到松樹主干；北京市平谷區桃園與四川廣安縣柑橘園則由于矮化果樹，作業平臺在林間行進時，激光雷達只能掃描到果樹冠層，其中柑橘園為柑橘樹有間隔種植，桃園桃樹為有行無列冠層相連種植。

圖6為四川省廣安縣柑橘園以最小二乘法與支持向量機算法得到的導航線對比。

由于柑橘樹冠層枝葉茂密，激光雷達往往只能掃描到外側點云，掃描數據并不能真實反映出冠層橫截面，由此采用最小二乘法擬合樹行的結果并不理想，如圖6淺色樹行部分最小二乘法擬合出的樹行擬合線不能作為實際樹行位置參考，由此最小二乘法生成的參考導航線算法不適用于掃描冠層的激光雷達林間導航。

本研究融合算法基于支持向量機生成的分類邊際線，在于找到分類點云的邊界，對于激光雷達只能掃描冠層外側的情況則不受影響，并且能夠在一定程度上排除果樹生長情況和冠層大小不一帶來的干擾。本研究融合算法生成導航線能盡可能的遠離左右兩側障礙物，避免在行間自主導航過程中產生的枝葉的碰撞，同時也能適用于多種林間作業環境。

1.3 橫向偏差計算

如圖5所示，激光雷達坐標原點，式（12）中參考導航線為L，所以橫向偏差為激光雷達到L理想導航線的距離。由于點云數據經過姿態傳感器校正，L趨向于水平，斜率較小，采用L距激光雷達的垂直距離作為橫向偏差，橫向偏差bias計算公式為

1.4 導航控制

作業平臺的移動是通過上位機通過串口通訊給下位機發送速度控制指令，下位機輸出PWM信號，通過電機驅動器，控制直流無刷電機轉速而實現。本研究中用電機驅動器驅動左右兩個電機，實現作業平臺的差速轉向。

本研究采用PD控制方式進行作業平臺的導航控制。導航控制的輸入量為作業平臺的橫向偏差kbias。輸出kU為左右兩側直流電機速度差值，用于調整作業平臺方向。

式中pK為比例系數；Kd為積分系數；Uk為k時刻輸出量，m/s；biask為k時刻橫向偏差，cm；biask-1為k-1時刻橫向偏差，cm。

2 試驗與結果分析

2.1 試驗設計

為檢驗基于激光雷達作業平臺的導航算法性能，分別在北京市平谷區桃園、四川廣安縣柑橘園、海淀區東升八家郊野公園北區松樹林開展試驗。試驗場景及場地參數如表2所示。

表2 試驗場景及場地參數 Table 2 Test scene and site parameters

激光雷達傳感器安裝在作業平臺的正前方，距離地面垂直高度950 mm（高度可調），水平掃描，掃描角度為360°，掃描半徑為15 m，角度分辨率為0.333°。履帶作業平臺的初始速度為0.5 m/s。

在樹林中隨機挑選3處行間（行1、行2、行3）作為導航試驗環境，每一行重復導航3次，記錄激光雷達數據、姿態傳感器數據與橫向偏差數據。

2.2 結果與分析

北京市平谷區桃園、四川廣安縣柑橘園、海淀區東升八家郊野公園北區松樹林3處場景中激光雷達實時生成參考導航線如圖7所示。

以北京市海淀區八家郊野公園松樹林試驗的場景為例，將作業平臺導航過程中的激光雷達點云數據與姿態傳感器數據用Cartographer建圖并生成行走軌跡，分別在行1、2、3中試驗導航效果如圖8所示。

表3為試驗時的橫向偏差統計結果。由表3可知，在9次導航試驗中最大橫向偏差為107.7 mm，橫向偏差絕對值最大為17.8 mm，橫向偏差的標準差最大為18.7 mm，9次試驗的導航行走軌跡作圖如圖9所示。其中行2第2次試驗中最大橫向偏差處，橫向偏差為98.9 mm；行3第2次試驗中最大橫向偏差處，橫向偏差為107.7 mm，這兩次試驗此處橫向偏差跳變明顯，此處橫向偏差過大主要是路面不平，激光雷達隨著車身而動，掃到地面或者松樹樹冠導致。9次試驗軌跡沿中線均勻分布，并且能夠快速響應并趨向平穩，基本滿足行間自主作業要求。

表3 試驗橫向偏差統計 Table 3 Test lateral deviation statistics

圖9中試驗軌跡小幅震蕩是由于作業平臺行進過程中，底盤震動較大，導致架設于底盤之上的激光雷達小幅震蕩，從而導致的測量誤差。后續改進試驗中，或可架設云臺進行改進。9次試驗軌跡圖中大幅度震蕩是由于PID控制導航時，微分系數較小，阻尼不夠，導致在震蕩中沒有迅速趨于平穩，同時由于沒有采用分段PID控制，導致導航控制不夠精細。后續研究中采用分段PID控制，應該能夠有所改進。9組試驗中橫向偏差絕對值的平均值最大為17.8 mm，標準偏差最大為18.7 mm，結合圖9的運行軌跡，表明本研究的基于激光雷達的作業平臺導航控制系統能夠讓作業平臺橫向偏差趨于0以獲得穩定的導航行為。

3 結 論

本文針對傳統導航方式在果園或林間的郁閉環境中GPS信號弱、光強影響大等問題，提出了基于激光雷達的林間作業平臺導航方法，結論如下：

1）設計了最小二乘法與支持向量機組合的樹林行間導航算法，在桃園、柑橘園和松樹林3種不同環境進行了測試，結果表明該算法均能有效提取得到參考導航路徑，可適應多種復雜環境。

2）在松樹林開展了平臺導航試驗，將所設計算法的預測導航線與當前位置的橫向偏差作為平臺PID控制器的反饋輸入，進行平臺行進的方向調節，結果表明：在作業平臺的速度為0.5 m/s條件下，最大橫向偏差為107.7 mm，絕對偏差的平均值不超過17.8 mm，證明該導航方法能滿足作業平臺在林間自主導航的需求，可為林間地面裝備的自主導航作業提供一種技術參考。