[0001] 本发明涉及农业设备技术领域，尤其是一种用于农作物打顶的机械臂补偿方法。

[0002] 基于棉花打顶的农业场景，通常需要用到棉花打顶设备，现有一种智能化的棉花打顶设备，通过在棉花打顶设备上设置相机，并使相机处于水平，以俯视的视角朝下拍摄农作物，棉花打顶设备上的阵列式机械臂模组也处于水平面，因此，相机与机械臂刀头高度差为已知数据，根据相机所拍摄的图像检测棉花顶芽所在位置结合深度传感器得出顶芽相对相机的三维位置，再经过一定里程后该位置处于机械臂下，使用机械臂对其切割，这样的前提是相机距离顶芽的高度与机械臂相对顶芽的高度一致。

[0003] 但是这样的结构通常使用农用拖拉机拉拽行驶，在行驶过程中难免导致相机高机械臂低或者相机低机械臂高这样的状况，这就导致相机在t时刻检测到的顶芽相对相机的位置，到了t+1时刻顶芽处于机械臂下方时，顶芽相对机械臂刀头就是未知的高度差。

[0029] 为使得本发明的目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

[0030] 实施例1

[0031] 本实施例提供一种用于农作物打顶的机械臂补偿方法，以计算补偿棉花打顶设备机械臂切割位置的距离，具体包括以下步骤：

[0032] S1、利用棉花打顶设备上设置的彩色相机与深度相机俯视视角朝下拍摄棉花顶部，同时棉花打顶设备移动，在多个棉花顶部经过；

[0033] 相机架设在一个移动车架上，车架在地里行驶，所以当相机所在平面水平时同一棉花顶芽到相机的竖直距离在不同时刻基本不变，而如果相机平面不水平则同一顶芽到相机竖直距离在不同时刻会有差异，从而机械臂在做切割时也会受到该差异影响；

[0034] S2、使用基于深度学习的关键点检测算法yolo‑pose，在彩色图像上检测顶芽，在拍摄的图像中得到棉花顶芽所在位置点数据，即顶芽在彩色图像的位置T；

[0035] S3、利用该位置T和事先标定好的彩色图与深度图的位置变换关系，找到彩色图的T位置在深度图对应的深度D，基于事先标定的彩色相机内参，取内参矩阵的逆乘以T坐标再乘以深度D得到顶芽在相机坐标系的位置Tc；

[0036] 具体地，事先标定好的彩色图与深度图的位置变换关系通过相机标定的方法来实现，相机标定是确定相机内参和外参的过程，以便将彩色图像中的像素位置映射到三维空间中，在这种情况下，需要将彩色图像上的顶芽位置映射到深度图像中对应的深度值，具体步骤如下：

[0037] Sa1、使用相机标定板或其他特定模式，拍摄一系列彩色图像和对应的深度图像，以获取相机的内参矩阵，包括相机焦距与主点坐标，和外参矩阵，即相机位置和姿态，用于将彩色图像的像素坐标转换为相机坐标系中的三维坐标；

[0038] 其中，内参矩阵包括相机的焦距(fx、fy)、主点坐标(cx、cy)参数，表示为：

[0039] [K＝\begin{bmatrix}f_x&0&c_x\0&f_y&c_y\0&0&1\end{bmatrix}][0040] 其中，(f_x)和(f_y)是相机在x和y方向上的焦距，(c_x)和(c_y)是主点在图像上的坐标；

[0041] 外参矩阵包括相机的位置和姿态信息，表示为旋转矩阵(R)和平移向量(T)的组合：

[0042] [[R|T]＝\begin{bmatrix}r_{11}&r_{12}&r_{13}&t_x\r_{21}&r_{22}&r_{23}&t_y\r_{31}&r_{32}&r_{33}&t_z\end{bmatrix}]

[0043] 其中，(R)是旋转矩阵，(T)是平移向量，用于描述相机的姿态和位置；

[0044] 获取内参矩阵(K)和外参矩阵([R|T])的具体数值，通过相机标定过程获得；

[0045] 以检测到的顶芽位置(T＝(x,y))为例，将其转换为相机坐标系中的归一化坐标(\bar{X}＝(u,v,1))：[\bar{X}＝K^{‑1}\cdot[x,y,1]^T]

[0046] 通过深度图像中的深度值(D)，将像素坐标(\bar{X})转换为相机坐标系中的三维坐标：[P＝D\cdot R^{‑1}\cdot K^{‑1}\cdot[x,y,1]^T]；

[0047] Sa2、根据相机标定得到的内参和外参矩阵，建立彩色图与深度图之间的对应关系，通过像素坐标的转换，将彩色图像中的像素位置映射到深度图像中对应的像素位置；

[0048] Sa3、利用彩色图中检测到的顶芽位置T，在深度图中找到对应的深度值D，即彩色图的T位置在深度图中对应的深度值；

[0049] Sa4、根据事先标定的彩色相机内参矩阵的逆，将彩色图像中的像素位置T乘以深度值D，即可得到顶芽在相机坐标系中的位置Tc；

