已知三维旋转矢量关系如下:(证明略)
参数说明:
ViV_iVi表示三维空间矢量
v=∣V∣=VVTv=|V|=\sqrt{VV^T}v=∣V∣=VVT表示矢量模值
uuu为与V同方向的单位矢量即:∣u∣=1|u|=1∣u∣=1
(1)Vv˙=V×V×V˙v+vV˙V\dot v=\frac{V\times V\times \dot V}{v}+v\dot VVv˙=vV×V×V˙+vV˙
(2)u˙=−V×V×V˙v3\dot u=-\frac{V\times V\times \dot V}{v^3}u˙=−v3V×V×V˙
(3)u˙×u=V˙×Vv2\dot u\times u=\frac{\dot V\times V}{v^2}u˙×u=v2V˙×V
(4)ϕ=θu\phi=\theta uϕ=θu;ϕ\phiϕ为等效旋转矢量θ=∣ϕ∣\theta=|\phi|θ=∣ϕ∣
已知:Q˙=Q。w\dot Q=Q。wQ˙=Q。w( Q 为4元数;。为4元数乘法;w为b系绕i系的转动速度)
得到:
w=uθ˙+u˙sinθ+u˙×u(1−cosθ)w=u\dot\theta+\dot usin\theta+\dot u\times u(1-cos\theta)w=uθ˙+u˙sinθ+u˙×u(1−cosθ)
根据公式(1)(4)得到:
uθ˙=ϕ×ϕ×ϕθ2u\dot\theta=\frac{\phi\times\phi\times\phi}{\theta^2}uθ˙=θ2ϕ×ϕ×ϕ
根据公式(2)得到:
u˙=−ϕ×ϕ×ϕ˙θ3\dot u=-\frac{\phi\times\phi\times\dot\phi}{\theta^3}u˙=−θ3ϕ×ϕ×ϕ˙
根据公式(3)得到:
u˙×u=ϕ˙×ϕθ2\dot u\times u=\frac{\dot\phi\times\phi}{\theta^2}u˙×u=θ2ϕ˙×ϕ
令a=−1−cosθθ2;b=(1−sinθθ)θ2a=-\frac{1-cos\theta}{\theta^2};b=\frac{(1-\frac{sin\theta}{\theta})}{\theta^2}a=−θ21−cosθ;b=θ2(1−θsinθ)将上述带入原式,解得为:
w=ϕ˙+aϕ×ϕ˙+b(ϕ×)2ϕ˙w=\dot\phi+a\phi\times\dot\phi+b(\phi\times)^2\dot\phiw=ϕ˙+aϕ×ϕ˙+b(ϕ×)2ϕ˙
将上述公式两边,左乘ϕ×\phi\timesϕ×,(ϕ×)2(\phi\times)^2(ϕ×)2,可以得到如下矩阵(推导过程略):得到
Bortz方程:
ϕ˙=w+12ϕ×w+1θ2(1−θ2cotθ2)(ϕ×)2w\dot\phi=w+\frac{1}{2}\phi\times w+\frac{1}{\theta^2}(1-\frac{\theta}{2}cot\frac{\theta}{2})(\phi\times)^2wϕ˙=w+21ϕ×w+θ21(1−2θcot2θ)(ϕ×)2w
展开cotθ2cot\frac{\theta}{2}cot2θ:
应满足如下条件:(1)θ=∣ϕ∣\theta=|\phi|θ=∣ϕ∣为向量时,将cotθ2cot\frac{\theta}{2}cot2θ用泰勒级数并忽略高次项
ϕ˙=w+12ϕ×w+1θ2(1−θ2(2θ−13θ2−145(θ2)3−...))(ϕ×)2w\dot\phi= w+\frac{1}{2}\phi\times w+\frac{1}{\theta^2}(1-\frac{\theta}{2}(\frac{2}{\theta}-\frac{1}{3}\frac{\theta}{2}-\frac{1}{45}(\frac{\theta}{2})^3-...))(\phi\times)^2w ϕ˙=w+21ϕ×w+θ21(1−2θ(θ2−312θ−451(2θ)3−...))(ϕ×)2w
≈w+12ϕ×w\approx w+\frac{1}{2}\phi\times w≈w+21ϕ×w
当t1,t2∈[tm−1,tm]t_1,t_2\in[t_{m-1},t_m]t1,t2∈[tm−1,tm]
ϕ(t)\phi(t)ϕ(t)从tm−1t_{m-1}tm−1时刻开始的等效旋转矢量的增量
ϕ(t)=Δθ(t)+σ(t)\phi(t)=\Delta \theta(t)+\sigma(t)ϕ(t)=Δθ(t)+σ(t)
Δθ(t)=∫0tw(t1)dt1(不可交换误差)\Delta \theta(t)=\int_0^tw(t_1)dt_1(不可交换误差)Δθ(t)=∫0tw(t1)dt1(不可交换误差)
σ(t)=∫0tΔθ(t1)×w(t1)dt1\sigma(t)=\int_0^t\Delta \theta(t_1)\times w(t_1)dt_1σ(t)=∫0tΔθ(t1)×w(t1)dt1
ϕ˙=w(t)+12Δϕ(t)×w(t)\dot\phi=w(t)+\frac{1}{2}\Delta\phi(t)\times w(t)ϕ˙=w(t)+21Δϕ(t)×w(t)