基于几何代数梯度的双机械臂同步校正方法

技术领域

本发明涉及多机械臂协作系统的标定方法，具体地，涉及一种基于几何代数梯度的双机械臂同步校正方法。

背景技术

现如今，由于多机械臂联合加工系统灵活度高、部署便捷、能完成复杂任务、效率高等特点，已经广泛应用于航空航天、能源及其他需要大型零件加工的重要制造领域。而在这些加工制造领域中，使用一台机械臂执行对待加工零件的实时测量、另一台机械臂根据测量结果进行精确加工的方式，可以显著提高加工的效率和精度。

在部署双机械臂加工系统后，首先需要对系统进行提前校准(如两台机械臂基坐标系间的变换关系、测量设备坐标系到夹持其的机械臂末端坐标系的变换关系、工具坐标系至夹持工具的机械臂末端坐标系的变换关系)，以保证在实际加工过程中的操作精度，校正算法的效率及精度也因此将直接影响到系统的工作状况。但是，双机械臂加工系统待校正参数相较于传统单机械臂加工装置显著增加，且参数间存在非线性关系，这都对校正算法提出了较高的要求。

对现有技术进行调研后发现，现行算法大多不具备同时校正的特性，且执行速度较慢，亟需提出一种可以同步校正双机械臂加工系统双机械臂-手眼-工具变换关系的高效、高精度校正方法。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于几何代数梯度的双机械臂同步校正方法，可以完成对双机械臂加工系统双机械臂、手眼、工具坐标系变换的同步校正，提高系统加工效率和精度。本发明适用于双机械臂协同加工系统的校准工作。

根据本发明提供的一种基于几何代数梯度的双机械臂同步校正方法，包括末端装载结构光相机的机械臂、末端装载标定靶球系统的另一机械臂；还包括以下步骤：

步骤一：搭建双机械臂系统；建立末端装载结构光相机的机械臂的相机坐标系、末端装载标定靶球系统的另一机械臂的靶球坐标系；通过所述末端装载结构光相机的机械臂对所述末端装载标定靶球系统的另一机械臂的拍摄，确定所述相机坐标系与所述靶球坐标系间的相对位姿，获取初始标定数据；

步骤二：根据所述双机械臂系统的初始标定数据生成离线的标定用姿态，通过执行标定用姿态，获取标定用数据集；

步骤三：修正标定用数据集符号；利用少量样本修正测量数据实现对对标定用数据集进行符号校准。

步骤四：基于几何代数梯度完成对于目标问题的优化，实现对双机械臂系统的同步校准。

优选地，所述标定靶球系统的靶球坐标系由三个经过精密测量的陶瓷球组成，将原点确定为位于三球球心三角形短边的中心、Z轴指向三球球心平面法向，X轴沿三角形短边，Y轴沿短边上过第三球球心的坐标系。

优选地，所述末端装载结构光相机的机械臂，用于拍摄另一机械臂的标定靶球系统及另一机械臂在执行标定过程中的测量工作。

优选地，所述标定用姿态需满足以下条件：双机械臂不与环境发生碰撞、双机械臂间不发生碰撞、测量标定靶球系统在结构光相机视野内。

优选地，所述执行标定用姿态的过程需满足以下条件：机械臂在运行过程中不与环境发生碰撞、机械臂之间不发生碰撞；在执行标定用姿态的过程中的每一次拍摄时，同时获取两台机械臂的关节角度和末端姿态，以及当前拍摄生成的点云数据，三者共同构成一个标定数据点，利用所有标定用姿态下的标定数据点，获取所述标定用数据集。

优选地，所述步骤三中利用少量样本修正测量数据的具体方法为：

S1：利用几何代数机械臂正运动学，获取符号正确的、表示两台机械臂末端相对于各自基座坐标系变换关系的运动子；

S2：在标定用数据集中利用其中的少量标定数据点，遍历用于表示相机坐标系至靶球坐标系变换关系运动子的符号组合，生成最优粗估计解；

