卡尔曼滤波在救援机器人定位中应用.docxVIP

卡尔曼滤波在救援机器人定位中的应用摘要：介绍了履带式机器人的特点和自身定位的方式，介绍了履带式里程计定位模型，以光电编码器和MTI（惯性检测单元）作为机器人的定位系统建立了履带机器人的运动方程，并采用卡尔曼滤波算法对传感器进行融合，获得机器人的坐标与状态。并用matlab对滤波算法的有效性进行了验证。关键词：履带机器人机器人;卡尔曼滤波;定位1.引言在自然灾害频发的今天，巨大的灾害往往造成大面积的建筑物坍塌和人员伤亡，废墟的复杂环境对救援工作增加了很多障碍，为了完成对受灾人员的快速救援，救援机器人应运而生。如何在废墟中对机器人进行定为就显得尤为重要，自主机器人需要实现自主导航并执行救援任务，必须准确的知道自己所处的位置。2.概述履带式是机器人是机器人发展的一个重要分支，履带式机器人具有跃障性好，便于控制等优点，是跃障型机器人动力部分的主要选择。本文中使用的机器人控制设备包括：云台摄像机，激光扫描仪，惯性检测单元，无线网卡，图像收发设备等。履带式移动机器人通常采用在履带电机上安装光电编码器，将电磁脉冲传递给DSP(数字信号处理)系统，由里程计通过航迹推理算法计算出机器人目前左右履带的速度，前后鳍履带速度，机器人的角度；以及在已建立地图的情况下，由机器人的初始位置推理出机器人的位置等信息。但由于履带长度，弹性形变以及地形等原因，由上述航迹算法推理出来的机器人位置信息存在较大的误差，直接影响了机器人定位的效果。为了降低误差，本文将通过卡尔曼滤波算法与将MTI（惯性检测单元）检测的机器人航向角度与光电编码器测得的航向叫进行滤波算法融合，修正履带空转等引起的编码器对角度测量的误差，增加定位的精确度。本文中的程序基于VC++ 下的MFC框架开发，通过多线程方法同时接受和处理上位机传递过来的数字信息，并对卡尔曼滤波和MIT融合算法进行验证。图1 救援机器人结构3.里程计模型本文中采用的机器人采用两个主要履带，以及四个鳍履带，两个主履带有各有一个驱动电机与电光编码器相连，两个前鳍履带与后鳍履带分别共用一个驱动电子与电光编码器，驱动电机驱动履带转动，光电编码器同时转动并收集信息。可以直接的的的到每个履带的线速度。图2 机器人平面运动模型模型返回的信息为X’=V，Y’=V分别为速度在笛卡尔坐标系中X方向与Y方向的分速度，以及机器人航向角度，V与分别为机器人的线速度与角速度。设信息采集周期为就可以推算出机器人速度以及机器人的方向角。设D为履带高度，P为光电编码器旋转一圈的脉冲数，N为单位时间间隔光电编码器的脉冲数可得履带线速度v[1-2]：由公式(1)可以推出左履带与右履带的速度vL和vR。设l为机器人左右履带的间距，可以推出机器人航向变化的角速度d：在已知机器人航向变化的角速度可以有t时刻的机器人的航向角度推导出机器人在t+1时刻的航向角度，设为机器人的航向角度：在已知机器人几何中心坐标在t时刻为,可以由上述变量推导出在t+1时刻机器人几何中心的坐标：有量程计模型可以看出，当前坐标计算需要不停的对前一时刻机器人的坐标进行迭代，而误差在迭代过程中不断的增加。如果不对误差进行正确的补偿，会使机器人的定位越来越不准确逐渐的是机器人偏离预想的运动轨迹。V为机器人的质心速度。4卡尔曼滤波算法卡尔曼滤波算法分为两部分：预测算法和校正算法，预测算法计算出当前状态的先验估计值X(u|u-1)（假设当前状态为u）传递给校正算法，校正算法通过传递过来的先验估计值计算出后验估计。如图，图预测部分首先建立系统的预测模型，卡尔曼滤波算法需要建立预测模型，预测模型用来预测下一系统状态。基于上一系统状态下预测当前状态：X(u|u-1) =A X(u-1|u-1)+BF(u-1) (7) 式(7)中，A为里程计测量变量增益，B为控制输入增益，X(u|u-1)是利用上一状态的先验估计值，X(u-1|u-1)是上一状态后验估计值，F(u-1)为控制函数。在实际应用中，里程计的测量值value可直接作为系统的先验估计值：Value=X(u|u-1)完成对系统状态初始值的设定，接下来需要对系统误差的协方差进行计算。本文用P表示协方差：P(u|u-1)=A P(u-1|u-1) A’+Q(8)式(8)中，P(u|u-1)是X(u|u-1)对应的误差协方差，P(u-1|u-1)是X(u-1|u-1)对应的误差协方差，矩阵A同(7)式。A’表示A的转置矩阵，Q是过程噪声的协方差矩阵。公式7，8完成了通过里程计对系统的预测。 校正部分通过卡尔曼滤波算法得到了里程计现在状态的预测结果，下面需要计算测量值，测量方程为：H为状态变量对观测变量的增益。由于含有噪声的观测值是系统当前状态的直接体现，本文中H=1。变量为观测噪声，本文中得到的观测值已包含观测噪声，所以无需再测量。本文中为MTi的测量值M