CN106068441B - 惯性单元的校准方法 - Google Patents
惯性单元的校准方法 Download PDFInfo
- Publication number
- CN106068441B CN106068441B CN201480064042.XA CN201480064042A CN106068441B CN 106068441 B CN106068441 B CN 106068441B CN 201480064042 A CN201480064042 A CN 201480064042A CN 106068441 B CN106068441 B CN 106068441B
- Authority
- CN
- China
- Prior art keywords
- estimated
- inertial
- reference frame
- vehicle
- state
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 80
- 238000012545 processing Methods 0.000 claims abstract description 9
- 230000001902 propagating effect Effects 0.000 claims abstract 3
- 238000005259 measurement Methods 0.000 claims description 39
- 230000008569 process Effects 0.000 claims description 23
- 238000004364 calculation method Methods 0.000 claims description 13
- 230000036962 time dependent Effects 0.000 claims 1
- 239000011159 matrix material Substances 0.000 description 56
- 238000006073 displacement reaction Methods 0.000 description 30
- 239000013598 vector Substances 0.000 description 23
- 230000001133 acceleration Effects 0.000 description 10
- 230000005484 gravity Effects 0.000 description 10
- 238000001914 filtration Methods 0.000 description 6
- 238000005457 optimization Methods 0.000 description 6
- 230000001419 dependent effect Effects 0.000 description 4
- 230000010354 integration Effects 0.000 description 4
- 238000013507 mapping Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012887 quadratic function Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000010200 validation analysis Methods 0.000 description 2
- 101100044298 Drosophila melanogaster fand gene Proteins 0.000 description 1
- 230000003321 amplification Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000011088 calibration curve Methods 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
- Automation & Control Theory (AREA)
Abstract
本发明涉及一种基于简化模式的校准方法,简化模式允许使用不变卡尔曼滤波器的处理,在简化模式下,导航方程中涉及的每个速度表现在工作参考系(Rt)中,工作参考系相对于惯性参考系(Ri)平移,并且工作参考系的原点沿着惯性参考轨迹移动,惯性参考轨迹的运载器被认为接近(用于利用已知位置处的太阳进行校准的地理原点、用于运动中校准的GPS轨迹等)。简化方法包括重复以下用于估计移动运载器状态(P)的步骤:‑传播(PROP),根据先前估计的状态、惯性传感器测量值以及运载器轨迹(P)的理论信息,确定估计的当前状态,‑使用运载器轨迹(P)的理论信息更新(MAJ)估计的状态。在传播和更新步骤期间仅估计传感器的确定的不确定性(偏差/漂移/比例因子误差等)。然后,扰动步骤(PERT)允许通过最优化方法来包括简化模式下忽略的值以及这些值的估计值。
Description
技术领域
本发明涉及惯性导航单元、以及包括这些单元的运载设备校准方法的领域。
背景技术
根据现有技术已知用于校准包括惯性单元的运载设备的许多方案。
为了执行校准,根据近似的遵循轨迹先验性地开始校准。例如,如果在地理参考系中,则地球表面的已知点处的停止的运载设备所遵循的轨迹为零运动,而如果在附属于地球中心的惯性参考系中,则所遵循的轨迹为绕极轴的旋转运动。该轨迹不精确(运载器经受电机的振动、当乘客进入和离开时的垂直运动等),但总是接近现实。已知的校准方案(也被新方法使用)在于将该先验轨迹视为源于虚拟传感器的测量。例如,针对安装有惯性测量单元(IMU)的停止的运载器,认为存在如下速度传感器,该速度传感器在地理参考系中总是给出响应“零”,而在处于其他参考系中的情况下给出先验轨迹的速度。还可以认为存在如下位置传感器,该位置传感器在处于地理参考系中的情况下总是给出响应“零”,而在其他参考系中给出先验轨迹的位置。还可以同时使用这两个传感器(因为它们不存在)。实际上与我们自身对轨迹的不确定相对应的测量噪声,归因于该虚拟传感器。例如,总是在固定的运载设备的情况下,如果选择了通过使用零位移信息进行校准,则将增加标准偏差为几厘米的噪声(例如,如果正在装载或卸载汽车的后备箱,则正在被校准的装置实际上可能远离先验轨迹而移动了几厘米)。通常使用卡尔曼型滤波器来整合该(虚拟)测量。
这种卡尔曼滤波器一般执行随时间重复的传播步骤和更新步骤:传播步骤计算运载设备的估计状态以及关于该估计状态的不确定性;更新步骤整合轨迹的先验信息。
然而,这些传播和更新步骤产生了妨碍使用已知的卡尔曼滤波器的非线性方程。
为了避免这种限制,提出了使用被称为“扩展卡尔曼滤波器”(EKF)的卡尔曼滤波器,其中,与传播和更新步骤相对应的非线性方程中使用的矩阵,通过与所选择的状态向量相关联的向量空间的给定点处的一阶展开,来形成线性化的对象。
文献EP 1 862 764提出了基于位移或低速信息的整合,使用扩展卡尔曼滤波器的校准方法。纵观该方法,根据惯性数据及运载设备的朝向的估计值,来计算运载设备的轨迹,然后利用扩展卡尔曼滤波器来整合速度或零位移(噪声附近)的(虚拟)观测。
这种校准方法仅在用来线性化扩展卡尔曼滤波器的方程的一阶展开的点足够精确、并且由该展开引起的估计误差实际上必须低(最多几度)的情况下才起作用。如果线性化点不是足够精确,则在传播和更新步骤期间通过扩展卡尔曼滤波器进行的计算,可能会适时导致错误估计。
已提出两个初步步骤,来确定在使用用来校准运载设备的扩展卡尔曼滤波器期间的线性化的精确点:
-研究垂直面,在此期间,在考虑加速度计观测重力的情况下估计垂直面 (大约2秒),
-研究大致方向,在此期间,通过基于简化模型的滤波处理来估计方向(大约1分钟)。
这些初步步骤不仅执行起来时间长且复杂,而且无法保护惯性测量单元免于未来的分歧(滤波器的估计值远离真实状态)。
发明内容
因此,本发明旨在使得能够比所引用的现有技术更加精确且快速地校准运载设备。
针对惯性参考系中的包括惯性测量单元的可移动运载设备,提出了一种校准方法,所述惯性测量单元包括多个惯性传感器和至少一个不变卡尔曼滤波器,所述方法包括重复以下用于估计所述运载设备的状态的步骤,所述状态表示所述运载设备的速度和朝向:
-传播步骤,根据先前估计的状态和惯性传感器的测量值,确定估计的当前状态,
-更新步骤,利用所述运载设备的轨迹的先验信息,更新估计的状态,
其中,所述惯性传感器(例如加速度计和陀螺仪或陀螺测试仪)测量在单元的参考系中表现出的加速度和角速度。这些数据出现在传播步骤中:通过整合由陀螺仪或陀螺测试仪测量的角速度(根据朝向更新)、以及由加速度计测量的加速度(速度的更新),来获得所估计的当前状态(朝向、速度和可选位置)。
首先通过使用估计的朝向将这些加速度投影到工作参考系中,然后当加速度计无法区分重力和加速度时,从这些加速度中减去重力。如果目的在于估计位置,则能够整合所得的速度,该操作还形成了部分传播。
更新步骤是由虚拟传感器给出的速度或位置信息的整合。该信息(源于轨迹的先验)被称为“零速度信息”或“零位移信息”。该步骤根据速度或位置信息更新所有数据(朝向/速度/位置)。实际上,零速度/位移信息包含朝向的隐含信息。
如果最初假定朝向被很好地估计,则加速度计在其参考系中测量等于真实加速度和重力的总和的物理量。在单元被停止的情况下,真实加速度很低,并且测量几乎仅包含重力。在传播步骤期间,利用估计的朝向矩阵,将测量的加速度投影在惯性参考系(或工作参考系)中。所获得的量为此时在惯性参考系 (或工作参考系)中表现的重力。减去重力(已知),并且得到非常接近零的加速度。随时间整合该量,并且该量导致了也是非常低的速度。
然而,如果假定估计的朝向相对于现实向东倾斜了几度,则当使用该错误的朝向估计将加速度信息投影在惯性参考系中时,获得的向量与重力不一致。当减去重力(已知)时,得到指向西的非零向量。因此,在随时间整合之后,导致了指向西的非零速度。因此,不良的朝向影响了速度估计。相反地,更新时的非零速度被解释为朝向误差。这就是卡尔曼滤波器所进行的处理(扩展、不变或其他)。因此,更新仅需要获取“零速度”或“零位移”(虚拟)信息,而无需真实的速度测量或朝向信息。该(虚拟)测量足以得出单元的朝向。
根据所提出的方法的一方面,为了允许不变卡尔曼滤波器的处理,使用简化模式,其中,更新方程中出现的每个速度表现在工作参考系中,所述工作参考系相对于惯性参考系平移,并且所述工作参考系的原点沿着惯性参考轨迹移动,所述惯性参考轨迹的运载设备被认为接近,在传播和更新步骤期间未估计传感器的偏差。
原点可以是地理参考系的原点(例如,用于地面上已知位置处的校准);该原点可以对应于GPS轨迹(例如,用于飞行中校准)。
而且,有利地,以如下方式并行执行影响估计值的附加量被估计的处理:
-在每个瞬间,递归计算相对于由简化模式提供的估计值的附加量的微分。
-通过最优化估计的质量标准,来计算附加量。
所采用的计算微分,将运载设备的估计状态表现为附加量的精细函数。
根据另一方面,还提出了一种能够在惯性参考系中移动的惯性测量单元,所述惯性测量单元包括:
-多个惯性传感器,
-不变卡尔曼滤波器,
所述单元包括执行所提出的校准方法的嵌入式计算机。
附图说明
通过以下纯粹示例性和非限制性、并且必须结合附图考虑的描述,本发明的其他特征、目的和优点将会显现,其中:
-图1例示了根据本发明的实施例的惯性测量单元。
-图2例示了根据本发明的实施例的惯性测量单元。
-图3A和图3B分别是扩展卡尔曼滤波器和不变卡尔曼滤波器的示意图。
-图4例示了根据本发明的实施例的惯性测量单元的校准方法的步骤。
-图5A和图5B是通过执行根据本发明的实施例的方法、并通过执行已知方法获得的方向校准曲线。
-图6A和图6B是通过执行本发明的方法获得的比较方向误差曲线,图6B 是图6A更近的视图。
-图7通过比较例示了使用简化校准模式和完整模式的处理、以及传统类型的处理随时间的进程。
在所有附图中,相似的元件具有相同的附图标记。
具体实施方式
参照图1,运载设备P能够在由惯性参考系Ri定义的预定惯性参考系中移动。
惯性参考系Ri的示例为以地球中心为中心的参考系,该参考系的轴z指向北极,轴x指向在时间t=0时的格林威治子午线与赤道的交叉点(然后,定义的点将由于地球的旋转而在我们的参考系中移动),并且轴y指向向量z×x的方向,×指定向量积)。
运载设备的参考系Rp相对于单元固定。例如,最常使用的轴指向单元的前方、右方和基底。参考系的原点为单元的固定点。地理参考系Rg的原点为单元的位置,并且由指向北方、西方和上方的三个轴组成。
还已知称为“工作参考系”的参考系Rt,参考系Rt是相对于惯性参考系Ri平移的参考系(即,利用惯性参考系Ri的轴来校准轴),并且参考系Rt的原点 C是地理参考系Rg的原点。
参照图2,运载设备P包括惯性测量单元IN。
惯性测量单元IN包括多个惯性传感器CV、CO和计算机,该计算机被构造为操作不变卡尔曼滤波器K。
惯性测量单元IN应当相对于工作参考系Rt固定。
多个传感器包括至少一个加速度传感器CV(例如加速度计)和至少一个角速度传感器CO(例如陀螺仪或陀螺测试仪)。
不变卡尔曼滤波器K是运载设备的状态的递归估计器,该状态表示该运载设备的速度和朝向。
不变卡尔曼滤波器K尤其被构造为,利用预先定义的传播方程(指的是传播),根据运载设备的先前估计状态,来确定运载设备的估计的当前状态。
根据惯性传感器的测量值预先确定传播方程。
不变卡尔曼滤波器K还被构造为,利用预先定义的更新方程来更新估计的当前状态,在该更新方程中,出现轨迹的先验信息(例如低速信息)。
不变卡尔曼滤波器K还被构造为,使得在工作参考系中表现出在更新方程中出现的每个速度。
惯性测量单元IN例如能够利用GNSS/GPS接收器来构建其工作参考系。
从校准过程的第一瞬间开始重复现在将描述的步骤。
第一估计值为任意值。假定已确定状态x的先前估计值(表示为)、以及表示该估计值的不确定性的矩阵P。
参照图3A,通过已知的扩展卡尔曼滤波器对当前估计值的确定,依赖于先前估计值(利用传播步骤)、以及在当前迭代期间获取的测量值(利用线性化和更新步骤)。
的计算涉及反过来依赖于的值的增益矩阵L。
这里,引入工作参考系Rt以及忽略传感器的偏差的简化模型,使得能够使用不变滤波器。
参照图3B,与普通的扩展卡尔曼滤波器不同,不变卡尔曼滤波器利用特别选择的方程(下文中将详细描述这些方程),计算状态的当前估计值
传统上,可以通过速度v和朝向T(T为旋转矩阵或四元数)或通过以下形式的位移矩阵X来表示运载设备的状态:
当前,可以使用其他表示方法。
这里,很清楚名称“位移矩阵”并不一定例示位移。该名称源于当引入位移矩阵用于机器人问题时所实现的用途(位移矩阵包含朝向并清楚地表示位移)。
P被称为状态x的估计不确定性。不变滤波器K的关注特性在于更新步骤中出现的增益矩阵L不依赖于先前估计值
这就构成了使得能够使用不变卡尔曼滤波器的简化***(并因此构成了无需针对垂直面和粗校准的研究步骤的收敛),从而具有以下特性:1)使用不平常的参考系来进入。2)惯性传感器的误差(偏差/漂移/比例因子误差)的决定性部分未知。
在实施例中,在传播方程中使用的重力被认为仅依赖于时间而不是位置,这就简化了这些方程的解析计算。
已知的精校准处理不涉及简化模型。已知的精校准处理直接整合模型中的所有变量,使得无法使用IEKF。
利用位移矩阵或者利用工作参考系的初始变量v和T(通过从位移矩阵中减去这些变量),来表示用于校准的IEKF的更新步骤:
■
■
■Kξ和Kv是源于稍后定义的指数映射的函数,例如其中Lt是依赖于P的矩阵。
■P遵循仅依赖于时间的方程。该方程通过在0和I3(大小为3×3的矩阵等式)周围线性化由和验证的方程来获得。
通过exp(u)=expm(D(u)),针对定义指数映射exp,其中,expm指定了已知的矩阵指数,并且其中,D()是主张大小为6×1的向量并且描绘大小为4×4的初等位移矩阵的线性应用,A(x1)指定了例如针对任意向量的反对称矩阵A(x1)u=x1×u,×为向量积。
由于D(u)类型的矩阵具有特定形式,因此能够使用以下闭公式:
在实施例中,刚刚描述的计算被迭代为收敛(简化模式)。
通过变型(完整模式),还能够通过下文中描述的方法来估计附加量(偏差 /漂移/比例因子误差等)。
当构成该简化的滤波器时,从关于其余参数(偏差/漂移/比例因子误差等) 的零值的校准过程的第一瞬间起,计算估计轨迹相对于这些参数的微分。估计的轨迹被表达为参数的精细函数。选定估计轨迹的二次质量标准(例如,随时间估计的速度的标准平方和)。该标准为容易最优化以获得参数值的参数(偏差/漂移/比例因子误差)的二次函数。即使估计参数的数量增大,该处理也保留了针对简化***构建的滤波器的收敛性。该最优化能够随时间递归地进行,能够在完成校准的单个时间进行,或者甚至能够在校准期间的任意瞬间进行。该选择对估计没有影响。在所有情况下,简化***的滤波和完整***的滤波同时进行。
在采用粗校准的处理中,也对简化***(不同于这里提出的简化***)执行粗校准,但是截取是临时的:1分钟的粗校准之后进行精校准。这里,两个模型从开始到结束同时作用:在每个瞬间,简化滤波器给出其估计值,然后,二次最优化计算完整状态(不进行卡尔曼滤波)作为简化状态的扰动。因为矩阵P遵循仅依赖于时间的方程(IEKF),所以能够在合理的时间实现轨迹的微分计算。如果简化***不是基于IEKF,则无法进行随后的对第一次忽略的参数的估计步骤。
惯性测量单元的校准方法
现在,将参照图4描述实现不变卡尔曼滤波器K的惯性测量单元IN(以及由此运载设备P)的校准方法。
在获取步骤ACQ中,利用关于轨迹的先验信息,来更新工作参考系Rt的轨迹。
在传播步骤PROP中,卡尔曼滤波器K利用其惯性传感器CO、CV的测量值,根据先前的估计状态来确定估计的当前状态。
在更新步骤MAJ中,卡尔曼滤波器K利用零速度(或零位移)的虚拟观测值,更新运载设备的估计状态。
随时间重复获取步骤ACQU、传播步骤PROP和更新MAJ步骤。
因此,校准方法包括几个连续迭代,每个新的迭代输入在先前迭代期间获得的更新的估计状态、以及新的观测值(在该新的迭代的获取步骤期间获取)。
还可以实现包括如下程序代码指令的计算机程序产品,当通过惯性测量单元中嵌入的计算机执行该程序时,该程序代码指令用于执行上述方法的步骤。
校准问题的一般形式
首先,将使用以下符号。
通过惯性参考系中的零速度观测来进行校准的已知构想如下。
假设近似已知运载设备的位置。方法利用的该假设的结果在于,局部重力已知并且仅为时间的函数。
运动方程被表达为:
其中,A(.)指定了与旋转向量相关联的反对称矩阵(通过关系 定义,其中,×为向量积)。
速度的虚拟测量值(wh为噪声)为:
将在正则区间引入信息,其中,Vi是与先验轨迹关联(例如与由于地球旋转而导致的先验轨迹关联)的运载设备P的期望速度(在惯性参考系中简单地表达出通常在地理参考系中进行的零速度观测)。
工作参考系中的运动方程的再形成
该部分详细描述了在本发明的实施例中进行的传播和更新方程。
工作参考系Rt(下文中也被参考为C)被相对于惯性参考系Ri平移地预先定义(即,利用惯性参考系Ri的轴来校准工作参考系Rt的轴),并且工作参考系Rt的原点C是地理参考系g的原点。
引用以下符号:
在工作参考系Rt中如下表达运载设备P的运动方程:
由于运载设备的位置xC对其他参数没有影响,因此可以删除该位置:
如下,还修改零速度信息:
能够通过位移矩阵来表示由不变卡尔曼滤波器K估计的运载设备P的状态,位移矩阵由地理参考系向工作参考系的通过矩阵、以及在工作参考系中表达的测量的运载设备的速度构成,如下构建位移矩阵:
当然,作为变型,还可以公知地表示运载设备的状态,例如矩阵T和向量 v。
惯性测量值和重力信息被写入初等位移矩阵:
这些方程被更加简单地重写为:
观测函数也被简单地写为:
不变滤波所基于的称为“左右同变性”的性质h如下。设为任意位移矩阵(即,的形式,其中,R为(3×3)的正交矩阵,u为任意向量)。得出:
以上可以被解释为与观测值空间中的变量变化相关联的状态空间中的变量变化(χ被替换为)。术语“不变”源于如下事实:滤波器被设计为对这种类型的再参数化不敏感。
适于先前动态的观测器如下:
更新步骤MAJ通常为如下形式:
其中,为当前估计的位移矩阵,Kn为增益函数(也被称为增益),Y为表示零速度的虚拟观测值的向量,并且为通过更新产生的估计的位移矩阵。
函数Kt对应于第t个零观测速度(因此对应于第t个卡尔曼滤波器的迭代),并且为Kt(u)=exp(Ltu)的形式,其中,exp()指定了称为指数映射的函数。Kt为函数,Lt为称为“增益矩阵”或更加简单地称为“增益”的矩阵。下面详细描述增益的计算处理。通过线性化由误差变量验证的方程,来计算矩阵Lt。通过η验证的方程的计算如下:
在传播步骤期间,已知χ和的演化方程,并因此能够计算η的演化方程:
引入由χ和验证的方程并使用vC小的事实,得出了:
这是误差变量的连续演化部分。当更新估计值时,误差变量间断地演化。在更新估计值期间的误差变量的演化尤其简单:
以如下方式进行该方程的线性化:指数映射主张向量并形成位移矩阵。尤其是,零向量的图像为矩阵等式I4。
因此能够使向量ξ和对应于误差变量,并且由ξ和ξ+验证的方程能够被写入并在0周围线性化。所得的方程涉及增益矩阵Lt:
ξ+=ξ-Lt(Hξ+w)
其中,03为仅包含0的3×3矩阵,并且
该方程为线性,并且利用黎卡提方程能够同时计算ξ的协方差Pt、以及用于最小化Pt的增益Lt:
其中,Q为噪声的协方差矩阵,R为噪声wh的协方差矩阵。
比较校准结果
下文中将通过已描述的校准方法(基于不变卡尔曼滤波器)获得的结果,与通过已知的校准方法(基于已知的扩展卡尔曼滤波器并在已知单元中使用) 获得的结果相比较。
通过卡尔曼滤波器处理的变量为运载设备的朝向(姿态)及其速度。已知认为运载设备的位置接近一百米。使用的传感器被认为不具有偏差,并且使用的观测值为零速度。已知运载设备的垂直面接近30度,而其方向完全未知。
图5A和图5B各自比较两个校准方法的分别收敛速度、以及由这两个方法提供的当前方向估计。
在图5A所示的示例中,以低频进行零速度信息的整合。观测出,在200 秒之后,根据本发明的校准方法(蓝色曲线)提供了非常接近真实值的方向估计,而由已知处理提供的估计方向(绿色曲线)继续摆动。
在讨论中的情形下,如果已知纬度,则能够预先计算在更新步骤中使用的增益。如图5B所示,滤波器花费很小的计算时间并且能够以高频整合零速度信息。以下附图展示了在与先前相同的情形下,该选择的结果。这里,利用所提出的方法的收敛比利用现有技术的方法的收敛快。
图6A和图6B研究了方向的先验估计对本发明的校准方法的结果的影响。
根据[0,2π]的统一定律随机描绘方向估计值,然后算法正常工作240秒。
图6A示出了方向误差很快接近零,而与执行根据本发明的方法期间的初始化无关。图6B是图6A的放大图。
与简化模式并行执行的完整模式
刚刚描述的简化模式被与简化模式并行执行的完整模式有利地完成。
这由图7所例示,图7示出了执行简化校准模式和完整模式的处理、以及传统类型的处理随时间的进程。
该完整模式使得能够进行二次最优化的校准。
以如下方式估计影响估计值的附加量:
-在每个瞬间,递归计算相对于由简化模式提供的估计值的附加量的微分。
-通过最优化估计的质量标准,来计算附加量。
能够在完成所述校准方法时进行通过最优化质量标准计算附加量。还能够在校准方法的任何瞬间,继续进行或以简化方式进行附加量的计算。
使用的微分计算将运载设备的估计状态表达为附加量的精细函数。
通过比较,已知的处理连续地执行:
-研究垂直面的处理;
-***状态的近似评估(粗校准);
-通过利用扩展卡尔曼滤波来整合虚拟位移观测值和/或零速度,来校准完整***)。
在所提出的情况下,通过二次最优化执行完整模式的示例如下:
如果惯性传感器的测量值经历与附加量(偏差/漂移/比例因子误差等)关联的决定性扰动,则这些测量值不像传统处理中被整合到卡尔曼滤波器中,所述附加量由向量β表示,并且必须在校准期间评估。使用以下过程:
针对任意给定值β,能够校正惯性测量值(减去由电流放大系数给出的、由比例因子划分等的偏差漂移),然后将简化模式应用到校正后的测量值。所得的估计值是附加量的函数因为β未知,所以不计算该量。
选择量β的基准值β0,并且实际仅计算估计值
选择在简化模式下出现的量的任意初始值
选择应用于轨迹估计的概率函数:
随时间重复以下步骤:
-传播步骤(PROP)
惯性测量值用来通过简化模型和矩阵Pt计算的传播步骤。在β0附近线性化由验证的方程,以递归演变精细应用这是在β0附近的一阶展开。利用通过矩阵表示Axt。
-更新步骤(MAJ):
通过在简化模型中描述的处理,使用轨迹的先验信息来更新的更新方程与相同(具有不变卡尔曼滤波器的特性),并且用来更新一阶展开Axt。
-扰动步骤(PERT):
可以在完成校准的单个时间,在校准期间的不同瞬间,或者在适时的各步骤应用该步骤。通过最优化来获得β的估计值 通过或的其他函数,给出状态x的精细估计值相当于Axt的第一阶。
函数f可以是二次的。这样,没有必要存储或Axt的所有值。
函数f可以涉及随时间递归计算的附加量。
例如可以通过如下方程来定义函数f:
其中,通过简化模式给出St,并且符号T指定了矩阵转置。
下文中使用的函数f的另一选择示例为:
可以在完成校准的单个时间,在校准期间的不同瞬间,或者在校准期间持续进行的计算。
可以采用递归最优化处理来计算
通过假定针对其余校准,可以选择性地将估计值用作新的基准值。
通过假定针对其余校准,精细估计值可以选择性地用作简化模型中的最终估计值。
下面描述执行方法的示例:
是大小为6×1的向量,其包含三个陀螺仪或陀螺测试仪的漂移(β1) 以及三个加速度计的偏差(β2)。
简化模式的变量为朝向和速度,并且能够被写入位移矩阵
选择β0=0作为偏差和漂移的基准值。因此使用惯性传感器的测量值,而无需简化模式中的修改。
如果考虑由β表示的偏差和漂移,则估计值遵循以下传播方程,将仅计算该估计值在基准值β0附近的对第一阶的近似:
以及以下更新方程:
其中,exp()是主张大小为6×1的向量并形成位移矩阵的指数映射。β是以下初等位移矩阵:
通过简化模式给出增益矩阵Lt。
将估计值写入的第一阶的简单方式为:
其中,D()是主张大小为6×1的向量并形成大小为4×4的初等位移矩阵的线性应用。矩阵Zt的大小为6×6,并且验证以下传播方程:
以及以下更新方程:
其中,通过简化模式给出Lt,通过简化模式给出位移矩阵和并通过简化模式给出旋转矩阵通过定义矩阵adi(ωt和ft为惯性传感器的测量值),并且通过定义矩阵函数ν主张大小为4×4的位移矩阵(这里为矩阵),并形成位于右上方的大小为3×1的向量(这里为)。函数Dexp是以下理解中的指数映射的导数:
exp(ξ+u)=[I4+D(Dexp(ξ)u)]exp(ξ)
其中,ξ和u是大小为3的向量,u的规格小于1,I4是大小为4的单位矩阵,并且Dexp(ξ)是大小为6×6的矩阵。
通过以下公式定义所选择的概率函数:
其被重写为:
因此,无需存储整个轨迹的信息,而只需通过展开f的表达式,来写入并支持矩阵S1和S2。
因此,完整算法(简化模式和完整模式)如下:
偏差和漂移的基准值为β0=0。以能够依赖于情形的任意方式初始化初始估计值和协方差矩阵(表示不确定性)。表示对的一阶展开的矩阵Zt被初始化为Z0=06×6。矩阵S1和S2被初始化为S1=06×6和S2=06×1。
然后,贯穿校准重复以下步骤:
-传播步骤(PROP):
不确定性的传播:
估计值的传播:
一阶展开的传播:
-wh次的更新步骤(MAJ):
不确定性的更新:
估计值的更新:
一阶展开的更新:
-扰动步骤(PERT):
偏差和漂移的估计值:
是线性单元的解。
简化模式的量的精细估计值:
函数exp可以被与第一阶等效的其他函数替代。
其他实施例
为了提高处理的鲁棒性和性能,所述方法可以被并行使用几次,在不同的朝向初始化,然后可以选择一个或更多个所得的估计值。可以使用不同的选择标准。
为了不增大与传播步骤相关联的计算负荷,由和Zt验证的方程可以表达为一般形式的类型:
在传播步骤的单个时间计算方程(1)、(2)、(3)和(4),在开始更新步骤时,将方程(5)和(6)应用于与并行操作的每个滤波器相关联的量和z。针对所有滤波器使用相同的增益。
通过针对每个滤波器计算可能性标准、然后选择最佳结果,来进行估计值的选择。该标准例如可以是随时间的速度估计值的二次函数。同样地,可以基于相干标准使用几个估计值,例如二者(或更多者)给出最接近的结果。然后,选择这些估计值中的一个或任意函数(例如平均值)作为最终估计值。
此外,通过利用虚拟位置观测值替换虚拟速度观测值,来执行描述的所有处理。简化状态还包含位置,并且置于如下形式的矩阵中:
观测功能为:
以与针对零速度观测相同的方式执行其余计算。
通过另一变型,可以通过以任意其他方式表示数据,来进行描述的所有处理。例如可以通过四元法或欧拉角以及所有的变化计算,来参数化朝向。
同样地,可以通过除了提出的处理之外的任意处理,选择增益函数Kn(.)。例如,可以将无损卡尔曼滤波器类型的过程应用于非线性误差方程,可以利用模拟最优化的恒定增益等。
此外,可以将任意初始化处理添加到校准过程的开始。例如可以使用加速度计来搜索垂直面。
描述的处理可以被整合到更广泛的架构中,例如组合扩展卡尔曼滤波器、一组卡尔曼滤波器等。
此外,如果惯性测量单元没有位于与位置信息所指的点相同的位置,而该点在单元的参考系中固定且已知(例如如果GPS接收器远离单元),则通过利用已知位置(在GPS和单元的情况下分开两个接收器的杠杆臂)替换零,根据零位移的虚拟观测值,能够应用与校准相同的过程。
Claims (11)
1.一种用于校准运载设备(P)的方法,所述运载设备在惯性参考系(Ri)中移动并且包括惯性测量单元(IN),所述惯性测量单元包括多个惯性传感器(CV、CO)和至少一个不变卡尔曼滤波器,所述方法包括重复以下用于估计所述运载设备(P)的状态的步骤,所述状态表示所述运载设备的速度和朝向:
-传播步骤(PROP),根据先前估计的状态和所述惯性传感器的测量值,确定估计的当前状态,
-更新步骤(MAJ),利用所述运载设备(P)的轨迹的先验信息,更新估计的当前状态,
所述方法的特征在于,为了允许至少一个不变卡尔曼滤波器的处理,实施简化模式,在所述简化模式下,在传播步骤和更新步骤期间不估计所述惯性传感器的偏差,更新方程中出现的每个速度表现在相对于所述惯性参考系(Ri)平移的工作参考系(Rt)中,其中所述工作参考系(Rt)具有沿着惯性参考轨迹移动的原点,其中所述运载设备被认为接近所述惯性参考轨迹。
2.根据权利要求1所述的方法,其特征在于,在所述传播步骤中利用传播方程来确定估计的当前状态,其中所述传播方程涉及仅依赖于时间的重力变量。
3.根据权利要求1或2所述的方法,其特征在于,所述惯性参考轨迹由所述惯性传感器未测量的先验信息构建。
4.根据权利要求1或2所述的方法,其特征在于,在所述简化模式下,所述更新方程中出现的每个位置表现在相对于所述惯性参考系(Ri)平移的工作参考系(Rt)中。
5.根据权利要求1或2所述的方法,其特征在于,还并行执行处理,在所述处理中,以如下方式对影响估计值的附加量进行估计:
-在每个瞬间,递归计算相对于由所述简化模式提供的估计值的所述附加量的微分,
-通过最优化估计值的质量标准,来计算所述附加量。
6.根据权利要求5所述的方法,其中,在完成所述方法时,进行通过最优化估计值的质量标准对所述附加量的计算。
7.根据权利要求5所述的方法,其中,持续进行,或在所述方法的一个或更多个瞬间进行通过最优化估计值的质量标准对所述附加量的计算。
8.根据权利要求1或2所述的方法,其中,在简化模式下重复执行所述传播步骤和更新步骤(MAJ)之前计算一系列增益,每个增益出现在所述更新步骤的迭代中。
9.根据权利要求1或2所述的方法,其特征在于,所述工作参考系的原点遵循GPS轨迹。
10.一种惯性测量单元(IN),其能够在惯性参考系(Ri)中移动,所述惯性测量单元包括:
-多个惯性传感器(CV、CO),
-至少一个不变卡尔曼滤波器,
所述惯性测量单元的嵌入式计算机,所述嵌入式计算机重复进行以下用于估计运载设备(P)的状态的步骤,所述状态表示所述运载设备的速度和朝向:
●传播步骤(PROP),根据先前估计的状态和所述惯性传感器的测量值,确定估计的当前状态,
●更新步骤(MAJ),利用所述运载设备(P)的轨迹的先验信息,更新估计的当前状态,
所述惯性测量单元的特征在于,为了允许所述至少一个不变卡尔曼滤波器的处理,所述嵌入式计算机被构造为执行根据权利要求1至9中的一项所述的方法。
11.一种计算机可读存储介质,其特征在于,该计算机可读存储介质存储有程序代码指令,当由惯性测量单元中嵌入的计算机执行该程序代码指令时,所述程序代码指令用于执行根据权利要求1至9中的一项所述的方法的步骤。
Applications Claiming Priority (5)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
FR1302705A FR3013829B1 (fr) | 2013-11-22 | 2013-11-22 | Procede d'alignement d'une centrale inertielle |
FR1302705 | 2013-11-22 | ||
FR1401512 | 2014-07-04 | ||
FR1401512A FR3013830B1 (fr) | 2013-11-22 | 2014-07-04 | Procede d'alignement d'une centrale inertielle |
PCT/EP2014/075439 WO2015075248A1 (fr) | 2013-11-22 | 2014-11-24 | Procédé d'alignement d'une centrale inertielle |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106068441A CN106068441A (zh) | 2016-11-02 |
CN106068441B true CN106068441B (zh) | 2019-03-22 |
Family
ID=50624613
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201480064042.XA Active CN106068441B (zh) | 2013-11-22 | 2014-11-24 | 惯性单元的校准方法 |
Country Status (5)
Country | Link |
---|---|
US (1) | US10859380B2 (zh) |
EP (1) | EP3071934B1 (zh) |
CN (1) | CN106068441B (zh) |
FR (2) | FR3013829B1 (zh) |
WO (1) | WO2015075248A1 (zh) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR3034514B1 (fr) | 2015-04-01 | 2017-04-21 | Sagem Defense Securite | Procede de suivi de navigation d'un porteur moblile avec un filtre de kalman etendu |
US10088318B2 (en) * | 2015-08-27 | 2018-10-02 | Qualcomm Incorporated | Cradle rotation insensitive inertial navigation |
US10386203B1 (en) * | 2015-11-05 | 2019-08-20 | Invensense, Inc. | Systems and methods for gyroscope calibration |
FR3043469B1 (fr) * | 2015-11-10 | 2019-10-18 | Safran Electronics & Defense | Procede de detection de mouvements parasites lors d'un alignement statique d'une centrale inertielle, et dispositif de detection associe |
WO2018182870A1 (en) * | 2017-03-27 | 2018-10-04 | Hrl Laboratories, Llc | Adaptive downhole inertial measurement unit calibration method and apparatus for autonomous wellbore drilling |
CN108759870B (zh) * | 2018-07-03 | 2022-03-25 | 哈尔滨工业大学 | 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法 |
FR3084176B1 (fr) * | 2018-07-23 | 2020-06-19 | Safran Electronics & Defense | Procede et dispositif d'aide a la navigation d'un porteur a l'aide d'un filtre de kalman invariant et d'un etat de navigation d'un deuxieme porteur |
CN109189087B (zh) * | 2018-08-20 | 2020-07-14 | 哈尔滨工业大学 | 一种垂直起降重复使用运载器的自适应容错控制方法 |
GB2579415B (en) * | 2018-11-30 | 2021-11-10 | Thales Holdings Uk Plc | Method and apparatus for determining a position of a vehicle |
FR3097316B1 (fr) * | 2019-06-14 | 2022-01-28 | Safran Electronics & Defense | Procédé de surveillance des performances d’unités de mesure inertielle |
CN110440830B (zh) * | 2019-08-20 | 2023-03-28 | 湖南航天机电设备与特种材料研究所 | 动基座下车载捷联惯导***自对准方法 |
CN114018282B (zh) * | 2021-09-27 | 2024-05-31 | 北京控制工程研究所 | 一种便捷的太阳敏感器在轨健康监测方法和*** |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1862764A1 (en) * | 2006-05-31 | 2007-12-05 | Honeywell International Inc. | High speed gyrocompass alignment via multiple kalman filter based hypothesis testing |
CN101501447A (zh) * | 2006-08-09 | 2009-08-05 | 萨甘安全防护公司 | 对准轴对称振动传感器惯性导航***的方法以及相应的惯性导航*** |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5610815A (en) * | 1989-12-11 | 1997-03-11 | Caterpillar Inc. | Integrated vehicle positioning and navigation system, apparatus and method |
US5375059A (en) * | 1990-02-05 | 1994-12-20 | Caterpillar Inc. | Vehicle position determination system and method |
US7209810B2 (en) * | 2002-01-10 | 2007-04-24 | Lockheed Martin Corp. | Locomotive location system and method |
US6697736B2 (en) * | 2002-02-06 | 2004-02-24 | American Gnc Corporation | Positioning and navigation method and system thereof |
US7193559B2 (en) * | 2003-01-21 | 2007-03-20 | Novatel, Inc. | Inertial GPS navigation system with modified kalman filter |
FR2922300B1 (fr) * | 2007-10-12 | 2009-11-20 | Saint Louis Inst | PROCEDE DE DETERMINATION DE l'ATTITUDE, DE LA POSITION ET DE LA VITESSE D'UN ENGIN MOBILE |
US20110238308A1 (en) * | 2010-03-26 | 2011-09-29 | Isaac Thomas Miller | Pedal navigation using leo signals and body-mounted sensors |
CA2848217C (en) * | 2011-09-14 | 2018-09-18 | Trusted Positioning Inc. | Method and apparatus for navigation with nonlinear models |
-
2013
- 2013-11-22 FR FR1302705A patent/FR3013829B1/fr not_active Expired - Fee Related
-
2014
- 2014-07-04 FR FR1401512A patent/FR3013830B1/fr not_active Expired - Fee Related
- 2014-11-24 US US15/037,653 patent/US10859380B2/en active Active
- 2014-11-24 EP EP14803092.7A patent/EP3071934B1/fr active Active
- 2014-11-24 WO PCT/EP2014/075439 patent/WO2015075248A1/fr active Application Filing
- 2014-11-24 CN CN201480064042.XA patent/CN106068441B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP1862764A1 (en) * | 2006-05-31 | 2007-12-05 | Honeywell International Inc. | High speed gyrocompass alignment via multiple kalman filter based hypothesis testing |
CN101501447A (zh) * | 2006-08-09 | 2009-08-05 | 萨甘安全防护公司 | 对准轴对称振动传感器惯性导航***的方法以及相应的惯性导航*** |
Non-Patent Citations (1)
Title |
---|
"Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem";Silvere Bonnabel;《Joint 48th IEEE Conference on Decision and Control and 28th Chinese Control Conference》;20091231;正文第1297-1304页 |
Also Published As
Publication number | Publication date |
---|---|
CN106068441A (zh) | 2016-11-02 |
EP3071934B1 (fr) | 2018-01-03 |
FR3013830A1 (fr) | 2015-05-29 |
FR3013830B1 (fr) | 2016-01-08 |
FR3013829A1 (fr) | 2015-05-29 |
EP3071934A1 (fr) | 2016-09-28 |
WO2015075248A1 (fr) | 2015-05-28 |
FR3013829B1 (fr) | 2016-01-08 |
US10859380B2 (en) | 2020-12-08 |
US20160290808A1 (en) | 2016-10-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106068441B (zh) | 惯性单元的校准方法 | |
CN109974714B (zh) | 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法 | |
CN102692225B (zh) | 一种用于低成本小型无人机的姿态航向参考*** | |
US20110004404A1 (en) | Enhanced inertial system performance | |
CN105606096B (zh) | 一种载体运动状态信息辅助的姿态和航向计算方法和*** | |
CN111895988A (zh) | 无人机导航信息更新方法及装置 | |
CN111076722B (zh) | 基于自适应的四元数的姿态估计方法及装置 | |
CN108627152B (zh) | 一种微型无人机基于多传感器数据融合的导航方法 | |
CN112857398B (zh) | 一种系泊状态下舰船的快速初始对准方法和装置 | |
CN112334736A (zh) | 用于校准物体磁力计的方法 | |
CN114623823B (zh) | 一种融合里程计的uwb多模态定位***、方法及装置 | |
US20100256906A1 (en) | Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering | |
CN115143954A (zh) | 一种基于多源信息融合的无人车导航方法 | |
CN117289322B (zh) | 一种基于imu、gps与uwb的高精度定位算法 | |
Sokolović et al. | INS/GPS navigation system based on MEMS technologies | |
CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
CN108627154A (zh) | 极地区域操作姿态与航向参考*** | |
CN114061570A (zh) | 车辆定位方法、装置、计算机设备和存储介质 | |
CN113959462A (zh) | 一种基于四元数的惯性导航***自对准方法 | |
Montorsi et al. | Design and implementation of an inertial navigation system for pedestrians based on a low-cost MEMS IMU | |
CN115200578A (zh) | 基于多项式优化的惯性基导航信息融合方法及*** | |
CN116772837A (zh) | 基于交互式多模型的gnss/sins组合导航方法 | |
Brunner et al. | Evaluation of attitude estimation algorithms using absolute magnetic reference data: Methodology and results | |
CN114001730B (zh) | 融合定位方法、装置、计算机设备和存储介质 | |
Zarei et al. | Performance improvement for mobile robot position determination using cubature Kalman filter |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |