一种基于UWB和IMU信息融合的室内定位方法

一种基于uwb和imu信息融合的室内定位方法
技术领域
1.本发明涉及机器人室内定位导航领域，尤其涉及一种基于uwb和imu信息融合的室内定位方法。


背景技术：

2.随着机器人智能化技术和人类社会生活的多样性发展，机器人装备的室内外工作已经成为一种必然的趋势。与室外环境具备全球化的实时定位系统不同，室内定位技术面临着环境复杂、干扰因素和信号微弱等难题。
3.对于需要预设基站的定位技术，目前因超宽带(uwb)具有脉冲间隔短、时间分辨率高可到达到厘米级别的测距精度，成为这些定位主流方案。它是一种无线电技术，在多个波段(3.1ghz-10.6ghz)上同时传输几百mhz的宽带(大于500ghz)。超宽带这一技术特点，在视距条件下，定位精度能到厘米级别；但当存在遮挡非视距(nlos)时,其提供的定位精度会大大降低。uwb定位常采用最小二乘法解算，但缺点是会因为出现奇异矩阵而无法解算的情况，会严重影响系统定位精度和稳定性。惯性导航是一种独立的导航技术，它不需要预先布置环境，主要使用惯性测量单元(imu)提供的测量值，即加速度和角速度。在已知初始位置的情况下，可以求出载体的位置、速度和姿态(方向)。因其这种技术特点，惯性导航技术是一种很好的解决非视距(nlos)的问题方案。但由于定位信息是通过积分得到且系统自身存在加速度计和陀螺仪偏差，故系统误差会随着时间增长变大
4.单一的传感器进行定位有自身优点，但同样存在着无法忽视的缺点。故将传感器进行数据融合互补传感器之间的优势，成为当前定位技术研究的趋势。李中道等人，提出了uwb与激光雷达的融合，能提高室内定位的精度，但依然没有解决uwb非视距的问题；zihajehzadehs等人采用级联kf方式融合uwb和imu，但是算法计算量复杂，时效性不足。


技术实现要素：

