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

动力学建模算例

1.双摆摆浪运动建模

1.1利用相对坐标法进行求解

(1) 坐标系定义

首先对模型进行简化，如下图所示

这是一个有根树系统，其中$B_i(i=0,1,2)$代表刚体，$B_0$为相对于惯性系静止的刚体，称为零刚体，$B_1$、$B_2$分别对应人的上半身和下半身。$O_i$代表铰链，$O_1$为$B_1$与$B_0$间的铰链，$O_2$为$B_2$与$B_1$间的铰链。以铰链为原点构建坐标系,令$O_0$与$O_1$重合，以$O_0$为原点构建$(O_0,\underline{{\mathbf{e}}^0})$，以$O_1$为原点构建$(O_1,\underline{{\mathbf{e}}^1})$，以$O_2$为原点构建$(O_2,\underline{{\mathbf{e}}^2})$。其中
$$\underline{{\mathbf{e}}^{\mathbf{i}}}=\left[\begin{array}{ccc}{\mathbf{e}}_1^{\mathbf{i}}& {\mathbf{e}}_2^{\mathbf{i}}& {\mathbf{e}}_3^{\mathbf{i}}\end{array}\right](i=0,1,2)\text{\hspace{1em}(1)}$$

${\mathbf{e}}_1^{\mathbf{i}}$、${\mathbf{e}}_2^{\mathbf{i}}$、${\mathbf{e}}_3^{\mathbf{i}}$为$(O_i,\underline{{\mathbf{e}}^{\mathbf{i}}})$系的三个坐标基。

涉及符号形状：
$\underline{{\mathbf{e}}^{\mathbf{i}}}$ [1X3]，元素为[3X1]向量->[3X3]
${\mathbf{e}}_{\mathbf{j}}^{\mathbf{i}}$ [3X1]，元素为标量。

(2) 多体系统的结构

根据关联矩阵与通路矩阵的定义，关联矩阵$\underline{{\mathbf{S}}_0}$、$\underline{\mathbf{S}}$和通路矩阵$\underline{\mathbf{T}}$为
$$\underline{{\mathbf{S}}_0}=\left[\begin{array}{cc}1& 0\end{array}\right]\text{\hspace{1em}(2)}$$

$$\underline{\mathbf{S}}=\left[\begin{array}{cc}-1& 1\\ 0& -1\end{array}\right]\text{\hspace{1em}(3)}$$

$$\underline{\mathbf{T}}=\left[\begin{array}{cc}-1& -1\\ 0& -1\end{array}\right]\text{\hspace{1em}(4)}$$

关联矩阵$\underline{\mathbf{S}}$表示了刚体与铰链的连接关系；通路矩阵$\underline{\mathbf{T}}$体现了内接刚体运动对外接刚体的影响。

涉及符号形状：
$\underline{\mathbf{S}}$[2X2]
$\underline{\mathbf{T}}$[2X2]

(3) 转动较系统的运动学

1)相对转动

定义广义坐标列阵$\underline{q}$与转轴矢量列正$\underline{\mathbf{p}}$
$$\underline{q}={\left[\begin{array}{cc}[{\theta }_1]& [{\theta }_2]\end{array}\right]}^{\mathrm{T}}\text{\hspace{1em}(5)}$$

$$\underline{\mathbf{p}}=diag\left(\begin{array}{cc}{\mathbf{p}}_1& {\mathbf{p}}_2\end{array}\right)=\left[\begin{array}{cc}[{\mathbf{e}}_3^0]& 0\\ 0& [{\mathbf{e}}_3^1]\end{array}\right]=\left[\begin{array}{cc}[{\mathbf{e}}_3^0]& 0\\ 0& {\mathbf{A}}_{0}1[{\mathbf{e}}_3^1]\end{array}\right]\text{\hspace{1em}(6)}$$

其中${\theta }_1$、${\theta }_2$分别对应$B_1$绕$B_0$转过的角度以及$B_2$绕$B_1$转过的角度，
$${\mathbf{e}}_3^0=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\text{\hspace{1em}(7)}$$

$${\mathbf{e}}_3^1=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\text{\hspace{1em}(8)}$$

刚体$B_1$相对于刚体$B_0$的角速度
$${\mathbf{\Omega }}_1={{\mathbf{p}}_1}^{\mathrm{T}}\dot{{\theta }_1}={\mathbf{e}}_3^0\dot{{\theta }_1}={\left[\begin{array}{ccc}0& 0& \dot{{\theta }_1}\end{array}\right]}^{\mathrm{T}}\text{\hspace{1em}(9)}$$