S3：利用最优粗估计解，修正余下所有用于表示相机坐标系至靶球坐标系变换关系运动子的符号。

优选地，所述步骤四中基于几何代数梯度完成对于目标问题的优化，具体方法为：

Step1：构建优化问题目标函数，所述优化问题目标函数以最小化最小二乘残差为目的，并使用拉格朗日松弛算子，以兼顾优化变量在运动子流形内的约束要求，其具体形式为：

其中，A为用于表示装载结构光相机的机械臂末端至其基座坐标系变换关系的运动子，B为用于表示相机坐标系下标定靶球系统位姿在相机坐标系下的运动子，C为用于表示夹持靶球系统的机械臂末端至其基坐标系变换关系的运动子，X为用于表示装载结构光相机的机械臂末端到结构光相机坐标系变换关系的运动子，Y为用于表示装载结构光相机的机械臂基座到夹持标定靶球系统的机械臂的基座的运动子，Z为用于表示标定靶球系统构成的坐标系到夹持其的机械臂末端相对关系的运动子，A,B,C,X,Y,Z均使用运动子(Motor)表示,除加减外其他运算均为几何积(Geometric Product)，

优选地，所述随机梯度算法为，使用基于动量的KatyushaX算法。

优选地，所述几何代数梯度如下，对于最小二乘残差项f(X,Y,Z)＝||BXA-ZCY||

式中E＝BXA-ZCY。

对于拉格朗日松弛项

式中

优选地，所述步骤一中，还包括建立末端装载标定靶球系统的另一机械臂的末端坐标系、两机械臂基座的基座坐标系；所述两机械臂的末端坐标系与基座坐标系间的变换关系、靶球坐标系在相机坐标系下的变换关系，可采用欧拉角或齐次变换矩阵的方式表示。

与现有技术相比，本发明具有如下的有益效果：

1、本发明可以完成对双机械臂加工系统双机械臂、手眼、工具坐标系变换的同步校正，提高系统加工效率和精度。

2、本发明可用于双机械臂测量-加工智能加工系统的系统校正，可以提高系统的加工、测量精度。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明一种基于几何代数梯度的双机械臂同步校正方法的逻辑流程示意图；

图2为本发明双机械臂标定系统结构示意图；

图3为本发明的标定靶球系统结构示意图；

图4为本发明的标定数据的符号定义示意图；

图5为本发明的标定用姿态生成流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

为了实现对双机械臂加工系统双机械臂、手眼、工具坐标系变换的同步校正，并提高系统加工效率和精度，本发明提供了一种基于几何代数梯度的双机械臂同步校正方法，包括末端装载结构光相机的机械臂、末端装载标定靶球系统的另一机械臂；还包括以下步骤：

步骤一：搭建双机械臂系统；建立末端装载结构光相机的机械臂的相机坐标系、末端装载标定靶球系统的另一机械臂的靶球坐标系；通过所述末端装载结构光相机的机械臂对所述末端装载标定靶球系统的另一机械臂的拍摄，确定所述相机坐标系与所述靶球坐标系间的相对位姿，获取初始标定数据；

步骤二：根据所述双机械臂系统的初始标定数据生成离线的标定用姿态，通过执行标定用姿态，获取标定用数据集；

步骤三：修正标定用数据集符号；利用少量样本修正测量数据实现对对标定用数据集进行符号校准。

步骤四：基于几何代数梯度完成对于目标问题的优化，实现对双机械臂系统的同步校准。本方法可用于双机械臂测量-加工智能加工系统的系统校正，可以提高系统的加工、测量精度。