5.本发明是为了解决上述现有技术存在的不足之处，提出一种基于uwb和imu信息融合的室内定位方法，旨在解决单一传感器定位不精确的问题，克服非视距和多径效应带来的影响，弥补移动机器人短期内定位数据丢失、降低定位数据的抖动，从而满足复杂室内环境下长时间稳定且高精度的定位需求，并能提高定位系统的稳定性。
6.本发明为达到上述发明目的，采用如下技术方案：
7.本发明一种基于uwb和imu信息融合的室内定位方法的特点在于，是在室内定位区域内设置有运动平台，在所述运动平台上设置有imu传感器和pc端，在所述imu传感器集成有一个未知位置的uwb定位标签；在所述运动平台周围设置有n个已知位置的uwb基站节点；其中，所述imu传感器包括：三轴加速度计、三轴陀螺仪；所述室内定位方法是按如下步骤进行：
8.步骤1、利用所述imu传感器采集所述运动平台的三轴加速度以及三轴角速度数据，并发送给pc端进行数据解算处理，得到运动平台的第一定位数据；
9.步骤2、获取所述uwb定位标签分别与n个uwb基站节点之间的距离数据，并发送给pc端进行数据解算处理，得到运动平台的第二定位数据；
10.步骤3、所述运动平台上pc端利用滤波器对所述第一定位数据和第二定位数据进行融合，得到最终的定位数据。
11.本发明所述的一种基于uwb和imu信息融合的室内定位方法的特点也在于，所述步骤1包括：
12.步骤1.1、建立坐标系：
13.以待测载体的重心为原点o，以待测载体的移动方向为y轴，以待测载体的右侧方向为x轴，按照右手定则确定z轴，从而建立运动平台的载体坐标系，记为b系；
14.步骤1.2、利用所述imu传感器得到所述运动平台在所述b系下i时刻的三轴加速度从而利用式(1)得到导航坐标系即n系下的三轴加速度
[0015][0016]
式(1)中，表示b系下转到n系下的旋转矩阵，gn表示n系下的世界坐标系的重力值
[0017]
步骤1.3、利用式(2)对n系下三轴加速度进行积分，得到运动平台在i时刻的速度[v
xi v
yi v
zi
]
t
，利用式(3)对[v
xi v
yi v
zi
]
t
积分得到位置[x'
i y'
i z'i]
t
，并作为所述第一定位数据：
[0018][0019][0020]
式(2)和式(3)中，[v
x i-1 v
y i-1 v
z i-1
]
t
表示运动平台在i-1时刻的速度，[x'
i-1 y'
i-1 z'
i-1
]
t
，表示运动平台在i-1时刻的位置。
[0021]
所述步骤2中的第二定位数据是按如下步骤获得：
[0022]
步骤2.1、以室内定位区域的外接矩形中的一个顶点为原点，与所述原点相邻的两条直角边分别为x轴和y轴，并按照右手定则得到z轴，从而构建室内定位坐标系；
[0023]
步骤2.2、在所述室内定位坐标系下，测量所有基站的坐标值，令(xn,yn,zn)表示第n个uwb基站节点在室内定位坐标系下的位置坐标；
[0024]
步骤2.3、利用式(4)构建所述uwb定位标签与每个uwb基站节点的关系方程：
[0025][0026]
式(4)中，(x"i,y"i,z"i)表示uwb定位标签的位置坐标；rn表示所述uwb定位标签到
第n个uwb基站节点的距离；
[0027]
步骤2.4、利用最小二乘法对所述关系方程进行解算，得到式(5)：
[0028][0029]
式(5)中，c表示与基站坐标值有关的常数；
[0030]
步骤2.5、利用式(6)得到所述uwb定位标签的位置坐标[x"
i y"
i z"i]
t
并作为当前i时刻下的第二定位数据：
[0031][0032]
式(6)中，f表示n个uwb基站节点的坐标值之差的矩阵，并有：
[0033][0034]
所述步骤3包括，利用滤波器对所述第一定位数据和第二定位数据进行融合：
[0035]
步骤3.1、利用式(8)和式(9)建立运动平台非线性方程组：
[0036]
xi＝f(x
i-1
,u
i-1
,w
i-1
)
ꢀꢀ
(8)
[0037]
zi＝h(xi,vi)
ꢀꢀꢀ
(9)
[0038]
式(8)和式(9)中，f(.)表示关于状态方程的非线性函数,h(.)表示关于观测方程的非线性函数,xi表示i时刻运动平台状态，x
i-1
表示i-1时刻运动平台状态，u
i-1
表示i-1时刻外界对系统的作用，w
i-1
为i-1时刻的过程噪声，zi表示i时刻的观测值，vi表示i时刻的观测噪声；
[0039]
步骤3.2、利用式(10)和式(11)分别设置初始状态的系统均值u0和初始时刻的误差协方差矩阵p0；
[0040]
u0＝e[x0]
ꢀꢀ
(10)
[0041]
p0＝var[x0]
ꢀꢀ
(11)
[0042]
式(10)和式(11)中，e表示求取均值，x0表示初始时刻的运动平台状态，var表示求取误差协方差；
[0043]
步骤3.3、当i》1时，用imu传感器定位数据建立滤波器状态方程，并用uwb定位信息作为滤波器观测方程：
[0044]
步骤3.3.1、时间更新：
[0045]
利用式(12)和式(13)计算先验误差协方差：
[0046][0047][0048]
式(12)和式(13)中，表示i时刻未优化的预测状态，表示i-1时刻优化后的
预测状态，表示i时刻的先验误差协方差，ai表示i时刻的状态转移矩阵，p
i-1
表示i-1时刻的误差协方差，wi表示i时刻的噪声驱动矩阵，q
i-1
表示i-1时刻的过程噪声协方差矩阵，t表示转置；
[0049]
步骤3.3.2、状态更新：
[0050]
利用式(14)计算i时刻的卡尔曼增益ki，并利式(15)得到i时刻的最优估计值同时利用式(16)得到更新后的误差协方差pi：
[0051][0052][0053][0054]
式(14)～式(16)中，ki表示i时刻的卡尔曼增益，hi表示i时刻的观测矩阵，vi表示i时刻的观测噪声驱动矩阵，ri表示i时刻的观测噪声协方差矩阵，表示i时刻优化后的预测状态，zi表示i时刻的观测值，pi为i时刻的误差协方差；
[0055]
步骤3.3.3、将i 1赋值给i后，判断i是否达到最后采样时间，若是，则表示得到最新融合后的定位信息，否则将i时刻的最优估计值和更新后的误差协方差pi带入步骤3.3.1和步骤3.3.2，并得到i 1时刻的预测状态。
[0056]
与现有技术相比，本发明方法的有益效果体现在：
[0057]
本发明利用改进扩展卡尔曼滤波算法将imu和uwb定位信息进行融合，通过uwb定位数据测量来约束imu定位方案中存在长时间发散的问题；同时通过imu测量数据作为预测来消除uwb系统中短时间内受到非视距和多径效应环境影响时的系统大幅度震荡。本发明解决单一传感器定位不精确的问题，并克服非视距和多径效应带来的影响。相较于经典uwb/imu融合算法，本发明采用扩展卡尔曼滤波算法，具有计算量简单时效性强的特点，减少迭代带来的误差，满足了复杂室内环境下长时间稳定且高精度的定位需求。
附图说明
[0058]
图1是本发明一种基于uwb和imu信息融合的室内定位方法系统示意图；
[0059]
图2是uwb和imu组合定位流程图；
[0060]
图3是运动平台的空间变换关系；
[0061]
图4是uwb的到达时间(tof)测距原理图。
具体实施方式
[0062]
本实施例中，如图1和图2所示，一种基于uwb和imu信息融合的室内定位方法，是在室内定位区域内设置有运动平台，在运动平台上设置有imu传感器和pc端，在imu传感器集成有一个未知位置的uwb定位标签；在运动平台周围设置有n个已知位置的uwb基站节点；基站的安装点应离墙角距离大于1.5米，离墙面距离大于1.0米，且保证基站安装在同一水平面。其中，imu传感器包括：三轴加速度计、三轴陀螺仪；室内定位方法是将imu测量值(三轴加速度计数据、三轴陀螺仪数据)和uwb定位信息(uwb定位标签和uwb基站相对位置关系数据)作为输入信息；imu测量值作为滤波器的预测，来消除uwb系统中短时间内受到非视距和多径效应环境影响时的系统大幅度震荡；uwb的测量值作为滤波器测量更新，通过uwb定位
数据测量来约束imu定位方案中存在长时间发散的问题；将imu传感器和uwb定位标签固定在运动平台上，采用滤波器融合uwb和imu传感器信息得到最新定位，具体的说，是按如下步骤进行：
[0063]
步骤1、利用imu传感器采集运动平台的三轴加速度以及三轴角速度数据，并发送给pc端进行数据解算处理，得到运动平台的第一定位数据；
[0064]
步骤1.1、建立坐标系：以待测载体的重心为原点o，以待测载体的移动方向为y轴，以待测载体的右侧方向为x轴，按照右手定则确定z轴，从而建立运动平台的载体坐标系，记为b系；
[0065]
步骤1.2、利用imu传感器得到运动平台在b系下i时刻的三轴加速度从而利用式(1)得到导航坐标系即n系下的三轴加速度
[0066][0067]
式(1)中，表示载体坐标系转到导航坐标系下的旋转矩阵，gn表示世界坐标系的重力值；如图3所示，载体的航向角为yaw(用ψ表示)，俯仰角pitch(用θ表示)，翻滚角roll(用φ表示)，式(1)中，c表示cos函数，s表示sin函数。
[0068]gn
＝[0 0 g]
t
[0069][0070]
步骤1.3、利用式(2)对n系下三轴加速度进行积分，得到运动平台在i时刻的速度[v
xi v
yi v
zi
]
t
，用式(3)对[v
xi v
yi v
zi
]
t
积分得到位置[x'
i y'
i z'i]
t
，作为第一定位数据：
[0071][0072][0073]
式(2)和式(3)中，[v
x i-1 v
y i-1 v
z i-1
]
t
表示运动平台在i-1时刻的速度，[x'
i-1
y'
i-1
z'
i-1
]
t
，表示运动平台在i-1时刻的位置。
[0074]
步骤2、获取uwb定位标签分别与n个uwb基站节点之间的距离数据，并发送给pc端进行数据解算处理，得到运动平台的第二定位数据；
[0075]
步骤2.1、以室内定位区域的外接矩形中的一个顶点为原点，与原点相邻的两条直角边分别为x轴和y轴，并按照右手定则得到z轴，从而构建室内定位坐标系；
[0076]
步骤2.2、在室内定位坐标系下，测量所有基站的坐标值，令(xn,yn,zn)表示第n个uwb基站节点在室内定位坐标系下的位置坐标；接通所有uwb基站的电源并启动uwb定位标签进入测距工作状态，此时uwb定位标签依次测量其到所有信号范围内的基站的距离，并通
过串口输入到pc端。如图4所示是uwb的到达时间(tof)测距原理图。其中t1表示定位标签向基站发送信号的时间与定位标签接收到定位基站返回信号的时间差。t2表示定位基站接受信号的时间与基站向标签返回信号的时间差。可以通过t1和t2和光速c计算定位标签和定位基站的距离。具体如式(4)所示：
[0077][0078]
r＝c*tof c＝3*10
8 m/s
ꢀꢀꢀ
(4)
[0079]
步骤2.3、利用式(5)构建uwb定位标签与每个uwb基站节点的关系方程：
[0080][0081]
式(5)中，(x"i,y"i,z"i)表示uwb定位标签的位置坐标；rn表示uwb定位标签到第n个uwb基站节点的距离；
[0082]
步骤2.4、利用最小二乘法对关系方程进行解算，得到式(6)，其中，c表示与基站坐标值有关的常数：
[0083][0084]
步骤2.5、利用式(7)得到uwb定位标签的位置坐标[x"
i y"
i z"i]
t
并作为当前i时刻下的第二定位数据：
[0085][0086]
式(7)中，f表示n个uwb基站节点的坐标值之差的矩阵，并有：
[0087][0088]
步骤3、运动平台上pc端利用滤波器对第一定位数据和第二定位数据进行融合，得到最终的定位数据。
[0089]
步骤3.1、利用式(9)和式(10)建立运动平台非线性方程组：
[0090]
xi＝f(x
i-1
,u
i-1
,w
i-1
)
ꢀꢀ
(9)
[0091]
zi＝h(xi,vi)
ꢀꢀꢀ
(10)
[0092]
式(9)和式(10)中，f(.)表示关于状态方程的非线性函数,h(.)表示关于观测方程的非线性函数,xi表示i时刻运动平台状态，x
i-1
表示i-1时刻运动平台状态，u
i-1
表示i-1时刻外界对系统的作用，w
i-1
为i-1时刻的过程噪声，zi表示i时刻的观测值，vi表示i时刻的观测噪声；
[0093]
对式(9)非线性方程f(.)在处线性化，泰勒级数展开得到式(11)：
[0094][0095][0096][0097]
式(11)～式(13)中，表示i-1时刻优化后的预测状态，ai表示i时刻的状态转移矩阵，式(12)右侧是f(.)的雅可比矩阵；wi表示i时刻的过程噪声矩阵，式(13)右侧是f对w的偏导，令并对式(10)非线性方程z(.)在处线性化：
[0098][0099][0100][0101]
式(14)～式(16)中，hi表示i时刻的观测矩阵，式(15)右侧是h(.)的雅可比矩阵，vi表示i时刻的观测噪声驱动矩阵，式(16)右侧是h对v的偏导；
[0102]
步骤3.2、利用式(17)和式(18)分别设置初始状态的系统均值u0和初始时刻的误差协方差矩阵p0；
[0103]
u0＝e[x0]
ꢀꢀ
(17)
[0104]
p0＝var[x0]
ꢀꢀ
(18)
[0105]
式(17)和式(18)中，e表示求取均值，x0表示初始时刻的运动平台状态，var表示求取误差协方差；
[0106]
步骤3.3、当i》1时，用imu传感器定位数据建立滤波器状态方程，并用uwb定位信息作为滤波器观测方程：
[0107]
步骤3.3.1、时间更新：
[0108]
利用式(19)和式(20)计算先验误差协方差：
[0109][0110][0111]
式(19)和式(20)中，表示i时刻未优化的预测状态，表示i-1时刻优化后的预测状态，表示i时刻的先验误差协方差，ai表示i时刻的状态转移矩阵，p
i-1
表示i-1时刻的误差协方差，wi表示i时刻的噪声驱动矩阵，q
i-1
表示i-1时刻的过程噪声协方差矩阵，t表示转置；
[0112]
步骤3.3.2、状态更新：
[0113]
利用式(21)计算i时刻的卡尔曼增益ki，并利式(22)得到i时刻的最优估计值同时利用式(23)得到更新后的误差协方差pi：
[0114][0115][0116]
[0117]
式(21)～式(23)中，ki表示i时刻的卡尔曼增益，hi表示i时刻的观测矩阵，vi表示i时刻的观测噪声驱动矩阵，ri表示i时刻的观测噪声协方差矩阵，表示i时刻优化后的预测状态，zi表示i时刻的观测值，pi为i时刻的误差协方差；
[0118]
步骤3.3.3、将i 1赋值给i后，判断i是否达到最后采样时间，若是，则表示得到最新融合后的定位信息，否则将i时刻的最优估计值和更新后的误差协方差pi带入步骤3.3.1和步骤3.3.2，并得到i 1时刻的预测状态。