本發(fā)明涉及一種導航方法，特別涉及一種農(nóng)機無人駕駛中使用的導航方法。

背景技術：

精準農(nóng)業(yè)技術被認為是21世紀農(nóng)業(yè)科技發(fā)展的前沿，是科技含最高、集成綜合性最強的現(xiàn)代農(nóng)業(yè)生產(chǎn)管理技術之一。精準農(nóng)業(yè)技術根據(jù)空間變異，定位、定時、定量的實施一套現(xiàn)代化農(nóng)事操作技術與管理的系統(tǒng)，是信息技術與農(nóng)業(yè)生產(chǎn)全面結(jié)合的一種新型農(nóng)業(yè)技術。

精準農(nóng)業(yè)的應用于快速發(fā)展，能夠充分挖掘農(nóng)田最大的生產(chǎn)潛力、合理利用水肥資源、減少環(huán)境污染，大幅度提高農(nóng)產(chǎn)品產(chǎn)量和品質(zhì)。

發(fā)展精準農(nóng)業(yè)技術是解決我國農(nóng)業(yè)由傳統(tǒng)農(nóng)業(yè)向現(xiàn)代化農(nóng)業(yè)發(fā)展過程中所面臨的確保農(nóng)產(chǎn)品總量、調(diào)整農(nóng)業(yè)產(chǎn)業(yè)結(jié)構、改善農(nóng)產(chǎn)品品質(zhì)和質(zhì)量、資源嚴重不足且利用率低、環(huán)境污染等問題的有效解決方式，也是中國農(nóng)業(yè)現(xiàn)代化發(fā)展與轉(zhuǎn)型升級的必經(jīng)之路。

衛(wèi)星導航技術是精準農(nóng)業(yè)技術的基本組成之一，使農(nóng)機實現(xiàn)自動行駛，農(nóng)機作業(yè)前只需要設定好參數(shù)后，導航系統(tǒng)便引導農(nóng)機進入自動作業(yè)模式，開始進行直線耕作。在農(nóng)機自動導航的過程中，農(nóng)田的環(huán)境惡劣且復雜，大型的農(nóng)田里可能會存在電線桿、田埂、土丘、牲畜以及隨時出現(xiàn)的勞作人員等，這些因素都為無人駕駛農(nóng)機的實現(xiàn)提出了新的挑戰(zhàn)。現(xiàn)有技術中，使用衛(wèi)星導航技術可以實現(xiàn)農(nóng)機在農(nóng)田里自動行走，但是農(nóng)機無法準確識別農(nóng)機前方的障礙物，即農(nóng)機無法感應農(nóng)田環(huán)境，更不用說根據(jù)感知到的農(nóng)田環(huán)境自動做停車等待還是繼續(xù)行駛等處理，農(nóng)機作業(yè)時必須得有駕駛員輔助操控農(nóng)機的動作，一不注意，農(nóng)機就會與前方障礙物碰撞；因此迫切需要研究出一套基于農(nóng)田環(huán)境感知的導航控制方法使無人駕駛的農(nóng)機具備對周邊環(huán)境進行感知的能力，一旦遇到上述農(nóng)田中存在電線桿、田埂、土丘、牲畜以及隨時出現(xiàn)的勞作人員等情況，能夠及時采取停車等待、避障等應急處理。

技術實現(xiàn)要素：

針對現(xiàn)有技術中的缺陷，本發(fā)明的目的在于克服上述現(xiàn)有技術中的不足之處，解決現(xiàn)有技術中無人駕駛的農(nóng)機無法感知農(nóng)田環(huán)境的技術問題，提供一種用于農(nóng)機無人駕駛的農(nóng)田環(huán)境感知方法，本發(fā)明實現(xiàn)了農(nóng)田環(huán)境的感知，感知精度高，識別農(nóng)機前方障礙物的準確度高，提高農(nóng)機無人駕駛時的可靠性。

本發(fā)明的目的是這樣實現(xiàn)的：一種基于農(nóng)田環(huán)境感知的農(nóng)機導航控制方法，具體包括以下步驟，

步驟1：農(nóng)機作業(yè)前，對攝像機進行標定，攝像機空間坐標變換，再對雷達視覺聯(lián)合標定，使得雷達和視覺信息在空間上融合；

步驟2：農(nóng)機作業(yè)時，距離檢測裝置一實時檢測雷達與地面間的高度變化△h_st，距離檢測裝置二實時檢測攝像機與地面間的高度變化△h_ct，工控機進行數(shù)據(jù)處理實時調(diào)整雷達與攝像機坐標的轉(zhuǎn)換關系，使雷達與攝像機在作業(yè)條件下實現(xiàn)在空間上的同步；

步驟3：工控機解算接收到的毫米波雷達數(shù)據(jù)，確定有效目標，選出農(nóng)機作業(yè)前方雷達感興趣的區(qū)域，確定最危險目標，同步進行攝像機圖像的采集；

步驟4：根據(jù)雷達的信息判斷出最危險目標的運動狀態(tài)，工控機根據(jù)最危險目標的運動狀態(tài)規(guī)劃農(nóng)機行走路徑，根據(jù)雷達與相機采集到的最危險目標的圖像數(shù)據(jù)，判斷出最危險目標的類型，工控機將解析出來的動作指令傳輸給導航箱，導航箱控制農(nóng)機做相應的動作；

其中，農(nóng)機作業(yè)時，農(nóng)機的行駛速度勻速；

所述距離檢測裝置一和距離檢測裝置二的結(jié)構相同，距離檢測裝置一安裝在農(nóng)機前側(cè)且設置在雷達正下方，距離檢測裝置二安裝在農(nóng)機下側(cè)且設置在攝像機正下方位置；所述距離檢測裝置一包括具有容納腔且可開合的殼體，容納腔為圓柱體狀，所述殼體內(nèi)設有可沿著殼體內(nèi)壁做上下直線運動的滑塊，殼體內(nèi)設有檢測滑塊頂部與殼體內(nèi)壁上側(cè)距離變化的距離傳感器，所述滑塊和殼體內(nèi)壁頂部之間連接有2個連接件，連接件關于滑塊在寬度方向上的中心對稱設置；所述連接件包括導向套，所述導向套內(nèi)設有可沿著導向套內(nèi)壁滑動的導向桿，滑塊的前后兩側(cè)與殼體內(nèi)壁之間留有可容納導向桿的間隙，所述導向套的底部開有導向孔并設有限制導向桿遠離導向套的限位階，所述導向桿穿過導向孔與滑塊外側(cè)連接，所述導向套的容納腔正上方對應的殼體上開有至少一個透氣孔，所述滑塊的底部安裝有可在地面上滾動的萬向滾輪；

