CN103630135A - 一种角速率输入的姿态算法结构与参数优化方法 - Google Patents

Abstract

本发明公开了一种新的角速率输入的姿态算法结构与参数优化方法，针对传统角速率输入的姿态算法圆锥误差补偿结构不能充分利用已有角速率信息的问题，提出了一种改进的姿态算法圆锥误差补偿结构，从而给出一种新的角速率输入的姿态算法结构；在定义经典圆锥运动形式和分析几个必要的圆锥运动特性的基础上，对提出的姿态算法圆锥误差补偿结构进行简化处理，获得了一种压缩的姿态算法圆锥误差补偿结构；定义了圆锥误差补偿误差准则，并推导了该误差准则的具体描述；选定了圆锥误差补偿优化目标和方法并实施了圆锥误差补偿结构参数的优化设计，获得一种新的角速率输入的带有圆锥误差补偿结构优化参数的姿态算法，使捷联惯导姿态解算综合性能得到提高。

Description

一种角速率输入的姿态算法结构与参数优化方法

技术领域

本发明涉及一种角速率输入的姿态算法结构与参数优化方法，属于惯性导航技术领域。

背景技术

自20世纪50年代末捷联惯性导航系统的概念被提出以来，经过50多年的发展，捷联惯导系统已经广泛应用于国防、交通等多个领域。总的来说，高精度的捷联惯导系统的性能主要依赖于两个方面：高精度的惯性器件和导航算法。所以，研究获得高精度的捷联惯性导航算法是诸多捷联惯导领域的学者们持续追求的目标之一，其中捷联惯导姿态算法是这一目标的首要任务。

自1971年后，捷联惯导姿态算法的设计工作均集中在获得一种近似积分旋转矢量微分方程的方法上。自1983年后，Miller、Ignagni和Savage等陆续将时域泰勒级数展开方法、频域泰勒级数展开方法和最小二乘方法等方法应用于传统的角增量输入的双速捷联惯导姿态算法结构上，进行姿态算法圆锥误差补偿的优化设计；自2000年后，黄昊、曾庆化和汤传业等先后将频域泰勒级数展开方法和最小二乘方法等方法应用于纯角速率输入的双速捷联惯导姿态算法结构，进行姿态算法圆锥误差补偿的优化设计，但姿态算法中所采用的圆锥误差补偿结构不甚合理，在圆锥误差补偿设计时未能考虑已有的由角速率拟合的角增量信息，导致圆锥误差补偿算法、姿态算法乃至整个捷联惯性导航系统的精度不能达到应有的水平。

发明内容

发明目的：针对上述现有技术，提出一种角速率输入的姿态算法结构与参数优化方法，在此基础上，能够更好的补偿姿态算法中的圆锥误差，从而获得一种高精度的捷联惯导姿态算法。

技术方案：一种角速率输入的姿态算法结构与参数优化方法，包括以下步骤：

步骤(1)，建立用于角速率输入的姿态算法的圆锥误差补偿结构：

在一个姿态更新周期内，以拟合角增量子样周期为时间间隔顺序抽取角速率采样值，并拟合角增量子样；然后，建立用于姿态更新的更新旋转矢量的解算形式和圆锥误差补偿结构；

步骤(2)，简化所述圆锥误差补偿结构，得到压缩圆锥误差补偿结构：

定义经典圆锥运动，忽略角增量子样的拟合误差，并只考虑角速率采样叉乘项、角增量子样叉乘项、角速率采样与角增量子样叉乘项的各自在x轴上的第一个分量，建立压缩圆锥误差补偿结构，得到压缩圆锥误差补偿项；

步骤(3)，基于步骤(2)所述的压缩圆锥误差补偿结构，建立角速率输入的圆锥误差补偿误差准则：

定义圆锥误差补偿项误差为圆锥误差补偿项的理论值与数值计算值在误差矢量的第一个轴向分量上的分量差；

步骤(4)，建立圆锥误差补偿结构参数优化目标和圆锥误差补偿结构参数优化方法：

圆锥误差补偿优化设计的目标：使圆锥误差补偿项误差的绝对数值达到最小；

圆锥误差补偿结构参数优化设计方法是：根据所述目标，将圆锥误差补偿项误差展开成关于圆锥频率参数的幂级数，令幂级数前3N-1个低阶项的系数为零，并求解得到的线性方程组，得到优化的圆锥误差补偿系数；其中N为一个姿态更新周期包含的拟合角增量子样周期数。

