基于IMM算法的双轮差速驱动机器人的定位算法.pdf

精密制造与自动化 2017年第2期 39 基于 IMM 算法的双轮差速驱动机器人的 定位算法 袁 铸 靳 果（河南工业职业技术学院 河南南阳 473000）摘 要 在移动机器人的定位系统中，卡尔曼滤波等算法对解决这种非线性、结构复杂的问题时，因定位精度不高，效果并不理想。针对二维平面中作非匀速运动的双轮机器人定位问题，提出一种基于交互式多模型算法的移动机器人跟踪定位方法，实验结果表明新方法在机器人作非匀速运动时，能够避免滤波发散，迅速地稳定下来，实现机器人的精确快速定位。关键词 移动机器人 卡尔曼滤波 交互式多模型 并行滤波 轮式移动机器人根据车轮数目的不同，可划分为单轮、双轮、三轮和四轮式移动机器人1。本文研究对象为双轮差速驱动的移动机器人，其特点是结构简单、易驱动、控制且机动方便灵活。目标定位系统中，难点是解决参数的联合估计问题，国内外学者也采用多种方法进行处理，杨世强2等人采用 UKF 算法，并引入在线的逆动力学模型，以减少移动机器人自身不确定因素带来的影响，但该方法预测和估计不仅依赖实时的传感器数据信息，而且要求系统中的各个参数为不相关的随机参数。王越超3等人引入变结构的控制思想，将定位作为系统误差的镇定问题，采用 EKF 对系统误差进行状态估计，从而实现跟踪定位，该方法可以有效地抑制由环境噪声带来的影响，但对于非匀速运动时的状态定位精度不高。本文针对移动机器人在二维平面中的非匀速运动，提出一种基于交互式多模型算法（IMM）的移动机器人跟踪定位方法，该算法可以有效地抑制单个模型时若目标模型与目标的运动状态不一致时所造成的误差，不管移动机器人是否加速或者转弯，都能够实现快速定位。1 1 移动机器人的运动模型移动机器人的运动模型 本文的双轮差速驱动移动机器人，假设其两驱动轮轴线的中心位置为质心 O，坐标为（x0,y0），驱动轮的半径为R，移动机器人的运动方向向量为P=（x0,y0,），则双轮差速驱动移动机器人的模型如图 1 所示4。图图 1 双轮差速驱动移动机器人的模型双轮差速驱动移动机器人的模型 根据刚体力学理论，则移动机器人的运动学方程为：x0.y0.=cos0sin001（vv）(1)（vv）=（1/21/21/l-1/l）（vrvl）(2)其中，为运动方向与x轴的夹角，v为质心 O 处的线速度，v为转向角速度，l 为驱动轮之间的轮距，vl为左驱动轮的线速度，vr为右驱动轮的线速度。当车轮的运动无滑动、纯滚动条件时，满足如下条件5-7：y0.cos-x0.sin=0 (3)同 时 定 义：u=v,vT,q.=lx0.,y0.,.T，s（q）=-sin,cos,0,A（q）=cos0sin001，可得出：万方数据 精密制造与自动化 2017年第2期 40 q.=A（q）u (4)易验证q.满足非完整约束方程：S（q）q.=0 (5)设采样周期为T，则将式(1)离散化得到的方程组为：x（k）=x（k-1）+v（k-1）T.cos（k-1）y（k）=y（k-1）+v（k-1）T.sin（k-1）（k）=（k-1）+v（k-1）T (6)在双轮差速驱动移动机器人的运动过程中，可通过分别控制左右两驱动轮的线速度来实现移动机器人的转弯及其他非匀速运动，故将式(6)中的方向角信息转化成移动机器人运动的线速度和加速度来表示，即移动机器人的运动模型和量测模型可表示为：X（k）=FX（k-1）+GW（k-1）Z（k）=HX（k）+V（k）(7)2 2 交互式多模型算法交互式多模型算法 交互式多模型算法，简称 IMM（Interacting Multiple Model）算法5，其最基本的思想就是目标不同的运动状态用不同的目标运动模型来匹配。假定移动机器人有匀速、匀加速运动等多种运动状态，分别用一个运动模型与之对应，现假定目标有 r 种运动状态，用 M1，M2，Mr 表示这 r 个运动模型。然后用不同的模型滤波器对不同的运动状态进行并行滤波，并通过估计状态的组合实现交互作用，用马尔可夫链完成对各个模型滤波器输出结果的切换，实现机器人运动状态的混合估计，即实现机器人的定位跟踪。在 IMM 算法中，由于多模型存在，可以将移动机器人的运动模型和量测模型描述为：X（k）=FM（i）X（k-1）+GM（i）（k-1）WM（i）（k-1）Z（k）=HM（i）（k）X（k）+VM（i）（k）(8)用 Mj（k）表示 k时刻目标的状态模型Mj，即M（k）=Mj。在量测序列Zk已知的条件下，用j（k）=P Mj（k）/Zk 表示Mj（k）发生的后验概率，那么显然有如下关系式成立：j（k）=1rj=1 （9）按照已知状态转移概率矩阵的齐次马尔可夫链，在不同时刻对各个模型进行切换，其转移概率可以表示为：Pij=PM（k）=MjM（k-1）=Mi 用lPij表