基于UWB无线定位与IMU融合的导航定位系统及方法与流程

基于uwb无线定位与imu融合的导航定位系统及方法
技术领域
1.本发明属于导航定位技术领域，尤其涉及一种基于uwb无线定位与imu融合的导航定位系统及方法。


背景技术：

2.全球卫星导航系统(global navigation satellite system,gnss) 可以为用户提供全球范围内的自主定位导航服务，如今已广泛的应用于各种领域。但由于受卫星时钟误差、星历数据误差、电离层影响、多径影响等诸多干扰，传统的卫星定位只能达到10m左右的定位精度，远无法满足一些对定位精度要求高的场合。载波相位差分技术即rtk(real time kinematic)的出现，将卫星定位精度提高到厘米级，进一步拓宽了卫星定位技术的使用场景，但rtk成本较高，另外也无法在室内环境使用。随着区域内定位服务需求的日益增加，局部定位服务( location position server )成为了研究的热点，常见的有wifi指纹技术、uwb技术、蓝牙技术等。其中uwb技术以其高传输率，高穿透力，及较高的定位精度等优势已经在仓库，隧道等地方的人员、物资定位取得广泛的应用，但由于其定位效果在非视距环境下较差，极大的限制了其使用范围如各种移动机器人的导航定位，存在遮挡、多径干扰、信号不稳定导致定位误差较大的问题。


技术实现要素：

