一种橡胶树的起割点检测系统及检测方法

摘要

本发明涉及农业机器人领域，具体涉及一种橡胶树的起割点检测系统及检测方法，包括末端执行器(4)、机械臂本体(5)、底座(6)、激光定位灯(7)和下摄像头(8)；所述机械臂本体(5)通过底座(6)安装在移动车体上；底座(6)为移动式，能够在多棵橡胶树(1)之间移动，承载机械臂本体(5)并为其提供动力；末端执行器(4)通过法兰(402)与机械臂本体(5)的末端连接；激光定位灯(7)安装在机械臂本体(5)的第二关节中心线上，指向机械臂本体(5)前方，下摄像头(8)布置在机械臂本体(5)上，向下俯视30°。本发明有机结合协作机器人的高精度和图像处理系统的灵活性，实现了橡胶树起割点的自动化检测，智能化程度高。

权利要求

1.一种利用橡胶树的起割点检测系统进行橡胶树的起割点检测的检测方法，其中，橡胶树的起割点检测系统，用于对橡胶树(1)的起割点进行检测；在橡胶树(1)的起割点Ps和中割点Pm处沿着割线竖直布置起割点标记(2)和中割点标记(3)；所述起割点检测系统包括末端执行器(4)、机械臂本体(5)、底座(6)、激光定位灯(7)和下摄像头(8)；

所述机械臂本体(5)通过底座(6)安装在移动车体上；移动车体上安装有工控机；底座(6)为移动式，能够在多棵橡胶树(1)之间移动，承载机械臂本体(5)并为其提供动力；

末端执行器(4)通过法兰(402)与机械臂本体(5)的末端连接；

末端执行器(4)包括上激光测距传感器(401)、法兰(402)、下激光测距传感器(403)、刀片(404)和眼摄像头(405)；上激光测距传感器(401)和下激光测距传感器(403)分别安装在刀片(404)的上方和下方，上激光测距传感器(401)和下激光测距传感器(403)的激光的发射方向垂直于法兰(402)的法兰面向外；上激光测距传感器(401)和下激光测距传感器(403)的激光点和刀片(404)的刀尖点在竖直方向上三点共线；上激光测距传感器(401)和下激光测距传感器(403)检测深度信息，即末端执行器(4)的进刀量f；眼摄像头(405)安装在刀片(404)的运动方向的前方，眼摄像头(405)的光轴与刀片(404)的刀尖点位于同一水平线上；

激光定位灯(7)安装在机械臂本体(5)的第二关节中心线上，指向机械臂本体(5)前方，即橡胶树(1)的方向；下摄像头(8)布置在机械臂本体(5)的位于第二关节上方的第三关节上，向下俯视30°，其高度能够捕捉到橡胶树割线，即橡胶树割线位置在下摄像头(8)的有效视野范围内；

其特征在于：所述方法包括以下步骤：

1)底座(6)在工控机的控制下移动至橡胶树(1)的正前方，机械臂本体(5)对准橡胶树(1)，激光定位灯(7)对准橡胶树(1)的中心轴线；在开始割胶作业前，人工测量橡胶树(1)的树干周长C；根据公式1计算树干半径R和割线导程P；

其中，R为树干半径，单位为mm；C为树干周长，单位为mm；γ为割线斜度，单位为°；P为割线导程，单位为mm；

为了减少树干表面的不良影响，提高检测效率，分别在橡胶树(1)的起割点Ps和中割点Pm处沿着割线与橡胶树(1)的轴线平行布置起割点标记(2)和中割点标记(3)；

2)开始割胶作业时，底座(6)上的机械臂本体(5)处于竖直零位姿态；在所述竖直零位姿态，上激光测距传感器(401)和下激光测距传感器(403)处于水平状态，激光点对准橡胶树(1)；激光定位灯(7)处于水平状态，对准橡胶树(1)；下摄像头(8)向下俯视30°，捕捉割线位置；激光定位灯(7)发出竖线激光，与橡胶树(1)的轴线重合，与割线的交点即为中割点Pm；中割点Pm表示割线的中间点；下摄像头(8)采集中割点Pm图像，并在工控机中与预设的图像处理模板比较，进行特征点匹配；以中割点Pm坐标来表征橡胶树(1)的空间位置；对中割点Pm进行眼到手变换，将中割点Pm由相机坐标系转换到机械臂基坐标系下；

