基于几何法的码垛机械臂逆向求解

李照祥 胡杰

摘要：逆向解算是机械臂抓取目标物的前提，是实现机械臂轨迹规划、动态导航避障的基础。常见的机械臂运动学解算方法有D-H法、解析法、POE法和人工智能法。此处，以物流、工业应用场景中常见的四轴码垛机械臂为研究对象，使用几何方法，针对其机械结构和几何特征推导出求解公式，实现码垛机械臂的定点逆向求解功能。然后，将上述逆向求解得出的结果代入D-H正向解算方法，求出的此时的末端执行器的坐标，并与目标点进行对比验证。最后，将逆向求解方法应用到实际的四轴码垛机械臂中，观察其实际运行时的执行效果。码垛机械臂的逆向求解方法的实现为机械臂开发和实景应用的设计提供了理论基础。

关键词：码垛机械臂;运动学分析;D-H建模;自动抓取

中图分类号 TP391    文献标识码：A

文章编号：1009-3044（2021）11-0216-04

Reverse Solution of Palletizing Manipulator Based on Geometric Method

LI Zhao-xiang， HU Jie

（School of Computer Science， Yangtze University，  Jingzhou 434000， China）

Abstract： Reverse calculation is the prerequisite for the robot arm to grasp the target， and it is the basis for the robot arm trajectory planning and dynamic navigation obstacle avoidance. The common kinematics calculation methods of mechanical arm include D-H method， analytical method， POE method and artificial intelligence method. Here， taking the common four-axis palletizing manipulator arm in logistics and industrial application scenarios as the research object， using geometric methods， the solution formula is derived for its mechanical structure and geometric characteristics， and the fixed-point reverse solution function of the palletizing manipulator arm is realized. Then， substitute the result obtained by the above-mentioned reverse solution into the D-H forward solution method to obtain the coordinates of the end effector at this time， and compare and verify with the target point. Finally， the inverse solution method is applied to the actual four-axis palletizing manipulator to observe its execution effect during actual operation. The realization of the inverse solution method of the palletizing manipulator provides a theoretical basis for the development of the manipulator and the design of real-world applications.

Key words： palletizing manipulator; kinematics analysis; D-H modeling; automatic grasping

1引言（Introduction）

物流行業的快速发展，传统的人工分拣方式已不再能满足物流对速度和成本的要求;工业上的机械自动化不仅能提升了生产效率，而且能降低工人劳动强度。传统上生产环节中简单又重复的分拣、搬运动作终将被机械臂取代[1]。其中，四自由度的码垛机械臂可像人工一样灵活地完成从一个平面抓起目标物，放置到另一个平面的执行动作流程，并且可以根据实际需求，选择设定固定执行序列或是动态实时响应。为了完成上述的抓取放置功能以及如何规划设计对应的生产线布局，码垛机械臂的逆向求解方法将是两者的前提基础。在已知目标物相对于机械臂基座的空间坐标位置的前提下，本文基于几何方法对四轴码垛机械臂进行分析，推导出了能自动计算码垛机械臂逆向求解方法。并使用较为熟悉的D-H方法对其进行正向运动学验证。

2机械臂模型的简化（Simplified Robotic Arm Model）

如图1所示，码垛机械臂可分为基座、大臂、小臂、末端夹具。分别在每个关节处建立笛卡尔坐标系。其中底座的电机（M1）可控制整个机械臂绕Z轴旋转，大臂和基座之间的电机（M2）可控制整个大臂绕Y轴旋转，小臂和大臂之间的电机（M3）可控制小臂绕Y轴旋转。末端夹具和小臂之间的电机（M4）可控制末端绕Y轴旋转。大臂、小臂、末端夹具的长度分别为：L1、L2、L3。

根据图1，可将其简化为如图2所示的三维空间及线段。简化流程是：可在基座中心处建立三维空间的笛卡尔坐标系，大臂、小臂、末端L型夹具的长度也可简化成三条长度为L1、L2、L3的线段。蓝色线段是机械臂的初始位置状态，红色（X0，Y0，Z0）表示待抓取点G的坐标，黑色虚线表示机械臂运动到G处的位置时，各个连杆所处的位置状态。因为机械结构的限制，小臂和大臂之间的夹角小于900，所以其抓取时的位置状态是唯一的。为了便于陈述，将三维模型图2分解为如图3和图4两部分。

当基座电机（M1）旋转r弧度后，大臂和目标点G将会处在同样一个平面内，此时就变成了如图3所示的共面情形了。如图4所示，目标点G在基座平面的投影为点G，原点和G的连线与机械臂初始位置所在平面的夹角为γ。

3逆向运动学解算（Inverse kinematics Solution）

通常，机械臂运动学求解方法有Denavit-Hartenberg法[2]（简称D-H法）、解析法和POE法[3]、基于多智能体的强化学习方法[4]、搜索方法[5]。在这里使用一种根据机械结构和本体的几何特征提取推导出的求解方法。

