CN103429986A - 惯性导航划桨算法 - Google Patents

Abstract

一种用于确定惯性导航系统中的组合的速度旋转补偿和划桨补偿的系统包括：陀螺仪，其被配置为提供具有来自三个正交轴的分量的测量的旋转角速率；加速度计，其被配置为提供具有来自三个正交轴的分量的测量的比力；以及处理器，其被配置为按单一的计算频率计算第一组合的速度旋转补偿和划桨补偿，该处理器被配置为：计算在第一个时间间隔内的瞬时旋转角速率与测量的比力变化的第一矢量积；以及将第一矢量积和第二矢量积相加，其中第二矢量积是在第二个时间间隔内的比力的变化的一部分和第一个时间间隔内测量的角速率的变化的矢量积；其中第一个时间间隔和第二个时间间隔每个是单一的计算频率的一个周期。

Description

惯性导航划桨算法

背景

当参考点不确定时，确定交通工具的位置和对地速度是困难的。然而，目前的导航系统利用全球定位系统的帮助来确定参考点。某些情况下，GPS是不可用的和/或速度太慢，并且导航系统是独立的。这些系统常常利用一组陀螺仪来测量角速度和线性加速度。这些最初通过电动常平架系统，使用导航系作为基于该系统所位于的交通工具的设置参考系来实现。这些系统包括许多昂贵的机械部件以保证适当的运动，并且还包括每个常平架上的传感器（例如，加速度计和陀螺仪），其用于测量姿态变化，即：用滚动角、俯仰角和偏航角参数表示的交通工具的方位变化。由于运动部件，常平架系统容易受到一些诱发误差的因素的影响，例如常平架锁定，当两个常平架并行且一致地转动时，易发生常平架锁定。此外，常平架的轴承和翼能够随着时间而阻延和/或磨损。甚至更多的，轴承并不是无摩擦的，这样也会在测量时引起误差。当系统经历误差时，在被证实能够重新使用前，它可从交通工具中被移除并且在非常特定的条件下被再校准。因此，这类系统仅仅有助于非常特定的市场的消费者。

捷联式惯性导航系统（SINS）具有直接固联在系统底盘上的组件。底盘也被固定到其所位于的交通工具上。因此，系统中没有部件是移动的。这减轻了由摩擦和/或故障组件引起的误差，其也需要少量的校准。三个传感器测量线性加速度

三个传感器测量旋转角速率

速度更新频率（例如，m）是用于对由加速度计和陀螺仪捕获的原始测量结果执行计算的时间间隔，并且在特定的时间阶段内逐渐地更新系统参数（例如，姿态、速度、位置等）。例如，更新率可以是一个系统的计算机时钟周期。由于计算的高计算量负担，原始测量结果在时间间隔t内被快速捕获，并且被计算设备上的软件方案处理。

传感器提供以矢量形式给出的与非重力加速度或比力（specific force）相关的原始数据，并且该数据被转化到与三个正交矢量分量的瞬时的交通工具姿态相关的参考系中。参考系B是在时间t等于t_m-1（即t=t_m-1）时的交通工具主体方位，其中该交通工具的主体方位在频率为m的上一个更新周期内被测量。例如，原始测量结果以频率m被更新，所以使用上一个更新周期(m-1)中捕获的原始测量结果来计算在当前时刻t的交通工具的主体方位，因为在本次更新周期（即，t=t_m）内的测量结果被捕获并计算与原始参考系B相关的交通工具的主体方位。系统中检测到的每个运动根据与上一个参考系B中的方位有关的交通工具主体的当前方位被计算。由传感器提供的原始测量的三个正交矢量分量以一组方向余弦或四元数给出，该方向余弦或四元数描述了就滚动角、俯仰角和偏航角而言的交通工具的空间方位和运动。P.G.Savage,“Strapdown Inertial Navigation IntegrationAlgorithm Design Part2:Velocity and Position Algorithms”Journal ofGuidance,Control,and Dynamics,vol21,No.2,1998,pp.208-221;和“Strapdown System Algorithms”Advances in Strapdown Inertial Systems,NATO AGARD Lecture Series No.133,1984年5月，都把捷联式系统中的速度算法表示为如下的解析形式：

${\mathrm{\Δv}}_{{\mathrm{SF}}_m}^{B_{m-1}}={\∫}_{t_{m-1}}^{t_m}{C}_{B_m}^{B_{m-1}}{a}_{\mathrm{SF}}^B\mathrm{dt}={\∫}_{t_m}^{t_{m-1}}[I+\frac{\sin \left|\mathrm{\φ}\left(t\right)\right|}{\left|\mathrm{\φ}\left(t\right)\right|}(\mathrm{\φ}\left(t\right)\×)+\frac{1-\cos \left|\mathrm{\φ}\left(t\right)\right|}{|\mathrm{\φ}\left(t\right){|}^2}{(\mathrm{\φ}\left(t\right)\×)}^2]a_{\mathrm{SF}}^B\mathrm{dt}---\left(1\right)$

其中：

是在从t_m-1到t_m的时间间隔中，由体坐标系B_m-1中的比力（SF）引起的速度变化；

B_m-1是在t_m-1时刻，其轴平行于指定的右手正交传感器输入轴的体坐标系B_m；

是体坐标系B_m-1中的加速度计测量的比力（SF）;

是从体坐标系B_m到B_m-1的方向余弦矩阵；以及

