CN112683263A - 一种基于改进模型的uwb/imu/odom多传感器数据融合移动机器人定位方法 - Google Patents

Abstract

本发明提出一种基于改进模型的UWB/IMU/ODOM多传感器数据融合移动机器人定位方法，首先建立机器人模型，以机器人x坐标，y坐标，偏航角和速度作为状态量，以机器人加速度和角速度为控制量，建立状态方程；其次针对ODOM和UWB数据构建两个观测方程；之后进行EKF数据融合，先完成状态估计，再进行UWB的观测校正，最后进行ODOM的观测二次校正，完成状态量估计和协方差估计。本发明完成了对机器人UWB/IMU/ODOM数据的融合，通过UWB闭环特性弥补IMU和ODOM漂移问题，通过IMU和ODOM短时间精度提高了UWB绝对坐标定位精度。本发明相对于其他机器人融合定位方法，使用了改进机器人模型，使用了速度、加速度、角速度、位置、角度5种信息，通过更加全面的信息提高定位精度。

Description

一种基于改进模型的UWB/IMU/ODOM多传感器数据融合移动机 器人定位方法

技术领域

本发明涉及一种基于改进模型的UWB/IMU/ODOM多传感器数据融合移动机器人定位方法，属于机器人建模与定位以及多传感器融合领域。

背景技术

移动机器人的定位方法可以分成两大种类：

1、自主定位

机器人自主定位就是带有各种状态信息传感器的机器人在本机上通过传感器数据计算出自身位置。

(1)如机器人采用里程计(odom)或者惯性测量单元(imu)，测出瞬时速度、角速度、加速度等机器人动态状态，通过积分完成位置测量；

(2)带有视觉传感器或者激光传感器的机器人可以通过地图特征匹配的方式自主匹配自身在地图中的位置，完成自身相对地图的定位；

2、辅助定位

机器人通过与外界设备的信息交互完成定位，定位信息由外界发送，机器人接收。带有信号接收装置的机器人可以通过接收基站或者使用GPS，直接获取自身的绝对位置。

两类方法中，自主定位由于信息获取直接，信息接收时损失较少，往往短时间内定位更加准确。但是如odom以及imu这种传感器定位是一个开环系统，每一次定位都会产生一定的误差，长期会造成很大的累计误差，造成定位漂移；而通过地图匹配的方式定位虽然是一个闭环系统，不存在漂移问题，但是需要预先建图，并且会受到地图中的动态障碍物干扰，以及特征误匹配等问题，也存在一定缺陷。

而辅助定位是一个闭环系统，不存在漂移问题，但是往往精度不能达到要求，也易受到通信干扰。

目前主要使用的机器人的定位方法基本都是将主动定位与辅助定位相结合使用，如使用卡尔曼滤波(KF)的方法将数据融合，提高定位精度。目前在室外定位主要使用GPS，也是用数据融合提高精度，而室内定位则也可使用同时定位与建图(slam)，但slam同样会受到地图中的动态障碍物干扰，稳定性相对不足。

发明内容

为解决现有技术存在的问题，本发明提出一种基于改进模型的UWB/IMU/ODOM多传感器数据融合移动机器人定位方法，使用了三路数据，其中IMU和ODOM是机器人自主定位数据，UWB是通过超宽频定位收集四个基站的坐标与信号强度，采用四点定位法计算出的机器人的绝对位置。

本发明的技术方案为:

所述一种基于改进模型的UWB/IMU/ODOM多传感器数据融合移动机器人定位方法，包括以下步骤：

步骤1：机器人建模

建立移动机器人的运动模型：

θ_k＝θ_k-1+ω_k-1*T

v_k＝v_k-1+a_k-1*T

其中T表示离散系统的时间间隔，k表示系统时刻序列，x表示机器人x坐标，y表示机器人y坐标，θ表示机器人当前的偏航角，v表示机器人当前速度，a表示机器人加速度，ω表示机器人的角速度；其中x，y，θ，v组成状态量X，a，ω组成控制量U；

建立如下状态方程：

X＝f(X,U+R_imu)

其中R^imu为IMU的随机观测噪声；

步骤2：构建机器人观测方程

针对ODOM和UWB数据构建两个观测方程：

采用UWB作为基准数据完成定位后，只采用其中定位结果的x和y坐标数据；建立UWB观测矩阵：

观测方程为：Z^uwb＝H^uwb*X+R^uwb

其中R^uwb是UWB的量测误差，X为状态量，Z^uwb为UWB观测量，包括通过UWB定位得到的机器人的x坐标与y坐标，通过观测矩阵H^uwb与状态量X相乘表示，H^uwb为UWB观测矩阵；

针对ODOM的数据，只采用ODOM的速度v和角度θ作为输入量；建立ODOM观测矩阵：

