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

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

Info

Publication number
CN115103437B
CN115103437B CN202210453099.5A CN202210453099A CN115103437B CN 115103437 B CN115103437 B CN 115103437B CN 202210453099 A CN202210453099 A CN 202210453099A CN 115103437 B CN115103437 B CN 115103437B
Authority
CN
China
Prior art keywords
ble
state quantity
matrix
imu
bluetooth
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.)
Active
Application number
CN202210453099.5A
Other languages
English (en)
Other versions
CN115103437A (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

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的形式如下：
其中，Pi是定位字段的名称，用于提高数据库的可读性；αi是磁偏角；UUIDi是BLE的标识；n表示区域中BLE的数量；是相邻BLE信息的集合，其中上标i和下标j标识第i个定位场和该场的第j个iBeacon，/>形式如下。距离BLE1米处测得的平均RSSI，这对于测距是必不可少的。
其中，和/>是BLE的标识；/>是距离BLE1米处测得的平均RSSI；/>是BLE的坐标。
上述进一步方案的有益效果是：这种储存结构不仅可以提高搜索数据的效率，还可以提高系统的可扩展性。
再进一步地，所述步骤S2包括以下步骤：
S201、将移动端收到的BLE的RSSI数据进行滑动平均滤波处理；
S202、将移动端收到的BLE根据RSSI排序，选取出RSSI最大的BLE；
S203、利用RSSI测距公式计算移动端与BLE的距离为dBLE，BLE坐标为(xBLE,yBLE,zBLE)；
上述进一步方案的有益效果是：本发明放弃传统的三边测量定位算法直接得到位置信息，从而避免车辆位置信息的误差过大。只得到距离信息可以在提高数据精度，同时只需1个BLE就可得到相应信息，从而减少了BLE个数。
再进一步地,所述步骤S201中滑动平均滤波处理公式为:
其中，RSSIm[n]是滑动平均滤波后的结果，RSSI保存第k时刻所接收的RSSI信号强度，L为滑动窗口的长度。
上述进一步方案的有益效果是：本发明与传统的RSSI处理方式不同，这种RSSI预处理方法将大大降低基于BLE的定位算法的方差，使得结果更加精准，本发明更加科学。
再进一步地，所述步骤S203中RSSI测距公式的表达式如下：
其中，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中加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵公式为:
其中，ax，ay，az为手机IMU测得的x，y，z轴的加速度，my与mx为测得的x，y轴下的磁感应强度。θ，ψ为滚转角、俯仰角与偏转角，R为设备坐标系到世界坐标系的旋转矩阵，为欧拉角与旋转矩阵之间的转换函数。
上述进一步方案的有益效果是：可以快速的得出手机设备坐标系到世界坐标系的欧拉角。本发明更加科学。
再进一步地,所述步骤S302中结合S301推算的R，利用手机传感器获取的陀螺仪数值修正旋转矩阵公式为：
其中，为n-1时刻的旋转矩阵估计值，/>为在n-1时刻预测值的基础上推算出n时刻的旋转矩阵估计值，/>为n时刻的旋转矩阵估计值，R为根据S301推算出来的旋转矩阵，ωx，ωy，ωz为手机IMU陀螺仪的测量值，Δt表示相邻两次IMU测量的时间间隔，Qnoise和Rnoise分别表示该系统噪声方差与测量噪声方差，KR表示卡尔曼增益，HR表示观测矩阵，I表示单位矩阵，/>与/>分别表示n时刻与在n-1时刻预测值的基础上推算出n时刻的估计方差。
上述进一步方案的有益效果是：引入陀螺仪测量值，使得欧拉角的估计更加准确有效。本发明更加科学。
再进一步地，所述步骤S4包括以下步骤：
S401、根据相邻状态量之间的关系，得到状态转移公式；
S402、根据估计值的状态转移方程计算出关于状态量与噪声量的雅可比矩阵；
再进一步地，所述步骤S401中状态量转移公式为：
其中，p表示定位坐标，v表示运动速度，ba表示建模为具有高斯噪声的随机游走过程的IMU偏置，g表示重力加速度，na表示IMU加速度计测量的白噪声，下标i表示第i时刻的预测值，Δt表示相邻两次IMU测量的时间间隔，am为IMU提供的加速度测量值。R为IMU设备坐标系到世界坐标系变换的旋转矩阵，通过S303中的9轴IMU位姿解算的方法得到。
上述进一步方案的有益效果是：在一段微小时间内采用匀变速运动模型对运动建模，贴近实际情况，可以得到平滑的状态转移结果。
再进一步地，所述步骤S402中状态转移方程关于状态量与噪声量的雅可比矩阵为：
其中，x，u，n分别为状态量、测量量与噪声量，三者包含的符号的含义在S402中介绍过，与/>分别表示状态量在第i与i+1时刻的估计值，/>为状态量转移方程公式，/>为转移方程，Fn为噪声矩阵，I为单位矩阵，Δt为相邻两次IMU测量的时间间隔，/>为第i时刻旋转矩阵的估计值。
再进一步地，所述步骤S5中估计方差矩阵与下一时刻状态量预测值计算公式为：
Pi、Pi+1|i分别为第i时刻与第i时刻前提下的i+1时刻的估计方差矩阵，Q为该系统噪声方差。
再进一步地，所述步骤S6中根据观测方程求出关于状态量的雅可比矩阵为：
(将/>记为A)
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))，为第j个提供观测距离的蓝牙信标位置，坐标形式为/>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
Pk+1＝(I-KH)Pk+1|k
P为第k时刻的估计方差矩阵，Z为m个蓝牙观测距离的集合。分别为第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的形式如下：
其中，Pi是定位字段的名称，用于提高数据库的可读性；αi是磁偏角；UUIDi是BLE的标识；n表示区域中BLE的数量；是相邻BLE信息的集合，其中上标i和下标j标识第i个定位场和该场的第j个iBeacon，/>形式如下。距离BLE1米处测得的平均RSSI，这对于测距是必不可少的。
其中，和/>是BLE的标识；/>是距离BLE1米处测得的平均RSSI；/>是BLE的坐标。
再进一步地，所述步骤S2包括以下步骤：
S201、将移动端收到的BLE的RSSI数据进行滑动平均滤波处理；
S202、将移动端收到的BLE根据RSSI排序，选取出RSSI最大的BLE；
S203、利用RSSI测距公式计算移动端与BLE的距离为dBLE，BLE坐标为(xBLE,yBLE,zBLE)；
再进一步地,所述步骤S201中滑动平均滤波处理公式为:
其中，RSSIm[n]是滑动平均滤波后的结果，RSSI保存第k时刻所接收的RSSI信号强度，L为滑动窗口的长度。
再进一步地，所述步骤S203中RSSI测距公式的表达式如下：
其中，PL(d0)为参考距离处的功率，实际为了计算方便取1m，RSSInorm为将RSSIm归一化后的结果，RSSInormMin，RSSInormmMax分别为RSSInorm中的最小值与最大值，而a0,a1,a2,a3,a4,是人为选择的多项式系数，dmax与dmin均是根据实际场地设置的最大最小距离。
再进一步地，所述步骤S3包括以下步骤：
S301、利用手机传感器获取的加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵R；
S302、利用手机传感器获取的陀螺仪数值修正旋转矩阵。
S303、利用状态量转移公式推算下一时刻的状态量，包括位置、速度、加速度计偏置和重力加速度信息；
再进一步地,所述步骤S301中加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵公式为:
其中，ax，ay，az为手机IMU测得的x，y，z轴的加速度，my与mx为测得的x，y轴下的磁感应强度。θ，ψ为滚转角、俯仰角与偏转角，R为设备坐标系到世界坐标系的旋转矩阵，为欧拉角与旋转矩阵之间的转换函数。
再进一步地,所述步骤S302中结合S301推算的R，利用手机传感器获取的陀螺仪数值修正旋转矩阵公式为：
其中，为n-1时刻的旋转矩阵估计值，/>为在n-1时刻预测值的基础上推算出n时刻的旋转矩阵估计值，/>为n时刻的旋转矩阵估计值，R为根据S301推算出来的旋转矩阵，ωx，ωy，ωz为手机IMU陀螺仪的测量值，Δt表示相邻两次IMU测量的时间间隔，Qnoise和Rnoise分别表示该系统噪声方差与测量噪声方差，KR表示卡尔曼增益，HR表示观测矩阵，I表示单位矩阵，/>与/>分别表示n时刻与在n-1时刻预测值的基础上推算出n时刻的估计方差。
再进一步地，所述步骤S4包括以下步骤：
S401、根据相邻状态量之间的关系，得到状态转移公式；
S402、根据估计值的状态转移方程计算出关于状态量与噪声量的雅可比矩阵；
再进一步地，所述步骤S401中状态量转移公式为：
其中，p表示定位坐标，v表示运动速度，ba表示建模为具有高斯噪声的随机游走过程的IMU偏置，g表示重力加速度，na表示IMU加速度计测量的白噪声，下标i表示第i时刻的预测值，Δt表示相邻两次IMU测量的时间间隔，am为IMU提供的加速度测量值。R为IMU设备坐标系到世界坐标系变换的旋转矩阵，通过S303中的9轴IMU位姿解算的方法得到。
再进一步地，所述步骤S402中状态转移方程关于状态量与噪声量的雅可比矩阵为：
其中，x，u，n分别为状态量、测量量与噪声量，三者包含的符号的含义在S402中介绍过，与/>分别表示状态量在第i与i+1时刻的估计值，/>为状态量转移方程公式，/>为转移方程，Fn为噪声矩阵，I为单位矩阵，Δt为相邻两次IMU测量的时间间隔，/>为第i时刻旋转矩阵的估计值。
再进一步地，所述步骤S5中估计方差矩阵与下一时刻状态量预测值计算公式为：
Pi、Pi+1|i分别为第i时刻与第i时刻前提下的i+1时刻的估计方差矩阵，Q为该系统噪声方差。
再进一步地，所述步骤S6中根据观测方程求出关于状态量的雅可比矩阵为：
(将/>记为A)
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))，为第j个提供观测距离的蓝牙信标位置，坐标形式为/>Hj是函数hj关于状态量xk+1的雅可比矩阵，δt为相邻两次蓝牙观测的时间间隔，Rk为第k时刻根据IMU信息解算出的旋转矩阵，m为k+1时刻提供观测距离的蓝牙信标的总个数，H为观测矩阵。
再进一步地，所述步骤S7更新的状态量与估计方差矩阵为：
K＝Pk+1|kHT(HPk+1|kHT+V)-1
Pk+1＝(I-KH)Pk+1|k
P为第k时刻的估计方差矩阵，Z为m个蓝牙观测距离的集合。分别为第k时刻与k+1时刻的状态估计量，I为单位矩阵。
本实施例中，根据上述提出的紧耦合蓝牙与智能手机IMU的室内车辆定位方法，进行仿真实验，来检验算法的定位准确率和成本损耗。
实验场景是一个巨大的地下停车场，整个定位区域面积约为160m×130m；BLE广播器的数量约为200个；定位面积约10000平方米。BLE设备的放置策略采用水晶形BLE放置(CIP)，但定位场与自由空间不同，BLE不能附着在空间中的某些位置，因此与CIP有一些区别。
本实施例中，我们选取一条长约400米的路线进行实际测量。利用 计算定位算法的定位误差。其中er为计算出的误差，/>与/>为估计的坐标值，Xreal与Yreal为真实的坐标值。实验得出的结论显示紧耦合蓝牙与IMU的室内车辆定位定位误差小于4米的概率为83.4％；而只依靠蓝牙进行的三角定位在定位效果要比该方法差。而实际场景的测试让该方法具有被大规模商用的可能性。/>