作为本发明的优选方案，所述步骤(1)中建立用于角速率输入的姿态算法的圆锥误差补偿结构具体步骤如下：首先，定义姿态更新周期的开始时刻为t_l-1，结束时刻为t_l，姿态更新周期为T_l，拟合角增量子样周期为T_k，角速率采样周期为T；其中，一个姿态更新周期包含的拟合角增量子样周期数为N，即T_l=NT_k；拟合一个角增量所用的顺序角速率采样数为M+1，即T_k=MT；从时刻t_l-1开始在一个姿态更新周期内，即时间区间[t_l-1,t_l]内以T_k为时间间隔顺序抽取的第i+1个角速率采样值为ω_i，其中按照时间发生的先后顺序，i取值为0，1，..，N；从时刻t_l-1开始在一个姿态更新周期内，即时间区间[t_l-1,t_l]内使用时间区间[t_l-1+(k-1)T_k,t_l-1+kT_k]内的角速率采样值拟合的第k个角增量子样为Δα_k，其中按照时间发生的先后顺序，k取值为1，2，..，N；然后，给出用于姿态更新的更新旋转矢量的解算形式和圆锥误差补偿结构：所述更新旋转矢量φ_l为φ_l=α_l+δφ_l，所述圆锥误差补偿项δφ_l为 $\mathrm{\δ}{\mathrm{\φ}}_l={T_k}^2\underset{i=0}{\overset{N-1}{\mathrm{\Σ}}}\underset{j=i+1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\η}}_{\mathrm{ij}}{\mathrm{\ω}}_i\×{\mathrm{\ω}}_j+\underset{i=1}{\overset{N-1}{\mathrm{\Σ}}}\underset{j=i+1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{\mathrm{ij}}\mathrm{\Δ}{\mathrm{\α}}_i\×\mathrm{\Δ}{\mathrm{\α}}_j+T_k\underset{i=0}{\overset{N}{\mathrm{\Σ}}}\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_{\mathrm{ij}}{\mathrm{\ω}}_i\×\mathrm{\Δ}{\mathrm{\α}}_j;$ 其中，

α_l为一个姿态更新周期内总的拟合角增量，η_ij、μ_ij和ξ_ij分别为对应于角速率采样叉乘项、角增量子样叉乘项和角速率采样与角增量子样叉乘项的圆锥误差补偿系数，i、j和k为符号下标。

步骤(2)简化所述圆锥误差补偿结构，得到压缩圆锥误差补偿结构的具体步骤如下：定义经典圆锥运动Φ(t)=[0 acosΩt asinΩt]^T，其中t为时刻，Φ(t)为载体系相对参考系在t时刻的旋转矢量，并将载体系定义为b系，将参考系定义为n系，a为半锥角，Ω为圆锥运动频率，[]^T为[]的转置；在此基础上，定义t时刻b系相对于n系的角速度在b系下的投影

为： ${\mathrm{\ω}}_{\mathrm{nb}}^b\left(t\right)={\left[\begin{array}{ccc}-2\mathrm{\Ω}{\sin }^2(a/2)& -\mathrm{\Ω}\sin a\sin \mathrm{\Ωt}& \mathrm{\Ω}\sin a\cos \mathrm{\Ωt}\end{array}\right]}^T;$ 令角速度采样值

其中i＝0，1，...，N，忽略角增量子样Δα_j的拟合误差，并令其中j=1，2，...，N，得到角速率采样叉乘项ω_i×ω_j的第一个分量[ω_i×ω_j]_x等于Ω²sin²asin[(j-i)ΩT_k]，角增量子样叉乘项Δα_i×Δα_j的第一个分量[Δα_i×Δα_j]_x等于

角速率采样与角增量子样叉乘项ω_i×Δα_j的第一个分量[ω_i×Δα_j]_x等于

只考虑角速率采样叉乘项、角增量子样叉乘项、角速率采样与角增量子样叉乘项的各自在x轴向上的第一个分量，所述圆锥误差补偿项δφ_l的补偿结构简化压缩为：

$\mathrm{\δ}{\mathrm{\φ}}_l={T_k}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{\mathrm{\ω}}_{N-n}\×{\mathrm{\ω}}_N+\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\mathrm{\Δ}{\mathrm{\α}}_{N-p}\×\mathrm{\Δ}{\mathrm{\α}}_N+T_k\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q{\mathrm{\ω}}_{N-q}\×{\mathrm{\α}}_N;$ 其中， $A_n=\underset{i=n}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\η}}_{(i-n),i},$ A_n为与η_ij相当的圆锥误差补偿系数，在量值上等于所有的η_(i-n)，i之和；

B_p为与μ_ij相当的圆锥误差补偿系数，在量值上等于所有的μ_(i-p)，i之和；

C_q为与ξ_ij相当的圆锥误差补偿系数，在量值上等于所有的ξ_(i-q)，i-ξ_i，(i-q+1)之和。

步骤(3)基于步骤(2)所述的压缩圆锥误差补偿结构，建立角速率输入的圆锥误差补偿误差准则的具体步骤如下：在圆锥运动条件下，圆锥误差补偿项数值计算误差表现为，误差矢量的第一个轴向分量是常值分量，其余两个轴向分量是周期分量，并将误差矢量的第一个轴向定义为x轴；定义圆锥误差补偿项误差e为圆锥误差补偿项的理论值与数值计算值在x轴上的分量之差，即

其中，δφ_x为更新旋转矢量圆锥误差补偿项理论值在x轴上的分量，

为更新旋转矢量圆锥误差补偿项数值计算值在x轴上的分量；圆锥误差补偿项理论值在x轴上的分量δφ_x等于将所述叉乘项分量[ω_i×ω_j]_x、[Δα_i×Δα_j]_x和[ω_i×Δα_j]_x的分析表达式代入圆锥误差补偿形式的压缩结构 ${\mathrm{\δ\φ}}_l={T_k}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{\mathrm{\ω}}_{N-n}\×{\mathrm{\ω}}_N+\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\mathrm{\Δ}{\mathrm{\α}}_{N-p}\×\mathrm{\Δ}{\mathrm{\α}}_N+T_k\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q{\mathrm{\ω}}_{N-q}\×\mathrm{\Δ}{\mathrm{\α}}_N$ 中，得到圆锥误差补偿项数值计算值在x轴上的分量为

从而导出圆锥误差补偿项误差e为：

$e=\frac{1}{2}{\sin }^{2}a[N\mathrm{\β}-\sin \left(\mathrm{N\β}\right)-2{\mathrm{\β}}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n\sin \left(\mathrm{N\β}\right)-8{\sin }^2\frac{\mathrm{\β}}{2}\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\sin \left(\mathrm{p\β}\right)-4\mathrm{\β}\sin \frac{\mathrm{\β}}{2}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q\sin (\mathrm{q\β}-\frac{\mathrm{\β}}{2})],\mathrm{\β}=\mathrm{\Ω}{T}_k,$

