一直玩的ROS的驱动开发,开发导航基本靠调包,奈何这次给的太多,来位大家分享一些自己知道的关于“移动机器人路径规划”的知识!

​ 不管是搞雷达SLAM还是视觉SLAM,ROS绝对是必不可少的好东西!不过大多数也就是在仿真跑起来看到效果,好点的能有个车来真实的跑跑,雷达建图导航确实炫酷,很容易吸引到学弟学妹们的崇拜!

​ 在操作ROS导航的时候,我们需要先从RVIZ里面读取我们当前鼠标的操作,看鼠标点到了哪里,这里会有个“clicked_point”的话题发布出来。

​ “clicked_point”这个话题会发布一个空间坐标点xyz,同时在RVIZ下的操作会有箭头的方向,这个方向是我们机器人到达点之后的位姿,这个位姿使用四元数进行表示。

​ 这里粘贴古月老师一段代码,关于发布目标点并让机器人到达该点的Demo。

#!/usr/bin/env python 
# -*- coding: utf-8 -*-
 
import roslib;
import rospy  
import actionlib  
from actionlib_msgs.msg import *  
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  

# 节点初始化 
rospy.init_node('move_test', anonymous=True)  
  
# 订阅move_base服务器的消息  
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  

rospy.loginfo("Waiting for move_base action server...")  

# 等待连接服务器,5s等待时间限制 
while move_base.wait_for_server(rospy.Duration(5.0)) == 0:
    rospy.loginfo("Connected to move base server")  

# 设定目标点  
target = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))  
goal = MoveBaseGoal()  
goal.target_pose.pose = target  
goal.target_pose.header.frame_id = 'map'  
goal.target_pose.header.stamp = rospy.Time.now()  

rospy.loginfo("Going to: " + str(target))  

# 向目标进发  
move_base.send_goal(goal)  

# 五分钟时间限制  
finished_within_time = move_base.wait_for_result(rospy.Duration(300))   

# 查看是否成功到达  
if not finished_within_time:  
    move_base.cancel_goal()  
    rospy.loginfo("Timed out achieving goal")  
else:  
    state = move_base.get_state()  
    if state == GoalStatus.SUCCEEDED:  
        rospy.loginfo("Goal succeeded!")
    else:  
      rospy.loginfo("Goal failed! ")  

​ 导航点发布这里用的是action通讯机制,在机器人进行导航的过程当中,需要时刻将自身所处的位姿进行回馈,当达到目标点之后也就完成了整个action通讯。

​ 在进行导航的过程当中,可能会有某些情况需要机器人停下来,比如身为主人的你不想让机器人前往这个点了,那么这个是就需要调用action通讯的cancel来取消当前的导航动作。导航的起始点和目标点的表示,它就是这样的!

target = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))

​ 这个数据分为Point点坐标,也就是机器人需要在当前map下达到的坐标点,另一个是Quaternion位姿,机器人到达目标点了,机器人的角度啊、车头朝哪啊等,就是通过这组四元数来表示的。

​ 问我啥是四元数?百度一下,你就知道!


​ 导航嘛,就是从当前点到达目标点的过程。

​ 我们现在出门离不了手机地图吧,我们知道自己在哪(起始点)知道自己要去哪(目标点),我们打开手机地图软件根据两个位置,很快速的就给我们一条路线。这时候再配合手机的GPS定位,可以实时的看到我们在哪,这就是导航!

​ 知道了两个点,那么就是如何来规划这个路线!这也就是我们这次讲的重点——移动机器人路径规划。

​ SLAM挺火啊最近,那么什么是SLAM?

SLAM,全称是Simultaneous Localization and Mapping,同时定位与建图。把你放到荒郊野外,没有地图你试试,这成”探索式建图导航“了都。就算给你地图,你不知道你在地图上的哪个位置,你导航个嘚嘚,玩都没的玩!画地图的时候,肯定是你一边走一边画吧!画的地方就是你现在所处的位置,或者说,你知道地图上的这个点位置在哪,这也就是定位。

​ 建图的时候肯定得有定位,不然直接原地起飞,地图早就不知丢哪了!

