CN106346480A - 一种基于ug和matlab的多自由度注塑机械臂建模方法 - Google Patents

Abstract

本发明公开了一种基于UG和MATLAB的多自由度注塑机械臂建模方法，运用UG的结构表达式驱动和MATLAB的link函数分别建立机械臂的结构模型和D‑H模型，将UG接口的全部操作编译成独立的M函数嵌入到MATLAB/Simulink模块的动态系统仿真模型中进行轨迹曲线的拟合。本发明得到在有限空间约束的条件下轨迹连续，关节平滑，末端运动时间较短，满足实际需求的一条理想轨迹通过协同仿真，以结构动力学和控制系统运动学协同仿真相结合为途径，寻求在有限空间约束下机械臂轨迹优化方法，实现了优化后轨迹关节平滑驱动。

Description

一种基于UG和MATLAB的多自由度注塑机械臂建模方法

技术领域

本发明涉及一种基于UG和MATLAB的多自由度注塑机械臂建模方法，属于机器人路径优化技术领域。

背景技术

针对多自由度注塑机械臂系统在有限空间约束条件下作高效两点重复运动，在满足机械臂性能的情况下，使系统在最短时间完成给定负载两点间抓运工作，提高生产效率，轨迹规划成为系统控制的关键问题。

文献(于天宇，李达，宋宝玉.基于MATLAB-Robotics工具箱的工业机器人轨迹规划及仿真研究[J].机械工程师，2011(7):81-83)利用D-H参数法建立机器人模型，通过调用MATLAB Robotics工具箱函数进行机器人运动学正反解运算，运用多项式插值拟合机器人运动轨迹曲线，对机器人多个关节轨迹进行规划和仿真；文献(李辉，黄文权，李开世.基于复杂路径下的六自由度机器人动力学仿真[J].机械设计与制造，2015(9):208-210)将Adams建立的机械臂导入Matlab中，利用Simulink仿真模块搭建联合仿真系统，实现机械系统与控制系统的联合仿真，但这种方法在复杂环境下可能由于干涉情况和运动策略考虑不够全面而无解。总体来说，当前国内外的研究主要集中在无约束或者前约束条件下生成和优化机械臂的运动轨迹，也有学者将机械臂的运动学性能、动力学性能和特殊的工作条件(如：运动时间、生产效率、运动空间等)等与机械臂轨迹规划相结合进行研究。在有限空间约束条件下，针对机械臂的轨迹规划化问题，基于结构动力学和控制系统运动学的协同仿真方法还未有文献记载。

发明内容

本发明要解决的技术问题是：针对多自由度注塑机械臂在有限空间约束条件下的轨迹规划问题，提供一种基于UG和MATLAB的多自由度注塑机械臂建模方法，建立了机械臂关节空间运动轨迹方程，将运动时间、生产效率、运动空间等进行约束，得到了一条满足实际工况要求的最优轨迹。

本发明采取的技术方案为：一种基于UG和MATLAB的多自由度注塑机械臂建模方法，该方法包括以下步骤：

(1)采用UG对多自由度注塑机械臂建立参数化驱动的模型；

(2)建立传递函数为M函数，M函数以机械臂的运动参数与控制参数为输入，以机械臂在有限空间的运动轨迹为输出；

(3)通过设定GUI参数和修改传递文件，将M函数传递给UG运用MATLAB RoboticsToolboxlink函数建立机械臂运动学模型，并在MATLAB中建立机械臂控制系统运动学方程；

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

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

步骤(9)中，在不考虑摩擦力等外界干扰的作用，机械臂的动力学方程为：

$D\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=\mathrm{\τ}---\left(8\right)$

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

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

$\mathrm{\τ}=f_0({\stackrel{\·\·}{q}}_d,{\stackrel{\·}{q}}_d,q_d,q,\stackrel{\·}{q})---\left(9\right)$

$\stackrel{\·\·}{q}=f_1(q,\stackrel{\·}{q},\mathrm{\τ})---\left(10\right)$

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

