一种收获机器人的植株行跟随方法与流程
本发明涉及农业装备技术领域，具体涉及一种根茎和块茎作物收获机器人的植株行跟随方法。
背景技术：
我国农村正面临劳动力短缺和人力成本不断上涨的困境，智能化作业装备成为农机装备技术研究中一个新热点，特别的，我国的萝卜、大蒜、莴苣、白菜等作物的种植面积大、收获机械化率低，而现有的收获装备大多是侧挂式。侧挂式收获机械需要外部提供动力，外部动力多由拖拉机来提供，拖拉机的重量较大，会对土壤形成巨大压力，破坏土壤结构，不利于作物生长。无论是侧挂式还是自走式收获机械，都需要操作人员时刻操纵拖拉机或收获机以对准收获对象作业，加大了操作难度，而且也增加了操作员的劳动强度。尤其在长期作业时，不仅会使得操作人员倍感疲劳，而且，也容易导致收获机械漏收和损伤率上升，鉴于此，一些农机企业和研究机构针对部分作物的收获实际需求，或者研制出一些具备部分智能的收获机械，亦或是研制收获机器人以及提供无人驾驶的拖拉机等。
目前，无论是智能收获机械，还是收获机器人或者无人驾驶拖拉机，在野外作业环境中的定位与导航，均基于卫星定位技术实现。如果需要提供高精度的定位与导航，这类卫星定位设备的成本较高，而且，它容易受到环境制约，比如山谷中的卫星信号较弱，即使加入惯性导航系统等数据融合技术，也可能存在定位精度会受到地面起伏不平等干扰。另外，对于呈曲线的作物垄来讲，基于卫星导航的技术需要地图提供辅助，自动跟随植株行时的定位精度不高。特别是我国的农作物种植大多不规范，植株行呈曲线状、不规则，植株偏离行中心等现象普遍存在。即使使用无人驾驶拖拉机进行起垄，仍然不能假设植株行是线性的。
另外，还有一类场地移动机器人或室内移动机器人，或者无人驾驶汽车，使用slam（simultaneouslocalizationandmapping）技术来进行定位与导航。这类技术往往成本很高或者存在应用局限性，而且，目前基于卫星定位和slam的定位与导航技术，都离不开地图或定位标志物的辅助，因此，很难直接应用于收获机器人的低成本、准确可靠的定位与导航问题。
所谓植株行自动跟随，是指野外环境下工作的机器人在对作业对象进行操作时，能够自动准确地感知其自身在环境中的三维空间位置，同时，能够准确感知其作业对象在三维空间中的相对位置和状态，从而正确、连续地跟随作业对象。如上所述，现有技术还不能直接应用于收获机器人的植株行自动跟随问题。
技术实现要素：
本发明的目的在于针对上述问题，提供一种收获机器人的植株行跟随方法，使用激光雷达实现高精度的植株识别与定位，以及植株行的方向预测，使用视觉传感器实现本体的航向角估计，从而在无需地图和定位标志物辅助的情况下，实现机器人本体对植株行的实时跟随，同时，对激光数据的聚类和曲线拟合，可以有效提升机器人环境感知的抗噪能力和自主行走的可靠性。
为此本发明公布了一种收获机器人的植株行跟随方法，包括在收获机器人本体正前方两侧分别设有至少两个激光雷达，并在机器人本体后端设有至少一个视觉传感器，具体通过以下步骤实现：
s1：所述激光雷达至少包括第一激光雷达和第二激光雷达，由第一激光雷达和第二激光雷达对标定物同时进行扫描并分别采集多个激光数据点，将获得的激光数据点通过标定策略分别计算出第二激光雷达相对于第一激光雷达的偏移参数，所述偏移参数包括l、h和a，其中l和h分别代表第二激光雷达的中心相对于第一激光雷达中心在x轴和y轴上的距离，a代表第二激光雷达扫描线零度位置相对于第一激光雷达扫描线零度位置的夹角；
s2：第一激光雷达所采集的激光数据点为第一点集d01，第二激光雷达所采集的激光数据点为第二点集d02，根据s1中计算得到的偏移参数对第二点集进行数据修正；
s3：对第一点集d01和第二点集d02分别进行聚类，分别得到备选点集g1和备选点集g2；
s4：将备选点集g1和备选点集g2合并为表征一个植株的点集g；
s5：将表征植株的点集g拟合成一个圆c，并得到表征植株中心的当前圆心坐标o(ox,oy)；
s6：通过获得的当前圆心坐标o(ox,oy)对植株行方向进行预测估算，得到植株行的当前方向估计dir；
s7：通过视觉传感器获得的图像帧，计算得到机器人的当前实际航向角估计值raw和位移估计；
s8：通过航向角估计raw、位移估计与方向预测dir，得到机器人本体当前轨迹控制输入参数，并对航向进行调整以保证机器人沿当前植株轨迹方向动作。
进一步，s1中的l、h和a通过以下方式计算：
s11：第一激光数据点集d01={（
计算得到d1={（
s12：计算拟合直线l1：y=kx+b，其中
k=
b=
其中，
s13：将d02通过以下公式进行极坐标换算得到数据集d2={（
s14：在数据集d2上，对l、h和a进行非线性优化：
j=
s15：给定l、h和a的初值，根据以下公式进行迭代，当j小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的l、h和a即为标定结果，
l→l+lr
h→h+lr
a→a+lr
其中，lr为学习速率，lr的取值范围为0.00001~0.01。
进一步，在步骤s2中，采用公式（5）~（6）对第二激光数据点集进行数据修正。
进一步，所述步骤s3具体包括以下步骤：
s31：将第一激光点集d01中距离r大于第三阀值的点去掉，得到点集d1；将第二激光点集d02中距离r大于第三阀值的点去掉，得到点集d2；
s32：将点集d1中的点投影到第一激光雷达的0度扫描线上，得到一个点列y1={y1,y2,…,yn}；
s33：将点列y1中第1个点取出，加入新建分组中；
s34：如果i≤n，则取出第i个点yi；否则，进行步骤s37；
s35：如果与前一个相邻点yi-1的距离小于第四阀值，将yi加入当前分组，并返回步骤s34，否则进行步骤s36；
s36：新建一个分组，并将新分组作为当前分组，返回步骤s34；
s37：将得到的所有分组进行比较，点个数最多的一个作为备选分组，备选分组对应的点即构成备选点集g1；
s38：对点集d2进行步骤s32~s37，得到备选点集g2，步骤s3结束。
进一步，所述步骤s5中所采用的拟合圆的方法为最小二乘法。
步骤s6包括以下步骤：
s61：将当前圆心ok加入到圆心点列c={o1,o2,…,ok-1}中；
s62：根据预设定值m，m的值为整数，是根据植株行的弯曲率决定的，m的大小与植株行的弯曲率成反比，从圆心点列c中取出后m个点，利用公式（3）~（4）计算出当前拟合直线y=kx+b，此拟合直线的斜率即为植株行的预测方向dir。
进一步，所述步骤s7包含以下步骤：
s71：对当前帧（第k帧）图像提取特征点集fk0，并与前一帧图像特征点进行匹配，得到当前帧图像的关键点集fk={pk,1,pk,2,…,pk,m}；
s72：对fk中的点pk,i，计算出它在第k帧图像平面xoy中的坐标(xi,yi)和第k-1帧图像平面ucv中的坐标(ui,vi)；
s73：设su和sv分别代表第k帧图像平面xoy原点相对于第k-1帧图像平面ucv原点在u和v方向上的位移，θ代表第k帧图像平面xoy相对第k-1帧图像平面ucv的角度偏转，按下列公式估计su、sv和θ：
j=
其中，
s74：给定su、sv和θ的初值，根据下述公式进行迭代，j小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的θ，即为航向估计结果；
su←su+lr
sv←sv+lr
θ←θ+lr
其中，lr为学习速率。
进一步，所述收获机器人为可对根茎和块茎类作物收获的收获机器人。
本发明的有益效果是：本发明公开了一种收获机器人的植株行跟随方法，在无需地图和定位标志物辅助的情况下，低成本地实现了高精度定位。该方法不仅能够实现收获机器人准确、连续地跟踪收获植株的功能，而且，还实现了机器人在野外收获环境中自主行驶的自动导航。本发明为实现根茎或块茎类作物的收获机器人在野外作业环境中准确、完全自主地完成收获作业提供了重要的技术基础。
附图说明
图1是本发明激光雷达标定示意图；
图2是本发明激光数据点聚类示意图；
图3是本发明植株行方向预测示意图；
图4是本发明机器人航向估计示意图。
具体实施方式
以上是本发明的技术方案内容，现对本发明的方案做如下具体说明：
参阅图1~图4，实施例公布了一种收获机器人的植株行跟随方法，包括在收获机器人本体正前方两侧分别设有至少两个激光雷达，两个以上激光雷达同时对统一植株进行扫描，该标定方法使得激光雷达扫描所采集的数据信息相比于单个激光雷达更加全面而准确，在本实施例中仅对在收获机器人本体正前方两侧设置两个低分辨率激光雷达进行举例说明，并在机器人本体后端设有一个rgb视觉传感器，具体通过以下步骤实现：
s1：由第一激光雷达和第二激光雷达对标定物同时进行扫描并分别采集多个激光数据点，将获得的激光数据点通过标定策略分别计算出第二激光雷达相对于第一激光雷达的偏移参数，所述偏移参数包括l、h和a，其中l和h分别代表第二激光雷达的中心相对于第一激光雷达中心在x轴和y轴上的距离，a代表第二激光雷达扫描线零度位置相对于第一激光雷达的夹角；
s2：第一激光雷达所采集的激光数据点为第一点集d01，第二激光雷达所采集的激光数据点为第二点集d02，根据s1中计算得到的偏移参数对第二点集进行数据修正；
s3：对第一点集d01和第二点集d02分别进行聚类，分别得到备选点集g1和备选点集g2；
s4：将备选点集g1和备选点集g2合并为表征一个植株的点集g；
s5：将表征植株的点集g拟合成一个圆c，并得到表征植株中心的当前圆心坐标o(ox,oy)；
s6：通过获得的当前圆心坐标o(ox,oy)对植株行方向进行预测估算，得到植株行的当前方向估计dir；
s7：通过视觉传感器获得的图像帧，计算得到机器人的当前实际航向角估计值raw和位移估计；
s8：通过航向角估计raw、位移估计与方向预测dir，得到机器人本体当前轨迹控制输入参数，并对航向进行调整以保证机器人沿当前植株轨迹方向动作。
进一步，s1中的l、h和a通过以下方式计算：
s11：第一激光数据点集d01={（
计算得到d1={（
s12：计算拟合直线l1：y=kx+b，其中
k=
b=
其中，
s13：将d02通过以下公式进行极坐标换算得到数据集d2={（
s14：在数据集d2上，对l、h和a进行非线性优化：
j=
s15：给定l、h和a的初值，根据以下公式进行迭代，当j小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的l、h和a即为标定结果，
l→l+lr
h→h+lr
a→a+lr
其中，lr为学习速率，lr的取值范围为0.00001~0.01。
进一步，在步骤s2中，采用公式（5）~（6）对第二激光数据点集进行数据修正。
进一步，所述步骤s3具体包括以下步骤：
s31：将第一激光点集d01中距离r大于第三阀值的点去掉，得到点集d1；将第二激光点集d02中距离r大于第三阀值的点去掉，得到点集d2；
s32：将点集d1中的点投影到第一激光雷达的0度扫描线上，得到一个点列y1={y1,y2,…,yn}；
s33：将点列y1中第1个点取出，加入新建分组中；
s34：如果i≤n，则取出第i个点yi；否则，进行步骤s37；
s35：如果与前一个相邻点yi-1的距离小于第四阀值，将yi加入当前分组，并返回步骤s34，否则进行步骤s36；
s36：新建一个分组，并将新分组作为当前分组，返回步骤s34；
s37：将得到的所有分组进行比较，点个数最多的一个作为备选分组，备选分组对应的点即构成备选点集g1；
s38：对点集d2进行步骤s32~s37，得到备选点集g2，步骤s3结束。
进一步，所述步骤s5中所采用的拟合圆的方法为最小二乘法。
步骤s6包括以下步骤：
s61：将当前圆心ok加入到圆心点列c={o1,o2,…,ok-1}中；
s62：根据预设定值m，m的值为整数，是根据植株行的弯曲率决定的，m的大小与植株行的弯曲率成反比，从圆心点列c中取出后m个点，利用公式（3）~（4）计算出当前拟合直线y=kx+b，此拟合直线的斜率即为植株行的预测方向dir。
进一步，所述步骤s7包含以下步骤：
s71：对当前帧（第k帧）图像提取sift(scale-invariantfeaturetransform)特征点集fk0，并与前一帧图像特征点进行匹配，得到当前帧图像的关键点集fk={pk,1,pk,2,…,pk,m}；
s72：对fk中的点pk,i，计算出它在第k帧图像平面xoy中的坐标(xi,yi)和第k-1帧图像平面ucv中的坐标(ui,vi)；
s73：设su和sv分别代表第k帧图像平面xoy原点相对于第k-1帧图像平面ucv原点在u和v方向上的位移，θ代表第k帧图像平面xoy相对第k-1帧图像平面ucv的角度偏转，按下列公式估计su、sv和θ：
j=
其中，
s74：给定su、sv和θ的初值，根据下述公式进行迭代，j小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的θ，即为航向估计结果；
su←su+lr
sv←sv+lr
θ←θ+lr
其中，lr为学习速率。
进一步，述收获机器人为可对根茎和块茎类作物收获的收获机器人。
原则上，激光雷达的分辨率越高越好，考虑到分辨率越高的激光雷达成本越高，本方法在设计时考虑此因素，因此，实施例中采用低分辨率的激光雷达即可达到预期效果，同时，分辨率越高的激光雷达采集的数据点更多，自动跟随方法的稳定性越高；再有，实施例中采用的视觉传感器为rgb视觉传感器。
并且，激光雷达的个数不局限于两个，视觉传感器的个数也不局限于1个，上述设备个数适应增加，采用本方法自然可以达到更好的效果。
实施例中所述的第一阀值为误差，其具体值不大于1e-4，所述第二阀值为迭代的步数，其具体值不小于5000，所述第三阀值不大于第一激光雷达与第二激光雷达之间的安装距离的1/2，所述第四阀值根据激光雷达的精度来确定，若采用低分辨率激光雷达，则其小于5mm。而上述实施例中的l、h、a、su、sv和θ给定的初值为-0.1~0.1之间的随机值，其大小和正负不对技术方案产生影响。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！