CN103644910A - 基于分段rts平滑算法的个人自主导航***定位方法 - Google Patents

Abstract

本发明公开了一种基于分段RTS平滑算法的个人自主导航***定位方法，先采集个人自主导航***正常工作模式下输出的角速率信息、比力信息、磁力计信息；进而根据***输出信息解算出未经误差补偿的个人定位信息；然后利用***输出数据及公式确定运动过程中的零速区间并根据零速区间对数据进行分段；在每个分段中使用改进的RTS平滑算法以得到误差修正后的个人定位信息。本发明在使用零速校正作为误差补偿修正算法的基础上，根据让个人行走时各传感器输出数据特点，利用零速区间对各数据进行分段，设计基于分段RTS平滑算法的个人自主导航***定位方法。在不增加***成本的条件下，提高了个人自主导航***定位精度；本发明方法简单，稳定性和可靠性高。

Description

基于分段RTS平滑算法的个人自主导航***定位方法

技术领域

本发明涉及一种导航***定位方法，尤其涉及一种基于分段RTS平滑算法的个人自主导航***定位方法。

背景技术

如今，个人自主导航***在西方已经成为单兵作战人员野外作业的必须品，个人自主导航***可以帮助快速反应部队等特殊兵种在已知或未知地域内快速了解地形，实现自身定位。因此，提高个人导航***的定位精度对提升全军技术实力意义重大。个人导航自主定位装置包括MEMS加速度计、MEMS磁力计、MEMS陀螺仪等，同其它惯性导航设备一样，工作时误差随时间发散严重，因此在***使用时必须设计有效的误差修正算法对惯性器件测量结果和导航输出进行修正。基于卡尔曼滤波的零速校正算法是一种常用的个人自主导航***误差修正算法。然而，零速校正只在个人自主导航***使用者脚步静态检测结果为静止时触发，即在脚步一步运动过程中，只有很短的时间用于修正误差，这将造成***误差的急剧修正现象，致使个人行走轨迹间断或突变。同时，单纯依靠基于卡尔曼滤波的零速校正算法修正的个人自主导航***定位精度较其它个人定位产品而言仍旧较低。

总体来说现有的以基于卡尔曼滤波的零速校正算法为误差修正算法的个人自主导航***存在急剧修正、轨迹不连续，导航精度相对较低难以满足个人导航精确可靠的要求等问题。

发明内容

本发明的目的就在于为了解决上述问题而提供一种基于分段RTS平滑算法的个人自主导航***定位方法。

本发明通过以下技术方案来实现上述目的：

本发明包括以下步骤：

(1)采集个人自主导航***正常工作模式下输出的角速率信息、比力信息、磁力计信息；

(2)根据***输出信息解算出未经误差补偿的个人定位信息；

(3)利用***输出数据及公式确定运动过程中的零速区间并根据零速区间对数据进行分段；

(4)在每个分段中使用改进的RTS平滑算法以得到误差修正后的个人定位信息，其中正向平滑滤波器的观测量为零速状态时步骤二中解的个人速度信息；逐个分段进行RTS平滑算法即可得到所有行走过程的平滑状态。

具体地，所述步骤(1)中，任意时刻k采集到的个人自主导航***输出信息为：

角速率信息： ${\mathrm{\ω}}^b\left(k\right)={\left[\begin{array}{ccc}{\mathrm{\ω}}^{b_x}\left(k\right)& {\mathrm{\ω}}^{b_y}\left(k\right)& {\mathrm{\ω}}^{b_z}\left(k\right)\end{array}\right]}^T;$

比力信息： $f^b\left(k\right)={\left[\begin{array}{ccc}{f}^{b_x}\left(k\right)& f^{b_y}\left(k\right)& f^{b_z}\left(k\right)\end{array}\right]}^T;$

磁力计信息： $B_k^n={\left[\begin{array}{cc}{B}_k^n\left(x\right)& B_k^n\left(y\right)\end{array}\right]}^T;$

其中：为当地地磁场在导航坐标系x、y轴方向上的分量，T表示转置操作；上标b表示载体坐标系，n表示导航坐标系；

所述步骤(2)中，利用公式：

