CN115164883A - 基于可穿戴marg传感器和蓝牙定位的混合追踪方法及*** - Google Patents
基于可穿戴marg传感器和蓝牙定位的混合追踪方法及*** Download PDFInfo
- Publication number
- CN115164883A CN115164883A CN202210844147.3A CN202210844147A CN115164883A CN 115164883 A CN115164883 A CN 115164883A CN 202210844147 A CN202210844147 A CN 202210844147A CN 115164883 A CN115164883 A CN 115164883A
- Authority
- CN
- China
- Prior art keywords
- sensor
- marg
- data
- bluetooth
- information
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000006073 displacement reaction Methods 0.000 claims abstract description 23
- 238000001914 filtration Methods 0.000 claims abstract description 22
- 238000005259 measurement Methods 0.000 claims description 37
- 230000001133 acceleration Effects 0.000 claims description 25
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000012937 correction Methods 0.000 claims description 9
- 230000005484 gravity Effects 0.000 claims description 7
- 230000004927 fusion Effects 0.000 abstract description 5
- 230000003993 interaction Effects 0.000 abstract description 4
- 230000005540 biological transmission Effects 0.000 description 5
- 230000033001 locomotion Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000004377 microelectronic Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 229920000642 polymer Polymers 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/80—Services using short range communication, e.g. near-field communication [NFC], radio-frequency identification [RFID] or low energy communication
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法及***,其利用实时传感器姿态数据计算获得该MARG传感器在各个方向的位移数据;并利用蓝牙指纹法计算获得MARG传感器的位置信息;进而利用卡尔曼滤波算法将MARG传感器的位置信息与和各个方向的位移数据融合,获得精确定位数据,实现对于可穿戴MARG传感器佩戴者的人体轨迹追踪。由于采用了融合滤波算法,因此,本发明的定位延迟和误差都得到了有效的降低,从而可以通过可穿戴设备获得高精度的人体轨迹、位置、朝向信息,方便地应用于博物馆、美术馆、大型游戏场馆等需要行人与环境互动的室内场景中。
Description
技术领域
本发明涉及传感器技术领域,特别涉及定位方法技术领域,具体是指一种基于可穿戴MARG传感器和蓝牙定位的混合追踪方法及***。
背景技术
现有技术中,基于卫星信号的GPS定位技术已被广泛应用。然而,在室内定位时,由于卫星信号弱经常存在定位不准的情况。
而PDR(Pedestrian Dead Reckoning)定位技术是一种更适用于室内的定位方法。PDR行人航迹推算也被称为人体轨迹推算,是一种基于自带IMU或MARG(Magnetic,AngularRate and Gravity)传感器进行室内定位的技术。其突出优势在于可抗干扰地提供实时、连续、精准的位置信息;经常被用于如室内运动员轨迹定位、消防员火场位置判断等领域。由于一般的MEMS(Microelectronic Mechanical System)传感器精度不高,通过将加速度两次积分得到行走距离,将陀螺仪积分得到航向变化值的方式无法获得准确的人体轨迹。因此1996年Levi,Robert W和Judd,Thomas提出了Dead reckoning航位推算算法,使得通过IMU传感器推算人体轨迹的精确度得到了大幅度的提升,通过近年来在PDR算法上的不断提升使得精度达到了1%左右。但是对于超过一定距离范围的人体轨迹如1000米则相对误差达到了10米所以在人体轨迹推算领域Dead reckoning航位推算算法依然无法被广泛的推广使用。
蓝牙RSSI中是Received Signal Strength Indication的简称,是接收的信号强度指示,无线发送层的可选部分,用来判定链接质量以及是否增大广播发送强度。通过接收到的信号强弱测定信号点与接收点的距离,进而根据相应数据进行定位计算的一种定位技术。蓝牙RSSI定位是指基于蓝牙协议,利用蓝牙RSSI信号衰减与距离之间的相关性关系来计算出蓝牙终端所在位置的一种定位方式。蓝牙设备在广播中,信号接收端离发射端越远,接收到的RSSI场强值就越弱;信号接收端离发射端越近,接收到的RSSI场强值就越强。接收端收到的无线信号dBm一般都是负值,信号值越大代表信号越强,RSSI信号范围在0~-100之间。预先已知三个或三个以上的蓝牙信标的位置,基于无线电衰减模型每个信标与被测信标测得距离,然后基于蓝牙信标定位模型得到被测信标的位置。但在实际应用中,RSSI受新号反射、散射、绕射等多重路径衰竭与遮挡的影响非常严重。目前业界使用Beacon定位的精度可以达到3~5米左右。
所以不论是基于惯性导航期间的行人航迹推算或者是基于蓝牙RSSI的室内定位方法,其精度都无法达到亚米级,难以满足精确定位人体轨迹、位置的目的。因此,如何对PDR定位技术进行改进,消除累积误差,提高定位精度,成为本领域亟待解决的问题。
发明内容
本发明的目的是克服了上述现有技术中的缺点,提供一种基于蓝牙RSSI的多点融合定位算法,从而消除为了消除传统PDR***中存在的累积误差,实现高精度的人体轨迹混合追踪方法及***。
为了实现上述的目的,本发明的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法,包括以下步骤:
(10)获取所述的MARG传感器的传感器数据,并根据所述的传感器数据计算初始三维姿态;
(20)根据所述的初始三维姿态进行姿态滤波获取实时传感器姿态数据;
(30)利用所述的实时传感器姿态数据计算获得在地理坐标系下的该MARG传感器在各个方向的位移数据;
(40)利用蓝牙指纹法计算获得所述的MARG传感器与各个蓝牙信标之间的距离,并计算出所述的MARG传感器的位置信息;
(50)利用卡尔曼滤波算法将所述的MARG传感器的位置信息与和各个方向的位移数据融合,获得精确定位数据。
该基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法中,所述的步骤(10)具体包括以下步骤:
(11)将MARG传感器佩戴在用户身上,所述的MARG传感器至少包括加速器计、陀螺仪和磁力计;
(12)所述的加速器计、陀螺仪和磁力计实时获得测量信息;
(13)嵌入式MCU或上位计算机与所述的加速器计、陀螺仪和磁力计进行通讯,获得所述的测量信息,并存储在存储器中;
(14)所述的嵌入式MCU或上位计算机利用所述的加速器计的测量信息,基于重力加速度计算获得所述的MARG传感器的俯仰角信息和翻滚角信息,利用所述的磁力计的测量信息获得航向角信息,并利用所述的陀螺仪的测量信息获取体坐标系姿态估计信息;所述的初始三维姿态包括所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息。
该基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法中,所述的步骤(20)具体为:
根据所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息,采用EKF滤波算法获得地理坐标系姿态估计数据,作为所述的实时传感器姿态数据。
该基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法中,所述的步骤(30)具体包括以下步骤:
(31)根据所述的实时传感器姿态数据该MARG传感器获得每一时刻的线性加速度;
(32)利用零速修正算法对所述的线性加速度的漂移进行修正,并计算获得在地理坐标系下,该MARG传感器在各个方向的位移数据。
该基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法中,所述的步骤(40)具体包括以下步骤:
(41)利用蓝牙指纹法计算获得所述的MARG传感器与至少三个蓝牙信标之间的距离;
(42)根据该MARG传感器与各个蓝牙信标之间的距离,利用KNN算法计算出所述的MARG传感器的位置信息。
本发明还提供一种基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***,其包括:可穿戴MARG传感器以及集成于该可穿戴MARG传感器的蓝牙设备,还包括集成于该可穿戴MARG传感器的嵌入式MCU或信号连接该可穿戴MARG传感器的上位计算机,其中,
所述的可穿戴MARG传感器用以实时获得传感器数据;
所述的嵌入式MCU或上位计算机用以获取所述的传感器数据,并根据所述的传感器数据计算初始三维姿态;根据所述的初始三维姿态进行姿态滤波获取实时传感器姿态数据;利用所述的实时传感器姿态数据计算获得在地理坐标系下的该MARG传感器在各个方向的位移数据;利用蓝牙指纹法计算获得所述的MARG传感器的蓝牙设备与各个蓝牙信标之间的距离,并计算出所述的MARG传感器的位置信息;并利用卡尔曼滤波算法将所述的MARG传感器的位置信息与和各个方向的位移数据融合,获得精确定位数据。
该基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***中,该可穿戴MARG传感器佩戴在用户身上,所述的MARG传感器至少包括加速器计、陀螺仪和磁力计;所述的加速器计、陀螺仪和磁力计用以实时获得测量信息;
所述的嵌入式MCU或上位计算机用以与所述的加速器计、陀螺仪和磁力计进行通讯,获得所述的测量信息,并存储在存储器中;并利用所述的加速器计的测量信息,基于重力加速度计算获得所述的MARG传感器的俯仰角信息和翻滚角信息,利用所述的磁力计的测量信息获得航向角信息,并利用所述的陀螺仪的测量信息获取体坐标系姿态估计信息;所述的初始三维姿态包括所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息。
该基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***中,所述的嵌入式MCU或上位计算机具体用以根据所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息,采用EKF滤波算法获得地理坐标系姿态估计数据,作为所述的实时传感器姿态数据。
该基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***中,所述的嵌入式MCU或上位计算机还用以根据所述的实时传感器姿态数据该MARG传感器获得每一时刻的线性加速度;并利用零速修正算法对所述的线性加速度的漂移进行修正,并计算获得在地理坐标系下,该MARG传感器在各个方向的位移数据。
该基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***中,所述的嵌入式MCU或上位计算机还用以利用蓝牙指纹法计算获得所述的MARG传感器与至少三个蓝牙信标之间的距离;根据该MARG传感器与各个蓝牙信标之间的距离,利用KNN算法计算出所述的MARG传感器的位置信息。
采用了该发明的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法及***,其利用实时传感器姿态数据计算获得该MARG传感器在各个方向的位移数据;并利用蓝牙指纹法计算获得MARG传感器的位置信息;进而利用卡尔曼滤波算法将MARG传感器的位置信息与和各个方向的位移数据融合,获得精确定位数据,实现对于可穿戴MARG传感器佩戴者的人体轨迹追踪。由于采用了融合滤波算法,因此,本发明的定位延迟和误差都得到了有效的降低,从而可以通过可穿戴设备获得高精度的人体轨迹、位置、朝向信息,方便地应用于博物馆、美术馆、大型游戏场馆等需要行人与环境互动的室内场景中。
附图说明
图1为本发明的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法的流程示意图。
图2为PDR航位推算原理示意图。
图3为蓝牙RSSI定位原理示意图。
图4为本发明的方法在实际应用中所采用的融合算法的框图。
具体实施方式
为了能够更清楚地理解本发明的技术内容,特举以下实施例详细说明。
请参阅图1所示,为本发明的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法的流程示意图。
在一种实施方式中,该基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法包括以下步骤:
(10)获取所述的MARG传感器的传感器数据,并根据所述的传感器数据计算初始三维姿态;
(20)根据所述的初始三维姿态进行姿态滤波获取实时传感器姿态数据;
(30)利用所述的实时传感器姿态数据计算获得在地理坐标系下的该MARG传感器在各个方向的位移数据;
(40)利用蓝牙指纹法计算获得所述的MARG传感器与各个蓝牙信标之间的距离,并计算出所述的MARG传感器的位置信息;
(50)利用卡尔曼滤波算法将所述的MARG传感器的位置信息与和各个方向的位移数据融合,获得精确定位数据。
在优选的实施方式中,所述的步骤(10)具体包括以下步骤:
(11)将MARG传感器佩戴在用户身上,所述的MARG传感器至少包括加速器计、陀螺仪和磁力计;
(12)所述的加速器计、陀螺仪和磁力计实时获得测量信息;
(13)嵌入式MCU或上位计算机与所述的加速器计、陀螺仪和磁力计进行通讯,获得所述的测量信息,并存储在存储器中;
(14)所述的嵌入式MCU或上位计算机利用所述的加速器计的测量信息,基于重力加速度计算获得所述的MARG传感器的俯仰角信息和翻滚角信息,利用所述的磁力计的测量信息获得航向角信息,并利用所述的陀螺仪的测量信息获取体坐标系姿态估计信息;所述的初始三维姿态包括所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息。
所述的步骤(20)具体为:
根据所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息,采用EKF滤波算法获得地理坐标系姿态估计数据,作为所述的实时传感器姿态数据。
所述的步骤(30)具体包括以下步骤:
(31)根据所述的实时传感器姿态数据该MARG传感器获得每一时刻的线性加速度;
(32)利用零速修正算法对所述的线性加速度的漂移进行修正,并计算获得在地理坐标系下,该MARG传感器在各个方向的位移数据。
在进一步优选的实施方式中,所述的步骤(40)具体包括以下步骤:
(41)利用蓝牙指纹法计算获得所述的MARG传感器与至少三个蓝牙信标之间的距离;
(42)根据该MARG传感器与各个蓝牙信标之间的距离,利用KNN算法计算出所述的MARG传感器的位置信息。
本发明还提供一种基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***。该***包括:可穿戴MARG传感器以及集成于该可穿戴MARG传感器的蓝牙设备,还包括集成于该可穿戴MARG传感器的嵌入式MCU或信号连接该可穿戴MARG传感器的上位计算机。该可穿戴MARG传感器佩戴在用户身上。其中,所述的可穿戴MARG传感器用以实时获得传感器数据。所述的嵌入式MCU或上位计算机用以获取所述的传感器数据,并根据所述的传感器数据计算初始三维姿态;根据所述的初始三维姿态进行姿态滤波获取实时传感器姿态数据;利用所述的实时传感器姿态数据计算获得在地理坐标系下的该MARG传感器在各个方向的位移数据;利用蓝牙指纹法计算获得所述的MARG传感器的蓝牙设备与各个蓝牙信标之间的距离,并计算出所述的MARG传感器的位置信息;并利用卡尔曼滤波算法将所述的MARG传感器的位置信息与和各个方向的位移数据融合,获得精确定位数据。
进一步的,该可穿戴MARG传感器至少包括加速器计、陀螺仪和磁力计;所述的加速器计、陀螺仪和磁力计用以实时获得测量信息;所述的嵌入式MCU或上位计算机用以与所述的加速器计、陀螺仪和磁力计进行通讯,获得所述的测量信息,并存储在存储器中;并利用所述的加速器计的测量信息,基于重力加速度计算获得所述的MARG传感器的俯仰角信息和翻滚角信息,利用所述的磁力计的测量信息获得航向角信息,并利用所述的陀螺仪的测量信息获取体坐标系姿态估计信息;所述的初始三维姿态包括所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息。
在实际应用中,本发明提出了一种基于MARG和蓝牙RSSI的混合室内定位算法,如图4所示,其融合了两种算法,实现了一种可达到亚米级精度的人体轨迹推算和目标定位方法,并同时提供了行人面向信息,从而满足了环境互动对精度的需求。
PDR航位推算的行人航迹推算算法,其原理如图2所示。能在无信标条件下,通常通过加速器计、陀螺仪来推算方向角,感知行人在行进过程中的加速度和方向角,并利用这些数据对行走路线进行相对定位,从而达到对行人进行定位跟踪的目的。
但是在实际使用中由于陀螺仪存在累计漂移因此方向角角的累计误差非常的大而不具有可用性。因此本方法使用了MARG(加速器计、陀螺仪、磁力计)传感器,磁力计的加入可以提供一个指向磁北极的恒定方向可以用来补偿陀螺仪的累计漂移。但是由于加速度计提供的加速度信息需要经过两次积分才能获取相对的运动距离信息,即使加速度计只有一个非常小的误差,经过两次积分后在较短的时间内误差依旧会积累到一个客观的地步。因此零速修正算法被用来修正加速度计的累计误差。
当行人处于静止状态时,认为行人此时的加速度都为零,则可以通过估算静止状态时加速器计、陀螺仪的漂移而获取整个周期内加速器计、陀螺仪的漂移值,进而以此对运动方向和运动速度进行修正。由于该修正不仅作用于加速度,同样也作用于速度,因此可以有效地消除由于加速度两次积分而带来的运动距离信息快速发散的问题。在实际使用中由于加速度计存在测量误差通常选取加速度三轴的读数的均方根小于某一个阈值来作为零速的判断依据,人体的运动模式的不同(慢走、快走、奔跑)及传感器的配带位置不同对阈值均有较大的影响,因此在零速修正算法中需考虑这两点。
具体来说,姿态估计部分采用EKF滤波算法。
首先定义四元数旋转矩阵:
由陀螺仪得到的角速度变化,可以定位为四元数对时间的积分:
同时定义G(q)
由此扩展卡尔曼滤波的传递函数如下:
噪声传统函数如下:
Q=(GyroNoise*dt)2*GGT得到预测四元数为:
q=Ftq
观测四元数:
首先对加速度和磁力计数据归一化:
得到预测值为:
pa=Normalize(-R(q)gn)
pm=Normalize(R(q)mn)
并构建出误差矩阵:
下一步更新卡尔曼增益:
S=HPHT+R
K=PHTS-1
q=q+K∈t
P=P-KHP
最终后验纠正姿态四元数:
位置估计算法:
由姿态估计部分得到每一时刻点的线性加速度:
ab=at+R(q)gn
通过两次积分可得到位置的变化:
蓝牙RSSI(Received Signal Strength Indication,接收信号强度指示)是无线传输层用来判定链接质量的重要指标。传输层根据RSSI判断是否需要增大发送端的发送强度。通常情况下,信号的能量以1mW为基准,以对数形式表示信号强度,即RSSI,单位为dBm(Decibel-milliwatts):分贝毫瓦。所以在无线信号中,1mW就是0dBm,能量小于1mW的信号RSSI为负数,能量大于1mW的信号RSSI为正数。RSSI受发送功率、路径衰减、接收增益和***处理增益四个元素影响。
由RSSI计算距离的公式如下:
d=10^((abs(RSSI)-A)/(10*n))
其中:d为计算所得距离;RSSI为接收信号强度(负值);A为发射端和接收端相隔1米时的信号强度;n为环境衰减因子。
如图3所示,实现蓝牙RSSI进行定位需要三个以上的蓝牙信标,由以上的RSSI计算距离公式获得设备与各个蓝牙信标的距离信息后,即可使用三角定位法获取设备的位置信息。但是由于信号的严重衰减和多径效应,使得三点定位算法得到的坐标精准度较差。蓝牙指纹法可以在不增加成本的前提下有效的提高定位精度。RSSI蓝牙指纹定位法分为两个阶段:离线阶段和在线阶段。离线阶段将每个位置的坐标记录以及保存特征(RSSI),最后收集完所有指纹得到指纹库。在线阶段则通过实际的特征查询指纹库获得定位。实际的特征中不可能刚好和指纹库中特征相同,可使用确定性算法、概率性算法或KNN(K-NearestNeighbor)最近邻算法来进行匹配。
具体来说,蓝牙指纹法KNN判定算法,对于已测得的位置信息和与beacon之间的对应关系如下:
P={p1[Rssib0,Rssib1,Rssib2],p2[Rssib0,Rssib1,Rssib2].....pn[Rssib0,Rssib1,Rssib2]}
对于通过蓝牙设备实时测得的Rssi指纹信息可使用KNN算法来进行判断,根据给定的新的目标实例Pt,和距离度量方法,可以在P中找到k个与Pt最邻近的实例点,我们将这k个近邻点的集合记作Nk取其中最小值最为KNN判定的最有可能的位置:
最后,通过线性卡尔曼滤波融合PDR和蓝牙指纹法位置数据:
pmeasure=λPpdr+(1-λ)Prssi
该发明的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法及***,其利用实时传感器姿态数据计算获得该MARG传感器在各个方向的位移数据;并利用蓝牙指纹法计算获得MARG传感器的位置信息;进而利用卡尔曼滤波算法将MARG传感器的位置信息与和各个方向的位移数据融合,获得精确定位数据,实现对于可穿戴MARG传感器佩戴者的人体轨迹追踪。由于采用了融合滤波算法,因此,本发明的定位延迟和误差都得到了有效的降低,从而可以通过可穿戴设备获得高精度的人体轨迹、位置、朝向信息,方便地应用于博物馆、美术馆、大型游戏场馆等需要行人与环境互动的室内场景中。
在此说明书中,本发明已参照其特定的实施例作了描述。但是,很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此,说明书和附图应被认为是说明性的而非限制性的。
Claims (10)
1.一种基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法,其特征在于,包括以下步骤:
(10)获取所述的MARG传感器的传感器数据,并根据所述的传感器数据计算初始三维姿态;
(20)根据所述的初始三维姿态进行姿态滤波获取实时传感器姿态数据;
(30)利用所述的实时传感器姿态数据计算获得在地理坐标系下的该MARG传感器在各个方向的位移数据;
(40)利用蓝牙指纹法计算获得所述的MARG传感器与各个蓝牙信标之间的距离,并计算出所述的MARG传感器的位置信息;
(50)利用卡尔曼滤波算法将所述的MARG传感器的位置信息与和各个方向的位移数据融合,获得精确定位数据。
2.根据权利要求1所述的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法,其特征在于,所述的步骤(10)具体包括以下步骤:
(11)将MARG传感器佩戴在用户身上,所述的MARG传感器至少包括加速器计、陀螺仪和磁力计;
(12)所述的加速器计、陀螺仪和磁力计实时获得测量信息;
(13)嵌入式MCU或上位计算机与所述的加速器计、陀螺仪和磁力计进行通讯,获得所述的测量信息,并存储在存储器中;
(14)所述的嵌入式MCU或上位计算机利用所述的加速器计的测量信息,基于重力加速度计算获得所述的MARG传感器的俯仰角信息和翻滚角信息,利用所述的磁力计的测量信息获得航向角信息,并利用所述的陀螺仪的测量信息获取体坐标系姿态估计信息;所述的初始三维姿态包括所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息。
3.根据权利要求2所述的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法,其特征在于,所述的步骤(20)具体为:
根据所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息,采用EKF滤波算法获得地理坐标系姿态估计数据,作为所述的实时传感器姿态数据。
4.根据权利要求3所述的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法,其特征在于,所述的步骤(30)具体包括以下步骤:
(31)根据所述的实时传感器姿态数据该MARG传感器获得每一时刻的线性加速度;
(32)利用零速修正算法对所述的线性加速度的漂移进行修正,并计算获得在地理坐标系下,该MARG传感器在各个方向的位移数据。
5.根据权利要求1所述的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪方法,其特征在于,所述的步骤(40)具体包括以下步骤:
(41)利用蓝牙指纹法计算获得所述的MARG传感器与至少三个蓝牙信标之间的距离;
(42)根据该MARG传感器与各个蓝牙信标之间的距离,利用KNN算法计算出所述的MARG传感器的位置信息。
6.一种基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***,其特征在于,该***包括:可穿戴MARG传感器以及集成于该可穿戴MARG传感器的蓝牙设备,还包括集成于该可穿戴MARG传感器的嵌入式MCU或信号连接该可穿戴MARG传感器的上位计算机,其中,
所述的可穿戴MARG传感器用以实时获得传感器数据;
所述的嵌入式MCU或上位计算机用以获取所述的传感器数据,并根据所述的传感器数据计算初始三维姿态;根据所述的初始三维姿态进行姿态滤波获取实时传感器姿态数据;利用所述的实时传感器姿态数据计算获得在地理坐标系下的该MARG传感器在各个方向的位移数据;利用蓝牙指纹法计算获得所述的MARG传感器的蓝牙设备与各个蓝牙信标之间的距离,并计算出所述的MARG传感器的位置信息;并利用卡尔曼滤波算法将所述的MARG传感器的位置信息与和各个方向的位移数据融合,获得精确定位数据。
7.根据权利要求6所述的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***,其特征在于,该可穿戴MARG传感器佩戴在用户身上,所述的MARG传感器至少包括加速器计、陀螺仪和磁力计;所述的加速器计、陀螺仪和磁力计用以实时获得测量信息;
所述的嵌入式MCU或上位计算机用以与所述的加速器计、陀螺仪和磁力计进行通讯,获得所述的测量信息,并存储在存储器中;并利用所述的加速器计的测量信息,基于重力加速度计算获得所述的MARG传感器的俯仰角信息和翻滚角信息,利用所述的磁力计的测量信息获得航向角信息,并利用所述的陀螺仪的测量信息获取体坐标系姿态估计信息;所述的初始三维姿态包括所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息。
8.根据权利要求7所述的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***,其特征在于,所述的嵌入式MCU或上位计算机具体用以根据所述的俯仰角信息、翻滚角信息、航向角信息以及体坐标系姿态估计信息,采用EKF滤波算法获得地理坐标系姿态估计数据,作为所述的实时传感器姿态数据。
9.根据权利要求8所述的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***,其特征在于,所述的嵌入式MCU或上位计算机还用以根据所述的实时传感器姿态数据该MARG传感器获得每一时刻的线性加速度;并利用零速修正算法对所述的线性加速度的漂移进行修正,并计算获得在地理坐标系下,该MARG传感器在各个方向的位移数据。
10.根据权利要求6所述的基于可穿戴MARG传感器和蓝牙定位的混合人体轨迹追踪***,其特征在于,所述的嵌入式MCU或上位计算机还用以利用蓝牙指纹法计算获得所述的MARG传感器与至少三个蓝牙信标之间的距离;根据该MARG传感器与各个蓝牙信标之间的距离,利用KNN算法计算出所述的MARG传感器的位置信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210844147.3A CN115164883A (zh) | 2022-07-18 | 2022-07-18 | 基于可穿戴marg传感器和蓝牙定位的混合追踪方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210844147.3A CN115164883A (zh) | 2022-07-18 | 2022-07-18 | 基于可穿戴marg传感器和蓝牙定位的混合追踪方法及*** |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115164883A true CN115164883A (zh) | 2022-10-11 |
Family
ID=83494394
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210844147.3A Pending CN115164883A (zh) | 2022-07-18 | 2022-07-18 | 基于可穿戴marg传感器和蓝牙定位的混合追踪方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115164883A (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105588566A (zh) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | 一种基于蓝牙与mems融合的室内定位***及方法 |
CN109298389A (zh) * | 2018-08-29 | 2019-02-01 | 东南大学 | 基于多粒子群优化的室内行人组合位姿估计方法 |
US10598507B1 (en) * | 2018-11-28 | 2020-03-24 | Carl LaMont | Systems, methods, and apparatus for locating objects |
CN113892942A (zh) * | 2021-08-24 | 2022-01-07 | 重庆大学 | 一种实时追踪人体下肢运动的穿戴设备 |
CN114323004A (zh) * | 2021-12-28 | 2022-04-12 | 北京骑胜科技有限公司 | 一种定位方法和*** |
-
2022
- 2022-07-18 CN CN202210844147.3A patent/CN115164883A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105588566A (zh) * | 2016-01-08 | 2016-05-18 | 重庆邮电大学 | 一种基于蓝牙与mems融合的室内定位***及方法 |
CN109298389A (zh) * | 2018-08-29 | 2019-02-01 | 东南大学 | 基于多粒子群优化的室内行人组合位姿估计方法 |
US10598507B1 (en) * | 2018-11-28 | 2020-03-24 | Carl LaMont | Systems, methods, and apparatus for locating objects |
CN113892942A (zh) * | 2021-08-24 | 2022-01-07 | 重庆大学 | 一种实时追踪人体下肢运动的穿戴设备 |
CN114323004A (zh) * | 2021-12-28 | 2022-04-12 | 北京骑胜科技有限公司 | 一种定位方法和*** |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110645979B (zh) | 基于gnss/ins/uwb组合的室内外无缝定位方法 | |
US10267646B2 (en) | Method and system for varying step length estimation using nonlinear system identification | |
Sun et al. | Indoor positioning tightly coupled Wi-Fi FTM ranging and PDR based on the extended Kalman filter for smartphones | |
US20070282565A1 (en) | Object locating in restricted environments using personal navigation | |
JP6054535B2 (ja) | 歩行者モーション認識基盤の歩行者位置推定装置、及びその方法 | |
TW200916730A (en) | Augmented navigation system and method of a moving object | |
You et al. | Hybrid indoor positioning system for pedestrians with swinging arms based on smartphone IMU and RSSI of BLE | |
US20200103477A1 (en) | Method and system for magnetic fingerprinting | |
US10999709B2 (en) | Method and system for determining a direction of movement of an object | |
KR100558367B1 (ko) | Gps및 ins를 이용한 수치지도 제작 시스템 및 방법 | |
TW201120414A (en) | Position determination apparatus and system and position determination method thereof | |
CN110057356B (zh) | 一种隧道内车辆定位方法及装置 | |
CN112004183B (zh) | 一种基于卷积神经网络融合IMU和WiFi信息的机器人自主定位方法 | |
CN112729301A (zh) | 一种基于多源数据融合的室内定位方法 | |
Luo et al. | Indoor multi-floor 3D target tracking based on the multi-sensor fusion | |
CN111025366A (zh) | 基于ins及gnss的网格slam的导航***及方法 | |
CN112362057A (zh) | 基于零速修正与姿态自观测的惯性行人导航算法 | |
CN112362044A (zh) | 室内定位方法、装置、设备和*** | |
CN113325455B (zh) | 用于跟踪和确定物体的室内位置的方法和*** | |
Chen et al. | Pedestrian positioning with physical activity classification for indoors | |
CN111197974A (zh) | 一种基于Android惯性平台的无气压计高度测算方法 | |
CN115164883A (zh) | 基于可穿戴marg传感器和蓝牙定位的混合追踪方法及*** | |
US20210306811A1 (en) | Method and system for determining a direction of movement of an object | |
CN115406435A (zh) | 基于wlan和mems的室内电子地图构建方法、装置及存储介质 | |
Zhao et al. | An economical and effective multi-sensor integration for portable navigation system |
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 |