其中β为与角增量子样周期T_k有关的圆锥频率参数。

所述步骤(4)中建立圆锥误差补偿结构参数优化目标和圆锥误差补偿结构参数优化方法的具体步骤为：圆锥误差补偿优化设计的目标是：使圆锥无差补偿项误差e的绝对数值达到最小；圆锥误差补偿结构参数优化设计方法是：根据三角函数关系式sin²(β／2)=(1-cosβ)／2，所述圆锥误差补偿项误差e得到为如下形式：

$\begin{array}{c}e=\frac{1}{2}{\sin }^{2}a[\mathrm{N\β}-\sin \left(\mathrm{N\β}\right)-2{\mathrm{\β}}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n\sin \left(\mathrm{n\β}\right)-2\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\{2\sin \left(\mathrm{p\β}\right)-\sin \left[(p+1)\mathrm{\β}\right]-\sin \left[(p-1)\mathrm{\β}\right]\}\\ +2\mathrm{\β}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q\{\cos \left(\mathrm{q\β}\right)-\cos (q-1)\mathrm{\β}\}]\end{array},$

再根据三角级数关系式 $\sin x=x-\frac{x^3}{3!}+...+\frac{{(-1)}^{k-1}{x}^{2k-1}}{(2k-1)!}+...$ 和 $\cos x=1-\frac{x^2}{2!}+...+\frac{{(-1)}^{k-1}{x}^{2(k-1)}}{2(k-1)!}+..,$ 得到圆锥误差补偿项误差e关于圆锥频率参数β的幂级数形式表达式：

$\begin{array}{c}e=\frac{1}{2}{\sin }^{2}a\underset{k=1}{\overset{\∞}{\mathrm{\Σ}}}\{\frac{{(-1)}^{k-1}}{(2k+1)!}{N}^{2k+1}-2\frac{{(-1)}^{k-1}}{(2k-1)!}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{n}^{2k-1}\\ -2\frac{{(-1)}^k}{(2k+1)!}\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p[2p^{2k+1}-{(p+1)}^{2k+1}-{(p-1)}^{2k+1}]+2\frac{{(-1)}^k}{\left(2k\right)!}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q[q^{2k}-{(q-1)}^{2k}]\}{\mathrm{\β}}^{2k+1}\end{array},$

令圆锥误差补偿项误差e关于β的幂级数项β^2k+1的系数为零，k=1，2，...，3N-1；得到线性方程组：

$\begin{array}{c}2\frac{{(-1)}^{k-1}}{(2k-1)!}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{n}^{2k-1}-2\frac{{(-1)}^k}{\left(2k\right)!}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q[q^{2k}-{(q-1)}^{2k}]+2\frac{{(-1)}^k}{(2k+1)!}\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p[2p^{2k+1}-{(p+1)}^{2k+1}-{(p-1)}^{2k+1}]\\ =\frac{{(-1)}^{k-1}}{(2k+1)!}{N}^{2k+1}\end{array},$

k=1，2，...，3N-1；解该线性方程组，得到优化的圆锥误差补偿系数A_n、B_p和C_q。

有益效果：(1)为获得高精度的捷联惯导姿态算法，本发明从改进姿态解算结构入手：

对传统的角速率输入的捷联惯导姿态算法结构中的圆锥误差补偿结构做深入的改进，设计一种新的圆锥误差补偿结构。新的角速率输入的圆锥误差补偿结构中不仅包括了原有角速率采样信息，而且还包括了新的拟合角增量信息。关于角速率采样序列和拟合角增量序列在时间上的分布关系，可设定的方式有很多种，本发明中使用的角速率采样序列ω_i(i=0，1，...，N)和拟合角增量序列Δα_j(j=1，2，...，N)为同步等时间间隔关系，且在一个姿态更新周期内用于圆锥误差补偿的角速率采样数比拟合角增量数大1个，如图2所示；

(2)优化设计圆锥误差补偿结构中的待定参数，以获得尽可能高的圆锥误差补偿精度：

基于圆锥运动特性，简化新设计的姿态算法圆锥误差补偿结构，得到压缩的圆锥误差补偿结构，以方便圆锥误差补偿结构参数优化设计过程的实施；

实施圆锥误差补偿结构参数的优化设计：基于压缩的圆锥误差补偿结构，定义圆锥误差补偿项误差；将圆锥误差补偿项误差展开成关于圆锥频率参数β的幂级数，令幂级数前3N-1个低阶项的系数为零，并求解得到的线性方程组，得到优化的圆锥误差补偿系数；其中N为一个姿态更新周期包含的拟合角增量子样周期数。

综上所述，本发明采用的角速率输入的姿态算法圆锥误差补偿结构充分考虑了角速率采样和拟合角增量子样信息的充分利用问题，从而为捷联惯导姿态解算提供一个优越的框架基础。同时，采用提出的姿态算法圆锥误差补偿结构简化处理方法，获得了姿态算法圆锥误差补偿结构的压缩形式，使圆锥误差补偿结构参数优化过程更简单。采用选定的圆锥误差补偿优化方法获得的圆锥误差补偿结构优化参数具有很好的适用性，使得带有圆锥误差补偿结构优化参数的姿态算法的综合性能得到提高。

附图说明

图1为本发明方法的流程图；

图2为角速率采样序列与拟合角增量序列分布关系示意图；