φ(t)是旋转矢量，其定义在大于t_m-1的时刻，坐标系B_m相对于坐标系B_m-1的大体方位；

等式1被一阶拟合为：

$\mathrm{\Δ}{v}_{{\mathrm{SF}}_m}^{B_{m-1}}={\∫}_{t_{m-1}}^{t_m}[I+(\mathrm{\α}\left(t\right)\×)]a_{\mathrm{SF}}^B\mathrm{dt}---\left(2\right)$

其中 $\mathrm{\α}\left(t\right)={\∫}_{t_{m-1}}^t{\mathrm{\ω}}_{\mathrm{IB}}^B\mathrm{d\τ},\mathrm{\α}\left(m\right)=\mathrm{\α}(t=t_m);$ 以及      (3)

是由陀螺仪提供的被测旋转角速率。

进一步的，矢量积的展开式为：

$\mathrm{\Δ}{v}_{{\mathrm{SF}}_m}^{B_{m-1}}=v_m+\frac{1}{2}{\mathrm{\α}}_m\×v_m+\frac{1}{2}{\∫}_{t_{m-1}}^{t_m}(\mathrm{\α}\left(t\right)\×a_{\mathrm{SF}}^B+v\left(t\right)\×{\mathrm{\ω}}_{\mathrm{IB}}^B)\mathrm{dt}---\left(4\right)$

其中 $v\left(t\right)={\∫}_{t_{m-1}}^{t_m}{a}_{\mathrm{SF}}^B\mathrm{d\τ},v\left(m\right)=v(t=t_m)---\left(5\right)$

这些分量通过方向余弦矩阵

被转换到坐标参考系（例如B）中后，根据参考系的合成的非重力加速度被生成。合成的非重力加速度的积分反映了交通工具从其原始位置的空间方位和运动的变化。因此，通过计算与已知的原始姿态、位置和速度的差别，可以确定当前的姿态、位置和速度。

具体地说，为了计算交通工具的近似速度，也能把系统误差包括到总的测量结果中。该误差可以由等式（4）的第三项表示。这项误差被公知为划桨误差或划桨补偿，并且它能被用来帮助在高度动态的环境中保持速度的准确性。因此等式（4）被简化为：

$\mathrm{\Δ}{v}_{\mathrm{SF}}^{B_{m-1}}=v_m+\mathrm{\Δ}{v}_{{\mathrm{rot}}_m}+\mathrm{\Δ}{v}_{{\mathrm{scul}}_m}=v_m+\mathrm{\Δ}{v}_{{\mathrm{rot}/\mathrm{scul}}_m}---\left(6\right)$

其中：

是在更新周期m的速度旋转补偿，或由于交通工具的旋转运动而导致的速度变化；以及

是在更新周期m的划桨补偿，或由于交通工具的划桨运动而导致的速度变化。

划桨误差是由SINS经历的视在且恒定的加速度的产物，其常常是由振动以及致使传感器出错的其它因素所引起的。本质上划桨误差是周期性的，并且如果转换功能中的测量结果是不正确的，则划桨误差因此可能被算错。

前面提到的本领域中用于计算被测加速度和旋转速度的已知方法被以下参考文献公开：P.G.Savage,“Strapdown Inertial Navigation IntegrationAlgorithm Design Part2:Velocity and Position Algorithms”Journal ofGuidance,Control,and Dynamics,vol21,No.2,1998,pp.208-221;and“Strapdown System Algorithms”Advances in Strapdown Inertial Systems,NATO AGARD Lecture Series No.133,1984年5月。Savage还提供了两种解析形式来描述等式6中的速度和划桨补偿。这两种解析形式适当地考虑了在捷联式系统的总的速度变化

的计算中的划桨补偿。这两种解析形式的计算能用软件实现。第一解析形式提供了表示速度旋转补偿的单独项，该速度旋转补偿是在周期m的旋转角速率（即角加速度）变化的积分和在周期m的主体的线性加速度率（即速度）变化的积分的矢量积：

$\mathrm{\Δ}{v}_{{\mathrm{rot}}_m}=\frac{1}{2}{\mathrm{\α}}_m\×v_m---\left(7\right)$

第二种解析形式提供了以积分形式描述划桨补偿的两项，对其的计算是基于速度旋转补偿。在这两种解析形式中，速度补偿按速度更新采样频率或周期率（例如50Hz）计算，因为它有助于确定速度更新。然而Savage论述：在第二解析形式中，有两项，划桨补偿在不同的频率（例如，l）求积分，其优选为比速度更新频率m快的周期率。

因此，划桨误差随后在速度更新频率间隔中可以被计算和更新几次。Savage将该计算在下列解析形式中表示为用于划桨补偿项的数字递归算法：

$\mathrm{\Δ}{\mathrm{\α}}_l={\∫}_{t_{l-1}}^{t_l}{\mathrm{\ω}}_{\mathrm{IB}}^B\mathrm{dt},{\mathrm{\α}}_l={\mathrm{\α}}_{l-1}+\mathrm{\Δ}{\mathrm{\α}}_l,{\mathrm{\α}}_m={\mathrm{\α}}_l(t_l=t_m),{\mathrm{\α}}_l=0,\mathrm{at},t=t_{m-1}---\left(8\right)$

$\mathrm{\Δ}{v}_l={\∫}_{t_{l-1}}^{t_l}{a}_{\mathrm{SF}}^B\mathrm{dt},v_l=v_{l-1}+\mathrm{\Δ}{v}_l,{v_m=v}_l(t_l=t_m),v_l=0,\mathrm{at},t=t_{m-1}---\left(9\right)$

