机器人面经

最新推荐文章于 2024-05-10 11:33:31 发布

Dawn_yc

最新推荐文章于 2024-05-10 11:33:31 发布

阅读量3.2k

点赞数 14

文章标签：人工智能

本文链接：https://blog.csdn.net/Dawn_yc/article/details/120536986

版权

机器人运动规划

在了解机械臂 Motion Planning 前，需要认识到的是机械臂的运动规划和移动机器人的运动规划还是不一样的。在移动机器人的运动规划中，机器人往往是以一个【点】存在，对移动机器人的规划，更多的是使用各种搜索算法，在已经构建好的地图模型上，搜索出一条路径从起始点到终止点的路径曲线。而机械臂的运动规划是多个刚体结构的运动。在考虑其运动规划时，需要同时考虑多个刚体的（或者说关节）的运动，因此，如何同时表征多个刚体（或关节）的运动就变得比较重要了。

目前了解的传统机械臂运动规划差不多就是分为路径规划和轨迹规划两个层次。

路径规划：

简单地说，路径规划就是在给定的位置A 与位置 B 之间为机器人找到一条符合约束条件的路径。

机械臂关节空间路径规划的输出是完成任务时的无碰撞的关节路径点,只有位置。与传统的移动机器人路径规划不同的是:高维空间导致的configuration space的难以表达以及计算量问题。所以目前机械臂的路径规划问题主流解法就是基于采样的方法,一个基于频繁采样和频繁状态检测的方法,比如 rrt, prm 以及其他变种,具体的可以看下莱斯大学Lydia Kavraki引导的 ompl 开源库,里面实现了目前主流的采样规划,这类方法好处是能用较小的计算量解决高维空间的规划问题,但是由于是概率完备的,虽然理论上在有足够多的采样次数下一定可以得到解,但是它求解效率不稳定,而且容易出现有解时求不出解的情况。

轨迹规划：

轨迹规划可以看做路径规划的后续,输入是路径,输出是带时间参数的轨迹,这部分一般就是指最优时间轨迹规划了TOPP(Time-Optimal Path Parameterization), 即在满足特定约束下在最短时间内执行完路径,这个约束可以是速度,加速度等.这部分也不单单是样条差值或多项式差值,还涉及到优化问题.

关节空间轨迹规划：研究如何利用受控参数在关节空间中规划机器人的运动，有许多不同阶次的多项式函数及抛物线过度的线性函数可用于实现这个目的。如三次样条差值（加速度不光滑），五阶多项式差值（光滑但加速度波动太大，发生龙格现象）。
笛卡尔空间轨迹规划：对笛卡尔空间中规划得到的轨迹（一般是直线，曲线可以看成多段直线），不断计算当前位姿，和进行逆运动学运算，得到关节角。

这两部分研究成果目前都比较多了尤其是路径规划方面,但是相对来讲轨迹规划其实已经实际应用在工业机器人里了,而路径方面虽然研究的比较多,但是采样规划的不稳定性,比如忽快忽慢,有解的情况下得不到解等等,想真正让智能机械臂具有自主能力,还是有待研究,而且着手点也比较多,比如采样规划中的采样方法,规划方法,状态检测方法等等。

一、路径规划

（一）路径规划算法

先说明一下，路径规划算法通常有两个评价指标：

完备性Complete：利用该算法，在有限时间内能解决所有有解问题；
最优性Optimality：利用该算法，能找到最优路径（路径最短、能量最少等）；

1、从游戏说起(二维路径规划算法)

运动规划算法的应用领域非常广，除了机器人外，另一个很大的应用领域就是电子游戏了。

△ 红警2：尤里的复仇，游戏截图

在玩像红警（暴露年龄系列）这类的即时战略游戏时，我们往往只需为游戏单位指定一个目标点，游戏中的人物就能够绕过障碍物自动走到目标点。

要想实现这一功能，这里面就需要应用到运动规划算法。为了简单，我们先假设我们要规划的机器人/游戏人物在地图上只是一个点，尺寸为0。

Walk To算法

最简单的“算法”应当数Walk To算法了，顾名思义，就是让机器人直接从当前位置朝目标点走，直到到达目标点为止。

△ Walk To算法示意

一些RPG游戏（如同样暴露年龄的传奇）就采用了这种简单的算法，用户通过鼠标点击给定人物前进方向。

△ 热血传奇游戏截图

显然，这个算法找到的路径是最短的（最优性），但无法应对路径上有障碍物的问题（不完备），全凭玩家操作避开障碍物。

Bug算法

Bug算法就是为了应对一些简单的障碍物而提出的，其说明如下：

沿着起始点与终点的连线M运动；

遇到障碍物，则绕着障碍物顺（或逆）时针运动，直到回到连线M。

△ Bug算法示意

显然，这个算法就是传说中的二维迷宫通用解法。所以这个算法在二维空间内是完备的，但其路径不能保证最优。

蚁群算法

蚁群算法（Ant Colony Optimization）其实是一种优化算法，但完全可以直接用到路径规划问题上。这种算法能够得到既完备又最优的路径，但由于收敛速度不快，并未得到太广泛应用。

△ 蚁群算法示意图，出处如图

人工势场法

势场对应着能量分布，最常见的势场就是重力场了，我们在不同高度会拥有不同重力势能。斜面上的球会自然沿着斜面往下滚。

受此现象的启发，人们便想到人工势场法，人工添加势场函数，让目标点处于谷底，距离目标点越远的势场越高，同时，为了避免发生碰撞，可以在障碍物四周添加排斥势场（障碍物处势能最大）。

△ 人工势场示意图，出处Coursera

人工势场非常直观，且对运算量要求不高，可以跟机器人的控制相结合。实验室的师兄们曾在中型组足球机器人比赛中使用过这个方法。

当然，势场法的一个问题就是没办法避开局部极小值问题，所以该方法是不完备的，同时也是非最优化的。

图搜索介绍分割线

另外有一大类算法则是先将运动规划问题转换成图（graph），之后利用各种图搜索算法解决问题。这里简单介绍一下图搜索算法。

图是图论（Graph Theory）里的一个概念，它表示一类用若干离散节点（vertices、node、points）与连接节点的边线（edges，lines，arcs）表示的拓扑结构。

△ 著名的邮差问题就可以转化成一个图论问题

对于在一个图上寻找到一条最短路径的问题，图论中已经有很多方法了，其中在规划领域最著名的两个分别是Dijkstra算法和A*算法。

△ Dijkstra和A*算法伪代码

上图给出了两种算法伪代码，按照这个说明基本可以自己写出两种算法了。下面动图则是我利用Dijkstra和A算法做的效果演示动画。因为A算法加入了启发函数，用于引导其搜索方向，所以大部分情况下A*算法会比Dijkstra算法规划速度快不少。

△ Dijkstra和A*算法演示

图搜索介绍分割线

要将图搜索算法用在运动规划里，就必须想办法将规划问题转换成图。

Visibility Graph可视图法

可视图法用封闭多面体描述障碍物；并将连接所有多面体的所有顶点，去除与障碍物相交的连线；之后将起始点与目标点与所有顶点连接，并去除与障碍物相交的连线。由此可得到一个图。

△ 可视图

之所以叫做可视图，大概可以这样理解：站在某个顶点上，环绕四周，把你能看到（无障碍物）的顶点连接起来。可以证明，该方法完备且最优。

