本發(fā)明涉及一種自動(dòng)掉頭路徑規(guī)劃及其控制方法，特別涉及一種農(nóng)機(jī)導(dǎo)航用農(nóng)田環(huán)境感知方法。

背景技術(shù)：

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

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

發(fā)展精準(zhǔn)農(nóng)業(yè)技術(shù)是解決我國(guó)農(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é)構(gòu)、改善農(nóng)產(chǎn)品品質(zhì)和質(zhì)量、資源嚴(yán)重不足且利用率低、環(huán)境污染等問題的有效解決方式，也是中國(guó)農(nóng)業(yè)現(xiàn)代化發(fā)展與轉(zhuǎn)型升級(jí)的必經(jīng)之路。

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

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

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

本發(fā)明的目的是這樣實(shí)現(xiàn)的：一種農(nóng)機(jī)導(dǎo)航用農(nóng)田環(huán)境感知方法，具體包括以下步驟，

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

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

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

步驟4：根據(jù)雷達(dá)的信息判斷出最危險(xiǎn)目標(biāo)的運(yùn)動(dòng)狀態(tài)，根據(jù)雷達(dá)與相機(jī)采集到的最危險(xiǎn)目標(biāo)的圖像數(shù)據(jù)，判斷出最危險(xiǎn)目標(biāo)的類型，工控機(jī)將動(dòng)作指令傳輸給導(dǎo)航箱，導(dǎo)航箱控制農(nóng)機(jī)做相應(yīng)的動(dòng)作；

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

所述距離檢測(cè)裝置一和距離檢測(cè)裝置二的結(jié)構(gòu)相同，距離檢測(cè)裝置一安裝在農(nóng)機(jī)前側(cè)且設(shè)置在雷達(dá)正下方，距離檢測(cè)裝置二安裝在農(nóng)機(jī)下側(cè)且設(shè)置在攝像機(jī)正下方位置；所述距離檢測(cè)裝置一包括具有容納腔的導(dǎo)向套和導(dǎo)桿，所述導(dǎo)向套安裝在農(nóng)機(jī)上，導(dǎo)向套頂部的內(nèi)壁上連接有正對(duì)導(dǎo)桿的距離傳感器，所述導(dǎo)桿可在導(dǎo)向套內(nèi)滑動(dòng)，所述導(dǎo)桿的上側(cè)可拆卸地連接有限制導(dǎo)桿在容納腔內(nèi)運(yùn)動(dòng)的限位板，所述導(dǎo)桿的底部安裝有可在地面上滾動(dòng)的萬向滾輪；

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

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

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

步驟1.2：以雷達(dá)的中心為原點(diǎn)建立雷達(dá)坐標(biāo)系00-x0y0z0，毫米波雷達(dá)所在平面由x0軸和y0軸確定并與z0軸相垂直，z0軸與地面平行且與農(nóng)機(jī)中心軸線重合；建立攝像機(jī)坐標(biāo)系oc-xcyczc，以攝像機(jī)的中心為原點(diǎn)oc，平面xcocyc平行于攝像機(jī)的成像平面，zc軸是攝像機(jī)的取景光軸且垂直于成像平面；建立車輛坐標(biāo)系ow-xwywzw，ow為農(nóng)機(jī)的農(nóng)機(jī)后軸的中心與車輛的中心軸線的交點(diǎn)，xw軸水平向右并垂直于農(nóng)機(jī)的縱向中軸線，zw水平向前且與農(nóng)機(jī)中心軸線重合，yw軸垂直于水面地面向上，雷達(dá)坐標(biāo)系的x0o0z0平面與車輛坐標(biāo)系的xwowzw平面平行；

步驟1.3：光軸與成像平面相交的點(diǎn)是圖像主點(diǎn)o’，車輛坐標(biāo)通過旋轉(zhuǎn)矩陣r和平移向量sc轉(zhuǎn)換后得到攝像機(jī)坐標(biāo)（xc，yc，zc，1）^t，任意點(diǎn)p的車輛坐標(biāo)為（xw，yw，zw，1）^t，將車輛坐標(biāo)轉(zhuǎn)換為攝像機(jī)坐標(biāo)，具體的轉(zhuǎn)換關(guān)系如下，

（1-1）