如图1所示，本实施例的具体流程包括：首先，搭建双机械臂系统，根据所搭建系统生成满足视野约束、关节限制约束、环境碰撞约束及自碰撞约束的标定用姿态；然后，控制机械臂运行至生成的标定用姿态并执行拍摄任务，生成标定用数据集；由于本发明使用的方法对数据集各数据点符号有严格约束，在使用标定用数据集前，需要对标定用数据集进行符号校准；最后，定义以最小化最小二乘问题损失函数的、利用拉格朗日算子兼顾约束优化变量在运动子流形上的优化问题，使用基于几何代数梯度的随机梯度下降方法，完成对整个系统的标定。

在本实施例中，以图2所示的双机械系统为例，使用本发明提出的方法，对该双机械臂系统手眼、双机械臂、工具(标定靶球系统)变换关系进行校准。具体步骤如下：

步骤一：构建双机械臂标定系统；所述的双机械臂标定系统具体为：包括两台机械臂，其中一台机械臂末端装载结构光相机，完成对标定物的扫描拍摄；另一台机械臂末端装载标定靶球系统，通过对该标定靶球系统的拍摄，可以确定相机坐标系与靶球(工具)坐标系间的相对位姿。所述双机械臂标定系统，包含两台机械臂，其中一台机械臂末端装载结构光相机，用于拍摄装夹在另一台机械臂末端的标定用靶球系统和在加工过程中执行测量工作。

标定靶球系统由三个经过精密测量的陶瓷球组成(如图3)，可以通过三球的球心，确定一原点位于三球球心三角形短边中心、Z轴指向三球球心平面法向，X轴沿三角形短边，Y轴沿短边上过第三球球心的坐标系。

另外，在标定过程中，各符号的定义如下(见图4)：定义A为用于表示装载结构光相机的机械臂末端至其基座坐标系变换关系的运动子，B为用于表示相机坐标系下标定靶球系统位姿在相机坐标系下的运动子，C为用于表示夹持靶球系统的机械臂末端至其基坐标系变换关系的运动子，X为用于表示装载结构光相机的机械臂末端到结构光相机坐标系变换关系的运动子，Y为用于表示装载结构光相机的机械臂基座到夹持标定靶球系统的机械臂的基座的运动子，Z为用于表示标定靶球系统构成的坐标系到夹持其的机械臂末端相对关系的运动子。需要注明的是，A,B,C,X,Y,Z均使用几何代数中的元素运动子(Motor)表示，第i个测量姿态下获取的数据分别记为A

步骤二：控制机械臂运行至生成的标定姿态并执行拍摄任务，生成标定用数据集，离线生成标定用姿态并执行标定获取标定数据集；所述标定用姿态，要求：机械臂不与环境发生碰撞、双机械臂间不发生碰撞、测量靶球系统在结构光相机视野内。

所述执行标定用姿态的过程，要求：机械臂在运行过程中不与环境发生碰撞、机械臂之间不发生碰撞；在执行每一次拍摄时，同时获取两台机械臂的关节角度和末端姿态，以及当前拍摄生成的点云数据，三者共同构成一个标定数据点，所有标定姿态下的数据点共同组成标定用数据集。

具体地，离线生成标定用姿态的过程如图5所示。

首先，可以根据环境估测待定参数X,Y,Z。利用估测的双机械臂、手眼、工具变换关系，在仿真环境中、按照一定的规则生成随机标定姿态动作。对于该标定姿态动作，还需要经过以下判断：如果该动作不满足机械臂的关节约束，则重新生成；如果该动作会导致机械臂与工作环境发生碰撞，则重新生成动作；如果该动作导致两台机械臂间发生碰撞，则重新生成动作；如果该动作无法保证靶球落在结构光相机视野内，则重新生成动作。

执行循环，直至生成指定数目个标定姿态n后，双机械臂标定姿态停止循环。

执行标定动作并采集标定用数据集；

为了能够自动、安全、高效地完成标定工作，在实际执行中，首先需要对工作环境进行建模，并使用随机树算法进行标定路径生成，以确保在执行过程中，双机械臂既不会发生自碰撞，也不会与环境中的障碍物发生碰撞。

