CN114509069A - 基于蓝牙aoa和imu融合的室内导航定位*** - Google Patents

Abstract

本发明涉及室内导航定位领域，特别公开了一种基于蓝牙AOA和IMU融合的室内导航定位***。基于蓝牙AOA和IMU融合的室内导航定位***包括IMU传感器、IMU位置计算单元、蓝牙AOA传感器、蓝牙AOA位置计算单元以及融合位置计算单元，采用基于扩展卡尔曼滤波的融合定位算法，将蓝牙AOA和IMU相互结合，其中IMU得到的数据作为滤波算法的先验信息，蓝牙AOA得到的数据作为滤波算法的观测信息，利用各自表现出的优点，可以有效提高融合定位***的导航定位精度，利用少数基站即可实现目标的高精度室内导航定位，实现其在高精度室内导航定位场景需求中的应用。

Description

基于蓝牙AOA和IMU融合的室内导航定位***

技术领域

本发明涉及室内导航定位领域，尤其涉及一种基于蓝牙AOA和IMU融合的室内导航定位***。

背景技术

传统室内定位方法包括红外定位、地磁导航定位、WIFI定位、蓝牙RSSI(ReceivedSignal Strength Indication)定位以及超宽带定位技术等。其中，传统蓝牙RSSI定位技术采用接收信号强度值的方法进行定位，即各定位基站(≥3)先对目标发射的无线信号进行接收，然后将当前接收信号强度值代入信号衰减公式，估计出目标的距离范围，最后通过三边测量算法完成目标定位。该方法的定位精度通常为2～5m，且由于目标与定位基站间的信号传输通道较为复杂，通常伴随着信号的多径传播以及周围环境造成的干扰，因此该定位方法的精度很难提高。

2019年蓝牙技术联盟正式推出蓝牙5.1协议规范，新增了AOA(Angle Of Arrival)寻向功能，该功能可帮助定位基站明确蓝牙信号的传输方向，即通过检测蓝牙信号到达基站各天线的相位数据，各基站可以解算出信号传输方向与基站自身法线的夹角，同时在不少于两个基站的情况下，可通过三角定位算法计算出目标所在位置。

AOA寻向功能的出现提高了蓝牙定位***的精度，然而蓝牙AOA定位技术也依旧面临着外界环境的干扰，如非视距误差、多径干扰、杂波信号、噪声及其他干扰等。因此需要提出一种有效的处理方法，解决室内复杂场景下的诸多干扰，提高蓝牙定位***的精度与稳定性。

惯性测量单元IMU由于依靠自身传感器进行自主导航，无需接收外来信号数据，有着自主性强、不受外界干扰的优点，因此在室内场景下常作为定位辅助设备。但IMU的定位精度与自身器件精度紧密相关，存在定位误差随时间发散、难以长时间独立导航的问题。

因此如何解决蓝牙定位***由于外界干扰无法有效定位，以及IMU长期定位精度差的问题，使定位***的连续性与稳定性得到增强，将成为本发明的重点研究内容。

发明内容

本发明针对蓝牙定位***由于外界干扰无法有效定位，以及IMU长期定位精度差的问题，提供一种基于蓝牙AOA和IMU融合的室内导航定位***。

本发明提供了以下方案：

一种基于蓝牙AOA和IMU融合的室内导航定位***，包括：IMU传感器、IMU位置计算单元、蓝牙AOA传感器、蓝牙AOA位置计算单元、融合位置计算单元。

所述的IMU传感器安装于载体目标上，用以采集载体运动过程中的三轴加速度数据与三轴角速度数据。

所述的IMU位置计算单元与IMU传感器连接，并根据三轴加速度数据与三轴角速度数据对目标进行定位，得到目标的第一定位数据。

所述蓝牙AOA传感器包括固定在目标上的未知发射节点和固定在空间已知位置上的两个基站接收节点，所述未知发射节点用于向空间中发射蓝牙信号数据，所述基站接收节点用于采样所述未知发射节点发射出的蓝牙信号数据获得蓝牙信号采样数据，并将蓝牙信号采样数据发送到所述蓝牙AOA位置计算单元。

所述蓝牙AOA位置计算单元根据所述蓝牙信号采样数据对所述目标进行定位，得到目标的第二定位数据。

所述融合位置计算单元对所述IMU位置计算单元和所述蓝牙AOA位置计算单元的数据进行扩展卡尔曼滤波融合，得到最终的目标定位数据。

