角速率输入条件下单子样旋转矢量姿态方法

摘要

本发明公开了一种角速率输入条件下单子样旋转矢量姿态方法，该方法基于角速率下多子样旋转矢量算法和角增量下单子样旋转矢量算法，将单子样算法应用到基于角速率的圆锥误差补偿算法中，具体利用当前时刻及前N个时刻的角速率来拟合圆锥误差补偿项，最优补偿项的系数由解线性方程组得到，可以在陀螺仪只提供角速率的情况下，利用角速率输入完成姿态解算并且保持解算频率与采样频率一致。本发明的方法既没有引入由角速率提取角增量带来的误差，又没有降低系统的姿态更新频率。

说明书

技术领域

本发明属于导航技术领域，具体涉及应用于捷联惯性导航系统的姿态解算方法。

背景技术

捷联式惯性导航系统(Strap-down Inertial Navigation System，SINS)是将加速度计和陀螺 仪直接安装在载体上，在计算机中实时计算姿态矩阵，即计算出载体坐标系与导航坐标系 之间的关系，从而把载体坐标系的加速度计信息转换为导航坐标系下的信息，然后进行导 航计算。

随着SINS应用越来越广泛，对其精度及动态性能要求也越来越高。姿态更新算法为捷 联惯导算法的核心，其中要解决的关键问题是刚体做有限转动时的旋转不可交换性误差。 国内外学者提出的多子样旋转矢量算法能够有效地对不可交换性误差进行了补偿。但如果 在现有采样频率下直接应用多子样旋转矢量姿态算法，会降低SINS姿态更新频率；若提高 采样频率，又会使硬件负担加重以及量化误差更加严重。

同时，上述算法均建立在陀螺仪输出为角增量的基础上，不能直接应用于输出为角速 率的情况。若从角速率中提取出角增量再应用于传统的多子样旋转矢量算法，这样会导致 补偿精度下降。有学者提出直接用角速率来计算旋转矢量的算法，避免了提取角增量引入 的误差。但其本身仍是一种多子样算法，同样存在姿态更新频率降低的问题。

发明内容

为了同时解决传统多子样旋转矢量姿态算法会降低SINS姿态更新频率和传统角增量 算法不能直接应用到角速率输入的问题，提出了一种角速率输入条件下的单子样旋转矢量 姿态方法。

本发明的技术方案为：一种角速率输入条件下的单子样旋转矢量姿态方法，具体包括 如下步骤：

步骤S1.获取设当前k时刻的姿态角为att_k=[θ γ ψ]^T，θ、γ和ψ分别为俯仰角、 横滚角和偏航角；

步骤S2、计算旋转矢量

步骤S3、计算当前k时刻的姿态四元数Q(t_k)，具体过程为：

姿态角与坐标转换矩阵的关系为：

$C_n^b=\left(\begin{array}{ccc}\cos \gamma \cos \Psi -\sin \gamma \sin \psi \sin \theta & \cos \gamma \sin \Psi +\sin \gamma \cos \Psi \sin \theta & -\sin \gamma \cos \theta \\ -\sin \Psi \cos \theta & \cos \Psi \cos \theta & \sin \theta \\ \sin \gamma \cos \Psi +\cos \gamma \sin \psi \sin \theta & \sin \gamma \sin \psi -\cos \gamma \cos \psi \sin \theta & \cos \gamma \cos \theta \end{array}\right)$

设：$C_n^b=\left(\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{23}\\ T_{13}& T_{23}& T_{33}\end{array}\right)$

则坐标转换矩阵与姿态四元数Q(t_k)的关系为：

$\left(\begin{array}{c}\left|q_1\right|=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}\\ \left|q_4\right|=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\end{array}\right)$

其中，Q(t_k)=[q₀  q₁  q₂q₃]^T，sign(q₀)的符号可任选，q₁,q₂,q₃的符号按下式确定：

