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

精品文档 1欢迎下载 基于光学编码器的移动机器人航位推算基于光学编码器的移动机器人航位推算 仿真实验报告 学号 15291122 姓名 任家辉 精品文档 2欢迎下载 北京交通大学 2014 年版 1 1实验目的实验目的 在移动机器人的传感技术中 实现机器人在环境中的定位是移动机器人要 解决的关键技术之一 能实现移动机器人的定位的方法很多 如卫星定位 超 声定位 激光测距定位 基于视觉的机器人定位 本实验主要是对基于光学编 码器 里程计 的定位原理进行学习 并利用 Matlab 环境实现其航位推算的仿 真 通过仿真实验 加深对里程计工作原理的了解 同时对移动机器人的定位 方法有一个基础的掌握 2 2实验内容及步骤实验内容及步骤 本实验主要包括以下几个方面的内容 1 Matlab 的编程学习 通过对该软件的学习编程 掌握一些数学工具的使 用 2 对航位推算的代码进行分析 结合里程计的工作原理 对计算方法进行 分析 掌握计算的基本过程 及编程中用到的 Matlab 函数的使用方法 3 学习 Matlab 的简单的绘图函数的使用 4 在例程的基础上 进行一定程度的修改 5 对于掌握的较好的同学 可以做更进一步的实验 增加噪声 观察因为 传感器的噪声导致机器人路径的偏差变化 精品文档 3欢迎下载 3 3 MatlabMatlab 中运行代码得到小车轨迹中运行代码得到小车轨迹 图一 写入代码 精品文档 4欢迎下载 图二 运行程序后得到小车运动轨迹 3 3收获及建议收获及建议 本次实验让我接触到了 MATLAB 我切身感觉到 MATLAB 给我们实验仿真带来 的好处 这次实验也让我得到由自己设定的运行轨迹 甚是欣喜 附录 1 样例代码 myDR m clc clear Sr 0 3 4 2 0 0 5 0 0 2 0 4 0 0 0 0 0 0 0 0 左轮单位时间内走过的距离 相当于是左编码器的采样值 给出了每个时 刻下编码器的读数 Sl 0 2 2 0 5 2 0 0 5 2 3 5 3 3 2 4 3 4 5 0 右轮单位时间内走过的距离 相当于是右编码器的采样值 O 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 小车在 T 时间相对于水平角度 X 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 用于存储小车当前 X 坐标 计 算的定位结果 Y 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 用于存储小车当前 Y 坐标 L 10 小车宽度 for i 1 19 X i 1 X i Sr i Sl i cos O i Sr i Sl i 2 L 2 计算下 一时刻 X Y i 1 Y i Sr i Sl i sin O i Sr i Sl i 2 L 2 计算下 一时刻 Y O i 1 O i Sr i Sl i L 计算下一时刻角度 end 精品文档 5欢迎下载 plot X Y s Color r LineWidth 2 MarkerEdgeColor k MarkerFaceColor g MarkerSize 8 绘制以 X Y 为坐标的关键点的轨迹图 它相当于各个时刻下的机器人 的位置 定位实现了 grid on xlabel X 给 X 轴加注 ylabel Y text 0 0 5 小车起点 text 10 3 8 小车终点 title 航位推算坐标图 加图注 myDR m 附录 2 航位推算基本原理的 温习 仅仅利用里程计来实现定位的基本原理如图 1 所示 图 1 精品文档 6欢迎下载 设 t 时刻 移动机器人的位姿为 分别表示机器人从 t 到 ttt x y t s t t 1 时刻机器人运动的距离和绝对航向 相对航向定义为前后两个时刻绝对航 向之差 用 表示 则 t 1 时刻移动机器人的位姿可表示为 3 1 1 1 11 cos sin tttt tttt ttt xxs yys 从 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 横 纵坐标以及航向角度所发生的变化 如果假定移动 机器人在行驶过程中理想 即路面平整 无打滑 则有 精品文档 7欢迎下载 3 2 2 2 2 R RL L RL SRL SSL SRL SSS RS 因为图 3 2 中有 则为等腰三角形 则成立 11 OOO MR 1 OO M 根据弦切角定理可知 22 sin 2 OMONR 3 3 cos 2 sin cos 222 sin cos 222 sin cos 222 xOMR s s 3 4 sin 2 sin sin 222 sin sin 222 sin sin 222 yOMR s s 当对传感器的信息采样时间足够短或者移动机器人做直线运动时 有以下 两个式子成立 则根据 3 3 和 3 4 式可以化0 sin 2 2 1 简为 3 5 cos 2 sin 2 xs ys 通过对 3 2 与 3 5 式联立求解可以得到移动机器人的离散运动方程 精品文档 8欢迎下载 3 6 1 1 1 cos 22 sin 22 R tL tR tL t ttt R tL tR tL t ttt R tL t tt SSSS xx L SSSS yy L SS L 其中 分别表示移动机器人左右两侧轮子从 t 到 t 1 时刻移动的里程 R t S L t S 将移动机器人的位置向量设为 采样周期为 dt dt 应该比较小 这 T ttt x y 样才能满足在采样周期内机器人做线性运动 其输入控制向量 它们的噪声可近似为 的高 T tR tL t uSS T tR tL t www 2 0 R t 2 0 L t 斯白噪声 根据 3 6 式 考虑到控制输入量噪声的影响 可得移动机器人的 位置向量为 3 7 1 1 1 cos 22 sin 22 2 t tttt t R tR tL tL tR tR tL tL t tt R tR tL tL tR tR tL tL t tt R tR tL tL t t x f X u wy SwSwSwSw x L SwSwSwSw y L SwSw 根据 3 6 式可以计算小车的航迹 输入量为小车左右轮在一个时间单位所走的路程 L 为小车的车宽 是已知量 通过公式可以计算出小车的当前相对位置 及 X Y 附录 3 参考资料 1 Matlab 绘图教程 大量实例 PPT ppt 2 matlab 简明实例教程 doc 3 自学 MATLAB 很好的资料 很多 全部免费 doc 4 一个