HW2-基于里程计的航位推算仿真实验-15291122-任家辉

实验报告1-航位推算基于光学编码器的移动机器人航位推算仿真实验报告学号：15291122姓名：任家辉北京交通大学2014年版实验目的在移动机器人的传感技术中，实现机器人在环境中的定位是移动机器人要解决的关键技术之一。能实现移动机器人的定位的方法很多，如卫星定位、超声定位、激光测距定位、基于视觉的机器人定位。本实验主要是对基于光学编码器（里程计）的定位原理进行学习，并利用Matlab环境实现其航位推算的仿真。通过仿真实验，加深对里程计工作原理的了解。同时对移动机器人的定位方法有一个基础的掌握。实验内容及步骤本实验主要包括以下几个方面的内容：Matlab的编程学习。通过对该软件的学习编程，掌握一些数学工具的使用。对航位推算的代码进行分析。结合里程计的工作原理，对计算方法进行分析，掌握计算的基本过程，及编程中用到的Matlab函数的使用方法。学习Matlab的简单的绘图函数的使用。在例程的基础上，进行一定程度的修改。对于掌握的较好的同学，可以做更进一步的实验：增加噪声。观察因为传感器的噪声导致机器人路径的偏差变化。3Matlab中运行代码得到小车轨迹图一写入代码图二运行程序后得到小车运动轨迹收获及建议本次实验让我接触到了MATLAB，我切身感觉到MATLAB给我们实验仿真带来的好处，这次实验也让我得到由自己设定的运行轨迹，甚是欣喜。附录1：样例代码%****************************myDR.m**************************clc;clear;Sr=[03420050020-400000000];%左轮单位时间内走过的距离，相当于是左编码器的采样值，给出了每个时刻下编码器的读数Sl=[02205200523533243450];%右轮单位时间内走过的距离，相当于是右编码器的采样值O=[0000000000000000000];%小车在T时间相对于水平角度X=[0000000000000000000];%用于存储小车当前X坐标（计算的定位结果）Y=[0000000000000000000];%用于存储小车当前Y坐标L=10;%小车宽度fori=1:19X(i+1)=X(i)+(Sr(i)+Sl(i))*cos(O(i)+(Sr(i)+Sl(i))/(2*L))/2;%计算下一时刻XY(i+1)=Y(i)+(Sr(i)+Sl(i))*sin(O(i)+(Sr(i)-Sl(i))/(2*L))/2;%计算下一时刻YO(i+1)=O(i)+(Sr(i)-Sl(i))/L;%计算下一时刻角度endplot(X,Y,'-s',...'Color','r',...'LineWidth',2,...'MarkerEdgeColor','k',...'MarkerFaceColor','g',...'MarkerSize',8);%绘制以（X,Y）为坐标的关键点的轨迹图，它相当于各个时刻下的机器人的位置（定位实现了！）gridon;xlabel('X');%给X轴加注ylabel('Y');text(0,-0.5,'小车起点');text(10,-3.8,'小车终点');title('航位推算坐标图');%加图注%****************************myDR.m**************************附录2：:航位推算基本原理的“温习”仅仅利用里程计来实现定位的基本原理如图1所示：（图1） 设t时刻，移动机器人的位姿为，，分别表示机器人从t到t+1时刻机器人运动的距离和绝对航向，相对航向定义为前后两个时刻绝对航向之差，用ω表示。则t+1时刻移动机器人的位姿可表示为: (3-1) 从(3-1)式可以看出，里程计作定位之用算法结构简单，工作时实时获得前后时间的行驶距离s和角度变化量ω。 在实际工程中，对于移动机器人平台可以在左右两侧分别添加里程计进行路径记录（如图3-2所示），这样可以通过两侧的路程差得到转过的角度，进行路径的递推，该推导过程如下：(图3-2) 对实际状态下的移动机器人移动情况进行建模分析，确定形象模型进行路径计算的方法，图6-1所示表示移动机器人从点O(x,y,θ)开始做圆弧运动，在Δt时间后到达点M(x+Δx,y+Δy,θ+Δθ),图中θ为初始航向角，L为移动机器人两轮间的宽度，R为机器人运动圆弧的半径，ΔSL、ΔSR、ΔS分别表示机器人左右两轮以及中心点从O到M所走过的距离，Δx、Δy、Δθ分别表示移动机器人从O到M横、纵坐标以及航向角度所发生的变化，如果假定移动机器人在行驶过程中理想，即路面平整，无打滑，则有： (3-2) 因为图3-2中有，则为等腰三角形，则成立，根据弦切角定理可知： (3-3) (3-4) 当对传感器的信息采样时间足够短或者移动机器人做直线运动时，有以下两个式子成立，，则根据(3-3)和(3-4)式可以化简为： (3-5)通过对(3-2)与(3-5)式联立求解可以得到移动机器人的离散运动方程：(3-6)其中，分别表示移动机器人左右两侧轮子从t到t+1时刻移动的里程，将移动机器人的位置向量设为，采样周期为dt（dt应该比较小，这样才能满足在采样周期内机器人做线性运动），其输入控制向量（它们的噪声可近似为，的高斯白噪声），根据(3-6)式，考虑到控制输入量噪声的影响，可得移动机器人的位置向量为：(3-7)根据（3-6）式可以计算小车的航迹，输入量为小车左右轮在一个时间单位所走的路程。L为小车的车宽，是已知量。通过公式可以计算出小车的当前相对位置，及X，Y。附录3：参考资料:[1]Matlab绘图教程(大量实