Modern Robotics刚体运动的矩阵表示和指数坐标表示
Modern Robotics刚体运动的矩阵表示和指数坐标表示
##摘要
表示三维空间运动的刚体的位置和姿态至少需要6个参数。可以通过固连于刚体的坐标系描述其位置和方向相对于一个固定坐标系的位置和方向，物体坐标系相对于固定坐标系的的构型（或者说位姿）可以表达为一个 4 × 4 4\times 4 4×4的矩阵。矩阵表示刚体的6维构型空间实际上是一种隐式表示法，使用了具有10个约束的 4 × 4 4\times 4 4×4的实矩阵的16维空间描述。刚体运动可以用的指数坐标表示（旋量表示），这是一种显式表示法。例如刚体的速度可以用一个在 R 6 \mathbb R^6 R6空间的点表示，包括三个角速度值和三个线速度值，可以称其为空间速度（spatial velocity ）或旋量（twist ）。一般地，即使是一个机器人的构型空间不是一个向量空间，其构型空间的可行的速度总是构成一个向量空间。
刚体运动的描述
Kenvin M. Lynch 和Frank C. Park对刚体运动的描述做了如下表格的总结。
| Items | Rotations | Rigid-Body Motion |
|---|---|---|
| 1 | R ∈ S O ( 3 ) : 3 × 3 R\in SO(3):3\times 3 R∈SO(3):3×3 matrices R T R = I , d e t R = 1 R^TR=I, det R=1 RTR=I,detR=1 |
$T\in SE(3):4\times 4 $ matrices T = [ R p 0 1 ] T=\left [ \begin{matrix} R & p\\ 0& 1\end{matrix} \right ] T=[R0p1], where R ∈ S O ( 3 ) , p ∈ R 3 R\in SO(3), p\in \mathbb R^3 R∈SO(3),p∈R3 |
| 2 | R − 1 = R T R^-1=R^T R−1=RT | T − 1 = [ R T − R T p 0 1 ] T^{-1}=\left [ \begin{matrix} R^T & -R^Tp\\ 0& 1\end{matrix} \right ] T−1=[RT0−RTp1] |
| 3 | change of coordinate frame: R a b R b c = R a c , R a b p b = p a R_{ab}R_{bc}=R_{ac}, R_{ab}p_b=p_a RabRbc=Rac,Rabpb=pa |
change of coordinate frame: T a b T b c = T a c , T a b p b = p a T_{ab}T_{bc}=T_{ac}, T_{ab}p_b=p_a TabTbc=Tac,Tabpb=pa |
| 4 | rotating a frame {b}: R = Rot ( ω ^ , θ ) R = \text {Rot}(\hat \omega ,\theta) R=Rot(ω^,θ) R s b ′ = R R s b : R_{sb'}=RR_{sb} : Rsb′=RRsb: rotate θ \theta θ about ω ^ s = ω ^ \hat \omega _s=\hat \omega ω^s=ω^ R s b ′ ′ = R s b R : R_{sb''}=R_{sb}R : Rsb′′=RsbR: rotate $ \theta $ about ω ^ b = ω ^ \hat \omega _b=\hat \omega ω^b=ω^ |
displacing a frame {b}: T = [ Rot ( ω ^ , θ ) p 0 1 ] T=\left [ \begin{matrix} \text{Rot}(\hat \omega ,\theta) & p\\ 0& 1\end{matrix} \right ] T=[Rot(ω^,θ)0p1] T s b ′ = T T s b T_{sb'}=TT_{sb} Tsb′=TTsb: rotate θ \theta θ about ω ^ s = ω ^ \hat \omega_s=\hat \omega ω^s=ω^ (moves {b} origin), translate p p p in {s} T s b ′ ′ = T s b T T_{sb''}=T_{sb}T Tsb′′=TsbT: translate p p p in {b}, rotate θ \theta θ about ω ^ \hat \omega ω^ in new body frame |
| 5 | unit rotation axis is ω ^ ∈ R 3 \hat \omega \in \mathbb R^3 ω^∈R3, where $ | |
| 6 | for a screw axis { q , s ^ , h } \{q, \hat s,h\} {q,s^,h} with finite h h h， S = [ ω v ] = [ s ^ − s ^ × q + h s ^ ] \mathcal S=\left [ \begin{matrix} \omega \\ v \end{matrix}\right]=\left [ \begin{matrix} \hat s \\ - \hat s\times q+h\hat s \end{matrix}\right] S=[ωv]=[s^−s^×q+hs^] |
|
| 7 | angular velocity is ω = ω ^ θ ˙ \omega =\hat \omega\dot \theta ω=ω^θ˙ | twist is V = S θ ˙ \mathcal V=\mathcal S\dot \theta V=Sθ˙ |
| 8 | for any 3-vector , e.g, ω ∈ R 3 \omega \in \mathbb R^3 ω∈R3, [ ω ] = [ 0 − ω 3 ω 2 ω 3 0 − ω 1 − ω 2 ω 1 0 ] ∈ s o ( 3 ) [\omega]=\left[ \begin{matrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1\\ -\omega_2 & \omega_1 & 0\end{matrix}\right] \in so(3) [ω]=⎣⎡0ω3−ω2−ω30ω1ω2−ω10⎦⎤∈so(3) identities , ω , x ∈ R 3 , R ∈ S O ( 3 ) \omega, x \in \mathbb R^3, R\in SO(3) ω,x∈R3,R∈SO(3) : [ ω ] = − [ ω ] T , [ ω ] x = − [ x ] ω , [\omega]=-[\omega]^T, [\omega]x=-[x]\omega, [ω]=−[ω]T,[ω]x=−[x]ω, [ ω ] [ x ] = ( [ x ] [ ω ] ) T , R [ ω ] R T = [ R ω ] [\omega ][x]=([x][\omega])^T, R[\omega]R^T=[R\omega] [ω][x]=([x][ω])T,R[ω]RT=[Rω] |
for V = [ ω v ] ∈ R 6 \mathcal V=\left [ \begin{matrix} \omega \\ v \end{matrix}\right] \in \mathbb R^6 V=[ωv]∈R6, [ V ] = [ [ ω ] v 0 0 ] ∈ s e ( 3 ) [\mathcal V]=\left [ \begin{matrix} [\omega]& v \\0 & 0\end{matrix}\right]\in se(3) [V]=[[ω]0v0]∈se(3) (the pair ( ω , v \omega, v ω,v) can be a twist V \mathcal V V or a “unit” screw axis S \mathcal S S , depending on the context) |
| 9 | R ˙ R − 1 = [ ω s ] , R − 1 R ˙ = [ ω b ] \dot R R^{-1}=[\omega_s], R^{-1}\dot R=[\omega_b] R˙R−1=[ωs],R−1R˙=[ωb] | T ˙ T − 1 = [ V s ] , T − 1 T ˙ = [ V b ] \dot TT^{-1}=[\mathcal V_s], T^{-1}\dot T=[\mathcal V_b] T˙T−1=[Vs],T−1T˙=[Vb] |
| 10 | [ A d T ] = [ R 0 [ p ] R R ] ∈ R 6 × 6 [Ad_T]=\left[ \begin{matrix} R &0\\ [p]R &R \end{matrix}\right]\in\mathbb R^{6\times 6} [AdT]=[R[p]R0R]∈R6×6 identities: [ Ad T ] − 1 = [ Ad T − 1 ] [\text{Ad}_T]^{-1}=[\text{Ad}_{T^{-1}}] [AdT]−1=[AdT−1], [ Ad T 1 ] [ Ad T 2 ] = [ Ad T 1 T 2 ] [\text{Ad}_{T_1}][\text{Ad}_{T_2}]=[\text{Ad}_{T_1T_2}] [AdT1][AdT2]=[AdT1T2] | |
| 11 | change of coordinate frame: ω ^ = R a b ω ^ b , ω a = R a b ω b \hat \omega=R_{ab}\hat\omega_b, \omega_a=R_{ab}\omega_b ω^=Rabω^b,ωa=Rabωb |
change of coordinate frame: S a = [ Ad T a b ] S b , V a = [ Ad T a b ] V b \mathcal S_a=[\text{Ad}_{T_{ab}}]\mathcal S_b, \mathcal V_a=[\text{Ad}_{T_{ab}}]\mathcal V_b Sa=[AdTab]Sb,Va=[AdTab]Vb |
| 12 | exp coords for R ∈ S O ( 3 ) : ω ^ θ ∈ R 3 R\in SO(3): \hat\omega\theta \in \mathbb R^3 R∈SO(3):ω^θ∈R3 | exp coords for T ∈ S E ( 3 ) : S θ ∈ R 6 T\in SE(3) : \mathcal S\theta\in \mathbb R^6 T∈SE(3):Sθ∈R6 |
| 13 | exp : [ ω ^ ] θ ∈ s o ( 3 ) → R ∈ S O ( 3 ) [\hat \omega]\theta \in so(3)\to R\in SO(3) [ω^]θ∈so(3)→R∈SO(3) R = Rot ( ω ^ , θ ) = e [ ω ^ ] θ = R=\text {Rot}(\hat \omega,\theta)=e^{[\hat \omega]\theta}= R=Rot(ω^,θ)=e[ω^]θ= I + sin θ [ ω ^ ] + ( 1 − cos θ ) [ ω ^ ] 2 I+\text{sin}\theta [\hat\omega]+(1-\text{cos}\theta)[\hat\omega ]^2 I+sinθ[ω^]+(1−cosθ)[ω^]2 |
exp : [ S ] θ ∈ s e ( 3 ) → T ∈ S E ( 3 ) [\mathcal S]\theta\in se(3)\to T\in SE(3) [S]θ∈se(3)→T∈SE(3) T = e [ S ] θ = [ e [ ω ] θ ∗ 0 1 ] T=e^{\mathcal {[S]}\theta}=\left[ \begin{matrix} e^{[\omega]\theta} &*\\ 0 &1 \end{matrix}\right] T=e[S]θ=[e[ω]θ0∗1] where ∗ = ( I θ + ( 1 − cos θ ) ) [ ω ] + ( θ − sin [ ω ] 2 ) v * =(I\theta+(1-\text {cos}\theta))[\omega]+(\theta-\text{sin}[\omega]^2)v ∗=(Iθ+(1−cosθ))[ω]+(θ−sin[ω]2)v |
| 14 | log : R ∈ S O ( 3 ) → [ ω ^ ] θ ∈ s o ( 3 ) R\in SO(3)\to [\hat\omega]\theta \in so(3) R∈SO(3)→[ω^]θ∈so(3) | log : T ∈ S E ( 3 ) → [ S ] θ ∈ s e ( 3 ) T\in SE(3)\to [\mathcal S]\theta \in se(3) T∈SE(3)→[S]θ∈se(3) |
| 15 | moment change of coord frame: m a = R a b m b m_a=R_{ab}m_b ma=Rabmb |
wrench change coord frame: F a = ( m a , f a ) = [ Ad T b a ] T F b \mathcal F_a=(m_a,f_a)=[\text {Ad}_{T_{ba}}]^T\mathcal F_b Fa=(ma,fa)=[AdTba]TFb |
以上表格总结了刚体运动的矩阵表示和指数坐标表示的联系。
第1行中，刚体的旋转矩阵是一个特殊正交群 S O ( 3 ) SO(3) SO(3), 刚体运动群或其次变换矩阵是一个特殊欧式群 S E ( 3 ) SE(3) SE(3)。
第7行中，刚体的旋转改变姿态，旋转的角速度可以表示为旋转轴的单位方向矢量与转角导数的乘积 ω = ω ^ θ ˙ \omega =\hat \omega\dot \theta ω=ω^θ˙，对于刚体的一般运动（旋转和平移），其速度包含角速度和线速度两部分，共有6个实数值。可以把刚体的一般运动看成螺旋运动，因此其速度可以看成是沿一个单位螺旋轴 S \mathcal S S同时做旋转运动和平移运动，把6个实数值写在一起， V = [ ω v ] = S θ ˙ \mathcal V=\left [ \begin{matrix} \omega \\ v \end{matrix}\right]=\mathcal S\dot \theta V=[ωv]=Sθ˙.因此$ \mathcal V$又称为空间速度。
第12行，刚体旋转运动的可以用指数坐标 ω ^ θ ∈ R 3 \hat \omega\theta \in \mathbb R^3 ω^θ∈R3表示，这是一个三维向量 ,又称为刚体旋转的指数坐标。刚体运动（旋转和平移）可以用指数坐标来 S θ ∈ R 6 \mathcal S\theta\in \mathbb R^6 Sθ∈R6表示，这是一个6维向量。 [ S ] θ ∈ s e ( 3 ) [\mathcal S]\theta\in se(3) [S]θ∈se(3)又称为运动旋量， S θ \mathcal S\theta Sθ也称为运动旋量坐标。
第13,14行，说明了指数坐标到刚体旋转群和刚体运动群的映射是指数运算（或称运动旋量的指数），刚体旋转群和刚体运动群到指数坐标的映射是对数运算。
第15行，说明了力矩，力的指数坐标表示的坐标变换。
参考文献：
Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017