[發明專利]用于農機無人駕駛的多傳感器融合的農田環境感知方法

在審

• 申請號: 201710141692.5
• 申請日: 2017-03-10
• 公開（公告）號: CN106874886A
• 公開（公告）日: 2017-06-20
• 發明（設計）人: 張瑞宏;奚小波;金亦富;張劍峰;單翔;蔡廣林;孫福華;葉偉偉;史揚杰;馬國梁
• 申請（專利權）人: 南京沃楊機械科技有限公司
• 主分類號: G06K9/00
• 分類號: G06K9/00;G06T7/80;G06K9/32;G01S13/88;G06K9/40;G05D1/02
• 代理公司: 南京蘇科專利代理有限責任公司32102
• 代理人: 沈良菊

1.一種用于農機無人駕駛的多傳感器融合的農田環境感知方法，其特征在于，具體包括以下步驟，

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

步驟2：農機作業時，實時調整雷達與攝像機坐標的轉換關系，使雷達與攝像機在作業條件下實現在空間上的同步；

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

步驟4：根據雷達的信息判斷出最危險目標的運動狀態，根據雷達與相機采集到的最危險目標的圖像數據，判斷出最危險目標的類型，工控機將動作指令傳輸給導航箱，導航箱控制農機做相應的動作；

其中，農機作業時，農機的行駛速度勻速。

2.根據權利要求1所述的用于農機無人駕駛的多傳感器融合的農田環境感知方法，其特征在于，所述步驟1中的將雷達坐標轉換成圖像像素坐標具體包括以下步驟，

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

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

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

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

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

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

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

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

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