空间离散法

空间离散法就更简单了，按照某一尺寸划分网格，包含障碍物的网格认为不可通过，这样便得到一个网格图，之后按照四连通或八连通的方法得到一个图。

显然如果网格尺寸太大的话，可能会造成连通路径堵塞，因此该方法是分辨率完备（Resolution Complete）且最优的。

△ 空间离散法示意图

这个算法在即时战略游戏里面用得非常多，虽然我不知道红警是不是这样实现的，但我知道开源红警OpenRA（openra.net）是采用的空间离散+A*算法（github上有源码）。

△ 开源红警OpenRAve截图，出处见水印

随机路图法PRM

随机路图（Probabilistic Road Maps，PRM）就是在规划空间内随机选取N个节点，之后连接各节点，并去除与障碍物接触的连线，由此得到一个随机路图。

显然，当采样点太少，或者分布不合理时，PRM算法是不完备的，但是随着采用点的增加，也可以达到完备。所以PRM是概率完备且不最优的。

△ PRM算法示意图

除了上述方法外，还有很多其他构建搜索图的方法，如Voronoi图法、Cell Decomposition等。

快速扩展随机树法RRT

除了基于图搜索的方法，还有另外一大类基于树状结构的搜索算法，其中最著名的就是快速扩展随机树法（Randomly Exploring Randomized Trees，RRT）了。

RRT算法是从起始点开始向外拓展一个树状结构，而树状结构的拓展方向是通过在规划空间内随机采点确定的。与PRM类似，该方法是概率完备且不最优的。

△ RRT算法示意图

2、拓展到机械臂(CSpace+多维)

机械臂与平面机器人的区别主要在两个部分，一个是规划空间不同，另一个是机械臂往往具有更高的自由度。

构形空间C-Space

以上算法都将机器人看做一个点，要想对机械臂进行规划，我们就应该想办法将机械臂用一个点来描述。于是，我们就要简单说一下构形空间（Configuration Space，或C-Space）了。

构形空间，顾名思义就是与机器人构形相关的空间了。

对于平面移动机器人，由于它具有一定尺寸，所以也不能直接当做点来处理。直观的看，如果我们把机器人当做一个点，就应该相应地将障碍物进行膨胀，这个膨胀处理的拓扑方法叫做闵科夫斯基和（Minkowski Sum）。

△ 平面机器人的C-Space

但是，机械臂的构形空间就没办法简单地用闵科夫斯基和来处理了。我们知道，用广义坐标（通常为各关节角度）可以将机械臂用一个点描述，如六自由度机器人可用六维向量空间的一个点(θ1,θ2,θ3,θ4, θ5, θ6)描述。

但是，相应的障碍物的描述就比较麻烦了，由于机械臂逆解存在多解和奇异等问题，所以从工作空间到构形空间的映射是非线性的，目前没有很好的方法将工作空间的障碍物直接映射到构形空间。对此，一般做法是对构形空间离散化，对构形空间的每个网格判断是否存在障碍物。

△ 两自由度机械臂构形空间

当然，上图只是将机械臂的构形空间投影到了平面上，由于机械臂旋转关节的连续性（0=2π），所以机械臂的构形空间并不是如上图所示的平面，而是类似一个圆环面。

△ 两自由度机械臂构形空间

可以看到，这个圆环面（本身只有两自由度）是嵌入到三维空间的。对于嵌入的概念，可以这样理解：就像是三维宇宙中被二向箔二维化后的太阳系。三自由度以上机械臂的构形空间就无法画出来了。

由于构形空间的拓扑结构发生了根本性的变化，所以研究人员第一个需要解决的问题就是上述这些规划算法在构形空间内是否还有效。但是，由于①做机器人的精通太高级数学的并不多；②大部分机构（连续旋转关节、平动关节等）形成的构形空间均是微分流形，在任一点的局部均与欧式空间同态，也就是说大部分算法都可以用。所以，在机器人运动规划方面较少看到算法完备性等数学证明。

△ 当然，这种机构的构形空间就不是微分流形了

对于二自由度机械臂，上述大多数算法都是可以直接使用的：

△ Bug算法在两自由度机械臂中的应用，出处如图

△ Dijkstra算法在两自由度机械臂中的应用，出处Cousera

高维度

高维度将会带来两个问题：①求解计算量增加；②障碍物无法描述。要看一个规划算法能否用在机械臂，就应该看它能否应对这两个问题。我们一个个看。

Walk To：这个算法当然在任何维度下都能直接使用，当然，它完全无法应对障碍物等情况，我们甚至不把它称为规划算法。

Bug：因为Bug算法需要绕障碍物顺（或逆）时针旋转，高维空间不存在这样的方向，所以自然是无法使用的。

蚁群算法：依旧是收敛慢，用的人不多。

人工势场法：由于势场法需要在C-Space中构建人工势场（与障碍物在C-Space中的分布有关），所以经典的人工势场法无法直接用在高维机械臂上。

当然，研究人员想出了解决方法，例如在机械臂上放几个控制点（Control Pointss）；在工作空间建立势场函数后，对每个控制点施加势场力；最后通过力与雅克比的关系（可由虚功原理推导出），将控制点处的力转换到每个关节力矩；关节力矩就是构形空间的势场力。

△ 基于控制点的势场法，图片出自本人论文

Hand-eye coordinative remote maintenance in a tokamak vessel

对于人工势场法，Khatib是这领域的大拿，他将其用于人形机器人的运动规划。

△ 势场法规划机器人，图片出自Khatib课件

人工势场法在多自由度机械臂中实时性好，可以与控制算法结合在一起。但是，依旧是既不完备也不最优，所以很有可能遇到无法求解的情况。

可视图法：与Bug算法类似，可视图法需要确定障碍物在C-Space中的分布情况，因此无法用在高维空间中。

空间离散法：理论上讲，空间离散法可以适用于任何维度的空间，但问题是它会引入很大的计算量。例如对于一个六自由度机械臂，我们按照6°分辨率（已经是很低的分辨率了）划分网格，那么将会产生60^6=4.67×1010个网格，单是对每个网格进行碰撞检测（如果碰撞检测速度为0.1ms），就需要1296小时。除了碰撞检测，规划算法的计算量也是随着空间维度指数增长的。所以一般大于3自由度的规划问题，空间离散法就不具有实际应用价值了。

概率路图法PRM和快速拓展随机树法RRT：这两种算法的相同点是，他们均不考虑障碍物在C-Space中的分布情况，而是采用碰撞检测函数对几个随机采样点进行碰撞检测。PRM和RRT算法在高维空间中都可以使用，且满足概率完备性（非最优），规划速度相当快。

△ 用RRT规划的机械臂

虽然基于采样的规划算法速度很快，但他们也有致命的缺点，那就是由随机采样引入的随机性。利用RRT和PRM算法进行运动规划，用户无法对规划结果进行预判，每次规划的结果都不一样，这就使得自动规划的机器人无法进入工业领域（极端追求稳定性）。

△ RRT每次规划结果不同

所以目前规划领域也主要集中在对PRM和RRT的改进上，大家都想要尽可能解决这类算法的不确定性，甚至能实现一些优化目标，如RRT*，Informed-RRT*，SBL等。

（二）CSpace 详解

1. Configuration-Space

