一种行人自主导航定位的自适应卡尔曼滤波方法

文档序号:9324523阅读:2435来源:国知局
一种行人自主导航定位的自适应卡尔曼滤波方法
【技术领域】
[0001] 本发明属于导航定位技术领域,尤其涉及一种行人自主导航定位的自适应卡尔曼 滤波方法。
【背景技术】
[0002] 近些年来导航定位技术依然在迅猛发展,而其中多数是基于卫星的导航系统,并 且只适用于室外的环境,如应用最广泛的GPS,在城市楼群、山区、森林和地下建筑等遮挡环 境下,其信号弱,可用性差。
[0003] 随着人们生活节奏的加快,行人自主导航定位的需求也显得越发迫切。尤其是 室内环境下,如火灾、楼宇坍塌等紧急环境的救援、商场内需要寻人或某个地方等。很多 人采用地图信息匹配的方法进行行人导航,如国外专利CN1705861A《Walker navigation device and program》,其利用存储在地图数据库中的地图信息来计算当前位置并显示。这 种方法需要先验的预知信息,对于火灾救援等临时突发紧急情况难以达到良好的处理效 果。本发明中应用MEMS-頂U固连于人体对行人进行室内实时定位,不需要依赖任何外部的 信息,可以实现全自主定位,并且价格便宜,体积小,容易实现可穿戴。利用惯性传感器实现 行人定位的经典方法是行人航位推算(PDR),其中大体可分为两类:一种是通过一步计数 和步长估计来实现定位,通常用陀螺仪/罗盘来增强航向信息。如专利CN201310388466. 9 《行人步长估计及航位推算方法》中所述,利用步数、步长、方向进行测量和计算。但是此 种方式往往只适用于特定的用户,在不寻常的行走模式,包括拥挤环境、上坡/下坡等环境 中,步行统计假设条件会被破坏,因此容易失败。另一种方法是采用直接积分策略,但是随 着行人移动距离和航向角的增加,惯性器件累计误差会越来越大,导致其定位精度不理想。 在考虑系统实时性及计算复杂度的前提下,通常采用卡尔曼滤波来消除漂移误差。
[0004] 现有的用于行人导航系统的滤波方法主要采用零速修正的方法,而后续跟进的滤 波一般均采取基础的经典卡尔曼滤波或扩展卡尔曼滤波。如国内编号为CN201310566710 的专利《基于捷联惯导解算和零速校正的自主导航系统定位方法》中所用的是经典卡尔曼 滤波,其通过判断人脚处于运动还是静止状态来区分更新选项。当检测为运动时,只启动时 间更新;当检测为静止时,则同时启动时间更新和量测更新。经典卡尔曼滤波是将系统默认 为线性系统来进行更新处理,而人体运动必然会伴有摆动等干扰因素,显然是非线性系统, 所以若应用经典卡尔曼滤波进行估计,修正效果并不理想。再如专利CN201310520233《一 种基于双MEMS-頂U的行人自主导航解算算法》中以双脚解算距离超过双脚间最大步长为触 发点,利用EKF完成估计修正,即用导航解算的协方差阵来更新卡尔曼滤波的协方差阵。此 方法是将系统局部线性化,比起经典卡尔曼滤波的方法更符合行人运动的实际情况,因此 定位精度有所提升,但仍然没解决噪声实时变化的影响问题,并且其每次更新都必须通过 计算雅可比矩阵才能完成,实时性变差。
[0005] 综上所述,在某种程度上本发明从成本、可靠性、复杂性以及精确性等角度都表现 出了其它相关研究不具备或不同时具备的新颖性和先进性。

【发明内容】

