基于IMU、数字室分和WiFi的室内综合定位方法与流程

本发明属于移动通信，尤其涉及基于imu、数字室分和wifi的室内综合定位方法。

背景技术：

1、随着位置服务的广泛需求，室内定位技术成为一个重要的研究领域，现有的室内定位方法主要基于wifi、蓝牙(ble)、超宽带(uwb)、视觉定位等手段。这些技术各有优缺点，难以单独实现高精度和高可靠的室内定位。

2、wifi定位依赖现有wifi ap(基站)，定位精度受信号覆盖范围影响，一般在3～30m，ble与uwb技术需要额外部署定位传感器，增加了部署成本与难度，视觉定位对环境光线要求较高，遭遇光线不佳环境时精度显著下降。此外，这些技术大多只能提供二维平面定位，无法满足三维空间需求。

3、对于imu(惯性测量单元)可以检测到机体的运动和姿态，基于imu的惯性导航可实现无基站辅助的的自主定位。但是，由于航向误差和位置漂移的存在，imu定向误差会随着时间增加，无法长时间保持高精度。

4、另外5g时代，数字室分系统(dis)也应用到室内无线通信领域，利用5g技术实现室内定位，但是同样面临定位精度有限、辨识与管理难度大、网络连接不稳定、多径效应显著、部署成本高等技术短板。

5、基于此，需要一种新型室内定位方法来综合利用现有技术，弥补自己的各自不足，实现高精度三维定位。

技术实现思路

1、发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供基于imu(inertial measurement unit，惯性测量单元)、数字室分和wifi的室内综合定位方法，解决现有室内定位技术难以实现高精度三维定位的难题，该方法有效融合imu、5g数字室分和wifi等多源定位信息，构建室内三维坐标，实现实时和连续的位置修正与导航，达到高精度三维定位的目的。本发明方法包括以下步骤：

2、步骤1，采集多源定位信息，包括imu信息、5g数字室分信息和wifi信息，并对时间戳进行同步转换；

3、步骤2，进行环境与传感器校准，完成wifi分析、imu分析和5g小基站分析；

4、步骤3，融合定位信息，得到设备的三维空间定位与姿态信息；

5、步骤4，分别进行wifi坐标系、5g小基站坐标系、imu输出的位置增量的坐标转换，得到设备的最终姿态结果。

6、步骤1包括：

7、步骤1-1，采集imu信息：在测量时间起点t0开始，连续采集imu的加速度、角速度和地磁信息，并以t0为时间戳进行标记，获得imu数据序列：

8、{a1，w1，m1，t01}，{a2，w2，m2，t02}，...，{an，wn，mn，t0n}，

9、其中an表示第n个数据样本的加速度数据，包含x轴、y轴和z轴三个方向上的加速度值；

10、wn表示第n个数据样本的角速度数据，包含绕x轴、y轴和z轴三个方向的角速度值；

11、mn表示第n个数据样本的地磁数据，包含x轴、y轴和z轴三个方向的地磁强度值；

12、t0n表示第n个数据样本对应的时间戳，位于测量时间起点t0之后；

13、步骤1-2，采集5g数字室分信息：在测量时间起点t1开始，连续采集相关小基站的位置坐标、信号强度ss(signal strength)和到达时间信息toa(time of arrival)，以t1为时间戳进行标记，获得5g数据序列：

14、{b1，s1，t1，t11}，{b2，s2，t2，t12}，...，{bm，sm，tm，t1m}，

15、其中bm表示第m个小基站的位置坐标；

16、sm表示从第m个小基站接收到的信号强度；

17、tm表示信号从第m个小基站发出到达定位目标的到达时间；

18、t1m表示第m个数据样本的采集时间戳，基于5g信息采集设备内部时钟；

19、步骤1-3，采集wifi信息：在测量时间起点t2开始，连续采集相关wifi接入点ap(access point)的mac地址、信号强度ss和到达时间信息toa，以t2为时间戳进行标记，获得wifi数据序列：

20、{w1，ss1，toa1，t21}，{w2，ss2，toa2，t22}，...，{wn，ssn，toan，t2n}，