工控機接收所述距離傳感器發(fā)送過來的數(shù)據(jù)信號并進行數(shù)據(jù)處理。

為了實現(xiàn)雷達和攝像機在空間上的初步同步，所述步驟1中的將車輛坐標轉(zhuǎn)換成圖像像素坐標具體包括以下步驟，

步驟1.1：農(nóng)機作業(yè)前，地面默認為水平，將毫米波雷達固定安裝在農(nóng)機的前側(cè)且位于農(nóng)機縱向中軸，雷達發(fā)射面向外，使雷達發(fā)射面與地面垂直；安裝攝像機時使攝像機的光軸與地面平行；

步驟1.2：以雷達的中心為原點建立雷達坐標系0₀-X₀Y₀Z₀，毫米波雷達所在平面由X₀軸和Y₀軸確定并與Z₀軸相垂直，Z₀軸與地面平行且與農(nóng)機中心軸線重合；建立攝像機坐標系Oc-XcYcZc，以攝像機的中心為原點Oc，平面XcOcYc平行于攝像機的成像平面，Zc軸是攝像機的取景光軸且垂直于成像平面；建立車輛坐標系Ow-XwYwZw，Ow為農(nóng)機后軸的中心與車輛的中心軸線的交點，Xw軸水平向右并垂直于農(nóng)機的縱向中軸線，Zw水平向前且與農(nóng)機中心軸線重合，Yw軸垂直于水面地面向上，雷達坐標系的X₀O₀Z₀平面與車輛坐標系的XwOwZw平面平行；

步驟1.3：光軸與成像平面相交的點是圖像主點O’，車輛坐標通過旋轉(zhuǎn)矩陣R和平移向量s_c轉(zhuǎn)換后得到攝像機坐標(x_c，y_c，z_c，1)^T，任意點P的車輛坐標為(x_w，y_w，z_w，1)^T，將車輛坐標轉(zhuǎn)換為攝像機坐標，具體的轉(zhuǎn)換關系如下，

式(1-1)中，R為一個三行三列的正交單位矩陣，s_c(x_c0，y_c0，z_c0)為初始條件下車輛坐標系到攝像機坐標系的1*3平移矩陣，x_c0為攝像機所在的中心軸與車輛中心軸線兩直線的距離，y_c0為初始條件下攝像機距離地面的高度，z_c0為攝像機距離農(nóng)機后軸的距離；

步驟1.4：將攝像機坐標(x_c，y_c，z_c，1)^T轉(zhuǎn)換到圖像物理坐標(x₁，y₁)^T，具體的轉(zhuǎn)換關系如下，

式(1-2)中，f為攝像機的焦距，焦距單位為mm；

步驟1.5：將圖像物理坐標(x₁，y₁)^T轉(zhuǎn)換到圖像像素坐標(u,v)，具體的轉(zhuǎn)換關系如下：

其中，dx,dy分別表示每個像素在橫軸和縱軸上單位大小，u₀、v₀分別為圖像像素坐標系下攝像機光軸和成像平面交點的橫縱坐標，坐標單位為pixel；

步驟1.6：根據(jù)以上公式(1-1)～(1-3)得到圖像像素坐標系到車輛坐標系的轉(zhuǎn)換公式，具體的為，

步驟1.7：為了使雷達和視覺信息在空間上融合，將步驟1.6中的坐標轉(zhuǎn)換關系更新為，

其中，s＝s_c+s₀，s₀的坐標設為(x_s0,y_s0,z_s0)，x_s0＝0，y_s0為初始條件下雷達距離地面的高度，z_s0為雷達與農(nóng)機后軸的距離。

為了提高農(nóng)機作業(yè)過程中雷達和攝像機的融合精度，所述步驟2中的工控機進行數(shù)據(jù)處理實時調(diào)整雷達與攝像機坐標的轉(zhuǎn)換關系，具體的為，根據(jù)農(nóng)機的實際路況實時調(diào)整平移向量s，掃描周期t下調(diào)整后的平移向量s_t＝s_c+s₀+△s_t，實時的車輛坐標與圖像像素坐標的轉(zhuǎn)換關系，具體的為，

其中，△h_ct為掃描周期t下攝像機與地面高度的變化值，△h_st為掃描周期t下雷達與地面高度的變化值，j為掃描周期數(shù)，(u_t,v_t)為農(nóng)機作業(yè)過程中掃描周期t下實時更新計算得到的圖像像素坐標。

為了進一步提高環(huán)境感知精度，所述步驟2中自適應調(diào)整后的平移向量中獲得△h_t的步驟如下，

所述步驟2中實時調(diào)整后的平移向量中獲得△h_t的步驟如下，

步驟2.1：實時計算雷達與地面間的高度變化量以及攝像機與地面間的高度變化量，具體的為，

掃描周期t中i時刻與i-1時刻雷達與地面高度距離變化值為△h_sti，采用平均法計算掃描周期t中雷達相對地面的高度變化量△h_st，

假設掃描周期t中采樣時刻i與采樣時刻i-1下攝像機與地面高度變化值為△h_cti，采用平均法計算掃描周期t中攝像機與地面的高度變化量△h_ct，

步驟2.2：實時計算出掃描周期t下自適應調(diào)整后的平移向量s，具體的為，

其中，k為一個掃描周期中的采樣點的總數(shù)；

此設計中，通過實時檢測雷達距離地面的高度變化以及攝像機距離地面的高度變化，實時更新平移向量s，提高攝像機和雷達空間同步的精度。

為了進一步提高解算雷達數(shù)據(jù)的準確性，所述步驟3中的解算雷達數(shù)據(jù)確定有效目標，具體包括以下步驟，

步驟3.1：對雷達接收到的數(shù)據(jù)按照毫米波雷達協(xié)議進行解算，得到前方物體相對雷達的角度α、距離r、相對速度v、前方物體的反射強度并為每個目標分配唯一一個ID；

步驟3.2：對隨機噪聲信號進行濾波，保證雷達數(shù)據(jù)的連續(xù)有效性，具體的為，定義z＝[r,α,v]^T為雷達的測量值，z(k)為毫米波雷達第k次輸出的測量值，

d²＝S(z(k)-z(k-1))(z(k)-z(k-1))^T＜r_s² (3-1)

過濾掉不符合式(3-1)的數(shù)據(jù)信號；其中，d為相鄰量測向量z(k)、z(k-1)之間的加權歐氏距離，S為加權矩陣，r_s為設定的閾值；