$\mathrm{\δ}{v}_{{\mathrm{scul}}_l}=\frac{1}{2}[({\mathrm{\α}}_{l-1}+\frac{1}{6}\mathrm{\Δ}{\mathrm{\α}}_{l-1})\×\mathrm{\Δ}{v}_l+(v_{l-1}+\frac{1}{6}\mathrm{\Δ}{v}_{l-1})\×\mathrm{\Δ}{\mathrm{\α}}_l]$

                                                        (10)

$\mathrm{\Δ}{v}_{{\mathrm{scul}}_l}=\mathrm{\Δ}{v}_{{\mathrm{scul}}_{l-1}}+\mathrm{\δ}{v}_{{\mathrm{scul}}_l},\mathrm{\Δ}{v}_{{\mathrm{scul}}_m}=\mathrm{\Δ}{v}_{{\mathrm{scul}}_l}(t=t_m),\mathrm{\Δ}{v}_{{\mathrm{scul}}_l}=0,\mathrm{at},t=t_{m-1}$

因此，该方法依赖系统的原始测量结果被计算的两种计算频率（m,l）。因为使用这两个周期频率，原始测量结果必须被分别计算并结合在一起，与仅按速度更新频率进行计算的系统相比，具有这两个频率会大量增加系统的计算负担。

在更精确地计算系统内的划桨误差的另一个方法中，M.B.Ignagni“Duality of Optimal Strapdown Sculling and Coning CompensationAlgorithms”Journal of the Institute of Navigation,Vol.45,No.2,1998,pp.85-95，论述了在一系列算法中使用系数的方法。这些系数算法被用来将系统的固有误差考虑进来，诸如不同的系统环境中的系统平台振动。因此Ignagni论述了一组特定的系数算法来解决每一个这些问题。系数算法与定义的快频率和慢频率的比率对应。因为这些系数的特定性质，不是所有的快频率和慢频率的比率都是合理的。例如，在Ignagni提供的方法中，诸如l/m=7或l/m>10的比率在计算上是不合理的。此外，Ignagni提供的技术没有改善Savage论述的技术中的大量计算负担。

在相等的一个时间间隔中（例如，系统时钟周期）计算错误的划桨补偿可能有害于惯性导航系统，因为算法是递归的且前面的计算为拟合计算提供了基础（见等式8-10）。系统中的轻微偏差和随后的错算可触发错误的读数。一些技术，诸如Savage和Ignagni提出的这些技术，已被改进来考虑这些系统缺陷，以使合适的划桨补偿或误差被计算并从系统捕获的原始测量结果中移除。这些计算增加了处理负担，其固有地增加了系统大小（即，大型处理器）。然而，增加的负担和系统大小不是导航系统所期望的。

发明内容

一种用于确定惯性导航系统中的组合的速度旋转补偿和划桨补偿的系统的实例包括：一组陀螺仪，其被配置来测量旋转角速率并提供具有来自三个正交轴的分量的测量的旋转角速率；一组加速度计，其被配置来测量比力并提供具有来自三个正交轴的分量的测量的比力；以及处理器，其被耦合所述一组陀螺仪和所述一组加速度计，并被配置为按单一的计算频率计算第一组合的速度旋转补偿和划桨补偿，所述处理器被配置为：计算在第一个时间间隔内的瞬时旋转角速率与测量的比力的变化的第一矢量积，其中所述瞬时旋转角速率是：在所述第一个时间间隔之前的第二个时间间隔内测量的旋转角速率、在所述第一个时间间隔内测量的旋转角速率的变化的一部分、以及在所述第二个时间间隔内测量的旋转角速率的变化的一部分的累加和；以及将所述第一矢量积和第二矢量积相加，其中所述第二矢量积是在所述第二个时间间隔内的比力的变化的一部分和所述第一个时间间隔内测量的角速率的变化的矢量积，其中所述第一个时间间隔和所述第二个时间间隔中的每一个是所述单一的计算频率的一个周期。

这样的系统的实现可包括以下特征中的一个或多个。所述速度旋转补偿具有等于零的初始的旋转角速率和比力。所述处理器还被配置为：确定所述第一个时间间隔内的由于所述组合的速度旋转补偿和所述划桨补偿导致的速度变化；其中所述速度变化等于——具有速度旋转变化的所述第一组合的速度旋转补偿和划桨补偿与第二组合的速度旋转补偿和划桨补偿的累加和；其中利用来自于所述第二个时间间隔和第三个时间间隔的数据来计算所述第二组合的速度旋转补偿和划桨补偿；以及所述第三个时间间隔在所述第二个时间间隔之前。所述处理器还被配置为：将所述惯性导航系统中的速度变化计算为在第一个计算机时钟周期频率内计算的系统速度以及所述第一个时间间隔内的速度旋转和划桨补偿两者的变化的累加和。所述单一的计算频率快于计算机时钟周期频率。所述速度旋转补偿是：所述第一个时间间隔中的测量的旋转角速率与所述第一个时间间隔中的测量的比力的第三矢量积减去所述第二个时间间隔中的测量的旋转角速率与所述第二个时间间隔中的测量的比力的第四矢量积得到的差的累加和与二分之一的乘积，其中所述累加和是在由所述单一的计算频率除所述计算机时钟周期频率所得分式限定的数量的子区间上计算的。所述组合的速度旋转补偿和划桨补偿被解析地表示为： $({\mathrm{\α}}_{l-1}+\frac{1}{2}\mathrm{\Δ}{\mathrm{\α}}_1+\frac{1}{12}\mathrm{\Δ}{\mathrm{\α}}_{l-1})\×\mathrm{\Δ}{v}_l+\frac{1}{12}\mathrm{\Δ}{v}_{l-1}\×\mathrm{\Δ}{\mathrm{\α}}_l,$ 其中α_l-1是在所述第一个时间间隔之前的第二个时间间隔测量的旋转角速率，Δα_l是在所述第二个时间间隔测量的旋转角速率的变化，Δα_l-1是在所述第二个时间间隔中测量的旋转角速率的变化，Δv_l是在所述第一时间间隔内测量的比力的变化，以及Δv_l-1是在所述第二时间间隔内测量的比力的变化。