根据在橡胶树几何模型中建立的中割点Pm与起割点Ps的几何位置关系，计算出起割点Ps的估计位置(Xs,Ys,Zs)；

式中，R为树干半径，单位为mm；d为刀片（ 404） 与橡胶树（ 1） 表面的安全距离，单位为mm；P为割线导程，单位为mm；Xm,Ym,Zm分别为中割点Pm的X坐标、Y坐标和Z坐标，单位为mm；

3)由工控机规划机械臂本体(5)轨迹，末端执行器(4)在机械臂本体(5)的带动下移动至靠近起割点Ps的估计位置(Xs,Ys,Zs)处；眼摄像头(405)记录起割点Ps处图像；对起割点Ps坐标进行眼在手变换，将起割点Ps的空间位置坐标由相机坐标系转换到机械臂末端坐标系下；为了准确测量刀片(404)与起割点Ps的相对偏差，同时避免末端执行器(4)在寻找起割点时剐蹭树皮甚至与树干发生碰撞，先检测图像中的实际刀尖点，再通过设立虚拟刀尖点替代实际刀尖点来进行起割点Ps的检测；同时由上激光测距传感器(401)和下激光测距传感器(403)的检测值来控制末端执行器(4)和橡胶树(1)的径向距离；

实际刀尖点的检测通过检测刀片(404)的下边缘直线和右边缘直线得到；刀片(404)的下边缘直线和右边缘直线的直线方程为：

其中：x1、y1、x2、y2为常数；ud,vd为刀片(404)的下边缘直线上点的亚像素坐标，ur,vr为刀片(404)的右边缘直线上点的亚像素坐标，单位均为pixel；

刀片(404)的下边缘直线和右边缘直线的直线方程的交点即为实际刀尖点的亚像素坐标(uk,vk)；虚拟刀尖点的设置为实际刀尖点正前方10mm；

在图像中，虚拟刀尖点与实际刀尖点的相对位置固定；先手动控制实际刀尖点与起割点重合，再控制机械臂本体(5)带动末端执行器(4)沿机械臂基座坐标系的X轴正方向移动

10mm，将图像中此时的起割点作为虚拟刀尖点，坐标为(uk’,vk’)；

计算虚拟刀尖点和起割点Ps的相对偏差Δ，不断调整机械臂本体(5)的位姿，减小相对偏差Δ；起割点Ps的坐标是(us,vs)；则其中，uk’,vk’分别为虚拟刀尖点的横坐标和纵坐标，单位为mm；us,vs分别为起割点Ps的横坐标和纵坐标，单位为mm；Δ为虚拟刀尖点和起割点Ps的相对偏差，单位为mm；

同时控制上测距传感器(401)和下激光测距传感器(403)检测到的深度信息即进刀量f在精度范围内，确保刀片(404)贴近橡胶树(1)；

4)当虚拟刀尖点和起割点Ps重合并且深度信息即进刀量f在精度范围内时，起割点检测完成；获得起割点Ps的精确位置；以当前位姿作为机械臂本体(5)进行割胶螺旋运动的起始位姿，更新螺旋轨迹参数。

2.如权利要求1所述的橡胶树的起割点检测的方法，其特征在于：步骤1中，适用于机械臂作业的割线斜度γ范围为25°～30°。

3.如权利要求1所述的橡胶树的起割点检测的方法，其特征在于：步骤1中，所述割线斜度γ为30°

4.如权利要求1所述的橡胶树的起割点检测的方法，其特征在于：步骤1中，起割点标记(2)和中割点标记(3)具有颜色。

5.如权利要求1所述的橡胶树的起割点检测的方法，其特征在于：步骤1中，起割点标记(2)和中割点标记(3)的颜色为红色。

6.如权利要求1所述的橡胶树的起割点检测的方法，其特征在于：步骤2中，刀片(404)与橡胶树(1)表面的安全距离d为5～10mm。

说明书

一种橡胶树的起割点检测系统及检测方法

技术领域

[0001] 本发明涉及农业机器人领域，具体涉及一种橡胶树的起割点检测系统及检测方法。

背景技术