首先，来看一下Motion Planning Problem的定义：给定初识状态 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-zYtMSwy3-1632832167710)(https://www.zhihu.com/equation?tex=x%280%29%3Dx_%7Bstart%7D)] 和期望的目标状态 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VI1u44v5-1632832167710)(https://www.zhihu.com/equation?tex=x_%7Bgoal%7D)] ，找到时间 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-QxAuP52Z-1632832167711)(https://www.zhihu.com/equation?tex=T)] 和一系列的控制量 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-6DivBdy5-1632832167711)(https://www.zhihu.com/equation?tex=u%3A%5B0%3AT%5D+%5Crightarrow+U)] 使运动满足 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-qxajdutc-1632832167712)(https://www.zhihu.com/equation?tex=x%28T%29+%3D+x_%7Bgoal%7D)] ，并且在所有时刻 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-XCHOx48R-1632832167712)(https://www.zhihu.com/equation?tex=t+%5Cin+%5B0%2C+T%5D)] 满足 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-E4gHF9s6-1632832167713)(https://www.zhihu.com/equation?tex=q%28t%29+%5Cin+C_%7Bfree%7D)] 。

其中的一些变量的含义，定义如下：

configuration： [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PLDhXITL-1632832167714)(https://www.zhihu.com/equation?tex=q+%5Cin+C+%5Csubseteq+R%5E%7Bn%7D)]
- 即所有关节的可达运动范围。（不考虑存在障碍物的情况）
configuration-space（C-space）： [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-GO4AgSZn-1632832167714)(https://www.zhihu.com/equation?tex=C+%3D+C_%7Bfree%7D+%5Ccup+C_%7Bobs%7D)]
- [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-B4b89O82-1632832167715)(https://www.zhihu.com/equation?tex=C_%7Bfree%7D)] ：不含有障碍物情况下的 configuration 集合。
- [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-kHUe4QiG-1632832167715)(https://www.zhihu.com/equation?tex=C_%7Bobs%7D)] ：含障碍物情况下的 configuration 集合。
state： [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-T1WaUOaK-1632832167716)(https://www.zhihu.com/equation?tex=x+%3D+q)] 或 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-JxKCNi9o-1632832167717)(https://www.zhihu.com/equation?tex=%28q%2C+v%29+%5Cin+X)]
- 机器人的所有可能状态，描述参数为关节位置和速度。
equations of motion：
- [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-2gYEcynG-1632832167718)(https://www.zhihu.com/equation?tex=%5Cdot%7Bx%7D+%3D+f%28x%2C+u%29%2C+u+%5Cin+U)]
- [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-5hfyQlp7-1632832167719)(https://www.zhihu.com/equation?tex=x%28T%29+%3D+x%280%29+%2B+%5Cint_%7B0%7D%5E%7BT%7D+f%28x%28t%29%2C+u%28t%29%29+dt)]
- 机器人在整个运动过程中的状态变化描述。

该小节视频后面还讲了 Motion Planning Problem 需要注意的一些变量信息，但是对于理解该 Configuration Space 的概念影响不大，这里就省略了。

2. Configuration-Space Obstacle

对于一般的无障碍物的 Motion Planning，相对比较简单，通过设定机器人末端的运动轨迹，做逆运动学计算，得出各个关节的运动轨迹。然后，让关节运动跟随所规划出来的关节运动曲线即可达成目标。

因而，这里主要讨论存在 Obstacle 障碍物情况下的 Motion Planning。

需要说明的是，这里的障碍物可以指的是实际存在在机器人周围的障碍物，也可以指的是关节的运动范围限制。因为，这两者都会影响到机器人的 Configuration-Space，进而影响到机器人的 Motion Planning。

这里以一个两自由度机械臂为例，讲述机器人的 Configuration-Space。可以看到，在机器人的周围有三个障碍物 A, B, C。考虑到机械臂的所有的构型情况。我们可以用一个二维的坐标图来表征机器人的 Configuration-Space 的所有可能情况。而将机器人的一种构型表征为该坐标图上的一个点。对于不可达到的点，使用灰色将该区域涂上颜色。这样，我们就能通过坐标图上的点的颜色，分辨该点是否可达。

如何去构建这样一张图，视频中并没有详述。对于机器人仿真，可以通过计算空间中所有的可能构型，然后通过【碰撞检测】来判断是否与障碍物有接触。如果有，则该位置点不可达。然后，在坐标图上可以标识出来。
**说明：**这里以二自由度机械臂举例，来讲述整个规划方法。该方法可以推而广之到高自由度的机械臂。只是那个时候，机械臂的 Configuration-Space 不会像二维运动这么简单而直观了。

通过构建上述的 Configuration-Space 坐标图，我们可以将我们机械臂的起始位姿 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-4xAPdGD9-1632832167722)(https://www.zhihu.com/equation?tex=x_%7Bstart%7D)] 和目标位姿 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-M6KZuC1z-1632832167723)(https://www.zhihu.com/equation?tex=x_%7Bgoal%7D)] 标识出来。

接下来，就是找一条曲线，可以在不进入 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Do1WzulX-1632832167724)(https://www.zhihu.com/equation?tex=C_%7Bobs%7D)] 的情况下，连接起始位姿 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-fvb9ngjd-1632832167724)(https://www.zhihu.com/equation?tex=x_%7Bstart%7D)] 和目标位姿 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-4qKX8P55-1632832167725)(https://www.zhihu.com/equation?tex=x_%7Bgoal%7D)]。这里就有点类似于移动机器人的路径规划。可以使用包括移动机器人中常使用的BFS，DFS，A*, PRM, RTT算法。

至此，我们的 Motion Planning 目标：**从 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-WAki07C1-1632832167727)(https://www.zhihu.com/equation?tex=x_%7Bstart%7D)] 到 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-q0m8VR8A-1632832167727)(https://www.zhihu.com/equation?tex=x_%7Bgoal%7D)] 构建一条连接通路的目标就达成了。**我们就可以让关节跟踪这条曲线，来达到我们的目标位姿。

二、轨迹规划

在真实需求中，当我们的机械臂已经拿到了一支笔，那怎么设定轨迹让它画一只鸭子之类的呢，或者怎么样让机械臂拿到杯子后挂在墙上呢？我们需要知道手臂状态（位置状态、速度状态）和时间的关系，从而拟合出一条连续（重点）的曲线供机械臂达到目的。这就属于“轨迹规划”内容了。

说白了就是给你两个位置点，起始点和终点，你自己定义中间点（via point），把起始点和终点用平滑曲线连起来就行。

其实就是拟合曲线嘛…

我理解的轨迹规划的核心目的在于**得到关节的驱动数值的连续平滑曲线，也就是说主要是求解曲线的未知参数。**如果没有关节的驱动，你怎么到达目标位置？

轨迹：机械手臂末端点或参做点的位置、速度、加速度对时间的历程。

轨迹规划的好处：轨迹规划后，可以使用不同形态类型的机械臂使用相同轨迹。

（一） Joint space、Certesian space下的轨迹规划

理想轨迹是物体运动的位置连续，速度也是连续的。我们都知道，假如速度不连续，忽大忽小，这就要求它的加速度突然变大，这会给机械臂关节造成力的负担，很大可能出现误差，因此我们的轨迹规划最好是连续的。在轨迹规划下，还分两种类型的规划：Joint space、Certesian space下的轨迹规划，在这先解释下。

Joint space、Actuator space、Cartesian space的区别

由该图可以看到：

Joint space：提供了关节驱动的数值 $\large \Theta _{i}$ ，使得我们可以顺向计算出手臂末端的位置 $\large _{}^{W}\textrm{P}$

Cartesian space：提供手臂P相对{W} 的向量位置，即 $\large _{}^{W}\textrm{P}$ ，逆向反推出达到这个位置需要的驱动数值 $\large \Theta _{i}$

Actuator space：提供了马达的状态数值，以确保得出最终关节的驱动数值（详见（三）扩展中的复合型关节的案例）

我们的机械臂是六个自由度的，六个关节都是转轴驱动的。也假设我们已经定义好了起始、中间、终点的位置信息和姿态信息。 $\large D$ 代表了我们的手末端的位姿， $\large \Theta$ 代表了各个关节角度，这两个是我们的需求点。

1. Joint space下的轨迹规划

知道位置，就能通过逆向运动学（IK），反向求解出有六个自由度的机械臂它六个转轴需要的角度。
拟合曲线，就是轨迹规划啦，得到这些点连续平滑的函数表示（多种）。就是找函数关系（横纵轴的映射）。
再借由顺向运动学（FK）验证这个函数能否经过我们在（1）定义好的点，使得机械臂通过我们规定的点，达到我们想要的位置，以此测试函数是否正确。
检验可行性，比如这个轨迹是不是可能会撞到现实空间中的某些东西之类的。

2. Cartesian space下的轨迹规划

拟合曲线，直接做轨迹规划，找出他们的函数式。
逆向求解出达到这个位置需要机械臂的每个转轴转动的度数，检验求解出的角度是否超出了机械臂转动角度的最大范围等。
检验可行性，检测该轨迹在现实中是否会碰到障碍物。

3. Joint 、Cartesian space下轨迹规划的优缺点

一般都是用Cartesian space做规划，因为这个更直接地符合我们的需求，直接根据需求得到轨迹就行了啊，步骤少。然而它有一个缺点是它计算量相对于Joint space大得多，因为我们是先对手臂需要的姿态 $\large \Theta$ 位置 $\large D$ 拟合它的曲线（从几个点变成了无数个点），然后才去计算驱动关节需要的角度——反推出好多好多个姿态位置下与之一一对应的所有驱动角度，这十分考核机械臂的算力，尤其对于实时的机械臂。而Joint space就不同了，我们先计算姿态 $\large \Theta$ 位置 $\large D$ 需要的关节驱动角度数值，然后再拟合这些角度数值的曲线，拟合曲线的计算比反推无数个姿态位置对应的无数个角度要简单得多。

我前面说了，我个人理解的轨迹规划的核心求解目的是——得到关节驱动的拟合曲线。如果使用Joint space的话，在第二步就能得到这个曲线，之后的3、4步都是验证而已。那为什么大家很少用计算量更少的Joint space呢？因为Cartesian更直观，他直接规划出位置的轨迹，而Joint规划的是关节角度的变换轨迹，不直观。然而我还是不太理解为啥很少人用 joint…难道是因为它步骤多么？

（二）如何拟合曲线

拟合曲线的方法很多，可以使用机器学习的监督算法，当然小规模地轨迹也可以直接使用excel 。

我们在轨迹规划中一般使用三次多项式，一整个曲线中往往是多段参数不同的多项式曲线组合起来的。

我们首先了解单段三次多项式（红色框内），再了解多段中的求解。

1. 三次多项式（Cubic Polynomials）（基础）

已知两侧的点坐标(假设某时刻下的角度、速度已知),用三次多项式求角度 $\large \theta$ 和时间 $\large t$ 的关系式，即需要求出 $\large a_{0....4}$ ：

一般解法

未知的 $\large a_{0....4}$ 很容易如下求解出来：

其中[ $\large \Theta$ ]是角度，[ $\large \Theta$ 点]代表角速度，[ $\large \Theta$ 点点]代表角加速度。

矩阵表达

如果用矩阵来表示上述运算，则是：

(注：此T矩阵与前几章的信息矩阵不同)

$\large \Theta _{i}$ 代表起点 $\large i$ 的位置， $\large \Theta _{i+1}$ 代表终点 $\large i+1$ 的位置， $\large \Theta _{i}$ 点代表起点i的速度， $\large \Theta _{i+1}$ 点代表终点 $\large i+1$ 的速度。

由于T矩阵满足性质有逆矩阵，所以该式可以变化为：

这个是单个三次多项式的求解。

2. 未知中间点速度情况下的求解

在上面的三项多项式中，我们是假设我们已知两侧的角度和速度才能求解多项式，在实际轨迹规划中，我们只知道头尾点的位置、速度条件，我们并不知道中间点(via point)的位置和速度，那如何定义它呢？有好多种方法：根据特殊的需求直接定义（不推荐）、让电脑自己算出适合的速度（例如多段三次多项式）、广义多项式等等方法。

(1)“手动”定义中间点求解

这些方式都是定死规则，一开始就定义好了中间点是哪个，所以就能很容易得到速度值，然后利用[三次多项式（Cubic Polynomials）（基础）](https://blog.csdn.net/aic1999/article/details/82532525#三次多项式（Cubic Polynomials）)中最后得到的矩阵求解即可。

(2)电脑自动生成中间点（推荐）

多段三次多项式

使用多段三次多项式的好处是，中间点不用自己定义，它是电脑自己算出来的，而且比手动定义的更有连续性。上面手动定义中间点的图也有个“自动定义”，但是那个是在“人定的死规则”下让电脑生成的我们能知道的中间点，并不是电脑自动算出来的连我们都不知道的中间点。

直接举例

[假设条件]

中间点是 $\large \Theta_{1}$ ,并且已知 $\large \Theta_{0}$ 、 $\large \Theta_{f}$ 的速度位置情况。即我们需要求解出两段多项式 : $\large \Delta t_{1}$ 与 $\large \Delta t_{2}$ 段的多项式，八个未知数 $\large a$ 。

[统一符号]

其中 $\large \Theta$ 是角度，[ $\large \Theta$ 点]代表角速度，即对 $\large \Theta$ 求导。[ $\large \Theta$ 点点]代表角加速度，即对[ $\large \Theta$ 点]求导。

在这里 $\large a_{xy}$ 。 $\large x$ 代表得是这个参数属于第几段（个）多项式， $\large y$ 代表的是该参数的幂次。

举例： $\large a_{13}$ 代表第二个多项式式子的参数，并且它是立方 $\large a$ ，即第二个多项式的 $\large a ^{3}$

一般解法：

由前面的公式：

（一个多项式包含头尾两个方程式）

可以直接求解出：

对 $\large \Theta_{0}$ 、 $\large \Theta_{f}$ 求导可以得出角速度 $\large \Theta$ 点：

（注：为了方便运算，在这里我们是假设了一开始和最后的点角速度是0的，该值可以根据实际状况改动。）

由于我们要保证我们定义的中间点的位置、速度是连续的，所以要求中间点符合条件：

在第一个多项式中，中间点在此可看作“终点” : $\large \Theta_{1} \left ( t=t_{1} \right )$ ， $\large \Delta t_{1}$ ∈[ $\large t_{0}$ , $\large t_{1}$ ] .

第二个多项式中，中间点在此可看作“起点”: $\large \Theta_{2} \left ( t=0 \right )$ ， $\large \Delta t_{2}$ ∈[ $\large t_{1}$ , $\large t_{f}$ ] 。

为了保证角速度的连续性，要有 $\large \Theta_{1} \left ( t=t_{1} \right )$ 的导数= $\large \Theta_{2} \left ( t=0 \right )$ 的导数.

为了保证角加速度的连续性，要有 $\large \Theta_{1} \left ( t=t_{1} \right )$ 的导数的导数= $\large \Theta_{2} \left ( t=0 \right )$ 的导数的导数.

于是有：

（一个中间点能产生速度、加速度两个方程式）

联立上面八个等式，就能求解出两个多项式的参数值：

矩阵解法（重点）：

上面的解法转换为矩阵就是：

左式第一个代表第一段多项式的起点，第二个代表终点。

左式第三个代表第二段多项式的起点，第四个代表终点。

左式第五个代表整个多段式的起点速度，第六个代表终点速度。

左式第七个代表第一个中间点的速度连续性条件，第八个代表加速度的条件。

有固定套路，记住就好了，下面例题要用的。

总结：用多段三次多项式拟合曲线好处是速度、加速度连续，但是需要结合前一段多项式，需要多个式子联立求解就比较麻烦。

广义三次多项式

假设我们有N+1个点，也就是说有N段需要求解的多项式，还表明除了头尾两点，说我们有N-1个中间点。

它的多项式表达式和多段多项式那个一样的，只是表达方式不同了而已，为：

可以看到一个多项式里还是有四个未知数，即我们整体有4N个未知数。

由上面多段多项式的例子可知，一个多项式包含头尾两条方程式，由于我们有N个多项式，所以我们就有2N个方程式。

而且上面的例子还表明了，一个中间点可以得到两个方程式（速度、加速度），由于我们有N-1个中间点，所以我们就有2（N-1）个方程式。

然而此时仅有2N+2（N-1）= 4N-2 个方程，我们的未知数有4N个，还差两条方程式，这样怎么联立求解出未知数？

最后两个条件方程式的补充定义方法：

1）定义加速度，头尾加速度都是0，符合真实世界的运动

2）定义速度，按照需求定义头尾的速度情况

（多段多项式的例题就是已经假设了头尾速度为0的情况下）

3）将头尾点看作“中间点”，以达到速度和加速度连续。这一条件针对的是头尾连续，做周期运动的机械臂。

其实广义三次多项式和多段那个一模一样嘛…广义多项式只是扩展了最后两个方程式的定义方法而已…

（三）相关例题

t	代表时刻点
x	t 时刻下，机械臂第二关节点在x轴上的位置
y	t 时刻下，机械臂第二关节点在y轴上的位置
theta	t 时刻下，机械臂第三杆与x轴的角度

直接套用多段三次多段式的公式就行：

1. Cartesian space下的求解

在Cartesian下可以直接求解，不用先转换成到达目标坐标的驱动关节的角度，所以直接将表格数据带入公式即可：

然后就能求解出A矩阵了，就知道整个多项式曲线以及它的轨迹了。

2. Joint space下的求解

首先要先 IK 逆向转换成角度…然后再运算，T矩阵和上面的是一样的，所以也不麻烦。

ROS功能应用

说起ROS中的应用功能，可就非常庞杂了，涵盖各种各样的功能包，简单做一个如下的分类：

底层驱动
上层功能
控制模块
常用组件

就这几种应用功能，我们来具体分析一下：

一、底层驱动

机器人开发是一个软硬件结合的领域，常常需要涉及很多传感器、执行器的驱动。常用的硬件一般都可以在ROS中找到匹配的驱动功能包，例如传感器可以参考wiki上的这个列表：http://wiki.ros.org/Sensors

针对常用的USB摄像头，ROS中有usb_cam、uvc_camera等功能类似的驱动包，运行还算比较稳定。我们用usb_cam驱动1080P的摄像头做识别，有的时候连续运行好几天都不会出问题，不过偶尔也会莫名其妙的挂掉。

一些伺服的驱动在ROS当中也可以找到，比如dynamixel的伺服，Kungfu Arm前端的灵巧手使用的就是dynamixel，相关的功能包是dynamixel_motor。用ROS驱动起来确实简单易用，但是运行时间长了，还是会有问题。有一次给Kungfu Arm录像，手就有点紧张了，手指不听使唤，把开水洒了一桌子，驱动也没有任何报错信息，重启之后再也没有重现类似的现象。

ROS支持的很多机器人（PR2、KUKA、shadow hand等）都使用到了一种实时工业以太网总线——EtherCAT。EtherCAT本身就有开源的协议实现方式，ROS将开源库集成为功能包——ethercat_soem，可以在很多机器人的软件源中看到。我们在项目的初期也使用过这种方式，驱动是没有问题，但是稳定性、实时性、功能方面，都还存在一些问题。

就个人感受而言，ROS中底层驱动相关的功能包，大部分都是对已有开源驱动的集成封装，添加统一的ROS接口，所以稳定性主要和原本的驱动相关，同时还要考虑ROS通信机制的影响。

建立机器人ROS驱动

机器人的ROS驱动并没有什么标准的格式或者规定。对于MoveIt而言，只要求你有个ROS node，它有两个功能：

发布关节角度/joint_states

如果连接实际机器人，MoveIt需要从机器人当前状态开始规划，因此这个ROS驱动需要能够实时获取机器人的各关节信息（如角度），并用过/joint_states消息发布；

接收规划结果，并下发给机器人

由于MoveIt规划的结果会以一个action的形式发布，所以我们的ROS驱动就应该提供一个action server，这个功能就是接收规划结果，下发给机器人，并反馈执行情况。action的类型是control_msgs/FollowJointTrajectory。

具体action的写法可以参照ROS官网**（坑）**。简单而言，一个action有五个部分：

action_name/goal：这个就是规划的路径，我们需要接收这个路径，并将所有路径点解析成机器人控制器可以识别的形式，之后下发给机器人，必须要有；
action_name/cancel：这个指令可以随时中断正在执行的动作，但并不是必须的功能；
action_name/feedback：这个是实时反馈执行状态，最简单的就是将机器人当前关节角度等信息反馈回去，非必须；
action_name/status：这个用于显示机器人状态，如正在执行动作、等待、执行结束等待，非必须；
action_name/result：这个就是在动作执行完之后给MoveIt反馈一个执行结果，这个是必须要有的，当然，为了简单，可以已接收到goal就反馈执行成功。

这部分在MoveIt部分是看不到文档的，所以也是阻碍初学者使用MoveIt控制自己机器人的最大问题之一。但是了解了它的机理之后，就比较简单了。

如果你是第一次使用MoveIt，极力推荐你先试试UR、Baxter等已经写好这部分驱动的机器人。

二、上层功能

上层功能是ROS最为擅长的一个领域，可以提供众多机器人的应用功能：SLAM、导航、定位、图像处理、机械臂控制等等，这个部分详细展开够说三天三夜了。我就针对两个部分谈一下自己的看法。

1. 机器人导航

现在很多学校或者公司做机器人导航的时候，都会基于ROS开发。

ROS的这套导航框架确实好用，很快就可以在机器人上把功能跑起来，而且基本不会涉及太多编码，但是还没来得及高兴，问题就出现了：现实和理想还是有很大差距的，功能实现的效果可能远远达不到我们的需求，更别提产品化应用了。

然后就是调整功能节点的各种参数，虽然可以有所改善，但是跟最终的应用还是会有很大差距。使用ROS快速开发后的兴奋，在这个时候就被泼了一盆凉水。

ROS中的功能包一般都是一些通用的功能，不太可能完全适配我们自己的机器人，所以产生各种问题也是情理之中。解决的办法就是：我们不能太依赖ROS中的资源，核心问题还是需要自己解决，或者针对自己的机器人开发相关的功能，或者优化已有的ROS资源。

在Kungfu Arm的开发过程中，我们只用了半年时间就利用ROS把机器人功能原型搭建起来了，但是问题多多，接下来的优化开发用了一年多的时间，现在还在持续的迭代。

2. MoveIt机械臂控制

MoveIt是ROS当中针对机械臂控制的运动规划平台，集成了机械臂运动规划、避障规划、运动学计算等功能模块。和ROS中的导航功能差不多，用moveit搭建一个简单的机械臂控制系统不难，真的要去实现一个产品化的控制系统就复杂了。

这是我们基于ROS开发的工业机器人控制系统，不仅把ROS核心通信机制抛离了，而且还针对moveit、ompl、fastik等功能包进行了代码级的优化和修改，大家可以参考专门介绍这款机器人的文章：

功夫手：一款基于ROS的工业机器人。

总而言之，ROS中丰富的上层应用资源，对机器人产品化的实现还是有很大帮助的，一方面可以利用这些功能包快速完成原型开发，另一方面也可以从这些源码资源中获得灵感。

MoveIt!入门教程-简介

说明

MOVEit！是目前针对移动操作最先进的软件。
它结合了运动规划，操纵，三维感知，运动学，控制和导航的最新进展
它提供了一个易于使用的平台，开发先进的机器人应用程序，评估新的机器人设计和建筑集成的机器人产品
它广泛应用于工业，商业，研发和其他领域。
MOVEit！是最广泛使用的开源软件的操作，并已被用于超过65个机器人

概念

系统结构

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Cz24IqkA-1632832167731)(http://images.ncnynl.com/ros/2016/Overview.0012.jpg)]

The move_group node（move_group节点）
如上图，其中最重要的就是move_group节点，充当整合器：整合多个独立的组件，并提供ROS风格的Action和service
User Interface（用户接口，三种接口可供调用）
- C++，利用move_group_interface包可以方便使用move_group。
- Python，利用moveit_commander包
- GUI（界面），利用Motion Planning 的 Rviz插件。
配置，move_group是ROS节点，它在ROS param server获取三种信息，
- URDF ，从ROS param server中查找robot_description，获取URDF，它是机器人的描述文件。
- SRDF ，从ROS param server中查找robot_description_semantic，获取SRDF.它一般通过MoveIt! Setup Assistant生成。
- MoveIt! configuration ，从ROS param server中获取更多信息，如joint limits, kinematics, motion planning, perception and other information。
- 配置文件由MoveIt! Setup Assistant生成，存放在MoveIt! config目录中。
Robot Interface（机器人接口）
- move_group通过ROS topics和actions与机器人通讯，获取机器人的状态（位置，节点等），获取点云或其他传感器数据再传递给机器人的控制器。
Joint State Information（节点状态信息）
- move_group监听 /joint_states 主题确定状态信息。例如：确定每个节点的位置。
- Move_group能够监听在这主题的多个发布器信息，即使是发布部分的信息（例如：独立的发布器可能是用于机械臂或移动机器人）。
- ove_group不会建立自己的节点状态发布器。这就需要在每个机器人单独来建立。
Transform Information（变换信息）
- Move_group通过ROS TF库来监视变换信息。这允许节点获取全局的姿态信息。
- 例如：navigation包发布机器人的map frame和base frame到TF，move_group可以使用TF找出这个变换信息，在内部使用。
- 注意：Move_group只是监听TF，你需要启动robot_state_publisher才能发布TF。
Controller Interface （控制器接口）
- 通过ROS的action接口，FollowJointTrajectoryAction接口来使用控制器。
- 一个机器人的服务器服务于这个action-这个服务器不是有move_group提供。
- move_group只会实例化客户端与机器人的控制器action服务器通讯。
Planning Scene（规划场景）
- move_group使用规划场景监视器来维护规划场景。
- 场景是世界的和机器人的状态的表现。
- 机器人状态包含机器人刚性连接到机器人的所有物体。
- 关于维护和更新规划场景的体系结构的详细信息在下面的规划场景部分中描述。
Extensible Capabilities（可扩展能力）
- move_group的结构被设计成容易扩展，独立的能力如抓放，运动学，运动规划。
- 扩展自公共类，但实际作为独立的插件运行。
- 插件可经由一系列的ROS yaml parameters 和ROS pluginlib库配置。

运动规划

The Motion Planning Plugin（运动规划插件）
- 运动规划器通过插件接口方式与MoveIt一起工作。这使得MoveIt更容易扩展，能同时与不同的库不同的运动规划器通讯。
- 运动规划器接口通过ROS Action或service方式提供。
- 针对 move_group的默认规划器通过MoveIt! Setup Assistant来配置使用OMPL或OMPL的MoveIt!接口
The Motion Plan Request（运动规划请求）
- 运动计划请求清楚地指定你想要的运动计划做什么。
- 通常情况下，你会要求运动规划器将一只手臂移动到一个不同的位置（在关节空间）或末端执行器到一个新的姿势。
- 默认情况下会进行碰撞检查（包括自碰撞）
- 您可以将一个对象附加到末端执行器（或机器人的任何一部分），例如，如果机器人拿起一个对象。这使得运动规划器考虑到对象的运动而进行路径规划。
- 你可以指定运动规划器检查的约束 - 内置的约束由kinematic constraints提供：
  - Position constraints（位置约束） - 限制连接的位置在某个空间区域
  - Orientation constraints（方向约束） - 限制连接的方向在指定的roll, pitch和yaw范围
  - Visibility constraints（可视化约束） - 限制连接的点在特定传感器的一个可视化的锥形范围
  - Joint constraints（节点约束） - 限制节点位于两个值之间
  - User-specified constraints（自定义约束） - 利用自定义回调函数来指定自定义的约束。
The Motion Plan Result（运动规划结果）
- move_group节点会根据的运动规划请求，产生一个期望的轨迹。这个轨迹会移动机械臂（一组节点）到期待的位置。
- move_group的结果是轨迹不仅仅是路径 - move_group会以希望最大的速度和加速度（需指定）来生成用于轨迹。
- 此轨迹需要遵循速度和加速度的限制

The Motion Planning Pipeline（运动规划管道）

Motion planners and Plan Request Adapters（运动规划器和规划请求适配器）
图示：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-rslxVBzm-1632832167731)(http://images.ncnynl.com/ros/2016/Overview.002.jpg)]
完整的运动规划管道链整合规划规划器与多个叫规划请求适配器的组件。
规划请求适配器允许规划请求预处理和规划反馈后处理。
预处理在某些情况有用，例如：机器人的起始状态稍微超出关节限制之外的情况。
后处理需要处理几个操作，例如：转换生成的路径为带时间参数的轨迹。
MoveIt提供一系列默认的运动规划器适配器，每个都执行一个指定的功能。
常用运动规划适配器：
- FixStartStateBounds,这个适配器修复在URDF文件里面描述的关节限制的开始状态。用于仿真器中，当机器人配置不正确的时候。当有一个或多个关节稍微超出限制，机器人可能会结束。在这种情况下，运动规划就不能正常执行，因为它认为已经超出了关节限制。FixStartStateBounds请求适配器会修复开始状态到设置的关节限制内。但这不是每一次都是合适的解决方案。例如有多少关节超出限制。适配器参数则指定有多少关节超出限制才启用修复。
- FixWorkspaceBounds，这个适配器会为规划指定一个默认的工作空间，一个立方体的大小为10米×10米×10米。如果规划请求的规划器没有填充这些区域，将会指定工作空间。
- FixStartStateCollision，The fix start state collision adapter will attempt to sample a new collision-free configuration near a specified configuration (in collision) by perturbing the joint values by a small amount. The amount that it will perturb the values by is specified by a “jiggle_factor” parameter that controls the perturbation as a percentage of the total range of motion for the joint. The other parameter for this adapter specifies how many random perturbations the adapter will sample before giving up.
- FixStartStatePathConstraints，This adapter is applied when the start state for a motion plan does not obey the specified path constraints. It will attempt to plan a path between the current configuration of the robot to a new location where the path constraint is obeyed. The new location will serve as the start state for planning.
- AddTimeParameterization，The motion planners will typically generate “kinematic paths”, i.e., paths that do not obey any velocity or acceleration constraints and are not time parameterized. This adapter will “time parameterize” the motion plans by applying velocity and acceleration constraints.

OMPL

OMPL (Open Motion Planning Library) 是一个开源的运动规划库，主要是执行随机规划器。MoveIt直接整合OMPL，使用其库里的运动规划器作为主要/默认的一套规划器。在OMPL规划器是抽象的，例如：OMPL没有机器人的概念。MoveIt!配置OMPL，提供一个后端处理，用于解决机器人的问题。
规划场景
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0i78QVmG-1632832167732)(http://images.ncnynl.com/ros/2016/Overview.0031.jpg)]
规划场景，用于显示机器人的世界，同时保存机器人自己的状态。它由Move_group节点内的规划场景监视器来维护。规划场景监视器监听：
- State Information（状态的信息）: joint_states 主题
- Sensor Information（传感器的信息）: using the world geometry monitor described below
- World geometry information（世界的几何图形信息）: from user input on the planning_scene topic (as a planning scene diff).
World Geometry Monitor（世界几何图形监视器），它通过来自机器人的传感器信息和来自用户的输入建立世界几何图形。它使用occupancy map monitor（occupancy地图监视器）建立围绕机器人的3D感知环境和通过planning_scene主题中附带的参数来增加对象的信息。
3D Perception（3D感知）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-bmqm4opl-1632832167733)(http://images.ncnynl.com/ros/2016/Overview.004.jpg)]
在MoveIt，3D感知是由occupancy map monitor处理，它使用插件结构处理不同的传感器输入。MoveIt有两个内置支持可以处理两种输入：
- Point clouds: handled by the point cloud occupancy map updater plugin
- Depth images: handled by the depth image occupancy map updater plugin
你可为occupancy map monitor编写自己的插件。
Octomap， Occupancy map monitor使用Octomap维持Occupancy map的环境。The Octomap can actually encode probabilistic information about individual cells although this information is not currently used in MoveIt!. The Octomap can directly be passed into FCL, the collision checking library that MoveIt! uses.
Depth Image Occupancy Map Updater，深度图像栅格地图的更新器包括它自己的过滤器，例如：它可以从深度图消除机器人的可见部分。它使用目前有关的机器人的信息（机器人状态）进行此操作。

Kinematics（运动学）

The Kinematics Plugin（运动学插件），MoveIt!使用插件结构，尤其是允许用户编写自己的逆运动学算法。Forward kinematics（正向运动学） and finding jacobians（查找雅可比矩阵）被整合到自己的RobotState类。默认逆运动学插件配置使用KDL numerical jacobian-based solver.由MoveIt! Setup Assistant自动配置。
IKFast Plugin（IKFast插件），通常，用户可以选择执行自己的运动学求解器，例如PR2的有自己的运动学求解器。要实现这样的求解的一种流行的方法是使用ikfast包产生的需要与您的特定工作的机器人的C++代码。
Collision Checking（冲突检测），在规划场景中，冲突检测通过CollisionWorld对象来配置，由FCL包（主要的CC库）来执行。
Collision Objects（冲突对象），MoveIt支持不同类型对象的冲突检测。
- Meshes（网格）
- Primitive Shapes（基本形状） - 例如： boxes（箱）, cylinders（圆柱）, cones（圆锥）, spheres（球） and planes（平面）
- Octomap - Octomap 对象能直接用于冲突检测
Allowed Collision Matrix (ACM)（免检冲突矩阵），在运动规划里，冲突检测会耗费甚至达到90%的计算资源。ACM编码需要检测的对象间的对应关系（机器人的或世界的）。如果在ACM关联两对象的值为1，那就不需要检测，这情况就比如两个对象相隔很远，永远不会发生碰撞。

Trajectory Processing（轨迹处理）

Time parameterization（时间参数化），运动规划器一般只会生成路径，这个路径不带时间信息。MoveIt包含轨迹处理程序。它对结合路径和时间参数化的关节限制的速度和加速度来生成轨迹。这些限制是在joint_limits.yaml中为每个机器人指定的。

三、控制模块

丰富的上层资源最终还是要落实到机器人上，在机器人控制部分，ROS提供了一个控制框架——ros_control，同时还有很多常用的控制器——ros_controllers。

ros_control是ROS为开发者提供的机器人控制中间件，包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等等，可以帮助机器人应用功能包快速落地，提高开发效率。

针对不同类型的机器人（移动机器人、机械臂等），ros_control可以提供多种类型的控制器（controller），但是这些控制器的接口各不相同。

为了提高代码的复用率，ros_control还提供一个硬件抽象层，负责机器人硬件资源的管理，而controller从抽象层请求资源即可，并不直接接触硬件。

1. 控制器管理器（Controller Manager）

每个机器人可能有多个控制器（controller），所以这里有一个控制器管理器的概念，提供一种通用的接口来管理不同的控制器。控制器管理器的输入就是ROS上层应用功能包的输出。

2. 控制器（Controller）

控制器可以完成每个joint的控制，读取硬件资源接口中的状态，再发布控制命令，并且提供PID控制器。

3. 硬件资源（Hardware Rescource）

为上下两层提供硬件资源的接口。

4. 机器人硬件抽象（RobotHW）

机器人硬件抽象和硬件资源直接打交道，通过write和read方法完成硬件操作，这一层也包含关节约束、力矩转换、状态转换等功能。

5. 真实机器人（Real Robot）

真实机器人上也需要有自己的嵌入式控制器，将接收到命令反映到执行器上，比如接收到旋转90度的命令后，就需要让执行器快速、稳定的旋转90度。

关于如何使用ros_control控制一款实体机器人，大家可以参考hans-robot发布的一款cute_robot：

https://github.com/hans-robot/cute_robot

应用功能中的控制模块提供了不少控制器：位置控制器、轨迹控制器、力控制器、速度控制器等等，这些控制器的框架设计和代码实现，都可以用于自己的机器人开发中。

ros control 概述

该ros_control包是一个重写pr2_mechanism包，使控制器通用的并不仅止于PR2所有机器人。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-itb1REAI-1632832167735)(http://wiki.ros.org/ros_control?action=AttachFile&do=get&target=gazebo_ros_control.png)] ros_control/documentation 中的图源

ros_control 包将来自机器人执行器编码器的关节状态数据和输入设定点作为输入。它使用通用的控制回路反馈机制（通常是 PID 控制器）来控制发送到执行器的输出（通常是工作量）。对于没有联合位置、努力等的一对一映射的物理机制，ros_control 变得更加复杂，但这些场景使用传输进行了解释。

控制器

在撰写本文时，包含在ros_controllers中的可用控制器插件列表。您当然可以创建自己的，而不仅限于以下列表。所有控制器都使用forward_command_controller向硬件接口发送命令。

effort_controllers - 控制关节所需的力/扭矩。
- Joint_effort_controller（力/扭矩输入直接输出到关节）
- Joint_position_controller（位置输入进入 PID 控制器，该控制器向关节输出力/扭矩）
- Joint_velocity_controller（速度输入进入 PID 控制器，该控制器向关节输出力/扭矩）
Joint_state_controller - 将注册到 hardware_interface:: JointStateInterface的所有资源的状态发布到sensor_msgs/JointState类型的主题。
- joint_state_controller
position_controllers - 一次设置一个或多个关节位置。
- joint_position_controller
- joint_group_position_controller
velocity_controllers - 一次设置一个或多个关节速度。
- joint_velocity_controller
- joint_group_velocity_controller
Joint_trajectory_controllers - 用于绘制整个轨迹的额外功能。
- position_controller
- velocity_controller
- effort_controller
- position_velocity_controller
- position_velocity_acceleration_controller

查看源文件以了解joint_trajectory_controller 是如何与position_controller、velocity_controller 等命名的。

硬件接口

在撰写本文时可用的硬件接口列表（通过硬件资源管理器）。硬件接口由 ROS 控制结合上述 ROS 控制器之一用于向硬件发送和接收命令。如果您的机器人的硬件接口尚不存在，您当然可以创建自己的并且不限于此列表：

joint command interface- 支持命令一系列关节的硬件接口。请注意，这些命令可以具有任何语义含义，只要它们都可以用单个双精度表示，它们不一定是了力矩命令。要指定此命令的含义，请参阅派生类：
- effort joint interface- 用于控制基于力矩的关节。
- velocity joint interface- 用于控制基于速度的关节。
- position joint interface- 用于控制基于位置的关节。
Joint State Interfaces- 支持读取一组命名关节的状态的硬件接口，每个关节都有一些位置、速度和作用力（力或扭矩）。
Actuator State Interfaces- 支持读取一组命名执行器的状态的硬件接口，每个执行器都有一些位置、速度和作用力（力或扭矩）。
执行器命令接口
- Effort Actuator Interface
- Velocity Actuator Interface
- Position Actuator Interface
力-扭矩传感器接口
IMU传感器接口

另请参阅hardware_interface的C++ API和包文档 wiki。

传动

传动是您的控制管道中的一个元素，它转换力/流变量，使其产生的功率 -保持不变。传输接口实现将力/流变量映射到输出力/流变量，同时保留功率。

机械传动是功率保持的传动：

P_in = P_out

F_in x V_in = F_out x V_out

其中 P、F 和 V 代表功率、力和速度。更一般地说，功率是作用力（例如力、电压）和流量（例如速度、电流）变量的乘积。对于比率为 n 的简单机械减速器，有：

effort map: F_joint = F_actuator * n

flow map:   V_joint = V_actuator / n

从上面可以看出，输入和输出之间的功率保持恒定。

传输URDF格式

请参阅URDF 传输。
传输接口

传输特定代码（非机器人特定）在跨传输类型共享的统一接口下实现双向（执行器 <-> 联合）努力和流程图。这是硬件接口不可知的。在撰写本文时可用的传输类型列表：
- 简易减速传动
- 差动传输
- 四连杆传动
用法：
- transmission_interface::ActuatorToJointStateInterface 从执行器变量填充关节状态。
- hardware_interface::JointStateInterface 将关节状态暴露给控制器。

关节限制

joint_limits_interface包含用于表示关节限制，用于从共同格式，如URDF和rosparam和方法填充它们为执行在不同种联合的命令的限制方法的数据结构。

Joint_limits_interface 不被控制器本身使用（它没有实现*HardwareInterface），而是在控制器更新后在机器人抽象的write()*方法（或等效方法）中运行。强制限制将覆盖控制器设置的命令，它不会在单独的原始数据缓冲区上运行。

四、常用组件

应用功能中还有一些常用模块，这里我把他们统称为常用组件，比如TF、URDF、Message等。

TF是ROS中非常重要的一个部分，可以根据机器人系统中的坐标系创建一棵TF树，然后帮助开发者完成坐标系之间的变换。在ROS系统中，TF是通过广播和监听的方式操作的，这种方法在复杂机器人系统中会产生很多冗余信息，效率不高。

刨除上层的广播和监听封装，TF的内核其实与ROS并没有关系，而是一个完成坐标运算的数学库。在我们的项目开发中，可以直接链接TF的底层数学库，帮助完成需要的坐标变换，Kungfu Arm的正向运动学就使用到了TF数学库。

URDF是ROS中实现机器人建模的重要工具，很多上层功能的算法实现，都依赖于机器人的URDF模型，所以如果直接移植ROS中的功能包源码，URDF模型部分还是需要维护的。

ROS为机器人提供了一个统一的平台，很重要的一个部分就是定义了一系列标准的接口，这些接口的定义与ROS系统的通信机制没有关系，完全可以在程序中调用，这样不仅免去了重复定义的问题，还可以保持和ROS统一的接口。

rface](http://wiki.ros.org/JointStateInterface) 将关节状态暴露给控制器。

关节限制

joint_limits_interface包含用于表示关节限制，用于从共同格式，如URDF和rosparam和方法填充它们为执行在不同种联合的命令的限制方法的数据结构。

四、常用组件

应用功能中还有一些常用模块，这里我把他们统称为常用组件，比如TF、URDF、Message等。

[外链图片转存中…(img-SOCoY9Wi-1632832167736)]

[外链图片转存中…(img-yRdSqAws-1632832167736)]

[外链图片转存中…(img-aMzRjl7t-1632832167737)]

本篇浅谈了ROS应用功能在机器人产品化开发当中的应用，这是ROS中资源最为丰富的一个部分，不仅可以帮助我们快速搭建功能原型，验证设计上的问题，同时还可以借助开源代码，助力后续的功能开发，绝对是机器人开发的优质资源。

Dawn_yc

关注

14
点赞
踩
55

收藏

觉得还不错? 一键收藏
1
评论
机器人面经

机器人运动规划在了解机械臂 Motion Planning 前，需要认识到的是机械臂的运动规划和移动机器人的运动规划还是不一样的。在移动机器人的运动规划中，机器人往往是以一个【点】存在，对移动机器人的规划，更多的是使用各种搜索算法，在已经构建好的地图模型上，搜索出一条路径从起始点到终止点的路径曲线。而机械臂的运动规划是多个刚体结构的运动。在考虑其运动规划时，需要同时考虑多个刚体的（或者说关节）的运动，因此，如何同时表征多个刚体（或关节）的运动就变得比较重要了。目前了解的传统机械臂运动规划差不多就是分为路
复制链接

扫一扫