发明内容
本发明针对机器人在自然环境下密植果园行间行驶场景,存在相机受光照影响大、北斗导航精度较低和2D激光雷达不能捕捉足够的环境特征的问题,提出一种基于激光雷达和视觉的果园路径识别方法,包括:
S1,对机器人所搭载的相机和激光雷达传感器进行标定;
S2,使用激光雷达采集点云数据,使用相机采集图像数据,并分别进行数据处理,拟合各自的左右边界线;
S3,机器人行间路径识别,融合左右两侧的边界线,并获取融合后的行中心线,使得机器人沿行间中心线自主行驶。
可选地,所述步骤S1包括:
S11,在相机采集的图像上进行棋盘格角点提取,获得图像坐标。角点提取分为角点检测和亚像素定位;
S12,根据针孔相机模型,将世界坐标变换到图像坐标
S13,采用极大似然估计建立模型对结果进行优化,求得稳定解;
S14,在考虑镜头畸变的情况下对径向畸变进行处理,利用最大似然估计方法建立目标函数,并求最优解;
S15,相机采集图像,对图像进行特征处理,获得图像坐标系下角点的三维坐标;
S16,通过激光雷达获取点云数据,对点云数据处理获得角点激光雷达坐标系下的三维坐标;
S17,对相机与激光雷达获得的点对进行配准,得到相机与激光雷达的相对位姿关系。
可选地,在步骤S2中,通过如下步骤获得激光雷达的左右边界线:通过激光雷达扫面果园三维环境获取点云数据;基于树干圆弧特征和扫描到树干上的点云数量,对树干点云数据进行聚类;对点云数据进行滤波:通过直通滤波过滤掉三个轴方向上的噪声点云,保留树干点云;将树干点云投影到图像坐标系中,并将其分割成左右竖行的点云集;对点云投影到图像上的像素点进行最小二乘直线拟合,得到树干边界线。
可选地,在步骤S2中,通过如下步骤获得相机的左右边界线:通过相机进行图像采集,获取果园图像,对图像像素进行聚类处理:将天空背景分离;对图像进行色彩通道分离,通过改变不同通道权重系数并做差对图像进行处理使得果树树冠、地面杂草分离开来;利用中值滤波对图像边缘信息进行增强处理;对图像进行滤波处理:提取地面杂草边界;对地面杂草进行边界线提取。
可选地,在步骤S3中,通过如下步骤获取融合后的行中心线:对以上直线进行平滑优化处理;当某条边界线出现较大偏差时,通过约束条件来修正该条边界线;在出错的边界线经修正后,计算左右平均后的道路边界线,包括:计算左侧平均边界线;计算右侧平均边界线;以及计算行间中心线。
本发明还提出一种基于激光雷达和视觉的果园路径识别机器人,所述机器人包括激光雷达、相机、存储器和计算器,其中,所述存储器存储计算机代码,当所述计算器执行所述代码时,所述机器人完成如下操作:
S1,使用激光雷达采集点云数据,使用相机采集图像数据,并分别进行数据处理,拟合各自的左右边界线,其中相机和激光雷达传感器已被标定;
S2,机器人行间路径识别,融合左右两侧的边界线,并获取融合后的行中心线,使得机器人沿行间中心线自主行驶。
可选地,机器人通过如下步骤获得激光雷达的左右边界线:通过激光雷达扫面果园三维环境获取点云数据;基于树干圆弧特征和扫描到树干上的点云数量,对树干点云数据进行聚类;对点云数据进行滤波:通过直通滤波过滤掉三个轴方向上的噪声点云,保留树干点云;将树干点云投影到图像坐标系中,并将其分割成左右竖行的点云集;对点云投影到图像上的像素点进行最小二乘直线拟合,得到树干边界线。
可选地,机器人通过如下步骤获得相机的左右边界线:通过相机进行图像采集,获取果园图像,对图像像素进行聚类处理:将天空背景分离;对图像进行色彩通道分离,通过改变不同通道权重系数并做差对图像进行处理使得果树树冠、地面杂草分离开来;利用中值滤波对图像边缘信息进行增强处理;对图像进行滤波处理:提取地面杂草边界;对地面杂草进行边界线提取。
可选地,机器人通过如下方式获取融合后的行中心线:对以上直线进行平滑优化处理;当某条边界线出现较大偏差时,通过约束条件来修正该条边界线;在出错的边界线经修正后,计算左右平均后的道路边界线,包括:计算左侧平均边界线;计算右侧平均边界线;以及计算行间中心线。
可选地,机器人通过如下方法进行路线纠错:计算图像上的中心线上任意一点与拟合的中心线一点之间的偏移距离;根据偏移距离反向调整转向角度。
本发明保证了机器人能够在半结构的自然果园环境中自主作业,本发明的具体创新点如下:
1.针对果园地面存在大量杂草需要花费大量人力成本的情况下,本发明利用相机采集行间地面图像数据并对其进行处理,不仅完成了果园导航任务,也减少了人力成本。
2.当单一传感器工作时,针对机器人前进到某一侧某部分树干被遮挡的地方,激光雷达获取目标点云数据困难,则根据树干拟合出的边界线存在误差,导致计算的行中心线不准确,和机器人利用相机导航时,前进到地面杂草分布不均的地方,则根据地面杂草拟合出的边界线也存在误差,也导致计算的行中心线错误的问题,本发明避免了单一传感器的不正常工作对导航带来的影响。
具体实施方式
下面参照附图描述本发明的实施方式,其中相同的部件用相同的附图标记表示。在不冲突的情况下,下述的实施例及实施例中的技术特征可以相互组合。
图1显示了本发明的方法的流程图,本发明的方法包括S1-S4。
S1,对机器人所搭载的相机和激光雷达传感器进行标定。
单一传感器不可避免的存在局限性,为了提高系统的稳健性,多采取多传感器融合的方案,融合又包含不同传感器的时间同步和空间同步,优选地,为了使相机和激光雷达数据再空间上同步,经过标定后,会获得点云投射到图像上的融合数据,这样就使得点云和图像得到了融合。
首先对机器人搭载的相机进行标定(例如采用张正友相机标定法),过程如下:
S11,在相机采集的图像上进行棋盘格角点提取,获得图像坐标。角点提取分为角点检测和亚像素定位。
S12,根据针孔相机模型,将世界坐标变换到图像坐标,齐次变换如下:
其中A为内参数矩阵,R为旋转矩阵,t为平移向量。在求解单应矩阵后,可进一步求取相机的内外参数。
S13,采用极大似然估计建立模型对结果进行优化,利用LM算法(Levenberg-Marquardt算法)求得稳定解。
S14,在考虑镜头畸变的情况下对径向畸变进行处理,可同样利用最大似然估计方法建立目标函数,并利用LM算法求最优解。
然后,再对相机与激光雷达标定。利用Aruco码作为标定物,将其贴附在一硬纸板上面,采用3D-3D配准的方法对单目相机和激光雷达进行标定。实施例中实现过程如下:
S15,相机采集Aruco码图像,利用其对应的Aruco_ros库对其进行特征处理,获得图像坐标系下角点的三维坐标。
S16,通过激光雷达获取点云数据,利用RANSAC算法分割硬纸板,同样利用Aruco_ros库对其进行处理获得角点激光雷达坐标系下的三维坐标。
S17,采用ICP算法对相机与激光雷达获得的点对进行配准,从而得到相机与激光雷达的相对位姿关系。
在步骤S2,使用激光雷达采集点云数据,使用相机采集图像数据,并分别进行数据处理,拟合各自的左右边界线。具体包括步骤S21-S22。
S21,通过激光雷达获取行间边界线。
1)通过激光雷达扫面果园三维环境获取点云数据。
2)对树干点云信息进行聚类处理,包括:基于树干圆弧特征和扫描到树干上的点云数量,采用RANSAC算法对树干点云聚类。
3)对点云数据进行滤波:通过直通滤波过滤掉三个轴方向上的噪声点云,保留树干点云。
4)将树干点云投影到图像坐标系中,并将其分割成左右竖行的点云集。
5)采用霍夫变换对点云投影到图像上的像素点进行最小二乘直线拟合,得到树干边界线。
S22,通过相机获取行间边界线。
通过相机进行图像采集,获取果园图像,对图像像素进行聚类处理,过程如下:1)提取图像ROI区域,将天空背景分离,加快处理速度。2)将所采集的JPEG格式图像依据颜色空间转换关系转换为HSV图像。3)在HSV色彩空间下对图像进行色彩通道分离,通过改变不同通道权重系数并做差对图像进行处理使得果树树冠、地面杂草分离开来。利用中值滤波对图像边缘信息进行增强处理。
然后,对图像进行滤波处理,过程如下:1)将以上经过处理的图像转化为灰度图像。2)选择直方图变换法确定一个合理的阈值,通过设置灰度值为0和255进行二值化处理。再利用canny边缘检测算法提取地面杂草边界。
最后,通过LSD直线检测算法对地面杂草进行边界线提取,LSD算法的优点是速度快,保证了移动机器人工作的实时性。
此时,相机和激光雷达都获取了包含道路区域的两条边界线。
在步骤S3,机器人行间路径识别,融合左右两侧的边界线,并获取融合后的行中心线,并加入偏离中心线纠正措施。
进行本步骤,是因为可能存在两种问题。第一个情况是,当机器人前进到某一侧某部分树干被遮挡的地方,则根据树干拟合出的边界线存在误差,导致计算的行中心线不准确。第二个情况是,若机器人前进到地面杂草分布不均的地方,则根据地面杂草边界信息拟合出的边界线也存在误差,导致计算的行中心线错误。
设左侧树干点云投影拟合的直线为l1:A1x+B1y+C1=0,左侧杂草边界拟合的直线方程为l2:A2x+B2y+C2=0,右侧树干点云投影拟合的直线为l3:A3x+B3y+C3=0,右侧杂草边界拟合的直线为l4:A4x+B4y+C4=0。
在正常情况下,即不存在上述两种情况时,为了减少噪声对拟合结果的影响,生成较稳定的直线结果,首先使用卡尔曼滤波对以上直线进行平滑优化处理。设lr(n|n)和lr(n|n-1)分别代表第n次预测的状态和已经更新的状态,已经预测的直线状态关键在于直线的斜率和截距,计算公式如下:
lr(n|n)=lr(n|n)x+br(n|n)
kn|n-1=tan(tan-1an|n-1|an|n-1-Δtha)
br(n|n-1)=br(n-1|n-1)+Δxsintan-1kr(n-1|n-1)-Δycostan-1kr(n-1|n-1)
[kr(n|n),br(n|n)]T=[kr(n|n-1),br(n|n-1)]T+K[kr-kr(n|n-1),br-br(n|n-1)]T
式中,Δx,Δy,Δtha分别是根据里程计确定的位置移动和角度旋转,K代表卡尔曼的增益系数,kr(n|n),kr(n|n-1),kr(n-1|n-1)分别是lr(n|n),lr(n|n-1),lr(n-1|n-1)直线的斜率,br(n|n),lb(n|n-1),br(n-1|n-1)直线的截距,kr和br是当前的直线测量结果。为了缩小预测结果和实际结果之间的差异,设定一个协方差矩阵∑k|k-1,这个协方差矩阵通过EKF算法得到。利用Mahalanobis平方距离d作为评估指标,该距离的计算公式如下:
默认经过卡尔曼滤波后,l
1与l
2之间的水平距离d
1等于l
3与l
4之间的水平距离d
2,即d
1=d
2。且左右两直线是平行的,即
当某条边界线出现较大偏差时,通过约束条件d1=d2来修正该条边界线。实施例中实现过程如下:1.取图像中心列上一点(u0,v0),则对应的l1、l2、l4上的点分别为(u′1,v0)、(u′2,v0)、(u′4,v0),l1和l2水平距离为l12=u′1-u′2,则纠正后的直线上一点坐标应该为(u′4-l12,v0),同理,再取图像中心列上一点(u1,v1),得到纠正后的直线上另一点坐标应该为(u4″-l12,v1),根据两点确定一条直线,由此可求得纠正后的直线方程l3。
同理,当出现第二种情况时,也可以求得纠正后的l2或l4。
在出错的边界线经修正后,计算左右平均后的道路边界线。实施例中实现过程如下:
1.计算左侧平均边界线。设左侧激光雷达拟合的树干直线方程l1:A1x+B1y+C1=0,左侧杂草边界拟合的直线方程l2:A1x+B1y+C2=0,右侧激光雷达拟合的树干直线方程l3:A2x+B2y+C3=0,右侧杂草边界拟合的直线方程l4:A2x+B2y+C4=0。左侧两条边界线之间的距离可由以下公式计算:
则左侧平均后的边界线方程为:A1x+B1y-d1=0。
2.计算右侧平均边界线。同理,右侧两条边界线之间的距离可由以下公式计算:
则右侧平均后的边界线方程为:A2x+B2y-d2=0。
3.计算行间中心线。实施例中实现过程如下:1.利用图像中心列上的任两个不同像素点来计算两条融合边界线上对应的坐标。设选中的像素点坐标分别为(u
0,v
0),(u
1,v
1),分别代入左右两边界线直线方程,则对应左侧边界线一点坐标为
右侧边界线坐标为
则对应的一个中心点坐标为
同理,第二个中心点坐标为
根据两点确定一条直线,可确定斜率为
可确定行中心线方程为:
若机器人某一时刻偏离了路线,则纠正其沿行中心路线行驶。实施例中实现过程如下:1)计算图像上的偏移距离。设拟合的中心线与图像中心线重合,当发生偏离时,计算图像中心线上任意一点(u3,v3),其对应行中心线上一点的横坐标计算公式为:
与对应的拟合中心线一点之间的偏移距离Δl,其公式为:Δl=u′3-u3
2)根据以下关系保证其沿道路中心线行驶:
在步骤S4,机器人沿行间中心线自主行驶。
本发明还提出一种机器人,所述机器人包括激光雷达、相机、存储器和计算器,其中,所述存储器存储计算机代码,当所述计算器执行所述代码时,所述机器人完成如前所述的各步骤的操作。
本发明具有如下优点:
1.果园环境内,如果使用GPS定位,由于存在枝叶的遮挡,则会存在GPS丢失的情况,由于果园行间距一般较小,定位精度也较低,本发明可以在无GPS的情况下,实现机器人果园行间正确导航。
2.如果在果园中放置导航牌对机器人进行引导,由于天气、果树生长等因素影响,存在导航牌旧化,被遮挡等问题,本发明避免了导航牌出现问题时对机器人环境感知作业的影响,同时简化了果园场景和减少了导航牌放置、维护、更新和除草等人力成本。
3.二维激光雷达获取环境信息有限,本发明采用三维激光雷达获取丰富的环境信息,避免了当二维激光雷达所采集数据不符合系统输入时带来的机器人环境感知作业出错。
4.本发明采用了相机与激光雷达融合的方法,弥补了单一传感器的缺陷,对系统精度有一定提升作用。
5.本发明中的导航策略加入了因不可抗拒因素导致的行中心偏离纠正方案,保证了机器人在偏离行中心线行驶时能够及时回归行中心线行驶。
以上所述的实施例,只是本发明较优选的具体实施方式,本领域的技术人员在本发明技术方案范围内进行的通常变化和替换都应包含在本发明的保护范围内。