Claims (1)

1.一种蓝牙和惯性测量单元的紧耦合室内定位方法，其特征在于，包括以下步骤：
S1、在定位区域布置BLE，采集其位置信息，将其存储至数据库D；
S2、采集RSSI信号进行处理，根据数据库D，利用RSSI信号传播模型获取BLE到车辆的距离信息；
S3、采集IMU提供的加速度、磁偏角与角速度，解算设备坐标系到世界坐标系的旋转矩阵；
S4、利用本时刻的状态量转移方程与旋转矩阵，分别得到方程关于状态量和噪声的雅可比矩阵，其中状态量包括位置、速度、加速度计偏置和重力加速度；
S5、利用两个雅可比矩阵求出估计方差矩阵与下一时刻的状态量预测值；
S6、利用蓝牙采样时刻的状态量，根据观测方程求出关于状态量的雅可比矩阵；
S7、根据解算出的雅可比矩阵，利用扩展卡尔曼滤波器紧耦合蓝牙与IMU并修正得到的状态量与估计方差矩阵，完成蓝牙与IMU的紧耦合室内定位；
进一步地，所述步骤S1包括以下步骤：
S101、在定位区域选择一个BLE设备的位置，以等边三角形的形式向周边扩散，并以逆时针方向放置其他相邻BLE设备，构成正六边形即一个BLE设备序列；
S102、将BEL设备的位置和相邻BLE设备的相关定位信息存储至数据库D；
再进一步地，步骤S102中数据库D的形式如下：
其中，Pi是定位字段的名称，用于提高数据库的可读性；αi是磁偏角；UUIDi是BLE的标识；n表示区域中BLE的数量；是相邻BLE信息的集合，其中上标i和下标j标识第i个定位场和该场的第j个iBeacon；/>形式如下：
其中，和/>是/>集合中的最大值与最小值；/>是距离BLE1米处测得的平均RSSI；/>是BLE的坐标；
再进一步地，所述步骤S2包括以下步骤：
S201、将移动端收到的BLE的RSSI数据进行滑动平均滤波处理；
S202、将移动端收到的BLE根据RSSI排序，选取出RSSI最大的BLE；
S203、利用RSSI测距公式计算移动端与BLE的距离为dBLE，BLE坐标为(xBLE,yBLE,zBLE)；
再进一步地，所述步骤S201中滑动平均滤波处理公式为：
其中，RSSIm[n]是滑动平均滤波后的结果，滑动平均滤波保存第k时刻所接收的RSSI信号强度，L为滑动窗口的长度；
再进一步地，所述步骤S203中RSSI测距公式的表达式如下：
其中，PL(d0)为参考距离处的功率，实际为了计算方便取1m，RSSInorm为将RSSIm归一化后的结果，RSSIm为RSSIm[n]的简写，均表示滑动平均滤波后的结果，RSSInormMin，RSSInormMax分别为RSSInorm中的最小值与最大值，而a0,a1,a2,a3,a4,是人为选择的多项式系数，dmax与dmin均是根据实际场地设置的最大最小距离；
再进一步地，所述步骤S3包括以下步骤：
S301、利用手机传感器获取的加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵R；
S302、利用手机传感器获取的陀螺仪数值修正旋转矩阵；
S303、利用状态量转移公式推算下一时刻的状态量，包括位置、速度、加速度计偏置和重力加速度信息；
再进一步地，所述步骤S301中加速度、磁偏角解算手机设备坐标系到世界坐标系的旋转矩阵公式为:
其中，ax，ay，az为手机IMU测得的x，y，z轴的加速度，my与mx为测得的x，y轴下的磁感应强度，θ，ψ为滚转角、俯仰角与偏转角，R为设备坐标系到世界坐标系的旋转矩阵，为欧拉角与旋转矩阵之间的转换函数；
再进一步地，所述步骤S302中结合S301推算的R，利用手机传感器获取的陀螺仪数值修正旋转矩阵公式为：
其中，为n-1时刻的旋转矩阵估计值，/>为在n-1时刻预测值的基础上推算出n时刻的旋转矩阵估计值，/>为n时刻的旋转矩阵估计值，R为根据S301推算出来的旋转矩阵，ωx，ωy，ωz为手机IMU陀螺仪的测量值，Δt表示相邻两次IMU测量的时间间隔，Qnoise和Rnoise分别表示该系统噪声方差与测量噪声方差，KR表示卡尔曼增益，HR表示观测矩阵，I表示单位矩阵，/>与/>分别表示n时刻与在n-1时刻预测值的基础上推算出n时刻的估计方差；
再进一步地，所述步骤S4包括以下步骤：
S401、根据相邻状态量之间的关系，得到状态转移公式；
S402、根据估计值的状态转移方程计算出关于状态量与噪声量的雅可比矩阵；
再进一步地，所述步骤S401中状态量转移公式为：
其中，p表示定位坐标，v表示运动速度，ba表示建模为具有高斯噪声的随机游走过程的IMU偏置，g表示重力加速度，na表示IMU加速度计测量的白噪声，下标i表示第i时刻的预测值，Δt表示相邻两次IMU测量的时间间隔，am为IMU提供的加速度测量值，R为IMU设备坐标系到世界坐标系变换的旋转矩阵，通过S303中的9轴IMU位姿解算的方法得到；
再进一步地，所述步骤S402中状态转移方程关于状态量与噪声量的雅可比矩阵为：
x＝[p v ba g]T，u＝[am]T,
其中，x，u，n分别为状态量、测量量与噪声量，三者包含的符号的含义在S402中介绍过，与/>分别表示状态量在第i与i+1时刻的估计值，/>为状态量转移方程公式，为转移方程，Fn为噪声矩阵，I为单位矩阵，Δt为相邻两次IMU测量的时间间隔，/>为第i时刻旋转矩阵的估计值；
再进一步地，所述步骤S5中估计方差矩阵与下一时刻状态量预测值计算公式为：
Pi、Pi+1|i分别为第i时刻与第i时刻前提下的i+1时刻的估计方差矩阵，Q为该系统噪声方差；
再进一步地，所述步骤S6中根据观测方程求出关于状态量的雅可比矩阵为：
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))，为第j个提供观测距离的蓝牙信标位置，坐标形式为/>Hj是函数hj关于状态量xk+1的雅可比矩阵，δt为相邻两次蓝牙观测的时间间隔，Rk为第k时刻根据IMU信息解算出的旋转矩阵，m为k+1时刻提供观测距离的蓝牙信标的总个数，H为观测矩阵；
再进一步地，所述步骤S7更新的状态量与估计方差矩阵为：
K＝Pk+1|kHT(HPk+1|kHT+V)-1
Pk+1＝(I-KH)Pk+1|k
P为第k时刻的估计方差矩阵，Z为m个蓝牙观测距离的集合，分别为第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 CN115103437A (zh) 2022-09-23
CN115103437B true 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)

