Rigid-Body Motion(旋转变换\Screw motion\Spatial velocities\Force)

\[SE(3)=T=\begin{bmatrix}R&p\\0&1\end{bmatrix}\]
SE(3)是一个４X4的矩阵．它具有的性质如下：

1. 两个SE(3)的乘积仍是一个SE(3)
2. SE(3)的逆：　\[T^{-1}=\begin{bmatrix}R^T&-R^Tp\\0&1\end{bmatrix}\]

3. \(T=(R,p)\in R^3, x,y\in R^3\),则有：

   • \(||Tx-Ty||=||x-y||,||．||\)指标准欧式距离，如\(||x||=\sqrt{x^Tx}\)
   • \(<Tx-Ty,Ty-Tz>=<x-y,y-z>\),\(<.>\)指内积，如\(<x,y>=x^Ty\)
     上述性质中，T是某一点的一个transformation，如T将一点x转换至Tx．３中第一个性质保持距离不变，第二个性质保持角度不变．
4. 
   在上图中，有\[R_{ab}R_{bc}=R_{ac}\] \[\vec u+\vec v=\vec w \qquad u_a=\vec u \qquad v_b=\vec v\]
   现在直接将\(\vec u\)与\(\vec v\)想加是没有意义的，因为他们是基于不同的坐标系的．所以：
   \[v_a=R_{ab}\to u_a+R_{ab}v_b=W_a\]
   \[\begin{bmatrix}R_{ab}&u_a\\0&1\end{bmatrix}\begin{bmatrix}R_{bc}&v_b\\0&1\end{bmatrix}=\begin{bmatrix}R_{ab}R_{bc}&u_a+R_{ab}v_b\\0&1\end{bmatrix}\]
   因此：
   \[T_{ab}T_{bc}=T_{ac}\]

5. \(T_{aa}=I\qquad T_{ab}^{-1}=T_{ab}\)

Screw Motion

这是在Exponential Coordinate下同时表示旋转和平移的一种表示．
给定\(w,v\in R^3\):
\[S=\begin{bmatrix}w\\v\end{bmatrix}\in R^6\]
S被称为twist，定义\([S]\in R^{4X4}\)是如下的矩阵：
\[[S]=\begin{bmatrix}[w]&v\\0&0\end{bmatrix} , [w]=\begin{bmatrix}0&-w_3&w_2\\w_3&0&-w_1\\-w_2&w_1&0\end{bmatrix}\]

根据以上的定义，我们需要推到出\(e^{[S]\theta}\).
现在给定一个旋转和平移的transform，如何求得对应在Exponential Coordinate下的\(e^{[S]\theta}\)?

以下给出算法：
已知\((R,p)\in SE(3)\),求\(e^{[S]\theta}\)：

1. 如果\(R=I\),那么\(w=0, v=p/||p||, \theta=||p||\)
2. 如果\(tr R=-1\),那么\(\theta=\pi , [w]=logR\)
3. 否则：
   \[\theta=cos^{-1}(\frac{trR-1}{2})\in [0,\pi)\]
   \[[w]=\frac{1}{2sin\theta}(R-R^T)\] \[v=G^{-1}(\theta)p\]
   其中，\(G^{-1}(\theta)=\frac{1}{\theta}I+\frac{1}{2}[w]+(\frac{1}{\theta}-\frac{1}{2}cot\frac{\theta}{2})[w]^2\)

给定\(S=(w,v)\in R^6,S'=(w',v')\in R^6,T=(R,p)\in SE(3)\),Adjoint map \(S'=Ad_T(S)\)定义：