步驟3.3：判定目標是否在農(nóng)機行駛的車道內(nèi)，當雷達前方物體滿足di≤ds時，目標在農(nóng)機行駛車道內(nèi)，否則，目標不在農(nóng)機行駛車道內(nèi)，農(nóng)機行駛車道內(nèi)的目標初選為有效目標，并對其按照由近及遠的準則進行排序編號；在農(nóng)機行駛車道外的目標為非危險目標，將其排除；其中，ds為安全距離閾值，ds＝L/2+ks，di為i采樣點下測得的目標與Z₀軸之間的距離，L為農(nóng)機上懸掛的犁具寬度，ks為設定的安全余量；

步驟3.4：對初選的有效目標進行有效性檢驗，最終確定有效目標；

步驟3.5：根據(jù)確定好的有效目標，通過毫米波雷達獲取的最近距離障礙物確定為候選的最危險目標，若dj≤dmin,dj為毫米波雷達獲取的農(nóng)機與ID是j的有效目標之間的距離，dmin為在毫米波雷達一個掃描周期內(nèi)所獲取的農(nóng)機與最近有效目標的距離，則ID是j的有效目標為最危險目標；

此設計中，先對干擾和噪聲信號產(chǎn)生的隨機噪聲信號進行濾除，提高雷達數(shù)據(jù)解算的準確性；通過對農(nóng)機行駛車道的判別，排除農(nóng)機行駛車道外的障礙物目標，初步選定同車道內(nèi)的障礙物為有效目標，對初選有效目標進行檢驗以進一步確定有效目標，提高有效目標識別的精確度；對有效目標按照距離由近到遠的順序為規(guī)則，確定最危險目標；

所述步驟3.4中對初選的有效目標進行有效性檢驗具體包括以下步驟，

步驟3.4.1：對初選的有效目標進行預測，選取狀態(tài)Sn＝[d_n,v_n,a_n],初選有效目標的狀態(tài)預測方程為，

其中，d_(n+1,n)、v_(n+1,n)、a_(n+1,n)是上一個掃描周期預測的有效障礙物目標的狀態(tài)信息，d_n,v_n,a_n分別表示毫米波雷達第n探測周期內(nèi)測得的有效障礙物目標的相對距離、相對速度、相對加速度，t是毫米波雷達的掃描周期；

步驟3.4.2：通過比較預測的第n+1周期有效目標的狀態(tài)信息和雷達實際測得的第n+1周期有效目標的狀態(tài)信息，具體如下，

其中，d₀、v₀、a₀是設定的有效障礙物目標測量值與預測值之間的誤差閾值；

步驟3.4.3：有效障礙物目標在雷達的掃描周期中被連續(xù)探測到m次以上，同時，滿足步驟3.4.2中公式(3-3)的有效目標與初選有效目標一致，則更新目標的相對距離、相對速度、相對角度、編號信息；否則，初選的有效目標不處于毫米波雷達探測目標中，使用有效目標預測信息對初選的有效目標進行跟蹤，若初選的有效目標在雷達的下一個掃描周期中仍然沒有被探測到，則停止使用對應的初選有效目標信息，更新有效目標信息，并返回步驟3.4.1循環(huán)執(zhí)行；

此設計中，通過上一個掃描預測的有效目標的狀態(tài)信息與測試出來的進行對比，判斷有效目標信息是否一致，以此來進一步排除虛假目標，使有效目標的確定得到進一步保障。

作為本發(fā)明的進一步改進，所述步驟4中判斷最危險目標的動靜狀態(tài)具體包括以下步驟，

所述步驟4中判斷最危險目標的動靜狀態(tài)，工控機根據(jù)最危險目標的運動狀態(tài)規(guī)劃農(nóng)機行走路徑具體包括以下步驟，

步驟4.1：根據(jù)步驟3.5中確定的最危險目標，不斷更新最危險目標的相對速度和相對距離信息，判斷最危險目標與雷達的距離是否在停車距離范圍內(nèi)，即z_d>z_min(4-1)，z_d為毫米波雷達探測到的雷達與最危險目標的相對距離，z_min為設定的停車距離閾值，最危險目標滿足公式(4-1)時，農(nóng)機繼續(xù)行駛；

步驟4.2：根據(jù)相對速度大小判定最危險目標的動靜狀態(tài)，具體如下，

v≠v_車 (4-2)

在連續(xù)的掃描周期內(nèi)，(4-2)式始終成立時，判定目標的狀態(tài)為動態(tài)，此時，工控機發(fā)出聲光報警，z_d≤z_min時，工控機將停車等待指令發(fā)送給導航箱，導航箱控制農(nóng)機做停車等待處理；否則，農(nóng)機繼續(xù)行駛，并返回至步驟3.1循環(huán)執(zhí)行，其中，v為雷達相對目標的速度大小，v_車為農(nóng)機的行駛速度；(4-2)式始終不成立時，判定目標為靜態(tài)，則工控機做出避障處理，具體的為，攝像機掃描出障礙物的邊緣輪廓，工控機根據(jù)農(nóng)機的犁具寬度和農(nóng)機最小轉(zhuǎn)彎半徑設定一條避障路徑；工控機按照設定好的避障路徑解析出農(nóng)機的前輪轉(zhuǎn)角，并將動作指令發(fā)送給導航箱，導航箱控制農(nóng)機的前輪轉(zhuǎn)角使農(nóng)機按照設定的避障路徑行走；

農(nóng)機的左右兩側(cè)中心位置分別安裝有雷達一和雷達二；農(nóng)機在避障過程中，雷達一和雷達二不斷掃描農(nóng)機左右兩側(cè)是否有障礙物，設定雷達一與障礙物的相對距離為d₁，設定雷達二與障礙物的相對距離為d₂，根據(jù)以下公式判斷農(nóng)機是否繼續(xù)按照避障路徑行走，

d₁<d_s0 (4-3)

d₂<d_s0 (4-4)

(4-3)或(4-4)任意一個式子成立時，工控機做出停車等待決策，導航箱控制農(nóng)機停下；否則，農(nóng)機繼續(xù)按照避障路徑行走；

其中，d_s0為設定的轉(zhuǎn)彎安全距離，農(nóng)機上安裝有角度傳感器，前輪轉(zhuǎn)向角由角度傳感器測得，角度傳感器將檢測到的轉(zhuǎn)向角信號傳輸給工控機；

此設計中，判定最危險目標的動靜態(tài)原理簡單，提高響應速度；根據(jù)最危險目標的動靜態(tài)情況做出農(nóng)機進一步的行走決策，若農(nóng)機為靜態(tài)，則農(nóng)機按照設定的避障路徑行走。

為了進一步提高提高獲得理論避障路徑的可靠性，所述步驟4.2中的避障路徑具體的為，

