码垛机器人运动学几何解法的研究*

□郭小宝 □赵 振 □陈落根杭州娃哈哈集团机电研究院智能装备研究所 杭州 310020

码垛机器人运动学几何解法的研究*

□郭小宝 □赵 振 □陈落根
杭州娃哈哈集团机电研究院智能装备研究所 杭州 310020

机器人的正向、逆向运动学是机器人运动控制系统与机械系统连接的桥梁，其封闭解问题至今还没有通用的解法，比较通用的是代数方程组的数值迭代算法，通常难以保证实时性和精度要求。基于几何方法通过分析码垛机器人的机构特点简化运动学模型，详细推导正向运动学和逆向运动学解法，推导过程简单、直观。最后通过MATLAB编程和SolidWorks三维实体模型运动仿真，验证几何解法的正确性。

多关节重载码垛机器人在工业生产搬运领域占有重要市场，随着中国制造自动化进程的加快，得到越来越多的应用［1-2］。

机器人的正向、逆向运动学是机器人运动控制系统与机械系统连接的桥梁，是指导机器人动作指令的核心部分，简单且能够快速执行的运动学算法是机器人实现高速、高精度运行的重要手段［3-4］。

机器正向运动学是已知关节变量，求解末端执行器的空间位置和姿态；逆向运动学是已知末端执行器的位置和姿态来求解关节变量。机器人运动学算法主要有解析法和数值法，解析法根据机构的结构组成特征建立约束方程组，采用多种方法从约束方程组中消去中间参数，获得单参数多项式后再求解，方法包括矢量代数法、几何法、矩阵法和四元数法等，优点是可以得到全部解，缺点在于难度较大，方法通用性不好。数值法是直接求解约束方程组，可以通过迭代运算求得任何机构的实数解，但一般不能得到全部解，一般而言，初值选取及搜索算法对收敛性和精度影响较大［5-7］。

笔者通过分析码垛机器人的机构特点，简化运动学模型，基于传统几何方法，详细推导正向运动学和逆向运动学解法，推导过程力求简单、直观。最后通过实际工程实例，采用MATLAB编程和基于SolidWorks三维实体模型运动仿真，验证几何解法的准确性。

1 码垛机器人运动学模型

图1所示的是双平行四边形码垛机器人，其基础部件包括底座、腰部、大臂、小臂、手腕、末端法兰，实现空间范围内的三平动和绕腰部轴线转动的4个自由度。由三角架及其连杆从动部件与大臂和小臂构成三组平行四边形，实现在搬运过程中末端手腕的水平。

图1 关节型码垛机器人三维模型

机器人的机构简图如图2所示，Oi-xiyizi（i=0～5）分别为底座、腰部、大臂、小臂、手腕、末端法兰的转动中心和连体坐标系，θi（i=1～5）分别为其转动关节的关节变量，di、ai为机器人的杆长参数。

考虑机构运动特点，可以将图2简化为图3，机器人末端法兰的连体坐标系O5-x5y5z5在底座的连体坐标系O0-x0y0z0下的位置坐标为x、y、z和欧拉角姿态坐标（Z-Y-Z顺规）为φ、ψ、θ，考虑到机构为四自由度机器人，具备三移动和绕Z轴转动的4个自由度，从而φ=0、ψ=0。

机器人的杆长参数和关节变量见表1。

基于平行四边形特点，驱动关节变量满足：

θ3=-θ2-θ4（1）

图2 机器人机构简图

图3 机器人简化机构简图

表1 机器人杆长参数和关节变量

从而机器人的4个独立驱动关节变量为腰部关节θ1、大臂关节θ2、腕部关节θ4和末端法兰θ5。

机器人回零状态（大臂竖直，小臂水平）下：θ1=0°，θ2=-90°，θ3=90°，θ4=0°，θ5=0°。

2 运动学几何解法分析

机器人运动学指机器人驱动关节变量与末端刚体独立位姿坐标之间的映射关系。

2.1 正向运动学

求解操作空间变量：u=（xy zθ）T

由图3容易求得末端法兰相对于底座连体坐标系的转角θ：

θ=θ1+θ5（2）

θ3根据式（1）求得。

在x'0轴和z0轴构成的平面内，O0O5沿x'0轴的分量可表示为：

l1=a1+a2sinθ2+a3sin（θ2+θ3）+a4（3）

O0O5沿z0轴的分量即z为：

z=d1+d2+a2cosθ2+a3cos（θ2+θ3）+d5（4）

末端法兰的连体坐标系O5-x5y5z5在底座的连体坐标系O0-x0y0z0下的位置坐标x、y可通过l1求出：

x=l1cosθ1

y=l1sinθ1（5）

从而完成正向运动学求解，表示为：

x=（a1+a2sinθ2+a3sin（θ2+θ3）+a4）cosθ1

y=（a1+a2sinθ2+a3sin（θ2+θ3）+a4）sinθ1

z=d1+d2+a2cosθ2+a3cos（θ2+θ3）+d