进一步地，所述IMU位置计算单元对三轴加速度数据与三轴角速度数据进行处理，用以计算出载体目标的方向姿态信息与位置坐标信息。

进一步地，所述IMU位置计算单元对三轴加速度数据与三轴角速度数据进行处理具体包括：利用三轴加速度计获取目标静止状态下的横滚角r和俯仰角p，利用三轴陀螺仪获取目标动态下的欧拉角数据，再将上述静止状态和动态下获取的数据进行互补融合。

进一步地，所述蓝牙AOA位置计算单元采用基于测角的定位方法，所述测角的定位方法采用一种改进的多重信号分类算法，该算法的步骤包括：

(1)基站节点对信号进行采样，获得蓝牙信号采样数据X；

(2)计算采样数据自相关矩阵R_x，并进一步得到前向平滑矩阵R^f；

(3)对前向平滑矩阵R^f进行特征分解，构造空间谱函数P(θ)；

(4)利用空间谱函数进行谱峰搜索，得到信号到达角矩阵[θ₁，θ₂，...，θ_D]；

(5)利用到达角数据[θ₁，θ₂，…，θ_D]重构阵列流形矩阵A，逆推得到信号自相关矩阵R_s；

(6)利用信号自相关矩阵R_s中，主对角线元素的大小关系，剔除多径信号，得到蓝牙信号到达角度θ；

(7)当基站获得蓝牙信号到达角度数据后，使用三角定位算法计算出载体目标位置坐标；

(8)利用当前时刻与上一时刻的载体目标位置坐标，确定目标当前的航向角信息。

与现有技术相比，本发明提供的基于蓝牙AOA和IMU融合的室内导航定位***，采用融合定位算法，将蓝牙AOA与IMU相互结合，IMU得到的数据作为扩展卡尔曼滤波的先验信息，蓝牙AOA得到的数据作为扩展卡尔曼滤波的观测信息。利用各自表现出的优点，利用少量的接收基站即可实现目标的高精度室内导航定位，有效提高了室内定位***的精度与稳定性。

附图说明

图1为本发明实施例基于蓝牙AOA和IMU融合的室内导航定位***的架构示意图；

图2为基于IMU的导航定位算法流程示意图；

图3为三角定位算法计算目标位置示意图；

具体实施方式

为使本发明的技术方案和优点更加清楚，以下结合附图和具体实施例对本发明提出的技术方案作进一步详细说明。

如图1所示，本发明实施例提供的基于蓝牙AOA和IMU融合的室内导航定位***，包括IMU传感器1、IMU位置计算单元2、蓝牙AOA传感器3、蓝牙AOA位置计算单元4、融合位置计算单元5。其中：

IMU传感器1安装于目标上，用于检测目标的三轴加速度与三轴角速度，并将三轴加速度数据与三轴角速度数据发送给IMU位置计算单元2。

IMU位置计算单元2根据三轴加速度数据和三轴角速度数据对目标进行定位，得到目标的第一定位数据。

蓝牙AOA传感器3包括固定在目标上的未知发射节点和固定在空间已知位置上的两个基站接收节点，所述未知发射节点用于向空间中发射蓝牙信号数据，所述基站接收节点用于采样所述未知发射节点发射出的蓝牙信号数据，并将蓝牙信号采样数据发送到所述蓝牙AOA位置计算单元4。

蓝牙AOA位置计算单元4根据所述蓝牙信号采样数据对所述目标进行定位，得到目标的第二定位数据。

融合位置计算单元5对IMU位置计算单元2和蓝牙AOA位置计算单元4的数据进行扩展卡尔曼滤波融合，得到最终的目标定位数据。

理论上，求解未知节点的位置至少需要两个基站接收节点。本发明结合IMU传感器1和蓝牙AOA传感器3，提高了定位***的精度与稳定性。MATLAB仿真结果表明，IMU传感器1提供的先验信息可以显著抑制蓝牙AOA传感器3在非视距条件下的观测误差，与仅使用蓝牙AOA传感器3或IMU传感器1相比，融合***的定位精度和稳定性有了明显提高。实验结果表明，该算法在实际的室内导航定位场景下具有一定的定位精度与稳定性。

基于IMU的导航定位处理算法：

惯性导航定位***通过一系列精确定义的参考坐标系，实时计算出***的姿态矩阵，通过将测量数据在不同坐标系间进行转换，计算出目标的速度和位置，实现导航定位功能。其中，参考坐标系及其坐标变换是惯性导航定位算法的基础，姿态更新是算法的核心。基于IMU的导航定位算法流程如图2所示。