$\left(\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(T_{32}-T_{23})\right]\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(T_{13}-T_{31})\right]\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(T_{21}-T_{12})\right]\end{array}\right)$

由此可以由当前的姿态角att_k计算当前的姿态四元数Q(t_k)；

步骤S4、姿态变化四元数q与旋转矢量的关系和计算步骤 S2中得到的旋转矢量对应的姿态变化四元数q，其中，表示转过的角度，u表 示旋转瞬轴和方向；

步骤S5、根据更新得到k+1时刻的姿态四元数Q(t_k+h)，其 中，q(h)为(t_k)时刻至(t_k+h)时刻对应的坐标系姿态变化四元数，即步骤S4得到的姿态变 化四元数q；

步骤S6、根据步骤S3中姿态变化四元数与坐标转换矩阵的关系以及坐标转换矩阵 与姿态角的关系，计算出k+1时刻的姿态角att_k，完成一次姿态更新；

步骤S7、重复步骤S1至步骤S6即实现了姿态更新。

进一步的，步骤S2所述的旋转矢量具体为利用当前时刻角速率和前四个时刻角速 率的五阶单子样算法的旋转矢量。

更进一步的，所述的旋转矢量的具体计算公式为：

$\Delta \hat{\Phi }=\frac{({251\omega }_0+{646\omega }_{-1}-{264\omega }_{-2}+{106\omega }_{-3}-{19}_{-4})h}{720}+\left(\begin{array}{c}\frac{65}{554}{\omega }_{-1}\times {\omega }_0-\frac{118}{5259}{\omega }_{-2}\times {\omega }_0\\ +\frac{100}{24163}{\omega }_{-3}\times {\omega }_0-\frac{40}{104201}{\omega }_{-4}\times {\omega }_0\end{array}\right)h^2,$

其中，ω₀为当前更新时刻的角速率，ω_-1,ω_-2,ω_-3,ω_-4为之前更新时刻的角速率采样值， h为更新间隔等于角速率采样间隔。

本发明的有益效果：本发明的方法基于角速率下多子样旋转矢量算法和角增量下单子 样旋转矢量算法提出来的，将单子样算法应用到基于角速率的圆锥误差补偿算法中，具体 利用当前时刻及前N个时刻的角速率来拟合圆锥误差补偿项，最优补偿项的系数由解线性 方程组得到，可以在陀螺仪只提供角速率的情况下，利用角速率输入完成姿态解算并且保 持解算频率与采样频率一致；本发明的方法既没有引入由角速率提取角增量带来的误差， 又没有降低系统的姿态更新频率。

附图说明

图1是本发明实施例的角速率输入下单子样算法的示意图。

图2是本发明实施例的角速率输入下单子样算法的流程图。

具体实施方式

下面结合附图和具体实施例对本发明的方法作进一步的说明：

经典的旋转矢量的原理如下：

设Q(t)为载体坐标系相对参考坐标系的姿态四元数，q(h)为t_k时刻至(t_k+h)时刻对应 的坐标系姿态变化四元数，则有如下关系：

$Q(t_k+h)=Q\left(t_k\right)\otimes q\left(h\right)---\left(1\right)$

设旋转矢量其中，表示转过的角度，u表示旋转瞬轴和方向，则旋转 矢量与姿态变化四元数的关系为：

$q\left(h\right)=\cos \frac{\Phi }{2}+u\sin \frac{\Phi }{2}---\left(2\right)$

因此若求得旋转矢量，便可以通过式(1)和式(2)来更新姿态四元数。

而不同的解算方法的关键便在于旋转矢量的计算方法不同。

一般的基于角速率的多子样旋转矢量算法原理如下：

设圆锥运动用旋转矢量可以表示为：

Φ₁=[0 αcosΩt αsinΩt]^T    (3)

式(3)中，α为圆锥运动的半锥角；Ω为圆锥运动的锥频率。