​ 也就是说啊,SLAM完成的工作是建图,我们拿到建好的地图之后,在地图上找到我们现在在哪,在地图上找到我们要去哪,在这两个地方之间规划出来一条路线并执行,这才是导航!

​ 导航的过程,你肯定得知道你走到哪了,这里也就是又需要定位!不过好一点的是我们有地图,我们知道自己走了多远,可以在地图上大致的知道自己在地图上的哪个点!

​ 机器人导航是怎么样的呢?也是这样!

​ 机器人自身有传感器,比如轮子的编码器、自身的陀螺仪、深度相机的视觉信息、雷达的信息等等,这些我们叫做里程计,通过里程计信息,机器人就可以对自身的运动做出一个大致的评估,这也就是Odom!

​ 这个是move_base的框架图!可以看出来最上面的move_base_simple和geometry_msgs这里是目标点的数据输入!

​ 整个框架先从左侧看,需要机器人的坐标关系tf和里程计Odom,我们需要这两个数据来完成定位,图上的定位算法是AMCL(Adaptive Monte Carlo Localization)自适应蒙特卡洛定位。(PS:听不懂?百度一下,你就知道!)

​ 右上角部分的是map_server加载当前的地图文件,然后就算传感器数据sensor souuces的接入,这里的传感器分LaserScan雷达数据或者PointCloud深度相机的点云数据。

​ 在框架的内部,首先经历的是global_planer全部规划器规划出一条路径,在运动的过程当中再由local_planer局部规划器规划出一条可实时“走”的路线。在这个过程当中,两个规划分别通过global_costmap全局代价地图和local_costmap局部代价地图来规划设计,其中遇到导航失败的情况(路径下出现障碍物、获取自身位置失败等)会启动recovery_behaviors恢复机制来尝试解决问题。

​ move_base框架所输出的是“cmd_vel”数据到base_controller底盘控制器,从而来驱动底盘电机带动机器人运动。

​ “cmd_vel”输出的是xyz三个方向的速度值和角速度值,这里我们需要根据自己底盘的情况,比如两轮差分轮结构、四轮差分结构、阿克曼转向结构、麦克纳姆轮全向结构等等,通过运动学正逆解公式,将“cmd_vel”的数据转换成轮子的速度值,最终通过PID将轮子的速度值转成PWM来实现对轮子电机的速度控制。

​ 到这里我得多说一句——轮子的编码器在这里至关重要!!!

​ 通过轮子的编码器,我们可以得到电机控制轮子转了几圈,将这个圈数和轮子的周长等连接起来,就可以获取里程计数据,里程计是我们可以直观的知道机器人跑到了哪里,就像我们可以根据速度和时间判断还有多久才能到或者走了多远是一样的。但编码器的功能不仅仅只是这样,因为我们对电机轮子转速的控制需要PID!

​ base_controller底盘控制是由单片机做的电机控制板,我们通过PWM信号来控制电机的转速,这里我们需要实现轮子转速值和电机脉冲值的转换,这里用的算法就是PID!(怎么实现的可以百度一下)

​ 不是不想写,内容比较多,要是写估计得再开篇博客了都!


​ 建图算法有很多,gmapping、hector、cartographer、rtabmap等等,这里我们重点来说的是路径规划算法!

开局给你一张图,告诉你起点和终点,你给我一条最优的路径,这就是路径规划算法干的事!

​ 深入搞过ROS的,对于Dwa、Teb、MPC等规划器应该是有了解的,我所涉及这里的也仅仅只是调参数,也不会去手写这些东西(我是菜鸡)。我们这里所涉及的,也仅仅只是一些算法的思想,怎么实现迁移的,各位读者加油!

​ 算法嘛,那数据结构肯定是少不了的,事实也确实是这样的!

​ 再讲算法之前,我还得普及一些基础概念,你可以理解为我在凑字数!

​ 数据结构啊,我是高中接触的编程,当时所涉及的也只有是算法竞赛,数据结构确实是至关重要。穷举贪心冒泡、分治减治广治、广度深度迭代、堆栈队列图优化,慢慢的心酸史。这里我要讲的是迪杰斯特拉(Dijkstra)路径规划算法和蒙特卡洛(AMCL)定位算法(等我再开篇博客)!