观测方程：Z^odom＝H^odom*X+R^odom

其中R^odom是ODOM的量测误差，X为状态量，Z^odom为ODOM观测量，包括机器人的速度v和偏航角θ，通过观测矩阵H^odom与状态量X相乘表示，H^odom为观测矩阵；

步骤3：EKF数据融合

首先将状态方程线性化得到

其中A为状态方程对于状态量X的偏导数，B为对于控制量U的偏导数，下标k表示系统时刻序列；

接下来进行EKF数据融合：

先完成状态估计：

其中

分别表示在第k时刻，状态估计后得到的状态量以及状态误差协方差；

进行UWB的观测校正：

其中X_k′，Σ_k′分别表示在第k时刻，采用UWB进行第一次观测校正后得到的状态量以及状态误差协方差；

继续进行odom的观测二次校正：

Σ_k＝(I-K^odomH^odom)Σ_k′

完成状态量估计X_k和协方差估计Σ_k，循环迭代就能得到每一时刻的移动机器人运动状态。

进一步的，步骤3中对状态方程线性化的过程为：

求解状态方程对于状态量X的偏导数：

以及对于控制量U的偏导数：

对于k时刻，得到线性化的状态方程为

有益效果

本发明使用了扩展卡尔曼滤波(EKF)，完成了对机器人UWB/IMU/ODOM数据的融合，通过UWB闭环特性弥补IMU和ODOM漂移问题，通过IMU和ODOM短时间精度提高了UWB绝对坐标定位精度。且EKF具有运算速度快的优点，能够在一般移动机器人上保证足够刷新频率。

本发明相对于其他机器人融合定位方法，使用了改进机器人模型，使用了速度、加速度、角速度、位置、角度5种信息，通过更加全面的信息提高定位精度。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1：实施例中长时间运动后末端轨迹对比图；

图2：实施例中的误差对比图。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本发明使用两路机器人自主定位数据IMU和ODOM，并采用超宽频定位技术UWB，收集四个基站的坐标与信号强度，通过四点定位法计算出机器人的绝对位置，通过上述三路数据，使用扩展卡尔曼滤波进行数据融合，通过UWB闭环特性弥补IMU和ODOM漂移问题，通过IMU和ODOM短时间精度提高了UWB绝对坐标定位精度，实现了快速稳定的多传感器数据融合定位。

本发明的方法包括以下步骤：

步骤1：机器人建模

移动机器人的模型是一个非线性模型，我们构建以下运动模型：

θ_k＝θ_k-1+ω_k-1*T

v_k＝v_k-1+a_k-1*T

对于地面移动机器人我们不考虑三维坐标，这样就可以降低在计算中的矩阵维数。其中T表示离散系统的时间间隔，k表示系统序列，x表示机器人x坐标，y表示机器人y坐标，θ表示机器人当前的偏航角，v表示机器人当前速度，这四个状态量可以很容易的构建出马尔科夫链，所以我们以这4个机器人属性作为我们的状态量X。而a表示机器人加速度，ω表示机器人的角速度，这两个机器人属性我们难以为其构建马尔科夫链，所以这里作为控制量U。

因此，我们可以建立如下状态方程：

X＝f(X,U+R_imu)

其中R^imu为IMU的随机观测噪声，我们在实际使用中能够获取的IMU数据就是存在噪声的，需要使用U+R^imu表示。

步骤2：构建机器人观测方程

由于IMU的观测量(机器人的加速度与角速度)加入状态方程中，所以针对剩下两个传感器ODOM和UWB数据构建两个观测方程。

采用UWB作为基准数据完成定位，只采用其x/y坐标数据，所以有如下观测矩阵：

观测方程为：Z^uwb＝H^uwb*X+R^uwb

其中R^uwb是UWB的量测误差，X为状态量，Z^uwb为UWB观测量，包括机器人的x坐标与y坐标，通过观测矩阵H^uwb与状态量X相乘表示，H^uwb为UWB观测矩阵。

针对ODOM的数据，我们从码盘结构分析发现，采用ODOM的x/y坐标数据能够计算出速度v和角度θ，反之亦然，并且我们只需要ODOM的瞬时数据，而不需要其积分的累计数据。所以我们这里只采用ODOM的速度v和角度θ作为输入量即可，可得观测矩阵：

观测方程：Z^odom＝H^odom*X+R^odom

其中R^odom是ODOM的量测误差，X为状态量，Z^odom为ODOM观测量，包括机器人的速度v和偏航角θ，通过观测矩阵H^odom与状态量X相乘表示，H^odom为观测矩阵。

步骤3：EKF数据融合

扩展卡尔曼滤波(EKF)是将状态方程和观测方程线性化之后再进行卡尔曼滤波(KF)，在这里我们的状态方程是非线性的而所有观测方程都是线性的，所以在这里我们只需要将状态方程线性化就行了。

