CN109945864A - 室内行车定位融合方法、装置、存储介质及终端设备 - Google Patents
室内行车定位融合方法、装置、存储介质及终端设备 Download PDFInfo
- Publication number
- CN109945864A CN109945864A CN201910138099.4A CN201910138099A CN109945864A CN 109945864 A CN109945864 A CN 109945864A CN 201910138099 A CN201910138099 A CN 201910138099A CN 109945864 A CN109945864 A CN 109945864A
- Authority
- CN
- China
- Prior art keywords
- matrix
- vector
- state
- certain moment
- error
- 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
Landscapes
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种室内行车定位融合方法、装置、存储介质及终端设备,所述方法包括:获取外部节点定位坐标和内部测量角度,初始化卡尔曼滤波器相关参数,根据当前时刻与上个时刻的状态变化为特征调整速度矢量,根据调整得到的新速度矢量预测状态向量和误差协方差,进而进行测量更新。本发明中避开了直接使用状态作为观测对象,而是根据汽车行驶的固有特点更加精确地利用惯性信息,利用垂直于行进方向的速度不确定性低的特点调整速度矢量,使融合***的鲁棒性和实时性大大提高,解决了目前室内行车定位直接采用惯性信息导致定位整体表现劣化的问题。
Description
技术领域
本发明涉及本发明涉及室内行车定位技术领域,尤其涉及一种室内行车定位融合方法、装置、存储介质及终端设备。
背景技术
目前,室内行车定位技术通常在室内区域内均匀布置若干个蓝牙装置用于发射蓝牙信号,实际应用时当手机获取到足够数量满足强度要求的节点后,运用三角定位、指纹分析、近邻法等方法估计出手机的方位信息,再使用卡尔曼滤波器融合手机自身采集到的惯性和方向,利用历史数据使轨迹更加平滑。但是,这种室内行车定位方法却存在一些缺陷,具体而言。目前手机等消费级智能终端内置的磁场传感器、加速度计和陀螺仪的精度有限,且室内环境存在诸多丰富的地磁干扰,因此对手机的姿态判定存在较大误差。目前的融合算法中采用的手机惯性信息都是未经过检验处理而直接采用的,其不准确性有时反而会劣化定位的整体表现。故而,目前的室内行车定位需要一种根据行车方式对惯性进行处理后进行融合以保证定位效果的定位技术。
发明内容
本发明提供了一种室内行车定位融合方法、装置、存储介质及终端设备,旨在解决目前室内行车定位直接采用惯性信息导致定位整体表现劣化的问题。
为了解决上述技术问题,本发明实施例提供了一种室内行车定位融合方法,包括:
获取外部节点定位坐标和内部测量角度;
初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q0、初始状态的误差协方差矩阵P0、测量协方差矩阵R,选择***模型,设定卡尔曼滤波器的***矩阵F,所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标;
根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rotk,根据旋转矩阵Rotk调整原速度矢量vk得到新速度矢量vk r=Rotkvk;
根据***模型得到***激励矩阵Bk和噪音激励矩阵Nk,根据所述***激励矩阵Bk与新速度矢量vk r的积结合噪音激励矩阵Nk得到激励值,根据某时刻的***矩阵Fk和上一时刻的状态向量sk-1结合激励值得到某时刻的预测状态向量sk-1 k;
根据某时刻的***矩阵Fk、***激励矩阵Bk和状态更新方程的误差协方差矩阵Qk,通过上一时刻的误差协方差矩阵Pk-1得到某时刻的预测误差协方差矩阵Pk-1 k;
根据外部节点定位坐标确定某时刻的测量向量zk和对应的测量矩阵H,计算某时刻的卡尔曼增益Kk,并更新某时刻的误差协方差矩阵Pk和某时刻的状态向量sk。
进一步地,所述初始的状态更新方程的误差协方差矩阵Q0设定为
进一步地,所述***模型选定为匀速运动模型,所述卡尔曼滤波器的***矩阵所述dt为状态更新的时间间隔,根据***模型得到***激励矩阵和噪音激励矩阵所述dtk为某时刻k与上一时刻k-1之间的时间差。
进一步地,所述旋转矩阵Rotk通过以下步骤得到:
采纳某时刻与上一时刻针对方向的状态变化差dθk;
根据状态变化差dθk构成旋转矩阵
进一步地,所述预测状态向量sk-1 k通过以下步骤得到:
根据***模型得到***激励矩阵Bk和噪音激励矩阵Nk,根据所述***激励矩阵Bk与新速度矢量vk r的积Bkvk r结合噪音激励矩阵Nk得到激励值Bkvk r+Nk;
根据某时刻的***矩阵Fk和上一时刻的状态向量sk-1计算得到原预测状态向量Fksk-1;
根据激励值Bkvk r调整原预测状态向量Fksk-1得到预测状态向量sk-1 k,计算式为sk-1 k=Fksk-1+Bkvk r+Nk。
进一步地,所述外部节点定位坐标通过以下步骤获得:
使节点在室内可通行区域内间隔一定距离的均匀布置,所述一定距离为节点蓝牙设备的信号稳定距离;
进入可行区域获取多个节点信号强度,筛选强度满足阈值的节点进行坐标估算,获得预估的外部节点定位坐标。
为了解决上述技术问题,本发明实施例还提供了一种室内行车定位融合装置,包括:
数据获取单元,获取外部节点定位坐标和内部测量角度;
初始化设定单元,初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q0、初始状态的误差协方差矩阵P0、测量协方差矩阵R,选择***模型,设定卡尔曼滤波器的***矩阵F,所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标;
速度矢量调整单元,根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rotk,根据旋转矩阵Rotk调整原速度矢量vk得到新速度矢量vk r=Rotkvk;
状态向量预测单元,根据***模型得到***激励矩阵Bk和噪音激励矩阵Nk,根据所述***激励矩阵Bk与新速度矢量vk r的积结合噪音激励矩阵Nk得到激励值;根据某时刻的***矩阵Fk和上一时刻的状态向量sk-1结合激励值得到某时刻的预测状态向量sk-1 k;
误差协方差预测单元,根据某时刻的***矩阵Fk、***激励矩阵Bk和状态更新方程的误差协方差矩阵Qk,通过上一时刻的误差协方差矩阵Pk-1得到某时刻的预测误差协方差矩阵Pk-1 k;
测量更新单元,根据外部节点定位坐标确定某时刻的测量向量zk和对应的测量矩阵H,计算某时刻的卡尔曼增益Kk,并更新某时刻的误差协方差矩阵Pk和某时刻的状态向量sk。
为了解决上述技术问题,本发明实施例还提供了一种计算机可读存储介质,所述计算机可读存储介质包括存储的计算机程序;其中,所述计算机程序在运行时控制所述计算机可读存储介质所在的设备执行上述实施例任一项所述的室内行车定位融合方法。
为了解决上述技术问题,本发明实施例还提供了一种终端设备,包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序,所述处理器在执行所述计算机程序时实现上述实施例任一项所述的室内行车定位融合方法。
与现有技术相比,本发明公开的一种室内行车定位融合方法、装置、计算机可读存储介质及终端设备,在获取外部节点定位坐标和内部测量角度后并不直接将其使用在卡尔曼滤波器中,还对速度矢量进行调整,即采纳当前时刻与上个时刻的状态变化差作为特征,避免误差累计而导致数据恶化。具体而言,本发明在获取数据和初始化卡尔曼滤波器后,并不直接将获取的数据在卡尔曼滤波器中进行融合,而是对获取的数据进行检验处理,利用传感器在短时间内的状态变化量反馈准确的特点,选用两个时刻间的变化差作为特征以避免误差的积累,利用特征结合汽车行驶的固有特点去修正获取的数据(速度矢量),通过修正后的速度矢量去预测状态向量和误差协方差,起到了修正预测状态向量和误差协方差的作用,而通过修正后的预测状态向量和误差协方差去更新测量数据,则起到了修正测量数据,保证定位准确性的作用。本发明中避开了直接使用状态作为观测对象,而是根据汽车行驶的固有特点更加精确地利用惯性信息,利用垂直于行进方向的速度不确定性低的特点调整速度矢量,使融合***的鲁棒性和实时性大大提高,解决了目前室内行车定位直接采用惯性信息导致定位整体表现劣化的问题,提供了一种对惯性信息进行筛选和处理后,提取其中与预估定位信息较为吻合的信息再进行融合的室内行车定位融合方法。
附图说明
图1是本发明提供的一种室内行车定位融合方法的一个优选实施例的流程图;
图2是本发明提供的一种室内行车定位融合装置的一个优选实施例的结构框图;
图3是本发明提供的一种终端设备的一个优选实施例的结构框图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本技术领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
如图1所示,本发明所述一种室内行车定位融合方法,包括:
获取外部节点定位坐标和内部测量角度;
初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q0、初始状态的误差协方差矩阵P0、测量协方差矩阵R,选择***模型,设定卡尔曼滤波器的***矩阵F,所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标;
根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rotk,根据旋转矩阵Rotk调整原速度矢量vk得到新速度矢量vk r=Rotkvk;
根据***模型得到***激励矩阵Bk和噪音激励矩阵Nk,根据所述***激励矩阵Bk与新速度矢量vk r的积结合噪音激励矩阵Nk得到激励值,根据某时刻的***矩阵Fk和上一时刻的状态向量sk-1结合激励值得到某时刻的预测状态向量sk-1 k;
根据某时刻的***矩阵Fk、***激励矩阵Bk和状态更新方程的误差协方差矩阵Qk,通过上一时刻的误差协方差矩阵Pk-1得到某时刻的预测误差协方差矩阵Pk-1 k;
根据外部节点定位坐标确定某时刻的测量向量zk和对应的测量矩阵H,计算某时刻的卡尔曼增益Kk,并更新某时刻的误差协方差矩阵Pk和某时刻的状态向量sk。
本实施例在获取外部节点定位坐标和内部测量角度后并不直接将其使用在卡尔曼滤波器中,还对速度矢量进行调整,即采纳当前时刻与上个时刻的状态变化差作为特征,避免误差累计而导致数据恶化。具体而言,本发明在获取数据和初始化卡尔曼滤波器后,并不直接将获取的数据在卡尔曼滤波器中进行融合,而是对获取的数据进行检验处理,利用传感器在短时间内的状态变化量反馈准确的特点,选用两个时刻间的变化差作为特征以避免误差的积累,利用特征结合汽车行驶的固有特点去修正获取的数据(速度矢量),通过修正后的速度矢量去预测状态向量和误差协方差,起到了修正预测状态向量和误差协方差的作用,而通过修正后的预测状态向量和误差协方差去更新测量数据,则起到了修正测量数据,保证定位准确性的作用。本发明中避开了直接使用状态作为观测对象,而是根据汽车行驶的固有特点更加精确地利用惯性信息,利用垂直于行进方向的速度不确定性低的特点调整速度矢量,使融合***的鲁棒性和实时性大大提高,解决了目前室内行车定位直接采用惯性信息导致定位整体表现劣化的问题,提供了一种对惯性信息进行筛选和处理后,提取其中与预估定位信息较为吻合的信息再进行融合的室内行车定位融合方法。
在另一个优选实施例中,所述初始的状态更新方程的误差协方差矩阵Q0设定为在本实施例中,定位功能的初期,由于***缺少足够的历史数据,可以选择更相信蓝牙/wifi定位所获得的结果,因此可以状态更新方程的误差协方差设定一个较大的值而随着定位时间的逐步延长,让其随着滤波器迭代过程收敛。需要注意的是,车行导航的状态更新方程的误差协方差的对角项并不相等。具体而言,由于车辆行驶本身的特性所致,实际场景中车辆无法在垂直于行进方向的水平面横移的,因此当转弯的偏移角度已经通过上一步骤补偿后,下一时刻的速度变化只会沿着车头面朝的方向发生变化,而其垂直于行进方向的水平面的速度和速度变化都应该趋近于零。换而言之,沿着行进方向的速度不确定性较高,垂直于行进方向的速度不确定性较低。因此对车行的状态方程而言,在运动方向上的对应加减速误差的方差a2,应该远大于对应于垂直于运动方向的误差b2(理论上应该为零)的方差,故而Q0的对角项并不相等。在k时刻的状态误差协方差矩阵如下,其中a>>b,Rot'k对应当前时刻速度方向θk的旋转矩阵。QK代表对速度不确定性的描述
在另一个优选实施例中,所述***模型选定为匀速运动模型,所述卡尔曼滤波器的***矩阵所述dt为状态更新的时间间隔,根据***模型得到***激励矩阵和噪音激励矩阵所述dtk为某时刻k与上一时刻k-1之间的时间差。本实施例中,***模型选定为匀速运动模型根据匀速运动模型确定卡尔曼滤波器的***矩阵F,***激励矩阵Bk和噪音激励矩阵Nk,从而根据***矩阵F,***激励矩阵Bk和噪音激励矩阵Nk实现对预测状态向量的修正,进一步保证了定位效果。
在另一个优选实施例中,所述旋转矩阵Rotk通过以下步骤得到:采纳某时刻与上一时刻针对方向的状态变化差dθk;根据状态变化差dθk构成旋转矩阵
本实施例中,在短时间内手机传感器对状态变化量的反馈是比较准确的,但由于其状态的推算包含了在时间上的积分,小度量的误差会逐渐累加的最后导致数据的恶化。正是利用这种特性,本发明避开直接使用手机的状态作为观测方程,而是采用短时间切片为单位,只采纳当前时刻与上个时刻的状态变化差作为特征。例如针对方向的变化,可以构成旋转矩阵,再利用旋转矩阵去调整原方程中的速度矢量vk=[vxk vyk],获得更准确的旋转相应。vk r=Rotkvk。其中vk r代表根据时间切片感知的方向变化对原速度进行旋转以后得到新的速度矢量。由于车辆行驶本身的特性所致,实际场景中车辆无法在垂直于行进方向的水平面横移的,因此当转弯的偏移角度已经通过上一步骤补偿后,下一时刻的速度变化只会沿着车头面朝的方向发生变化,而其垂直于行进方向的水平面的速度和速度变化都应该趋近于零。换而言之,沿着行进方向的速度不确定性较高,垂直于行进方向的速度不确定性较低。因此新的速度矢量vk r不需考虑其他方向的加速度的投影,故而vk r=Rotkvk。本实施例中避开了直接使用状态作为观测对象,而是根据汽车行驶的固有特点更加精确地利用惯性信息,利用垂直于行进方向的速度不确定性低的特点调整速度矢量,使融合***的鲁棒性和实时性大大提高。
在另一个优选实施例中,所述预测状态向量sk-1 k通过以下步骤得到:根据***模型得到***激励矩阵Bk和噪音激励矩阵Nk,根据所述***激励矩阵Bk与新速度矢量vk r的积Bkvk r结合噪音激励矩阵Nk得到激励值Bkvk r+Nk;根据某时刻的***矩阵Fk和上一时刻的状态向量sk-1计算得到原预测状态向量Fksk-1;根据激励值Bkvk r调整原预测状态向量Fksk-1得到预测状态向量sk-1 k,计算式为sk-1 k=Fksk-1+Bkvk r+Nk。本实施例中预测状态向量sk-1 k融合了激励值,不但是根据***矩阵Fk和上一时刻的状态向量sk-1计算得的,通过融合激励值修正了本实施例中的预测状态向量sk-1 k避免了直接使用测量数据带来的不良影响,进一步保障了定位效果。
在另一个优选实施例中,所述外部节点定位坐标通过以下步骤获得:使节点在室内可通行区域内间隔一定距离的均匀布置,所述一定距离为节点蓝牙设备的信号稳定距离;进入可行区域获取多个节点信号强度,筛选强度满足阈值的节点进行坐标估算,获得预估的外部节点定位坐标。
本实施例中,所述外部节点定位坐标,指由外部节点配合得到的行为坐标,所述内部测量角度为设备本身自采集的角度数据。角度采集方法包括但不限于如下几种方法:a)对于包含电子罗盘的智能终端,待测目标的航向角可以直接通过电子罗盘读取出,如手机网页端可以直接读取到手机角度数据;b)根据磁场传感器数据计算出角度数据,实现电子罗盘的功能;c)可以获得磁场传感器,陀螺仪和加速度传感器数据时,综合计算角度数据。进一步的,由于蓝牙设备在m(m=5)米的范围内强度随着距离衰减的规律比较稳定,如果距离超过范围,距离的区分精度就会降低。在本实施例中,采用均匀布置节点的模型,在提前规划出的可通行区域内,东西南北方向每隔5米布置一个节点,这样每一个大小为25平方米的正方形区域都可以有四个节点覆盖。当用户手持手机打开定位程序,进入被蓝牙设备覆盖的区域内,匹配定位算法便会开始运行。首先手机将统计最近2s中内采集到的多个蓝牙节点信号强度,并从中筛选出强度满足预订标准的节点,获得预估的输出坐标。虽然室内环境千差万别,难以找出一种固定的、适用于各种室内环境的蓝牙设备布局方案。但本实施例采用了多层级定位,免去了采集样本训练指纹数据库的过程。即使是在不同的室内布局,一旦布局方案确定了,整个室内的可通行区域就可以确定,每个小区域内的蓝牙空间分布关系也可以提前确定。
本发明进行室内行车定位融合的一个具体实例为:
定位区域的划分:将室内空间区域按建筑规划划分成多个可通行区域,从而避免定位点落在墙内或其他不可穿行区域;手机终端通过接收的蓝牙节点实现预估定位。所述手机终端的节点定位具体为:步骤1:当位于定位区域内时,手机终端搜索附近最近的蓝牙模块并获取蓝牙信号强度;步骤2:每获取一组蓝牙节点,根据其强度对其进行筛选;步骤3:根据蓝牙节点的距离和强度确定预估的输出坐标。步骤4:获取到手机终端的运动信息特征,通过卡尔曼滤波算法将基于运动信息的状态预测和基于信号强度测量的位置估计高斯拟合,进一步完善完成当前时刻用户运动状态的估计。
本发明中蓝牙定位方法包括但不限于:邻近节点算法,三边法,指纹场匹配算法,TOA测距法、TDOA测距法等等。本发明不对节点室内定位技术和方法做任何限定。本发明中基于运动信息的状态融合避开直接使用手机的状态作为观测,而采用短时间切片为单位,只采纳当前时刻与上个时刻的状态变化差作为特征,运用筛选处理后得到的角度变化,使***对运动的描述和预测更加准确。另一个创新点在于本发明根据车行的运动规律对下一时刻的位置有了更清晰的限制,不同于人类步行的运动状态,车辆行驶是无法瞬间转向和掉头的,因此在定位场景上的也有如下特征:在短时间内车辆无法在垂直于前进方向的水平面上左右横移。
进一步地,所述定位空间部署方案具体标准为:蓝牙模块的有效覆盖半径为10米,在5米范围之内,其强度的衰减明显,区分度强。为实现精度为米级的定位,将蓝牙模块布置于25平方米的大正方形区域的4个顶点上,在扩大定位区域时,只需将多个正方形按单边重合的方式进行拓展。
与现有技术相比,本发明所提方案进行室内定位具有如下优点和技术效果:基于多层级定位方法,适用于各种复杂的室内环境;蓝牙模块铺设方法简单,降低铺设成本;定位精度高鲁棒性好,所实现的精度在2米之内;无需提前采集数据离线训练指纹数据库,缩短铺设时间。
本发明实施例还提供了一种室内行车定位融合装置,能够实现上述任一实施例所述的室内行车定位融合方法的所有流程,装置中的各个模块、单元的作用以及实现的技术效果分别与上述实施例所述的驾驶员驾驶类型识别方法的作用以及实现的技术效果对应相同,这里不再赘述。
如图2所示,本发明提供的一种室内行车定位融合装置的一个优选实施例,包括:
数据获取单元,获取外部节点定位坐标和内部测量角度;
初始化设定单元,初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q0、初始状态的误差协方差矩阵P0、测量协方差矩阵R,选择***模型,设定卡尔曼滤波器的***矩阵F,所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标;
速度矢量调整单元,根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rotk,根据旋转矩阵Rotk调整原速度矢量vk得到新速度矢量vk r=Rotkvk;
状态向量预测单元,根据***模型得到***激励矩阵Bk和噪音激励矩阵Nk,根据所述***激励矩阵Bk与新速度矢量vk r的积结合噪音激励矩阵Nk得到激励值;根据某时刻的***矩阵Fk和上一时刻的状态向量sk-1结合激励值得到某时刻的预测状态向量sk-1 k;
误差协方差预测单元,根据某时刻的***矩阵Fk、***激励矩阵Bk和状态更新方程的误差协方差矩阵Qk,通过上一时刻的误差协方差矩阵Pk-1得到某时刻的预测误差协方差矩阵Pk-1 k;
测量更新单元,根据外部节点定位坐标确定某时刻的测量向量zk和对应的测量矩阵H,计算某时刻的卡尔曼增益Kk,并更新某时刻的误差协方差矩阵Pk和某时刻的状态向量sk。
进一步地,所述初始的状态更新方程的误差协方差矩阵Q0设定为
进一步地,所述***模型选定为匀速运动模型,所述卡尔曼滤波器的***矩阵所述dt为状态更新的时间间隔,根据***模型得到***激励矩阵和噪音激励矩阵所述dtk为某时刻k与上一时刻k-1之间的时间差。
进一步地,所述旋转矩阵Rotk通过以下步骤得到:采纳某时刻与上一时刻针对方向的状态变化差dθk;根据状态变化差dθk构成旋转矩阵
进一步地,所述预测状态向量sk-1 k通过以下步骤得到:根据***模型得到***激励矩阵Bk和噪音激励矩阵Nk,根据所述***激励矩阵Bk与新速度矢量vk r的积Bkvk r结合噪音激励矩阵Nk得到激励值Bkvk r+Nk;根据某时刻的***矩阵Fk和上一时刻的状态向量sk-1计算得到原预测状态向量Fksk-1;根据激励值Bkvk r调整原预测状态向量Fksk-1得到预测状态向量sk-1 k,计算式为sk-1 k=Fksk-1+Bkvk r+Nk。
进一步地,所述外部节点定位坐标通过以下步骤获得:使节点在室内可通行区域内间隔一定距离的均匀布置,所述一定距离为节点蓝牙设备的信号稳定距离;进入可行区域获取多个节点信号强度,筛选强度满足阈值的节点进行坐标估算,获得预估的外部节点定位坐标。
本发明实施例还提供了一种计算机可读存储介质,所述计算机可读存储介质包括存储的计算机程序;其中,所述计算机程序在运行时控制所述计算机可读存储介质所在的设备执行述实施例任一项所述的室内行车定位融合方法。
如图3所示,本发明实施例还提供了一种终端设备,所述终端设备包括处理器10、存储器20以及存储在所述存储器20中且被配置为由所述处理器10执行的计算机程序,所述处理器10在执行所述计算机程序时实现上述实施例任一项所述的室内行车定位融合方法。
优选地,所述计算机程序可以被分割成一个或多个模块/单元(如计算机程序1、计算机程序2、······),所述一个或者多个模块/单元被存储在所述存储器20中,并由所述处理器10执行,以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段,该指令段用于描述所述计算机程序在所述终端设备中的执行过程。
所述处理器10可以是中央处理单元(Central Processing Unit,CPU),还可以是其他通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现成可编程门阵列(Field-Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等,通用处理器可以是微处理器,或者所述处理器10也可以是任何常规的处理器,所述处理器10是所述终端设备的控制中心,利用各种接口和线路连接所述终端设备的各个部分。
所述存储器20主要包括程序存储区和数据存储区,其中,程序存储区可存储操作***、至少一个功能所需的应用程序等,数据存储区可存储相关数据等。此外,所述存储器20可以是高速随机存取存储器,还可以是非易失性存储器,例如插接式硬盘,智能存储卡(Smart Media Card,SMC)、安全数字(Secure Digital,SD)卡和闪存卡(Flash Card)等,或所述存储器20也可以是其他易失性固态存储器件。
需要说明的是,上述终端设备可包括,但不仅限于,处理器、存储器,本领域技术人员可以理解,图3结构框图仅仅是上述终端设备的示例,并不构成对终端设备的限定,可以包括比图示更多或更少的部件,或者组合某些部件,或者不同的部件。
综上,本发明公开的一种室内行车定位融合方法、装置、计算机可读存储介质及终端设备,在获取外部节点定位坐标和内部测量角度后并不直接将其使用在卡尔曼滤波器中,还对速度矢量进行调整,即采纳当前时刻与上个时刻的状态变化差作为特征,避免误差累计而导致数据恶化。具体而言,本发明在获取数据和初始化卡尔曼滤波器后,并不直接将获取的数据在卡尔曼滤波器中进行融合,而是对获取的数据进行检验处理,利用传感器在短时间内的状态变化量反馈准确的特点,选用两个时刻间的变化差作为特征以避免误差的积累,利用特征结合汽车行驶的固有特点去修正获取的数据(速度矢量),通过修正后的速度矢量去预测状态向量和误差协方差,起到了修正预测状态向量和误差协方差的作用,而通过修正后的预测状态向量和误差协方差去更新测量数据,则起到了修正测量数据,保证定位准确性的作用。本发明中避开了直接使用状态作为观测对象,而是根据汽车行驶的固有特点更加精确地利用惯性信息,利用垂直于行进方向的速度不确定性低的特点调整速度矢量,使融合***的鲁棒性和实时性大大提高,解决了目前室内行车定位直接采用惯性信息导致定位整体表现劣化的问题,提供了一种对惯性信息进行筛选和处理后,提取其中与预估定位信息较为吻合的信息再进行融合的室内行车定位融合方法。
以上所述是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也视为本发明的保护范围。
Claims (9)
1.一种室内行车定位融合方法,其特征在于,包括:
获取外部节点定位坐标和内部测量角度;
初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q0、初始状态的误差协方差矩阵P0、测量协方差矩阵R,选择***模型,设定卡尔曼滤波器的***矩阵F,所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标;
根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rotk,根据旋转矩阵Rotk调整原速度矢量vk得到新速度矢量vk r=Rotkvk;
根据***模型得到***激励矩阵Bk和噪音激励矩阵Nk,根据所述***激励矩阵Bk与新速度矢量vk r的积结合噪音激励矩阵Nk得到激励值,根据某时刻的***矩阵Fk和上一时刻的状态向量sk-1结合激励值得到某时刻的预测状态向量sk-1 k;
根据某时刻的***矩阵Fk、***激励矩阵Bk和状态更新方程的误差协方差矩阵Qk,通过上一时刻的误差协方差矩阵Pk-1得到某时刻的预测误差协方差矩阵Pk-1 k;
根据外部节点定位坐标确定某时刻的测量向量zk和对应的测量矩阵H,计算某时刻的卡尔曼增益Kk,并更新某时刻的误差协方差矩阵Pk和某时刻的状态向量sk。
2.根据权利要求1所述的一种室内行车定位融合方法,其特征在于,所述初始的状态更新方程的误差协方差矩阵Q0设定为
3.根据权利要求1所述的一种室内行车定位融合方法,其特征在于,所述***模型选定为匀速运动模型,所述卡尔曼滤波器的***矩阵所述dt为状态更新的时间间隔,根据***模型得到***激励矩阵和噪音激励矩阵所述dtk为某时刻k与上一时刻k-1之间的时间差。
4.根据权利要求1所述的一种室内行车定位融合方法,其特征在于,所述旋转矩阵Rotk通过以下步骤得到:
采纳某时刻与上一时刻针对方向的状态变化差dθk;
根据状态变化差dθk构成旋转矩阵
5.根据权利要求1所述的一种室内行车定位融合方法,其特征在于,所述预测状态向量sk-1 k通过以下步骤得到:
根据***模型得到***激励矩阵Bk和噪音激励矩阵Nk,根据所述***激励矩阵Bk与新速度矢量vk r的积Bkvk r结合噪音激励矩阵Nk得到激励值Bkvk r+Nk;
根据某时刻的***矩阵Fk和上一时刻的状态向量sk-1计算得到原预测状态向量Fksk-1;
根据激励值Bkvk r调整原预测状态向量Fksk-1得到预测状态向量sk-1 k,计算式为sk-1 k=Fksk-1+Bkvk r+Nk。
6.根据权利要求1-5任一项所述的一种室内行车定位融合方法,其特征在于,所述外部节点定位坐标通过以下步骤获得:
使节点在室内可通行区域内间隔一定距离的均匀布置,所述一定距离为节点蓝牙设备的信号稳定距离;
进入可行区域获取多个节点信号强度,筛选强度满足阈值的节点进行坐标估算,获得预估的外部节点定位坐标。
7.一种室内行车定位融合装置,其特征在于,包括:
数据获取单元,获取外部节点定位坐标和内部测量角度;
初始化设定单元,初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q0、初始状态的误差协方差矩阵P0、测量协方差矩阵R,选择***模型,设定卡尔曼滤波器的***矩阵F,所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标;
速度矢量调整单元,根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rotk,根据旋转矩阵Rotk调整原速度矢量vk得到新速度矢量vk r=Rotkvk;
状态向量预测单元,根据***模型得到***激励矩阵Bk和噪音激励矩阵Nk,根据所述***激励矩阵Bk与新速度矢量vk r的积结合噪音激励矩阵Nk得到激励值;根据某时刻的***矩阵Fk和上一时刻的状态向量sk-1结合激励值得到某时刻的预测状态向量sk-1 k;
误差协方差预测单元,根据某时刻的***矩阵Fk、***激励矩阵Bk和状态更新方程的误差协方差矩阵Qk,通过上一时刻的误差协方差矩阵Pk-1得到某时刻的预测误差协方差矩阵Pk-1 k;
测量更新单元,根据外部节点定位坐标确定某时刻的测量向量zk和对应的测量矩阵H,计算某时刻的卡尔曼增益Kk,并更新某时刻的误差协方差矩阵Pk和某时刻的状态向量sk。
8.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质包括存储的计算机程序;其中,所述计算机程序在运行时控制所述计算机可读存储介质所在的设备执行如权利要求1-5任一项所述的室内行车定位融合方法。
9.一种终端设备,其特征在于,包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序,所述处理器在执行所述计算机程序时实现如权利要求1-5任一项所述的室内行车定位融合方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910138099.4A CN109945864B (zh) | 2019-02-25 | 2019-02-25 | 室内行车定位融合方法、装置、存储介质及终端设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910138099.4A CN109945864B (zh) | 2019-02-25 | 2019-02-25 | 室内行车定位融合方法、装置、存储介质及终端设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109945864A true CN109945864A (zh) | 2019-06-28 |
CN109945864B CN109945864B (zh) | 2020-12-18 |
Family
ID=67006870
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910138099.4A Active CN109945864B (zh) | 2019-02-25 | 2019-02-25 | 室内行车定位融合方法、装置、存储介质及终端设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109945864B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110824423A (zh) * | 2019-11-26 | 2020-02-21 | 北京壹氢科技有限公司 | 一种多无人车协同导航定位方法及*** |
CN116812427A (zh) * | 2023-08-28 | 2023-09-29 | 河北因朵科技有限公司 | 一种无人库房自动取档与归档控制***及方法 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7894512B2 (en) * | 2007-07-31 | 2011-02-22 | Harris Corporation | System and method for automatic recovery and covariance adjustment in linear filters |
CN103292813A (zh) * | 2013-05-24 | 2013-09-11 | 哈尔滨工程大学 | 一种提高水面艇编队导航精度的信息滤波方法 |
US20150247739A1 (en) * | 2008-03-06 | 2015-09-03 | Texas Instruments Incorporated | Processes for more accurately calibrating and operating e-compass for tilt error, circuits, and systems |
CN104932522A (zh) * | 2015-05-27 | 2015-09-23 | 深圳市大疆创新科技有限公司 | 一种飞行器的自主降落方法和*** |
CN105588566A (zh) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | 一种基于蓝牙与mems融合的室内定位***及方法 |
CN106405433A (zh) * | 2016-11-04 | 2017-02-15 | 首都师范大学 | 一种基于扩展卡尔曼粒子滤波的soc估计方法及*** |
CN108228347A (zh) * | 2017-12-21 | 2018-06-29 | 上海电机学院 | 一种任务感知的Docker自适应调度*** |
-
2019
- 2019-02-25 CN CN201910138099.4A patent/CN109945864B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7894512B2 (en) * | 2007-07-31 | 2011-02-22 | Harris Corporation | System and method for automatic recovery and covariance adjustment in linear filters |
US20150247739A1 (en) * | 2008-03-06 | 2015-09-03 | Texas Instruments Incorporated | Processes for more accurately calibrating and operating e-compass for tilt error, circuits, and systems |
CN103292813A (zh) * | 2013-05-24 | 2013-09-11 | 哈尔滨工程大学 | 一种提高水面艇编队导航精度的信息滤波方法 |
CN104932522A (zh) * | 2015-05-27 | 2015-09-23 | 深圳市大疆创新科技有限公司 | 一种飞行器的自主降落方法和*** |
CN105588566A (zh) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | 一种基于蓝牙与mems融合的室内定位***及方法 |
CN106405433A (zh) * | 2016-11-04 | 2017-02-15 | 首都师范大学 | 一种基于扩展卡尔曼粒子滤波的soc估计方法及*** |
CN108228347A (zh) * | 2017-12-21 | 2018-06-29 | 上海电机学院 | 一种任务感知的Docker自适应调度*** |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110824423A (zh) * | 2019-11-26 | 2020-02-21 | 北京壹氢科技有限公司 | 一种多无人车协同导航定位方法及*** |
CN110824423B (zh) * | 2019-11-26 | 2021-08-17 | 北京壹氢科技有限公司 | 一种多无人车协同导航定位方法及*** |
CN116812427A (zh) * | 2023-08-28 | 2023-09-29 | 河北因朵科技有限公司 | 一种无人库房自动取档与归档控制***及方法 |
CN116812427B (zh) * | 2023-08-28 | 2023-11-14 | 河北因朵科技有限公司 | 一种无人库房自动取档与归档控制***及方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109945864B (zh) | 2020-12-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110118560B (zh) | 一种基于lstm和多传感器融合的室内定位方法 | |
CN110146909A (zh) | 一种定位数据处理方法 | |
Pei et al. | Optimal heading estimation based multidimensional particle filter for pedestrian indoor positioning | |
TWI489126B (zh) | 一種動態無線訊號強度修正系統與方法 | |
JP6855524B2 (ja) | 低速特徴からのメトリック表現の教師なし学習 | |
CN103925923B (zh) | 一种基于自适应粒子滤波器算法的地磁室内定位*** | |
CN110118554A (zh) | 基于视觉惯性的slam方法、装置、存储介质和设备 | |
CN108759833A (zh) | 一种基于先验地图的智能车辆定位方法 | |
CN105704652B (zh) | 一种wlan/蓝牙定位中的指纹库采集和优化方法 | |
CN109883423A (zh) | 基于卡尔曼滤波的定位方法、***、设备及存储介质 | |
CN108957512A (zh) | 定位装置及方法以及自动行走设备 | |
CN107036594A (zh) | 智能电站巡检智能体的定位与多粒度环境感知技术 | |
CN110196044A (zh) | 一种基于gps闭环检测的变电站巡检机器人建图方法 | |
CN106168485B (zh) | 步行航迹数据推算方法及装置 | |
CN110308729A (zh) | 基于视觉与imu或里程计的agv组合导航定位方法 | |
CN108020813A (zh) | 定位方法、定位装置和电子设备 | |
CN110187375A (zh) | 一种基于slam定位结果提高定位精度的方法及装置 | |
CN104105067A (zh) | 基于地磁信息的实时定位***及方法 | |
CN108332759A (zh) | 一种基于3d激光的地图构建方法及*** | |
CN106679672A (zh) | 一种基于dbn和卡尔曼滤波算法的agv定位算法 | |
CN110260866A (zh) | 一种基于视觉传感器的机器人定位与避障方法 | |
CN109769206A (zh) | 一种室内定位融合方法、装置、存储介质及终端设备 | |
CN110231592A (zh) | 室内定位方法、装置、计算机可读存储介质及终端设备 | |
CN109945864A (zh) | 室内行车定位融合方法、装置、存储介质及终端设备 | |
CN108801267A (zh) | 一种融合多传感器的室内无锚点定位方法 |
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 |