图3为姿态更新周期、拟合角增量子样周期和角速率采样周期之间的关系示意图。

具体实施方式

下面结合附图对本发明做更进一步的解释。

一种角速率输入的姿态算法结构与参数优化方法，包括以下步骤：

步骤(1)，建立用于角速率输入的姿态算法的圆锥误差补偿结构：

在一个姿态更新周期内，以拟合角增量子样周期为时间间隔顺序抽取角速率采样值，并拟合角增量子样；然后，建立用于姿态更新的更新旋转矢量的解算形式和圆锥误差补偿结构；

步骤(2)，简化所述圆锥误差补偿结构，得到压缩圆锥误差补偿结构：

定义经典圆锥运动，忽略角增量子样的拟合误差，并只考虑角速率采样叉乘项、角增量子样叉乘项、角速率采样与角增量子样叉乘项的各自在x轴上的第一个分量，建立压缩圆锥误差补偿结构，得到压缩圆锥误差补偿项；

步骤(3)，基于步骤(2)所述的压缩圆锥误差补偿结构，建立角速率输入的圆锥误差补偿误差准则：

定义圆锥误差补偿项误差为圆锥误差补偿项的理论值与数值计算值在误差矢量的第一个轴向分量上的分量差；

步骤(4)，建立圆锥误差补偿结构参数优化目标和圆锥误差补偿结构参数优化方法：

圆锥误差补偿优化设计的目标：使圆锥误差补偿项误差的绝对数值达到最小；

圆锥误差补偿结构参数优化设计方法是：根据所述目标，将圆锥误差补偿项误差展开成关于圆锥频率参数β的幂级数，令幂级数前3N-1个低阶项的系数为零，并求解得到的线性方程组，得到优化的圆锥误差补偿系数；其中N为一个姿态更新周期包含的拟合角增量子样周期数。

其中，步骤(1)中建立用于角速率输入的姿态算法的圆锥误差补偿结构具体步骤如下：

首先，定义姿态更新周期的开始时刻为t_l-1，结束时刻为t_l，姿态更新周期为T_l，拟合角增量子样周期为T_k，角速率采样周期为T；其中，一个姿态更新周期包含的拟合角增量子样周期数为N，即T_l=NT_k；拟合一个角增量所用的顺序角速率采样数为M+1，即T_k=MT；从时刻T_l-1开始在一个姿态更新周期内，即时间区间[t_l-1,t_l]内以T_k为时间间隔顺序抽取的第i+1个角速率采样值为ω_i，其中按照时间发生的先后顺序，i取值为0，1，...，N；从时刻t_l-1开始在一个姿态更新周期内，即时间区间[t_l-1,t_l]内使用时间区间[t_l-1+(k-1)T_k,t_l-1+kT_k]内的角速率采样值拟合的第k个角增量子样为Δα_k，其中按照时间发生的先后顺序，k取值为1，2，...，N；然后，给出用于姿态更新的更新旋转矢量的解算形式和圆锥误差补偿结构：

现代捷联惯导系统中使用的经典姿态更新数值计算通式为：

$C_{b\left(l\right)}^n=C_{b(l-1)}^n\·C_{b\left(l\right)}^{b(l-1)}---\left(4\right),$

其中，

和

分别为第l-1个和第l个更新周期结束时刻载体的姿态矩阵，

和

分别为第l个更新周期内的更新姿态矩阵和数值更新旋转矢量，

为的各元素构成的叉乘反对称矩阵，|·|为取模值。

新的角速率输入的更新旋转矢量解算形式和圆锥误差补偿结构为：

φ_l=α_l+δφ_l       (5)；

其中，α_l为一个姿态更新周期内总的拟合角增量，

T_k=MT，[s₀ s₁ … s_M]＝HC^-1，

H=[1 1／2 … 1／(M+1)]，

$C={\left(c_{\mathrm{ij}}\right)}_{(N+1)\×(N+1)},c_{\mathrm{ij}}={\left(\frac{i-1}{M}\right)}^{j-1}---\left(6\right);$

其中，其中Δα_k为第l个姿态更新周期内的第k个拟合角增量子样，为第k个拟合角增量子样周期内的第m+1个角速率采样，T_k为姿态更新周期内的拟合角增量子样时间间隔(即拟合角增量子样周期)，T为固定的角速率采样时间间隔，N为一个姿态更新周期内的拟合角增量子样数，M为一个拟合角增量子样周期内用于拟合Δα_k的顺序角速率采样的个数，s_k为使用角速率采样

对Δα_k进行近似的拟合系数；

圆锥误差补偿项δφ_l为：

${\mathrm{\ω}}_0\≡{\mathrm{\ω}}_0^1,$ ${\mathrm{\ω}}_k\≡{\mathrm{\ω}}_M^k,$ k=1，2，...，M；

其中，η_ij、μ_ij和ξ_ij分别为对应于角速率采样叉乘项、角增量子样叉乘项和角速率采样与角增量子样叉乘项的圆锥误差补偿系数，i、j和k为符号下标。记ω₀为第1个角增量子样周期内第1个角速率采样

记ω_k为第k个角增量子样周期内最后一个角速率采样

姿态更新周期、拟合角增量子样周期和角速率采样周期之间的关系如图3所示。

步骤(2)简化所述圆锥误差补偿结构，得到压缩圆锥误差补偿结构的具体步骤如下：

定义经典圆锥运动形式为：

Φ(t)=[0 acosΩt asinΩt]^T        (1)