\[\begin{align*}S&=\begin{bmatrix} w' \\ v' \end{bmatrix} \\ &= \begin{bmatrix} R&0 \\ [p]R&R \end{bmatrix}\end{align*}\]
\([Ad_T]\)是个６X6矩阵．
使用以上的定义，我们可以有：
\[S'=[Ad_T]S\]
还可以这样表示：
\[[S']=T[S]T^{-1}\]
\[\begin{bmatrix}[w']&v'\\0&0\end{bmatrix}=\begin{bmatrix}R[w]R^T&[p]Rw+Rv\\0&0\end{bmatrix}??\]

性质：

1. \(Ad_{T_1}(Ad_{T_2}(S))=Ad_{T_1T_2}(S)\)
2. \(Ad_T^{-1}=Ad_{T^{-1}}\)
3. \(Ad_{T^{-1}}(Ad_T(S))=Ad_{T^{-1}T}(S)=Ad_I(S)=S\)

假定\(S_a=(w_a,v_a)\)是在a坐标系下的screw motion,\(S_b=(w_b,v_b)\)是在b坐标系下的screw motion,则：
\[s_b=Ad_{T_{ba}}(S_a)\]\[s_a=Ad_{T_{ab}}(S_b)\]

Spatial Velocities

\(\dot RR^{-1}\)或者\(R^{-1}\dot R会得到角速度向量．　类似的我们可以得到：\)

\[\begin{align*}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}\end{align*}\]

S表示固定参参考系，B表示移动参考系（body coordinate），\(T_{sb}\in SE(3)\):
\[T_{sb}(t)=\begin{bmatrix}R(t) & p(t) \\ 0 & 1\end{bmatrix}\]
所以有：
\[T_{sb}^{-1}\dot T_{sb}=[V_b]=\begin{bmatrix}[w_b&v_b\\0&0]\end{bmatrix}\]
以上称之为移动参考系中的Spatial Velocity.
\[\dot T_{sb}T_{sb}^{-1}=[V_s]=\begin{bmatrix}[w_s]&v_s\\0&0\end{bmatrix}\]
以上称之为固定参考系中的Spatial Velocity.
\(V_s\)和\(V_b\)关系如下：
\[V_s=\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} = Ad_{T_{sb}}(V_b)\]
\[V_b=\begin{bmatrix} w_b \\ v_b \end{bmatrix}=\begin{bmatrix} R^T&0 \\ -R^T[p]&R^T \end{bmatrix}\begin{bmatrix} w_s \\ v_s \end{bmatrix} = Ad_{T_{bs}}(V_s)\]

Spacial Force

假设一个力f作用在刚体的一点p上．给定一个a参考系，\(f_a \in R^3\)是在a坐标系下的f的表示，产生力矩\(m_a\):
\[m_a=r_a\times f_a\]
其中\(r_a\in R^3\)表示坐标系a中原点到p的向量，将力\(f_a\)和力矩\(m_a\)形成\(F_a=(m_a,f_a)\),这个称之为在a坐标系下的Spatial Force.
现在确定两个坐标系a与b中Spatial Force：　\(F_a={m_a,f_a}\)与\(F_b={m_b,f_b}\)的关系:
\(T_{ab}=\begin{bmatrix}R_{ab} & p_{ab} \\ 0 & 1\end{bmatrix}\)

在上图中，显然\[f_b=R_{ba}f_a\]
\(r_b=R_{ba}(r_a-p_{ab})\),这是因为\(r_a-p_{ab}\)是在a坐标系中表示的，转换到b坐标系下需要诚意R_{ba},注意与之前的在同一个坐标系中表示旋转的向量点的不同:
\[r_b=R_{ab}^T(r_a-p_{ab})\]

力矩\(m_b=r_b\times f_b\)就可以写成：
\[\begin{align*}m_b &= R_{ab}^T(r_a-p_{ab})\times R_{ab}^Tf_a\\ &=[R_{ab}^Tr_a]R_{ab}^Tf_a-[R_{ab}^Tp_{ab}]R_{ab}^Tf_a\\ &=R_{ab}^T[r_a]f_a-R_{ab}^T[p_{ab}]f_a\\ &=R_{ab}^Tm_a+R_{ab}^T[p_{ab}]^Tf_a\end{align*}\]
因为\([p_{ab}]^T=-[p_{ab}]\),所以：
\[\begin{bmatrix} m_b \\ f_b \end{bmatrix}=\begin{bmatrix} R_{ab} & 0 \\ [p_{ab}]R_{ab} & R_{ab}\end{bmatrix}^T\begin{bmatrix} m_a \\ f_a \end{bmatrix}\]
或者可以表示如下：
\[F_b=Ad_{T_{ab}}^T(F_a)=[Ad_{T_{ab}}]^TF_a\]

转载于:https://www.cnblogs.com/rogerjin/p/6635389.html