为了准确、完整地描述载体的空间运动状态，需要选取合适的参考坐标系。由载体体轴确定的坐标系到惯性导航***采用的坐标系之间的变换矩阵，称为载体姿态矩阵。其中，陀螺仪与加速度等测量仪器的坐标属于载体坐标系，而加速度、速度和位置的最终输出属于导航坐标系。假设在初始时刻，载体坐标系与导航坐标系重合，将惯性测量单元依次绕载体坐标系的Z、Y、X轴进行旋转，则将三次旋转产生的坐标变换矩阵用欧拉角法分别表示为：

式中，y表示航向角，绕载体坐标系的Z轴旋转；p表示俯仰角，绕载体坐标系的Y轴旋转；r表示横滚角，绕载体坐标系的X轴旋转。分别利用加速度计和陀螺仪传感器对欧拉角进行测量，其中加速度计可在静止状态下，通过重力加速度在各个轴上产生的分量，对当前姿态的横滚角r和俯仰角p进行测量。而陀螺仪可通过对角速度进行时间积分与累加，对动态环境下的角度进行测量。两者的测量过程可用以下公式进行表示：

静止状态下，利用加速度计输出值[a_x a_y a_z]^T测量欧拉角的过程如下：

对上式进行解算，得到：

载体非静止状态下，利用陀螺仪输出值[g_x g_y g_z]^T测量欧拉角的过程如下：

其中：

对上式进行解算，得到：

利用加速度计和陀螺仪获取到欧拉角数据后，将两者数据进行互补融合，降低陀螺仪由于连续积分造成的测量误差：

0≤K≤1

其中K为比例系数，具体数值可根据实际情况调整。

利用欧拉角数据可实时计算载体坐标系与导航坐标系的坐标变换。假设载体坐标系为B系，导航坐标系为N系，令

为坐标变换矩阵，则：

同时，利用加速度计测量的载体坐标系下的加速度值如下所示：

则通过坐标变换矩阵

得到导航坐标系下的加速度值如下：

将重力加速度从测量值中去除，得：

当采样间隔较短时，载体的速度变化等于载体在导航坐标系中相对于时间Δt的瞬时加速度的积分：

则载体在导航坐标系下的速度表达如下：

最后积分得到载体的位置如下：

基于蓝牙AOA的导航定位处理算法：

基于无线信号的定位方法很多，一般分为测距、测角定位方法以及非测距定位方法。在定位过程中，如果用节点之间的距离信息或信号到达的角度信息来计算位置，则称为测距定位方法。如使用节点间的连接和多跳路由信息来估计距离，而不直接测量角度和距离信息，则称为非测距定位方法。蓝牙AOA定位***采用基于测角的定位方法，一般分为两步。第一步利用基站接收节点对未知发射节点发射的蓝牙信号进行采样处理，获得未知发射节点相对于基站接收节点角度数据，第二步是使用获得的角度数据计算坐标。基于蓝牙AOA的定位算法如下。

针对传统测角算法精度欠缺，且由于蓝牙信号在室内传播过程中易受多径效应的影响，使得蓝牙信号的传播过程受到极大干扰。因此本发明使用一种改进的多重信号分类算法，该算法的步骤包括：

(1)基站节点对信号进行采样，获得蓝牙信号采样数据X；

(2)计算采样数据自相关矩阵R_x，并进一步得到前向平滑矩阵R^f；

(3)对前向平滑矩阵R^f进行特征分解，构造空间谱函数P(θ)；

(4)利用空间谱函数进行谱峰搜索，得到信号到达角矩阵[θ₁，θ₂，...，θ_D]；

(5)利用到达角数据[θ₁，θ₂，...，θ_D]重构阵列流形矩阵A，逆推得到信号自相关矩阵R_s；

(6)利用信号自相关矩阵R_s中，主对角线元素的大小关系，剔除多径信号，得到蓝牙信号到达角度θ；

步骤(1)中，假设该算法使用M根接收天线对空间中的蓝牙信号进行采样，相邻两根天线的距离为d。由于多径效应影响，假设空间中存在D-1个多径信号，其与蓝牙信号的波长λ相同。同时，假设各信号到达接收天线的角度为：[θ₁，θ₂，...，θ_D]，则接收天线采样后的信号数据可表达如下：

X＝AS+N (1)

其中，X为信号采样数据，A为阵列流型矩阵，S为信号向量，N为噪声向量。各符号的具体数学表达如下：