[0002] 天然橡胶以其很强的弹性和良好的绝缘性、可塑性、抗拉耐磨等优点，广泛应用于工业、农业、国防和机械制造、日常生活等各个领域。世界上能产胶的植物超过2000种，而巴
西三叶橡胶树的产量占据世界天然橡胶总产量的99％以上。割胶是少数几个未能实现机械
化的农业领域之一，长期依赖人工。

[0003] 割胶作业对于胶工操作，尤其是在起割点(即入刀处)检测的精度要求较高。起割点检测的效果直接影响割线的质量，进而影响橡胶的产量，甚至会对橡胶树的健康造成伤
害。近年来，产胶区劳动力大量外移，胶工老龄化现象日益加剧，严重影响了橡胶产业的健
康发展。目前尚未存在适用于天然橡胶割胶作业的自动化机械设备，研发此类设备以及方
法很有现实意义。

发明内容

[0004] 本发明的目的在于提出一种橡胶树起割点的检测系统及检测方法，其智能化程度高，能够自主完成起割点的检测；精度高，能够实现割胶刀的准确入刀；抗干扰性强，不受树
干表面情况影响；集成度高，系统稳定可控；结构模块性强，利于后期扩展。

[0005] 本发明的目的是通过以下技术方案实现的：

[0006] 一种橡胶树的起割点检测系统，用于对橡胶树1的起割点进行检测；在橡胶树1的起割点Ps和中割点Pm处沿着割线竖直布置起割点标记2和中割点标记3；所述起割点检测系
统包括末端执行器4、机械臂本体5、底座6、激光定位灯7和下摄像头8；

[0007] 所述机械臂本体5通过底座6安装在移动车体上；移动车体上安装有工控机；底座6为移动式，能够在多棵橡胶树1之间移动，承载机械臂本体5并为其提供动力；

[0008] 末端执行器4通过法兰402与机械臂本体5的末端连接；

[0009] 末端执行器4包括上激光测距传感器401、法兰402、下激光测距传感器403、刀片404和眼摄像头405；上激光测距传感器401和下激光测距传感器403分别安装在刀片404的
上方和下方，上激光测距传感器401和下激光测距传感器403的激光的发射方向垂直于法兰
402的法兰面向外；上激光测距传感器401和下激光测距传感器403的激光点和刀片404的刀
尖点在竖直方向上三点共线；上激光测距传感器401和下激光测距传感器403检测深度信
息，即末端执行器4的进刀量f；眼摄像头405安装在刀片404的运动方向的前方，眼摄像头
405的光轴与刀片404的刀尖点位于同一水平线上；

[0010] 激光定位灯7安装在机械臂本体5的第二关节中心线上，指向机械臂本体5前方，即橡胶树1的方向；下摄像头8布置在机械臂本体5的位于第二关节上方的第三关节上，向下俯
视30°，其高度能够捕捉到橡胶树割线，即橡胶树割线位置在下摄像头8的有效视野范围内。

[0011] 所述方法包括以下步骤：

[0012] 1)底座6在工控机的控制下移动至橡胶树1的正前方，机械臂本体5对准橡胶树1，激光定位灯7对准橡胶树1的中心轴线；在开始割胶作业前，人工测量橡胶树1的树干周长C；
根据公式1计算树干半径R和割线导程P；

[0013]

[0014] 其中，R为树干半径，单位为mm；C为树干周长，单位为mm；γ为割线斜度，单位为°；P为割线导程，单位为mm；

[0015] 为了减少树干表面的不良影响，提高检测效率，分别在橡胶树1的起割点Ps和中割点Pm处沿着割线与橡胶树1的轴线平行布置起割点标记2和中割点标记3；

[0016] 2)开始割胶作业时，底座6上的机械臂本体5处于竖直零位姿态；在所述竖直零位姿态，上激光测距传感器401和下激光测距传感器403处于水平状态，激光点对准橡胶树1；
激光定位灯7处于水平状态，对准橡胶树1；下摄像头8向下俯视30°，捕捉割线位置；激光定
位灯7发出竖线激光，与橡胶树1的轴线重合，与割线的交点即为中割点Pm；中割点Pm表示割
线的中间点；下摄像头8采集中割点Pm图像，并在工控机中与预设的图像处理模板比较，进
行特征点匹配；以中割点Pm坐标来表征橡胶树1的空间位置；对中割点Pm进行眼到手
(EyeToHand)变换，将中割点Pm由相机坐标系转换到机械臂基坐标系下；