以障礙物的中心為圓心做特征圓，特征圓的半徑為r_min+w/2，避障路徑由圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三組成，圓弧段一的一端與農(nóng)機原始的直線路徑相切，圓弧段一的另一端與直線段一的一端相切，直線段一的另一端和直線段二的一端分別與圓弧段二相切，直線段二的另一端與圓弧段三相切，圓弧段二為特征圓上的一段，圓弧段一和圓弧段三關于圓弧段二的中心線對稱設置，農(nóng)機依次經(jīng)過圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三繞過障礙物，其中，r_min為農(nóng)機的最小轉(zhuǎn)彎半徑，w為農(nóng)機的作業(yè)寬度，障礙物的外接圓半徑小于最小轉(zhuǎn)彎半徑r_min；圓弧段一的半徑為r_min，所述圓弧段三的半徑為r_min，圓弧段一的起點記為H點，圓弧段一的圓心記為O₁點，直線段一與農(nóng)機原始的直線路徑的相交點記為J，直線段一與圓弧段二的相切點記為D，農(nóng)機原始路徑與特征圓的相交點分別記為K和K’，JK＝w/2，圓弧段二的圓心記為O點，O的坐標設為(a，b)，圓弧段二的中心點記為B點，J點的坐標記為(x1，y1)，JD的方程可以寫成：

y＝k(x-x₁)+y₁ (4-5)；

特征圓的方程可以寫成：

(x-a)²+(y-b)²＝r²

r＝r_min+w/2 (4-6)

通過(4-5)和(4-6)可以求出k，D點為JD和特征圓的相交點，以此解出D點坐標；

設點O₁的坐標為(x₂,y₂)，則點O₁到直線JD的距離為：

y₂＝y(tǒng)₁+r_min (4-8)

根據(jù)公式(4-7)和(4-8)求出O₁的坐標；則H點的坐標為(x₂，y₁)，B點的坐標為(a，b+r)。

作為本發(fā)明的進一步改進，所述步驟4中根據(jù)雷達與相機采集到的最危險目標的圖像數(shù)據(jù)，判斷出最危險目標的類型，導航箱控制農(nóng)機做相應的動作，具體包括以下步驟，

步驟4.1a：最危險目標為動態(tài)的情形下，攝像機對最危險目標進行識別，攝像機獲取最危險目標的圖像，將圖像與訓練好的人體樣本訓練庫進行匹配比較，輸出目標識別結(jié)果；

步驟4.2a：導航箱根據(jù)輸出的目標識別結(jié)果控制農(nóng)機動作，若為非人體，則導航箱發(fā)出聲光報警，并控制農(nóng)機做停車等待處理；若目標識別結(jié)果為人體，則導航箱發(fā)出聲光報警，判斷人體是否偏離農(nóng)機行駛車道或者人體向遠離農(nóng)機方向運動，用以下公式判斷，

z_wn+1>z_wn (4-3)

di>ds (4-4)

若雷達探測到的人體目標滿足(4-3)或(4-4)，則農(nóng)機繼續(xù)向前行駛，否則導航箱控制農(nóng)機做停車等待處理；z_wn為第n探測掃描周期雷達相對最危險目標的距離，z_w(n+1)為下一個掃描周期雷達相對最危險目標的距離；

此設計中，先判斷最危險目標的動靜狀態(tài)，若最危險目標始終為靜態(tài)，則認為最危險目標為電線桿、樹木等非生命體，否則，認為最危險目標為農(nóng)作人員或者牲畜，通過攝像機采集最危險目標的圖像并識別出最危險目標是否為人體，輸出目標識別結(jié)果，若為人體，則導航箱發(fā)出聲光報警，因為勞作人員自身有避險意識，勞作人員在聽到農(nóng)機的報警聲后可能會走出農(nóng)機行駛車道或者往遠離農(nóng)機運動方向行走，利用勞作人員的習慣性反應設置判斷程序，適應性好，農(nóng)機自動避開電線桿、牲畜等非人體的同時還可提醒農(nóng)機前方的農(nóng)作人員自覺避讓，并根據(jù)農(nóng)作人員的行為做繼續(xù)行駛或停車等待處理。

作為本發(fā)明的進一步改進，所述距離傳感器安裝在滑塊頂部的中心，距離傳感器正對殼體內(nèi)壁頂部。

作為本發(fā)明的進一步改進，所述距離傳感器安裝在殼體內(nèi)壁頂部的中心，距離傳感器正對滑塊頂部的中心。

農(nóng)機作業(yè)前，攝像機與雷達的標定是在水平地面的條件下進行的；而農(nóng)機作業(yè)時，農(nóng)田地面凹凸不平，由于雷達和攝像機并非安裝于農(nóng)機的同一位置，雷達和攝像機相對地面的高度不同且隨著地形發(fā)生變化；距離檢測裝置一的工作過程具體的為，萬向滾輪沿著凹凸不平的地面滾動，當?shù)孛嫱钩鰰r，凸出地面給萬向滾輪向上的作用力，滑塊沿著殼體內(nèi)壁向上滑動，滑塊推動導向桿向上滑動，距離傳感器檢測滑塊的上升距離，即雷達與地面間的高度變化量；向上凸的地面逐漸水平時，滑塊逐漸向下滑動；當?shù)孛嫦蛳掳歼M時，滑塊在自重的作用下，滑塊向下滑動，直至萬向滾輪與地面接觸，距離傳感器檢測滑塊與殼體內(nèi)壁間的高度變化，距離傳感器實時將檢測到的雷達與當前地面間的高度變化值發(fā)送給工控機；距離檢測裝置二的工作原理跟距離檢測裝置一的工作原理相同，距離檢測裝置二實時將檢測到的攝像機與當前地面間的高度變化值發(fā)送給工控機；

