CN109029435A - 提高惯性-地磁组合动态定姿精度的方法 - Google Patents
提高惯性-地磁组合动态定姿精度的方法 Download PDFInfo
- Publication number
- CN109029435A CN109029435A CN201810649488.9A CN201810649488A CN109029435A CN 109029435 A CN109029435 A CN 109029435A CN 201810649488 A CN201810649488 A CN 201810649488A CN 109029435 A CN109029435 A CN 109029435A
- Authority
- CN
- China
- Prior art keywords
- attitude
- carrier
- neuron network
- geomagnetic
- artificial neuron
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 65
- 230000005389 magnetism Effects 0.000 title abstract 3
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 67
- 238000005070 sampling Methods 0.000 claims abstract description 60
- 230000001133 acceleration Effects 0.000 claims abstract description 48
- 230000008569 process Effects 0.000 claims abstract description 28
- 210000002569 neuron Anatomy 0.000 claims description 63
- 238000012549 training Methods 0.000 claims description 26
- 239000013598 vector Substances 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000002939 conjugate gradient method Methods 0.000 claims description 3
- 230000014509 gene expression Effects 0.000 claims description 3
- 230000005358 geomagnetic field Effects 0.000 claims description 3
- 238000013528 artificial neural network Methods 0.000 abstract 2
- 239000010410 layer Substances 0.000 description 13
- 238000002474 experimental method Methods 0.000 description 12
- 238000012545 processing Methods 0.000 description 5
- 230000003068 static effect Effects 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 230000000295 complement effect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000002401 inhibitory effect Effects 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000004913 activation Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000012887 quadratic function Methods 0.000 description 1
- 239000002356 single layer Substances 0.000 description 1
- 238000012612 static experiment Methods 0.000 description 1
- 238000002604 ultrasonography Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明提供一种提高惯性‑地磁组合动态定姿精度的方法，利用人工神经元网络，针对人体肢体运动等复杂的三维运动，首先从三维加速度计输出的离散采样时间序列中提取合适的特征，随后利用人工神经元网络根据上述特征准确估计出载体线加速度的幅度，最后根据上述估计结果实时调整扩展卡尔曼算法的观测和过程协方差，进而抑制载体线加速度对上述算法的影响，从而提高惯性‑地磁组合的姿态解算精度。
Description
技术领域
本发明涉及传感器技术领域，特别涉及一种利用人工神经元网络提高惯性-地磁组合动态定姿精度的方法。
背景技术
惯性-地磁组合是一种传感器组合，其硬件主要由三轴加速度计、三轴陀螺仪、三轴地磁传感器以及一些必要的信号处理单元构成，其中，信号处理单元包括前置放大器、带通滤波器、模数转换器、中央处理器、供电回路、通信回路等，主要用于实现传感器信号的后续处理与传输。该组合的软件程序由中央处理器运行，主要包括必要的处理器初始化程序、传感器数据采集程序、传感器误差校正程序、姿态解算程序、数据传输程序等。所谓的“姿态”指的是载体坐标系相对于参考坐标系之间的坐标转换关系，通常用欧拉角、四元数、旋转矩阵或等效旋转矢量等表示。参考坐标系通常选为当地地理坐标系，也可选为惯性坐标系或地球坐标系。姿态解算程序是姿态解算算法的具体实现。
目前常用的姿态解算算法主要有两类，即扩展卡尔曼算法和互补滤波算法。此外，其他一些基于统计线性化技术的算法，比如粒子滤波算法、无迹滤波算法等，或者基于最小二乘技术的算法，比如最优卡尔曼算法、扩展QUEST算法等，也都能被用于实现姿态解算，只不过鉴于惯性-地磁组合的高采样率和低运算能力的特点，这些算法并不适用于惯性-地磁组合。考虑到精度问题，扩展卡尔曼算法在惯性-地磁组合中的应用频度明显高于互补滤波算法，并因此成为主流算法。
相比于视觉、超声、微波等其他定姿技术，惯性-地磁组合的突出优点是其姿态解算不需依赖任何外部人工源，因此其使用不受区域限制，并且由于其广泛采用小体积的MEMS(微机械系统)传感器，这就使其非常便于携带。以上两种原因使得惯性-地磁组合特别适合应用于户外人体运动跟踪等领域。然而由于载体线加速度干扰，惯性-地磁组合的动态精度普遍不高，这就限制了其应用领域的进一步拓展。
发明内容
本发明所要解决的技术问题是：为了解决载体线加速度干扰所造成的惯性-地磁组合姿态解算精度下降的问题，本发明提供一种提高惯性-地磁组合动态定姿精度的方法，该方法首先从加速度计的输出中构建合适的特征，随后利用人工神经元网络建立上述特征与载体线加速度的幅度这两者之间的非线性函数关系，最后利用(表示载体线加速度的幅度的)人工神经元网络的输出调整扩展卡尔曼算法的过程和观测噪声协方差，从而抑制载体线加速度所造成的影响，提高扩展卡尔曼算法的动态姿态解算精度。
目前，已经提出一些针对性的方法来试图解决载体线加速度干扰问题。第一种方法是所谓的阈值方法。
该方法的基本思想是判断以下条件是否成立：
或者判断以下条件是否成立：
其中，k表示当前采样时刻，表示采样时刻t时加速度计的输出，g为当地重力加速度值，ε为预先设定的阈值，T0＞0为时间偏移量且为整数。
如果以上条件成立，则不对扩展卡尔曼算法的过程噪声以及观测噪声协方差做任何调整，否则大幅度地调大观测噪声协方差值，但维持过程噪声协方差值不变，从而使得扩展卡尔曼算法在上述情形下主要依赖陀螺仪的输出实现姿态解算，以达到抑制载体线加速度干扰的目的。由于载体线加速度的幅度的变化范围与具体的运动类型有关，因此上述方法的阈值非常难以确定，然而阈值又很大程度上确定了上述方法的性能。
第二种方法是非阈值方法。该方法将上述特征与载体线加速度的幅度ak之间的关系定义为线性或二次函数关系，从而利用上述关系实时调整扩展卡尔曼算法的观测噪声协方差值。这种方法主要适用于载体线加速度的幅度较小的情形。
第三种方法尝试利用姿态先验估计误差和后验估计误差来实时调整扩展卡尔曼算法的观测噪声和过程噪声协方差，然而这种方法只适用于简单的运动形式。
本发明解决其技术问题所要采用的技术方案是：一种提高惯性-地磁组合动态定姿精度的方法，该方法的基本思想是以人工神经元网络作为加速度计输出和扩展卡尔曼算法的过程噪声和观测噪声协方差之间的桥梁，以期通过前者自适应调整后者，从而达到抑制载体线加速度干扰的目的。
具体实施步骤如下：
由三轴加速度计、三轴陀螺仪以及三轴地磁传感器构成的惯性-地磁组合，且利用人工神经元网络提高动态定姿精度；
所述方法包括训练和工作两个阶段；训练阶段，根据模拟的传感器输出数据和扩展卡尔曼算法得出人工神经元网络训练样本集，随后采用训练算法对人工神经元网络进行训练；工作阶段，将训练后的人工神经元网络用于调整扩展卡尔曼算法的观测和过程协方差，从而提高惯性-地磁组合的姿态解算精度；
步骤1～8涉及人工神经元网络的训练，即训练阶段。
步骤1：利用下式通过MATLAB等软件模拟每个采样时刻传感器的输出：
其中，和分别为采样时刻k时三轴陀螺仪、三轴加速度计以及三轴地磁传感器的输出，和分别为当地地理坐标系表示的重力加速度矢量和地磁场矢量，Bi(i＝x,y,z)为均匀分布于区间[-Acc,Acc]内的一个随机变量，用来模拟载体给定轴向上的线加速度，其中Acc为某一具体数值，表示载体在任意轴向上的最大可能的线加速度值，
其中，N1和N均为表示采样个数的具体数值，N1为“分水岭”，式(1)和(2)表示在采样个数少于N1(换句话说，自传感器上电运行起直至N1T时刻止的这段时间内，其中T为采样时间)时，载体不存在线加速度，然而一旦采样个数超过N1，则载体存在线加速度。
步骤2：在当前采样时刻k，以下式作为特征，
其中||·||表示取矢量的模，|·|表示取标量的绝对值，Xm·k表示采样时刻k所选取的特征，人工神经元网络将利用该特征估计载体线加速度的幅度；Xm·k随后作为人工神经元网络(即单隐层BP神经元网络)的输入；存储上式的计算结果，即Xm·k。
步骤3：预先设置采样时刻k时载体线加速度的幅度为ak＝1，随后设置迭代次数k1＝1；
步骤4：将扩展卡尔曼算法的过程和观测噪声协方差调整为
其中Q和R分别为调整之前采样时刻k时由三轴陀螺仪、三轴加速度计和三轴地磁传感器的输出噪声的方差构成的过程噪声协方差和观测噪声协方差，Q′k和R′k分别为过程噪声协方差和观测噪声协方差调整之后的结果；ak的取值范围为0≤ak≤1；在载体处于完全静止时，ak＝1，随着载体线加速度幅值的增大，ak会逐渐减小直至接近0；因此，虽然利用变量ak来表示所谓的线加速度的“幅度”，然而实际上，线加速度的“幅度”与ak之间成反比例关系。
步骤5：利用扩展卡尔曼算法计算载体的姿态。
首先以四元数作为载体姿态，并利用陀螺仪输出数据构建状态方程，
式中，qk+1和qk分别为采样时刻k+1和采样时刻k时以四元数表示的姿态，εk为采样时刻k时的状态噪声，其噪声协方差阵为Q′k，Ωk表示为
其中ω1、ω2、ω3分别为采样时刻k时三轴向上的陀螺仪输出。随后利用加速度计和地磁传感器输出数据构建观测方程，
其中ηk为采样时刻k时的观测噪声，其噪声协方差阵为R′k，和分别为采样时刻k时由三轴加速度计和三轴地磁传感器的输出噪声构成的三维矢量，C(qk+1)为旋转矩阵，表示为
进一步地，将非线性的观测方程(7)线性化为如下形式，
zk+1＝Hkqk+1+ηk (9)
随后利用扩展卡尔曼算法递推估算出姿态，具体的递推公式如表1所示，
表1扩展卡尔曼算法递推估计公式
上表中，εk(l)表示为采样时刻k时状态噪声矢量εk的第l个分量，ηj(m)具有类似的定义，函数δkj定义为
在估计出当前时刻k的姿态qk后，本步骤的最后一项工作是，将以四元数表示的姿态转化为以欧拉角表示的姿态，即将qk转化为θk＝[Pk Rk Ak]T，其中Pk，Rk和Ak分别为当前时刻k的载体的俯仰角、滚转角和方位角。
步骤6：利用下式计算姿态最大估计误差，
其中和分别为俯仰角、滚转角和方位角的峰峰值或最大值误差；随后设置并令k1＝k1+1。
步骤7：如果k1小于预先给定的阈值kmax，则返回至步骤5；否则记录ak，随后设置k＝k+1，然后返回至步骤3，直至k＝N。
步骤8：利用步骤2获得的Xm·k(作为人工神经元网络的输入)和步骤7获得的ak(作为人工神经元网络的输出)采用合适的训练算法训练人工神经元网络。其中人工神经元网络采用单隐层BP网。
上述步骤1～8涉及到一些关键参数和算法，本专利针对这些参数和算法的推荐值为Acc＝12、kmax＝100、人工神经元网络的隐层神经元个数nly＝35、N1＝300、N＝3000，人工神经元网络的训练算法采用共轭梯度法(“Conjugate Gradient Back-Propagation withFletcher-Reeves Updates”算法)。需要注意的是，采用上述推荐值仅仅意味着可以使人工神经元网络达到较为理想的训练效果。本专利并未对上述变量的取值做任何限定。
步骤9～14为工作阶段。
步骤9：惯性-地磁组合上电运行。
步骤10：在采样时刻k到来时，采样分别获得三轴陀螺仪、三轴加速度计以及三轴地磁传感器的输出和
步骤11：在当前采样时刻k，利用步骤2中的式(3)计算特征Xm·k，随后将其作为训练后的人工神经元网络的输入，进而获得表示载体线加速度幅度的ak。
步骤12：利用步骤4中的式(4)调整扩展卡尔曼算法的观测和过程噪声协方差。
步骤13：利用扩展卡尔曼算法解算姿态，进而获得当前采样时刻k的姿态输出，其中扩展卡尔曼算法的具体流程参见步骤5。
步骤14：等待下一个采样时刻k+1的到来，然后返回至步骤10，直至用户终止姿态解算。
本发明的有益效果是：本发明提供的一种提高惯性-地磁组合动态定姿精度的方法，以人工神经元网络作为媒介，在加速度计的输出和扩展卡尔曼算法的过程噪声和观测噪声的协方差之间建立起一个新的桥梁，所建立的桥梁可以完美地阐释(载体进行复杂运动时)上述两者之间的复杂的函数关系，从而可以依托该桥梁的作用利用前者动态地调整后者，进而最大限度地抑制载体线加速度的影响，提高惯性-地磁组合的动态姿态解算精度。
附图说明
下面结合附图和实施例对本发明作进一步说明。
图1是本发明最佳实施例的原理示意图。
图2是BP神经元网络的结构示意图。
具体实施方式
现在结合附图对本发明作详细的说明。此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
如图1所示，本发明提出的用于抑制载体线加速度影响的基于人工神经元网络的提高惯性-地磁组合动态定姿精度的方法，该方法的基本思想是以人工神经元网络作为加速度计输出和扩展卡尔曼算法的过程噪声和观测噪声协方差之间的桥梁，以期通过前者自适应调整后者，从而达到抑制载体线加速度干扰的目的，其中，由三轴加速度计、三轴陀螺仪以及三轴地磁传感器构成的惯性-地磁组合，且利用人工神经元网络提高动态定姿精度；
所述方法包括训练和工作两个阶段；训练阶段，根据模拟的传感器输出数据和扩展卡尔曼算法得出人工神经元网络训练样本集，随后采用训练算法对人工神经元网络进行训练；工作阶段，将训练后的人工神经元网络用于调整扩展卡尔曼算法的观测和过程协方差，从而提高惯性-地磁组合的姿态解算精度。
步骤1～8涉及人工神经元网络的训练，即训练阶段。
步骤1：利用下式通过MATLAB等软件模拟每个采样时刻传感器的输出：
其中，和分别为采样时刻k时三轴陀螺仪、三轴加速度计以及三轴地磁传感器的输出，和分别为当地地理坐标系表示的重力加速度矢量和地磁场矢量，Bi(i＝x,y,z)为均匀分布于区间[-Acc,Acc]内的一个随机变量，用来模拟载体给定轴向上的线加速度，其中Acc为某一具体数值，表示载体在任意轴向上的最大可能的线加速度值，
其中，N1和N均为表示采样个数的具体数值，N1为“分水岭”，式(1)和(2)表示在采样个数少于N1(换句话说，自传感器上电运行起直至N1T时刻止的这段时间内，其中T为采样时间)时，载体不存在线加速度，然而一旦采样个数超过N1，则载体存在线加速度。
步骤2：在当前采样时刻k，以下式作为特征，
其中||·||表示取矢量的模，|·|表示取标量的绝对值，Xm·k表示采样时刻k所选取的特征，人工神经元网络将利用该特征估计载体线加速度的幅度；Xm·k随后作为人工神经元网络(即单隐层BP神经元网络)的输入；存储上式的计算结果，即Xm·k。
步骤3：预先设置采样时刻k时载体线加速度的幅度为ak＝1，随后设置迭代次数k1＝1。
步骤4：将扩展卡尔曼算法的过程和观测噪声协方差调整为
其中Q和R分别为调整之前采样时刻k时由三轴陀螺仪、三轴加速度计和三轴地磁传感器的输出噪声的方差构成的过程噪声协方差和观测噪声协方差，Q′k和R′k分别为过程噪声协方差和观测噪声协方差调整之后的结果；ak的取值范围为0≤ak≤1；在载体处于完全静止时，ak＝1，随着载体线加速度幅值的增大，ak会逐渐减小直至接近0；因此，虽然利用变量ak来表示所谓的线加速度的“幅度”，然而实际上，线加速度的“幅度”与ak之间成反比例关系。
步骤5：利用扩展卡尔曼算法计算载体的姿态。
首先以四元数作为载体姿态，并利用陀螺仪输出数据构建状态方程，
式中，qk+1和qk分别为采样时刻k+1和采样时刻k时以四元数表示的姿态，εk为采样时刻k时的状态噪声，其噪声协方差阵为Q′k，Ωk表示为
其中ω1、ω2、ω3分别为采样时刻k时三轴向上的陀螺仪输出。随后利用加速度计和地磁传感器输出数据构建观测方程，
其中ηk为采样时刻k时的观测噪声，其噪声协方差阵为R′k，和分别为采样时刻k时由三轴加速度计和三轴地磁传感器的输出噪声构成的三维矢量，C(qk+1)为旋转矩阵，表示为
进一步地，将非线性的观测方程线性化为如下形式，
zk+1＝Hkqk+1+ηk (9)
随后利用扩展卡尔曼算法递推估算出姿态，具体的递推公式如表1所示，
表1扩展卡尔曼算法递推估计公式
上表中，εk(l)表示为采样时刻k时状态噪声矢量εk的第l个分量，ηj(m)具有类似的定义，函数δkj定义为
在估计出当前时刻k的姿态qk后，本步骤的最后一项工作是，将以四元数表示的姿态转化为以欧拉角表示的姿态，即将qk转化为θk＝[Pk Rk Ak]T，其中Pk，Rk和Ak分别为当前时刻k的载体的俯仰角、滚转角和方位角。
步骤6：利用下式计算姿态最大估计误差，
其中和分别为俯仰角、滚转角和方位角的峰峰值或最大值误差；随后设置并令k1＝k1+1。
步骤7：如果k1小于预先给定的阈值kmax，则返回至步骤5；否则记录ak，随后设置k＝k+1，然后返回至步骤3，直至k＝N。
步骤8：利用步骤2获得的Xm·k(作为人工神经元网络的输入)和步骤7获得的ak(作为人工神经元网络的输出)采用合适的训练算法训练人工神经元网络。其中人工神经元网络采用单隐层BP网，所述的单隐层BP神经元网络的具体结构如图2所示，在工作模式下，BP神经元网络的输入和输出分别为由步骤2计算得到的特征Xm·k和步骤11获得的表示载体线加速度幅度的ak，因此BP神经元网络的输入和输出均只包含一个神经元。隐层的层数为单层，隐层神经元的个数为nly，输入层至隐层之间的连接权值为Wi，隐层至输出层之间的连接权值为Wj。神经元的激活函数设为如下形式，
上述步骤1～8涉及到一些关键参数和算法，本专利针对这些参数和算法的推荐值为Acc＝12、kmax＝100、人工神经元网络的隐层神经元个数nly＝35、N1＝300、N＝3000，人工神经元网络的训练算法采用共轭梯度法(“Conjugate Gradient Back-Propagation withFletcher-Reeves Updates”算法)。需要注意的是，采用上述推荐值仅仅意味着可以使人工神经元网络达到较为理想的训练效果。本专利并未对上述变量的取值做任何限定。
步骤9～14为工作阶段。
步骤9：惯性-地磁组合上电运行。
步骤10：在采样时刻k到来时，采样获得三轴陀螺仪、三轴加速度计以及三轴地磁传感器的输出和
步骤11：在当前采样时刻k，利用步骤2中的式(3)计算特征Xm·k，随后将其作为训练后的人工神经元网络的输入，进而获得表示载体线加速度幅度的ak。
步骤12：利用步骤4所示的式(4)调整扩展卡尔曼算法的观测和过程噪声协方差。
步骤13：利用扩展卡尔曼算法解算姿态，进而获得当前采样时刻k的姿态输出，其中扩展卡尔曼算法的具体流程参见步骤5。
步骤14：等待下一个采样时刻k+1的到来，然后返回至步骤10，直至用户终止姿态解算。
为了体现本专利所提出的提高惯性-地磁组合动态定姿精度的方法(即将人工神经元网络与扩展卡尔曼算法相结合所构成的新的姿态解算算法，以下简称本方法)在定姿精度上的优势，我们将本方法与目前为止国内外普遍采用的具有竞争力的其他四种方法(解决相同的目的)进行了实验对比。我们进行了两种实验，第一种实验是动态实验，要求实验者手持惯性-地磁组合做三维随机运动，第二种实验是静态实验，实验中保持惯性-地磁组合静止一段时间。实验所采用的惯性-地磁组合为目前代表国内外先进技术水平的MTi(产自荷兰XSens公司)。我们将MTi内置的加速度计等传感器数据作为本方法的输入。第一种实验采用整个实验过程中三个欧拉角的最大姿态估计误差(MSEE)作为方法评价依据；第二种实验采用整个实验过程中三个欧拉角的最大Allan方差(MAV，相关时间取为3T)作为方法评价依据。每种实验有10人参与，每人做2次实验，一共进行20次实验。具体的对比结果如表2所示，其中所列的MSEE和MAV为上述20次实验的平均值。
从表2可以明显地看出，只有本方法能够使惯性-地磁组合同时具有良好的静动态性能，而其他方法则要么使惯性-地磁组合具有良好的静态但很差的动态性能，要么使其具有良好的动态但很差的静态性能，亦即不能使惯性-地磁组合同时兼具良好的静动态性能。上述对比结果明显地体现出本方法在定姿精度上的优势。
表2各种提高惯性-地磁组合定姿精度的方法对比
以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关的工作人员完全可以在不偏离本发明的范围内，进行多样的变更以及修改。本项发明的技术范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。
Claims (3)
1.一种提高惯性-地磁组合动态定姿精度的方法，其特征在于：包括由三轴加速度计、三轴陀螺仪以及三轴地磁传感器构成的惯性-地磁组合，且利用人工神经元网络提高动态定姿精度；
所述方法包括训练和工作两个阶段；训练阶段，根据模拟的传感器输出数据和扩展卡尔曼算法得出人工神经元网络训练样本集，随后采用训练算法对人工神经元网络进行训练；工作阶段，将训练后的人工神经元网络用于调整扩展卡尔曼算法的观测和过程协方差，从而提高惯性-地磁组合的姿态解算精度；
所述训练阶段包括以下步骤：
步骤1：利用下式通过MATLAB软件模拟每个采样时刻传感器的输出：
其中，和分别为采样时刻k时三轴陀螺仪、三轴加速度计以及三轴地磁传感器的输出，和分别为当地地理坐标系表示的重力加速度矢量和地磁场矢量，Bi(i＝x,y,z)为均匀分布于区间[-Acc,Acc]内的一个随机变量，用来模拟载体给定轴向上的线加速度，其中Acc为某一具体数值，表示载体在任意轴向上的最大可能的线加速度值，
其中，N1和N均为表示采样个数的具体数值，N1为“分水岭”，式(1)和(2)表示在采样个数少于N1(换句话说，自传感器上电运行起直至N1T时刻止的这段时间内，其中T为采样时间)时，载体不存在线加速度，然而一旦采样个数超过N1，则载体存在线加速度；
步骤2：在当前采样时刻k，以下式作为特征，
其中||·||表示取矢量的模，|·|表示取标量的绝对值，Xm·k表示采样时刻k所选取的特征，人工神经元网络将利用该特征估计载体线加速度的幅度；Xm·k随后作为人工神经元网络(即单隐层BP神经元网络)的输入；存储上式的计算结果，即Xm·k；
步骤3：预先设置采样时刻k时载体线加速度的幅度为ak＝1，随后设置迭代次数k1＝1；
步骤4：将扩展卡尔曼算法的过程和观测噪声协方差调整为公式(4)，
其中Q和R分别为调整之前采样时刻k时由三轴陀螺仪、三轴加速度计和三轴地磁传感器的输出噪声的方差构成的过程噪声协方差和观测噪声协方差，Q′k和R′k分别为过程噪声协方差和观测噪声协方差调整之后的结果；ak的取值范围为0≤ak≤1；在载体处于完全静止时，ak＝1，随着载体线加速度幅值的增大，ak会逐渐减小直至接近0；因此，虽然利用变量ak来表示所谓的线加速度的“幅度”，然而实际上，线加速度的“幅度”与ak之间成反比例关系；
步骤5：利用扩展卡尔曼算法计算载体的姿态；
首先，以四元数作为载体姿态，并利用陀螺仪输出数据构建状态方程，
式中，qk+1和qk分别为采样时刻k+1和采样时刻k时以四元数表示的姿态，εk为采样时刻k时的状态噪声，其噪声协方差阵为Q′k，Ωk表示为
其中ω1、ω2、ω3分别为采样时刻k时三轴向上的陀螺仪输出；随后利用加速度计和地磁传感器输出数据构建观测方程，
其中ηk为采样时刻k时的观测噪声，其噪声协方差阵为R′k，和分别为采样时刻k时由三轴加速度计和三轴地磁传感器的输出噪声构成的三维矢量，C(qk+1)为旋转矩阵，表示为：
将非线性的观测方程(7)线性化为如下形式，
zk+1＝Hkqk+1+ηk (9)
随后利用扩展卡尔曼算法递推估算出姿态，在估计出当前时刻k的姿态qk后，将以四元数表示的姿态转化为以欧拉角表示的姿态，即将qk转化为θk＝[Pk Rk Ak]T，其中Pk，Rk和Ak分别为当前时刻k的载体的俯仰角、滚转角和方位角；
步骤6：利用下式计算姿态最大估计误差，
其中和分别为俯仰角、滚转角和方位角的峰峰值或最大值误差；随后设置并令k1＝k1+1；
步骤7：如果k1小于预先给定的阈值kmax，则返回至步骤5；否则记录ak，随后设置k＝k+1，然后返回至步骤3，直至k＝N；
步骤8：利用步骤2获得的Xm·k(作为人工神经元网络的输入)和步骤7获得的ak(作为人工神经元网络的输出)采用训练算法训练人工神经元网络；
所述工作阶段包括以下步骤：
步骤9：给惯性-地磁组合上电运行；
步骤10：在采样时刻k到来时，采样分别获得三轴陀螺仪、三轴加速度计以及三轴地磁传感器的输出和
步骤11：在当前采样时刻k，利用步骤2中的式(3)计算特征Xm·k，随后将其作为训练后的人工神经元网络的输入，进而获得表示载体线加速度幅度的ak；
步骤12：利用步骤4中的式(4)调整扩展卡尔曼算法的观测和过程噪声协方差；
步骤13：利用扩展卡尔曼算法解算姿态，进而获得当前采样时刻k的姿态输出，其中扩展卡尔曼算法的具体流程参见步骤5；
步骤14：等待下一个采样时刻k+1的到来，然后返回至步骤10，直至用户终止姿态解算。
2.如权利要求1所述的提高惯性-地磁组合动态定姿精度的方法，其特征在于：所述步骤8中的人工神经元网络具体采用单隐层BP网。
3.如权利要求2所述的提高惯性-地磁组合动态定姿精度的方法，其特征在于：所述步骤8中的人工神经元网络的训练算法采用共轭梯度法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810649488.9A CN109029435B (zh) | 2018-06-22 | 2018-06-22 | 提高惯性-地磁组合动态定姿精度的方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810649488.9A CN109029435B (zh) | 2018-06-22 | 2018-06-22 | 提高惯性-地磁组合动态定姿精度的方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109029435A true CN109029435A (zh) | 2018-12-18 |
| CN109029435B CN109029435B (zh) | 2021-11-02 |
Family
ID=64610264
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810649488.9A Active CN109029435B (zh) | 2018-06-22 | 2018-06-22 | 提高惯性-地磁组合动态定姿精度的方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109029435B (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110017808A (zh) * | 2019-03-26 | 2019-07-16 | 北京理工大学 | 利用地磁信息和加速计解算飞行器姿态的方法 |
| CN110470294A (zh) * | 2019-08-09 | 2019-11-19 | 西安电子科技大学 | 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法 |
| CN111248922A (zh) * | 2020-02-11 | 2020-06-09 | 中国科学院半导体研究所 | 基于加速度计和陀螺仪的人体呼吸情况采集贴及制备方法 |
| CN111982102A (zh) * | 2020-08-11 | 2020-11-24 | 天津泰达滨海清洁能源集团有限公司 | 一种复杂环境下基于bp-ekf的uwb-imu定位方法 |
| CN112284382A (zh) * | 2020-10-23 | 2021-01-29 | 哈尔滨工程大学 | 组合式导航信息处理装置、水下导航系统 |
| CN113029173A (zh) * | 2021-03-09 | 2021-06-25 | 北京信息科技大学 | 车辆导航方法及装置 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103076619A (zh) * | 2012-12-27 | 2013-05-01 | 山东大学 | 一种消防员室内外3d无缝定位及姿态检测系统及方法 |
| US8957812B1 (en) * | 2010-11-12 | 2015-02-17 | Position Imaging, Inc. | Position tracking system and method using radio signals and inertial sensing |
| CN105698792A (zh) * | 2016-01-26 | 2016-06-22 | 上海实汇机电科技有限公司 | 一种基于自适应鲁邦融合算法的动态mems惯性姿态测量系统 |
| CN107084722A (zh) * | 2017-04-24 | 2017-08-22 | 常州大学 | 一种用于提高惯性‑地磁组合静动态综合性能的方法 |
| CN107478223A (zh) * | 2016-06-08 | 2017-12-15 | 南京理工大学 | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 |
| CN107621261A (zh) * | 2017-09-08 | 2018-01-23 | 常州大学 | 用于惯性‑地磁组合姿态解算的自适应optimal‑REQUEST算法 |
-
2018
- 2018-06-22 CN CN201810649488.9A patent/CN109029435B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8957812B1 (en) * | 2010-11-12 | 2015-02-17 | Position Imaging, Inc. | Position tracking system and method using radio signals and inertial sensing |
| CN103076619A (zh) * | 2012-12-27 | 2013-05-01 | 山东大学 | 一种消防员室内外3d无缝定位及姿态检测系统及方法 |
| CN105698792A (zh) * | 2016-01-26 | 2016-06-22 | 上海实汇机电科技有限公司 | 一种基于自适应鲁邦融合算法的动态mems惯性姿态测量系统 |
| CN107478223A (zh) * | 2016-06-08 | 2017-12-15 | 南京理工大学 | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 |
| CN107084722A (zh) * | 2017-04-24 | 2017-08-22 | 常州大学 | 一种用于提高惯性‑地磁组合静动态综合性能的方法 |
| CN107621261A (zh) * | 2017-09-08 | 2018-01-23 | 常州大学 | 用于惯性‑地磁组合姿态解算的自适应optimal‑REQUEST算法 |
Non-Patent Citations (2)
| Title |
|---|
| XAVIER ROBERT-LACHAINE 等: "Effect of local magnetic field disturbances on inertial measurement", 《APPLIED ERGONOMICS》 * |
| 戎海龙: "一种适用于惯性_地磁组合的自适应卡尔曼算法", 《计算机工程与应用》 * |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110017808A (zh) * | 2019-03-26 | 2019-07-16 | 北京理工大学 | 利用地磁信息和加速计解算飞行器姿态的方法 |
| CN110017808B (zh) * | 2019-03-26 | 2021-08-27 | 北京理工大学 | 利用地磁信息和加速计解算飞行器姿态的方法 |
| CN110470294A (zh) * | 2019-08-09 | 2019-11-19 | 西安电子科技大学 | 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法 |
| CN111248922A (zh) * | 2020-02-11 | 2020-06-09 | 中国科学院半导体研究所 | 基于加速度计和陀螺仪的人体呼吸情况采集贴及制备方法 |
| CN111982102A (zh) * | 2020-08-11 | 2020-11-24 | 天津泰达滨海清洁能源集团有限公司 | 一种复杂环境下基于bp-ekf的uwb-imu定位方法 |
| CN111982102B (zh) * | 2020-08-11 | 2022-09-06 | 天津泰达滨海清洁能源集团有限公司 | 一种复杂环境下基于bp-ekf的uwb-imu定位方法 |
| CN112284382A (zh) * | 2020-10-23 | 2021-01-29 | 哈尔滨工程大学 | 组合式导航信息处理装置、水下导航系统 |
| CN113029173A (zh) * | 2021-03-09 | 2021-06-25 | 北京信息科技大学 | 车辆导航方法及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109029435B (zh) | 2021-11-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109029435B (zh) | 提高惯性-地磁组合动态定姿精度的方法 | |
| US10976341B2 (en) | Multi sensor position and orientation measurement system | |
| CN106500695B (zh) | 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法 | |
| Abdolkarimi et al. | A wavelet-extreme learning machine for low-cost INS/GPS navigation system in high-speed applications | |
| Calusdian et al. | Adaptive-gain complementary filter of inertial and magnetic data for orientation estimation | |
| Jin et al. | The adaptive Kalman filter based on fuzzy logic for inertial motion capture system | |
| CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及系统 | |
| US20140222369A1 (en) | Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method | |
| CN109846487B (zh) | 基于MIMU/sEMG融合的大腿运动姿态测量方法和装置 | |
| CN110672078B (zh) | 一种基于地磁信息的高旋弹丸姿态估计方法 | |
| CN113175926B (zh) | 一种基于运动状态监测的自适应水平姿态测量方法 | |
| CN111649747A (zh) | 一种基于imu的自适应ekf姿态测量改进方法 | |
| CN107084722B (zh) | 一种用于提高惯性-地磁组合静动态综合性能的方法 | |
| Wenk et al. | Posture from motion | |
| CN110375773B (zh) | Mems惯导系统姿态初始化方法 | |
| CN109916399A (zh) | 一种阴影下的载体姿态估计方法 | |
| CN107621261B (zh) | 用于惯性-地磁组合姿态解算的自适应optimal-REQUEST算法 | |
| CN109443393A (zh) | 一种基于盲分离算法的捷联惯导信号提取方法及系统 | |
| CN110849364B (zh) | 基于动中通的自适应卡尔曼姿态估计方法 | |
| CN112985384B (zh) | 一种抗干扰磁航向角优化系统 | |
| CN107679016B (zh) | 一种基于lms算法的船用捷联惯导系统水平阻尼方法 | |
| Brückner et al. | Evaluation of inertial sensor fusion algorithms in grasping tasks using real input data: Comparison of computational costs and root mean square error | |
| Bruckner et al. | Evaluation of inertial sensor fusion algorithms in grasping tasks using real input data: Comparison of computational costs and root mean square error | |
| CN110307842B (zh) | 用于惯性-地磁组合的快速扩展卡尔曼方法 | |
| Huang et al. | Neural inertial navigation system on pedestrian |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |