CN109945864B - 室内行车定位融合方法、装置、存储介质及终端设备 - Google Patents

Abstract

本发明公开了一种室内行车定位融合方法、装置、存储介质及终端设备，所述方法包括：获取外部节点定位坐标和内部测量角度，初始化卡尔曼滤波器相关参数，根据当前时刻与上个时刻的状态变化为特征调整速度矢量，根据调整得到的新速度矢量预测状态向量和误差协方差，进而进行测量更新。本发明中避开了直接使用状态作为观测对象，而是根据汽车行驶的固有特点更加精确地利用惯性信息，利用垂直于行进方向的速度不确定性低的特点调整速度矢量，使融合系统的鲁棒性和实时性大大提高，解决了目前室内行车定位直接采用惯性信息导致定位整体表现劣化的问题。

Description

室内行车定位融合方法、装置、存储介质及终端设备

技术领域

本发明涉及本发明涉及室内行车定位技术领域，尤其涉及一种室内行车定位融合方法、装置、存储介质及终端设备。

背景技术

目前，室内行车定位技术通常在室内区域内均匀布置若干个蓝牙装置用于发射蓝牙信号，实际应用时当手机获取到足够数量满足强度要求的节点后，运用三角定位、指纹分析、近邻法等方法估计出手机的方位信息，再使用卡尔曼滤波器融合手机自身采集到的惯性和方向，利用历史数据使轨迹更加平滑。但是，这种室内行车定位方法却存在一些缺陷，具体而言。目前手机等消费级智能终端内置的磁场传感器、加速度计和陀螺仪的精度有限，且室内环境存在诸多丰富的地磁干扰，因此对手机的姿态判定存在较大误差。目前的融合算法中采用的手机惯性信息都是未经过检验处理而直接采用的，其不准确性有时反而会劣化定位的整体表现。故而，目前的室内行车定位需要一种根据行车方式对惯性进行处理后进行融合以保证定位效果的定位技术。

发明内容

本发明提供了一种室内行车定位融合方法、装置、存储介质及终端设备，旨在解决目前室内行车定位直接采用惯性信息导致定位整体表现劣化的问题。

为了解决上述技术问题，本发明实施例提供了一种室内行车定位融合方法，包括：

获取外部节点定位坐标和内部测量角度；

初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q₀、初始状态的误差协方差矩阵P₀、测量协方差矩阵R，选择系统模型，设定卡尔曼滤波器的系统矩阵F，所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标；

根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rot_k，根据旋转矩阵Rot_k调整原速度矢量v_k得到新速度矢量v_k ^r＝Rot_kv_k；

根据系统模型得到系统激励矩阵B_k和噪音激励矩阵N_k，根据所述系统激励矩阵B_k与新速度矢量v_k ^r的积结合噪音激励矩阵N_k得到激励值，根据当前时刻的系统矩阵F_k和上一时刻的状态向量s_k-1结合激励值得到当前时刻的预测状态向量s_k-1 ^k；

根据当前时刻的系统矩阵F_k、系统激励矩阵B_k和状态更新方程的误差协方差矩阵Q_k,通过上一时刻的误差协方差矩阵P_k-1得到当前时刻的预测误差协方差矩阵P_k-1 ^k；

根据外部节点定位坐标确定当前时刻的测量向量z_k和对应的测量矩阵H，计算当前时刻的卡尔曼增益K_k，并更新当前时刻的误差协方差矩阵P_k和当前时刻的状态向量s_k。

进一步地，所述初始的状态更新方程的误差协方差矩阵Q₀设定为

进一步地，所述系统模型选定为匀速运动模型，所述卡尔曼滤波器的系统矩阵

所述dt为状态更新的时间间隔，根据系统模型得到系统激励矩阵

和噪音激励矩阵

所述dt_k为当前时刻k与上一时刻k-1之间的时间差。

进一步地，所述旋转矩阵Rot_k通过以下步骤得到：

采纳当前时刻与上一时刻针对方向的状态变化差dθ_k；

根据状态变化差dθ_k构成旋转矩阵

进一步地，所述预测状态向量s_k-1 ^k通过以下步骤得到：

根据系统模型得到系统激励矩阵B_k和噪音激励矩阵N_k，根据所述系统激励矩阵B_k与新速度矢量v_k ^r的积B_kv_k ^r结合噪音激励矩阵N_k得到激励值B_kv_k ^r+N_k；

根据当前时刻的系统矩阵F_k和上一时刻的状态向量s_k-1计算得到原预测状态向量F_ks_k-1；

根据激励值B_kv_k ^r调整原预测状态向量F_ks_k-1得到预测状态向量s_k-1 ^k，计算式为s_k-1 ^k＝F_ks_k-1+B_kv_k ^r+N_k。

进一步地，所述外部节点定位坐标通过以下步骤获得：

使节点在室内可通行区域内间隔一定距离的均匀布置，所述一定距离为节点蓝牙设备的信号稳定距离；

进入可行区域获取多个节点信号强度，筛选强度满足阈值的节点进行坐标估算，获得预估的外部节点定位坐标。

为了解决上述技术问题，本发明实施例还提供了一种室内行车定位融合装置，包括：

数据获取单元，获取外部节点定位坐标和内部测量角度；

初始化设定单元，初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q₀、初始状态的误差协方差矩阵P₀、测量协方差矩阵R，选择系统模型，设定卡尔曼滤波器的系统矩阵F，所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标；

速度矢量调整单元，根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rot_k，根据旋转矩阵Rot_k调整原速度矢量v_k得到新速度矢量v_k ^r＝Rot_kv_k；

状态向量预测单元，根据系统模型得到系统激励矩阵B_k和噪音激励矩阵N_k，根据所述系统激励矩阵B_k与新速度矢量v_k ^r的积结合噪音激励矩阵N_k得到激励值；根据当前时刻的系统矩阵F_k和上一时刻的状态向量s_k-1结合激励值得到当前时刻的预测状态向量s_k-1 ^k；

误差协方差预测单元，根据当前时刻的系统矩阵F_k、系统激励矩阵B_k和状态更新方程的误差协方差矩阵Q_k,通过上一时刻的误差协方差矩阵P_k-1得到当前时刻的预测误差协方差矩阵P_k-1 ^k；

测量更新单元，根据外部节点定位坐标确定当前时刻的测量向量z_k和对应的测量矩阵H，计算当前时刻的卡尔曼增益K_k，并更新当前时刻的误差协方差矩阵P_k和当前时刻的状态向量s_k。

为了解决上述技术问题，本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序；其中，所述计算机程序在运行时控制所述计算机可读存储介质所在的设备执行上述实施例任一项所述的室内行车定位融合方法。

为了解决上述技术问题，本发明实施例还提供了一种终端设备，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器在执行所述计算机程序时实现上述实施例任一项所述的室内行车定位融合方法。

与现有技术相比，本发明公开的一种室内行车定位融合方法、装置、计算机可读存储介质及终端设备，在获取外部节点定位坐标和内部测量角度后并不直接将其使用在卡尔曼滤波器中，还对速度矢量进行调整，即采纳当前时刻与上个时刻的状态变化差作为特征，避免误差累计而导致数据恶化。具体而言，本发明在获取数据和初始化卡尔曼滤波器后，并不直接将获取的数据在卡尔曼滤波器中进行融合，而是对获取的数据进行检验处理，利用传感器在短时间内的状态变化量反馈准确的特点，选用两个时刻间的变化差作为特征以避免误差的积累，利用特征结合汽车行驶的固有特点去修正获取的数据(速度矢量)，通过修正后的速度矢量去预测状态向量和误差协方差，起到了修正预测状态向量和误差协方差的作用，而通过修正后的预测状态向量和误差协方差去更新测量数据，则起到了修正测量数据，保证定位准确性的作用。本发明中避开了直接使用状态作为观测对象，而是根据汽车行驶的固有特点更加精确地利用惯性信息，利用垂直于行进方向的速度不确定性低的特点调整速度矢量，使融合系统的鲁棒性和实时性大大提高，解决了目前室内行车定位直接采用惯性信息导致定位整体表现劣化的问题，提供了一种对惯性信息进行筛选和处理后，提取其中与预估定位信息较为吻合的信息再进行融合的室内行车定位融合方法。

