CN115103437A - 一种蓝牙和惯性测量单元的紧耦合室内定位方法 - Google Patents

一种蓝牙和惯性测量单元的紧耦合室内定位方法 Download PDF

Info

Publication number
CN115103437A
CN115103437A CN202210453099.5A CN202210453099A CN115103437A CN 115103437 A CN115103437 A CN 115103437A CN 202210453099 A CN202210453099 A CN 202210453099A CN 115103437 A CN115103437 A CN 115103437A
Authority
CN
China
Prior art keywords
ble
matrix
bluetooth
imu
state quantity
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
Application number
CN202210453099.5A
Other languages
English (en)
Other versions
CN115103437B (zh
Inventor
武畅
刘禹宏
袁翼飞
孔孝童
游远
夏堃
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202210453099.5A priority Critical patent/CN115103437B/zh
Publication of CN115103437A publication Critical patent/CN115103437A/zh
Application granted granted Critical
Publication of CN115103437B publication Critical patent/CN115103437B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • H04W64/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • G01S5/0263—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems
    • G01S5/0264—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems at least one of the systems being a non-radio wave positioning system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明提供了一种蓝牙和惯性测量单元的紧耦合室内定位方法，包括以下步骤：在定位区域布置蓝牙信标(BLE)，采集其位置信息，将其存储至数据库D；采集接收信号的强度指示(Received Signal Strength Indicator，RSSI)信号进行处理，根据数据库D，利用RSSI信号传播模型获取BLE到车辆的距离信息；采集惯性测量单元(Inertial Measurement Unit，IMU)提供的测量值，解算设备坐标系到世界坐标系的旋转矩阵；利用本时刻的状态量转移方程与旋转矩阵，分别得到方程关于状态量和噪声的雅可比矩阵；利用两个雅可比矩阵求出估计方差矩阵与下一时刻的状态量预测值；利用蓝牙采样的状态量，根据观测方程求出关于状态量的雅可比矩阵；根据解算的雅可比矩阵，利用扩展卡尔曼滤波器紧耦合蓝牙与IMU并修正得到的状态量与估计方差矩阵，完成蓝牙与IMU的紧耦合室内定位。本发明在定位精度上比传统的蓝牙定位算法有所提升。

Description

一种蓝牙和惯性测量单元的紧耦合室内定位方法
技术领域
本发明属于室内定位技术领域，尤其涉及一种蓝牙和惯性测量单元的紧耦合室内定位方法。
背景技术
目前传统的车辆室内定位方案中，基于蓝牙和IMU的室内定位技术具有实现成本低，覆盖范围广的优点，因此受到了广泛应用。但基于蓝牙的定位方案存在前期需要构建指纹数据库，这种方法工作量巨大，要耗费大量人力物力；且复杂的室内建筑对RSSI信号的传播也会造成影响，加入如中值滤波器等滤波器虽然会一定程度上稳定RSSI信号，但随之带来的时延影响对实时定位是无法接收的。因此，本发明提出一种蓝牙和惯性测量单元的紧耦合室内定位方法，在定位精度上比传统的蓝牙定位算法有所提升。
发明内容
针对现有技术中的上述不足，本发明提供的一种蓝牙和IMU的紧耦合室内定位方法，在定位精度上比传统的蓝牙定位算法有所提升。
为了达到以上目的，本发明采用的技术方案为：
本方案提供一种蓝牙和IMU的紧耦合室内定位方法，其特征在于，包括以下步骤：
S1、在定位区域布置BLE，采集其位置信息，将其存储至数据库D；
S2、采集RSSI信号进行处理，根据数据库D，利用RSSI信号传播模型获取BLE到车辆的距离信息；
S3、采集IMU提供的加速度、磁偏角与角速度，解算设备坐标系到世界坐标系的旋转矩阵；
S4、利用本时刻的状态量转移方程与旋转矩阵，分别得到方程关于状态量和噪声的雅可比矩阵，其中状态量包括位置、速度、加速度计偏置和重力加速度；
S5、利用两个雅可比矩阵求出估计方差矩阵与下一时刻的状态量预测值；
S6、利用蓝牙采样时刻的状态量，根据根据观测方程求出关于状态量的雅可比矩阵；
S7、根据解算出的雅可比矩阵，利用扩展卡尔曼滤波器紧耦合蓝牙与IMU并修正得到的状态量与估计方差矩阵，完成蓝牙与IMU的紧耦合室内定位；
本发明的有益效果是：本发明提出一种蓝牙和IMU的紧耦合室内定位方法，对融合定位方式的不同带来的影响进行了全面的分析与测试。采用中值滤波器等方法稳定了RSSI信号，同时引入IMU的测量值减小了时延，并将测距信息进行坐标系的转换，减轻了用户使用该方案时手机随意摆放造成的不利因素的影响。通过蓝牙测距量修正了IMU预测的位置、速度与加速度计偏置，从而提高了定位算法的精度。
进一步地，所述步骤S1包括以下步骤：
S101、在定位区域选择一个BLE设备的位置，以等边三角形的形式向周边扩散，并以逆时针方向放置其他相邻BLE设备，构成正六边形即一个BLE设备序列；
S102、将BEL设备的位置和相邻BLE设备的相关定位信息存储至数据库D。
3、根据权利要求2所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S102中数据库D的形式如下：
Figure BDA0003617679590000021
Figure BDA0003617679590000031
其中，Pi是定位字段的名称，用于提高数据库的可读性；αi是磁偏角；UUIDi是BLE的标识；n表示区域中BLE的数量；
Figure BDA0003617679590000032
是相邻BLE信息的集合，其中上标i和下标j标识第i个定位场和该场的第j个iBeacon，
Figure BDA0003617679590000033
形式如下。距离BLE1米处测得的平均RSSI，这对于测距是必不可少的。
Figure BDA0003617679590000034
其中，
Figure BDA0003617679590000035
和
Figure BDA0003617679590000036
是BLE的标识；
Figure BDA0003617679590000037
是距离BLE1米处测得的平均RSSI；
Figure BDA0003617679590000038
是BLE的坐标。
上述进一步方案的有益效果是：这种储存结构不仅可以提高搜索数据的效率，还可以提高系统的可扩展性。
再进一步地，所述步骤S2包括以下步骤：
S201、将移动端收到的BLE的RSSI数据进行滑动平均滤波处理；
S202、将移动端收到的BLE根据RSSI排序，选取出RSSI最大的BLE；
S203、利用RSSI测距公式计算移动端与BLE的距离为dBLE，BLE坐标为(xBLE,yBLE,zBLE)；
上述进一步方案的有益效果是：本发明放弃传统的三边测量定位算法直接得到位置信息，从而避免车辆位置信息的误差过大。只得到距离信息可以在提高数据精度，同时只需1个BLE就可得到相应信息，从而减少了BLE个数。
再进一步地,所述步骤S201中滑动平均滤波处理公式为:
Figure BDA0003617679590000039
其中，RSSIm[n]是滑动平均滤波后的结果，RSSI保存第k时刻所接收的RSSI信号强度，L为滑动窗口的长度。
上述进一步方案的有益效果是：本发明与传统的RSSI处理方式不同，这种RSSI预处理方法将大大降低基于BLE的定位算法的方差，使得结果更加精准，本发明更加科学。
再进一步地，所述步骤S203中RSSI测距公式的表达式如下：
Figure BDA0003617679590000041
Figure BDA0003617679590000042
其中，PL(d0)为参考距离处的功率，实际为了计算方便取1m，RSSInorm为将RSSIm归一化后的结果，RSSInormMin，RSSInormmMax分别为RSSInorm中的最小值与最大值，而a0,a1,a2,a3,a4,是人为选择的多项式系数，dmax与dmin均是根据实际场地设置的最大最小距离。
上述进一步方案的有益效果是：本发明与传统的RSSI测距模型不同，传统方法在计算时并没有考虑室内传播场通常会因障碍物的衰落和表面的折射而变得更加复杂。我们所采用的模型可以很好地根据现场采集的RSSI信号值适应不同的领域，从而一定程度上规避了复杂室内传播场的干扰，本发明更加科学。
再进一步地，所述步骤S3包括以下步骤：
S301、利用手机传感器获取的加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵R；
S302、利用手机传感器获取的陀螺仪数值修正旋转矩阵。
S303、利用状态量转移公式推算下一时刻的状态量，包括位置、速度、加速度计偏置和重力加速度信息；
上述进一步方案的有益效果是：考虑到蓝牙观测量的较大误差，放弃了传统基于EKF的蓝牙与IMU融合方案中将位姿也考虑进状态量共同优化的想法，转而利用9轴IMU解算位姿的方法。该方法在本发明中可以提供更准确的位姿估计。本发明更加科学。
再进一步地,所述步骤S301中加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵公式为:
Figure BDA0003617679590000051
Figure BDA0003617679590000052
Figure BDA0003617679590000053
Figure BDA0003617679590000054
Figure BDA0003617679590000055
其中，ax，ay，az为手机IMU测得的x，y，z轴的加速度，my与mx为测得的x，y轴下的磁感应强度。θ，
Figure BDA0003617679590000056
ψ为滚转角、俯仰角与偏转角，R为设备坐标系到世界坐标系的旋转矩阵，
Figure BDA0003617679590000057
为欧拉角与旋转矩阵之间的转换函数。
上述进一步方案的有益效果是：可以快速的得出手机设备坐标系到世界坐标系的欧拉角。本发明更加科学。
再进一步地,所述步骤S302中结合S301推算的R，利用手机传感器获取的陀螺仪数值修正旋转矩阵公式为：
Figure BDA0003617679590000058
Figure BDA0003617679590000059
Figure BDA00036176795900000510
Figure BDA00036176795900000511
Figure BDA0003617679590000061
Figure BDA0003617679590000062
其中，
Figure BDA0003617679590000063
为n-1时刻的旋转矩阵估计值，
Figure BDA0003617679590000064
为在n-1时刻预测值的基础上推算出n时刻的旋转矩阵估计值，
Figure BDA0003617679590000065
为n时刻的旋转矩阵估计值，R为根据S301推算出来的旋转矩阵，ωx，ωy，ωz为手机IMU陀螺仪的测量值，Δt表示相邻两次IMU测量的时间间隔，Qnoise和Rnoise分别表示该系统噪声方差与测量噪声方差，KR表示卡尔曼增益，HR表示观测矩阵，I表示单位矩阵，
Figure BDA0003617679590000066
与
Figure BDA0003617679590000067
分别表示n时刻与在n-1时刻预测值的基础上推算出n时刻的估计方差。
上述进一步方案的有益效果是：引入陀螺仪测量值，使得欧拉角的估计更加准确有效。本发明更加科学。
再进一步地，所述步骤S4包括以下步骤：
S401、根据相邻状态量之间的关系，得到状态转移公式；
S402、根据估计值的状态转移方程计算出关于状态量与噪声量的雅可比矩阵；
再进一步地，所述步骤S401中状态量转移公式为：
Figure BDA0003617679590000068
其中，p表示定位坐标，v表示运动速度，ba表示建模为具有高斯噪声
Figure BDA0003617679590000069
的随机游走过程的IMU偏置，g表示重力加速度，na表示IMU加速度计测量的白噪声，下标i表示第i时刻的预测值，Δt表示相邻两次IMU测量的时间间隔，am为IMU提供的加速度测量值。R为IMU设备坐标系到世界坐标系变换的旋转矩阵，通过S303中的9轴IMU位姿解算的方法得到。
上述进一步方案的有益效果是：在一段微小时间内采用匀变速运动模型对运动建模，贴近实际情况，可以得到平滑的状态转移结果。
再进一步地，所述步骤S402中状态转移方程关于状态量与噪声量的雅可比矩阵为：
Figure BDA0003617679590000071
Figure BDA0003617679590000072
Figure BDA0003617679590000073
Figure BDA0003617679590000074
其中，x，u，n分别为状态量、测量量与噪声量，三者包含的符号的含义在S402中介绍过，
Figure BDA0003617679590000075
与
Figure BDA0003617679590000076
分别表示状态量在第i与i+1时刻的估计值，
Figure BDA0003617679590000077
为状态量转移方程公式，
Figure BDA0003617679590000078
为转移方程，Fn为噪声矩阵，I为单位矩阵，Δt为相邻两次IMU测量的时间间隔，
Figure BDA0003617679590000079
为第i时刻旋转矩阵的估计值。
再进一步地，所述步骤S5中估计方差矩阵与下一时刻状态量预测值计算公式为：
Figure BDA00036176795900000710
Figure BDA00036176795900000711
Pi、Pi+1|i分别为第i时刻与第i时刻前提下的i+1时刻的估计方差矩阵，Q为该系统噪声方差。
再进一步地，所述步骤S6中根据观测方程求出关于状态量的雅可比矩阵为：
Figure BDA0003617679590000081
Figure BDA0003617679590000082
(将
Figure BDA0003617679590000083
记为A)
Figure BDA0003617679590000084
Figure BDA0003617679590000085
Figure BDA0003617679590000086
Figure BDA0003617679590000087
H＝[H1 H2 … Hm]T,1≤j≤m
其中，hj(xk+1)为IMU解算位置到第j个提供观测距离的蓝牙信标的距离，xk+1是第k+1时刻的状态量，第k+1时刻对应的是第k+1次蓝牙观测的时刻，pk+1为第k+1时刻S4中提供的位置估计坐标形式为(px(k+1),py(k+1),pz(k+1))，
Figure BDA0003617679590000088
为第j个提供观测距离的蓝牙信标位置，坐标形式为
Figure BDA0003617679590000089
Hj是函数hj关于状态量xk+1的雅可比矩阵，δt为相邻两次蓝牙观测的时间间隔，Rk为第k时刻根据IMU信息解算出的旋转矩阵，m为k+1时刻提供观测距离的蓝牙信标的总个数，H为观测矩阵。
上述进一步方案的有益效果是：区别于传统的基于EKF的蓝牙与IMU融合方案，将IMU偏置也考虑进状态量共同修正，以此在后续定位中达到较高的精度。本发明更加科学。
再进一步地，所述步骤S7更新的状态量与估计方差矩阵为：
K＝Pk+1|kHT(HPk+1|kHT+V)-1
Figure BDA0003617679590000091
Pk+1＝(I-KH)Pk+1|k
P为第k时刻的估计方差矩阵，Z为m个蓝牙观测距离的集合。
Figure BDA0003617679590000092
分别为第k时刻与k+1时刻的状态估计量，I为单位矩阵。
上述进一步方案的有益效果是：卡尔曼滤波以预测值、实测值、修正值的顺序递推，极大地跟随真值变动，是贴近真值的适应动态环境的跟随算法。同时紧耦合的系统结构可以有效地提升系统的精度。本发明利用卡尔曼滤波对定位结果进行优化，可以得到平滑的输出结果。本发明更加科学。
附图说明
图1为本发明的方法流程图。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
实施例
本发明提出一种蓝牙和IMU的紧耦合室内定位方法。本发明提出一种特定策略来利用扩展卡尔曼滤波器融合蓝牙与智能手机IMU提供的定位信息。通过蓝牙的观测量来修正智能手机IMU预测的位置、速度、加速度计的偏置与重力加速度。本发明通过低成本的融合定位算法将定位精度降到4米以下。如图1，本发明提供了一种蓝牙和IMU的紧耦合室内定位方法，其实现方法包括如下步骤：
S1、在定位区域布置BLE，采集其位置信息，将其存储至数据库D；
S2、采集RSSI信号进行处理，根据数据库D，利用RSSI信号传播模型获取BLE到车辆的距离信息；
S3、采集IMU提供的加速度、磁偏角与角速度，解算设备坐标系到世界坐标系的旋转矩阵；
S4、利用本时刻的状态量转移方程与旋转矩阵，分别得到方程关于状态量和噪声的雅可比矩阵，其中状态量包括位置、速度、加速度计偏置和重力加速度；
S5、利用两个雅可比矩阵求出估计方差矩阵与下一时刻的状态量预测值；
S6、利用蓝牙采样时刻的状态量，根据根据观测方程求出关于状态量的雅可比矩阵；
S7、根据解算出的雅可比矩阵，利用扩展卡尔曼滤波器紧耦合蓝牙与IMU并修正得到的状态量与估计方差矩阵，完成蓝牙与IMU的紧耦合室内定位；
本发明的有益效果是：本发明提出一种蓝牙和IMU的紧耦合室内定位方法，对融合定位方式的不同带来的影响进行了全面的分析与测试。采用中值滤波器等方法稳定了RSSI信号，同时引入IMU的测量值减小了时延，并将测距信息进行坐标系的转换，减轻了用户使用该方案时手机随意摆放造成的不利因素的影响。通过蓝牙测距量修正了IMU预测的位置、速度与加速度计偏置，从而提高了定位算法的精度。
进一步地，所述步骤S1包括以下步骤：
S101、在定位区域选择一个BLE设备的位置，以等边三角形的形式向周边扩散，并以逆时针方向放置其他相邻BLE设备，构成正六边形即一个BLE设备序列；
S102、将BEL设备的位置和相邻BLE设备的相关定位信息存储至数据库D。
3、根据权利要求2所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S102中数据库D的形式如下：
Figure BDA0003617679590000111
其中，Pi是定位字段的名称，用于提高数据库的可读性；αi是磁偏角；UUIDi是BLE的标识；n表示区域中BLE的数量；
Figure BDA0003617679590000112
是相邻BLE信息的集合，其中上标i和下标j标识第i个定位场和该场的第j个iBeacon，
Figure BDA0003617679590000113
形式如下。距离BLE1米处测得的平均RSSI，这对于测距是必不可少的。
Figure BDA0003617679590000114
其中，
Figure BDA0003617679590000115
和
Figure BDA0003617679590000116
是BLE的标识；
Figure BDA0003617679590000117
是距离BLE1米处测得的平均RSSI；
Figure BDA0003617679590000118
是BLE的坐标。
再进一步地，所述步骤S2包括以下步骤：
S201、将移动端收到的BLE的RSSI数据进行滑动平均滤波处理；
S202、将移动端收到的BLE根据RSSI排序，选取出RSSI最大的BLE；
S203、利用RSSI测距公式计算移动端与BLE的距离为dBLE，BLE坐标为(xBLE,yBLE,zBLE)；
再进一步地,所述步骤S201中滑动平均滤波处理公式为:
Figure BDA0003617679590000119
其中，RSSIm[n]是滑动平均滤波后的结果，RSSI保存第k时刻所接收的RSSI信号强度，L为滑动窗口的长度。
再进一步地，所述步骤S203中RSSI测距公式的表达式如下：
Figure BDA0003617679590000121
Figure BDA0003617679590000122
其中，PL(d0)为参考距离处的功率，实际为了计算方便取1m，RSSInorm为将RSSIm归一化后的结果，RSSInormMin，RSSInormmMax分别为RSSInorm中的最小值与最大值，而a0,a1,a2,a3,a4,是人为选择的多项式系数，dmax与dmin均是根据实际场地设置的最大最小距离。
再进一步地，所述步骤S3包括以下步骤：
S301、利用手机传感器获取的加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵R；
S302、利用手机传感器获取的陀螺仪数值修正旋转矩阵。
S303、利用状态量转移公式推算下一时刻的状态量，包括位置、速度、加速度计偏置和重力加速度信息；
再进一步地,所述步骤S301中加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵公式为:
Figure BDA0003617679590000123
Figure BDA0003617679590000124
Figure BDA0003617679590000125
Figure BDA0003617679590000126
Figure BDA0003617679590000127
其中，ax，ay，az为手机IMU测得的x，y，z轴的加速度，my与mx为测得的x，y轴下的磁感应强度。θ，
Figure BDA0003617679590000131
ψ为滚转角、俯仰角与偏转角，R为设备坐标系到世界坐标系的旋转矩阵，
Figure BDA0003617679590000132
为欧拉角与旋转矩阵之间的转换函数。
再进一步地,所述步骤S302中结合S301推算的R，利用手机传感器获取的陀螺仪数值修正旋转矩阵公式为：
Figure BDA0003617679590000133
Figure BDA0003617679590000134
Figure BDA0003617679590000135
Figure BDA0003617679590000136
Figure BDA0003617679590000137
Figure BDA0003617679590000138
其中，
Figure BDA0003617679590000139
为n-1时刻的旋转矩阵估计值，
Figure BDA00036176795900001310
为在n-1时刻预测值的基础上推算出n时刻的旋转矩阵估计值，
Figure BDA00036176795900001311
为n时刻的旋转矩阵估计值，R为根据S301推算出来的旋转矩阵，ωx，ωy，ωz为手机IMU陀螺仪的测量值，Δt表示相邻两次IMU测量的时间间隔，Qnoise和Rnoise分别表示该系统噪声方差与测量噪声方差，KR表示卡尔曼增益，HR表示观测矩阵，I表示单位矩阵，
Figure BDA00036176795900001312
与
Figure BDA00036176795900001313
分别表示n时刻与在n-1时刻预测值的基础上推算出n时刻的估计方差。
再进一步地，所述步骤S4包括以下步骤：
S401、根据相邻状态量之间的关系，得到状态转移公式；
S402、根据估计值的状态转移方程计算出关于状态量与噪声量的雅可比矩阵；
再进一步地，所述步骤S401中状态量转移公式为：
Figure BDA0003617679590000141
其中，p表示定位坐标，v表示运动速度，ba表示建模为具有高斯噪声
Figure BDA0003617679590000142
的随机游走过程的IMU偏置，g表示重力加速度，na表示IMU加速度计测量的白噪声，下标i表示第i时刻的预测值，Δt表示相邻两次IMU测量的时间间隔，am为IMU提供的加速度测量值。R为IMU设备坐标系到世界坐标系变换的旋转矩阵，通过S303中的9轴IMU位姿解算的方法得到。
再进一步地，所述步骤S402中状态转移方程关于状态量与噪声量的雅可比矩阵为：
Figure BDA0003617679590000143
Figure BDA0003617679590000144
Figure BDA0003617679590000145
Figure BDA0003617679590000146
其中，x，u，n分别为状态量、测量量与噪声量，三者包含的符号的含义在S402中介绍过，
Figure BDA0003617679590000151
与
Figure BDA0003617679590000152
分别表示状态量在第i与i+1时刻的估计值，
Figure BDA0003617679590000153
为状态量转移方程公式，
Figure BDA0003617679590000154
为转移方程，Fn为噪声矩阵，I为单位矩阵，Δt为相邻两次IMU测量的时间间隔，
Figure BDA0003617679590000155
为第i时刻旋转矩阵的估计值。
再进一步地，所述步骤S5中估计方差矩阵与下一时刻状态量预测值计算公式为：
Figure BDA0003617679590000156
Figure BDA0003617679590000157
Pi、Pi+1|i分别为第i时刻与第i时刻前提下的i+1时刻的估计方差矩阵，Q为该系统噪声方差。
再进一步地，所述步骤S6中根据观测方程求出关于状态量的雅可比矩阵为：
Figure BDA0003617679590000158
Figure BDA0003617679590000159
(将
Figure BDA00036176795900001510
记为A)
Figure BDA00036176795900001511
Figure BDA00036176795900001512
Figure BDA00036176795900001513
Figure BDA00036176795900001514
H＝[H1 H2 … Hm]T,1≤j≤m
其中，hj(xk+1)为IMU解算位置到第j个提供观测距离的蓝牙信标的距离，xk+1是第k+1时刻的状态量，第k+1时刻对应的是第k+1次蓝牙观测的时刻，pk+1为第k+1时刻S4中提供的位置估计坐标形式为(px(k+1),py(k+1),pz(k+1))，
Figure BDA0003617679590000161
为第j个提供观测距离的蓝牙信标位置，坐标形式为
Figure BDA0003617679590000162
Hj是函数hj关于状态量xk+1的雅可比矩阵，δt为相邻两次蓝牙观测的时间间隔，Rk为第k时刻根据IMU信息解算出的旋转矩阵，m为k+1时刻提供观测距离的蓝牙信标的总个数，H为观测矩阵。
再进一步地，所述步骤S7更新的状态量与估计方差矩阵为：
K＝Pk+1|kHT(HPk+1|kHT+V)-1
Figure BDA0003617679590000163
Pk+1＝(I-KH)Pk+1|k
P为第k时刻的估计方差矩阵，Z为m个蓝牙观测距离的集合。
Figure BDA0003617679590000164
分别为第k时刻与k+1时刻的状态估计量，I为单位矩阵。
本实施例中，根据上述提出的紧耦合蓝牙与智能手机IMU的室内车辆定位方法，进行仿真实验，来检验算法的定位准确率和成本损耗。
实验场景是一个巨大的地下停车场，整个定位区域面积约为160m×130m；BLE广播器的数量约为200个；定位面积约10000平方米。BLE设备的放置策略采用水晶形BLE放置(CIP)，但定位场与自由空间不同，BLE不能附着在空间中的某些位置，因此与CIP有一些区别。
本实施例中，我们选取一条长约400米的路线进行实际测量。利用
Figure BDA0003617679590000165
Figure BDA0003617679590000166
计算定位算法的定位误差。其中er为计算出的误差，
Figure BDA0003617679590000167
与
Figure BDA0003617679590000168
为估计的坐标值，Xreal与Yreal为真实的坐标值。实验得出的结论显示紧耦合蓝牙与IMU的室内车辆定位定位误差小于4米的概率为83.4％；而只依靠蓝牙进行的三角定位在定位效果要比该方法差。而实际场景的测试让该方法具有被大规模商用的可能性。

Claims (15)

1.一种蓝牙和惯性测量单元的紧耦合室内定位方法，其特征在于，包括以下步骤：
S1、在定位区域布置BLE，采集其位置信息，将其存储至数据库D；
S2、采集RSSI信号进行处理，根据数据库D，利用RSSI信号传播模型获取BLE到车辆的距离信息；
S3、采集IMU提供的加速度、磁偏角与角速度，解算设备坐标系到世界坐标系的旋转矩阵；
S4、利用本时刻的状态量转移方程与旋转矩阵，分别得到方程关于状态量和噪声的雅可比矩阵，其中状态量包括位置、速度、加速度计偏置和重力加速度；
S5、利用两个雅可比矩阵求出估计方差矩阵与下一时刻的状态量预测值；
S6、利用蓝牙采样时刻的状态量，根据根据观测方程求出关于状态量的雅可比矩阵；
S7、根据解算出的雅可比矩阵，利用扩展卡尔曼滤波器紧耦合蓝牙与IMU并修正得到的状态量与估计方差矩阵，完成蓝牙与IMU的紧耦合室内定位。
2.根据权利要求1所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S1包括以下步骤：
S101、在定位区域选择一个BLE设备的位置，以等边三角形的形式向周边扩散，并以逆时针方向放置其他相邻BLE设备，构成正六边形即一个BLE设备序列；
S102、将BEL设备的位置和相邻BLE设备的相关定位信息存储至数据库D。
3.根据权利要求2所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S102中数据库D的形式如下：
Figure RE-FDA0003759501560000021
其中，Pi是定位字段的名称，用于提高数据库的可读性；αi是磁偏角；UUIDi是BLE的标识；n表示区域中BLE的数量；
Figure RE-FDA0003759501560000022
是相邻BLE信息的集合，其中上标i和下标j标识第i个定位场和该场的第j个iBeacon，
Figure RE-FDA0003759501560000023
形式如下。距离BLE1米处测得的平均RSSI，这对于测距是必不可少的。
Figure RE-FDA0003759501560000024
其中，
Figure RE-FDA0003759501560000025
和
Figure RE-FDA0003759501560000026
是BLE的标识；
Figure RE-FDA0003759501560000027
是距离BLE1米处测得的平均RSSI；
Figure RE-FDA0003759501560000028
是BLE的坐标。
4.根据权利要求3所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S2包括以下步骤：
S201、将移动端收到的BLE的RSSI数据进行滑动平均滤波处理；
S202、将移动端收到的BLE根据RSSI排序，选取出RSSI最大的BLE；
S203、利用RSSI测距公式计算移动端与BLE的距离为dBLE，BLE坐标为(xBLE，yBLE，zBLE)。
5.根据权利要求4所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S201中滑动平均滤波处理公式为：
Figure RE-FDA0003759501560000029
其中，RSSIm[n]是滑动平均滤波后的结果，RSSI保存第k时刻所接收的RSSI信号强度，L为滑动窗口的长度。
6.根据权利要求5所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S203中RSSI测距公式的表达式如下：
Figure RE-FDA0003759501560000031
Figure RE-FDA0003759501560000032
其中，PL(d0)为参考距离处的功率，实际为了计算方便取1m，RSSInom为将RSSIm归一化后的结果，RSSInormMin，RSSInormmMax分别为RSSInorm中的最小值与最大值，而a0，a1，a2，a3，a4，是人为选择的多项式系数，dmax与dmin均是根据实际场地设置的最大最小距离。
7.根据权利要求6所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S3包括以下步骤：
S301、利用手机传感器获取的加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵R；
S302、利用手机传感器获取的陀螺仪数值修正旋转矩阵；
S303、利用状态量转移公式推算下一时刻的状态量，包括位置、速度、加速度计偏置和重力加速度信息。
8.根据权利要求7所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S301中加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵公式为：
Figure RE-FDA0003759501560000033
Figure RE-FDA0003759501560000034
Figure RE-FDA0003759501560000035
Figure RE-FDA0003759501560000036
Figure RE-FDA0003759501560000041
其中，ax，ay，az为手机IMU测得的x，y，z轴的加速度，my与mx为测得的x，y轴下的磁感应强度。θ，
Figure RE-FDA0003759501560000042
ψ为滚转角、俯仰角与偏转角，R为设备坐标系到世界坐标系的旋转矩阵，
Figure RE-FDA0003759501560000043
为欧拉角与旋转矩阵之间的转换函数。
9.根据权利要求8所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S302中结合S301推算的R，利用手机传感器获取的陀螺仪数值修正旋转矩阵公式为：
Figure RE-FDA0003759501560000044
Figure RE-FDA0003759501560000045
Figure RE-FDA0003759501560000046
Figure RE-FDA0003759501560000047
Figure RE-FDA0003759501560000048
Figure RE-FDA0003759501560000049
其中，
Figure RE-FDA00037595015600000410
为n-1时刻的旋转矩阵估计值，
Figure RE-FDA00037595015600000411
为在n-1时刻预测值的基础上推算出n时刻的旋转矩阵估计值，
Figure RE-FDA00037595015600000412
为n时刻的旋转矩阵估计值，R为根据S301推算出来的旋转矩阵，ωx，ωy，ωz为手机IMU陀螺仪的测量值，Δt表示相邻两次IMU测量的时间间隔，Qnoise和Rnoise分别表示该系统噪声方差与测量噪声方差，KR表示卡尔曼增益，HR表示观测矩阵，I表示单位矩阵，
Figure RE-FDA00037595015600000413
与
Figure RE-FDA00037595015600000414
分别表示n时刻与在n-1时刻预测值的基础上推算出n时刻的估计方差。
10.根据权利要求9所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S4包括以下步骤：
S401、根据相邻状态量之间的关系，得到状态转移公式；
S402、根据估计值的状态转移方程计算出关于状态量与噪声量的雅可比矩阵。
11.根据权利要求10所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S401中状态量转移公式为：
Figure RE-FDA0003759501560000051
其中，p表示定位坐标，v表示运动速度，ba表示建模为具有高斯噪声
Figure RE-FDA0003759501560000054
的随机游走过程的IMU偏置，g表示重力加速度，na表示IMU加速度计测量的白噪声，下标i表示第i时刻的预测值，Δt表示相邻两次IMU测量的时间间隔，am为IMU提供的加速度测量值。R为IMU设备坐标系到世界坐标系变换的旋转矩阵，通过S303中的9轴IMU位姿解算的方法得到。
12.根据权利要求11所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S402中状态转移方程关于状态量与噪声量的雅可比矩阵为：
x＝[p v ba g]T，u＝[am]T，
Figure RE-FDA0003759501560000055
Figure RE-FDA0003759501560000052
Figure RE-FDA0003759501560000053
Figure RE-FDA0003759501560000061
其中，x，u，n分别为状态量、测量量与噪声量，三者包含的符号的含义在S402中介绍过，
Figure RE-FDA0003759501560000062
与
Figure RE-FDA0003759501560000063
分别表示状态量在第i与i+1时刻的估计值，
Figure RE-FDA0003759501560000064
为状态量转移方程公式，
Figure RE-FDA0003759501560000065
为转移方程，Fn为噪声矩阵，I为单位矩阵，Δt为相邻两次IMU测量的时间间隔，
Figure RE-FDA0003759501560000066
为第i时刻旋转矩阵的估计值。
13.根据权利要求12所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S5中估计方差矩阵与下一时刻状态量预测值计算公式为：
Figure RE-FDA0003759501560000067
Figure RE-FDA0003759501560000068
Pi、Pi+1|i分别为第i时刻与第i时刻前提下的i+1时刻的估计方差矩阵，Q为该系统噪声方差。
14.根据权利要求13所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S6中根据观测方程求出关于状态量的雅可比矩阵为：
Figure RE-FDA0003759501560000069
Figure RE-FDA00037595015600000610
(将
Figure RE-FDA00037595015600000611
记为A)
Figure RE-FDA0003759501560000071
Figure RE-FDA0003759501560000072
Figure RE-FDA0003759501560000073
Figure RE-FDA0003759501560000074
H＝[H1 H2 … Hm]T，1≤j≤m
其中，hj(xk+1)为IMU解算位置到第j个提供观测距离的蓝牙信标的距离，xk+1是第k+1时刻的状态量，第k+1时刻对应的是第k+1次蓝牙观测的时刻，pk+1为第k+1时刻S4中提供的位置估计坐标形式为(px(k+1)，py(k+1)，pz(k+1))，
Figure RE-FDA0003759501560000075
为第j个提供观测距离的蓝牙信标位置，坐标形式为
Figure RE-FDA0003759501560000076
Hj是函数hj关于状态量xk+1的雅可比矩阵，δt为相邻两次蓝牙观测的时间间隔，Rk为第k时刻根据IMU信息解算出的旋转矩阵，m为k+1时刻提供观测距离的蓝牙信标的总个数，H为观测矩阵。
15.根据权利要求14所述的蓝牙和IMU的紧耦合室内定位方法，其特征在于，所述步骤S7更新的状态量与估计方差矩阵为：
K＝Pk+1|kHT(HPk+1|kHT+V)-1
Figure RE-FDA0003759501560000077
Pk+1＝(I-KH)Pk+1|k
P为第k时刻的估计方差矩阵，Z为m个蓝牙观测距离的集合，
Figure RE-FDA0003759501560000078
分别为第k时刻与k+1时刻的状态估计量，I为单位矩阵。
CN202210453099.5A 2022-04-27 2022-04-27 一种蓝牙和惯性测量单元的紧耦合室内定位方法 Active CN115103437B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210453099.5A CN115103437B (zh) 2022-04-27 2022-04-27 一种蓝牙和惯性测量单元的紧耦合室内定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210453099.5A CN115103437B (zh) 2022-04-27 2022-04-27 一种蓝牙和惯性测量单元的紧耦合室内定位方法

Publications (2)

Publication Number Publication Date
CN115103437A true CN115103437A (zh) 2022-09-23
CN115103437B CN115103437B (zh) 2024-03-15

Family

ID=83287818

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210453099.5A Active CN115103437B (zh) 2022-04-27 2022-04-27 一种蓝牙和惯性测量单元的紧耦合室内定位方法

Country Status (1)

Country Link
CN (1) CN115103437B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115334448A (zh) * 2022-08-15 2022-11-11 重庆大学 一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105588566A (zh) * 2016-01-08 2016-05-18 重庆邮电大学 一种基于蓝牙与mems融合的室内定位系统及方法
CN109827569A (zh) * 2019-02-21 2019-05-31 奇瑞汽车股份有限公司 无人车定位方法及系统
KR20210026795A (ko) * 2019-09-02 2021-03-10 경북대학교 산학협력단 Ｉｍｕ 센서와 카메라를 이용한 하이브리드 실내 측위 시스템
CN112881979A (zh) * 2021-01-19 2021-06-01 上海工程技术大学 一种基于ekf滤波的初始状态自适应融合定位方法
CN113242518A (zh) * 2021-05-08 2021-08-10 电子科技大学 一种基于水晶型信标布局的改进位置指纹定位方法
CN113938825A (zh) * 2021-10-15 2022-01-14 太原理工大学 一种基于蓝牙aoa的综采工作面采煤机定位方法及系统
CN114088091A (zh) * 2022-01-21 2022-02-25 北京慧拓无限科技有限公司 一种基于多传感器的井工矿位姿融合方法和系统

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105588566A (zh) * 2016-01-08 2016-05-18 重庆邮电大学 一种基于蓝牙与mems融合的室内定位系统及方法
CN109827569A (zh) * 2019-02-21 2019-05-31 奇瑞汽车股份有限公司 无人车定位方法及系统
KR20210026795A (ko) * 2019-09-02 2021-03-10 경북대학교 산학협력단 Ｉｍｕ 센서와 카메라를 이용한 하이브리드 실내 측위 시스템
CN112881979A (zh) * 2021-01-19 2021-06-01 上海工程技术大学 一种基于ekf滤波的初始状态自适应融合定位方法
CN113242518A (zh) * 2021-05-08 2021-08-10 电子科技大学 一种基于水晶型信标布局的改进位置指纹定位方法
CN113938825A (zh) * 2021-10-15 2022-01-14 太原理工大学 一种基于蓝牙aoa的综采工作面采煤机定位方法及系统
CN114088091A (zh) * 2022-01-21 2022-02-25 北京慧拓无限科技有限公司 一种基于多传感器的井工矿位姿融合方法和系统

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
YUAN YOU; CHANG WU: "Hybrid Indoor Positioning System for Pedestrians With Swinging Arms Based on Smartphone IMU and RSSI of BLE", IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT, 27 March 2021 (2021-03-27) *
YUAN YOU; CHANG WU: "Indoor Positioning System With Cellular Network Assistance Based on Received Signal Strength Indication of Beacon", IEEE ACCESS, vol. 8, 30 December 2019 (2019-12-30), XP011764766, DOI: 10.1109/ACCESS.2019.2963099 *
周牧;王斌;田增山;张千坤;: "室内BLE/MEMS跨楼层融合定位算法", 通信学报, no. 05, 25 May 2017 (2017-05-25) *
孙璧文;樊启高;武亚恒;孙艳;: "基于PDR/UWB紧耦合的足绑式行人导航技术", 传感器与微系统, no. 03, 20 March 2017 (2017-03-20) *
游远: "基于BLE与IMU的数据融合室内定位技术研究与实现", 中国优秀硕士学位论文全文数据库, 21 March 2022 (2022-03-21) *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115334448A (zh) * 2022-08-15 2022-11-11 重庆大学 一种基于蓝牙和惯性传感器的无人自跟随装置的精确动态定位方法

Also Published As

Publication number Publication date
CN115103437B (zh) 2024-03-15

Similar Documents

Publication Publication Date Title
CN113945206B (zh) 一种基于多传感器融合的定位方法及装置
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN107734457B (zh) 智慧停车场导航系统及方法
CN110118560B (zh) 一种基于lstm和多传感器融合的室内定位方法
CN110197615B (zh) 用于生成地图的方法及装置
CN111491367A (zh) 一种基于群智感知和多融合技术的室内定位算法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN107702712A (zh) 基于惯性测量双层wlan指纹库的室内行人组合定位方法
CN113566820B (zh) 一种基于位置指纹和pdr算法的融合人行定位方法
CN112525197B (zh) 基于图优化算法超宽带惯性导航融合位姿估计方法
CN111060099A (zh) 一种无人驾驶汽车实时定位方法
CN113324544B (zh) 一种基于图优化的uwb/imu的室内移动机器人协同定位方法
CN105674989A (zh) 一种基于手机内置传感器的室内目标运动轨迹估计方法
CN109769206B (zh) 一种室内定位融合方法、装置、存储介质及终端设备
CN112797985B (zh) 基于加权扩展卡尔曼滤波的室内定位方法及室内定位系统
CN112729301A (zh) 一种基于多源数据融合的室内定位方法
CN108810802A (zh) 一种基于iBeacon的室内停车场车辆定位方法
US20230304802A1 (en) Passive combined indoor positioning system and method based on intelligent terminal sensor
CN115103437B (zh) 一种蓝牙和惯性测量单元的紧耦合室内定位方法
CN115435782A (zh) 一种多源信息约束下的抗干扰位置估计方法及装置
CN114916059A (zh) 基于区间随机对数阴影模型的WiFi指纹稀疏地图扩建方法
CN112762935A (zh) 一种基于船体姿态监测的定位方法及系统
CN114710744B (zh) 一种深度融合WiFi测距与PDR推算的室内定位方法
CN115540854A (zh) 一种基于uwb辅助的主动定位方法、设备和介质

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