智能机器人技术——刚体运动
本文最后更新于:2022年10月25日 凌晨
智能机器人技术作业记录
智能机器人技术第三章——刚体运动
- \[ 已知一固定的空间坐标系 \{s\} 及其 \hat{x}_{s} 、 \hat{y}_{s} 、 \hat{z}_{s}轴坐标, 坐标系 \{a\} 的 \hat{x}_{a}轴沿 (0,0,1) 方向,\\ \hat{y}_{a} 轴沿 (-1,0,0) 方向; 坐标系 \{b\} 的 \hat{x}_{b} 轴沿 (1,0,0) 方向, \hat{y}_{b} 轴沿 (0,0,-1)方向。 \]
a) 手绘 3 个坐标系, 注意画在不同位置以便区分。
\[ 由右手定则可知,坐标系 \{a\} 的 \hat{z}_{a} 轴沿 (0,-1,0) 方向,坐标系 \{b\} 的 \hat{z}_{b} 轴沿 (0,1,0) 方向。结果如图所示: \]
b)
\[ 计算旋转矩阵 R_{s a} 和 R_{s b} \]
。
\[ \begin{aligned} R_{sa} &=\left[\begin{array}{lll} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \end{array}\right] \\ R_{sb} &=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & -1 & 0 \end{array}\right] \end{aligned} \]
c)
\[ 已知 R_{s b}, 在不使用逆矩阵的情况下计算 R_{s b}^{-1}, 并验证坐标系画的是否正确。 \]
由旋转矩阵的性质得\(R^{-1}=R^T\)
\[ \begin{aligned} R_{sb}^{-1}=R^T_{sb} &=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{array}\right] \end{aligned} \]
可知坐标系绘画正确。
d)
\[ 已知 R_{s a} 和 R_{s b}, 计算 R_{a b}, 并验证坐标系画的是否正确。 \]
\[ \begin{aligned} R_{as}=R_{sa}^{-1}=R^T_{sa} &=\left[\begin{array}{ccc} 0 & 0 & 1 \\ -1 & 0 & 0 \\ 0 & -1 & 0 \end{array}\right] \end{aligned} \]
\[ \begin{aligned} R_{ab}=R_{as}R_{sb} &=\left[\begin{array}{ccc} 0 & -1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & -1 \end{array}\right] \end{aligned} \]
可知坐标系绘画正确。
e)
\[ 将 R=R_{s b} 作为变换算子, 表示绕 \hat{x} 轴转动 -90^{\circ} 。计算 R_{1}=R_{s a} R 与 R_{2}=R R_{s a}, 并回答新姿态 R_{1} 与 R_{2} 分别对应的是 R_{s a} 绕哪个坐标系的 \hat{x} 轴转动得到的结果? \]
\[ \begin{aligned} R_{1}=R_{sa}R &=\left[\begin{array}{ccc} 0 & 0 & -1 \\ 0 & 1 & 0 \\ 1 & 0 & 0 \end{array}\right] \end{aligned} \]
\[ \begin{aligned} R_{2}=RR_{sa} &=\left[\begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right] \end{aligned} \]
\[ R_1表示绕坐标系 \{a\} 的 \hat{x}_{a} 轴,R_2表示绕坐标系 \{s\} 的 \hat{x}_{s} 轴。 \]
f)
\[ 利用 R_{s b} 将点 p_{b}=(1,2,3) 从 \{b\} 系变换到 \{s\} 系。 \]
\[ p_{s}=R_{sb}p_{b}=(1,3,-2) \]
g)
\[ 已知 \{s\} 系中一点 p_{s}=(1,2,3), 计算 p^{\prime}=R_{s b} p_{s} 和 p^{\prime \prime}=R_{s b}^{T} p_{s} 。每一推导过程均可以解释为坐标变换 (无须移动点的位置) 或移动点的位置 (无须改变坐标系)。 \]
\[ p^{\prime}=R_{s b} p_{s}=(1,3,-2):其几何意义为移动点的位置,将向量p_s绕 \hat{x} 轴转动 -90^{\circ} (无须改变坐标系)。 \]
\[ p^{\prime \prime}=R_{s b}^{T} p_{s}=(1,-3,2):其几何意义为坐标变换,将 \{s\} 系中一点 p_{s}变换到 \{b\} 系 (无须移动点的位置) 。 \]
h)已知 \(\{s\}\) 系中的角速度 \(\omega_{s}=(3,2,1)\), 计算其在 \(\{a\}\) 系中的表示。
\[ \omega_{a}=R_{as}\omega_s=(1,-3,-2) \]
i) 计算 \(R_{s a}\) 的矩阵对数 \([\widehat{\omega}] \theta\), 并提取其中的元素: 单位角速度 \(\widehat{\omega}\) 和转动量 \(\theta\) (可以利用编程手段)。
使用到的函数:
MatrixLog3:
1 |
|
so3ToVec:
1 |
|
AxisAng3:
1 |
|
解得:
\[ [\widehat{\omega}] \theta = \left[\begin{array}{ccc} 0 & -1.2092 & -1.2092 \\ 1.2092 & 0 & -1.2092 \\ 1.2092 & 1.2092 & 0 \end{array}\right] \]
单位角速度 \(\widehat{\omega}=(0.5774,-0.5774,0.5774)\) 和转动量 \(\theta=2.0944\)
j) 计算与转动 \(\widehat{\omega} \theta=(1,2,0)\) 的指数坐标对应的矩阵指数。
使用到的函数:
VecToso3:
1 |
|
MatrixExp3:
1 |
|
\[ \begin{aligned} R &=\left[\begin{array}{ccc} -0.2938 & 0.6469 & 0.7037 \\ 0.6469 & 0.6765 & -0.3518 \\ -0.7037 & 0.3518 & -0.6173 \end{array}\right] \end{aligned} \]
2.题干如 1 , 并且 \(\{a\}\) 系原点相对 \(\{s\}\) 系的坐标为 \((3,0,0),\{b\}\) 系原点相对 \(\{s\}\) 系的坐标为 \((0,2,0)\)。
a) 手绘 3 个坐标系, 注意它们之间的相对位置关系。
图中\(\{s\}\)系的三个坐标轴长度均为单位长度,以此绘得结果如下:
b)
\[ 计算齐次变换矩阵 T_{s a} 和 T_{s b} 。 \]
\[ \because T=\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]\\ \therefore T_{sa}=\left[\begin{array}{cccc} 0 & -1 & 0 & 3 \\ 0 & 0 & -1 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]\\ T_{sb}=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 2 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] \]
c)
\[ 已知 T_{s b}, 在不使用逆矩阵的情况下计算 T_{s b}^{-1}, 并验证坐标系画的是否正确。 \]
\[ \because T^{-1}=\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]^{-1}=\left[\begin{array}{cc} R^{\mathrm{T}} & -R^{\mathrm{T}} p \\ 0 & 1 \end{array}\right]\\ \therefore T_{sb}^{-1}=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 1 & 0 & -2 \\ 0 & 0 & 0 & 1 \end{array}\right] \]
可知坐标系绘画正确。
d)
\[ 已知 T_{s a} 和 T_{s b}, 计算 T_{a b}, 并验证坐标系画的是否正确。 \]
\[ \because T_{as}=T_{sa}^{-1}=\left[\begin{array}{cccc} 0 & 0 & 1 & 0 \\ -1 & 0 & 0 & 3 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]\\ \therefore T_{ab}=T_{as}T_{sb}=\left[\begin{array}{cccc} 0 & -1 & 0 & 0 \\ -1 & 0 & 0 & 3 \\ 0 & 0 & -1 & -2 \\ 0 & 0 & 0 & 1 \end{array}\right] \]
可知坐标系绘画正确。
e)
\[ 将 T=T_{s b} 作为变换算子, 表示绕\hat{x}轴转动-90^{\circ} 与沿 \hat{y} 移动 2 个单位。计算T_{1}= T_{s a} T 与 T_{2}=T T_{s a}, 并回答新姿态 T_{1} 与 T_{2} 分别对应的是 T_{s a} 绕哪个坐标系的变换得到的结果? \]
\[ T_{1}=T_{sa}T=\left[\begin{array}{cccc} 0 & 0 & -1 & 1 \\ 0 & 1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right],相对 \{a\} 系变换得到的结果,表示绕\hat{x}_{a}轴转动-90^{\circ} 与沿 \hat{x} 移动 2 个单位。 \]
\[ T_{2}=TT_{sa}=\left[\begin{array}{cccc} 0 & -1 & 0 & 3 \\ 1 & 0 & 0 & 2 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right],相对 \{s\} 系变换得到的结果,表示绕\hat{x}_{s}轴转动-90^{\circ} 与沿 \hat{x} 移动 3 个单位,沿 \hat{y} 移动 2 个单位。 \]
f)
\[ 利用 T_{s b} 将点 p_{b}=(1,2,3) 从 \{b\} 系变换到 \{s\} 系。 \]
\[ p_{s}=T_{sb}p_{b}=(1,5,-2) \]
k)
\[ 已知 \{s\} 系中一点 p_{s}=(1,2,3), 计算 p^{\prime}=T_{s b} p_{s} 和 p^{\prime \prime}=T_{s b}^{-1} p_{s} 。每一推导过程均可以解释为坐标变换 (无须移动点的位置) 或移动点的位置 (无须改变坐标系)。 \]
\[ p^{\prime}=T_{s b} p_{s}=(1,5,-2):其几何意义为移动点的位置; \]
\[ p^{\prime \prime}=T_{s b}^{-1} p_{s}=(1,-3,0):其几何意义为坐标变换。 \]
g)已知 \(\{s\}\) 系中的旋量 \(\mathcal{V}=(3,2,1,-1,-2,-3)\), 计算其在 \(\{a\}\) 系中的表示。
\[ \mathcal{V}_{a}=\left[\begin{array}{c} \omega_{a} \\ v_{a} \end{array}\right]=\left[\begin{array}{cc} R^{\mathrm{T}} & 0 \\ -R^{\mathrm{T}}[p] & R^{\mathrm{T}} \end{array}\right]\left[\begin{array}{c} \omega_{s} \\ v_{s} \end{array}\right]=\left[\mathrm{Ad}_{T_{a s}}\right] \mathcal{V}_{s}\\ \because \left[\mathrm{Ad}_{T}\right]=\left[\begin{array}{cc} R & 0 \\ {[p] R} & R \end{array}\right] \in \mathbb{R}^{6 \times 6} \\ \therefore \mathcal{V}_{a}=(1,-3,-2,-9,1,-1) \]
h) 计算 \(T_{s a}\) 的矩阵对数 \([\delta] \theta\), 并提取其中的元素: 单位螺旋轴 \(\mathcal{S}\) 和转动量 \(\theta\) 。
使用到的函数:
MatrixLog6:
1 |
|
se3ToVec:
1 |
|
AxisAng6:
1 |
|
解得:
\[ [\delta] \theta=\left[\begin{array}{cccc} 0 & -1.2092 & -1.2092 & 2.2092 \\ 1.2092 & 0 & -1.2092 & -2.2092 \\ 1.2092 & 1.2092 & 0 & -1.4184 \\ 0 & 0 & 0 & 0 \end{array}\right] \]
单位螺旋轴 \(\mathcal{S}=(0.5774,-0.5774,0.5774,1.0548,-1.0548,-0.6772)\) 和转动量 \(\theta=2.0944\)
i) 计算与转动 \(\mathcal{S} \theta=(0,1,2,3,0,0)\) 的指数坐标对应的矩阵指数。
使用到的函数:
VecTose3:
1 |
|
MatrixExp6:
1 |
|
\[ \begin{aligned} T &=\left[\begin{array}{cccc} -0.6173 & -0.7037 & 0.3518 & 1.0555 \\ 0.7037 & -0.2938 & 0.6469 & 1.9407 \\ -0.3518 & 0.6469 & 0.6765 & -0.9704 \\ 0 & 0 & 0 & 1 \end{array}\right] \end{aligned} \]
3.目前工业机器人领域经常需要定义 4 种坐标系: 参考坐标系 \(\{a\}\), 末端或工具坐标系 \(\{b\}\) 、图像坐标系 \(\{c\}\) 、件坐标系 \(\{d\}\), 如下所示。
a)
\[ 基于图中所给尺寸, 确定 T_{a d} 和 T_{c d} 。 \]
\[ T_{ad}=\left[\begin{array}{cccc} 1 & 0 & 0 & -1 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] \]
\[ T_{cd}=\left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & -1 & 2 \\ 0 & 0 & 0 & 1 \end{array}\right] \]
b)
\[ 若T_{bc}=[1~~0~~0~~4;0~~1 ~~0~~0;0~~0~~1~~0;0~~0~~0~~1],求T_{ab}。 \]
\[ T_{ab}=T_{ac}T_{cb}=T_{ad}T_{dc}T_{bc}^{-1}=T_{ad}T_{cd}^{-1}T_{bc}^{-1}\\ =\left[\begin{array}{cccc} 0 & 1 & 0 & -1 \\ 1 & 0 & 0 & -3 \\ 0 & 0 & -1 & 2 \\ 0 & 0 & 0 & 1 \end{array}\right] \]