本發(fā)明與現(xiàn)有技術相比，本發(fā)明將毫米波雷達和攝像機結(jié)合起來感知農(nóng)田環(huán)境，實時檢測雷達和攝像機距離地面的高度變化，將高度變化量加入到雷達和攝像機坐標轉(zhuǎn)換的平移向量中，農(nóng)機作業(yè)時，使攝像機和雷達在空間上實現(xiàn)真正的同步，提高攝像機和雷達的融合精度；對噪聲和干擾信號產(chǎn)生的隨機噪聲信號進行濾除，提高雷達探測信號的準確性；根據(jù)設定好的農(nóng)機航向確定為農(nóng)機行駛車道，將農(nóng)機行駛車道內(nèi)的障礙物目標初選為有效目標，再對初選的有效目標進行進一步檢驗，以進一步確定有效目標，提高雷達感知同車道內(nèi)障礙物目標的有效性和準確性；選取最危險目標并跟蹤最危險目標，攝像機以最危險目標的動靜態(tài)為基礎進行目標識別，若最危險目標為動態(tài)，只需要識別出動態(tài)目標是否為人體即可，不需要識別出具體類型，減小運算量，提高響應速度，導航箱根據(jù)圖像識別結(jié)果控制農(nóng)機動作，避免農(nóng)機在無人駕駛時與障礙物碰撞；若識別結(jié)果為人體時，導航箱聲光報警提醒勞作人員避開農(nóng)機，利用人的習慣思維這一特性，不斷檢測人體是否偏離農(nóng)機行駛車道或者人體向遠離農(nóng)機方向運動，導航箱根據(jù)檢測結(jié)果控制農(nóng)機是否做停車等待處理，適應性好；若最危險目標為靜態(tài)，攝像機掃描出障礙物的輪廓，獲取避障路徑，控制農(nóng)機的前輪轉(zhuǎn)向角使農(nóng)機按照設定的避障路徑行走，同時，雷達一和雷達二檢測農(nóng)機避障過程中是否有新的障礙物存在，若有，農(nóng)機停車等待，以確保農(nóng)機的安全作業(yè)；本發(fā)明可應用于農(nóng)機無人駕駛時農(nóng)田環(huán)境自動感知的導航工作中。

附圖說明

圖1為本發(fā)明基于毫米波雷達和攝像機的感知農(nóng)田環(huán)境方法的流程圖。

圖2為本發(fā)明中攝像機坐標系和車輛坐標系的關系示意圖。

圖3為本發(fā)明中攝像機坐標系與圖像物理坐標系的關系示意圖。

圖4為本發(fā)明中圖像物理坐標系與圖像像素坐標系的關系示意圖。

圖5為本發(fā)明中農(nóng)機行駛過程中農(nóng)田環(huán)境示意圖。

圖6為本發(fā)明中農(nóng)機行駛過程中車道判別示意圖。

圖7為本發(fā)明中對初選有效目標進行檢驗以進一步確定有效目標的流程圖。

圖8為本發(fā)明中的避障路徑軌跡圖。

圖9為本發(fā)明中距離檢測裝置一的一個結(jié)構示意圖。

圖10為本發(fā)明中距離檢測裝置一的一個A-A向視圖。

圖11為本發(fā)明中距離檢測裝置的另一個結(jié)構示意圖。

其中，1滑塊，2殼體，3透氣孔，4導向套，5導向桿，6限位階，7容納腔，8距離傳感器，9萬向滾輪。

具體實施方式

下面結(jié)合附圖對本發(fā)明作進一步的說明。

實施例1

如圖1～10所示的一種基于農(nóng)田環(huán)境感知的農(nóng)機導航控制方法，具體包括以下步驟：

步驟1：農(nóng)機作業(yè)前，對攝像機進行標定，攝像機空間坐標變換，再對雷達視覺聯(lián)合標定，使得雷達和視覺信息在空間上融合；

步驟2：農(nóng)機作業(yè)時，距離檢測裝置一實時檢測雷達與地面間的高度變化△h_st，距離檢測裝置二實時檢測攝像機與地面間的高度變化△h_ct，工控機進行數(shù)據(jù)處理實時調(diào)整雷達與攝像機坐標的轉(zhuǎn)換關系，使雷達與攝像機在作業(yè)條件下實現(xiàn)在空間上的同步；

步驟3：工控機解算接收到的毫米波雷達數(shù)據(jù)，確定有效目標，選出農(nóng)機作業(yè)前方雷達感興趣的區(qū)域，確定最危險目標，同步進行攝像機圖像的采集；

步驟4：根據(jù)雷達的信息判斷出最危險目標的運動狀態(tài)，工控機根據(jù)最危險目標的運動狀態(tài)規(guī)劃農(nóng)機行走路徑，根據(jù)雷達與相機采集到的最危險目標的圖像數(shù)據(jù)，判斷出最危險目標的類型，工控機將解析出來的動作指令傳輸給導航箱，導航箱控制農(nóng)機做相應的動作；

其中，農(nóng)機作業(yè)時，農(nóng)機的行駛速度勻速；

距離檢測裝置一和距離檢測裝置二的結(jié)構相同，距離檢測裝置一安裝在農(nóng)機前側(cè)且設置在雷達正下方，距離檢測裝置二安裝在農(nóng)機下側(cè)且設置在攝像機正下方位置；如圖9和圖10所示，距離檢測裝置一包括具有容納腔7且可開合的殼體2，容納腔7為圓柱體狀，殼體2內(nèi)設有可沿著殼體2內(nèi)壁做上下直線運動的滑塊1，滑塊1呈長方體狀，滑塊1頂部的中心安裝有距離傳感器8，距離傳感器8正對殼體2內(nèi)壁頂部；滑塊1和殼體2內(nèi)壁頂部之間連接有2個連接件，連接件關于滑塊1在寬度方向上的中心對稱設置；連接件包括導向套4，導向套4內(nèi)設有可沿著導向套4內(nèi)壁滑動的導向桿，滑塊1的前后兩側(cè)與殼體2內(nèi)壁之間留有可容納導向桿的間隙，導向套4的底部開有導向孔并設有限制導向桿遠離導向套4的限位階6，導向桿穿過導向孔與滑塊1外側(cè)連接，導向套4的容納腔7正上方對應的殼體2上開有至少一個透氣孔3，滑塊1的底部安裝有可在地面上滾動的萬向滾輪9；

工控機接收距離傳感器8發(fā)送過來的數(shù)據(jù)信號并進行數(shù)據(jù)處理；

為了實現(xiàn)攝像機和毫米波雷達在空間上的初步同步，如圖2～4所示，步驟1中的將車輛坐標轉(zhuǎn)換成圖像像素坐標具體包括以下步驟，

步驟1中的將車輛坐標轉(zhuǎn)換成圖像像素坐標具體包括以下步驟，

步驟1.1：農(nóng)機作業(yè)前，地面默認為水平，將毫米波雷達固定安裝在農(nóng)機的前側(cè)且位于農(nóng)機縱向中軸，雷達發(fā)射面向外，使雷達發(fā)射面與地面垂直；安裝攝像機時使攝像機的光軸與地面平行；