21、其中wn表示wifi第n个接入点ap的mac地址，用于识别ap；

22、ssn表示从第n个接入点ap接收到的信号强度；

23、toan表示信号从第n个接入点ap发出后到达定位目标设备的时间；

24、t2n表示第n个数据样本的采集时间戳；

25、步骤1-4，以全球系统时间gst(global system time全球系统时间)为绝对时间基准，对imu数据序列、5g数据序列和wifi数据序列的时间戳进行同步转换，得到同步后的数据序列：

26、imu数据序列：{a1，w1，m1，t01_sync}，{a2，w2，m2，t02_sync}，...，{an，wn，mn，t0n_sync}，t0n_sync表示第n个imu数据样本的采集时间点；

27、5g数据序列：{b1，s1，t1，t11_sync}，{b2，s2，t2，t12_sync}，...，{bm，sm，tm，t1m_sync}，t1m_sync表示第n个5g数据样本的采集时间点；

28、wifi数据序列：{w1，ss1，toa1，t21_sync}，{w2，ss2，toa2，t22_sync}，...，{wn，ssn，toan，t2n_sync}；t2n_sync表示第n个wifi数据样本的采集时间点；

29、步骤1-5，基于imu数据序列、5g数据序列和wifi数据序列中位置相关性，建立信息间的对应关系。

30、步骤2包括：

31、步骤2-1，选择环境：设定测试环境，测试环境内有wifi ap和5g小基站，使用imu传感器采集姿态信息；

32、步骤2-2，wifi分析：在测试环境内不同位置，分别测量每个wifi接入点ap的接收信号强度指示rssi(received signal strength indicator，接收信号强度指示)，统计全部wifi ap在各位置的综合接收信号强度指示rssi，分析接收信号强度指示rssi与环境位置的关系；

33、步骤2-3，imu分析；

34、步骤2-4，5g小基站分析。

35、4、根据权利要求3所述的方法，其特征在于，步骤2-2包括：

36、步骤2-2-1，收集测试数据：测试环境下，在各个位置测量从两个以上wifi接入点ap接收到的接收信号强度指示rssi，录入测试结果矩阵，同时记录各位置的环境信息；

37、步骤2-2-2，数据预处理：去除测试结果矩阵中异常值与噪声，对矩阵进行平滑处理，同时标定各位置的绝对坐标；

38、步骤2-2-3，利用测量结果拟合wifi路径损耗模型：rssi＝a–10qlog10(d)，其中a为插补常数，q为路径损耗指数，d为wifi接入点与目标设备之间的距离；

39、步骤2-2-4，比较同一位置不同墙壁和人流条件下的接收信号强度指示rssi，统计衰减量，建立墙壁材料和人流密度到rssi衰减的映射关系：

40、定义测试点位置为p，墙壁数量为z，第i面墙壁的材质为material_i，每种墙壁材质到rssi衰减量的对应关系为attenuation_i＝f(material_i)；其中attenuation_i表示第i面墙壁导致的wifi信号衰减量，material_i表示第i面墙壁的材质，f()是一个函数,输入变量是墙壁材质，输出结果是wifi信号在该材质墙上的衰减量；

41、定义人流密度为population_density，单位为人/平方米，人流密度到接收信号强度指示rssi衰减量的对应关系为attenuation_pop＝k*population_density，其中k为环境参数，例如人流密度每增加0.1人/平方米，接收信号强度指示rssi减小2db，则k＝20；attenuation_pop表示人流密度导致的wifi信号额外衰减量；

42、测试点位置p的接收信号强度指示rssi衰减量rssi_attenuation(p)为：

43、rssi_attenuation(p)＝σattenuation_i+attenuation_pop

44、步骤2-2-5，建立环境信息模型：在matlab中构建一个仿真环境，布置两个以上接入点ap与测试位置，输入wifi路径损耗模型、墙壁材料和人流密度到接收信号强度指示rssi衰减的映射关系、测试位置绝对坐标与环境信息，环境信息模型能够输出每个位置预期的接收信号强度指示rssi值，公式为：

