CN114102609A - 基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法 - Google Patents

Abstract

本发明公开了基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，包括以下步骤：根据采摘机器人的机械结构建立采摘执行器中香蕉果柄夹持件的位置和姿态与各自由度的正运动学模型；基于牛顿迭代算法根据香蕉果柄夹持件的目标位置和姿态求得满足设定精度的逆运动学解。本发明根据采摘机器人的正运动学模型和几何关系求得底座转角θ、末端转角

的逆运动学解析解，再由采摘机器人的正运动学模型构造水平位移x和垂直位移z满足的方程组，并求出其雅可比矩阵的逆矩阵，然后采用牛顿迭代算法根据果柄夹持机构的目标位置和姿态求得满足设定精度的水平位移x和垂直位移z的逆运动学解，精度高、实时性较好。

Description

基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法

技术领域

本发明涉及机器人采摘技术领域，具体为基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法。

背景技术

香蕉是岭南特色水果和经济作物，适收期的香蕉串质量约30kg，果柄切割处高度超过2m。目前，香蕉串采摘以人工方式为主，由多人协作完成，劳动强度大、人工成本高、采摘效率低，严重制约着香蕉的产业发展和经济效益，而机器人可实现香蕉串采摘的机械化和智能化，不仅可以提高采摘效率、降低成本，还能解决农业劳动力短缺问题。采摘机器人先根据香蕉串果柄位姿和周围环境规划出末端采摘执行器中香蕉果柄夹持件的运动轨迹，从而得到各采样时刻香蕉果柄夹持件的目标位置和姿态，然后据此求得机器人各自由度的逆运动学解，输入关节伺服控制器，进而控制香蕉果柄夹持件准确定位到香蕉串果柄切割处并将其夹住，再启动链锯切断果柄，完成香蕉串采摘。显然，逆运动学求解是实现机器人末端香蕉果柄夹持件运动控制和精准定位的前提和关键。

通常，机器人的逆运动学求解方法主要有解析解法和数值解法。其中，解析解法因计算量小、精度高、实时性好而被广泛应用于机器人的逆运动学求解，但要求机器人的结构必须满足Pieper准则；而对于结构不符合Pieper准则的机器人，则可采用数值解法求得满足给定精度的逆运动学解。相关研究人员采用神经网络、强化学习、粒子群优化算法、遗传算法、果蝇优化算法、差分进化算法等数值解法求机器人的逆运动学解。但神经网络结构不好确定、训练的参数多、训练样本量大且难以获取、网络训练时间长，对机器人控制系统的硬件配置要求较高；强化学习也需要大量的数据样本，并且训练过程较长；粒子群优化算法迭代优化过程复杂，粒子总数难以确定；遗传算法求解逆运动学过程中容易早熟，局部搜索能力差，不一定每次都能求得满足给定精度的逆运动学解。

发明内容

本发明针对上述现有技术存在的不足，提供基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法。

为了解决上述技术问题，本发明提供了如下的技术方案：

基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，包括以下步骤：

根据采摘机器人的机械结构建立采摘执行器中香蕉果柄夹持件的位置和姿态与各自由度的正运动学模型；

基于牛顿迭代算法根据香蕉果柄夹持件的目标位置和姿态求得满足设定精度的逆运动学解。

作为本发明的进一步技术方案为，所述采摘机器人包括履带、移动平台、水平滑块、垂直滑块、长主臂、短主臂、副臂、三角构件和采摘执行器，所述履带上设置移动平台，所述移动平台的上端和侧边分别设置用于安装水平滑块和垂直滑块的滑轨，所述水平滑块上设置短主臂，垂直滑块上设置长主臂，所述长主臂和短主臂的端部设置三角构件，所述三角构件与副臂连接，所述副臂的端部设置采摘执行器。

作为本发明的进一步技术方案为，所述采摘执行器包括末端转动杆、香蕉果柄夹持件和香蕉果柄切割件，所述末端转动杆与副臂的端部转动连接，所述末端转动杆与香蕉果柄夹持件连接，所述香蕉果柄夹持件上设置香蕉果柄切割件。

作为本发明的进一步技术方案为，所述根据采摘机器人的机械结构建立采摘执行器中香蕉果柄夹持件的位姿与各自由度的正运动学模型；具体包括：

根据采摘机器人的4个自由度对果柄夹持件的位置和姿态进行定位；

确定香蕉果柄夹持件的位置、姿态与各自由度变量的函数关系式。

作为本发明的进一步技术方案为，所述根据采摘机器人的4个自由度对果柄夹持件的位置和姿态进行定位；具体包括：

采摘机器人设置底座转动自由度θ、水平滑块沿导轨的水平运动自由度x、垂直滑块沿导轨的垂直运动自由度z和末端转动自由度

四个自由度；

