二 等效旋转矢量：
1 一些重要的三维矢量运算关系（证明请自己找）
$ u为单位矢量 ;u’是u的一阶导数$
(1):V1×(V2×V3)=(V1∗V3)V2−(V1∗V2)V3(1):V_1\times(V_2\times V_3)=(V_1*V_3)V_2-(V_1*V_2)V_3(1):V1×(V2×V3)=(V1∗V3)V2−(V1∗V2)V3
(2):(u×)(u′×)(u×)=(u′×)(u×)(u′×)=0(2):(u\times)(u'\times )(u\times)=(u'\times)(u\times )(u'\times)=0(2):(u×)(u′×)(u×)=(u′×)(u×)(u′×)=0
(3):(V2×)(V3×)=−(V1∗V2)+V2V1T(3):(V_2\times)( V_3\times)=-(V_1*V_2)+V_2V_1^T(3):(V2×)(V3×)=−(V1∗V2)+V2V1T
(4):(V1×)(V2×)−(V2×)(V1×)=V2V1T−V1V2T=(V1×V2)×(4):(V_1\times)( V_2\times)-(V_2\times)( V_1\times)=V_2V_1^T-V_1V_2^T=(V_1\times V_2)\times(4):(V1×)(V2×)−(V2×)(V1×)=V2V1T−V1V2T=(V1×V2)×
(5):V1∗(V2×V3)=V2∗(V3×V1)=V3∗(V1×V2)(5):V_1*(V_2\times V_3)=V_2*(V_3\times V_1)=V_3*(V_1\times V_2)(5):V1∗(V2×V3)=V2∗(V3×V1)=V3∗(V1×V2)
参数说明：u:作为旋转轴的单位矢量r:绕u旋转前的矢量 OA=rr':绕u旋转后的矢量OA'=r'r//:r在u上的投影OO'=rr//r//':r'在u上的投影OO'=rr//'r⊥：r相对于u的垂直分量r'⊥;r’相对于u的垂直分量θ ：r与r'的夹角O'B⊥O'A: OB'=rxu
2 已知:（没看懂的把上边的图画一遍）
r=OO′→+O′A→=r//+r⊥r=\overrightarrow{OO'}+\overrightarrow{O'A}=r//+r⊥r=OO′+O′A=r//+r⊥
r⊥=O′B→×u=(r×u)ur⊥=\overrightarrow{O'B}\times u=(r\times u)ur⊥=O′B×u=(r×u)u
不难证明（将r’用r和u表示）：
O′A′→=O′B→∗cosθ+O′A→∗sinθ\overrightarrow{O'A'}=\overrightarrow{O'B}*cos θ+\overrightarrow{O'A}*sin θO′A′=O′B∗cosθ+O′A∗sinθ
（A）r′=O′O→+O′A′→=(r∗u)u+(u×r)×u∗cosθ+u×r∗sinθ（A）r'=\overrightarrow{O'O}+\overrightarrow{O'A'}=(r*u)u+(u\times r)\times u*cos θ+u\times r*sin θ（A）r′=O′O+O′A′=(r∗u)u+(u×r)×u∗cosθ+u×r∗sinθ
罗德里格旋转公式（如果r;r’分别为两个坐标系的单位矢量;D就是过渡矩阵P）
已知：
三重矢积公式:V∗V∗V3=V×V×V3+v2∗V3;V*V*V_3=V\times V\times V_3+v^2*V_3;V∗V∗V3=V×V×V3+v2∗V3;
得到：r∗u∗u=u∗r∗u=(u×u)×r+v2∗r=(I+(u×)2)r;r*u*u=u*r*u=(u\times u)\times r+v^2*r=(I+(u\times)^2)r;r∗u∗u=u∗r∗u=(u×u)×r+v2∗r=(I+(u×)2)r;
带入公式：A
r′=r'=r′=O′O→+O′A′→=(r∗u)u+(u×r)×u∗cosθ+u×r∗sinθ\overrightarrow{O'O}+\overrightarrow{O'A'}=(r*u)u+(u\times r)\times u*cos θ+u\times r*sin θO′O+O′A′=(r∗u)u+(u×r)×u∗cosθ+u×r∗sinθ
=(I+(u×)2)r+(r×(u×u)−u×u×r)r∗cosθ+u×r∗sinθ=(I+(u\times)^2)r+(r\times(u\times u)-u\times u\times r)r*cosθ+u\times r*sin θ=(I+(u×)2)r+(r×(u×u)−u×u×r)r∗cosθ+u×r∗sinθ
=(I−sinθ∗(u×)+(1−cosθ)(u×)2)r=D∗r=(I-sin θ*(u\times)+(1-cosθ)(u\times)^2)r=D*r=(I−sinθ∗(u×)+(1−cosθ)(u×)2)r=D∗r
D=[I+sinθ∗(u×)+(1−cosθ)(u×)2](D为罗德里格旋转公式)D=[I+sin θ*(u\times)+(1-cosθ)(u\times)^2](D为罗德里格旋转公式)D=[I+sinθ∗(u×)+(1−cosθ)(u×)2](D为罗德里格旋转公式)
等效旋转矢量
φ=θ∗u(其中：φ=∣θ∣)φ=θ*u(其中：φ=|θ|)φ=θ∗u(其中：φ=∣θ∣)
当从坐标系b系到i系的时候 D=CbiD=C_b^iD=Cbi
将D用等效旋转矢量时表示如下u=φθu=\frac{φ}{θ}u=θφ：
D=Cbi=[I+sinθ∗(φ×)θ+(1−cosθ)(φ×)2θ2]=MRV(φ)D=C_b^i=[I+\frac{sinθ*(φ\times)}{θ}+\frac{(1-cosθ)(φ\times)^2}{θ^2}]=M_{RV}(φ)D=Cbi=[I+θsinθ∗(φ×)+θ2(1−cosθ)(φ×)2]=MRV(φ)
旋转矩阵（Givens矩阵）
取：φ1=[001]φ_1 =\left[ \begin{matrix} 0 & 0& 1 \\ \end{matrix} \right] φ1=[001]
φ2=[010]φ_2 =\left[ \begin{matrix} 0 & 1& 0 \\ \end{matrix} \right] φ2=[010]
φ3=[100]φ_3 =\left[ \begin{matrix} 1& 0& 0 \\ \end{matrix} \right] φ3=[100]
带入MRV(φM_{RV}(φMRV(φ)中得到三个矩阵，成为旋转矩阵（Gives矩阵）
求解等效旋转矢量
tr(Cbi)=λ1+λ2+λ3=1+2cosθtr(C_b^i)=\lambda_1+\lambda_2+\lambda_3=1+2cosθtr(Cbi)=λ1+λ2+λ3=1+2cosθ
Cbi−(Cbi)T=2sinθθ(φ×)C_b^i-(C_b^i)^T=2\frac {sinθ}{θ}(φ\times)Cbi−(Cbi)T=2θsinθ(φ×)
可得
θ=arcostr(Cbi)−12θ=arcos\frac{tr(C_b^i)-1}{2} θ=arcos2tr(Cbi)−1
φ×=θ2sinθ[Cbi−(Cbi)T]φ\times=\frac{θ}{2sinθ}[C_b^i-(C_b^i)^T] φ×=2sinθθ[Cbi−(Cbi)T]
利用φ×φ\timesφ×即可求得φφφ
