双机器人协作轨迹生成

核心思想:加约束

生成双机器人协作轨迹的核心问题在于:两个机器人总共是12个自由度,如果想给两个机器人各生成一条确定的轨迹,那对于某一个时间点,应该有且仅有一组(12个)关节角满足要求,也就是说,每个时间点都需要写出来12条约束方程,然后解出来一组解。

一般来说,对工件进行加工会限制6个或以下的自由度。如三轴铣床就是限制三个自由度,五轴加工中心的话就是限制五个自由度,单机器人加工的话就限制了6个自由度。所以通过工件的加工需求,我们可以先写出来6条方程。

我们已经获得了加工工件所需的6自由度轨迹,但是希望用双机器人系统进行加工,那我们就需要寻找方法将剩下的6个自由度限制住,这将会是这篇文章的重点。

举个栗子

假如有一天,我想做一个T型管

T型管

我需要把这个相贯线(俩圆柱的交线)焊起来

我手上有俩UR5,我希望其中一台UR5可以夹住管子的左边管口,另外一个机器人拿着工具给相贯线做加工

根据工件先限制6个自由度

参数化定义俩圆柱

建立模型

这里参考了曾疆伟,周剑秋的文章《6-DOF工业机器人相贯线焊接运动规划》

用到的参数
图示
  • R:端正的圆柱的半径
  • r:歪的圆柱的半径
  • e:俩圆柱轴线的距离
  • omega:俩轴线的夹角

在这个模型里,坐标系(center)原点选在了俩轴线公垂线和端正的圆柱的轴线的交点上,Z轴与端正的圆柱的轴线重合,Y轴与公垂线重合

这篇文章用参数方程来描述相贯线,参数是tau,类似于柱坐标里面的theta

计算参数定义域

首先我们需要确定这个tau的定义域

确定参数的范围

一通算完之后,就算出来了tau的范围(论文里只有半边,程序里给加上了另外半边),然后因为我希望这个参数方程可以连续地转完一圈,所以这后面地tau2给翻了个转,这样的话tau1就是从小到大,tau2从大到小

计算tau的定义域

计算相贯线位置

又是一通写圆柱参数方程、旋转omega、平移e、连立,就得到了相贯线的参数方程(这个论文里有,不过丢了下半边)

相贯线参数方程

计算合适的加工坐标系姿态

现在我们算出来了相贯线上每一点的位置,接下来就需要确定在这些点上我们工具的姿态(旋转),主要思路就是把Z轴(工具所指向的方向),Y轴(速度方向)确定下来

  • 任取相贯线上一点
  • 得到两圆柱面在这点的法线
  • 令加工坐标系Z轴与两法线的角平分线重合
  • 对参数方程求导得到该点的切线方程
  • 令加工坐标系Y轴与相贯线在该点切线方向重合
计算姿态
Z轴朝向示意图

sa1是端正圆柱面在该处的法矢,sb1是歪圆柱面在该处的法矢,mu是俩切平面的二面角(俩平面法矢的夹角的补角),对应的phi就是Z轴与俩平面的线面角

对Z轴的限制

sp1是相贯线在该处的切矢,b1是sp1在XoY平面的投影,那他们的夹角theta就是切矢和XoY平面的线面角

对Y轴的限制

因为这里引入了acos哦,所以有时候算出来可能是nan,所以要处理一下

出现nan的原因,theta是±pi/2

解决方法是通过相邻的theta判断现在这个nan到底应该是正的还是负的

处理nan

先绕Z轴转,使X轴与相贯线处端正圆柱面的外法矢平行(此时Y轴应该是与该处端正圆柱面的切平面平行),然后绕X轴转,使得Y轴与相贯线的切矢平行,最后绕Y轴转,使得Z轴在俩圆柱面该处切平面的二分面上(见后图)。

通过三个转角确定T
相关线上各点法线矢量及切线矢量

到这里,我们就确定了加工坐标系的姿态了,再加上前面算出来的位置,就确定了加工坐标系相对于前面定义的参考坐标系的位姿(6个自由度)

获得匀速轨迹

通过观察这个模型参数方程的形式,可以看出,如果对参数进行等间距插补,得到相贯线的轨迹点,是没有办法获得距离相近的轨迹点的,特别是在参数接近其上下限的时候,甚至可能出现间隔很大的轨迹点

并且,在某些情况下(如下图),两段相贯线之间并不是平滑连接的,此时就不能让加工坐标系的Y轴始终沿切线方向了,需要在两段相贯线连接处前后进行调整

连接处姿态不连续
用到的函数

首先把上下两段相贯线拼起来

拼接前面算的两段相贯线

平滑连接两段相贯线:判断连接处的姿态是否突变(delta_rpy),如果有的话,对连接处前后的轨迹点进行修正,其效果是通过连续过渡,使前半段的最后一个轨迹点的rpy减小delta_rpy/2,并使后半段的第一个轨迹点的rpy增加delta_rpy/2

连接处平滑

在相距较远的轨迹点之间进行线性插补:先算出来轨迹点的间距,然后按照目标精度进行插补

线性插补

到这里,我们就获得了完整、连续的轨迹了,也就是一连串的加工坐标系(traj)相对于参考坐标系(center)的位姿,成功的限制了6个自由度

插补出来的轨迹

通过添加自定义的约束限制剩余自由度

这后面几个约束方程主要就是从双机器人之间的关系来考虑了