刚体$B_2$相对于刚体$B_1$的角速度
$${\mathbf{\Omega }}_2={{\mathbf{p}}_2}^{\mathrm{T}}\dot{{\theta }_2}={\mathbf{e}}_3^1\dot{{\theta }_2}={\left[\begin{array}{ccc}0& 0& \dot{{\theta }_2}\end{array}\right]}^{\mathrm{T}}\text{\hspace{1em}(10)}$$

则有
$$\mathbf{\Omega }={\left[\begin{array}{cc}{\mathbf{\Omega }}_1& {\mathbf{\Omega }}_2\end{array}\right]}^{\mathrm{T}}\text{\hspace{1em}(11)}$$

对于刚体$B_1$相对于刚体$B_0$的角加速度
$${\dot{\mathbf{\Omega }}}_1={{\mathbf{p}}_1}^{\mathrm{T}}\ddot{{\theta }_1}+{\mathbf{w}}_1={\left[\begin{array}{ccc}0& 0& \ddot{{\theta }_1}\end{array}\right]}^{\mathrm{T}}+{\left[\begin{array}{ccc}0& 0& \frac{\partial {\mathbf{p}}_1}{\partial {\theta }_1}\dot{{\theta }_1}\dot{{\theta }_1}\end{array}\right]}^{\mathrm{T}}\text{\hspace{1em}(12)}$$

对于刚体$B_2$相对于刚体$B_1$的角加速度
$${\dot{\mathbf{\Omega }}}_2={{\mathbf{p}}_2}^{\mathrm{T}}\ddot{{\theta }_2}+{\mathbf{w}}_2={\left[\begin{array}{ccc}0& 0& \ddot{{\theta }_2}\end{array}\right]}^{\mathrm{T}}+{\left[\begin{array}{ccc}0& 0& \frac{\partial {\mathbf{p}}_2}{\partial {\theta }_2}\dot{{\theta }_2}\dot{{\theta }_2}\end{array}\right]}^{\mathrm{T}}\text{\hspace{1em}(13)}$$

则有

$$\dot{\mathbf{\Omega }}={\left[\begin{array}{cc}{\dot{\mathbf{\Omega }}}_1& {\dot{\mathbf{\Omega }}}_2\end{array}\right]}^{\mathrm{T}}\text{\hspace{1em}(14)}$$

$$\underline{\mathbf{w}}={\left[\begin{array}{cc}\underline{{\mathbf{w}}_1}& \underline{{\mathbf{w}}_2}\end{array}\right]}^{\mathrm{T}}=\underline{0}\text{\hspace{1em}(15)}$$

其中$\underline{\mathbf{w}}$是由于转轴相对刚体的运动而产生

写成矩阵的形式有
$$\underline{\mathbf{\Omega }}={\underline{\mathbf{p}}}^{\mathrm{T}}\underline{\dot{q}}\text{\hspace{1em}(16)}$$

$$\underline{\dot{\mathbf{\Omega }}}={\underline{\mathbf{p}}}^{\mathrm{T}}\underline{\ddot{q}}+\underline{\mathbf{w}}\text{\hspace{1em}(17)}$$

2)角速度与角加速度

对于树系统{B}中的刚体，其先对于惯性空间的角速度即为零刚体$B_0$的角速度以及沿途各刚体相对角速度之和，所以有
$${\mathbf{\omega }}_1={\mathbf{\omega }}_0-\sum _{i=1}^2{T}_{i1}{\mathbf{\Omega }}_{\mathbf{i}}={\mathbf{\omega }}_0+{\mathbf{\Omega }}_1=\left[\begin{array}{c}0\\ 0\\ \dot{{\theta }_1}\end{array}\right]\text{\hspace{1em}(18)}$$

$${\mathbf{\omega }}_2={\mathbf{\omega }}_0-\sum _{i=1}^2{T}_{i2}{\mathbf{\Omega }}_{\mathbf{i}}={\mathbf{\omega }}_0+{\mathbf{\Omega }}_1+{\mathbf{\Omega }}_2=\left[\begin{array}{c}0\\ 0\\ \dot{{\theta }_1}+\dot{{\theta }_2}\end{array}\right]\text{\hspace{1em}(19)}$$

写成矩阵的形式有

$$\underline{\mathbf{\omega }}=-{\underline{T}}^{\mathrm{T}}\underline{\mathbf{\Omega }}+{\mathbf{\omega }}_0{\underline{1}}_2\text{\hspace{1em}(20)}$$

