双复摆模型动力学建模(三)

(5)相对树系统求解动力学方程

本章参考《多体系统动力学》(刘延柱)P95的相对坐标法求解有根树系统，利用基于高斯原理的动力学方程，直接使用结论。

$$\underline{A}\underline{\ddot{q}}=\underline{B}\text{\hspace{1em}(55)}$$

其中

$$\underline{A}={\underline{\mathbf{\alpha }}}^{\mathrm{T}}\cdot \underline{m}\underline{\mathbf{\alpha }}+{\underline{\mathbf{\beta }}}^{\mathrm{T}}\cdot \underline{\mathbf{J}}\cdot \underline{\mathbf{\beta }}\text{\hspace{1em}(56)}$$

$$\underline{B}={\underline{\mathbf{\alpha }}}^{\mathrm{T}}\cdot ({\underline{F}}^g-\underline{m}\underline{\mathbf{u}})+{\underline{\mathbf{\beta }}}^{\mathrm{T}}\cdot ({\underline{\mathbf{M}}}^g-\underline{\mathbf{J}}\cdot \underline{\mathbf{\sigma }}-\underline{\mathbf{\epsilon }})+{\underline{\mathbf{\gamma }}}^{\mathrm{T}}\cdot {\underline{\mathbf{F}}}^e+\underline{\mathbf{p}}\cdot {\underline{\mathbf{M}}}^a\text{\hspace{1em}(57)}$$

其中

$${\mathbf{\epsilon }}_i={\mathbf{\omega }}_i\times ({\mathbf{J}}_i\cdot {\mathbf{\omega }}_i)(i=1,2)$$

对于此系统，有$\underline{m}$对各刚体质量进行描述

$$\underline{m}=diag(m_1,m_2,\dots ,m_n)=\left[\begin{array}{cc}{m}_1& 0\\ 0& m_2\end{array}\right]$$

其中$m_n$为第$n$个刚体的质量

有转动惯量矩阵$\underline{\mathbf{J}}$对各方向的角速度与角动量的关系进行描述。

$$\underline{\mathbf{J}}=diag({\underline{J}}_1,{\underline{J}}_2,\dots ,{\underline{J}}_n)\left[\begin{array}{cc}{\underline{J}}_1& 0\\ 0& {\underline{J}}_2\end{array}\right]$$

默认${\underline{J}}_n$为刚体n中心惯量主轴的惯量矩阵。
$\underline{\mathbf{\alpha }}$,$\underline{\mathbf{\beta }}$上述已给出。

根据书中给出动力学方程中的一项
$${\mathbf{\epsilon }}_i={\mathbf{\omega }}_i\times ({\mathbf{J}}_i\cdot {\mathbf{\omega }}_{\mathbf{i}})$$

其中并矢与矢量的标量积可以用其列阵写为矩阵乘法

$${\mathbf{J}}_i\cdot {\mathbf{\omega }}_{\mathbf{i}}={\underline{J}}_i{\underline{\omega }}_i$$

有

$$\begin{gathered} {\mathbf{\epsilon }}_1={\mathbf{\omega }}_1\times ({\mathbf{A}}_{01}{\underline{J}}_1{\mathbf{A}}_{01}^{\mathrm{T}}\cdot {\mathbf{\omega }}_1)= \\ \left[\begin{array}{c}0\\ 0\\ {\dot{\theta }}_1\end{array}\right]\times \left[\begin{array}{c}\left[\begin{array}{ccc}C{\theta }_1& -S{\theta }_1& 0\\ 0S{\theta }_1& C{\theta }_1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{J}_{1}1& 0& 0\\ 0& J_{2}2& 0\\ 0& 0& J_{3}3\end{array}\right]\left[\begin{array}{ccc}C{\theta }_1& S{\theta }_1& 0\\ -S{\theta }_1& C{\theta }_1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ 0\\ {\dot{\theta }}_1\end{array}\right]\end{array}\right]= \\ \left[\begin{array}{c}0\\ 0\\ 0\end{array}\right] \end{gathered}$$

同理