$D\left(q\right)=\left[\begin{array}{ccc}{P}_1+P_2+P_3+2P_4\cos q_3& P_2+P_3+P_4\cos q_3& P_3+P_4\cos q_3\\ P_2+P_3+P_4\cos q_3& P_3+P_4\cos q_3& P_4{\mathrm{cosq}}_3\\ P_3+P_4\cos q_3& P_4\cos q_3& P_3\end{array}\right]$

$C(q,\stackrel{\·}{q})=\left[\begin{array}{ccc}-2P_4{\stackrel{\·}{q}}_3\sin q_3& -P_4{\stackrel{\·}{q}}_3\sin q_3& -P_4{\stackrel{\·}{q}}_3\sin q_3\\ P_4{\stackrel{\·}{q}}_2\sin q_3& P_4{\stackrel{\·}{q}}_2\sin q_3& P_4{\stackrel{\·}{q}}_2\sin q_3\\ P_4{\stackrel{\·}{q}}_1\sin q_3& P_4{\stackrel{\·}{q}}_1\sin q_3& 0\end{array}\right]$

$G\left(q\right)=\left[\begin{array}{c}{P}_{5}g{\mathrm{cosq}}_1+P_{6}g\cos (q_1+q_2)+P_{7}g\cos (q_1+q_2+q_3)+P_{8}g\cos \left(q_1\right)\\ P_{6}g\cos (q_1+q_2)+P_{7}g\cos (q_1+q_2+q_3)\\ P_{7}g\cos (q_1+q_2+q_3)\end{array}\right]$

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—重力加速度。

本发明的有益效果：与现有技术相比，本发明效果如下；

(1)通过协同仿真，以结构动力学和控制系统运动学协同仿真相结合为途径，寻求在有限空间约束下机械臂轨迹优化方法，实现了优化后轨迹关节平滑驱动；

(2)因多自由度机械臂系统动力学模型高度复杂，耦合性强，具有非线性时变性，运用结构动力学和控制系统运动学协同仿真规划机械臂轨迹精确性高，并采用M函数的机械臂控制系统建模仿真，通用性强，是一种简单可靠、行之有效的机械臂控制系统仿真方法；

(3)以有限空间为约束条件，在机械臂轨迹优化的仿真过程中，通过M函数设定的参数按某种规律变化的时变参数，规划过程中的关节角变化、运动时间优化和末端轨迹优化等，波动小，计算稳定，收敛迅速。

附图说明

图1是有限空间约束条件下注塑机械臂模型；

图2是注塑机械臂D-H坐标系；

图3是注塑机械臂运动路径(A：起点，B：终点)

图4是注塑机械臂协同仿真前长臂关节运动轨迹；

图5是注塑机械臂协同仿真前末端运动轨迹；

图6是注塑机械臂关节角变化曲线；

图7是机械臂协同仿真机制原理；

图8是NX嵌入MATLAB/Simulink具体实现流程；

图9是控制系统协同仿真结构图；

图10是注塑机械臂末端轨迹拟合曲线；

图11是注塑机械臂协同仿真长臂关节运动轨迹图；

图12是注塑机械臂协同仿真末端运动轨迹图。

具体实施方式

下面结合附图及具体的实施例对本发明进行进一步介绍。

实施例1：一种基于UG和MATLAB的多自由度注塑机械臂建模方法，该方法包括以下步骤：

(1)对多自由度注塑机械臂建模：首先对机械臂的模型进行简化，运用UG三维软件建立机械臂三维模型，通过表达式驱动其尺寸变化，将模型尺寸存储数据库，任意修改、增加数据或设计变量，模型自动同步更新，实现表达式驱动机械臂长臂建模，有限空间约束条件下机械臂在有限空间运动轨迹示意图，如图1所示，图中，A、B：注塑机；C：机械臂；D：传输带；

表1 表达式驱动机械长臂建模

