智能机器人技术作业记录

智能机器人技术——一阶运动学与静力学

一、计算/解答题, 请写出解题过程 (30 分)。

1. 如下图所示有一处于初始位形的 RRP 机器人 (即讲义例 3 ), 求 (10 分):

1. 写出各关节相对空间坐标系 \(\{s\}\) 的旋量坐标。求解当 \(\theta=\left(90^{\circ}, 90^{\circ}, 1\right)\) 时的正向运动学; 手绘此时的机器人, 标注 \(\{s\}\) 系和 \(\{b\}\) 系, 求解此时的空间雅克比 \(J_{s}\);

2. 写出各关节相对末端坐标系 \(\{b\}\) 的旋量坐标。求解当 \(\theta=\left(90^{\circ}, 90^{\circ}, 1\right)\) 时的正向运动学, 确认与 a) 中结果相同; 求解此时的物体雅克比 \(J_{b}\) 。

此时的机器人如图所示：

初始位形M：

\[ M=\left[\begin{array}{cccc} -1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 3 \\ 0 & 1 & 0 & 2 \\ 0 & 0 & 0 & 1 \end{array}\right] \]

相对空间坐标系 \(\{s\}\) 的旋量坐标为：

\[ \mathcal{S}_{i}：~\begin{array}{|c||c|c|} \hline i & \omega_{i} & v_{i} \\ \hline \hline 1 & (0,0,1) & (0, 0,0) \\ \hline 2 & (1,0,0) & (0, 2,0) \\ \hline 3 & (0,0,0) & (0, 1,0) \\ \hline \end{array}\\ \]

故当\(\theta=\left(90^{\circ}, 90^{\circ}, 1\right)\) 时的正向运动学PoE与空间雅克比 \(J_{s}\)： \[ T=\left[\begin{array}{cccc} 0 & 0 & -1 & -3 \\ -1 & 0 & 0 & 0\\ 0 & 1 & 0 & 2\\ 0 & 0 & 0 & 1\\ \end{array}\right] \]

\[ J_{s}(\theta)=\left[\begin{array}{ccc} 0 & 0 & 0 \\ 0 & 1 & 0 \\ 1 & 0 & 0 \\ 0 & -2 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right] \]

使用到的函数如下：

function T = FKinSpace(M, Slist, thetalist)
T = M;
for i = size(thetalist): -1: 1
    T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T;
end
end

function Js = JacobianSpace(Slist, thetalist)
Js = Slist;
T = eye(4);
for i = 2: length(thetalist)
    T = T * MatrixExp6(VecTose3(Slist(:, i - 1) * thetalist(i - 1)));
	Js(:, i) = Adjoint(T) * Slist(:, i);
end
end

初始位形M：

\[ M=\left[\begin{array}{cccc} -1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 3 \\ 0 & 1 & 0 & 2 \\ 0 & 0 & 0 & 1 \end{array}\right] \]

相对空间坐标系 \(\{b\}\) 的旋量坐标为：

\[ \mathcal{B}_{i}：~\begin{array}{|c||c|c|} \hline i & \omega_{i} & v_{i} \\ \hline \hline 1 & (0,1,0) & (3, 0,0) \\ \hline 2 & (-1,0,0) & (0, 3,0) \\ \hline 3 & (0,0,0) & (0, 0,1) \\ \hline \end{array}\\ \]

故当\(\theta=\left(90^{\circ}, 90^{\circ}, 1\right)\) 时的正向运动学PoE与物体雅克比 \(J_{b}\)：

\[ T=\left[\begin{array}{cccc} 0 & 0 & -1 & -3 \\ -1 & 0 & 0 & 0\\ 0 & 1 & 0 & 2\\ 0 & 0 & 0 & 1\\ \end{array}\right]\\(与a中结果相同)\\ J_{b}(\theta)=\left[\begin{array}{ccc} 0 & -1 & 0 \\ 0 & 0 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 4 & 0 \\ 0 & 0 & 1 \end{array}\right] \]

使用到的函数如下：

function T = FKinBody(M, Blist, thetalist)
T = M;
for i = 1: size(thetalist)
    T = T * MatrixExp6(VecTose3(Blist(:, i) * thetalist(i)));
end
end

function Jb = JacobianBody(Blist, thetalist)
Jb = Blist;
T = eye(4);
for i = length(thetalist) - 1: -1: 1   
    T = T * MatrixExp6(VecTose3(-1 * Blist(:, i + 1) * thetalist(i + 1)));
	Jb(:, i) = Adjoint(T) * Blist(:, i);
end
end

2. 如下图所示有一处于初始位形的 RRRP 空间开链机器人, \(p\) 为\(\{b\}\) 系原点相对于 \(\{s\}\) 系的坐标, 求 (10 分):

1. 试求当 \(\theta=(0,0, \pi / 2, L)\) 时的物体雅克比 \(J_{b}(\theta)\);
2. 试求当 \(\theta=(0,0, \pi / 2, L)\) 且 \(\dot{\theta}=(1,1,1,1)\) 时的 \(\dot{p}\)。

相对空间坐标系 \(\{b\}\) 的旋量坐标为：

\[ \mathcal{B}_{i}：~\begin{array}{|c||c|c|} \hline i & \omega_{i} & v_{i} \\ \hline \hline 1 & (0,0,1) & (-L, 0,0) \\ \hline 2 & (1,0,0) & (0, 0,L) \\ \hline 3 & (0,0,1) & (0, 0,0) \\ \hline 4 & (0,0,0) & (0, 1,0) \\ \hline \end{array}\\ \]

使用JacobianBody(Blist, thetalist)计算得，当 \(\theta=(0,0, \pi / 2, L)\) 时的物体雅克比 \(J_{b}(\theta)\)：