其中t为时刻，Φ(t)为载体系(定义为b系)相对参考系(定义为n系)在t时刻的旋转矢量，a为半锥角，Ω为圆锥运动频率，[]^T为[]的转置。

则时间区间[t，t+T_l]内的更新旋转矢量的近似分析表达式

在此基础上，与式(1)定义的圆锥运动相一致，定义t时刻b系相对于n系的角速度在b系下的投影为：

${\mathrm{\ω}}_{\mathrm{nb}}^b\left(t\right)={\left[\begin{array}{ccc}-2\mathrm{\Ω}{\sin }^2(a/2)& -\mathrm{\Ω}\sin a\sin \mathrm{\Ωt}& \mathrm{\Ω}\sin a\cos \mathrm{\Ωt}\end{array}\right]}^T;---\left(3\right)$

在b系下令角速度采样值ω_i为

其中i=0，1，..，N；当角速率采样频率足够高时，忽略角增量子样Δα_j的拟合误差，并令

其中j=1，2，...，N，得到角速率采样叉乘项ω_i×ω_j的在x轴向上的第一个分量[ω_i×ω_j]_x为

[ω_i×ω_j]_x=Ω²sin²asin[(j-i)ΩT_k]        (8)，

角增量子样叉乘项在x轴向上的Δα_i×Δα_j的第一个分量[Δα_i×Δα_j]_x为

${[\mathrm{\Δ}{\mathrm{\α}}_i\×{\mathrm{\α}}_j]}_x=4{\sin }^2\frac{\mathrm{\Ω}{T}_k}{2}{\sin }^{2}a\sin \left[(j-i)\mathrm{\Ω}{T}_k\right]---\left(9\right),$

角速率采样与角增量子样叉乘项ω_i×Δα_j在x轴向上的的第一个分量[ω_i×Δα_j]_x为

${[{\mathrm{\ω}}_i\×\mathrm{\Δ}{\mathrm{\α}}_j]}_x=2\mathrm{\Ω}\sin \frac{\mathrm{\Ω}{T}_k}{2}{\sin }^{2}a\sin \left[(j-i)\mathrm{\Ω}{T}_k\frac{{\mathrm{\ΩT}}_k}{2}\right]---\left(10\right);$

显然，式(8)、式(9)和式(10)中的叉乘项在x轴向上的分量与j-i有关，而与i、j无关，若只考虑x轴向上的第一分量，式(7)描述的圆锥误差补偿项δφ_l的补偿结构简化压缩为：

${\mathrm{\δ\φ}}_l={T_k}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{\mathrm{\ω}}_{N-n}\×{\mathrm{\ω}}_N+\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\mathrm{\Δ}{\mathrm{\α}}_{N-p}\×\mathrm{\Δ}{\mathrm{\α}}_N+T_k\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q{\mathrm{\ω}}_{N-q}\×\mathrm{\Δ}{\mathrm{\α}}_N---\left(11\right);$

其中，

A_n为与η_ij相当的圆锥误差补偿系数，在量值上等于所有的η_(i-n)，i之和；

B_p为与μ_ij相当的圆锥误差补偿系数，在量值上等于所有的μ_(i-p)，i之和；

C_q为与ξ_ij相当的圆锥误差补偿系数，在量值上等于所有的ξ_(i-q)，i-ξ_i，_(i-q+1)之和；

k=1，2，...，M。

步骤(3)基于步骤(2)所述的压缩圆锥误差补偿结构，建立角速率输入的圆锥误差补偿误差准则的具体步骤如下：

在圆锥运动条件下，圆锥误差补偿项数值计算误差表现为，误差矢量的第一个轴向分量是常值分量，其余两个轴向分量是周期分量，并将误差矢量的第一个轴向定义为x轴；定义圆锥误差补偿项误差e为圆锥误差补偿项的理论值与数值计算值在x轴上的分量之差，即

其中，δφ_x为更新旋转矢量圆锥误差补偿项理论值在x轴上的分量，

为更新旋转矢量圆锥误差补偿项数值计算值在x轴上的分量。

根据式(2)，时间区间[t_l-1，t_l-1+T_l]内的圆锥误差补偿项理论值在x轴上的分量δφ_x为：

将所述叉乘项分量[ω_i×ω_j]_x、[Δα_i×Δα_j]_x和[ω_i×Δα_j]_x的分析表达式：式(8)、式(9)和式(10)代入圆锥误差补偿形式的压缩结构式(11)中，得到圆锥误差补偿项数值计算值δφ_l在x轴上的分量

为：

$\begin{array}{c}{\mathrm{\δ\φ}}_x={T_k}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{\mathrm{\Ω}}^2{\sin }^{2}a\sin \left(\mathrm{n\Ω}{T}_k\right)+4\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p{\sin }^2\frac{\mathrm{\Ω}{T}_k}{2}{\sin }^{2}a\sin \left(\mathrm{p\Ω}{T}_k\right)\\ +2T_k\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q\mathrm{\Ω}\sin \frac{\mathrm{\Ω}{T}_k}{2}{\sin }^{2}a\sin \left[(q-\frac{1}{2})\mathrm{\Ω}{T}_k\right]\end{array}---\left(13\right),$

从而导出圆锥误差补偿项误差e为：

$e=\frac{1}{2}{\sin }^{2}a[N\mathrm{\β}-\sin \left(\mathrm{N\β}\right)-2{\mathrm{\β}}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n\sin \left(\mathrm{n\β}\right)-8{\sin }^2\frac{\mathrm{\β}}{2}\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\sin \left(\mathrm{p\β}\right)4\mathrm{\β}\sin \frac{\mathrm{\β}}{2}{C}_q\sin (\mathrm{q\β}-\frac{\mathrm{\β}}{2})]---\left(14\right),$

