CN108169743A - 农机无人驾驶用农田环境感知方法 - Google Patents

Abstract

本发明提供了农机控制技术领域内的一种农机无人驾驶用农田环境感知方法，具体包括以下步骤，步骤1：农机作业前，对摄像机进行标定，将雷达和视觉信息在空间上融合；步骤2：农机作业时，距离检测装置一检测雷达与地面间的高度变化，距离检测装置二检测摄像机与地面间的高度变化，实时调整雷达与摄像机坐标的转换，使雷达与摄像机在空间上同步；步骤3：工控机解算接收到的毫米波雷达数据，确定有效目标，确定最危险目标，同步采集摄像机图像；步骤4：根据雷达信息判断最危险目标状态，根据雷达与摄像机采集到的最危险目标的图像数据，判断最危险目标的类型，导航箱控制农机做相应的动作；本发明中数据融合精度高，提高识别障碍物的准确度。

Description

农机无人驾驶用农田环境感知方法

技术领域

本发明涉及一种无人驾驶中的环境感知方法，特别涉及一种农机无人驾驶中的农田环境感知方法。

背景技术

精准农业技术被认为是21世纪农业科技发展的前沿，是科技含最高、集成综合性最强的现代农业生产管理技术之一。精准农业技术根据空间变异，定位、定时、定量的实施一套现代化农事操作技术与管理的系统，是信息技术与农业生产全面结合的一种新型农业技术。

精准农业的应用于快速发展，能够充分挖掘农田最大的生产潜力、合理利用水肥资源、减少环境污染，大幅度提高农产品产量和品质。

发展精准农业技术是解决我国农业由传统农业向现代化农业发展过程中所面临的确保农产品总量、调整农业产业结构、改善农产品品质和质量、资源严重不足且利用率低、环境污染等问题的有效解决方式，也是中国农业现代化发展与转型升级的必经之路。

卫星导航技术是精准农业技术的基本组成之一,使农机实现自动行驶，农机作业前只需要设定好参数后，导航系统便引导农机进入自动作业模式，开始进行直线耕作。在农机自动导航的过程中，农田的环境恶劣且复杂，大型的农田里可能会存在电线杆、田埂、土丘、牲畜以及随时出现的劳作人员等，这些因素都为无人驾驶农机的实现提出了新的挑战。现有技术中，使用卫星导航技术可以实现农机在农田里自动行走，但是农机无法准确识别农机前方的障碍物，即农机无法感应农田环境，更不用说根据感知到的农田环境自动做停车等待还是继续行驶等处理，农机作业时必须得有驾驶员辅助操控农机的动作，一不注意，农机就会与前方障碍物碰撞；因此迫切需要研究出一套农田环境感知方法使无人驾驶的农机具备对周环境进行感知的能力，一旦遇到上述农田中存在电线杆、田埂、土丘、牲畜以及随时出现的劳作人员等情况，能够及时采取停车等待等应急处理。

发明内容

针对现有技术中的缺陷，本发明的目的在于克服上述现有技术中的不足之处，解决现有技术中无人驾驶的农机无法感知农田环境的技术问题，提供一种用于农机无人驾驶的农田环境感知方法，本发明实现了农田环境的感知，感知精度高，识别农机前方障碍物的准确度高，提高农机无人驾驶时的可靠性。

本发明的目的是这样实现的：一种农机无人驾驶用农田环境感知方法，具体包括以下步骤，

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

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

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

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

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

所述距离检测装置一和距离检测装置二的结构相同，距离检测装置一安装在农机前侧且设置在雷达正下方，距离检测装置二安装在农机下侧且设置在摄像机正下方位置；所述距离检测装置一包括具有容纳腔的壳体，容纳腔为圆柱体状，所述壳体内设有可沿着壳体内壁做上下直线运动的滑块，滑块呈长方体状，壳体内设有检测滑块顶部与壳体内壁上侧距离变化的距离传感器，所述滑块和壳体内壁顶部之间连接有至少一个连接件，所述连接件随着滑块的移动而移动，所述导向套的底部开有所述滑块的底部安装有可在地面上滚动的万向滚轮；

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

为了实现雷达和摄像机在空间上的初步同步，所述步骤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为初始条件下车辆坐标系到摄像机坐标系的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下实时更新计算得到的图像像素坐标。

