刚体运动学笔记

文章目录

    • @[toc]
  • 1.三维空间旋转矩阵
    • 1.1.旋转矩阵
  • 2.绕任意轴旋转矩阵
    • 2.1.叉乘运算性质
    • 2.2.向量积与矩阵形式
    • 2.3.绕轴旋转的线速度
    • 2.4.一阶线性常微分方程
    • 2.5.轴角法表示的旋转矩阵
    • 2.6.矩阵对数&指数坐标
  • 3.三维空间刚体运动
    • 3.1.齐次变换矩阵性质
    • 3.2.齐次变换矩阵 T {\bm T} T的作用
      • 3.2.1.描述刚体位姿
      • 3.2.2.对向量,点,坐标变换参考坐标系
      • 3.2.3.运动向量, 点, 位姿
  • 4.欧拉角
  • ref

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^]= 1000cosθsinθ0sinθ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θ0sinθ010sinθ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θ0sinθcosθ0001
  • 三维空间旋转矩阵性质
    • 正交性
      • 旋转矩阵三个列向量是一组标准正交基
      • 每个列向量均为单位向量, 相互正交
      • 旋转矩阵为正交矩阵, 有正交性
    • 行列式恒为1
      • 右手系下 d e t ( R s b ) = 1 {\rm det}(\textit{\textbf{R}}_{sb})=1 det(Rsb)=1 (含义:描述缩放比例, 旋转矩阵不会缩放物体)
    • 可逆性
      • 旋转矩阵的逆矩阵=转置矩阵 R s b − 1 = R b s      \textit{\textbf{R}}_{sb}^{-1} = \textit{\textbf{R}}_{bs}\;\; Rsb1=Rbs R s b − 1 = R s b T \textit{\textbf{R}}_{sb}^{-1}=\textit{\textbf{R}}_{sb}^{\rm T} Rsb1=RsbT
    • 封闭性
      • 旋转矩阵 ⋅ \cdot 旋转矩阵=旋转矩阵
    • 满足结合律不满足交换律
      • 结合律: ( R a b ⋅ R b c ) ⋅ R c d = R a b ⋅ ( R b c ⋅ R c d ) = R a d (\textit{\textbf{R}}_{ab} \cdot \textit{\textbf{R}}_{bc}) \cdot \textit{\textbf{R}}_{cd} = \textit{\textbf{R}}_{ab} \cdot (\textit{\textbf{R}}_{bc} \cdot \textit{\textbf{R}}_{cd}) = \textit{\textbf{R}}_{ad} (RabRbc)Rcd=Rab(RbcRcd)=Rad
      • 不满足交换律: R a c = R a b ⋅ R b c ≠ R b c ⋅ R a b \textit{\textbf{R}}_{ac} = \textit{\textbf{R}}_{ab} \cdot \textit{\textbf{R}}_{bc} \neq \textit{\textbf{R}}_{bc} \cdot \textit{\textbf{R}}_{ab} Rac=RabRbc=RbcRab

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= 0azayaz0axayax0 b=[a]×b
  • 叉乘反交换律: a × b = − b × a \textit{\textbf{a}}\times\textit{\textbf{b}}=-\textit{\textbf{b}}\times\textit{\textbf{a}} a×b=b×a
    • [ a ] × b = − [ b ] × a [\textit{\textbf{a}}]_\times\textit{\textbf{b}}=-[\textit{\textbf{b}}]_\times\textit{\textbf{a}} [a]×b=[b]×a
  • 反对称阵: [ a ] × = − [ a ] × T [\textit{\textbf{a}}]_\times=-[\textit{\textbf{a}}]_\times^{\rm T} [a]×=[a]×T
  • 二重向量积展开: a × ( b × c ) = ( a ⋅ c ) b − ( a ⋅ b ) c \textit{\textbf{a}}\times(\textit{\textbf{b}}\times\textit{\textbf{c}})=(\textit{\textbf{a}}\cdot\textit{\textbf{c}})\textit{\textbf{b}}-(\textit{\textbf{a}}\cdot\textit{\textbf{b}})\textit{\textbf{c}} a×(b×c)=(ac)b(ab)c
    • 向量点积运算: a ⋅ b = a T b = b T a \textit{\textbf{a}}\cdot\textit{\textbf{b}}=\textit{\textbf{a}}^{\rm T}\textit{\textbf{b}}=\textit{\textbf{b}}^{\rm T}\textit{\textbf{a}} ab=aTb=bTa
    • [ a ] × [ b ] × c = ( a T c ) b − ( a T b ) c [\textit{\textbf{a}}]_\times[\textit{\textbf{b}}]_\times\textit{\textbf{c}}=(\textit{\textbf{a}}^{\rm T}\textit{\textbf{c}})\textit{\textbf{b}}-(\textit{\textbf{a}}^{\rm T}\textit{\textbf{b}})\textit{\textbf{c}} [a]×[b]×c=(aTc)b(aTb)c