附图说明

图1是本发明提供的一种室内行车定位融合方法的一个优选实施例的流程图；

图2是本发明提供的一种室内行车定位融合装置的一个优选实施例的结构框图；

图3是本发明提供的一种终端设备的一个优选实施例的结构框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本技术领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本发明所述一种室内行车定位融合方法，包括：

获取外部节点定位坐标和内部测量角度；

初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q₀、初始状态的误差协方差矩阵P₀、测量协方差矩阵R，选择系统模型，设定卡尔曼滤波器的系统矩阵F，所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标；

根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rot_k，根据旋转矩阵Rot_k调整原速度矢量v_k得到新速度矢量v_k ^r＝Rot_kv_k；

根据系统模型得到系统激励矩阵B_k和噪音激励矩阵N_k，根据所述系统激励矩阵B_k与新速度矢量v_k ^r的积结合噪音激励矩阵N_k得到激励值，根据当前时刻的系统矩阵F_k和上一时刻的状态向量s_k-1结合激励值得到当前时刻的预测状态向量s_k-1 ^k；

根据当前时刻的系统矩阵F_k、系统激励矩阵B_k和状态更新方程的误差协方差矩阵Q_k,通过上一时刻的误差协方差矩阵P_k-1得到当前时刻的预测误差协方差矩阵P_k-1 ^k；

根据外部节点定位坐标确定当前时刻的测量向量z_k和对应的测量矩阵H，计算当前时刻的卡尔曼增益K_k，并更新当前时刻的误差协方差矩阵P_k和当前时刻的状态向量s_k。

本实施例在获取外部节点定位坐标和内部测量角度后并不直接将其使用在卡尔曼滤波器中，还对速度矢量进行调整，即采纳当前时刻与上个时刻的状态变化差作为特征，避免误差累计而导致数据恶化。具体而言，本发明在获取数据和初始化卡尔曼滤波器后，并不直接将获取的数据在卡尔曼滤波器中进行融合，而是对获取的数据进行检验处理，利用传感器在短时间内的状态变化量反馈准确的特点，选用两个时刻间的变化差作为特征以避免误差的积累，利用特征结合汽车行驶的固有特点去修正获取的数据(速度矢量)，通过修正后的速度矢量去预测状态向量和误差协方差，起到了修正预测状态向量和误差协方差的作用，而通过修正后的预测状态向量和误差协方差去更新测量数据，则起到了修正测量数据，保证定位准确性的作用。本发明中避开了直接使用状态作为观测对象，而是根据汽车行驶的固有特点更加精确地利用惯性信息，利用垂直于行进方向的速度不确定性低的特点调整速度矢量，使融合系统的鲁棒性和实时性大大提高，解决了目前室内行车定位直接采用惯性信息导致定位整体表现劣化的问题，提供了一种对惯性信息进行筛选和处理后，提取其中与预估定位信息较为吻合的信息再进行融合的室内行车定位融合方法。

在另一个优选实施例中，所述初始的状态更新方程的误差协方差矩阵Q₀设定为

在本实施例中，定位功能的初期，由于系统缺少足够的历史数据，可以选择更相信蓝牙/wifi定位所获得的结果，因此可以状态更新方程的误差协方差设定一个较大的值

而随着定位时间的逐步延长，让其随着滤波器迭代过程收敛。需要注意的是，车行导航的状态更新方程的误差协方差的对角项并不相等。具体而言，由于车辆行驶本身的特性所致，实际场景中车辆无法在垂直于行进方向的水平面横移的，因此当转弯的偏移角度已经通过上一步骤补偿后，下一时刻的速度变化只会沿着车头面朝的方向发生变化，而其垂直于行进方向的水平面的速度和速度变化都应该趋近于零。换而言之，沿着行进方向的速度不确定性较高，垂直于行进方向的速度不确定性较低。因此对车行的状态方程而言，在运动方向上的对应加减速误差的方差a²，应该远大于对应于垂直于运动方向的误差b²(理论上应该为零)的方差，故而Q₀的对角项并不相等。在k时刻的状态误差协方差矩阵如下，其中a＞＞b，

Rot'_k对应当前时刻速度方向θ_k的旋转矩阵。Q_K代表对速度不确定性的描述

