果园作业农业机器人行间定位方法技术

本发明专利技术提供了一种果园作业农业机器人行间定位方法，具体步骤包括：在机器人行间运动初始位姿处采集二维激光雷达扫描测距数据，对激光雷达数据根据欧式距离标准检测果树干位置并存储在世界坐标系坐标系中，在先前时刻定位数据基础上依据里程计信息进行短途航迹推算预测当前时刻机器人位姿值，把当前时刻在机器人坐标系中检测到的树干中心位置点根据机器人位姿预测值转换到世界坐标系中，把世界坐标系中两次不同时刻检测的树干中心点进行匹配，最后利用匹配结果校正当前时刻机器人位姿值，完成机器人果树行间实时准确定位。本发明专利技术可以让果园作业农业机器人实现不依赖卫星信号的定位导航，克服了果园浓密树冠对卫星信号的遮挡。

【技术实现步骤摘要】

本专利技术涉及的是农业机器人
的机器人定位方法，具体是一种果园作业农业机器人果树行间定位的方法。
技术介绍
定位是移动机器人应用时的一个基础问题，是实现自主导航的前提条件。果园中作业的农业机器人也是这样，定位信息可以帮助机器人预判地头和规划路径，实现地头转向，自主驶向下一行。同时，定位信息结合果树株距信息还可以帮助机器人预判果树位置，提高机器人检测果树树干的精度，增加果园植保作业效果等。卫星定位导航作为一种比较成熟的农业机械定位导航方法在宽阔的大田环境已经被应用多年。但是，农业机器人在果园行间作业时，浓密的树冠会严重遮挡卫星信号，使得卫星定位信息变得不再可靠。单纯依靠里程计的推算定位，由于误差的积累，定位精度也不能满足应用需求。因此，有必要开发一种不依赖卫星信号的果园作业农业机器人行间精确定位方法。
技术实现思路
本专利技术的目的是克服现有卫星等定位导航技术的不足，提出一种不依赖卫星信号的果树行间农业机器人精确定位方法。首先在运动初始时刻通过二维激光雷达扫描果树行作业空间中果树干相对与机器人的距离，在运动过程中再将实时检测的树干位置中心点与先前时刻已经存储在世界坐标系中的树干中心点进行匹配，并应用匹配结果校正根据里程计数据估计的机器人位姿，实现了密植果园作业机器人行间在线准确定位。本专利技术是通过以下方案实现的，本专利技术包括步骤如下：第一步，在机器人起始位姿处采集一帧二维激光雷达扫描的测距数据。第二步，基于动态阈值欧氏距离准则，根据激光雷达扫描测距数据确定果树干中心位置。所述的动态阈值随激光雷达测距数据的变化而改变。第三步，将机器人初始位姿时检测到的树干中心位置转换并存储到世界坐标系的树干集合中。所述的世界坐标系与运动初始时刻机器人坐标系相重合，由机器人初始时刻位姿决定。机器人坐标系与机器人本体固联在一起，世界坐标系固定在地面上不随机器人运动。所述的树干集合，当机器人果树行间的运动超越树干集合覆盖的空间范围后需要依据机器人校正位姿值处的树干检测结果对此集合进行更新。第四步，从先前已知位姿出发，果树行间运动过程中机器人在先前已知定位值基础上运用里程计数据进行短途航迹推算，预测当前时刻机器人位姿值。第五步，机器人在当前时刻实时运用激光雷达扫描果树树干中心点位置，并运用此时刻机器人位姿预测值把在机器人坐标系中检测到的树干中心位置坐标转换到世界坐标系当中，再与先前已经存储在树干集合中的树干位置进行匹配。第六步，利用匹配结果校正当前机器人位姿预测值，进而实现机器人实时准确定位。所述的匹配结果为存储在树干集合中的匹配成功的果树树干世界坐标值。与现有技术相比，本专利技术的有益效果是：农业机器人在果树行间定位时可以不依赖卫星定位信号，避免了果园浓密树冠干扰卫星信号而导致的较大定位误差，满足了农业机器人在果树行间的作业要求。附图说明图1是果园作业农业机器人定位流程图。图2是树干位置中心点匹配示意图。图3是果园作业农业机器人位姿校正示意图。具体实施方式以下结合附图对本专利技术的方法作进一步描述，本实施例在以本专利技术技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本专利技术的保护范围不限于下述的实施例。如图1所示，本实施例包括：在机器人运动初始位姿处采集二维激光雷达扫描测距数据、对激光雷达测距数据根据欧氏距离标准检测果树树干并存储在世界坐标系Trunk集合中、在任一k-1时刻已知定位值基础上依据里程计信息进行短途航迹推算预测k时刻机器人位姿值、将k时刻在机器人坐标系XRORYR中检测到的部分树干中心点坐标转换到世界坐标系XWOWYW中、把世界坐标系中两次不同时刻检测出的树干中心点进行匹配、利用匹配结果校正k时刻机器人位姿值。这六步可以得到机器人任意k时刻的准确定位，实现机器人在果树行间任意时刻的定位。下面具体进行说明：第一步，在农业机器人果园作业初始位姿处采集一帧二维激光雷达扫描测距数据点，这些点按照激光扫描雷达的角度分辨率从-5度到185度顺序排列。第二步，基于动态阈值欧氏距离根据激光雷达扫描测距数据检测果树干位置。动态阈值公式为：Tcluster=C0+min(ρi,ρi+1)×2(1-cosΔα)]]>式中，C0为常量，该值可以人工设定；ρi，ρi+1为激光雷达相邻两个扫描点的测距值；Δα为激光雷达的角度分辨率。激光雷达每帧数据中相邻两个扫描点之间的欧氏距离为Δd，那么果树干激光扫描点集合定义为：(1)相邻两个扫描点之间的距离Δd＜Tcluster；(2)起始点：当前Δd＜Tcluster，并且先前Δd＞Tcluster；(3)结束点：当前Δd＜Tcluster，并且随后Δd＞Tcluster。这样一帧激光雷达扫描数据经处理后，就被分割成表示树干的点簇，每一点簇中的点分布在近似圆弧上，其对应的圆心就是果树干的中心位置坐标。第三步，以果树行间运动初始时机器人坐标系为世界坐标系，把在机器人坐标系中检测的树干中心点位置坐标转换到世界坐标系中，并存储在树干集合Trunk中。随后机器人在果树行间运动时，当其位置超越该集合覆盖的空间范围后，需要按照机器人校正后位姿值用实时的树干检测结果更新此集合中存储的树干对象。第四步，机器人从某k-1时刻的已知位姿出发，根据里程计数据进行短途航迹推算，预测机器人k时刻的位姿值。机器人位姿用向量Xr＝(xr，yr，θr)T表示，其中(xr，yr)表示机器人坐标系原点在世界坐标系中的坐标，即机器人的定位位置；θr为机器人在世界坐标系中的航向。则基于里程计的机器人位姿预测为：XrF(k)＝f(Xr(k-1)，u(k))式中，Xr(k-1)为机器人k-1时刻的实际位姿，u(k)为机器人从k-1到k时刻间隔内里程计测得的移动距离和航向变化量，f为机器人运动模型。机器人位姿预测值由于轮胎打滑、预测模型误差等因素的干扰，不能作为k时刻机器人准确的定位值。第五步，将运动过程中实时检测到的部分树干中心点位置，与前面已经存储在树干Trunk集合中的树干中心点位置进行匹配。如图2所示，在k时刻实时用激光雷达检测果树干位置，并利用k时刻机器人位姿的预测值将这些树干位置中心点转换到世界坐标系中：xwiywi=sinθrF(k)cosθrF(k)-cosθrF(k)sinθrF(k)xriyri+xrF(k)yrF(k)]]>式中，[xrF(k)，yrF(k)，θrF(k)]T为k时刻机器人位姿的预测值，(xri，yri)为某树干i的位置点在机器人坐标系中的坐标值。树干集合Trunk中存储的数据也是树干中心在世界坐标系中的坐标，这样k时刻实时检测的就与树干集合中存在部分数据之间的关联对应，是同一树干的两次测量。果园作业机器人在k时刻检测到的树干i的中心与集合Trunk中树干j的中心之间的距离为：di,j=(xwi-xj,t)2+(ywi-yj,t)2]]>式中，(xwi，ywi)为k时刻检测到的树干i的中心点坐标，(xj，t，yj，t)为树干集合Trunk中存储的树干j的中心点坐标。若距离小于阈值，则认为两者之间是匹配的。匹配时无需对k时刻检测到的所有树干进行匹配计算，只需从机器人检测到的左右侧树干本文档来自技高网...