​ 我们先来聊一下深度优先搜索和广度优先搜索!

​ 深度优先搜索嘛,不撞南墙不回头!就是沿着一条走到底,走不通了再换其他的路!

​ 广度优先搜索的话,就是一个个都去试试,这层不通了再去下一层!

​ 我们这里以从A到H的规划为例,ABCDEFGH的顺序反应的就是深度与广度的不同!

​ 有兴趣的读者可以去阅读《算法神探》一书,在这本小说中你紧跟Frank侦探的步伐,通过一系列的算法来找到罪犯!该书的电子版可在古月居公众号获取!


开局一张图,告诉你起点和终点,你告诉我怎么走!

​ 在这张地图上面,有七个城市分别为ABCDEFG,它们之间相互的距离已经在图上标出。比如A城市和F城市之间的距离为16m(这个国家很小,数据我随便写的,没过脑子,不想改图了,将就一下,有那个意思!)。

​ 再比如A城市和E城市之间是无法直接达到的,路线的话有A—G—E、A—F—E、A—B—F—E等等有好几条路线,我们需要的就是找出最优的路线。

​ 人可以很直观的计算出来,但是机器人不一样,机器人需要程序来计算,这里我们需要来写个程序,求解路线!这也就是”开局一张图,告诉你起点和终点,你告诉我怎么走!“!

​ 我们先来做个”表“!如下所示。

A B C D E F G
A 0 12 16 14
B 12 0 10 7
C 10 0 3 5 6
D 3 0 4
E 5 4 0 2 8
F 16 7 6 2 0 9
G 14 8 9 0

​ 如果两个城市之间可以直接达到的,我们填写实际距离,如果是无法直接到达的,我们以“∞”来表示。接下来呢~

​ 我们现在想从A城市到E城市,A城市到E城市之间有很多条路可以选择,我们需要找出距离最近的一条路线。

​ A城市到E城市不能直接到达,中间要经过其他的城市,那么这个时候我们有以下几种选择:

​ 选择1:A城市—G城市—E城市,共计22m;

​ 选择2:A城市—F城市—E城市,共计18m;

​ 选择3:A城市—B城市—F城市—E城市,共计27m;

​ 等等......

​ 还有其他的几种选择,我们不一一列举。Dijkstra算法可以就这个“图表”计算出最优路径,当然我们这里规定的最优是距离最短,这个距离值,也就是大家耳濡目染的一个名词——“权值”。

Dijkstra算法采用的是一种贪心的策略,声明一个数组dis来保存源点到各个顶点的最短距离和一个保存已经找到了最短路径的顶点的集合:T,初始时,原点 s 的路径权重被赋为 0 (dis[s] = 0)。若对于顶点 s 存在能直接到达的边(s,m),则把dis[m]设为w(s, m),同时把所有其他(s不能直接到达的)顶点的路径长度设为无穷大。初始时,集合T只有顶点s。然后,从dis数组选择最小值,则该值就是源点s到该值对应的顶点的最短路径,并且把该点加入到T中,OK,此时完成一个顶点,然后,我们需要看看新加入的顶点是否可以到达其他顶点并且看看通过该顶点到达其他点的路径长度是否比源点直接到达短,如果是,那么就替换这些顶点在dis中的值。然后,又从dis中找出最小值,重复上述动作,直到T中包含了图的所有顶点。

​ 咳咳!这些东西先往边靠靠,看不懂!

​ 贪心算法(又称贪婪算法)是指,在对问题求解时,总是做出在当前看来是最好的选择。也就是说,不从整体最优上加以考虑,他所做出的仅是在某种意义上的局部最优解。贪心算法不是对所有问题都能得到整体最优解,但对范围相当广泛的许多问题他能产生整体最优解或者是整体最优解的近似解。

​ 还是这张图,我们要从A出发到E,A目前有三个选择,分别是B、F、G,根据贪心策略,我们选择的为B,此时路径长度为12m;在B的时候有两个选择,分别是C、F,根据贪心的策略我们选择F,此时路径的长度为12m+7m=19m,相比较A—F和A—G,这是完全不可取的,但是这个数值我们需要记录下来,因为后面可能出现比这个数值更加不可取的情况。