步骤三：修正标定用数据集符号；利用少量样本修正测量数据实现对对标定用数据集进行符号校准。由于本方法对数据集各数据点符号有严格约束，在使用标定用数据集前，需要对数据集进行符号校准。其中，利用少量样本修正测量数据的具体方法为：

S1：利用几何代数机械臂正运动学，获取符号正确的、表示两台机械臂末端相对于各自基座坐标系变换关系的运动子(Motor)；

S2：在标定用数据集中利用其中的少量标定数据点，遍历用于表示相机坐标系至靶球坐标系变换关系运动子的符号组合，生成最优粗估计解；

S3：利用最优粗估计解，修正余下所有用于表示相机坐标系至靶球(工具)坐标系变换关系运动子的符号。

在标定用数据集的原始数据中，无论是机械臂末端与基座间的变换关系(A,C)，还是靶球坐标系在结构光相机坐标系下的表示(B)，都可采用欧拉角或齐次变换矩阵的方式。由于这些表示方式与本发明中使用的几何代数运动子的转换为一对二——正负两个运动子对应同一个齐次变换矩阵，且本发明中使用的优化算法对运动子符号非常敏感，因此需要在执行优化算法前，对数据集的符号进行修正。以齐次变换矩阵至运动子的变换为例，修正过程如下：

首先，对机械臂末端至基座齐次变换矩阵进行转换。由于测量过程中机械臂的关节角度已知，可以利用基于几何代数的机械臂正运动学，结合名义机械臂几何参数，求解出名义运动子

其次，还需要对在结构光相机坐标系下靶球系统的齐次变换矩阵进行变换和符号校核，流程如下：先选取少量标定数据(m个，m不小于5)，遍历所有B的运动子符号组合(2

最后，还需要对余下未判定符号的B运动子做出调整。首先将余下的齐次变换矩阵B

通过以上方法可以实现对A,B,C齐次变换矩阵到运动子(Motor)的转化及符号校准。

步骤四：基于几何代数梯度完成对于目标问题的优化，实现对双机械臂系统的同步校准。具体地，定义以最小化最小二乘问题损失函数的、利用拉格朗日算子兼顾约束优化变量在运动子流形上的优化问题，使用基于几何代数梯度的随机梯度下降方法，完成对系统的标定校准。基于几何代数梯度完成对于目标问题的优化，具体方法为：

Step1：构建优化问题目标函数，所述优化问题目标函数以最小化最小二乘残差为目的，并使用拉格朗日松弛算子，以兼顾优化变量在运动子流形内的约束要求；

具体的，最小二乘问题损失函数为表达式如下：

其中，A为用于表示装载结构光相机的机械臂末端至其基座坐标系变换关系的运动子，B为用于表示相机坐标系下标定靶球系统位姿在相机坐标系下的运动子，C为用于表示夹持靶球系统的机械臂末端至其基坐标系变换关系的运动子，X为用于表示装载结构光相机的机械臂末端到结构光相机坐标系变换关系的运动子，Y为用于表示装载结构光相机的机械臂基座到夹持标定靶球系统的机械臂的基座的运动子，Z为用于表示标定靶球系统构成的坐标系到夹持其的机械臂末端相对关系的运动子。A,B,C,X,Y,Z均使用运动子(Motor)表示,除加减外其他运算均为几何积(Geometric Product)，||■||为几何代数范数。n为标定姿态个数。

用于约束优化变量在运动子(Motor)流形上的拉格朗日乘子项表达式如下：

式中，

结合此拉格朗日松弛项的目标函数如下：

式中，γ

Step2：使用随机梯度算法的下降方式，利用几何代数梯度，完成对目标问题的优化。

具体地，几何代数梯度如下，对于f(X,Y,Z)＝||BXA-ZCY||

式中E＝BXA-ZCY。

对于

式中

随机梯度算法为，使用基于动量的KatyushaX算法。

进一步地，如下表所示算法，

表中

使用以上方法可以完成对双机械臂系统的标定，可以有效提高系统加工效率及精度。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。