CN110125945B - 一种收获机器人的植株行跟随方法 - Google Patents

一种收获机器人的植株行跟随方法 Download PDF

Info

Publication number
CN110125945B
CN110125945B CN201811444693.8A CN201811444693A CN110125945B CN 110125945 B CN110125945 B CN 110125945B CN 201811444693 A CN201811444693 A CN 201811444693A CN 110125945 B CN110125945 B CN 110125945B
Authority
CN
China
Prior art keywords
point set
plant
laser
laser radar
point
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.)
Active
Application number
CN201811444693.8A
Other languages
English (en)
Other versions
CN110125945A (zh
Inventor
杨钦涌
杨金明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing Zhitian Technology Co ltd
Original Assignee
Chongqing Zhitian Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Chongqing Zhitian Technology Co ltd filed Critical Chongqing Zhitian Technology Co ltd
Priority to CN201811444693.8A priority Critical patent/CN110125945B/zh
Publication of CN110125945A publication Critical patent/CN110125945A/zh
Application granted granted Critical
Publication of CN110125945B publication Critical patent/CN110125945B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J11/00—Manipulators not otherwise provided for
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B25J19/021—Optical sensing devices
    • B25J19/022—Optical sensing devices using lasers
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Optics & Photonics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公布了一种收获机器人的植株行跟随方法，包括在收获机器人本体正前方两侧分别设有至少两个激光雷达，并在机器人本体后端设有至少一个视觉传感器，使用激光雷达实现准确的植株识别与定位，以及植株行的方向预测，使用视觉传感器实现本体的航向角估计，从而在无需地图和定位标志物辅助的情况下，实现机器人本体对植株行的实时跟随，同时，对激光数据的聚类和曲线拟合，可以有效提升机器人环境感知的抗噪能力和自主行走的可靠性。

Description

一种收获机器人的植株行跟随方法
技术领域
本发明涉及农业装备技术领域，具体涉及一种根茎和块茎作物收获机器人的植株行跟随方法。
背景技术
我国农村正面临劳动力短缺和人力成本不断上涨的困境，智能化作业装备成为农机装备技术研究中一个新热点，特别的，我国的萝卜、大蒜、莴苣、白菜等作物的种植面积大、收获机械化率低，而现有的收获装备大多是侧挂式。侧挂式收获机械需要外部提供动力，外部动力多由拖拉机来提供，拖拉机的重量较大，会对土壤形成巨大压力，破坏土壤结构，不利于作物生长。无论是侧挂式还是自走式收获机械，都需要操作人员时刻操纵拖拉机或收获机以对准收获对象作业，加大了操作难度，而且也增加了操作员的劳动强度。尤其在长期作业时，不仅会使得操作人员倍感疲劳，而且，也容易导致收获机械漏收和损伤率上升，鉴于此，一些农机企业和研究机构针对部分作物的收获实际需求，或者研制出一些具备部分智能的收获机械，亦或是研制收获机器人以及提供无人驾驶的拖拉机等。
目前，无论是智能收获机械，还是收获机器人或者无人驾驶拖拉机，在野外作业环境中的定位与导航，均基于卫星定位技术实现。如果需要提供高精度的定位与导航，这类卫星定位设备的成本较高，而且，它容易受到环境制约，比如山谷中的卫星信号较弱，即使加入惯性导航***等数据融合技术，也可能存在定位精度会受到地面起伏不平等干扰。另外，对于呈曲线的作物垄来讲，基于卫星导航的技术需要地图提供辅助，自动跟随植株行时的定位精度不高。特别是我国的农作物种植大多不规范，植株行呈曲线状、不规则，植株偏离行中心等现象普遍存在。即使使用无人驾驶拖拉机进行起垄，仍然不能假设植株行是线性的。
另外，还有一类场地移动机器人或室内移动机器人，或者无人驾驶汽车，使用SLAM（simultaneous localization and mapping）技术来进行定位与导航。这类技术往往成本很高或者存在应用局限性，而且，目前基于卫星定位和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={（
Figure 969724DEST_PATH_IMAGE001
,
Figure 893818DEST_PATH_IMAGE002
）|i=1,2,…,N}，第二激光数据点集D02={（
Figure 731324DEST_PATH_IMAGE003
,
Figure 563014DEST_PATH_IMAGE004
）|i=1,2,…,M}；第一激光数据点集D01中数据点的极坐标为（
Figure 67944DEST_PATH_IMAGE001
,
Figure 959677DEST_PATH_IMAGE002
），利用公式
Figure 284479DEST_PATH_IMAGE005
=
Figure 123122DEST_PATH_IMAGE001
cos(
Figure 544876DEST_PATH_IMAGE002
) (1)
Figure 545193DEST_PATH_IMAGE006
=
Figure 419608DEST_PATH_IMAGE001
sin(
Figure 530784DEST_PATH_IMAGE002
) (2)
计算得到D1={（
Figure 807044DEST_PATH_IMAGE005
,
Figure 135520DEST_PATH_IMAGE006
）|i=1,2,…,N }；
S12：计算拟合直线L1：y=kx+b，其中
k=
Figure 966072DEST_PATH_IMAGE007
(3)
b=
Figure 349780DEST_PATH_IMAGE008
(4)
其中，
Figure 683810DEST_PATH_IMAGE009
=
Figure 790044DEST_PATH_IMAGE010
，
Figure 842313DEST_PATH_IMAGE011
=
Figure 826450DEST_PATH_IMAGE012
，
Figure 14986DEST_PATH_IMAGE013
=
Figure 793586DEST_PATH_IMAGE014
，
Figure 333152DEST_PATH_IMAGE015
=
Figure 120979DEST_PATH_IMAGE016
；
S13：将D02通过以下公式进行极坐标换算得到数据集D2={（
Figure 429601DEST_PATH_IMAGE017
,
Figure 880567DEST_PATH_IMAGE018
）|i=1,2,…,M}；
Figure 907429DEST_PATH_IMAGE017
=L-
Figure 967789DEST_PATH_IMAGE003
cos(
Figure 130917DEST_PATH_IMAGE019
+A) (5)
Figure 251320DEST_PATH_IMAGE018
=H+
Figure 499898DEST_PATH_IMAGE003
sin(
Figure 629528DEST_PATH_IMAGE019
+A) (6)
S14：在数据集D2上，对L、H和A进行非线性优化：
J=
Figure 912742DEST_PATH_IMAGE020
(7)
Figure 702581DEST_PATH_IMAGE021
=
Figure 438456DEST_PATH_IMAGE022
(8)
Figure 840619DEST_PATH_IMAGE023
=
Figure 243918DEST_PATH_IMAGE024
(9)
Figure 706123DEST_PATH_IMAGE025
=
Figure 929294DEST_PATH_IMAGE026
(10)
S15：给定L、H和A的初值，根据以下公式进行迭代，当J小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的L、H和A即为标定结果，
L → L + lr
Figure 338410DEST_PATH_IMAGE021
（11）
H → H + lr
Figure 820383DEST_PATH_IMAGE023
（12）
A → A + lr
Figure 719069DEST_PATH_IMAGE025
（13）
其中，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=
Figure 429536DEST_PATH_IMAGE027
(14)
Figure 173501DEST_PATH_IMAGE028
=-
Figure 285814DEST_PATH_IMAGE029
(15)
Figure 89821DEST_PATH_IMAGE030
=-
Figure 553164DEST_PATH_IMAGE031
(16)
Figure 835241DEST_PATH_IMAGE032
=
Figure 300595DEST_PATH_IMAGE033
(17)
其中，
Figure 541083DEST_PATH_IMAGE034
，
Figure 960563DEST_PATH_IMAGE035
，
Figure 311910DEST_PATH_IMAGE036
，
Figure 133236DEST_PATH_IMAGE037
，
Figure 544625DEST_PATH_IMAGE038
，
S74：给定SU、SV和θ的初值，根据下述公式进行迭代，J小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的θ，即为航向估计结果；
SU ← SU + lr
Figure 451402DEST_PATH_IMAGE028
（18）
SV ← SV + lr
Figure 340860DEST_PATH_IMAGE030
（19）
θ ← θ+ lr
Figure 49315DEST_PATH_IMAGE032
（20）
其中，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={（
Figure 631606DEST_PATH_IMAGE001
,
Figure 25679DEST_PATH_IMAGE002
）|i=1,2,…,N}，第二激光数据点集D02={（
Figure 718828DEST_PATH_IMAGE003
,
Figure 983587DEST_PATH_IMAGE004
）|i=1,2,…,M}；第一激光数据点集D01中数据点的极坐标为（
Figure 736780DEST_PATH_IMAGE001
,
Figure 618148DEST_PATH_IMAGE002
），利用公式
Figure 114989DEST_PATH_IMAGE005
=
Figure 263948DEST_PATH_IMAGE001
cos(
Figure 922462DEST_PATH_IMAGE002
) (1)
Figure 291127DEST_PATH_IMAGE006
=
Figure 326079DEST_PATH_IMAGE001
sin(
Figure 96589DEST_PATH_IMAGE002
) (2)
计算得到D1={（
Figure 926005DEST_PATH_IMAGE005
,
Figure 47544DEST_PATH_IMAGE006
）|i=1,2,…,N }；
S12：计算拟合直线L1：y=kx+b，其中
k=
Figure 620608DEST_PATH_IMAGE007
(3)
b=
Figure 481510DEST_PATH_IMAGE008
(4)
其中，
Figure 544144DEST_PATH_IMAGE009
=
Figure 825084DEST_PATH_IMAGE010
，
Figure 467418DEST_PATH_IMAGE011
=
Figure 743678DEST_PATH_IMAGE012
，
Figure 180476DEST_PATH_IMAGE013
=
Figure 745449DEST_PATH_IMAGE014
，
Figure 925895DEST_PATH_IMAGE015
=
Figure 994345DEST_PATH_IMAGE016
；
S13：将D02通过以下公式进行极坐标换算得到数据集D2={（
Figure 398782DEST_PATH_IMAGE017
,
Figure 684007DEST_PATH_IMAGE018
）|i=1,2,…,M}；
Figure 402565DEST_PATH_IMAGE017
=L-
Figure 856680DEST_PATH_IMAGE003
cos(
Figure 635280DEST_PATH_IMAGE019
+A) (5)
Figure 174846DEST_PATH_IMAGE018
=H+
Figure 493831DEST_PATH_IMAGE003
sin(
Figure 802453DEST_PATH_IMAGE019
+A) (6)
S14：在数据集D2上，对L、H和A进行非线性优化：
J=
Figure 751954DEST_PATH_IMAGE020
(7)
Figure 247658DEST_PATH_IMAGE021
=
Figure 370335DEST_PATH_IMAGE022
(8)
Figure 34928DEST_PATH_IMAGE023
=
Figure 155330DEST_PATH_IMAGE024
(9)
Figure 872751DEST_PATH_IMAGE025
=
Figure 736801DEST_PATH_IMAGE026
(10)
S15：给定L、H和A的初值，根据以下公式进行迭代，当J小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的L、H和A即为标定结果，
L → L + lr
Figure 551174DEST_PATH_IMAGE021
（11）
H → H + lr
Figure 311319DEST_PATH_IMAGE023
（12）
A → A + lr
Figure 843932DEST_PATH_IMAGE025
（13）
其中，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-invariant feature transform)特征点集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=
Figure 511673DEST_PATH_IMAGE027
(14)
Figure 882350DEST_PATH_IMAGE028
=-
Figure 344555DEST_PATH_IMAGE029
(15)
Figure 567726DEST_PATH_IMAGE030
=-
Figure 773579DEST_PATH_IMAGE031
(16)
Figure 562544DEST_PATH_IMAGE032
=
Figure 195650DEST_PATH_IMAGE033
(17)
其中，
Figure 640538DEST_PATH_IMAGE034
，
Figure 446820DEST_PATH_IMAGE035
，
Figure 293554DEST_PATH_IMAGE036
，
Figure 894299DEST_PATH_IMAGE037
，
Figure 826483DEST_PATH_IMAGE038
，
S74：给定SU、SV和θ的初值，根据下述公式进行迭代，J小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的θ，即为航向估计结果；
SU ← SU + lr
Figure 170877DEST_PATH_IMAGE028
（18）
SV ← SV + lr
Figure 385300DEST_PATH_IMAGE030
（19）
θ ← θ+ lr
Figure 360209DEST_PATH_IMAGE032
（20）
其中，lr为学习速率。
进一步，述收获机器人为可对根茎和块茎类作物收获的收获机器人。
原则上，激光雷达的分辨率越高越好，考虑到分辨率越高的激光雷达成本越高，本方法在设计时考虑此因素，因此，实施例中采用低分辨率的激光雷达即可达到预期效果，同时，分辨率越高的激光雷达采集的数据点更多，自动跟随方法的稳定性越高；再有，实施例中采用的视觉传感器为RGB视觉传感器。
并且，激光雷达的个数不局限于两个，视觉传感器的个数也不局限于1个，上述设备个数适应增加，采用本方法自然可以达到更好的效果。
实施例中所述的第一阀值为误差，其具体值不大于1E-4，所述第二阀值为迭代的步数，其具体值不小于5000，所述第三阀值不大于第一激光雷达与第二激光雷达之间的安装距离的1/2，所述第四阀值根据激光雷达的精度来确定，若采用低分辨率激光雷达，则其小于5mm。而上述实施例中的L、H、A、SU、SV和θ给定的初值为-0.1~0.1之间的随机值，其大小和正负不对技术方案产生影响。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种收获机器人的植株行跟随方法，其特征在于，在收获机器人本体正前方两侧分别设有至少两个激光雷达，并在机器人本体后端设有至少一个视觉传感器，具体通过以下步骤实现：
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；
步骤S6包括以下步骤：
S61：将当前圆心Ok加入到圆心点列C={O1,O2,…,Ok-1}的末尾；
S62：根据预设定值M，M的值为整数，是根据植株行的弯曲率决定的，M的大小与植株行的弯曲率成反比，从圆心点列C中取出后M个点，利用：k=
Figure DEST_PATH_IMAGE002
，b=
Figure DEST_PATH_IMAGE004
；其中，
Figure DEST_PATH_IMAGE006
=
Figure DEST_PATH_IMAGE008
，
Figure DEST_PATH_IMAGE010
=
Figure DEST_PATH_IMAGE012
，
Figure DEST_PATH_IMAGE014
=
Figure DEST_PATH_IMAGE016
，
Figure DEST_PATH_IMAGE018
=
Figure DEST_PATH_IMAGE020
；计算出当前拟合直线y=kx+b，此拟合直线的斜率即为植株行的预测方向Dir；
S7：通过视觉传感器获得的图像帧，计算得到机器人的当前实际航向角估计值Raw和位移估计；
S8：通过航向角估计Raw、位移估计与方向预测Dir，得到机器人本体当前轨迹控制输入参数，并对航向进行调整以保证机器人沿当前植株轨迹方向动作。
2.根据权利要求1所述的一种收获机器人的植株行跟随方法，其特征在于，S1中的L、H和A通过以下方式计算：
S11：第一激光数据点集D01={（
Figure DEST_PATH_IMAGE022
,
Figure DEST_PATH_IMAGE024
）|i=1,2,…,N}，第二激光数据点集D02={（
Figure DEST_PATH_IMAGE026
,
Figure DEST_PATH_IMAGE028
）|i=1,2,…,M}；第一激光数据点集D01中数据点的极坐标为（
Figure DEST_PATH_IMAGE022A
,
Figure DEST_PATH_IMAGE024A
），利用公式
Figure DEST_PATH_IMAGE032
=
Figure DEST_PATH_IMAGE022AA
cos(
Figure DEST_PATH_IMAGE024AA
) (1)
Figure DEST_PATH_IMAGE036
=
Figure DEST_PATH_IMAGE022AAA
sin(
Figure DEST_PATH_IMAGE024AAA
) (2)
计算得到D1={（
Figure DEST_PATH_IMAGE032A
,
Figure DEST_PATH_IMAGE036A
）|i=1,2,…,N };
S12：计算拟合直线L1：y=kx+b，其中
k=
Figure DEST_PATH_IMAGE002A
(3)
b=
Figure DEST_PATH_IMAGE004A
(4)
其中，
Figure DEST_PATH_IMAGE006A
=
Figure DEST_PATH_IMAGE008A
，
Figure DEST_PATH_IMAGE010A
=
Figure DEST_PATH_IMAGE012A
，
Figure DEST_PATH_IMAGE014A
=
Figure DEST_PATH_IMAGE016A
，
Figure DEST_PATH_IMAGE018A
=
Figure DEST_PATH_IMAGE020A
；
S13：将D02通过以下公式进行极坐标换算得到数据集D2={（
Figure DEST_PATH_IMAGE042
,
Figure DEST_PATH_IMAGE044
）|i=1,2,…,M}；
Figure DEST_PATH_IMAGE042A
=L-
Figure DEST_PATH_IMAGE026A
cos(
Figure DEST_PATH_IMAGE048
+A) (5)
Figure DEST_PATH_IMAGE044A
=H+
Figure DEST_PATH_IMAGE026AA
sin(
Figure DEST_PATH_IMAGE048A
+A)(6)
S14：在数据集D2上，对L、H和A进行非线性优化：
Figure DEST_PATH_IMAGE053
(7)
Figure DEST_PATH_IMAGE055
=
Figure DEST_PATH_IMAGE057
(8)
Figure DEST_PATH_IMAGE059
=
Figure DEST_PATH_IMAGE061
(9)
Figure DEST_PATH_IMAGE063
=
Figure DEST_PATH_IMAGE065
(10)
S15：给定L、H和A的初值，根据以下公式进行迭代，当J小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的L、H和A即为标定结果，
L←L + lr
Figure DEST_PATH_IMAGE055A
（11）
H← H + lr
Figure DEST_PATH_IMAGE059A
（12）
A← A + lr
Figure DEST_PATH_IMAGE063A
（13）
其中，lr为学习速率，lr的取值范围为0.00001~0.01。
3.根据权利要求2所述的一种收获机器人的植株行跟随方法，其特征在于，在步骤S2中，采用公式（5）~（6）对第二激光数据点集进行数据修正。
4.根据权利要求3所述的一种收获机器人的植株行跟随方法，其特征在于，所述步骤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结束。
5.根据权利要求1所述的一种收获机器人的植株行跟随方法，其特征在于，所述步骤S5中所采用的圆拟合的方法为最小二乘法。
6.根据权利要求1所述的一种收获机器人的植株行跟随方法，其特征在于，所述步骤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=
Figure DEST_PATH_IMAGE070
(14)
Figure DEST_PATH_IMAGE072
= -
Figure DEST_PATH_IMAGE074
(15)
Figure DEST_PATH_IMAGE076
= -
Figure DEST_PATH_IMAGE078
(16)
Figure DEST_PATH_IMAGE080
=
Figure DEST_PATH_IMAGE082
(17)
其中，
Figure DEST_PATH_IMAGE084
，
Figure DEST_PATH_IMAGE086
，
Figure DEST_PATH_IMAGE088
，
Figure DEST_PATH_IMAGE090
，
Figure DEST_PATH_IMAGE092
，
S74：给定SU、SV和θ的初值，根据下述公式进行迭代，J小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的θ，即为航向估计结果；
SU ← SU + lr
Figure DEST_PATH_IMAGE072A
（18）
SV ← SV + lr
Figure DEST_PATH_IMAGE076A
（19）
θ ← θ+ lr
Figure DEST_PATH_IMAGE080A
（20）
其中，lr为学习速率。
7.根据权利要求1所述的一种收获机器人的植株行跟随方法，其特征在于，所述收获机器人为可对根茎和块茎类作物收获的收获机器人。
CN201811444693.8A 2018-11-29 2018-11-29 一种收获机器人的植株行跟随方法 Active CN110125945B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811444693.8A CN110125945B (zh) 2018-11-29 2018-11-29 一种收获机器人的植株行跟随方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811444693.8A CN110125945B (zh) 2018-11-29 2018-11-29 一种收获机器人的植株行跟随方法