其中β=ΩT_k，β为与角增量子样周期T_k有关的圆锥频率参数。

步骤(4)中建立圆锥误差补偿结构参数优化目标和圆锥误差补偿结构参数优化方法的具体步骤为：

圆锥误差补偿优化设计的目标是：使圆锥无差补偿项误差e的绝对数值达到最小；圆锥误差补偿结构参数优化设计方法是：根据三角函数关系式sin²(β／2)=(1-cosβ)／2，有

$8{\sin }^2\frac{\mathrm{\β}}{2}\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\sin \left(\mathrm{p\β}\right)=2\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\{2\sin \left(\mathrm{p\β}\right)-\sin [(p+1)\mathrm{\β}]-\sin [(p-1)\mathrm{\β}\left]\right\}---\left(15\right)$

$4\mathrm{\β}\sin \frac{\mathrm{\β}}{2}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q\sin (\mathrm{q\β}-\frac{1}{2}\mathrm{\β})=-2\mathrm{\β}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q\{\cos \left(\mathrm{q\β}\right)-\cos (q-1)\mathrm{\β}\}---\left(16\right),$

再根据式(15)和式(16)描述的关系，圆锥误差补偿项误差e式(14)可写为：

$\begin{array}{c}e=\frac{1}{2}{\sin }^{2}a[\mathrm{N\β}-\sin \left(\mathrm{N\β}\right)-2{\mathrm{\β}}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n\sin \left(\mathrm{n\β}\right)-2\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\{2\sin \left(\mathrm{p\β}\right)-\sin \left[(p+1)\mathrm{\β}\right]-\sin \left[(p-1)\mathrm{\β}\right]\}\\ +2\mathrm{\β}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q\{\cos \left(\mathrm{q\β}\right)-\cos (q-1)\mathrm{\β}\}]\end{array}---\left(17\right)$

再根据三角级数关系式：

$\sin x=x-\frac{x^3}{3!}+...+\frac{{(-1)}^{k-1}{x}^{2k-1}}{(2k-1)!}+...---\left(18\right)$

$\cos x=1-\frac{x^2}{2!}+...+\frac{{(-1)}^{k-1}{x}^{2(k-1)}}{2(k-1)!}+..---\left(19\right),$

得到圆锥误差补偿项误差e式(17)关于圆锥频率参数β的幂级数形式表达式：

$\begin{array}{c}e=\frac{1}{2}{\sin }^{2}a\underset{k=1}{\overset{\∞}{\mathrm{\Σ}}}\{\frac{{(-1)}^{k-1}}{(2k+1)!}{N}^{2k+1}2\frac{{(-1)}^{k-1}}{(2k-1)!}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{n}^{2k-1}\\ -2\frac{{(-1)}^k}{(2k+1)!}\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p[2p^{2k+1}-{(p+1)}^{2k+1}-{(p-1)}^{2k+1}]+2\frac{{(-1)}^k}{\left(2k\right)!}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q[q^{2k}-{(q-1)}^{2k}]\}{\mathrm{\β}}^{2k+1}\end{array}---\left(20\right),$

令圆锥误差补偿项误差e关于β的幂级数项β^2k+1的系数为零，k=1，2，..，3N-1；得到线性方程组：

$\begin{array}{c}2\frac{{(-1)}^{k-1}}{(2k-1)!}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{n}^{2k-1}-2\frac{{(-1)}^k}{\left(2k\right)!}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q[q^{2k}-{(q-1)}^{2k}]+2\frac{{(-1)}^k}{(2k+1)!}\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p[2p^{2k+1}-{(p+1)}^{2k+1}-{(p-1)}^{2k+1}]\\ =\frac{{(-1)}^{k-1}}{(2k+1)!}{N}^{2k+1}\end{array}---\left(21\right),$

其中，k=1，2，...，3N-1；解由式(21)给出的线性方程组，得到优化的圆锥误差补偿系数A_n、B_p和C_q。

至此，获得一种由式(4)、式(5)、式(6)和式(7)共同描述并附加由式(21)确定的圆锥误差补偿系数的捷联惯导姿态算法，以及获得一种单独由式(7)描述的捷联惯导圆锥误差补偿结构。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种角速率输入的姿态算法结构与参数优化方法，其特征在于包括以下步骤：

步骤(1)，建立用于角速率输入的姿态算法的圆锥误差补偿结构：

在一个姿态更新周期内，以拟合角增量子样周期为时间间隔顺序抽取角速率采样值，并拟合角增量子样；然后，建立用于姿态更新的更新旋转矢量的解算形式和圆锥误差补偿结构；

步骤(2)，简化所述圆锥误差补偿结构，得到压缩圆锥误差补偿结构：

定义经典圆锥运动，忽略角增量子样的拟合误差，并只考虑角速率采样叉乘项、角增量子样叉乘项、角速率采样与角增量子样叉乘项的各自在x轴上的第一个分量，建立压缩圆锥误差补偿结构，得到压缩圆锥误差补偿项；

步骤(3)，基于步骤(2)所述的压缩圆锥误差补偿结构，建立角速率输入的圆锥误差补偿误差准则：

定义圆锥误差补偿项误差为圆锥误差补偿项的理论值与数值计算值在误差矢量的第一个轴向分量上的分量差；

步骤(4)，建立圆锥误差补偿结构参数优化目标和圆锥误差补偿结构参数优化方法：

圆锥误差补偿优化设计的目标：使圆锥误差补偿项误差的绝对数值达到最小；

