如何利用ROS MoveIt快速搭建机器人运动规划平台

Posted 2023-05-03

参考技术A 最近几年各种移动机器人开始涌现出来，不论是轮式的还是履带式的，如何让移动机器人移动都是最核心的工作。要让机器人实现环境感知、机械臂控制、导航规划等一系列功能，就需要操作系统的支持，而ROS就是最重要的软件平台之一，它在科研领域已经有广泛的应用。不过有关ROS的书籍并不多，国内可供的学习社区就更少了。本期硬创公开课就带大家了解一下如何利用ROS来设计移动机器人。分享嘉宾李金榜：EAI科技创始人兼CEO，毕业于北京理工大学，硕士学位。曾在网易、雪球、腾讯技术部有多年linux底层技术研发经验。2015年联合创立EAI科技，负责SLAM算法研发及相关定位导航软件产品开发。EAI科技，专注机器人移动，提供消费级高性能激光雷达、slam算法和机器人移动平台。移动机器人的三个部分所谓的智能移动，是指机器人能根据周围的环境变化，自主地规划路线、避障，到达目标地。机器人是模拟人的各种行为，想象一下，人走动需要哪些器官的配合？首先用眼睛观察周围环境，然后用脑去分析如何走才能到达目标地，接着用腿走过去，周而复始，直到到达目标地址为至。机器人如果要实现智能移动，也需要眼、脑和腿这三部分的紧密配合。腿“腿”是机器人移动的基础。机器人的“腿”不局限于类人或类动物的腿，也可以是轮子、履带等，能让机器人移动起来的部件，都可以笼统地称为“腿”。类人的腿式优点是：既可以在复杂路况（比如爬楼梯）下移动、也可以更形象地模仿人的动作（比如跳舞），缺点是：结构和控制单元比较复杂、造价高、移动慢等。所以大部分移动的机器人都是轮式机器人，其优势在于轮子设计简单、成本低、移动快。而轮式的也分为多种：两轮平衡车、三轮、四轮和多轮等等。目前最经济实用的是两个主动轮+一个万向轮。眼睛机器人的眼睛其实就是一个传感器。它的作用是观察周围的环境，适合做机器人眼睛的有激光雷达、视觉（深度相机、单双相机）、辅助（超声波测距、红外测距)等。“脑”机器人的大脑就负责接收“眼睛”传输的数据，实时计算出路线，指挥腿去移动。其实就是要把看到的东西转换为数据语言。针对如何描述数据，如何实现处理逻辑等一系列问题。ROS系统给我们提供一个很好的开发框架。ROS简介ROS是建立在linux之上的操作系统。它的前身是斯坦福人工智能实验室为了支持斯坦福智能机器人而建立项目，主要可以提供一些标准操作系统服务，例如硬件抽象，底层设备控制，常用功能实现，进程间消息以及数据包管理。ROS是基于一种图状架构，从而不同节点的进程能接受、发布、聚合各种信息（例如传感，控制，状态，规划等等）。目前ROS主要支持Ubuntu操作系统。有人问ROS能否装到虚拟机里，一般来说是可以的，但是我们建议装个双系统，用Ubuntu专门跑ROS。实际上，ROS可以分成两层，低层是上面描述的操作系统层，高层则是广大用户群贡献的实现不同功能的各种软件包，例如定位绘图，行动规划，感知，模拟等等。ROS（低层）使用BSD许可证，所有是开源软件，并能免费用于研究和商业用途，而高层的用户提供的包则使用很多种不同的许可证。用ROS实现机器人的移动对于二维空间，使用线速度+角速度可以实现轮式机器的随意移动。线速度：描述机器人前后移动的速度大小角速度：描述机器人转动的角速度大小所以控制机器人移动主要是要把线速度角速度转换为左右轮的速度大小，然后，通过轮子直径和轮间距，可以把线速度和角速度转化为左轮和右轮的速度大小。这里有一个关键问题就是编码器的选择和pid的调速。编码器的选择：一般编码器和轮子是在一个轴上，目前来说，速度在0.7m/s以下的话，编码器选600键到1200键之间都ok。不过需要注意的是，编码器最好用双线的，A、B两线输出，A向和B向输出相差90度，这样可以防抖动。防抖动就是可以在之后里程计算时可以更准确。左轮和右轮的速度大小的控制，通过轮子编码器反馈，通过PID实时调整电机的PMW来实现。实时计算出小车的里程计(odom)，得到小车移动位置的变化。计算车的位置变化是通过编码器来计算的，如果轮子打滑等情况，那么计算的变化和实际的变化可能不同。要解决这个问题，其实是看那个问题更严重。要走5米只走了4.9米重要，还是要走180度只走了179度重要。其实角度的不精确对小车的影响更大。一般来说，小车的直线距离精确度可以控制在厘米范围内，在角度方面可以控制精准度在1%~2%。因为角度是比较重要的参数，所以很多人就用陀螺仪来进行矫正。所以有时候大家问小车精度有多高？其实现在这样已经精度比较高了，难免打滑等问题，不可能做到百分之百的精准。小车在距离和角度方面做到现在这样对于自建地图导航已经是可以接受的，要提高更高的精度可能就要其他设备辅助，比如激光雷达来进行辅助，激光雷达可以进行二次检测进行纠正。激光雷达数据的存储格式，它首先会有一个大小范围，如果超出范围是无效的。还有就是有几个采样点，这样就可以激光雷达可以告诉你隔多少度有一个采样点。另外最后那个Intensities是告诉大家数据的准确率，因为激光雷达也是取最高点的数据，是有一定的准确率的。上面的ppt其实就是用激光雷达扫了一个墙的形状。激光雷达扫出一个静态形状其实没有意义，雷达建图的意义其实在于建立房间的地图。如何绘制地图？第一步是收集眼睛数据：针对激光雷达，ROS在sensor_msgs包中定义了专用了数据结构来存储激光消息的相关信息，成为LaserScan。它指定了激光的有效范围、扫描点采样的角度及每个角度的测量值。激光雷达360度实时扫描，能实时测出障碍物的距离、形状和实时变化。第二步就是把眼睛看到的数据转化为地图：ROS的gmapping把激光雷达的/scan数据转换为栅格map数据，其中黑色代表障碍物、白色代表空白区域，可以顺利通行、灰色：未知领域。随着机器人的移动，激光雷达可以在多个不同方位观测同一个位置是否有障碍物，如果存在障碍物的阈值超过设置值是，就标定此处是存在障碍物；否则标定不存在障碍物。把障碍物、空白区域和未知领域的尺寸用不同灰度表示出来，就是栅格地图。便于下一步定位和导航。有时候会出现很直的墙，机器人却无法直着行走，这时的问题可能就是机器人的轮子出现打滑等其他问题，而走歪了，这时绘制出的地图也可能是歪的。这种情况可以通过加一个陀螺仪来避免这个情况。因为激光雷达的特性，有时候遇到黑色或镜面会导致测距不准。目前的解决方法就是不用激光雷达，或者用激光雷达和超声波进行辅助处理。ROS的地图是分多层的，我可以在不同高度放多台激光雷达来一起叠加，共同绘制一张地图。地图绘制结束之后，就可以进行定位和导航等工作。如何定位和导航？定位：其实是概率性的定位，而不是100%的精度。根据激光雷达扫描周围障碍物的形状，与地图的形状做匹配，判断机器人所在位置的概率机器人的定位是否成功，与地图特征有很大关系，如果区域特征明显，那么机器人就很容易判断自己的位置。如果出现难以定位的问题，可能需要人给指定初始位置，或者加led来进行位置识别，或者其他的定位设备来协助定位。目前的视觉通过色彩或者光的技术越来越多。导航：全局路径规划+局部调整（动态避障）导航其实就是全局定位，首先根据现有地图进行规划，但是在运行过程中会进行局部的路线规划。但是总体还是根据全局路径来走。导航中工作量还很大，比如扫地机的路径规划和服务机器人的路径规划是不一样的，扫地机器人可能要全覆盖的有墙角的地图，而服务机器人主要围绕指定的路径或者最短路径来进行规划，这部分是ROS工作量最大的一块。路径规划根据不同应用场景变化比较大，但是ROS提供基础的路径规划的开发包，在这个基础上我们会做自己的路径规划。机器人描述和坐标系变换在导航时，哪些区域可以通过，取决于机器人形状等信息，ROS通过URDF（UnifiedRobotDescriptionFormat)就是描述机器人硬件尺寸布局，比如轮子的位置、底盘大小、激光雷达安装位置，这些都会影响到坐标系的转换。坐标系遵循的前提是每个帧只能有一个父帧，再往上进行一些眼神或者关联。激光雷达的安装位置直接影响/scan输出数据。所以激光雷达和机器人的相对位置是需要做坐标变换，才能把激光雷达的数据转化为机器人视角的数据。ROS的坐标系，最终归结为三个标准框架，可以简化许多常见的机器人问题：1）全局准确，但局部不连续的帧（’map”）2）全局不准确，但局部光滑框架（’odom”）3）机器人自身框架（’base_link”）多种传感器(像激光雷达、深度摄像头和陀螺仪加速度计等)都可以计算base_link和odom的坐标关系，但由于“每个帧只能有一个父帧”，所以只能有一个节点(比如robot_pose_ekf融合多传感器)发布base_link和odom的坐标关系。Baselink自身的坐标系，因为不同元件装在机器人上不同位置，都要对应到baselink的坐标系中，因为所有的传感器都是要通过机器人的视角来“看”。有些朋友问我，激光雷达在建地图的时候，小车移动后地图就乱了，这是因为小车的底盘坐标系和激光雷达的坐标系没有标定准确。map和odom之间的关联因为小车移动需要一个局部联系，比如小车在向前走，不停的累加，这是里程计的作用，map起到全局的、不连续的作用，经过激光雷达和map对应。如果要学习ROS的话，坐标系的变化是重要的点。坐标系的变换还有一个点，就是每个帧都只有一个父帧，有时候两个坐标都和它有关联的话，就是A和B关联，B再和C关联，而不是B/C都和A关联。三个坐标帧的父子关系如下：map–>odom–>base_link其实，map和odom都应该和base_link关联，但为了遵守“每个帧只能有一个父帧”的原则，根据map和base_link以及odom->base_link的关系，计算出map与odom的坐标关系并发布。odom->base_link的坐标关系是由里程计节点计算并发布的。map->base_link的坐标关系是由定位节点计算出来，但并不发布，而是利用接收odom->base_link的坐标关系，计算出map->odom的坐标关系，然后发布。只有里程计的时候，没有激光雷达，也可以跑，但是要先根据预设地图进行简单避障。精彩问答Q：还有ROS的实时性有什么改进进展吗？A：实时改进要看ROS2.0的设计，其实ROS2.0的进展网上有公开。但是实际上他的进展离实际应用还有一定距离，至少今年下半年还达不到稳定，不过可以去研究下他的代码，他对内存管理，线程管理，在实时性上有了很大改善。Q：vSLAM对内存和CPU要求颇高。实际工程中，李老师使用的是什么硬件配置？可以做多大的地图呢？A：确实如此，目前来说我们还是使用激光雷达和传感器辅助来进行，这个和地图大小没有太大关系，主要是与地形障碍物复杂程度有关。

