果园作业农业机器人行间定位方法.pdf

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

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

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

技术领域

本发明涉及的是农业机器人技术领域的机器人定位方法，具体是一种果园作业农业机器
人果树行间定位的方法。

背景技术

定位是移动机器人应用时的一个基础问题，是实现自主导航的前提条件。果园中作业的
农业机器人也是这样，定位信息可以帮助机器人预判地头和规划路径，实现地头转向，自主
驶向下一行。同时，定位信息结合果树株距信息还可以帮助机器人预判果树位置，提高机器
人检测果树树干的精度，增加果园植保作业效果等。

卫星定位导航作为一种比较成熟的农业机械定位导航方法在宽阔的大田环境已经被应用
多年。但是，农业机器人在果园行间作业时，浓密的树冠会严重遮挡卫星信号，使得卫星定
位信息变得不再可靠。单纯依靠里程计的推算定位，由于误差的积累，定位精度也不能满足
应用需求。因此，有必要开发一种不依赖卫星信号的果园作业农业机器人行间精确定位方法。

发明内容

本发明的目的是克服现有卫星等定位导航技术的不足，提出一种不依赖卫星信号的果树
行间农业机器人精确定位方法。首先在运动初始时刻通过二维激光雷达扫描果树行作业空间
中果树干相对与机器人的距离，在运动过程中再将实时检测的树干位置中心点与先前时刻已
经存储在世界坐标系中的树干中心点进行匹配，并应用匹配结果校正根据里程计数据估计的
机器人位姿，实现了密植果园作业机器人行间在线准确定位。

本发明是通过以下方案实现的，本发明包括步骤如下：

第一步，在机器人起始位姿处采集一帧二维激光雷达扫描的测距数据。

第二步，基于动态阈值欧氏距离准则，根据激光雷达扫描测距数据确定果树干中心位置。

所述的动态阈值随激光雷达测距数据的变化而改变。

第三步，将机器人初始位姿时检测到的树干中心位置转换并存储到世界坐标系的树干集
合中。

所述的世界坐标系与运动初始时刻机器人坐标系相重合，由机器人初始时刻位姿决定。
机器人坐标系与机器人本体固联在一起，世界坐标系固定在地面上不随机器人运动。

所述的树干集合，当机器人果树行间的运动超越树干集合覆盖的空间范围后需要依据机
器人校正位姿值处的树干检测结果对此集合进行更新。

第四步，从先前已知位姿出发，果树行间运动过程中机器人在先前已知定位值基础上运
用里程计数据进行短途航迹推算，预测当前时刻机器人位姿值。

第五步，机器人在当前时刻实时运用激光雷达扫描果树树干中心点位置，并运用此时刻
机器人位姿预测值把在机器人坐标系中检测到的树干中心位置坐标转换到世界坐标系当中，
再与先前已经存储在树干集合中的树干位置进行匹配。

第六步，利用匹配结果校正当前机器人位姿预测值，进而实现机器人实时准确定位。

所述的匹配结果为存储在树干集合中的匹配成功的果树树干世界坐标值。

与现有技术相比，本发明的有益效果是：农业机器人在果树行间定位时可以不依赖卫星
定位信号，避免了果园浓密树冠干扰卫星信号而导致的较大定位误差，满足了农业机器人在
果树行间的作业要求。

附图说明

图1是果园作业农业机器人定位流程图。

图2是树干位置中心点匹配示意图。

图3是果园作业农业机器人位姿校正示意图。

具体实施方式

以下结合附图对本发明的方法作进一步描述，本实施例在以本发明技术方案为前提下进
行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施
例。

如图1所示，本实施例包括：在机器人运动初始位姿处采集二维激光雷达扫描测距数据、
对激光雷达测距数据根据欧氏距离标准检测果树树干并存储在世界坐标系Trunk集合中、在
任一k-1时刻已知定位值基础上依据里程计信息进行短途航迹推算预测k时刻机器人位姿值、
将k时刻在机器人坐标系XRORYR中检测到的部分树干中心点坐标转换到世界坐标系
XWOWYW中、把世界坐标系中两次不同时刻检测出的树干中心点进行匹配、利用匹配结果校
正k时刻机器人位姿值。这六步可以得到机器人任意k时刻的准确定位，实现机器人在果树
行间任意时刻的定位。

下面具体进行说明：

第一步，在农业机器人果园作业初始位姿处采集一帧二维激光雷达扫描测距数据点，这
些点按照激光扫描雷达的角度分辨率从-5度到185度顺序排列。

