捷联惯导系统模型及仿真（三）

2. 编程实现–姿态解算、速度解算

2.1 姿态解算

姿态解算（更新）的本质就是解 姿态的微分方程。
而表示姿态的方法不同，如欧拉角、方向余弦矩阵、四元数、旋转矢量；
因此，分别有各自的表示方法下的微分方程，也就得到了各自的解算方法。

• 符号的规定：
  陀螺的采样时间间隔 为导航解算中最小的时间单位；
  姿态的更新周期—— [tk,tk+1] [ t k , t k + 1 ] ；
  速度的更新周期—— [tm−1,tm] [ t m − 1 , t m ]
  位置的更新周期—— [tl−1,tl] [ t l − 1 , t l ]

2.1.1 欧拉角法

此处，仅介绍其微分方程。
（其应用：一是在仿真程序数据生成时；二是在大失准角的初始对准中）

秦书P279

力学中，常用 欧拉角 确定动坐标系相对参考坐标系的角位置关系。
如：运载体的航向角 Ψ Ψ ，俯仰角 θ θ ，横滚角 γ γ 实质上就是一组欧拉角，它们描述了机体坐标系（动）相对于地理坐标系（参考）的角位置关系。

• 地理坐标系 g g 系，绕 Z,X,Y Z , X , Y 分别转过 Ψ,θ,γ Ψ , θ , γ

• ωnb ω n b 为姿态速率，即，机体坐标系 b b 系相对于导航坐标系 n(g) n ( g ) 系的角速度。
  且按 ω^bnb=ω~bib−ω^bin ω ^ n b b = ω ~ i b b − ω ^ i n b 确定，

⎡⎣⎢⎢ωbnbxωbnbyωbnbz⎤⎦⎥⎥=CγCθ⎡⎣⎢00Ψ˙⎤⎦⎥+Cγ⎡⎣⎢θ˙00⎤⎦⎥+⎡⎣⎢0γ˙0⎤⎦⎥=⎡⎣⎢cosγ0sinγ010−cosθsinγsinθcosθcosγ⎤⎦⎥⎡⎣⎢θ˙γ˙Ψ˙⎤⎦⎥ [ ω n b x b ω n b y b ω n b z b ] = C γ C θ [ 0 0 Ψ ˙ ] + C γ [ θ ˙ 0 0 ] + [ 0 γ ˙ 0 ] = [ c o s γ 0 − c o s θ s i n γ 0 1 s i n θ s i n γ 0 c o s θ c o s γ ] [ θ ˙ γ ˙ Ψ ˙ ]

那么，欧拉角微分方程就是：

⎡⎣⎢θ˙γ˙Ψ˙⎤⎦⎥=C−1ω⎡⎣⎢⎢ωbnbxωbnbyωbnbz⎤⎦⎥⎥ [ θ ˙ γ ˙ Ψ ˙ ] = C ω − 1 [ ω n b x b ω n b y b ω n b z b ]

2.1.2 方向余弦矩阵法

此处仅介绍姿态矩阵的微分方程：

秦书P280

有四种形式，此处仅给出实际中常用的两种，且推导过程略。

C˙bn=−ωbnb×Cbn C ˙ n b = − ω n b b × C n b

C˙nb=Cnb×ωbnb C ˙ b n = C b n × ω n b b

2.1.3 四元数法

Q˙=12Q⊗ωbnb Q ˙ = 1 2 Q ⊗ ω n b b

2.1.4 旋转矢量法

四元数可以用来表示某一时刻，如 tk t k ，的姿态，此时称为姿态四元数；（一般记为大写 Q(tk) Q ( t k ) ）
也可以用来表示，从 tk t k 到 tk+1 t k + 1 时刻姿态的变化，此时称为姿态变换四元数；（一般记为小写 q(h) q ( h ) ，其中 h=tk+1−tk h = t k + 1 − t k ）

旋转矢量是另外一种 表示姿态变化的方式，因此和姿态变化四元数存在如下关系：

q(h)=cosϕ2+Φϕsinϕ2 q ( h ) = c o s ϕ 2 + Φ ϕ s i n ϕ 2

• 其中， Φ Φ 即为坐标系从 tk t k 到 tk+1 t k + 1 时刻姿态变化的等效旋转矢量， ϕ=|Φ| ϕ = | Φ |