[0017] 根据在橡胶树几何模型中建立的中割点Pm与起割点Ps的几何位置关系，计算出起割点Ps的估计位置(Xs,Ys,Zs)；

[0018]

[0019] 式中，R为树干半径，单位为mm；d为刀片404与橡胶树1表面的安全距离，单位为mm；P为割线导程，单位为mm；Xm,Ym,Zm分别为中割点Pm的X坐标、Y坐标和Z坐标，单位为mm；

[0020] 3)由工控机规划机械臂本体5轨迹，末端执行器4在机械臂本体5的带动下移动至靠近起割点Ps的估计位置(Xs,Ys,Zs)处；眼摄像头405记录起割点Ps处图像；对起割点Ps坐标
进行眼在手(EyeInHand)变换，将起割点Ps的空间位置坐标由相机坐标系转换到机械臂末
端坐标系下；为了准确测量刀片404与起割点Ps的相对偏差，同时避免末端执行器4在寻找
起割点时剐蹭树皮甚至与树干发生碰撞，先检测图像中的实际刀尖点，再通过设立虚拟刀
尖点替代实际刀尖点来进行起割点Ps的检测；同时由上激光测距传感器401和下激光测距
传感器403的检测值来控制末端执行器4和橡胶树1的径向距离；

[0021] 实际刀尖点的检测通过检测刀片404的下边缘直线和右边缘直线得到；刀片404的下边缘直线和右边缘直线的直线方程为：

[0022]

[0023] 其中：x1、y1、x2、y2为常数；ud,vd为刀片404的下边缘直线上点的亚像素坐标，ur,vr为刀片404的右边缘直线上点的亚像素坐标，单位均为pixel；

[0024] 刀片404的下边缘直线和右边缘直线的直线方程的交点即为实际刀尖点的亚像素坐标(uk,vk)；虚拟刀尖点的设置为实际刀尖点正前方10mm；

[0025] 在图像中，虚拟刀尖点与实际刀尖点的相对位置固定；先手动控制实际刀尖点与起割点重合，再控制机械臂本体5带动末端执行器4沿机械臂基座坐标系的X轴正方向移动
10mm，将图像中此时的起割点作为虚拟刀尖点，坐标为(uk’,vk’)；

[0026] 计算虚拟刀尖点和起割点Ps的相对偏差Δ，不断调整机械臂本体5的位姿，减小相对偏差Δ；起割点Ps的坐标是(us,vs)；则

[0027]

[0028] 其中，uk’,vk’分别为虚拟刀尖点的横坐标和纵坐标，单位为mm；us,vs分别为起割点Ps的横坐标和纵坐标，单位为mm；Δ为虚拟刀尖点和起割点Ps的相对偏差，单位为mm；

[0029] 同时控制上测距传感器401和下激光测距传感器403检测到的深度信息即进刀量f在精度范围内，确保刀片404贴近橡胶树1；

[0030] 4)当虚拟刀尖点和起割点Ps重合并且深度信息即进刀量f在精度范围内时，起割点检测完成；获得起割点Ps的精确位置；以当前位姿作为机械臂本体5进行割胶螺旋运动的
起始位姿，更新螺旋轨迹参数。

[0031] 步骤1中，适用于机械臂作业的割线斜度γ范围为25°～30°。

[0032] 步骤1中，所述割线斜度γ为30°

[0033] 步骤1中，起割点标记2和中割点标记3具有颜色。

[0034] 步骤1中，起割点标记2和中割点标记3的颜色为红色。

[0035] 步骤2中，刀片404与橡胶树1表面的安全距离d为5～10mm。

[0036] 本发明的有益效果在于：

[0037] 1、本发明智能化程度高，自主完成起割点的检测，为切割过程做好准备。

[0038] 2、本发明精度高，能够实现割胶刀的准确入刀。

[0039] 3、本发明抗干扰性强，不受树干表面情况影响。

[0040] 4、本发明集成度高，系统稳定可控。

[0041] 5、本发明结构模块性强，利于后期扩展。

[0042] 本发明有机结合协作机器人的高精度和图像处理系统的灵活性，实现了橡胶树起割点的自动化检测，智能化程度高。