45、pl(x，y，z)＝σpli+attn_wall+attn_human

46、其中pli是每个api的路径损耗，其中api表示第i个接入点，attn_wall是墙壁引起的总体wifi信号衰减量(它综合考虑了墙壁的材质和数量两个因素)；attn_human是人流密度，pl(x，y，z)是坐标为(x,y,z)的位置期望接收到的wifi信号强度，也就是综合路径损耗；

47、测试点预期的接收信号强度指示rssi为：

48、rssi(x，y，z)＝σ(pti-pl(x，y，z))

49、其中pti为每个api的发射功率(dbm)；rssi(x,y,z)表示坐标为(x,y,z)的位置预测的wifi综合信号强度；

50、步骤2-2-6，结果验证：将环境信息模型输出的rssi结果与测试数据进行比对，计算误差，调整模型参数，直到误差达到可接受水平，最终得到修正后的环境信息模型，具体包括：

51、设环境信息模型输出的测试点位置p的接收信号强度指示rssi预测值为rssi_pred(p)；

52、测试点位置p的实际接收信号强度指示rssi测量值为rssi_real(p)；

53、则模型预测结果和实测结果的误差x1为：

54、x1＝rssi_pred(p)-rssi_real(p)

55、为评估整个模型的预测误差，计算所有测试点的平均误差x2：

56、

57、其中r为测试点总数；

58、如果平均误差x2超过预设的阈值θ，则调整模型，重新运行；

59、对测试环境简单，数据量少的情况，θ取6-8db；

60、对测试点较多，环境复杂的情况，θ取3-5db；

61、如果要求高精度可以将θ设定为2～3db；

62、如果精度要求不高，也可以取10db左右。

63、步骤2-3包括：

64、步骤2-3-1，imu测试：

65、在目标环境下，分别在以下条件下采集imu原始数据：

66、静止状态：imu平放在桌面上不动，连续采集1分钟数据，理论上输出为0；

67、人行走：测试者手持imu缓慢行走，连续采集3分钟的数据；

68、不同姿态：imu采集仰躺、侧躺、俯卧的加速度与角速度数据，每姿态1分钟；

69、步骤2-3-2，静止状态误差分析：

70、比较实测数据与0值，得到：

71、x轴陀螺输出平均为0.1°/s，说明存在0.1°/s的漂移误差，z轴加速度计输出平均值为0.05m/s2，说明存在0.05m/s2的零偏误差；

72、步骤2-3-3，imu误差模型：

73、在matlab中构建imu仿真模型，输入理论静止状态，并在imu仿真模型中加入误差源：

74、x轴陀螺漂移误差源：增加一个0.1°/s的随机漂移值；

75、z轴加速度计零偏误差源：增加一个0.05m/s2的常值误差；

76、运行imu仿真模型，输出的数据为添加误差源后的imu预期输出；

77、步骤2-3-4，结果验证：将imu仿真模型输出的数据与实测静止状态数据进行比对，计算误差，如果误差满足要求，说明建立的imu误差模型能够描述实际imu的误差源与特征；如果误差较大(当imu性能参数恶化、运动的状态复杂、使用时时间过长都会使得imu输出误差较大)，调整imu仿真模型中的误差源参数，如陀螺漂移改为0.08°/s，零偏改为0.04m/s2，直到误差减小到可接受程度；

78、步骤2-3-5，动态条件扩展：重复步骤2-3-3与步骤2-3-4，得到描述imu在人行走与不同姿态状态下的imu仿真模型，通过步骤2-3-3得到静止状态模型，通过重复步骤2-3-3～步骤2-3-4，得到动态条件模型，将动态条件模型与静止状态模型集成，得到描述多种工作条件下(包括静止状态、运动状态、不同方向、不同姿态、不同运动模式、不同环境以及上述状态的组合，形成的更复杂工作条件)imu误差特性的imu误差模型(imu仿真模型是构建imu误差模型的基础和中间步骤。imu误差模型是在仿真模型的基础上，通过与实测数据比较和调整的参数得到的，用来描述imu误差特征的模型)。