此处简单推导一下，姿态变换四元数用于姿态更新：
rn(k+1)=Cn(k+1)b(k+1)rb(k+1) r n ( k + 1 ) = C b ( k + 1 ) n ( k + 1 ) r b ( k + 1 ) ，对应 rn(k+1)=Q(tk+1)⊗rb(k+1)⊗Q(tk+1)∗ r n ( k + 1 ) = Q ( t k + 1 ) ⊗ r b ( k + 1 ) ⊗ Q ( t k + 1 ) ∗
上面两个式子分别用 余弦矩阵和四元数乘表示了向量的坐标变换。
而 rn(k+1)=Cn(k+1)n(k)Cn(k)b(k)Cb(k)b(k+1)rb(k+1) r n ( k + 1 ) = C n ( k ) n ( k + 1 ) C b ( k ) n ( k ) C b ( k + 1 ) b ( k ) r b ( k + 1 )
对应
rn(k+1)=p∗(h)⊗{Q(tk)⊗[q(h)⊗rb(k+1)⊗q∗(h)]⊗Q(tk)∗}⊗p(h) r n ( k + 1 ) = p ∗ ( h ) ⊗ { Q ( t k ) ⊗ [ q ( h ) ⊗ r b ( k + 1 ) ⊗ q ∗ ( h ) ] ⊗ Q ( t k ) ∗ } ⊗ p ( h )
这样就得到：

Q(tk+1)=p∗(h)⊗Q(tk)⊗q(h) Q ( t k + 1 ) = p ∗ ( h ) ⊗ Q ( t k ) ⊗ q ( h )

在一个姿态更新周期里， Cn(k+1)n(k)≈I C n ( k ) n ( k + 1 ) ≈ I ，即， p(h)≈1 p ( h ) ≈ 1
此时，做一下近似：

Q(tk+1)=Q(tk)⊗q(h)=M(Q(tk))q(h)=M′(q(h))Q(tk) Q ( t k + 1 ) = Q ( t k ) ⊗ q ( h ) = M ( Q ( t k ) ) q ( h ) = M ′ ( q ( h ) ) Q ( t k )

【注】：因为忽略了 q(h) q ( h ) ，所以若干次姿态更新后，应做适当的修正。详见P260

于是，问题的关键落到了姿态变化四元数 q(h) q ( h ) ，或者说旋转矢量 Φ Φ 的求解上。

---

1. 旋转矢量微分方程：
   此处不做推导，仅给出工程上常用的近似公式（P264）：
   
   Φ˙=ωbnb+12Φ×ωbnb+112Φ×(Φ×ωbnb) Φ ˙ = ω n b b + 1 2 Φ × ω n b b + 1 12 Φ × ( Φ × ω n b b )
2. 旋转矢量微分方程的求解
   首先，需要牢记一点，在计算机编程实现时，只能数值求解，无法解析求解。
   同样，此处不做推导，仅给出旋转矢量 Φ Φ 的求解结果（P267）：
   
   Φ(h)=ΔΘ1+ΔΘ2+ΔΘ3+3380ΔΘ1×ΔΘ3+5780ΔΘ2×(ΔΘ3−ΔΘ1) Φ ( h ) = Δ Θ 1 + Δ Θ 2 + Δ Θ 3 + 33 80 Δ Θ 1 × Δ Θ 3 + 57 80 Δ Θ 2 × ( Δ Θ 3 − Δ Θ 1 )
   
   【注】：
   （1）.这是旋转矢量的三子样解，是假设在 [tk,tk+1] [ t k , t k + 1 ] 时间段内运载体的角速度 ωbnb ω n b b 采用抛物线拟合得到的，即， ωbnb(tk+τ)=a+2bτ+3cτ2 ω n b b ( t k + τ ) = a + 2 b τ + 3 c τ 2 。
   （2）. ΔΘ1,ΔΘ2,ΔΘ3 Δ Θ 1 , Δ Θ 2 , Δ Θ 3 分别为三个时间段 [tk,tk+h3],[tk+h3,tk+2h3], [ t k , t k + h 3 ] , [ t k + h 3 , t k + 2 h 3 ] ,
   [tk+2h3,tk+1] [ t k + 2 h 3 , t k + 1 ] 的角增量。
   （3）.采用常数、直线、抛物线、三次抛物线拟合角速度，得到的旋转矢量公式在P268。