圆锥误差补偿结构参数优化设计方法是：根据所述目标，将圆锥误差补偿项误差展开成关于圆锥频率参数的幂级数，令幂级数前3N-1个低阶项的系数为零，并求解得到的线性方程组，得到优化的圆锥误差补偿系数；其中N为一个姿态更新周期包含的拟合角增量子样周期数。

2.根据权利要求1所述的一种角速率输入的姿态算法结构与参数优化方法，其特征在于：所述步骤(1)中建立用于角速率输入的姿态算法的圆锥误差补偿结构具体步骤如下：首先，定义姿态更新周期的开始时刻为t_l-1，结束时刻为t_l，姿态更新周期为T_l，拟合角增量子样周期为T_k，角速率采样周期为T；其中，一个姿态更新周期包含的拟合角增量子样周期数为N，即T_l＝NT_k；拟合一个角增量所用的顺序角速率采样数为M+1，即T_k=MT；从时刻t_l-1开始在一个姿态更新周期内，即时间区间[t_l-1，t_l]内以T_k为时间间隔顺序抽取的第i+1个角速率采样值为ω_i，其中按照时间发生的先后顺序，i取值为0，1，...，N；从时刻t_l-1开始在一个姿态更新周期内，即时间区间[t_l-1,t_l]内使用时间区间[t_l-1+(k-1)T_k,t_l-1+kT_k]内的角速率采样值拟合的第k个角增量子样为Δα_k，其中按照时间发生的先后顺序，k取值为1，2，...，N；然后，给出用于姿态更新的更新旋转矢量的解算形式和圆锥误差补偿结构：所述更新旋转矢量φ_t为φ_t=α_l+δφ_l，所述圆锥误差补偿项δφ_l为 $\mathrm{\δ}{\mathrm{\φ}}_l={T_k}^2\underset{i=0}{\overset{N-1}{\mathrm{\Σ}}}\underset{j=i+1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\η}}_{\mathrm{ij}}{\mathrm{\ω}}_i\×{\mathrm{\ω}}_j+\underset{i=1}{\overset{N-1}{\mathrm{\Σ}}}\underset{j=i+1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{\mathrm{ij}}\mathrm{\Δ}{\mathrm{\α}}_i\×\mathrm{\Δ}{\mathrm{\α}}_j+T_k\underset{i=0}{\overset{N}{\mathrm{\Σ}}}\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_{\mathrm{ij}}{\mathrm{\ω}}_i\×\mathrm{\Δ}{\mathrm{\α}}_j;$ 其中， ${\mathrm{\α}}_l=\underset{k=1}{\overset{N}{\mathrm{\Σ}}}\mathrm{\Δ}{\mathrm{\α}}_k,$ α_l为一个姿态更新周期内总的拟合角增量，η_ij、μ_ij和ξ_ij分别为对应于角速率采样叉乘项、角增量子样叉乘项和角速率采样与角增量子样叉乘项的圆锥误差补偿系数，i、j和k为符号下标。

3.根据权利要求2所述的一种角速率输入的姿态算法结构与参数优化方法，其特征在于：步骤(2)简化所述圆锥误差补偿结构，得到压缩圆锥误差补偿结构的具体步骤如下：定义经典圆锥运动Φ(t)=[0 acosΩt asinΩt]^T，其中t为时刻，Φ(t)为载体系相对参考系在t时刻的旋转矢量，并将载体系定义为b系，将参考系定义为n系，a为半锥角，Ω为圆锥运动频率，[]^T为[]的转置；在此基础上，定义t时刻b系相对于n系的角速度在b系下的投影

为： ${\mathrm{\ω}}_{\mathrm{nb}}^b\left(t\right)={\left[\begin{array}{ccc}-2\mathrm{\Ω}{\sin }^2(a/2)& -\mathrm{\Ω}\sin a\sin \mathrm{\Ωt}& \mathrm{\Ω}\sin a\cos \mathrm{\Ωt}\end{array}\right]}^T;$ 令角速度采样值ω_i为

其中i=0，1，...，N，忽略角增量子样Δα_j的拟合误差，并令其中j=1，2，...，N，得到角速率采样叉乘项ω_i×ω_j的第一个分量[ω_i×ω_j]_x等于Ω²sin²asin[(j-i)ΩT_k]，角增量子样叉乘项Δα_i×Δα_j的第一个分量[Δα_i×Δα_j]_x等于

角速率采样与角增量子样叉乘项ω_i×Δα_j的第一个分量[ω_i×Δα_j]_x等于

只考虑角速率采样叉乘项、角增量子样叉乘项、角速率采样与角增量子样叉乘项的各自在x轴向上的第一个分量，所述圆锥误差补偿项δφ_l的补偿结构简化压缩为： $\mathrm{\δ}{\mathrm{\φ}}_l={T_k}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{\mathrm{\ω}}_{N-n}\×{\mathrm{\ω}}_N+\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\mathrm{\Δ}{\mathrm{\α}}_{N-p}\×\mathrm{\Δ}{\mathrm{\α}}_N+T_k\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q{\mathrm{\ω}}_{N-q}\×{\mathrm{\α}}_N;$ 其中， $A_n=\underset{i=n}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\η}}_{(i-n),j},$ A_n为与η_ij相当的圆锥误差补偿系数，在量值上等于所有的η_(i-n)，i之和；

B_p为与μ_ij相当的圆锥误差补偿系数，在量值上等于所有的μ_(i-p)，i之和；

C_q为与ξ_ij相当的圆锥误差补偿系数，在量值上等于所有的ξ_(i-q)，i-ξ_i，(i-q+1)之和。