79、步骤2-4包括：

80、步骤2-4-1，5g小基站测试：在测试环境下测试4个5g小基站的以下参数：

81、房间位置：

82、小基站1位于办公室东南角，坐标为(x1，y1)；

83、小基站2位于办公室西北角，坐标为(x2，y2)；

84、小基站3位于办公室中央，坐标为(x3，y3)；

85、小基站4位于办公室西南角，坐标为(x4，y4)；

86、发射方向：

87、小基站1的主要发射方向朝东；

88、小基站2的主要发射方向朝西；

89、小基站3和小基站4的发射方向朝向办公室中央；

90、发射功率：

91、小基站1发射功率为100mw；

92、小基站2发射功率为200mw；

93、小基站3发射功率为150mw；

94、小基站4发射功率为180mw；

95、定位误差：在每个小基站覆盖区域内随机选取x3个测试点(x3为可配置的正整数)，采集5g信号与imu、wifi定位信息计算位置，与理论坐标比对得到定位误差；

96、步骤2-4-2，5g环境模型：

97、在matlab中构建测试环境的3d的5g环境模型，添加4个小基站在房间的位置与坐标、发射方向与功率参数，运行5g环境模型能够显示每个小基站的理论覆盖范围与信号强度分布图；

98、步骤2-4-3，5g误差模型：

99、在理论覆盖范围内的各点，根据测试结果分配一个定位误差值，5g误差模型描述了每个小基站定位误差的空间分布；

100、步骤2-4-4，建立5g综合环境与误差模型：

101、将5g环境模型与5g误差模型集成到同一个matlab模型中，得到5g综合环境与误差模型。

102、步骤3包括：

103、步骤3-1，获取原始定位信息；

104、步骤3-2，通过信息融合定位算法进行计算。

105、步骤3-1包括：

106、步骤3-1-1，wifi定位：获取wifi ap的rssi值，利用wifi路径损耗模型计算出与每个ap的距离，计算设备位置；

107、步骤3-1-2，解算imu姿态：获取imu输出的陀螺与加速度数据，采用卡尔曼滤波算法计算出设备的当前姿态角与位置增量信息；

108、步骤3-1-3，5g定位：获取设备连接的5g小基站id，根据5g综合环境与误差模型，初步得到设备所在位置和预期定位误差。

109、步骤3-2中，所述信息融合定位算法的输入包括：原始的wifi、imu与5g定位信息；环境信息模型、imu仿真模型与5g综合环境与误差模型所提供的先验信息；

110、步骤3-2-1，利用环境信息模型与5g综合环境与误差模型对wifi与5g定位信息进行环境误差补偿，得到优化后的位置信息；

111、步骤3-2-2，imu信息优化：利用imu仿真模型对imu输出的姿态与位置增量信息进行误差补偿，得到优化后的姿态与位置更新信息；

112、步骤3-2-3，多源信息融合：将优化后的位置信息、姿态与位置更新信息进行卡尔曼滤波融合，同时利用环境信息模型、imu仿真模型与5g综合环境与误差模型对滤波过程进行辅助，得到设备最终的三维空间定位信息，具体包括：

113、步骤3-2-3-1，通过环境信息模型与5g综合环境与误差模型对wifi与5g原始定位结果进行优化，得到(x1，y1，z1)与(x2，y2，z2)，(x1，y1，z1)是5g的三维定位坐标，(x2，y2，z2)是wifi的三维定位坐标；

114、x1表示设备的东西方向坐标，一般单位为米；y1表示设备的南北方向坐标，一般单位也为米；z1表示设备的垂直高度坐标，一般单位为米；(x1,y1,z1)作为三维空间直角坐标系中的位置向量，可以唯一确定设备在三维空间中的位置；

115、步骤3-2-3-2，通过imu仿真模型对imu输出的姿态与位置增量进行优化，得到设备前两秒的位置变化(x3，y3，z3)；(x3，y3，z3)是imu的三维定位坐标；