一种用于按单一的计算频率计算惯性导航系统的组合的速度旋转补偿和划桨补偿的装置，包括：第一测量工具，其用于测量绕着三个正交轴的旋转角速率并提供具有来自三个正交轴的分量的测量的旋转角速率；第二测量工具，其用于测量比力并提供具有来自三个正交轴的分量的测量的比力；以及处理工具，其用于计算速度旋转补偿和划桨补偿的累加和，其中所述累加和是以下两项矢量积的累加和：第一个时间间隔中的瞬时旋转角速率与所述测量的比力的变化的第一矢量积，其中所述瞬时旋转角速率是在所述第一个时间间隔之前的第二个时间间隔内测量的旋转角速率、在所述第二个时间间隔内测量的旋转角速率的变化的一部分、以及在所述第一个时间间隔内测量的旋转角速率的变化的一部分的累加和；以及在所述第二个时间间隔内的比力的变化的一部分与所述第一个时间间隔内测量的角速率的变化的第二矢量积。

这样的装置的实现可包括以下特征中的一个或多个。所述第一时间间隔和所述第二时间间隔中的任意一项是所述单一的计算频率的一个周期。所述单一的计算频率要快于计算机时钟周期频率。所述速度旋转补偿是：在所述第一个时间间隔中测量的旋转角速率与在所述第一个时间间隔中测量的比力的第三矢量积减去在所述第二个时间间隔中测量的旋转角速率与在所述第二个时间间隔中测量的比力的第四矢量积得到的矢量差的累加和与二分之一的乘积，其中所述累加和是在由所述单一的计算频率除所述计算机时钟周期频率所得分式限定的数量的子区间上计算的。

一种非短暂的处理器可读的介质，包括被配置成导致处理器执行如下操作的指令：计算在第一个时间间隔内的瞬时旋转角速率与测量的比力变化的第一矢量积，其中所述瞬时旋转角速率是：在所述第一个时间间隔之前的第二个时间间隔内测量的旋转角速率、在所述第二个时间间隔内测量的旋转角速率的变化的一部分、以及在所述第一个时间间隔内测量的旋转角速率的变化的一部分的累加和；将所述第一矢量积和第二矢量积相加，其中所述第二矢量积是在所述第二个时间间隔内的所述比力的变化的一部分和在所述第一个时间间隔内测量的角速率的变化的矢量积；以及按单一的计算频率计算所述第一矢量积和所述第二矢量积，其中所述第一个时间间隔和所述第二个时间间隔中的任意一个是所述单一的计算频率的一个周期；其中所述测量的旋转角速率由一组陀螺仪提供，所述陀螺仪耦合到所述处理器并被配置来测量具有来自三个正交轴的分量的旋转角速率；以及其中所述测量的比力由一组加速度计提供，所述加速度计耦合到所述处理器并被配置来测量具有来自三个正交轴的分量的比力。

附图简述

图1是包括惯性导航系统的交通工具的简图。

图2是惯性导航系统的组件的框图。

图3是利用惯性导航系统来确定交通工具的速度变化的过程的流程图。

详述

期望找到有效且准确的方法来对划桨误差进行补偿，同时还能降低例如Savage和Ignagni的已知技术的计算负担和处理时间。为了消除在Savage中论述的两个采样频率的额外计算负担，已经发现了以快频率计算速度和划桨补偿将在很大程度上减少计算系统的总的速度变化

的计算负担，同时与以前的技术相比较，如果不是更准确的话，仍然提供同样准确的结果。速度以和划桨补偿相同频率的修正频率被更新，减少和潜在地消除了对划桨补偿的中间数据的计算。该新方法通过速度旋转补偿项的展开式来实现（见等式7），以根据快频率形成速度旋转补偿的解析式。在一些推导后，速度旋转补偿和划桨补偿可以被结合在一起以形成仅根据快频率的算法。以单一的频率处理这些补偿能够降低系统的计算负担、降低存储需求（即，用于划桨补偿的中间数据）、以及降低系统规模。例如，新方法中提供的计算负担可以提供等于或小于本领域已知的仅仅是诸如Savage的划桨补偿的计算负担。

现在参照图1，交通工具100包括惯性导航系统105。导航系统105优选较小且紧凑，并被位于交通工具100上。交通工具的一些实例包括飞机、汽车或导弹。系统105可包括减少外界因素影响的外罩，该外界因素可能会影响在系统105内收集数据。这些外界因素中的一些因素可能包括极端温度、风、振动或堆积物。为了准确地测量交通工具100上的运动（即，比力），系统105可以固定在交通工具100上，并根据地面被校准为水平的。