$${\mathbf{\epsilon }}_2={\mathbf{\omega }}_2\times ({\mathbf{A}}_{02}{\underline{J}}_2{\mathbf{A}}_{02}^{\mathrm{T}}\cdot {\mathbf{\omega }}_2)=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$$

所以

$$\underline{\mathbf{\epsilon }}=\underline{0}$$

前面已经得出

$$\underline{\mathbf{\sigma }}=\underline{0}$$

外力对质心的主矢为

$${\underline{\mathbf{F}}}^g={\left[\begin{array}{cc}{\mathbf{F}}_1^g& {\mathbf{F}}_2^g\end{array}\right]}^{\mathrm{T}}$$

其中在惯性系下的列阵

$${\mathbf{F}}_1^g={\left[\begin{array}{ccc}{m}_{1}g& 0& 0\end{array}\right]}^{\mathrm{T}}$$

$${\mathbf{F}}_2^g={\left[\begin{array}{ccc}{m}_{2}g& 0& 0\end{array}\right]}^{\mathrm{T}}$$

外力对质心的主矩在惯性系下的列阵为

$${\underline{\mathbf{M}}}^g={\left[\begin{array}{cc}{\mathbf{M}}_1^g& {\mathbf{M}}_2^g\end{array}\right]}^{\mathrm{T}}={\left[\begin{array}{cc}0& 0\end{array}\right]}^{\mathrm{T}}$$

$B_{i(j)}$作用于$B_j$的主动力产生的相对于$O_j$主矩为${\mathbf{M}}_j^a$，则

$${\underline{\mathbf{M}}}^a={\left[\begin{array}{cc}{\mathbf{M}}_1^a& {\mathbf{M}}_2^a\end{array}\right]}^{\mathrm{T}}$$

这也是电机输出的力矩，即控制力矩。

系统没有力元$\underline{\mathbf{\gamma }}=0$

矢量列正$\underline{\mathbf{u}}$，在前文已经给出，至此，构建动力学方程的所以矩阵已经得到，带入式$(55)$、式$(56)$、式$(57)$中，得到（所有的向量张量都化到惯性系下计算）