\[ J_{b}(\theta)=\left[\begin{array}{cccC} 0 & 0 & 0 &0\\ 0 & -1 & 0 &0\\ 1 & 0 & 1 &0\\ -L & 0 & -L&0 \\ L & 0 & 0 &1\\ 0 & L & 0&0 \end{array}\right] \]

当 \(\dot{\theta}=(1,1,1,1)\)时：

\[ \mathcal{V}_{b}=J_{b}(\theta) \dot{\theta}=\left[\begin{array}{c} \omega_{b} \\ v_{b} \end{array}\right] =\left[\begin{array}{c}0\\-1\\2\\-2L\\L+1\\L\end{array}\right]\\ \because R_{s b}=\left[\begin{array}{rrc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right] \\ \therefore \dot{p}=R_{s b} v_{b}=\left[\begin{array}{c} -L-1 \\ -2 L \\ L \end{array}\right] \]

3. 如下图所示有一处于初始位形的 PRPRRR 空间开链机器人, 此时基坐标系原点与末端坐标系原点之间距离为 \(L\), 求 (10 分):

1. 空间雅克比 \(J_{S}\) 的前三列;

2. 物体雅克比 \(J_{b}\) 的后两列;

3. 初始位形时的 \(J_{S}(0)\);

4. 初始位形时, 若在末端坐标系的 \(-\hat{z}_{b}\) 方向产生 \(100 \mathrm{~N}\) 的力, 需要关节提供多少力或力矩?

空间雅克比 \(J_{S}\) 的前三列：

\[ \begin{aligned} &\mathcal{V}_{s 1}(\theta): \quad \omega_{s 1}=\left[\begin{array}{l} 0 \\ 0 \\ 0 \end{array}\right], v_{s 1}=\left[\begin{array}{l} 0 \\ 0 \\ 1 \end{array}\right] \\ &\mathcal{V}_{s 2}(\theta): \quad \omega_{s 2}=\left[\begin{array}{l} 0 \\ 0 \\ 1 \end{array}\right], q_{2}=\left[\begin{array}{l} 0 \\ 0 \\ 0 \end{array}\right], v_{s 2}=\left[\begin{array}{l} 0 \\ 0 \\ 0 \end{array}\right] \\ &\mathcal{V}_{s 3}(\theta): \quad \omega_{s 3}=\left[\begin{array}{l} 0 \\ 0 \\ 0 \end{array}\right], v_{s 3}=e^{\hat{z} \theta_{2}}\left[\begin{array}{l} 0 \\ 1 \\ 0 \end{array}\right]=\left[\begin{array}{c} -\sin \theta_{2} \\ \cos \theta_{2} \\ 0 \end{array}\right] \end{aligned} \]

物体雅克比 \(J_{b}\) 的后两列：

\[ \begin{aligned} &\mathcal{V}_{b 6}(\theta): \quad \omega_{b 6}=\left[\begin{array}{l} 1 \\ 0 \\ 0 \end{array}\right], q_{6}=\left[\begin{array}{l} 0 \\ 0 \\ 0 \end{array}\right], v_{b 6}=\left[\begin{array}{l} 0 \\ 0 \\ 0 \end{array}\right] \\ &\mathcal{V}_{b 5}(\theta): \quad \omega_{b 5}=e^{\hat{x}\left(-\theta_{6}\right)}\left[\begin{array}{l} 0 \\ 0 \\ 1 \end{array}\right]=\left[\begin{array}{c} 0 \\ \sin \theta_{6} \\ \cos \theta_{6} \end{array}\right], q_{5}=\left[\begin{array}{l} 0 \\ 0 \\ 0 \end{array}\right], v_{b 5}=\left[\begin{array}{l} 0 \\ 0 \\ 0 \end{array}\right] \end{aligned} \]

初始位形时θ=0，故$ J_{S}(0)$：

\[ \because \mathcal{S}_{i}：~\begin{array}{|c||c|c|} \hline i & \omega_{i} & v_{i} \\ \hline \hline 1 & (0,0,0) & (0, 0,1) \\ \hline 2 & (0,0,1) & (0,0,0) \\ \hline 3 & (0,0,0) & (0, 1,0) \\ \hline 4 & (0,1,0) & (0, 0,0) \\ \hline 5 & (0,0,1) & (L, 0,0) \\ \hline 6 & (1,0,0) & (0, 0,-L) \\ \hline \end{array}\\ \therefore J_{s}(0)=\left[\begin{array}{cccccc} 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & L & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 1 & 0 & 0 & 0 & 0 & -L \end{array}\right] \]

\[ \begin{aligned} &\because r_{s b}=\left[\begin{array}{l} 0 \\ L \\ 0 \end{array}\right], f_{b}=\left[\begin{array}{c} 0 \\ 0 \\ -100 \end{array}\right]\\ &\therefore\mathcal{F}_{s}=\left[\begin{array}{c} m_{s} \\ f_{s} \end{array}\right]=\left[\begin{array}{c} r_{s b} \times f_{b} \\ f_{b} \end{array}\right]=\left[\begin{array}{c} -100 L \\ 0 \\ 0 \\ 0 \\ 0 \\ -100 \end{array}\right]\\ &\therefore \tau=J_{s}^{T}(0) \mathcal{F}_{s}\\ &=\left[\begin{array}{cccccc} 0 & 0 & 0 & 0 & 0 & 1 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & L & 0 & 0 \\ 1 & 0 & 0 & 0 & 0 & -L \end{array}\right]\left[\begin{array}{c} -100 L \\ 0 \\ 0 \\ 0 \\ 0 \\ -100 \end{array}\right]=\left[\begin{array}{c} -100 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{array}\right] \end{aligned} \]