[0050] S4、使用跟踪方法sort将多帧之间同一棉花顶芽关联起来用同一ID表示；

[0051] 使用跟踪方法SORT将多帧之间同一棉花顶芽关联起来用同一ID表示的具体过程如下：

[0052] 对于每一帧的检测结果，为每个检测框分配一个唯一的ID，作为标识符；

[0053] 针对当前帧的检测结果，通过跟踪算法进行预测，根据上一帧的跟踪结果，预测当前帧中每个目标的位置；

[0054] 利用邻算法进行数据关联，将当前帧的检测结果与上一帧的跟踪结果进行匹配；

[0055] 根据数据关联的结果，更新每个目标的状态信息；

[0056] 将在多帧之间被匹配到的同一目标用相同的ID表示，以便在后续帧中持续追踪该目标；

[0057] 处理遮挡或目标消失的情况，需要通过策略来预测目标的轨迹；

[0058] 将跟踪结果输出，包括每个目标的位置、速度和唯一ID信息，用于后续分析或控制操作；

[0059] S5、使用arttan((z1‑z2)/(y1‑y2))计算出相机相对水平面的俯仰角，同一棉花顶芽在多帧间得到了不同的坐标，坐标是相机坐标系下，xy是水平面，z是顶芽到相机的竖直距离，其中z1表示某一帧棉花顶芽z坐标，z2表示同一顶芽另一帧的z坐标，y1表示某一帧棉花顶芽y坐标，y2表示同一顶芽另一帧的y坐标；

[0060] 得到相机相对水平面的θ角，同样也是机械臂模组相对水平面的θ角；

[0061] S6、获取t时刻相机获取到的T顶芽位置，并设定为点A，获取t时刻开始车架行驶了d距离后，T顶芽所在位置，并设定为点C；

[0062] “t时刻”为S5中得到的相机获取到的顶芽位置的时间点，即“t时刻”为S5中计算出的俯仰角θ后，在车架行驶了距离d后所对应的时间点；

[0063] S7、根据车架的水平状态，计算出需要补偿水平距离w和竖直距离h；

[0064] 当车架不水平时，需要在预估切割位置B点处水平补偿w距离，高度补偿h距离，通过步骤S5得到了θ，根据三角函数，h＝d*sin(θ)，w＝d‑d*cos(θ)；

[0065] 如果车架水平，则会以为T顶芽在B位置。所以需要计算出车架不水平时需要补偿水平距离w和竖直距离h。

[0066] 一种农作物打顶设备，包括机架100与视觉识别组件500，所述视觉识别组件500固定安装于机架100侧壁位置，所述视觉识别组件500用于棉花顶空间位置的识别；

[0067] 所述机架100侧壁位置固定连接有用于双目识别摄像头502安装承载的安装底座501，所述安装底座501的顶端固定连接有集线座503，所述集线座503用于双目识别摄像头
502上线缆的整理及防护。

[0068] 一种存储介质，包括总控机，其上设计有计算机程序，其上存储有计算机程序指令，当计算机程序指令被执行时实现上述用于农作物打顶的机械臂补偿方法。

[0069] 本实施例结合了深度学习的关键点检测、相机标定、跟踪算法以及补偿计算等技术，旨在实现对棉花顶芽位置的精确定位和自动化切割操作，具体讲述本实施例实现了以下技术效果：

[0070] 通过使用基于深度学习的关键点检测算法YOLO‑Pose，结合相机标定的内参和外参，能够准确地检测和定位棉花顶芽在彩色图像中的位置，并将其映射到相机坐标系中的三维位置；使用跟踪方法SORT，在多帧之间实现对同一棉花顶芽的关联跟踪，并为每个目标分配唯一的ID，确保在连续帧中对目标进行持续追踪；根据相机相对水平面的俯仰角、车架行驶距离等信息，计算出需要补偿的水平距离和竖直距离，以确保在车架不水平的情况下，对棉花顶芽进行准确的切割操作；结合上述技术效果，实现了对棉花顶芽位置的精确控制和自动化切割操作，提高了作业的精准度和效率，减少了人工干预和操作成本。

[0071] 实施例2

[0072] 本实施例提供一种用于农作物打顶的机械臂补偿方法，在使用跟踪方法SORT将多帧之间同一棉花顶芽关联起来用同一ID表示时，通过策略来预测目标的轨迹，处理遮挡或目标消失的情况；具体步骤如下：

[0073] S1、利用棉花打顶设备上设置的彩色相机与深度相机俯视视角朝下拍摄棉花顶部，同时棉花打顶设备移动，在多个棉花顶部经过；

[0074] 相机架设在一个移动车架上，车架在地里行驶，所以当相机所在平面水平时同一棉花顶芽到相机的竖直距离在不同时刻基本不变，而如果相机平面不水平则同一顶芽到相机竖直距离在不同时刻会有差异，从而机械臂在做切割时也会受到该差异影响；

[0075] S2、使用基于深度学习的关键点检测算法yolo‑pose，在彩色图像上检测顶芽，在拍摄的图像中得到棉花顶芽所在位置点数据，即顶芽在彩色图像的位置T；