$$\begin{gathered} \underline{A}=\left[\begin{array}{cc}{\mathbf{\alpha }}_{11}& {\mathbf{\alpha }}_{12}\\ {\mathbf{\alpha }}_{21}& {\mathbf{\alpha }}_{22}\end{array}\right]\cdot \left[\begin{array}{cc}{m}_1{\mathbf{\alpha }}_{11}& m_1{\mathbf{\alpha }}_{21}\\ m_2{\mathbf{\alpha }}_{12}& m_2{\mathbf{\alpha }}_{22}\end{array}\right]+\left[\begin{array}{cc}{\mathbf{e}}_3^0& {\mathbf{e}}_3^0\\ \underline{0}& {\mathbf{e}}_3^1\end{array}\right]\cdot \left[\begin{array}{cc}{\mathbf{J}}_1& 0\\ 0& {\mathbf{J}}_2\end{array}\right]\cdot \left[\begin{array}{cc}{\mathbf{e}}_3^0& \underline{0}\\ {\mathbf{e}}_3^0& {\mathbf{e}}_3^1\end{array}\right]= \\ \left[\begin{array}{cc}{m}_1{\mathbf{\alpha }}_{11}\cdot {\mathbf{\alpha }}_{11}+m_2{\mathbf{\alpha }}_{12}\cdot {\mathbf{\alpha }}_{12}& m_1{\mathbf{\alpha }}_{11}\cdot {\mathbf{\alpha }}_{21}+m_2{\mathbf{\alpha }}_{12}\cdot {\mathbf{\alpha }}_{22}\\ m_1{\mathbf{\alpha }}_{21}\cdot {\mathbf{\alpha }}_{11}+m_2{\mathbf{\alpha }}_{22}\cdot {\mathbf{\alpha }}_{12}& m_1{\mathbf{\alpha }}_{21}\cdot {\mathbf{\alpha }}_{21}+m_2{\mathbf{\alpha }}_{22}\cdot {\mathbf{\alpha }}_{22}\end{array}\right]+ \\ \left[\begin{array}{cc}{\mathbf{e}}_3^0& {\mathbf{e}}_3^0\\ \underline{0}& {\mathbf{e}}_3^1\end{array}\right]\cdot \left[\begin{array}{cc}{\underline{J}}_1{\underline{e}}_3^0& 0\\ {\underline{J}}_2{\underline{e}}_3^0& {\underline{J}}_2{\underline{e}}_3^1\end{array}\right]= \\ \left[\begin{array}{cc}{m}_1{\mathbf{\alpha }}_{11}\cdot {\mathbf{\alpha }}_{11}+m_2{\mathbf{\alpha }}_{12}\cdot {\mathbf{\alpha }}_{12}+{\mathbf{e}}_3^0{\underline{J}}_1{\underline{e}}_3^0+{\mathbf{e}}_3^0{\underline{J}}_2{\underline{e}}_3^0& m_1{\mathbf{\alpha }}_{11}\cdot {\mathbf{\alpha }}_{21}+m_2{\mathbf{\alpha }}_{12}\cdot {\mathbf{\alpha }}_{22}+{\mathbf{e}}_3^0{\underline{J}}_2{\underline{e}}_3^1\\ m_1{\mathbf{\alpha }}_{21}\cdot {\mathbf{\alpha }}_{11}+m_2{\mathbf{\alpha }}_{22}\cdot {\mathbf{\alpha }}_{12}+{\mathbf{e}}_3^1{\underline{J}}_2{\underline{e}}_3^0& m_1{\mathbf{\alpha }}_{21}\cdot {\mathbf{\alpha }}_{21}+m_2{\mathbf{\alpha }}_{22}\cdot {\mathbf{\alpha }}_{22}+{\mathbf{e}}_3^1{\underline{J}}_2{\underline{e}}_3^1\end{array}\right]= \\ \left[\begin{array}{cc}{A}_{11}& A_{12}\\ A_{21}& A_{22}\end{array}\right] \end{gathered}$$

$$A_{11}=m_1{\rho }_1^2+m_2(l^2+{\rho }_2^2+2C{\theta }_{2}l{\rho }_2)+J_1+J_2$$

$$A_{12}=m_2({\rho }_2^2+C{\theta }_{2}l{\rho }_2)+J_2$$

$$A_{21}=m_2({\rho }_2^2+C{\theta }_{2}l{\rho }_2)+J_2$$

$$A_{22}=m_2{\rho }_2^2+J_2$$

其中

