3D仿真学习笔记（一）-- 多体系统数理基础

看到我请叫我学C++

已于 2023-12-22 16:16:50 修改

阅读量1.9k

点赞数 4

分类专栏：三维仿真文章标签：多体系统

于 2022-01-07 16:12:35 首次发布

本文链接：https://blog.csdn.net/qq_45006390/article/details/122358573

版权

三维仿真专栏收录该内容

7 篇文章

订阅专栏

1 低序体阵列描述法

多体系统中典型体 $B_j$ 的 $n$ 阶低序体的序号定义为：

$\begin{matrix} L^n(j) = i &(1.1) \end{matrix}$

低序体算子用 $L$ 表示，满足条件：

$\begin{matrix} L^n(j) = L[L^{n-1}(j)] &(1.2) \end{matrix}$

补充以下定义：

$\begin{matrix} L^0(j) = j &(1.3) \end{matrix}$

$\begin{matrix} L^n(0)= 0 & (1.4) \end{matrix}$

特别的，当体 $B_j$ 和体 $B_i$ 为两相邻低序体时，有：

$\begin{matrix} L^1(j) = i & (1.5) \end{matrix}$

任一多体系统拓扑图如下图所示，字母 $R$ 表示惯性参考系并设为体 $B_0$ ，选 $B_0$ 的靠近体为 $B_1$ ，然后沿 $B_0$ 到 $B_1$ 的方向按自然增长数列为各体从一个分支到另一个分支进行编号。下图所示的多体系统的低序体阵列通过式 (1.1) ~ (1.5) 就可以得出，如表中所示

2 相邻典型体间的运动学描述

基于多体系统理论，采用对 $B_j$ 相邻的低序体 $B_i$ 进行运动变换的方式就可以获得 $B_j$ 的位姿。分别在惯性体 $B_0$ 、典型体 $B_j$ 和 $B_i$ 上构建右手直角坐标体系，用 $O_0-x_0y_0z_0$ 、 $O_j-x_jy_jz_j$ 和 $O_i-x_iy_iz_i$ 来表示，并且将惯性体 $B_0$ 所在的坐标系设为参考坐标系，而 $B_j$ 的位姿相对于 $B_i$ 的位姿等价于坐标系 $O_j-x_jy_jz_j$ 和 $O_i-x_iy_iz_i$ 的相对位姿，且可运用其次变换矩阵进行描述。

2.1 理想状态下相邻典型体间的运动学描述

（1）齐次线性变换

设有序数组 $(X,Y,Z)$ 与之相对应有序数组 $(X1,Y1,Z1)$ 满足以下的关系式：

$Z:\left\{\begin{matrix} X1 = b_{11}X+b_{12}Y+b_{13}Z\\ Y1 = b_{21}X + b_{22}Y + b_{23}Z\\ Z1 = b_{31}X + b_{32}Y + b_{33}Z \end{matrix}\right.$

$Z$ 把有序数组 $(X,Y,Z)$ 转换为 $(X1,Y1,Z1)$ 的一次齐次线性变换（简称齐次变化）。在 $Z$ 变换中将有序数组 $(X1,Y1,Z1)$ 称为 $(X,Y,Z)$ 的像， $(X,Y,Z)$ 则称为 $(X1,Y1,Z1)$ 的原像，齐次变换矩阵式如下：

$Z^D_C = (b_{ij}) = \begin{bmatrix} b_{11} & b_{12} & b_{13}\\ b_{21} & b_{22} & b_{23}\\ b_{31} & b_{32} & b_{33} \end{bmatrix}$

若有序数组 $(X,Y,Z)$ 表示 $C$ 空间坐标系中的 1 个任意位置坐标，而有序数组 $(X1,Y1,Z1)$ 表示 $D$ 空间坐标系中与之对应的 1 个位置坐标，则称 $Z$ 是把 $C$ 坐标系下的位置坐标 $(X,Y,Z)$ 转换到 $D$ 坐标系下的位置坐标 $(X1,Y1,Z1)$ 的一个齐次变换。

（2）三维齐次坐标变换

如下图所示，设 $e$ 点在原坐标系 $O^1(X^1,Y^1,Z^1)$ 中的坐标点为 $(x^1,y^1,z^1)$ ，当坐标系 $O^1(X^1,Y^1,Z^1)$ 沿 $X^1$ 方向移动距离 $x$ 到新坐标系 $O^2(U^2,V^2,W^2)$ 后，则 $e$ 点在新坐标系 $O^2(U^2,V^2,W^2)$ 中坐标值 $(u^2,v^2,w^2)$ 与在原坐标系 $O^1(X^1,Y^1,Z^1)$ 中坐标值 $O^1(X^1,Y^1,Z^1)$ 的关系为：

$\begin{matrix} \begin{bmatrix} x^1 \\ y^1 \\ z^1 \\ 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & x\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} u^2\\ v^2\\ w^2\\ 1 \end{bmatrix} = Trans(x) \cdot \begin{bmatrix} u^2\\ v^2\\ w^2\\ 1 \end{bmatrix} &(1.8) \end{matrix}$