交通工具100提供了系统105计算位置、姿态以及速度的参考系。参考系B由交通工具100的当前的姿态、位置和速度来定义，所述当前的姿态、位置和速度与交通工具100的上一个姿态、位置和速度有关联。参考系B由其初始轴与导航系统100的轴线平行来计算。当交通工具100在一段时间移动了一段距离，由加速度计测量的瞬时比力会被系统105感觉到，并被映射到系统105中的参考系B的方向余弦矩阵。在一段时间诸如一个计算周期内，参考系B内的系统105的瞬时姿态被积分，来提供系统105中的速度变化。

如上所论述的，惯性导航系统中的速度变化的计算采用递归计算方法。每一个测量结果被捕获、转换并被在系统中进行积分。由系统经历的微小误差也被考虑到输出中。该输出被用来计算系统的下一个定位、速度以及位置。因此，在对拟合测量结果进行计算和积分之前，能被减小的误差越多，那么在系统的输出中所发现的误差则越少。

还参考图2，系统105包括：三个陀螺仪202、206、210，其被置于三个正交轴上；以及三个加速度计204、208、212，其被置于三个正交轴上。虽然显示了三个陀螺仪和三个加速度计，但是可以使用三个以上的陀螺仪或三个以上的加速度计。每个陀螺仪是以矢量形式提供绕着以参考系B为基准定义的轴的角速度

或旋转速度的传感器。在惯性系统中，三个正交轴可称为滚动、俯仰、偏航（即，飞行方向或方向角）。因此，每个陀螺仪202、206、210提供关于滚动、俯仰或偏航的轴上的测量结果。这些测量结果帮助确定交通工具100的方位。

由轴线上的陀螺仪202、206、210测量的角速度被转换成矢量，所述矢量提供了交通工具在参考系B中的瞬时姿态218。姿态218是交通工具在参考系中的方位。经过解析，姿态218隐含在等式（1）的余弦矩阵中（见等式(1)-(3)）。将来自于陀螺仪202、206、210的三个正交轴上的测量结果转换到导航参考系，以提供瞬时的交通工具姿态218。姿态218能在单独的模快中计算，其中该模块能计算将来自于陀螺仪202、206、210的原始数据测量结果转换到导航参考系的解析转换。姿态218根据参考系B被描述，其中参考系B是在上一个时间间隔内根据交通工具姿态定义的参考系。将所产生的参考与来自于加速度计204、208、212的非重力加速度结合在一起，以提供系统中的总的速度变化。（等式(1)）。

每个加速度计204、208、212是提供惯性参考系中的交通工具的线性非重力加速度的矢量形式的传感器。由于加速度提供定向力，合成的矢量称为由系统105经历的比力。在比力中的错误计算常常是由系统平台的振动引起的。将参考系B中的由加速度计测量的比力在一个计算机时钟周期中积分，以确定系统105在角方向在特定的时间t的速度。通过将这三个陀螺仪中的每个陀螺仪测量的角速度矢量转换到参考系B,以确定角方向或方位。然而，因为系统105被捷联或固联在它所位于的交通工具100上，由于交通工具的运动，陀螺仪202、206、210将振动，并且随后这些计算可能被扭曲。为了提供改进的准确性，以前面提到的划桨和速度补偿的形式对测量结果执行误差分析。如果该误差分析以快速采样频率执行，则被测量数据可以被更频繁地纠正，提高被测加速度分辨率并且因此改善计算的交通工具的速度和位置。

系统105还包括处理器214和存储器220。处理器214优选是智能设备，例如个人电脑中央处理单元（CPU）（诸如由因特尔公司或

公司制造的这些设备）、微控制器、专用集成电路（ASIC）等等。存储器220包括随机存储器（RAM）和只读存储器（ROM）。磁盘驱动36包括硬盘驱动器、CD-ROM驱动器、和/或压缩驱动器，以及可以包括驱动器的其它形式。例如在存储器220中，系统105存储包括指令的处理器可读、处理器可执行的软件代码，其中上述指令用于控制处理器214来执行下文所描述的功能。通过网络连接下载、从硬盘加载等等，软件可以被载入存储器220中。进一步地，软件可能不是可直接执行的，例如在执行前需要编译。

为了以更快的周期频率计算速度补偿分量，算法可以被推导作为等式（6）的替代形式，并且被存储为惯性导航系统105的存储器105中的计算机可读代码。新推导的算法可以与划桨补偿相结合并被处理器214计算，以确定系统105在一段时间内（诸如一个计算机时钟周期）的速度的总的变化。算法提供了：速度补偿是第一个周期（当前）l的速度与加速度的矢量积与第二个周期（第一个周期之前的）l-1的速度与加速度的矢量积之间的差。这两个矢量积之间的差在一组时间间隔中（例如1到M）被累加。该累加可以在展开式中被解析表示，其通过较快的周期率与较慢的周期率（例如计算机时钟周期率）之间的已知关系来描述。快周期率与慢周期率之间的关系可以被定义为M＝l/m。因此在快周期率的速度补偿的新的解析式变为：

$\mathrm{\Δ}{v}_{{\mathrm{rot}}_m}=\frac{1}{2}{\mathrm{\α}}_m\×v_m=\frac{1}{2}\underset{l=1}{\overset{l=M}{\mathrm{\Σ}}}[{\mathrm{\α}}_l\×v_l-{\mathrm{\α}}_{l-1}\×v_{l-1}]---\left(11\right)$

