绕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θ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θ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θ0−sinθ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}\;\; Rsb−1=Rbs R s b − 1 = R s b T \textit{\textbf{R}}_{sb}^{-1}=\textit{\textbf{R}}_{sb}^{\rm T} Rsb−1=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} (Rab⋅Rbc)⋅Rcd=Rab⋅(Rbc⋅Rcd)=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=Rab⋅Rbc=Rbc⋅Rab
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−az0axay−ax0⋅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)=(a⋅c)b−(a⋅b)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}} a⋅b=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×b∣c∣=∣a∣∣b∣sinθ
2.3.绕轴旋转的线速度
∣ v ω ∣ = ∣ ω ∣ ⋅ ∣ p ∣ ⋅ sin ∠ A O P |\textit{\textbf{v}}_\omega|=|{\bm\omega}|\cdot|\textit{\textbf{p}}|\cdot\sin\angle AOP ∣vω∣=∣ω∣⋅∣p∣⋅sin∠AOP
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˙sbRsb−1=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)=x0→x(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)=x0→x(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=ed1t0⋮00ed2t⋮0⋯⋯⋯00⋮ednt
非对角阵, 先对角化 A = P D P − 1 {\bm A}={\bm PDP}^{-1} A=PDP−1: e A t = P e D t P − 1 e^{
{\bm A}t}={\bm P}e^{Dt}{\bm P}^{-1} eAt=PeDtP−1
若 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)=e−A
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=0∞n![ω^×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θ[ω^]×+(1−cosθ)[ω^]×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} T−1=[R01×3p1]−1=[RT01×3−RTp1]
封闭性: 齐次变换矩阵乘积仍为齐次变换矩阵
满足结合律: ( 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} (Tab⋅Tbc)⋅Tcd=Tab⋅(Tbc⋅Tcd)=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=Tab⋅Tbc=Tbc⋅Tab
3.2.齐次变换矩阵 T {\bm T} T的作用
描述刚体位置和姿态(位姿)
1. T {\bm T} T作为状态量
对向量,点,坐标系变换参考坐标系
运动(旋转+平移)向量,点,位姿
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=RsbT⋅Rsc
[ 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⋅[psc−psb0]
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=Tsb⋅Tbc
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′=Tsb′pb=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′′
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′′=Rsb′Ry(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)