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

权利要求书

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时刻，得到线性化的状态方程为