基于可穿戴MARG传感器和蓝牙定位的混合追踪方法及系统与流程

基于可穿戴marg传感器和蓝牙定位的混合追踪方法及系统
技术领域
1.本发明涉及传感器技术领域，特别涉及定位方法技术领域，具体是指一种基于可穿戴marg传感器和蓝牙定位的混合追踪方法及系统。


背景技术：

2.现有技术中，基于卫星信号的gps定位技术已被广泛应用。然而，在室内定位时，由于卫星信号弱经常存在定位不准的情况。
3.而pdr(pedestrian dead reckoning)定位技术是一种更适用于室内的定位方法。pdr行人航迹推算也被称为人体轨迹推算，是一种基于自带imu或marg(magnetic,angular rate and gravity)传感器进行室内定位的技术。其突出优势在于可抗干扰地提供实时、连续、精准的位置信息；经常被用于如室内运动员轨迹定位、消防员火场位置判断等领域。由于一般的mems(microelectronic mechanical system)传感器精度不高，通过将加速度两次积分得到行走距离，将陀螺仪积分得到航向变化值的方式无法获得准确的人体轨迹。因此1996年levi,robert w和judd,thomas提出了dead reckoning航位推算算法，使得通过imu传感器推算人体轨迹的精确度得到了大幅度的提升，通过近年来在pdr算法上的不断提升使得精度达到了1％左右。但是对于超过一定距离范围的人体轨迹如1000米则相对误差达到了10米所以在人体轨迹推算领域dead reckoning航位推算算法依然无法被广泛的推广使用。
4.蓝牙rssi中是received signal strength indication的简称，是接收的信号强度指示，无线发送层的可选部分，用来判定链接质量以及是否增大广播发送强度。通过接收到的信号强弱测定信号点与接收点的距离，进而根据相应数据进行定位计算的一种定位技术。蓝牙rssi定位是指基于蓝牙协议，利用蓝牙rssi信号衰减与距离之间的相关性关系来计算出蓝牙终端所在位置的一种定位方式。蓝牙设备在广播中，信号接收端离发射端越远，接收到的rssi场强值就越弱；信号接收端离发射端越近，接收到的rssi场强值就越强。接收端收到的无线信号dbm一般都是负值，信号值越大代表信号越强，rssi信号范围在0～-100之间。预先已知三个或三个以上的蓝牙信标的位置，基于无线电衰减模型每个信标与被测信标测得距离，然后基于蓝牙信标定位模型得到被测信标的位置。但在实际应用中，rssi受新号反射、散射、绕射等多重路径衰竭与遮挡的影响非常严重。目前业界使用beacon定位的精度可以达到3～5米左右。
5.所以不论是基于惯性导航期间的行人航迹推算或者是基于蓝牙rssi的室内定位方法，其精度都无法达到亚米级，难以满足精确定位人体轨迹、位置的目的。因此，如何对pdr定位技术进行改进，消除累积误差，提高定位精度，成为本领域亟待解决的问题。


技术实现要素：