$$\begin{gathered} \underline{B}=\left[\begin{array}{cc}{\mathbf{\alpha }}_{11}& {\mathbf{\alpha }}_{12}\\ {\mathbf{\alpha }}_{21}& {\mathbf{\alpha }}_{22}\end{array}\right]\cdot \left[\begin{array}{c}\left[\begin{array}{c}{\mathbf{F}}_1^g\\ {\mathbf{F}}_2^g\end{array}\right]-\left[\begin{array}{cc}{m}_1& 0\\ 0& m_2\end{array}\right]\left[\begin{array}{c}{\mathbf{u}}_1\\ {\mathbf{u}}_2\end{array}\right]\end{array}\right]+ \\ \left[\begin{array}{cc}{\mathbf{e}}_3^0& {\mathbf{e}}_3^0\\ \underline{0}& {\mathbf{e}}_3^1\end{array}\right]\cdot \left[\begin{array}{c}\left[\begin{array}{c}{\mathbf{M}}_1^g\\ {\mathbf{M}}_2^g\end{array}\right]-\left[\begin{array}{cc}{\mathbf{J}}_1& 0\\ 0& {\mathbf{J}}_2\end{array}\right]\cdot \underline{0}-\underline{0}\end{array}\right]+\underline{0}\cdot {\underline{\mathbf{F}}}^e+\left[\begin{array}{cc}[{\mathbf{e}}_3^0]& 0\\ 0& {\mathbf{A}}_{0}1[{\mathbf{e}}_3^1]\end{array}\right]\cdot \left[\begin{array}{c}{\mathbf{M}}_1^a\\ {\mathbf{M}}_2^a\end{array}\right]= \\ \left[\begin{array}{c}{\mathbf{\alpha }}_{11}\cdot ({\mathbf{F}}_1^g-m_1{\mathbf{u}}_1)+{\mathbf{\alpha }}_{12}\cdot ({\mathbf{F}}_2^g-m_2{\mathbf{u}}_2)\\ {\mathbf{\alpha }}_{21}\cdot ({\mathbf{F}}_1^g-m_1{\mathbf{u}}_1)+{\mathbf{\alpha }}_{22}\cdot ({\mathbf{F}}_2^g-m_2{\mathbf{u}}_2)\end{array}\right]+\left[\begin{array}{c}{\mathbf{e}}_3^0\cdot {\mathbf{M}}_1^g+{\mathbf{e}}_3^0{\mathbf{M}}_2^g\\ {\mathbf{e}}_3^1\cdot {\mathbf{M}}_2^g\end{array}\right]+\left[\begin{array}{c}[{\mathbf{e}}_3^0]\cdot {\mathbf{M}}_1^a\\ {\mathbf{A}}_{0}1[{\mathbf{e}}_3^1]\cdot {\mathbf{M}}_2^a\end{array}\right]= \\ \left[\begin{array}{c}\left[\begin{array}{c}-S{\theta }_1{\rho }_1\\ C{\theta }_1{\rho }_1\\ 0\end{array}\right]\cdot \left[\begin{array}{c}{m}_{1}g+m_{1}C{\theta }_1{\rho }_1{\dot{\theta }}_1{\dot{\theta }}_1\\ m_{1}S{\theta }_1{\rho }_1{\dot{\theta }}_1{\dot{\theta }}_1\\ 0\end{array}\right]+\left[\begin{array}{c}-S{\theta }_{1}l-S({\theta }_1+{\theta }_2){\rho }_2\\ C{\theta }_{1}l+C({\theta }_1+{\theta }_2){\rho }_2\\ 0\end{array}\right]\cdot \\ \left[\begin{array}{c}{m}_{2}g+m_2(C{\theta }_{1}l{\dot{\theta }}_1{\dot{\theta }}_1+C({\theta }_1+{\theta }_2){\rho }_2(\dot{{\theta }_1}+\dot{{\theta }_2})({\dot{\theta }}_1+{\dot{\theta }}_2))\\ m_2(S{\theta }_{1}l{\dot{\theta }}_1{\dot{\theta }}_1+S({\theta }_1+{\theta }_2){\rho }_2(\dot{{\theta }_1}+\dot{{\theta }_2})({\dot{\theta }}_1+{\dot{\theta }}_2))\\ 0\end{array}\right]\\ \left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]\cdot \left[\begin{array}{c}{m}_{1}g+m_{1}C{\theta }_1{\rho }_1{\dot{\theta }}_1{\dot{\theta }}_1\\ m_{1}S{\theta }_1{\rho }_1{\dot{\theta }}_1{\dot{\theta }}_1\\ 0\end{array}\right]+\left[\begin{array}{c}-S({\theta }_1+{\theta }_2){\rho }_2\\ C({\theta }_1+{\theta }_2){\rho }_2\\ 0\end{array}\right]\cdot \\ \left[\begin{array}{c}{m}_{2}g+m_2(C{\theta }_{1}l{\dot{\theta }}_1{\dot{\theta }}_1+C({\theta }_1+{\theta }_2){\rho }_2(\dot{{\theta }_1}+\dot{{\theta }_2})({\dot{\theta }}_1+{\dot{\theta }}_2))\\ m_2(S{\theta }_{1}l{\dot{\theta }}_1{\dot{\theta }}_1+S({\theta }_1+{\theta }_2){\rho }_2(\dot{{\theta }_1}+\dot{{\theta }_2})({\dot{\theta }}_1+{\dot{\theta }}_2))\\ 0\end{array}\right]\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]+\left[\begin{array}{c}{M}_1^a\\ M_2^a\end{array}\right]= \\ \left[\begin{array}{c}-S{\theta }_1{\rho }_1(m_{1}g+m_{1}C{\theta }_1{\rho }_1{\dot{\theta }}_1{\dot{\theta }}_1)+C{\theta }_1{\rho }_1(m_{1}S{\theta }_1{\rho }_1{\dot{\theta }}_1{\dot{\theta }}_1)+\\ (-S{\theta }_{1}l-S({\theta }_1+{\theta }_2){\rho }_2)(m_{2}g+m_2(C{\theta }_{1}l{\dot{\theta }}_1{\dot{\theta }}_1+C({\theta }_1+{\theta }_2){\rho }_2(\dot{{\theta }_1}+\dot{{\theta }_2})({\dot{\theta }}_1+{\dot{\theta }}_2)))+\\ (C{\theta }_{1}l+C({\theta }_1+{\theta }_2){\rho }_2)m_2(S{\theta }_{1}l{\dot{\theta }}_1{\dot{\theta }}_1+S({\theta }_1+{\theta }_2){\rho }_2(\dot{{\theta }_1}+\dot{{\theta }_2})({\dot{\theta }}_1+{\dot{\theta }}_2))+M_1^a\\ -S({\theta }_1+{\theta }_2){\rho }_2(m_{2}g+m_2(C{\theta }_{1}l{\dot{\theta }}_1{\dot{\theta }}_1+C({\theta }_1+{\theta }_2){\rho }_2(\dot{{\theta }_1}+\dot{{\theta }_2})({\dot{\theta }}_1+{\dot{\theta }}_2)))+\\ C({\theta }_1+{\theta }_2){\rho }_2(m_2(S{\theta }_{1}l{\dot{\theta }}_1{\dot{\theta }}_1+S({\theta }_1+{\theta }_2){\rho }_2(\dot{{\theta }_1}+\dot{{\theta }_2})({\dot{\theta }}_1+{\dot{\theta }}_2)))+M_2^a\end{array}\right]= \\ \left[\begin{array}{c}-m_{1}g{\rho }_{1}S{\theta }_1-m_2(g(lS{\theta }_1+{\rho }_{2}S({\theta }_1+{\theta }_2))+{\rho }_{2}l({\dot{\theta }}_2^2+2{\dot{\theta }}_1{\dot{\theta }}_2)S{\theta }_2)+M_1^a\\ -m_2{\rho }_2(gS({\theta }_1+{\theta }_2)+l{\dot{\theta }}_1^{2}S{\theta }_2)+M_2^a\end{array}\right] \end{gathered}$$

