一种基于零空间维护算法的紧耦合UWB-IMU机器人自主定位方法技术

本发明专利技术公开的基于零空间维护算法的紧耦合UWB

【技术实现步骤摘要】
一种基于零空间维护算法的紧耦合UWB
‑
IMU机器人自主定位方法


[0001]本专利技术属于移动机器人自主导航与定位领域，具体涉及一种基于FEJ
‑
EKF实现UWB与惯导器件信息融合的机器人自主定位方法。

技术介绍

[0002]在过去的几十年里，全球定位系统(GPS)已经被广泛地应用于移动机器人、飞行器和地面车辆的定位和导航任务中。然而，多路径和非视线(NLOS)问题限制了搭载GPS模块的机器人在仓库、机场、工厂等室内环境中的应用。同时定位和建图(SLAM)技术，即提供姿态估计的同时构建环境地图，已经成为室内导航的一种替代方案。近年来，惯性测量单元(IMU)已被证明可以与激光传感器，视觉传感器实现紧密融合以提升机器人自主定位效果。然而基于相机和激光雷达的机器人自主定位方法在前端特征提取上都需要大量的计算资源，而UWB技术因其小尺寸、低成本和低功率特性而为室内导航提供了另一种可行的方案。
[0003]由于UWB节点之间的模块间距离测量有噪声，仅依赖距离测量很难实现高精度定位。同时，在基于UWB进行状态估计时需要环境中的UWB锚点(anchor)位置信息，现有的解决方案通常忽略了该信息，误差大的同时为SLAM系统的部署带来了不便。

技术实现思路