其中$\underline{1}n = \begin{bmatrix} 1_1 & 1_2 & \cdots & 1{n}\end{bmatrix}^{\mathrm{T}} $

由于
$$\underline{\mathbf{\Omega }}={\underline{\mathbf{p}}}^{\mathrm{T}}\underline{\dot{q}}\text{\hspace{1em}(21)}$$

写成广义坐标的形式有
$$\underline{\mathbf{\omega }}=-{\underline{T}}^{\mathrm{T}}{\underline{\mathbf{p}}}^{\mathrm{T}}\underline{\dot{q}}+{\mathbf{\omega }}_0{\underline{1}}_2=\underline{\mathbf{\beta }}\underline{\dot{q}}+{\mathbf{\omega }}_0{\underline{1}}_2\text{\hspace{1em}(22)}$$

其中
$$\underline{\mathbf{\beta }}=-(\underline{\mathbf{p}}\underline{\mathbf{T}}{)}^{\mathrm{T}}=\left[\begin{array}{cc}{\underline{\mathbf{p}}}_1& \underline{0}\\ {\underline{\mathbf{p}}}_1& {\underline{\mathbf{p}}}_2\end{array}\right]=\left[\begin{array}{cc}{\mathbf{e}}_3^0& \underline{0}\\ {\mathbf{e}}_3^0& {\mathbf{e}}_3^1\end{array}\right]\text{\hspace{1em}(23)}$$

对于树形结构的角加速度，对式$20$求导，得

$$\begin{gathered} {\dot{\mathbf{\omega }}}_1={\dot{\mathbf{\omega }}}_0-\sum _{i=1}^2{T}_{i1}({\dot{\mathbf{\Omega }}}_i+{\mathbf{\omega }}_1\times {\mathbf{\Omega }}_i)= \\ {\dot{\mathbf{\omega }}}_0+{\left[\begin{array}{ccc}{w}_{1y}{\dot{\theta }}_1& -w_{1x}{\dot{\theta }}_1& \ddot{{\theta }_1}+\frac{\partial {\mathbf{p}}_1}{\partial {\theta }_1}\dot{{\theta }_1}\dot{{\theta }_1}\end{array}\right]}^{\mathrm{T}}= \\ \left[\begin{array}{c}{\dot{\omega }}_{0x}+w_{1y}{\dot{\theta }}_1\\ {\dot{\omega }}_{0y}-w_{1x}{\dot{\theta }}_1\\ {\dot{\omega }}_{0z}+\ddot{{\theta }_1}+\frac{\partial {\mathbf{p}}_1}{\partial {\theta }_1}\dot{{\theta }_1}\dot{{\theta }_1}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ \ddot{{\theta }_1}+\frac{\partial {\mathbf{p}}_1}{\partial {\theta }_1}\dot{{\theta }_1}\dot{{\theta }_1}\end{array}\right]\text{\hspace{1em}(24)} \end{gathered}$$