式（1-1）中，r為一個(gè)三行三列的正交單位矩陣，sc為初始條件下車輛坐標(biāo)系到攝像機(jī)坐標(biāo)系的1*3平移矩陣，xc0為攝像機(jī)所在的中心軸與車輛中心軸線兩直線的距離，yc0為初始條件下攝像機(jī)距離地面的高度，zc0為攝像機(jī)距離農(nóng)機(jī)后軸的距離；

步驟1.4：將攝像機(jī)坐標(biāo)（xc，yc，zc，1）^t轉(zhuǎn)換到圖像物理坐標(biāo)（x1，y1）^t，具體的轉(zhuǎn)換關(guān)系如下，

（1-2）

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

步驟1.5：將圖像物理坐標(biāo)（x1，y1）^t轉(zhuǎn)換到圖像像素坐標(biāo)（u,v），具體的轉(zhuǎn)換關(guān)系如下：

（1-3）

其中，dx,dy分別表示每個(gè)像素在橫軸和縱軸上單位大小，u0、v0分別為圖像像素坐標(biāo)系下攝像機(jī)光軸和成像平面交點(diǎn)的橫縱坐標(biāo)，坐標(biāo)單位為pixel；

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

（1-4）；

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

（1-5）；

其中，s=sc+s0，s0的坐標(biāo)設(shè)為（xs0,ys0,zs0），xs0=0，ys0為初始條件下雷達(dá)距離地面的高度，zs0為雷達(dá)與農(nóng)機(jī)后軸的距離。

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

（2-1）

（2-2）

其中，△hct為掃描周期t下攝像機(jī)與地面高度的變化值，△hst為掃描周期t下雷達(dá)與地面高度的變化值，j為掃描周期數(shù)，（ut,vt）為農(nóng)機(jī)作業(yè)過程中掃描周期t下實(shí)時(shí)更新計(jì)算得到的圖像像素坐標(biāo)。

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

步驟2.1：農(nóng)機(jī)作業(yè)前，在雷達(dá)正下方安裝有用來檢測(cè)雷達(dá)與地面間的高度變化的高度檢測(cè)裝置一；在攝像機(jī)正下方的農(nóng)機(jī)下側(cè)安裝有用來檢測(cè)攝像機(jī)與地面的高度變化量的高度檢測(cè)裝置二；

步驟2.2：實(shí)時(shí)計(jì)算雷達(dá)與地面高度變化量以及攝像機(jī)與地面的高度變化量，具體的為，

掃描周期t中i時(shí)刻與i-1時(shí)刻雷達(dá)與地面高度距離變化值為△hsti，采用平均法計(jì)算掃描周期t中雷達(dá)相對(duì)地面的高度變化量△hst，

（2-3）；

假設(shè)掃描周期t中采樣時(shí)刻i與采樣時(shí)刻i-1下攝像機(jī)與地面高度變化值為△hcti，采用平均法計(jì)算掃描周期t中攝像機(jī)與地面的高度變化量△hct，

（2-4），

步驟2.3：實(shí)時(shí)計(jì)算出掃描周期t下自適應(yīng)調(diào)整后的平移向量s，具體的為，

（2-5）

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

此設(shè)計(jì)中，通過實(shí)時(shí)檢測(cè)雷達(dá)距離地面的高度變化以及攝像機(jī)距離地面的高度變化，實(shí)時(shí)更新平移向量s，提高攝像機(jī)和雷達(dá)空間同步的精度。

為了進(jìn)一步提高獲得雷達(dá)距離地面高度變化值和攝像機(jī)距離地面高度變化值的準(zhǔn)確度，所述步驟2中實(shí)時(shí)調(diào)整后的平移向量中獲得△ht的步驟如下，

步驟2.1：實(shí)時(shí)計(jì)算雷達(dá)與地面間的高度變化量以及攝像機(jī)與地面間的高度變化量，具體的為，

掃描周期t中i時(shí)刻與i-1時(shí)刻雷達(dá)與地面高度距離變化值為△hsti，采用平均法計(jì)算掃描周期t中雷達(dá)相對(duì)地面的高度變化量△hst，

（2-3）；

假設(shè)掃描周期t中采樣時(shí)刻i與采樣時(shí)刻i-1下攝像機(jī)與地面高度變化值為△hcti，采用平均法計(jì)算掃描周期t中攝像機(jī)與地面的高度變化量△hct，

（2-4）；

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

（2-5）

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