如图2所示，假设：末端夹具的垂直高度是h，并且末端抓手始终与基座所处的平面保持平行;机械臂位于初始位置时，大臂与基座保持垂直，小臂与大臂保持垂直，并且大臂、小臂的长度已知;电机M1、M2、M3、M4所需转动的角度分别为：（γ、α、β、θ）。

规定：γ>0表示M1左转;α>0表示M2绕坐标轴+Y2顺时针旋转、β>0表示M3绕坐标轴+Y3旋转顺时针旋转;θ>0表示M4绕坐标+Y4轴旋转。

已知目标点、大臂、小臂参数，那么一定可以构成如图3中红色虚线部分所示的等腰三角形。若目标点为：G（X0，Y0，Z0），设定γ、为M1所需转动的角度大小：

[γ‘'=arctanY0X0]                               （1）

已知末端夹具的长度和高度分别为：L3、h;可求解红色虚线三角形边长L4：

[L'4‘=X02+Y02-L3'2+Z0+h2]              （2）

至此，图3中的等腰三角形三边长均已知，且：L1=L1'、L2=L2'、L3=L3'。可根据余弦定理和正切三角函数关系求解α、β：

[α1=arccosL1'2+L4'2-L2'22·L1'·L4'α2=arcsinHL4'β1=L1'2+L2'2-L4'22L1'·L2']                   （3）

故驱动大臂和小臂转动的电机角度求解公式为：

[α=π2-α1+α2  β=π2-β1]                            （4）

根据机械臂初始位置时，末端夹具和基座所在平面保持平行关系，那么末端夹具与小臂连接处关节需转动的角度θ为：

[θ=-α+β]                                （5）

需要注意的是，M1旋转角度γ不仅与式（1）有关，还与待抓取点G所在的XOY平面投影点所在的象限有关。如图4所示，将坐标系分为四个象限，当G投影位于一、二象限时，电机M1为向左正转，反之三、四象限为反转;故γ满足如下关系式：

[][γ=γ'X0>0π-γ'X0≤0，Y0>0-π2+γ'X0≤0，Y0<0]                     （6）

综上，可求出机械臂抓取到待抓取物时各个关节相对于初始位置时的旋转角度：（γ、α、β、θ）。

4正向运动学解算（Forward kinematics Solution）

上述中描述了基于几何法的码垛机械臂逆求解公式推導过程，并给出了计算各个关节旋转角度的计算公式。为了验证上述各个关节角度求解的正确性，现在将使用由Denavit和Hartenberg于1955年提出的对机器人进行表示和建模D-H方法，参照改方法对该型号的码垛机械臂进行建模，求解相邻连杆之间的位姿变换矩阵，最后推导出正向运动学方程。

4.1 D-H参数坐标系的建立

D-H参数坐标系如图5所示，参照第5篇文献简述的方法[6]，其步骤如下：

结合实际的机械臂的构造，上述坐标系0、1、2的坐标原点重合，故L0=0;机械臂在如图5的状态下，此时各个关节的旋转角度θi应该为：

[θ0T=0-π2π2π20T]                 （7）

根据建立的坐标系求解D-H参数，如下表1：

4.2 求解坐标系之间的变换矩阵和公式

[i-1iT=cosθi-sinθi0ai-1sinθicosαi-1cosθicosαi-1-sinai-1-disinai-1sinθisinai-1cosθisinai-1cosai-1dicosai-10001]  （8）

根据表1和式（8）可得5个变换矩阵：

[01T=cosθ1-sinθ100sinθ1cosθ10000100001]  [12T=cosθ2-sinθ2000010-sinθ2-cosθ2000001]

[23T=cosθ3-sinθ30L1sinθ3cosθ30000100001] [34T=cosθ4-sinθ40L2sinθ4cosθ40000100001]

根据码垛机械臂的特点，末端夹具始终和地面保持平行的状态，并且X5和X4轴之间的方向永远保持不变，即：θ5=0。

[45T=100h00-1-L301000001]

由上述的矩阵，可求得机械臂末端夹具末端与基坐标系之间得变换关系[7]为：

[05T=01T12T23T34T45T]                               （9）

带入上述各式后得：

[05T=t11t12t13t14t21t22t23t24t31t32t33t34t41t42t43t44]（10）

其中t14，t24，t34，为夹具末端（x5，y5，z5）在基坐标系下的坐标，其表达式如下：

[t14=c1c2c3-c1s2s3（c4h+s4L3+L2）-c1c2s3+c1s2c3s4h-s4L3+c1c2L1t34=s1c2c3-s1s2s3（c4h+s4L3+L2）-（s1c2s3+s1s2c3）s4h-s4L3+s1c2L1t34=-（s2c3+c2s3）（c4h+s4L3+L2）+（s2s3-c2c3）s4h-s4L3-s2L1]  （11）

该表达式的意义为：若已知码垛机械臂各个关节的旋转角度，那么就可以求解出夹具末端相对于基坐标系的坐标值，这个过程也称为正向解算。除此之外，还有改进的Modified-DH模型方法[8]。

5 数值算例（Numerical Verification）

为了说明本码垛机械臂的逆向解算方法的准确性，接下来将给出相对于基坐标系的待抓取目标点坐标G（x，y，z），利用逆向求解方法求得每个关节需要旋转的角度;然后再根据D-H正向解算方法求得的表达式计算出夹具末端相对于基坐标系的坐标值，最后和待抓取点坐标进行比较。

需要注意的是，第2节中的逆向解算方法求出的角度（γ、α、β、θ）表示的是相对于图1中机械臂所示各个关节转动的相对变化角度。在带入式（11）时，需要结合D-H参数法中θi的几何意义对其转换，转换表达式为：

[θ1=γθ2=-π2-αθ3=π2+βθ4=π2-θ2+θ3]                      （12）

验证方法是在Linux系统下使用C++语言编写与前面对应的正逆向求解函数，对逆向求解结果带入正向D-H函数，对结果进行比较。同时将计算出的关节旋转角度通过串口通信方式发送给机械臂控制器（MCU），在不考虑机械结构静态误差的情况下，观察机械臂末端是否能够到达指定的坐标点。实验采用的实体机械臂由铝合金材料和步进电机组成，如图7所示，机械臂末端夹爪正运动到某一个待抓取目标物附近。

为了验证该逆向求解方法的正确性，在Z轴正方向所在的空间分别选择了四个待抓取目标点，分别所处四个不同象限。为了观察直观，表中Gi和Ti的单位为米（m），角度单位为度。表2为根据几何法求解机械臂末端到达Gi点所需各个关节所需旋转的角度;表3由表2经过式（12）转换得来。

首先基于几何法的码垛机械臂逆向求解方法得出的各个关节应旋转的角度（γ、α、β、θ）如表2所示;根据式（12）所示的转换规则得出如表3所示的角度θi，最后由D-H方法推导出的式（11）可正向求解出末端相对于基坐标系的坐标值Ti。经过编码测试[9]，基于几何法的码垛机械臂逆向求解方法得出的角度带入D-H正向解算公式的结果完全等于待抓取点Gi。

6 结论（Conclusion）

（1）根据码垛机械臂的连杆式机械结构，将其简化为电机位于机械臂的各个关节处，并直接驱动机械臂运动，然后在对其进行运动学分析。

（2）根据上述简化的机械臂运动模型，提出了基于几何法的码垛机械臂逆向解算方法，并推导出了机械臂末端由起始位置到达待抓取点求所需的各个关节旋转角度求解公式。

（3）为了验证该方法的正确性，根据D-H方法，对码垛机械臂进行了坐标系建模，提取出了D-H参数，并推导出了基于各个关节旋转角度求解机械臂末端相对于基坐标系坐标值的公式。

最后，根据第三节提出的几何法求解Gi位置的关节角度和式（11）的计算结果Ti进行比较，验证了基于几何法的码垛机械臂逆向求解方法的正确性，与此同时，实体机械臂末端也能移动到待抓取物体所在位置处。因此，若已知待抓取目标点，利用该方法能求解各个关节所应转动的角度，能让码垛机械臂的末端准确地到达待抓取点所处的位置，完成抓取和放置物体的功能。

参考文献：

[1] 《中国机器人产业发展报告2019》发布[J].传感器世界，2019，25（9）：42.

[2] Hartenburg R.S.，Denavit J.，Freudenstein F..Kinematic Synthesis of Linkages[J]. Journal of Applied Mechanics，1965，32（2）.

[3] 冷舒，吴克，居鹤华.机械臂运动学建模及解算方法综述[J].宇航学报，2019，40（11）：1262-1273.

[4] 趙毓，管公顺，郭继峰，等.基于多智能体强化学习的空间机械臂轨迹规划[J/OL].航空学报：1-11[2020-11-11].http：//kns.cnki.net/kcms/detail/11.1929.V.20200713.1501.004.html.

[5] 冯晨旭，胡三宝.基于改进天牛须搜索的机械臂运动学逆解[J/OL].武汉大学学报（工学版）：1-8[2020-11-11].https：//doi.org/10.14188/j.1671-8844.2020-12-012.

[6] Tony Owen. Introduction to Robotics[J]. Robotica，1991，9（4）.

[7] 朱训栋，徐海港，赵栋杰.码垛机械臂运动学位置正逆解分析[J].农业工程，2017，7（4）：155-158.

[8] 彭光宇，董洪波，马斌.两种DH模型的机器人运动学建模对比研究[J].机械研究与应用，2019，32（6）：62-65.

[9] 潘磊，钱炜，张志艳，等.四自由度机械臂运动学分析及Matlab仿真[J].机械科学与技术，2013，32（3）：421-425.

【通联编辑：梁书】