3. 旋转矢量算法的优化
   但是，实际运载体的角速度随时间变化的曲线并非上述给出的拟合曲线，因此，由拟合曲线推导出的旋转矢量算法自然存在有误差。
   下面，假设运载体的角运动为锥运动，推导优化算法。
   
   因为，理论已证明：如果算法能够保证在锥运动下漂移最小，那么在其余环境条件下该算法的漂移也最小。
   
   同样，此处不做推导，仅给出优化算法求得的旋转矢量 Φ Φ 的结果（P278）：
   单子样、双子样无变化；
   优化三子样算法求得得旋转矢量为：
   
   Φ(h)=ΔΘ1+ΔΘ2+ΔΘ3+920ΔΘ1×ΔΘ3+2740ΔΘ2×(ΔΘ3−ΔΘ1) Φ ( h ) = Δ Θ 1 + Δ Θ 2 + Δ Θ 3 + 9 20 Δ Θ 1 × Δ Θ 3 + 27 40 Δ Θ 2 × ( Δ Θ 3 − Δ Θ 1 )

---

2.1.4-2 旋转矢量法得编程实现

此处，编写两个函数：（要求尽可能独立和模块化）

• 第一个用来实现姿态四元数得更新
  函数输入参数：
  tk t k 时刻得姿态四元数—— Q(tk) Q ( t k ) ；
  反应 tk t k 到 tk+1 t k + 1 时刻姿态变化的姿态变化四元数—— q(h) q ( h ) ；
  函数输出参数：
  tk+1 t k + 1 时刻得姿态四元数—— Q(tk+1) Q ( t k + 1 ) ；

function Qk1=updateQk(Qk,qh)
% This function is to update Q(tk) to Q(tk+1)
% Qk1 = Qk \otimes qh = M(Qk)qh
% Make sure Qk,qh are both a 4-by-1 column vector
% changshen_xu
% 2018-07-17
% 
% M = [Qk(1),-Qk(2),-Qk(3),-Qk(4);
%      Qk(2), Qk(1),-Qk(4), Qk(3);
%      Qk(3), Qk(4), Qk(1),-Qk(2);
%      Qk(4),-Qk(3), Qk(2), Qk(1)];
% temp = M*qh;
% Qk1 = temp/sqrt(temp'*temp);

% Qk1 = Qk \otimes qh = M'(qh)Qk
Mq = [qh(1),-qh(2),-qh(3),-qh(4);
      qh(2), qh(1), qh(4),-qh(3);
      qh(3),-qh(4), qh(1), qh(2);
      qh(4), qh(3),-qh(2), qh(1)];
temp2 = Mq*Qk;
Qk1 = temp2/sqrt(temp2'*temp2);
end

• 第二个用来实现旋转矢量的求解，并输出对应的姿态变化四元数
  函数输入参数：（此处使用优化三子样算法）
  陀螺在的三个时间段的角增量 ΔΘ1,ΔΘ2,ΔΘ3 Δ Θ 1 , Δ Θ 2 , Δ Θ 3
  函数输出参数：
  姿态变化四元数 q(h) q ( h )

function qh=getqh(theta1,theta2,theta3)
% This function is to get the qh.
% By changshen_xu
% Date: 2018-07-17

% Optimized three-sample algorithm to get the rotation vector \Phi.
s1 = cross(theta1,theta3);
s2 = theta3-theta1;
s3 = cross(theta2,s2);
Phi = theta1+theta2+theta3+0.45*s1+0.675*s3;

phi = sqrt(Phi'*Phi);  % phi = |Phi|
qh = zeros(4,1);
qh(1) = cos(phi/2);
qh(2) = Phi(1)/phi*sin(phi/2);
qh(3) = Phi(2)/phi*sin(phi/2);
qh(4) = Phi(3)/phi*sin(phi/2);
end

此处注意，在仿真过程中，陀螺的采样值亦是由数据生成函数产生的。
因此，并不能直接获得秦书上所说的角增量，实际上恰恰是角速率。故此处需要一个由角速率到角增量的积分转换函数。