为了进一步提高环境感知精度，所述步骤2中自适应调整后的平移向量中获得△h_t的步骤如下，

步骤2.1：农机作业前，在雷达正下方安装有用来检测雷达与地面间的高度变化的高度检测装置一；在摄像机正下方的农机下侧安装有用来检测摄像机与地面的高度变化量的高度检测装置二；

步骤2.2：实时计算雷达与地面高度变化量以及摄像机与地面的高度变化量，具体的为，

扫描周期t中i时刻与i-1时刻雷达与地面高度距离变化值为△h_sti，采用平均法计算扫描周期t中雷达相对地面的高度变化量△h_st，

假设扫描周期t中采样时刻i与采样时刻i-1下摄像机与地面高度变化值为△h_cti，采用平均法计算扫描周期t中摄像机与地面的高度变化量△h_ct，

步骤2.3：实时计算出扫描周期t下自适应调整后的平移向量s，具体的为，

其中，k为一个扫描周期中的采样点的总数；

此设计中，通过实时检测雷达距离地面的高度变化以及摄像机距离地面的高度变化，实时更新平移向量s，提高摄像机和雷达空间同步的精度。

为了进一步提高距离检测装置结构的可靠性，所述连接件设置有2个，连接件关于滑块在宽度方向上的中心对称设置；所述连接件包括导向套，所述导向套内设有可沿着导向套内壁滑动的导向杆，所述导向套的底部开有导向孔并设有限制导向杆远离导向套的限位阶，所述导向杆穿过导向孔与滑块外侧连接，所述导向套的容纳腔正上方对应的壳体上开有至少一个透气孔；此设计中，滑块向上运动时，滑块推动导向杆向上运动，导向套的容纳腔正上方对应的壳体上开有至少一个透气孔，导向杆逐渐向壳体顶部靠近时，壳体内的空气通过透气孔排出壳体外，导向杆向上运动很轻松；当滑块向下滑动时，滑块拉着导向杆向下运动；导向杆起着连接滑块的作用，同时可避免滑块无限制向下滑动，连接件的结构巧妙，万向滚轮感应农田环境，距离传感器检测滑块距离壳体内壁顶部的高度变化就可得到地面的高低起伏，检测精度高。

作为本发明的进一步改进，所述距离传感器安装在滑块顶部的中心，距离传感器正对壳体内壁顶部。

作为本发明的进一步改进，所述距离传感器安装在壳体内壁顶部的中心，距离传感器正对滑块顶部的中心；

为了进一步提高解算雷达数据的准确性，所述步骤3中的解算雷达数据确定有效目标，具体包括以下步骤，

步骤3.1：对雷达接收到的数据按照毫米波雷达协议进行解算，得到前方物体相对雷达的角度α、距离r、相对速度v、前方物体的反射强度并为每个目标分配唯一一个ID；

步骤3.2：对随机噪声信号进行滤波，保证雷达数据的连续有效性，具体的为，定义z＝[r,α,v]^T为雷达的测量值，z(k)为毫米波雷达第k次输出的测量值，

d²＝S(z(k)-z(k-1))(z(k)-z(k-1))^T＜r_s ² (3-1)

过滤掉不符合式(3-1)的数据信号；其中，d为相邻量测向量z(k)、z(k-1)之间的加权欧氏距离，S为加权矩阵，r_s为设定的阈值；

步骤3.3：判定目标是否在农机行驶的车道内，当雷达前方物体满足di≤ds时，目标在农机行驶车道内，否则，目标不在农机行驶车道内，农机行驶车道内的目标初选为有效目标，并对其按照由近及远的准则进行排序编号；在农机行驶车道外的目标为非危险目标，将其排除；其中，ds为安全距离阈值，ds＝L/2+ks，di为i采样点下测得的目标与Z₀轴之间的距离，L为农机上悬挂的犁具宽度，ks为设定的安全余量；

步骤3.4：对初选的有效目标进行有效性检验，最终确定有效目标；

步骤3.5：根据确定好的有效目标，通过毫米波雷达获取的最近距离障碍物确定为候选的最危险目标，若dj≤dmin,dj为毫米波雷达获取的农机与ID是j的有效目标之间的距离，dmin为在毫米波雷达一个扫描周期内所获取的农机与最近有效目标的距离，则ID是j的有效目标为最危险目标；

