基于農田環境感知的農機導航控制方法技術

本發明專利技術提供了農機控制技術領域內的一種基于農田環境感知的農機導航控制方法，具體包括以下步驟，步驟1：農機作業前，對攝像機進行標定，將雷達和視覺信息在空間上融合；步驟2：農機作業時，距離檢測裝置一檢測雷達與地面間的高度變化，距離檢測裝置二檢測攝像機與地面間的高度變化，實時調整雷達與攝像機坐標的轉換，使雷達與攝像機在空間上同步；步驟3：工控機解算接收的毫米波雷達數據，確定有效目標，確定最危險目標，同步采集攝像機圖像；步驟4：工控機根據雷達信息判斷最危險目標狀態并規劃行走路徑，根據雷達與攝像機采集到的圖像數據，判斷最危險目標的類型，導航箱控制農機動作；本發明專利技術中數據融合精度高，提高識別障礙物的準確度。

Agricultural machinery navigation control method based on farmland environment perception

The present invention provides a technique in the field of agricultural control of agricultural farmland navigation control method based on the environmental perception, including the following steps: Step 1 agricultural operations before the camera calibration, the radar and visual information fusion in space; step 2: agricultural machinery operation, distance detection device and detection radar the ground between the height change, two distance height change detection device of camera and ground, real-time adjustment of radar and camera coordinate conversion, the radar and camera synchronization in space; step 3: IPC solution of millimeter wave radar data receiving, determine the effective goal, determine the most dangerous targets, synchronous image acquisition cameras step 4: IPC; according to the radar information to determine the most dangerous state of target and path planning, according to the radar and camera to capture image data. According to the invention, the type of the most dangerous target is controlled, and the navigation box controls the movement of the agricultural machinery. The invention has high data fusion precision and improves the accuracy of the identification obstacle.