θ=θ1+θ5

2.2 逆向运动学

根据式（5），关节1转角θ1容易由x、y求出，考虑到反正切函数不能确定θ1所在象限，从而通过四象限反正切函数求得：

θ1=arctan［2（y，x）］ （7）

由式（2）可求出θ5：

θ5=θ-θ1（8）

在三角形△O2O3O4中，边O2O4可用已知末端法兰的坐标值求得，记为m：

O2O4相对于x'0轴的夹角为α，可用已知末端法兰的坐标值求得：

三角形△O2O3O4中∠O3O2O4设为β，根据余弦定理可得：同理，求∠O2O3O4，记为γ：

由于机构特点，θ2和θ3会有两组解：｛θ2=-α-β

θ3=180°-γ或（13）

θ3根据式（1）求得。从而完成逆向运动学求解。

3 工程实例验证

3.1 MATLAB验证

在MATLAB开发环境下，根据前文的几何算法编写如图4所示的正向、逆向运动学算法脚本，脚本不足30行代码，完全顺序结构，没有任何数值迭代，具有很高的执行速度。

图4 运动学算法的MATLAB脚本截图

运动学测试点见表2，通过MATLAB校验，测试点满足正向运动学和逆向运动学算法，从而验证了几何解法的有效性和正确性。

表2 运动学算法验证点对应关系

3.2 运动仿真验证

为了反映运动学几何算法的真实性，在Solid-Works三维建模软件下，设计真实的码垛机器人模型，定义好配合和约束条件，为运动学仿真准备。

首先，规划机器人工作空间末端法兰的运动轨迹，即逆向运动学的输入u=（x y z θ）T，其中x=1 000 mm，θ=0°，在y-z平面的轨迹如图5所示。

通过逆向运动学和轨迹规划，计算出各驱动轴的关节角位移，如图6所示。

将各关节的角位移导入基于SolidWorks设计的码垛机器人模型作为各关节驱动马达的输入量，经过实时运动仿真，获得如图7所示的真实模拟轨迹，该仿真的轨迹与预先规划的轨迹完全一致，从而验证了几何运动学解法的正确性。

4 结论

基于几何学方法研究了双平行四边形码垛机器人运动学的解析算法，得出了以下结论。

图5 机器人末端控制点在y-z平面内轨迹

（1）分析了双平行四边形码垛机器人，它是三平动一转动的机构，独立驱动的关节分别为腰部、大臂、手腕和末端法兰。

（2）建立了机器人的机构简化模型，为几何法求解运动学奠定了基础。

（3）基于立体几何和矢量的方法，详细推导了正向运动学和逆向运动学算法，获得了简单直观的运动学计算公式。

图6 驱动马达的角位移曲线

（4）以MATLAB和SolidWorks软件为工具，通过三维运动仿真，验证了机器人几何解法的正确性和有效性。

图7 码垛机器人运动仿真末端真实轨迹

［1］游玮.双平行四边形高速重载搬运机器人动态设计与控制研究［D］.哈尔滨：哈尔滨工业大学，2011.

［2］王昊杰，薛强.一种三坐标并联动力头——Sprint Z3的运动学逆解及尺度综合 ［J］.机床与液压，2011（17）：53-56.

［3］黄真.高等空间机构学［M］.北京：高等教育出版社，2004.

［4］陆晶，高国琴，朱彩红，等.新型三平移并联机构的运动分析和工作空间分析 ［J］.机械设计与制造，2007（11）：163-165.

［5］ Li Yangmin，Xu Qingsong.Kinematic Analysis ofa 3-PRS Parallel Manipulator［J］.Robotics and Computer Integrated Manufacturing，2007，23（4）：395-408.

［6］刘辛军，汪劲松，李剑锋，等.一种新型空间3自由度并联机构的正反解及工作空间分析 ［J］.机械工程学报，2001（10）：36-39.

［7］赵杰，朱延河，蔡鹤皋.Delta型并联机器人运动学正解几何解法［J］.哈尔滨工业大学学报，2003（1）：25-27.

Forward，inverse kinematics of the robot is a bridge to connect control system and mechanical system for robot motion and currently there is no universal solved process for the closed form solution.Although the numerical iterative algorithm of algebraic equations is in common use，however it is difficult to guarantee timeliness and accuracy requirements.By analyzing the mechanical feature of palletizing robot to simplify kinematics model based on the method of geometry，it is possible toderive detailed solving process for the forward and the inverse kinematics.The derivation process is simple and intuitive.Finally the correctness of the Geometric solution is verified by MATLABprogramming and motion simulation of SolidWorks 3-D solid model.

码垛机器人；几何解法；正向运动学；逆向运动学

Palletizing Robot；Geometric Solution；Forward Kinematics；Inverse Kinematics

TP242.2

A

1672-0555（2015）04-044-04

*国家科技重大专项（编号：2011ZX04013-011）

2015年8月

郭小宝（1988-），男，硕士，助理工程师，主要从事机器人机构学及其运动控制的研究