此设计中，先对干扰和噪声信号产生的随机噪声信号进行滤除，提高雷达数据解算的准确性；通过对农机行驶车道的判别，排除农机行驶车道外的障碍物目标，初步选定同车道内的障碍物为有效目标，对初选有效目标进行检验以进一步确定有效目标，提高有效目标识别的精确度；对有效目标按照距离由近到远的顺序为规则，确定最危险目标；

所述步骤3.4中对初选的有效目标进行有效性检验具体包括以下步骤，

步骤3.4.1：对初选的有效目标进行预测，选取状态Sn＝[d_n,v_n,a_n],初选有效目标的状态预测方程为，

其中，d_(n+1,n)、v_(n+1,n)、a_(n+1,n)是上一个扫描周期预测的有效障碍物目标的状态信息，d_n,v_n,a_n分别表示毫米波雷达第n探测周期内测得的有效障碍物目标的相对距离、相对速度、相对加速度，t是毫米波雷达的扫描周期；

步骤3.4.2：通过比较预测的第n+1周期有效目标的状态信息和雷达实际测得的第n+1周期有效目标的状态信息，具体如下，

其中，d₀、v₀、a₀是设定的有效障碍物目标测量值与预测值之间的误差阈值；

步骤3.4.3：有效障碍物目标在雷达的扫描周期中被连续探测到m次以上，同时，满足步骤3.4.2中公式(3-3)的有效目标与初选有效目标一致，则更新目标的相对距离、相对速度、相对角度、编号信息；否则，初选的有效目标不处于毫米波雷达探测目标中，使用有效目标预测信息对初选的有效目标进行跟踪，若初选的有效目标在雷达的下一个扫描周期中仍然没有被探测到，则停止使用对应的初选有效目标信息，更新有效目标信息，并返回步骤3.4.1循环执行；

此设计中，通过上一个扫描预测的有效目标的状态信息与测试出来的进行对比，判断有效目标信息是否一致，以此来进一步排除虚假目标，使有效目标的确定得到进一步保障。

作为本发明的进一步改进，所述步骤4中判断最危险目标的动静状态具体包括以下步骤，

步骤4.1：根据步骤2.5中确定的最危险目标，不断更新最危险目标的相对速度和相对距离信息，判断最危险目标与雷达的距离是否在停车距离范围内，即z_d>z_min(4-1)，z_d为毫米波雷达探测到的雷达与最危险目标的相对距离，z_min为设定的停车距离阈值，最危险目标满足公式(4-1)时，农机继续行驶；

步骤4.2：根据相对速度大小判定最危险目标的动静状态，具体如下，

v≠v_车(4-2)

在一个扫描周期内，(4-2)式始终成立时，判定目标的状态为动态，此时，导航箱发出声光报警并在z_d＝z_min时做停车等待处理；否则，农机继续行驶，并返回至步骤3.1循环执行其中，v为雷达相对目标的速度大小，v_车为农机的行驶速度；

此设计中，判定最危险目标的动静态原理简单，提高响应速度。

所述步骤4中根据雷达与相机采集到的最危险目标的图像数据，判断出最危险目标的类型具体包括以下步骤，

步骤4.1a：若最危险目标始终为静态，则导航箱控制农机做停车等待处理；否则，摄像机对最危险目标进行识别；

步骤4.2a：摄像机获取最危险目标的图像，将图像与训练好的人体样本训练库进行匹配比较，输出目标识别结果；

步骤4.3a：导航箱根据输出的目标识别结果控制农机动作，若为非人体，则导航箱发出声光报警，并控制农机做停车等待处理；若目标识别结果为人体，则导航箱发出声光报警，判断人体是否偏离农机行驶车道或者人体向远离农机方向运动，用以下公式判断，

z_wn+1>z_wn (4-3)

di>ds (4-4)

若雷达探测到的人体目标满足(4-3)或(4-4)，则农机继续向前行驶，否则导航箱控制农机做停车等待处理；z_wn为第n探测扫描周期雷达相对最危险目标的距离，z_w(n+1)为下一个扫描周期雷达相对最危险目标的距离；