Publications (2)

Publication Number Publication Date
CN110125945A CN110125945A (zh) 2019-08-16
CN110125945B true CN110125945B (zh) 2022-04-26

Family

ID=67568203

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811444693.8A Active CN110125945B (zh) 2018-11-29 2018-11-29 一种收获机器人的植株行跟随方法

Country Status (1)

Country Link
CN (1) CN110125945B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111267093A (zh) * 2019-11-05 2020-06-12 广州霞光技研有限公司 一种scara机器人连续轨迹控制实现方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103231708B (zh) * 2013-04-12 2015-08-05 安徽工业大学 一种基于双目视觉的智能车辆避障方法
CN103954234B (zh) * 2014-04-29 2018-01-05 南京理工大学 一种车辆轮廓及轴距自标定测量算法
CN111163628A (zh) * 2017-05-09 2020-05-15 蓝河技术有限公司 使用图像数据的自动植物检测
CN107561547B (zh) * 2017-08-14 2020-05-12 广州供电局有限公司 输电线路到目标物的距离测量方法、装置及***
CN107590827A (zh) * 2017-09-15 2018-01-16 重庆邮电大学 一种基于Kinect的室内移动机器人视觉SLAM方法

Also Published As

Publication number Publication date
CN110125945A (zh) 2019-08-16

