智能机器人技术——刚体运动

本文最后更新于：2022年10月25日 凌晨

智能机器人技术作业记录

智能机器人技术第三章——刚体运动

  1. \[ 已知一固定的空间坐标系 \{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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
function so3mat = MatrixLog3(R)
acosinput = (trace(R) - 1) / 2;
if acosinput >= 1
    so3mat = zeros(3);
elseif acosinput <= -1
    if ~NearZero(1 + R(3, 3))
        omg = (1 / sqrt(2 * (1 + R(3, 3)))) ...
              * [R(1, 3); R(2, 3); 1 + R(3, 3)];
    elseif ~NearZero(1 + R(2, 2))
        omg = (1 / sqrt(2 * (1 + R(2, 2)))) ...
              * [R(1, 2); 1 + R(2, 2); R(3, 2)];
    else
        omg = (1 / sqrt(2 * (1 + R(1, 1)))) ...
              * [1 + R(1, 1); R(2, 1); R(3, 1)];
    end
    so3mat = VecToso3(pi * omg);
else
	theta = acos(acosinput);   
    so3mat = theta * (1 / (2 * sin(theta))) * (R - R');
end
end

so3ToVec：

1
2
3
function omg = so3ToVec(so3mat)
omg = [so3mat(3, 2); so3mat(1, 3); so3mat(2, 1)]; 
end

AxisAng3：

1
2
3
4
function [omghat, theta] = AxisAng3(expc3)
theta = norm(expc3);
omghat = expc3 / theta;
end

解得：

\[ [\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
2
3
function so3mat = VecToso3(omg)
so3mat = [0, -omg(3), omg(2); omg(3), 0, -omg(1); -omg(2), omg(1), 0];
end

MatrixExp3：

1
2
3
4
5
6
7
8
9
10
function  R = MatrixExp3(so3mat)
omgtheta = so3ToVec(so3mat);
if NearZero(norm(omgtheta))
    R = eye(3);
else
    [omghat, theta] = AxisAng3(omgtheta);
    omgmat = so3mat / theta;
    R = eye(3) + sin(theta) * omgmat + (1 - cos(theta)) * omgmat * omgmat;
end
end

\[ \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
2
3
4
5
6
7
8
9
10
11
12
13
function expmat = MatrixLog6(T)
[R, p] = TransToRp(T);
omgmat = MatrixLog3(R);
if isequal(omgmat, zeros(3))
    expmat = [zeros(3), T(1: 3, 4); 0, 0, 0, 0];
else
    theta = acos((trace(R) - 1) / 2); 
    expmat = [omgmat, (eye(3) - omgmat / 2 ...
                      + (1 / theta - cot(theta / 2) / 2) ...
                        * omgmat * omgmat / theta) * p;
              0, 0, 0, 0];  
end
end

se3ToVec：

1
2
3
function V = se3ToVec(se3mat)
V = [se3mat(3, 2); se3mat(1, 3); se3mat(2, 1); se3mat(1: 3, 4)]; 
end

AxisAng6：

1
2
3
4
5
6
7
function [S, theta] = AxisAng6(expc6)
theta = norm(expc6(1: 3));
if NearZero(theta)
    theta = norm(expc6(4: 6));
end
S = expc6 / theta;    
end

解得：

\[ [\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
2
3
function se3mat = VecTose3(V)
se3mat = [VecToso3(V(1: 3)), V(4: 6); 0, 0, 0, 0];
end

MatrixExp6：

1
2
3
4
5
6
7
8
9
10
11
12
13
function T = MatrixExp6(se3mat)omgtheta = so3ToVec(se3mat(1: 3, 1: 3));
if NearZero(norm(omgtheta))
    T = [eye(3), se3mat(1: 3, 4); 0, 0, 0, 1];
else
    [omghat, theta] = AxisAng3(omgtheta);
    omgmat = se3mat(1: 3, 1: 3) / theta; 
    T = [MatrixExp3(se3mat(1: 3, 1: 3)), ...
         (eye(3) * theta + (1 - cos(theta)) * omgmat ...
          + (theta - sin(theta)) * omgmat * omgmat) ...
            * se3mat(1: 3, 4) / theta;
         0, 0, 0, 1];
end
end

\[ \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] \]

#机器人
智能机器人技术——刚体运动
http://enderfga.cn/2022/03/21/robot2/
作者
Enderfga
发布于
2022年3月21日
许可协议