其中，M＝l/m且是在t_m-1到t_m内的子区间的数量；以及

t_m-1是前一个时钟周期m-1的时间；以及

t_m是当前时钟周期m的时间。

值m可以等于一个计算机时钟周期，该时钟周期慢于被选的快周期率l。例如，计算机处理器（例如，时钟）能以60赫兹（Hz）运行，并且快频率l可以被选在120Hz，其允许在一个时钟周期内发生两次计算。这种快周期率l可被用来计算总的补偿误差。除了第一项α₀,v₀，以及最后一项α_l×v_l-α_l-1×v_l-1，等式（11）中的累加运算的展开式提供了所有项的消除。第一项等于零，因为初始的加速度或速度不会被系统105感触到，并且零速度被用来更新系统105。最后一项可以由快周期率l的加速度和速度替代，该加速度和速度以前在等式（8）和（9）中被定义：

α_l＝α_l-1+Δα_l                  (8)

v_l＝v_l-1+Δv_l                 (9)

等式（8）和（9）提供了：第一个周期中（当前）的加速度和速度可以被定义为——第二个周期中的加速度和速度（在第一个周期之前）加上在第一个周期经历的加速度的任何变化Δα与速度的任何变化Δv。将等式（8）和（9）代入展开的等式（11）中，允许加速度的变化（Δα）与速度的变化（Δv）在快周期率l的时间间隔被考虑。快周期率l中的速度旋转补偿可以被解析地表示为：

$\mathrm{\Δ}{v}_{{\mathrm{rot}}_m}=\frac{1}{2}\underset{l=1}{\overset{l=M}{\mathrm{\Σ}}}({\mathrm{\α}}_{l-1}\×\mathrm{\Δ}{v}_l-v_{l-1}\×\mathrm{\Δ}{\mathrm{\α}}_l+\mathrm{\Δ}{\mathrm{\α}}_l\×\mathrm{\Δ}{v}_l)=\underset{l=1}{\overset{l=M}{\mathrm{\Σ}}}\mathrm{\δ}{v}_{{\mathrm{rot}}_l}---\left(12\right)$

其中， $\mathrm{\δ}{v}_{{\mathrm{rot}}_l}=\frac{1}{2}({\mathrm{\α}}_{l-1}\×\mathrm{\Δ}{v}_l-v_{l-1}\×\mathrm{\Δ}{\mathrm{\α}}_l+\mathrm{\Δ}{\mathrm{\α}}_l\×\mathrm{\Δ}{v}_l)---\left(13\right)$

速度旋转补偿（例如，等式（13））和划桨补偿（例如，等式（10））可以按相同的频率l计算。通过按快频率计算等式（10）和（12），系统105中的很小的变化也能被检测到。因此，当下个计算被执行时，这些变化可以被适当地考虑并被补偿，使得能为交通工具100提供准确的惯性读数。尽管该计算按较快的频率被执行，更多的总的系统计算也被执行，但是相比以前的系统诸如在Ignagni和Savage中所讨论的那些，系统的计算负担被显著地降低了。在先前这些算法中，旋转补偿和划桨补偿中的每个不仅被单独计算，还按不同的频率被计算。例如，先前这些算法1）按快频率执行一种计算，2）按快频率存储计算结果，3）按慢频率执行不同的计算以及，随后4）将存储结果作为因素包含到在慢频率执行的计算中。因此，这些系统的计算负担很高。

通过按快频率消除划桨补偿数据的中间数据处理（即，步骤2-4），减少了计算负担。因此系统的负担可相当于在先前技术中论述过（即步骤1）的仅仅一个补偿项的负担（即划桨或速度）。已发现根据以下解析式，速度项和划桨项可以按单一的快周期率被计算：

$\mathrm{\δ}{v}_{{\mathrm{rot}/\mathrm{scul}}_l}={\mathrm{\δv}}_{{\mathrm{rot}}_l}+\mathrm{\δ}{v}_{{\mathrm{scul}}_l}=({\mathrm{\α}}_{l-1}+\frac{1}{2}\mathrm{\Δ}{\mathrm{\α}}_l+\frac{1}{12}\mathrm{\Δ}{\mathrm{\α}}_{l-1})\×\mathrm{\Δ}{v}_l+\frac{1}{12}\mathrm{\Δ}{v}_{l-1}\×\mathrm{\Δ}{\mathrm{\α}}_l$

                                                              (14)

$\mathrm{\Δ}{v}_{{\mathrm{rot}/\mathrm{scul}}_l}=\mathrm{\Δ}{v}_{{\mathrm{rot}/\mathrm{scul}}_{l-1}}+\mathrm{\δ}{v}_{{\mathrm{rot}/\mathrm{scul}}_l,}\mathrm{\Δ}{v}_{{\mathrm{rot}/\mathrm{scul}}_m}=\mathrm{\Δ}{v}_{{\mathrm{rot}/\mathrm{scul}}_l}(t=t_m),\mathrm{\Δ}{v}_{{\mathrm{rot}/\mathrm{scul}}_l}=0,\mathrm{at},t=t_{m-1}$

该算法可以被数字地处理来计算由于系统所位于的交通工具的旋转导致的速度增量变化。

惯性导航系统105在时间间隔t内的最终速度变化可以被解析表示为：

$\mathrm{\Δ}{v}_{{\mathrm{SF}}_b}^{B_{m-1}}=v_m+\mathrm{\Δ}{v}_{{\mathrm{rot}/\mathrm{scul}}_m}---\left(15\right)$