Similar Documents

Publication Publication Date Title
US11991940B2 (en) Method for autonomous detection of crop location based on tool depth and location
US10874044B2 (en) Real-time field mapping for autonomous agricultural platform
Gai et al. Using a depth camera for crop row detection and mapping for under-canopy navigation of agricultural robotic vehicle
Shalal et al. Orchard mapping and mobile robot localisation using on-board camera and laser scanner data fusion–Part B: Mapping and localisation
Li et al. Review of research on agricultural vehicle autonomous guidance
Mousazadeh A technical review on navigation systems of agricultural autonomous off-road vehicles
Steward et al. The use of agricultural robots in weed management and control
Meng et al. Development of agricultural implement system based on machine vision and fuzzy control
Nørremark et al. The development and assessment of the accuracy of an autonomous GPS-based system for intra-row mechanical weed control in row crops
Tillett et al. A robotic system for plant-scale husbandry
Shalal et al. A review of autonomous navigation systems in agricultural environments
Subramanian et al. Sensor fusion using fuzzy logic enhanced kalman filter for autonomous vehicle guidance in citrus groves
CN103186773A (zh) 一种基于一维Hough变换和专家***的早期苗田垄线识别算法
Velasquez et al. Multi-sensor fusion based robust row following for compact agricultural robots
Zhang et al. Factor graph-based high-precision visual positioning for agricultural robots with fiducial markers
CN110125945B (zh) 一种收获机器人的植株行跟随方法
Hutsol et al. Robotic technologies in horticulture: analysis and implementation prospects
de Silva et al. Leaving the lines behind: Vision-based crop row exit for agricultural robot navigation
Wang et al. Research on visual navigation and remote monitoring technology of agricultural robot
Hameed et al. Task and motion planning for selective weed conrol using a team of autonomous vehicles
EP2741161A2 (de) Selbstfahrendes Bodenbearbeitungsgerät und Verfahren zur Positionsbestimmung bei einem selbstfahrenden Bodenbearbeitungsgerät
Pradhan et al. Robotic Seeding or Sowing System in Smart Agriculture
CN117053808B (zh) 一种用于大田作物种植环境下的农机具自动导航方法
Wu et al. A navigation method for paddy field management based on seedlings coordinate information
Arif et al. COMPUTER VISION BASED NAVIGATION MODULE FOR SUSTAINABLE BROAD-ACRE AGRICULTURE ROBOTS.

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