Families Citing this family (1)

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

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.2021,全文. *
Yuan You ; Chang Wu.Indoor Positioning System With Cellular Network Assistance Based on Received Signal Strength Indication of Beacon.IEEE Access.2019,第8卷全文. *
基于BLE与IMU的数据融合室内定位技术研究与实现;游远;中国优秀硕士学位论文全文数据库;20220321;全文 *
基于PDR/UWB紧耦合的足绑式行人导航技术;孙璧文;樊启高;武亚恒;孙艳;;传感器与微系统;20170320(03);全文 *
室内BLE/MEMS跨楼层融合定位算法;周牧;王斌;田增山;张千坤;;通信学报;20170525(05);全文 *

Also Published As

Publication number Publication date
CN115103437A (zh) 2022-09-23

Similar Documents

Publication Publication Date Title
CN107734457B (zh) 智慧停车场导航系统及方法
CN107659893B (zh) 一种误差补偿方法、装置、电子设备及可读存储介质
Tian et al. Pedestrian dead reckoning for MARG navigation using a smartphone
CN103471595B (zh) 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN110197615B (zh) 用于生成地图的方法及装置
CN108151747A (zh) 一种利用声信号与惯性导航融合的室内定位系统及定位方法
CN105589064A (zh) Wlan位置指纹数据库快速建立和动态更新系统及方法
CN105509739A (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN105044668A (zh) 一种基于多传感器装置的wifi指纹数据库构建方法
CN106767828A (zh) 一种手机室内定位解决方法
CN107449444A (zh) 一种多星图姿态关联的星敏感器内参数标定方法及其装置
CN102802260A (zh) 基于矩阵相关的wlan室内定位方法
CN111536967A (zh) 一种基于ekf的多传感器融合温室巡检机器人跟踪方法
CN111698774B (zh) 基于多源信息融合的室内定位方法及装置
CN113324544B (zh) 一种基于图优化的uwb/imu的室内移动机器人协同定位方法
CN103543434A (zh) 室内定位系统、手机及定位方法
CN115103437B (zh) 一种蓝牙和惯性测量单元的紧耦合室内定位方法
CN113566820B (zh) 一种基于位置指纹和pdr算法的融合人行定位方法
CN105674989A (zh) 一种基于手机内置传感器的室内目标运动轨迹估计方法
CN109839613B (zh) 一种使用路径信息校准的射频定位方法和装置
CN113124880A (zh) 一种基于两种传感器数据融合的建图及定位方法、装置
CN106153037A (zh) 一种机器人的室内定位方法、装置及系统
CN102830391B (zh) 一种红外搜索与跟踪系统准确性指标计算方法
CN114580452A (zh) 一种基于嗅觉算法的远距离感知rfid电子界址的方法
CN112762935B (zh) 一种基于船体姿态监测的定位方法及系统

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