在一个计算机周期m期间按快频率l下的单一计算被计算的解析式15，可提供系统105的速度变化，其精确地考虑了划桨误差并减小了计算负担。

现在参照图3，并进一步参照图1-2，用于按单一的计算频率确定速度和划桨补偿的方法300包括示出的步骤。方法300利用递归处理来确定当前数据。方法300计算在时间t_m-1的第一组数据，其中该时间是在上一个时钟周期m-1的时间t。方法300计算到时间t_m期间的测量结果的积分，其中该时间t_m是当前时钟周期m的时间t。划桨和速度补偿测量结果按为l的采样周期率被计算，其快于计算机时钟周期，其也可以被称为更新周期m。

在步骤305中，惯性导航系统内的加速度计204、208、212和陀螺仪202、206、210分别提供在一个时间间隔的线性加速度

以及旋转角速率

的原始测量结果。以矢量形式提供原始测量结果，并从三个正交轴中获取原始测量结果。原始测量结果被发送给系统105中的处理器214，在此，存储在存储器220中的代码结合处理器214被用来执行对原始测量结果的计算。例如，陀螺仪旋转角速率矢量

被发送给处理器214以确定交通工具100的瞬时姿态218并定义导航参考系B。通过方向余弦矩阵来定义参考系（见等式(1)），其中该方向余弦矩阵通过旋转矢量来计算瞬时交通工具姿态218，在此旋转矢量定义了参考系B相对于前一个参考系B_m-1（即在上一个计算机时钟周期的交通工具100的参考系）的总体方位。

在步骤310中，利用通过方向余弦矩阵定义的导航参考系B，线性加速度矢量

被转换到相同的参考系中。上述转换是通过将交通工具的瞬时姿态218与比力或线性加速度的矢量积进行积分而实现的。矢量积可以拟合到一阶来进一步地降低计算负担（见等式（2））。尽管如此，如果系统需要更高的准确性，另外的拟合能被利用。矢量积的一阶拟合等于当前计算机时钟周期（例如慢频率，m）的系统速度、速度旋转补偿以及划桨补偿的总和。因此，系统105的速度的总变化可被定义为——将当前周期的系统105的速度同组合的划桨补偿和速度补偿中的变化相加。划桨补偿可进一步地在数字递归算法中定义，并且按快速计算频率l来计算（见等式8-10）。

步骤315中，速度补偿通过速度旋转补偿项的展开式来推导，其也按快频率l定义。通过将快速计算频率与慢速计算频率相关联（M=l/m）并按快速计算频率对速度变化的子区间进行累加（见等式（12）-（13）），速度旋转补偿项的展开被实现。速度旋转补偿按快速计算频率被解析表示（见等式（12）），并且与按快速计算频率的划桨补偿结合在一起（见等式（10）），以提供组合的速度旋转和划桨补偿算法，其能按单一的频率来计算。

步骤320中，组合的速度旋转和划桨补偿按单一的快速计算频率l被计算。组合补偿算法随后被用来确定在特定的时间间隔

内（诸如一个计算机时钟周期）的系统中的速度变化。该算法可以在慢速周期率m（诸如系统时钟周期）的任意数量的子区间l上计算。在进一步的实施方式中，该算法可以被扩展来包括更高阶分量，以提供在等式（1）的另外的项中反映的额外的计算以及准确性。例如通过用

代替等式14中的α(t)，第二项在算法内被定义。另外通过用

代替等式14中的α(t)，第三项也被定义。

其它实施方式也在所附权利要求的范围和精神中。例如，由于软件的性质，上文所描述的功能可以使用由处理器、硬件、固件、硬布线设备、或以上这些设备的任意组合所执行的软件来实现。实现功能的这些特征也可以物理地被置于各个位置中，包括被分布，以便于部分功能在不同的物理位置实现。在此所用的包括权利要求中所用的，在由“至少一个”开头的项目列表中使用的“或”，表示分隔的列表，例如列表“A、B、或C中的至少一个”则意味着A或B或C或AB或AC或BC或ABC（即A和B和C）。

此外，可公开多于一个的发明。

Claims (12)

1.一种用于确定惯性导航系统中的组合的速度旋转补偿和划桨补偿的系统，所述系统包括：

一组陀螺仪，其被配置来测量旋转角速率并提供具有来自三个正交轴的分量的测量的旋转角速率；

一组加速度计，其被配置来测量比力并提供具有来自三个正交轴的分量的测量的比力；以及

处理器，其被耦合到所述一组陀螺仪和所述一组加速度计，并被配置为按单一的计算频率计算第一组合的速度旋转补偿和划桨补偿，所述处理器被配置为：

计算在第一个时间间隔期间的瞬时旋转角速率与测量的比力变化的第一矢量积，其中所述瞬时旋转角速率是：在所述第一个时间间隔之前的第二个时间间隔期间测量的旋转角速率、在所述第一个时间间隔期间测量的旋转角速率的变化的一部分、以及在所述第二个时间间隔期间测量的旋转角速率的变化的一部分的累加和；以及

将所述第一矢量积和第二矢量积相加，其中所述第二矢量积是在所述第二个时间间隔期间的比力变化的一部分和所述第一个时间间隔期间测量的角速率的变化的矢量积，

其中所述第一个时间间隔和所述第二个时间间隔中的每一个是所述单一的计算频率的一个周期。

2.根据权利要求1所述的系统，其中所述速度旋转补偿具有均等于零的初始的旋转角速率和比力。