根据式(3)，可得到实时的姿态四元数为：

$Q\left(t\right){\left(\begin{array}{cccc}\cos \left(\frac{\alpha }{2}\right)& 0& \sin \left(\frac{\alpha }{2}\right)\cos \mathrm{\Omega t}& \sin \alpha \sin \mathrm{\Omega t}\end{array}\right)}^T$

四元数的微分方程为：

$\stackrel{·}{Q}\left(t\right)=\frac{1}{2}Q\left(t\right)\otimes \omega \left(t\right)---\left(2\right)$

式(5)中，ω(t)为角速率，由式(4)和式(5)可得：

$\omega \left(t\right)=\left(\begin{array}{c}-2\Omega {\sin }^2(\alpha /2)\\ -\Omega \sin \alpha \sin \mathrm{\Omega t}\\ \Omega \sin \alpha \cos \mathrm{\Omega t}\end{array}\right)---\left(3\right)$

由于姿态更新周期一般为毫秒级，则时间间隔h内的旋转矢量Φ(h)可视为小量。综合 考虑计算实时性以及精度等因素，工程上常用微分方程

(4)计算旋转矢量：

$\stackrel{·}{\Phi }\left(h\right)={\omega }_{\mathrm{nb}}^b+\frac{\Phi \left(h\right)}{2}\times {\omega }_{\mathrm{nb}}^b---\left(4\right)$

式(4)中，为载体系相对于导航系的旋转角速率在载体系下的投影。在输入为角速率 时，解微分方程(4)一般先对t_k时刻至(t_k+h)时刻的做拟合。采用四次抛物线时，拟合得 到是一个关于时间的函数：

${\omega }_{\mathrm{nb}}^b(t_k+\tau )=a+2\mathrm{b\tau }+3c{\tau }^2+4d{\tau }^3+5e{\tau }^4---\left(5\right)$

再将Φ(h)在h=0处用泰勒级数展开，并将Φ(h)的各阶导数用ω_i来表示，即可得旋转 矢量的角速率表达式：

$\left(\begin{array}{c}\Phi \left(h\right)=\frac{({7\omega }_1+32{\omega }_2+12{\omega }_3+32{\omega }_4+7{\omega }_5)h}{90}+h^2(\frac{38}{2835}{\omega }_1\times {\omega }_2+\frac{1}{135}{\omega }_1\times {\omega }_3+\frac{2}{135}{\omega }_1\times {\omega }_4)+\\ \frac{37}{11340}{\omega }_1\times {\omega }_5+\frac{8}{189}{\omega }_2\times {\omega }_3+\frac{128}{2835}{\omega }_2\times {\omega }_4+\frac{2}{135}{\omega }_2\times {\omega }_5+\frac{8}{189}{\omega }_3\times {\omega }_4+\frac{1}{135}{\omega }_3\times {\omega }_5+\frac{38}{2835}{\omega }_4\times {\omega }_5)\end{array}\right)---\left(6\right)$

用Φ(h)计算得到估计值后，再用其带入式(1)中的q(h)去做姿态更新，将得到姿 态角估计值。

下面就该算法进行详细说明：

当之前时刻采样点数取为N时，可以将角速率输入下单子样算法表示为：

$\Delta \hat{\Phi }=\Delta \hat{\theta }+\underset{i=1}{\overset{N}{\Sigma }}{G}_i({\omega }_{-1}\times {\omega }_0)h^2---\left(7\right)$

式(7)中，ω₀为当前更新时刻的角速率，ω_-1,ω_-2,ω_-3,ω_-4,…为之前更新时刻的角速率采 样值，更新间隔h等于角速率采样间隔Δt。设当前时刻为t_k，ω_-1对应的时刻为(t_k-h)，则 为t_k到(t_k-h)内的角增量。G_i待求圆锥误差补偿系数，下面将依次对和G_i的计算方 法进行推导。

1.角增量的计算