参数名称	计算公式	计算值
			P0	100	100
P1	＝6*p0	600
			P2	＝6*p0/p1	2

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

表2 机械臂D-H参数表

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

机械臂的控制需要在确定末端姿态的情况下，求解各个关节的变量值。通过运动学分析，得到机械臂末端位姿矩阵：

$P=T_6^0-T_1^0\left({\mathrm{\θ}}_1\right)T_2^1\left({\mathrm{\θ}}_2\right)T_3^2\left({\mathrm{\θ}}_3\right)T_4^3\left({\mathrm{\θ}}_4\right)T_5^4\left({\mathrm{\θ}}_5\right)T_6^5\left({\mathrm{\θ}}_6\right)=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$

工作空间决定机械臂的末端位置，θ₁-θ₆为相对起点的转动角度，通过逆运动学分析求解θ₁,θ₂,θ₃,θ₄,θ₅,θ₆ ^[7].

${\mathrm{\θ}}_1=arctan2(P_x,P_y)-arctan2(d_2,\±\sqrt{P_x+P_y-{d_2}^2})---\left(1\right)$

${\mathrm{\θ}}_3=arctan2(a_3,d_4)-arctan2(k,\±\sqrt{{a_3}^2+{d_4}^2-k^2})---\left(2\right)$

其中：

$k=\frac{{p_x}^2+{p_y}^2+{p_z}^2-{a_2}^2-{a_3}^2-{d_2}^2-{d_4}^2}{2a_2}$

式中，正、负号对应的两个解对应于θ₁的两个可能解。

$\begin{array}{c}{\mathrm{\θ}}_{23}={\mathrm{\θ}}_2+{\mathrm{\θ}}_3=arctan2\[-(a_3+a_2{c}_3)P_z(c_1{P}_x+s_1{P}_y)+(a_2{s}_3-d_4)\\ (-d_4+a_2{c}_3)P_z+(c_1{P}_x+s_1{P}_y)(a_2{s}_3+a_3)\]\end{array},---\left(3\right)$

θ₂＝θ₂₃-θ₃ (4)

θ₄＝arctan(-a_xs₁+a_yc₁,-a_xc₁c₂₃-a_ys₁c₂₃+a_zs₂₃) (5)

θ₅＝arctan2(s₅,c₅) (6)

其中：

s₅＝-a_x(c₁c₂₃c₄+s₁s₄)-a_y(s₁c₂₃c₄-c₁s₄)+a_z(s₂₃c₄)

c₅＝a_x(-c₁s₂₃)+a_y(-s₁s₂₃)+a_z(-c₂₃)

θ₆＝arctan2(s₆,c₆) (7)

其中：

s₆＝-n_x(c₁c₂₃c₄-s₁c₄)-n_y(s₁c₂₃s₄+c₁c₄)+n_z(s₂₃s₄)

c₆＝n_x[(c₁c₂₃c₄+s₁s₄)c₅-c₁s₂₃s₅]

+n_y[(s₁c₂₃c₄-c₁s₄)c₅-s₁s₂₃s₅]

-n_z(s₂₃c₄c₅+c₂₃s₅)

式中：S_i,C_i—第i(i＝1,2,…,6)个关节正弦值和余弦值。

(4)通过MATLAB Robotics Toolbox强大的矩阵计算功能，实现基于MATLAB的智能机械臂空间轨迹运动求解，运用MATLAB Robotics Toolboxlink函数建立机械臂模型；

上述步骤(4)中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为移动关节；

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

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

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

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

$\begin{array}{c}T=\\ 1.0e+003*\\ \begin{array}{cccc}0.0010& 0& 0& 1.1000\\ 0& 0.0000& 0.0010& 0\\ 0& -0.0010& 0.0000& 0.3000\\ 0& 0& 0& 0.0010\end{array}\end{array}$

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

