[发明专利]一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法

权利要求书

1.一种基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，包括模块1、模块2、模块3和模块4共四个计算模块，其特征在于：

所述模块1，建立轮式-IMU里程计(WIO)的测量模型，推导系统的状态方程；

所述模块2，建立视觉里程计(VO)的测量模型，推导系统的观测方程；

所述模块3，用误差状态卡尔曼滤波(ESKF)对WIO和VO的运动估计信息进行融合；

所述模块4，建立自适应模糊神经推理系统(ANFIS)，根据WIO和VO的直接信息，推断里程计的运动状态，实时调整ESKF中预测模型和测量模型的协方差矩阵；该模块在使用前需要根据GPS数据对ANFIS进行训练；

所述模块1的输入是轮式里程计数据和IMU数据，输出是相邻时刻IMU坐标系的位姿估计；模块2的输入是双目相机拍摄的照片，输出是相邻时刻相机坐标系的位姿估计；模块3的输入是模块1和模块2的输出，输出是相邻时刻IMU坐标系的位姿估计；模块4的输入是模块1中WIO和模块2中VO的中间数据，输出是模块3中需要使用的WIO和VO的协方差矩阵；

整个定位程序的输入是IMU数据、轮式里程计数据、相机数据，输出是IMU坐标系的位姿变化。

2.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，所述模块1中建立WIO测量模型，具体如下：

IMU包括三轴陀螺仪和三轴加速度计，考虑到测试系统加速度计中的噪声极大地影响了位移估计，选择仅使用陀螺仪来估计方向变化；作为补偿，引入车轮编码器进行平移运动计算，并与陀螺仪集成，融合为轮式-IMU里程计WIO，提供完整的六自由度运动估计；WIO为ESKF系统提供移动机器人运动状态的预测；

下面的式子(1)展示了i+1时刻WIO测得的IMU坐标系{I}的位置变化：

其中是i+1时刻测得的IMU坐标系的姿态变化，是轮式里程计坐标系{W}相对于IMU坐标系的位置标定关系，是轮式里程计坐标系相对于IMU坐标系的姿态标定关系，是i+1时刻测得的轮式里程计坐标系的位置变化，I是单位矩阵。

3.根据权利要求1所述的基于ANFIS-ESKF的地面移动机器人多传感器融合定位方法，其特征在于：所述模块1中建立系统状态方程，具体如下：

定义系统的状态为：

其中pⁱ为i时刻IMU坐标系相对于初始时刻的位置，qⁱ为i时刻IMU坐标系相对于初始时刻的姿态，为i时刻IMU角速度偏移；

那么根据WIO测量模型得到系统的名义状态方程为：

其中是i时刻轮式里程计的测量值，是i时刻IMU坐标系相对于0时刻的姿态，是i时刻IMU测得的角速度，是i时刻IMU的角速度偏移，Δt是IMU的采样周期，[γⁱ]_×指的是γⁱ对应的反对称矩阵；pⁱ⁺¹表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的位置，表示i+1时刻IMU坐标系相对于初始时刻IMU坐标系的姿态对时间的导数，表示i+1时刻IMU角速度偏移对时间的导数；

误差状态方程为：

其中θⁱ是qⁱ对应的李代数，ω_nⁱ是IMU的测量噪声，是IMU角速度偏移的导数；表示i时刻轮式里程计的测量噪声；ω_nⁱ表示i时刻IMU的角速度测量噪声；Δt表示IMU采样周期。