[0043] 本发明集成了机械臂的轨迹规划和手眼视觉系统，能够实现空间坐标在不同坐标系下的转换，系统性强。从中割点到起割点的坐标计算结合了橡胶树的几何模型，有效提高
了起割点的检测效率，降低了时间成本。

[0044] 本发明采用虚拟刀尖点代替实际刀尖点进行起割点的精确检测，有效避免了橡胶树表面的不良干扰，保护了刀片，在保证检测速度的同时提高了起割点的检测精度。

[0045] 本发明为模块化设计，适应性强，可以针对不同环境下的橡胶树割胶作业进行调整。

附图说明

[0046] 图1为本发明中，橡胶树1的割线部分的结构示意图；

[0047] 图2为本发明中，检测中割点时的状态示意图；

[0048] 图3为本发明，检测起割点时的状态示意图；

[0049] 图4为本发明的末端执行器4的结构示意图。

[0050] 附图标记：

[0051] 1、橡胶树                       2、起割点标记

[0052] 3、中割点标记                   4、末端执行器

[0053] 401、上激光测距传感器           402、法兰

[0054] 403、下激光测距传感器           404、刀片

[0055] 405、眼摄像头                   5、机械臂本体

[0056] 6、底座                         7、激光定位灯

[0057] 8、下摄像头

[0058] Ps、起割点

[0059] Pm、中割点

[0060] γ、割线斜度

具体实施方式

[0061] 下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

[0062] 如图1～图4所示，一种橡胶树的起割点检测系统，用于对橡胶树1的起割点进行检测。在橡胶树1的起割点Ps和中割点Pm处沿着割线竖直布置起割点标记2和中割点标记3。所
述起割点检测系统包括末端执行器4、机械臂本体5、底座6、激光定位灯7和下摄像头8。

[0063] 所述机械臂本体5通过底座6安装在移动车体上。移动车体上安装有工控机作为系统的控制核心。底座6为移动式，能够在多棵橡胶树1之间移动，承载机械臂本体5并为其提
供动力。

[0064] 末端执行器4通过法兰402与机械臂本体5的末端连接。

[0065] 末端执行器4包括上激光测距传感器401、法兰402、下激光测距传感器403、刀片404和眼摄像头405。上激光测距传感器401和下激光测距传感器403分别安装在刀片404的
上方和下方，上激光测距传感器401和下激光测距传感器403的激光的发射方向垂直于法兰
402的法兰面向外。上激光测距传感器401和下激光测距传感器403的激光点和刀片404的刀
尖点在竖直方向上三点共线。上激光测距传感器401和下激光测距传感器403检测深度信
息，即末端执行器4的进刀量f。眼摄像头405安装在刀片404的运动方向的前方，眼摄像头
405的光轴与刀片404的刀尖点位于同一水平线上。

[0066] 激光定位灯7安装在机械臂本体5的第二关节中心线上，指向机械臂本体5前方，即橡胶树1的方向。下摄像头8布置在机械臂本体5的位于第二关节上方的第三关节上，向下俯
视30°，其高度能够捕捉到橡胶树割线，即橡胶树割线位置在下摄像头8的有效视野范围内。

[0067] 利用所述橡胶树的起割点检测系统进行橡胶树的起割点检测的检测方法，包括以下步骤：

[0068] 1、底座6在工控机的控制下移动至橡胶树1的正前方，机械臂本体5对准橡胶树1，激光定位灯7对准橡胶树1的中心轴线。在开始割胶作业前，人工测量橡胶树1的树干周长C。
根据公式1计算树干半径R和割线导程P。

[0069]

[0070] 其中，R为树干半径，单位为mm；C为树干周长，单位为mm；γ为割线斜度，单位为°；P为割线导程，单位为mm。

[0071] 适用于机械臂作业的割线斜度γ范围为25°～30°，优选地，割线斜度γ为30°。

[0072] 为了减少树干表面的不良影响，提高检测效率，分别在橡胶树1的起割点Ps和中割点Pm处沿着割线与橡胶树1的轴线平行布置起割点标记2和中割点标记3。起割点标记2和中
割点标记3的颜色以利于图像识别为宜，与割线产生明显交点。优选地，起割点标记2和中割
点标记3的颜色为红色。红色在林间为警戒色，也能够避免受到动物的侵扰。

