四元数

$\left.\mathbf{q}=\left[\begin{array}{cccc}q_0&&q_1&&q_2&&q_3\end{array}\right.\right]^T$

定义$\psi$, $\theta$, $\phi$分别为绕Z轴、Y轴、X轴的旋转角度

称为欧拉角

$\left.\left[\begin{array}{c}\phi\\\theta\\\psi\end{array}\right.\right]=\left[\begin{array}{c}\arctan\frac{2(q_0q_1+q_2q_3)}{1-2(q_1^2+q_2^2)}\\\arcsin(2(q_0q_2-q_3q_1))\\\arctan\frac{2(q_0q_3+q_1q_2)}{1-2(q_2^2+q_3^2)}\end{array}\right]$

欧拉角转四元数

$\begin{cases}z=\sin\left(\frac\theta2\right)\\ w=\cos\left(\frac\theta2\right)\end{cases}$ $\mathbf{q}=\begin{bmatrix}\cos\frac \theta2\\\mathbf{u}\mathrm{sin}\frac \theta2\end{bmatrix},\mathbf{u}=\begin{bmatrix} 1\\0 \\0 \end{bmatrix}$

四元数旋转公式

$\mathbf{v}^{\prime}=\mathbf{qvq}^{*}$

复合性质

$\begin{array}{b}\mathbf{v}^{\prime\prime}&=\mathbf{q}_{2}\left(\mathbf{q}_{1}\mathbf{v}\mathbf{q}_{1}^{*}\right)\mathbf{q}_{2}{}^{*}\\ &=\left(\mathbf{q}_2\mathbf{q}_1\right)\mathbf{v}\left(\mathbf{q}_2\mathbf{q}_1\right)^*\end{array}$ $\mathbf{q}_{\mathrm{d}}=\Delta\mathbf{q}\cdot\mathbf{q}_{\mathrm{s}}\\ \mathbf{q}_\mathrm{d}\cdot\left(\mathbf{q}_\mathrm{s}\right)^{-1}=\Delta\mathbf{q}\cdot\mathbf{q}_\mathrm{s}\cdot\left(\mathbf{q}_\mathrm{s}\right)^{-1}\\\Delta\mathbf{q}=\mathbf{q}_\mathrm{d}\cdot\left(\mathbf{q}_\mathrm{s}\right)^{-1}$

DH

$_{i-1}\mathbf{T}_i=\mathbf{D}_{z_i-1},_{d_i}\mathbf{R}_{z_i-1},_{\theta_i}\mathbf{D}_{x_i-1},_{a_i}\mathbf{R}_{x_i-1},_{\alpha_i}=\begin{pmatrix}\cos\theta_i&-\sin\theta_i\cos\alpha_i&\sin\theta_i\sin\alpha_i&a_i\cos\theta_i\\\sin\theta_i&\cos\theta_i\cos\alpha_i&-\cos\theta_i\sin\alpha_i&a_i\sin\theta_i\\0&\sin\alpha_i&\cos a_i&d_i\\0&0&0&1\end{pmatrix}$ $\begin{array}{cccc} \text{Joint} & \theta & d & a & \alpha \\ \hline 1 & 0 & 0.156 & 0 & \frac{\pi}{2} \\ 2 & \frac{\pi}{2} & 0 & 0.0815 & 0 \\ 3 & 0 & 0 & 0.127 & 0 \\ 4 & 0 & 0 & 0.1075 & 0 \\ 5 & 0 & 0 & 0.074 & -\frac{\pi}{2} \\ 6 & 0 & 0.05 & 0 & 0 \\ \end{array}$

5dof

$\begin{array}{cccc} \text{Joint} & \theta & d & a & \alpha \\ \hline 0 & \theta_0 & 0 & 0 & \frac{\pi}{2} \\ 1 & \theta_1 & 0 & L_1 & 0 \\ 2 & \theta_2 & 0 & L_2 & 0 \\ 3 & \theta_3 & 0 & L_3 & 0 \\ 4 & \theta_4 & 0 & 0 & \frac{\pi}{2} \\ \end{array}$

3dof机械臂

$\begin{align*} B_x &= x - L_3\cos(\gamma) \\ B_y &= y - L_3\sin(\gamma) \\ \beta &= \cos^{-1}\left(\frac{B_x^2 + B_y^2 + L_1^2 - L_2^2}{2L_1\sqrt{B_x^2 + B_y^2}}\right) \ \\ \alpha &= \operatorname{atan2}(B_y, B_x)\\ \theta_1&=\alpha\pm\beta\\ \gamma&= \theta_1 + \theta_2 +\theta_3 \end{align*}$

雅可比矩阵

$J_i=\begin{bmatrix}\hat z\times \vec d[:2]\\1\end{bmatrix}\\ \begin{bmatrix}v_x\\v_y\\ \omega\end{bmatrix}=J\begin{bmatrix}\dot q_1\\\dot q_2\\ \dot q_3\end{bmatrix}$ $\begin{bmatrix}\text d q_1\\\text d q_2\\ \text d q_3\end{bmatrix}=J^{-1}\begin{bmatrix}\text dv_x\\\text dv_y\\ \text d\omega\end{bmatrix}$