ROS系统MoveIt玩转双臂机器人系列--浅议机器人运动学与D-H建模

一、概述

 　　机器人运动学研究的是机械臂各个连杆之间的位移关系、速度关系和加速度关系。比较经典的一本书推荐大家读读熊有伦的《机器人技术基础》下载网址在这。本篇博文将从刚体的位姿描述讲起，逐步过渡到D-H法运动学建模的方法与步骤，结合前几篇博客所树的Rob机器人的手臂建立D-H运动学模型，并编写一个逆运动学运动学求解的程序。

　　（1）位姿描述

　　我们知道，刚体在世界坐标系里需要通过位置和姿态两个维度来描述。首先，位置描述很容易理解，就是坐标，例如点P的位姿通过坐标Px，Py，Pz来描述，如下所示。

　　目前它是三维形式，但是我们需要在其下面添加一维，使其变为四维形式，如下所示，这就是齐次坐标，多增加的这一维 ω 称为比例因子，这里取1。

 　　姿态的描述的描述相对复杂一些，它表征的是刚体相对于世界坐标系的旋转情况，我们首先需要在刚体上添加一个坐标系，用这个坐标系的姿态来代表刚体的姿态，如图一所示，在世界坐标系中的刚体，我们建立坐标系X‘，Y‘，Z‘，然后分别额在这个坐标系上取单位失向量n，o，b。

 