作為本發(fā)明的進(jìn)一步改進(jìn)，所述彈簧關(guān)于導(dǎo)桿中心對(duì)稱設(shè)置，此設(shè)計(jì)使得導(dǎo)桿兩端受力平衡；彈簧的中心軸與導(dǎo)桿上側(cè)平面垂直，此設(shè)計(jì)使彈簧作用于導(dǎo)桿的力與地面垂直，導(dǎo)桿在導(dǎo)向套內(nèi)滑動(dòng)更加輕松。

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

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

步驟3.2：對(duì)隨機(jī)噪聲信號(hào)進(jìn)行濾波，保證雷達(dá)數(shù)據(jù)的連續(xù)有效性，具體的為，定義z=[r,α,v]^t為雷達(dá)的測(cè)量值，z(k)為毫米波雷達(dá)第k次輸出的測(cè)量值，

（3-1）

過濾掉不符合式（3-1）的數(shù)據(jù)信號(hào)；其中，d為相鄰量測(cè)向量z(k)、z(k-1)之間的加權(quán)歐氏距離，s為加權(quán)矩陣，rs為設(shè)定的閾值；

步驟3.3：判定目標(biāo)是否在農(nóng)機(jī)行駛的車道內(nèi)，當(dāng)雷達(dá)前方物體滿足di≤ds時(shí)，目標(biāo)在農(nóng)機(jī)行駛車道內(nèi)，否則，目標(biāo)不在農(nóng)機(jī)行駛車道內(nèi)，農(nóng)機(jī)行駛車道內(nèi)的目標(biāo)初選為有效目標(biāo)，并對(duì)其按照由近及遠(yuǎn)的準(zhǔn)則進(jìn)行排序編號(hào)；在農(nóng)機(jī)行駛車道外的目標(biāo)為非危險(xiǎn)目標(biāo)，將其排除；其中，ds為安全距離閾值，ds=l/2+ks，di為i采樣點(diǎn)下測(cè)得的目標(biāo)與z0軸之間的距離，l為農(nóng)機(jī)上懸掛的犁具寬度，ks為設(shè)定的安全余量；

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

圖6中為障礙物在行駛車道內(nèi)時(shí)的顯示，障礙物e距離農(nóng)機(jī)中心o農(nóng)機(jī)的距離小于l/2+ks，障礙物e在農(nóng)機(jī)行駛車道內(nèi)；

步驟3.4：對(duì)初選的有效目標(biāo)進(jìn)行有效性檢驗(yàn)，最終確定有效目標(biāo)；

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

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

為了進(jìn)一步提高有效目標(biāo)確定的精確度，所述步驟3.4中對(duì)初選的有效目標(biāo)進(jìn)行有效性檢驗(yàn)具體包括以下步驟，

步驟3.4.1：對(duì)初選的有效目標(biāo)進(jìn)行預(yù)測(cè)，選取狀態(tài)sn=[dn,vn,an],初選有效目標(biāo)的狀態(tài)預(yù)測(cè)方程為，

（3-2）

其中，是上一個(gè)掃描周期預(yù)測(cè)的有效障礙物目標(biāo)的狀態(tài)信息，分別表示毫米波雷達(dá)第n探測(cè)周期內(nèi)測(cè)得的有效障礙物目標(biāo)的相對(duì)距離、相對(duì)速度、相對(duì)加速度，t是毫米波雷達(dá)的掃描周期；

步驟3.4.2：通過比較預(yù)測(cè)的第n+1周期有效目標(biāo)的狀態(tài)信息和雷達(dá)實(shí)際測(cè)得的第n+1周期有效目標(biāo)的狀態(tài)信息，具體如下，

（3-3）

其中，d0、v0、a0是設(shè)定的有效障礙物目標(biāo)測(cè)量值與預(yù)測(cè)值之間的誤差閾值；

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

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

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

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

步驟4.2：根據(jù)相對(duì)速度大小判定最危險(xiǎn)目標(biāo)的動(dòng)靜狀態(tài)，具體如下，v≠v車（4-2）

在一個(gè)掃描周期內(nèi)，（4-2）式始終成立時(shí)，判定目標(biāo)的狀態(tài)為動(dòng)態(tài)，此時(shí)，導(dǎo)航箱發(fā)出聲光報(bào)警并在zd=zmin時(shí)做停車等待處理；否則，農(nóng)機(jī)繼續(xù)行駛，并返回至步驟3.1循環(huán)執(zhí)行其中，v為雷達(dá)相對(duì)目標(biāo)的速度大小，v車為農(nóng)機(jī)的行駛速度；