步驟1.2：以雷達的中心為原點建立雷達坐標系0₀-X₀Y₀Z₀，毫米波雷達所在平面由X₀軸和Y₀軸確定并與Z₀軸相垂直，Z₀軸與地面平行且與農(nóng)機中心軸線重合；建立攝像機坐標系Oc-XcYcZc，以攝像機的中心為原點Oc，平面XcOcYc平行于攝像機的成像平面，Zc軸是攝像機的取景光軸且垂直于成像平面；建立車輛坐標系Ow-XwYwZw，Ow為農(nóng)機后軸的中心與車輛的中心軸線的交點，Xw軸水平向右并垂直于農(nóng)機的縱向中軸線，Zw水平向前且與農(nóng)機中心軸線重合，Yw軸垂直于水面地面向上，雷達坐標系的X₀O₀Z₀平面與車輛坐標系的XwOwZw平面平行；

步驟1.3：光軸與成像平面相交的點是圖像主點O’，車輛坐標通過旋轉(zhuǎn)矩陣R和平移向量s_c轉(zhuǎn)換后得到攝像機坐標(x_c，y_c，z_c，1)^T，任意點P的車輛坐標為(x_w，y_w，z_w，1)^T，將車輛坐標轉(zhuǎn)換為攝像機坐標，具體的轉(zhuǎn)換關系如下，

式(1-1)中，R為一個三行三列的正交單位矩陣，s_c(x_c0，y_c0，z_c0)為初始條件下車輛坐標系到攝像機坐標系的1*3平移矩陣，x_c0為攝像機所在的中心軸與車輛中心軸線兩直線的距離，y_c0為初始條件下攝像機距離地面的高度，z_c0為攝像機距離農(nóng)機后軸的距離；

步驟1.4：將攝像機坐標(x_c，y_c，z_c，1)^T轉(zhuǎn)換到圖像物理坐標(x₁，y₁)^T，具體的轉(zhuǎn)換關系如下，

式(1-2)中，f為攝像機的焦距，焦距單位為mm；

步驟1.5：將圖像物理坐標(x₁，y₁)^T轉(zhuǎn)換到圖像像素坐標(u,v)，具體的轉(zhuǎn)換關系如下：

其中，dx,dy分別表示每個像素在橫軸和縱軸上單位大小，u₀、v₀分別為圖像像素坐標系下攝像機光軸和成像平面交點的橫縱坐標，坐標單位為pixel；

步驟1.6：根據(jù)以上公式(1-1)～(1-3)得到圖像像素坐標系到車輛坐標系的轉(zhuǎn)換公式，具體的為，

步驟1.7：為了使雷達和視覺信息在空間上融合，將步驟1.6中的坐標轉(zhuǎn)換關系更新為，

其中，s＝s_c+s₀，s₀的坐標設為(x_s0,y_s0,z_s0)，x_s0＝0，y_s0為初始條件下雷達距離地面的高度，z_s0為雷達與農(nóng)機后軸的距離；

將雷達坐標借助共用的車輛坐標系轉(zhuǎn)化為圖像坐標，雷達數(shù)據(jù)經(jīng)三維坐標逆變換，完成目標信息匹配至視覺信息，雷達與攝像頭空間的相對位置借助車輛坐標系求取即可；步驟2中的實時調(diào)整圖像像素坐標與車輛坐標的轉(zhuǎn)換關系，具體的為，根據(jù)農(nóng)機的實際路況實時調(diào)整平移向量s，掃描周期t下調(diào)整后的平移向量s_t＝s_c+s₀+△s_t，實時的車輛坐標與圖像像素坐標的轉(zhuǎn)換關系，具體的為，

其中，△h_ct為掃描周期t下攝像機與地面高度的變化值，△h_st為掃描周期t下雷達與地面高度的變化值，j為掃描周期數(shù)；

步驟2中實時調(diào)整后的平移向量中獲得△h_t的步驟如下，

步驟2.1：實時計算雷達與地面間的高度變化量以及攝像機與地面間的高度變化量，具體的為，

掃描周期t中i時刻與i-1時刻雷達與地面高度距離變化值為△h_sti，采用平均法計算掃描周期t中雷達相對地面的高度變化量△h_st，

假設掃描周期t中采樣時刻i與采樣時刻i-1下攝像機與地面高度變化值為△h_cti，采用平均法計算掃描周期t中攝像機與地面的高度變化量△h_ct，

步驟2.2：實時計算出掃描周期t下自適應調(diào)整后的平移向量s，具體的為，

其中，k為一個掃描周期中的采樣點的總數(shù)。

步驟3中的解算雷達數(shù)據(jù)確定有效目標，具體包括以下步驟，

步驟3.1：對雷達接收到的數(shù)據(jù)按照毫米波雷達協(xié)議進行解算，得到前方物體相對雷達的角度α、距離r、相對速度v、前方物體的反射強度并為每個目標分配唯一一個ID；

步驟3.2：對隨機噪聲信號進行濾波，保證雷達數(shù)據(jù)的連續(xù)有效性，具體的為，定義z＝[r,α,v]^T為雷達的測量值，z(k)為毫米波雷達第k次輸出的測量值，

d²＝S(z(k)-z(k-1))(z(k)-z(k-1))^T＜r_s² (3-1)

過濾掉不符合式(3-1)的數(shù)據(jù)信號；其中，d為相鄰量測向量z(k)、z(k-1)之間的加權歐氏距離，S為加權矩陣，r_s為設定的閾值；

步驟3.3：判定目標是否在農(nóng)機行駛的車道內(nèi)，當雷達前方物體滿足di≤ds時，目標在農(nóng)機行駛車道內(nèi)，否則，目標不在農(nóng)機行駛車道內(nèi)，農(nóng)機行駛車道內(nèi)的目標初選為有效目標，并對其按照由近及遠的準則進行排序編號；在農(nóng)機行駛車道外的目標為非危險目標，將其排除；其中，ds為安全距離閾值，ds＝L/2+ks，di為i采樣點下測得的目標與Z₀軸之間的距離，L為農(nóng)機上懸掛的犁具寬度，ks為設定的安全余量；

下面舉例說明，從圖5中可以看出，B、C這2個障礙物距離農(nóng)機中心的縱向距離大于ds，在農(nóng)機行駛車道外；A、D這2個障礙物距離農(nóng)機中心的縱向距離小于ds，在農(nóng)機行駛車道內(nèi)，則A和D初選為有效目標；

圖6中為障礙物在行駛車道內(nèi)時的顯示，障礙物E距離農(nóng)機中心O_{農(nóng)機}的距離小于L/2+ks，E在農(nóng)機行駛車道內(nèi)；

步驟3.4：對初選的有效目標進行有效性檢驗，最終確定有效目標；

步驟3.5：根據(jù)確定好的有效目標，通過毫米波雷達獲取的最近距離障礙物確定為候選的最危險目標，若dj≤dmin,dj為毫米波雷達獲取的農(nóng)機與ID是j的有效目標之間的距離，dmin為在毫米波雷達一個掃描周期內(nèi)所獲取的農(nóng)機與最近有效目標的距離，則ID是j的有效目標為最危險目標；