通过控制采摘机器人4个自由度的运动调节采摘执行器中香蕉果柄夹持件的位置和姿态，使其定位到香蕉串果柄切割处以便完成采摘作业；其中，对香蕉果柄夹持件的位置和姿态进行精确控制，建立4个自由度变量描述香蕉果柄夹持件位置和姿态的采摘机器人正运动学模型；

短主臂与水平方向夹角为α，副臂与水平方向夹角为β，末端转动杆与香蕉果柄夹持件的夹角为γ；在水平运动方向和垂直运动方向的交点处建立局部坐标系o-xyz，并随底座一起转动；而在底座转动中心建立基坐标系O-XYZ；其中，局部坐标系o-xyz在基坐标系O-XYZ中的位置向量p为：

p＝[p_x,p_y,p_z]＝[-364,0,657]；

考虑到采摘机器人的连杆机构和局部坐标系o-xyz随底座一起转动，故水平运动自由度和垂直运动自由度只在xoz平面内调整连杆机构的位置，因此，先在xoz平面内建立末端转动中心M的位置与水平位移x、垂直位移z的函数关系；根据采摘机器人各连杆之间的几何关系，可得节点A、C、M的位置表达式为：

式(3)中，

联立式(1)～(3)可得末端转动中心M的位置表达式为：

作为本发明的进一步技术方案为，确定香蕉果柄夹持件的位置、姿态角与各自由度变量的函数关系式；具体包括：

确定主臂夹角α、副臂夹角β与水平位移x、垂直位移z的关系；利用半角公式可实现正弦函数和余弦函数的转换，即：

联立式(1)、(2)和半角公式(5)、(6)，同时考虑主臂夹角α的取值范围，可得主臂夹角α与水平位移x、垂直位移z的相关函数表达式为：

同理，可得副臂夹角β与水平位移x、垂直位移z的相关函数表达式为：

将sinα式(8)、cosβ式(13)代入式(4)，即可得xoz平面内末端转动中心M的位置(x_M,z_M)关于水平位移x、垂直位移z的非线性函数表达式；

结合局部坐标系o-xyz在基坐标系O-XYZ中的位姿p，同时考虑底座转角θ的作用，根据几何关系可得末端转动中心M在基坐标系O-XYZ中的位置表达式为：

对于香蕉果柄夹持件的位姿，还需考虑末端转角

的作用，根据机器人的连杆结构和几何关系，可得香蕉果柄夹持件相对于末端转动中心M在基坐标系O-XYZ中的位置为：

再将M的位置表达式(15)代入式(16)可得：

将sinα式(8)、cosβ式(13)代入式(17)，即得各自由度变量θ、x、z、

描述香蕉果柄夹持件位置的函数关系式；

由于底座转角θ、末端转角

均为偏摆自由度，故香蕉果柄夹持件的姿态Ψ为：

至此，得到式(17)、(18)所示采摘机器人的正运动学模型，即香蕉果柄夹持件的位置(X_H,Y_H,Z_H)、姿态Ψ与各自由度变量θ、x、z、

的函数关系式。

作为本发明的进一步技术方案为，所述基于牛顿迭代算法根据香蕉果柄夹持件的目标位置和姿态求得满足设定精度的逆运动学求解，具体包括：

根据香蕉果柄夹持件的目标位置和姿态角对底座转角θ和末端转角

进行求解；

基于牛顿迭代法求解水平位移x和垂直位移z。

作为本发明的进一步技术方案为，所述根据香蕉果柄夹持件的目标位置和姿态角对底座转角θ和末端转角

进行求解；具体包括：由于基坐标系O-XYZ建在底座转动中心，故机器人连杆在XOY平面内的投影经过基坐标系原点O，因此，先根据几何关系由香蕉果柄夹持件的目标位置(X_H,Y_H,Z_H)和姿态Ψ求得末端转动中心M的位置分量X_M、Y_M，再通过反正切函数求解底座转角θ；联立式(16)、(18)可得M的位置分量X_M、Y_M为：

因为底座转角θ的取值范围为(-π,π)，而反正切函数的取值范围为(-π/2,π/2)，因此，根据M的位置分量X_M、Y_M所处象限进行修正，可得底座转角θ的逆解表达式为：

求得底座转角θ后，再根据末端转角

的取值范围(-2π/3,2π/3)，由式(18)可得末端转角

的逆解表达式：

作为本发明的进一步技术方案为，所述基于牛顿迭代法求解水平位移x和垂直位移z，具体包括：

若采摘机器人的水平位移x、垂直位移z满足的方程组为F(X)，其雅可比矩阵为J(X)，其中X＝(x,z)^T，则牛顿迭代法的计算表达式为：

X_k+1＝X_k-J^-1(X_k)F(X_k) (22)

式(22)中，X_k、X_k+1分别表示第k、k+1次迭代的输出值，(k＝0,1,…,n)；J^-1(X_k)为雅可比矩阵J(X_k)的逆矩阵；

