移动机器人建图与导航代码实现——1.Hector SLAM
Hector SLAM
这一部分利用hector slam完成建图和定位,暂无全局定位功能,使用2D激光雷达和里程计。测试中使用的GUI改编自https://www.cnblogs.com/scomup/p/7074847.html 。
整体分为4步:
1.motion prediction,即估计当前姿态和上一时刻的变化量
2.scan matching,通过激光点云修正上一步的估计,也是该算法的重点
3.map update,更新地图
4.pose update,更新姿态
地图
首先我们需要确定地图的格式,采用栅格地图。设一个二维数组\(\overline{M}\),值域为\([-\infty, +\infty]\),数值越大,表示该点为障碍物的概率越大,反之越小。将其映射到\((0, 1)\)上,可以看作概率,因此设\(M_{ij} = \frac{e^{\overline{M}_{ij}}}{1+e^{\overline{M}_{ij}}}\),即可看作是概率栅格地图。源码在GridMap.py中,地图更新部分有很慢的Python实现,在注释部分中,也有较快的C++实现(c_extern/map_update.cpp),测试效率差30倍。
地图更新
考虑激光点云中的一个点\(P\)和当前机器人位置\(Q\),\(P\)附近的点是障碍物的概率应该增大,从\(Q\)到\(P\)线的点是障碍物的概率应该减小。
motion prediction
这一步是为了将激光点云粗略地从机器人坐标系映射到世界坐标系。设k时刻的里程计对应变换矩阵为\(T_k\),经过修正的姿态为\(\widetilde{\xi}_k\),则可以初步估计k+1时刻姿态为\(\widetilde{\xi}_kT^{-1}_kT_{k+1}\)。
scan matching
设激光点云有n个点,我们希望估计一个姿态\(\xi=\begin{bmatrix}x\\y\\ \theta\end{bmatrix}\),使得点云尽可能分布在障碍物上,使用最小二乘,即求
\(\xi=argmin_{\xi}\sum_{i=0}^n[1-M(S_i(\xi))]^2\),
其中\(S_i(\xi)\)表示第i个点在姿态\(\xi\)下的坐标,\(M\)为已有的地图。设姿态变化量为\(\Delta\xi\),优化目标为
\(\sum_{i=0}^n[1-M(S_i(\xi+\Delta\xi))]^2\),
对其泰勒展开
\(\sum_{i=0}^n[1-M(S_i(\xi))-\nabla M(S_i(\xi))\frac{\partial S_i(\xi)}{\partial \xi}\Delta \xi]^2\)
求偏导并令为0
\(2\sum_{i=0}^n[\nabla M(S_i(\xi))\frac{\partial S_i(\xi)}{\partial \xi}]^T[1-M(S_i(\xi))-\nabla M(S_i(\xi))\frac{\partial S_i(\xi)}{\partial \xi}\Delta \xi]=0\)
求得
\(\Delta\xi=H^{-1}\sum_{i=1}^n[\nabla M(S_i(\xi))\frac{\partial S_i(\xi)}{\partial \xi}]^T[1-M(S_i(\xi))]\)
其中
\(H=\sum_{i=1}^n[\nabla M(S_i(\xi))\frac{\partial S_i(\xi)}{\partial \xi}]^T[\nabla M(S_i(\xi))\frac{\partial S_i(\xi)}{\partial \xi}]\)
更新姿态为\(\xi=\xi+\Delta\xi\)。
这一部分代码见SLAM.py。
栅格地图上偏微分求法
\begin{align*} \nabla M(S_i(\xi))\frac{\partial S_i(\xi)}{\partial \xi} =&\begin{bmatrix} \frac{\partial M(S_i(\xi))}{\partial x}\\ \frac{\partial M(S_i(\xi))}{\partial y}\\ \frac{\partial M(S_i(\xi))}{\partial \theta} \end{bmatrix}\\ =&\begin{bmatrix} \frac{\partial M(S_i(\xi))}{\partial x}\\ \frac{\partial M(S_i(\xi))}{\partial y}\\ (-xsin\theta-ycos\theta)\frac{\partial M(S_i(\xi))}{\partial x} + (xcos\theta-ysin\theta)\frac{\partial M(S_i(\xi))}{\partial y} \end{bmatrix}\\ =&\begin{bmatrix} 1 & 0\\ 0 & 1\\ -xsin\theta-ycos\theta & xcos\theta-ysin\theta \end{bmatrix} \begin{bmatrix} \frac{\partial M(S_i(\xi))}{\partial x}\\ \frac{\partial M(S_i(\xi))}{\partial y} \end{bmatrix} \end{align*}
其中的
\[ \begin{bmatrix} \frac{\partial M(S_i(\xi))}{\partial x} \\ \frac{\partial M(S_i(\xi))}{\partial y} \end{bmatrix} \]
可以用附近的四个点做双线性插值,不妨设\(S_i(\xi)=\begin{bmatrix}x\\y\end{bmatrix}\),
则有
\begin{align*} \frac{\partial M(S_i(\xi))}{\partial x} \approx&([y]+1-y)(M_{[x]+1,[y]}-M_{[x],[y]})+(y-[y])(M_{[x]+1,[y+1]}-M_{[x],[y+1]}) \end{align*} \begin{align*} \frac{\partial M(S_i(\xi))}{\partial y} \approx&([x]+1-x)(M_{[x],[y]+1}-M_{[x],[y]})+(x-[x])(M_{[x]+1,[y+1]}-M_{[x+1],[y]}) \end{align*}
转载于:https://www.cnblogs.com/7391-KID/p/10903392.html
移动机器人建图与导航代码实现——1.Hector SLAM相关推荐
- 移动机器人建图与导航代码实现——3.Path Tracking
Path Tracking 这一部分利用极坐标下的线性控制器完成路径跟踪,主要思路就是把路径拆开,变成p2p问题,即从当前姿态到目标姿态. 极坐标下的线性控制器 设速度\(v\),角速度\(\omeg ...
- 【SLAM建图和导航仿真实例】(三)- 使用RTAB-MAP进行SLAM建图和导航
引言 在这个-SLAM建图和导航仿真实例-项目中,主要分为三个部分,分别是 (一)模型构建 (二)根据已知地图进行定位和导航 (三)使用RTAB-MAP进行建图和导航 该项目的slam_bot已经上传 ...
- 【SLAM建图和导航仿真实例】(一)- 模型构建
引言 在这个-SLAM建图和导航仿真实例-项目中,主要分为三个部分,分别是 (一)模型构建 (二)根据已知地图进行定位和导航 (三)使用RTAB-MAP进行建图和导航 该项目的slam_bot已经上传 ...
- 机器人操作系统ROS—深度相机+激光雷达实现vSLAM建图与导航 转载
原文:机器人操作系统ROS-深度相机+激光雷达实现vSLAM建图与导航_wx5d23599e462fa的技术博客_51CTO博客 这次将带大家学习如何使用深度相机实现机器人的视觉SLAM建图及导航. ...
- ROS 多机器人建图与导航
一.环境配置 参考turtlebot3-多机交互程序:多机交互-创客智造 小车: Ubuntu-mate16.04+ROSkinetic: 必要的bringup.launch文件(包括雷达信息.IMU ...
- 【SLAM建图和导航仿真实例】(二)- 根据已知地图进行定位和导航
引言 在这个-SLAM建图和导航仿真实例-项目中,主要分为三个部分,分别是 (一)模型构建 (二)根据已知地图进行定位和导航 (三)使用RTAB-MAP进行建图和导航 该项目的slam_bot已经上传 ...
- Rtabmap 视觉建图与导航 Ubuntu 18.04 Kobuki
Rtabmap 视觉建图与导航 ROS 部署 创建工作空间 安装Rtabmap RGBD相机配置 Kobuki配置 建图 导航 ROS 部署 请参阅我之前的文章:Ubuntu 18.04 ROS 创建 ...
- walking机器人入门教程-视觉建图-rtabmap使用视觉建图和导航
系列文章目录 walking机器人入门教程-目录 walking机器人入门教程-硬件清单 walking机器人入门教程-软件清单 walking机器人入门教程-测试底盘 walking机器人入门教程- ...
- 室内移动机器人建图,定位,导航方案
采用cartographer进行建图,以及纯定位,重定位. 导航采用move_base. 可能遇到的问题 遇到长廊效应,当特征点数量小于一定的阀值时,应该增加IMU.编码器的可信度,减小激光雷达的可信 ...
最新文章
- 我十年学习编程的历史
- git学习资料整理(知乎搜集的)
- map reduce相关程序
- github java开源项目经验_10月份Github上最热门的Java开源项目
- [WC2018]州区划分(FWT_OR卷积)
- oracle cdc 关闭,Oracle CDC部署流程
- css 可编辑,如何设置DIV可编辑
- c语言利用遍历求树高的程序,用C语言实现二叉树的遍历极其应用
- 【Azure Services Platform Step by Step-第5篇】.NET Services 概述
- 屏蔽关闭百度搜索风云热点的方法
- 2d-x中Lua类型强转问题
- Excel VBA数组使用方法
- Iredmail搭建
- 视频会议系统架构和开源解决系统
- CentOS搭建tg的MTProxy代理
- 手把手教你搭建高逼格监控平台,动起来吧
- mysql中between..and的用法
- IBM X 345服务器无法从光驱启动配置解决方法——非常规方法
- MAC无法重装anaconda3
- Python的turtle模块画爱心箭(附源码)