116、步骤3-2-3-3，定义卡尔曼滤波器状态量xk＝[x，y，z，φ，θ，ψ，vx，vy，vz]t，其中(x，y，z)为位置坐标，(φ，θ，ψ)为姿态角，其中φ为横滚角，表示围绕x轴的旋转角度，θ为俯仰角，表示围绕y轴的旋转角度，ψ为航向角，表示围绕z轴的旋转角度；v为速度；t表示矩阵的转置；vx为x轴方向的速度分量；vy为y轴方向的速度分量；vz为z轴方向的速度分量；

117、步骤3-2-3-4，根据imu输出构建设备运动学方程：

118、xk＝axk-1+buk-1+wk-1，

119、更新协方差p'k＝apk-1at+q；

120、其中a为状态转移矩阵，根据imu输出的角速度与加速度，建立机动学方程，推算出当前状态xk与上一状态xk-1之间的映射关系；

121、b为输入矩阵，q为系统噪声协方差，xk-1表示上一时刻的状态向量，在初始化时设置为初始状态x0；uk-1为imu的角速度与加速度输出，通过矩阵b将uk-1转化为对应于状态量xk的增加量；pk-1为上一时刻的协方差；

122、步骤3-2-3-5，设置zk＝[x1，y1，z1，x2，y2，z2]t作为观测值，建立观测方程：

123、zk＝hxk+vk，

124、其中h为观测矩阵，vk为观测噪声；

125、步骤3-2-3-6，卡尔曼滤波，包括如下步骤：

126、步骤3-2-3-6-1，设置初始状态x0＝[x0，y0，z0，0，0，0，0，0，0]t，p0为初始协方差；x0是初始化时设备的x轴方向的位置坐标；y0是初始化时设备的y轴方向的位置坐标；z0是初始化时设备的z轴方向的位置坐标；x0是初始状态向量，包含位置、姿态角和速度的初始值；

127、步骤3-2-3-6-2，利用设备运动学方程计算x'k与p'k；x'k是预测状态向量，表示利用系统模型根据前一状态预测出的当前状态估计值；p'k是预测误差协方差矩阵，表示根据系统模型预测出的当前状态误差协方差矩阵；

128、步骤3-2-3-6-3，计算观测残差yk＝zk-hx'k，更新增益kk，计算最优状态估计xk＝x'k+kkyk，更新pk；

129、步骤3-2-3-6-4，设置xk为下次迭代的初始值，重复步骤3-2-3-6-2～步骤3-2-3-6-3；

130、步骤3-2-3-7，结果输出：迭代n次后，输出xk＝[x，y，z，φ，θ，ψ]t作为设备的三维空间定位与姿态信息。

131、步骤4包括：

132、步骤4-1，wifi定位结果(x1，y1，z1)在wifi坐标系下，5g定位结果(x2，y2，z2)在5g基站坐标系下，imu输出的位置增量在设备自身坐标系下；

133、步骤4-2，选择一个测试环境下的参考坐标系；

134、步骤4-3，wifi坐标系与参考坐标系之间的转换关系为：

135、(xr，yr，zr)＝rr2w(x1，y1，z1)+tr2w

136、其中，(xr，yr，zr)为参考坐标系下的坐标；rr2w为wifi坐标系与参考坐标系之间的旋转矩阵，tr2w为wifi坐标系原点在参考坐标系下的位置向量；

137、步骤4-4，5g小基站坐标系与参考坐标系之间的转换矩阵为rr2g，原点偏移量tr2g；

138、步骤4-5，imu输出的位置增量(δx，δy，δz)在设备自身坐标系下，通过设备在参考坐标系下的初始位置(x0，y0，z0)和初始姿态，将imu输出的位置增量(δx，δy，δz)转换到参考坐标系下，具体包括：

139、步骤4-5-1，定义设备初始在参考坐标系下的姿态为欧拉角(yaw0,pitch0,roll0)，其中yaw0表示初始状态下的偏航角，pitch0表示初始状态下的俯仰角，roll0表示初始状态下的滚转角；