[0073] 2、开始割胶作业时，底座6上的机械臂本体5处于竖直零位姿态。如图2所示状态，所述竖直零位姿态为机器人运动学中人为设置的机械臂本体5的各关节处于零点的状态。
在竖直零位姿态，上激光测距传感器401和下激光测距传感器403处于图4中所示的水平状
态，激光点对准橡胶树1。激光定位灯7处于水平状态，对准橡胶树1。下摄像头8向下俯视
30°，捕捉割线位置。激光定位灯7发出竖线激光，与橡胶树1的轴线重合，与割线的交点即为
中割点Pm。中割点Pm表示割线的中间点。下摄像头8采集中割点Pm图像，并在工控机中与预设
的图像处理模板比较，进行特征点匹配。以中割点Pm坐标来表征橡胶树1的空间位置。对中
割点Pm进行眼到手(EyeToHand)变换，将中割点Pm由相机坐标系转换到机械臂基坐标系下。

[0074] 根据在橡胶树几何模型中建立的中割点Pm与起割点Ps的几何位置关系，计算出起割点Ps的估计位置(Xs,Ys,Zs)。

[0075]

[0076] 式中，R为树干半径，单位为mm；d为刀片404与橡胶树1表面的安全距离，单位为mm，优选地，d为5～10mm；P为割线导程，单位为mm；Xm,Ym,Zm分别为中割点Pm的X坐标、Y坐标和Z
坐标，单位为mm。

[0077] 3、由工控机规划机械臂本体5轨迹，末端执行器4在机械臂本体5的带动下移动至靠近起割点Ps的估计位置(Xs,Ys,Zs)处，如图3所示。眼摄像头405记录起割点Ps处图像。对起
割点Ps坐标进行眼在手(EyeInHand)变换，将起割点Ps的空间位置坐标由相机坐标系转换到
机械臂末端坐标系下。为了准确测量刀片404与起割点Ps的相对偏差，同时避免末端执行器
4在寻找起割点时剐蹭树皮甚至与树干发生碰撞，先检测图像中的实际刀尖点，再通过设立
虚拟刀尖点替代实际刀尖点来进行起割点Ps的检测。同时由上激光测距传感器401和下激
光测距传感器403的检测值来控制末端执行器4和橡胶树1的径向距离。

[0078] 实际刀尖点的检测通过检测刀片404的下边缘直线和右边缘直线得到。刀片404的下边缘直线和右边缘直线的直线方程为：

[0079]

[0080] 其中：x1、y1、x2、y2为常数。ud,vd为刀片404的下边缘直线上点的亚像素坐标，ur,vr为刀片404的右边缘直线上点的亚像素坐标，单位均为pixel。

[0081] 刀片404的下边缘直线和右边缘直线的直线方程的交点即为实际刀尖点的亚像素坐标(uk,vk)。虚拟刀尖点的设置为实际刀尖点正前方10mm，相当于实际刀尖点向机械臂基
座坐标系的X轴正方向移动了10mm，即使上测距传感器401和下激光测距传感器403检测到
的深度信息小于起割点的位置，刀片404也不会与橡胶树1发生剐蹭。

[0082] 在图像中，虚拟刀尖点与实际刀尖点的相对位置固定。先手动控制实际刀尖点与起割点重合，再控制机械臂本体5带动末端执行器4沿机械臂基座坐标系的X轴正方向移动
了10mm，将图像中此时的起割点作为虚拟刀尖点，坐标为(uk’,vk’)。

[0083] 计算虚拟刀尖点和起割点Ps的相对偏差Δ，不断调整机械臂本体5的位姿，减小相对偏差Δ。起割点Ps的坐标是(us,vs)。则

[0084]

[0085] 其中，uk’,vk’分别为虚拟刀尖点的横坐标和纵坐标，单位为mm；us,vs分别为起割点Ps的横坐标和纵坐标，单位为mm；Δ为虚拟刀尖点和起割点Ps的相对偏差，单位为mm。

[0086] 同时控制上测距传感器401和下激光测距传感器403检测到的深度信息即进刀量f在精度范围内，确保刀片404贴近橡胶树1。

[0087] 4、当虚拟刀尖点和起割点Ps重合并且深度信息即进刀量f在精度范围内时，起割点检测完成。获得起割点Ps的精确位置。以当前位姿作为机械臂本体5进行割胶螺旋运动的
起始位姿，更新螺旋轨迹参数。