CN106874886A - 用于农机无人驾驶的多传感器融合的农田环境感知方法 - Google Patents
用于农机无人驾驶的多传感器融合的农田环境感知方法 Download PDFInfo
- Publication number
- CN106874886A CN106874886A CN201710141692.5A CN201710141692A CN106874886A CN 106874886 A CN106874886 A CN 106874886A CN 201710141692 A CN201710141692 A CN 201710141692A CN 106874886 A CN106874886 A CN 106874886A
- Authority
- CN
- China
- Prior art keywords
- radar
- agricultural machinery
- target
- video camera
- risk object
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 27
- 230000004927 fusion Effects 0.000 title claims abstract description 19
- 230000001149 cognitive effect Effects 0.000 title claims abstract description 16
- 230000000007 visual effect Effects 0.000 claims abstract description 8
- 230000000875 corresponding effect Effects 0.000 claims abstract description 7
- 238000012552 review Methods 0.000 claims abstract description 4
- 238000001514 detection method Methods 0.000 claims description 26
- 230000008859 change Effects 0.000 claims description 25
- 238000013519 translation Methods 0.000 claims description 21
- 238000006243 chemical reaction Methods 0.000 claims description 15
- 238000003384 imaging method Methods 0.000 claims description 12
- 230000003287 optical effect Effects 0.000 claims description 12
- 230000004888 barrier function Effects 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 9
- 230000009471 action Effects 0.000 claims description 7
- 238000005096 rolling process Methods 0.000 claims description 7
- 239000000523 sample Substances 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000012549 training Methods 0.000 claims description 6
- 230000008447 perception Effects 0.000 claims description 5
- 230000003068 static effect Effects 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 3
- 230000001360 synchronised effect Effects 0.000 abstract description 3
- 238000005516 engineering process Methods 0.000 description 11
- 238000013461 design Methods 0.000 description 8
- 238000009313 farming Methods 0.000 description 5
- 238000011161 development Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 244000144972 livestock Species 0.000 description 4
- 238000012271 agricultural production Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 238000011897 real-time detection Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 238000003912 environmental pollution Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 206010052804 Drug tolerance Diseases 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 210000003608 fece Anatomy 0.000 description 1
- 230000026781 habituation Effects 0.000 description 1
- 239000007788 liquid Substances 0.000 description 1
- 239000010871 livestock manure Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 239000002689 soil Substances 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/10—Terrestrial scenes
- G06V20/188—Vegetation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/88—Radar or analogous systems specially adapted for specific applications
- G01S13/882—Radar or analogous systems specially adapted for specific applications for altimeters
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/20—Image preprocessing
- G06V10/25—Determination of region of interest [ROI] or a volume of interest [VOI]
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/20—Image preprocessing
- G06V10/30—Noise filtering
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/50—Context or environment of the image
- G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
- G06V20/58—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/50—Context or environment of the image
- G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
- G06V20/588—Recognition of the road, e.g. of lane markings; Recognition of the vehicle driving pattern in relation to the road
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
- G06T2207/30256—Lane; Road marking
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
- G06T2207/30261—Obstacle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Multimedia (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Health & Medical Sciences (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Electromagnetism (AREA)
- Health & Medical Sciences (AREA)
- Computer Networks & Wireless Communication (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明提供了农机路径控制技术领域内的一种用于农机无人驾驶的多传感器融合的农田环境感知方法,具体包括以下步骤,步骤1:农机作业前,对摄像机进行标定,将雷达和视觉信息在空间上融合;步骤2:农机作业时,实时调整雷达与摄像机坐标的转换关系,使雷达与摄像机实现在空间上的同步;步骤3:工控机解算接收到的毫米波雷达数据,确定有效目标,选出农机作业前方雷达感兴趣的区域,确定最危险目标,同步采集摄像机图像;步骤4:根据雷达信息判断最危险目标状态,根据雷达与摄像机采集到的最危险目标的图像数据,判断最危险目标的类型,导航箱控制农机做相应的动作;本发明数据融合的准确度高,提高识别农机前方障碍物的准确度。
Description
技术领域
本发明涉及一种自动掉头路径规划及其控制方法,特别涉及一种用于农机无人驾驶的多传感器融合的农田环境感知方法。
背景技术
精准农业技术被认为是21世纪农业科技发展的前沿,是科技含最高、集成综合性最强的现代农业生产管理技术之一。精准农业技术根据空间变异,定位、定时、定量的实施一套现代化农事操作技术与管理的***,是信息技术与农业生产全面结合的一种新型农业技术。
精准农业的应用于快速发展,能够充分挖掘农田最大的生产潜力、合理利用水肥资源、减少环境污染,大幅度提高农产品产量和品质。
发展精准农业技术是解决我国农业由传统农业向现代化农业发展过程中所面临的确保农产品总量、调整农业产业结构、改善农产品品质和质量、资源严重不足且利用率低、环境污染等问题的有效解决方式,也是中国农业现代化发展与转型升级的必经之路。
卫星导航技术是精准农业技术的基本组成之一,使农机实现自动行驶,农机作业前只需要设定好参数后,导航***便引导农机进入自动作业模式,开始进行直线耕作。在农机自动导航的过程中,农田的环境恶劣且复杂,大型的农田里可能会存在电线杆、田埂、土丘、牲畜以及随时出现的劳作人员等,这些因素都为无人驾驶农机的实现提出了新的挑战。现有技术中,使用卫星导航技术可以实现农机在农田里自动行走,但是农机无法准确识别农机前方的障碍物,即农机无法感应农田环境,更不用说根据感知到的农田环境自动做停车等待还是继续行驶等处理,农机作业时必须得有驾驶员辅助操控农机的动作,一不注意,农机就会与前方障碍物碰撞;因此迫切需要研究出一套农田环境感知方法使无人驾驶的农机具备对周环境进行感知的能力,一旦遇到上述农田中存在电线杆、田埂、土丘、牲畜以及随时出现的劳作人员等情况,能够及时采取停车等待等应急处理。
发明内容
针对现有技术中的缺陷,本发明的目的在于克服上述现有技术中的不足之处,解决现有技术中无人驾驶的农机无法感知农田环境的技术问题,提供一种用于农机无人驾驶的农田环境感知方法,本发明实现了农田环境的感知,感知精度高,识别农机前方障碍物的准确度高,提高农机无人驾驶时的可靠性。
本发明的目的是这样实现的:一种用于农机无人驾驶的多传感器融合的农田环境感知方法,具体包括以下步骤,
步骤1:农机作业前,对摄像机进行标定,摄像机空间坐标变换,再对雷达视觉联合标定,使得雷达和视觉信息在空间上融合;
步骤2:农机作业时,实时调整雷达与摄像机坐标的转换关系,使雷达与摄像机在作业条件下实现在空间上的同步;
步骤3:工控机解算接收到的毫米波雷达数据,确定有效目标,选出农机作业前方雷达感兴趣的区域,确定最危险目标,同步进行摄像机图像的采集;
步骤4:根据雷达的信息判断出最危险目标的运动状态,根据雷达与相机采集到的最危险目标的图像数据,判断出最危险目标的类型,工控机将动作指令传输给导航箱,导航箱控制农机做相应的动作;
其中,农机作业时,农机的行驶速度匀速。
为了实现雷达和摄像机在空间上的初步同步,所述步骤1中的将雷达坐标转换成图像像素坐标具体包括以下步骤,
步骤1.1:农机作业前,地面默认为水平,将毫米波雷达固定安装在农机的前侧且位于农机纵向中轴,雷达发射面向外,使雷达发射面与地面垂直;安装摄像机时使摄像机的光轴与地面平行;
步骤1.2:以雷达的中心为原点建立雷达坐标系00-X0Y0Z0,毫米波雷达所在平面由X0轴和Y0轴确定并与Z0轴相垂直,Z0轴与地面平行且与农机中心轴线重合;建立摄像机坐标系Oc-XcYcZc,以摄像机的中心为原点Oc,平面XcOcYc平行于摄像机的成像平面,Zc轴是摄像机的取景光轴且垂直于成像平面;建立车辆坐标系Ow-XwYwZw,Ow为农机的农机后轴的中心与车辆的中心轴线的交点,Xw轴水平向右并垂直于农机的纵向中轴线,Zw水平向前且与农机中心轴线重合,Yw轴垂直于水面地面向上,雷达坐标系的X0O0Z0平面与车辆坐标系的XwOwZw平面平行;
步骤1.3:光轴与成像平面相交的点是图像主点O’,车辆坐标通过旋转矩阵R和平移向量sc转换后得到摄像机坐标(xc,yc,zc,1)T,任意点P的车辆坐标为(xw,yw,zw,1)T,将车辆坐标转换为摄像机坐标,具体的转换关系如下,
式(1-2)中,R为一个三行三列的正交单位矩阵,sc为初始条件下车辆坐标系到摄像机坐标系的1*3平移矩阵,xc0为摄像机所在的中心轴与车辆中心轴线两直线的距离,yc0为摄像机距离农机后轴的距离,zc0为初始条件下摄像机距离地面的高度;
步骤1.4:将摄像机坐标(xc,yc,zc,1)T转换到图像物理坐标(x1,y1)T,具体的转换关系如下,
式(1-3)中,f为摄像机的焦距,焦距单位为mm;
步骤1.5:将图像物理坐标(x1,y1)T转换到图像像素坐标(u,v),具体的转换关系如下:
其中,dx,dy分别表示每个像素在横轴和纵轴上单位大小,u0、v0分别为图像像素坐标系下摄像机光轴和成像平面交点的横纵坐标,坐标单位为pixel;
步骤1.6:根据以上公式(1-1)~(1-4)得到图像像素坐标系到车辆坐标系的转换公式,具体的为,
步骤1.7:为了使雷达和视觉信息在空间上融合,雷达坐标与摄像机的坐标转换关系更新为,
其中,s=sc+s0,s0的坐标设为(xs0,ys0,zs0),xs0=0,ys0为初始条件下雷达距离地面的高度,zs0为雷达与农机后轴的距离。
为了提高农机作业过程中雷达和摄像机的融合精度,所述步骤2中的实时调整图像像素坐标与车辆坐标的转换关系,具体的为,根据农机的实际路况实时调整平移向量s,扫描周期t下实时调整后的平移向量st=sc+s0+△st,实时的车辆坐标与图像像素坐标的转换关系,具体的为,
其中,△hct为扫描周期t下摄像机与地面高度的变化值,△hst为扫描周期t下雷达与地面高度的变化值,j为扫描周期数,(ut,vt)为农机作业过程中扫描周期t下实时更新计算得到的图像像素坐标。
为了进一步提高环境感知精度,所述步骤2中实时调整后的平移向量中获得△ht的步骤如下,
步骤2.1:农机作业前,在雷达正下方安装有用来检测雷达与地面间的高度变化的高度检测装置一;在摄像机正下方的农机下侧安装有用来检测摄像机与地面的高度变化量的高度检测装置二;
步骤2.2:实时计算雷达与地面高度变化量以及摄像机与地面的高度变化量,具体的为,
扫描周期t中i时刻与i-1时刻雷达与地面高度距离变化值为△hsti,采用平均法计算扫描周期t中雷达相对地面的高度变化量△hst,
假设扫描周期t中采样时刻i与采样时刻i-1下摄像机与地面高度变化值为△hcti,采用平均法计算扫描周期t中摄像机与地面的高度变化量△hct,
步骤2.3:实时计算出扫描周期t下实时调整后的平移向量s,具体的为,
其中,k为一个扫描周期中的采样点的总数;
此设计中,通过实时检测雷达距离地面的高度变化以及摄像机距离地面的高度变化,实时更新平移向量s,提高摄像机和雷达空间同步的精度。
为了实现雷达距离地面的高度变化和摄像机距离地面的高度变化的检测,所述高度检测装置一和高度检测装置二的结构相同,所述高度检测装置一包括可开合且具有容纳腔的壳体,壳体的底部连接有挡板,壳体上在高度方向上分别间隔连接有轴承一和轴承二,轴承一和轴承二内均连接有螺母,螺母上螺纹连接有丝杆,螺纹升角为35°~40°,所述丝杆的顶部可拆卸地安装有限位板,限位板限制丝杆在容纳腔内运动,所述壳体的顶部可拆卸地安装有正对限位板的距离传感器;所述壳体的底部可拆卸地连接有挡板,所述轴承一的上侧为壳体的限位阶一,所述挡板和限位阶一使得轴承一无法在高度方向上移动;所述壳体上开有凹槽,轴承上侧设有具有弹性的挡片,所述挡片安装在凹槽内,所述轴承二的下侧为壳体的限位阶二,所述挡片和限位阶二使得另一个轴承无法在高度方向上移动;所述丝杆的底部安装有可在地面上滚动的万向滚轮;农田中的地面是凹凸不平的,雷达坐标和摄像机坐标进行空间融合时,平移向量中△ht是实时改变的,导致雷达和摄像机无法实现空间同步,此设计中,地面凸出时,万向滚轮沿着凸出的地面滚动,凸出的地面给丝杆向上的作用力,丝杆向上直线运动,丝杆带动轴承的内圈旋转,限位板上升,距离传感器检测到距离变化量;当地面向下凹时,丝杆在自身重力作用下下降,直到万向滚轮与凹进去的地面接触,限位板在不断下降,距离传感器检测到距离变化量,从而实现实时检测雷达距离地面的高度变化和摄像机距离地面的高度变化;高度检测装置一和高度检测装置二的结构设计巧妙,很好地感应凹凸不平的地面。
为了进一步提高解算雷达数据的准确性,所述步骤3中的解算雷达数据确定有效目标,具体包括以下步骤,
步骤3.1:对雷达接收到的数据按照毫米波雷达协议进行解算,得到前方物体相对雷达的角度α、距离r、相对速度v、前方物体的反射强度并为每个目标分配唯一一个ID;
步骤3.2:对随机噪声信号进行滤波,保证雷达数据的连续有效性,具体的为,定义z=[r,α,v]T为雷达的测量值,z(k)为毫米波雷达第k次输出的测量值,
d2=S(z(k)-z(k-1))(z(k)-z(k-1))T<rs 2 (3-1)
过滤掉不符合式(3-1)的数据信号;其中,d为相邻量测向量z(k)、z(k-1)之间的加权欧氏距离,S为加权矩阵,rs为设定的阈值;
步骤3.3:判定目标是否在农机行驶的车道内,当雷达前方物体满足di≤ds时,目标在农机行驶车道内,否则,目标不在农机行驶车道内,农机行驶车道内的目标初选为有效目标,并对其按照由近及远的准则进行排序编号;在农机行驶车道外的目标为非危险目标,将其排除;其中,ds为安全距离阈值,ds=L/2+ks,di为i采样点下测得的目标与Z0轴之间的距离,L为农机上悬挂的犁具宽度,ks为设定的安全余量;
步骤3.4:对初选的有效目标进行有效性检验,最终确定有效目标;
步骤3.5:根据确定好的有效目标,通过毫米波雷达获取的最近距离障碍物确定为候选的最危险目标,若dj≤dmin,dj为毫米波雷达获取的农机与ID是j的有效目标之间的距离,dmin为在毫米波雷达一个扫描周期内所获取的农机与最近有效目标的距离,则ID是j的有效目标为最危险目标;
此设计中,先对干扰和噪声信号产生的随机噪声信号进行滤除,提高雷达数据解算的准确性;通过对农机行驶车道的判别,排除农机行驶车道外的障碍物目标,初步选定同车道内的障碍物为有效目标,对初选有效目标进行检验以进一步确定有效目标,提高有效目标识别的精确度;对有效目标按照距离由近到远的顺序为规则,确定最危险目标。
为了进一步提高有效目标确定的精确度,所述步骤3.4中对初选的有效目标进行有效性检验具体包括以下步骤,
步骤3.4.1:对初选的有效目标进行预测,选取状态Sn=[dn,vn,an],初选有效目标的状态预测方程为,
其中,d(n+1,n)、v(n+1,n)、a(n+1,n)是上一个扫描周期预测的有效障碍物目标的状态信息,dn,vn,an分别表示毫米波雷达第n探测周期内测得的有效障碍物目标的相对距离、相对速度、相对加速度,t是毫米波雷达的扫描周期;
步骤3.4.2:通过比较预测的第n+1周期有效目标的状态信息和雷达实际测得的第n+1周期有效目标的状态信息,具体如下,
其中,d0、v0、a0是设定的有效障碍物目标测量值与预测值之间的误差阈值;
步骤3.4.3:有效障碍物目标在雷达的扫描周期中被连续探测到m次以上,同时,满足步骤3.4.2中公式(3-3)的有效目标与初选有效目标一致,则更新目标的相对距离、相对速度、相对角度、编号信息;否则,初选的有效目标不处于毫米波雷达探测目标中,使用有效目标预测信息对初选的有效目标进行跟踪,若初选的有效目标在雷达的下一个扫描周期中仍然没有被探测到,则停止使用对应的初选有效目标信息,更新有效目标信息,并返回步骤3.4.1循环执行;
此设计中,通过上一个扫描预测的有效目标的状态信息与测试出来的进行对比,判断有效目标信息是否一致,以此来进一步排除虚假目标,使有效目标的确定得到进一步保障。
作为本发明的进一步改进,所述步骤4中判断最危险目标的动静状态具体包括以下步骤,
步骤4.1:根据步骤2.5中确定的最危险目标,不断更新最危险目标的相对速度和相对距离信息,判断最危险目标与雷达的距离是否在停车距离范围内,即zd>zmin(4-1),zd为毫米波雷达探测到的雷达与最危险目标的相对距离,zmin为设定的停车距离阈值,最危险目标满足公式(4-1)时,农机继续行驶;
步骤4.2:根据相对速度大小判定最危险目标的动静状态,具体如下,
v≠v车 (4-2)
在一个扫描周期内,(4-2)式始终成立时,判定目标的状态为动态,此时,导航箱发出声光报警并在zd=zmin时做停车等待处理;否则,农机继续行驶,并返回至步骤3.1循环执行其中,v为雷达相对目标的速度大小,v车为农机的行驶速度;
此设计中,判定最危险目标的动静态原理简单,提高响应速度。
作为本发明的进一步改进,所述步骤4中根据雷达与相机采集到的最危险目标的图像数据,判断出最危险目标的类型具体包括以下步骤,
步骤4.1a:若最危险目标始终为静态,则导航箱控制农机做停车等待处理;否则,摄像机对最危险目标进行识别;
步骤4.2a:摄像机获取最危险目标的图像,将图像与训练好的人体样本训练库进行匹配比较,输出目标识别结果;
步骤4.3a:导航箱根据输出的目标识别结果控制农机动作,若为非人体,则导航箱发出声光报警,并控制农机做停车等待处理;若目标识别结果为人体,则导航箱发出声光报警,判断人体是否偏离农机行驶车道或者人体向远离农机方向运动,用以下公式判断,
zwn+1>zwn (4-3)
di>ds (4-4)
若雷达探测到的人体目标满足(4-3)或(4-4),则农机继续向前行驶,否则导航箱控制农机做停车等待处理;zwn为第n探测扫描周期雷达相对最危险目标的距离,zw(n+1)为下一个扫描周期雷达相对最危险目标的距离;
此设计中,先判断最危险目标的动静状态,若最危险目标始终为静态,则认为最危险目标为电线杆、树木等非生命体,否则,认为最危险目标为农作人员或者牲畜,通过摄像机采集最危险目标的图像并识别出最危险目标是否为人体,输出目标识别结果,若为人体,则导航箱发出声光报警,因为劳作人员自身有避险意识,劳作人员在听到农机的报警声后可能会走出农机行驶车道或者往远离农机运动方向行走,利用劳作人员的习惯性反应设置判断程序,适应性好,避免农机与电线杆、牲畜等非人体的碰撞的同时提醒农机前方的农作人员自觉避让,并根据农作人员的行为做继续行驶或停车等待处理。
本发明与现有技术相比,本发明将毫米波雷达和相机结合起来感知农田环境,实时检测雷达和摄像机距离地面的高度变化,将高度变化量加入到雷达和摄像机坐标转换的平移向量中,农机作业时,使摄像机和雷达在空间上实现真正的同步,提高摄像机和雷达的融合精度;对噪声和干扰信号产生的随机噪声信号进行滤除,提高雷达探测信号的准确性;根据设定好的农机航向确定为农机行驶车道,将农机行驶车道内的障碍物目标初选为有效目标,再对初选的有效目标进行进一步检验,以进一步确定有效目标,提高雷达感知同车道内障碍物目标的有效性和准确性;选取最危险目标并跟踪最危险目标,摄像机以最危险目标的动静态为基础进行目标识别,若最危险目标为动态,只需要识别出动态物是否为人体即可,不需要识别出具体类型,减小运算量,提高响应速度,导航箱根据图像识别结果控制农机动作,避免农机在无人驾驶时与障碍物碰撞;若识别结果为人体时,导航箱声光报警提醒劳作人员避开农机,利用人的习惯思维这一特性,不断检测人体是否偏离农机行驶车道或者人体向远离农机方向运动,导航箱根据检测结果控制农机是否做停车等待处理,适应性好。
附图说明
图1为本发明基于毫米波雷达和摄像机的感知农田环境方法的流程图。
图2为本发明中摄像机坐标系和车辆坐标系的关系示意图。
图3为本发明中摄像机坐标系与图像物理坐标系的关系示意图。
图4为本发明中图像物理坐标系与图像像素坐标系的关系示意图。
图5为本发明中农机行驶过程中农田环境示意图。
图6为本发明中农机行驶过程中车道判别示意图。
图7为本发明中对初选有效目标进行检验以进一步确定有效目标的流程图。
图8为高度检测装置一的结构示意图。
其中,1丝杆,2轴承一,3轴承二,4壳体,5距离传感器,6容纳腔,7限位板,8挡片,9凹槽,10限位阶二,11限位阶一,12螺母,13挡板,14万向滚轮。
具体实施方式
下面结合附图对本发明作进一步的说明。
如图1~8所示的一种用于农机无人驾驶的多传感器融合的农田环境感知方法,具体包括以下步骤:
步骤1:农机作业前,对摄像机进行标定,摄像机空间坐标变换,再对雷达视觉联合标定,使得雷达和视觉信息在空间上融合;
步骤2:农机作业时,实时调整雷达与摄像机坐标的转换关系,使雷达与摄像机在作业条件下实现在空间上的同步;
步骤3:工控机解算接收到的毫米波雷达数据,确定有效目标,选出农机作业前方雷达感兴趣的区域,确定最危险目标,同步进行摄像机图像的采集;
步骤4:根据雷达的信息判断出最危险目标的运动状态,根据雷达与相机采集到的最危险目标的图像数据,判断出最危险目标的类型,工控机将动作指令传输给导航箱,导航箱控制农机做相应的动作;
其中,农机作业时,农机的行驶速度匀速;
为了实现摄像机和毫米波雷达在空间上的同步,如图2~4所示,步骤1中的将雷达坐标转换成图像像素坐标具体包括以下步骤,
步骤1.1:农机作业前,地面默认为水平,将毫米波雷达固定安装在农机的前侧且位于农机纵向中轴,雷达发射面向外,使雷达发射面与地面垂直;安装摄像机时使摄像机的光轴与地面平行;
步骤1.2:以雷达的中心为原点建立雷达坐标系00-X0Y0Z0,毫米波雷达所在平面由X0轴和Y0轴确定并与Z0轴相垂直,Z0轴与地面平行且与农机中心轴线重合;建立摄像机坐标系Oc-XcYcZc,以摄像机的中心为原点Oc,平面XcOcYc平行于摄像机的成像平面,Zc轴是摄像机的取景光轴且垂直于成像平面;建立车辆坐标系Ow-XwYwZw,Ow为农机的农机后轴的中心与车辆的中心轴线的交点,Xw轴水平向右并垂直于农机的纵向中轴线,Zw 水平向前且与农机中心轴线重合,Yw轴垂直于水面地面向上,雷达坐标系的X0O0Z0平面与车辆坐标系的XwOwZw平面平行;
步骤1.3:光轴与成像平面相交的点是图像主点O’,车辆坐标通过旋转矩阵R和平移向量sc转换后得到摄像机坐标(xc,yc,zc,1)T,任意点P的车辆坐标为(xw,yw,zw,1)T,将车辆坐标转换为摄像机坐标,具体的转换关系如下,
式(1-2)中,R为一个三行三列的正交单位矩阵,sc为初始条件下车辆坐标系到摄像机坐标系的1*3平移矩阵,xc0为摄像机所在的中心轴与车辆中心轴线两直线的距离,yc0为摄像机距离农机后轴的距离,zc0为初始条件下摄像机距离地面的高度;
步骤1.4:将摄像机坐标(xc,yc,zc,1)T转换到图像物理坐标(x1,y1)T,具体的转换关系如下,
式(1-3)中,f为摄像机的焦距,焦距单位为mm;
步骤1.5:将图像物理坐标(x1,y1)T转换到图像像素坐标(u,v),具体的转换关系如下,
其中,dx,dy分别表示每个像素在横轴和纵轴上单位大小,u0、v0分别为图像像素坐标系下摄像机光轴和成像平面交点的横纵坐标,坐标单位为pixel;
步骤1.6:根据以上公式(1-1)~(1-4)得到图像像素坐标系到车辆坐标系的转换公式,具体的为,
步骤1.7:为了使雷达和视觉信息在空间上进一步融合,雷达坐标与摄像机的坐标转换关系更新为,
其中,s=sc+s0,s0的坐标设为(xs0,ys0,zs0),xs0=0,ys0为初始条件下雷达距离地面的高度,zs0为雷达与农机后轴的距离;
将雷达坐标借助共用的车辆坐标系转化为图像坐标,雷达数据经三维坐标逆变换,完成目标信息匹配至视觉信息,雷达与摄像头空间的相对位置借助车辆坐标系求取即可;步骤2中的实时调整图像像素坐标与车辆坐标的转换关系,具体的为,根据农机的实际路况实时调整平移向量s,扫描周期t下实时调整后的平移向量st=sc+s0+△st,实时的车辆坐标与图像像素坐标的转换关系,具体的为,
其中,△hct为扫描周期t下摄像机与地面高度的变化值,△hst为扫描周期t下雷达与地面高度的变化值,j为扫描周期数,(ut,vt)为农机作业过程中扫描周期t下实时更新计算得到的图像像素坐标;
所述步骤2中实时调整后的平移向量中获得△ht的步骤如下,
步骤2.1:农机作业前,在雷达正下方安装有用来检测雷达与地面间的高度变化的高度检测装置一;在摄像机正下方的农机下侧安装有用来检测摄像机与地面的高度变化量的高度检测装置二;
步骤2.2:实时计算雷达与地面高度变化量以及摄像机与地面的高度变化量,具体的为,
扫描周期t中i时刻与i-1时刻雷达与地面高度距离变化值为△hsti,采用平均法计算扫描周期t中雷达相对地面的高度变化量△hst,
假设扫描周期t中采样时刻i与采样时刻i-1下摄像机与地面高度变化值为△hcti,采用平均法计算扫描周期t中摄像机与地面的高度变化量△hct,
步骤2.3:实时计算出扫描周期t下实时调整后的平移向量s,具体的为,
其中,k为一个扫描周期中的采样点的总数;
为了实现雷达距离地面的高度变化和摄像机距离地面的高度变化的检测,高度检测装置一和高度检测装置二的结构相同;如图8所示,高度检测装置一包括所述高度检测装置一包括可开合且具有容纳腔6的壳体4,壳体4的底部连接有挡板13,壳体4上在高度方向上分别间隔连接有轴承一2和轴承二3,轴承一2和轴承二3内均连接有螺母12,螺母12上螺纹连接有丝杆1,螺纹升角为35°~40°,丝杆1的顶部可拆卸地安装有限位板7,限位板7限制丝杆1在容纳腔6内运动,壳体4的顶部可拆卸地安装有正对限位板7的距离传感器5;壳体4的底部可拆卸地连接有挡板13,轴承一2的上侧为壳体4的限位阶一11,挡板13和限位阶一11使得轴承一2无法在高度方向上移动;壳体4上开有凹槽9,轴承上侧设有具有弹性的挡片8,挡片8安装在凹槽9内,轴承二3的下侧为壳体4的限位阶二10,挡片8和限位阶二10使得另一个轴承无法在高度方向上移动;丝杆1的底部安装有可在地面上滚动的万向滚轮14;农田中的地面是凹凸不平的,雷达坐标和摄像机坐标进行空间融合时,平移向量中△ht是实时改变的,导致雷达和摄像机无法实现空间同步,当雷达下方或摄像机下方的地面凸出时,万向滚轮14沿着凸出的地面滚动,凸出的地面给丝杆1向上的作用力,丝杆1向上做直线运动,丝杆1通过螺母12带动轴承的内圈旋转,限位板7上升,距离传感器5检测到距离变化量;当地面向下凹时,由于螺母12与丝杆1之间的连接为不自锁连接,因此,丝杆1在自身重力作用下下降,直到万向滚轮14与凹进去的地面接触,限位板7在不断下降,距离传感器5检测到距离变化量,从而实现实时检测雷达距离地面的高度变化和摄像机距离地面的高度变化;高度检测装置一和高度检测装置二的结构设计巧妙,能很好地感应凹凸不平的地面;
步骤3中的解算雷达数据确定有效目标,具体包括以下步骤,
步骤3.1:对雷达接收到的数据按照毫米波雷达协议进行解算,得到前方物体相对雷达的角度α、距离r、相对速度v、前方物体的反射强度并为每个目标分配唯一一个ID;
步骤3.2:对随机噪声信号进行滤波,保证雷达数据的连续有效性,具体的为,定义z=[r,α,v]T为雷达的测量值,z(k)为毫米波雷达第k次输出的测量值,
d2=S(z(k)-z(k-1))(z(k)-z(k-1))T<rs 2 (3-1)
过滤掉不符合式(3-1)的数据信号;其中,d为相邻量测向量z(k)、z(k-1)之间的加权欧氏距离,S为加权矩阵,rs为设定的阈值;
步骤3.3:判定目标是否在农机行驶的车道内,当雷达前方物体满足di≤ds时,目标在农机行驶车道内,否则,目标不在农机行驶车道内,农机行驶车道内的目标初选为有效目标,并对其按照由近及远的准则进行排序编号;在农机行驶车道外的目标为非危险目标,将其排除;其中,ds为安全距离阈值,ds=L/2+ks,di为i采样点下测得的目标与Z0轴之间的距离,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=[dn,vn,an],初选有效目标的状态预测方程为,
其中,d(n+1,n)、v(n+1,n)、a(n+1,n)是上一个扫描周期预测的有效障碍物目标的状态信息,dn,vn,an分别表示毫米波雷达第n探测周期内测得的有效障碍物目标的相对距离、相对速度、相对加速度,t是毫米波雷达的扫描周期;
步骤3.4.2:通过比较预测的第n+1周期有效目标的状态信息和雷达实际测得的第n+1周期有效目标的状态信息,具体如下,
其中,d0、v0、a0是设定的有效障碍物目标测量值与预测值之间的误差阈值;
步骤3.4.3:有效障碍物目标在雷达的扫描周期中被连续探测到m次以上,同时,满足步骤3.4.2中公式(3-3)的有效目标与初选有效目标一致,则更新目标的相对距离、相对速度、相对角度、编号信息;否则,初选的有效目标不处于毫米波雷达探测目标中,使用有效目标预测信息对初选的有效目标进行跟踪,若初选的有效目标在雷达的下一个扫描周期中仍然没有被探测到,则停止使用对应的初选有效目标信息,更新有效目标信息,并返回步骤3.4.1循环执行;
步骤4中判断最危险目标的动静状态具体包括以下步骤,
步骤4.1:根据步骤2.5中确定的最危险目标,不断更新最危险目标的相对速度和相对距离信息,判断最危险目标与雷达的距离是否在停车距离范围内,即zd>zmin(4-1),zd为毫米波雷达探测到的雷达与最危险目标的相对距离,zmin为设定的停车距离阈值,最危险目标满足公式(4-1)时,农机继续行驶;
步骤4.2:根据相对速度大小判定最危险目标的动静状态,具体如下,
v≠v车 (4-2)
在一个扫描周期内,(4-2)式始终成立时,判定目标的状态为动态,此时,导航箱发出声光报警并在zd=zmin时做停车等待处理;否则,农机继续行驶,并返回至步骤3.1循环执行其中,v为雷达相对目标的速度大小,v车为农机的行驶速度;
步骤4中根据雷达与相机采集到的最危险目标的图像数据,判断出最危险目标的类型具体包括以下步骤,
步骤4.1a:若最危险目标始终为静态,则导航箱控制农机做停车等待处理;否则,摄像机对最危险目标进行识别;
步骤4.2a:摄像机获取最危险目标的图像,将图像与训练好的人体样本训练库进行匹配比较,输出目标识别结果;
步骤4.3a:导航箱根据输出的目标识别结果控制农机动作,若为非人体,则导航箱发出声光报警,并控制农机做停车等待处理;若目标识别结果为人体,则导航箱发出声光报警,判断人体是否偏离农机行驶车道或者人体向远离农机方向运动,用以下公式判断,
zwn+1>zwn (4-3)
di>ds (4-4)
若雷达探测到的人体目标满足(4-3)或(4-4),则农机继续向前行驶,否则导航箱控制农机做停车等待处理;zwn为第n探测扫描周期雷达相对最危险目标的距离,zw(n+1)为下一个扫描周期雷达相对最危险目标的距离;
本发明与现有技术相比,本发明将毫米波雷达和相机结合起来感知农田环境,实时检测雷达和摄像机距离地面的高度变化,将高度变化量加入到雷达和摄像机坐标转换的平移向量中,提高摄像机和雷达的融合精度;高度检测装置一的结构巧妙,能很好地感应地面高度的变化,减小因高度变化带来的平移向量的误差,使摄像机和雷达在空间上实现真正的同步;对噪声和干扰信号产生的随机噪声信号进行滤除,提高雷达探测信号的准确性;根据设定好的农机航向确定为农机行驶车道,将农机行驶车道内的障碍物目标初选为有效目标,再对初选的有效目标进行进一步检验,以进一步确定有效目标,提高雷达感知同车道内障碍物目标的有效性和准确性;选取最危险目标并跟踪最危险目标,摄像机以最危险目标的动静态为基础进行目标识别,若最危险目标为动态,只需要识别出动态物是否为人体即可,不需要识别出具体类型,减小运算量,提高响应速度,导航箱根据图像识别结果控制农机动作,避免农机在无人驾驶时与障碍物碰撞;若识别结果为人体时,导航箱声光报警提醒劳作人员避开农机,利用人的习惯思维这一特性,不断检测人体是否偏离农机行驶车道或者人体向远离农机方向运动,导航箱根据检测结果控制农机是否做停车等待处理,适应性好。
本发明并不局限于上述实施例,在本发明公开的技术方案的基础上,本领域的技术人员根据所公开的技术内容,不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形,这些替换和变形均在本发明保护范围内。
Claims (9)
1.一种用于农机无人驾驶的多传感器融合的农田环境感知方法,其特征在于,具体包括以下步骤,
步骤1:农机作业前,对摄像机进行标定,摄像机空间坐标变换,再对雷达视觉联合标定,使得雷达和视觉信息在空间上融合;
步骤2:农机作业时,实时调整雷达与摄像机坐标的转换关系,使雷达与摄像机在作业条件下实现在空间上的同步;
步骤3:工控机解算接收到的毫米波雷达数据,确定有效目标,选出农机作业前方雷达感兴趣的区域,确定最危险目标,同步进行摄像机图像的采集;
步骤4:根据雷达的信息判断出最危险目标的运动状态,根据雷达与相机采集到的最危险目标的图像数据,判断出最危险目标的类型,工控机将动作指令传输给导航箱,导航箱控制农机做相应的动作;
其中,农机作业时,农机的行驶速度匀速。
2.根据权利要求1所述的用于农机无人驾驶的多传感器融合的农田环境感知方法,其特征在于,所述步骤1中的将雷达坐标转换成图像像素坐标具体包括以下步骤,
步骤1.1:农机作业前,地面默认为水平,将毫米波雷达固定安装在农机的前侧且位于农机纵向中轴,雷达发射面向外,使雷达发射面与地面垂直;安装摄像机时使摄像机的光轴与地面平行;
步骤1.2:以雷达的中心为原点建立雷达坐标系00-X0Y0Z0,毫米波雷达所在平面由X0轴和Y0轴确定并与Z0轴相垂直,Z0轴与地面平行且与农机中心轴线重合;建立摄像机坐标系Oc-XcYcZc,以摄像机的中心为原点Oc,平面XcOcYc平行于摄像机的成像平面,Zc轴是摄像机的取景光轴且垂直于成像平面;建立车辆坐标系Ow-XwYwZw,Ow为农机的农机后轴的中心与车辆的中心轴线的交点,Xw轴水平向右并垂直于农机的纵向中轴线,Zw 水平向前且与农机中心轴线重合,Yw轴垂直于水面地面向上,雷达坐标系的X0O0Z0平面与车辆坐标系的XwOwZw平面平行;
步骤1.3:光轴与成像平面相交的点是图像主点O’,车辆坐标通过旋转矩阵R和平移向量sc转换后得到摄像机坐标(xc,yc,zc,1)T,任意点P的车辆坐标为(xw,yw,zw,1)T,将车辆坐标转换为摄像机坐标,具体的转换关系如下,
式(1-2)中,R为一个三行三列的正交单位矩阵,sc为初始条件下车辆坐标系到摄像机坐标系的1*3平移矩阵,xc0为摄像机所在的中心轴与车辆中心轴线两直线的距离,yc0为初始条件下摄像机距离地面的高度,zc0为摄像机距离农机后轴的距离;
步骤1.4:将摄像机坐标(xc,yc,zc,1)T转换到图像物理坐标(x1,y1)T,具体的转换关系如下,
式(1-3)中,f为摄像机的焦距,焦距单位为mm;
步骤1.5:将图像物理坐标(x1,y1)T转换到图像像素坐标(u,v),具体的转换关系如下:
其中,dx,dy分别表示每个像素在横轴和纵轴上单位大小,u0、v0分别为图像像素坐标系下摄像机光轴和成像平面交点的横纵坐标,坐标单位为pixel;
步骤1.6:根据以上公式(1-1)~(1-4)得到图像像素坐标系到车辆坐标系的转换公式,具体的为,
步骤1.7:为了使雷达和视觉信息在空间上融合,将步骤1.6中的坐标转换关系更新为,
其中,s=sc+s0,s0的坐标设为(xs0,ys0,zs0),xs0=0,ys0为初始条件下雷达距离地面的高度,zs0为雷达与农机后轴的距离。
3.根据权利要求2所述的用于农机无人驾驶的多传感器融合的农田环境感知方法,其特征在于:所述步骤2中的实时调整图像像素坐标与车辆坐标的转换关系,具体的为,根据农机的实际路况实时调整平移向量s,扫描周期t下实时调整后的平移向量st=sc+s0+△st,实时的车辆坐标与图像像素坐标的转换关系,具体的为,
其中,△hct为扫描周期t下摄像机与地面高度的变化值,△hst为扫描周期t下雷达与地面高度的变化值,j为扫描周期数,(ut,vt)为农机作业过程中扫描周期t下实时更新计算得到的图像像素坐标。
4.根据权利要求3所述的用于农机无人驾驶的多传感器融合的农田环境感知方法,其特征在于:
所述步骤2中实时调整后的平移向量中获得△ht的步骤如下,
步骤2.1:农机作业前,在雷达正下方安装有用来检测雷达与地面间的高度变化的高度检测装置一;在摄像机正下方的农机下侧安装有用来检测摄像机与地面的高度变化量的高度检测装置二;
步骤2.2:实时计算雷达与地面高度变化量以及摄像机与地面的高度变化量,具体的为,
扫描周期t中i时刻与i-1时刻雷达与地面高度距离变化值为△hsti,采用平均法计算扫描周期t中雷达相对地面的高度变化量△hst,
假设扫描周期t中采样时刻i与采样时刻i-1下摄像机与地面高度变化值为△hcti,采用平均法计算扫描周期t中摄像机与地面的高度变化量△hct,
步骤2.3:实时计算出扫描周期t下实时调整后的平移向量s,具体的为,
其中,k为一个扫描周期中的采样点的总数。
5.根据权利要求4所述的用于农机无人驾驶的多传感器融合的农田环境感知方法,所述高度检测装置一和高度检测装置二的结构相同,所述高度检测装置一包括可开合且具有容纳腔的壳体,壳体的底部连接有挡板,壳体上在高度方向上分别间隔连接有轴承一和轴承二,轴承一和轴承二内均连接有螺母,螺母上螺纹连接有丝杆,螺纹升角为35°~40°,所述丝杆的顶部可拆卸地安装有限位板,限位板限制丝杆在容纳腔内运动,所述壳体的顶部可拆卸地安装有正对限位板的距离传感器;所述壳体的底部可拆卸地连接有挡板,所述轴承一的上侧为壳体的限位阶一,所述挡板和限位阶一使得轴承一无法在高度方向上移动;所述壳体上开有凹槽,轴承上侧设有具有弹性的挡片,所述挡片安装在凹槽内,所述轴承二的下侧为壳体的限位阶二,所述挡片和限位阶二使得另一个轴承无法在高度方向上移动;所述丝杆的底部安装有可在地面上滚动的万向滚轮。
6.根据权利要求1~5任一项所述的用于农机无人驾驶的多传感器融合的农田环境感知方法,其特征在于,所述步骤3中的解算雷达数据确定有效目标,具体包括以下步骤,
步骤3.1:对雷达接收到的数据按照毫米波雷达协议进行解算,得到前方物体相对雷达的角度α、距离r、相对速度v、前方物体的反射强度并为每个目标分配唯一一个ID;
步骤3.2:对随机噪声信号进行滤波,保证雷达数据的连续有效性,具体的为,定义z=[r,α,v]T为雷达的测量值,z(k)为毫米波雷达第k次输出的测量值,
d2=S(z(k)-z(k-1))(z(k)-z(k-1))T<rs 2 (3-1)
过滤掉不符合式(3-1)的数据信号;其中,d为相邻量测向量z(k)、z(k-1)之间的加权欧氏距离,S为加权矩阵,rs为设定的阈值;
步骤3.3:判定目标是否在农机行驶的车道内,当雷达前方物体满足di≤ds时,目标在农机行驶车道内,否则,目标不在农机行驶车道内,农机行驶车道内的目标初选为有效目标,并对其按照由近及远的准则进行排序编号;在农机行驶车道外的目标为非危险目标,将其排除;其中,ds为安全距离阈值,ds=L/2+ks,di为i采样点下测得的目标与Z0轴之间的距离,L为农机上悬挂的犁具宽度,ks为设定的安全余量;
步骤3.4:对初选的有效目标进行有效性检验,最终确定有效目标;
步骤3.5:根据确定好的有效目标,通过毫米波雷达获取的最近距离障碍物确定为候选的最危险目标,若dj≤dmin,dj为毫米波雷达获取的农机与ID是j的有效目标之间的距离,dmin为在毫米波雷达一个扫描周期内所获取的农机与最近有效目标的距离,则ID是j的有效目标为最危险目标。
7.根据权利要求6所述的用于农机无人驾驶的多传感器融合的农田环境感知方法,其特征在于,所述步骤3.4中对初选的有效目标进行有效性检验具体包括以下步骤,
步骤3.4.1:对初选的有效目标进行预测,选取状态Sn=[dn,vn,an],初选有效目标的状态预测方程为,
其中,d(n+1,n)、v(n+1,n)、a(n+1,n)是上一个扫描周期预测的有效障碍物目标的状态信息,dn,vn,an分别表示毫米波雷达第n探测周期内测得的有效障碍物目标的相对距离、相对速度、相对加速度,t是毫米波雷达的扫描周期;
步骤3.4.2:通过比较预测的第n+1周期有效目标的状态信息和雷达实际测得的第n+1周期有效目标的状态信息,具体如下,
其中,d0、v0、a0是设定的有效障碍物目标测量值与预测值之间的误差阈值;
步骤3.4.3:有效障碍物目标在雷达的扫描周期中被连续探测到m次以上,同时,满足步骤3.4.2中公式(3-3)的有效目标与初选有效目标一致,则更新目标的相对距离、相对速度、相对角度、编号信息;否则,初选的有效目标不处于毫米波雷达探测目标中,使用有效目标预测信息对初选的有效目标进行跟踪,若初选的有效目标在雷达的下一个扫描周期中仍然没有被探测到,则停止使用对应的初选有效目标信息,更新有效目标信息,并返回步骤3.4.1循环执行。
8.根据权利要求7所述的用于农机无人驾驶的多传感器融合的农田环境感知方法,其特征在于,所述步骤4中判断最危险目标的动静状态具体包括以下步骤,
步骤4.1:根据步骤3.5中确定的最危险目标,不断更新最危险目标的相对速度和相对距离信息,判断最危险目标与雷达的距离是否在停车距离范围内,即zd>zmin(4-1),zd为毫米波雷达探测到的雷达与最危险目标的相对距离,zmin为设定的停车距离阈值,最危险目标满足公式(4-1)时,农机继续行驶;
步骤4.2:根据相对速度大小判定最危险目标的动静状态,具体如下,
v≠v车 (4-2)
在一个扫描周期内,(4-2)式始终成立时,判定目标的状态为动态,此时,导航箱发出声光报警并在zd=zmin时做停车等待处理;否则,农机继续行驶,并返回至步骤3.1循环执行其中,v为雷达相对目标的速度大小,v车为农机的行驶速度。
9.根据权利要求8所述的用于农机无人驾驶的多传感器融合的农田环境感知方法,其特征在于,所述步骤4中根据雷达与相机采集到的最危险目标的图像数据,判断出最危险目标的类型具体包括以下步骤,
步骤4.1a:若最危险目标始终为静态,则导航箱控制农机做停车等待处理;否则,摄像机对最危险目标进行识别;
步骤4.2a:摄像机获取最危险目标的图像,将图像与训练好的人体样本训练库进行匹配比较,输出目标识别结果;
步骤4.3a:导航箱根据输出的目标识别结果控制农机动作,若为非人体,则导航箱发出声光报警,并控制农机做停车等待处理;若目标识别结果为人体,则导航箱发出声光报警,判断人体是否偏离农机行驶车道或者人体向远离农机方向运动,用以下公式判断,
zwn+1>zwn (4-3)
di>ds (4-4)
若雷达探测到的人体目标满足(4-3)或(4-4),则农机继续向前行驶,否则导航箱控制农机做停车等待处理;zwn为第n探测扫描周期雷达相对最危险目标的距离,zw(n+1)为下一个扫描周期雷达相对最危险目标的距离。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710141692.5A CN106874886A (zh) | 2017-03-10 | 2017-03-10 | 用于农机无人驾驶的多传感器融合的农田环境感知方法 |
CN201711268643.4A CN108154084B (zh) | 2017-03-10 | 2017-12-05 | 用于农机无人驾驶的多传感器融合的农田环境感知方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710141692.5A CN106874886A (zh) | 2017-03-10 | 2017-03-10 | 用于农机无人驾驶的多传感器融合的农田环境感知方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN106874886A true CN106874886A (zh) | 2017-06-20 |
Family
ID=59170104
Family Applications (2)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710141692.5A Pending CN106874886A (zh) | 2017-03-10 | 2017-03-10 | 用于农机无人驾驶的多传感器融合的农田环境感知方法 |
CN201711268643.4A Active CN108154084B (zh) | 2017-03-10 | 2017-12-05 | 用于农机无人驾驶的多传感器融合的农田环境感知方法 |
Family Applications After (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711268643.4A Active CN108154084B (zh) | 2017-03-10 | 2017-12-05 | 用于农机无人驾驶的多传感器融合的农田环境感知方法 |
Country Status (1)
Country | Link |
---|---|
CN (2) | CN106874886A (zh) |
Cited By (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107862287A (zh) * | 2017-11-08 | 2018-03-30 | 吉林大学 | 一种前方小区域物体识别及车辆预警方法 |
CN107918350A (zh) * | 2017-11-30 | 2018-04-17 | 陕西华恒水务集团有限公司 | 一种农用机械安全监控方法 |
CN107966700A (zh) * | 2017-11-20 | 2018-04-27 | 天津大学 | 一种用于无人驾驶汽车的前方障碍物检测***及方法 |
CN108407797A (zh) * | 2018-01-19 | 2018-08-17 | 洛阳中科龙网创新科技有限公司 | 一种基于深度学习的实现农机自动换挡的方法 |
CN108594244A (zh) * | 2018-04-28 | 2018-09-28 | 吉林大学 | 基于立体视觉和激光雷达的障碍物识别迁移学习方法 |
CN109471096A (zh) * | 2018-10-31 | 2019-03-15 | 奇瑞汽车股份有限公司 | 多传感器目标匹配方法、装置及汽车 |
CN109508007A (zh) * | 2018-12-11 | 2019-03-22 | 东南大学 | 一种基于多源信息融合的农机轨迹跟踪、避障***及方法 |
CN109532826A (zh) * | 2017-09-21 | 2019-03-29 | 天津所托瑞安汽车科技有限公司 | 一种基于车道线视觉识别技术优化的雷达防碰撞预警方法 |
CN109733284A (zh) * | 2019-02-19 | 2019-05-10 | 广州小鹏汽车科技有限公司 | 一种应用于车辆的安全泊车辅助预警方法及*** |
CN109747530A (zh) * | 2017-11-02 | 2019-05-14 | 郭宇铮 | 一种双摄像头和毫米波融合汽车感知*** |
CN110618424A (zh) * | 2019-09-27 | 2019-12-27 | 中科九度(北京)空间信息技术有限责任公司 | 一种基于多传感器融合的远距离高压线发现方法 |
CN111060904A (zh) * | 2019-12-25 | 2020-04-24 | 中国汽车技术研究中心有限公司 | 一种基于毫米波与视觉融合感知的盲区监测方法 |
CN111123260A (zh) * | 2018-11-01 | 2020-05-08 | 北京自动化控制设备研究所 | 一种毫米波雷达和可见光摄像的环境物体状态识别方法 |
CN111886638A (zh) * | 2018-03-28 | 2020-11-03 | 京瓷株式会社 | 图像处理装置、拍摄装置以及移动体 |
CN113460064A (zh) * | 2020-03-31 | 2021-10-01 | 北京新能源汽车股份有限公司 | 一种前方目标车辆的识别方法、装置及电动汽车 |
CN114312812A (zh) * | 2022-03-04 | 2022-04-12 | 国汽智控(北京)科技有限公司 | 基于动态感知的车辆控制方法、装置及电子设备 |
CN114911266A (zh) * | 2022-06-15 | 2022-08-16 | 南京邮电大学 | 一种无人机搜索航路建立方法及装置 |
CN116106895A (zh) * | 2023-02-21 | 2023-05-12 | 郑州大学 | 基于视觉与无线感知融合的多点微动智能监测***与方法 |
CN117672007A (zh) * | 2024-02-03 | 2024-03-08 | 福建省高速公路科技创新研究院有限公司 | 一种基于雷视融合的道路施工区域安全预警*** |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113093746B (zh) * | 2021-03-31 | 2024-01-23 | 上海三一重机股份有限公司 | 作业机械环境感知方法、装置、***及作业机械 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8994821B2 (en) * | 2011-02-24 | 2015-03-31 | Lockheed Martin Corporation | Methods and apparatus for automated assignment of geodetic coordinates to pixels of images of aerial video |
CN102508246B (zh) * | 2011-10-13 | 2013-04-17 | 吉林大学 | 车辆前方障碍物检测跟踪方法 |
CN104637059A (zh) * | 2015-02-09 | 2015-05-20 | 吉林大学 | 基于毫米波雷达和机器视觉的夜间前方车辆检测方法 |
WO2016204213A1 (ja) * | 2015-06-16 | 2016-12-22 | 株式会社デンソー | 車両制御装置、及び車両制御方法 |
CN104965202B (zh) * | 2015-06-18 | 2017-10-27 | 奇瑞汽车股份有限公司 | 障碍物探测方法和装置 |
CN105831056A (zh) * | 2016-04-25 | 2016-08-10 | 南通市广益机电有限责任公司 | 一种无人驾驶果园喷雾机雷达防碰撞及在线喷雾混药控制*** |
-
2017
- 2017-03-10 CN CN201710141692.5A patent/CN106874886A/zh active Pending
- 2017-12-05 CN CN201711268643.4A patent/CN108154084B/zh active Active
Cited By (28)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109532826A (zh) * | 2017-09-21 | 2019-03-29 | 天津所托瑞安汽车科技有限公司 | 一种基于车道线视觉识别技术优化的雷达防碰撞预警方法 |
CN109747530A (zh) * | 2017-11-02 | 2019-05-14 | 郭宇铮 | 一种双摄像头和毫米波融合汽车感知*** |
CN107862287A (zh) * | 2017-11-08 | 2018-03-30 | 吉林大学 | 一种前方小区域物体识别及车辆预警方法 |
CN107966700A (zh) * | 2017-11-20 | 2018-04-27 | 天津大学 | 一种用于无人驾驶汽车的前方障碍物检测***及方法 |
CN107918350A (zh) * | 2017-11-30 | 2018-04-17 | 陕西华恒水务集团有限公司 | 一种农用机械安全监控方法 |
CN108407797A (zh) * | 2018-01-19 | 2018-08-17 | 洛阳中科龙网创新科技有限公司 | 一种基于深度学习的实现农机自动换挡的方法 |
CN111886638B (zh) * | 2018-03-28 | 2023-01-03 | 京瓷株式会社 | 图像处理装置、拍摄装置以及移动体 |
CN111886638A (zh) * | 2018-03-28 | 2020-11-03 | 京瓷株式会社 | 图像处理装置、拍摄装置以及移动体 |
CN108594244B (zh) * | 2018-04-28 | 2021-05-11 | 吉林大学 | 基于立体视觉和激光雷达的障碍物识别迁移学习方法 |
CN108594244A (zh) * | 2018-04-28 | 2018-09-28 | 吉林大学 | 基于立体视觉和激光雷达的障碍物识别迁移学习方法 |
CN109471096B (zh) * | 2018-10-31 | 2023-06-27 | 奇瑞汽车股份有限公司 | 多传感器目标匹配方法、装置及汽车 |
CN109471096A (zh) * | 2018-10-31 | 2019-03-15 | 奇瑞汽车股份有限公司 | 多传感器目标匹配方法、装置及汽车 |
CN111123260A (zh) * | 2018-11-01 | 2020-05-08 | 北京自动化控制设备研究所 | 一种毫米波雷达和可见光摄像的环境物体状态识别方法 |
CN109508007A (zh) * | 2018-12-11 | 2019-03-22 | 东南大学 | 一种基于多源信息融合的农机轨迹跟踪、避障***及方法 |
CN109733284A (zh) * | 2019-02-19 | 2019-05-10 | 广州小鹏汽车科技有限公司 | 一种应用于车辆的安全泊车辅助预警方法及*** |
CN109733284B (zh) * | 2019-02-19 | 2021-10-08 | 广州小鹏汽车科技有限公司 | 一种应用于车辆的安全泊车辅助预警方法及*** |
CN110618424A (zh) * | 2019-09-27 | 2019-12-27 | 中科九度(北京)空间信息技术有限责任公司 | 一种基于多传感器融合的远距离高压线发现方法 |
CN111060904B (zh) * | 2019-12-25 | 2022-03-15 | 中国汽车技术研究中心有限公司 | 一种基于毫米波与视觉融合感知的盲区监测方法 |
CN111060904A (zh) * | 2019-12-25 | 2020-04-24 | 中国汽车技术研究中心有限公司 | 一种基于毫米波与视觉融合感知的盲区监测方法 |
CN113460064A (zh) * | 2020-03-31 | 2021-10-01 | 北京新能源汽车股份有限公司 | 一种前方目标车辆的识别方法、装置及电动汽车 |
CN113460064B (zh) * | 2020-03-31 | 2024-01-23 | 北京新能源汽车股份有限公司 | 一种前方目标车辆的识别方法、装置及电动汽车 |
CN114312812A (zh) * | 2022-03-04 | 2022-04-12 | 国汽智控(北京)科技有限公司 | 基于动态感知的车辆控制方法、装置及电子设备 |
CN114911266A (zh) * | 2022-06-15 | 2022-08-16 | 南京邮电大学 | 一种无人机搜索航路建立方法及装置 |
CN114911266B (zh) * | 2022-06-15 | 2023-05-09 | 南京邮电大学 | 一种无人机搜索航路建立方法及装置 |
CN116106895A (zh) * | 2023-02-21 | 2023-05-12 | 郑州大学 | 基于视觉与无线感知融合的多点微动智能监测***与方法 |
CN116106895B (zh) * | 2023-02-21 | 2024-01-26 | 郑州大学 | 基于视觉与无线感知融合的多点微动智能监测***与方法 |
CN117672007A (zh) * | 2024-02-03 | 2024-03-08 | 福建省高速公路科技创新研究院有限公司 | 一种基于雷视融合的道路施工区域安全预警*** |
CN117672007B (zh) * | 2024-02-03 | 2024-04-26 | 福建省高速公路科技创新研究院有限公司 | 一种基于雷视融合的道路施工区域安全预警*** |
Also Published As
Publication number | Publication date |
---|---|
CN108154084A (zh) | 2018-06-12 |
CN108154084B (zh) | 2020-06-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106874886A (zh) | 用于农机无人驾驶的多传感器融合的农田环境感知方法 | |
CN106909148A (zh) | 基于农田环境感知的农机无人驾驶导航方法 | |
CN106950952A (zh) | 用于农机无人驾驶的农田环境感知方法 | |
CN106874887A (zh) | 基于农田环境感知的农机导航控制方法 | |
CN107092039A (zh) | 农机导航用农田环境感知方法 | |
CN106891889A (zh) | 农机无人驾驶用农田环境感知方法 | |
CN110745140B (zh) | 一种基于连续图像约束位姿估计的车辆换道预警方法 | |
CN105518702B (zh) | 一种对目标物体的检测方法、检测装置以及机器人 | |
CN103413313B (zh) | 基于电力机器人的双目视觉导航***及方法 | |
CN107797118A (zh) | 基于相机的拖车识别和盲区调整 | |
JP5337905B2 (ja) | 速度計測システム、速度計測方法及びプログラム | |
CN105700525B (zh) | 基于Kinect传感器深度图机器人工作环境不确定性地图构建法 | |
CN113378741B (zh) | 一种基于多源传感器的飞机牵引车辅助感知方法及其*** | |
CN108256413A (zh) | 可通行区域检测方法及装置、存储介质、电子设备 | |
CN1940591A (zh) | 使用传感器融合进行目标跟踪的***和方法 | |
CN109325476B (zh) | 一种基于三维视觉的人体异常姿态检测***及方法 | |
CN113071518B (zh) | 一种自动无人驾驶方法、小巴、电子设备以及存储介质 | |
JP2009175932A (ja) | 移動ロボットの走行領域判別装置及び走行領域判別方法 | |
CN111077890A (zh) | 一种基于gps定位与自动避障的农业机器人的实现方法 | |
CN108873914A (zh) | 一种基于深度图像数据的机器人自主导航***及方法 | |
CN114299106A (zh) | 一种基于视觉传感与轨迹预测的高空抛物预警***及方法 | |
CN111830470A (zh) | 联合标定方法及装置、目标对象检测方法、***及装置 | |
CN110435689B (zh) | 基于模块化控制的智能障碍探测车的探测方法 | |
CN107767366A (zh) | 一种输电线路拟合方法及装置 | |
KR20200141871A (ko) | 차선 정보 획득 장치 및 방법 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
CB02 | Change of applicant information |
Address after: Zhe Lishui Economic Development Zone Zhetang town Nanjing city Jiangsu province 210000 Ning Road No. 368 Applicant after: NANJING WOYANG MACHINERY TECHNOLOGY CO., LTD. Address before: 210000 Jiangsu city in Yangzhou Province Economic Development Zone Lishui Zhetang town zhe Ning Road No. 368 Applicant before: NANJING WOYANG MACHINERY TECHNOLOGY CO., LTD. |
|
CB02 | Change of applicant information | ||
WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20170620 |
|
WD01 | Invention patent application deemed withdrawn after publication |