此设计中，先判断最危险目标的动静状态，若最危险目标始终为静态，则认为最危险目标为电线杆、树木等非生命体，否则，认为最危险目标为农作人员或者牲畜，通过摄像机采集最危险目标的图像并识别出最危险目标是否为人体，输出目标识别结果，若为人体，则导航箱发出声光报警，因为劳作人员自身有避险意识，劳作人员在听到农机的报警声后可能会走出农机行驶车道或者往远离农机运动方向行走，利用劳作人员的习惯性反应设置判断程序，适应性好，避免农机与电线杆、牲畜等非人体的碰撞的同时提醒农机前方的农作人员自觉避让，并根据农作人员的行为做继续行驶或停车等待处理。

农机作业前，摄像机与雷达的标定是在水平地面的条件下进行的；而农机作业时，农田地面凹凸不平，由于雷达和摄像机并非安装于农机的同一位置，雷达和摄像机相对地面的高度不同且随着地形发生变化；距离检测装置一的工作过程具体的为，万向滚轮沿着凹凸不平的地面滚动，当地面凸出时，凸出地面给万向滚轮向上的作用力，滑块沿着壳体内壁向上滑动，滑块推动导向杆向上滑动，距离传感器检测滑块的上升距离，即雷达与地面间的高度变化量；向上凸的地面逐渐水平时，滑块逐渐向下滑动；当地面向下凹进时，滑块在自重的作用下，滑块向下滑动，直至万向滚轮与地面接触，距离传感器检测滑块与壳体内壁间的高度变化，距离传感器实时将检测到的雷达与当前地面间的高度变化值发送给工控机；距离检测装置二的工作原理跟距离检测装置一的工作原理相同，距离检测装置二实时将检测到的摄像机与当前地面间的高度变化值发送给工控机；

本发明与现有技术相比，本发明将毫米波雷达和相机结合起来感知农田环境，实时检测雷达和摄像机距离地面的高度变化，将高度变化量加入到雷达和摄像机坐标转换的平移向量中，农机作业时，使摄像机和雷达在空间上实现真正的同步，提高摄像机和雷达的融合精度；对噪声和干扰信号产生的随机噪声信号进行滤除，提高雷达探测信号的准确性；根据设定好的农机航向确定为农机行驶车道，将农机行驶车道内的障碍物目标初选为有效目标，再对初选的有效目标进行进一步检验，以进一步确定有效目标，提高雷达感知同车道内障碍物目标的有效性和准确性；选取最危险目标并跟踪最危险目标，摄像机以最危险目标的动静态为基础进行目标识别，若最危险目标为动态，只需要识别出动态目标是否为人体即可，不需要识别出具体类型，减小运算量，提高响应速度，导航箱根据图像识别结果控制农机动作，避免农机在无人驾驶时与障碍物碰撞；若识别结果为人体时，导航箱声光报警提醒劳作人员避开农机，利用人的习惯思维这一特性，不断检测人体是否偏离农机行驶车道或者人体向远离农机方向运动，导航箱根据检测结果控制农机是否做停车等待处理，适应性好；本发明可应用于农田地坑深度不大的农机导航工作中，这里的地坑深度不大指的是农田中任一地坑深度小于导向套内容纳腔的高度。

附图说明

图1为本发明基于毫米波雷达和摄像机的感知农田环境方法的流程图。

图2为本发明中摄像机坐标系和车辆坐标系的关系示意图。

图3为本发明中摄像机坐标系与图像物理坐标系的关系示意图。

图4为本发明中图像物理坐标系与图像像素坐标系的关系示意图。

图5为本发明中农机行驶过程中农田环境示意图。

图6为本发明中农机行驶过程中车道判别示意图。

图7为本发明中对初选有效目标进行检验以进一步确定有效目标的流程图。

图8为本发明中距离检测装置一的一个结构示意图。

图9为本发明中距离检测装置一的一个A-A向视图。

图10为本发明中距离检测装置一的另一个A-A向视图。

图11为本发明中距离检测装置的另一个结构示意图。

其中，1滑块，2壳体，3透气孔，4导向套，5导向杆，6限位阶，7容纳腔，8距离传感器，9万向滚轮。

具体实施方式

下面结合附图对本发明作进一步的说明。

实施例1

如图1～9所示的一种农机无人驾驶用农田环境感知方法，具体包括以下步骤：

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

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

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

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

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

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

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

