基于改进扩展卡尔曼滤波算法的移动机器人定位方法研究

陈庆武，张志安，何　雨，韩明明，黄学功

(南京理工大学 机械工程学院，江苏 南京 210094)

在机器人位姿跟踪过程中，由于测量噪声和系统噪声的存在，测量系统通过传感器获取的信息只是针对客观环境的片面描述[1]，而单一传感器的可靠性以及输入信息的精确性和充足性难以获得保证. 针对这个问题，本文采用扩展卡尔曼滤波进行多传感信息融合[2]，在提高位姿跟踪精度的基础上，通过扩展观测矩阵，使得该算法能够灵活适应于不同传感器类型和不同传感器数量的测量系统，使得程序代码可快速部署于不同应用场合，解决移动机器人位姿跟踪多传感器测量系统的可扩展性问题，增加算法的灵活性.

1　改进扩展卡尔曼滤波算法

扩展卡尔曼滤波算法[3]主要应用于非线性测量系统[4]，通过对系统方程和观测方程进行线性化处理来估计后验，在进行扩展卡尔曼滤波时，所观测非线性动态系统的状态转移概率由非线性函数g控制，即

xt=g(xt-1)+ξt-1,

(1)

式中：xt为观测目标在t时刻的状态向量，随机变量ξt-1是一个高斯随机向量，表示由状态转移引入的不确定性，其维数与状态向量相同，均值为0，方差为Rt.

测量概率由非线性函数h控制，即

zt=h(xt)+δt,

(2)

式中：zt为观测目标在t时刻的测量值；h为预测值映射到测量空间的非线性函数；δt是均值为0，方差为Qt的多变量高斯分布测量噪声.

扩展卡尔曼滤波算法分为两个阶段： 预测和更新. 式(3)与(4)是进行当前状态预测和误差协方差前向更新的预测阶段.

(3)

(4)

(5)

(6)

(7)

在位姿跟踪类问题中，扩展卡尔曼滤波主要用于估计所观测6自由度移动目标的位置、姿态和速度信息，假设xt是所观测系统在t时刻的12维状态向量，包含三维坐标及其对应轴的速度、三维角度及其对应轴的角速度，其表达式为

xt=[xyzx′y′z′φθψφ′θ′ψ′]T.

(8)

在特定传感器类型和数量的实际工程应用场合中，所观测的状态一般都是上式状态变量的一部分，标准的扩展卡尔曼滤波的观测矩阵H必须通过测量模型h函数的雅克比矩阵[5]给出，为支持扩展传感器的类型和数量，使得算法可重复快速部署使用，本文假定系统中所使用的每一个传感器都可以对所有要预测的状态变量进行更新，即可进行状态空间的局部更新.

若某个传感器某次测量只测得n个状态变量值，则相应预测空间到测量空间的映射为一个具有n个非零变量、维数为n×12、秩为n的变换矩阵. 通过这种方式扩展观测矩阵H，任何传感器都可以对状态向量进行局部更新而不用重新设计算法，使得基于扩展卡尔曼滤波的位姿跟踪算法更加具有普遍性，增加测量系统的灵活性.

2　定位算法

假设地面移动平台-大地系统运动学状态，即运动学模型的目标观测状态为

xt=[xtytθtvtωt]T,

(9)

式中：xt和yt为机器人坐标系下t时刻机器人的坐标；θt为机器人坐标位置与原点连线和x轴方向的夹角；vt和ωt为机器人在t时刻的速度和角速度. 本文通过设计里程计模型来进行地面移动平台状态估计，通过设计惯性传感器[6]IMU模型和全球定位GPS模型来进行状态更新.

2.1　预测阶段

对于两轮或两履带差速驱动的地面移动机器人，假设其在运行过程中通过驱动电机编码器得到的左侧轮子速度为vl，右侧轮子速度为vr，轮距为W，则机器人的线速度和角速度为

图 1　地面移动机器人恒定v和ω无噪声移动模型Fig.1　The constant v and ω noiseless movement model of ground mobile robot

(10)