由上文定义可知

$$\underline{\ddot{q}}=\left[\begin{array}{c}\ddot{{\theta }_1}\\ \ddot{{\theta }_2}\end{array}\right]$$

将矩阵拆开可以得到

$$\begin{gathered} (m_1{\rho }_1^2+m_2(l^2+{\rho }_2^2+2C{\theta }_{2}l{\rho }_2)+J_1+J_2)\ddot{{\theta }_1}+(m_2({\rho }_2^2+C{\theta }_{2}l{\rho }_2)+J_2)\ddot{{\theta }_2}= \\ -m_{1}g{\rho }_{1}S{\theta }_1-m_2(g(lS{\theta }_1+{\rho }_{2}S({\theta }_1+{\theta }_2))+{\rho }_{2}l({\dot{\theta }}_2^2+2{\dot{\theta }}_1{\dot{\theta }}_2)S{\theta }_2)+M_1^a \end{gathered}$$

$$\begin{gathered} (m_2({\rho }_2^2+C{\theta }_{2}l{\rho }_2)+J_2)\ddot{{\theta }_1}+(m_2{\rho }_2^2+J_2)\ddot{{\theta }_2}= \\ -m_2{\rho }_2(gS({\theta }_1+{\theta }_2)+l{\dot{\theta }}_1^{2}S{\theta }_2)+M_2^a \end{gathered}$$