图1

 　　刚体上的每个坐标轴 X‘，Y‘，Z‘ 都和世界坐标轴 X, Y, Z 之间在空间上存在夹角，我们以这些夹角为基础对其姿态进行描述，便得到下面这个式子，矩阵 R 就是刚体相对于世界坐标系的姿态描述。此外还有其他的描述方法，比如欧拉角描述方法，四元数描述方法等。

 　　我们将上面的 R 矩阵由三行三列的矩阵变换为四行三列的矩阵，最后一行补零，就得到了刚体姿态的其次矩阵，然后在将位置齐次矩阵加入进来组成四行四列的矩阵，就是刚体位姿描述的整体矩阵：T（也可以称为转换矩阵），如下式：

　　我们也可以将其理解为这是相邻的两个坐标系之间的转换关系或相对关系，只是这个相对关系通过位置和姿态两个维度来描述的。我们的机械臂是由一系列杆件和运动关节组成，如果我们以一定的规则在机械臂的关节处建立坐标系，就得到一系列的坐标系，如图2所示，如果我们又知道这一系列坐标系中每相邻两个的转换关系（即转换矩阵），那么通过推导我们就知道了首位两个坐标系之间的转换关系了，从而就可以实现机械臂的运动学描述了。

图2

　　

　　（2）正逆运动学

