專利名稱：一種果園機械導(dǎo)航定位方法
技術(shù)領(lǐng)域：
本發(fā)明屬于導(dǎo)航技術(shù)領(lǐng)域，尤其涉及一種果園機械的導(dǎo)航定位方法。
背景技術(shù)：
精確導(dǎo)航是實現(xiàn)智能化果園機械裝備的重要支持技術(shù)之一。導(dǎo)航定位方法的研究作為果園機械導(dǎo)航的基礎(chǔ)工作，其性能直接影響果園機械導(dǎo)航控制精度，因此，提高果園機械導(dǎo)航定位的精度，是提高果園機械自動行走質(zhì)量的首要問題。目前，果園機械導(dǎo)航定位方法常見的是利用視覺傳感器或者激光掃描儀獲取導(dǎo)航信息，視覺圖像具有收集的信息豐富、目標信息完整的優(yōu)點，但由于果園環(huán)境中復(fù)雜，存在大量的噪聲信號，會造成圖像信息丟失，導(dǎo)致存在實時性的問題；激光導(dǎo)航具有探測距離遠，受光照等外界環(huán)境因素影響小等優(yōu)點，在果園環(huán)境下，果樹恰好可以看作障礙物，可以利用激光掃描的果樹位置信息作為導(dǎo)航信息，但激光掃描也存在獲取障礙物信息不完整， 在障礙物缺失時無法獲取導(dǎo)航信息的缺點。鑒于上述利用單一傳感器的導(dǎo)航方法中出現(xiàn)的問題，現(xiàn)有技術(shù)中也提出了利用組合定位的導(dǎo)航方法，主要利用視覺傳感器和激光的組合，利用視覺傳感器獲取用于果園機械導(dǎo)航的基本導(dǎo)航參數(shù)，利用激光檢測障礙物，通過卡爾曼濾波等方法實現(xiàn)兩種信息的融合，獲得用于果園機械的導(dǎo)航信息，但由于視覺傳感器受環(huán)境影響大，果園機械并不能穩(wěn)定的提取導(dǎo)航路徑，同時在果園地頭，由于果樹的缺失，視覺傳感器和激光都無法為果園機械的轉(zhuǎn)彎提供有效導(dǎo)航參數(shù)。

發(fā)明內(nèi)容
針對上述現(xiàn)有技術(shù)中存在的問題與不足，本發(fā)明的目的在于提供一種高精度的果園機械導(dǎo)航定位方法，該方法通過將多種傳感器組合使用，實現(xiàn)在整個果園環(huán)境下的導(dǎo)航定位。本發(fā)明提供一種果園機械導(dǎo)航定位方法，包括
(1)當(dāng)果園機械在果樹行間行駛時，通過融合激光掃描信息和視覺傳感器檢測得到的信息構(gòu)建果園機械前方三維環(huán)境，提取有效導(dǎo)航定位信息。(2)當(dāng)果園機械即將行駛到果樹行盡頭時，利用卡爾曼濾波器融合果園機械內(nèi)部傳感器測量得到的信息，獲得果園機械有效導(dǎo)航定位信息。(3)當(dāng)果園機械行駛到果園地頭時，首先采用最優(yōu)控制規(guī)劃果園機械地頭轉(zhuǎn)彎路徑，利用GPS傳感器測量得到其位置信息，通過比較理論路徑坐標與實際位置坐標，得到果園機械有效導(dǎo)航定位信息。步驟1中，以激光掃描得到的果園機械兩側(cè)的果樹位置信息為主要導(dǎo)航信息，獲取用于果園機械導(dǎo)航的初始航向偏差和橫向偏差，以視覺傳感器測量得到的果樹行間信息為輔助導(dǎo)航信息，通過兩者信息的融合，構(gòu)建果園機械前方三維環(huán)境，預(yù)測果園機械在果樹行間的通過性。
步驟1中，所述激光掃描信息和視覺傳感器信息的融合是利用激光掃描儀對圖像實時標定，將激光掃描儀與CCD攝像機安裝在同一豎直位置，分別建立激光掃描直角坐標系和視覺坐標系，用激光掃描所擬合出的路徑與J^軸的截距已知，視覺傳感器擬合的路徑與y軸的截距與其相等，因此能夠建立圖像中像素點數(shù)量與實際環(huán)境中距離長度的對應(yīng)關(guān)系。在圖像中建立兩個在實際環(huán)境中都垂直于水平面的平面，并在實際環(huán)境中對這兩個面進行等面積劃分，在圖像柵格中反映出實際環(huán)境中相對應(yīng)空間位置的障礙物信息。通過判斷二值化圖像中障礙物信息量，預(yù)測機器人在果樹行間的通過性。步驟2中，利用航向角傳感器測量果園機械的航向角，利用角度傳感器測量果園機械的前輪轉(zhuǎn)角，利用速度傳感器測量果園機械的速度，然后構(gòu)建卡爾曼濾波器，對三者信息進行融合，獲取用于果園機械導(dǎo)航的航向偏差和前輪轉(zhuǎn)角。步驟3中，首先利用最優(yōu)控制規(guī)劃轉(zhuǎn)彎路徑，然后利用GPS傳感器測量得到果園機械的位置信息，比較理論路徑坐標與(實際位置)坐標，獲得用于果園機械導(dǎo)航的橫向偏差和航向偏差。本發(fā)明與現(xiàn)有技術(shù)相比具有如下優(yōu)點和效果
(1)本發(fā)明提出了果園機械行間行走的多源信息融合方法，利用激光掃描信息實時標定視覺檢測得到的信息，可以構(gòu)建果園機械前方的三維環(huán)境，預(yù)測果園機械在果樹行間的通過性，并為行間導(dǎo)航控制提供更精確的橫向偏差和航向偏差。(2)本發(fā)明組合了多種傳感器用于果園機械的導(dǎo)航定位。將果園機械的導(dǎo)航定位分為三部分，充分考慮果園環(huán)境的特點以及各種傳感器的優(yōu)缺點，每段利用不同傳感器獲取果園機械的導(dǎo)航信息，提高了果園機械的導(dǎo)航精度和適應(yīng)性。