当之前的角速率样点个数取为4时，可同样按照式(5)对进行拟合，可以推出的 多项式拟合如下：

$\Delta \hat{\theta }=\frac{h}{720}({251\omega }_0+{646\omega }_{-1}-{264\omega }_{-2}+{106\omega }_{-3}-19{\omega }_{-4})---\left(8\right)$

由式(3)可得：

${\omega }_{-i}={\left(\begin{array}{ccc}{\omega }_{-\mathrm{ix}}& {\omega }_{-\mathrm{iy}}& {\omega }_{-\mathrm{iz}}\end{array}\right)}^T=\left(\begin{array}{c}-2\Omega {\sin }^2\left(\frac{\alpha }{2}\right)\\ -\Omega \sin \left(\alpha \right)\sin \Omega (t_k-\mathrm{ih})\\ \Omega \sin \left(\alpha \right)\cos \Omega (t_k-\mathrm{ih})\end{array}\right),0\le i\le 4---\left(9\right)$

从式(9)可知：角速率表达式中只有ω_-ix非周期项；其余两项都是与锥运动同频率的交变 量，在误差分析中可以将其略去。由于ω_-ix为常值，所以将式(9)代入式(8)可得的x方向 的分量

$\Delta {\hat{\theta }}_x={\omega }_{-\mathrm{ix}}h=-2\mathrm{\Omega h}{\sin }^2\left(\frac{\alpha }{2}\right)---\left(10\right)$

2.圆锥误差补偿项系数G_i的求法

为简化起见，令圆锥误差补偿项

$C_p=\underset{i=1}{\overset{N}{\Sigma }}{G}_i({\omega }_{-i}\times {\omega }_0)h^2---\left(11\right)$

只考虑非周期项，并考虑到sinα≈α，得：

${\phi }_{ϵ}=\Delta {\Phi }_x-(\Delta {\hat{\theta }}_x+C_{\mathrm{px}})=\frac{{\alpha }^2}{2}[\mathrm{\Omega h}-\sin \left(\mathrm{\Omega h}\right)-2{\left(\mathrm{\Omega h}\right)}^2\underset{i=1}{\overset{N}{\Sigma }}{G}_i\sin \mathrm{i\Omega h}]---\left(12\right)$

令λ=Ωh，将式(12)括号中的前两项进行泰勒展开，得：

$\mathrm{\Omega h}-\sin \left(\mathrm{\Omega h}\right)=\left(\begin{array}{cccc}{\lambda }^3& ···& {\lambda }^{2N+1}& ···\end{array}\right)\left(\begin{array}{c}{C}_1\\ ·\\ ·\\ ·\\ C_N\\ ·\\ ·\\ ·\end{array}\right)$

其中，C_m=(-1)^m+1/(2m+1)!,m=1,2,3,…构成列向量C。

同理，对式(12)中的第三项进行泰勒展开，得：

$2{\left(\mathrm{\Omega h}\right)}^2\underset{i=1}{\overset{N}{\Sigma }}{G}_i\sin \mathrm{i\Omega h}=\left(\begin{array}{cccc}{\lambda }^3& ···& {\lambda }^{2N+1}& ···\end{array}\right)\left(\begin{array}{cccc}{A}_{11}& A_{12}& ···& A_{1N}\\ ···& ···& ···& ···\\ A_{N1}& A_{N2}& ···& A_{\mathrm{NN}}\\ ···& ···& ···& ···\end{array}\right)\left(\begin{array}{c}{G}_1\\ G_2\\ ···\\ G_N\end{array}\right)---\left(14\right)$

在式(14)中，待求系数G_i(1≤i≤N)构成列向量G， A_ij=2(-1)^i-1j^2i-1/(2i-1)!,1≤i,1≤j≤N构成矩阵A。

对比式(13)和式(14)可知，当λ²ⁱ⁺¹(1≤i≤N)对应项系数相同时，即满足C=AG时， 算法误差最小。同时，N越大，误差中残留的有关λ项的最低阶数(2N+3)越高，算法误差 越小。但必须综合考虑计算实时性以及实际情况来选择N。

3.角速率输入下五阶单子样算法的计算公式

将N取为4，按照上面的计算方法计算出和G_i后，代入式(7)即可得出利用当前角速 率和前四个时刻角速率的五阶单子样算法的旋转矢量计算公式：

$\left(\begin{array}{c}\Phi \left(h\right)=\frac{({7\omega }_1+32{\omega }_2+12{\omega }_3+32{\omega }_4+7{\omega }_5)h}{90}+h^2(\frac{38}{2835}{\omega }_1\times {\omega }_2+\frac{1}{135}{\omega }_1\times {\omega }_3+\frac{2}{135}{\omega }_1\times {\omega }_4)+\\ \frac{37}{11340}{\omega }_1\times {\omega }_5+\frac{8}{189}{\omega }_2\times {\omega }_3+\frac{128}{2835}{\omega }_2\times {\omega }_4+\frac{2}{135}{\omega }_2\times {\omega }_5+\frac{8}{189}{\omega }_3\times {\omega }_4+\frac{1}{135}{\omega }_3\times {\omega }_5+\frac{38}{2835}{\omega }_4\times {\omega }_5)\end{array}\right)---\left(15\right)$

图2给出了角速率输入下单子样算法的姿态更新流程：

(1)、获取设当前姿态角为att_k=[θγψ]^T，θ、γ和ψ分别为俯仰角、横滚角和偏航 角。

(2)、选取要使用的算法的阶数，如果选5阶，则按照式(15)计算如果是其它阶数， 则先依上文的方法推导出该阶数算法的计算公式。

(3)、姿态角与坐标转换矩阵的关系为：

$C_n^b=\left(\begin{array}{ccc}\cos \gamma \cos \Psi -\sin \gamma \sin \psi \sin \theta & \cos \gamma \sin \Psi +\sin \gamma \cos \Psi \sin \theta & -\sin \gamma \cos \theta \\ -\sin \Psi \cos \theta & \cos \Psi \cos \theta & \sin \theta \\ \sin \gamma \cos \Psi +\cos \gamma \sin \psi \sin \theta & \sin \gamma \sin \psi -\cos \gamma \cos \psi \sin \theta & \cos \gamma \cos \theta \end{array}\right)---\left(16\right)$

设$C_n^b=\left(\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{23}\\ T_{13}& T_{23}& T_{33}\end{array}\right)$

则与姿态四元数Q(t_k)的关系为：

$\left(\begin{array}{c}\left|q_1\right|=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}\\ \left|q_4\right|=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\end{array}\right)---\left(17\right)$

其中，Q(t_k)=[q₀ q₁ q₂ q₃]^T，sign(q₀)的符号可任选，q₁,q₂,q₃的符号按下式确定。

$\left(\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(T_{32}-T_{23})\right]\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(T_{13}-T_{31})\right]\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(T_{21}-T_{12})\right]\end{array}\right)---\left(18\right)$

因此可以由当前的姿态角att_k计算当前的姿态四元数Q(t_k)。

(4)、由姿态变化四元数q与旋转矢量的关系可以计算步骤 (1)中得到的旋转矢量对应的姿态变化四元数q。其中，表示转过的角度，u表示 旋转瞬轴和方向。具体的，将旋转矢量带入式子中，即用替换得到Φ， 带入式子中进而得到姿态变化四元数q。

(5)、再由式(1)更新得到k+1时刻的姿态四元数Q(t_k+h)。

(6)、再综合式(17)中姿态四元数与的关系以及式(16)中与姿态角的关系，可以计 算出k+1时刻的姿态角attk。至此，完成一次姿态更新，

(7)、不断重复步骤(1)至步骤(6)即实现了姿态更新。