在另一个优选实施例中，所述系统模型选定为匀速运动模型，所述卡尔曼滤波器的系统矩阵

所述dt为状态更新的时间间隔，根据系统模型得到系统激励矩阵

和噪音激励矩阵

所述dt_k为当前时刻k与上一时刻k-1之间的时间差。本实施例中，系统模型选定为匀速运动模型根据匀速运动模型确定卡尔曼滤波器的系统矩阵F，系统激励矩阵B_k和噪音激励矩阵N_k，从而根据系统矩阵F，系统激励矩阵B_k和噪音激励矩阵N_k实现对预测状态向量的修正，进一步保证了定位效果。

在另一个优选实施例中，所述旋转矩阵Rot_k通过以下步骤得到：采纳当前时刻与上一时刻针对方向的状态变化差dθ_k；根据状态变化差dθ_k构成旋转矩阵

本实施例中，在短时间内手机传感器对状态变化量的反馈是比较准确的，但由于其状态的推算包含了在时间上的积分，小度量的误差会逐渐累加的最后导致数据的恶化。正是利用这种特性，本发明避开直接使用手机的状态作为观测方程，而是采用短时间切片为单位，只采纳当前时刻与上个时刻的状态变化差作为特征。例如针对方向的变化，可以构成旋转矩阵，

再利用旋转矩阵去调整原方程中的速度矢量v_k＝[vx_k vy_k]，获得更准确的旋转相应。v_k ^r＝Rot_kv_k。其中v_k ^r代表根据时间切片感知的方向变化对原速度进行旋转以后得到新的速度矢量。由于车辆行驶本身的特性所致，实际场景中车辆无法在垂直于行进方向的水平面横移的，因此当转弯的偏移角度已经通过上一步骤补偿后，下一时刻的速度变化只会沿着车头面朝的方向发生变化，而其垂直于行进方向的水平面的速度和速度变化都应该趋近于零。换而言之，沿着行进方向的速度不确定性较高，垂直于行进方向的速度不确定性较低。因此新的速度矢量v_k ^r不需考虑其他方向的加速度的投影，故而v_k ^r＝Rot_kv_k。本实施例中避开了直接使用状态作为观测对象，而是根据汽车行驶的固有特点更加精确地利用惯性信息，利用垂直于行进方向的速度不确定性低的特点调整速度矢量，使融合系统的鲁棒性和实时性大大提高。

在另一个优选实施例中，所述预测状态向量s_k-1 ^k通过以下步骤得到：根据系统模型得到系统激励矩阵B_k和噪音激励矩阵N_k，根据所述系统激励矩阵B_k与新速度矢量v_k ^r的积B_kv_k ^r结合噪音激励矩阵N_k得到激励值B_kv_k ^r+N_k；根据当前时刻的系统矩阵F_k和上一时刻的状态向量s_k-1计算得到原预测状态向量F_ks_k-1；根据激励值B_kv_k ^r调整原预测状态向量F_ks_k-1得到预测状态向量s_k-1 ^k，计算式为s_k-1 ^k＝F_ks_k-1+B_kv_k ^r+N_k。本实施例中预测状态向量s_k-1 ^k融合了激励值，不但是根据系统矩阵F_k和上一时刻的状态向量s_k-1计算得的，通过融合激励值修正了本实施例中的预测状态向量s_k-1 ^k避免了直接使用测量数据带来的不良影响，进一步保障了定位效果。

在另一个优选实施例中，所述外部节点定位坐标通过以下步骤获得：使节点在室内可通行区域内间隔一定距离的均匀布置，所述一定距离为节点蓝牙设备的信号稳定距离；进入可行区域获取多个节点信号强度，筛选强度满足阈值的节点进行坐标估算，获得预估的外部节点定位坐标。