4.根据权利要求3所述的一种角速率输入的姿态算法结构与参数优化方法，其特征在于：步骤(3)基于步骤(2)所述的压缩圆锥误差补偿结构，建立角速率输入的圆锥误差补偿误差准则的具体步骤如下：在圆锥运动条件下，圆锥误差补偿项数值计算误差表现为，误差矢量的第一个轴向分量是常值分量，其余两个轴向分量是周期分量，并将误差矢量的第一个轴向定义为x轴；定义圆锥误差补偿项误差e为圆锥误差补偿项的理论值与数值计算值在x轴上的分量之差，即

；其中，δφ_x为更新旋转矢量圆锥误差补偿项理论值在x轴上的分量，

为更新旋转矢量圆锥误差补偿项数值计算值在x轴上的分量；圆锥误差补偿项理论值在x轴上的分量δφ_x等于

将所述叉乘项分量[ω_i×ω_j]_x、[Δα_i×Δα_j]_x和[ω_i×Δα_j]_x的分析表达式代入圆锥误差补偿形式的压缩结构 ${\mathrm{\δ\φ}}_l={T_k}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{\mathrm{\ω}}_{N-n}\×{\mathrm{\ω}}_N+\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\mathrm{\Δ}{\mathrm{\α}}_{N-p}\×\mathrm{\Δ}{\mathrm{\α}}_N+T_k\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q{\mathrm{\ω}}_{N-q}\×\mathrm{\Δ}{\mathrm{\α}}_N$ 中，得到圆锥误差补偿项数值计算值在x轴上的分量

为

从而导出圆锥误差补偿项误差e为：

$e=\frac{1}{2}{\sin }^{2}a[N\mathrm{\β}-\sin \left(\mathrm{N\β}\right)-2{\mathrm{\β}}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n\sin \left(\mathrm{N\β}\right)-8{\sin }^2\frac{\mathrm{\β}}{2}\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\sin \left(\mathrm{p\β}\right)-4\mathrm{\β}\sin \frac{\mathrm{\β}}{2}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q\sin (\mathrm{q\β}-\frac{\mathrm{\β}}{2})],\mathrm{\β}=\mathrm{\Ω}{T}_k,$

其中β为与角增量子样周期T_k有关的圆锥频率参数。

5.根据权利要求4所述的一种角速率输入的姿态算法结构与参数优化方法，其特征在于：所述步骤(4)中建立圆锥误差补偿结构参数优化目标和圆锥误差补偿结构参数优化方法的具体步骤为：圆锥误差补偿优化设计的目标是：使圆锥无差补偿项误差e的绝对数值达到最小；圆锥误差补偿结构参数优化设计方法是：根据三角函数关系式sin²(β／2)=(1-cosβ)／2，所述圆锥误差补偿项误差e得到为如下形式：

$\begin{array}{c}e=\frac{1}{2}{\sin }^{2}a[\mathrm{N\β}-\sin \left(\mathrm{N\β}\right)-2{\mathrm{\β}}^2\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n\sin \left(\mathrm{n\β}\right)-2\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p\{2\sin \left(\mathrm{p\β}\right)-\sin \left[(p+1)\mathrm{\β}\right]-\sin \left[(p-1)\mathrm{\β}\right]\}\\ +2\mathrm{\β}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q\{\cos \left(\mathrm{q\β}\right)-\cos (q-1)\mathrm{\β}\}]\end{array},$ 再根据三角级数关系式 $\sin x=x-\frac{x^3}{3!}+...+\frac{{(-1)}^{k-1}{x}^{2k-1}}{(2k-1)!}+...$ 和 $\cos x=1-\frac{x^2}{2!}+...+\frac{{(-1)}^{k-1}{x}^{2(k-1)}}{2(k-1)!}+..,$ 得到圆锥误差补偿项误差e关于圆锥频率参数β的幂级数形式表达式：

$\begin{array}{c}e=\frac{1}{2}{\sin }^{2}a\underset{k=1}{\overset{\∞}{\mathrm{\Σ}}}\{\frac{{(-1)}^{k-1}}{(2k+1)!}{N}^{2k+1}-2\frac{{(-1)}^{k-1}}{(2k-1)!}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{n}^{2k-1}\\ -2\frac{{(-1)}^k}{(2k+1)!}\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p[2p^{2k+1}-{(p+1)}^{2k+1}-{(p-1)}^{2k+1}]+2\frac{{(-1)}^k}{\left(2k\right)!}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q[q^{2k}-{(q-1)}^{2k}]\}{\mathrm{\β}}^{2k+1}\end{array},$

令圆锥误差补偿项误差e关于β的幂级数项β^2k+1的系数为零，k=1，2，...，3N-1；得到线性方程组：

$\begin{array}{c}2\frac{{(-1)}^{k-1}}{(2k-1)!}\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{A}_n{n}^{2k-1}-2\frac{{(-1)}^k}{\left(2k\right)!}\underset{q=1}{\overset{N}{\mathrm{\Σ}}}{C}_q[q^{2k}-{(q-1)}^{2k}]+2\frac{{(-1)}^k}{(2k+1)!}\underset{p=1}{\overset{N-1}{\mathrm{\Σ}}}{B}_p[2p^{2k+1}-{(p+1)}^{2k+1}-{(p-1)}^{2k+1}]\\ =\frac{{(-1)}^{k-1}}{(2k+1)!}{N}^{2k+1}\end{array},$ k=1，2，...，3N-1；解该线性方程组，得到优化的圆锥误差补偿系数A_n、B_p和C_q。

Images