本文的公式推导见于蔡自兴的《机器人学》，目的是得到刘金琨的《机器人控制系统的设计与MATLAB仿真》2.12示例中的动力学模型。

拉格朗日函数

拉格朗日函数$L$是系统的动能和位能之差$L=K-P$，拉格朗日方程可表示为

$$F_i=\frac{\mathbf{d}L}{\mathbf{d}t}\frac{\partial L}{\partial \dot{q}}-\frac{\partial L}{\partial q_i},i=1,2,\cdots ,n$$

式中，$q_i$为坐标，$\cdot q_i$为速度，$F_i$为力或力矩，$n$为连杆数目。

二关节机器人系统建模

现考虑二连杆机械手，如下图所示，$m_1,m_2$为两个连杆的质量，$d_1,d_2$为二者长度，$g$为重力加速度。${\theta }_1,{\theta }_2$为广义坐标，设两杆在$xOy$平面内摆动，$y$为竖直坐标轴，重力方向为$-y$，则${\theta }_1$是杆1和$-y$方向的夹角，${\theta }_2$为杆2和杆1的夹角。

则杆1的动能和位能可表示为

$$\begin{array}{rlrl}{K}_1=& \frac{1}{2}{m}_1{v}_1^2& =& \frac{1}{2}{m}_1{d}_1^2\cdot {\theta }_1^2\\ P_1=& m_{1}g{h}_1& =& -m_{1}g{d}_1\cos {\theta }_1\end{array}$$

杆2在水平和竖直方向的位置可以记作

$$\begin{gathered} x_2=d_1\sin {\theta }_1+d_2\sin ({\theta }_1+{\theta }_2) \\ {y}_2=-d_1\cos {\theta }_1-d_2\cos ({\theta }_1+{\theta }_2) \end{gathered}$$

则其速度可以表示为

$$v_2^2={\dot{x}}_2^2+{\dot{y}}_2^2=d_1\cos {\theta }_1{\dot{\theta }}_1+d_2\cos ({\theta }_1+{\theta }_2)({\dot{\theta }}_1+{\dot{\theta }}_2)+d_1\sin {\theta }_1{\dot{\theta }}_1+d_2\sin ({\theta }_1+{\theta }_2)({\dot{\theta }}_1+{\dot{\theta }}_2)$$

从而可以求得其动能和位能

$$\begin{array}{rlrl}{K}_2=& \frac{1}{2}{m}_2{v}_2^2& =& \frac{1}{2}{m}_2{d}_1^2\cdot {\theta }_1^2+\frac{1}{2}{m}_2{d}_2^2({\dot{\theta }}_1+{\dot{\theta }}_2{)}^2+m_2{d}_1{d}_2\cos {\theta }_2({\dot{\theta }}_1^2+{\dot{\theta }}_1{\dot{\theta }}_2)\\ P_2=& m_{2}g{h}_2& =& -m_{2}g{d}_2\cos {\theta }_1-m_{2}g{d}_2\cos ({\theta }_1+{\theta }_2)\end{array}$$

则二连杆机械手系统的总动能和总位能分别为

$$\begin{array}{rlrl}K& =K_1+K_2& =& \frac{1}{2}(m_1+m_2){\dot{d}}_1^2{\dot{\theta }}_1^2+\frac{1}{2}{m}_2{d}_2^2({\dot{\theta }}_1+{\dot{\theta }}_2{)}^2+m_2{d}_1{d}_2\cos {\theta }_2({\dot{\theta }}_1^2+{\dot{\theta }}_1{\dot{\theta }}_2)\\ P& =P_1+P_2& =& -(m_1+m_2)g{d}_1\cos {\theta }_1-m_{2}g{d}_2\cos ({\theta }_1+{\theta }_2)\end{array}$$

拉格朗日量$L=K-P$，这个式子太长而且就是简单地做减法，所以就不写了。

构建拉格朗日方程

$L$的偏导数为

$$\begin{array}{rl}\frac{\partial L}{\partial {\theta }_1}& =-(m_1+m_2)g{d}_1\sin {\theta }_1-m_{2}g{d}_2\sin ({\theta }_1+{\theta }_2)\\ \frac{\partial L}{\partial {\theta }_2}& =-m_2{d}_1{d}_2\sin {\theta }_2({\dot{\theta }}_1^2+{\dot{\theta }}_1{\dot{\theta }}_2)-m_{2}g{d}_2\sin ({\theta }_1+{\theta }_2)\\ \frac{\partial L}{\partial {\dot{\theta }}_1}& =(m_1+m_2)d_1^2{\dot{\theta }}_1+m_2{d}_2^2({\dot{\theta }}_1+{\dot{\theta }}_2)+m_2{d}_1{d}_2\cos {\theta }_2(2{\dot{\theta }}_1+{\dot{\theta }}_2)\\ \frac{\partial L}{\partial {\dot{\theta }}_2}& =m_2{d}_2^2{\dot{\theta }}_1+m_2{d}_2^2{\theta }_2+m_2{d}_1{d}_2\cos {\theta }_2{\dot{\theta }}_1\end{array}$$