确定关于水平位移x和垂直位移z的方程组F(X)，及其雅可比矩阵的逆矩阵J^-1(X)；

选取采摘机器人正运动学模型的位置表达式(17)中X_H、Z_H分量，以及姿态Ψ表达式(18)构造关于水平位移x和垂直位移z的方程组F(x,z)，即

将sinα式(8)、cosβ式(13)代入式(23)，再分别对x、z求偏导数，可得其雅可比矩阵J(x,z)：

式(24)中，各元素的表达式为：

其中：

而式(24)所示雅可比矩阵J(x,z)的逆矩阵J^-1(x,z)计算表达式为：

将式(23)、(25)代入式(22)，即得计算采摘机器人水平位移x、垂直位移z的牛顿迭代表达式。

作为本发明的进一步技术方案为，所述基于牛顿迭代法求解水平位移和垂直位移，具体为，选择水平位移和垂直位移的实时值作为牛顿迭代初值，进行一次迭代获取下一时刻的水平位移和垂直位移。

本发明的有益效果是：

本发明提出了一种基于牛顿迭代法的香蕉采摘机器人逆运动学数值计算方法，先根据采摘机器人的正运动学模型和几何关系求得底座转角θ、末端转角

的逆运动学解析解，再由机器人的正运动学模型构造水平位移x和垂直位移z满足的方程组，并求出其雅可比矩阵的逆矩阵，然后采用牛顿迭代算法根据香蕉果柄夹持件的目标位置和姿态求得满足设定精度的水平位移x和垂直位移z的逆运动学解，精度高、实时性较好。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。

图1是本发明提出的基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法流程图；

图2是本发明提出的采摘机器人的机械结构示意图；

图3是本发明提出的采摘机器人的运动机构侧视图；

图4是本发明提出的采摘机器人连杆机构俯视图；

图5是本发明提出的基于牛顿迭代算法根据香蕉果柄夹持件的目标位置和姿态求得满足设定精度的逆运动学解流程图；

图6是本发明提出的基于牛顿迭代法计算x、z逆运动学解的算法流程图；

图7是本发明提出的初始位姿下采摘机器人的连杆模型图；

图8a是本发明提出的三次样条插值规划果柄夹持机构的位置轨迹图；

图8b是本发明提出的三次样条插值规划果柄夹持机构的姿态轨迹图；

图9a是本发明提出的水平位移x的逆运动学解轨迹图；

图9b是本发明提出的垂直位移z的逆运动学解轨迹图；

图9c是本发明提出的底座转角θ的逆运动学解轨迹图；

图9d是本发明提出的末端转角

的逆运动学解轨迹图；

图10本发明提出的各自由度逆解求得果柄夹持机构位置的误差轨迹图。

具体实施方式

以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

参见图1，本发明提供基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，包括以下步骤：

根据采摘机器人的机械结构建立采摘执行器中香蕉果柄夹持件的位置和姿态与各自由度的正运动学模型；

基于牛顿迭代算法根据香蕉果柄夹持件的目标位置和姿态求得满足设定精度的逆运动学解。

由于牛顿迭代初值对逆解的精度和算法的收敛速度影响较大，而采摘机器人的位姿在短时间内变化较小，因此，对于采摘机器人的连续运动，选取水平位移和垂直位移的实时值或上一采样时刻的逆解作为牛顿迭代初值，只需1次牛顿迭代即可求得高精度逆解。

参见图2，本发明实施例中，所述采摘机器人包括履带、移动平台、水平滑块、垂直滑块、长主臂、短主臂、副臂、三角构件和采摘执行器，所述履带上设置移动平台，所述移动平台的上端和侧边分别设置用于安装水平滑块和垂直滑块的滑轨，所述水平滑块上设置短主臂，垂直滑块上设置长主臂，所述长主臂和短主臂的端部设置三角构件，所述三角构件与副臂连接，所述副臂的端部设置采摘执行器。

底座转动自由度驱动整个机器人转动，可大范围调整采摘执行器的位置和姿态；水平运动自由度和垂直运动自由度分别通过水平滑块C、垂直滑块A连同短主臂CE、长主臂AD沿导轨作水平运动、垂直运动，进而改变采摘执行器的位置；而末端转动自由度主要调整采摘执行器的姿态，同时小范围调整其位置。

其中，所述采摘执行器包括末端转动杆、香蕉果柄夹持件和香蕉果柄切割件，所述末端转动杆与副臂的端部转动连接，所述末端转动杆与香蕉果柄夹持件连接，所述香蕉果柄夹持件上设置香蕉果柄切割件。采摘香蕉串时，先控制夹持机构准确定位到香蕉串果柄切割处并可靠地夹持住香蕉串果柄，再启动切割机构将果柄切断。