3.本发明的目的是提供一种基于uwb无线定位与imu融合的导航定位系统及方法，采用拓展卡尔曼滤波器紧耦合的方式融合uwb标签数据和imu数据，有效减小uwb非视距误差对定位精度产生的影响，，并且采用双定位标签融合陀螺仪得到全局可靠航向角数据，为各类室内外移动机器人导航定位提供可靠的位置和航向角数据支持，解决单一标签没有角度信息，无法为移动机器人提供航向角的问题。
4.本发明解决其技术问题是通过以下的技术方案实现的：
5.本发明提供了一种基于uwb无线定位与imu融合的导航定位系统,包括：
6.安装于指定工作区域内的至少四个uwb基站；
7.由两个uwb定位标签和两个imu组成的标签装置；其中一个所述uwb定位标签与一个imu以刚体连接方式安装在一起组成第一复合标签，另外一个所述uwb定位标签和imu以刚体连接安装在一起组成第二复合标签，所述第一复合标签与第二复合标签以设定距离间隔安装在固定组件上组成所述标签装置；
8.主控装置，用于接收所述标签装置数据，采用卡尔曼滤波器紧耦合的方式对uwb标签数据和imu数据进行实时融合，得到当前位置数据和航向角数据；
9.由所述标签装置及主控装置组成的定位装置。
10.进一步地，所述第一复合标签与第二复合标签之间的距离为0.5m~1.0m。
11.本发明还提供了一种应用上述导航定位系统进行导航定位的方法，包括如下步骤：
12.步骤1，在多个uwb基站中选取其中一个作为参考坐标原点，精确测量其他基站的相对位置坐标，从而建立参考坐标系；
13.步骤2，获取uwb定位标签到各基站的uwb测距信息；获取imu的加速度计和陀螺仪信息进行姿态解算，得到标签装置的imu线性加速度信息；
14.步骤3，采用紧耦合的方式构建卡尔曼滤波器模型，对uwb测距信息和imu线性加速度信息进行融合；所述卡尔曼滤波器模型包括位置卡尔曼滤波器模型及角度卡尔曼滤波器模型；
15.步骤4，启动布置于工作区域内的定位装置，进行位置卡尔曼滤波器模型初始化及角度卡尔曼滤波器模型初始化，得到初始位置坐标和航向角，并进入位置跟踪状态及角度跟踪状态；
16.步骤5，初始化完成后，实时获取uwb定位标签和imu的测量数据，并对位置卡尔曼滤波器模型和角度卡尔曼滤波器模型进行实时更新，得到当前位置数据和航向角数据。
17.进一步地，所述卡尔曼滤波器模型用于：系统状态预测，计算公式为： u(k)
18.上式中，a为系统的状态转移矩阵，表示本次系统的最优估计值,在本系统构建的公式中指由位置、速度、加速度计误差一同构成的矩阵，表示系统的控制矩阵，表示系统的控制量，本系统中指加速度矩阵；系统协方差预预测，计算公式为:上式中，a为系统的状态转移矩阵，为本次系统先验协方差最优化估计值，为上一次系统的先验协方差最优化估计值，ｑ为系统的过程噪声协方差矩阵；系统的卡尔曼增益计算，计算公式为:上式中，表示k时刻系统的卡尔曼增益，h为系统的测量矩阵，r为测量噪声协方差矩阵，；滤波器更新，最优状态估计公式为:上式中，表示k时刻的系统最优状态估计值，表示k时刻的系统测量值，为k-1时刻的系统最优状态估计值,为k-2时刻系统的最优状态估计值，为k时刻系统的卡尔曼增益，h为系统的测量矩阵；滤波器更新，最优状态协方差公式：
上式中，为k时刻系统后验估计协方差，为k时刻系统的卡尔曼增益，h为系统的测量矩阵，为k-1时刻系统的后验协方差最优化估计结果。
19.借由上述方案，通过基于uwb无线定位与imu融合的导航定位系统及方法，可以同时提供精确的位置信息和航向角信息，可广泛应用移动机器人的室内外导航。
20.上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，并可依照说明书的内容予以实施，以下以本发明的较佳实施例详细说明如后。
附图说明
21.图1是本发明基于uwb无线定位与imu融合的导航定位系统的结构示意图；
22.图2是本发明基于uwb无线定位与imu融合的导航定位系统的架构图；
23.图3是本发明基于uwb无线定位与imu融合的导航定位方法的流程图。
具体实施方式
24.下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实例用于说明本发明，但不用来限制本发明的范围。
25.参图1、图2所示，本实施例提供了一种基于uwb无线定位与imu融合的导航定位系统,包括：
26.安装于指定工作区域内的至少四个uwb基站11（本实施例分别为uwb基站1、uwb基站2、uwb基站3、uwb基站4）；
27.由两个uwb定位标签和两个imu组成的标签装置；其中第一uwb定位标21签与第一imu23以刚体连接方式安装在一起组成第一复合标签，第二uwb定位标签22和第二imu24以刚体连接安装在一起组成第二复合标签，第一复合标签与第二复合标签以设定距离间隔安装在固定组件上组成标签装置；
28.主控装置3，用于接收标签装置数据，采用卡尔曼滤波器紧耦合的方式对uwb标签数据和imu数据进行实时融合，得到当前位置数据和航向角数据；
29.由标签装置及主控装置3组成的定位装置。
30.该基于uwb无线定位与imu融合的导航定位系统，使用uwb测距信息作为观测量，使用imu信息作为运动估计量，以紧耦合的方式设计拓展卡尔曼滤波器，对标签的位置进行最优化估计。可以有效解决uwb定位受遮挡、多路劲干扰产生测距误差，影响定位精度的问题。标签装置采用双定位标签设计，利用标签与基站之间的向量关系，可以测量标签装置的航向角，设计卡尔曼滤波器融合该航向角数据和陀螺仪数据可以有效消除陀螺仪测量产生的累积误差，从而提供一个全局精确可靠的航向角。该系统可以同时提供精确的位置信息和航向角信息，可广泛应用移动机器人的室内外导航。
31.在本实施例中，第一复合标签与第二复合标签之间的距离为0.8m。
32.参图3所示，本实施例还提供了一种应用上述导航定位系统进行导航定位的方法，包括如下步骤：
33.步骤1，在多个uwb基站中选取其中一个作为参考坐标原点，精确测量其他基站的相对位置坐标，从而建立参考坐标系。
34.参图1所示，按照uwb基站布置原则，在指定的场地范围内安装至少４个uwb基站，获取所有基站的坐标,在场地范围内设置坐标原点，以及坐标轴方向，求取坐标转移矩阵r,建立基站参考坐标系r(x,y,z)。
35.其中基站的安装应当遵守以下几个原则：1）基站的布置形状应尽量接近长宽比为1:1的矩形，最大不应超过３: 1。2）基站安装点离墙角距离应该大于1.0m，离墙面距离应该大于0.5m。3）基站尽量安装于同一平面，并确保基站平面和标签工作平面之间的距离大于1.0m。
36.在基站安装完成之后，需要对基站坐标进行精确的测绘。一般采用全站仪来完成该工作，在测量得到所有基站的坐标点之后,在场地范围内设置坐标原点，以及坐标轴方向，求取坐标转移矩阵r,建立基站参考坐标系r(x,y,z)。
37.步骤2，获取uwb定位标签到各基站的uwb测距信息；获取imu的加速度计和陀螺仪信息进行姿态解算，得到标签装置的imu线性加速度信息。
38.接通所有基站的电源，确保基站进入测距工作状态，将标签装置置于基站包络范围内，启动标签装置，此时系统将自动开启twr测距功能，标签装置将会依次测量其到所有信号范围内的基站的距离，并通过串口将距离信息传输到主控装置。
39.采取twr(two-way_ranging)方式创建距离测量系统，获取uwb标签和各个基站之间的距离,其基本原理如下。基站a和标签是b两个uwb模块。测距首先由a发起，b收到之后再发回一个相应(responds)，a再接收这个相应，完成一次测距，在a和b每一次发送数据和收到数据的时候。都要记录当前时间戳。这样，通过时间戳相减，就可以得到传输时间差,公式如下:
40.其中即为电磁波从基站a到标签b飞行时间的，有了该值，再乘以光速c就可以得到距离d。
41.步骤3，采用紧耦合的方式构建卡尔曼滤波器模型，对uwb测距信息和imu线性加速度信息进行融合；所述卡尔曼滤波器模型包括位置卡尔曼滤波器模型及角度卡尔曼滤波器模型。
42.建立uwb距离伪观测方程、建立imu运动模型方程、构建拓展卡尔曼滤波器，对uwb测距信息和imu得到的线性加速度信息进行紧耦合。
43.当接收到串口传回来的距离信息之后，主控装置将读取imu的实时数据，并对uwb测距信息和imu线性加速度信息进行融合。
44.步骤4，启动布置于工作区域内的定位装置，进行位置卡尔曼滤波器模型初始化及角度卡尔曼滤波器模型初始化，得到初始位置坐标和航向角，并进入位置跟踪状态及角度跟踪状态。
45.将定位装置放于待定区域内，启动装置，待主控启动完成，系统将会自动进入初始化模式，计算初始位置坐标和航向角；初始化完成后，即可进行动态数据输出。
46.通常情况下主控装置将先初始化位置卡尔曼滤波器模型，此时应该保持定位装置不动，待初始化完成，定位装置即可输出初始位置，并进入位置跟踪状态。
47.待位置初始化完成之后，主控装置将加载角度卡尔曼滤波器模型，建立双标签角度观测方程、陀螺仪测量方程，并融合标签观测角和陀螺仪测量得到的旋转角，融合标签观测角和陀螺仪测量得到的旋转角，得出当前方向角。
48.主控装置初始化角度卡尔曼滤波器模型，此时应该保持定位装置不动，待初始化完成，定位装置即可输出初始角度，并进入角度跟踪状态。
49.步骤5，初始化完成后，实时获取uwb定位标签和imu的测量数据，并对位置卡尔曼滤波器模型和角度卡尔曼滤波器模型进行实时更新，得到当前位置数据和航向角数据。（对上述测量系统数据和卡尔曼滤波器输出进行实时更新，并输出当前融合得到的位置数据和航向角数据）
50.在位置和角度都初始化完成之后，定位装置将进入更新状态。此时定位装置将实时接收uwb标签和imu的测量数据，并对位置卡尔曼滤波器和角度卡尔曼滤波器进行实时更新，并通过串口输出当前融合解算得到的位置数据和航向角数据。
51.在本实施例中，所述卡尔曼滤波器模型用于：系统状态预测，计算公式为： u(k)上式中，a为系统的状态转移矩阵，表示本次系统的最优估计值,在本系统构建的公式中指由位置、速度、加速度计误差一同构成的矩阵，表示系统的控制矩阵，表示系统的控制量，本系统中指加速度矩阵；系统协方差预预测，计算公式为:上式中，a为系统的状态转移矩阵，为本次系统先验协方差最优化估计值，为上一次系统的先验协方差最优化估计值,ｑ为系统的过程噪声协方差矩阵；系统的卡尔曼增益计算，计算公式为:上式中，表示k时刻系统的卡尔曼增益，h为系统的测量矩阵，r为测量噪声协方差矩阵，；滤波器更新，最优状态估计公式为:上式中，表示k时刻的系统最优状态估计值，表示k时刻的系统测量值，为k-1时刻的系统最优状态估计值,为k-2时刻系统的最优状态估计值，为k时刻系统的卡尔曼增益，h为系统的测量矩阵;滤波器更新，最优状态协方差公式：
上式中，为k时刻系统的后验估计协方差，为k时刻系统的卡尔曼增益，h为系统的测量矩阵，为k-1时刻系统的后验协方差最优化估计结果。
52.本发明采取紧耦合的方式对imu数据和uwb数据进行实时融合，imu瞬时性能的优越性可以在极大程度上逆补uwb易受非视距误差影响的先天不足，有效提高系统的定位精度和鲁棒性。本发明创造式地采用双标签系统构成标签装置的整体结构，由两个标签位置构成的向量以及位置状态约束关系可以得出标签装置的观测方位角，再采用卡尔曼滤波器融合和陀螺仪数据便可以得到一个实时性高，全局精确可靠的方位角，弥补了传统的uwb定位系统无方位角信息，无法为移动机器人提供航向角的缺陷。
53.以上所述仅是本发明的优选实施方式，并不用于限制本发明，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。