怎么查网站死链,网站建设上传视频,做美食网站的目的和意义,wordpress 主题名字参考引用 GMapping ROS-Wiki从零开始搭二维激光SLAMGMapping 漫谈小白学移动机器人机器人工匠阿杰wpr_simulation 移动机器人激光SLAM导航#xff08;文章链接汇总#xff09;
1. GMapping
1.1 FastSLAM 问题分解 概率论相关公式 1. 条件概率公式#xff1a; p ( x , y ) … 参考引用 GMapping ROS-Wiki从零开始搭二维激光SLAMGMapping 漫谈小白学移动机器人机器人工匠阿杰wpr_simulation 移动机器人激光SLAM导航文章链接汇总
1. GMapping
1.1 FastSLAM 问题分解 概率论相关公式 1. 条件概率公式 p ( x , y ) p ( x ∣ y ) p ( y ) p ( y ∣ x ) p ( x ) p(x,y)p(x|y)p(y)p(y|x)p(x) p(x,y)p(x∣y)p(y)p(y∣x)p(x)2. 贝叶斯公式 p ( x ∣ y ) p ( y ∣ x ) p ( x ) p ( y ) η p ( y ∣ x ) p ( x ) , η 为归一化常数 p(x|y)\frac{p(y|x)p(x)}{p(y)}\eta p(y|x)p(x), \eta为归一化常数 p(x∣y)p(y)p(y∣x)p(x)ηp(y∣x)p(x),η为归一化常数3. 条件贝叶斯公式 p ( x ∣ y , z ) p ( y ∣ x , z ) p ( x ∣ z ) p ( y ∣ z ) η p ( y ∣ x , z ) p ( x ∣ z ) p(x|y,z)\frac{p(y|x,z)p(x|z)}{p(y|z)}\eta p(y|x,z)p(x|z) p(x∣y,z)p(y∣z)p(y∣x,z)p(x∣z)ηp(y∣x,z)p(x∣z)4. 条件联合概率公式 p ( x , y ∣ z ) p ( x ∣ y , z ) p ( y ∣ z ) p(x,y|z)p(x|y,z)p(y|z) p(x,y∣z)p(x∣y,z)p(y∣z) FastSLAM 算法采用 RBPFRao-Blackwellized Particle Filters 方法将 SLAM 分解成两个问题机器人定位问题 已知机器人定位进行地图构建问题。在定位问题中每一个粒子只需要表示一个可能的姿态但 SLAM 中的姿态和地图都是变量所以一个粒子需要同时保存所有历史时刻的机器人位姿和整个地图 p ( x 1 : t ∣ z 1 : t , u 1 : t − 1 ) p(x_{1:t}\mid z_{1:t},u_{1:t-1}) p(x1:t∣z1:t,u1:t−1) 利用上一时刻里程计数据和当前时刻的传感器观测来估计机器人的轨迹/位姿定位 p ( m ∣ x 1 : t , z 1 : t ) p(m\mid x_{1:t},z_{1:t}) p(m∣x1:t,z1:t) 利用当前时刻的机器人的轨迹/位姿和传感器观测来估计环境地图建图通过上一个时刻的地图和运动模型预测当前的位姿然后计算权重即在当前位姿下得到当前观测的可能性再进行重采样进而更新粒子地图如此往复 p ( x 1 : t , m ∣ z 1 : t , u 1 : t − 1 ) p ( m ∣ x 1 : t , z 1 : t ) ⋅ p ( x 1 : t ∣ z 1 : t , u 1 : t − 1 ) . \begin{matrix}p(x_{1:t},m\mid z_{1:t},u_{1:t-1})\\p(m\mid x_{1:t},z_{1:t})\cdot p(x_{1:t}\mid z_{1:t},u_{1:t-1}).\end{matrix} p(x1:t,m∣z1:t,u1:t−1)p(m∣x1:t,z1:t)⋅p(x1:t∣z1:t,u1:t−1).
1.2 GMapping 改善方案
1.2.1 改善提议分布降低粒子数量 问题建图对机器人位姿要求较高对任意一个粒子仅仅依靠运动模型采样里程计的结果构建地图误差会非常大通过运动模型得到的提议分布proposal太过于分散与真实分布相差过大则需要较多的粒子才能更好的表示机器人位姿的估计这样将会造成内存使用与计算量急剧上升 方案 1采用极大似然估计 里程计的概率模型比较平滑范围较大如果对整个范围采样将需要很多的粒子如果能找到一个位姿优值在其周围进行小范围采样就可以降低粒子数量因此直接采用极大似然估计的方式根据粒子的位姿预测分布与地图的匹配程度通过扫描匹配找到粒子的最优位姿参数就用该位姿参数直接当做新粒子的位姿 方案 2激光雷达的观测模型 粒子滤波采用里程计运动模型作为提议分布导致大部分粒子都偏离真实位置而与运动模型相反激光雷达的观测模型则可以给出一个相对集中的分布通过激光雷达最近一帧的观测scan把 proposal 分布限制在一个狭小的有效区域然后再对 proposal 分布进行采样这样可以用更少的粒子来覆盖机器人位姿的概率分布从下图可以看到激光雷达观测模型方差较小 总结如下 1、首先按照里程计运动模型给出预测图 1 中所有粒子通过里程计运动模型预测出新的位置如图 2 所示2、以图 2 粒子的预测位置为初始值对每一个粒子进行一次扫描匹配通过扫描匹配得到该粒子的最优位姿如图 3使得当前观测与该粒子所携带的地图最为贴合3、得到每个粒子的最优位姿后再在每个粒子的附近撒播 k 个粒子用高斯函数来模拟提议分布如图 4 所示根据这 k 个粒子的里程计和观测模型来计算高斯分布均值和方差4、新的粒子从高斯分布中采样得到对于每个粒子还需要赋予一个权值以供后续的重采样步骤使用
1.2.2 限制重采样次数缓解粒子耗散 问题GMapping 采用粒子滤波算法对移动机器人轨迹进行估计必然少不了粒子重采样过程但随着重采样次数的增多会出现所有粒子都从一个粒子复制而来粒子的多样性会消失 解决方案自适应重采样 重采样的目的是抛弃那些明显远离真实值的粒子增强那些离真实值近的粒子 如果所有的粒子都在真实值附近且分布均匀那么就没有理由执行重采样在回环发生之前即使有些粒子已经远离了真实值但现有的观测不足以区分正确的粒子和错误的粒子也没有理由执行重采样只有在回环发生之后新的观测彻底拉开正确粒子与错误粒子权重差距此时重采样才能有效 根据所有粒子自身权重的离散程度也就是权重方差来决定是否进行粒子重采样的操作 Neff 越大粒子的权重差距越小当 Neff 小于某个阈值说明粒子的权重差距较大即粒子的分布与真实分布差距很大在粒子层面表现为某些粒子离真实值很近而很多粒子离真实值较远。这正是回环发生时经常出现的情况此时就应该进行重采样否则不进行采样 N e f f 1 ∑ i 1 N ( w ~ ( i ) ) 2 N_{\mathrm{eff}}\frac1{\sum_{i1}^N\left(\tilde{w}^{(i)}\right)^2} Neff∑i1N(w~(i))21
1.3 GMapping 解析
1.3.1 简介
GMapping 是一个基于 2D 激光雷达使用 RBPF 算法完成二维栅格地图构建的 SLAM 算法 通过里程计数据获取粒子群的先验位姿再通过雷达数据与地图的匹配程度对所有粒子进行打分通过分数高的粒子群来近似机器人的真实位姿作为 ROS 节点称为 slam_gmapping 硬件要求 需要一个提供里程计数据并配备一个水平安装的、固定的 2D 激光雷达的移动机器人slam_gmapping 节点将尝试将每次传入的扫描scan转化为 odom (里程计) tf 坐标系 优缺点 可实时构建室内环境地图在小场景中计算量少且地图精度较高对激光雷达扫描频率要求较低不适合大场景建图 对于 200 x 200 米的范围如果栅格分辨率是 5cm每个栅格占用一个字节内存那么每个粒子携带的地图都要 16M 的内存如果是 100 粒子就是 1.6G 内存
1.3.2 话题节点
订阅的话题Topic /tf (tf/tfMessage)订阅激光雷达坐标系、基坐标系和里程计坐标系之间的变换/scan (sensor_msgs/LaserScan)订阅 2D 激光雷达扫描数据 发布的话题Topic map_metadata (nav_msgs/MapMetaData)发布 Meta元地图数据map (nav_msgs/OccupancyGrid)发布占据栅格地图数据~entropy (std_msgs/Float64)发布机器人定位的置信度越大则定位错误的可能性越大 Service dynamic_map (nav_msgs/GetMap)获取地图数据
1.3.3 TF 变换 有关 TF 基础请查看往期博客ROS学习5ROS常用组件 必要的 TF 变换 scan frame → base_link激光雷达坐标系 与 基坐标系 之间的变换一般由 robot_state_publisher 或 static_transform_publisher 发布base_link → odom基坐标系 与 里程计坐标系 之间的变换一般由里程计节点发布 发布的 TF 变换 map → odom地图坐标系 与 里程计坐标系 之间的变换估计机器人在地图 map 坐标系中的位姿
1.4 建图测试 本小节使用 移动机器人激光SLAM导航二运动控制与传感器篇 中安装的测试环境 wpr_simulationGMapping 已在此环境中通过脚本安装 沿用 移动机器人激光SLAM导航三Hector SLAM 篇 中 2.4 小节创建的功能包 slam_pkg在 slam_pkg 中的 launch 文件夹中新建 gmapping.launch 文件launchinclude file$(find wpr_simulation)/launch/wpb_stage_robocup.launch /node pkggmapping typeslam_gmapping nameslam_gmapping /node pkgrviz typerviz namerviz args-d $(find slam_pkg)/rviz/slam.rviz /node pkgwpr_simulation typekeyboard_vel_ctrl namekeyboard_vel_ctrl /
/launch编译并启动 gmapping.launch 建图$ cd ~/wpr_ws
$ catkin_make
$ source devel/setup.bash
$ roslaunch slam_pkg gmapping.launch机器人运动控制节点 keyboard_vel_ctrl#include ros/ros.h
#include geometry_msgs/Twist.h
#include stdio.h
#include termios.hstatic float linear_vel 0.1;
static float angular_vel 0.1;
static int k_vel 3;int GetCh() {static struct termios oldt, newt;tcgetattr(STDIN_FILENO, oldt);newt oldt;newt.c_lflag ~(ICANON);tcsetattr(STDIN_FILENO, TCSANOW, newt);int c getchar();tcsetattr(STDIN_FILENO, TCSANOW, oldt);return c;
}int main(int argc, char** argv) {ros::init(argc, argv, keyboard_vel_cmd);printf(键盘控制 WPR 机器人: \n);printf(w - 向前加速 \n);printf(s - 向后加速 \n);printf(a - 向左加速 \n);printf(d - 向右加速 \n);printf(q - 左旋加速 \n);printf(e - 右旋加速 \n);printf(空格 - 刹车 \n);printf(x - 退出 \n);printf(------------- \n);ros::NodeHandle n;ros::Publisher cmd_vel_pub n.advertisegeometry_msgs::Twist(/cmd_vel, 10);geometry_msgs::Twist base_cmd;base_cmd.linear.x 0;base_cmd.linear.y 0;base_cmd.angular.z 0;while(n.ok()) {int cKey GetCh();if (cKey w) {base_cmd.linear.x linear_vel;if (base_cmd.linear.x linear_vel*k_vel)base_cmd.linear.x linear_vel*k_vel;cmd_vel_pub.publish(base_cmd);printf( - linear.x %.2f linear.y %.2f angular.z %.2f \n,base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);} else if (cKey s) {base_cmd.linear.x -linear_vel;if (base_cmd.linear.x -linear_vel*k_vel)base_cmd.linear.x -linear_vel*k_vel;cmd_vel_pub.publish(base_cmd);printf( - linear.x %.2f linear.y %.2f angular.z %.2f \n,base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);} else if (cKey a) {base_cmd.linear.y linear_vel;if (base_cmd.linear.y linear_vel*k_vel)base_cmd.linear.y linear_vel*k_vel;cmd_vel_pub.publish(base_cmd);printf( - linear.x %.2f linear.y %.2f angular.z %.2f \n,base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);} else if (cKey d) {base_cmd.linear.y -linear_vel;if (base_cmd.linear.y -linear_vel*k_vel)base_cmd.linear.y -linear_vel*k_vel;cmd_vel_pub.publish(base_cmd);printf( - linear.x %.2f linear.y %.2f angular.z %.2f \n,base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);} else if (cKey q) {base_cmd.angular.z angular_vel;if (base_cmd.angular.z angular_vel*k_vel)base_cmd.angular.z angular_vel*k_vel;cmd_vel_pub.publish(base_cmd);printf( - linear.x %.2f linear.y %.2f angular.z %.2f \n,base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);} else if (cKey e) {base_cmd.angular.z -angular_vel;if (base_cmd.angular.z -angular_vel*k_vel)base_cmd.angular.z -angular_vel*k_vel;cmd_vel_pub.publish(base_cmd);printf( - linear.x %.2f linear.y %.2f angular.z %.2f \n,base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);} else if (cKey ) {base_cmd.linear.x 0;base_cmd.linear.y 0;base_cmd.angular.z 0;cmd_vel_pub.publish(base_cmd);printf( - linear.x %.2f linear.y %.2f angular.z %.2f \n,base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);} else if (cKey x) {base_cmd.linear.x 0;base_cmd.linear.y 0;base_cmd.angular.z 0;cmd_vel_pub.publish(base_cmd);printf( - linear.x %.2f linear.y %.2f angular.z %.2f \n,base_cmd.linear.x,base_cmd.linear.y,base_cmd.angular.z);printf(退出 \n);return 0;} else {printf( - 未定义指令 \n);}}return 0;
}1.5 建图参数设置 Parameters launchinclude file$(find wpr_simulation)/launch/wpb_stage_robocup.launch /node pkggmapping typeslam_gmapping nameslam_gmappingparam namemaxUrange value3.0 /param namemap_update_interval value0.5 /param namelinearUpdate value0.1 //nodenode pkgrviz typerviz namerviz args-d $(find slam_pkg)/rviz/slam.rviz /node pkgwpr_simulation typekeyboard_vel_ctrl namekeyboard_vel_ctrl /
/launch2. 地图的保存与加载 使用 map_server 软件包实现占据栅格地图的保存与加载 2.1 保存地图 GMapping 建图 $ cd ~/wpr_ws
$ source devel/setup.bash
$ roslaunch slam_pkg gmapping.launch保存地图 $ rosrun map_server map_saver -f mymap通过上述保存地图指令会在 /home 目录生成 mymap.pgm地图图片和 mymap.yaml地图信息 两个文件 mymap.yaml image: mymap.pgm # 到包含占用数据的图像文件的路径可以是绝对的也可以是相对于 YAML 文件的位置
resolution: 0.050000 # 地图分辨率单位米/像素
origin: [-100.000000, -100.000000, 0.000000] # 地图中左下方像素的二维姿态为 (x, y, 偏航)偏航为逆时针旋转(偏航 0表示无转动)
negate: 0 # 白/黑的空闲/被占用 语义是否应该颠倒 (阈值的解释不受影响)
occupied_thresh: 0.65 # 占据概率大于该阈值的像素被认为是完全占据的
free_thresh: 0.196 # 占有率小于这个阈值的像素被认为是完全自由的2.2 加载地图
加载地图$ roscore$ cd ~
$ rosrun map_server map_server mymap.yaml$ rosrun rviz rviz3. 2D 激光雷达运动畸变去除
详解2D激光雷达运动畸变去除