集香蕉串夹持和切割于一体的混联式采摘机器人，安装在履带式移动平台上，可在崎岖不平的香蕉园机耕道运动。采摘机器人具有四个自由度，能够较好地调节末端采摘执行器的位姿，同时主臂和副臂较长，使得末端采摘执行器的作业空间大，满足高位生长的香蕉串采摘需求。此外，各自由度的驱动电机功率较大，且主臂和副臂为刚度大的并联机构，故机器人的负载能力较强，可达60kg，完全能够承受香蕉串的重量。

在步骤100中，根据采摘机器人的机械结构建立采摘执行器中香蕉果柄夹持件的位置和姿态与各自由度的正运动学模型；具体包括：采摘机器人设置4个自由度，分别是底座转动自由度、水平滑块C沿导轨的水平运动自由度、垂直滑块A沿导轨的垂直运动自由度、末端转动自由度(末端转角)；通过控制机器人4个自由度的运动可调节采摘执行器中香蕉果柄夹持件的位置和姿态，使其准确定位到香蕉串果柄切割处以便完成采摘作业。因此，为了对香蕉果柄夹持件的位置和姿态进行精确控制，建立4个自由度变量(θ、x、z、

)描述香蕉果柄夹持件位置和姿态的采摘机器人正运动学模型。

参见图3，短主臂CE与水平方向夹角为α(逆时针为正方向)，副臂DF与水平方向夹角为β(顺时针为正方向)，末端转动杆FK与香蕉果柄夹持件LM的夹角为γ(常数)；在水平运动方向和垂直运动方向的交点处建立局部坐标系o-xyz，并随底座一起转动；而在底座转动中心建立基坐标系O-XYZ；其中，局部坐标系o-xyz在基坐标系O-XYZ中的位置向量p为：

p＝[p_x,p_y,p_z]＝[-364,0,657]；

考虑到采摘机器人的连杆机构和局部坐标系o-xyz随底座一起转动，故水平运动自由度和垂直运动自由度只在xoz平面内调整连杆机构的位置，因此，先在xoz平面内建立末端转动中心M的位置与水平位移x、垂直位移z的函数关系；根据图3所示采摘机器人各连杆之间的几何关系，可得节点A、C、M的位置表达式为：

式(3)中，

联立式(1)～(3)可得末端转动中心M的位置表达式为：

由式(4)可知，M的位置不仅与水平位移x、垂直位移z有关，还与主臂夹角α、副臂夹角β有关；由表1中采摘机器人连杆参数可知，相关连杆长度不满足M的水平运动和垂直运动解耦条件：l₁l₄＝l₂l₃，从而导致机器人的正运动学模型比较复杂；本发明进一步确定主臂夹角α、副臂夹角β与水平位移x、垂直位移z的关系。

香蕉串采摘执行器位于机器人的末端，机器人采摘香蕉串时，先控制果柄夹持件准确定位到香蕉串果柄切割处并可靠地夹持住香蕉串果柄，再启动切割件将果柄切断。为研究方便，将末端转动自由度的转轴简化为连杆MN，香蕉果柄夹持件简化为连杆NH，表示香蕉果柄夹持件的中心H到末端转轴MN的距离。由于平行四边形机构EFKJ和CEIG通过三角形构件IEJ连接，并铰连在水平滑块C上，使得香蕉果柄夹持件等效连杆NH始终保持水平状态。

表1

表2

利用半角公式可实现正弦函数和余弦函数的转换，即：

联立式(1)、(2)和半角公式(5)、(6)，同时考虑主臂夹角α的取值范围，可得主臂夹角α与水平位移x、垂直位移z的相关函数表达式为：

同理，可得副臂夹角β与水平位移x、垂直位移z的相关函数表达式为：

将sinα式(8)、cosβ式(13)代入式(4)，即可得xoz平面内末端转动中心M的位置(x_M,z_M)关于水平位移x、垂直位移z的非线性函数表达式。接下来，结合局部坐标系o-xyz在基坐标系O-XYZ中的位姿p，同时考虑底座转角θ的作用，如图4所示，根据几何关系可得末端转动中心M在基坐标系O-XYZ中的位置表达式为：

而对于香蕉果柄夹持件的位置和姿态，还需考虑末端转角

的作用，如图4所示，因此，根据机器人的连杆结构和几何关系，可得香蕉果柄夹持件相对于末端转动中心M在基坐标系O-XYZ中的位置为：

再将M的位置表达式(15)代入式(16)可得：

将sinα式(8)、cosβ式(13)代入式(17)，即得各自由度变量θ、x、z、

描述香蕉果柄夹持件位置的函数关系式。

由于底座转角θ、末端转角

均为偏摆自由度，故香蕉果柄夹持件的姿态Ψ为：

至此，得到式(17)、(18)所示采摘机器人的正运动学模型，即香蕉果柄夹持件的位置(X_H,Y_H,Z_H)、姿态Ψ与各自由度变量θ、x、z、

的函数关系式。