为了实现摄像机和毫米波雷达在空间上的初步同步，如图2～4所示，步骤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)中，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-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下实时更新计算得到的图像像素坐标；

步骤2中实时调整后的平移向量中获得△h_t的步骤如下，

步骤2.1：实时计算雷达与地面间的高度变化量以及摄像机与地面间的高度变化量，具体的为，

扫描周期t中i时刻与i-1时刻雷达与地面高度距离变化值为△h_sti，采用平均法计算扫描周期t中雷达相对地面的高度变化量△h_st，

假设扫描周期t中采样时刻i与采样时刻i-1下摄像机与地面高度变化值为△h_cti，采用平均法计算扫描周期t中摄像机与地面的高度变化量△h_ct，

步骤2.2：实时计算出扫描周期t下自适应调整后的平移向量s，具体的为，

其中，k为一个扫描周期中的采样点的总数。

步骤3中的解算雷达数据确定有效目标，具体包括以下步骤，

步骤3.1：对雷达接收到的数据按照毫米波雷达协议进行解算，得到前方物体相对雷达的角度α、距离r、相对速度v、前方物体的反射强度并为每个目标分配唯一一个ID；

步骤3.2：对随机噪声信号进行滤波，保证雷达数据的连续有效性，具体的为，定义z＝[r,α,v]^T为雷达的测量值，z(k)为毫米波雷达第k次输出的测量值，

d²＝S(z(k)-z(k-1))(z(k)-z(k-1))^T＜r_s ² (3-1)

过滤掉不符合式(3-1)的数据信号；其中，d为相邻量测向量z(k)、z(k-1)之间的加权欧氏距离，S为加权矩阵，r_s为设定的阈值；

步骤3.3：判定目标是否在农机行驶的车道内，当雷达前方物体满足di≤ds时，目标在农机行驶车道内，否则，目标不在农机行驶车道内，农机行驶车道内的目标初选为有效目标，并对其按照由近及远的准则进行排序编号；在农机行驶车道外的目标为非危险目标，将其排除；其中，ds为安全距离阈值，ds＝L/2+ks，di为i采样点下测得的目标与Z₀轴之间的距离，L为农机上悬挂的犁具宽度，ks为设定的安全余量；

下面举例说明，从图5中可以看出，B、C这2个障碍物距离农机中心的纵向距离大于ds，在农机行驶车道外；A、D这2个障碍物距离农机中心的纵向距离小于ds，在农机行驶车道内，则A和D初选为有效目标；

图6中为障碍物在行驶车道内时的显示，障碍物E距离农机中心O_农机的距离小于L/2+ks，E在农机行驶车道内；

步骤3.4：对初选的有效目标进行有效性检验，最终确定有效目标；

步骤3.5：根据确定好的有效目标，通过毫米波雷达获取的最近距离障碍物确定为候选的最危险目标，若dj≤dmin,dj为毫米波雷达获取的农机与ID是j的有效目标之间的距离，dmin为在毫米波雷达一个扫描周期内所获取的农机与最近有效目标的距离，则ID是j的有效目标为最危险目标；

步骤3.4中对初选的有效目标进行有效性检验具体包括以下步骤，

步骤3.4.1：对初选的有效目标进行预测，选取状态Sn＝[d_n,v_n,a_n],初选有效目标的状态预测方程为，

其中，d_(n+1,n)、v_(n+1,n)、a_(n+1,n)是上一个扫描周期预测的有效障碍物目标的状态信息，d_n,v_n,a_n分别表示毫米波雷达第n探测周期内测得的有效障碍物目标的相对距离、相对速度、相对加速度，t是毫米波雷达的扫描周期；

步骤3.4.2：通过比较预测的第n+1周期有效目标的状态信息和雷达实际测得的第n+1周期有效目标的状态信息，具体如下，

其中，d₀、v₀、a₀是设定的有效障碍物目标测量值与预测值之间的误差阈值；

步骤3.4.3：有效障碍物目标在雷达的扫描周期中被连续探测到m次以上，同时，满足步骤3.4.2中公式(3-3)的有效目标与初选有效目标一致，则更新目标的相对距离、相对速度、相对角度、编号信息；否则，初选的有效目标不处于毫米波雷达探测目标中，使用有效目标预测信息对初选的有效目标进行跟踪，若初选的有效目标在雷达的下一个扫描周期中仍然没有被探测到，则停止使用对应的初选有效目标信息，更新有效目标信息，并返回步骤3.4.1循环执行；