假定在Δt时间内保持vt和ωt为恒定值，当ωt不等于0时，无人车以半径r进行圆运动.

(11)

圆心坐标为

(12)

则t+Δt时刻的状态向量为

(13)

当ωt等于0时，

(14)

可以通过式(13)与(14)得到运动模型的雅克比矩阵JA，扩展卡尔曼滤波模型的预测可以写成

xt+1=g(xt),

(15)

(16)

(17)

处理噪声的协方差矩阵表达式为

Q=E[N·NT]=E[GμμμTGT]=G.E[μμT]GT,

(18)

其中，

(19)

里程计模型的处理噪声为

2.2　更新阶段

当只用惯性测量元件测量三轴角度和三轴角速度时，映射函数为

(20)

将预测映射到IMU测量空间

HIMUxt=[φθψφ′θ′ψ′]T.

(21)

当只利用GPS测量移动机器人的坐标时，映射函数为

(22)

移动机器人的世界坐标系和GPS的UTM坐标系[7](universal transverse mercartor grid system，通用横墨卡托格网系统坐标)之间的转换关系如式(23)[8]所示，其中c代表余弦函数cos，s代表正弦函数sin，φ,θ和ψ分别代表UTM坐标系下的横滚角、俯仰轴和偏航角，移动机器人在UTM坐标系下的初始位置由xUTM0,yUTM0和zUTM0表示.

(23)

里程计和GPS数据之间的转化关系式为

(24)

3　实　验

3.1　实验设计

采用如图 2 与 3 所示轮履复合移动平台作为实验平台，该平台可通过轮履切换机构改变驱动方式为轮子驱动或履带驱动，驱动电机自带2500线光电式增量编码器，可以一定周期通过CAN总线向主控设备返回光电编码器的位置信息，经过处理可得到电机转速和里程计数据.

图 2　轮履复合移动平台三维设计图Fig.2　3D design of wheel-track mobile platform

图 3　实验平台实物图Fig.3　Entity of experimental platform

实验采用的全球定位模块为SkyTra公司的GPS+北斗双模定位模块S1216F8-BD，其封装如图 4 所示，可以多种波特率与外部设备进行串口数据传输，输出的 GPS/北斗定位数据采用 NMEA-0183 协议(默认)，控制协议为 SkyTraq 协议.

惯性传感器IMU采用美国“英美盛”公司推出的全球首款整合性6轴运动处理器件MPU6050，如图 5 所示，该传感器内部集成了3轴MEMS陀螺仪及3轴MEMS加速度计，可以通过IIC接口输出完整的6轴运动信息.

图 4　GPS和北斗双定位模块S1216及其天线Fig.4　GPS and Beidou dual positioning module S1216 and its antenna

图 5　IMU(MPU6050)Fig.5　IMU (MPU6050)

本实验采用上下位机的分层结构进行控制系统设计，下位机采用深圳大疆创新科技有限公司的stm32f427单片机主控板，通过多种总线接口与传感器直接进行信息交互，再将得到的传感器信息通过串口发送至上位机英伟达Jetson TX1开发板，上位机负责进行数据存储并进行上述改进扩展卡尔曼滤波算法的运算. Jetson TX1开发板安装有Ubuntu操作系统，在Ubuntu环境下运行ROS(robot operation system，机器人操作系统)以进行程序开发[9]. 实验整体硬件连接如图 8 所示，GPS模块、IMU模块和电机驱动器分别通过串口线、IIC总线和CAN总线与stm32f4单片机进行数据交互，单片机再通过串口线将传感器数据上传至Jetson TX1开发板.

ROS是一个适用于机器人控制的开源程序框架，它的核心是节点，用来执行某个相对简单的任务或进程，多个节点之间可以互相针对特定的主题传递消息或者提供服务. 在ROS中，所有主题消息都可以被消息记录包加上时间戳存储到后缀名为“.bag”的文件中，且可以通过ROS提供的工具进行消息重复回放，用以将实验数据存储下来用于未来的分析、处理、开发和算法验证.

图 6　stm32f4单片机主控板Fig.6　stm32f4 microcontroller main control board