[0004]为了解决机器人在室内环境中无法实现低成本自主定位的问题，本专利技术的目的是提供一种基于零空间维护算法的紧耦合UWB
‑
IMU机器人自主定位方法，通过扩展卡尔曼滤波将UWB信息和IMU融合进行位姿估计，以提升定位精度的UWB
‑
IMU自主定位精度，且能够在线估计UWB锚点的位置。此外，通过引入了FEJ(First Estimate Jacobian)
‑
EKF方法降低SLAM系统不一致性，通过在相同的状态点线性化展开计算可观性矩阵，维护可观性矩阵的零空间不变，保证系统能观性与理想系统一致，提高UWB
‑
IMU机器人自主定位的能观性。
[0005]本专利技术的目的是通过下述技术方案实现的。
[0006]本专利技术公开的基于零空间维护算法的紧耦合UWB
‑
IMU机器人自主定位方法，包括以下步骤：
[0007]步骤一：选定估计系统所维护的误差状态变量。整个UWB
‑
惯性估计系统的状态向量X
u
包括IMU的状态X
I
和一组n个锚点的位置状态X
A
：
[0008][0009]其中表示从全局坐标系到IMU坐标系的旋转的单位四元数，
G
p
I
和
G
v
I
表示IMU在全局坐标系下的位置和速度。b
g
和b
a
表示陀螺仪和加速度计的零偏不稳定性。则表示环
境中第i个UWB锚点在全局坐标系中的位置。若要使用误差状态卡尔曼滤波，需要给出IMU状态误差量：
[0010][0011]其中，和分别是机器人位置、速度、陀螺仪零偏和加速度计零偏的加性误差；是一个旋转向量，也称轴角，是一个旋转向量，也称轴角，与表示姿态的四元数的误差等价：
[0012][0013]其中表示轴角中的旋转轴，表示沿旋转轴转动角度。
[0014]其中锚点位置的初值通过最小二乘法给定：
[0015][0016]其中
[0017][0018][0019]其中是uwb标签的位置，是第i个uwb锚点的位置；是来自于第i个锚点的距离量测；d
bias
则是距离量测的零偏值。
[0020]步骤二：基于IMU进行误差状态预测。基于IMU信息，估计状态的前向传播如下表示：
[0021][0022]其中t
k+1∣k
表示根据时刻k信息得出的k+1时刻的预测值。对状态变量的雅可比矩阵Φ
I
(t
k+1∣k
)和G
Ik
为：
[0023][0024][0025]其中其中表示从时刻k到k+1的旋转矩阵。则是三维李群SO(3)上的右雅可比矩阵。另外，是与向量q对应的反对称矩阵。
[0026]从时刻k到k+1的状态估计协方差表示为：
[0027][0028]其中Q
d
为离散时间噪声协方差矩阵：
[0029][0030]步骤三：使用UWB信息进行流形上的状态更新。UWB获得的是一维距离信息，其通过TOF原理进行测距，获得在环境中布置的UWB锚点与机器人上固连的UWB标签节点之间的距离信息：
[0031][0032]其中||
·
||表示欧氏距离，和则是在全局坐标系中的第i个标签节点与第j个锚点。该距离信息表示如下：
[0033][0034]其中表示UWB标签节点与锚点之间的欧氏距离，函数则与IMU状态建立联系：
[0035][0036]其中表示IMU坐标系中的第i个标签节点的位置矢量。此式对IMU位置，姿态及环境中的UWB锚点位置求雅可比可得：
[0037][0038][0039][0040]机器人上固连m个标签节点，环境中放置的n个UWB锚点，构建如下UWB测量模型：
[0041]z
d,k
＝h(x
k
)+η
d,k (18)
[0042]其中是堆叠起来的UWB测量；将模型线性化得：
[0043][0044]其中雅可比矩阵H
k+1
为：
[0045][0046]根据式(15)(16)和(17)计算得出：
[0047][0048]给定传播状态和P
k+1∣k
，得到残差及其协方差：
[0049][0050]执行一个标准的EKF更新过程来校正估计的状态和协方差：
[0051][0052][0053]P
k+1
＝P
k+1∣k
‑
K
k+1
H
k+1
P
k+1∣k (25)
[0054]步骤四:对基于EKF的SLAM(定位和建图)进行可观性分析，分析结果表明，基于EKF的SLAM系统存在不一致性问题。
[0055]从时刻k到k+n的可观性矩阵定义如下：
[0056][0057]其中H
i
,i∈[k,k+n]是测量雅可比矩阵；Φ
i
是状态传播矩阵。为了分析不一致性问题，以下分为两步对比说明基于EKF的SLAM系统存在不一致性问题。
[0058]S4
‑
1:首先分析理想情况下EKF
‑
SLAM的可观测性，其中在计算雅可比矩阵H
i
和Φ
i
时使用状态真值。在理想情况下，第m个UWB锚点在时刻i∈[k,k+n]的距离测量对应的可观矩阵为：
[0059][0本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于零空间维护算法的紧耦合UWB
‑
IMU机器人自主定位方法，其特征在于：包括以下步骤，步骤一：选定估计系统所维护的误差状态变量；整个UWB
‑
惯性估计系统的状态向量X
u
包括IMU的状态X
I
和一组n个锚点的位置状态X
A
：其中表示从全局坐标系到IMU坐标系的旋转的单位四元数，
G
p
I
和
G
v
I
表示IMU在全局坐标系下的位置和速度；b
g
和b
a
表示陀螺仪和加速度计的零偏不稳定性；则表示环境中第i个UWB锚点在全局坐标系中的位置；若要使用误差状态卡尔曼滤波，需要给出IMU状态误差量：其中，
G
p
I
、
G
v
I
、b
g
和b
a
分别是机器人位置、速度、陀螺仪零偏和加速度计零偏的加性误差；是一个旋转向量，也称轴角，是一个旋转向量，也称轴角，与表示姿态的四元数的误差等价：其中k表示轴角中的旋转轴，表示沿旋转轴转动角度；其中锚点位置的初值通过最小二乘法给定：其中其中其中是uwb标签的位置，是第i个uwb锚点的位置；是来自于第i个锚点的距离量测；d
bias
则是距离量测的零偏值；步骤二：基于IMU进行误差状态预测；基于IMU信息，估计状态的前向传播如下表示：
其中t
k+1∣k
表示根据时刻k信息得出的k+1时刻的预测值；对状态变量的雅可比矩阵Φ
I
(t
k+1∣k
)和为：为：其中其中表示从时刻k到k+1的旋转矩阵；J
r
(δθ)则是三维李群SO(3)上的右雅可比矩阵；另外，是与向量q对应的反对称矩阵；从时刻k到k+1的状态估计协方差表示为：其中Q
d
为离散时间噪声协方差矩阵：步骤三：使用UWB信息进行流形上的状态更新；UWB获得的是一维距离信息，其通过TOF原理进行测距，获得在环境中布置的UWB锚点与机器人上固连的UWB标签节点之间的距离信息：
其中||
·
||表示欧氏距离，和则是在全局坐标系中的第i个标签节点与第j个锚点；该距离信息表示如下：其中表示UWB标签节点与锚点之间的欧氏距离，函数则与IMU状态建立联系：其中表示IMU坐标系中的第i个标签节点的位置矢量；此式对IMU位置，姿态及环境中的UWB锚点位置求雅可比可得：的UWB锚点位置求雅可比可得：的UWB锚点位置求雅可比可得：机器人上固连m个标签节点，环境中放置的n个UWB锚点，构建如下UWB测量模型：z
d,k
＝h(x
k
)+η
d,k
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)其中是堆叠起来的UWB测量；将模型线性化得：其中雅可比矩阵H
k+1
为：根据式(15)(16)和(17)计算得出：
给定传播状态x
k+1∣k
和P
k+1∣k
，得到残差及其协方差：执行一个标准的EKF更新过程来校正估计的状态和协方差：x
k+1
＝x
...

【专利技术属性】
技术研发人员：王佳楠，李春雨，张璟泽，罗子娟，李雪松，单家元，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：