前言
本篇博客摘抄从Rotation Matrix到Twist与Wrench的基础数学操作
正文
-
Rotation Matrix:
各列向量模长为1
各列向量之间相互正交
d e t ∣ R ∣ = 1 det|R|=1 det∣R∣=1
R T R = I R^TR=I RTR=I
闭包: A B AB AB still in group
结合律: ( A B ) C = A ( B C ) (AB)C=A(BC) (AB)C=A(BC)
单位元: A I = I A = A AI=IA=A AI=IA=A
三种作用: (1) 表示刚体姿态 (2) 旋转变换运算子 (3) 改变刚体运动的参考系 -
skew-symmetric矩阵: 来源于Rotation Matrix的微分(足够短的时间内对 R R R内的各列向量作旋转变化:角速度,即单位时间内的旋转矩阵) : [ w ] r = w × r [w]r=w\times r [w]r=w×r
(1)
[ x ] = [ 0 − x 3 x 2 x 3 0 − x 1 − x 2 x 1 0 ] = − [ x ] T [\boldsymbol{x}] = \begin{bmatrix} 0 & -x_3 &x_2\\ x_3 & 0 & -x_1\\ -x_2 & x_1 & 0\\ \end{bmatrix} =-[\boldsymbol{x}]^T [x]=⎣ ⎡0x3−x2−x30x1x2−x10⎦ ⎤=−[x]T
(2)
R [ w ] R T = [ R w ] R[w]R^T=[Rw] R[w]RT=[Rw]
proof: 假设 r i T r^T_i riT为R的行向量,
R [ w ] R T = R [ w ] [ r 1 r 2 r 3 ] = [ r 1 T r 2 T r 3 T ] [ w ] [ r 1 r 2 r 3 ] = [ r 1 T ( w × r 1 ) r 1 T ( w × r 2 ) r 1 T ( w × r 3 ) r 2 T ( w × r 1 ) r 2 T ( w × r 2 ) r 2 T ( w × r 3 ) r 3 T ( w × r 1 ) r 3 T ( w × r 2 ) r 3 T ( w × r 3 ) ] = [ 0 − r 3 T w r 2 T w r 3 T w 0 − r 1 T w − r 2 T w r 1 T w 0 ] = [ R w ] \begin{split} R[w]R^T &= R[w][r_1\ r_2\ r_3] = \begin{bmatrix} r^T_1 \\ r^T_2 \\ r^T_3 \end{bmatrix}[w][r_1\ r_2\ r_3] =\begin{bmatrix} r^T_1(w\times r_1)& r^T_1(w\times r_2) & r^T_1(w\times r_3)\\ r^T_2(w\times r_1) & r^T_2(w\times r_2) & r^T_2(w\times r_3)\\ r^T_3(w\times r_1) & r^T_3(w\times r_2) & r^T_3(w\times r_3) \end{bmatrix}\\ &=\begin{bmatrix} 0 & -r^T_3w & r^T_2w\\ r^T_3w & 0 & -r^T_1w\\ -r^T_2w & r^T_1w & 0 \end{bmatrix}=[Rw] \end{split} R[w]RT=R[w][r1 r2 r3]=⎣ ⎡r1Tr2Tr3T⎦ ⎤[w][r1 r2 r3]=⎣ ⎡r1T(w×r1)r2T(w×r1)r3T(w×r1)r1T(w×r2)r2T(w×r2)r3T(w×r2)r1T(w×r3)r2T(w×r3)r3T(w×r3)⎦ ⎤=⎣ ⎡0r3Tw−r2Tw−r3Tw0r1Twr2Tw−r1Tw0⎦ ⎤=[Rw]
w × r w\times r w×r出来的向量正交于 r r r
(3) R R R可以表示刚体在空间中的姿态, w s w_s ws是以世界坐标系为参考坐标系的微分旋转变换子,
[ w s ] R = R ˙ [w_s]R=\dot{R} [ws]R=R˙, 表示在足够短的时间内, R R R的变化.
[ w s ] = R ˙ R − 1 [w_s]=\dot{R}R^{-1} [ws]=R˙R−1类似的 w b w_b wb以物体坐标系为参考, 同样可以作用于 R R R实现微分旋转变换, w s w_s ws与 w b w_b wb作为两向量,之间的关系有:
w s = R s b w b w_s= R_{sb}w_b ws=Rsbwb, w b = R s b − 1 w s = R − 1 w s = R T w s w_b= R^{-1}_{sb}w_s=R^{-1}w_s=R^Tw_s wb=Rsb−1ws=R−1ws=RTws[ w b ] = [ R T w s ] = R T [ w s ] R = R T [ R ˙ R T ] R = R T R ˙ = R − 1 R ˙ \begin{split} [w_b] &= [R^Tw_s]\\ &= R^T[w_s]R\\ &= R^T[\dot{R}R^T]R\\ &= R^T\dot{R} = R^{-1}\dot{R} \end{split} [wb]=[RTws]=RT[ws]R=RT[R˙RT]R=RTR˙=R−1R˙
summary:
R ˙ R − 1 = [ w s ] R − 1 R ˙ = [ w b ] \begin{split} \dot{R}R^{-1} &= [w_s]\\ R^{-1}\dot{R}&=[w_b] \end{split} R˙R−1R−1R˙=[ws]=[wb]
谨记: 向量就是向量, 与坐标系的选择无关,只是数值上会随坐标系变化而变化而已. -
Exponential Coordinate Representation of Rotation:
基础:
x ˙ ( t ) = A x ( t ) \dot{x}(t)=Ax(t) x˙(t)=Ax(t)
x ( t ) = e A t x 0 x(t) = e^{At}x_0 x(t)=eAtx0
e A t = I + A t + ( A t ) 2 2 ! + ( A t ) 3 3 ! + ⋯ e^{At} = I+At+\frac{(At)^2}{2!}+\frac{(At)^3}{3!}+\cdots eAt=I+At+2!(At)2+3!(At)3+⋯
对于任一方阵 A A A与标量 t t t, 即使 A B ≠ B A AB\neq BA AB=BA
A e A t = e A t A Ae^{At} = e^{At}A AeAt=eAtA 因此有:
x ˙ ( t ) = e A t A x 0 \dot{x}(t)=e^{At}Ax_0 x˙(t)=eAtAx0
如果A可以通过正交分解: A = P D P − 1 A=PDP^{-1} A=PDP−1
e A t = I + A t + ( A t ) 2 2 ! + ⋯ = I + P D P − 1 t + P D P − 1 P D P − 1 t 2 2 ! + ⋯ = P ( I + D t + ( D t ) 2 2 ! + ⋯ ) P − 1 = P e D t P − 1 \begin{split} e^{At} &= I + At + \frac{(At)^2}{2!} + \cdots \\ &= I+PDP^{-1}t + PDP^{-1}PDP^{-1}\frac{t^2}{2!}+ \cdots \\ &= P (I+Dt + \frac{(Dt)^2}{2!}+\cdots) P^{-1}\\ &= Pe^{Dt}P^{-1} \end{split} eAt=I+At+2!(At)2+⋯=I+PDP−1t+PDP−1PDP−12!t2+⋯=P(I+Dt+2!(Dt)2+⋯)P−1=PeDtP−1
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 & \ddots & \vdots \\ 0 & 0 & \cdots & e^{d_nt} \end{bmatrix} eDt=⎣ ⎡ed1t0⋮00ed2t⋮0⋯⋯⋱⋯00⋮ednt⎦ ⎤
e A t e^{At} eAt的性质有:
(a) d ( e A t ) / d t = A e A t = e A t A d(e^{At})/dt=Ae^{At}=e^{At}A d(eAt)/dt=AeAt=eAtA
(b) A = P D P − 1 A=PDP^{-1} A=PDP−1 , e A t = P e D t P − 1 e^{At}=Pe^{Dt}P^{-1} eAt=PeDtP−1
(c ) A B = B A AB=BA AB=BA条件满足才能有 A B + B A = 2 A B AB+BA=2AB AB+BA=2AB, e A e B = e A + e B e^{A}e^{B}=e^A+e^B eAeB=eA+eB
(d) ( e A ) − 1 = e − A (e^A)^{-1}=e^{-A} (eA)−1=e−Aw w w的旋转变换子 ,对于一个向量而言,对其作微分旋转有:
p ˙ = w ^ × p = [ w ^ ] p \dot{p}=\hat{w}\times p = [\hat{w}]p p˙=w^×p=[w^]p
因此有:
p ( t ) = e [ w ] t p ( 0 ) p(t) = e^{[w]t}p(0) p(t)=e[w]tp(0)
上式速度为恒量,而如果时间为恒量则有:
p ( t ) = e [ w ] θ p ( 0 ) p(t) = e^{[w]\theta}p(0) p(t)=e[w]θp(0)通过直接的手算可以知道: [ w ^ ] 3 = − [ w ^ ] [\hat{w}]^3=-[\hat{w}] [w^]3=−[w^], [ w ^ ] 4 = − [ w ^ ] 2 [\hat{w}]^4=-[\hat{w}]^2 [w^]4=−[w^]2, [ w ^ ] 5 = − [ w ^ ] 3 = [ w ^ ] [\hat{w}]^5=-[\hat{w}]^3=[\hat{w}] [w^]5=−[w^]3=[w^]
e [ w ^ ] θ = I + [ w ^ ] θ + [ w ^ ] 2 θ 2 2 ! + [ w ^ ] 3 θ 3 3 ! + ⋯ = I + ( θ − θ 3 3 ! + θ 5 5 ! − ⋯ ) [ w ^ ] + ( θ 2 2 ! − θ 4 4 ! + θ 6 6 ! − ⋯ ) [ w ^ ] 2 \begin{split} e^{[\hat{w}]\theta} &= I + [\hat{w}]\theta + [\hat{w}]^2 \frac{\theta^2}{2!}+ [\hat{w}]^3 \frac{\theta^3}{3!}+\cdots\\ &=I+ (\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}-\cdots)[\hat{w}]+(\frac{\theta^2}{2!}-\frac{\theta^4}{4!}+\frac{\theta^6}{6!}-\cdots)[\hat{w}]^2 \end{split} e[w^]θ=I+[w^]θ+[w^]22!θ2+[w^]33!θ3+⋯=I+(θ−3!θ3+5!θ5−⋯)[w^]+(2!θ2−4!θ4+6!θ6−⋯)[w^]2
s i n θ = ( θ − θ 3 3 ! + θ 5 5 ! − ⋯ ) c o s θ = ( 1 − θ 2 2 ! + θ 4 4 ! − θ 6 6 ! + ⋯ ) \begin{split} sin\theta &= (\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}-\cdots) \\ cos\theta &= (1-\frac{\theta^2}{2!}+\frac{\theta^4}{4!}-\frac{\theta^6}{6!}+\cdots) \end{split} sinθcosθ=(θ−3!θ3+5!θ5−⋯)=(1−2!θ2+4!θ4−6!θ6+⋯)
回代即可得到罗德里格斯公式
R o t ( w ^ , θ ) = e [ w ^ ] θ = I + s i n θ [ w ^ ] + ( 1 − c o s θ ) [ w ^ ] 2 Rot(\hat{w},\theta) = e^{[\hat{w}]\theta} = I+sin\theta[\hat{w}]+(1-cos\theta)[\hat{w}]^2 Rot(w^,θ)=e[w^]θ=I+sinθ[w^]+(1−cosθ)[w^]2Exp的逆操作: Log操作 对于上面的罗德里格斯公式通过计算会有:
[ c θ + w ^ 1 2 ( 1 − c θ ) w ^ 1 w ^ 2 ( 1 − c θ ) − w ^ 3 s θ w ^ 1 w ^ 3 ( 1 − c θ ) + w ^ 2 s θ w ^ 1 w ^ 2 ( 1 − c θ ) + w ^ 3 s θ c θ + w ^ 2 2 ( 1 − c θ ) w ^ 2 w ^ 3 ( 1 − c θ ) − w ^ 1 s θ w ^ 1 w ^ 2 ( 1 − c θ ) − w ^ 2 s θ w ^ 2 w ^ 3 ( 1 − c θ ) + w ^ 1 s θ c θ + w ^ 3 2 ( 1 − c θ ) ] \begin{bmatrix} c_\theta+\hat{w}^2_1(1-c_\theta) & \hat{w}_1\hat{w}_2(1-c_\theta)-\hat{w}_3s_\theta & \hat{w}_1\hat{w}_3(1-c_\theta)+\hat{w}_2s_\theta\\ \hat{w}_1\hat{w}_2(1-c_\theta)+\hat{w}_3s_{\theta} & c_\theta+\hat{w}^2_2(1-c_\theta) & \hat{w}_2\hat{w}_3(1-c_\theta)-\hat{w}_1s_\theta\\ \hat{w}_1\hat{w}_2(1-c_\theta)-\hat{w}_2s_{\theta} &\hat{w}_2\hat{w}_3(1-c_\theta)+\hat{w}_1s_{\theta} & c_\theta+\hat{w}^2_3(1-c_\theta) \end{bmatrix} ⎣ ⎡cθ+w^12(1−cθ)w^1w^2(1−cθ)+w^3sθw^1w^2(1−cθ)−w^2sθw^1w^2(1−cθ)−w^3sθcθ+w^22(1−cθ)w^2w^3(1−cθ)+w^1sθw^1w^3(1−cθ)+w^2sθw^2w^3(1−cθ)−w^1sθcθ+w^32(1−cθ)⎦ ⎤
还原算法:
(1) 如果: R = I R=I R=I,且 θ = 0 \theta=0 θ=0 同时 w ^ \hat{w} w^是未定义
(2) 如果: t r R = − 1 trR = -1 trR=−1, θ = π \theta=\pi θ=π, w ^ = 1 2 ( 1 + r 33 ) [ r 13 r 23 1 + r 33 ] \hat{w}=\frac{1}{\sqrt{2(1+r_{33})}}\begin{bmatrix}r_{13}\\r_{23}\\1+r_{33}\end{bmatrix} w^=2(1+r33)1⎣ ⎡r13r231+r33⎦ ⎤ 或者 w ^ = 1 2 ( 1 + r 22 ) [ r 12 1 + r 22 r 33 ] \hat{w}=\frac{1}{\sqrt{2(1+r_{22})}}\begin{bmatrix}r_{12}\\1+r_{22}\\r_{33}\end{bmatrix} w^=2(1+r22)1⎣ ⎡r121+r22r33⎦ ⎤ 或者 w ^ = 1 2 ( 1 + r 11 ) [ 1 + r 11 r 21 r 31 ] \hat{w}=\frac{1}{\sqrt{2(1+r_{11})}}\begin{bmatrix}1+r_{11}\\r_{21}\\r_{31}\end{bmatrix} w^=2(1+r11)1⎣ ⎡1+r11r21r31⎦ ⎤
(3) 否则, θ = c o s − 1 ( 1 2 ( t r R − 1 ) ) ∈ [ 0 , π ) \theta = cos^{-1}(\frac{1}{2}(trR-1))\in[0,\pi) θ=cos−1(21(trR−1))∈[0,π)
[ w ^ ] = 1 2 s i n θ ( R − R T ) [\hat{w}] = \frac{1}{2sin\theta}(R-R^T) [w^]=2sinθ1(R−RT) -
一个关于齐次变换矩阵T的点:
T − 1 = [ R p 0 1 ] − 1 = [ R T − R T p 0 1 ] T^{-1} =\begin{bmatrix}R& p \\ 0 & 1\end{bmatrix}^{-1}=\begin{bmatrix}R^T& -R^Tp\\0 & 1\end{bmatrix} T−1=[R0p1]−1=[RT0−RTp1] -
Twists
body twist的介绍: 类比 w b = R − 1 R ˙ w_b=R^{-1}\dot{R} wb=R−1R˙有:
T − 1 T ˙ = [ R T − R T p 0 1 ] [ R ˙ p ˙ 0 0 ] = [ R T R ˙ R T p ˙ 0 0 ] = [ [ w b ] v b 0 0 ] T^{-1}\dot{T}=\begin{bmatrix} R^T & -R^Tp\\ 0 &1 \end{bmatrix}\begin{bmatrix} \dot{R} & \dot{p}\\ 0 & 0\\ \end{bmatrix} = \begin{bmatrix} R^T\dot{R} & R^T\dot{p}\\ 0 & 0 \\ \end{bmatrix} = \begin{bmatrix} [w_b] & v_b\\ 0 & 0 \end{bmatrix} T−1T˙=[RT0−RTp1][R˙0p˙0]=[RTR˙0RTp˙0]=[[wb]0vb0]
也可以这样理解:
T ˙ = T [ [ w b ] v b 0 0 ] \dot{T} = T \begin{bmatrix} [w_b] & v_b\\ 0 & 0 \end{bmatrix} T˙=T[[wb]0vb0]
body frame下满足右乘.
V b = [ w b v b ] ∈ R 6 \mathcal{V}_b = \begin{bmatrix}w_b \\ v_b\end{bmatrix}\in \mathbb{R}^6 Vb=[wbvb]∈R6
T − 1 T ˙ = [ V b ] = [ [ w b ] v b 0 0 ] ∈ s e ( 3 ) T^{-1}\dot{T}=[\mathcal{V}_b]=\begin{bmatrix}[w_b] & v_b \\ 0 & 0\end{bmatrix}\in se(3) T−1T˙=[Vb]=[[wb]0vb0]∈se(3)spatial frame版本:
T ˙ T − 1 = [ R ˙ q ˙ 0 0 ] [ R T − R T p 0 1 ] = [ R ˙ R T p ˙ − R ˙ R T p 0 0 ] = [ [ w s ] v s 0 0 ] \begin{split} \dot{T}T^{-1} &= \begin{bmatrix} \dot{R} & \dot{q}\\ 0 & 0 \end{bmatrix}\begin{bmatrix} R^T & -R^Tp\\ 0 & 1\\ \end{bmatrix} = \begin{bmatrix} \dot{R}R^T & \dot{p}-\dot{R}R^Tp\\ 0 & 0\\ \end{bmatrix}=\begin{bmatrix}[w_s] & v_s \\ 0 & 0\end{bmatrix} \end{split} T˙T−1=[R˙0q˙0][RT0−RTp1]=[R˙RT0p˙−R˙RTp0]=[[ws]0vs0]
v s v_s vs中带有的 − R ˙ R T p -\dot{R}R^Tp −R˙RTp是有角速度并将刚体速度向量移动到世界坐标系中引起的. -
V b \mathcal{V}_b Vb与 V s \mathcal{V}_s Vs之间的关系:
[ V b ] = T − 1 T ˙ = T − 1 [ V s ] T [\mathcal{V}_b] = T^{-1}\dot{T}=T^{-1}[\mathcal{V_s}]T [Vb]=T−1T˙=T−1[Vs]T
V s = T [ V b ] T − 1 \mathcal{V}_s=T[\mathcal{V}_b]T^{-1} Vs=T[Vb]T−1
展开有:
V s = [ R [ w b ] R T − R [ w b ] R T p + R v b 0 0 ] \mathcal{V}_s= \begin{bmatrix}R[w_b]R^T & -R[w_b]R^Tp+Rv_b \\ 0 & 0\end{bmatrix} Vs=[R[wb]RT0−R[wb]RTp+Rvb0]
引入下面两个等式有:
R [ w b ] R T = [ R w ] R[w_b]R^T=[Rw] R[wb]RT=[Rw], [ w ] p = − p [ w ] [w]p = -p[w] [w]p=−p[w](向量叉乘,前后调换位置,符号也调换正负), 对比 [ V s ] [\mathcal{V_s}] [Vs]有:
[ w s v s ] = [ R 0 [ p ] R R ] [ w b v b ] \begin{bmatrix} w_s\\ v_s\end{bmatrix} = \begin{bmatrix}R & 0 \\ [p]R & R\end{bmatrix}\begin{bmatrix}w_b \\ v_b\end{bmatrix} [wsvs]=[R[p]R0R][wbvb] -
Adjoint representation(日后的动力学Gauss-Newton法也有用到): 用于向量在坐标系之间的来回切换:
对于 T = ( R , p ) ∈ S E ( 3 ) T = (R,p)\in SE(3) T=(R,p)∈SE(3), adjoint representation有:
[ A d T ] = [ R 0 [ p ] R R ] ∈ R 6 × 6 [Ad_T] = \begin{bmatrix}R & 0 \\ [p]R & R\end{bmatrix} \in \mathbb{R}^{6\times 6} [AdT]=[R[p]R0R]∈R6×6
V ∈ R 6 \mathcal{V}\in \mathbb{R}^6 V∈R6 , adjoint map 有:
V ′ = [ A d T ] V \mathcal{V}' = [Ad_T]\mathcal{V} V′=[AdT]V 也记作: V ′ = A d T ( V ) \mathcal{V}'=Ad_T(\mathcal{V}) V′=AdT(V)
而如果 V \mathcal{V} V写作矩阵形式有:
[ V ′ ] = T [ V ] T − 1 [\mathcal{V}']=T[\mathcal{V}]T^{-1} [V′]=T[V]T−1
此外还有: 如果 T 1 , T 2 ∈ S E ( 3 ) T_1,T_2\in SE(3) T1,T2∈SE(3) 和 V = ( w , v ) \mathcal{V}=(w,v) V=(w,v),然后有
A d T 1 ( A d T 2 ( V ) ) = A d T 1 T 2 ( V ) o r [ A d T 1 ] [ A d T 2 ] V = [ A d T 1 T 2 ] V Ad_{T_1}(Ad_{T_2}(\mathcal{V})) = Ad_{T_1T_2}(\mathcal{V}) or [Ad_{T_1}][Ad_{T_2}]\mathcal{V}=[Ad_{T_1T_2}]\mathcal{V} AdT1(AdT2(V))=AdT1T2(V)or[AdT1][AdT2]V=[AdT1T2]V
还有: T ∈ S E ( 3 ) T\in SE(3) T∈SE(3), [ A d T 1 ] − 1 = [ A d T − 1 ] [Ad_{T_1}]^{-1}=[Ad_{T^{-1}}] [AdT1]−1=[AdT−1]
以及: A d T − 1 ( A d T ( V ) ) = A d T − 1 T ( V ) = A d I ( V ) = V Ad_{T^{-1}}(Ad_T(\mathcal{V}))=Ad_{T^{-1}T}(\mathcal{V})=Ad_I(\mathcal{V})=\mathcal{V} AdT−1(AdT(V))=AdT−1T(V)=AdI(V)=V -
旋量看twist: S = { q , S ^ , h } \mathcal{S}=\{q,\hat{S},h\} S={q,S^,h}, h h h是沿转轴线速度与角速度之间的比值.
V = [ w v ] = [ s ^ θ ˙ − s ^ θ ˙ × q + h s ^ θ ˙ ] \mathcal{V}=\begin{bmatrix} w\\ v \end{bmatrix}= \begin{bmatrix} \hat{s} \dot{\theta}\\ -\hat{s}\dot{\theta}\times q+ h\hat{s}\dot{\theta} \end{bmatrix} V=[wv]=[s^θ˙−s^θ˙×q+hs^θ˙]
S = [ w v ] ∈ R 6 S=\begin{bmatrix}w\\ v \end{bmatrix}\in \mathbb{R}^6 S=[wv]∈R6, S S S是带有单位化要求的 V \mathcal{V} V
[ S ] = [ [ w ] v 0 0 ] [\mathcal{S}]=\begin{bmatrix}[w] & v \\ 0 & 0\end{bmatrix} [S]=[[w]0v0] , [ w ] = [ 0 − w 3 w 2 w 3 0 − w 1 − w 2 w 1 0 ] [w]=\begin{bmatrix}0 & -w_3 & w_2\\ w_3 & 0 &-w_1\\ -w_2 & w_1 &0\end{bmatrix} [w]=⎣ ⎡0w3−w2−w30w1w2−w10⎦ ⎤
S a \mathcal{S_a} Sa与 S b \mathcal{S_b} Sb之间的关系同样满足:
S a = [ A d T a b ] S b ; S b = [ A d T b a ] S a ; \mathcal{S_a}=[Ad_{T_{ab}}]\mathcal{S_b}; \mathcal{S_b}=[Ad_{T_{ba}}]\mathcal{S_a}; Sa=[AdTab]Sb;Sb=[AdTba]Sa; -
旋量的指数坐标:
e [ S ] θ = I + [ S ] θ + [ S 2 ] θ 2 2 ! + [ S 3 ] θ 3 3 ! + ⋯ = [ e [ w ] θ G ( θ ) v 0 1 ] , G ( θ ) = I θ + [ w ] θ 2 2 ! + [ w 2 ] θ 3 3 ! + ⋯ \begin{split} e^{[\mathcal{S}]\theta}&=I + [\mathcal{S}]\theta+ [\mathcal{S^2}]\frac{\theta^2}{2!}+ [\mathcal{S^3}]\frac{\theta^3}{3!}+\cdots\\ &=\begin{bmatrix} e^{[w]\theta} & G(\theta)v\\ 0 & 1 \end{bmatrix}, G(\theta) = I \theta+[\mathcal{w}]\frac{\theta^2}{2!}+[\mathcal{w}^2]\frac{\theta^3}{3!}+\cdots \end{split} e[S]θ=I+[S]θ+[S2]2!θ2+[S3]3!θ3+⋯=[e[w]θ0G(θ)v1],G(θ)=Iθ+[w]2!θ2+[w2]3!θ3+⋯
G ( θ ) = I θ + [ w ] θ 2 2 ! + [ w ] θ 3 3 ! + ⋯ = I θ + ( θ 2 2 ! − θ 4 4 ! + θ 6 6 ! − ⋯ ) [ w ] + ( θ 3 3 ! − θ 5 5 ! + θ 7 7 ! − ⋯ ) [ w ] 2 = I θ + ( 1 − c o s θ ) [ w ] + ( θ − s i n θ ) [ w ] 2 \begin{split} G(\theta)&=I\theta+[w]\frac{\theta^2}{2!}+[w]\frac{\theta^3}{3!}+\cdots \\ &= I\theta+(\frac{\theta^2}{2!}-\frac{\theta^4}{4!}+\frac{\theta^6}{6!}-\cdots)[w]+(\frac{\theta^3}{3!}-\frac{\theta^5}{5!}+\frac{\theta^7}{7!}-\cdots)[w]^2\\ &= I\theta+(1-cos\theta)[w]+(\theta-sin\theta)[w]^2 \end{split} G(θ)=Iθ+[w]2!θ2+[w]3!θ3+⋯=Iθ+(2!θ2−4!θ4+6!θ6−⋯)[w]+(3!θ3−5!θ5+7!θ7−⋯)[w]2=Iθ+(1−cosθ)[w]+(θ−sinθ)[w]2
总结有:
e [ S ] θ = [ e [ w ] θ I θ + ( 1 − c o s θ ) [ w ] + ( θ − s i n θ ) [ w ] 2 0 1 ] e^{[\mathcal{S}]\theta} = \begin{bmatrix} e^{[w]\theta} & I\theta+(1-cos\theta)[w]+(\theta-sin\theta)[w]^2\\ 0 & 1\\ \end{bmatrix} e[S]θ=[e[w]θ0Iθ+(1−cosθ)[w]+(θ−sinθ)[w]21]
如果 w = 0 w=0 w=0 且 ∣ ∣ v ∣ ∣ = 1 ||v||=1 ∣∣v∣∣=1有:
e [ S ] θ = [ I v θ 0 1 ] e^{[\mathcal{S}]\theta} = \begin{bmatrix} I & v\theta\\ 0 & 1 \end{bmatrix} e[S]θ=[I0vθ1] -
旋量坐标的Log操作:
e [ S ] θ = [ R p 0 1 ] e^{[\mathcal{S}]\theta} = \begin{bmatrix}R & p \\ 0 & 1\end{bmatrix} e[S]θ=[R0p1]
对应的log矩阵( v θ v\theta vθ来源于 G ( θ ) v G(\theta)v G(θ)v的log操作):
[ S ] θ = [ [ w ] θ v θ 0 0 ] [\mathcal{S}]\theta = \begin{bmatrix} [w]\theta & v\theta\\ 0 & 0 \end{bmatrix} [S]θ=[[w]θ0vθ0]
对应的运算为:
给定 e [ S ] θ = T e^{[\mathcal{S}]\theta}=T e[S]θ=T, S θ ∈ R 6 \mathcal{S}\theta\in \mathbb{R}^6 Sθ∈R6为 T T T的指数坐标, [ S ] θ ∈ s e ( 3 ) [\mathcal{S}]\theta\in se(3) [S]θ∈se(3)为 T T T的矩阵log坐标。
(1) 如果 R = I R=I R=I, 则 w = 0 , v = p / ∣ ∣ p ∣ ∣ w=0,v=p/||p|| w=0,v=p/∣∣p∣∣,同时 θ = ∣ ∣ p ∣ ∣ \theta=||p|| θ=∣∣p∣∣
(2) 否则对 S O ( 3 ) SO(3) SO(3)使用矩阵对数操作去确定 w ^ \hat{w} w^以及 θ \theta θ:
v = G − 1 ( θ ) p v = G^{-1}(\theta)p v=G−1(θ)p
G − 1 ( θ ) = 1 θ I − 1 2 [ w ] + ( 1 θ − 1 2 c o t θ 2 ) [ w ] 2 G^{-1}(\theta)=\frac{1}{\theta}I-\frac{1}{2}[w]+(\frac{1}{\theta}-\frac{1}{2}cot\frac{\theta}{2})[w]^2 G−1(θ)=θ1I−21[w]+(θ1−21cot2θ)[w]2 -
Wrench : 力也是向量，不受坐标系的变换而影响。
F a = [ m a f a ] ∈ R 6 \mathcal{F_a} = \begin{bmatrix} m_a \\ f_a\end{bmatrix} \in \mathbb{R}^6 Fa=[mafa]∈R6
m a m_a ma为moment, m a = r a × f a m_a=r_a\times f_a ma=ra×fa
功率是不随坐标系而改变
V b T F b = V a T F a \mathcal{V_b}^T\mathcal{F_b}=\mathcal{V_a}^T\mathcal{F_a} VbTFb=VaTFa， V a = [ A d T a b ] V b \mathcal{V_a}=[Ad_{T_{ab}}]\mathcal{V_ b} Va=[AdTab]Vb
V b T F b = ( [ A d T a b ] V b T ) F a = V b T [ A d T a b ] T F a \mathcal{V_b}^T \mathcal{F_b} = ([Ad_{T_{ab}}]\mathcal{V_b}^T)\mathcal{F}_a=\mathcal{V_b}^T[Ad_{T_{ab}}]^T\mathcal{F}_a VbTFb=([AdTab]VbT)Fa=VbT[AdTab]TFa
即:
F b = A d T a b T ( F a ) = [ A d T a b ] T F a F a = A d T b a T ( F b ) = [ A d T b a ] T F b \begin{split} \mathcal{F}_b &=Ad^T_{T_{ab}}(\mathcal{F_a})=[Ad_{T_{ab}}]^T\mathcal{F_a}\\ \mathcal{F}_a &=Ad^T_{T_{ba}}(\mathcal{F_b})=[Ad_{T_{ba}}]^T\mathcal{F_b}\\ \end{split} FbFa=AdTabT(Fa)=[AdTab]TFa=AdTbaT(Fb)=[AdTba]TFb
F s \mathcal{F}_s Fs是wrench在世界坐标系中的表达； F b \mathcal{F}_b Fb是wrench在物体坐标系上的表达。
扫一扫
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