具体采用的线性化过程为：

求解雅克比矩阵A，即对于状态量X的偏导数：

以及对于控制量U的偏导数：

此时，对于X＝X_k,U＝U_k时，有：

k表示离散系统中的时间序列序号。

接下来进行EKF数据融合：

先完成状态估计：

其中

分别表示在第k时刻，状态估计后得到的状态量以及状态误差协方差。

进行UWB的观测校正：

其中X_k′，Σ_k′分别表示在第k时刻，采用UWB进行第一次观测校正后得到的状态量以及状态误差协方差。

继续进行odom的观测二次校正：

Σ_k＝(I-KH^odom)Σ_k′

完成状态量估计和协方差估计，循环迭代就能得到每一时刻的移动机器人运动状态。

本实施例中针对上述方法我们进行matlab仿真来验证算法的有效性，用以上模型完成移动机器人以固定速度角速度的圆周运动，对比ODOM、IMU、UWB以及融合定位的运动轨迹与误差。

仿真的步长T＝0.005s，仿真时间为500s，总步数为100000。

附图1是长时间运动后末端轨迹对比图，可见4条轨迹明显区分开了，ODOM和IMU对比于EKF和UWB轨迹产生了明显的漂移偏差；

附图2是误差对比图ODOM和IMU的误差明显随时间波动性递增，UWB和EKF的误差稳定在一个范围内，作为融合定位的EKF明显的精度高于UWB。

表1为EKF相对于UWB提高的精度，EKF的误差相对于UWB减少了67％，能够平均将精度提高3倍。

表1 EKF相对于UWB提高的精度

	1	2	3	4	5	6	7	8	平均
										UWB	0.0637	0.0631	0.0626	0.0615	0.0616	0.0640	0.0644	0.0641	0.0631
EKF	0.0207	0.0236	0.0191	0.0190	0.0215	0.0230	0.0194	0.0165	0.0204
										误差减少比例	0.6748	0.6260	0.6949	0.6912	0.6518	0.6402	0.6978	0.7419	0.6773

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (2)

1.一种基于改进模型的UWB/IMU/ODOM多传感器数据融合移动机器人定位方法，其特征在于：包括以下步骤：

步骤1：机器人建模

建立移动机器人的运动模型：

θ_k＝θ_k-1+ω_k-1*T

v_k＝v_k-1+a_k-1*T

其中T表示离散系统的时间间隔，k表示系统时刻序列，x表示机器人x坐标，y表示机器人y坐标，θ表示机器人当前的偏航角，v表示机器人当前速度，a表示机器人加速度，ω表示机器人的角速度；其中x，y，θ，v组成状态量X，a，ω组成控制量U；

建立如下状态方程：

X＝f(X,U+R_imu)

其中R^imu为IMU的随机观测噪声；

步骤2：构建机器人观测方程

针对ODOM和UWB数据构建两个观测方程：

采用UWB作为基准数据完成定位后，只采用其中定位结果的x和y坐标数据；建立UWB观测矩阵：

观测方程为：Z^uwb＝H^uwb*X+R^uwb

其中R^uwb是UWB的量测误差，X为状态量，Z^uwb为UWB观测量，包括通过UWB定位得到的机器人的x坐标与y坐标，通过观测矩阵H^uwb与状态量X相乘表示，H^uwb为UWB观测矩阵；

针对ODOM的数据，只采用ODOM的速度v和角度θ作为输入量；建立ODOM观测矩阵：

观测方程：Z^odom＝H^odom*X+R^odom

其中R^odom是ODOM的量测误差，X为状态量，Z^odom为ODOM观测量，包括机器人的速度v和偏航角θ，通过观测矩阵H^odom与状态量X相乘表示，H^odom为观测矩阵；

步骤3：EKF数据融合

首先将状态方程线性化得到

其中A为状态方程对于状态量X的偏导数，B为对于控制量U的偏导数，下标k表示系统时刻序列；

接下来进行EKF数据融合：

先完成状态估计：

其中

分别表示在第k时刻，状态估计后得到的状态量以及状态误差协方差；

进行UWB的观测校正：

其中X_k′，Σ_k′分别表示在第k时刻，采用UWB进行第一次观测校正后得到的状态量以及状态误差协方差；

继续进行odom的观测二次校正：

Σ_k＝(I-K^odomH^odom)Σ_k′

完成状态量估计X_k和协方差估计Σ_k，循环迭代就能得到每一时刻的移动机器人运动状态。

2.根据权利要求1所述一种基于改进模型的UWB/IMU/ODOM多传感器数据融合移动机器人定位方法，其特征在于：步骤3中对状态方程线性化的过程为：

求解状态方程对于状态量X的偏导数：

以及对于控制量U的偏导数：

对于k时刻，得到线性化的状态方程为

Images