[0006] 本发明所要解决的技术问题在于克服现有技术的不足,提供一种行人自主导航定 位的自适应卡尔曼滤波方法,该方法简单、稳定性强,在不增加任何额外硬件成本的情况 下,有效提1? 了彳丁人导航定位的精度。
[0007] 本发明采用以下技术方案:
[0008] -种行人自主导航定位的自适应卡尔曼滤波方法,包括以下步骤:
[0009] 步骤(1),将MEMS-頂U系统固定连接于人体,頂U初始对准后,行人开始运动,运动 过程中采集IMU数据;
[0010] 步骤(2),合理选取观测量建立融合人体运动特征的自适应卡尔曼滤波模型;
[0011] 步骤(3),同时满足"四条件"时进行自适应卡尔曼滤波,同步修正晃动噪声干扰, 完成各状态量的估计。
[0012] 步骤(4),利用估计误差修正运动人体信息。
[0013] 进一步的,在步骤(1)中,頂U输出数据是通过固连于人体上的测量传感器件测量 所得;测量传感器件包括三轴加速度计、三轴陀螺仪和三轴磁力计。
[0014] 进一步的,在步骤(1)中的初始对准选取北东地为导航坐标系,俯仰角Θ和滚转 角Y由经过补偿后的加速度计输出值计算获得,偏航角供由磁场强度B、俯仰角Θ和滚转 角Y计算获得。计算公式为:
[0015] 其中,k代表k时刻,b代表载体(行人)坐标系,η代表导航坐标系,4表示k时 刻载体坐标系下行人的加速度,S4"表示k时刻导航坐标系下的磁场强度。
[0016] 进一步的,在步骤(2)中,滤波模型为包括3个位置误差、3个姿态角误差(俯仰角 Θ、滚转角Y和偏航角P )、3个速度误差、3个重力不确定误差、三轴陀螺仪零偏误差和三 轴加速度计零偏误差,共计18维的状态空间方程。
[0018] W(t)为系统过程噪声矩阵,其表达式为:
;G(t)为 对应的噪声矩阵系数;F(t)为系统状态矩阵;Z(t)为观测量;H(t)为观测矩阵;V(t)为观 测噪声矩阵。
[0019] 被估计状态向量为:
[0020] 其中,δ r为位置误差,δ V为速度误差,δ ψ为姿态角误差,\^为加速度计零偏 误差,eb为陀螺仪零偏误差,Sg为重力不确定误差。
[0021] 观测向量和观测矩阵为:
[0022] Zk= [Avk Δω, Δ ψ,]τ
[0024] 其中,Δ urk包括磁力计计算的航向角与捷联解算出的航向角作差获得的航向角 误差,利用加速度计计算出的俯仰角和横滚角与捷联解算出的姿态角作差获得的俯仰角和 横滚角误差。
[0025] 进一步的,所述的步骤(3)中的"四条件"包括:
[0026] (1)三轴加速度计合成的加速度幅值输出量满足阈值条件;
[0027] (2)加速度计z轴加速度输出量满足阈值条件;
[0028] (3)三轴陀螺仪合成的角速度幅值输出量满足阈值条件;
[0029] (4)陀螺仪y轴角速度输出量满足阈值条件;
[0030] 当上述四项条件同时满足时,进行自适应滤波。
[0031] 其中,条件(1)中三轴加速度计合成的加速度幅值表示为
,定义给定的阈值条件为th amin= 9m/ s2< I a k I < th_x= Ilm/s 2。条件(3)中三轴陀螺仪合成的角速度幅值表示为
,定义给定的阈值条件为I ?k| < th"max= 50° /s〇
[0032] 在行人运动过程中,z轴加速度和y轴角速度变化最明显,由于安装偏差、行走晃 动等影响,頂U会有所倾斜。因此在静止时刻y轴角速度不是完全零值,z轴加速度也不是 完全g值,但都不会大于所设定阈值。条件(2)和条件(4)中的阈值在初始静止阶段分别 以取加速度计输出的平均值和陀螺仪输出的平均值而设定。
[0033] 进一步的,在步骤(3)中,采用自适应卡尔曼滤波进行误差估计,利用观测数据 进行递推滤波的同时,通过时变噪声统计估计器实时估计和修正系统噪声以及观测噪声的 统计特性,从而降低模型误差、抑制滤波发散,提高滤波精度。选取遗忘因子b = 0.98。
[0034] 进一步的,在步骤(4)中,通过公式 反馈修正行人速度、位置和姿态信息。
[0035] 本发明比现有技术具有以下有益效果及优点:
[0036] (1)本发明可穿戴,不需要预设节点,比其它需要预先知道室内布局环境的定位方 法更方便,适用性更广。另外,相比于其它基于WiFi、Zigbee等无线网络的室内定位技术, 本发明稳定性更高。
[0037] (2)本发明针对人体运动特性,采用的"四条件"识别方法简单,实时性高,起到良 好的触发作用。比应用步长限制条件判断零瞬态的方法应用范围更广,不针对指定用户。
[0038] (3)在不增加任何额外硬件成本的情况下,利用自适应卡尔曼滤波器解决惯性器 件长时间工作产生的累积漂移影响定位精度的问题。自适应体现在利用观测数据进行递推 滤波的同时,用时变噪声统计估计器实时估计和修正系统噪声以及观测噪声的统计特性, 从而降低模型误差、抑制滤波发散,提高滤波精度。此方法针对行人运动这种噪声实时变换 的情况有良好的效果。
【附图说明】
[0039] 图1为本发明的流程图
[0040] 图2为本发明的工作原理图;
[0041] 图3为自适应卡尔曼滤波框图;
[0042] 图4为行人自主导航定位的自适应卡尔曼滤波方法实施例结果对比图。
【具体实施方式】
[0043] 下面结合附图和【具体实施方式】对本发明作进一步描述:
[0044] 图1是本发明的流程图,其中,数据采集的頂U模块(1-1)包括加速度计、陀螺 仪和磁力计。将数据进行捷联惯性导航解算(1-2),之后选取合适观测量得到滤波模型 (1-3),随后用"四条件"触发AKF模块(1-4)进行滤波。人体在正常运动过程中,与地面接 触时间为毫秒级或更长,其中某一时刻人体与地面接触会达到瞬间静止状态,当同时满足 所述"四条件"时即为瞬间静止状态。AKF指自适应卡尔曼滤波,AKF模块实现对系统噪声 以及观测噪声统计特性的实时估计,从而降低模型误差、抑制滤波发散,提高滤波精度。最 后,用估计量完成定位信息的校正(1-5)。
[0045] 图2是本发明的工作原理图。其中,MEMS-頂U(2-1)包括三轴加速度计、三轴陀螺 仪和三轴磁力计。加速度计输出加速度信息(2-2),陀螺仪输出角速度信息(2-3),将以上 两种直接输出信息进行捷联惯性导航解算(2-4),可得到位置、速度、姿态信息(2-5)。当检 测到行人处于瞬间静止状态(2-7),即触发滤波器,以零速度修正量(2-8)、零角速度修正 量(2-9)、姿态角修正量(2-6)同时成为观测量,利用自适应卡尔曼滤波器(2-10)进行估 计,随后进行反馈修正(2-11),得到最终输出的位置、速度、姿态信息(2-12)。
[0046] -种行人自主导航定位的自适应卡尔曼滤波方
当前第1页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1