　　我们知道机器人运动学研究的是连杆之间的位移关系、速度关系和加速度关系，它又分为正运动学和逆运动学。

　　正运动学：知道机械臂各个关节的关节变量（如每个转动关节转了多少度，移动关节移动了对少距离）推导出末端执行器相对于世界坐标系的位姿关系（坐标系n相对坐标系0）。（解决的是我在哪的问题）

　　逆运动学：知道末端执行器相对于世界坐标系的位姿关系（坐标系n相对坐标系0），推导出要到达这个位姿械臂各个关节的关节变量（如每个转动关节需要转多少度，移动关节需要移动多少距离）。（解决的是如何到那的问题）

 二、D-H法

 　　通过第一部分我们就可以分析知道，所谓的机器人运动学建模通俗讲分为以下几个步骤：

　　1. 以一定的规则在机械臂上创建一系列的坐标系。

　　2. 通过这些坐标系中每相邻两个坐标系之间的关系推导出最后一个坐标系（坐标系n）相对于第一个坐标系（坐标系0）的转换关系（转换矩阵）。

　　3. 通过这个转换关系获得机械臂正运动学矩阵。

　　4. 通过正运动学矩阵获得逆运动学的求解方法。

　　具体以什么样的规则创建坐标系？如何才能快速获得两个相邻坐标系之间的转换关系？如何求取逆运动学解？这一些列问题早在1995年两位前辈就给出了答案（膜拜，经典的东西往往经久不衰），它们就是 Denavit 和 Hartenberg，下面我们一步步看看它们如何解决这些问题的。

（1）描述连杆和连杆连接的参数

 　　上述两位大师将相邻两个连杆（刚体）之间的关系简化成4个参数来描述，其中另个参数用来描述连杆，两个参数用来描述两个连杆的连接关系，先来说说描述连杆的两个参数：ai-1 和 αi-1 ，如图3所示。

图3

　　Link length 连杆长度 ai-1 ，关节轴 i-1 和关节轴 i 之间的公垂线（图3红色直线）的长度ai-1。

　　Link twist   连杆转角 αi-1，假设作一个平面,并使该平面与两关节轴之间的公垂线垂直（图3粉红色平面）,然后把关节轴 i-1 和关节轴 i 投影到该平面上,在平面内轴 i-1 按照右手法则转向轴 i ，测量两轴角之间的夹角为αi-1.