步骤4中判断最危险目标的动静状态具体包括以下步骤，

步骤4.1：根据步骤2.5中确定的最危险目标，不断更新最危险目标的相对速度和相对距离信息，判断最危险目标与雷达的距离是否在停车距离范围内，即z_d>z_min(4-1)，z_d为毫米波雷达探测到的雷达与最危险目标的相对距离，z_min为设定的停车距离阈值，最危险目标满足公式(4-1)时，农机继续行驶；

步骤4.2：根据相对速度大小判定最危险目标的动静状态，具体如下，

v≠v_车 (4-2)

在一个扫描周期内，(4-2)式始终成立时，判定目标的状态为动态，此时，导航箱发出声光报警并在z_d＝z_min时做停车等待处理；否则，农机继续行驶，并返回至步骤3.1循环执行其中，v为雷达相对目标的速度大小，v_车为农机的行驶速度；

步骤4中根据雷达与相机采集到的最危险目标的图像数据，判断出最危险目标的类型具体包括以下步骤，

步骤4.1a：若最危险目标始终为静态，则导航箱控制农机做停车等待处理；否则，摄像机对最危险目标进行识别；

步骤4.2a：摄像机获取最危险目标的图像，将图像与训练好的人体样本训练库进行匹配比较，输出目标识别结果；

步骤4.3a：导航箱根据输出的目标识别结果控制农机动作，若为非人体，则导航箱发出声光报警，并控制农机做停车等待处理；若目标识别结果为人体，则导航箱发出声光报警，判断人体是否偏离农机行驶车道或者人体向远离农机方向运动，用以下公式判断，

z_wn+1>z_wn (4-3)

di>ds (4-4)

若雷达探测到的人体目标满足(4-3)或(4-4)，则农机继续向前行驶，否则导航箱控制农机做停车等待处理；z_wn为第n探测扫描周期雷达相对最危险目标的距离，z_w(n+1)为下一个扫描周期雷达相对最危险目标的距离。

农机作业前，摄像机与雷达的标定是在水平地面的条件下进行的；而农机作业时，农田地面凹凸不平，由于雷达和摄像机并非安装于农机的同一位置，雷达和摄像机相对地面的高度不同且随着地形发生变化；距离检测装置一的工作过程具体的为，万向滚轮9沿着凹凸不平的地面滚动，当地面凸出时，凸出地面给万向滚轮9向上的作用力，滑块1沿着壳体2内壁向上滑动，滑块1推动导向杆5向上滑动，距离传感器8检测滑块1的上升距离，即雷达与地面间的高度变化量；向上凸的地面逐渐水平时，滑块1逐渐向下滑动；当地面向下凹进时，滑块1在自重的作用下，滑块1向下滑动，直至万向滚轮9与地面接触，距离传感器8检测滑块1与壳体2内壁间的高度变化，距离传感器8实时将检测到的雷达与当前地面间的高度变化值发送给工控机；距离检测装置二的工作原理跟距离检测装置一的工作原理相同，距离检测装置二实时将检测到的摄像机与当前地面间的高度变化值发送给工控机；

本发明与现有技术相比，本发明将毫米波雷达和相机结合起来感知农田环境，实时检测雷达和摄像机距离地面的高度变化，将高度变化量加入到雷达和摄像机坐标转换的平移向量中，农机作业时，使摄像机和雷达在空间上实现真正的同步，提高摄像机和雷达的融合精度；对噪声和干扰信号产生的随机噪声信号进行滤除，提高雷达探测信号的准确性；根据设定好的农机航向确定为农机行驶车道，将农机行驶车道内的障碍物目标初选为有效目标，再对初选的有效目标进行进一步检验，以进一步确定有效目标，提高雷达感知同车道内障碍物目标的有效性和准确性；选取最危险目标并跟踪最危险目标，摄像机以最危险目标的动静态为基础进行目标识别，若最危险目标为动态，只需要识别出动态目标是否为人体即可，不需要识别出具体类型，减小运算量，提高响应速度，导航箱根据图像识别结果控制农机动作，避免农机在无人驾驶时与障碍物碰撞；若识别结果为人体时，导航箱声光报警提醒劳作人员避开农机，利用人的习惯思维这一特性，不断检测人体是否偏离农机行驶车道或者人体向远离农机方向运动，导航箱根据检测结果控制农机是否做停车等待处理，适应性好；本发明可应用于农田地坑深度不大的农机导航工作中，这里的地坑深度不大指的是农田中任一地坑深度小于导向套内容纳腔的高度。