图 7　英伟达Jetson TX1开发板Fig.7　NVIDIA Jetson TX1 development board

本实验将利用ROS的消息记录包对实验过程中的传感器数据进行存储，通过离线回放实验数据进行不同传感器配置下的算法可行性验证及其效率比较. 按照图 8 实验硬件连接简图构建实验平台，在平台上安装两个GPS模块和两个IMU模块.

图 8　实验硬件连接简图Fig.8　Diagram of experimental hardware connection

数据处理过程的数据流向如图 9 中虚线所示，图 9 中实线箭头部分为实验数据采集时的数据流向，通过遥控实验平台在某校广场进行闭环运动，从事先标记的地点出发，stm32f4单片机同时对驱动电机、两个GPS模块和2个IMU模块进行数据采集，并将数据上传给Jetson TX1上的ROS，ROS利用ROSBAG消息记录与回放功能包记录所有传感器数据并进行存储，实验平台绕广场一周回到出发点后，实验数据采集完毕并保存在上位机中.

图 9　实验数据流向及流程图Fig.9　Experiment data flow and flowsheet

3.2　实验结果分析

通过ROS消息记录与回放功能包回放采集到的多传感器数据，设计五种不同传感器组合并使用同一个改进Kalman滤波算法应用方案进行数据处理，分别得到五种传感器配置下的算法定位信息，以此来判断算法对不同传感器种类和数量测量系统的兼容性. 对两个GPS模块的定位信息求平均值作为实验平台的标准定位信息，通过算法定位信息与标准定位信息进行比较判断算法的定位精度. 将数据处理得到的不同传感器类型组合下算法定位数据导入到谷歌地图[10]中，如图 10～12 所示，分别表示由里程计、里程计和IMU、里程计与IMU和GPS传感器配置组成的测量系统经过改进扩展卡尔曼滤波算法所得到的定位信息，三种传感器组合所得到的路径图大致匹配，说明上述算法可以对不同传感器组合达到很好的兼容性.

由数据处理得到的五种不同传感器组合下算法定位信息误差分析如表 1 所示，分别计算机器人坐标系下起点坐标与终点坐标的误差值、算法定位信息与两个GPS模块定位信息求平均值得到的标准定位信息的位置测量标准差. 图 13 与 14 分别为起点与终点坐标误差与位置测量标准差.

表 1　不同传感器配置的测量系统定位误差

图 10　里程计-路径图Fig.10　Path map of odometer

图 11　里程计和IMU-路径图Fig.11　Path map of odometer and IMU

图 12　里程计、IMU和GPS-路径图Fig.12　Path map of odometer, IMU and GPS

图 13　起点与终点坐标误差Fig.13　Coordinate errors between start and end position

图 14　位置测量标准差Fig.14　Standard deviation of position measurement

从图 13 与 14 可以看出，当只使用驱动电机的里程计数据时，测量系统的定位误差已超过可接受范围，随着传感器种类的增加，融合多种不同传感器信息的定位精度得到显著提升，当增加同一种传感器数量时，经过算法处理的定位精度也得到进一步提高. 综上所述，不同传感器组合下，起点与终点坐标在机器人坐标系下的x轴和y轴方向误差随着传感器种类和数量的增加而逐渐减小，实验过程的位置测量标准差也随之降低. 说明通过扩展算法的观测矩阵不仅能对不同传感器组合下的测量系统达到很好的兼容性，并且随着传感器的增加，算法定位精度也随之提高.

4　结束语

本文针对移动机器人位姿跟踪过程中测量系统传感器信息处理问题进行了详细研究. 首先介绍了扩展卡尔曼滤波算法，通过扩展观测矩阵引入传感器信息对观测向量进行局部更新的方法，在保证定位精度的前提下，实现算法对不同传感器类型和数量测量系统的兼容性. 然后，进行地面移动机器人位姿跟踪算法模型设计，建立里程计模型与传感器数学模型，搭建实验平台进行算法的可行性和可靠性验证. 理论分析和实验结果表明，该方法能提高测量系统的灵活性和可靠性.