参见图5，在步骤200中，基于牛顿迭代算法根据香蕉果柄夹持件的目标位置和姿态求得满足设定精度的逆运动学求解，具体包括：

步骤201，根据香蕉果柄夹持件的目标位置和姿态角对底座转角θ和末端转角

进行求解；

步骤202，基于牛顿迭代法求解水平位移x和垂直位移z。

在各采样时刻根据香蕉果柄夹持件的目标位置(X_H,Y_H,Z_H)和姿态Ψ在线求得4个自由度θ、x、z、

的逆运动学解。

其中，步骤201中，所述根据香蕉果柄夹持件的目标位置和姿态角对底座转角θ和末端转角

进行求解；具体包括：由于基坐标系O-XYZ建在底座转动中心，故机器人连杆在XOY平面内的投影经过基坐标系原点O，因此，先根据几何关系由香蕉果柄夹持件的目标位置(X_H,Y_H,Z_H)和姿态Ψ求得末端转动中心M的位置分量X_M、Y_M，再通过反正切函数求解底座转角θ。联立式(16)、(18)可得M的位置分量X_M、Y_M为：

因为底座转角θ的取值范围为(-π,π)，而反正切函数的取值范围为(-π/2,π/2)，因此，根据M的位置分量X_M、Y_M所处象限进行修正，可得底座转角θ的逆解表达式为：

由于反正切函数中分母X_M不能为零，若X_M＝0，则结合Y_M判断底座转角θ是π/2还是-π/2。求得底座转角θ后，再根据末端转角

的取值范围(-2π/3,2π/3)，由式(18)可得末端转角

的逆解表达式：

在步骤202中，所述基于牛顿迭代法求解水平位移x和垂直位移z，具体包括：采摘机器人的底座转角θ、末端转角

确定后，由式(8)、(13)、(17)可知，香蕉果柄夹持件的位置分量X_H、Y_H、Z_H相互耦合，是水平位移x和垂直位移z的复杂非线性函数，难以通过传统的方程组解法直接进行求解。而数值迭代法比较适合求解非线性方程组，故本发明采用牛顿迭代法求解水平位移x、垂直位移z的逆运动学解。

若采摘机器人的水平位移x、垂直位移z满足的方程组为F(X)，其雅可比矩阵为J(X)，其中X＝(x,z)^T，则牛顿迭代法的计算表达式为：

X_k+1＝X_k-J^-1(X_k)F(X_k) (22)

式(22)中，X_k、X_k+1分别表示第k、k+1次迭代的输出值，(k＝0,1,…,n)；J^-1(X_k)为雅可比矩阵J(X_k)的逆矩阵。显然，要先确定关于水平位移x和垂直位移z的方程组F(X)，及其雅可比矩阵的逆矩阵J^-1(X)。

选取采摘机器人正运动学模型的位置表达式(17)中X_H、Z_H分量，以及姿态Ψ表达式(18)构造关于水平位移x和垂直位移z的方程组F(x,z)，即

将sinα式(8)、cosβ式(13)代入式(23)，再分别对x、z求偏导数，可得其雅可比矩阵J(x,z)：

式(24)中，各元素的表达式为：

其中：

而式(24)所示雅可比矩阵J(x,z)的逆矩阵J^-1(x,z)计算表达式为：

将式(23)、(25)代入式(22)，即得计算采摘机器人水平位移x、垂直位移z的牛顿迭代表达式。

参见图6，为本发明提出的基于牛顿迭代法求解水平位移x、垂直位移z的流程图，先根据香蕉果柄夹持件的目标位置(X_H,Y_H,Z_H)和姿态Ψ，由式(20)、(21)求得底座转角θ、末端转角

的逆解。设定牛顿迭代初值x₀、z₀，迭代次数k＝0，给定误差阈值ε、最大迭代次数η等结束条件。由式(23)-(25)和牛顿迭代式(22)计算x_k+1、z_k+1，并计算与上次迭代值x_k、z_k的误差e_k+1，若e_k+1<ε，则迭代结束，输出x、z的逆解；若e_k+1>ε且k<η，则更新迭代值x_k、z_k和迭代次数k，继续迭代；如果e_k+1>ε且k>η，表明在最大迭代次数η内没有求得满足给定精度的逆解，可能迭代初值选择不合理，需修改迭代初值x₀、z₀后重新进行求解。

本发明实施例中，由于牛顿迭代初值对算法的收敛速度和逆解的精度影响较大，考虑到采摘机器人的位姿在短时间内变化较小，因此，对于连续运动，可选择水平位移x和垂直位移z的实时值或上一采样时刻的逆解作为牛顿迭代初值x₀、z₀，通常只需要1次迭代即可求得高精度的逆解。

本发明根据香蕉采摘机器人特殊的机械结构和几何关系，建立采摘执行器中果柄夹持件的位置(X_H,Y_H,Z_H)和姿态Ψ由4个自由度变量(θ、x、z、

)描述的正运动学模型。