2.2.向量积与矩阵形式

  • { a = a × b ∣ c ∣ = ∣ a ∣ ∣ b ∣ sin ⁡ θ \left\{\begin{matrix}\textit{\textbf{a}}=\textit{\textbf{a}}\times \textit{\textbf{b}} \\|\textit{\textbf{c}}|=|\textit{\textbf{a}}||\textit{\textbf{b}}|\sin {\theta }\end{matrix}\right. {a=a×bc=a∣∣bsinθ

2.3.绕轴旋转的线速度

  • ∣ v ω ∣ = ∣ ω ∣ ⋅ ∣ p ∣ ⋅ sin ⁡ ∠ A O P |\textit{\textbf{v}}_\omega|=|{\bm\omega}|\cdot|\textit{\textbf{p}}|\cdot\sin\angle AOP vω=ωpsinAOP
  • v ω = ω × p = [ ω ] × p \textit{\textbf{v}}_{\omega}={\bm\omega}\times\textit{\textbf{p}}=[{\bm\omega}]_{\times}\textit{\textbf{p}} vω=ω×p=[ω]×p
  • R ˙ s b = [ x ^ ˙    y ^ ˙    z ^ ˙ ] = [    [ ω s ] × x ^ b      [ ω s ] × y ^ b      [ ω s ] × z ^ b    ] = [ ω s ] × [ x ^ b      y ^ b      z ^ b ] = [ ω s ] × R s b \dot{\bm R}_{sb}=[\dot{\hat{\bm x}}\; \dot{\hat{\bm y}}\; \dot{\hat{\bm z}}] = [\;[{\bm\omega}_s]_{\times}\hat{\bm x}_b \;\; [{\bm\omega}_s]_{\times}\hat{\bm y}_b \;\; [{\bm\omega}_s]_{\times}\hat{\bm z}_b\;] = [{\bm \omega}_s]_{\times}[\hat{\bm x}_b\;\;\hat{\bm y}_b\;\;\hat{\bm z}_b] = [{\bm \omega}_s]_{\times}{\bm R}_{sb} R˙sb=[x^˙y^˙z^˙]=[[ωs]×x^b[ωs]×y^b[ωs]×z^b]=[ωs]×[x^by^bz^b]=[ωs]×Rsb
    • 旋转角速度 ω s {\bm\omega}_s ωs与旋转矩阵 R s b {\bm R}_{sb} Rsb的关系: [ ω s ] × = R ˙ s b R s b − 1 = R ˙ s b R s b T [{\bm\omega}_s]_{\times}=\dot{\bm R}_{sb}{\bm R}_{sb}^{-1}=\dot{\bm R}_{sb}{\bm R}_{sb}^{\rm T} [ωs]×=R˙sbRsb1=R˙sbRsbT

2.4.一阶线性常微分方程

  • x ˙ ( t ) = a x ( t ) , x ( 0 ) = x 0 → x ( t ) = e a t x 0 \dot{x}(t)=ax(t), x(0)=x_0\rightarrow x(t)=e^{at}x_0 x˙(t)=ax(t),x(0)=x0x(t)=eatx0
  • 向量的一阶线性微分方程: x ˙ ( t ) = A x ( t ) , x ( 0 ) = x 0 → x ( t ) = e A t x 0 \dot{\bm x}(t)={\bm A}{\bm x}(t), {\bm x}(0)={\bm x}_0\rightarrow {\bm x}(t)=e^{{\bm A}t}{\bm x}_0 x˙(t)=Ax(t),x(0)=x0x(t)=eAtx0
  • 矩阵指数 e A t e^{{\bm A}t} eAt性质
    • 对角矩阵: 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= ed1t000ed2t000ednt
    • 非对角阵, 先对角化 A = P D P − 1 {\bm A}={\bm PDP}^{-1} A=PDP1: e A t = P e D t P − 1 e^{{\bm A}t}={\bm P}e^{Dt}{\bm P}^{-1} eAt=PeDtP1
    • A B = B A {\bm AB}={\bm BA} AB=BA有互换性, 则 e A e B = e A + B e^{\bm A}e^{\bm B} = e^{{\bm A}+{\bm B}} eAeB=eA+B
    • ( e A ) ( − 1 ) = e − A (e^{\bm A})^{(-1)}=e^{-{\bm A}} (eA)(1)=eA

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

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

2.6.矩阵对数&指数坐标

  • 矩阵对数: 反对称阵 [ ω ^ ] × θ [\hat{\bm\omega}]_\times\theta [ω^]×θ称为矩阵 R \bm R R的矩阵对数
  • 指数坐标: 三维向量 ω ^ θ \hat{\bm\omega}\theta ω^θ称为矩阵 R \bm R R的指数坐标
    • { ω 1 = r 11 + 1 2 ω 2 = r 12 2 ω 1 ω 3 = r 13 2 ω 1 \left\{\begin{matrix}\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{matrix}\right. ω1=2r11+1 ω2=2ω1r12ω3=2ω1r13

3.三维空间刚体运动

  • 二维平面刚体运动: T s b = [ R s b O O ′ → 0 1 × 2 1 ] {\bm T}_{sb}=\begin{bmatrix} {\bm R}_{sb} &\overrightarrow{OO'}\\ 0_{1\times 2} &1\end{bmatrix} Tsb=[Rsb01×2OO 1]

3.1.齐次变换矩阵性质

  • 可逆性: T − 1 = [ R p 0 1 × 3 1 ] − 1 = [ R T − R T p 0 1 × 3 1 ] {\bm T}^{-1}=\begin{bmatrix} {\bm R} & {\bm p}\\ {\bm 0}_{1\times 3} & 1\end{bmatrix}^{-1}=\begin{bmatrix} {\bm R}^{\rm T} & -{\bm R}^{\rm T}{\bm p}\\ {\bm 0}_{1\times 3} & 1\end{bmatrix} T1=[R01×3p1]1=[RT01×3RTp1]
  • 封闭性: 齐次变换矩阵乘积仍为齐次变换矩阵
  • 满足结合律: ( T a b ⋅ T b c ) ⋅ T c d = T a b ⋅ ( T b c ⋅ T c d ) = T a d ({\bm T}_{ab}\cdot{\bm T}_{bc})\cdot{\bm T}_{cd}={\bm T}_{ab}\cdot({\bm T}_{bc}\cdot{\bm T}_{cd})={\bm T}_{ad} (TabTbc)Tcd=Tab(TbcTcd)=Tad
  • 不满足交换律: T a c = T a b ⋅ T b c ≠ T b c ⋅ T a b {\bm T}_{ac}={\bm T}_{ab}\cdot{\bm T}_{bc}\neq{\bm T}_{bc}\cdot{\bm T}_{ab} Tac=TabTbc=TbcTab

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

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

3.2.1.描述刚体位姿

  • 给定世界坐标系 { s } \{s\} {s}下局部坐标系 { a } { b } { c } \{a\}\{b\}\{c\} {a}{b}{c}的旋转矩阵    R s a    R s b    R s c    \;{\bm R}_{sa}\;{\bm R}_{sb}\;{\bm R}_{sc}\; RsaRsbRsc, 原点坐标 p s a    p s b    p s c {\bm p}_{sa}\;{\bm p}_{sb}\;{\bm p}_{sc} psapsbpsc
      • { c } \{c\} {c}相对 { b } \{b\} {b}的位姿 R b c = R s b T ⋅ R s c {\bm R}_{bc} = {{\bm R}_{sb}}^{\rm T} \cdot {\bm R}_{sc} Rbc=RsbTRsc
      • [ p b c 0 ] = T b s ⋅ [ p s c − p s b 0 ] \begin{bmatrix}{\bm p}_{bc} \\0\end{bmatrix} = {\bm T}_{bs}\cdot \begin{bmatrix}{\bm p}_{sc}-{\bm p}_{sb} \\0\end{bmatrix} [pbc0]=Tbs[pscpsb0]
      • T b c = [ R b c p b c 0 1 × 3 1 ] {\bm T}_{bc}=\begin{bmatrix}{\bm R}_{bc} & {\bm p}_{bc}\\{\bm 0}_{1\times 3} & 1\end{bmatrix} Tbc=[Rbc01×3pbc1]

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

  • 空间中仅有旋转能改变向量坐标, 平移不能改变
    • [ v s 0 ] = T s b [ v b 0 ] = [ R s b p s b 0 1 × 3 1 ] [ v b 0 ] = [ R s b v b 0 ] \begin{bmatrix}{\bm v}_s \\0\end{bmatrix} = {\bm T}_{sb}\begin{bmatrix}{\bm v}_b \\0\end{bmatrix} = \begin{bmatrix}{\bm R}_{sb} & {\bm p}_{sb} \\\textbf{0}_{1\times3} & 1\end{bmatrix}\begin{bmatrix}{\bm v}_{b}\\0\end{bmatrix}=\begin{bmatrix}{\bm R}_{sb}{\bm v}_b\\0\end{bmatrix} [vs0]=Tsb[vb0]=[Rsb01×3psb1][vb0]=[Rsbvb0]
    • v s = R s b v b {\bm v}_s={\bm R}_{sb}{\bm v}_b vs=Rsbvb
    • 对向量坐标变换可只左乘旋转矩阵, 无需化齐次矩阵
  • 点齐次坐标增加一项1
    • [ p s 1 ] = T s b [ p b 1 ] = [ R s b p s b 0 1 × 3 1 ] [ p b 1 ] = [ R s b p b + p s b 1 ] \begin{bmatrix}{\bm p}_s \\1\end{bmatrix} = {\bm T}_{sb}\begin{bmatrix}{\bm p}_b \\1\end{bmatrix} = \begin{bmatrix}{\bm R}_{sb} & {\bm p}_{sb} \\\textbf{0}_{1\times3} & 1\end{bmatrix}\begin{bmatrix}{\bm p}_{b}\\1\end{bmatrix}=\begin{bmatrix}{\bm R}_{sb}{\bm p}_b+{\bm p}_{sb} \\1\end{bmatrix} [ps1]=Tsb[pb1]=[Rsb01×3psb1][pb1]=[Rsbpb+psb1]
    • p s = R s b p b + p s b {\bm p}_{s}={\bm R}_{sb}{\bm p}_b+{\bm p}_{sb} ps=Rsbpb+psb
  • 齐次变换矩阵计算 T s c = T s b ⋅ T b c {\bm T}_{sc}={\bm T}_{sb}\cdot{\bm T}_{bc} Tsc=TsbTbc

3.2.3.运动向量, 点, 位姿

  • 左乘运动 p s ′ = T s b ′ p b = T T s b p b = T p s {\bm p}_s'={\bm T}_{sb'}{\bm p}_b={\bm TT}_{sb}{\bm p}_b={\bm Tp}_s ps=Tsbpb=TTsbpb=Tps
    • 先变换坐标系, 在{s}运动
  • 右乘运动 p s ′ ′ = T s b ′ ′ p b = T s b T p b = T s b p b ′ ′ {\bm p}_s''={\bm T}_{sb''}{\bm p}_b={\bm T}_{sb}{\bm T}{\bm p}_b={\bm T}_{sb}{\bm p}_b'' ps′′=Tsb′′pb=TsbTpb=Tsbpb′′
    • 先在{b}运动, 再变换坐标系

4.欧拉角

  • 横滚roll, 俯仰pitch, 偏航yaw
  • Y-P-R顺序: 初态物体坐标系{b}与世界坐标系{s}重合, 先绕{b}的z轴旋转–>{b’}, 绕{b’}的y轴旋转–>{b’‘}, 绕{b’‘}的x轴旋转–>{b’‘’}
  • 相对物体坐标系旋转: 用右乘旋转矩阵方法计算
    • R s b = I {\bm R}_{sb}={\bm I} Rsb=I
    • R s b ′ = R s b R z ( y a w ) = R z ( y a w ) {\bm R}_{sb'}={\bm R}_{sb}R_z({\rm yaw})={\bm R}_z({\rm yaw}) Rsb=RsbRz(yaw)=Rz(yaw)
    • R s b ′ ′ = R s b ′ R y ( p i t c h ) = R z ( y a w ) R y ( p i t c h ) {\bm R}_{sb''}={\bm R}_{sb'}R_y({\rm pitch})={\bm R}_z({\rm yaw}){\bm R}_y({\rm pitch}) Rsb′′=RsbRy(pitch)=Rz(yaw)Ry(pitch)
    • R s b ′ ′ ′ = R s b ′ ′ R x ( r o l l ) = R z ( y a w ) R y ( p i t c h ) R x ( r o l l ) {\bm R}_{sb'''}={\bm R}_{sb''}R_x({\rm roll})={\bm R}_z({\rm yaw}){\bm R}_y({\rm pitch}){\bm R}_x({\rm roll}) Rsb′′′=Rsb′′Rx(roll)=Rz(yaw)Ry(pitch)Rx(roll)
  • R s b ′ ′ ′ {\bm R}_{sb'''} Rsb′′′ → \rightarrow { tan ⁡ γ = cos ⁡ β sin ⁡ γ cos ⁡ β cos ⁡ γ = r 32 r 33 sin ⁡ β = − r 31 tan ⁡ α = sin ⁡ α cos ⁡ β cos ⁡ α cos ⁡ β = r 21 r 11 \left\{\begin{matrix}\tan\gamma = \frac{\cos\beta\sin\gamma}{\cos\beta\cos\gamma} = \frac{r{32}}{r_{33}}\\\sin\beta = -r_{31}\\\tan\alpha = \frac{\sin\alpha\cos\beta}{\cos\alpha\cos\beta} = \frac{r{21}}{r_{11}}\end{matrix}\right. tanγ=cosβcosγcosβsinγ=r33r32sinβ=r31tanα=cosαcosβsinαcosβ=r11r21
    → \rightarrow { r o l l = arctan ⁡ 2 ( r 32 , r 33 ) p i t c h = arcsin ⁡ ( − r 31 ) y a w = arctan ⁡ 2 ( r 21 , r 11 ) \left\{\begin{matrix}{\rm roll} = \arctan2(r_{32}, r_{33}) \\{\rm pitch} = \arcsin(-r_{31})\\{\rm yaw} = \arctan2(r_{21},r_{11})\end{matrix}\right. roll=arctan2(r32,r33)pitch=arcsin(r31)yaw=arctan2(r21,r11)

ref

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

你可能感兴趣的:(机器人,笔记,线性代数,矩阵,力学)