在拉格朗日方程中，还要得到$\frac{\mathbf{d}L}{\mathbf{d}t}\frac{\partial L}{\partial \dot{q}}$，即

$$\begin{array}{rl}\frac{\mathbf{d}L}{\mathbf{d}t}\frac{\partial L}{\partial {\dot{\theta }}_1}& =(m_1+m_2)d_1^2{\ddot{\theta }}_1+m_2{d}_2^2({\ddot{\theta }}_1+{\ddot{\theta }}_2)+m_2{d}_1{d}_2\cos {\theta }_2(2{\ddot{\theta }}_1+{\ddot{\theta }}_2)-m_2{d}_1{d}_2\sin {\theta }_2{\dot{\theta }}_2(2{\dot{\theta }}_1+{\dot{\theta }}_2)\\ \frac{\mathbf{d}L}{\mathbf{d}t}\frac{\partial L}{\partial {\dot{\theta }}_2}& =m_2{d}_2^2{\ddot{\theta }}_1+m_2{d}_2^2{\dot{\theta }}_2+m_2{d}_1{d}_2(\cos {\theta }_2{\ddot{\theta }}_1-\sin {\theta }_2{\dot{\theta }}_2{\dot{\theta }}_1)\end{array}$$

将其整理后，带入拉格朗日方程

$$F_1=\frac{\mathbf{d}L}{\mathbf{d}t}\frac{\partial L}{\partial {\dot{\theta }}_1}-\frac{\partial L}{\partial {\theta }_1}{F}_2=\frac{\mathbf{d}L}{\mathbf{d}t}\frac{\partial L}{\partial {\dot{\theta }}_2}-\frac{\partial L}{\partial {\theta }_2}$$

将各项参数带入后，这两个式子可以统一表示为矩阵形式

$$\left[\begin{array}{c}{F}_1\\ F_2\end{array}\right]=\left[\begin{array}{cc}{D}_{11}& D_{12}\\ D_{21}& D_{22}\end{array}\right]\left[\begin{array}{c}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right]+\left[\begin{array}{cc}{D}_{111}& D_{122}\\ D_{211}& D_{222}\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1^2\\ {\dot{\theta }}_2^2\end{array}\right]+\left[\begin{array}{cc}{D}_{112}& D_{121}\\ D_{212}& D_{221}\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1{\dot{\theta }}_2\\ {\dot{\theta }}_2{\dot{\theta }}_1\end{array}\right]+\left[\begin{array}{c}{D}_1\\ D_2\end{array}\right]$$

其中，作用在二阶导数，即角加速度的矩阵为惯量项；作用在角速度平方的矩阵，为向心加速度系数；作用在${\dot{\theta }}_1{\dot{\theta }}_2$上的矩阵，为科氏力系数；最后的$D_1,D_2$则为重力项。其具体表达形式为

$$\begin{array}{rl}{D}_{11}& =(m_1+m_2)d_1^2+m_2{d}_2^2+2m_2{d}_1{d}_2\cos {\theta }_2\\ D_{22}& =m_2{d}_2^2\\ D_{12}& =m_2(d_2^2+d_1{d}_2\cos {\theta }_2)\\ D_{122}& =-m_2{d}_1{d}_2\sin {\theta }_2\\ D_{121}& =m_2{d}_1{d}_2\sin {\theta }_2\\ D_{122}& =D_{121}=-m_2{d}_1{d}_2\sin {\theta }_2\\ D_1& =(m_1+m_2)g{d}_1\sin {\theta }_1+m_{2}g{d}_2\sin ({\theta }_1+{\theta }_2)\\ D_2& =m_{2}g{d}_2\sin ({\theta }_1+{\theta }_2)\end{array}$$

其余参数为0。

理想动力学模型

记$\Theta =\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\end{array}\right],F=\left[\begin{array}{c}{F}_1\\ F_2\end{array}\right]$，将向心力和科氏力合二为一，且不考虑重力干扰，则上式可记作

$$F=D(\Theta )\ddot{\Theta }+C(\Theta ,\dot{\Theta })\dot{\Theta }$$

记$p_1=(m_1+m_2)d_1^2,p_2=m_2{d}_2^2,p_3=m_2{d}_1{d}_2$，则

$$\begin{array}{r}D(\Theta )=\left[\begin{array}{cc}{p}_1+p_2+2p_3\cos {\theta }_2& p_2+p_3\cos {\theta }_2\\ p_2+p_3\cos {\theta }_2& p_2\end{array}\right]\\ C(\Theta ,\dot{\Theta })=\left[\begin{array}{cc}-p_3{\dot{\theta }}_2\sin {\theta }_2& -p_3({\dot{\theta }}_1+{\dot{\theta }}_2)\sin {\theta }_2\\ p_3{\dot{\theta }}_1\sin {\theta }_2& 0\end{array}\right]\end{array}$$

此即 刘金琨的《机器人控制系统的设计与MATLAB仿真》2.12示例中的动力学模型。