惯性单元的校准方法

惯性单元的校准方法
【专利摘要】本发明涉及一种基于简化模式的校准方法，简化模式允许使用不变卡尔曼滤波器的处理，在简化模式下，导航方程中涉及的每个速度表现在工作参考系(Rt)中，工作参考系相对于惯性参考系(Ri)平移，并且工作参考系的原点沿着惯性参考轨迹移动，惯性参考轨迹的运载器被认为接近(用于利用已知位置处的太阳进行校准的地理原点、用于运动中校准的GPS轨迹等)。简化方法包括重复以下用于估计移动运载器状态(P)的步骤：?传播(PROP)，根据先前估计的状态、惯性传感器测量值以及运载器轨迹(P)的理论信息，确定估计的当前状态，?使用运载器轨迹(P)的理论信息更新(MAJ)估计的状态。在传播和更新步骤期间仅估计传感器的确定的不确定性(偏差/漂移/比例因子误差等)。然后，扰动步骤(PERT)允许通过最优化方法来包括简化模式下忽略的值以及这些值的估计值。
【专利说明】
惯性单元的校准方法
技术领域
[0001] 本发明设及惯性导航单元、W及包括运些单元的运载设备校准方法的领域。
【背景技术】
[0002] 根据现有技术已知用于校准包括惯性单元的运载设备的许多方案。
[0003] 为了执行校准，根据近似的遵循轨迹先验性地开始校准。例如，如果在地理参考系 中，则地球表面的已知点处的停止的运载设备所遵循的轨迹为零运动，而如果在附属于地 球中屯、的惯性参考系中，则所遵循的轨迹为绕极轴的旋转运动。该轨迹不精确(运载器经受 电机的振动、当乘客进入和离开时的垂直运动等），但总是接近现实。已知的校准方案(也被 新方法使用)在于将该先验轨迹视为源于虚拟传感器的测量。例如，针对安装有惯性测量单 元(IMU)的停止的运载器，认为存在如下速度传感器，该速度传感器在地理参考系中总是给 出响应"零"，而在处于其他参考系中的情况下给出先验轨迹的速度。还可W认为存在如下 位置传感器，该位置传感器在处于地理参考系中的情况下总是给出响应"零"，而在其他参 考系中给出先验轨迹的位置。还可W同时使用运两个传感器（因为它们不存在）。实际上与 我们自身对轨迹的不确定相对应的测量噪声，归因于该虚拟传感器。例如，总是在固定的运 载设备的情况下，如果选择了通过使用零位移信息进行校准，则将增加标准偏差为几厘米 的噪声(例如，如果正在装载或卸载汽车的后备箱，则正在被校准的装置实际上可能远离先 验轨迹而移动了几厘米）。通常使用卡尔曼型滤波器来整合该(虚拟)测量。
[0004] 运种卡尔曼滤波器一般执行随时间重复的传播步骤和更新步骤:传播步骤计算运 载设备的估计状态W及关于该估计状态的不确定性;更新步骤整合轨迹的先验信息。
[0005] 然而，运些传播和更新步骤产生了妨碍使用已知的卡尔曼滤波器的非线性方程。
[0006] 为了避免运种限制，提出了使用被称为"扩展卡尔曼滤波器"化KF)的卡尔曼滤波 器，其中，与传播和更新步骤相对应的非线性方程中使用的矩阵，通过与所选择的状态向量 相关联的向量空间的给定点处的一阶展开，来形成线性化的对象。
[0007] 文献EP 1 862 764提出了基于位移或低速信息的整合，使用扩展卡尔曼滤波器的 校准方法。纵观该方法，根据惯性数据及运载设备的朝向的估计值，来计算运载设备的轨 迹，然后利用扩展卡尔曼滤波器来整合速度或零位移(噪声附近)的(虚拟)观测。
[000引运种校准方法仅在用来线性化扩展卡尔曼滤波器的方程的一阶展开的点足够精 确、并且由该展开引起的估计误差实际上必须低（最多几度）的情况下才起作用。如果线性 化点不是足够精确，则在传播和更新步骤期间通过扩展卡尔曼滤波器进行的计算，可能会 适时导致错误估计。
[0009] 已提出两个初步步骤，来确定在使用用来校准运载设备的扩展卡尔曼滤波器期间 的线性化的精确点：
[0010] -研究垂直面，在此期间，在考虑加速度计观测重力的情况下估计垂直面（大约2 秒），
[0011] -研究大致方向，在此期间，通过基于简化模型的滤波处理来估计方向（大约1分 钟）。
[0012] 运些初步步骤不仅执行起来时间长且复杂，而且无法保护惯性测量单元免于未来 的分歧(滤波器的估计值远离真实状态）。

【发明内容】

[0013] 因此，本发明旨在使得能够比所引用的现有技术更加精确且快速地校准运载设 备。
[0014] 针对惯性参考系中的包括惯性测量单元的可移动运载设备，提出了一种校准方 法，所述惯性测量单元包括多个惯性传感器和至少一个不变卡尔曼滤波器，所述方法包括 重复W下用于估计所述运载设备的状态的步骤，所述状态表示所述运载设备的速度和朝 向：
[0015] -传播步骤，根据先前估计的状态和惯性传感器的测量值，确定估计的当前状态，
[0016] -更新步骤，利用所述运载设备的轨迹的先验信息，更新估计的状态，
[0017] 其中，所述惯性传感器(例如加速度计和巧螺仪或巧螺测试仪)测量在单元的参考 系中表现出的加速度和角速度。运些数据出现在传播步骤中：通过整合由巧螺仪或巧螺测 试仪测量的角速度(根据朝向更新）、W及由加速度计测量的加速度(速度的更新），来获得 所估计的当前状态(朝向、速度和可选位置）。
[0018] 首先通过使用估计的朝向将运些加速度投影到工作参考系中，然后当加速度计无 法区分重力和加速度时，从运些加速度中减去重力。如果目的在于估计位置，则能够整合所 得的速度，该操作还形成了部分传播。
[0019] 更新步骤是由虚拟传感器给出的速度或位置信息的整合。该信息（源于轨迹的先 验)被称为"零速度信息"或"零位移信息"。该步骤根据速度或位置信息更新所有数据(朝 向/速度/位置）。实际上，零速度/位移信息包含朝向的隐含信息。
[0020] 如果最初假定朝向被很好地估计，贝阳日速度计在其参考系中测量等于真实加速度 和重力的总和的物理量。在单元被停止的情况下，真实加速度很低，并且测量几乎仅包含重 力。在传播步骤期间，利用估计的朝向矩阵，将测量的加速度投影在惯性参考系（或工作参 考系）中。所获得的量为此时在惯性参考系（或工作参考系）中表现的重力。减去重力（已 知），并且得到非常接近零的加速度。随时间整合该量，并且该量导致了也是非常低的速度。
[0021] 然而，如果假定估计的朝向相对于现实向东倾斜了几度，则当使用该错误的朝向 估计将加速度信息投影在惯性参考系中时，获得的向量与重力不一致。当减去重力（已知） 时，得到指向西的非零向量。因此，在随时间整合之后，导致了指向西的非零速度。因此，不 良的朝向影响了速度估计。相反地，更新时的非零速度被解释为朝向误差。运就是卡尔曼滤 波器所进行的处理(扩展、不变或其他）。因此，更新仅需要获取"零速度"或"零位移"（虚拟） 信息，而无需真实的速度测量或朝向信息。该(虚拟)测量足W得出单元的朝向。
[0022] 根据所提出的方法的一方面，为了允许不变卡尔曼滤波器的处理，使用简化模式， 其中，更新方程中出现的每个速度表现在工作参考系中，所述工作参考系相对于惯性参考 系平移，并且所述工作参考系的原点沿着惯性参考轨迹移动，所述惯性参考轨迹的运载设 备被认为接近，在传播和更新步骤期间未估计传感器的偏差。
[0023] 原点可W是地理参考系的原点（例如，用于地面上已知位置处的校准）；该原点可 W对应于GI^轨迹(例如，用于飞行中校准）。
[0024] 而且，有利地，W如下方式并行执行影响估计值的附加量被估计的处理：
[0025] -在每个瞬间，递归计算相对于由简化模式提供的估计值的附加量的微分。
[0026] -通过最优化估计的质量标准，来计算附加量。
[0027] 所采用的计算微分，将运载设备的估计状态表现为附加量的精细函数。
[0028] 根据另一方面，还提出了一种能够在惯性参考系中移动的惯性测量单元，所述惯 性测量单元包括：
[0029] -多个惯性传感器，
[0030] -不变卡尔曼滤波器，
[0031 ]所述单元包括执行所提出的校准方法的嵌入式计算机。
【附图说明】
[0032] 通过W下纯粹示例性和非限制性、并且必须结合附图考虑的描述，本发明的其他 特征、目的和优点将会显现，其中：
[0033] -图1例示了根据本发明的实施例的惯性测量单元。
[0034] -图2例示了根据本发明的实施例的惯性测量单元。
[0035] -图3A和图3B分别是扩展卡尔曼滤波器和不变卡尔曼滤波器的示意图。
[0036] -图4例示了根据本发明的实施例的惯性测量单元的校准方法的步骤。
[0037] -图5A和图5B是通过执行根据本发明的实施例的方法、并通过执行已知方法获得 的方向校准曲线。
[0038] -图6A和图6B是通过执行本发明的方法获得的比较方向误差曲线，图6B是图6A更 近的视图。
[0039] -图7通过比较例示了使用简化校准模式和完整模式的处理、W及传统类型的处理 随时间的进程。
[0040] 在所有附图中，相似的元件具有相同的附图标记。
【具体实施方式】
[0041] 参照图1，运载设备P能够在由惯性参考系Ri定义的预定惯性参考系中移动。
[0042] 惯性参考系Ri的示例为W地球中屯、为中屯、的参考系，该参考系的轴Z指向北极，轴 X指向在时间t = 0时的格林威治子午线与赤道的交叉点(然后，定义的点将由于地球的旋转 而在我们的参考系中移动），并且轴y指向向量Z X X的方向，X指定向量积）。
[0043] 运载设备化的参考系相对于单元固定。例如，最常使用的轴指向单元的前方、右方 和基底。参考系的原点为单元的固定点。地理参考系Rg的原点为单元的位置，并且由指向北 方、西方和上方的Ξ个轴组成。
[0044] 还已知称为"工作参考系"的参考系化，参考系化是相对于惯性参考系Ri平移的参 考系（即，利用惯性参考系Ri的轴来校准轴），并且参考系Rt的原点C是地理参考系Rg的原 点。
[0045] 参照图2,运载设备P包括惯性测量单元IN。
[0046] 惯性测量单元IN包括多个惯性传感器CV、C0和计算机，该计算机被构造为操作不 变卡尔曼滤波器κ。
[0047] 惯性测量单元IN应当相对于工作参考系化固定。
[0048] 多个传感器包括至少一个加速度传感器CV(例如加速度计）和至少一个角速度传 感器C0(例如巧螺仪或巧螺测试仪）。
[0049] 不变卡尔曼滤波器K是运载设备的状态的递归估计器，该状态表示该运载设备的 速度和朝向。
[0050] 不变卡尔曼滤波器K尤其被构造为，利用预先定义的传播方程(指的是传播），根据 运载设备的先前估计状态，来确定运载设备的估计的当前状态。
[0051] 根据惯性传感器的测量值预先确定传播方程。
[0052] 不变卡尔曼滤波器K还被构造为，利用预先定义的更新方程来更新估计的当前状 态，在该更新方程中，出现轨迹的先验信息(例如低速信息）。
[0053] 不变卡尔曼滤波器K还被构造为，使得在工作参考系中表现出在更新方程中出现 的每个速度。
[0054] 惯性测量单元IN例如能够利用GNSS/GPS接收器来构建其工作参考系。
[0055] 从校准过程的第一瞬间开始重复现在将描述的步骤。
[0化6] 第一估计值藏为任意值。假定已确定状态X的先前估计值(表示为iY.s)、W及表示 该估计值的不确定性的矩阵P。
[0化7] 参照图3A，通过已知的扩展卡尔曼滤波器对当前估计值^>*的确定，依赖于先前估 计值:f (利用传播步骤）、W及在当前迭代期间获取的测量值(利用线性化和更新步骤）。
[0化引齡'的计算设及反过来依赖于f的值的增益矩阵L。
[0059] 运里，引入工作参考系化W及忽略传感器的偏差的简化模型，使得能够使用不变 滤波器。
[0060] 参照图3B，与普通的扩展卡尔曼滤波器不同，不变卡尔曼滤波器利用特别选择的 方程(下文中将详细描述运些方程），计算状态的当前估计值蒙*。
[0061] 传统上，可W通过速度V和朝向Τ(Τ为旋转矩阵或四元数)或通过W下形式的位移 矩阵X来表示运载设备的状态：
[0062]
[0063] 当前，可W使用其他表示方法。
[0064] 运里，很清楚名称"位移矩阵"并不一定例示位移。该名称源于当引入位移矩阵用 于机器人问题时所实现的用途(位移矩阵包含朝向并清楚地表示位移）。
[0065] Ρ被称为状态X的估计不确定性。不变滤波器Κ的关注特性在于更新步骤中出现的 增益矩阵L不依赖于先前估计值象。
[0066] 运就构成了使得能够使用不变卡尔曼滤波器的简化系统(并因此构成了无需针对 垂直面和粗校准的研究步骤的收敛），从而具有W下特性：1)使用不平常的参考系来进入。 2)惯性传感器的误差(偏差/漂移/比例因子误差)的决定性部分未知。
[0067] 在实施例中，在传播方程中使用的重力被认为仅依赖于时间而不是位置，运就简 化了运些方程的解析计算。
[0068] 已知的精校准处理不设及简化模型。已知的精校准处理直接整合模型中的所有变 量，使得无法使用I邸F。
[0069]利用位移矩阵或者利用工作参考系的初始变量V和Τ(通过从位移矩阵中减去运些 变量），来表示用于校准的I邸F的更新步骤：
[0072] ·Κ谢Κ。是源于稍后定义的指数映射的函数，例巧
庚 中以是依赖于Ρ的矩阵。
[0073] ·Ρ遵循仅依赖于时间的方程。该方程通过在0和Ι3(大小为3X3的矩阵等式)周围 线性化由如：-Ρ-7Υ 郝?)'7'-7Υ I验证的方程来获得。
[0074] 通过e邱(U) =e邱m(D(u))，针对a居.破6定义指数映射exp,其中，expm指定了已知的 矩阵指数，并且其中，DO是主张大小为6X1的向量
、并且描绘大小为4X4的初等位 移矩時
的线性应用，A(xi)指定了例如针对任意向量《e肢3的反对称矩阵A(xi)u = xiXu, X为向重积。
[0075] 由于D(u)类型的矩阵具有特定形式，因此能够使用W下闭公式：
[0076]
[0077] 在实施例中，刚刚描述的计算被迭代为收敛(简化模式）。
[0078] 通过变型(完整模式），还能够通过下文中描述的方法来估计附加量(偏差/漂移/ 比例因子误差等）。
[0079] 当构成该简化的滤波器时，从关于其余参数(偏差/漂移/比例因子误差等）的零值 的校准过程的第一瞬间起，计算估计轨迹相对于运些参数的微分。估计的轨迹被表达为参 数的精细函数。选定估计轨迹的二次质量标准(例如，随时间估计的速度的标准平方和）。该 标准为容易最优化W获得参数值的参数(偏差/漂移/比例因子误差）的二次函数。即使估计 参数的数量增大，该处理也保留了针对简化系统构建的滤波器的收敛性。该最优化能够随 时间递归地进行，能够在完成校准的单个时间进行，或者甚至能够在校准期间的任意瞬间 进行。该选择对估计没有影响。在所有情况下，简化系统的滤波和完整系统的滤波同时进 行。
[0080] 在采用粗校准的处理中，也对简化系统（不同于运里提出的简化系统）执行粗校 准，但是截取是临时的：1分钟的粗校准之后进行精校准。运里，两个模型从开始到结束同时 作用:在每个瞬间，简化滤波器给出其估计值，然后，二次最优化计算完整状态(不进行卡尔 曼滤波)作为简化状态的扰动。因为矩阵P遵循仅依赖于时间的方程（IEKF)，所W能够在合 理的时间实现轨迹的微分计算。如果简化系统不是基于IEKF，则无法进行随后的对第一次 忽略的参数的估计步骤。
[0081 ]惯性测量单元的校准方法
[0082] 现在，将参照图4描述实现不变卡尔曼滤波器Κ的惯性测量单元IN(W及由此运载 设备P)的校准方法。
[0083] 在获取步骤ACQ中，利用关于轨迹的先验信息，来更新工作参考系Rt的轨迹。
[0084] 在传播步骤PROP中，卡尔曼滤波器K利用其惯性传感器C0、CV的测量值，根据先前 的估计状态来确定估计的当前状态。
[0085] 在更新步骤MAJ中，卡尔曼滤波器K利用零速度(或零位移）的虚拟观测值，更新运 载设备的估计状态。
[0086] 随时间重复获取步骤AC卵、传播步骤PROP和更新MAJ步骤。
[0087] 因此，校准方法包括几个连续迭代，每个新的迭代输入在先前迭代期间获得的更 新的估计状态、W及新的观测值(在该新的迭代的获取步骤期间获取）。
[0088] 还可W实现包括如下程序代码指令的计算机程序产品，当通过惯性测量单元中嵌 入的计算机执行该程序时，该程序代码指令用于执行上述方法的步骤。校准问题的一般形 式
[0089] 首先，将使用W下符号。
[0090]
[0091] 通过惯性参考系中的零速度观测来进行校准的已知构想如下。
[0092] 假设近似已知运载设备的位置。方法利用的该假设的结果在于，局部重力已知并 且仅为时间的函数。
[0093] 运动方程被表达为：
[0097] 其中，A(.)指定了与旋转向量相关联的反对称矩阵(通过关系V?,:，:《2e吸3，A(ui)U2 = UlXU2定义，其中，X为向量积）。
[0098] 速度的虚拟测量值(Wh为噪声)为：
[0099]
[0100] 将在正则区间
引入信息，其中，Vi是与先验轨迹关联(例如与 由于地球旋转而导致的先验轨迹关联)的运载设备P的期望速度(在惯性参考系中简单地表 达出通常在地理参考系中进行的零速度观测）。
[0101] 工作参考系中的运动方程的再形成
[0102] 该部分详细描述了在本发明的实施例中进行的传播和更新方程。
[0103] 工作参考系化(下文中也被参考为C)被相对于惯性参考系Ri平移地预先定义（即， 利用惯性参考系Ri的轴来校准工作参考系Rt的轴），并且工作参考系Rt的原点C是地理参考 系g的原点。
[0104] 引用W下符号：
[0105]
[0106] 在工作参考系化中如下表达运载设备P的运动方程：
[0110] 由于运载设备的位置XC对其他参数没有影响，因此可W删除该位置：
[0113] 如下，还修改零速度信息：
[0111]
[0112]
[0114]
[0115] 能够通过位移矩阵来表示由不变卡尔曼滤波器K估计的运载设备P的状态，位移矩 阵由地理参考系向工作参考系的通过矩阵、w及在工作参考系中表达的测量的运载设备的 速度构成，如下构建位移矩阵：
[0116]
[0117] 当然，作为变型，还可W公知地表示运载设备的状态，例如矩阵T和向量V。
[0118] 惯性测量值和重力信息被写入初等位移矩阵：
[0122] 运些方程被更加简单地重写为：
[0123] j = ；r.'' + g.^ + ；r."'
[0124] 观测函数也被简单地写为：
[0125]
[01%]不变滤波所基于的称为"左右同变性"的性质h如下。设0为任意位移矩阵（即，
的形式，其中，R为(3X3)的正交矩阵，U为任意向量联3)。得出：
[0127]
[0128] W上可W被解释为与观测值空间中的变量变化相关联的状态空间中的变量变化 (X被替换为《矮）。术语"不变"源于如下事实:滤波器被设计为对运种类型的再参数化不敏 感。
[0129] 适于先前动态的观测器如下：
[0130]
[0131] 更新步骤MAJ通常为如下形式：
[0132]
[0133] 其中，i为当前估计的位移矩阵，Κη为增益函数(也被称为增益），Υ为表示零速度的 虚拟观测值的向量，并且为通过更新产生的估计的位移矩阵。
[0134] 函数Kt对应于第t个零观测速度（因此对应于第t个卡尔曼滤波器的迭代），并且为 Kt(u) = e邱(Ltu)的形式，其中，expO指定了称为指数映射的函数。Kt为函数，Lt为称为"增 益矩阵"或更加简单地称为"增益"的矩阵。下面详细描述增益的计算处理。通过线性化由误 差变量巧=尤厂1验证的方程，来计算矩阵k。通过η验证的方程的计算如下：
[0135] 在传播步骤期间，已知X和爱的演化方程，并因此能够计算η的演化方程：
[0136]
[0137] 引入由X和《验证的方程并使用VC小的事实，得出了：
[013引 ? 二圳-η^-ψν
[0139] 运是误差变量的连续演化部分。当更新估计值愛时，误差变量间断地演化。在更新 估计值期间的误差变量的演化尤其简单：
[0140]
[0141] W如下方式进行该方程的线性化:指数映射主张向量股6并形成位移矩阵。尤其是， 零向量的图像为矩阵等式
[0142] 因此能够使向量胶6、ξ和ξ+(η = θ邱(ξ)，ri+ = e邱(ξ+))对应于误差变量，并且由ξ和 Γ验证的方程能够被写入并在0周围线性化。所得的方程设及增益矩阵
[0145] 其中
〇3为仅包含0的3X3矩阵，并且
[0146] 该方程为线性，并且利用黎卡提方程能够同时计算ξ的协方差Pt、W及用于最小化 Pt的增益
[0151] 其中，Q为噪^
均协方差矩阵，R为噪声Wh的协方差矩阵。
[0152] 比较校准结果
[0153] 下文中将通过已描述的校准方法(基于不变卡尔曼滤波器)获得的结果，与通过已 知的校准方法(基于已知的扩展卡尔曼滤波器并在已知单元中使用)获得的结果相比较。
[0154] 通过卡尔曼滤波器处理的变量为运载设备的朝向（姿态)及其速度。已知认为运载 设备的位置接近一百米。使用的传感器被认为不具有偏差，并且使用的观测值为零速度。已 知运载设备的垂直面接近30度，而其方向完全未知。
[0155] 图5A和图5B各自比较两个校准方法的分别收敛速度、W及由运两个方法提供的当 前方向估计。
[0156] 在图5A所示的示例中，W低频进行零速度信息的整合。观测出，在200秒之后，根据 本发明的校准方法(蓝色曲线)提供了非常接近真实值的方向估计，而由已知处理提供的估 计方向（绿色曲线)继续摆动。
[0157] 在讨论中的情形下，如果已知缔度，则能够预先计算在更新步骤中使用的增益。如 图5B所示，滤波器花费很小的计算时间并且能够W高频整合零速度信息。W下附图展示了 在与先前相同的情形下，该选择的结果。运里，利用所提出的方法的收敛比利用现有技术的 方法的收敛快。
[0158] 图6A和图6B研究了方向的先验估计对本发明的校准方法的结果的影响。
[0159] 根据[0,2π]的统一定律随机描绘方向估计值，然后算法正常工作240秒。
[0160] 图6Α示出了方向误差很快接近零，而与执行根据本发明的方法期间的初始化无 关。图6Β是图6Α的放大图。
[0161] 与简化模式并行执行的完整模式
[0162] 刚刚描述的简化模式被与简化模式并行执行的完整模式有利地完成。
[0163] 运由图7所例示，图7示出了执行简化校准模式和完整模式的处理、W及传统类型 的处理随时间的进程。
[0164] 该完整模式使得能够进行二次最优化的校准。
[0165] W如下方式估计影响估计值的附加量：
[0166] -在每个瞬间，递归计算相对于由简化模式提供的估计值的附加量的微分。
[0167] -通过最优化估计的质量标准，来计算附加量。
[0168] 能够在完成所述校准方法时进行通过最优化质量标准计算附加量。还能够在校准 方法的任何瞬间，继续进行或W简化方式进行附加量的计算。
[0169] 使用的微分计算将运载设备的估计状态表达为附加量的精细函数。
[0170] 通过比较，已知的处理连续地执行：
[0171] -研究垂直面的处理；
[0172] -系统状态的近似评估(粗校准）；
[0173] -通过利用扩展卡尔曼滤波来整合虚拟位移观测值和/或零速度，来校准完整系 统)。
[0174] 在所提出的情况下，通过二次最优化执行完整模式的示例如下：
[0175] 如果惯性传感器的测量值经历与附加量(偏差/漂移/比例因子误差等)关联的决 定性扰动，则运些测量值不像传统处理中被整合到卡尔曼滤波器中，所述附加量由向量0表 示，并且必须在校准期间评估。使用W下过程：
[0176] 针对任意给定值β，能够校正惯性测量值(减去由电流放大系数给出的、由比例因 子划分等的偏差漂移），然后将简化模式应用到校正后的测量值。所得的估计值是附加量的 函数因为β未知，所W不计算该量。
[0177] 选择量β的基准值齡，并且实际仅计算估计值。
[0178] 选择在简化模式下出现的量的任意初始值為。
[0179] 选择应用于轨迹估计的概率函数:/(兩(例，与(例...戈(约)
[0180] 随时间重复W下步骤：
[0181] -传播步骤(PROP)
[0182] 惯性测量值用来通过简化模型和矩阵Pt计算巧的传播步骤。在β〇附近线性化 由Μβ)验证的方程，w递归演变精细应用始 <:巧^《1巧），运是^口牌齡附近的一阶展开。 利用通过矩阵表示Axt。
[0183] -更新步骤(MAJ):
[0184] 通过在简化模型中描述的处理，使用轨迹的先验信息来更新Slip,,')。X, (Pi的更新 方程与X,货。)相同（具有不变卡尔曼滤波器的特性），并且用来更新一阶展开Axt。
[0185] -扰动步骤(P邸T):
[0186] 可W在完成校准的单个时间，在校准期间的不同瞬间，或者在适时的各步骤应用 该步骤。通过最优化来获得β的估计值房：居二。巧... .f,(例）。通过 .焉=心,(句或I的其他函数，给出状态X的精细估计值采，相当于Axt的第一阶。
[0187] 函数巧TW是二次的。运样，没有必要存储韦（属)或Axt的所有值。
[0188] 函数f可W设及随时间递归计算的附加量。
[0189] 例如可W通过如下方程来定义函数f:
[0190]
[0191] 其中，通过简化模式给出St，并且符号巧旨定了矩阵转置。
[0192] 下文中使用的函数f的另一选择示例为：
[0193]
[0194] 可W在完成校准的单个时间，在校准期间的不同瞬间，或者在校准期间持续进行 名的计算。
[01巧]可W采用递归最优化处理来计算名。
[0196] 通过假定历=名，针对其余校准，可W选择性地将估计值^用作新的基准值。
[0197] 通过假定^ (Λ)=-节(居)，针对其余校准，精细估计值.?, 可W选择性地用作简化 模型中的最终估计值。
[0198] 下面描述执行方法的示例：
[0199]
曼大小为6Χ 1的向量，其包含Ξ个巧螺仪或巧螺测试仪的漂移(&)W及 Ξ个加速度计的偏差(&)。
[0200] 简化模式的变量为朝向和速度，并且能够被写入位移矩P
[0201] 选择β〇 = 0作为偏差和漂移的基准值。因此使用惯性传感器的测量值，而无需简化 模式中的修改。
[0202]如果考虑由β表示的偏差和漂移，则估计值
遵循W下传 播方程，将仅计算该估计值在基准值阶附近的对第一阶的近似：
[0206] 其中，expO是主张大小为6Χ 1的向量并形成位移矩阵的指数映射。β是W下初等 位移矩阵：
[0207]
[0208] 通过简化模式给出增益矩阵k。
[0209] 将估计值写入尤(片)的第一阶的简单方式为：
[0210]
[02川其中，DO是主张大小为6 XI的向量
阵形成大小为4X4的初等位移矩阵
的线性应用。矩阵Zt的大小为6 X 6，并且验证W下传播方程：
[0212]
[0213] W及W下更新方程：
[0214]
[0215] 其中，通过简化模式给出U，通过简化模式给出位移矩阵义和艇，并通过简化模式 给出旋转矩阵
定义矩阵adi( ω t和ft为惯性传感器的测量 值），并且通过
皂义矩阵Ad^i，函数V主张大小 为4X4的位移矩阵(运里为矩巧
，并形成位于右上方的大小为3 XI的向量 (运里为^)。函数Dexp是W下理解中的指数映射的导数：
[0216] θχρ(ξ+ιι) = [I4+D化e邱（ξ)ιι) ]θχρ(ξ)
[0217]其中，ξ和u是大小为3的向量，u的规格小于1，?4是大小为4的单位矩阵，并且Dexp (。是大小为6X6的矩阵。
[0218]通过W下公式定义所选择的概率函数：
[0222] 因此，无需存储整个轨迹的信息，而只需通过展开f的表达式，来写入 /(例=/?、片+/? + ￡'(.)"."。舶并支持矩阵51和52。
[0223] 因此，完整算法(简化模式和完整模式)如下：
[0224] 偏差和漂移的基准值为β〇 = 0。^能够依赖于情形的任意方式初始化初始估计值 点(0)和协方差矩阵F(表示不确定性）。表示对之（片)的一阶展开的矩阵Zt被初始化为Ζο = 〇6Χ6。矩阵Si和S2被初始化为Si = 〇6Χ6和S2 = 〇6Χ1。
[0225] 然后，贯穿校准重复W下步骤：
[02%]-传播步骤(PROP):
[0227]不确定性的传播：
[0240] -阶展开的更新：
[0244] -扰动步骤(P邸Τ):
[0245] 偏差和漂移的估计值：
[0246] β是线性单元二--靴解。
[0247] 简化模式的量的精细估计值：
[024引
[0249] 函数exp可W被与第一阶等效的其他函数替代。
[0250] 其他实施例
[0251] 为了提高处理的鲁棒性和性能，所述方法可W被并行使用几次，在不同的朝向初 始化，然后可W选择一个或更多个所得的估计值。可W使用不同的选择标准。
[0252] 为了不增大与传播步骤相关联的计算负荷，由龙和Zt验证的方程可W表达为一般 形式的类型：
[0259] 在传播步骤的单个时间计算方程（1)、（2)、（3)和(4)，在开始更新步骤时，将方程 (5)和(6)应用于与并行操作的每个滤波器相关联的量I和Z。针对所有滤波器使用相同的增 益 ΓΤΠ· 〇
[0260] 通过针对每个滤波器计算可能性标准、然后选择最佳结果，来进行估计值的选择。 该标准例如可W是随时间的速度估计值的二次函数。同样地，可W基于相干标准使用几个 估计值，例如二者(或更多者)给出最接近的结果。然后，选择运些估计值中的一个或任意函 数(例如平均值)作为最终估计值。
[0261] 此外，通过利用虚拟位置观测值替换虚拟速度观测值，来执行描述的所有处理。简 化状态还包含位置，并且置于如下形式的矩阵中：
[0265] W与针对零速度观测相同的方式执行其余计算。
[0266] 通过另一变型，可W通过W任意其他方式表示数据，来进行描述的所有处理。例如 可W通过四元法或欧拉角W及所有的变化计算，来参数化朝向。
[0267] 同样地，可W通过除了提出的处理之外的任意处理，选择增益函数Κη(.)。例如，可 W将无损卡尔曼滤波器类型的过程应用于非线性误差方程，可W利用模拟最优化的恒定增 兴绝 ΓΤΠ- 〇
[0268] 此外，可W将任意初始化处理添加到校准过程的开始。例如可W使用加速度计来 捜索垂直面。
[0269] 描述的处理可W被整合到更广泛的架构中，例如组合扩展卡尔曼滤波器、一组卡 尔曼滤波器等。
[0270] 此外，如果惯性测量单元没有位于与位置信息所指的点相同的位置，而该点在单 元的参考系中固定且已知（例如如果GPS接收器远离单元），则通过利用已知位置(在GPS和 单元的情况下分开两个接收器的杠杆臂)替换零，根据零位移的虚拟观测值，能够应用与校 准相同的过程。
【主权项】
1. 一种用于校准运载设备(P)的方法，所述运载设备在惯性参考系(Ri)中移动并且包 括惯性测量单元（IN)，所述惯性测量单元包括多个惯性传感器(RV、RO)和至少一个不变卡 尔曼滤波器，所述方法包括重复以下用于估计所述运载设备(P)的状态的步骤，所述状态表 示所述运载设备的速度和朝向： -传播步骤(PROP)，根据先前估计的状态和所述惯性传感器的测量值，确定估计的当前 状态， -更新步骤(MAJ)，利用所述运载设备(P)的轨迹的先验信息，更新估计的状态， 所述方法的特征在于，为了允许至少一个不变卡尔曼滤波器的处理，实施简化模式，在 所述简化模式下，在传播和更新步骤期间不估计所述传感器的偏差，更新方程中出现的每 个速度表现在工作参考系(Rt)中，所述工作参考系相对于所述惯性参考系(Ri)平移，并且 所述工作参考系的原点沿着惯性参考轨迹移动，所述惯性参考轨迹的所述运载设备被认为 接近。2. 根据前述权利要求所述的方法，其特征在于，所述传播方程产生仅依赖于时间的重 力变量。3. 根据前述权利要求中的一项所述的方法，其特征在于，所述惯性参考轨迹由所述惯 性传感器未测量的先验信息构建。4. 根据前述权利要求中的一项所述的方法，其特征在于，在所述简化模式下，所述更新 方程中出现的每个位置表现在工作参考系(Rt)中，所述工作参考系相对于所述惯性参考系 (Ri)平移。5. 根据前述权利要求中的一项所述的方法，其特征在于，还并行执行处理，在所述处理 中，以如下方式估计影响估计值的附加量： -在每个瞬间，递归计算相对于由所述简化模式提供的估计值的所述附加量的微分， -通过最优化估计值的质量标准，来计算所述附加量。6. 根据权利要求5所述的方法，其中，在完成所述校准方法时，进行通过最优化质量标 准对所述附加量的计算。7. 根据权利要求5所述的方法，其中，持续进行，或在所述校准方法的一个或更多个瞬 间进行通过最优化质量标准对所述附加量的计算。8. 根据前述权利要求中的一项所述的校准方法，其中，在简化模式下重复执行所述传 播和更新步骤(MAJ)之前计算一系列增益，每个增益出现在所述更新步骤的迭代中。9. 根据前述权利要求中的一项所述的方法，其特征在于，所述工作参考系（Rt)的原点 为地理参考系的原点。10. 根据权利要求1至8中的一项所述的方法，其特征在于，所述工作参考系的原点遵循 GPS轨迹。11. 一种惯性测量单元（IN)，其能够在惯性参考系（Ri)中移动，所述惯性测量单元包 括： -多个惯性传感器(cv、co)， -至少一个不变卡尔曼滤波器， 所述单元的嵌入式计算机，所述嵌入式计算机重复进行以下用于估计运载设备(P)的 状态的步骤，所述状态表示所述运载设备的速度和朝向： ?传播步骤(PROP)，根据先前估计的状态和所述惯性传感器的测量值，确定估计的当 前状态， ?更新步骤(MAJ)，利用所述运载设备(P)的轨迹的先验信息，更新估计的状态， 所述惯性测量单元的特征在于，为了允许所述不变卡尔曼滤波器的处理，所述计算机 被构造为执行根据权利要求1至10中的一项所述的校准方法。12. -种计算机程序产品，其特征在于，所述计算机程序产品包括程序代码指令，当也 能够实现由惯性测量单元中嵌入的计算机执行的该程序时，所述程序代码指令用于执行根 据权利要求1至10中的一项所述的方法的步骤。
【文档编号】G01C25/00GK106068441SQ201480064042
【公开日】2016年11月2日
【申请日】2014年11月24日 公开号201480064042.X, CN 106068441 A, CN 106068441A, CN 201480064042, CN-A-106068441, CN106068441 A, CN106068441A, CN201480064042, CN201480064042.X, PCT/2014/75439, PCT/EP/14/075439, PCT/EP/14/75439, PCT/EP/2014/075439, PCT/EP/2014/75439, PCT/EP14/075439, PCT/EP14/75439, PCT/EP14075439, PCT/EP1475439, PCT/EP2014/075439, PCT/EP2014/75439, PCT/EP2014075439, PCT/EP201475439
【发明人】阿克塞尔·巴劳, 西佛·博纳伯
【申请人】萨基姆防卫安全, 工业方法的开发和研究公司