【技術實現步驟摘要】
基于農田環境感知的農機導航控制方法
本專利技術涉及一種導航方法，特別涉及一種農機無人駕駛中使用的導航方法。
技術介紹
精準農業技術被認為是21世紀農業科技發展的前沿，是科技含最高、集成綜合性最強的現代農業生產管理技術之一。精準農業技術根據空間變異，定位、定時、定量的實施一套現代化農事操作技術與管理的系統，是信息技術與農業生產全面結合的一種新型農業技術。精準農業的應用于快速發展，能夠充分挖掘農田最大的生產潛力、合理利用水肥資源、減少環境污染，大幅度提高農產品產量和品質。發展精準農業技術是解決我國農業由傳統農業向現代化農業發展過程中所面臨的確保農產品總量、調整農業產業結構、改善農產品品質和質量、資源嚴重不足且利用率低、環境污染等問題的有效解決方式，也是中國農業現代化發展與轉型升級的必經之路。衛星導航技術是精準農業技術的基本組成之一，使農機實現自動行駛，農機作業前只需要設定好參數后，導航系統便引導農機進入自動作業模式，開始進行直線耕作。在農機自動導航的過程中，農田的環境惡劣且復雜，大型的農田里可能會存在電線桿、田埂、土丘、牲畜以及隨時出現的勞作人員等，這些因素都為無人駕駛農機的實現提出了新的挑戰。現有技術中，使用衛星導航技術可以實現農機在農田里自動行走，但是農機無法準確識別農機前方的障礙物，即農機無法感應農田環境，更不用說根據感知到的農田環境自動做停車等待還是繼續行駛等處理，農機作業時必須得有駕駛員輔助操控農機的動作，一不注意，農機就會與前方障礙物碰撞；因此迫切需要研究出一套基于農田環境感知的導航控制方法使無人駕駛的農機具備對周邊環境進行感知的能力，一旦遇到上述農田中存在電線桿、田埂、土丘、牲畜以及隨時出現的勞作人員等情況，能夠及時采取停車等待、避障等應急處理。
技術實現思路
針對現有技術中的缺陷，本專利技術的目的在于克服上述現有技術中的不足之處，解決現有技術中無人駕駛的農機無法感知農田環境的技術問題，提供一種用于農機無人駕駛的農田環境感知方法，本專利技術實現了農田環境的感知，感知精度高，識別農機前方障礙物的準確度高，提高農機無人駕駛時的可靠性。本專利技術的目的是這樣實現的：一種基于農田環境感知的農機導航控制方法，具體包括以下步驟，步驟1：農機作業前，對攝像機進行標定，攝像機空間坐標變換，再對雷達視覺聯合標定，使得雷達和視覺信息在空間上融合；步驟2：農機作業時，距離檢測裝置一實時檢測雷達與地面間的高度變化△hst，距離檢測裝置二實時檢測攝像機與地面間的高度變化△hct，工控機進行數據處理實時調整雷達與攝像機坐標的轉換關系，使雷達與攝像機在作業條件下實現在空間上的同步；步驟3：工控機解算接收到的毫米波雷達數據，確定有效目標，選出農機作業前方雷達感興趣的區域，確定最危險目標，同步進行攝像機圖像的采集；步驟4：根據雷達的信息判斷出最危險目標的運動狀態，工控機根據最危險目標的運動狀態規劃農機行走路徑，根據雷達與相機采集到的最危險目標的圖像數據，判斷出最危險目標的類型，工控機將解析出來的動作指令傳輸給導航箱，導航箱控制農機做相應的動作；其中，農機作業時，農機的行駛速度勻速；所述距離檢測裝置一和距離檢測裝置二的結構相同，距離檢測裝置一安裝在農機前側且設置在雷達正下方，距離檢測裝置二安裝在農機下側且設置在攝像機正下方位置；所述距離檢測裝置一包括具有容納腔且可開合的殼體，容納腔為圓柱體狀，所述殼體內設有可沿著殼體內壁做上下直線運動的滑塊，殼體內設有檢測滑塊頂部與殼體內壁上側距離變化的距離傳感器，所述滑塊和殼體內壁頂部之間連接有2個連接件，連接件關于滑塊在寬度方向上的中心對稱設置；所述連接件包括導向套，所述導向套內設有可沿著導向套內壁滑動的導向桿，滑塊的前后兩側與殼體內壁之間留有可容納導向桿的間隙，所述導向套的底部開有導向孔并設有限制導向桿遠離導向套的限位階，所述導向桿穿過導向孔與滑塊外側連接，所述導向套的容納腔正上方對應的殼體上開有至少一個透氣孔，所述滑塊的底部安裝有可在地面上滾動的萬向滾輪；工控機接收所述距離傳感器發送過來的數據信號并進行數據處理。為了實現雷達和攝像機在空間上的初步同步，所述步驟1中的將車輛坐標轉換成圖像像素坐標具體包括以下步驟，步驟1.1：農機作業前，地面默認為水平，將毫米波雷達固定安裝在農機的前側且位于農機縱向中軸，雷達發射面向外，使雷達發射面與地面垂直；安裝攝像機時使攝像機的光軸與地面平行；步驟1.2：以雷達的中心為原點建立雷達坐標系00-X0Y0Z0，毫米波雷達所在平面由X0軸和Y0軸確定并與Z0軸相垂直，Z0軸與地面平行且與農機中心軸線重合；建立攝像機坐標系Oc-XcYcZc，以攝像機的中心為原點Oc，平面XcOcYc平行于攝像機的成像平面，Zc軸是攝像機的取景光軸且垂直于成像平面；建立車輛坐標系Ow-XwYwZw，Ow為農機后軸的中心與車輛的中心軸線的交點，Xw軸水平向右并垂直于農機的縱向中軸線，Zw水平向前且與農機中心軸線重合，Yw軸垂直于水面地面向上，雷達坐標系的X0O0Z0平面與車輛坐標系的XwOwZw平面平行；步驟1.3：光軸與成像平面相交的點是圖像主點O’，車輛坐標通過旋轉矩陣R和平移向量sc轉換后得到攝像機坐標(xc，yc，zc，1)T，任意點P的車輛坐標為(xw，yw，zw，1)T，將車輛坐標轉換為攝像機坐標，具體的轉換關系如下，式(1-1)中，R為一個三行三列的正交單位矩陣，sc(xc0，yc0，zc0)為初始條件下車輛坐標系到攝像機坐標系的1*3平移矩陣，xc0為攝像機所在的中心軸與車輛中心軸線兩直線的距離，yc0為初始條件下攝像機距離地面的高度，zc0為攝像機距離農機后軸的距離；步驟1.4：將攝像機坐標(xc，yc，zc，1)T轉換到圖像物理坐標(x1，y1)T，具體的轉換關系如下，式(1-2)中，f為攝像機的焦距，焦距單位為mm；步驟1.5：將圖像物理坐標(x1，y1)T轉換到圖像像素坐標(u,v)，具體的轉換關系如下：其中，dx,dy分別表示每個像素在橫軸和縱軸上單位大小，u0、v0分別為圖像像素坐標系下攝像機光軸和成像平面交點的橫縱坐標，坐標單位為pixel；步驟1.6：根據以上公式(1-1)～(1-3)得到圖像像素坐標系到車輛坐標系的轉換公式，具體的為，步驟1.7：為了使雷達和視覺信息在空間上融合，將步驟1.6中的坐標轉換關系更新為，其中，s＝sc+s0，s0的坐標設為(xs0,ys0,zs0)，xs0＝0，ys0為初始條件下雷達距離地面的高度，zs0為雷達與農機后軸的距離。為了提高農機作業過程中雷達和攝像機的融合精度，所述步驟2中的工控機進行數據處理實時調整雷達與攝像機坐標的轉換關系，具體的為，根據農機的實際路況實時調整平移向量s，掃描周期t下調整后的平移向量st＝sc+s0+△st，實時的車輛坐標與圖像像素坐標的轉換關系，具體的為，其中，△hct為掃描周期t下攝像機與地面高度的變化值，△hst為掃描周期t下雷達與地面高度的變化值，j為掃描周期數，(ut,vt)為農機作業過程中掃描周期t下實時更新計算得到的圖像像素坐標。為了進一步提高環境感知精度，所述步驟2中自適應調整后的平移向量中獲得△ht的步驟如下，所述步驟2中實時調整后的平移向量中獲得△ht的步驟如下，步驟2.1本文檔來自技高網...