[0076] S3、利用该位置T和事先标定好的彩色图与深度图的位置变换关系，找到彩色图的T位置在深度图对应的深度D，基于事先标定的彩色相机内参，取内参矩阵的逆乘以T坐标再乘以深度D得到顶芽在相机坐标系的位置Tc；

[0077] S4、使用跟踪方法sort将多帧之间同一棉花顶芽关联起来用同一ID表示；

[0078] 使用跟踪方法SORT将多帧之间同一棉花顶芽关联起来用同一ID表示的具体过程如下：

[0079] 对于每一帧的检测结果，为每个检测框分配一个唯一的ID，作为标识符；

[0080] 针对当前帧的检测结果，通过跟踪算法进行预测，根据上一帧的跟踪结果，预测当前帧中每个目标的位置；

[0081] 利用邻算法进行数据关联，将当前帧的检测结果与上一帧的跟踪结果进行匹配；

[0082] 根据数据关联的结果，更新每个目标的状态信息；

[0083] 将在多帧之间被匹配到的同一目标用相同的ID表示，以便在后续帧中持续追踪该目标；

[0084] 处理遮挡或目标消失的情况，需要通过策略来预测目标的轨迹；

[0085] 引入卡尔曼滤波器预测目标的轨迹，具体为为：

[0086] ①卡尔曼滤波器初始化：

[0087] 初始化目标的状态向量(X)和状态协方差矩阵(P)。

[0088] 初始化过程噪声协方差矩阵(Q)和观测噪声协方差矩阵(R)。

[0089] ②预测步骤：

[0090] 使用状态转移矩阵(A)和控制输入向量(B)对目标的状态向量进行预测：[\hat{X}^‑＝A\cdot X+B\cdot U]

[0091] 预测目标的状态协方差矩阵：[P^‑＝A\cdot P\cdotA^T+Q]

[0092] ③更新步骤：

[0093] 计算卡尔曼增益(K)：[K＝P^‑\cdot H^T\cdot(H\cdotP^‑\cdot H^T+R)^{‑1}][0094] 更新目标的状态向量和状态协方差矩阵：[X＝\hat{X}^‑+K\cdot(Z‑H\cdot\hat{X}^‑)][P＝(I‑K\cdot H)\cdot P^‑]；假设在某一帧中，目标的状态向量为(X＝[x,y,\dot{x},\dot{y}])，观测到的目标位置为(Z＝[z_x,z_y])；

[0095] 设定状态转移矩阵(A)、观测矩阵(H)、过程噪声协方差矩阵(Q)和观测噪声协方差矩阵(R)的具体数值；

[0096] 将跟踪结果输出，包括每个目标的位置、速度和唯一ID信息，用于后续分析或控制操作；

[0097] S5、使用arttan((z1‑z2)/(y1‑y2))计算出相机相对水平面的俯仰角，同一棉花顶芽在多帧间得到了不同的坐标，坐标是相机坐标系下，xy是水平面，z是顶芽到相机的竖直距离，其中z1表示某一帧棉花顶芽z坐标，z2表示同一顶芽另一帧的z坐标，y1表示某一帧棉花顶芽y坐标，y2表示同一顶芽另一帧的y坐标；

[0098] S6、获取t时刻相机获取到的T顶芽位置，并设定为点A，获取t时刻开始车架行驶了d距离后，T顶芽所在位置，并设定为点C；

[0099] “t时刻”为S5中得到的相机获取到的顶芽位置的时间点，即“t时刻”为S5中计算出的俯仰角θ后，在车架行驶了距离d后所对应的时间点；

[0100] S7、根据车架的水平状态，计算出需要补偿水平距离w和竖直距离h；

[0101] 当车架不水平时，需要在预估切割位置B点处水平补偿w距离，高度补偿h距离，通过步骤S5得到了θ，根据三角函数，h＝d*sin(θ)，w＝d‑d*cos(θ)；

[0102] 如果车架水平，则会以为T顶芽在B位置。所以需要计算出车架不水平时需要补偿水平距离w和竖直距离h。

[0103] 在本实施例中，通过引入卡尔曼滤波器预测目标的轨迹，结合使用跟踪方法SORT对多帧之间同一棉花顶芽进行关联和持续追踪，以处理遮挡或目标消失的情况：

[0104] 通过卡尔曼滤波器对目标的轨迹进行预测，利用状态转移矩阵和观测矩阵，结合过程噪声和观测噪声协方差矩阵，实现对目标位置和速度的准确预测；

[0105] 利用跟踪方法SORT对多帧之间的同一棉花顶芽进行关联，为每个目标分配唯一的ID，通过数据关联和更新，实现目标的持续追踪和标识；

[0106] 通过预测目标轨迹和更新状态信息，处理目标遮挡或消失的情况，确保目标在连续帧中的准确追踪和轨迹更新，提高跟踪算法的鲁棒性和准确性；

[0107] 根据车架状态和车架行驶距离，计算需要的水平和竖直补偿距离，实现对棉花顶芽位置的精确控制和切割操作，提高了作业的精准度和效率。