[发明专利]一种有限空间约束的注塑机械臂动力学协同仿真方法

权利要求书

1.一种有限空间约束的注塑机械臂动力学协同仿真方法，其特征在于：该方法包括以下步骤：

(1)对多自由度注塑机械臂建模：首先对机械臂的模型进行简化，运用UG三维软件建立机械臂三维模型，通过表达式驱动其尺寸变化，将模型尺寸存储数据库，任意修改、增加数据或设计变量，模型自动同步更新，实现表达式驱动机械臂长臂建模；

(2)建立注塑机械臂动力学模型：在有限空间约束的条件下，根据步骤(1)中的机械臂模型，建立机械臂的D-H空间坐标系和D-H参数，D-H空间坐标系包括系统全局坐标系和关节坐标系；

(3)根据机械臂末端的位置和方位，将笛卡尔空间映射到关节空间，求得机械臂的自由运动空间和各关节变量；

(4)运用MATLAB Robotics Toolboxlink函数建立机械臂模型；

(5)在MATLAB中建立机械臂控制系统运动学方程，驱动函数得到机械臂各关节位姿，调用drivebot函数可调节控制界面的滑条转动各关节，观察机械臂在空间中运动的轨迹、范围；

(6)根据有限空间约束条件下的实际工况，在满足机械臂运动范围、与其它零件没有干涉和符合工作要求的前提下选择一条行程最短、能量消耗最小和机械臂空间变换最少的路径；

(7)调用工具箱中jtral函数生成机械臂的关节轨迹，从而对末端路径规划； jtral函数为计算两点之间关节空间轨迹的函数，在无约束时利用五次多项式插值对速度和加速度进行求解，只要已知初始点与终止点之间的关节角度值即可确定路径的变化情况，其调用格式为[q q_d q_dd]＝jtral(qz,qr,t)，其中，qz：初始点的关节角度值，qr：终止点的关节角度值，t：初始点运动到终止点时的运行时间，利用运动学正解函数T＝fkine(rbt,q)求得机械臂初始、终止位置状态的齐次变换矩阵；

(8)逆向运动求解，由机械臂终止位置各个关节位姿，取机械臂各关节离散点，通过对所取的离散点进行连续求逆，得到θ₁-θ₆对应的六关节的角转动变量，机械臂6个关节所对应的部分转角；

(9)将UG作为MATLAB/Simulink中的运动分析计算引擎，实时提供机械臂在当前运动参数和控制参数下的运动分析，根据当前运动情况计算下一时刻机械臂的运动参数和控制参数，将MATLAB/Simulink的控制系统模块嵌入到UG中，在每个时间迭代步，计算机械臂的实时运动参数以确定最优轨迹，在协同仿真模式下，NX和MATLAB/Simulink的仿真条件根据当前系统运行条件解算确定。

2.根据权利要求1所述的一种有限空间约束的注塑机械臂动力学协同仿真方法，其特征在于：MATLAB Robotics Toolboxlink函数的link函数如下：

L＝link([alpha A theta D sigma],’CONVENTION’)

其中：CONVENTION分为标准的D-H参数的’standard’和改进的D-H参数’modified’；alpha代表扭转角；A代表杆件长度；theta代表关节角；D代表横距；sigma代表关节类型：0为旋转关节，非0为移动关节。

3.根据权利要求1所述的一种有限空间约束的注塑机械臂动力学协同仿真方法，其特征在于：步骤(7)中，初始、终止位置状态的齐次变换矩阵如下：

初始位置角度：qz＝[0,0,0,0,0,0]，位姿矩阵如下所示：

终止位置角度：qr＝[pi/2,-pi/13,pi/7,0,pi/6,0]，位姿矩阵如下所示：

4.根据权利要求1所述的一种有限空间约束的注塑机械臂动力学协同仿真方法，其特征在于：步骤(9)中，在UG运动仿真模块设定仿真接口，将处理与UG接口的全部操作编译成独立的M函数，该M函数嵌入到MATLAB/Simulink模块的动态系统仿真模型中，编译的M函数以机械臂的运动参数与控制参数为输入，以机械臂在有限空间的运动轨迹为输出，通过设定GUI参数和修改传递文件，将M函数传递给UG。

5.根据权利要求1所述的一种有限空间约束的注塑机械臂动力学协同仿真方法，其特征在于：步骤(9)中，在不考虑摩擦力等外界干扰的作用，机械臂的动力学方程为：

式中：D(q)—n×n阶对称正定的惯量矩阵；阶离心力和哥氏力项；G(q)—重力项；

基于M函数的Simulink控制器与机械臂的仿真模型函数关系式为：

式中：τ—控制器输出与受控对象输入的关节驱动力矩/力，q—关节的角速度和角位移；q_d—期望关节轨迹的角加速度，角速度和角位移；

P＝[P₁,P₂,P₃,P₄,P₅,P₆,P₇,P₈]为线性后的参数向量

式中：P₁＝I₁+m₁r₁²+(m₂+m₃)l₁²；P₂＝I₂+m₂r₂²+m₃(l₁+l₂)²；

P₃＝I₃+m₃r₃²；P₄＝m₂r₂l₁+m₃r₃(l₁+l₂)；

P₅＝m₁r₁；P₆＝m₂r₂；P₇＝m₃r₃；

P₈＝(m₂+m₃)l₁；

g—重力加速度。