1.1. 運動学

1.1.1. 自由度

ロボットの自由度 \(F\) は，運動の自由度 \(D\) ，関節数 \(J\) ，各関節の自由度 \(f_i\) を用いて，

\[F = DN - \sum_{i=1}^{J} (D-f_i) = D(N-J) + \sum_{i=1}^{J} f_i.\]

閉ループがある場合は，閉ループ数 \(L\) を用いて，

\[F = \sum_{i=1}^{J} f_i - LD.\]

1.1.2. 姿勢表現

(ZYZ)オイラー角は，座標系 \(\Sigma_A\) から座標系 \(\Sigma_B\) への回転を，

\[ \begin{align}\begin{aligned}{}^AR_B = \text{Rot}(\mathbf{z}_A,\phi)\text{Rot}(\mathbf{y}_{A'},\theta)\text{Rot}(\mathbf{z}_{A''},\psi) =\\\begin{split}= \begin{bmatrix} \cos\phi \cos\theta \cos\psi - \sin\phi \sin\psi & -\cos\phi \cos\theta \sin\psi - \sin\phi \cos\psi & \cos\phi \sin\theta \\ \sin\phi \cos\theta \cos\psi + \cos\phi \sin\psi & -\sin\phi \cos\theta \sin\psi + \cos\phi \cos\psi & \sin\phi \sin\theta \\ -\sin\theta \cos\psi & \sin\theta \sin\psi & \cos\theta \end{bmatrix}\end{split}\end{aligned}\end{align} \]

と表現する．

また，ロール・ピッチ・ヨー角(ZYXオイラー角)は，座標系 \(\Sigma_A\) から座標系 \(\Sigma_B\) への回転を，

\[ \begin{align}\begin{aligned}{}^AR_B = \text{Rot}(\mathbf{z}_A,\phi)\text{Rot}(\mathbf{y}_{A'},\theta)\text{Rot}(\mathbf{x}_{A''},\psi) =\\\begin{split}= \begin{bmatrix} \cos\phi \cos\theta & \cos\phi \sin\theta \sin\psi - \sin\phi \cos\psi & \cos\phi \sin\theta \cos\psi + \sin\phi \sin\psi \\ \sin\phi \cos\theta & \sin\phi \sin\theta \sin\psi + \cos\phi \cos\psi & \sin\phi \sin\theta \cos\psi - \cos\phi \sin\psi \\ -\sin\theta & \cos\theta \sin\psi & \cos\theta \cos\psi \end{bmatrix} &\end{split}\end{aligned}\end{align} \]

と表現する．

四元数は，

\[\tilde{q} = q_0 + q_1i + q_2j + q_3k\]

であり，基底 \(i,j,k\) は，

\[i^2 = j^2 = k^2 = ijk = -1\]

の関係を満たす．四元数 \(\tilde{q},\tilde{p}\) の積は，

\[\begin{split}\tilde{q}\tilde{p} = (q_0p_0 - q_1p_1 - q_2p_2 - q_3p_3) + (q_0p_1 + q_1p_0 + q_2p_3 - q_3p_2)i \\ + (q_0p_2 - q_1p_3 + q_2p_0 + q_3p_1)j + (q_0p_3 + q_1p_2 - q_2p_1 + q_3p_0)k.\end{split}\]

四元数 \(\tilde{q}\) の共役は，

\[\tilde{q}^* = q_0 - q_1i - q_2j - q_3k = q_0 - \mathbf{q} .\]

絶対値は，

\[|\tilde{q}| = \sqrt{\tilde{q}\tilde{q}^*} = \sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2} .\]

逆数は，

\[\tilde{q}^{-1} = \frac{\tilde{q}^*}{|\tilde{q}|^2} .\]

四元数を用いた回転は，原点を通る単位ベクトル \(\mathbf{n}\) を軸とし，角度 \(\theta\) だけ回転する場合，

\[\tilde{q} = \cos\frac{\theta}{2} + \mathbf{n}\sin\frac{\theta}{2}\]

となり，ある座標系の位置ベクトル \(\mathbf{r}\) を回転させた位置ベクトル \(\mathbf{r}'\) に対応する四元数 \(\tilde{r}'\) は，

\[\tilde{r}' = \tilde{q}\tilde{r}\tilde{q}^{-1} .\]

なお， \(\tilde{r} = r_x i + r_y j + r_z k\) である．

四元数の時間微分は，

\[\frac{d\tilde{q}}{dt} = \lim_{\Delta t \to 0} \frac{\tilde{q}(t + \Delta t) - \tilde{q}(t)}{\Delta t} = \lim_{\Delta t \to 0} \frac{\tilde{q}(t)\tilde{q}(\Delta t) - \tilde{q}(t)}{\Delta t} = \lim_{\Delta t \to 0} \frac{\Delta \theta}{2\Delta t}\mathbf{n}\tilde{q} = \frac{1}{2}\mathbf{\omega}\tilde{q} .\]

これを行列で表すと，

\[\begin{split}\frac{d}{dt} \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_1 & -\omega_2 & -\omega_3 \\ \omega_1 & 0 & -\omega_3 & \omega_2 \\ \omega_2 & \omega_3 & 0 & -\omega_1 \\ \omega_3 & -\omega_2 & \omega_1 & 0 \end{bmatrix} \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} .\end{split}\]

1.1.3. 同次変換

\(\Sigma_A\) から \(\Sigma_B\) への同次変換行列 \({}^AT_B\) は，

\[\begin{split}{}^AT_B \equiv \begin{bmatrix} {}^AR_B & {}^A\mathbf{p}_B \\ \mathbf{0}^T & 1 \end{bmatrix} = \begin{bmatrix} I & {}^A\mathbf{p}_B \\ \mathbf{0}^T & 1 \end{bmatrix} \begin{bmatrix} {}^AR_B & \mathbf{0} \\ \mathbf{0}^T & 1 \end{bmatrix}\end{split}\]

で表される．ここで， \({}^AR_B\) は回転行列， \({}^A\mathbf{p}_B\) は位置ベクトルである．

\({}^AT_B\) の逆行列は，

\[\begin{split}{}^AT_B^{-1} = {}^BT_A = \begin{bmatrix} {}^BR_A & {}^B\mathbf{p}_A \\ \mathbf{0}^T & 1 \end{bmatrix} = \begin{bmatrix} {}^AR_B^T & -{}^AR_B^T {}^A\mathbf{p}_B \\ \mathbf{0}^T & 1 \end{bmatrix}\end{split}\]

1.1.4. 座標系の速度，加速度

原点位置の移動速度は，単純に

\[{}^A\dot{\mathbf{p}}_B = \frac{d}{dt}{}^A\mathbf{p}_B\]

と時間微分すればよい．姿勢の変化速度（角速度ベクトル）は， \({}^A\mathbf{\phi}_B\) をオイラー角とすると，

\[\begin{split}{}^A\mathbf{\omega}_B = \begin{bmatrix} 0 & -\sin\phi & -\cos\phi\sin\theta \\ 0 & \cos\phi & \sin\phi\sin\theta \\ 1 & 0 & \cos\theta \end{bmatrix} {}^A\dot{\mathbf{\phi}}_B\end{split}\]

と表現できる．これは，直交座標系の各軸回りの回転速度の合成となっており，どんな姿勢変化でも表すことができる．しかし，回転軸が変化するとき角速度ベクトルの積分値には意味がない．

次に，移動する座標系上の点について考える．

\[{}^A\mathrm{r} = {}^A\mathbf{p}_B + {}^AR_B {}^B\mathrm{r}\]

の時間微分を取ると，

\[ \begin{align}\begin{aligned}{}^A\dot{\mathrm{r}} = {}^A\dot{\mathbf{p}}_B + \frac{d}{dt}({}^AR_B{}^B\mathrm{r}) = {}^A\dot{\mathbf{p}}_B + {}^A\dot{R}_B {}^B\mathrm{r} + {}^AR_B {}^B\dot{\mathrm{r}}=\\= {}^A\dot{\mathbf{p}}_B + {}^A\omega_B \times ({}^AR_B {}^B\mathrm{r}) + {}^AR_B {}^B\dot{\mathrm{r}}.\end{aligned}\end{align} \]

さらに時間微分を取ると，

\[{}^A\ddot{\mathrm{r}} = {}^A\ddot{\mathbf{p}}_B + {}^A\dot{\omega}_B \times ({}^AR_B {}^B\mathrm{r}) + {}^A\omega_B \times [{}^A\omega_B \times ({}^AR_B {}^B\mathrm{r})] + 2{}^A\omega_B \times ({}^AR_B {}^B\dot{\mathrm{r}}) + {}^AR_B {}^B\ddot{\mathrm{r}}.\]

角速度は，

\[{}^A\mathbf{\omega} = {}^AR_B {}^B\mathbf{\omega} + {}^A\omega_B .\]

さらに時間微分を取ると，

\[{}^A\dot{\mathbf{\omega}} = {}^A\dot{\omega}_B + {}^AR_B {}^B\dot{\mathbf{\omega}} + {}^A\omega_B \times ({}^AR_B {}^B\mathbf{\omega}) .\]

1.1.5. 修正DH法

リンク座標系 \(i-1\) から見たリンク座標系 \(i\) への同次変換行列は，

\[ \begin{align}\begin{aligned}{}^{i-1}T_i = \text{Trans}(\mathbf{x}_{i-1},a_{i-1})\text{Rot}(\mathbf{x}_{i-1},\alpha_{i-1})\text{Trans}(\mathbf{z}_i,d_i)\text{Rot}(\mathbf{z}_i,\theta_i) =\\\begin{split}= \begin{bmatrix} 1 & 0 & 0 & a_{i-1} \\ 0 & \cos\alpha_{i-1} & -\sin\alpha_{i-1} & 0 \\ 0 & \sin\alpha_{i-1} & \cos\alpha_{i-1} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos\theta_i & -\sin\theta_i & 0 & 0 \\ \sin\theta_i & \cos\theta_i & 0 & 0 \\ 0 & 0 & 1 & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}\end{split}\\\begin{split}= \begin{bmatrix} \cos\theta_i & -\sin\theta_i & 0 & a_{i-1} \\ \sin\theta_i \cos\alpha_{i-1} & \cos\theta_i \cos\alpha_{i-1} & -\sin\alpha_{i-1} & -d_i \sin\alpha_{i-1} \\ \sin\theta_i \sin\alpha_{i-1} & \cos\theta_i \sin\alpha_{i-1} & \cos\alpha_{i-1} & d_i \cos\alpha_{i-1} \\ 0 & 0 & 0 & 1 \end{bmatrix}\end{split}\end{aligned}\end{align} \]

で表される．ここで， \(a_{i-1}\) はリンク長， \(\alpha_{i-1}\) はリンクねじれ角， \(d_i\) はリンクオフセット， \(\theta_i\) は関節角である．