柔性机器人动力学方程

机械臂的动力学在机械臂的控制中具有十分重要的意义，建立机械臂的动力学模型，是描述控制系统的依据，也是设计控制器的前提。机械臂动力学建模的常用方法是拉格朗日法和牛顿-欧拉法。采用牛顿-欧拉法建立机械臂动力学模型时，要计算每个部分加速度，然后消去内作用力，牛顿-欧拉法是解决动力学问题的力平衡方法。但是，当机械臂变得复杂，此方法的计算也将变得复杂。拉格朗日法依据的是能量平衡原理，不需要对内作用力进行求解。对于多自由度复杂度高的机械臂，拉格朗日法比牛顿-欧拉法的求解更适用。

1 刚性机械臂动力学建模

拉格朗日公式是一种基于能量的动力学方法，本节使用拉格朗日公式推导机械臂动力学方程，建立刚性关节机械臂动力学模型。拉格朗日法建立机械臂动力学模型的一般过程如下。

机械系统的动能 E和势能 P的差值为拉格朗日函数，则刚性关节机械臂的拉格朗日函数表达式为：

L\left( {q,\dot q} \right) = E\left( {q,\dot q} \right) - P\left( q \right)

式中q,\dot q为关节位置、速度，E\left( {q,\dot q} \right) 为机械臂系统的动能，P\left( q \right)为机械臂系统的势能。为得到运动的动力学方程，可对式求导，即

\frac{{\rm{d}}}{{{\rm{d}}t}}\frac{{\partial L}}{{\partial \dot q}} - \frac{{\partial L}}{{\partial q}} = \tau

\tau 为机械臂的驱动力矩，对于机械臂

\frac{{\rm{d}}}{{{\rm{d}}t}}\frac{{\partial E}}{{\partial \dot q}} - \frac{{\partial E}}{{\partial q}} + \frac{{\partial P}}{{\partial q}} = \tau

建立刚性关节机械臂动力学模型的步骤如下：

步骤1：机械臂动能的表达式

根据力学可知连杆的动能表示为：

E = \frac{1}{2}m{v^2} + \frac{1}{2}\bar I{\omega ^2}

第一项是基于连杆质心速度的动能，第二项是连杆角速度的动能。其中，m 为连杆质量，v为连杆质心的速度,\omega 为连杆的角速度，\bar I 为连杆的转动惯量。

可以通过推导出该点的位置方程来得出机械臂上一点的速度。该点的位置方程式可以通过相对于机械臂基座坐标的坐标转换矩阵{}_i^RT 来表示.机器臂末端的坐标系与机械臂基座的坐标系之间的转换，即

{}_i^RT = {}_1^RT{}_2^1T{}_3^2T \cdots {}_i^{i - 1}T = {A_1}{A_2}{A_3} \cdots {A_i}

对于一个六自由度机械臂，式列写为

{}_6^0T = {}_1^0T{}_2^1T{}_3^2T \cdots {}_6^5T = {A_1}{A_2}{A_3} \cdots {A_6}

可以得到质量单元m_i 的动能方程：

{\rm{d}}{E_i} = \frac{1}{2}Trace\left[ {\left( {\mathop \sum \limits_{p = 1}^i \left( {{U_{ip}}\frac{{{\rm{d}}{q_p}}}{{{\rm{d}}t}}} \right){r_i}} \right){{\left( {\mathop \sum \limits_{r = 1}^i \left( {{U_{ir}}\frac{{{\rm{d}}{q_r}}}{{{\rm{d}}t}}} \right){r_i}} \right)}^{\rm T}}} \right]{\rm{d}}{m_i}

对上式进行积分并整理得总动能表达式：

{K_i} = \mathop \smallint \nolimits^ {\rm{d}}{K_i} = \frac{1}{2}Trace\left[ {\mathop \sum \limits_{p = 1}^i \mathop \sum \limits_{r = 1}^i {U_{ip}}\left( {\mathop \smallint \nolimits^ {r_i}r_i^{\rm T}{\rm{d}}{m_i}} \right)U_{ir}^{\rm T}{{\dot q}_p}{{\dot q}_r}} \right]

{j_i} = \mathop \smallint \nolimits^ {r_i}r_i^{\rm T}{\rm{d}}{m_i} 为伪惯量矩阵，则机械臂系统动能的最终表达式为

{K_i} = \mathop \smallint \nolimits^ d{K_i}

步骤2：机械臂系统的势能

系统的势能是每个连杆势能的总和，机械臂系统的总势能方程如下：

P = \mathop \sum \limits_{i = 1}^n \left[ { - {m_i}{g^{\rm T}}\left( {{}_i^0T{{\bar r}_i}} \right)} \right]