【技術保護點】
一種基于農田環境感知的農機導航控制方法，其特征在于，具體包括以下步驟，步驟1：農機作業前，對攝像機進行標定，攝像機空間坐標變換，再對雷達視覺聯合標定，使得雷達和視覺信息在空間上融合；步驟2：農機作業時，距離檢測裝置一實時檢測雷達與地面間的高度變化△h

【技術特征摘要】
1.一種基于農田環境感知的農機導航控制方法，其特征在于，具體包括以下步驟，步驟1：農機作業前，對攝像機進行標定，攝像機空間坐標變換，再對雷達視覺聯合標定，使得雷達和視覺信息在空間上融合；步驟2：農機作業時，距離檢測裝置一實時檢測雷達與地面間的高度變化△hst，距離檢測裝置二實時檢測攝像機與地面間的高度變化△hct，工控機進行數據處理實時調整雷達與攝像機坐標的轉換關系，使雷達與攝像機在作業條件下實現在空間上的同步；步驟3：工控機解算接收到的毫米波雷達數據，確定有效目標，選出農機作業前方雷達感興趣的區域，確定最危險目標，同步進行攝像機圖像的采集；步驟4：根據雷達的信息判斷出最危險目標的運動狀態，工控機根據最危險目標的運動狀態規劃農機行走路徑，根據雷達與相機采集到的最危險目標的圖像數據，判斷出最危險目標的類型，工控機將解析出來的動作指令傳輸給導航箱，導航箱控制農機做相應的動作；其中，農機作業時，農機的行駛速度勻速；所述距離檢測裝置一和距離檢測裝置二的結構相同，距離檢測裝置一安裝在農機前側且設置在雷達正下方，距離檢測裝置二安裝在農機下側且設置在攝像機正下方位置；所述距離檢測裝置一包括具有容納腔且可開合的殼體，容納腔為圓柱體狀，所述殼體內設有可沿著殼體內壁做上下直線運動的滑塊，殼體內設有檢測滑塊頂部與殼體內壁上側距離變化的距離傳感器，所述滑塊和殼體內壁頂部之間連接有2個連接件，連接件關于滑塊在寬度方向上的中心對稱設置；所述連接件包括導向套，所述導向套內設有可沿著導向套內壁滑動的導向桿，滑塊的前后兩側與殼體內壁之間留有可容納導向桿的間隙，所述導向套的底部開有導向孔并設有限制導向桿遠離導向套的限位階，所述導向桿穿過導向孔與滑塊外側連接，所述導向套的容納腔正上方對應的殼體上開有至少一個透氣孔，所述滑塊的底部安裝有可在地面上滾動的萬向滾輪；工控機接收所述距離傳感器發送過來的數據信號并進行數據處理。2.根據權利要求1所述的基于農田環境感知的農機導航控制方法，其特征在于，所述步驟1中的將雷達坐標轉換成圖像像素坐標具體包括以下步驟，步驟1.1：農機作業前，地面默認為水平，將毫米波雷達一固定安裝在農機的前側且位于農機縱向中軸，雷達發射面向外，使雷達發射面與地面垂直；安裝攝像機時使攝像機的光軸與地面平行；步驟1.2：以雷達的中心為原點建立雷達坐標系00-X0Y0Z0，毫米波雷達所在平面由X0軸和Y0軸確定并與Z0軸相垂直，Z0軸與地面平行且與農機中心軸線重合；建立攝像機坐標系Oc-XcYcZc，以攝像機的中心為原點Oc，平面XcOcYc平行于攝像機的成像平面，Zc軸是攝像機的取景光軸且垂直于成像平面；建立車輛坐標系Ow-XwYwZw，Ow為農機后軸的中心與車輛的中心軸線的交點，Xw軸水平向右并垂直于農機的縱向中軸線，Zw水平向前且與農機中心軸線重合，Yw軸垂直于水面地面向上，雷達坐標系的X0O0Z0平面與車輛坐標系的XwOwZw平面平行；步驟1.3：光軸與成像平面相交的點是圖像主點O’，車輛坐標通過旋轉矩陣R和平移向量sc轉換后得到攝像機坐標(xc，yc，zc，1)T，任意點P的車輛坐標為(xw，yw，zw，1)T，將車輛坐標轉換為攝像機坐標，具體的轉換關系如下，式(1-1)中，R為一個三行三列的正交單位矩陣，sc(xc0，yc0，zc0)為初始條件下車輛坐標系到攝像機坐標系的1*3平移矩陣，xc0為攝像機所在的中心軸與車輛中心軸線兩直線的距離，yc0為初始條件下攝像機距離地面的高度，zc0為攝像機距離農機后軸的距離；步驟1.4：將攝像機坐標(xc，yc，zc，1)T轉換到圖像物理坐標(x1，y1)T，具體的轉換關系如下，式(1-2)中，f為攝像機的焦距，焦距單位為mm；步驟1.5：將圖像物理坐標(x1，y1)T轉換到圖像像素坐標(u,v)，具體的轉換關系如下：其中，dx,dy分別表示每個像素在橫軸和縱軸上單位大小，u0、v0分別為圖像像素坐標系下攝像機光軸和成像平面交點的橫縱坐標，坐標單位為pixel；步驟1.6：根據以上公式(1-1)～(1-3)得到圖像像素坐標系到車輛坐標系的轉換公式，具體的為，步驟1.7：為了使雷達和視覺信息在空間上融合，將步驟1.6中的坐標轉換關系更新為，其中，s＝sc+s0，s0的坐標設為(xs0,ys0,zs0)，xs0＝0，ys0為初始條件下雷達距離地面的高度，zs0為雷達與農機后軸的距離。3.根據權利要求2所述的基于農田環境感知的農機導航控制方法，其特征在于，所述步驟2中的工控機進行數據處理實時調整雷達與攝像機坐標的轉換關系，具體的為，根據農機的實際路況實時調整平移向量s，掃描周期t下調整后的平移向量st＝sc+s0+△st，實時的車輛坐標與圖像像素坐標的轉換關系，具體的為，其中，△hct為掃描周期t下攝像機與地面高度的變化值，△hst為掃描周期t下雷達與地面高度的變化值，j為掃描周期數，(ut,vt)為農機作業過程中掃描周期t下實時更新計算得到的圖像像素坐標。4.根據權利要求3所述的基于農田環境感知的農機導航控制方法，其特征在于，所述步驟2中實時調整后的平移向量中獲得△ht的步驟如下，步驟2.1：實時計算雷達與地面間的高度變化量以及攝像機與地面間的高度變化量，具體的為，掃描周期t中i時刻與i-1時刻雷達與地面高度距離變化值為△hsti，采用平均法計算掃描周期t中雷達相對地面的高度變化量△hst，假設掃描周期t中采樣時刻i與采樣時刻i-1下攝像機與地面高度變化值為△hcti，采用平均法計算掃描周期t中攝像機與地面的高度變化量△hct，步驟2.2：實時計算出掃描周期t下自適應調整后的平移向量s，具體的為，其中，k為一個掃描周期中的采樣點的總數。5.根據權利要求4所述的基于農田環境感知的農機導航控制方法，其特征在于，所述步驟3中的解算雷達數據確定有效目標，具體包括以下步驟，步驟3.1：對雷達接收到的數據按照毫米波雷達協議進行解算，得到前方物體相對雷達的角度α、距離r、相對速度v、前方物體的反射強度并為每個目標分配唯一一個ID；步驟3.2：對隨機噪聲信號進行濾波，保證雷達數據的連續有效性，具體的為，定義z＝[r,α,v]T為雷達的測量值，z(k)為毫米波雷達第k次輸出的測量值，d2＝S(z(k)-z(k-1))(z(k)-z(k-1))T＜rs2(3-1)過濾掉不符合式(3-1)的數據信號；其中，d為相鄰量測向量z(k)、z(k-1)之間的加權歐氏距離，S為加權矩陣，rs為設定的閾值；步驟3.3：判定目標是否在農機行駛的車道內，當雷達前方物體滿足di≤ds時，目標在農機行駛車道內，否則，目標不在農機行駛車道內，農機行駛車道內的目標初選為有效目標，并對其按照由近及遠的準則進行排序編號...

【專利技術屬性】
技術研發人員：張瑞宏，奚小波，金亦富，張劍峰，單翔，蔡廣林，孫福華，葉偉偉，史揚杰，馬國梁，
申請(專利權)人：南京沃楊機械科技有限公司，
類型：發明
國別省市：江蘇,32