实施例2

本实施例与实施例1的不同之处在于，如图10所示，壳体2呈长方体状。

实施例3

本实施例与实施例1和实施例2的不同之处在于，如图11所示，距离传感器8安装在壳体2内壁顶部的中心，距离传感器8正对滑块1顶部的中心。

本发明并不局限于上述实施例，在本发明公开的技术方案的基础上，本领域的技术人员根据所公开的技术内容，不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形，这些替换和变形均在本发明保护范围内。

Claims (10)

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)中，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-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为雷达与农机后轴的距离。

3.根据权利要求2所述的农机无人驾驶用农田环境感知方法，其特征在于,所述步骤2中的工控机进行数据处理实时调整雷达与摄像机坐标的转换关系，具体的为，根据农机的实际路况实时调整平移向量s，扫描周期t下调整后的平移向量s_t＝s_c+s₀+△s_t，实时的车辆坐标与图像像素坐标的转换关系，具体的为，

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

4.根据权利要求3所述的农机无人驾驶用农田环境感知方法，其特征在于,

所述步骤2中实时调整后的平移向量中获得△h_t的步骤如下，

步骤2.1：实时计算雷达与地面间的高度变化量以及摄像机与地面间的高度变化量，具体的为，

扫描周期t中i时刻与i-1时刻雷达与地面高度距离变化值为△h_sti，采用平均法计算扫描周期t中雷达相对地面的高度变化量△h_st，

假设扫描周期t中采样时刻i与采样时刻i-1下摄像机与地面高度变化值为△h_cti，采用平均法计算扫描周期t中摄像机与地面的高度变化量△h_ct，

步骤2.2：实时计算出扫描周期t下自适应调整后的平移向量s，具体的为，

其中，k为一个扫描周期中的采样点的总数。

5.根据权利要求1～4任一项所述的农机无人驾驶用农田环境感知方法，其特征在于，所述连接件设置有2个，连接件关于滑块在宽度方向上的中心对称设置；所述连接件包括导向套，所述导向套内设有可沿着导向套内壁滑动的导向杆，所述导向套的底部开有导向孔并设有限制导向杆远离导向套的限位阶，所述导向杆穿过导向孔与滑块外侧连接，所述导向套的容纳腔正上方对应的壳体上开有至少一个透气孔。

6.根据权利要5所述的农机无人驾驶用农田环境感知方法，其特征在于，所述距离传感器安装在滑块顶部的中心，距离传感器正对壳体内壁顶部。

7.根据权利要求5所述的农机无人驾驶用农田环境感知方法，其特征在于，所述距离传感器安装在壳体内壁顶部的中心，距离传感器正对滑块顶部的中心。

8.根据权利要求6或7所述的农机无人驾驶用农田环境感知方法，其特征在于,所述步骤3中的解算雷达数据确定有效目标，具体包括以下步骤，

步骤3.1：对雷达接收到的数据按照毫米波雷达协议进行解算，得到前方物体相对雷达的角度α、距离r、相对速度v、前方物体的反射强度并为每个目标分配唯一一个ID；

步骤3.2：对随机噪声信号进行滤波，保证雷达数据的连续有效性，具体的为，定义z＝[r,α,v]^T为雷达的测量值，z(k)为毫米波雷达第k次输出的测量值，

d²＝S(z(k)-z(k-1))(z(k)-z(k-1))^T＜r_s ² (3-1)

过滤掉不符合式(3-1)的数据信号；其中，d为相邻量测向量z(k)、z(k-1)之间的加权欧氏距离，S为加权矩阵，r_s为设定的阈值；

步骤3.3：判定目标是否在农机行驶的车道内，当雷达前方物体满足di≤ds时，目标在农机行驶车道内，否则，目标不在农机行驶车道内，农机行驶车道内的目标初选为有效目标，并对其按照由近及远的准则进行排序编号；在农机行驶车道外的目标为非危险目标，将其排除；其中，ds为安全距离阈值，ds＝L/2+ks，di为i采样点下测得的目标与Z₀轴之间的距离，L为农机上悬挂的犁具宽度，ks为设定的安全余量；

