刚体运动学笔记

1.三维空间旋转矩阵

1.1.旋转矩阵

• 绕x轴: R x ( θ ) = [ x ^ ′      y ^ ′      z ^ ′ ] = [ 1 0 0 0 cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ ] \textit{\textbf{R}}_x(\theta )=[\hat{x}'\;\; \hat{y}'\;\; \hat{z}'] =\begin{bmatrix} 1 & 0 & 0\\ 0 & \cos{\theta } & -\sin{\theta } \\ 0 & \sin{\theta } & \cos{\theta }\end{bmatrix} Rx​(θ)=[x^′y^​′z^′]= ​100​0cosθsinθ​0−sinθcosθ​ ​
• 绕y轴: R y ( θ ) = [ x ^ ′      y ^ ′      z ^ ′ ] = [ cos ⁡ θ 0 sin ⁡ θ 0 1 0 − sin ⁡ θ 0 cos ⁡ θ ] \textit{\textbf{R}}_y(\theta )=[\hat{x}'\;\; \hat{y}'\;\; \hat{z}'] =\begin{bmatrix} \cos {\theta } & 0 & \sin {\theta }\\ 0 & 1 & 0 \\ -\sin {\theta } &0 & \cos {\theta }\end{bmatrix} Ry​(θ)=[x^′y^​′z^′]= ​cosθ0−sinθ​010​sinθ0cosθ​ ​
• 绕z轴: R z ( θ ) = [ x ^ ′      y ^ ′      z ^ ′ ] = [ cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ 0 0 0 1 ] \textit{\textbf{R}}_z(\theta )=[\hat{x}'\;\; \hat{y}'\;\; \hat{z}'] =\begin{bmatrix} \cos {\theta } & -\sin {\theta } & 0\\ \sin {\theta } & \cos {\theta } & 0 \\ 0 & 0 & 1\end{bmatrix} Rz​(θ)=[x^′y^​′z^′]= ​cosθsinθ0​−sinθcosθ0​001​ ​
• 三维空间旋转矩阵性质
  • 正交性
    • 旋转矩阵三个列向量是一组标准正交基
    • 每个列向量均为单位向量, 相互正交
    • 旋转矩阵为正交矩阵, 有正交性
  • 行列式恒为1
    • 右手系下 $\det ({\text{R}}_{sb})=1$ (含义:描述缩放比例, 旋转矩阵不会缩放物体)
  • 可逆性
    • 旋转矩阵的逆矩阵=转置矩阵 ${\text{R}}_{sb}^{-1}={\text{R}}_{bs}$ ${\text{R}}_{sb}^{-1}={\text{R}}_{sb}^{\mathrm{T}}$
  • 封闭性
    • 旋转矩阵$\cdot$旋转矩阵=旋转矩阵
  • 满足结合律不满足交换律
    • 结合律: $({\text{R}}_{ab}\cdot {\text{R}}_{bc})\cdot {\text{R}}_{cd}={\text{R}}_{ab}\cdot ({\text{R}}_{bc}\cdot {\text{R}}_{cd})={\text{R}}_{ad}$
    • 不满足交换律: ${\text{R}}_{ac}={\text{R}}_{ab}\cdot {\text{R}}_{bc}\ne {\text{R}}_{bc}\cdot {\text{R}}_{ab}$

---

2.绕任意轴旋转矩阵

2.1.叉乘运算性质

• 向量积形式
  • a × b = [ 0 − a z a y a z 0 − a x − a y a x 0 ] ⋅ b = [ a ] × ⋅ b \textit{\textbf{a}}\times\textit{\textbf{b}}=\begin{bmatrix} 0 & -a_z & a_y\\ a_z & 0 & -a_x\\ -a_y & a_x & 0\end{bmatrix}\cdot\textit{\textbf{b}}=[\textit{\textbf{a}}]_{\times}\cdot\textit{\textbf{b}} a×b= ​0az​−ay​​−az​0ax​​ay​−ax​0​ ​⋅b=[a]×​⋅b
• 叉乘反交换律: $\text{a}\times \text{b}=-\text{b}\times \text{a}$
  • $[\text{a}{]}_{\times }\text{b}=-[\text{b}{]}_{\times }\text{a}$
• 反对称阵: $[\text{a}{]}_{\times }=-[\text{a}{]}_{\times }^{\mathrm{T}}$
• 二重向量积展开: $\text{a}\times (\text{b}\times \text{c})=(\text{a}\cdot \text{c})\text{b}-(\text{a}\cdot \text{b})\text{c}$
  • 向量点积运算: $\text{a}\cdot \text{b}={\text{a}}^{\mathrm{T}}\text{b}={\text{b}}^{\mathrm{T}}\text{a}$
  • $[\text{a}{]}_{\times }[\text{b}{]}_{\times }\text{c}=({\text{a}}^{\mathrm{T}}\text{c})\text{b}-({\text{a}}^{\mathrm{T}}\text{b})\text{c}$

2.2.向量积与矩阵形式

• $\{\begin{array}{c}\text{a}=\text{a}\times \text{b}\\ |\text{c}|=|\text{a}||\text{b}|\sin \theta \end{array}$

2.3.绕轴旋转的线速度

• $|{\text{v}}_{\omega }|=|\mathbf{\omega }|\cdot |\text{p}|\cdot \sin \angle AOP$
• ${\text{v}}_{\omega }=\mathbf{\omega }\times \text{p}=[\mathbf{\omega }{]}_{\times }\text{p}$
• ${\dot{\mathbf{R}}}_{sb}=[\dot{\hat{\mathbf{x}}}\dot{\hat{\mathbf{y}}}\dot{\hat{\mathbf{z}}}]=[[{\mathbf{\omega }}_s{]}_{\times }{\hat{\mathbf{x}}}_b[{\mathbf{\omega }}_s{]}_{\times }{\hat{\mathbf{y}}}_b[{\mathbf{\omega }}_s{]}_{\times }{\hat{\mathbf{z}}}_b]=[{\mathbf{\omega }}_s{]}_{\times }[{\hat{\mathbf{x}}}_b{\hat{\mathbf{y}}}_b{\hat{\mathbf{z}}}_b]=[{\mathbf{\omega }}_s{]}_{\times }{\mathbf{R}}_{sb}$
  • 旋转角速度${\mathbf{\omega }}_s$与旋转矩阵${\mathbf{R}}_{sb}$的关系: $[{\mathbf{\omega }}_s{]}_{\times }={\dot{\mathbf{R}}}_{sb}{\mathbf{R}}_{sb}^{-1}={\dot{\mathbf{R}}}_{sb}{\mathbf{R}}_{sb}^{\mathrm{T}}$

2.4.一阶线性常微分方程

• $\dot{x}(t)=ax(t),x(0)=x_0\to x(t)=e^{at}{x}_0$
• 向量的一阶线性微分方程: $\dot{\mathbf{x}}(t)=\mathbf{A}\mathbf{x}(t),\mathbf{x}(0)={\mathbf{x}}_0\to \mathbf{x}(t)=e^{\mathbf{A}t}{\mathbf{x}}_0$
• 矩阵指数$e^{\mathbf{A}t}$性质
  • 对角矩阵: e D t = [ e d 1 t 0 ⋯ 0 0 e d 2 t ⋯ 0 ⋮ ⋮ ⋮ 0 0 ⋯ e d n t ] e^{Dt}=\begin{bmatrix} e^{d_1t} & 0 & \cdots & 0\\ 0 & e^{d_2t} & \cdots & 0\\ \vdots & \vdots & & \vdots \\ 0 & 0 & \cdots & e^{d_nt}\end{bmatrix} eDt= ​ed1​t0⋮0​0ed2​t⋮0​⋯⋯⋯​00⋮edn​t​ ​
  • 非对角阵, 先对角化$\mathbf{A}={\mathbf{P}DP}^{-1}$: $e^{\mathbf{A}t}=\mathbf{P}{e}^{Dt}{\mathbf{P}}^{-1}$
  • 若$\mathbf{A}B=\mathbf{B}A$有互换性, 则$e^{\mathbf{A}}{e}^{\mathbf{B}}=e^{\mathbf{A}+\mathbf{B}}$
  • $(e^{\mathbf{A}}{)}^{(-1)}=e^{-\mathbf{A}}$

2.5.轴角法表示的旋转矩阵

• $P$点初始坐标${\mathbf{p}}_0$,绕转轴单位向量$\hat{\omega }$旋转了$\theta$角 --等效–> 绕转轴单位向量$\hat{\omega }$以单位速度1$rad/s$旋转了时间$\theta$
  • 有微分方程: $\dot{\mathbf{p}}(t)=[\mathbf{\omega }{]}_{\times }\mathbf{p}(t)$, 解得$\mathbf{p}(\theta )=e^{[\hat{\mathbf{\omega }}{]}_{\times }\theta }{\mathbf{p}}_0$
  • 绕单位轴$\hat{\mathbf{\omega }}$旋转$\theta$角度的旋转矩阵: $\mathrm{Rot}(\hat{\mathbf{\omega }},\theta )=e^{[\hat{\mathbf{\omega }}{]}_{\times }\theta }={\sum }_{n=0}^{\infty }\frac{[{\hat{\mathbf{\omega }}}_{\times }^n{\theta }^n]}{n!}$
• 罗德里格斯公式: $\mathrm{Rot}(\hat{\mathbf{\omega }},\theta )=e^{[\hat{\mathbf{\omega }}{]}_{\times }\theta }=\mathbf{I}+\sin \theta [\hat{\mathbf{\omega }}{]}_{\times }+(1-\cos \theta )[\hat{\mathbf{\omega }}{]}_{\times }^2$
  • 可以简便地计算任意矩阵

2.6.矩阵对数&指数坐标

• 矩阵对数: 反对称阵$[\hat{\mathbf{\omega }}{]}_{\times }\theta$称为矩阵$\mathbf{R}$的矩阵对数
• 指数坐标: 三维向量$\hat{\mathbf{\omega }}\theta$称为矩阵$\mathbf{R}$的指数坐标
  • $\{\begin{array}{c}{\omega }_1=\sqrt{\frac{r_{11}+1}{2}}\\ {\omega }_2=\frac{r_{12}}{2{\omega }_1}\\ {\omega }_3=\frac{r_{13}}{2{\omega }_1}\end{array}$

---

3.三维空间刚体运动

• 二维平面刚体运动: ${\mathbf{T}}_{sb}=\left[\begin{array}{cc}{\mathbf{R}}_{sb}& O{O}^{\prime }\\ 0_{1\times 2}& 1\end{array}\right]$

3.1.齐次变换矩阵性质

• 可逆性: ${\mathbf{T}}^{-1}={\left[\begin{array}{cc}\mathbf{R}& \mathbf{p}\\ 0_{1\times 3}& 1\end{array}\right]}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathrm{T}}& -{\mathbf{R}}^{\mathrm{T}}\mathbf{p}\\ 0_{1\times 3}& 1\end{array}\right]$
• 封闭性: 齐次变换矩阵乘积仍为齐次变换矩阵
• 满足结合律: $({\mathbf{T}}_{ab}\cdot {\mathbf{T}}_{bc})\cdot {\mathbf{T}}_{cd}={\mathbf{T}}_{ab}\cdot ({\mathbf{T}}_{bc}\cdot {\mathbf{T}}_{cd})={\mathbf{T}}_{ad}$
• 不满足交换律: ${\mathbf{T}}_{ac}={\mathbf{T}}_{ab}\cdot {\mathbf{T}}_{bc}\ne {\mathbf{T}}_{bc}\cdot {\mathbf{T}}_{ab}$

3.2.齐次变换矩阵$\mathbf{T}$的作用

1. 描述刚体位置和姿态(位姿)
   • 1.$\mathbf{T}$作为状态量
2. 对向量,点,坐标系变换参考坐标系
3. 运动(旋转+平移)向量,点,位姿
   • 2.3.$\mathbf{T}$为算子, 与向量/矩阵相乘完成坐标系变换/运动

3.2.1.描述刚体位姿

• 给定世界坐标系$\{s\}$下局部坐标系$\{a\}\{b\}\{c\}$的旋转矩阵${\mathbf{R}}_{sa}{\mathbf{R}}_{sb}{\mathbf{R}}_{sc}$, 原点坐标${\mathbf{p}}_{sa}{\mathbf{p}}_{sb}{\mathbf{p}}_{sc}$
  • 得
    • $\{c\}$相对$\{b\}$的位姿 ${\mathbf{R}}_{bc}={{\mathbf{R}}_{sb}}^{\mathrm{T}}\cdot {\mathbf{R}}_{sc}$
    • $\left[\begin{array}{c}{\mathbf{p}}_{bc}\\ 0\end{array}\right]={\mathbf{T}}_{bs}\cdot \left[\begin{array}{c}{\mathbf{p}}_{sc}-{\mathbf{p}}_{sb}\\ 0\end{array}\right]$
    • ${\mathbf{T}}_{bc}=\left[\begin{array}{cc}{\mathbf{R}}_{bc}& {\mathbf{p}}_{bc}\\ 0_{1\times 3}& 1\end{array}\right]$

3.2.2.对向量,点,坐标变换参考坐标系

• 空间中仅有旋转能改变向量坐标, 平移不能改变
  • $\left[\begin{array}{c}{\mathbf{v}}_s\\ 0\end{array}\right]={\mathbf{T}}_{sb}\left[\begin{array}{c}{\mathbf{v}}_b\\ 0\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}_{sb}& {\mathbf{p}}_{sb}\\ {\text{0}}_{1\times 3}& 1\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_b\\ 0\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_{sb}{\mathbf{v}}_b\\ 0\end{array}\right]$
  • ${\mathbf{v}}_s={\mathbf{R}}_{sb}{\mathbf{v}}_b$
  • 对向量坐标变换可只左乘旋转矩阵, 无需化齐次矩阵
• 点齐次坐标增加一项1
  • $\left[\begin{array}{c}{\mathbf{p}}_s\\ 1\end{array}\right]={\mathbf{T}}_{sb}\left[\begin{array}{c}{\mathbf{p}}_b\\ 1\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}_{sb}& {\mathbf{p}}_{sb}\\ {\text{0}}_{1\times 3}& 1\end{array}\right]\left[\begin{array}{c}{\mathbf{p}}_b\\ 1\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_{sb}{\mathbf{p}}_b+{\mathbf{p}}_{sb}\\ 1\end{array}\right]$
  • ${\mathbf{p}}_s={\mathbf{R}}_{sb}{\mathbf{p}}_b+{\mathbf{p}}_{sb}$
• 齐次变换矩阵计算 ${\mathbf{T}}_{sc}={\mathbf{T}}_{sb}\cdot {\mathbf{T}}_{bc}$

3.2.3.运动向量, 点, 位姿

• 左乘运动 ${\mathbf{p}}_s^{\prime }={\mathbf{T}}_{s{b}^{\prime }}{\mathbf{p}}_b={\mathbf{T}T}_{sb}{\mathbf{p}}_b={\mathbf{T}p}_s$
  • 先变换坐标系, 在{s}运动
• 右乘运动 ${\mathbf{p}}_s^{\prime \prime }={\mathbf{T}}_{s{b}^{\prime \prime }}{\mathbf{p}}_b={\mathbf{T}}_{sb}\mathbf{T}{\mathbf{p}}_b={\mathbf{T}}_{sb}{\mathbf{p}}_b^{\prime \prime }$
  • 先在{b}运动, 再变换坐标系

---

4.欧拉角

• 横滚roll, 俯仰pitch, 偏航yaw
• Y-P-R顺序: 初态物体坐标系{b}与世界坐标系{s}重合, 先绕{b}的z轴旋转–>{b’}, 绕{b’}的y轴旋转–>{b’‘}, 绕{b’‘}的x轴旋转–>{b’‘’}
• 相对物体坐标系旋转: 用右乘旋转矩阵方法计算
  • ${\mathbf{R}}_{sb}=\mathbf{I}$
  • ${\mathbf{R}}_{s{b}^{\prime }}={\mathbf{R}}_{sb}{R}_z(\mathrm{yaw})={\mathbf{R}}_z(\mathrm{yaw})$
  • ${\mathbf{R}}_{s{b}^{\prime \prime }}={\mathbf{R}}_{s{b}^{\prime }}{R}_y(\mathrm{pitch})={\mathbf{R}}_z(\mathrm{yaw}){\mathbf{R}}_y(\mathrm{pitch})$
  • ${\mathbf{R}}_{s{b}^{\prime \prime \prime }}={\mathbf{R}}_{s{b}^{\prime \prime }}{R}_x(\mathrm{roll})={\mathbf{R}}_z(\mathrm{yaw}){\mathbf{R}}_y(\mathrm{pitch}){\mathbf{R}}_x(\mathrm{roll})$
• ${\mathbf{R}}_{s{b}^{\prime \prime \prime }}$ $\to$ $\{\begin{array}{c}\tan \gamma =\frac{\cos \beta \sin \gamma }{\cos \beta \cos \gamma }=\frac{r32}{r_{33}}\\ \sin \beta =-r_{31}\\ \tan \alpha =\frac{\sin \alpha \cos \beta }{\cos \alpha \cos \beta }=\frac{r21}{r_{11}}\end{array}$
  $\to$ $\{\begin{array}{c}\mathrm{roll}=\arctan 2(r_{32},r_{33})\\ \mathrm{pitch}=\arcsin (-r_{31})\\ \mathrm{yaw}=\arctan 2(r_{21},r_{11})\end{array}$

---

ref

• 《四足机器人控制算法—建模，控制与实现》