其中${\mathbf{\omega }}_1\times {\mathbf{\Omega }}_i$是由于非惯性系相对于惯性系的转动行而产生，${\mathbf{\omega }}_0={\left[\begin{array}{ccc}{w}_{0x}& w_{0y}& w_{0z}\end{array}\right]}^{\mathrm{T}}={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^{\mathrm{T}}$，${\dot{\mathbf{\omega }}}_0={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^{\mathrm{T}}$,${\ddot{\mathbf{\omega }}}_0={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^{\mathrm{T}}$,$\mathbf{\omega }$和$\mathbf{\Omega }$均为惯性系下坐标，注意式$(11)$和式$(12)$中的$\mathbf{\Omega }$为相对系下的坐标。

$$\begin{gathered} {\dot{\mathbf{\omega }}}_2={\dot{\mathbf{\omega }}}_0-\sum _{i=1}^2{T}_{i2}({\dot{\mathbf{\Omega }}}_i+{\mathbf{\omega }}_2\times {\mathbf{\Omega }}_i)= \\ {\dot{\mathbf{\omega }}}_0+{\left[\begin{array}{ccc}{w}_{2y}{\dot{\theta }}_1& -w_{2x}{\dot{\theta }}_1& \ddot{{\theta }_1}+\frac{\partial {\mathbf{p}}_1}{\partial {\theta }_1}\dot{{\theta }_1}\dot{{\theta }_1}\end{array}\right]}^{\mathrm{T}}+{\left[\begin{array}{ccc}{w}_{2y}{\dot{\theta }}_2& -w_{2x}{\dot{\theta }}_2& \ddot{{\theta }_2}+\frac{\partial {\mathbf{p}}_2}{\partial {\theta }_2}\dot{{\theta }_2}\dot{{\theta }_2}\end{array}\right]}^{\mathrm{T}}= \\ \left[\begin{array}{c}{\dot{\omega }}_{0x}+w_{2y}{\dot{\theta }}_1+w_{2y}{\dot{\theta }}_2\\ {\dot{\omega }}_{0y}-w_{2x}{\dot{\theta }}_1-w_{2x}{\dot{\theta }}_2\\ {\dot{\omega }}_{0z}+\ddot{{\theta }_1}+\frac{\partial {\mathbf{p}}_1}{\partial {\theta }_1}\dot{{\theta }_1}\dot{{\theta }_1}+\ddot{{\theta }_2}+\frac{\partial {\mathbf{p}}_2}{\partial {\theta }_2}\dot{{\theta }_2}\dot{{\theta }_2}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ \ddot{{\theta }_1}+\frac{\partial {\mathbf{p}}_1}{\partial {\theta }_1}\dot{{\theta }_1}\dot{{\theta }_1}+\ddot{{\theta }_2}+\frac{\partial {\mathbf{p}}_2}{\partial {\theta }_2}\dot{{\theta }_2}\dot{{\theta }_2}\end{array}\right]\text{\hspace{1em}(25)} \end{gathered}$$

写成矩阵的形式有
$$\underline{\dot{\mathbf{\omega }}}=-{\underline{T}}^{\mathrm{T}}(\underline{\dot{\mathbf{\Omega }}}+\underline{\mathbf{h}})+{\dot{\mathbf{\omega }}}_0{\underline{1}}_2\text{\hspace{1em}(26)}$$

其中

$$\underline{\mathbf{h}}={\left[\begin{array}{cc}{\mathbf{\omega }}_0\times {\mathbf{\Omega }}_1& {\mathbf{\omega }}_1\times {\mathbf{\Omega }}_2\end{array}\right]}^{\mathrm{T}}={\left[\begin{array}{cc}0& 0\end{array}\right]}^{\mathrm{T}}\text{\hspace{1em}(27)}$$

由式$(16)$、式$(17)$将式$(26)$写成广义坐标的形式，有

$$\underline{\dot{\mathbf{\omega }}}=\underline{\mathbf{\beta }}\underline{\ddot{q}}+\underline{\mathbf{\sigma }}\text{\hspace{1em}(28)}$$

其中

$$\underline{\mathbf{\sigma }}={\dot{\mathbf{\omega }}}_0{\underline{1}}_2-{\underline{T}}^{\mathrm{T}}(\underline{\mathbf{w}}+\underline{\mathbf{h}})=0\text{\hspace{1em}(29)}$$

3) 体铰矢量与通路矢量

为分析刚体质心的速度与加速度，还需要刚体的尺寸、位置等信息。
定义体铰矢量${\mathbf{c}}_{ij}$如下图所示，${\mathbf{c}}_{ij}$为刚体${\mathbf{B}}_i$质心$O_{ci}$指向与其相连的铰$O_j$的矢量。

由于不相连的两个体、铰间的体铰矢量并不会在运动学和动力学方程中体现出来，因此利用$\underline{\mathbf{S}}$确定需要的体较矢量，其余置零，得到加权体铰矢量${\mathbf{C}}_{ij}$

$${\mathbf{C}}_{ij}=S_{ij}{\mathbf{c}}_{ij}\text{\hspace{1em}(29)}$$

称$\underline{\mathbf{C}}$为体铰矢量矩阵，其描述系统内各刚体上铰的分布状况。
定义通路矢量如下图所示，${\mathbf{b}}_{ij}$表示刚体${\mathbf{B}}_i$内接铰到连接${\mathbf{B}}_j$的铰的距离。
可利用$\underline{T}$求得

$$\underline{\mathbf{d}}=-\underline{\mathbf{C}}\underline{T}\text{\hspace{1em}(30)}$$

通过定义可以得到此题的$\underline{\mathbf{C}}$和$\underline{\mathbf{d}}$(会不断变化)。

$$\underline{\mathbf{C}}=\left[\begin{array}{cc}-{\mathbf{c}}_{11}& {\mathbf{c}}_{12}\\ 0& -{\mathbf{c}}_{22}\end{array}\right]\text{\hspace{1em}(31)}$$

$$\underline{\mathbf{d}}=\left[\begin{array}{cc}-{\mathbf{c}}_{11}& -{\mathbf{c}}_{11}+{\mathbf{c}}_{12}\\ 0& -{\mathbf{c}}_{22}\end{array}\right]\text{\hspace{1em}(32)}$$

其中

$${\mathbf{c}}_{11}={\mathbf{A}}_{10}{\left[\begin{array}{ccc}-{\rho }_1& 0& 0\end{array}\right]}^{\mathrm{T}}$$

$${\mathbf{c}}_{12}={\mathbf{A}}_{10}{\left[\begin{array}{ccc}l-{\rho }_1& 0& 0\end{array}\right]}^{\mathrm{T}}$$

$${\mathbf{c}}_{22}={\mathbf{A}}_{20}{\left[\begin{array}{ccc}-{\rho }_2& 0& 0\end{array}\right]}^{\mathrm{T}}$$

其中${\mathbf{A}}_{ij}$为$(O_j,\underline{{\mathbf{e}}^{\mathbf{j}}})$系转到$(O_i,\underline{{\mathbf{e}}^{\mathbf{i}}})$系的坐标转移矩阵，其中

$${\mathbf{A}}_{01}=\left[\begin{array}{ccc}C{\theta }_1& S{\theta }_1& 0\\ -S{\theta }_1& C{\theta }_1& 0\\ 0& 0& 1\end{array}\right]$$

$${\mathbf{A}}_{12}=\left[\begin{array}{ccc}C{\theta }_2& S{\theta }_2& 0\\ -S{\theta }_2& C{\theta }_2& 0\\ 0& 0& 1\end{array}\right]$$

$${\mathbf{A}}_{02}={\mathbf{A}}_{01}{\mathbf{A}}_{12}=\left[\begin{array}{ccc}C({\theta }_1+{\theta }_2)& S({\theta }_1+{\theta }_2)& 0\\ -S({\theta }_1+{\theta }_2)& C({\theta }_1+{\theta }_2)& 0\\ 0& 0& 1\end{array}\right]$$

$${\mathbf{A}}_{10}={\mathbf{A}}_{01}^{\mathrm{T}}=\left[\begin{array}{ccc}C{\theta }_1& -S{\theta }_1& 0\\ S{\theta }_1& C{\theta }_1& 0\\ 0& 0& 1\end{array}\right]$$

$${\mathbf{A}}_{21}={\mathbf{A}}_{12}^{\mathrm{T}}=\left[\begin{array}{ccc}C{\theta }_2& -S{\theta }_2& 0\\ S{\theta }_2& C{\theta }_2& 0\\ 0& 0& 1\end{array}\right]$$

$${\mathbf{A}}_{20}={\mathbf{A}}_{02}^{\mathrm{T}}=\left[\begin{array}{ccc}C({\theta }_1+{\theta }_2)& -S({\theta }_1+{\theta }_2)& 0\\ S({\theta }_1+{\theta }_2)& C({\theta }_1+{\theta }_2)& 0\\ 0& 0& 1\end{array}\right]$$

所以

$${\mathbf{c}}_{11}={\left[\begin{array}{ccc}-C{\theta }_1{\rho }_1& -S{\theta }_1{\rho }_1& 0\end{array}\right]}^{\mathrm{T}}$$

$${\mathbf{c}}_{12}={\left[\begin{array}{ccc}C{\theta }_1(l-{\rho }_1)& S{\theta }_1(l-{\rho }_1)& 0\end{array}\right]}^{\mathrm{T}}$$

$${\mathbf{c}}_{22}={\left[\begin{array}{ccc}-C({\theta }_1+{\theta }_2){\rho }_2& -S({\theta }_1+{\theta }_2){\rho }_2& 0\end{array}\right]}^{\mathrm{T}}$$

$${\mathbf{d}}_{11}={\left[\begin{array}{ccc}C{\theta }_1{\rho }_1& S{\theta }_1{\rho }_1& 0\end{array}\right]}^{\mathrm{T}}$$

$${\mathbf{d}}_{12}={\left[\begin{array}{ccc}C{\theta }_{1}l& S{\theta }_{1}l& 0\end{array}\right]}^{\mathrm{T}}$$

$${\mathbf{d}}_{22}={\left[\begin{array}{ccc}C({\theta }_1+{\theta }_2){\rho }_2& S({\theta }_1+{\theta }_2){\rho }_2& 0\end{array}\right]}^{\mathrm{T}}$$