[发明专利]一种收获机器人的植株行跟随方法

权利要求书

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(O_x,O_y)；

S6：通过获得的当前圆心坐标O(O_x,O_y)对植株行方向进行预测估算，得到植株行的当前方向估计Dir；

步骤S6包括以下步骤：

S61：将当前圆心O_k加入到圆心点列C={O₁,O₂,…,O_k-1}的末尾；

S62：根据预设定值M，M的值为整数，是根据植株行的弯曲率决定的，M的大小与植株行的弯曲率成反比，从圆心点列C中取出后M个点，利用：k=，b=；其中，=，=，=，=；计算出当前拟合直线y=kx+b，此拟合直线的斜率即为植株行的预测方向Dir；

S7：通过视觉传感器获得的图像帧，计算得到机器人的当前实际航向角估计值Raw和位移估计；

S8：通过航向角估计Raw、位移估计与方向预测Dir，得到机器人本体当前轨迹控制输入参数，并对航向进行调整以保证机器人沿当前植株轨迹方向动作。

2.根据权利要求1所述的一种收获机器人的植株行跟随方法，其特征在于，S1中的L、H和A通过以下方式计算：

S11：第一激光数据点集D01={（,）|i=1,2,…,N}，第二激光数据点集D02={（,）|i=1,2,…,M}；第一激光数据点集D01中数据点的极坐标为（,），利用公式

=cos() (1)

=sin() (2)

计算得到D1={（,）|i=1,2,…,N };

S12：计算拟合直线L1：y=kx+b，其中

k= (3)

b= (4)

其中，=，=，=，=；

S13：将D02通过以下公式进行极坐标换算得到数据集D2={（,）|i=1,2,…,M}；

=L-cos(+A) (5)

=H+sin(+A)(6)

S14：在数据集D2上，对L、H和A进行非线性优化：

(7)

= (8)

= (9)

= (10)

S15：给定L、H和A的初值，根据以下公式进行迭代，当J小于第一阀值或者迭代步数达到第二阀值时迭代结束，此时的L、H和A即为标定结果，

L←L + lr （11）

H← H + lr （12）

A← A + lr （13）

其中，lr为学习速率，lr的取值范围为0.00001~0.01。

3.根据权利要求2所述的一种收获机器人的植株行跟随方法，其特征在于，在步骤S2中，采用公式（5）~（6）对第二激光数据点集进行数据修正。