本发明针对采摘机器人的水平位移x和垂直位移z相互耦合呈复杂非线性关系，难以求得逆运动学解析解的问题，提出了一种基于牛顿迭代法的香蕉采摘机器人逆运动学数值计算方法。先根据采摘机器人的正运动学模型和几何关系求得底座转角θ、末端转角

的逆运动学解析解，再由机器人的正运动学模型构造水平位移x和垂直位移z满足的方程组，并求出其雅可比矩阵的逆矩阵，然后采用牛顿迭代算法根据果柄夹持件的目标位置和姿态求得满足设定精度的水平位移x和垂直位移z的逆运动学解。

本发明在MATLAB软件平台上按照表1所示参数建立香蕉采摘机器人的连杆模型，如图7所示，分别对果柄夹持件的单个目标位姿和连续运动中各采样时刻目标位姿的逆运动学求解进行仿真试验。

采摘机器人的果柄夹持件从初始位姿到单个目标位姿的逆解，出于一般性考虑，在表2所示采摘机器人自由度变量的取值范围内，任意选取3组目标值(θ_d,x_d,z_d,

)，由采摘机器人的正运动学模型式(17)、(18)求得果柄夹持件的目标位置(X_H,Y_H,Z_H)和姿态Ψ，如表3中组序1、2、3，而组序0表示采摘机器人初始状态下各自由度的初值和果柄夹持件的初始位置和姿态。

表3

当果柄夹持件分别取组序1、2、3的目标位姿时，先采用式(19)～(21)求得底座转角θ、末端转角

的逆运动学解，再通过图6所示牛顿迭代算法求出水平位移x、垂直位移z的逆运动学解，如表4所示。其中，牛顿迭代初值均取组序0中初始水平位移x₀＝360mm、垂直位移z₀＝-270mm。

表4

表4中底座转角θ、末端转角

的逆解与表3中目标值θ_d、

相同，表明解析解法可求得高精度逆解。水平位移和垂直位移任取的3组目标值x_d、z_d(组序1、2、3)虽与牛顿迭代初值x₀、z₀相差较大，但经过2次牛顿迭代后的值x₂、z₂误差小于0.1mm；而经过3次牛顿迭代后的值x₃、z₃无误差。结果表明，对于任意给定的果柄夹持件目标位置和姿态，本发明方法都能求出采摘机器人各自由度的高精度逆运动学解，且牛顿迭代次数不超过3次。

为了进一步验证本发明方法逆运动学求解的精度、实时性和适用性，在果柄夹持件跟踪规划的轨迹运动到香蕉串果柄切割处的过程中，根据连续采样时刻果柄夹持件的目标位置和姿态求解各自由度的逆运动学解。

选取表3中组序0和组序3的位置和姿态角分别作为果柄夹持件在起始时刻(t＝0.0s)、结束时刻(t＝6.0s)的位姿，选取(1710.4,-1489.1,1391.6)和-16.7°作为中间过渡时刻(t＝3.5s)的位置和姿态角，同时设定起始、结束时刻果柄夹持件位置和姿态角的一阶导数(速度和角速度)均为0，即

根据上述约束条件，通过三次样条插值方法分别规划出果柄夹持件连续运动的位置分量轨迹X_H(t)、Y_H(t)、Z_H(t)和姿态轨迹Ψ(t)，如图8所示，据此可得采样时刻t_s果柄夹持件的目标位置(X_H(t_s),Y_H(t_s),Z_H(t_s))和姿态Ψ(t_s)，然后采用本发明方法求出各自由度的逆运动学解θ(t_s)、x(t_s)、z(t_s)、

进而得到连续采样时刻各自由度的逆运动学解轨迹如图9所示(采样周期T_s＝25ms)。

由于在较短的采样周期T_s内各自由度值变化不大，因此，为了减少牛顿迭代次数，提高逆解算法的实时性，选取上一采样时刻水平位移x和垂直位移z的逆解作为牛顿迭代初值，且只做1次牛顿迭代即输出逆解。图9中各自由度的逆运动学解轨迹均连续、平滑，没有奇异点，表明本发明逆运动学求解方法适用性好，在每个采样时刻都能求得有效逆解。

为了便于说明水平位移x和垂直位移z的逆运动学求解精度，将图9中各自由度的逆运动学解代入正运动学模型式(17)求出果柄夹持件的位置，与图8(a)中规划位置的误差轨迹如图10所示，3个位置分量X_H、Y_H、Z_H的最大误差均为10^-3mm数量级，其中：X_H的最大误差e_mX为0.889×10^-3mm；Y_H的最大误差e_mY为-1.286×10^-3mm；Z_H的最大误差e_mZ为-0.853×10^{- 3}mm。这表明在连续运动中，取上一采样时刻的逆解作为牛顿迭代初值，只需经过1次牛顿迭代即可求得水平位移x和垂直位移z较高精度的逆运动学解。此外，由于底座转角θ、末端转角

