技術領域

本發明屬于導航技術領域，尤其涉及一種果園機械的導航定位方法。

背景技術

精確導航是實現智能化果園機械裝備的重要支持技術之一。導航定位方法的研究作為果園機械導航的基礎工作，其性能直接影響果園機械導航控制精度，因此，提高果園機械導航定位的精度，是提高果園機械自動行走質量的首要問題。

目前，果園機械導航定位方法常見的是利用視覺傳感器或者激光掃描儀獲取導航信息，視覺圖像具有收集的信息豐富、目標信息完整的優點，但由于果園環境中復雜，存在大量的噪聲信號，會造成圖像信息丟失，導致存在實時性的問題；激光導航具有探測距離遠，受光照等外界環境因素影響小等優點，在果園環境下，果樹恰好可以看作障礙物，可以利用激光掃描的果樹位置信息作為導航信息，但激光掃描也存在獲取障礙物信息不完整，在障礙物缺失時無法獲取導航信息的缺點。

鑒于上述利用單一傳感器的導航方法中出現的問題，現有技術中也提出了利用組合定位的導航方法，主要利用視覺傳感器和激光的組合，利用視覺傳感器獲取用于果園機械導航的基本導航參數，利用激光檢測障礙物，通過卡爾曼濾波等方法實現兩種信息的融合，獲得用于果園機械的導航信息，但由于視覺傳感器受環境影響大，果園機械并不能穩定的提取導航路徑，同時在果園地頭，由于果樹的缺失，視覺傳感器和激光都無法為果園機械的轉彎提供有效導航參數。

發明內容

針對上述現有技術中存在的問題與不足，本發明的目的在于提供一種高精度的果園機械導航定位方法，該方法通過將多種傳感器組合使用，實現在整個果園環境下的導航定位。

本發明提供一種果園機械導航定位方法，包括：

（1）當果園機械在果樹行間行駛時，通過融合激光掃描信息和視覺傳感器檢測得到的信息構建果園機械前方三維環境，提取有效導航定位信息。

（2）當果園機械即將行駛到果樹行盡頭時，利用卡爾曼濾波器融合果園機械內部傳感器測量得到的信息，獲得果園機械有效導航定位信息。

（3）當果園機械行駛到果園地頭時，首先采用最優控制規劃果園機械地頭轉彎路徑，利用GPS傳感器測量得到其位置信息，通過比較理論路徑坐標與實際位置坐標，得到果園機械有效導航定位信息。

步驟1中，以激光掃描得到的果園機械兩側的果樹位置信息為主要導航信息，獲取用于果園機械導航的初始航向偏差和橫向偏差，以視覺傳感器測量得到的果樹行間信息為輔助導航信息，通過兩者信息的融合，構建果園機械前方三維環境，預測果園機械在果樹行間的通過性。

步驟1中，所述激光掃描信息和視覺傳感器信息的融合是：利用激光掃描儀對圖像實時標定，將激光掃描儀與CCD攝像機安裝在同一豎直位置，分別建立激光掃描直角坐標系和視覺坐標系，用激光掃描所擬合出的路徑與y′軸的截距已知，視覺傳感器擬合的路徑與y軸的截距與其相等，因此能夠建立圖像中像素點數量與實際環境中距離長度的對應關系。在圖像中建立兩個在實際環境中都垂直于水平面的平面，并在實際環境中對這兩個面進行等面積劃分，在圖像柵格中反映出實際環境中相對應空間位置的障礙物信息。通過判斷二值化圖像中障礙物信息量，預測機器人在果樹行間的通過性。

步驟2中，利用航向角傳感器測量果園機械的航向角，利用角度傳感器測量果園機械的前輪轉角，利用速度傳感器測量果園機械的速度，然后構建卡爾曼濾波器，對三者信息進行融合，獲取用于果園機械導航的航向偏差和前輪轉角。

步驟3中，首先利用最優控制規劃轉彎路徑，然后利用GPS傳感器測量得到果園機械的位置信息，比較理論路徑坐標與（實際位置）坐標，獲得用于果園機械導航的橫向偏差和航向偏差。

本發明與現有技術相比具有如下優點和效果：

（1）本發明提出了果園機械行間行走的多源信息融合方法，利用激光掃描信息實時標定視覺檢測得到的信息，可以構建果園機械前方的三維環境，預測果園機械在果樹行間的通過性，并為行間導航控制提供更精確的橫向偏差和航向偏差。

（2）本發明組合了多種傳感器用于果園機械的導航定位。將果園機械的導航定位分為三部分，充分考慮果園環境的特點以及各種傳感器的優缺點，每段利用不同傳感器獲取果園機械的導航信息，提高了果園機械的導航精度和適應性。

附圖說明

圖1為果園機械導航定位系統結構框圖。

圖2為果園機械在整個果園環境中的運動示意圖。

圖3為果園機械在果樹行間信息流向圖。

圖4為果園機械在即將行駛到果樹行盡頭信息流向圖。