​ 于是我们需要退回到A,B已经是不可选的,我们需要在F、G之间做出选择,根据贪心的策略,我们选择G,此时路径长度为14m;G点可以直接到达E点,此时路径总长为14m+8m=22m。目前看来我们已经完成了任务,但这是最优的吗?

​ 不一定,因为“贪心策略”,它只会选择当前最好的,而不是整体方面最好的,这就是“贪”!

​ 我们在来看下A—F的情况,如果选择A—F,此时的路径距离为16m,F点可直接达到E,此时路径距离为16m+2m=18m,比A—G—E的22m还要短。

​ 看!“贪心”出问题了吧!那么还有没有其他更短的呢?我们需要来遍历整个数据。我们每次找到最短路径的情况下,我们就需要来替换掉当前所保存的路径。有点绕口,原谅我语言表述不清晰!(PS:老子曰(yue):“道可道,非常道。名可名,非常名。”)

Dijkstra算法=广度优先搜索+“贪心”策略

​ 所以路径规划用Dijkstra算法的话,很容易出现重新规划路径的情况!原因嘛,你品,你细品!

​ 我们现在再来看这段话:

Dijkstra算法采用的是一种贪心的策略,声明一个数组dis来保存源点到各个顶点的最短距离和一个保存已经找到了最短路径的顶点的集合:T,初始时,原点 s 的路径权重被赋为 0 (dis[s] = 0)。

若对于顶点 s 存在能直接到达的边(s,m),则把dis[m]设为w(s, m),同时把所有其他(s不能直接到达的)顶点的路径长度设为无穷大。初始时,集合T只有顶点s。

然后,从dis数组选择最小值,则该值就是源点s到该值对应的顶点的最短路径,并且把该点加入到T中,OK,此时完成一个顶点,

然后,我们需要看看新加入的顶点是否可以到达其他顶点并且看看通过该顶点到达其他点的路径长度是否比源点直接到达短,如果是,那么就替换这些顶点在dis中的值。然后,又从dis中找出最小值,重复上述动作,直到T中包含了图的所有顶点。

​ 还是这张图,我们再来分析一组数据,我们这次规划从A到D的路径。

​ A到D的距离是比较远的,我们来描述下使用Dijkstra该怎么实现这个路径规划。

首先,A的附近有B、F、G三个点,根据贪心策略,我们选择B节点,此时路径距离为12m;

我们从B出发,有C、F两个点,根据贪心策略,我们选择F节点,此时的路径距离为12m+7m=19m。这里的19m比A—F的16m、A—G的14m要大。

我们需要退回A点,根据贪心策略,我们选择G,此时的路径长度为14m,上一次选择的路径距离为19m,根据贪心策略可选。

在G点,我们可以选择F点和E点,根据贪心策略,我们选择E点,此时路径距离为14m+8m=22m,大于A—B—F的19m,所以不可选。

我们退回A点,此时我们只能选择F点,路径距离为16m。在F点,我们可以选择B、C、E、G,根据贪心策略,我们选择E点,此时的路径长度为16m+2m=18m,比之前两次的更短,可选!

在E点,我们可以选择C、D、G三个点,我们的目标就是D,所以直接可以达到。此时的路径长度为16m+2m+4m=22m。

到此为止,我们从A到D的最短路径已经求出,路径为A—F—E—D,路径为22m!不可能再出现臂22m更短的距离,因为我们保证了“之前”的路径是最短的!

是不是很神奇~

​ 在这个过程当中,“贪心”出大错呀!我们每次“贪心”的选项,都是以广度优先为准的。

​ 所以说,Dijkstra算法=广度优先搜索+“贪心”策略


​ 好了,算法有了,那么代码该怎么实现呢?很简单!

百度一下,你就知道!

​ 事实确实如此,关于Dijkstra算法的代码,百度什么的一大堆,C版本的、Python版本的等等。

​ 这个算法怎么搞到自己的ROS系统里面,我不会!写这篇博客纯属是为了大礼包!

​ 我玩机器人还是在驱动和框架方面玩的多,习惯了调包调参来完成自己的项目设计,面向CSDN、Github等编程准没错!

​ 算法的话,呃!细心的会发现,这里的内容其实是数据结构的!

