[发明专利]提高惯性-地磁组合动态定姿精度的方法

摘要

本发明提供一种提高惯性‑地磁组合动态定姿精度的方法，利用人工神经元网络，针对人体肢体运动等复杂的三维运动，首先从三维加速度计输出的离散采样时间序列中提取合适的特征，随后利用人工神经元网络根据上述特征准确估计出载体线加速度的幅度，最后根据上述估计结果实时调整扩展卡尔曼算法的观测和过程协方差，进而抑制载体线加速度对上述算法的影响，从而提高惯性‑地磁组合的姿态解算精度。

主权项

1.一种提高惯性‑地磁组合动态定姿精度的方法，其特征在于：包括由三轴加速度计、三轴陀螺仪以及三轴地磁传感器构成的惯性‑地磁组合，且利用人工神经元网络提高动态定姿精度；所述方法包括训练和工作两个阶段；训练阶段，根据模拟的传感器输出数据和扩展卡尔曼算法得出人工神经元网络训练样本集，随后采用训练算法对人工神经元网络进行训练；工作阶段，将训练后的人工神经元网络用于调整扩展卡尔曼算法的观测和过程协方差，从而提高惯性‑地磁组合的姿态解算精度；所述训练阶段包括以下步骤：步骤1：利用下式通过MATLAB软件模拟每个采样时刻传感器的输出：其中，和分别为采样时刻k时三轴陀螺仪、三轴加速度计以及三轴地磁传感器的输出，和分别为当地地理坐标系表示的重力加速度矢量和地磁场矢量，B_i(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(q_k+1)为旋转矩阵，表示为：将非线性的观测方程(7)线性化为如下形式，zk+1＝Hkqk+1+ηk   (9)随后利用扩展卡尔曼算法递推估算出姿态，在估计出当前时刻k的姿态qk后，将以四元数表示的姿态转化为以欧拉角表示的姿态，即将qk转化为θk＝[Pk Rk Ak]T，其中Pk，Rk和Ak分别为当前时刻k的载体的俯仰角、滚转角和方位角；步骤6：利用下式计算姿态最大估计误差，其中和分别为俯仰角、滚转角和方位角的峰峰值或最大值误差；随后设置并令k₁＝k₁+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，直至用户终止姿态解算。