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

《卡尔曼滤波在救援机器人定位中的应用》由会员上传分享，免费在线阅读，更多相关内容在工程资料-天天文库。

1、卡尔曼滤波在救援机器人定位中的应用摘要：介绍了履带式机器人的特点和白身定位的方式，介绍了履带式里程计定位模型，以光电编码器和MTI（惯性检测单元）作为机器人的定位系统建立了履带机器人的运动方程，并采用R尔曼滤波算法对传感器进行融合，获得机器人的坐标与状态。并用matlab对滤波算法的有效性进行了验证。关键词：履帶机器人机器人;卡尔曼滤波;定位1•引言在自然灾害频发的今天，巨大的灾害往往造成大血积的建筑物坍塌和人员伤广，废墟的复杂坏境对救援工作增加了很多障碍，为了完成对受灾人员的快速救援，救援机器人应运而生。如何在废墟屮对机器人进行定为就显得尤为重要，自丄机器人需要实现自主导航并执行救援

2、任务，必须准确的知道自己所处的位置。2.概述履带式是机器人是机器人发展的一个重要分支，履带式机器人具冇跃障性好，便于控制等优点，是跃障型机器人动力部分的主要选择。本文屮使川的机器人控制设备包括:云台摄像机，激光扫描仪，惯性检测单元，无线网卡，图像收发设备等。履带式移动机器人通常采用在履带电机上安装光电编码器，将电磁脉冲传递给DSP（数字信号处理）系统，由里程计通过航迹推理算法计算出机器人日前左右履带的速度，前后鳍履带速度，机器人的角度；以及在已建立地图的情况下，山机器人的初始位置推理出机器人的位置等信息。但由于履带长度，弹性形变以及地形等原因，由上述航迹算法推理出来的机器人位置信息存在

3、较人的谋差，直接影响了机器人定位的效果。为了降低课差，木文将通过卡尔曼滤波算法与将MTI（惯性检测单元）检测的机器人航向角度与光电编码器测得的航向叫进行滤波算法融合，修正履带空转等引起的编码器对角度测量的误差，增加定位的精确度。本文中的程序基于VC++下的MFC框架开发，通过多线程方法同时接受和处理上位机传递过来的数字信息，并对卡尔曼滤波和MIT融合算法进行验证。图1救援机器人结构3•里程计模型本文中采用的机器人采用两个主要履带，以及四个鳍履带，两个主履带有各有一个驱动电机与电光编码器相连，两个前鳍履带与后鳍履带分别共用一个驱动电子与电光编码器，驱动电机驱动履带转动，光电编码器同时转动

4、并收集信息。可以直接的的的到每个履带的线速度。图2机器人平面运动模型模型返I叫的信息为X^VcosO,r=Vsin0分别为速度在笛卡尔坐标系屮X方向与Y方向的分速度，以及机器人航向角度V与3分别为机器人的线速度与角速度。设信息采集周期为M就可以推算出机器人速度以及机器人的方向角。设D为履带高度，P为光电编码器旋转一圈的脉冲数，N为单位时间间隔M光电编码器的脉冲数町得履带线速度v(1'21：Nti・DPAtk}山公式(1)可以推出左履带与右履带的速度Vl和vR0设I为机器人左右履带的间距，可以推出机器人航向变化的角速度3十在已知机器人航向变化的角速度3d可以有t时刻的机器人的航向角度推导

5、出机器人在t+1时刻的航向角度，设6为机器人的航向角度：9t+i=+3dAt(3)在已知机器人儿何中心处标在t时刻为(xt,yj可以山上述变量推导出在t+1时刻机器人儿何中心的处标：Vl+vrv=⑷xt+1=xt4-vcos0U+1•At(5)Yt+i=yt+vsin9u+1-At(6)有量程计模型可以看出，当前朋标计算需要不停的对前一吋刻机器人的坐标进行迭代，而误差在迭代过程中不断的增加。如果不对误差进行正确的补偿，会使机器人的定位越来越不准确逐渐的是机器人偏离预想的运动轨迹。V为机器人的质心速度。4卡尔曼滤波算法卡尔曼滤波算法分为两部分：预测算法和校正算法，预测算法计算出当前状态的

6、先验佔计值X(u

7、u-1)(假设当前状态为u)传递给校正算法，校正算法通过传递过来的先验估计值计算出后验估计。如图，预测算法校正算法图预测部分首先建立系统的预测模型，卡尔曼滤波算法盂要建立预测模型，预测模型用来预测下一系统状态。基于上一系统状态下预测当前状态：X(u

8、u-1)=AX(u-11u-l)+B-F(u-l)(刀式⑺中，A为里程计测量变量增益，B为控制输入增益，X(u

9、u-l)S利用上一状态的先验估计值，X(u-l

10、u-l)是上一状态后验估计值，F(u-l)为控制函数。在实际应用小，里程计的测量值value可直接作为系统的先验估计值：Value=X(u

11、u-1)完成对系统状态初

12、始值的设定，接下來需要对系统误差的协方差进行计算。木文用P表示协方差：P(u

13、u-1)=AP(u-l

14、u-l)A^Q(8)式⑻中，P(u

15、u・l)是X(u

16、u・l)对应的误差协方差，P(u-11u・l)是X(u-11u对应的误差协方差,矩阵A同(7)式。A'表示A的转置矩阵，Q是过程噪声的协方差矩阵。公式7,8完成了通过里程计对系统的预测。校正部分通过卡尔曼滤波算法得到了里程计现在状态的预测结果，下面需要计算测量值，测屋方程为:Zk=HXk+v