3.根据权利要求1所述的系统，其中所述处理器还被配置为：

确定所述第一个时间间隔内的由于所述组合的速度旋转补偿和划桨补偿导致的速度变化；

其中所述速度变化等于——具有速度旋转变化的所述第一组合的速度旋转补偿和划桨补偿与第二组合的速度旋转补偿和划桨补偿的累加和；

其中利用来自于所述第二个时间间隔和第三个时间间隔的数据来计算所述第二组合的速度旋转补偿和划桨补偿；以及

所述第三个时间间隔在所述第二个时间间隔之前。

4.根据权利要求3所述的系统，其中所述处理器还被配置为：将所述惯性导航系统中的速度变化计算为在第一个计算机时钟周期频率期间计算的系统速度以及所述第一个时间间隔内的所述速度旋转补偿和划桨补偿的变化的累加和。

5.根据权利要求1所述的系统，其中所述单一的计算频率快于计算机时钟周期频率。

6.根据权利要求5所述的系统，其中所述速度旋转补偿是：在所述第一个时间间隔中的测量的旋转角速率与所述第一个时间间隔期间的测量的比力的第三矢量积减去所述第二个时间间隔期间的测量的旋转角速率与所述第二个时间间隔期间的测量的比力的第四矢量积所得到的差的累加和与二分之一的乘积，其中所述累加和是在由一分式所限定的数量的子区间上计算的，所述分式为所述单一的计算频率除所述计算机时钟周期频率。

7.根据权利要求1所述的系统，其中所述组合的速度旋转补偿和划桨补偿被解析地表示为： $({\mathrm{\α}}_{l-1}+\frac{1}{2}\mathrm{\Δ}{\mathrm{\α}}_1+\frac{1}{12}\mathrm{\Δ}{\mathrm{\α}}_{l-1})\×\mathrm{\Δ}{v}_l+\frac{1}{12}\mathrm{\Δ}{v}_{l-1}\×\mathrm{\Δ}{\mathrm{\α}}_l,$ 其中α_l-1是在所述第一个时间间隔之前的第二个时间间隔期间测量的旋转角速率，Δα_l是在所述第二个时间间隔期间测量的旋转角速率的变化，Δα_l-1是在所述第二个时间间隔期间测量的旋转角速率的变化，Δv_l是在所述第一时间间隔期间测量的比力的变化，以及Δv_l-1是在所述第二时间间隔期间测量的比力的变化。

8.一种用于按单一的计算频率计算惯性导航系统的组合的速度旋转补偿和划桨补偿的装置，所述装置包括：

第一测量工具，其用于测量绕着三个正交轴的旋转角速率并提供具有来自三个正交轴的分量的测量的旋转角速率；

第二测量工具，其用于测量比力并提供具有来自三个正交轴的分量的测量的比力；以及

处理工具，其用于计算速度旋转补偿和划桨补偿的累加和，其中所述累加和是以下两项矢量积的累加和：

第一个时间间隔中的瞬时旋转角速率与测量的比力的变化的第一矢量积，其中所述瞬时旋转角速率是在所述第一个时间间隔之前的第二个时间间隔期间测量的旋转角速率、在所述第二个时间间隔期间测量的旋转角速率的变化的一部分、以及在所述第一个时间间隔期间测量的旋转角速率的变化的一部分的累加和；以及

在所述第二个时间间隔内的比力的变化的一部分与所述第一个时间间隔期间测量的角速率的变化的第二矢量积。

9.根据权利要求8所述的装置，其中所述第一时间间隔和所述第二时间间隔中的每一个是所述单一的计算频率的一个周期。

10.根据权利要求9所述的装置，其中所述单一的计算频率要快于计算机时钟周期频率。

11.根据权利要求10所述的装置，其中所述速度旋转补偿是：在所述第一个时间间隔期间测量的旋转角速率与在所述第一个时间间隔期间测量的比力的第三矢量积减去在所述第二个时间间隔期间测量的旋转角速率与在所述第二个时间间隔期间测量的比力的第四矢量积所得到的差的累加和与二分之一的乘积，其中所述累加和是在由一分式所限定的数量的子区间上计算的，所述分式为所述单一的计算频率除所述计算机时钟周期频率。

12.一种非短暂的处理器可读的介质，包括被配置成导致处理器执行如下操作的指令：

计算在第一个时间间隔中的瞬时旋转角速率与测量的比力的变化的第一矢量积，其中所述瞬时旋转角速率是：在所述第一个时间间隔之前的第二个时间间隔期间测量的旋转角速率、在所述第二个时间间隔期间测量的旋转角速率的变化的一部分、以及在所述第一个时间间隔期间测量的旋转角速率的变化的一部分的累加和；

将所述第一矢量积和第二矢量积相加，其中所述第二矢量积是在所述第二个时间间隔内的比力的变化的一部分和在所述第一个时间间隔期间测量的角速率的变化的矢量积；以及

按单一的计算频率计算所述第一矢量积和所述第二矢量积，其中所述第一个时间间隔和所述第二个时间间隔中的每一个是所述单一的计算频率的一个周期；

其中所述测量的旋转角速率由一组陀螺仪提供，所述一组陀螺仪耦合到所述处理器并被配置来测量具有来自三个正交轴的分量的旋转角速率；以及

其中所述测量的比力由一组加速度计提供，所述一组加速度计耦合到所述处理器并被配置来测量具有来自三个正交轴的分量的比力。

Images