步驟3.4中對初選的有效目標進行有效性檢驗具體包括以下步驟，

步驟3.4.1：對初選的有效目標進行預測，選取狀態(tài)Sn＝[d_n,v_n,a_n],初選有效目標的狀態(tài)預測方程為，

其中，d_(n+1,n)、v_(n+1,n)、a_(n+1,n)是上一個掃描周期預測的有效障礙物目標的狀態(tài)信息，d_n,v_n,a_n分別表示毫米波雷達第n探測周期內(nèi)測得的有效障礙物目標的相對距離、相對速度、相對加速度，t是毫米波雷達的掃描周期；

步驟3.4.2：通過比較預測的第n+1周期有效目標的狀態(tài)信息和雷達實際測得的第n+1周期有效目標的狀態(tài)信息，具體如下，

其中，d₀、v₀、a₀是設定的有效障礙物目標測量值與預測值之間的誤差閾值；

步驟3.4.3：有效障礙物目標在雷達的掃描周期中被連續(xù)探測到m次以上，同時，滿足步驟3.4.2中公式(3-3)的有效目標與初選有效目標一致，則更新目標的相對距離、相對速度、相對角度、編號信息；否則，初選的有效目標不處于毫米波雷達探測目標中，使用有效目標預測信息對初選的有效目標進行跟蹤，若初選的有效目標在雷達的下一個掃描周期中仍然沒有被探測到，則停止使用對應的初選有效目標信息，更新有效目標信息，并返回步驟3.4.1循環(huán)執(zhí)行；

步驟4中判斷最危險目標的動靜狀態(tài)，工控機根據(jù)最危險目標的運動狀態(tài)規(guī)劃農(nóng)機行走路徑具體包括以下步驟，

步驟4.1：根據(jù)步驟2.5中確定的最危險目標，不斷更新最危險目標的相對速度和相對距離信息，判斷最危險目標與雷達的距離是否在停車距離范圍內(nèi)，即z_d>z_min(4-1)，z_d為毫米波雷達探測到的雷達與最危險目標的相對距離，z_min為設定的停車距離閾值，最危險目標滿足公式(4-1)時，農(nóng)機繼續(xù)行駛；

步驟4.2：根據(jù)相對速度大小判定最危險目標的動靜狀態(tài)，具體如下，

v≠v_車 (4-2)

在連續(xù)的掃描周期內(nèi)，(4-2)式始終成立時，判定目標的狀態(tài)為動態(tài)，此時，工控機發(fā)出聲光報警，z_d≤z_min時，工控機將停車等待指令發(fā)送給導航箱，導航箱控制農(nóng)機做停車等待處理；否則，農(nóng)機繼續(xù)行駛，并返回至步驟3.1循環(huán)執(zhí)行，其中，v為雷達相對目標的速度大小，v_車為農(nóng)機的行駛速度；(4-2)式始終不成立時，判定目標為靜態(tài)，則工控機做出避障處理，具體的為，攝像機掃描出障礙物的邊緣輪廓，工控機根據(jù)農(nóng)機的犁具寬度和農(nóng)機最小轉(zhuǎn)彎半徑設定一條避障路徑；工控機按照設定好的避障路徑解析出農(nóng)機的前輪轉(zhuǎn)角，并將動作指令發(fā)送給導航箱，導航箱控制農(nóng)機的前輪轉(zhuǎn)角使農(nóng)機按照設定的避障路徑行走；

農(nóng)機的左右兩側(cè)中心位置分別安裝有雷達一和雷達二；農(nóng)機在避障過程中，雷達一和雷達二不斷掃描農(nóng)機左右兩側(cè)是否有障礙物，設定雷達一與障礙物的相對距離為d₁，設定雷達二與障礙物的相對距離為d₂，根據(jù)以下公式判斷農(nóng)機是否繼續(xù)按照避障路徑行走，

d₁<d_s0 (4-3)

d₂<d_s0 (4-4)

(4-3)或(4-4)任意一個式子成立時，工控機做出停車等待決策，導航箱控制農(nóng)機停下；否則，農(nóng)機繼續(xù)按照避障路徑行走；

其中，d_s0為設定的轉(zhuǎn)彎安全距離，農(nóng)機上安裝有角度傳感器，前輪轉(zhuǎn)向角由角度傳感器測得，角度傳感器將檢測到的轉(zhuǎn)向角信號傳輸給工控機；

步驟4中根據(jù)雷達與相機采集到的最危險目標的圖像數(shù)據(jù)，判斷出最危險目標的類型具體包括以下步驟，

步驟4.1a：若最危險目標始終為靜態(tài)，則導航箱控制農(nóng)機做停車等待處理；否則，攝像機對最危險目標進行識別；

步驟4.2a：攝像機獲取最危險目標的圖像，將圖像與訓練好的人體樣本訓練庫進行匹配比較，輸出目標識別結(jié)果；

步驟4.3a：導航箱根據(jù)輸出的目標識別結(jié)果控制農(nóng)機動作，若為非人體，則導航箱發(fā)出聲光報警，并控制農(nóng)機做停車等待處理；若目標識別結(jié)果為人體，則導航箱發(fā)出聲光報警，判斷人體是否偏離農(nóng)機行駛車道或者人體向遠離農(nóng)機方向運動，用以下公式判斷，

z_wn+1>z_wn (4-3)

di>ds (4-4)

若雷達探測到的人體目標滿足(4-3)或(4-4)，則農(nóng)機繼續(xù)向前行駛，否則導航箱控制農(nóng)機做停車等待處理；z_wn為第n探測掃描周期雷達相對最危險目標的距離，z_w(n+1)為下一個掃描周期雷達相對最危險目標的距離；

如圖8所示，步驟4.2中的避障路徑具體的為，

以障礙物的中心為圓心O做特征圓，特征圓的半徑為r_min+w/2，避障路徑由圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三組成，圓弧段一的一端與農(nóng)機原始的直線路徑相切，圓弧段一的另一端與直線段一的一端相切，直線段一的另一端和直線段二的一端分別與圓弧段二相切，直線段二的另一端與圓弧段三相切，圓弧段二為特征圓上的一段，圓弧段一和圓弧段三關于圓弧段二的中心線對稱設置，農(nóng)機依次經(jīng)過圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三繞過障礙物；其中，r_min為農(nóng)機的最小轉(zhuǎn)彎半徑，w為農(nóng)機的作業(yè)寬度，障礙物的外接圓半徑小于最小轉(zhuǎn)彎半徑r_min；圓弧段一的半徑為r_min，圓弧段三的半徑為r_min，圓弧段一的起點記為H點，圓弧段一的圓心記為O₁點，直線段一與農(nóng)機原始的直線路徑的相交點記為J，直線段一與圓弧段二的相切點記為D，農(nóng)機原始路徑與特征圓的相交點分別記為K和K’，JK＝w/2，圓弧段二的圓心記為O點，O的坐標設為(a，b)，圓弧段二的中心點記為B點，J點的坐標記為(x1，y1)，JD的方程可以寫成：