140、步骤4-5-2，构建从设备自身坐标系到参考坐标系的旋转转换矩阵r：

141、r＝r_yaw(yaw0)*r_pitch(pitch0)*r_roll(roll0)

142、其中r_yaw是横滚角旋转矩阵，r_pitch是俯仰角旋转矩阵，r_roll是航向角旋转矩阵；

143、步骤4-5-3，进行坐标转换：

144、(δxr,δyr,δzr)＝r*(δx,δy,δz)

145、其中δxr是imu位置增量δx经r矩阵转换到参考坐标系后的值，δyr是imu位置增量δy经r矩阵转换到参考坐标系后的值；δzr是imu位置增量δz经r矩阵转换到参考坐标系后的值；

146、步骤4-5-4，初始位置为(x0,y0,z0)，转换后的imu输出为：

147、(δxr+x0,δyr+y0,δzr+z0)；

148、步骤4-6，综合wifi定位结果(xr1，yr1，zr1)、5g定位结果(xr2，yr2，zr2)与imu输出(δxr，δyr，δzr)，得到设备在参考坐标系下的位置(x，y，z)；

149、对于姿态信息，需要将不同来源的姿态表示转换到统一的欧拉角或四元数表示，进行融合优化，具体包括：

150、步骤4-6-1，imu输出的欧拉角姿态θ_imu为：

151、θ_imu＝[yaw1，pitch1，roll1]

152、其中yaw1表示imu输出的偏航角，pitch1表示imu输出的俯仰角，roll1表示imu输出的滚转角；

153、步骤4-6-2，5g相位跟踪获得的相对角度变化量δθ_5g为：

154、δθ_5g＝[δyaw，δpitch，δroll]

155、其中δyaw表示5g相位跟踪获得的偏航角变化量，δpitch表示5g相位跟踪获得的俯仰角变化量，δroll表示5g相位跟踪获得的滚转角变化量；

156、步骤4-6-3，将5g的相对变化量转换到绝对姿态角：

157、θ_5g＝θ_5g'+δθ_5g

158、θ_5g表示通过5g相位跟踪得到的设备的绝对姿态角，包含yaw角、pitch角和roll角；θ_5g'表示上一时刻设备的绝对姿态角；

159、步骤4-6-4，wifi通过到达角aoa(angle of arrival)直接测量得到的欧拉角为：

160、θ_wifi＝[yaw2，pitch2，roll2]，

161、其中yaw2表示wifi通过到达角aoa直接测量得到的偏航角，pitch2表示wifi通过到达角aoa直接测量得到的俯仰角，roll2表示wifi通过到达角aoa直接测量得到的滚转角；

162、步骤4-6-5，在欧拉角表示下，姿态融合的加权算法为：

163、θ_fuse＝w1θ_imu+w2θ_5g+w3*θ_wifi

164、其中w1、w2、w3是权重参数，权重参数满足：

165、w1+w2+w3＝1

166、步骤4-6-6，四元数表示下，设q_imu，q_5g，q_wifi分别为imu、5g、wifi三种源的姿态四元数，融合算法为：

167、

168、其中q_fuse表示融合后的四元数姿态；q_imu表示imu计算所得的四元数姿态；q_5g表示5g相位跟踪所得的四元数姿态；q_wifi表示wifi天线阵列所测量的四元数姿态；conjugate(q_imu)表示q_imu的共轭四元数；表示四元数乘法，*表示共轭；

169、步骤4-6-7，通过重复步骤4-6-1～步骤4-6-6，得到设备的最终姿态结果。

170、有益效果：单一的定位技术如(5g/wifi/imu)虽然系统简单，易于实现，但是定位精度较差，难以满足高精度定位需求，受某一因素的限制和影响较大；鲁棒性差，容易受到环境变化的影响，如信号遮挡、多径效应等；适用场景受限，某些场合无法正常工作。而本发明采用多源定位融合(5g+wifi+imu)，其定位精度高，可以实现亚米级精度，满足大多数高精度定位应；鲁棒性强，即使某一源失效，仍可正常定位；适用广泛，几乎适用于任何室内场景。