X＝[x₁(t)，x₂(t)，...x_M(t)]^T

S＝[S₁(t),S₂(t),..S_D(t)]^T

N＝[n₁(t)，n₂(t)，...，n_M(t)]^T

其中，

步骤(2)中，采样数据自相关矩阵R_x可经如下计算过程得到：

其中，R_s＝E[SS^H]表示信号自相关矩阵，R_N＝σ²I表示噪声矩阵，σ²为噪声功率，I为单位向量。同时，该算法将M根接收天线组成的天线阵列分成相互重叠的p个子阵，各子阵的天线数为m。其中：m＝M-p+1且m≥D+1。此时前向平滑矩阵R^f可经如下计算过程得到：

其中：

步骤(3)(4)中，对前向平滑矩阵R^f进行特征分解，得到M个正实特征值λ₁，λ₂，...，λ_M，分别对应着M个特征向量v₁，v₂，...，v_M。将特征值降序排列得：λ₁≥λ₂≥…≥λ_M。其中，D个较大的特征值对应着信号分量，M-D个较小的特征值对应于噪声分量。同理，矩阵R^f中的特征向量也分别对应着信号分量与噪声分量。同时由于R^f矩阵为厄密特矩阵，不同特征值对应的特征向量相互正交。利用该特点构造空间谱函数P(θ)如下：

其中，a(θ)为各信号对应的流型向量。E_n＝[v_D+1，v_D+2，...，v_M]为噪声矩阵，由噪声分量对应的特征向量构成。式(4)分母为a(θ)与E_n的内积，当a(θ)和E_n的各列正交时，分母理论上为零。由于实际信号采样存在一定的量化误差与噪声干扰，使得a(θ)和E_n的正交性达不到理论效果，从而分母实际上为一个极小值，因此P(θ)为一极大值。利用该函数，通过遍历所有可能的信号到达角度θ，计算出P(θ)的输出结果并绘制谱峰图。通过寻找P(θ)图形的波峰，即可得出各信号到达接收天线的角度信息，获得信号到达角数据矩阵[θ₁，θ₂，...，θ_D]。

步骤(5)(6)中当获得信号到达角数据矩阵[θ₁，θ₂，...，θ_D]后，可进一步计算出阵列流型矩阵A。前面介绍接收天线采样后的信号表达式及其自相关矩阵为：

基于以上公式，我们对R_s进行变换，得：

A^H(R_x-R_N)A＝A^HAR_sA^HA

R_s＝(A^HA)^-1A^H(R_x-R_N)A(A^HA)^-1

当信噪比较大时，上式中的噪声项R_N可忽略不计，R_s进一步简化为：

R_s＝(A^HA)^-1A^HR_xA(A^HA)^-1 (5)

此时，我们便得到了信号自相关矩阵R_s的计算表达式。

同时由于：

明显看出，信号自相关矩阵R_s中，主对角线元素表示各信号的幅值大小。因此当获得各信号的到达角数据后，通过计算阵列流形矩阵A，再利用式(5)计算出信号自相关矩阵R_s，通过比较R_s中主对角线元素的大小，即可获得对应信号的幅值大小。而由于多径信号的传播距离远于直达信号，因此多径信号的路径损耗要高于直达信号，这使得多径信号的幅值相对更小。因此R_s中幅值最大的信号即为蓝牙直达信号，其对应角度即为所求的蓝牙信号到达角信息。

当基站获得蓝牙信号到达角数据后，即可使用三角定位算法计算出目标位置坐标。如图3所示，假设目标坐标为(x，y)，基站坐标分别为(x₁，y₁)，(x₂，y₂)，基站接收的蓝牙信号到达角分别为α，β。则目标位置坐标可用公式表示如下：

最后，利用当前时刻与上一时刻的载***置坐标，确定目标当前的航向角信息：

基于蓝牙AOA和IMU融合的室内导航定位算法：

在惯性导航***中，由于受误差积分影响，加速度计和陀螺仪数据容易出现发散。蓝牙AOA可以在室内定位***中提供亚米级至厘米级的定位精度，但在室内复杂环境下，蓝牙无线信号易受外界干扰从而对定位精度产生影响。因此，通过使用数学方法将蓝牙AOA和IMU定位***的数据进行融合优化，可以有效提高对目标的定位精度。

利用扩展卡尔曼滤波算法，可以将非线性模型进行线性化，从而对目标进行最优估计。基于蓝牙AOA和IMU的扩展卡尔曼滤波融合算法包括：