$\begin{array}{c}T=\\ 1.0e+003*\\ \begin{array}{cccc}-0.0005& -0.0000& -0.0009& -0.0500\\ 0.0008& -0.0002& -0.0005& 1.0588\\ -0.0002& -0.0010& 0.0001& 0.3435\\ 0& 0& 0& 0.0010\end{array}\end{array}$

(8)逆向运动求解，由机械臂终止位置各个关节位姿，取机械臂各关节离散点，通过对所取的离散点进行连续求逆，得到θ₁-θ₆对应的六关节的角转动变量。机械臂6个关节所对应的部分转角，机械臂6个关节所对应的部分转角，如表3所示，图4、图5所示分别为协同仿真前长臂和末端运动轨迹图，图6所示为机械臂各关节变化曲线；

表3 机械臂各关节所对应的部分转角

(9)将UG作为MATLAB/Simulink中的运动分析计算引擎，实时提供机械臂在当前运动参数和控制参数下的运动分析，根据当前运动情况计算下一时刻机械臂的运动参数和控制参数，将MATLAB/Simulink的控制系统模块嵌入到UG中，在每个时间迭代步，计算机械臂的实时运动参数以确定最优轨迹，在协同仿真模式下，NX和MATLAB/Simulink的仿真条件根据当前系统运行条件解算确定，因此该方法与实际工况更加符合，仿真的准确度与置信度更高。协同仿真机制与机械臂控制系统设计相结合，其原理，如图7所示；

步骤(9)中，UG嵌入MATLAB/Simulink时，在UG运动仿真模块设定仿真接口，将处理与UG接口的全部操作编译成独立的M函数，该M函数嵌入到MATLAB/Simulink模块的动态系统仿真模型中，编译的M函数以机械臂的运动参数与控制参数为输入，以机械臂在有限空间的运动轨迹为输出，通过设定GUI参数和修改传递文件，将M函数传递给UG，其协同仿真的具体实现方法，如图8所示；

步骤(9)中，机械臂的动力学方程在不考虑摩擦力等外界干扰的作用时为：

$D\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=\mathrm{\τ}---\left(8\right)$

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

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

$\mathrm{\τ}=f_0({\stackrel{\·\·}{q}}_d,{\stackrel{\·}{q}}_d,q_d,q,\stackrel{\·}{q})---\left(9\right)$

$\stackrel{\·\·}{q}=f_1(q,\stackrel{\·}{q},\mathrm{\τ})---\left(10\right)$

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

$D\left(q\right)=\left[\begin{array}{ccc}{P}_1+P_2+P_3+2P_4\cos q_3& P_2+P_3+P_4\cos q_3& P_3+P_4\cos q_3\\ P_2+P_3+P_4\cos q_3& P_3+P_4\cos q_3& P_4{\mathrm{cosq}}_3\\ P_3+P_4\cos q_3& P_4\cos q_3& P_3\end{array}\right]$

$C(q,\stackrel{\·}{q})=\left[\begin{array}{ccc}-2P_4{\stackrel{\·}{q}}_3\sin q_3& -P_4{\stackrel{\·}{q}}_3\sin q_3& -P_4{\stackrel{\·}{q}}_3\sin q_3\\ P_4{\stackrel{\·}{q}}_2\sin q_3& P_4{\stackrel{\·}{q}}_2\sin q_3& P_4{\stackrel{\·}{q}}_2\sin q_3\\ P_4{\stackrel{\·}{q}}_1\sin q_3& P_4{\stackrel{\·}{q}}_1\sin q_3& 0\end{array}\right]$

$G\left(q\right)=\left[\begin{array}{c}{P}_{5}g{\mathrm{cosq}}_1+P_{6}g\cos (q_1+q_2)+P_{7}g\cos (q_1+q_2+q_3)+P_{8}g\cos \left(q_1\right)\\ P_{6}g\cos (q_1+q_2)+P_{7}g\cos (q_1+q_2+q_3)\\ P_{7}g\cos (q_1+q_2+q_3)\end{array}\right]$

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—重力加速度。