y＝k(x-x₁)+y₁ (4-5)；

特征圓的方程可以寫成：

(x-a)²+(y-b)²＝r²

r＝r_min+w/2 (4-6)

通過(4-5)和(4-6)可以求出k，D點為JD和特征圓的相交點，以此解出D點坐標；

設點O₁的坐標為(x₂,y₂)，則點O₁到直線JD的距離為：

y₂＝y(tǒng)₁+r_min (4-8)

根據(jù)公式(4-7)和(4-8)求出O₁的坐標；則H點的坐標為(x₂，y₁)，B點的坐標為(a，b+r)；

步驟4中根據(jù)雷達與相機采集到的最危險目標的圖像數(shù)據(jù)，判斷出最危險目標的類型，導航箱控制農(nóng)機做相應的動作，具體包括以下步驟，

步驟4.1a：最危險目標為動態(tài)的情形下，攝像機對最危險目標進行識別，攝像機獲取最危險目標的圖像，將圖像與訓練好的人體樣本訓練庫進行匹配比較，輸出目標識別結(jié)果；

步驟4.2a：導航箱根據(jù)輸出的目標識別結(jié)果控制農(nóng)機動作，若為非人體，則導航箱發(fā)出聲光報警，并控制農(nóng)機做停車等待處理；若目標識別結(jié)果為人體，則導航箱發(fā)出聲光報警，判斷人體是否偏離農(nóng)機行駛車道或者人體向遠離農(nóng)機方向運動，用以下公式判斷，

z_wn+1>z_wn (4-3)

di>ds (4-4)

若雷達探測到的人體目標滿足(4-3)或(4-4)，則農(nóng)機繼續(xù)向前行駛，否則導航箱控制農(nóng)機做停車等待處理；z_wn為第n探測掃描周期雷達相對最危險目標的距離，z_w(n+1)為下一個掃描周期雷達相對最危險目標的距離；

農(nóng)機作業(yè)前，攝像機與雷達的標定是在水平地面的條件下進行的；而農(nóng)機作業(yè)時，農(nóng)田地面凹凸不平，由于雷達和攝像機并非安裝于農(nóng)機的同一位置，雷達和攝像機相對地面的高度不同且隨著地形發(fā)生變化；距離檢測裝置一的工作過程具體的為，萬向滾輪9沿著凹凸不平的地面滾動，當?shù)孛嫱钩鰰r，凸出地面給萬向滾輪9向上的作用力，滑塊1沿著殼體2內(nèi)壁向上滑動，滑塊1推動導向桿向上滑動，距離傳感器8檢測滑塊1的上升距離，即雷達與地面間的高度變化量；向上凸的地面逐漸水平時，滑塊1逐漸向下滑動；當?shù)孛嫦蛳掳歼M時，滑塊1在自重的作用下，滑塊1向下滑動，直至萬向滾輪9與地面接觸，距離傳感器8檢測滑塊1與殼體2內(nèi)壁間的高度變化，距離傳感器8實時將檢測到的雷達與當前地面間的高度變化值發(fā)送給工控機；距離檢測裝置二的工作原理跟距離檢測裝置一的工作原理相同，距離檢測裝置二實時將檢測到的攝像機與當前地面間的高度變化值發(fā)送給工控機；

本發(fā)明與現(xiàn)有技術相比，本發(fā)明將毫米波雷達和攝像機結(jié)合起來感知農(nóng)田環(huán)境，通過距離檢測裝置一和距離檢測裝置二的設置分別實時檢測雷達和攝像機距離地面的高度變化，將高度變化量加入到雷達和攝像機坐標轉(zhuǎn)換的平移向量中；距離檢測裝置一和距離檢測裝置二的結(jié)構設計巧妙，檢測精度高；農(nóng)機作業(yè)時，使攝像機和雷達在空間上實現(xiàn)真正的同步，提高攝像機和雷達的融合精度；對噪聲和干擾信號產(chǎn)生的隨機噪聲信號進行濾除，提高雷達探測信號的準確性；根據(jù)設定好的農(nóng)機航向確定為農(nóng)機行駛車道，將農(nóng)機行駛車道內(nèi)的障礙物目標初選為有效目標，再對初選的有效目標進行進一步檢驗，以進一步確定有效目標，提高雷達感知同車道內(nèi)障礙物目標的有效性和準確性；選取最危險目標并跟蹤最危險目標，攝像機以最危險目標的動靜態(tài)為基礎進行目標識別，若最危險目標為動態(tài)，只需要識別出動態(tài)目標是否為人體即可，不需要識別出具體類型，減小運算量，提高響應速度，導航箱根據(jù)圖像識別結(jié)果控制農(nóng)機動作，避免農(nóng)機在無人駕駛時與障礙物碰撞；若識別結(jié)果為人體時，導航箱聲光報警提醒勞作人員避開農(nóng)機，利用人的習慣思維這一特性，不斷檢測人體是否偏離農(nóng)機行駛車道或者人體向遠離農(nóng)機方向運動，導航箱根據(jù)檢測結(jié)果控制農(nóng)機是否做停車等待處理，適應性好；若最危險目標為靜態(tài)，攝像機掃描出障礙物的輪廓，獲取避障路徑，控制農(nóng)機的前輪轉(zhuǎn)向角使農(nóng)機按照設定的避障路徑行走，同時，雷達一和雷達二檢測農(nóng)機避障過程中是否有新的障礙物存在，若有，農(nóng)機停車等待，以確保農(nóng)機的安全作業(yè)；本發(fā)明可應用于農(nóng)機無人駕駛時農(nóng)田環(huán)境自動感知的導航工作中。

實施例2

與實施例1的不同之處在于，如圖11所示，距離傳感器8安裝在殼體22內(nèi)壁頂部的中心，距離傳感器8正對滑塊1頂部的中心。

本發(fā)明并不局限于上述實施例，在本發(fā)明公開的技術方案的基礎上，本領域的技術人員根據(jù)所公開的技術內(nèi)容，不需要創(chuàng)造性的勞動就可以對其中的一些技術特征作出一些替換和變形，這些替換和變形均在本發(fā)明保護范圍內(nèi)。