步骤3.4：对初选的有效目标进行有效性检验，最终确定有效目标；

步骤3.5：根据确定好的有效目标，通过毫米波雷达获取的最近距离障碍物确定为候选的最危险目标，若dj≤dmin,dj为毫米波雷达获取的农机与ID是j的有效目标之间的距离，dmin为在毫米波雷达一个扫描周期内所获取的农机与最近有效目标的距离，则ID是j的有效目标为最危险目标；

其中，所述步骤3.4中对初选的有效目标进行有效性检验具体包括以下步骤，

步骤3.4.1：对初选的有效目标进行预测，选取状态Sn＝[d_n,v_n,a_n],初选有效目标的状态预测方程为，

其中，d_(n+1,n)、v_(n+1,n)、a_(n+1,n)是上一个扫描周期预测的有效障碍物目标的状态信息，d_n,v_n,a_n分别表示毫米波雷达第n探测周期内测得的有效障碍物目标的相对距离、相对速度、相对加速度，t是毫米波雷达的扫描周期；

步骤3.4.2：通过比较预测的第n+1周期有效目标的状态信息和雷达实际测得的第n+1周期有效目标的状态信息，具体如下，

其中，d₀、v₀、a₀是设定的有效障碍物目标测量值与预测值之间的误差阈值；

步骤3.4.3：有效障碍物目标在雷达的扫描周期中被连续探测到m次以上，同时，满足步骤3.4.2中公式(3-3)的有效目标与初选有效目标一致，则更新目标的相对距离、相对速度、相对角度、编号信息；否则，初选的有效目标不处于毫米波雷达探测目标中，使用有效目标预测信息对初选的有效目标进行跟踪，若初选的有效目标在雷达的下一个扫描周期中仍然没有被探测到，则停止使用对应的初选有效目标信息，更新有效目标信息，并返回步骤3.4.1循环执行。

9.根据权利要求8所述的农机无人驾驶用农田环境感知方法，其特征在于，所述步骤4中判断最危险目标的动静状态具体包括以下步骤，

步骤4.1：根据步骤3.5中确定的最危险目标，不断更新最危险目标的相对速度和相对距离信息，判断最危险目标与雷达的距离是否在停车距离范围内，即z_d>z_min(4-1)，z_d为毫米波雷达探测到的雷达与最危险目标的相对距离，z_min为设定的停车距离阈值，最危险目标满足公式(4-1)时，农机继续行驶；

步骤4.2：根据相对速度大小判定最危险目标的动静状态，具体如下，

v≠v_车 (4-2)

在一个扫描周期内，(4-2)式始终成立时，判定目标的状态为动态，此时，导航箱发出声光报警并在z_d＝z_min时做停车等待处理；否则，农机继续行驶，并返回至步骤3.1循环执行其中，v为雷达相对目标的速度大小，v_车为农机的行驶速度。

10.根据权利要求9所述的农机无人驾驶用农田环境感知方法，其特征在于，所述步骤4中根据雷达与相机采集到的最危险目标的图像数据，判断出最危险目标的类型具体包括以下步骤，

步骤4.1a：若最危险目标始终为静态，则导航箱控制农机做停车等待处理；否则，摄像机对最危险目标进行识别；

步骤4.2a：摄像机获取最危险目标的图像，将图像与训练好的人体样本训练库进行匹配比较，输出目标识别结果；

步骤4.3a：导航箱根据输出的目标识别结果控制农机动作，若为非人体，则导航箱发出声光报警，并控制农机做停车等待处理；若目标识别结果为人体，则导航箱发出声光报警，判断人体是否偏离农机行驶车道或者人体向远离农机方向运动，用以下公式判断，

z_wn+1>z_wn (4-3)

di>ds (4-4)

若雷达探测到的人体目标满足(4-3)或(4-4)，则农机继续向前行驶，否则导航箱控制农机做停车等待处理；z_wn为第n探测扫描周期雷达相对最危险目标的距离，z_w(n+1)为下一个扫描周期雷达相对最危险目标的距离。