6.本发明的目的是克服了上述现有技术中的缺点，提供一种基于蓝牙rssi的多点融合定位算法，从而消除为了消除传统pdr系统中存在的累积误差，实现高精度的人体轨迹混
合追踪方法及系统。
7.为了实现上述的目的，本发明的基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪方法，包括以下步骤：
8.(10)获取所述的marg传感器的传感器数据，并根据所述的传感器数据计算初始三维姿态；
9.(20)根据所述的初始三维姿态进行姿态滤波获取实时传感器姿态数据；
10.(30)利用所述的实时传感器姿态数据计算获得在地理坐标系下的该marg传感器在各个方向的位移数据；
11.(40)利用蓝牙指纹法计算获得所述的marg传感器与各个蓝牙信标之间的距离，并计算出所述的marg传感器的位置信息；
12.(50)利用卡尔曼滤波算法将所述的marg传感器的位置信息与和各个方向的位移数据融合，获得精确定位数据。
13.该基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪方法中，所述的步骤(10)具体包括以下步骤：
14.(11)将marg传感器佩戴在用户身上，所述的marg传感器至少包括加速器计、陀螺仪和磁力计；
15.(12)所述的加速器计、陀螺仪和磁力计实时获得测量信息；
16.(13)嵌入式mcu或上位计算机与所述的加速器计、陀螺仪和磁力计进行通讯，获得所述的测量信息，并存储在存储器中；
17.(14)所述的嵌入式mcu或上位计算机利用所述的加速器计的测量信息，基于重力加速度计算获得所述的marg传感器的俯仰角信息和翻滚角信息，利用所述的磁力计的测量信息获得航向角信息，并利用所述的陀螺仪的测量信息获取体坐标系姿态估计信息；所述的初始三维姿态包括所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息。
18.该基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪方法中，所述的步骤(20)具体为：
19.根据所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息，采用ekf滤波算法获得地理坐标系姿态估计数据，作为所述的实时传感器姿态数据。
20.该基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪方法中，所述的步骤(30)具体包括以下步骤：
21.(31)根据所述的实时传感器姿态数据该marg传感器获得每一时刻的线性加速度；
22.(32)利用零速修正算法对所述的线性加速度的漂移进行修正，并计算获得在地理坐标系下，该marg传感器在各个方向的位移数据。
23.该基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪方法中，所述的步骤(40)具体包括以下步骤：
24.(41)利用蓝牙指纹法计算获得所述的marg传感器与至少三个蓝牙信标之间的距离；
25.(42)根据该marg传感器与各个蓝牙信标之间的距离，利用knn算法计算出所述的marg传感器的位置信息。
26.本发明还提供一种基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪系统，其包括：可穿戴marg传感器以及集成于该可穿戴marg传感器的蓝牙设备，还包括集成于该可穿戴marg传感器的嵌入式mcu或信号连接该可穿戴marg传感器的上位计算机，其中，
27.所述的可穿戴marg传感器用以实时获得传感器数据；
28.所述的嵌入式mcu或上位计算机用以获取所述的传感器数据，并根据所述的传感器数据计算初始三维姿态；根据所述的初始三维姿态进行姿态滤波获取实时传感器姿态数据；利用所述的实时传感器姿态数据计算获得在地理坐标系下的该marg传感器在各个方向的位移数据；利用蓝牙指纹法计算获得所述的marg传感器的蓝牙设备与各个蓝牙信标之间的距离，并计算出所述的marg传感器的位置信息；并利用卡尔曼滤波算法将所述的marg传感器的位置信息与和各个方向的位移数据融合，获得精确定位数据。
29.该基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪系统中，该可穿戴marg传感器佩戴在用户身上，所述的marg传感器至少包括加速器计、陀螺仪和磁力计；所述的加速器计、陀螺仪和磁力计用以实时获得测量信息；
30.所述的嵌入式mcu或上位计算机用以与所述的加速器计、陀螺仪和磁力计进行通讯，获得所述的测量信息，并存储在存储器中；并利用所述的加速器计的测量信息，基于重力加速度计算获得所述的marg传感器的俯仰角信息和翻滚角信息，利用所述的磁力计的测量信息获得航向角信息，并利用所述的陀螺仪的测量信息获取体坐标系姿态估计信息；所述的初始三维姿态包括所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息。
31.该基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪系统中，所述的嵌入式mcu或上位计算机具体用以根据所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息，采用ekf滤波算法获得地理坐标系姿态估计数据，作为所述的实时传感器姿态数据。
32.该基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪系统中，所述的嵌入式mcu或上位计算机还用以根据所述的实时传感器姿态数据该marg传感器获得每一时刻的线性加速度；并利用零速修正算法对所述的线性加速度的漂移进行修正，并计算获得在地理坐标系下，该marg传感器在各个方向的位移数据。
33.该基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪系统中，所述的嵌入式mcu或上位计算机还用以利用蓝牙指纹法计算获得所述的marg传感器与至少三个蓝牙信标之间的距离；根据该marg传感器与各个蓝牙信标之间的距离，利用knn算法计算出所述的marg传感器的位置信息。
34.采用了该发明的基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪方法及系统，其利用实时传感器姿态数据计算获得该marg传感器在各个方向的位移数据；并利用蓝牙指纹法计算获得marg传感器的位置信息；进而利用卡尔曼滤波算法将marg传感器的位置信息与和各个方向的位移数据融合，获得精确定位数据，实现对于可穿戴marg传感器佩戴者的人体轨迹追踪。由于采用了融合滤波算法，因此，本发明的定位延迟和误差都得到了有效的降低，从而可以通过可穿戴设备获得高精度的人体轨迹、位置、朝向信息，方便地应用于博物馆、美术馆、大型游戏场馆等需要行人与环境互动的室内场景中。
附图说明
35.图1为本发明的基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪方法的流程示意图。
36.图2为pdr航位推算原理示意图。
37.图3为蓝牙rssi定位原理示意图。
38.图4为本发明的方法在实际应用中所采用的融合算法的框图。
具体实施方式
39.为了能够更清楚地理解本发明的技术内容，特举以下实施例详细说明。
40.请参阅图1所示，为本发明的基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪方法的流程示意图。
41.在一种实施方式中，该基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪方法包括以下步骤：
42.(10)获取所述的marg传感器的传感器数据，并根据所述的传感器数据计算初始三维姿态；
43.(20)根据所述的初始三维姿态进行姿态滤波获取实时传感器姿态数据；
44.(30)利用所述的实时传感器姿态数据计算获得在地理坐标系下的该marg传感器在各个方向的位移数据；
45.(40)利用蓝牙指纹法计算获得所述的marg传感器与各个蓝牙信标之间的距离，并计算出所述的marg传感器的位置信息；
46.(50)利用卡尔曼滤波算法将所述的marg传感器的位置信息与和各个方向的位移数据融合，获得精确定位数据。
47.在优选的实施方式中，所述的步骤(10)具体包括以下步骤：
48.(11)将marg传感器佩戴在用户身上，所述的marg传感器至少包括加速器计、陀螺仪和磁力计；
49.(12)所述的加速器计、陀螺仪和磁力计实时获得测量信息；
50.(13)嵌入式mcu或上位计算机与所述的加速器计、陀螺仪和磁力计进行通讯，获得所述的测量信息，并存储在存储器中；
51.(14)所述的嵌入式mcu或上位计算机利用所述的加速器计的测量信息，基于重力加速度计算获得所述的marg传感器的俯仰角信息和翻滚角信息，利用所述的磁力计的测量信息获得航向角信息，并利用所述的陀螺仪的测量信息获取体坐标系姿态估计信息；所述的初始三维姿态包括所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息。
52.所述的步骤(20)具体为：
53.根据所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息，采用ekf滤波算法获得地理坐标系姿态估计数据，作为所述的实时传感器姿态数据。
54.所述的步骤(30)具体包括以下步骤：
55.(31)根据所述的实时传感器姿态数据该marg传感器获得每一时刻的线性加速度；
56.(32)利用零速修正算法对所述的线性加速度的漂移进行修正，并计算获得在地理
坐标系下，该marg传感器在各个方向的位移数据。
57.在进一步优选的实施方式中，所述的步骤(40)具体包括以下步骤：
58.(41)利用蓝牙指纹法计算获得所述的marg传感器与至少三个蓝牙信标之间的距离；
59.(42)根据该marg传感器与各个蓝牙信标之间的距离，利用knn算法计算出所述的marg传感器的位置信息。
60.本发明还提供一种基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪系统。该系统包括：可穿戴marg传感器以及集成于该可穿戴marg传感器的蓝牙设备，还包括集成于该可穿戴marg传感器的嵌入式mcu或信号连接该可穿戴marg传感器的上位计算机。该可穿戴marg传感器佩戴在用户身上。其中，所述的可穿戴marg传感器用以实时获得传感器数据。所述的嵌入式mcu或上位计算机用以获取所述的传感器数据，并根据所述的传感器数据计算初始三维姿态；根据所述的初始三维姿态进行姿态滤波获取实时传感器姿态数据；利用所述的实时传感器姿态数据计算获得在地理坐标系下的该marg传感器在各个方向的位移数据；利用蓝牙指纹法计算获得所述的marg传感器的蓝牙设备与各个蓝牙信标之间的距离，并计算出所述的marg传感器的位置信息；并利用卡尔曼滤波算法将所述的marg传感器的位置信息与和各个方向的位移数据融合，获得精确定位数据。
61.进一步的，该可穿戴marg传感器至少包括加速器计、陀螺仪和磁力计；所述的加速器计、陀螺仪和磁力计用以实时获得测量信息；所述的嵌入式mcu或上位计算机用以与所述的加速器计、陀螺仪和磁力计进行通讯，获得所述的测量信息，并存储在存储器中；并利用所述的加速器计的测量信息，基于重力加速度计算获得所述的marg传感器的俯仰角信息和翻滚角信息，利用所述的磁力计的测量信息获得航向角信息，并利用所述的陀螺仪的测量信息获取体坐标系姿态估计信息；所述的初始三维姿态包括所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息。
62.在实际应用中，本发明提出了一种基于marg和蓝牙rssi的混合室内定位算法，如图4所示，其融合了两种算法，实现了一种可达到亚米级精度的人体轨迹推算和目标定位方法，并同时提供了行人面向信息，从而满足了环境互动对精度的需求。
63.pdr航位推算的行人航迹推算算法，其原理如图2所示。能在无信标条件下，通常通过加速器计、陀螺仪来推算方向角，感知行人在行进过程中的加速度和方向角，并利用这些数据对行走路线进行相对定位，从而达到对行人进行定位跟踪的目的。
64.但是在实际使用中由于陀螺仪存在累计漂移因此方向角角的累计误差非常的大而不具有可用性。因此本方法使用了marg(加速器计、陀螺仪、磁力计)传感器，磁力计的加入可以提供一个指向磁北极的恒定方向可以用来补偿陀螺仪的累计漂移。但是由于加速度计提供的加速度信息需要经过两次积分才能获取相对的运动距离信息，即使加速度计只有一个非常小的误差，经过两次积分后在较短的时间内误差依旧会积累到一个客观的地步。因此零速修正算法被用来修正加速度计的累计误差。
65.当行人处于静止状态时，认为行人此时的加速度都为零，则可以通过估算静止状态时加速器计、陀螺仪的漂移而获取整个周期内加速器计、陀螺仪的漂移值，进而以此对运动方向和运动速度进行修正。由于该修正不仅作用于加速度，同样也作用于速度，因此可以有效地消除由于加速度两次积分而带来的运动距离信息快速发散的问题。在实际使用中由
于加速度计存在测量误差通常选取加速度三轴的读数的均方根小于某一个阈值来作为零速的判断依据，人体的运动模式的不同(慢走、快走、奔跑)及传感器的配带位置不同对阈值均有较大的影响，因此在零速修正算法中需考虑这两点。
66.具体来说，姿态估计部分采用ekf滤波算法。
67.首先定义四元数旋转矩阵：
[0068][0069]
由陀螺仪得到的角速度变化，可以定位为四元数对时间的积分：
[0070][0071]
同时定义g(q)
[0072][0073]
由此扩展卡尔曼滤波的传递函数如下:
[0074][0075]
噪声传统函数如下：
[0076]
q＝(gyronoise*dt)2*gg
t
得到预测四元数为：
[0077]
q＝f
tq[0078][0079]
观测四元数：
[0080]
首先对加速度和磁力计数据归一化：
[0081][0082][0083]
得到预测值为：
[0084]
pa＝normalize(-r(q)gn)
[0085]
pm＝normalize(r(q)mn)
[0086]
并构建出误差矩阵：
[0087][0088]
下一步更新卡尔曼增益：
[0089][0090]
s＝hph
t
r
[0091]
k＝ph
t
s-1
[0092]
q＝q k∈
t
[0093]
p＝p-khp
[0094]
最终后验纠正姿态四元数：
[0095][0096][0097]
位置估计算法：
[0098]
由姿态估计部分得到每一时刻点的线性加速度：
[0099]ab
＝a
t
r(q)gn[0100]
通过两次积分可得到位置的变化：
[0101][0102]
蓝牙rssi(received signal strength indication，接收信号强度指示)是无线传输层用来判定链接质量的重要指标。传输层根据rssi判断是否需要增大发送端的发送强度。通常情况下，信号的能量以1mw为基准，以对数形式表示信号强度，即rssi，单位为dbm(decibel-milliwatts)：分贝毫瓦。所以在无线信号中，1mw就是0dbm，能量小于1mw的信号rssi为负数，能量大于1mw的信号rssi为正数。rssi受发送功率、路径衰减、接收增益和系统处理增益四个元素影响。
[0103]
由rssi计算距离的公式如下：
[0104]
d＝10^((abs(rssi)-a)/(10*n))
[0105]
其中：d为计算所得距离；rssi为接收信号强度(负值)；a为发射端和接收端相隔1米时的信号强度；n为环境衰减因子。
[0106]
如图3所示，实现蓝牙rssi进行定位需要三个以上的蓝牙信标，由以上的rssi计算距离公式获得设备与各个蓝牙信标的距离信息后，即可使用三角定位法获取设备的位置信息。但是由于信号的严重衰减和多径效应，使得三点定位算法得到的坐标精准度较差。蓝牙指纹法可以在不增加成本的前提下有效的提高定位精度。rssi蓝牙指纹定位法分为两个阶段：离线阶段和在线阶段。离线阶段将每个位置的坐标记录以及保存特征(rssi)，最后收集完所有指纹得到指纹库。在线阶段则通过实际的特征查询指纹库获得定位。实际的特征中不可能刚好和指纹库中特征相同，可使用确定性算法、概率性算法或knn(k-nearest neighbor)最近邻算法来进行匹配。
[0107]
具体来说，蓝牙指纹法knn判定算法，对于已测得的位置信息和与beacon之间的对应关系如下：
[0108]
p＝{p1[rssi
b0
,rssi
b1
,rssi
b2
],p2[rssi
b0
,rssi
b1
,rssi
b2
].....pn[rssi
b0
,rssi
b1
,rssi
b2
]}
[0109]
对于通过蓝牙设备实时测得的rssi指纹信息可使用knn算法来进行判断，根据给定的新的目标实例pt，和距离度量方法，可以在p中找到k个与pt最邻近的实例点，我们将这k个近邻点的集合记作nk取其中最小值最为knn判定的最有可能的位置：
[0110][0111]
最后，通过线性卡尔曼滤波融合pdr和蓝牙指纹法位置数据：
[0112]
p
measure
＝λp
pdr
(1-λ)p
rssi
[0113]
该发明的基于可穿戴marg传感器和蓝牙定位的混合人体轨迹追踪方法及系统，其利用实时传感器姿态数据计算获得该marg传感器在各个方向的位移数据；并利用蓝牙指纹法计算获得marg传感器的位置信息；进而利用卡尔曼滤波算法将marg传感器的位置信息与和各个方向的位移数据融合，获得精确定位数据，实现对于可穿戴marg传感器佩戴者的人体轨迹追踪。由于采用了融合滤波算法，因此，本发明的定位延迟和误差都得到了有效的降低，从而可以通过可穿戴设备获得高精度的人体轨迹、位置、朝向信息，方便地应用于博物馆、美术馆、大型游戏场馆等需要行人与环境互动的室内场景中。
[0114]
在此说明书中，本发明已参照其特定的实施例作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。