步骤3：化简得到机械臂的动力学方程

化简得到机械臂的动力学方程：

{\tau _i} = \mathop \sum \limits_{j = 1}^n {H_{ij}}{\ddot q_{ij}} + \mathop \sum \limits_{j = 1}^n \mathop \sum \limits_{k = 1}^n {F_{ijk}}{\dot q_j}{\dot q_k} + {G_i}

第一部分是角速度-惯量项，第二部分是科里奥利力和向心力项，第三部分是重力项，其中，

{M_{ij}} = \mathop \sum \limits_{p = \max \left( {i,j} \right)}^n Trace\left( {{U_{pj}}{j_p}U_{pi}^{\rm T}} \right)

{C_{ijk}} = \mathop \sum \limits_{p = \max \left( {i,j,k} \right)}^n Trace\left( {{U_{pjK}}{j_p}U_{pi}^{\rm T}} \right)

{G_i} = \mathop \sum \limits_{p = i}^n - {m_p}{g^{\rm T}}{U_{pi}}{\bar r_p}

整理成矩阵形式，得到六自由度机械臂动力学方程：

\tau = M\left( q \right)\ddot q + C\left( {q,\dot q} \right)\dot q + G\left( q \right)

2 柔性关节

机械臂的柔性关节主要由动力装置（电机）、传动装置（减速器）、轴系和传感器等组成。机械臂在运动过程中，电机转角随着给定的指令而变化，关节处出现柔性材料引起的振动，而关节又驱动连杆运动，因此，柔性最终表现为连杆末端的振动。柔性关节机械臂的动力学模型受许多因素的影响，例如关节处的摩擦，柔性等非线性，关节与电子转子之间的耦合，电机的动态特性等，基于上述因素，建立柔性关节机械臂的动力学模型比较复杂，因此需要对其模型进行适当简化。本文采用Spong提出用弹簧弹性系数方法来描述柔性关节的特性。

假设1：电机的旋转轴与关节的转动轴同轴，并且电机转子是以旋转轴为对称刚体；

假设2：电机的响应速度非常很快，关节的电气动力学比机械动力学快得多，不考虑电机的动力学对系统的影响，忽略电机内部的响应时间，将电机当作一个力矩源；

假设3：将柔性关节假设是电动机转子与连杆之间的线性弹簧，即弹簧的弹性系数就是关节的刚度系数。

柔性关节控制

柔性关节示意图

关节力矩与关节刚度系数之间的关系如下

{\tau _i} = K\left( {{\theta _i} - {q_i}} \right)

弹簧的弹性势能为：

{P_{K,i}} = \frac{1}{2}{K_i}{\left( {{\theta _i} - {q_i}} \right)^2}

根据假设1、假设2、假设3，基于拉格朗日法建立柔性关节机械臂动力学模型，对于六自由度柔性关节串联机械臂，其动能包括每个连杆的动能和每个关节的电机转子的动能，势能包括每个连杆的重力势能和每个关节的弹簧的弹性势能。根据3.2.1节刚性关节机械臂动力学模型推导过程，可以得到柔性关节机械臂的动力学模型如下

M\left( q \right)\ddot q + C\left( {q,\dot q} \right)\dot q + G\left( q \right) = K\left( {\theta - q} \right)

J\ddot \theta + K\left( {\theta - q} \right) = \tau

3 状态空间模型

采用泰勒展开法对输入输出进行线性化。机械臂的状态变量定义为x = {\left[ {\begin{array}{*{20}{c}}q&{\dot q}\end{array}} \right]^{\rm T}} 泰勒展开式得到：

x\left( {t + \Delta t} \right)=\left[ {\begin{array}{*{20}{c}}{q\left( {t + \Delta t} \right)}\\{\dot q\left( {t + \Delta t} \right)}\end{array}} \right]=\left[ {\begin{array}{*{20}{c}}{q\left( t \right) + \Delta t\dot q\left( t \right) + \frac{{\Delta {t^2}}}{2}\ddot q\left( t \right)}\\{\dot q\left( t \right) + \Delta t\ddot q\left( t \right)}\end{array}} \right]

可得机械臂的状态变量x 和关节角加速度\ddot q

\ddot q = M{\left( q \right)^{ - 1}}\left( {\tau - C\left( {q,\dot q} \right)\dot q - G\left( q \right)} \right)

当前时刻的机械臂状态变量为x(n)，采样周期为T，并且通过泰勒展开式离散化机械臂的动力学方程，得到机械臂的离散状态空间方程如下：