aⁿ(t)=fⁿ(t)-gⁿ；vⁿ(t)=vⁿ(0)+∫aⁿ(t)dt；sⁿ(t)=sⁿ(0)+∫vⁿ(t)dt；

解算出未经误差补偿的个人定位信息；其中：为t时刻更新的方向余弦矩阵，其更新采用四元数法，aⁿ(t)为由载体的运动所产生的加速度，gⁿ=[0 0 g]，g为当地重力加速度，M_d为当地地球表面的磁偏角。

所述步骤(3)中，利用公式：

判断出行人自主导航***的零速区间，若上式成立则IMU静止；其中：N为步骤为中确定的窗口函数；Ω_n={l∈：n≤l<n+N-1}，||·||表示求范数，

ln(.)表示求以e为底数的对数，分别表示MEMS加速度计和MEMS陀螺仪的噪声方差，γ为设定精确度。

所述步骤(3)中，根据检测到的零速区间对数据进行分段，根据行人运动过程中一步运动中只有一个零速区间，因此定义一个零速区间的结束到下一个零速区间的结束为固定区间平滑的一个分段。

所述步骤(4)中，在每个分段中使用RTS平滑算法，包括正向滤波、反向平滑，正向卡尔曼滤波模型为：

$\left\{\begin{array}{c}{X}_n=F_{n,n-1}{X}_{n-1}+G_{n-1}{W}_{n-1}\\ Z_n=H_n{X}_n+N_n\end{array}\right.$ 在正向卡尔曼滤波算法中，实时存储卡尔曼滤波的各估计状态和滤波器的参数；其中：滤波器的观测量为个人自主导航***零速状态时步骤二中解算得到的速度量，F_n,n-1为t_n-1时刻至t_n时刻的一步转移阵，G_n-1为***噪声驱动阵，W_n-1为***激励噪声序列，H_n为量测阵，N_n为量测噪声序列，假定W_k-1和H_k是互相独立的零均值高斯白噪声，协方差阵分别为Q和R。

所述步骤(4)中，正向滤波公式为：

分段内非零速区间滤波只做时间更新：

${\hat{X}}_{n+1/n}=F_{n+1,n}{\hat{X}}_n;{\hat{P}}_{n+1/n}=F_n{P}_n{F}_n^T+G{Q}_n{G}^T$

分段内零速区间滤波做完整更新(时间更新+量测更新)：

$P_{n+1}^{-1}={\left({\hat{P}}_{n+1/n}\right)}^{-1}+{\left(H_{n+1}^T{R}_{n+1}^{-1}{H}_{n+1}\right)}^{-1};K=P_{n+1}{H}_{n+1}^T{R}_{n+1}^{-1};{\hat{X}}_{n+1}={\hat{X}}_{n+1/n}+K(Z_{n+1}-H_{n+1}{\hat{X}}_{n+1/n})$

反向平滑公式为：

${\hat{X}}_{n|N}={\hat{X}}_{n|n}+P_{n|n}{F}^T{P^{-1}}_{n+1|n}[{\hat{X}}_{n+1|N}-{\hat{X}}_{n+1|n}];$ P_n|N=P_n|n+P_n|nF^TP^-1 _n+1|n[P_n+1|N-P_n+1|n]A_n ^T。

本发明的有益效果在于：

本发明是一种基于分段RTS平滑算法的个人自主导航***定位方法，与现有技术相比，本发明在使用零速校正作为误差补偿修正算法的基础上，根据让个人行走时各传感器输出数据特点，利用零速区间对各数据进行分段，设计基于分段RTS平滑算法的个人自主导航***定位方法。在不增加***成本的条件下，提高了个人自主导航***定位精度；本发明方法简单，稳定性和可靠性高。

附图说明

图1是本发明是实施例提供的基于分段RTS平滑算法的个人自主导航***定位方法定位结果与未加RTS平滑算法时定位结果比较图。

具体实施方式

下面结合附图对本发明作进一步说明：

本发明包括以下步骤：

(1)采集个人自主导航***正常工作模式下输出的角速率信息、比力信息、磁力计信息；任意时刻k采集到的个人自主导航***输出信息为：

角速率信息： ${\mathrm{\&omega;}}^b\left(k\right)={\left[\begin{array}{ccc}{\mathrm{\&omega;}}^{b_x}\left(k\right)& {\mathrm{\&omega;}}^{b_y}\left(k\right)& {\mathrm{\&omega;}}^{b_z}\left(k\right)\end{array}\right]}^T;$

比力信息： $f^b\left(k\right)={\left[\begin{array}{ccc}{f}^{b_x}\left(k\right)& f^{b_y}\left(k\right)& f^{b_z}\left(k\right)\end{array}\right]}^T;$

磁力计信息： $B_k^n={\left[\begin{array}{cc}{B}_k^n\left(x\right)& B_k^n\left(y\right)\end{array}\right]}^T;$

其中：

为当地地磁场在导航坐标系x、y轴方向上的分量，T表示转置操作；上标b表示载体坐标系，n表示导航坐标系；

(2)根据***输出信息解算出未经误差补偿的个人定位信息；利用公式：

aⁿ(t)=fⁿ(t)-gⁿ；vⁿ(t)=vⁿ(0)+∫aⁿ(t)dt；sⁿ(t)=sⁿ(0)+∫vⁿ(t)dt；

解算出未经误差补偿的个人定位信息；其中：

为t时刻更新的方向余弦矩阵，其更新采用四元数法，aⁿ(t)为由载体的运动所产生的加速度，gⁿ=[0 0 g]，g为当地重力加速度，M_d为当地地球表面的磁偏角。

(3)利用***输出数据及公式确定运动过程中的零速区间并根据零速区间对数据进行分段；利用公式：

判断出行人自主导航***的零速区间，若上式成立则IMU静止；其中：N为步骤为中确定的窗口函数；Ω_n={l∈：n≤l<n+N-1}，||·||表示求范数，

ln(.)表示求以e为底数的对数，

分别表示MEMS加速度计和MEMS陀螺仪的噪声方差，γ为设定精确度，根据检测到的零速区间对数据进行分段，根据行人运动过程中一步运动中只有一个零速区间，因此定义一个零速区间的结束到下一个零速区间的结束为固定区间平滑的一个分段。

(4)在每个分段中使用改进的RTS平滑算法以得到误差修正后的个人定位信息，其中正向平滑滤波器的观测量为零速状态时步骤二中解的个人速度信息；逐个分段进行RTS平滑算法即可得到所有行走过程的平滑状态。在每个分段中使用RTS平滑算法，包括正向滤波、反向平滑，正向卡尔曼滤波模型为：

$\left\{\begin{array}{c}{X}_n=F_{n,n-1}{X}_{n-1}+G_{n-1}{W}_{n-1}\\ Z_n=H_n{X}_n+N_n\end{array}\right.$ 在正向卡尔曼滤波算法中，实时存储卡尔曼滤波的各估计状态和滤波器的参数；其中：滤波器的观测量为个人自主导航***零速状态时步骤二中解算得到的速度量，F_n,n-1为t_n-1时刻至t_n时刻的一步转移阵，G_n-1为***噪声驱动阵，W_n-1为***激励噪声序列，H_n为量测阵，N_n为量测噪声序列，假定W_k-1和H_k是互相独立的零均值高斯白噪声，协方差阵分别为Q和R，正向滤波公式为：

分段内非零速区间滤波只做时间更新：

${\hat{X}}_{n+1/n}=F_{n+1,n}{\hat{X}}_n;{\hat{P}}_{n+1/n}=F_n{P}_n{F}_n^T+G{Q}_n{G}^T$

分段内零速区间滤波做完整更新(时间更新+量测更新)：

$P_{n+1}^{-1}={\left({\hat{P}}_{n+1/n}\right)}^{-1}+{\left(H_{n+1}^T{R}_{n+1}^{-1}{H}_{n+1}\right)}^{-1};K=P_{n+1}{H}_{n+1}^T{R}_{n+1}^{-1};{\hat{X}}_{n+1}={\hat{X}}_{n+1/n}+K(Z_{n+1}-H_{n+1}{\hat{X}}_{n+1/n})$

反向平滑公式为：

${\hat{X}}_{n|N}={\hat{X}}_{n|n}+P_{n|n}{F}^T{P^{-1}}_{n+1|n}[{\hat{X}}_{n+1|N}-{\hat{X}}_{n+1|n}];$ P_n|N=P_n|n+P_n|nF^TP^-1 _n+1|n[P_n+1|N-P_n+1|n]A_n ^T。

逐个分段进行RTS平滑算法即可得到所有行走过程的平滑状态。

其中，滤波器的观测量为个人自主导航***零速状态时步骤二中解算得到的速度量，被估计状态向量为：

$X={\left[\begin{array}{ccc}{\mathrm{\&delta;\&phi;}}^T& \mathrm{\&delta;}{v}^T& {\mathrm{\&delta;s}}^T\end{array}\right]}^T$

量测向量，即速度误差：

$Z={\mathrm{\&delta;v}}^T={\left[\begin{array}{ccc}{v}_x& v_x& v_z\end{array}\right]}^T$

量测阵为：

$H=\left[\begin{array}{ccc}{0}_{3\&times;3}& I_{3\&times;3}& 0_{3\&times;3}\end{array}\right]$

状态转移矩阵为：

$F=\left[\begin{array}{ccc}I& 0& 0\\ \mathrm{\&Delta;tS}\left(f_t^n\right)& I& I\\ 0& \mathrm{\&Delta;tI}& I\end{array}\right]$

其中，状态估计包括：位置误差δs^T、速度误差δv^T、姿态误差δφ^T，以上每一项都是三维的。F_n,n-1为t_n-1时刻至t_n时刻的一步转移阵，G_n-1为***噪声驱动阵，W_n-1为***激励噪声序列，H_n为量测阵，N_n为量测噪声序列，以上每一项都是三维的，为沿地理系的载体运动加速度的反对称阵。假定W_k-1和H_k是互相独立的零均值高斯白噪声，协方差阵分别为Q和R。

结合以下实验对本发明的优益效果做进一步的说明：

利用自研三轴惯性测量组件(集成了微机械***三轴磁力计、加速度计、陀螺仪)搭建真实个人导航***模型，设备参数如表1所示，通过合理的零速检测试验验证基于分段RTS平滑算法的个人自主导航***定位方法在真实环境中的可靠性、实用性、准确性，试验场景选在室外空旷的哈尔滨工程大学军工操场，

表1自研微型惯性测量单元惯性测量组件各传感器性能指标

实验过程中相关参数设置如下：

个人导航自主定位***采样频率：100Hz

静态检测窗口函数：N=5

静态判断不等式参数：γ=0.5e⁵

实验开始前，测试者在实验场进行15分钟的***静止预热，完成***的初始化；

试验：

测试者在操场围绕方形花坛步行(1.05m/s)，实验过程中实时采集并保存微型惯性测量单元的输出数据；

将实验数据进行离线分析，为了比较验证本发明基于分段RTS平滑算法的个人自主导航***定位方法的定位结果，同时给出未平滑时个人自主导航***定位结果，

如图1所示：从图1可以看出，本发明基于分段RTS平滑算法的个人自主导航***定位方法可提高个人导航***定位精度，且有效避免了急剧误差修正问题，使行走轨迹连续性更强：。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于分段RTS平滑算法的个人自主导航***定位方法，其特征在于：包括以下步骤：

(1)采集个人自主导航***正常工作模式下输出的角速率信息、比力信息、磁力计信息；

(2)根据***输出信息解算出未经误差补偿的个人定位信息；

(3)利用***输出数据及公式确定运动过程中的零速区间并根据零速区间对数据进行分段；

(4)在每个分段中使用改进的RTS平滑算法以得到误差修正后的个人定位信息，其中正向平滑滤波器的观测量为零速状态时步骤二中解的个人速度信息；逐个分段进行RTS平滑算法即可得到所有行走过程的平滑状态。

2.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航***定位方法，其特征在于：所述步骤(1)中，任意时刻k采集到的个人自主导航***输出信息为：

角速率信息： ${\mathrm{\&omega;}}^b\left(k\right)={\left[\begin{array}{ccc}{\mathrm{\&omega;}}^{b_x}\left(k\right)& {\mathrm{\&omega;}}^{b_y}\left(k\right)& {\mathrm{\&omega;}}^{b_z}\left(k\right)\end{array}\right]}^T;$

比力信息： $f^b\left(k\right)={\left[\begin{array}{ccc}{f}^{b_x}\left(k\right)& f^{b_y}\left(k\right)& f^{b_z}\left(k\right)\end{array}\right]}^T;$

磁力计信息： $B_k^n={\left[\begin{array}{cc}{B}_k^n\left(x\right)& B_k^n\left(y\right)\end{array}\right]}^T;$

其中：

为当地地磁场在导航坐标系x、y轴方向上的分量，T表示转置操作；上标b表示载体坐标系，n表示导航坐标系。

3.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航***定位方法，其特征在于：所述步骤(2)中，利用公式：

aⁿ(t)=fⁿ(t)-gⁿ；vⁿ(t)=vⁿ(0)+∫aⁿ(t)dt；sⁿ(t)=sⁿ(0)+∫vⁿ(t)dt；

解算出未经误差补偿的个人定位信息；其中：

为t时刻更新的方向余弦矩阵，其更新采用四元数法，aⁿ(t)为由载体的运动所产生的加速度，gⁿ=[0 0 g]，g为当地重力加速度，M_d为当地地球表面的磁偏角。

4.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航***定位方法，其特征在于：所述步骤(3)中，利用公式：

判断出行人自主导航***的零速区间，若上式成立则IMU静止；其中：N为步骤为中确定的窗口函数；Ω_n={l∈：n≤l<n+N-1}，||·||表示求范数，ln(.)表示求以e为底数的对数，

分别表示MEMS加速度计和MEMS陀螺仪的噪声方差，γ为设定精确度。

5.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航***定位方法，其特征在于：所述步骤(3)中，根据检测到的零速区间对数据进行分段，根据行人运动过程中一步运动中只有一个零速区间，因此定义一个零速区间的结束到下一个零速区间的结束为固定区间平滑的一个分段。

6.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航***定位方法，其特征在于：所述步骤(4)中，在每个分段中使用RTS平滑算法，包括正向滤波、反向平滑，正向卡尔曼滤波模型为：

$\left\{\begin{array}{c}{X}_n=F_{n,n-1}{X}_{n-1}+G_{n-1}{W}_{n-1}\\ Z_n=H_n{X}_n+N_n\end{array}\right.,$ 在正向卡尔曼滤波算法中，实时存储卡尔曼滤波的各估计状态和滤波器的参数；其中：滤波器的观测量为个人自主导航***零速状态时步骤二中解算得到的速度量，F_n，n-1为t_n-1时刻至t_n时刻的一步转移阵，G_n-1为***噪声驱动阵，W_n-1为***激励噪声序列，H_n为量测阵，N_n为量测噪声序列，假定W_k-1和H_k是互相独立的零均值高斯白噪声，协方差阵分别为Q和R。

7.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航***定位方法，其特征在于：所述步骤(4)中，正向滤波公式为：

分段内非零速区间滤波只做时间更新：

${\hat{X}}_{n+1/n}=F_{n+1,n}{\hat{X}}_n;{\hat{P}}_{n+1/n}=F_n{P}_n{F}_n^T+G{Q}_n{G}^T$

分段内零速区间滤波做完整更新(时间更新+量测更新)：

$P_{n+1}^{-1}={\left({\hat{P}}_{n+1/n}\right)}^{-1}+{\left(H_{n+1}^T{R}_{n+1}^{-1}{H}_{n+1}\right)}^{-1};K=P_{n+1}{H}_{n+1}^T{R}_{n+1}^{-1};{\hat{X}}_{n+1}={\hat{X}}_{n+1/n}+K(Z_{n+1}-H_{n+1}{\hat{X}}_{n+1/n})$

反向平滑公式为：

${\hat{X}}_{n|N}={\hat{X}}_{n|n}+P_{n|n}{F}^T{P^{-1}}_{n+1|n}[{\hat{X}}_{n+1|N}-{\hat{X}}_{n+1|n}];$ P_n|N=P_n|n+P_n|nF^TP^-1 _n+1|n[P_n+1|N-P_n+1|n]A_n ^T。

Images