我们现在知道了加工坐标系(traj)相对于参考坐标系(center)的位姿_{traj}^{center}T,和两个机器人的工具坐标系(tool_left、tool_right)相对于两个机器人末端(left、right)的位姿_{tool\_left}^{left}T_{tool\_right}^{right}T,那可以很容易的算出

\left\{ \begin{matrix} {}_{left}^{center}T={}_{traj}^{center}T/{}_{tool\_left}^{left}T \\ {}_{right}^{center}T=I/{}_{tool\_right}^{right}T \\ \end{matrix} \right.

接下来就是添加约束了,思路是这样的:

  • 设定一个相对于世界坐标系(world)固定的坐标系(fixed)
  • 这个固定坐标系(fixed)与两个机器人的末端坐标系(left、right)有一定的几何约束

值得注意的是,虽然这个固定坐标系(fixed)相对于世界坐标系(world)是固定的,但他相对于其他坐标系就不是了,也正因为如此,需要通过添加与末端坐标系(left、right)的约束来确定他相对于这两个坐标系的位姿

位置约束

位置约束相对来说是比较简单的,思路是:连接两个机器人的末端坐标系(left、right),得到一条线段,然后在这条线段上选取一个点,作为固定坐标系(fixed)的原点

位置约束

姿态约束

约束1:位置约束的延伸

在前面位置约束的时候,我们相当于是对两个末端坐标系的位置做了加权相加得到的固定坐标系位置

而这里,则把加权的范围延伸到姿态,对RPY角也进行加权相加得到固定坐标系的RPY角

可见,几种特殊情况如下

  • 当left_distance_proportion为1时,此时固定坐标系(fixed)与右侧机器人末端(right)重合,右侧机器人不会进行任何运动
  • 当left_distance_proportion为0时,此时固定坐标系(fixed)与左侧机器人末端(left)重合,左侧机器人不会进行任何运动
  • 当left_distance_proportion为0.5时,此时固定坐标系在俩机器人末端中间(位置和姿态都是),俩机器人运动幅度相当
控制运动幅度分配的约束

约束2:令俩机器人末端连线垂直于固定坐标系的YoZ面,并使左侧机器人的Z轴在固定坐标系的XoZ面上

这样设置约束是为了确保两个机器人的末端不会穿越两个机器人中间面(固定坐标系的YoZ面),起到一定的避碰效果

具有一定避碰效果的约束条件

约束3:使左侧机器人末端坐标系的Z轴始终朝下,并使两机器人末端连线在末端坐标系的XoZ面上

在焊接加工过程中,焊接位置分为平焊、横焊、立焊、仰焊等,其中平焊最易保证焊接质量

平焊定义为:水平焊接,焊缝中心线是垂直的,焊缝表面向上

因此我们选择让(假设)夹有焊枪的左侧机器人枪头始终朝下,以改善加工质量

针对于焊接工艺而设计的约束

计算末端各时刻的位姿

现在我们知道了固定坐标系(fixed)相对于参考坐标系(center)的位姿后,就可以计算出俩机器人末端相对于基座的位姿了

{}_{center}^{world}T={}_{fixed}^{world}T/{}_{fixed}^{center}T

\left\{ \begin{matrix} {}_{left}^{world}T={}_{center}^{world}T\times {}_{left}^{center}T \\ {}_{right}^{world}T={}_{center}^{world}T\times {}_{right}^{center}T \\ \end{matrix} \right.

运动学逆解

知道了位姿,就能塞进逆解函数里算了,具体怎么算就不说了,根据机器人而异,这里只讲一点细节

为了保证两个机器人能够协作,必须保证两边的位姿、关节角都是一一对应的,因此,如果其中任何一边无解,那两个机器人的对应关节角都应该舍去

去除无解的路径点

在进行逆运动学求解的过程中,会大量用到反三角函数,而理论上反三角函数又是一对多的映射关系(角度每增加2pi,其三角函数值并不变),因此,算出来的关节角很可能出现2pi的跳变,所以需要对算出来的关节角做一点处理,以保证其连续变化

使角度值连续

同时,由于机器人关节实际上并不能无限地旋转,因此需要控制关节角的上下限(UR5的软限位为±2pi),下面这段程序的功能是使

  • 后三个关节关节角最小值尽可能接近-360度(充分利用限位的空间)
  • 前三个关节关节角保持着初始位置附近(减小关节转动)
处理关节角跳变和上下限问题
关节角连续性处理(蓝:处理前;红:处理后)
选择与机器人初始构型相近的关节角(蓝:处理前;红:处理后)

输出

经过这么一通算的结果不输出出来用真的可惜,于是整了下面两个函数来输出数据

输出动画
约束1
约束2
约束3
用txt输出关节角

关节角输出后可以通过对应机器人的控制程序进行控制,这里使用的是ROS官方包控制的UR5,这个方法因机器人而异,故不详细展开

实机测试

总结

总结起来,轨迹规划其实本质上是个加约束的过程,目的就是最后能够唯一确定一组解

对于自由度刚刚好的情况,工件的加工轨迹就已经限制了所有的自由度,没有优化的空间

对于有冗余自由度的情况,就可以根据实际的加工需求来进行进一步的约束,如本文提到的三种约束方式

本文的约束方式都是有几何意义在里面,是显式的约束

但是有的时候我们的需求可能并不能直接体现在几何关系上面,可能只有一个优化指标能通过公式表达出来,这个时候剩下自由度的限制就转化为了最优化问题,而这又是另外的故事了

发表评论

您的电子邮箱地址不会被公开。 必填项已用*标注

此站点使用Akismet来减少垃圾评论。了解我们如何处理您的评论数据