基于农田环境感知的农机无人驾驶导航方法与流程

技术特征：

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

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.根据权利要求4所述的基于农田环境感知的农机无人驾驶导航方法，其特征在于，所述步骤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的有效目标为最危险目标。

6.根据权利要求5所述的基于农田环境感知的农机无人驾驶导航方法，其特征在于，所述步骤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循环执行。

7.根据权利要求6所述的基于农田环境感知的农机无人驾驶导航方法，其特征在于，所述步骤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_车为农机的行驶速度；(4-2)式始终不成立时，判定目标为静态，则工控机做出避障处理，具体的为，摄像机扫描出障碍物的边缘轮廓，工控机根据农机的犁具宽度和农机最小转弯半径设定一条避障路径；工控机按照设定好的避障路径解析出农机的前轮转角，并将动作指令发送给导航箱，导航箱控制农机按照设定的避障路径行走；

农机的左右两侧中心位置分别安装有雷达一和雷达二；农机在避障过程中，雷达一和雷达二不断扫描农机左右两侧是否有障碍物，设定雷达一与障碍物的相对距离为d₁，设定雷达二与障碍物的相对距离为d₂，根据以下公式判断农机是否继续按照避障路径行走，

d₁<d_s0 (4-3)

d₂<d_s0 (4-4)

(4-3)或(4-4)任意一个式子成立时，工控机做出停车等待决策，导航箱控制农机停下；否则，农机继续按照避障路径行走；

其中，d_s0为设定的转弯安全距离。

8.根据权利要求7所述的基于农田环境感知的农机无人驾驶导航方法，其特征在于，所述步骤4.2中的避障路径具体的为，

以障碍物的中心为圆心做特征圆，特征圆的半径为r_min+w/2，避障路径由圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三组成，圆弧段一的一端与农机原始的直线路径相切，圆弧段一的另一端与直线段一的一端相切，直线段一的另一端和直线段二的一端分别与圆弧段二相切，直线段二的另一端与圆弧段三相切，圆弧段二为特征圆上的一段，圆弧段一和圆弧段三关于圆弧段二的中心线对称设置，农机依次经过圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三绕过障碍物，其中，r_min为农机的最小转弯半径，w为农机的作业宽度，障碍物的外接圆半径小于最小转弯半径r_min；圆弧段一的半径为r_min，所述圆弧段三的半径为r_min，圆弧段一的起点记为H点，圆弧段一的圆心记为O₁点，直线段一与农机原始的直线路径的相交点记为J，直线段一与圆弧段二的相切点记为D，农机原始路径与特征圆的相交点分别记为K和K’，JK＝w/2，圆弧段二的圆心记为O点，O的坐标设为(a，b)，圆弧段二的中心点记为B点，J点的坐标记为(x1，y1)，JD的方程可以写成：

y＝k(x-x₁)+y₁ (4-5)；

特征圆的方程可以写成：

(x-a)²+(y-b)²＝r²

r＝r_min+w/2 (4-6)

通过(4-5)和(4-6)可以求出k，D点为JD和特征圆的相交点，以此解出D点坐标；

设点O₁的坐标为(x₂,y₂)，则点O₁到直线JD的距离为：

根据公式(4-7)和(4-8)求出O₁的坐标；则H点的坐标为(x₂，y₁)，B点的坐标为(a，b+r)。

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

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

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

z_wn+1>z_wn (4-3)

di>ds (4-4)

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