初始化：

初始化***状态向量

初始化***状态协方差矩阵

对***状态做一步预测：

对***状态协方差矩阵做一步预测：

求扩展卡尔曼增益矩阵：

状态更新：

协方差矩阵更新：

P_k＝(I-K_kH_k)P_k|k-1

在二维平面内，假设目标在k时刻的状态向量包含位置、速度、加速度、航向角以及航向角速度信息，用式(6)表示如下：

在实际过程中加速度与角速度受空气阻力或其他外部因素干扰，因此引入过程噪声。根据匀加速直线运动方程得到如下方程：

将上述方程以矩阵形式表示，则状态方程如式(8)所示：

其中：

上式中A_k+1表示状态转换矩阵，ω(k+1)表示均值为0，方差为Q的过程噪声。同时由于蓝牙AOA传感器检测目标与基站之间的相对角度，因此观测方程如式(9)所示：

式中H(k)为观测矩阵，V(k)为观测噪声矩阵，

表示目标与基站间的相对角度，如下所示：

由于观测方程(10)是非线性的，需要对其进行线性化，将其进行一阶泰勒展开。H(k)是线性化观测矩阵，即雅可比矩阵，其求解过程如式(11)所示。在视距条件下，测量噪声服从高斯分布，均值为0，方差为σ。在非视距条件下，测量噪声仍服从高斯分布，但其均值不为零，方差增大。R为噪声方差矩阵，如式(12)所示：

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

Claims (4)

1.基于蓝牙AOA和IMU融合的室内导航定位***，其特征在于，包括IMU传感器、IMU位置计算单元、蓝牙AOA传感器、蓝牙AOA位置计算单元、融合位置计算单元；

所述IMU传感器安装于载体目标上，用以采集载体运动过程中的三轴加速度数据与三轴角速度数据；

所述IMU位置计算单元与IMU传感器连接，并根据三轴加速度数据与三轴角速度数据对目标进行定位，得到目标的第一定位数据；

所述蓝牙AOA传感器包括固定在目标上的未知发射节点和固定在空间已知位置上的两个基站接收节点，所述未知发射节点用于向空间中发射蓝牙信号数据，所述基站接收节点用于采样所述未知发射节点发射出的蓝牙信号数据获得蓝牙信号采样数据，并将蓝牙信号采样数据发送到所述蓝牙AOA位置计算单元；

所述蓝牙AOA位置计算单元根据所述蓝牙信号采样数据对所述目标进行定位，得到目标的第二定位数据；

所述融合位置计算单元对所述IMU位置计算单元和所述蓝牙AOA位置计算单元的数据进行扩展卡尔曼滤波融合，得到最终的目标定位数据。

2.如权利要求1所述的基于蓝牙AOA和IMU融合的室内导航定位***，其特征在于，所述IMU位置计算单元对三轴加速度数据与三轴角速度数据进行处理，用以计算出载体目标的方向姿态信息与位置坐标信息。

3.如权利要求2所述的基于蓝牙AOA和IMU融合的室内导航定位***，其特征在于，利用三轴加速度计获取目标静止状态下的横滚角r和俯仰角p，利用三轴陀螺仪获取目标动态下的欧拉角数据，将静止状态和动态下获取的数据进行互补融合。

4.如权利要求1所述的基于蓝牙AOA和IMU融合的室内导航定位***，其特征在于，所述蓝牙AOA位置计算单元采用基于测角的定位方法，所述测角的定位方法采用一种改进的多重信号分类算法，该算法的步骤包括：

(1)基站节点对信号进行采样，获得蓝牙信号采样数据X；

(2)计算采样数据自相关矩阵R_x，并进一步得到前向平滑矩阵R^f；

(3)对前向平滑矩阵R^f进行特征分解，构造空间谱函数P(θ)；

(4)利用空间谱函数进行谱峰搜索，得到信号到达角矩阵[θ₁，θ₂，...，θ_D]；

(5)利用到达角数据[θ₁，θ₂，...，θ_D]重构阵列流形矩阵A，逆推得到信号自相关矩阵R_s；

(6)利用信号自相关矩阵R_s中，主对角线元素的大小关系，剔除多径信号，得到蓝牙信号到达角度θ；

(7)当基站获得蓝牙信号到达角度数据后，使用三角定位算法计算出载体目标位置坐标；

(8)利用当前时刻与上一时刻的载体目标位置坐标，确定目标当前的航向角信息。

Images