[发明专利]基于农田环境感知的农机导航控制方法

权利要求书

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

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

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

步骤3：工控机解算接收到的毫米波雷达数据，确定有效目标，选出农机作业前方雷达感兴趣的区域，确定最危险目标，同步进行摄像机图像的采集；

步骤4：根据雷达的信息判断出最危险目标的运动状态，工控机根据最危险目标的运动状态规划农机行走路径，根据雷达与相机采集到的最危险目标的图像数据，判断出最危险目标的类型，工控机将解析出来的动作指令传输给导航箱，导航箱控制农机做相应的动作；

其中，农机作业时，农机的行驶速度匀速；

所述距离检测装置一和距离检测装置二的结构相同，距离检测装置一安装在农机前侧且设置在雷达正下方，距离检测装置二安装在农机下侧且设置在摄像机正下方位置；所述距离检测装置一包括具有容纳腔且可开合的壳体，容纳腔为圆柱体状，所述壳体内设有可沿着壳体内壁做上下直线运动的滑块，壳体内设有检测滑块顶部与壳体内壁上侧距离变化的距离传感器，所述滑块和壳体内壁顶部之间连接有2个连接件，连接件关于滑块在宽度方向上的中心对称设置；所述连接件包括导向套，所述导向套内设有可沿着导向套内壁滑动的导向杆，滑块的前后两侧与壳体内壁之间留有可容纳导向杆的间隙，所述导向套的底部开有导向孔并设有限制导向杆远离导向套的限位阶，所述导向杆穿过导向孔与滑块外侧连接，所述导向套的容纳腔正上方对应的壳体上开有至少一个透气孔，所述滑块的底部安装有可在地面上滚动的万向滚轮；

工控机接收所述距离传感器发送过来的数据信号并进行数据处理；

所述步骤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)中，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：根据以上公式(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为雷达与农机后轴的距离；

所述步骤2中的工控机进行数据处理实时调整雷达与摄像机坐标的转换关系，具体的为，根据农机的实际路况实时调整平移向量s，扫描周期t下调整后的平移向量s_t＝s_c+s₀+△s_t，实时的车辆坐标与图像像素坐标的转换关系，具体的为，

其中，△h_ct为扫描周期t下摄像机与地面高度的变化值，△h_st为扫描周期t下雷达与地面高度的变化值，j为扫描周期数，(u_t,v_t)为农机作业过程中扫描周期t下实时更新计算得到的图像像素坐标；

农机行驶车道内的目标初选为有效目标，距离农机最近的障碍物为候选的最危险目标。