本实施例中，所述外部节点定位坐标，指由外部节点配合得到的行为坐标，所述内部测量角度为设备本身自采集的角度数据。角度采集方法包括但不限于如下几种方法：a)对于包含电子罗盘的智能终端，待测目标的航向角可以直接通过电子罗盘读取出，如手机网页端可以直接读取到手机角度数据；b)根据磁场传感器数据计算出角度数据，实现电子罗盘的功能；c)可以获得磁场传感器，陀螺仪和加速度传感器数据时，综合计算角度数据。进一步的，由于蓝牙设备在m(m＝5)米的范围内强度随着距离衰减的规律比较稳定，如果距离超过范围，距离的区分精度就会降低。在本实施例中，采用均匀布置节点的模型，在提前规划出的可通行区域内，东西南北方向每隔5米布置一个节点，这样每一个大小为25平方米的正方形区域都可以有四个节点覆盖。当用户手持手机打开定位程序，进入被蓝牙设备覆盖的区域内，匹配定位算法便会开始运行。首先手机将统计最近2s中内采集到的多个蓝牙节点信号强度，并从中筛选出强度满足预订标准的节点，获得预估的输出坐标。虽然室内环境千差万别，难以找出一种固定的、适用于各种室内环境的蓝牙设备布局方案。但本实施例采用了多层级定位，免去了采集样本训练指纹数据库的过程。即使是在不同的室内布局，一旦布局方案确定了，整个室内的可通行区域就可以确定，每个小区域内的蓝牙空间分布关系也可以提前确定。

本发明进行室内行车定位融合的一个具体实例为：

定位区域的划分：将室内空间区域按建筑规划划分成多个可通行区域，从而避免定位点落在墙内或其他不可穿行区域；手机终端通过接收的蓝牙节点实现预估定位。所述手机终端的节点定位具体为：步骤1：当位于定位区域内时，手机终端搜索附近最近的蓝牙模块并获取蓝牙信号强度；步骤2：每获取一组蓝牙节点，根据其强度对其进行筛选；步骤3：根据蓝牙节点的距离和强度确定预估的输出坐标。步骤4：获取到手机终端的运动信息特征，通过卡尔曼滤波算法将基于运动信息的状态预测和基于信号强度测量的位置估计高斯拟合，进一步完善完成当前时刻用户运动状态的估计。

本发明中蓝牙定位方法包括但不限于：邻近节点算法，三边法，指纹场匹配算法，TOA测距法、TDOA测距法等等。本发明不对节点室内定位技术和方法做任何限定。本发明中基于运动信息的状态融合避开直接使用手机的状态作为观测，而采用短时间切片为单位，只采纳当前时刻与上个时刻的状态变化差作为特征，运用筛选处理后得到的角度变化，使系统对运动的描述和预测更加准确。另一个创新点在于本发明根据车行的运动规律对下一时刻的位置有了更清晰的限制，不同于人类步行的运动状态，车辆行驶是无法瞬间转向和掉头的，因此在定位场景上的也有如下特征：在短时间内车辆无法在垂直于前进方向的水平面上左右横移。

进一步地，所述定位空间部署方案具体标准为：蓝牙模块的有效覆盖半径为10米，在5米范围之内，其强度的衰减明显，区分度强。为实现精度为米级的定位，将蓝牙模块布置于25平方米的大正方形区域的4个顶点上，在扩大定位区域时，只需将多个正方形按单边重合的方式进行拓展。

与现有技术相比，本发明所提方案进行室内定位具有如下优点和技术效果：基于多层级定位方法，适用于各种复杂的室内环境；蓝牙模块铺设方法简单，降低铺设成本；定位精度高鲁棒性好，所实现的精度在2米之内；无需提前采集数据离线训练指纹数据库，缩短铺设时间。

本发明实施例还提供了一种室内行车定位融合装置，能够实现上述任一实施例所述的室内行车定位融合方法的所有流程，装置中的各个模块、单元的作用以及实现的技术效果分别与上述实施例所述的驾驶员驾驶类型识别方法的作用以及实现的技术效果对应相同，这里不再赘述。

如图2所示，本发明提供的一种室内行车定位融合装置的一个优选实施例，包括：

数据获取单元，获取外部节点定位坐标和内部测量角度；

初始化设定单元，初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q₀、初始状态的误差协方差矩阵P₀、测量协方差矩阵R，选择系统模型，设定卡尔曼滤波器的系统矩阵F，所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标；

速度矢量调整单元，根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rot_k，根据旋转矩阵Rot_k调整原速度矢量v_k得到新速度矢量v_k ^r＝Rot_kv_k；