​ 深度优先、广度优先、贪心策略、最短路径......

​ 一个程序,代码是组成,数据结构是框架,那么算法就是灵魂。

​ 我学数据结构应该是在17年,当时看的《啊哈!算法》、《大话数据结构》两本书,然后又在洛谷社区CodeVS等参加一些竞赛类的活动,确实对我现在的开发很有帮助。

​ 机器人开发是一个很综合的学科,除了工程能力之外,理论的支撑也是至关重要,可能一些边边角角的知识,都会对你的项目大有帮助。

​ 在数据结构里面,除Dijkstra算法,弗洛伊德算法(Floyd)也是很经典的,这是五行代码就可以实现的最优路径求解!

​ 除了这两个之外,路径规划算法也还有RRT算法、A算法、D算法、LPA算法、D Lite算法等等,大家可以自行研究。

​ 这里说明一点,我们在ROS系统下使用的地图为栅格地图,这篇博文的地图为拓扑地图。


​ ROS系统下导航所使用的为栅格地图,我们建好地图在保存的时候有两个文件,pgm图片和yaml参数,如下图。

​ 这是我们在ROS系统下使用gmapping算法建好的地图,与这个地图想对应的yaml文件内容如下。

image: JHRobot.pgm
resolution: 0.050000
origin: [-12.200000, -15.400000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

​ 单词啥意思自己翻译下~

​ 那么什么是栅格地图?栅格地图该怎么使用?这个栅格地图的数据是怎么表示的?我们继续聊~

机器人学中地图的表示方法有四种:特征地图、拓扑地图、栅格地图以及直接表征法(Appearance Based Methods)。

***特征地图用有关的几何特征(如点、直线、面)表示环境。

***拓扑地图把室内环境表示为带结点和相关连接线的拓扑结构图,其中结点表示环境中的重要位置点(拐角、门、电梯、楼梯等),边表示结点间的连接关系,如走廊等。

***栅格地图则是把环境划分成一系列栅格,其中每一栅格给定一个可能值,表示该栅格被占据的概率。

***直接表征法省去了特征或栅格表示这一中间环节,直接用传感器读取的数据来构造机器人的位姿空间。每种方法各有自己的特点和适用范围,其中特征地图和栅格地图应用最普遍。	

​ 以上内容是百度百科的,我们需要关心的只有一句话!

栅格地图则是把环境划分成一系列栅格,其中每一栅格给定一个可能值,表示该栅格被占据的概率。

​ 就拿上面我们的地图来说,黑色区域是障碍物,白色区域为空白区域、可行驶区域,灰色的那是我地图没建好的区域。

​ 那么在导航的时候呢,栅格地图是这样的!

​ 这个栅格地图是正在进行导航的情况,可以看到除了黑色和灰色之外,还有一些其他的颜色,而且还有一个方框。

​ 周边红色的点是雷达数据,可以看到在两个角的数据是很集中和密集的。我们可以看到一个方框的轮廓,这个就是在进行局部规划的区域,对于全局路径那肯定是有一条全局路径的规划。

​ 对于栅格地图,我们通过“8邻域”方法来进行求解。

​ 在栅格地图当中,一个单元格周围有8个区域可以被选择是否占用。除此之外,还有4邻域、对角邻域、16邻域等等。

​ 在我们下图的栅格地图当中(黎万洪老师的图,搬过来用),我们规定白色是可行驶区域,黑色是禁止行驶区域即障碍物,黄色为起始点,紫色为目标点,绿色为规划出的最优路径。

​ 在我们实际导航的过程中,这条绿色的路径就是全局路径。我们继续回到RVIZ下的这张图。

​ 我们看到除了路径之外,栅格地图上还有其他的颜色区域,其中红色的区域和边框线基本上是吻合的。其他的颜色区域,大家可以跑下mbot机器人来看下是个什么东西。


​ 算法很虚,在没有载体的情况下算法就是纯理论的公式。我们如何用好这些公式,并将功能和实际的项目融合完成客户的需求,这样的算法才有意义。

​ 这篇博文到这里就结束了,水了不少字,表述了我的一些理解。

​ 这篇博文,看看就够了~