组合体惯量法B：原理—机械臂动力学建模

原创

ZC_Robot机器人技术

修改于 2020-10-20 22:41:58

3.7K2

修改于 2020-10-20 22:41:58

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

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中建立空间七自由度机械臂模型如图。