状态向量预测单元，根据系统模型得到系统激励矩阵B_k和噪音激励矩阵N_k，根据所述系统激励矩阵B_k与新速度矢量v_k ^r的积结合噪音激励矩阵N_k得到激励值；根据当前时刻的系统矩阵F_k和上一时刻的状态向量s_k-1结合激励值得到当前时刻的预测状态向量s_k-1 ^k；

误差协方差预测单元，根据当前时刻的系统矩阵F_k、系统激励矩阵B_k和状态更新方程的误差协方差矩阵Q_k,通过上一时刻的误差协方差矩阵P_k-1得到当前时刻的预测误差协方差矩阵P_k-1 ^k；

测量更新单元，根据外部节点定位坐标确定当前时刻的测量向量z_k和对应的测量矩阵H，计算当前时刻的卡尔曼增益K_k，并更新当前时刻的误差协方差矩阵P_k和当前时刻的状态向量s_k。

进一步地，所述初始的状态更新方程的误差协方差矩阵Q₀设定为

进一步地，所述系统模型选定为匀速运动模型，所述卡尔曼滤波器的系统矩阵

所述dt为状态更新的时间间隔，根据系统模型得到系统激励矩阵

和噪音激励矩阵

所述dt_k为当前时刻k与上一时刻k-1之间的时间差。

进一步地，所述旋转矩阵Rot_k通过以下步骤得到：采纳当前时刻与上一时刻针对方向的状态变化差dθ_k；根据状态变化差dθ_k构成旋转矩阵

进一步地，所述预测状态向量s_k-1 ^k通过以下步骤得到：根据系统模型得到系统激励矩阵B_k和噪音激励矩阵N_k，根据所述系统激励矩阵B_k与新速度矢量v_k ^r的积B_kv_k ^r结合噪音激励矩阵N_k得到激励值B_kv_k ^r+N_k；根据当前时刻的系统矩阵F_k和上一时刻的状态向量s_k-1计算得到原预测状态向量F_ks_k-1；根据激励值B_kv_k ^r调整原预测状态向量F_ks_k-1得到预测状态向量s_k-1 ^k，计算式为s_k-1 ^k＝F_ks_k-1+B_kv_k ^r+N_k。

进一步地，所述外部节点定位坐标通过以下步骤获得：使节点在室内可通行区域内间隔一定距离的均匀布置，所述一定距离为节点蓝牙设备的信号稳定距离；进入可行区域获取多个节点信号强度，筛选强度满足阈值的节点进行坐标估算，获得预估的外部节点定位坐标。

本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序；其中，所述计算机程序在运行时控制所述计算机可读存储介质所在的设备执行述实施例任一项所述的室内行车定位融合方法。

如图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 (8)

1.一种室内行车定位融合方法，其特征在于，包括：

获取外部节点定位坐标和内部测量角度；所述内部测量角度用于指示车辆的航向角；

初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q₀、初始状态的误差协方差矩阵P₀、测量协方差矩阵R，选择系统模型，设定卡尔曼滤波器的系统矩阵F，所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标；

根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rot_k，根据旋转矩阵Rot_k调整原速度矢量v_k得到新速度矢量v_k ^r＝Rot_kv_k；

根据系统模型得到系统激励矩阵B_k和噪音激励矩阵N_k，根据所述系统激励矩阵B_k与新速度矢量v_k ^r的积结合噪音激励矩阵N_k得到激励值，根据当前时刻的系统矩阵F_k和上一时刻的状态向量s_k-1结合激励值得到当前时刻的预测状态向量s_k-1 ^k；

根据当前时刻的系统矩阵F_k、系统激励矩阵B_k和状态更新方程的误差协方差矩阵Q_k,通过上一时刻的误差协方差矩阵P_k-1得到当前时刻的预测误差协方差矩阵P_k-1 ^k；

根据外部节点定位坐标确定当前时刻的测量向量z_k和对应的测量矩阵H，计算当前时刻的卡尔曼增益K_k，并更新当前时刻的误差协方差矩阵P_k和当前时刻的状态向量s_k；

其中，k表示当前时刻，k-1表示上一时刻；

