在了解机械臂 Motion Planning 前,需要认识到的是机械臂的运动规划和移动机器人的运动规划还是不一样的。在移动机器人的运动规划中,机器人往往是以一个【点】存在,对移动机器人的规划,更多的是使用各种搜索算法,在已经构建好的地图模型上,搜索出一条路径从起始点到终止点的路径曲线。而机械臂的运动规划是多个刚体结构的运动。在考虑其运动规划时,需要同时考虑多个刚体的(或者说关节)的运动,因此,如何同时表征多个刚体(或关节)的运动就变得比较重要了。
目前了解的传统机械臂运动规划差不多就是分为路径规划和轨迹规划两个层次。
路径规划:
简单地说,路径规划就是在给定的位置A 与位置 B 之间为机器人找到一条符合约束条件的路径。
机械臂关节空间路径规划的输出是完成任务时的无碰撞的关节路径点,只有位置。与传统的移动机器人路径规划不同的是:高维空间导致的configuration space的难以表达以及计算量问题。所以目前机械臂的路径规划问题主流解法就是基于采样的方法,一个基于频繁采样和频繁状态检测的方法,比如 rrt, prm 以及其他变种,具体的可以看下莱斯大学Lydia Kavraki引导的 ompl 开源库,里面实现了目前主流的采样规划,这类方法好处是能用较小的计算量解决高维空间的规划问题,但是由于是概率完备的,虽然理论上在有足够多的采样次数下一定可以得到解,但是它求解效率不稳定,而且容易出现有解时求不出解的情况。
轨迹规划:
轨迹规划可以看做路径规划的后续,输入是路径,输出是带时间参数的轨迹,这部分一般就是指最优时间轨迹规划了TOPP(Time-Optimal Path Parameterization), 即在满足特定约束下在最短时间内执行完路径,这个约束可以是速度,加速度等.这部分也不单单是样条差值或多项式差值,还涉及到优化问题.
关节空间轨迹规划:研究如何利用受控参数在关节空间中规划机器人的运动,有许多不同阶次的多项式函数及抛物线过度的线性函数可用于实现这个目的。如三次样条差值(加速度不光滑),五阶多项式差值(光滑但加速度波动太大,发生龙格现象)。
笛卡尔空间轨迹规划:对笛卡尔空间中规划得到的轨迹(一般是直线,曲线可以看成多段直线),不断计算当前位姿,和进行逆运动学运算,得到关节角。
这两部分研究成果目前都比较多了尤其是路径规划方面,但是相对来讲轨迹规划其实已经实际应用在工业机器人里了,而路径方面虽然研究的比较多,但是采样规划的不稳定性,比如忽快忽慢,有解的情况下得不到解等等,想真正让智能机械臂具有自主能力,还是有待研究,而且着手点也比较多,比如采样规划中的采样方法,规划方法,状态检测方法等等。
先说明一下,路径规划算法通常有两个评价指标:
运动规划算法的应用领域非常广,除了机器人外,另一个很大的应用领域就是电子游戏了。
△ 红警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算法示意图
机械臂与平面机器人的区别主要在两个部分,一个是规划空间不同,另一个是机械臂往往具有更高的自由度。
以上算法都将机器人看做一个点,要想对机械臂进行规划,我们就应该想办法将机械臂用一个点来描述。于是,我们就要简单说一下构形空间(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°分辨率(已经是很低的分辨率了)划分网格,那么将会产生606=4.67×1010个网格,单是对每个网格进行碰撞检测(如果碰撞检测速度为0.1ms),就需要1296小时。除了碰撞检测,规划算法的计算量也是随着空间维度指数增长的。所以一般大于3自由度的规划问题,空间离散法就不具有实际应用价值了。
概率路图法PRM和快速拓展随机树法RRT:这两种算法的相同点是,他们均不考虑障碍物在C-Space中的分布情况,而是采用碰撞检测函数对几个随机采样点进行碰撞检测。PRM和RRT算法在高维空间中都可以使用,且满足概率完备性(非最优),规划速度相当快。
△ 用RRT规划的机械臂
虽然基于采样的规划算法速度很快,但他们也有致命的缺点,那就是由随机采样引入的随机性。利用RRT和PRM算法进行运动规划,用户无法对规划结果进行预判,每次规划的结果都不一样,这就使得自动规划的机器人无法进入工业领域(极端追求稳定性)。
所以目前规划领域也主要集中在对PRM和RRT的改进上,大家都想要尽可能解决这类算法的不确定性,甚至能实现一些优化目标,如RRT*,Informed-RRT*,SBL等。
首先,来看一下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)]
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:
该小节视频后面还讲了 Motion Planning Problem 需要注意的一些变量信息,但是对于理解该 Configuration Space 的概念影响不大,这里就省略了。
对于一般的无障碍物的 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、Actuator space、Cartesian space的区别
由该图可以看到:
Joint space:提供了关节驱动的数值,使得我们可以顺向计算出手臂末端的位置
Cartesian space:提供手臂P相对{W} 的向量位置,即,逆向反推出达到这个位置需要的驱动数值
Actuator space:提供了马达的状态数值,以确保得出最终关节的驱动数值(详见(三)扩展中的复合型关节的案例)
我们的机械臂是六个自由度的,六个关节都是转轴驱动的。也假设我们已经定义好了起始、中间、终点的位置信息和姿态信息。代表了我们的手末端的位姿,代表了各个关节角度,这两个是我们的需求点。
拟合曲线,直接做轨迹规划,找出他们的函数式。
逆向求解出达到这个位置需要机械臂的每个转轴转动的度数,检验求解出的角度是否超出了机械臂转动角度的最大范围等。
检验可行性,检测该轨迹在现实中是否会碰到障碍物。
一般都是用Cartesian space做规划,因为这个更直接地符合我们的需求,直接根据需求得到轨迹就行了啊,步骤少。然而它有一个缺点是它计算量相对于Joint space大得多,因为我们是先对手臂需要的姿态位置拟合它的曲线(从几个点变成了无数个点),然后才去计算驱动关节需要的角度——反推出好多好多个姿态位置下与之一一对应的所有驱动角度,这十分考核机械臂的算力,尤其对于实时的机械臂。而Joint space就不同了,我们先计算姿态位置需要的关节驱动角度数值,然后再拟合这些角度数值的曲线,拟合曲线的计算比反推无数个姿态位置对应的无数个角度要简单得多。
我前面说了,我个人理解的轨迹规划的核心求解目的是——得到关节驱动的拟合曲线。如果使用Joint space的话,在第二步就能得到这个曲线,之后的3、4步都是验证而已。那为什么大家很少用计算量更少的Joint space呢?因为Cartesian更直观,他直接规划出位置的轨迹,而Joint规划的是关节角度的变换轨迹,不直观。然而我还是不太理解为啥很少人用 joint…难道是因为它步骤多么?
拟合曲线的方法很多,可以使用机器学习的监督算法,当然小规模地轨迹也可以直接使用excel 。
我们在轨迹规划中一般使用三次多项式,一整个曲线中往往是多段参数不同的多项式曲线组合起来的。
我们首先了解单段三次多项式(红色框内),再了解多段中的求解。
已知两侧的点坐标(假设某时刻下的角度、速度已知),用三次多项式求角度 和时间 的关系式,即需要求出:
一般解法
未知的很容易如下求解出来:
其中[]是角度,[点]代表角速度,[点点]代表角加速度。
矩阵表达
如果用矩阵来表示上述运算,则是:
(注:此T矩阵与前几章的信息矩阵不同)
代表起点的位置,代表终点的位置,点代表起点i的速度,点代表终点的速度。
由于T矩阵满足性质有逆矩阵,所以该式可以变化为:
这个是单个三次多项式的求解。
在上面的三项多项式中,我们是假设我们已知两侧的角度和速度才能求解多项式,在实际轨迹规划中,我们只知道头尾点的位置、速度条件,我们并不知道中间点(via point)的位置和速度,那如何定义它呢?有好多种方法:根据特殊的需求直接定义(不推荐)、让电脑自己算出适合的速度(例如多段三次多项式)、广义多项式等等方法。
这些方式都是定死规则,一开始就定义好了中间点是哪个,所以就能很容易得到速度值,然后利用[三次多项式(Cubic Polynomials)(基础)](https://blog.csdn.net/aic1999/article/details/82532525#三次多项式(Cubic Polynomials))中最后得到的矩阵求解即可。
使用多段三次多项式的好处是,中间点不用自己定义,它是电脑自己算出来的,而且比手动定义的更有连续性。上面手动定义中间点的图也有个“自动定义”,但是那个是在“人定的死规则”下让电脑生成的我们能知道的中间点,并不是电脑自动算出来的连我们都不知道的中间点。
直接举例
[假设条件]
中间点是,并且已知、的速度位置情况。即我们需要求解出两段多项式 : 与 段的多项式,八个未知数。
[统一符号]
其中 是角度,[点]代表角速度,即对求导。[点点]代表角加速度,即对[点]求导。
在这里。代表得是这个参数属于第几段(个)多项式,代表的是该参数的幂次。
举例: 代表第二个多项式式子的参数,并且它是立方,即第二个多项式的
一般解法:
由前面的公式:
(一个多项式包含头尾两个方程式)
可以直接求解出:
对、求导可以得出角速度点:
(注:为了方便运算,在这里我们是假设了一开始和最后的点角速度是0的,该值可以根据实际状况改动。)
由于我们要保证我们定义的中间点的位置、速度是连续的,所以要求中间点符合条件:
在第一个多项式中,中间点在此可看作“终点” : ,∈[ , ] .
第二个多项式中,中间点在此可看作“起点”: ,∈[ , ] 。
为了保证角速度的连续性,要有的导数=的导数.
为了保证角加速度的连续性,要有的导数的导数=的导数的导数.
于是有:
(一个中间点能产生速度、加速度两个方程式)
联立上面八个等式,就能求解出两个多项式的参数值:
矩阵解法(重点):
上面的解法转换为矩阵就是:
左式第一个代表第一段多项式的起点,第二个代表终点。
左式第三个代表第二段多项式的起点,第四个代表终点。
左式第五个代表整个多段式的起点速度,第六个代表终点速度。
左式第七个代表第一个中间点的速度连续性条件,第八个代表加速度的条件。
有固定套路,记住就好了,下面例题要用的。
总结:用多段三次多项式拟合曲线好处是速度、加速度连续,但是需要结合前一段多项式,需要多个式子联立求解就比较麻烦。
假设我们有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轴的角度 |
直接套用多段三次多段式的公式就行:
在Cartesian下可以直接求解,不用先转换成到达目标坐标的驱动关节的角度,所以直接将表格数据带入公式即可:
然后就能求解出A矩阵了,就知道整个多项式曲线以及它的轨迹了。
首先要先 IK 逆向转换成角度…然后再运算,T矩阵和上面的是一样的,所以也不麻烦。
说起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驱动并没有什么标准的格式或者规定。对于MoveIt而言,只要求你有个ROS node,它有两个功能:
如果连接实际机器人,MoveIt需要从机器人当前状态开始规划,因此这个ROS驱动需要能够实时获取机器人的各关节信息(如角度),并用过/joint_states消息发布;
由于MoveIt规划的结果会以一个action的形式发布,所以我们的ROS驱动就应该提供一个action server,这个功能就是接收规划结果,下发给机器人,并反馈执行情况。action的类型是control_msgs/FollowJointTrajectory。
具体action的写法可以参照ROS官网**(坑)**。简单而言,一个action有五个部分:
这部分在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中丰富的上层应用资源,对机器人产品化的实现还是有很大帮助的,一方面可以利用这些功能包快速完成原型开发,另一方面也可以从这些源码资源中获得灵感。
说明
概念
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(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(用户接口,三种接口可供调用)
配置,move_group是ROS节点,它在ROS param server获取三种信息,
Robot Interface(机器人接口)
Joint State Information(节点状态信息)
Transform Information(变换信息)
Controller Interface (控制器接口)
Planning Scene(规划场景)
Extensible Capabilities(可扩展能力)
运动规划
The Motion Planning Pipeline(运动规划管道)
OMPL
Kinematics(运动学)
Trajectory Processing(轨迹处理)
丰富的上层资源最终还是要落实到机器人上,在机器人控制部分,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包是一个重写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向硬件接口发送命令。
查看源文件以了解joint_trajectory_controller 是如何与position_controller、velocity_controller 等命名的。
硬件接口
在撰写本文时可用的硬件接口列表(通过硬件资源管理器)。硬件接口由 ROS 控制结合上述 ROS 控制器之一用于向硬件发送和接收命令。如果您的机器人的硬件接口尚不存在,您当然可以创建自己的并且不限于此列表:
另请参阅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 传输。
传输接口
传输特定代码(非机器人特定)在跨传输类型共享的统一接口下实现双向(执行器 <-> 联合)努力和流程图。这是硬件接口不可知的。在撰写本文时可用的传输类型列表:
用法:
关节限制
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和方法填充它们为执行在不同种联合的命令的限制方法的数据结构。
Joint_limits_interface 不被控制器本身使用(它没有实现*HardwareInterface),而是在控制器更新后在机器人抽象的write()*方法(或等效方法)中运行。强制限制将覆盖控制器设置的命令,它不会在单独的原始数据缓冲区上运行。
应用功能中还有一些常用模块,这里我把他们统称为常用组件,比如TF、URDF、Message等。
[外链图片转存中…(img-SOCoY9Wi-1632832167736)]
TF是ROS中非常重要的一个部分,可以根据机器人系统中的坐标系创建一棵TF树,然后帮助开发者完成坐标系之间的变换。在ROS系统中,TF是通过广播和监听的方式操作的,这种方法在复杂机器人系统中会产生很多冗余信息,效率不高。
刨除上层的广播和监听封装,TF的内核其实与ROS并没有关系,而是一个完成坐标运算的数学库。在我们的项目开发中,可以直接链接TF的底层数学库,帮助完成需要的坐标变换,Kungfu Arm的正向运动学就使用到了TF数学库。
[外链图片转存中…(img-yRdSqAws-1632832167736)]
URDF是ROS中实现机器人建模的重要工具,很多上层功能的算法实现,都依赖于机器人的URDF模型,所以如果直接移植ROS中的功能包源码,URDF模型部分还是需要维护的。
[外链图片转存中…(img-aMzRjl7t-1632832167737)]
ROS为机器人提供了一个统一的平台,很重要的一个部分就是定义了一系列标准的接口,这些接口的定义与ROS系统的通信机制没有关系,完全可以在程序中调用,这样不仅免去了重复定义的问题,还可以保持和ROS统一的接口。
本篇浅谈了ROS应用功能在机器人产品化开发当中的应用,这是ROS中资源最为丰富的一个部分,不仅可以帮助我们快速搭建功能原型,验证设计上的问题,同时还可以借助开源代码,助力后续的功能开发,绝对是机器人开发的优质资源。