　　再看一下描述两个连杆的连接关系的两个参数 di 和 θi，如图4所示。

图4

　　link offset 连杆偏距 di，相邻两个连杆之间有一个公共的关节, 沿着两个相邻连杆公共法线（图4两条黄线）的距离可以用一个参数描述为连杆偏距di。（当 i 为移动关节时,连杆偏距为一变量）

 　　joint angle 关节角 θi， 两个相邻连杆的两条公共法线（图4两条黄线）绕公共轴线（图4红色轴线）旋转的夹角（图4蓝色夹角） θi。

　　用这四个参数就可以完成相邻两个连杆（刚体）之间的关系的描述。

（2）在机械臂上建立坐标系

　　Denavit 和 Hartenberg 确定了连杆关系的描述方法之后，又制定了坐标系的建立规则，长坐标系{0}到坐标系{n}的确定步骤如下：

　　a. 确定关节轴，并画出轴的延长线。

　　b. 找出关节轴 i -1 和 i 的公垂线（图3红色直线）关节轴 i -1 的交点 ，作为坐标系 i-1 的原点 Oi-1。

　　c. 规定 Zi-1 的指向是沿着第 i-1 个关节轴。

　　d. 规定 Xi-1 轴的指向是沿着轴 i-1 和 i 的公垂线（图3红色直线）的方向，如果关节轴 i-1 和 i 相交，则Xi-1轴垂直于关节轴 i-1 和 i 所在的平面。

　　e. Yi-1轴的方向由右手定则确定 Yi-1 =  Zi-1 × Xi-1 。

 　　此外，对于坐标系{0} ，Z0轴沿着关节轴1的方向,当坐标系{1}的关节变量为0时，设定参考坐标系{0}与{1}重合，且a0=0, α0=0，当关节1为转动关节，d1=0；当关节1为移动关节，θ1=0。

　　坐标系{n} 通常规定:　　　

　　　　对于转动关节 n，设定θn=0，此时 Xn 和 Xn-1 轴的方向相同，选取坐标系{n} 的原点位置,使之满足 dn=0;
　　　　对于移动关节 n， 设定Xn轴的方向使之满足θn=0，当dn=0时,选取坐标系{n} 的原点位于Xn-1轴与关节轴n的交点位置。

 （3）根据机械臂上所建立的坐标系确定描述连杆和连杆连接的参数

　　建立完坐标系以后，就需要根据坐标系来确定小节（1）中所说的描述连杆和连杆连接的四个参数，由于我们是根据特定规则来建立的坐标系，所以我们很容易得到四个参数，得到的方法如图5所示。

图5

  三、Rob手臂的D-H法建模

 　　下面我们解结合前面博文所介绍的Rob机器人的一个手臂，讲解如何在它的手臂上建立坐标系，获得描述参数，最后得到运动学关系，首先手臂的模型抽象如下图6所示。

图6

　　为了方便起见，我们让手臂抬起来伸直，如图7所示，这样方便我们建立坐标系，然后我们按照第二部分的（2）小节介绍的方法建立坐标系，首先绘制关节轴线（关节转轴） i1 到 i5（图7中绿色虚线），然后根据规则确定原点 O1 到 O6的位置，然后确定每个坐标系Z轴的方向（图7红色轴，和关节轴线共线），然后确定各个坐标系的X轴，最后根据右手定则确定各个坐标系的Y轴，此外各个连杆的物理尺寸如图7中的蓝色虚线所标识。

 

图7

 　　一旦确定了坐标系，我们根据第二部分的（3）小节中图5所示的法则就可以快速确定每个连杆（坐标系）之间的描述参数，如下表所示。

　　至此其实我们已经得到了Rob手臂上的坐标系{0}到坐标系{6}之间每两个相邻坐标系的转换关系了，就是上表中 i 行的数据。但是拿到这些数据怎么转换成第一部分的（1）小节中所介绍的转换矩阵 T 呢？如何根据两个相邻坐标系的转换关系得到坐标系{6}到坐标系{0}的转换关系呢？又如何求取逆运动学解呢？这几个问题在下一篇博客中揭晓，并给出逆运动学求解的C++源代码。

 

<-- 本篇完 -->