技術特征：

1.一種基于農田環(huán)境感知的農機導航控制方法，其特征在于，具體包括以下步驟，

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

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

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

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

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

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

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

2.根據(jù)權利要求1所述的基于農田環(huán)境感知的農機導航控制方法，其特征在于，所述步驟1中的將雷達坐標轉換成圖像像素坐標具體包括以下步驟，

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

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

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

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

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

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

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

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

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

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

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

3.根據(jù)權利要求2所述的基于農田環(huán)境感知的農機導航控制方法，其特征在于，所述步驟2中的工控機進行數(shù)據(jù)處理實時調整雷達與攝像機坐標的轉換關系，具體的為，根據(jù)農機的實際路況實時調整平移向量s，掃描周期t下調整后的平移向量s_t＝s_c+s₀+△s_t，實時的車輛坐標與圖像像素坐標的轉換關系，具體的為，

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

4.根據(jù)權利要求3所述的基于農田環(huán)境感知的農機導航控制方法，其特征在于，

所述步驟2中實時調整后的平移向量中獲得△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下自適應調整后的平移向量s，具體的為，

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

5.根據(jù)權利要求4所述的基于農田環(huán)境感知的農機導航控制方法，其特征在于，所述步驟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：判定目標是否在農機行駛的車道內，當雷達前方物體滿足di≤ds時，目標在農機行駛車道內，否則，目標不在農機行駛車道內，農機行駛車道內的目標初選為有效目標，并對其按照由近及遠的準則進行排序編號；在農機行駛車道外的目標為非危險目標，將其排除；其中，ds為安全距離閾值，ds＝L/2+ks，di為i采樣點下測得的目標與Z₀軸之間的距離，L為農機上懸掛的犁具寬度，ks為設定的安全余量；

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

步驟3.5：根據(jù)確定好的有效目標，通過毫米波雷達獲取的最近距離障礙物確定為候選的最危險目標，若dj≤dmin,dj為毫米波雷達獲取的農機與ID是j的有效目標之間的距離，dmin為在毫米波雷達一個掃描周期內所獲取的農機與最近有效目標的距離，則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探測周期內測得的有效障礙物目標的相對距離、相對速度、相對加速度，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í)行。

6.根據(jù)權利要求5所述的基于農田環(huán)境感知的農機導航控制方法，其特征在于，所述步驟4中判斷最危險目標的動靜狀態(tài)具體包括以下步驟，

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

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

v≠v_車 (4-2)

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

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

d₁<d_s0 (4-3)

d₂<d_s0 (4-4)

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

其中，d_s0為設定的轉彎安全距離。

7.根據(jù)權利要求5所述的基于農田環(huán)境感知的農機導航控制方法，其特征在于，所述步驟4.2中的避障路徑具體的為，

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

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

特征圓的方程可以寫成：

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)。

8.根據(jù)權利要求7所述的基于農田環(huán)境感知的農機導航控制方法，其特征在于，所述步驟4中根據(jù)雷達與相機采集到的最危險目標的圖像數(shù)據(jù)，判斷出最危險目標的類型，導航箱控制農機做相應的動作，具體包括以下步驟，

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

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

z_wn+1>z_wn (4-3)

di>ds (4-4)

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

9.根據(jù)權利要1～8任一項所述的基于農田環(huán)境感知的農機導航控制方法，其特征在于，所述距離傳感器安裝在滑塊頂部的中心，距離傳感器正對殼體內壁頂部。

10.根據(jù)權利要求1～8任一項所述的基于農田環(huán)境感知的農機導航控制方法，其特征在于，所述距離傳感器安裝在殼體內壁頂部的中心，距離傳感器正對滑塊頂部的中心。