[发明专利]基于农田环境感知的农机无人驾驶导航方法

权利要求书

1.一种基于农田环境感知的农机无人驾驶导航方法，其特征在于，具体包括以下步骤，

步骤1：农机作业前，对摄像机进行标定，摄像机空间坐标变换，再对雷达视觉联合标定，使得雷达和视觉信息在空间上融合；

步骤2：农机作业时，距离检测装置一实时检测雷达与地面间的高度变化△h_st，距离检测装置二实时检测摄像机与地面间的高度变化△h_ct，工控机进行数据处理实时调整雷达与摄像机坐标的转换关系，使雷达与摄像机在作业条件下实现在空间上的同步；

步骤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-1）

式（1-1）中，R为一个三行三列的正交单位矩阵， s_c（x_c0，y_c0，z_c0）为初始条件下车辆坐标系到摄像机坐标系的1*3平移矩阵，x_c为摄像机所在的中心轴与车辆中心轴线两直线的距离，y_c为初始条件下摄像机距离地面的高度，z_c为摄像机距离农机后轴的距离；

步骤1.4：将摄像机坐标（x_c，y_c，z_c，1）^T转换到图像物理坐标（x₁，y₁）^T，具体的转换关系如下，

（1-2）

式（1-2）中，f为摄像机的焦距，焦距单位为mm；

步骤1.5：将图像物理坐标（x₁，y₁）^T转换到图像像素坐标（u,v），具体的转换关系如下：

（1-3）

其中，dx,dy分别表示每个像素在横轴和纵轴上单位大小，u₀、v₀分别为图像像素坐标系下摄像机光轴和成像平面交点的横纵坐标，坐标单位为pixel；

步骤1.6：根据以上公式（1-1）～（1-3）得到图像像素坐标系到车辆坐标系的转换公式，具体的为，

（1-4）；

步骤1.7：为了使雷达和视觉信息在空间上融合，将步骤1.6中的坐标转换关系更新为，

（1-5）；

其中，s=s_c+s₀，s₀的坐标设为（x_s0,y_s0,z_s0），x_s0=0，y_s0为初始条件下雷达距离地面的高度，z_s0为雷达与农机后轴的距离。