基于分段rts平滑算法的个人自主导航系统定位方法
【专利摘要】本发明公开了一种基于分段RTS平滑算法的个人自主导航系统定位方法，先采集个人自主导航系统正常工作模式下输出的角速率信息、比力信息、磁力计信息；进而根据系统输出信息解算出未经误差补偿的个人定位信息；然后利用系统输出数据及公式确定运动过程中的零速区间并根据零速区间对数据进行分段；在每个分段中使用改进的RTS平滑算法以得到误差修正后的个人定位信息。本发明在使用零速校正作为误差补偿修正算法的基础上，根据让个人行走时各传感器输出数据特点，利用零速区间对各数据进行分段，设计基于分段RTS平滑算法的个人自主导航系统定位方法。在不增加系统成本的条件下，提高了个人自主导航系统定位精度；本发明方法简单，稳定性和可靠性高。
【专利说明】基于分段RTS平滑算法的个人自主导航系统定位方法
【技术领域】
[0001]本发明涉及一种导航系统定位方法，尤其涉及一种基于分段RTS平滑算法的个人自主导航系统定位方法。
【背景技术】
[0002]如今，个人自主导航系统在西方已经成为单兵作战人员野外作业的必须品，个人自主导航系统可以帮助快速反应部队等特殊兵种在已知或未知地域内快速了解地形，实现自身定位。因此，提高个人导航系统的定位精度对提升全军技术实力意义重大。个人导航自主定位装置包括MEMS加速度计、MEMS磁力计、MEMS陀螺仪等，同其它惯性导航设备一样，工作时误差随时间发散严重，因此在系统使用时必须设计有效的误差修正算法对惯性器件测量结果和导航输出进行修正。基于卡尔曼滤波的零速校正算法是一种常用的个人自主导航系统误差修正算法。然而，零速校正只在个人自主导航系统使用者脚步静态检测结果为静止时触发，即在脚步一步运动过程中，只有很短的时间用于修正误差，这将造成系统误差的急剧修正现象，致使个人行走轨迹间断或突变。同时，单纯依靠基于卡尔曼滤波的零速校正算法修正的个人自主导航系统定位精度较其它个人定位产品而言仍旧较低。
[0003]总体来说现有的以基于卡尔曼滤波的零速校正算法为误差修正算法的个人自主导航系统存在急剧修正、轨迹不连续，导航精度相对较低难以满足个人导航精确可靠的要求等问题。
【发明内容】
[0004]本发明的目的就在于为了解决上述问题而提供一种基于分段RTS平滑算法的个人自主导航系统定位方法。
[0005]本发明通过以下技术方案来实现上述目的:
[0006]本发明包括以下步骤:
[0007](I)采集个人自主导航系统正常工作模式下输出的角速率信息、比力信息、磁力计
信息；
[0008](2)根据系统输出信息解算出未经误差补偿的个人定位信息；
[0009](3)利用系统输出数据及公式确定运动过程中的零速区间并根据零速区间对数据进行分段；
[0010](4)在每个分段中使用改进的RTS平滑算法以得到误差修正后的个人定位信息，其中正向平滑滤波器的观测量为零速状态时步骤二中解的个人速度信息；逐个分段进行RTS平滑算法即可得到所有行走过程的平滑状态。
[0011]具体地，所述步骤(I)中，任意时刻k采集到的个人自主导航系统输出信息为:
[0012]角速率信息⑷= ⑷ahy{k) Z⑷];;
[0013]比力信息:/(々)=[产(A:)fHk.)产㈨]';[0014]
【权利要求】
1.一种基于分段RTS平滑算法的个人自主导航系统定位方法，其特征在于:包括以下步骤: (1)采集个人自主导航系统正常工作模式下输出的角速率信息、比力信息、磁力计信息； (2)根据系统输出信息解算出未经误差补偿的个人定位信息； (3)利用系统输出数据及公式确定运动过程中的零速区间并根据零速区间对数据进行分段； (4)在每个分段中使用改进的RTS平滑算法以得到误差修正后的个人定位信息，其中正向平滑滤波器的观测量为零速状态时步骤二中解的个人速度信息；逐个分段进行RTS平滑算法即可得到所有行走过程的平滑状态。
2.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航系统定位方法，其特征在于:所述步骤(1)中，任意时刻k采集到的个人自主导航系统输出信息为:
3.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航系统定位方法，其特征在于:所述步骤(2)中，利 用公式:
r (O = c； (t)fb (O ； an (t) =fn (t) -gn ； vn (t) =vn (O) + f an(t)dt ；sn(t) =Sn(O) + f vn(t)dt ；ψ磁紙=-_?(β:ω/β:(χ))-札解算出未经误差补偿的个人定位信息；其中:c:(t)为t时刻更新的方向余弦矩阵，其更新采用四元数法，an(t)为由载体的运动所产生的加速度，gn=[0O g]，g为当地重力加速度，Md为当地地球表面的磁偏角。
4.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航系统定位方法，其特征在于:所述步骤(3)中，利用公式: 去
5.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航系统定位方法，其特征在于:所述步骤(3)中，根据检测到的零速区间对数据进行分段，根据行人运动过程中一步运动中只有一个零速区间，因此定义一个零速区间的结束到下一个零速区间的结束为固定区间平滑的一个分段。
6.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航系统定位方法，其特征在于:所述步骤(4)中，在每个分段中使用RTS平滑算法，包括正向滤波、反向平滑，正向卡尔曼滤波模型为:
7.根据权利要求1所述的基于分段RTS平滑算法的个人自主导航系统定位方法，其特征在于:所述步骤(4)中，正向滤波公式为: 分段内非零速区间滤波只做时间更新:
【文档编号】G01C25/00GK103644910SQ201310589676
【公开日】2014年3月19日 申请日期:2013年11月22日 优先权日:2013年11月22日
【发明者】于飞, 于春阳, 兰海钰, 周广涛, 刘凤, 卢宝峰, 史宏洋, 赵博, 白红美, 林萌萌, 姜鑫, 梁宏 申请人:哈尔滨工程大学