对于多自由度机械臂， 为了研究机械臂的运动特性， 因此需要建立多自由度机械臂的半实物仿真系统以及全数值仿真系统， 而对其动力学的研究又是其中必不可少的环节之一。考虑到实时系统下， 计算机的运算速度以及数据通讯速度， 用于模拟机械臂运动的正向动力学需满足实时性、 快速性以及稳定性。 为此，有必要研究一种针对多自由度冗余机械臂的实时动力学用于模拟机械臂的实际运动情况。

1 机械臂动力学

机械臂动力学模型矩阵元素表示的一般方程如下：

（1） $\tau = H(q)\ddot q + C(q,\dot q)\dot q+G(q)$

式中$\tau$为关节驱动力矩， $H(q)$为惯性力矩，$\ddot q$为加速度，$\dot q$为速度，$q$为关节角度，$C(q,\dot q)$为离心力项，$G(q)$为重力项。

在上述方程中，机械臂的惯量矩阵 $H(q)$将机械臂的惯量效应从矩阵中分离出来； 将离心力和科氏力效应$C(q,\dot q)$分离出来； 分离出重力效应$G(q)$，将这些矩阵称为该动力学模型的模型矩阵，其与关节角度和已知的动力学参数相关.

2 组合惯量法

适当的变形则有

(2) $\tau^* = H(q)\ddot q$

其中$\tau^*=\tau-C(q,\dot q)\dot q+G(q)$

上面在求解出$H(q)$和$\tau^*$之后便可以进一步求解出关节角加速度 $\ddot q$。 由上可以看出，正向动力学的算法需要计算$\tau^*，H(q)，\ddot q$。

而$\tau^*，H(q)$的计算涉及到机械臂的逆向动力学。因此，机械臂基于逆动力学的机械臂正向动力学的包括逆动力学计算模块、离心力和科氏力计算模块，惯性力矩阵计算模块，$\tau^*=\tau-C(q,\dot q)\dot q+G(q)$计算模块。

2.1 逆动力学模块

机械臂逆动力学即已知机械臂的运动求解加速度的过程，最简单有效的方法为牛顿欧拉递推算法。

$^{i}f_{i}=_{i+1}^{i}R^{i+1}f_{i+1}+^{i}F_i$

$^{i}n_i=^{i}N_i+_{i+1}^{i}R^{i+1}n_{i+1}+^{i}P_{C_i}\times ^{i}F_i+^{i}P_{i+1}\times _{i+1}^{i}R^{i+1}f_{i+1}$

2.2 离心力等项计算模块

关节驱动力矩可以看成与$q,\dot q,\ddot q$ 的函数。

$\tau =D(q，\dot q，\ddot q)$

对比可以看出

$C(q,\dot q)\dot q+G(q)=D(q，\dot q，0)$

说明离心力项和重力项为关节空间中产生零加速度的关节驱动力矩，且该驱动力矩可以利用机械臂逆动力学算法求出

2.3 惯量矩阵计算模块

在逆动力学中令$\dot q =0 , \ddot q =e_j$, e_j是$R^n$中第j个单位向量，由此计算出的$\tau$就是$H(q)$的第$j$列。

通过上述分析，基于关节空间惯量矩阵的算法实现流程如下：

3 数值积分

空间机械臂的正向动力学分为模型建立以及数值积分部分。模型建立过程则是为了根据当前时刻的角度、角速度以及驱动力矩求解当前时刻的角加速度，数值积分则是根据当前时刻角度、角速度以及求出的当前时刻角加速度值求解下一时刻角度和角速度。

建立空间机械臂的动力学模型，关节角加速度是关于关节角、角速度以及力矩等的函数：

$\ddot q =Q(q，\dot q，\tau )$

令$y_1=q$，$y_2=\dot q$, 则可得到

$\dot y=f(y,t)$

其中

$y=(y_1,y_2)^T$,$f=(y_2,Q(q，\dot q，\tau ))$.

机械臂关节的角度和角速度的求解构成了标准的常微分方程组的初值问题。考虑到实际控制系统，因此需要采用定步长数值积分进行计算。龙格-库塔(Runge-Kutta)方法是一种在工程上应用广泛的高精度单步算法。由于此算法精度高，采取措施对误差进行抑制，所以其实现原理也较复杂。该算法是构建在数学支持的基础之上的。通常所说的龙格-库塔法是指四阶而言的，我们可以仿二阶、三阶的情形推导出常用的标准四阶龙格-库塔法公式。龙格-库塔法具有精度高，收敛，稳定（在一定条件下），计算过程中可以改变步长，不需要计算高阶导数等优点，但仍需计算 在一些点上的值，如四阶龙格-库塔法每计算一步需要计算四次 的值，这给实际计算带来一定的复杂性，因此，多用来计算“表头”。

对于该类问题我们可以采用4阶龙格库塔积分方法求解。

$y_{n+1}=y_{n}+h*(K_1+2K_2+2K_3+K_4)/6$

$K_1=f(x_n,y_n)$

$K_2=f(x_n+h/2,y_n+h/2*K_1)$

$K_3=f(x_n+h/2,y_n+h/2*K_2)$

$K_4=f(x_n+h/2,y_n+K_3)$

4 模型验证

在Matlab中编程实现上述算法，利用Simulink中“MATLAB Fcn”模块调用M函数进行动力学计算，并将其封装为动力学计算子模块。为了验证基座位姿受控情况下空间机械臂的正向动力学算法，采用离线动力学计算软件对比仿真分析。在进行动力学模型验证时，最好使用相同的数值积分器，由于SimMechanics可以利用Simulink中的数值积分器，为此，选择多体系统仿真软件SimMechanics。在SimMechanics中建立空间七自由度机械臂模型如图。

为了验证本文建立的机械臂动力学模型，给机械臂各个关节施加相同的驱动力矩为：

sin(t),0.5cos(t),0.5sin(t),0.1cos(t),0.1sin(t).0.05sin(t),0.01sin(t) Nmm。由此得出机械臂各个关节运动情况

由上图仿真结果可以看出，三种模型仿真得出的机械臂各个关节角度以及关节角速度曲线一致，机械臂运动情况相同。由此可以验证本节两种正向动力学算法的正确性。

不同的仿真软件基于不同的动力学原理，但是其本质是一样的。衡量一个动力学模型和软件的指标是计算效率，计算精度，收敛性，稳定性，通用性和代码可移植性等。

在不同的应用场合下其应用侧重点不一样，如离线方仿真软件对计算速度要求不高而对通用性等特性要求高，而实时仿真软件则对通用性要求不高但对计算效率以及稳定性要求较高。

对于针对特定类型的对象建立的实时算法，由于不需要计算和存储多余的量，且其建模过程中已经根据对象的特点尽量避免了虚运算，因而其速度更快。

机械臂不同建模方法其物理本质是一样的，但是其计算效率以及精度却不相同，主要与变量的表示方法有关。

正向动力学的计算精度与计算过程中的截断误差以及数值积分的累计误差有关。其计算效率主要涉及计算量与存储量。计算量和存储量越大，其计算效率越低。

程序 组合体惯量法A: matlab程序—机械臂动力学建模