此設(shè)計(jì)中，判定最危險(xiǎn)目標(biāo)的動(dòng)靜態(tài)原理簡(jiǎn)單，提高響應(yīng)速度。

所述步驟4中根據(jù)雷達(dá)與相機(jī)采集到的最危險(xiǎn)目標(biāo)的圖像數(shù)據(jù)，判斷出最危險(xiǎn)目標(biāo)的類型具體包括以下步驟，

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

步驟4.2a：攝像機(jī)獲取最危險(xiǎn)目標(biāo)的圖像，將圖像與訓(xùn)練好的人體樣本訓(xùn)練庫(kù)進(jìn)行匹配比較，輸出目標(biāo)識(shí)別結(jié)果；

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

zwn+1>zwn（4-3）

di>ds（4-4）

若雷達(dá)探測(cè)到的人體目標(biāo)滿足（4-3）或（4-4），則農(nóng)機(jī)繼續(xù)向前行駛，否則導(dǎo)航箱控制農(nóng)機(jī)做停車等待處理；zwn為第n探測(cè)掃描周期雷達(dá)相對(duì)最危險(xiǎn)目標(biāo)的距離，zw（n+1）為下一個(gè)掃描周期雷達(dá)相對(duì)最危險(xiǎn)目標(biāo)的距離；

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

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

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

附圖說明

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

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

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

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

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

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

圖7為本發(fā)明中對(duì)初選有效目標(biāo)進(jìn)行檢驗(yàn)以進(jìn)一步確定有效目標(biāo)的流程圖。

圖8為距離檢測(cè)裝置一的結(jié)構(gòu)示意圖。

其中，1導(dǎo)桿，2導(dǎo)向套，3距離傳感器，4限位板，5容納腔，6萬向滾輪，7沉頭螺釘。

具體實(shí)施方式

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

如圖1～8所示的一種農(nóng)機(jī)導(dǎo)航用農(nóng)田環(huán)境感知方法，具體包括以下步驟：

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

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

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

步驟4：根據(jù)雷達(dá)的信息判斷出最危險(xiǎn)目標(biāo)的運(yùn)動(dòng)狀態(tài)，根據(jù)雷達(dá)與相機(jī)采集到的最危險(xiǎn)目標(biāo)的圖像數(shù)據(jù)，判斷出最危險(xiǎn)目標(biāo)的類型，工控機(jī)將動(dòng)作指令傳輸給導(dǎo)航箱，導(dǎo)航箱控制農(nóng)機(jī)做相應(yīng)的動(dòng)作；

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

距離檢測(cè)裝置一和距離檢測(cè)裝置二的結(jié)構(gòu)相同，距離檢測(cè)裝置一安裝在農(nóng)機(jī)前側(cè)且設(shè)置在雷達(dá)正下方，距離檢測(cè)裝置二安裝在農(nóng)機(jī)下側(cè)且設(shè)置在攝像機(jī)正下方位置；如圖8所示，距離檢測(cè)裝置一包括具有容納腔5的導(dǎo)向套2和導(dǎo)桿1，導(dǎo)向套2安裝在農(nóng)機(jī)上，導(dǎo)向套2頂部的內(nèi)壁上連接有正對(duì)導(dǎo)桿1的距離傳感器3，導(dǎo)桿1可在導(dǎo)向套2內(nèi)滑動(dòng)，導(dǎo)桿1的上側(cè)可拆卸地連接有限制導(dǎo)桿1在容納腔5內(nèi)運(yùn)動(dòng)的限位板4，導(dǎo)桿1的底部安裝有可在地面上滾動(dòng)的萬向滾輪6；限位板4可通過沉頭螺釘7與導(dǎo)桿1連接。

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

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

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

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

步驟1.2：以雷達(dá)的中心為原點(diǎn)建立雷達(dá)坐標(biāo)系00-x0y0z0，毫米波雷達(dá)所在平面由x0軸和y0軸確定并與z0軸相垂直，z0軸與地面平行且與農(nóng)機(jī)中心軸線重合；建立攝像機(jī)坐標(biāo)系oc-xcyczc，以攝像機(jī)的中心為原點(diǎn)oc，平面xcocyc平行于攝像機(jī)的成像平面，zc軸是攝像機(jī)的取景光軸且垂直于成像平面；建立車輛坐標(biāo)系ow-xwywzw，ow為農(nóng)機(jī)的農(nóng)機(jī)后軸的中心與車輛的中心軸線的交點(diǎn)，xw軸水平向右并垂直于農(nóng)機(jī)的縱向中軸線，zw水平向前且與農(nóng)機(jī)中心軸線重合，yw軸垂直于水面地面向上，雷達(dá)坐標(biāo)系的x0o0z0平面與車輛坐標(biāo)系的xwowzw平面平行；