$\begin{matrix} Trans(x) = \begin{bmatrix} 1 & 0 & 0 & x\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} &(1.9) \end{matrix}$

上式中 $Trans(x)$ 表示沿 $X^1$ 轴平移的平移矩阵。

同理沿 $Y^1$ ， $Z^1$ 轴平移变换矩阵为：

$\begin{matrix} Trans(y) = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & y\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} &(1.10) \end{matrix}$

$\begin{matrix} Trans(z) = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & z\\ 0 & 0 & 0 & 1 \end{bmatrix} &(1.11) \end{matrix}$

$\begin{matrix} T(I) = Trans(x) \cdot Trans(y) \cdot Trans(z) = \begin{bmatrix} 1 & 0 & 0 & x\\ 0 & 1 & 0 & y\\ 0 & 0 & 1 & z\\ 0 & 0 & 0 & 1 \end{bmatrix} &(1.12) \end{matrix}$

如下图所示，设 $e$ 点在原坐标系 $O^1(X^1,Y^1,Z^1)$ 中的坐标点为 $(x^1,y^1,z^1)$ ，当坐标系 $O^1$ 绕 $X^1$ 方向旋转角度 $\theta_x$ 至新坐标系 $O^2(U^2,V^2,W^2)$ 后，则 $e$ 点在新坐标系中的点 $(u^2,v^2,w^2)$ 与在原坐标系 $O^1(X^1,Y^1,Z^1)$ 中的坐标点 $(x^1,y^1,z^1)$ 的关系为：

$\begin{matrix} \begin{bmatrix} x^1 \\ y^1 \\ z^1 \\ 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & cos \theta_x & -sin \theta_x & 0\\ 0 & sin \theta_x & cos \theta_x & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} u^2\\ v^2\\ w^2\\ 1 \end{bmatrix} = Rot(\theta_x) \cdot \begin{bmatrix} u^2\\ v^2\\ w^2\\ 1 \end{bmatrix} &(1.13) \end{matrix}$

$\begin{matrix} Rot(\theta_x) = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & cos \theta_x & -sin \theta_x & 0\\ 0 & sin \theta_x & cos \theta_x & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} &(1.14) \end{matrix}$

同理绕 $Y^1$ ， $Z^1$ 轴旋转变换矩阵为：

$\begin{matrix} Rot(\theta_y) = \begin{bmatrix} cos \theta_y & 0 & sin \theta_y & 0\\ 0 & 1 & 0 & 0\\ -sin \theta_y & 0 & cos \theta_y & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} &(1.15) \end{matrix}$

$\begin{matrix} Rot(\theta_z) = \begin{bmatrix} cos \theta_z & -sin \theta_z & 0 & 0\\ sin \theta_z & cos \theta_z & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} &(1.16) \end{matrix}$

$\begin{matrix} T_R(I) = Rot(\theta_x) \cdot Rot(\theta_y) \cdot Rot(\theta_z) = \begin{bmatrix} cos \theta_y cos \theta_z & -cos \theta_y sin\theta_z & sin\theta_y & 0\\ sin \theta_xsin\theta_ycos\theta_z+cos\theta_xsin\theta_z & -sin\theta_xsin\theta_ysin\theta_z+cos\theta_xcos\theta_z & -sin\theta_xcos\theta_z & 0\\ -cos\theta_xsin\theta_ycos\theta_z + sin\theta_xsin\theta_z & cos\theta_xsin\theta_ysin\theta_z+sin\theta_xcos\theta_z & cos\theta_xcos\theta_y & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} &(1.17) \end{matrix}$

当坐标系 $O^1$ 分别沿 $X^1$ ， $Y^1$ 和 $Z^1$ 轴平移 $x$ ， $y$ ， $z$ 后，再绕 $X^1$ ， $Y^1$ 和 $Z^1$ 轴分别旋转 $\theta_x$ 、 $\theta_y$ 、 $\theta_z$ 角度，则坐标系 $O^1$ 中的 $e$ 点 $(x^1,y^1,z^1)$ 转换到新坐标系 $O^2(U^2,V^2,W^2)$ 的齐次变换矩阵为：

$\begin{matrix} T=T(I) \cdot T_R(I) = \begin{bmatrix} cos \theta_y cos \theta_z & -cos \theta_y sin\theta_z & sin\theta_y & x\\ sin \theta_xsin\theta_ycos\theta_z+cos\theta_xsin\theta_z & -sin\theta_xsin\theta_ysin\theta_z+cos\theta_xcos\theta_z & -sin\theta_xcos\theta_z & y\\ -cos\theta_xsin\theta_ycos\theta_z + sin\theta_xsin\theta_z & cos\theta_xsin\theta_ysin\theta_z+sin\theta_xcos\theta_z & cos\theta_xcos\theta_y & z\\ 0 & 0 & 0 & 1 \end{bmatrix} &(1.18) \end{matrix}$