《概率机器人》里程计运动模型gmapping中代码解析
来源:互联网 发布:g84攻丝车床编程实例 编辑:程序博客网 时间:2024/05/21 00:55
里程计运动模型(odometery motion model)用距离测量代替控制。实际经验表明虽然里程计虽然仍存在误差,但通常比速度运动模型更加的精确。相比于速度运动模型运动信息
为了提取相对的距离,
上图就是里程计的测距模型,同样这些旋转和平移都是有噪声的。
首先由里程计算
算法的2-4行是从里程计的读数获取相对运动参数
第5-7行是相同的,就是计算位姿为
第8-10行是计算误差概率,
第11行返回各自的误差概率P1 ,P2 ,P3相乘得到的组合误差概率,假定不同误差源之间是相互独立的,变量
基于里程计运动模型的采样算法
数学推导:根据上上面的图应该不难推导出算法中的
这里的时刻t的位姿用
那么根据给定的运动模型采样算法,,对于不同的误差参数也会有不同的概率分布:
第一个模型的采样参数是中等的,可以说是正常的,第二个和第三个扥别是比较大的平移和旋转的误差所造成的。
Gmapping中实现里程计运动模型的采样程序如下:
这个函数的输入是机器人的当前位姿pnew和上一时刻的机器人的位姿pold,
OrientedPoint MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{ double sxy=0.3*srr; //srr我理解为两轮子里程计的方差 OrientedPoint delta=absoluteDifference(pnew, pold); //具体如下面的介绍 OrientedPoint noisypoint(delta);//存储噪声估计 noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y)); noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x)); noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y)); noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);if (noisypoint.theta>M_PI) noisypoint.theta-=2*M_PI; return absoluteSum(p,noisypoint);}
首先解释一下函数:
OrientedPoint delta=absoluteDifference(pnew,pold); double
具体的 内容如下
orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){ orientedpoint<T,A> delta=p1-p2; delta.theta=atan2(sin(delta.theta), cos(delta.theta)); double s=sin(p2.theta), c=cos(p2.theta); return orientedpoint<T,A>(c*delta.x+s*delta.y, -s*delta.x+c*delta.y, delta.theta);}
就是计算位姿的变化量,这个OrientedPoint delta的计算 结果对应的理论公式的结果就是
计算新旧帧的绝对误差.具体就不再深入。
那么对于其中的三行代码是分别给位姿的三个两添加噪声进去,为什么要这样写呢?
noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y)); noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x)); noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
首先我们对sampleGaussian(
接下来的程序
noisypoint.theta=fmod(noisypoint.theta, 2*M_PI); if (noisypoint.theta>M_PI) noisypoint.theta-=2*M_PI;
百度一下fmod()函数是对浮点型数据进行取模运算,就是计算
noisypoint.theta/2*M_PI的余数。 因为要把角度差限定在
最后是返回累计的噪声:
absoluteSum(p,noisypoint);
这个函数的实现代码是
template <class T, class A>orientedpoint<T,A> absoluteSum(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){ double s=sin(p1.theta), c=cos(p1.theta); return orientedpoint<T,A>(c*p2.x-s*p2.y, s*p2.x+c*p2.y, p2.theta) + p1;
P1是当前机器人的位姿,前面的c*p2.x-s*p2.y, s*p2.x+c*p2.y, p2.theta
是分别在P1的位姿上加上各自的噪声分量。
*这是我个人的理解,可能有一些偏差,或者错误,有错误还请指正,当然不喜勿喷
- 《概率机器人》里程计运动模型gmapping中代码解析
- 《概率机器人》速度运动模型gmapping中代码解析
- 机器人 里程计 下的 粒子滤波 相关介绍~~,持续更新(gmapping)
- 移动机器人运动模型
- 二轮差动模型,机器人底盘chassis里程计计算。
- 先锋机器人gmapping配置详解
- gmapping 算法解析
- 运动机器人在rviz中同步显示
- 机器人概率定位学习笔记第三篇_使用python建立机器人运动函数(move fuction)
- gmapping
- 手机控制urdf小车模型运动(二)-创建简单的机器人模型
- (二)ROS中控制机器人运动(示例运行)
- pku1573机器人运动
- css3 机器人运动
- 机器人的运动范围
- 机器人的运动范围
- 机器人的运动范围
- 机器人的运动范围
- pom.xml packaging 修改为 war 时候报错
- django 大数据检测网站制作总结
- Mysql的基本使用
- CSDN积分获取
- java数据结构与算法之数组篇
- 《概率机器人》里程计运动模型gmapping中代码解析
- 如何用C语言解决逻辑问题幸运的数字
- 简单算术表达式求值
- 使用WampServer和DVWA在Windows10上搭建渗透测试环境
- 抽象工厂模式
- EA&UML日拱一卒-状态图::内部迁移
- 仅通过js实现颜色渐变
- [bzoj4419][Shoi2013]发微博(STL)
- 自定义 ViewGroup(一)