步驟1.3：光軸與成像平面相交的點(diǎn)是圖像主點(diǎn)o’，車輛坐標(biāo)通過旋轉(zhuǎn)矩陣r和平移向量sc轉(zhuǎn)換后得到攝像機(jī)坐標(biāo)（xc，yc，zc，1）^t，任意點(diǎn)p的車輛坐標(biāo)為（xw，yw，zw，1）^t，將車輛坐標(biāo)轉(zhuǎn)換為攝像機(jī)坐標(biāo)，具體的轉(zhuǎn)換關(guān)系如下，

（1-1）

式（1-1）中，r為一個(gè)三行三列的正交單位矩陣，sc為初始條件下車輛坐標(biāo)系到攝像機(jī)坐標(biāo)系的1*3平移矩陣，xc0為攝像機(jī)所在的中心軸與車輛中心軸線兩直線的距離，yc0為初始條件下攝像機(jī)距離地面的高度，zc0為攝像機(jī)距離農(nóng)機(jī)后軸的距離；

步驟1.4：將攝像機(jī)坐標(biāo)（xc，yc，zc，1）^t轉(zhuǎn)換到圖像物理坐標(biāo)（x1，y1）^t，具體的轉(zhuǎn)換關(guān)系如下，

（1-2）

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

步驟1.5：將圖像物理坐標(biāo)（x1，y1）^t轉(zhuǎn)換到圖像像素坐標(biāo)（u,v），具體的轉(zhuǎn)換關(guān)系如下：

（1-3）

其中，dx,dy分別表示每個(gè)像素在橫軸和縱軸上單位大小，u0、v0分別為圖像像素坐標(biāo)系下攝像機(jī)光軸和成像平面交點(diǎn)的橫縱坐標(biāo)，坐標(biāo)單位為pixel；

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

（1-4）；

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

（1-5）；

其中，s=sc+s0，s0的坐標(biāo)設(shè)為（xs0,ys0,zs0），xs0=0，ys0為初始條件下雷達(dá)距離地面的高度，zs0為雷達(dá)與農(nóng)機(jī)后軸的距離；

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

（2-1）

（2-2）

其中，△hct為掃描周期t下攝像機(jī)與地面高度的變化值，△hst為掃描周期t下雷達(dá)與地面高度的變化值，j為掃描周期數(shù)，（ut,vt）為農(nóng)機(jī)作業(yè)過程中掃描周期t下實(shí)時(shí)更新計(jì)算得到的圖像像素坐標(biāo)；

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

步驟2.1：實(shí)時(shí)計(jì)算雷達(dá)與地面間的高度變化量以及攝像機(jī)與地面間的高度變化量，具體的為，

掃描周期t中i時(shí)刻與i-1時(shí)刻雷達(dá)與地面高度距離變化值為△hsti，采用平均法計(jì)算掃描周期t中雷達(dá)相對(duì)地面的高度變化量△hst，

（2-3）；

假設(shè)掃描周期t中采樣時(shí)刻i與采樣時(shí)刻i-1下攝像機(jī)與地面高度變化值為△hcti，采用平均法計(jì)算掃描周期t中攝像機(jī)與地面的高度變化量△hct，

（2-4）；

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

（2-5）

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

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

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

步驟3.2：對(duì)隨機(jī)噪聲信號(hào)進(jìn)行濾波，保證雷達(dá)數(shù)據(jù)的連續(xù)有效性，具體的為，定義z=[r,α,v]^t為雷達(dá)的測(cè)量值，z(k)為毫米波雷達(dá)第k次輸出的測(cè)量值，

（3-1）

過濾掉不符合式（3-1）的數(shù)據(jù)信號(hào)；其中，d為相鄰量測(cè)向量z(k)、z(k-1)之間的加權(quán)歐氏距離，s為加權(quán)矩陣，rs為設(shè)定的閾值；

