双机器人协作轨迹生成

核心思想：加约束

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

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

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

举个栗子

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

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

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

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

参数化定义俩圆柱

建立模型

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

R：端正的圆柱的半径
r：歪的圆柱的半径
e：俩圆柱轴线的距离
omega：俩轴线的夹角

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

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

计算参数定义域

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

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

计算相贯线位置

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

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

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

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

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

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

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

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

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

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

到这里，我们就确定了加工坐标系的姿态了，再加上前面算出来的位置，就确定了加工坐标系相对于前面定义的参考坐标系的位姿（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.$