第二步，基于动态阈值欧氏距离根据激光雷达扫描测距数据检测果树干位置。动态阈值
公式为：

$T_{\mathrm{cluster}}=C_0+\min ({\mathrm{\ρ}}_i,{\mathrm{\ρ}}_{i+1})\×\sqrt{2(1-\cos \mathrm{\Δ\α})}$

式中，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时刻机器人位姿的预
测值将这些树干位置中心点转换到世界坐标系中：

$\left[\begin{array}{c}{x}_{\mathrm{wi}}\\ y_{\mathrm{wi}}\end{array}\right]=\left[\begin{array}{cc}\sin {\mathrm{\θ}}_{\mathrm{rF}}\left(k\right)& \cos {\mathrm{\θ}}_{\mathrm{rF}}\left(k\right)\\ -\cos {\mathrm{\θ}}_{\mathrm{rF}}\left(k\right)& \sin {\mathrm{\θ}}_{\mathrm{rF}}\left(k\right)\end{array}\right]\left[\begin{array}{c}{x}_{\mathrm{ri}}\\ y_{\mathrm{ri}}\end{array}\right]+\left[\begin{array}{c}{x}_{\mathrm{rF}}\left(k\right)\\ y_{\mathrm{rF}}\left(k\right)\end{array}\right]$

式中，[xrF(k)，yrF(k)，θrF(k)]T为k时刻机器人位姿的预测值，(xri，yri)为某树干i的位置点
在机器人坐标系中的坐标值。

树干集合Trunk中存储的数据也是树干中心在世界坐标系中的坐标，这样k时刻实时检
测的就与树干集合中存在部分数据之间的关联对应，是同一树干的两次测量。果园作业机器
人在k时刻检测到的树干i的中心与集合Trunk中树干j的中心之间的距离为：

$d_{i,j}=\sqrt{{(x_{\mathrm{wi}}-x_{j,t})}^2+{(y_{\mathrm{wi}}-y_{j,t})}^2}$

式中，(xwi，ywi)为k时刻检测到的树干i的中心点坐标，(xj，t，yj，t)为树干集合Trunk中存
储的树干j的中心点坐标。若距离小于阈值，则认为两者之间是匹配的。匹配时无需对k时
刻检测到的所有树干进行匹配计算，只需从机器人检测到的左右侧树干当中各取一棵与集合
Trunk中存储的树干匹配即可。

第六步，利用匹配结果实现对机器人位姿预测值进行校正，得出机器人k时刻的准确位
姿。

树干中心点匹配之后，由于树干集合Trunk中存储的树干中心点在世界坐标系中的位置
值是根据机器人位姿校正值得到的，更加准确，因此利用树干集合里匹配成功的树干中心点
计算出k时刻机器人位姿准确值。

如图3所示，用匹配成功的集合Trunk中存储的机器人左右侧各一棵树干的中心点位置
坐标(xl，t，yl，t)与(xr，t，yr，t)，假设yl，t≤yr，t，计算机器人在k时刻的位姿校正值：

式中，(ρ1，α1)和(ρ2，α2)是机器人在利用激光雷达在k时刻检测到的匹配成功的两个树
干中心在机器人坐标系中的极坐标表示。图中过左侧树干的中心作平行于XW的平行线交ρ1
于点A，则上式中：

$L=\sqrt{{(x_{l,t}-x_{r,t})}^2+{(y_{l,t}-y_{r,t})}^2}$

$\mathrm{\β}=\arctan \left(\frac{|y_{l,t}-y_{r,t}|}{|x_{l,t}-x_{r,t}|}\right)$

$\mathrm{\β}+{\mathrm{\φ}}_2=\arccos \left(\frac{L^2+{\mathrm{\ρ}}_2^2-{\mathrm{\ρ}}_1^2}{2L{\mathrm{\ρ}}_2}\right),$

${\mathrm{\φ}}_3=\arccos \left(\frac{{\mathrm{\ρ}}_1^2+{\mathrm{\ρ}}_2^2-L^2}{2{\mathrm{\ρ}}_1{\mathrm{\ρ}}_2}\right),$

φ1＝180°-φ2-φ3，

当yl，t＞yr，t时，位姿校正公式推导类似，不再赘述。至此便完成了果园作业农业机器人
在果树行间的定位过程。

本实施例在机器人运动过程中实时检测的果树树干位置，与先前存储的果树干位置之间
进行匹配，匹配结果用来校正里程计短途航迹推算，进而实时获得机器人位姿校正值，完成
机器人果树行间准确定位。避免了使用易受果园浓密树冠干扰的卫星定位方式。