机械臂的物理参数：m₁＝4.8kg,m₂＝2.7kg,m₃＝0.7kgl₁＝0.6m,l₂＝0.4m,l₃＝0.1m；r₁＝0.3m,r₂＝0.2m,r₃＝0.05m

将M函数文件嵌入到相应模块中，Simulink中连接各个模块的连线能够传递向量，由各个模块组成的系统协同仿真控制系统结构，如图9所示。其中左侧部分为输入量，右侧部分为输出量；

为使机械臂末端轨迹光滑，将得到的轨迹曲线进行拟合，通过对平滑拟合曲线取点得到末端的转动变量，拟合曲线，如图10所示。

以有限空间为约束条件，在机械臂轨迹优化的仿真过程中，通过M函数设定的参数按某种规律变化的时变参数，规划过程中的关节角变化、运动时间优化和末端轨迹优化等，波动小，计算稳定，收敛迅速，由此可见，联合仿真在机械臂轨迹规划的过程中起到了重要作用，最终得到了满足实际需求的轨迹近似最优解。协同仿真长臂和抓手关节运动轨迹图，如图11、图12所示。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内，因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (2)

1.一种基于UG和MATLAB的多自由度注塑机械臂建模方法，其特征在于：该方法包括以下步骤：

(1)采用UG对多自由度注塑机械臂建立参数化驱动的模型；

(2)建立传递函数为M函数，M函数以机械臂的运动参数与控制参数为输入，以机械臂在有限空间的运动轨迹为输出；

(3)通过设定GUI参数和修改传递文件，将M函数传递给UG运用MATLAB RoboticsToolboxlink函数建立机械臂运动学模型，并在MATLAB中建立机械臂控制系统运动学方程；

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

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

2.根据权利要求1所述的一种基于UG和MATLAB的多自由度注塑机械臂建模方法，其特征在于：步骤(3)中，在不考虑摩擦力等外界干扰的作用，机械臂的动力学方程为：

$D\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=\mathrm{\τ}---\left(8\right)$

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

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

$\mathrm{\τ}=f_0({\stackrel{\·\·}{q}}_d,{\stackrel{\·}{q}}_d,q_d,q,\stackrel{\·}{q})---\left(9\right)$

$\stackrel{\·\·}{q}=f_1(q,\stackrel{\·}{q},\mathrm{\τ})---\left(10\right)$

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

$D\left(q\right)=\left[\begin{array}{ccc}{P}_1+P_2+P_3+2P_4\cos q_3& P_2+P_3+P_{4}cos{q}_3& P_3+P_4\cos q_3\\ P_2+P_3+P_{4}cos{q}_3& P_3+P_4\cos q_3& P_4\cos q_3\\ P_3+P_4\cos q_3& P_4\cos q_3& P_3\end{array}\right]$

$C(q,\stackrel{\·}{q})=\left[\begin{array}{ccc}-2P_4{\stackrel{\·}{q}}_3\sin q_3& -P_4{\stackrel{\·}{q}}_3\sin q_3& -P_4{\stackrel{\·}{q}}_3\sin q_3\\ P_4{\stackrel{\·}{q}}_2\sin q_3& P_4{\stackrel{\·}{q}}_2\sin q_3& P_4{\stackrel{\·}{q}}_2\sin q_3\\ P_4{\stackrel{\·}{q}}_1\sin q_3& P_4{\stackrel{\·}{q}}_1\sin q_3& 0\end{array}\right]$

$G\left(q\right)=\left[\begin{array}{c}{P}_5{\mathrm{gcosq}}_1+P_{6}g\cos (q_1+q_2)+P_{7}g\cos (q_1+q_2+q_3)+P_{8}g\cos \left(q_1\right)\\ P_{6}g\cos (q_1+q_2)+P_{7}g\cos (q_1+q_2+q_3)\\ P_{7}g\cos (q_1+q_2+q_3)\end{array}\right]$

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—重力加速度。