步驟3.3：判定目標(biāo)是否在農(nóng)機(jī)行駛的車道內(nèi)，當(dāng)雷達(dá)前方物體滿足di≤ds時(shí)，目標(biāo)在農(nóng)機(jī)行駛車道內(nèi)，否則，目標(biāo)不在農(nóng)機(jī)行駛車道內(nèi)，農(nóng)機(jī)行駛車道內(nèi)的目標(biāo)初選為有效目標(biāo)，并對(duì)其按照由近及遠(yuǎn)的準(zhǔn)則進(jìn)行排序編號(hào)；在農(nóng)機(jī)行駛車道外的目標(biāo)為非危險(xiǎn)目標(biāo)，將其排除；其中，ds為安全距離閾值，ds=l/2+ks，di為i采樣點(diǎn)下測(cè)得的目標(biāo)與z0軸之間的距離，l為農(nóng)機(jī)上懸掛的犁具寬度，ks為設(shè)定的安全余量；

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

圖6中為障礙物在行駛車道內(nèi)時(shí)的顯示，障礙物e距離農(nóng)機(jī)中心o農(nóng)機(jī)的距離小于l/2+ks，e在農(nóng)機(jī)行駛車道內(nèi)；

步驟3.4：對(duì)初選的有效目標(biāo)進(jìn)行有效性檢驗(yàn)，最終確定有效目標(biāo)；

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

步驟3.4中對(duì)初選的有效目標(biāo)進(jìn)行有效性檢驗(yàn)具體包括以下步驟，

步驟3.4.1：對(duì)初選的有效目標(biāo)進(jìn)行預(yù)測(cè)，選取狀態(tài)sn=[dn,vn,an],初選有效目標(biāo)的狀態(tài)預(yù)測(cè)方程為，

（3-2）

其中，是上一個(gè)掃描周期預(yù)測(cè)的有效障礙物目標(biāo)的狀態(tài)信息，分別表示毫米波雷達(dá)第n探測(cè)周期內(nèi)測(cè)得的有效障礙物目標(biāo)的相對(duì)距離、相對(duì)速度、相對(duì)加速度，t是毫米波雷達(dá)的掃描周期；

步驟3.4.2：通過比較預(yù)測(cè)的第n+1周期有效目標(biāo)的狀態(tài)信息和雷達(dá)實(shí)際測(cè)得的第n+1周期有效目標(biāo)的狀態(tài)信息，具體如下，

（3-3）

其中，d0、v0、a0是設(shè)定的有效障礙物目標(biāo)測(cè)量值與預(yù)測(cè)值之間的誤差閾值；

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

步驟4中判斷最危險(xiǎn)目標(biāo)的動(dòng)靜狀態(tài)具體包括以下步驟，

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

步驟4.2：根據(jù)相對(duì)速度大小判定最危險(xiǎn)目標(biāo)的動(dòng)靜狀態(tài)，具體如下，v≠v車（4-2）

在一個(gè)掃描周期內(nèi)，（4-2）式始終成立時(shí)，判定目標(biāo)的狀態(tài)為動(dòng)態(tài)，此時(shí)，導(dǎo)航箱發(fā)出聲光報(bào)警并在zd=zmin時(shí)做停車等待處理；否則，農(nóng)機(jī)繼續(xù)行駛，并返回至步驟3.1循環(huán)執(zhí)行其中，v為雷達(dá)相對(duì)目標(biāo)的速度大小，v車為農(nóng)機(jī)的行駛速度；

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

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

步驟4.2a：攝像機(jī)獲取最危險(xiǎn)目標(biāo)的圖像，將圖像與訓(xùn)練好的人體樣本訓(xùn)練庫(kù)進(jìn)行匹配比較，輸出目標(biāo)識(shí)別結(jié)果；

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

zwn+1>zwn（4-3）

di>ds（4-4）

若雷達(dá)探測(cè)到的人體目標(biāo)滿足（4-3）或（4-4），則農(nóng)機(jī)繼續(xù)向前行駛，否則導(dǎo)航箱控制農(nóng)機(jī)做停車等待處理；zwn為第n探測(cè)掃描周期雷達(dá)相對(duì)最危險(xiǎn)目標(biāo)的距離，zw（n+1）為下一個(gè)掃描周期雷達(dá)相對(duì)最危險(xiǎn)目標(biāo)的距離。

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

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

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