所述旋转矩阵Rot_k通过以下步骤得到：

采纳当前时刻与上一时刻针对方向的状态变化差dθ_k；

根据状态变化差dθ_k构成旋转矩阵

2.根据权利要求1所述的一种室内行车定位融合方法，其特征在于，所述初始的状态更新方程的误差协方差矩阵Q₀设定为

3.根据权利要求1所述的一种室内行车定位融合方法，其特征在于，所述系统模型选定为匀速运动模型，所述卡尔曼滤波器的系统矩阵

所述dt为状态更新的时间间隔，根据系统模型得到系统激励矩阵

和噪音激励矩阵

所述dt_k为当前时刻k与上一时刻k-1之间的时间差。

4.根据权利要求1所述的一种室内行车定位融合方法，其特征在于，所述预测状态向量s_k-1 ^k通过以下步骤得到：

根据系统模型得到系统激励矩阵B_k和噪音激励矩阵N_k，根据所述系统激励矩阵B_k与新速度矢量v_k ^r的积B_kv_k ^r结合噪音激励矩阵N_k得到激励值B_kv_k ^r+N_k；

根据当前时刻的系统矩阵F_k和上一时刻的状态向量s_k-1计算得到原预测状态向量F_ks_k-1；

根据激励值B_kv_k ^r调整原预测状态向量F_ks_k-1得到预测状态向量s_k-1 ^k，计算式为s_k-1 ^k＝F_ks_k-1+B_kv_k ^r+N_k。

5.根据权利要求1-4任一项所述的一种室内行车定位融合方法，其特征在于，所述外部节点定位坐标通过以下步骤获得：

使节点在室内可通行区域内间隔一定距离的均匀布置，所述一定距离为节点蓝牙设备的信号稳定距离；

进入可行区域获取多个节点信号强度，筛选强度满足阈值的节点进行坐标估算，获得预估的外部节点定位坐标。

6.一种室内行车定位融合装置，其特征在于，包括：

数据获取单元，获取外部节点定位坐标和内部测量角度；所述内部测量角度用于指示车辆的航向角；

初始化设定单元，初始化状态向量s、初始的状态更新方程的误差协方差矩阵Q₀、初始状态的误差协方差矩阵P₀、测量协方差矩阵R，选择系统模型，设定卡尔曼滤波器的系统矩阵F，所述初始化状态向量中的初始化位置坐标来自获取的外部节点定位坐标；

速度矢量调整单元，根据当前时刻与上个时刻的状态变化为特征构建旋转矩阵Rot_k，根据旋转矩阵Rot_k调整原速度矢量v_k得到新速度矢量v_k ^r＝Rot_kv_k；

状态向量预测单元，根据系统模型得到系统激励矩阵B_k和噪音激励矩阵N_k，根据所述系统激励矩阵B_k与新速度矢量v_k ^r的积结合噪音激励矩阵N_k得到激励值；根据当前时刻的系统矩阵F_k和上一时刻的状态向量s_k-1结合激励值得到当前时刻的预测状态向量s_k-1 ^k；

误差协方差预测单元，根据当前时刻的系统矩阵F_k、系统激励矩阵B_k和状态更新方程的误差协方差矩阵Q_k,通过上一时刻的误差协方差矩阵P_k-1得到当前时刻的预测误差协方差矩阵P_k-1 ^k；

测量更新单元，根据外部节点定位坐标确定当前时刻的测量向量z_k和对应的测量矩阵H，计算当前时刻的卡尔曼增益K_k，并更新当前时刻的误差协方差矩阵P_k和当前时刻的状态向量s_k；

其中，k表示当前时刻，k-1表示上一时刻；

所述旋转矩阵Rot_k通过以下步骤得到：

采纳当前时刻与上一时刻针对方向的状态变化差dθ_k；

根据状态变化差dθ_k构成旋转矩阵

7.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序；其中，所述计算机程序在运行时控制所述计算机可读存储介质所在的设备执行如权利要求1-4任一项所述的室内行车定位融合方法。

8.一种终端设备，其特征在于，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器在执行所述计算机程序时实现如权利要求1-4任一项所述的室内行车定位融合方法。

Images