【技术保护点】
果园作业农业机器人行间定位方法，其特征在于，定位具体方法：第一步，在机器人行间运动起始位姿处采集一帧二维激光雷达扫描数据；第二步，基于动态阈值欧氏距离准则，根据激光雷达扫描测距数据检测树干位置；第三步，将世界坐标系选择与运动初始时刻机器人坐标系相重合，并检测到的树干中心位置转换并存储到世界坐标系中；第四步，从先前时刻已知位姿出发，机器人运用里程计数据进行短途航迹推算，得到机器人当前时刻位姿预测值；第五步，机器人当前时刻在机器人坐标系中实时检测果树树干中心点位置，并根据当前时刻的位姿预测值把得到的树干中心位置转换到世界坐标系当中，与前面已经存储在世界坐标系中的树干中心点位置进行匹配；第六步，利用匹配结果实现对机器人位姿预测值进行校正，得出机器人当前时刻位姿校正值，进而实现准确定位。

【技术特征摘要】
1.果园作业农业机器人行间定位方法，其特征在于，定位具体方法：第一步，在机器人行间运动起始位姿处采集一帧二维激光雷达扫描数据；第二步，基于动态阈值欧氏距离准则，根据激光雷达扫描测距数据检测树干位置；第三步，将世界坐标系选择与运动初始时刻机器人坐标系相重合，并检测到的树干中心位置转换并存储到世界坐标系中；第四步，从先前时刻已知位姿出发，机器人运用里程计数据进行短途航迹推算，得到机器人当前时刻位姿预测值；第五步，机器人当前时刻在机器人坐标系中实时检测果树树干中心点位置，并根据当前时刻的位姿预测值把得到的树干中心位置转换到世界坐标系当中，与前面已经存储在世界坐标系中的树干中心点位置进行匹配；第六步，利用匹配结果实现对机器人位姿预测值进行校正...

【专利技术属性】
技术研发人员：周俊，李延华，胡晨，
申请(专利权)人：南京农业大学，
类型：发明
国别省市：江苏;32