圖1為果園機械導(dǎo)航定位系統(tǒng)結(jié)構(gòu)框圖。圖2為果園機械在整個果園環(huán)境中的運動示意圖。圖3為果園機械在果樹行間信息流向圖。圖4為果園機械在即將行駛到果樹行盡頭信息流向圖。圖5為果園機械在果園地頭信息流向圖。圖6為視覺坐標系。圖7為行間有效導(dǎo)航參數(shù)提取流程圖。圖8為果園機械在果園環(huán)境中導(dǎo)航定位技術(shù)流程圖。
具體實施例方式為使本發(fā)明的目的、技術(shù)方案和優(yōu)點更加清楚，下面結(jié)合實施例對本發(fā)明作進一步詳細的描述，但本發(fā)明的實施方式不限于此。本實施采用果園機械為本體，其導(dǎo)航系統(tǒng)主要包括導(dǎo)航傳感器組合、PC機、 RS232串口、A/D轉(zhuǎn)換卡和圖像信息采集卡。導(dǎo)航傳感器主要由激光掃描儀、視覺傳感器、 GPS傳感器和內(nèi)部傳感器組(航向角傳感器、角度傳感器及速度傳感器)組成。激光掃描儀采集果園機械前方兩側(cè)果樹的位置信息；視覺傳感器檢測果園機械前方障礙物信息；GPS在地頭轉(zhuǎn)彎時采集果園機械的位置信息；速度傳感器測量果園機械速度信息，角度傳感器測量果園機械前輪轉(zhuǎn)角；航向角傳感器測量果園機械航向角。PC機進行數(shù)據(jù)采集與處理、多源信息融合、路徑規(guī)劃及導(dǎo)航控制等任務(wù)。激光掃描儀、GPS傳感器與航向角傳感器采集到的數(shù)據(jù)通過RS232串口傳送給PC機，角度傳感器通過A/D轉(zhuǎn)換卡將采集到的角度模擬信號傳送給PC機，速度傳感器通過A/D轉(zhuǎn)換卡將采集到的速度信號傳送給PC機，視覺傳感器將采集到的信息通過圖像采集卡傳送到PC機上。所有軟件程序均采用Visual C++編程工具在Windows XP操作系統(tǒng)下開發(fā)。果園機械導(dǎo)航定位系統(tǒng)框圖如圖1所示。為實現(xiàn)果園機械在整個果園環(huán)境中的自動行走，本發(fā)明將導(dǎo)航定位分為三部分， 如圖2所示
1、當(dāng)果園機械在果樹行間行駛時，即圖2中的AB段，通過融合激光掃描信息和視覺傳感器檢測得到的信息構(gòu)建果園機械前方三維環(huán)境，提取有效導(dǎo)航定位信息，預(yù)測果園機械在果樹行間的通過性。此部分信息的流向如圖3所示。2、當(dāng)果園機械即將行駛到果樹行盡頭時，即圖2中的BC段，此時果樹的數(shù)量不能滿足激光掃描獲取有效導(dǎo)航信息，GPS傳感器受樹冠的遮擋不能穩(wěn)定接收衛(wèi)星信號，利用卡爾曼濾波器融合果園機械內(nèi)部傳感器測量得到的速度、角度及航向角信息，獲得果園機械有效導(dǎo)航定位信息。此部分信息的流向如圖4所示。3、當(dāng)果園機械行駛到果園地頭時，GPS傳感器能夠穩(wěn)定接收衛(wèi)星信號，首先采用最優(yōu)控制規(guī)劃果園機械地頭轉(zhuǎn)彎路徑，利用GPS傳感器測量得到其位置信息，通過比較理論路徑坐標與實際位置坐標，得到果園機械有效導(dǎo)航定位信息。此部分信息的流向如圖5所
7J\ ο具體步驟包括
1.果樹行間的導(dǎo)航定位
(1)通過激光掃描獲取果樹位置信息；以激光掃描儀的位置為原點，果園機械本體前進方向為r軸，建立直角坐標系，將激光掃描得到的果樹極坐標位置點轉(zhuǎn)換到直角坐標系， 完成坐標系內(nèi)所有果樹樹干特征參數(shù)的提取，獲得主要導(dǎo)航路徑信息，并得到導(dǎo)航路徑與 1’軸的截距。(2)通過視覺傳感器獲取果樹行間信息，首先對獲取的圖像進行預(yù)處理；然后采用圖像分割的方法定位樹干與地面的交點，通過這些交點進行路徑的擬合，作為輔助導(dǎo)航信肩、ο(3)利用激光掃描儀對圖像實時標定，實現(xiàn)激光信息和視覺信息的融合。建立視覺坐標系，如圖6所示，以攝像機為中心，照片的底邊為χ軸，其垂直方向為y軸，建立直角坐標系xoy，L為通過圖像擬合出的路徑、w為機械本體寬度的一半，H為機械本體的高度，1 為路徑L與y軸的截距。將激光掃描儀與CXD攝像機安裝在同一豎直位置，兩個坐標系中的截距相等，建立圖像中像素點數(shù)量與實際環(huán)境中距離長度的對應(yīng)關(guān)系。(4)以圖像中路徑為中心，建立距路徑L距離為W、高度為H的兩個面，兩個面在實際環(huán)境中都垂直于水平面。在實際環(huán)境中對這兩個面進行等面積劃分，然后利用激光掃描信息對圖像信息進行標定，在圖像柵格中反映出實際環(huán)境中相對應(yīng)空間位置的障礙物信息，從而可以構(gòu)建果園機械前方的三維環(huán)境，預(yù)測果園機械在果樹行間的通過性。(5)激光掃描信息通過RS232串口 1傳送給計算機，圖像信息通過圖像采集卡傳送給計算機。
行間有效導(dǎo)航參數(shù)提取技術(shù)流程如圖7所示。首先利用激光掃描儀獲取果園機械前方位置信息，然后對激光信息進行分析處理，完成果樹樹干位置信息的提取，獲取果樹行中線。同時，利用視覺傳感器采集果園機械前方環(huán)境圖像，并對圖像進行處理，識別出果樹行接地點，獲取果樹行中線。將處理后的激光掃描信息和視覺傳感器采集的信息進行融合， 構(gòu)建果園機械前方三維環(huán)境，完成對前方障礙物的檢測，提取有效的導(dǎo)航定位信息。2.即將行駛到果樹行盡頭時的導(dǎo)航定位
當(dāng)果園機械即將行駛到果樹行盡頭時，激光能掃描到的果樹數(shù)量不能提供準確的導(dǎo)航信息，由于樹冠遮擋，GPS也不能穩(wěn)定接收衛(wèi)星信號，此時融合果園機械內(nèi)部傳感器信息，為果園機械提供有效導(dǎo)航信息。利用航向角傳感器檢測得到果園機械航向角信息，利用角度傳感器檢測得到果園機械前輪轉(zhuǎn)角信息，利用速度傳感器檢測得到果園機械速度信息。速度信息和前輪轉(zhuǎn)角信息通過A/D轉(zhuǎn)換卡傳送給計算機，航向角信息通過RS232串口 2傳送給計算機。構(gòu)建卡爾曼濾波器融合三者信息，得到用于果園機械導(dǎo)航的航向偏差和前輪轉(zhuǎn) 本發(fā)明采用如下公式
在不考慮果園機械重力和側(cè)滑時，根據(jù)果園機械的運動學(xué)關(guān)系，果園機械的運動學(xué)方程式為
f 二 Pr cos G
Jf-Pr H 錄
X、y為果園機械后輪軸中心點的坐標，m ； θ為果園機械的航向角，rad ； α為果園機械的前輪轉(zhuǎn)角，rad ；V為果園機械的速度，m/s ；L為果園機械前后輪的軸距，m ；u為轉(zhuǎn)向角速度，rad/s0 系統(tǒng)的非線性狀態(tài)方程為
0"
系統(tǒng)差分方程為
=Γ 1 “ Saf^1 ^ 0 1 0#t0 0 13η, _0
系統(tǒng)觀測方程為
在力系統(tǒng)離散觀測序列，■^力k時刻的測量噪聲et^力k-Ι時刻的測量噪聲。
3.果園地頭的定位導(dǎo)航
在果園地頭，激光能掃描到的果樹數(shù)量不能提供準確的導(dǎo)航信息，同時因為不受樹冠的遮擋，GPS可以穩(wěn)定的接收衛(wèi)星信號。因此，在地頭轉(zhuǎn)彎處，利用GPS傳感器獲取果園機械有效導(dǎo)航信息。首先基于最優(yōu)控制理論規(guī)劃果園機械轉(zhuǎn)彎的理論路徑，利用GPS傳感器獲取果園機械的實際位置信息，將理論路徑坐標與實際位置坐標相比較，獲取有效的導(dǎo)航信息，即果園機械的橫向偏差和航向偏差。果園機械在整個果園環(huán)境中技術(shù)流程如圖8所示。首先判斷激光掃描到的果樹數(shù)量能否滿足精確的路徑規(guī)劃要求。如果能夠滿足，說明在果樹行間，分別利用激光掃描儀和視覺傳感器獲取果園機械前方兩側(cè)果樹位置信息和前方環(huán)境信息，并對信息分析與處理， 完成行間主要導(dǎo)航信息和行間輔助導(dǎo)航信息的提取，并融合兩者的信息，構(gòu)建果園機械前方三維環(huán)境，完成前方環(huán)境中障礙物的檢測，提取用于果園機械導(dǎo)航的有效導(dǎo)航信息，進行路徑規(guī)劃處理，獲得果園機械的橫向偏差和航向偏差。當(dāng)掃描到的果樹數(shù)量不能夠滿足要求時，判斷果園機械是否已到地頭，如果沒到地頭，利用航向角傳感器檢測果園機械航向角信息，利用角度傳感器檢測得到果園機械前輪轉(zhuǎn)角信息，利用速度傳感器檢測得到果園機械速度信息。并構(gòu)建卡爾曼濾波器融合三者的信息，求解果園機械的航向偏差和前輪轉(zhuǎn)角。當(dāng)果園機械已經(jīng)行駛到地頭時，首先基于最優(yōu)控制理論規(guī)劃果園機械轉(zhuǎn)彎的理論路徑，利用GPS傳感器獲取果園機械的實際位置信息，將理論路徑坐標與實際位置坐標相比較，得到果園機械的橫向偏差和航向偏差。
權(quán)利要求
1.一種果園機械導(dǎo)航定位方法，其特征在于，包括以下步驟1)當(dāng)果園機械在果樹行間行駛時，通過融合激光掃描信息和視覺傳感器檢測得到的信息，構(gòu)建果園機械前方三維環(huán)境，提取有效導(dǎo)航信息；2)當(dāng)果園機械即將行駛到果樹行盡頭時，利用卡爾曼濾波器融合果園機械內(nèi)部傳感器測量得到的信息，獲得用于果園機械導(dǎo)航定位的信息；3)當(dāng)果園機械行駛到果園地頭時，首先采用最優(yōu)控制規(guī)劃果園機械地頭轉(zhuǎn)彎路徑，利用GPS傳感器測量得到其位置信息，通過比較理論路徑坐標與實際位置坐標，獲得用于果園機械導(dǎo)航定位的信息。
2.根據(jù)權(quán)利要求1所述的果園機械導(dǎo)航定位方法，其特征在于果園機械在果樹行間導(dǎo)航定位是以激光掃描得到的果園機械兩側(cè)果樹位置信息為主要導(dǎo)航信息，獲取用于果園機械導(dǎo)航的初始航向偏差和橫向偏差，以視覺傳感器測量得到的果樹行間信息為輔助導(dǎo)航信息，通過兩者信息的融合，構(gòu)建果園機械前方三維環(huán)境，預(yù)測果園機械在果樹行間的通過性。
3.根據(jù)權(quán)利要求2所述的果園機械導(dǎo)航定位方法，其特征在于利用激光掃描信息實時標定視覺傳感器檢測得到的信息，將激光掃描儀和視覺傳感器建立直角坐標系，建立圖像中像素點數(shù)量與實際環(huán)境中距離長度的對應(yīng)關(guān)系；然后以圖像中路徑為中心，在圖像上建立兩個在實際環(huán)境中垂直于水平面的面，在圖像柵格中反映實際環(huán)境對應(yīng)空間的障礙物 fn息ο
4.根據(jù)權(quán)利要求1所述的果園機械導(dǎo)航定位方法，其特征在于果園機械在即將行駛到果樹行盡頭時的導(dǎo)航定位是利用航向角傳感器測量果園機械的航向角，利用角度傳感器測量果園機械的前輪轉(zhuǎn)角，利用速度傳感器測量果園機械的速度，然后構(gòu)建卡爾曼濾波器，對三者信息進行融合，獲取用于果園機械導(dǎo)航的橫向偏差和前輪轉(zhuǎn)角。
5.根據(jù)權(quán)利要求1所述的果園機械導(dǎo)航定位方法，其特征在于果園機械行駛到果園地頭的導(dǎo)航定位是首先利用最優(yōu)控制規(guī)劃轉(zhuǎn)彎路徑，然后利用GPS傳感器得到果園機械的定位坐標，比較理論路徑坐標與實際位置坐標，獲得用于果園機械導(dǎo)航的橫向偏差和航向偏差。
全文摘要
本發(fā)明公開了一種果園機械導(dǎo)航定位方法。該方法將果園機械在果園中的導(dǎo)航定位分為三部分當(dāng)果園機械在果樹行間行駛時，通過融合激光掃描信息和視覺傳感器檢測得到的信息，得到果園機械前方三維環(huán)境，提取有效的導(dǎo)航信息；當(dāng)果園機械即將行駛到果樹行盡頭時，通過卡爾曼濾波器融合果園機械各內(nèi)部傳感器測量得到的信息，獲得用于果園機械導(dǎo)航定位的信息；當(dāng)果園機械行駛到果園地頭時，利用最優(yōu)控制規(guī)劃轉(zhuǎn)彎路徑，采用GPS傳感器測量得到果園機械位置信息。本發(fā)明可為果園機械導(dǎo)航控制提供精確的橫向偏差和航向偏差，并通過多傳感器的組合定位，實現(xiàn)果園機械在整個果園環(huán)境下的自動行走。
文檔編號G05D1/02GK102368158SQ201110272318
公開日2012年3月7日 申請日期2011年9月15日 優(yōu)先權(quán)日2011年9月15日
發(fā)明者武濤, 王虎, 蔣浩然, 袁池, 陳軍, 雷王利, 韓冰, 高 浩 申請人:西北農(nóng)林科技大學(xué)