CN108089185A - 基于农田环境感知的农机无人驾驶导航方法 - Google Patents
基于农田环境感知的农机无人驾驶导航方法 Download PDFInfo
- Publication number
- CN108089185A CN108089185A CN201711268681.XA CN201711268681A CN108089185A CN 108089185 A CN108089185 A CN 108089185A CN 201711268681 A CN201711268681 A CN 201711268681A CN 108089185 A CN108089185 A CN 108089185A
- Authority
- CN
- China
- Prior art keywords
- radar
- target
- agricultural machine
- camera
- distance
- 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.)
- Granted
Links
- 238000001514 detection method Methods 0.000 claims abstract description 48
- 238000006243 chemical reaction Methods 0.000 claims abstract description 30
- 230000008859 change Effects 0.000 claims abstract description 27
- 230000000007 visual effect Effects 0.000 claims abstract description 11
- 230000033001 locomotion Effects 0.000 claims abstract description 10
- 230000000875 corresponding effect Effects 0.000 claims abstract description 8
- 238000000034 method Methods 0.000 claims description 40
- 238000012545 processing Methods 0.000 claims description 22
- 238000013519 translation Methods 0.000 claims description 22
- 239000013598 vector Substances 0.000 claims description 22
- 230000003068 static effect Effects 0.000 claims description 20
- 230000008447 perception Effects 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 15
- 238000003384 imaging method Methods 0.000 claims description 12
- 239000011159 matrix material Substances 0.000 claims description 12
- 230000003287 optical effect Effects 0.000 claims description 12
- 230000009471 action Effects 0.000 claims description 10
- 238000005259 measurement Methods 0.000 claims description 10
- 230000009466 transformation Effects 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 9
- 230000004888 barrier function Effects 0.000 claims description 8
- 238000012935 Averaging Methods 0.000 claims description 6
- 238000001914 filtration Methods 0.000 claims description 6
- 230000001276 controlling effect Effects 0.000 claims description 5
- 238000012549 training Methods 0.000 claims description 4
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000029777 axis specification Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000009432 framing Methods 0.000 claims description 3
- 238000012163 sequencing technique Methods 0.000 claims description 3
- 238000011897 real-time detection Methods 0.000 claims 1
- 238000005516 engineering process Methods 0.000 abstract description 13
- 230000001360 synchronised effect Effects 0.000 abstract description 6
- 230000004927 fusion Effects 0.000 abstract description 4
- 208000010877 cognitive disease Diseases 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 6
- 238000013461 design Methods 0.000 description 5
- 238000011161 development Methods 0.000 description 4
- 230000018109 developmental process Effects 0.000 description 4
- 244000144972 livestock Species 0.000 description 4
- 230000002650 habitual effect Effects 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 238000012271 agricultural production Methods 0.000 description 2
- 230000001174 ascending effect Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000003912 environmental pollution Methods 0.000 description 2
- 238000009313 farming Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000007726 management method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 125000004122 cyclic group Chemical group 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000003337 fertilizer Substances 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000002689 soil Substances 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Classifications
-
- 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- 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/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
- G01S13/867—Combination of radar systems with cameras
-
- 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/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
- G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- 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/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
- G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
- G01S2013/932—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles using own vehicle data, e.g. ground speed, steering wheel direction
-
- 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/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
- G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
- G01S2013/9322—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles using additional data, e.g. driver condition, road state or weather data
-
- 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/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
- G01S13/931—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of land vehicles
- G01S2013/9327—Sensor installation details
- G01S2013/93274—Sensor installation details on the side of the vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Traffic Control Systems (AREA)
- Closed-Circuit Television Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了农机控制技术领域内的一种基于农田环境感知的农机无人驾驶导航方法,具体包括以下步骤,步骤1:农机作业前,对摄像机进行标定,将雷达和视觉信息在空间上融合;步骤2:农机作业时,距离检测装置一检测雷达与地面间的高度变化,距离检测装置二检测摄像机与地面间的高度变化,实时调整雷达与摄像机坐标的转换,使雷达与摄像机在空间上同步;步骤3:工控机解算接收到的毫米波雷达数据,确定有效目标,确定最危险目标,同步采集摄像机图像;步骤4:根据雷达信息判断最危险目标状态,工控机根据运动状态规划行走路径,根据雷达与摄像机采集到的最危险目标的图像数据,判断最危险目标的类型,导航箱控制农机做相应的动作;本发明中数据融合精度高,提高识别障碍物的准确度。
Description
技术领域
本发明涉及一种无人驾驶中的环境感知方法,特别涉及一种农机无人驾驶中的农田环境感知方法。
背景技术
精准农业技术被认为是21世纪农业科技发展的前沿,是科技含最高、集成综合性最强的现代农业生产管理技术之一。精准农业技术根据空间变异,定位、定时、定量的实施一套现代化农事操作技术与管理的***,是信息技术与农业生产全面结合的一种新型农业技术。
精准农业的应用于快速发展,能够充分挖掘农田最大的生产潜力、合理利用水肥资源、减少环境污染,大幅度提高农产品产量和品质。
发展精准农业技术是解决我国农业由传统农业向现代化农业发展过程中所面临的确保农产品总量、调整农业产业结构、改善农产品品质和质量、资源严重不足且利用率低、环境污染等问题的有效解决方式,也是中国农业现代化发展与转型升级的必经之路。
卫星导航技术是精准农业技术的基本组成之一,使农机实现自动行驶,农机作业前只需要设定好参数后,导航***便引导农机进入自动作业模式,开始进行直线耕作。在农机自动导航的过程中,农田的环境恶劣且复杂,大型的农田里可能会存在电线杆、田埂、土丘、牲畜以及随时出现的劳作人员等,这些因素都为无人驾驶农机的实现提出了新的挑战。现有技术中,使用卫星导航技术可以实现农机在农田里自动行走,但是农机无法准确识别农机前方的障碍物,即农机无法感应农田环境,更不用说根据感知到的农田环境自动做停车等待还是继续行驶等处理,农机作业时必须得有驾驶员辅助操控农机的动作,一不注意,农机就会与前方障碍物碰撞;因此迫切需要研究出一套基于农田环境感知的导航控制方法使无人驾驶的农机具备对周边环境进行感知的能力,一旦遇到上述农田中存在电线杆、田埂、土丘、牲畜以及随时出现的劳作人员等情况,能够及时采取停车等待、避障等应急处理。
发明内容
针对现有技术中的缺陷,本发明的目的在于克服上述现有技术中的不足之处,解决现有技术中无人驾驶的农机无法感知农田环境的技术问题,提供一种用于农机无人驾驶的农田环境感知方法,本发明实现了农田环境的感知,感知精度高,识别农机前方障碍物的准确度高,提高农机无人驾驶时的可靠性。
本发明的目的是这样实现的:一种基于农田环境感知的农机无人驾驶导航方法,具体包括以下步骤,
步骤1:农机作业前,对摄像机进行标定,摄像机空间坐标变换,再对雷达视觉联合标定,使得雷达和视觉信息在空间上融合;
步骤2:农机作业时,距离检测装置一实时检测雷达与地面间的高度变化△hst,距离检测装置二实时检测摄像机与地面间的高度变化△hct,工控机进行数据处理实时调整雷达与摄像机坐标的转换关系,使雷达与摄像机在作业条件下实现在空间上的同步;
步骤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-1)中,R为一个三行三列的正交单位矩阵,sc(xc0,yc0,zc0)为初始条件下车辆坐标系到摄像机坐标系的1*3平移矩阵,xc0为摄像机所在的中心轴与车辆中心轴线两直线的距离,yc0为初始条件下摄像机距离地面的高度,zc0为摄像机距离农机后轴的距离;
步骤1.4:将摄像机坐标(xc,yc,zc,1)T转换到图像物理坐标(x1,y1)T,具体的转换关系如下,
式(1-2)中,f为摄像机的焦距,焦距单位为mm;
步骤1.5:将图像物理坐标(x1,y1)T转换到图像像素坐标(u,v),具体的转换关系如下:
其中,dx,dy分别表示每个像素在横轴和纵轴上单位大小,u0、v0分别为图像像素坐标系下摄像机光轴和成像平面交点的横纵坐标,坐标单位为pixel;
步骤1.6:根据以上公式(1-1)~(1-3)得到图像像素坐标系到车辆坐标系的转换公式,具体的为,
步骤1.7:为了使雷达和视觉信息在空间上融合,将步骤1.6中的坐标转换关系更新为,
其中,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中实时调整后的平移向量中获得△ht的步骤如下,
步骤2.1:实时计算雷达与地面间的高度变化量以及摄像机与地面间的高度变化量,具体的为,
扫描周期t中i时刻与i-1时刻雷达与地面高度距离变化值为△hsti,采用平均法计算扫描周期t中雷达相对地面的高度变化量△hst,
假设扫描周期t中采样时刻i与采样时刻i-1下摄像机与地面高度变化值为△hcti,采用平均法计算扫描周期t中摄像机与地面的高度变化量△hct,
步骤2.2:实时计算出扫描周期t下自适应调整后的平移向量s,具体的为,
其中,k为一个扫描周期中的采样点的总数;
此设计中,通过实时检测雷达距离地面的高度变化以及摄像机距离地面的高度变化,实时更新平移向量s,提高摄像机和雷达空间同步的精度。
为了进一步提高解算雷达数据的准确性,所述步骤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中判断最危险目标的动静状态,工控机根据最危险目标的运动状态规划农机行走路径具体包括以下步骤,
步骤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车为农机的行驶速度;(4-2)式始终不成立时,判定目标为静态,则工控机做出避障处理,具体的为,摄像机扫描出障碍物的边缘轮廓,工控机根据农机的犁具宽度和农机最小转弯半径设定一条避障路径;工控机按照设定好的避障路径解析出农机的前轮转角,并将动作指令发送给导航箱,导航箱控制农机的前轮转角使农机按照设定的避障路径行走;
农机的左右两侧中心位置分别安装有雷达一和雷达二;农机在避障过程中,雷达一和雷达二不断扫描农机左右两侧是否有障碍物,设定雷达一与障碍物的相对距离为d1,设定雷达二与障碍物的相对距离为d2,根据以下公式判断农机是否继续按照避障路径行走,
d1<ds0 (4-3)
d2<ds0 (4-4)
(4-3)或(4-4)任意一个式子成立时,工控机做出停车等待决策,导航箱控制农机停下;否则,农机继续按照避障路径行走;
其中,ds0为设定的转弯安全距离,农机上安装有角度传感器,前轮转向角由角度传感器测得,角度传感器将检测到的转向角信号传输给工控机;
此设计中,判定最危险目标的动静态原理简单,提高响应速度;根据最危险目标的动静态情况做出农机进一步的行走决策,若农机为静态,则农机按照设定的避障路径行走。
为了进一步提高提高获得理论避障路径的可靠性,所述步骤4.2中的避障路径具体的为,
以障碍物的中心为圆心做特征圆,特征圆的半径为rmin+w/2,避障路径由圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三组成,圆弧段一的一端与农机原始的直线路径相切,圆弧段一的另一端与直线段一的一端相切,直线段一的另一端和直线段二的一端分别与圆弧段二相切,直线段二的另一端与圆弧段三相切,圆弧段二为特征圆上的一段,圆弧段一和圆弧段三关于圆弧段二的中心线对称设置,农机依次经过圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三绕过障碍物,其中,rmin为农机的最小转弯半径,w为农机的作业宽度,障碍物的外接圆半径小于最小转弯半径rmin;圆弧段一的半径为rmin,所述圆弧段三的半径为rmin,圆弧段一的起点记为H点,圆弧段一的圆心记为O1点,直线段一与农机原始的直线路径的相交点记为J,直线段一与圆弧段二的相切点记为D,农机原始路径与特征圆的相交点分别记为K和K’,JK=w/2,圆弧段二的圆心记为O点,O的坐标设为(a,b),圆弧段二的中心点记为B点,J点的坐标记为(x1,y1),JD的方程可以写成:
y=k(x-x1)+y1 (4-5);
特征圆的方程可以写成:
(x-a)2+(y-b)2=r2
r=rmin+w/2 (4-6)
通过(4-5)和(4-6)可以求出k,D点为JD和特征圆的相交点,以此解出D点坐标;
设点O1的坐标为(x2,y2),则点O1到直线JD的距离为:
y2=y1+rmin (4-8)
根据公式(4-7)和(4-8)求出O1的坐标;则H点的坐标为(x2,y1),B点的坐标为(a,b+r)。
作为本发明的进一步改进,所述步骤4中根据雷达与相机采集到的最危险目标的图像数据,判断出最危险目标的类型,导航箱控制农机做相应的动作,具体包括以下步骤,
步骤4.1a:最危险目标为动态的情形下,摄像机对最危险目标进行识别,摄像机获取最危险目标的图像,将图像与训练好的人体样本训练库进行匹配比较,输出目标识别结果;
步骤4.2a:导航箱根据输出的目标识别结果控制农机动作,若为非人体,则导航箱发出声光报警,并控制农机做停车等待处理;若目标识别结果为人体,则导航箱发出声光报警,判断人体是否偏离农机行驶车道或者人体向远离农机方向运动,用以下公式判断,
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为本发明中的避障路径轨迹图。
图9为本发明中距离检测装置一的结构示意图。
其中,1导杆,2导向套,3距离传感器,4限位板,5容纳腔,6万向滚轮,7沉头螺钉。
具体实施方式
下面结合附图对本发明作进一步的说明。
如图1~9所示的一种基于农田环境感知的农机无人驾驶导航方法,具体包括以下步骤:
步骤1:农机作业前,对摄像机进行标定,摄像机空间坐标变换,再对雷达视觉联合标定,使得雷达和视觉信息在空间上融合;
步骤2:农机作业时,距离检测装置一实时检测雷达与地面间的高度变化△hst,距离检测装置二实时检测摄像机与地面间的高度变化△hct,工控机进行数据处理实时调整雷达与摄像机坐标的转换关系,使雷达与摄像机在作业条件下实现在空间上的同步;
步骤3:工控机解算接收到的毫米波雷达数据,确定有效目标,选出农机作业前方雷达感兴趣的区域,确定最危险目标,同步进行摄像机图像的采集;
步骤4:根据雷达的信息判断出最危险目标的运动状态,工控机根据最危险目标的运动状态规划农机行走路径,根据雷达与相机采集到的最危险目标的图像数据,判断出最危险目标的类型,工控机将解析出来的动作指令传输给导航箱,导航箱控制农机做相应的动作;
其中,农机作业时,农机的行驶速度匀速;
距离检测装置一和距离检测装置二的结构相同,距离检测装置一安装在农机前侧且设置在雷达正下方,距离检测装置二安装在农机下侧且设置在摄像机正下方位置;如图9所示,距离检测装置一和距离检测装置二的结构相同,距离检测装置一安装在农机前侧且设置在雷达正下方,距离检测装置二安装在农机下侧且设置在摄像机正下方位置;如图8所示,距离检测装置一包括具有容纳腔5的导向套2和导杆1,导向套2安装在农机上,导向套2顶部的内壁上连接有正对导杆1的距离传感器3,导杆1可在导向套2内滑动,导杆1的上侧可拆卸地连接有限制导杆1在容纳腔5内运动的限位板4,导杆1的底部安装有可在地面上滚动的万向滚轮6;限位板4可通过沉头螺钉7与导杆1连接;
工控机接收距离传感器8发送过来的数据信号并进行数据处理;
为了实现摄像机和毫米波雷达在空间上的初步同步,如图2~4所示,步骤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-1)中,R为一个三行三列的正交单位矩阵,sc(xc0,yc0,zc0)为初始条件下车辆坐标系到摄像机坐标系的1*3平移矩阵,xc0为摄像机所在的中心轴与车辆中心轴线两直线的距离,yc0为初始条件下摄像机距离地面的高度,zc0为摄像机距离农机后轴的距离;
步骤1.4:将摄像机坐标(xc,yc,zc,1)T转换到图像物理坐标(x1,y1)T,具体的转换关系如下,
式(1-2)中,f为摄像机的焦距,焦距单位为mm;
步骤1.5:将图像物理坐标(x1,y1)T转换到图像像素坐标(u,v),具体的转换关系如下:
其中,dx,dy分别表示每个像素在横轴和纵轴上单位大小,u0、v0分别为图像像素坐标系下摄像机光轴和成像平面交点的横纵坐标,坐标单位为pixel;
步骤1.6:根据以上公式(1-1)~(1-3)得到图像像素坐标系到车辆坐标系的转换公式,具体的为,
步骤1.7:为了使雷达和视觉信息在空间上融合,将步骤1.6中的坐标转换关系更新为,
其中,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:实时计算雷达与地面间的高度变化量以及摄像机与地面间的高度变化量,具体的为,
扫描周期t中i时刻与i-1时刻雷达与地面高度距离变化值为△hsti,采用平均法计算扫描周期t中雷达相对地面的高度变化量△hst,
假设扫描周期t中采样时刻i与采样时刻i-1下摄像机与地面高度变化值为△hcti,采用平均法计算扫描周期t中摄像机与地面的高度变化量△hct,
步骤2.2:实时计算出扫描周期t下自适应调整后的平移向量s,具体的为,
其中,k为一个扫描周期中的采样点的总数。
步骤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-2)式始终不成立时,判定目标为静态,则工控机做出避障处理,具体的为,摄像机扫描出障碍物的边缘轮廓,工控机根据农机的犁具宽度和农机最小转弯半径设定一条避障路径;工控机按照设定好的避障路径解析出农机的前轮转角,并将动作指令发送给导航箱,导航箱控制农机的前轮转角使农机按照设定的避障路径行走;
农机的左右两侧中心位置分别安装有雷达一和雷达二;农机在避障过程中,雷达一和雷达二不断扫描农机左右两侧是否有障碍物,设定雷达一与障碍物的相对距离为d1,设定雷达二与障碍物的相对距离为d2,根据以下公式判断农机是否继续按照避障路径行走,
d1<ds0 (4-3)
d2<ds0 (4-4)
(4-3)或(4-4)任意一个式子成立时,工控机做出停车等待决策,导航箱控制农机停下;否则,农机继续按照避障路径行走;
其中,ds0为设定的转弯安全距离,农机上安装有角度传感器,前轮转向角由角度传感器测得,角度传感器将检测到的转向角信号传输给工控机;
步骤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)为下一个扫描周期雷达相对最危险目标的距离;
如图8所示,步骤4.2中的避障路径具体的为,
以障碍物的中心为圆心O做特征圆,特征圆的半径为rmin+w/2,避障路径由圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三组成,圆弧段一的一端与农机原始的直线路径相切,圆弧段一的另一端与直线段一的一端相切,直线段一的另一端和直线段二的一端分别与圆弧段二相切,直线段二的另一端与圆弧段三相切,圆弧段二为特征圆上的一段,圆弧段一和圆弧段三关于圆弧段二的中心线对称设置,农机依次经过圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三绕过障碍物;其中,rmin为农机的最小转弯半径,w为农机的作业宽度,障碍物的外接圆半径小于最小转弯半径rmin;圆弧段一的半径为rmin,圆弧段三的半径为rmin,圆弧段一的起点记为H点,圆弧段一的圆心记为O1点,直线段一与农机原始的直线路径的相交点记为J,直线段一与圆弧段二的相切点记为D,农机原始路径与特征圆的相交点分别记为K和K’,JK=w/2,圆弧段二的圆心记为O点,O的坐标设为(a,b),圆弧段二的中心点记为B点,J点的坐标记为(x1,y1),JD的方程可以写成:
y=k(x-x1)+y1 (4-5);
特征圆的方程可以写成:
(x-a)2+(y-b)2=r2
r=rmin+w/2 (4-6)
通过(4-5)和(4-6)可以求出k,D点为JD和特征圆的相交点,以此解出D点坐标;
设点O1的坐标为(x2,y2),则点O1到直线JD的距离为:
y2=y1+rmin (4-8)
根据公式(4-7)和(4-8)求出O1的坐标;则H点的坐标为(x2,y1),B点的坐标为(a,b+r);
步骤4中根据雷达与相机采集到的最危险目标的图像数据,判断出最危险目标的类型,导航箱控制农机做相应的动作,具体包括以下步骤,
步骤4.1a:最危险目标为动态的情形下,摄像机对最危险目标进行识别,摄像机获取最危险目标的图像,将图像与训练好的人体样本训练库进行匹配比较,输出目标识别结果;
步骤4.2a:导航箱根据输出的目标识别结果控制农机动作,若为非人体,则导航箱发出声光报警,并控制农机做停车等待处理;若目标识别结果为人体,则导航箱发出声光报警,判断人体是否偏离农机行驶车道或者人体向远离农机方向运动,用以下公式判断,
zwn+1>zwn (4-3)
di>ds (4-4)
若雷达探测到的人体目标满足(4-3)或(4-4),则农机继续向前行驶,否则导航箱控制农机做停车等待处理;zwn为第n探测扫描周期雷达相对最危险目标的距离,zw(n+1)为下一个扫描周期雷达相对最危险目标的距离;
农机作业前,摄像机与雷达的标定是在水平地面的条件下进行的;而农机作业时,农田地面凹凸不平,由于雷达和摄像机并非安装于农机的同一位置,雷达和摄像机相对地面的高度不同且随着地形发生变化;距离检测装置一的工作过程具体的为,万向滚轮6沿着凹凸不平的地面滚动,当地面凸出时,凸出地面给万向滚轮6向上的作用力,导杆1沿着导向套2内壁向上滑动,距离传感器3检测导杆1的上升距离,即雷达与地面间的高度变化量;向上凸的地面逐渐水平时,导杆1逐渐向下滑动;当地面向下凹进时,导杆1在自重的作用下,导杆1向下滑动,直至万向滚轮6与地面接触,距离传感器3检测导杆1的下降距离,距离传感器3实时将检测到的雷达与当前地面间的高度变化值发送给工控机;距离检测装置二的工作原理跟距离检测装置一的工作原理相同,距离检测装置二实时将检测到的摄像机与当前地面间的高度变化值发送给工控机;
本发明与现有技术相比,本发明将毫米波雷达和摄像机结合起来感知农田环境,通过距离检测装置一和距离检测装置二的设置分别实时检测雷达和摄像机距离地面的高度变化,将高度变化量加入到雷达和摄像机坐标转换的平移向量中;农机作业时,使摄像机和雷达在空间上实现真正的同步,提高摄像机和雷达的融合精度;对噪声和干扰信号产生的随机噪声信号进行滤除,提高雷达探测信号的准确性;根据设定好的农机航向确定为农机行驶车道,将农机行驶车道内的障碍物目标初选为有效目标,再对初选的有效目标进行进一步检验,以进一步确定有效目标,提高雷达感知同车道内障碍物目标的有效性和准确性;选取最危险目标并跟踪最危险目标,摄像机以最危险目标的动静态为基础进行目标识别,若最危险目标为动态,只需要识别出动态目标是否为人体即可,不需要识别出具体类型,减小运算量,提高响应速度,导航箱根据图像识别结果控制农机动作,避免农机在无人驾驶时与障碍物碰撞;若识别结果为人体时,导航箱声光报警提醒劳作人员避开农机,利用人的习惯思维这一特性,不断检测人体是否偏离农机行驶车道或者人体向远离农机方向运动,导航箱根据检测结果控制农机是否做停车等待处理,适应性好;若最危险目标为静态,摄像机扫描出障碍物的轮廓,获取避障路径,控制农机的前轮转向角使农机按照设定的避障路径行走,同时,雷达一和雷达二检测农机避障过程中是否有新的障碍物存在,若有,农机停车等待,以确保农机的安全作业;本发明可应用于农机无人驾驶时农田环境自动感知的导航工作中。
本发明并不局限于上述实施例,在本发明公开的技术方案的基础上,本领域的技术人员根据所公开的技术内容,不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形,这些替换和变形均在本发明保护范围内。
Claims (9)
1.一种基于农田环境感知的农机无人驾驶导航方法,其特征在于,具体包括以下步骤,
步骤1:农机作业前,对摄像机进行标定,摄像机空间坐标变换,再对雷达视觉联合标定,使得雷达和视觉信息在空间上融合;
步骤2:农机作业时,距离检测装置一实时检测雷达与地面间的高度变化△hst,距离检测装置二实时检测摄像机与地面间的高度变化△hct,工控机进行数据处理实时调整雷达与摄像机坐标的转换关系,使雷达与摄像机在作业条件下实现在空间上的同步;
步骤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,将车辆坐标转换为摄像机坐标,具体的转换关系如下,
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mi>c</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mi>c</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>z</mi>
<mi>c</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>R</mi>
</mtd>
<mtd>
<msub>
<mi>s</mi>
<mi>c</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mn>0</mn>
<mi>T</mi>
</msup>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>z</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
式(1-1)中,R为一个三行三列的正交单位矩阵,sc(xc0,yc0,zc0)为初始条件下车辆坐标系到摄像机坐标系的1*3平移矩阵,xc0为摄像机所在的中心轴与车辆中心轴线两直线的距离,yc0为初始条件下摄像机距离地面的高度,zc0为摄像机距离农机后轴的距离;
步骤1.4:将摄像机坐标(xc,yc,zc,1)T转换到图像物理坐标(x1,y1)T,具体的转换关系如下,
<mrow>
<msub>
<mi>z</mi>
<mi>c</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>f</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mi>f</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mi>c</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mi>c</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>z</mi>
<mi>c</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
式(1-2)中,f为摄像机的焦距,焦距单位为mm;
步骤1.5:将图像物理坐标(x1,y1)T转换到图像像素坐标(u,v),具体的转换关系如下:
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>u</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>v</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mn>1</mn>
<mrow>
<mi>d</mi>
<mi>x</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>u</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mfrac>
<mn>1</mn>
<mrow>
<mi>d</mi>
<mi>y</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<msub>
<mi>v</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,dx,dy分别表示每个像素在横轴和纵轴上单位大小,u0、v0分别为图像像素坐标系下摄像机光轴和成像平面交点的横纵坐标,坐标单位为pixel;
步骤1.6:根据以上公式(1-1)~(1-3)得到图像像素坐标系到车辆坐标系的转换公式,具体的为,
<mrow>
<msub>
<mi>z</mi>
<mi>c</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>u</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>v</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mn>1</mn>
<mrow>
<mi>d</mi>
<mi>x</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>u</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mfrac>
<mn>1</mn>
<mrow>
<mi>d</mi>
<mi>y</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<msub>
<mi>v</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>f</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mi>f</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>R</mi>
</mtd>
<mtd>
<msub>
<mi>s</mi>
<mi>c</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mn>0</mn>
<mi>T</mi>
</msup>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>z</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>4</mn>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
步骤1.7:为了使雷达和视觉信息在空间上融合,将步骤1.6中的坐标转换关系更新为,
<mrow>
<msub>
<mi>z</mi>
<mi>c</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>u</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>v</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mn>1</mn>
<mrow>
<mi>d</mi>
<mi>x</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>u</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mfrac>
<mn>1</mn>
<mrow>
<mi>d</mi>
<mi>y</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<msub>
<mi>v</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>f</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mi>f</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>R</mi>
</mtd>
<mtd>
<mi>s</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mn>0</mn>
<mi>T</mi>
</msup>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>z</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>5</mn>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
其中,s=sc+s0,s0的坐标设为(xs0,ys0,zs0),xs0=0,ys0为初始条件下雷达距离地面的高度,zs0为雷达与农机后轴的距离。
3.根据权利要求2所述的基于农田环境感知的农机无人驾驶导航方法,其特征在于,所述步骤2中的工控机进行数据处理实时调整雷达与摄像机坐标的转换关系,具体的为,根据农机的实际路况实时调整平移向量s,扫描周期t下调整后的平移向量st=sc+s0+△st,实时的车辆坐标与图像像素坐标的转换关系,具体的为,
<mrow>
<msub>
<mi>z</mi>
<mi>c</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>u</mi>
<mi>t</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mi>t</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mn>1</mn>
<mrow>
<mi>d</mi>
<mi>x</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>u</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mfrac>
<mn>1</mn>
<mrow>
<mi>d</mi>
<mi>y</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<msub>
<mi>v</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>f</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mi>f</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>R</mi>
</mtd>
<mtd>
<msub>
<mi>s</mi>
<mi>t</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mn>0</mn>
<mi>T</mi>
</msup>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>z</mi>
<mi>w</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
<mrow>
<msub>
<mi>&Delta;s</mi>
<mi>t</mi>
</msub>
<mo>=</mo>
<msup>
<mrow>
<mo>(</mo>
<mn>0</mn>
<mo>,</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>t</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mrow>
<mi>t</mi>
<mo>=</mo>
<mi>j</mi>
</mrow>
</munderover>
<mo>(</mo>
<mrow>
<msub>
<mi>&Delta;h</mi>
<mrow>
<mi>c</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>&Delta;h</mi>
<mrow>
<mi>s</mi>
<mi>t</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
<mo>,</mo>
<mn>0</mn>
<mo>)</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>-</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,△hct为扫描周期t下摄像机与地面高度的变化值,△hst为扫描周期t下雷达与地面高度的变化值,j为扫描周期数,(ut,vt)为农机作业过程中扫描周期t下实时更新计算得到的图像像素坐标。
4.根据权利要求3所述的基于农田环境感知的农机无人驾驶导航方法,其特征在于,
所述步骤2中实时调整后的平移向量中获得△ht的步骤如下,
步骤2.1:实时计算雷达与地面间的高度变化量以及摄像机与地面间的高度变化量,具体的为,
扫描周期t中i时刻与i-1时刻雷达与地面高度距离变化值为△hsti,采用平均法计算扫描周期t中雷达相对地面的高度变化量△hst,
<mrow>
<msub>
<mi>&Delta;h</mi>
<mrow>
<mi>s</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mi>k</mi>
</mrow>
</munderover>
<msub>
<mi>h</mi>
<mrow>
<mi>s</mi>
<mi>t</mi>
<mi>i</mi>
</mrow>
</msub>
</mrow>
<mi>k</mi>
</mfrac>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>-</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
假设扫描周期t中采样时刻i与采样时刻i-1下摄像机与地面高度变化值为△hcti,采用平均法计算扫描周期t中摄像机与地面的高度变化量△hct,
<mrow>
<msub>
<mi>&Delta;h</mi>
<mrow>
<mi>c</mi>
<mi>t</mi>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mi>k</mi>
</mrow>
</munderover>
<msub>
<mi>h</mi>
<mrow>
<mi>c</mi>
<mi>t</mi>
<mi>i</mi>
</mrow>
</msub>
</mrow>
<mi>k</mi>
</mfrac>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>-</mo>
<mn>4</mn>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
步骤2.2:实时计算出扫描周期t下自适应调整后的平移向量s,具体的为,
<mrow>
<mi>s</mi>
<mo>=</mo>
<msub>
<mi>s</mi>
<mi>c</mi>
</msub>
<mo>+</mo>
<msub>
<mi>s</mi>
<mn>0</mn>
</msub>
<mo>+</mo>
<mrow>
<mo>(</mo>
<mn>0</mn>
<mo>,</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>t</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mrow>
<mi>t</mi>
<mo>=</mo>
<mi>j</mi>
</mrow>
</munderover>
<mfrac>
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mi>k</mi>
</mrow>
</munderover>
<mrow>
<mo>(</mo>
<msub>
<mi>h</mi>
<mrow>
<mi>c</mi>
<mi>t</mi>
<mi>i</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>h</mi>
<mrow>
<mi>s</mi>
<mi>t</mi>
<mi>i</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mi>k</mi>
</mfrac>
<mo>,</mo>
<mn>0</mn>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>-</mo>
<mn>5</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,k为一个扫描周期中的采样点的总数。
5.根据权利要求4所述的基于农田环境感知的农机无人驾驶导航方法,其特征在于,所述步骤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的有效目标为最危险目标。
6.根据权利要求5所述的基于农田环境感知的农机无人驾驶导航方法,其特征在于,所述步骤3.4中对初选的有效目标进行有效性检验具体包括以下步骤,
步骤3.4.1:对初选的有效目标进行预测,选取状态Sn=[dn,vn,an],初选有效目标的状态预测方程为,
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>d</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>n</mi>
<mo>)</mo>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>n</mi>
<mo>)</mo>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>n</mi>
<mo>)</mo>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mi>t</mi>
</mtd>
<mtd>
<mfrac>
<msup>
<mi>t</mi>
<mn>2</mn>
</msup>
<mn>2</mn>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mi>t</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>d</mi>
<mi>n</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mi>n</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mi>n</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>-</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,d(n+1,n)、v(n+1,n)、a(n+1,n)是上一个扫描周期预测的有效障碍物目标的状态信息,dn,vn,an分别表示毫米波雷达第n探测周期内测得的有效障碍物目标的相对距离、相对速度、相对加速度,t是毫米波雷达的扫描周期;
步骤3.4.2:通过比较预测的第n+1周期有效目标的状态信息和雷达实际测得的第n+1周期有效目标的状态信息,具体如下,
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>d</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>d</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>n</mi>
<mo>)</mo>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>n</mi>
<mo>)</mo>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mrow>
<mo>(</mo>
<mi>n</mi>
<mo>+</mo>
<mn>1</mn>
<mo>,</mo>
<mi>n</mi>
<mo>)</mo>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>&le;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>d</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>a</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>-</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
其中,d0、v0、a0是设定的有效障碍物目标测量值与预测值之间的误差阈值;
步骤3.4.3:有效障碍物目标在雷达的扫描周期中被连续探测到m次以上,同时,满足步骤3.4.2中公式(3-3)的有效目标与初选有效目标一致,则更新目标的相对距离、相对速度、相对角度、编号信息;否则,初选的有效目标不处于毫米波雷达探测目标中,使用有效目标预测信息对初选的有效目标进行跟踪,若初选的有效目标在雷达的下一个扫描周期中仍然没有被探测到,则停止使用对应的初选有效目标信息,更新有效目标信息,并返回步骤3.4.1循环执行。
7.根据权利要求6所述的基于农田环境感知的农机无人驾驶导航方法,其特征在于,所述步骤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车为农机的行驶速度;(4-2)式始终不成立时,判定目标为静态,则工控机做出避障处理,具体的为,摄像机扫描出障碍物的边缘轮廓,工控机根据农机的犁具宽度和农机最小转弯半径设定一条避障路径;工控机按照设定好的避障路径解析出农机的前轮转角,并将动作指令发送给导航箱,导航箱控制农机按照设定的避障路径行走;
农机的左右两侧中心位置分别安装有雷达一和雷达二;农机在避障过程中,雷达一和雷达二不断扫描农机左右两侧是否有障碍物,设定雷达一与障碍物的相对距离为d1,设定雷达二与障碍物的相对距离为d2,根据以下公式判断农机是否继续按照避障路径行走,
d1<ds0 (4-3)
d2<ds0 (4-4)
(4-3)或(4-4)任意一个式子成立时,工控机做出停车等待决策,导航箱控制农机停下;否则,农机继续按照避障路径行走;
其中,ds0为设定的转弯安全距离。
8.根据权利要求7所述的基于农田环境感知的农机无人驾驶导航方法,其特征在于,所述步骤4.2中的避障路径具体的为,
以障碍物的中心为圆心做特征圆,特征圆的半径为rmin+w/2,避障路径由圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三组成,圆弧段一的一端与农机原始的直线路径相切,圆弧段一的另一端与直线段一的一端相切,直线段一的另一端和直线段二的一端分别与圆弧段二相切,直线段二的另一端与圆弧段三相切,圆弧段二为特征圆上的一段,圆弧段一和圆弧段三关于圆弧段二的中心线对称设置,农机依次经过圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三绕过障碍物,其中,rmin为农机的最小转弯半径,w为农机的作业宽度,障碍物的外接圆半径小于最小转弯半径rmin;圆弧段一的半径为rmin,所述圆弧段三的半径为rmin,圆弧段一的起点记为H点,圆弧段一的圆心记为O1点,直线段一与农机原始的直线路径的相交点记为J,直线段一与圆弧段二的相切点记为D,农机原始路径与特征圆的相交点分别记为K和K’,JK=w/2,圆弧段二的圆心记为O点,O的坐标设为(a,b),圆弧段二的中心点记为B点,J点的坐标记为(x1,y1),JD的方程可以写成:
y=k(x-x1)+y1 (4-5);
特征圆的方程可以写成:
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>-</mo>
<mi>a</mi>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<mi>y</mi>
<mo>-</mo>
<mi>b</mi>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>=</mo>
<msup>
<mi>r</mi>
<mn>2</mn>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>r</mi>
<mo>=</mo>
<mfrac>
<mrow>
<mo>|</mo>
<mrow>
<mi>k</mi>
<mi>a</mi>
<mo>-</mo>
<msub>
<mi>kx</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<mi>b</mi>
<mo>+</mo>
<msub>
<mi>y</mi>
<mn>1</mn>
</msub>
</mrow>
<mo>|</mo>
</mrow>
<msqrt>
<mrow>
<msup>
<mi>k</mi>
<mn>2</mn>
</msup>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msqrt>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>r</mi>
<mo>=</mo>
<msub>
<mi>r</mi>
<mi>min</mi>
</msub>
<mo>+</mo>
<mi>w</mi>
<mo>/</mo>
<mn>2</mn>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>-</mo>
<mn>6</mn>
<mo>)</mo>
</mrow>
</mrow>
通过(4-5)和(4-6)可以求出k,D点为JD和特征圆的相交点,以此解出D点坐标;
设点O1的坐标为(x2,y2),则点O1到直线JD的距离为:
<mrow>
<msub>
<mi>r</mi>
<mi>min</mi>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mo>|</mo>
<mrow>
<msub>
<mi>kx</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<mi>b</mi>
</mrow>
<mo>|</mo>
</mrow>
<msqrt>
<mrow>
<msup>
<mi>k</mi>
<mn>2</mn>
</msup>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msqrt>
</mfrac>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>-</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
<mo>,</mo>
<msub>
<mi>y</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<msub>
<mi>y</mi>
<mn>1</mn>
</msub>
<mo>+</mo>
<msub>
<mi>r</mi>
<mrow>
<mi>m</mi>
<mi>i</mi>
<mi>n</mi>
</mrow>
</msub>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>-</mo>
<mn>8</mn>
<mo>)</mo>
</mrow>
</mrow>
根据公式(4-7)和(4-8)求出O1的坐标;则H点的坐标为(x2,y1),B点的坐标为(a,b+r)。
9.根据权利要求7所述的基于农田环境感知的农机无人驾驶导航方法,其特征在于,所述步骤4中根据雷达与相机采集到的最危险目标的图像数据,判断出最危险目标的类型,导航箱控制农机做相应的动作,具体包括以下步骤,
步骤4.1a:最危险目标为动态的情形下,摄像机对最危险目标进行识别,摄像机获取最危险目标的图像,将图像与训练好的人体样本训练库进行匹配比较,输出目标识别结果;
步骤4.2a:导航箱根据输出的目标识别结果控制农机动作,若为非人体,则导航箱发出声光报警,并控制农机做停车等待处理;若目标识别结果为人体,则导航箱发出声光报警,判断人体是否偏离农机行驶车道或者人体向远离农机方向运动,用以下公式判断,
zwn+1>zwn (4-3)
di>ds (4-4)
若雷达探测到的人体目标满足(4-3)或(4-4),则农机继续向前行驶,否则导航箱控制农机做停车等待处理;zwn为第n探测扫描周期雷达相对最危险目标的距离,zw(n+1)为下一个扫描周期雷达相对最危险目标的距离。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN2017101421779 | 2017-03-10 | ||
CN201710142177.9A CN106909148A (zh) | 2017-03-10 | 2017-03-10 | 基于农田环境感知的农机无人驾驶导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108089185A true CN108089185A (zh) | 2018-05-29 |
CN108089185B CN108089185B (zh) | 2020-09-29 |
Family
ID=59187577
Family Applications (2)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710142177.9A Pending CN106909148A (zh) | 2017-03-10 | 2017-03-10 | 基于农田环境感知的农机无人驾驶导航方法 |
CN201711268681.XA Active CN108089185B (zh) | 2017-03-10 | 2017-12-05 | 基于农田环境感知的农机无人驾驶导航方法 |
Family Applications Before (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710142177.9A Pending CN106909148A (zh) | 2017-03-10 | 2017-03-10 | 基于农田环境感知的农机无人驾驶导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (2) | CN106909148A (zh) |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108107887A (zh) * | 2017-03-10 | 2018-06-01 | 南京沃杨机械科技有限公司 | 农机导航用农田环境感知方法 |
CN109001743A (zh) * | 2018-09-06 | 2018-12-14 | 中国铁道科学研究院集团有限公司通信信号研究所 | 有轨电车防撞*** |
CN109358625A (zh) * | 2018-10-25 | 2019-02-19 | 丰疆智慧农业股份有限公司 | 自动驾驶农机及其动力*** |
CN110245701A (zh) * | 2019-06-11 | 2019-09-17 | 云南电网有限责任公司曲靖供电局 | 一种基于无人机航拍图像的电力线检测方法 |
CN110596696A (zh) * | 2018-06-13 | 2019-12-20 | 通用汽车环球科技运作有限责任公司 | 用于改善的雷达波束成形的设备及方法 |
CN113064149A (zh) * | 2021-03-12 | 2021-07-02 | 中国煤炭科工集团太原研究院有限公司 | 一种基于测距雷达的掘进装备导航定位方法 |
CN113778081A (zh) * | 2021-08-19 | 2021-12-10 | 中国农业科学院农业资源与农业区划研究所 | 一种基于激光雷达和视觉的果园路径识别方法和机器人 |
CN113848880A (zh) * | 2021-08-30 | 2021-12-28 | 中车大连电力牵引研发中心有限公司 | 一种基于改进Q-learning的农机路径优化方法 |
CN113924462A (zh) * | 2020-01-03 | 2022-01-11 | 移动眼视觉科技有限公司 | 用于确定物体尺寸的导航***和方法 |
CN109001743B (zh) * | 2018-09-06 | 2024-08-02 | 中国铁道科学研究院集团有限公司通信信号研究所 | 有轨电车防撞*** |
Families Citing this family (24)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109144052B (zh) * | 2017-07-07 | 2021-12-28 | 奥特埃克斯股份有限公司 | 用于自动驾驶车辆的导航***及其方法 |
CN107817798A (zh) * | 2017-10-30 | 2018-03-20 | 洛阳中科龙网创新科技有限公司 | 一种基于深度学习***的农用机械避障方法 |
CN107703945A (zh) * | 2017-10-30 | 2018-02-16 | 洛阳中科龙网创新科技有限公司 | 一种多目标融合的智能农用机械路径规划方法 |
CN107862287A (zh) * | 2017-11-08 | 2018-03-30 | 吉林大学 | 一种前方小区域物体识别及车辆预警方法 |
CN109917802B (zh) * | 2017-12-13 | 2022-02-22 | 翔升(上海)电子技术有限公司 | 基于无人机的牧群行进轨迹监控方法及*** |
CN108508889B (zh) * | 2018-03-02 | 2024-04-05 | 西安工业大学 | 小型智能除草及松土装置和控制方法 |
CN108600992A (zh) * | 2018-06-26 | 2018-09-28 | 洛阳中科龙网创新科技有限公司 | 一种用于车路协同业务的智能网联农机*** |
CN109062215A (zh) * | 2018-08-24 | 2018-12-21 | 北京京东尚科信息技术有限公司 | 机器人及基于其的目标跟随避障方法、***、设备和介质 |
CN109606473A (zh) * | 2018-10-25 | 2019-04-12 | 丰疆智慧农业股份有限公司 | 农机和避震方法 |
KR20200078359A (ko) * | 2018-12-21 | 2020-07-01 | 가부시끼 가이샤 구보다 | 파식계 작업기 및 파식계 작업기의 자동 주행 제어 시스템, 포장 작업차 및 주행 경로 생성 시스템 |
CN109765903B (zh) * | 2019-02-28 | 2022-03-08 | 北京智行者科技有限公司 | 一种自动驾驶规划方法 |
CN110362072A (zh) * | 2019-06-11 | 2019-10-22 | 江苏理工学院 | 一种基于自动导航的多种农机协同作业工作方法 |
CN110794397B (zh) * | 2019-10-18 | 2022-05-24 | 北京全路通信信号研究设计院集团有限公司 | 一种基于相机和雷达的目标检测方法及*** |
CN110764512A (zh) * | 2019-11-18 | 2020-02-07 | 江苏农林职业技术学院 | 一种农机驾驶控制*** |
CN110908387A (zh) * | 2019-12-13 | 2020-03-24 | 齐鲁工业大学 | 一种动态环境水面无人艇路径规划方法、介质及电子设备 |
CN113064411A (zh) * | 2019-12-30 | 2021-07-02 | 北京猎户星空科技有限公司 | 一种机器人避障方法、装置、电子设备及存储介质 |
CN111505623B (zh) * | 2020-04-24 | 2023-04-18 | 中南大学 | 无人驾驶车辆行驶过程中的障碍物检测方法、***、车辆 |
CN111754577B (zh) * | 2020-07-10 | 2023-07-11 | 南京艾格慧元农业科技有限公司 | 目标识别***及基于其的拖拉机倒车与农具连接方法 |
CN112526989B (zh) * | 2020-11-20 | 2023-11-21 | 广州极飞科技股份有限公司 | 一种农业无人车导航方法、装置、农业无人车及存储介质 |
CN112622893A (zh) * | 2020-12-25 | 2021-04-09 | 北京理工大学前沿技术研究院 | 一种多传感器融合的靶车自动驾驶避障方法及*** |
CN113390410B (zh) * | 2021-08-04 | 2023-01-13 | 北京云恒科技研究院有限公司 | 一种适用于无人机的惯性组合导航方法 |
CN113985886B (zh) * | 2021-11-02 | 2024-01-19 | 珠海格力电器股份有限公司 | 设备作业路径规划方法、装置、计算机设备和存储介质 |
CN115309168B (zh) * | 2022-10-11 | 2023-01-10 | 天地科技股份有限公司 | 一种井下无人车控制方法及装置 |
CN117672007B (zh) * | 2024-02-03 | 2024-04-26 | 福建省高速公路科技创新研究院有限公司 | 一种基于雷视融合的道路施工区域安全预警*** |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101335431A (zh) * | 2008-07-27 | 2008-12-31 | 广西电力工业勘察设计研究院 | 基于机载激光雷达数据的架空送电线路优化选线方法 |
CN101641610A (zh) * | 2007-02-21 | 2010-02-03 | 电子地图北美公司 | 用于包含绝对及相对坐标的车辆导航及领航的***及方法 |
US20100063664A1 (en) * | 2008-09-11 | 2010-03-11 | Noel Wayne Anderson | High integrity perception program |
CN101975951A (zh) * | 2010-06-09 | 2011-02-16 | 北京理工大学 | 一种融合距离和图像信息的野外环境障碍检测方法 |
US20110231061A1 (en) * | 2009-09-17 | 2011-09-22 | Reeve David R | Gnss integrated multi-sensor control system and method |
CN103885359A (zh) * | 2014-02-26 | 2014-06-25 | 中山大学 | 一种基于嵌入式arm的智能车载终端 |
WO2016179637A1 (en) * | 2015-05-12 | 2016-11-17 | Precision Autonomy Pty Ltd | Systems and methods of unmanned vehicle control and monitoring |
CN106441319A (zh) * | 2016-09-23 | 2017-02-22 | 中国科学院合肥物质科学研究院 | 一种无人驾驶车辆车道级导航地图的生成***及方法 |
CN108107887A (zh) * | 2017-03-10 | 2018-06-01 | 南京沃杨机械科技有限公司 | 农机导航用农田环境感知方法 |
-
2017
- 2017-03-10 CN CN201710142177.9A patent/CN106909148A/zh active Pending
- 2017-12-05 CN CN201711268681.XA patent/CN108089185B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101641610A (zh) * | 2007-02-21 | 2010-02-03 | 电子地图北美公司 | 用于包含绝对及相对坐标的车辆导航及领航的***及方法 |
CN101335431A (zh) * | 2008-07-27 | 2008-12-31 | 广西电力工业勘察设计研究院 | 基于机载激光雷达数据的架空送电线路优化选线方法 |
US20100063664A1 (en) * | 2008-09-11 | 2010-03-11 | Noel Wayne Anderson | High integrity perception program |
US20110231061A1 (en) * | 2009-09-17 | 2011-09-22 | Reeve David R | Gnss integrated multi-sensor control system and method |
CN101975951A (zh) * | 2010-06-09 | 2011-02-16 | 北京理工大学 | 一种融合距离和图像信息的野外环境障碍检测方法 |
CN103885359A (zh) * | 2014-02-26 | 2014-06-25 | 中山大学 | 一种基于嵌入式arm的智能车载终端 |
WO2016179637A1 (en) * | 2015-05-12 | 2016-11-17 | Precision Autonomy Pty Ltd | Systems and methods of unmanned vehicle control and monitoring |
CN106441319A (zh) * | 2016-09-23 | 2017-02-22 | 中国科学院合肥物质科学研究院 | 一种无人驾驶车辆车道级导航地图的生成***及方法 |
CN108107887A (zh) * | 2017-03-10 | 2018-06-01 | 南京沃杨机械科技有限公司 | 农机导航用农田环境感知方法 |
Non-Patent Citations (2)
Title |
---|
CHIA-CHEN KUO等: "Application of virtual reality in ecological farmland navigating system", 《PROCEEDINGS. SEVENTH INTERNATIONAL CONFERENCE ON HIGH PERFORMANCE COMPUTING AND GRID IN ASIA PACIFIC REGION, 2004》 * |
刘大学: "用于越野自主导航车的激光雷达与视觉融合方法研究", 《中国博士学位论文全文数据库 信息科技辑》 * |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108107887A (zh) * | 2017-03-10 | 2018-06-01 | 南京沃杨机械科技有限公司 | 农机导航用农田环境感知方法 |
CN110596696A (zh) * | 2018-06-13 | 2019-12-20 | 通用汽车环球科技运作有限责任公司 | 用于改善的雷达波束成形的设备及方法 |
CN110596696B (zh) * | 2018-06-13 | 2024-03-15 | 通用汽车环球科技运作有限责任公司 | 用于改善的雷达波束成形的设备及方法 |
CN109001743A (zh) * | 2018-09-06 | 2018-12-14 | 中国铁道科学研究院集团有限公司通信信号研究所 | 有轨电车防撞*** |
CN109001743B (zh) * | 2018-09-06 | 2024-08-02 | 中国铁道科学研究院集团有限公司通信信号研究所 | 有轨电车防撞*** |
CN109358625A (zh) * | 2018-10-25 | 2019-02-19 | 丰疆智慧农业股份有限公司 | 自动驾驶农机及其动力*** |
CN110245701A (zh) * | 2019-06-11 | 2019-09-17 | 云南电网有限责任公司曲靖供电局 | 一种基于无人机航拍图像的电力线检测方法 |
CN113924462A (zh) * | 2020-01-03 | 2022-01-11 | 移动眼视觉科技有限公司 | 用于确定物体尺寸的导航***和方法 |
CN113064149A (zh) * | 2021-03-12 | 2021-07-02 | 中国煤炭科工集团太原研究院有限公司 | 一种基于测距雷达的掘进装备导航定位方法 |
CN113778081A (zh) * | 2021-08-19 | 2021-12-10 | 中国农业科学院农业资源与农业区划研究所 | 一种基于激光雷达和视觉的果园路径识别方法和机器人 |
CN113848880A (zh) * | 2021-08-30 | 2021-12-28 | 中车大连电力牵引研发中心有限公司 | 一种基于改进Q-learning的农机路径优化方法 |
CN113848880B (zh) * | 2021-08-30 | 2023-12-22 | 中车大连电力牵引研发中心有限公司 | 一种基于改进Q-learning的农机路径优化方法 |
Also Published As
Publication number | Publication date |
---|---|
CN108089185B (zh) | 2020-09-29 |
CN106909148A (zh) | 2017-06-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108089185B (zh) | 基于农田环境感知的农机无人驾驶导航方法 | |
CN108154084B (zh) | 用于农机无人驾驶的多传感器融合的农田环境感知方法 | |
CN108082181B (zh) | 基于农田环境感知的农机导航控制方法 | |
CN108107887B (zh) | 农机导航用农田环境感知方法 | |
CN106950952B (zh) | 用于农机无人驾驶的农田环境感知方法 | |
CN108169743B (zh) | 农机无人驾驶用农田环境感知方法 | |
US10551854B2 (en) | Method for detecting target object, detection apparatus and robot | |
EP3825903A1 (en) | Method, apparatus and storage medium for detecting small obstacles | |
KR102420476B1 (ko) | 차량의 위치 추정 장치, 차량의 위치 추정 방법, 및 이러한 방법을 수행하도록 프로그램된 컴퓨터 프로그램을 저장하는 컴퓨터 판독가능한 기록매체 | |
CN111368607B (zh) | 一种机器人、障碍物的检测方法及检测装置 | |
CN113424079A (zh) | 障碍物检测方法、装置、计算机设备和存储介质 | |
CN105700525B (zh) | 基于Kinect传感器深度图机器人工作环境不确定性地图构建法 | |
CN110765894A (zh) | 目标检测方法、装置、设备及计算机可读存储介质 | |
CN113378741B (zh) | 一种基于多源传感器的飞机牵引车辅助感知方法及其*** | |
CN107796373B (zh) | 一种基于车道平面几何模型驱动的前方车辆单目视觉的测距方法 | |
CN113850102B (zh) | 基于毫米波雷达辅助的车载视觉检测方法及*** | |
CN102806913A (zh) | 一种新型的车道线偏离检测方法及装置 | |
CN106584451A (zh) | 一种基于视觉导航的变电站自动构图机器人及方法 | |
CN111123262B (zh) | 自动驾驶3d建模方法、装置及*** | |
CN111077890A (zh) | 一种基于gps定位与自动避障的农业机器人的实现方法 | |
CN109947093A (zh) | 一种基于双目视觉的智能避障算法 | |
CN110136186A (zh) | 一种用于移动机器人目标测距的检测目标匹配方法 | |
CN113064430A (zh) | 一种基于Android手机的质量巡检小车避障及路径规划算法 | |
CN111126363B (zh) | 自动驾驶车辆的物体识别方法及装置 | |
CN115151836A (zh) | 用于探测车辆周围环境中的移动对象的方法和机动车辆 |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
TR01 | Transfer of patent right | ||
TR01 | Transfer of patent right |
Effective date of registration: 20240208 Address after: No. 31 Changxing West Road, Yangshe Town, Zhangjiagang City, Suzhou City, Jiangsu Province, 215600 Patentee after: Suzhou Xili Agricultural Robot Co.,Ltd. Country or region after: China Address before: No.368 Zhening East Road, Zhetang Town, Lishui Economic Development Zone, Nanjing, Jiangsu 210000 Patentee before: NANJING WOYANG MACHINERY TECHNOLOGY Co.,Ltd. Country or region before: China |