可由式(19)～(21)求得无误差的逆运动学解析解，故不做赘述。

为了进一步说明采摘机器人连续运动状态下对于不同的采样周期T_s，只需1次牛顿迭代即可求得水平位移x和垂直位移z的高精度逆解，采样周期T_s分别取10、25、50、100、200ms，重复前述方法求出连续采样时刻各自由度逆运动学解对应的果柄夹持机构3个位置分量的最大误差，如表5所示。由结果可知，随着采样周期T_s变大，各自由度逆运动学解对应果柄夹持件的位置误差也变大，但最大误差小于0.1mm；当T_s取200ms时，果柄夹持件位置分量的最大误差为e_mY＝-8.297×10^-2mm。这表明采摘机器人在连续运动状态下，基于牛顿迭代法的逆运动学求解方法具有较高的精度和较好的实时性。

表5

本发明提供的方法可以求得香蕉采摘机器人底座转角θ、末端转角

无误差的逆运动学解析解。对于果柄夹持件单个任意目标位置和姿态，且牛顿迭代初值取固定的初始位姿时，2次牛顿迭代后水平位移x和垂直位移z的逆解误差小于0.1mm，而3次迭代可得到无误差的逆解；对于果柄夹持件连续运动中各采样时刻的目标位置和姿态，若牛顿迭代初值取上一采样时刻的逆解，则只需1次迭代即可求得水平位移x和垂直位移z的高精度逆解。本发明不仅可以求出采摘机器人任意目标位姿对应的各自由度的高精度逆运动学解，而且实时性较好。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，其特征在于，包括以下步骤：

根据采摘机器人的机械结构建立采摘执行器中香蕉果柄夹持件的位置和姿态与各自由度的正运动学模型；

基于牛顿迭代算法根据香蕉果柄夹持件的目标位置和姿态求得满足设定精度的逆运动学解。

2.根据权利要求1所述的基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，其特征在于，所述采摘机器人包括履带、移动平台、水平滑块、垂直滑块、长主臂、短主臂、副臂、三角构件和采摘执行器，所述履带上设置移动平台，所述移动平台的上端和侧边分别设置用于安装水平滑块和垂直滑块的滑轨，所述水平滑块上设置短主臂，垂直滑块上设置长主臂，所述长主臂和短主臂的端部设置三角构件，所述三角构件与副臂连接，所述副臂的端部设置采摘执行器。

3.根据权利要求2所述的基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，其特征在于，所述采摘执行器包括末端转动杆、香蕉果柄夹持件和香蕉果柄切割件，所述末端转动杆与副臂的端部转动连接，所述末端转动杆与香蕉果柄夹持件连接，所述香蕉果柄夹持件上设置香蕉果柄切割件。

4.根据权利要求1所述的基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，其特征在于，所述根据采摘机器人的机械结构建立采摘执行器中香蕉果柄夹持件的位置和姿态与各自由度的正运动学模型；具体包括：

根据采摘机器人的4个自由度对果柄夹持件的位置和姿态进行定位；

确定香蕉果柄夹持件的位置、姿态与各自由度变量的函数关系式。

5.根据权利要求4所述的基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，其特征在于，所述根据采摘机器人的4个自由度对果柄夹持件的位置和姿态进行定位；具体包括：

采摘机器人设置底座转动自由度θ、水平滑块沿导轨的水平运动自由度x、垂直滑块沿导轨的垂直运动自由度z和末端转动自由度

四个自由度；

通过控制采摘机器人4个自由度的运动调节采摘执行器中香蕉果柄夹持件的位置和姿态，使其定位到香蕉串果柄切割处以便完成采摘作业；其中，对香蕉果柄夹持件的位置和姿态进行精确控制，建立4个自由度变量描述香蕉果柄夹持件位置和姿态的采摘机器人正运动学模型；

短主臂与水平方向夹角为α，副臂与水平方向夹角为β，末端转动杆与香蕉果柄夹持件的夹角为γ；在水平运动方向和垂直运动方向的交点处建立局部坐标系o-xyz，并随底座一起转动；而在底座转动中心建立基坐标系O-XYZ；其中，局部坐标系o-xyz在基坐标系O-XYZ中的位置向量p为：

p＝[p_x,p_y,p_z]＝[-364,0,657]；

考虑到采摘机器人的连杆机构和局部坐标系o-xyz随底座一起转动，故水平运动自由度和垂直运动自由度只在xoz平面内调整连杆机构的位置，因此，先在xoz平面内建立末端转动中心M的位置与水平位移x、垂直位移z的函数关系；根据采摘机器人各连杆之间的几何关系，可得节点A、C、M的位置表达式为：

式(3)中，