x\left( {n + 1} \right) = x\left( {t + \left( {n + 1} \right)T} \right) = Ax\left( n \right) + B\left( \tau \right) + {G_P}

y\left( n \right) = Cx\left( n \right)

其中， A = \left[ {\begin{array}{*{20}{c}}{{I_6}}&{T{I_6}}\\{{0_{6 \times 6}}}&{{I_6}}\end{array}} \right] , B = \left[ {\begin{array}{*{20}{c}}{\frac{{{T^2}}}{2}M{{\left( q \right)}^{ - 1}}}\\{TM{{\left( q \right)}^{ - 1}}}\end{array}} \right] {G_P} = \left[ {\begin{array}{*{20}{c}}{ - \frac{{{T^2}}}{2}M{{\left( q \right)}^{ - 1}}\left( {C\left( {q,\dot q} \right)\dot q + G\left( q \right)} \right)}\\{-TM{{\left( q \right)}^{-1}}\left( {C\left( {q,\dot q} \right)\dot q + G\left( q \right)}\right)}\end{array}}\right] C = \left[ {\begin{array}{*{20}{c}}{{I_6}}&{{0_{6 \times 6}}}\end{array}} \right]

方程是非线性强耦合多变量方程，直接建立其状态空间方程比较困难。因而，可以对方程进行线性化和离散化，离散状态空间方程可以被得到。本节采用泰勒展开法对输入输出进行线性化。机械臂的状态变量定义为 x = {[\theta ,{q^{\rm{T}}},\dot \theta ,{\dot q^{\rm{T}}}]^{\rm{T}}} 记 {x_1} = \left[ {\begin{array}{*{20}{c}}\theta \\q\end{array}} \right] {x_2} = \left[ {\begin{array}{*{20}{c}}{\dot \theta }\\{\dot q}\end{array}} \right] ,x可以通过泰勒展开式得到：

x\left( {t + \Delta t} \right) = \left[ {\begin{array}{*{20}{c}}{{x_1}\left( {t + \Delta t} \right)}\\{{x_2}\left( {t + \Delta t} \right)}\end{array}} \right] = \left[ {\begin{array}{*{20}{c}}{{x_1}\left( t \right) + \Delta t{{\dot x}_1}\left( t \right) + \frac{{\Delta {t^2}}}{2}{{\ddot x}_1}\left( t \right)}\\{{x_2}\left( t \right) + \Delta t{{\dot x}_2}\left( t \right)}\end{array}} \right]

由式可得：

\ddot q = {M^{ - 1}}K\theta - {M^{ - 1}}Kq - {M^{ - 1}}C\dot q - {M^{ - 1}}G

\ddot \theta = {M^{ - 1}}\tau - {M^{ - 1}}K\theta + {M^{ - 1}}Kq

令f = {M^{ - 1}}K\theta - {M^{ - 1}}Kq - {M^{ - 1}}C\dot q - {M^{ - 1}}G ,g = - {M^{ - 1}}K\theta + {M^{ - 1}}Kq ，机械臂在当前时刻的状态变量为x(n),采样周期为T,状态变量的各个分量通过泰勒展开式进行离散，得到离散状态空间方程：

x\left( {n + 1} \right) = x\left( {t + \left( {n + 1} \right)T} \right) = Ax\left( n \right) + B{\tau ^{\rm{m}}}\left( n \right) + {G_p}

y\left( n \right) = Cx\left( n \right)

其中

A = \left[ {\begin{array}{*{20}{c}}{{I_6}}&{T{I_6}}\\{{O_{6*6}}}&{{I_6}}\end{array}} \right] B = \left[ {\begin{array}{*{20}{c}}{\begin{array}{*{20}{c}}0\\{ - \frac{{{T^2}}}{2}{M^{ - 1}}}\end{array}}\\{\begin{array}{*{20}{c}}0\\{ - T{M^{ - 1}}}\end{array}}\end{array}} \right] {G_p} = \left[ {\begin{array}{*{20}{c}}{\begin{array}{*{20}{c}}{\frac{{{T^2}}}{2}f}\\{\frac{{{T^2}}}{2}g}\end{array}}\\{\begin{array}{*{20}{c}}{Tf}\\{Tg}\end{array}}\end{array}} \right]

仿真曲线

实线为柔性关节的角度变化曲线，虚线为刚性关节的角度变化曲线，比较可得，柔性关节的角度变化与刚性关节的角度变化一致。区别在于，由于弹簧的影响，柔性关节的角度曲线在刚性关节角度的上方和下方都在较小的范围内波动。符合柔性关节的实际性能，仿真结果验证了建立的动力学模型是正确的。