联立式(1)～(3)可得末端转动中心M的位置表达式为：

6.根据权利要求4所述的基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，其特征在于，确定香蕉果柄夹持件的位置、姿态与各自由度变量的函数关系式；具体包括：

确定主臂夹角α、副臂夹角β与水平位移x、垂直位移z的关系；利用半角公式可实现正弦函数和余弦函数的转换，即：

联立式(1)、(2)和半角公式(5)、(6)，同时考虑主臂夹角α的取值范围，可得主臂夹角α与水平位移x、垂直位移z的相关函数表达式为：

同理，可得副臂夹角β与水平位移x、垂直位移z的相关函数表达式为：

将sinα式(8)、cosβ式(13)代入式(4)，即可得xoz平面内末端转动中心M的位置(x_M,z_M)关于水平位移x、垂直位移z的非线性函数表达式；

结合局部坐标系o-xyz在基坐标系O-XYZ中的位姿p，同时考虑底座转角θ的作用，根据几何关系可得末端转动中心M在基坐标系O-XYZ中的位置表达式为：

对于香蕉果柄夹持件的位置和姿态，还需考虑末端转角

的作用，根据机器人的连杆结构和几何关系，可得香蕉果柄夹持件相对于末端转动中心M在基坐标系O-XYZ中的位置为：

再将M的位置表达式(15)代入式(16)可得：

将sinα式(8)、cosβ式(13)代入式(17)，即得各自由度变量θ、x、z、

描述香蕉果柄夹持件位置的函数关系式；

由于底座转角θ、末端转角

均为偏摆自由度，故香蕉果柄夹持件的姿态Ψ为：

至此，得到式(17)、(18)所示采摘机器人的正运动学模型，即香蕉果柄夹持件的位置(X_H,Y_H,Z_H)、姿态角Ψ与各自由度变量θ、x、z、

的函数关系式。

7.根据权利要求1所述的基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，其特征在于，所述基于牛顿迭代算法根据香蕉果柄夹持件的目标位置和姿态求得满足设定精度的逆运动学求解，具体包括：

根据香蕉果柄夹持件的目标位置和姿态对底座转角θ和末端转角

进行求解；

基于牛顿迭代法求解水平位移x和垂直位移z。

8.根据权利要求7所述的基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，其特征在于，所述根据香蕉果柄夹持件的目标位置和姿态对底座转角θ和末端转角

进行求解；具体包括：由于基坐标系O-XYZ建在底座转动中心，故机器人连杆在XOY平面内的投影经过基坐标系原点O，因此，先根据几何关系由香蕉果柄夹持件的目标位置(X_H,Y_H,Z_H)和姿态Ψ求得末端转动中心M的位置分量X_M、Y_M，再通过反正切函数求解底座转角θ；联立式(16)、(18)可得M的位置分量X_M、Y_M为：

因为底座转角θ的取值范围为(-π,π)，而反正切函数的取值范围为(-π/2,π/2)，因此，根据M的位置分量X_M、Y_M所处象限进行修正，可得底座转角θ的逆解表达式为：

求得底座转角θ后，再根据末端转角

的取值范围(-2π/3,2π/3)，由式(18)可得末端转角

的逆解表达式：

9.根据权利要求7所述的基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，其特征在于，所述基于牛顿迭代法求解水平位移x和垂直位移z，具体包括：

若采摘机器人的水平位移x、垂直位移z满足的方程组为F(X)，其雅可比矩阵为J(X)，其中X＝(x,z)^T，则牛顿迭代法的计算表达式为：

X_k+1＝X_k-J^-1(X_k)F(X_k) (22)

式(22)中，X_k、X_k+1分别表示第k、k+1次迭代的输出值，(k＝0,1,…,n)；J^-1(X_k)为雅可比矩阵J(X_k)的逆矩阵；

确定关于水平位移x和垂直位移z的方程组F(X)，及其雅可比矩阵的逆矩阵J^-1(X)；

选取采摘机器人正运动学模型的位置表达式(17)中X_H、Z_H分量，以及姿态Ψ表达式(18)构造关于水平位移x和垂直位移z的方程组F(x,z)，即

将sinα式(8)、cosβ式(13)代入式(23)，再分别对x、z求偏导数，可得其雅可比矩阵J(x,z)：

式(24)中，各元素的表达式为：

其中：

而式(24)所示雅可比矩阵J(x,z)的逆矩阵J^-1(x,z)计算表达式为：

将式(23)、(25)代入式(22)，即得计算采摘机器人水平位移x、垂直位移z的牛顿迭代表达式。

10.根据权利要求9所述的基于牛顿迭代法的香蕉采摘机器人逆运动学计算方法，其特征在于，所述基于牛顿迭代法求解水平位移和垂直位移，具体为，选择水平位移和垂直位移的实时值作为牛顿迭代初值，进行一次迭代获取下一时刻的水平位移和垂直位移。

Images