一种基于人机协作和视觉检测的机械臂精确路径规划方法

摘要

本发明公开了一种基于人机协作和视觉检测的机械臂精确路径规划方法，具体为：基于视觉辅助算法的待加工工件初始路径获取和实时检测末端执行器跟踪误差计算；通过底层控制设计获取机械臂运行路径点偏差和交互力数据；根据得到的跟踪误差、机械臂运行路径点偏差和交互力数据，按照设定的空间迭代学习算法更新率进行迭代学习更新路径；机械臂开始沿新的路径坐标进行跟踪，直至路径跟踪效果可接受，即获得了空间中工件加工路径的精确坐标。本发明可以避免离线示教编程带来的程序设计难度高、工作量大等问题，也可以解决传统视觉方法下无法检测到工件被遮挡部分的加工路径的问题。

权利要求

1.一种基于人机协作和视觉检测的机械臂精确路径规划方法，其特征在于，双目摄像机将拍摄到的包含期望路径的图像通过数据线发送至电脑端，机械臂末端的力传感器将捕获的纠正力数据通过数据线发送至电脑端，机械臂控制器将当前末端执行器空间坐标通过WiFi发送至电脑端；电脑端对图像进行处理后结合纠正力数据和末端执行器空间路径点偏差进行空间迭代学习，解算出新的空间路径坐标，机械臂运行完毕后电脑端将更新完的期望路径坐标通过WiFi发送给机械臂控制器，机械臂开始沿新的路径坐标进行跟踪，直至路径跟踪效果可接受，即获得了空间中工件加工路径的准确坐标，具体步骤为：步骤1：基于视觉辅助算法的待加工工件初始路径获取和实时检测末端执行器跟踪误差计算；

双目摄像机将拍摄的图像发送至电脑端，在电脑端选取目标路径区域、提起路径中心像素坐标并进行像素匹配解算空间点的三维坐标后获取初始路径，并实时对末端执行器进行模板匹配和像素匹配解算末端执行器的三维坐标以检测机械臂的跟踪误差；

所述视觉辅助算法首先对双目相机的左右相机分别进行标定，得到相机内参数，然后获取相机拍摄的图像；在任务起始时对拍摄的图像进行路径区域选取，然后对选取的图像进行滤波、二值化和中心像素提取后得到目标路径中心处的像素图，对左右相机下处理得到的像素路径进行对应像素匹配，得到初始的空间路径坐标；在机械臂运行过程中，对双目相机拍得的进行末端执行器区域选取，然后进行模板匹配，得到末端执行器的像素坐标，经过左右图像像素匹配后得到末端执行器的空间坐标，进而得到跟踪误差；

步骤2：通过底层控制设计获取机械臂运行路径点偏差和交互力数据；

在机械臂运行过程中，操作者通过人机交互施加纠正力纠正机械臂的运行轨迹，此时机械臂末端的力传感器捕获得到交互力数据，而机械臂底层控制器根据各关节的姿态解算出末端执行器的空间坐标，即得到末端执行器当前的路径点坐标，进而由末端执行器原计划运行的路径点坐标和受交互力影响后末端执行器实际运行的路径点坐标得机械臂的运行路径点偏差；

所述底层控制设计中推导得到空间域下闭环系统的动力学模型为：

其中，Md、Cd、Kd分别为期望的惯性矩阵、阻尼矩阵和刚度矩阵； 是机械臂在运动任‑1

务方向上的速度； 表示空间微分运算符； t＝f (s)表示空间域到时域

的转换关系；E(s)为跟踪误差向量；Fh(s)为交互力；

步骤3：空间迭代学习更新路径；

电脑端根据得到的跟踪误差、机械臂运行路径点偏差和交互力数据，按照设定的空间迭代学习算法更新率进行迭代学习，得到新的路径点坐标并保存；待机械臂运行完毕后，若路径跟踪效果可以接受，则机械臂刚执行的路径坐标即为理想的空间加工路径坐标，迭代学习结束；若路径跟踪效果不能接受，机械臂则按刚保存的更新后的路径坐标运行，并循环上述迭代学习过程，直至路径跟踪效果达到理想情况，迭代学习结束，得到机械臂坐标系下工件加工路径的准确空间坐标；

所述空间迭代学习中，结合底层控制输出的数据，根据设置的更新率，估测出期望路径，然后再根据处理后的交互力和估测出的期望路径得到更新后的路径点坐标，待机械臂运行完毕后将更新后的整个空间路径的路径点坐标发送至机械臂控制器进行下一次迭代学习，空间迭代学习更新率如下：式中， 为空间域下对真实加工路径的估计值，X(s)为参考路径，s为机械臂运行过程中沿加工路径方向的位移，λ为一个正的比例参数，S是空间周期，表示加工路径由起点至终点的位移；

在系统中增加了一个视觉辅助系统，增加视觉辅助系统后，利用双目相机检测初始路径，并可实时检测机械臂末端执行器的跟踪误差，此时设计新的空间迭代学习更新率如下：Xinitial(s)是视觉辅助系统测得的初始空间路径坐标，α∈[0,1]是一个防止过度校正的比例因子，ξ(s)为基于视觉测量的路径跟踪误差，加入视觉系统后，空间周期S定义如下：S＝||Xinitial(S)‑Xinitial(0)||

在视觉校正过程中，如果机器人末端执行器的位置标记点被阻塞，并且在任务路径工作区定义的目标区域中无法检测到，即进入视觉盲区，则将ξ设置为0；对应的公式表达如下：Xvision(s)是视觉辅助系统检测出的末端执行器的空间坐标，Target region是视觉范围内包含加工路径的区域；

步骤4：对于机器视觉拍摄空间路径存在视觉盲区的情况，在机械臂运动至视野范围内空间路径的终点后，由人手拖动机械臂沿盲区内的空间路径行进；此时迭代学习算法中，盲区的视觉实时检测误差为0，盲区内的初始路径为空；第一次跟踪轨迹时由人手拖动示教，示教过程中的路径点即为初始路径，第二次跟踪轨迹时在第一次跟踪时记录的盲区路径点的基础上进行迭代学习，除视觉实时检测误差为0外，其他数据来源和处理方法与视野范围内数据的处理方法相同。

说明书

一种基于人机协作和视觉检测的机械臂精确路径规划方法

技术领域

[0001] 本发明属于智能机械设备技术领域，尤其涉及一种基于人机协作和视觉检测的机械臂精确路径规划方法。

背景技术

[0002] 近年来，随着电子硬件、图像处理、人工智能等技术的发展，机器人技术开始逐步引用于家庭服务、医疗康复、消防减灾等领域，传统机械臂的应用也得到了很大的拓展，开始作用于更加复杂的加工生产以及检测环节。路径规划是机械臂运动控制的基本操作，针对不同的路径规划问题有不同的解决方案。除了传统的直接编程思想，现有的路径规划方案主要有示教编程、视觉引导、脑机结合等，各种方案处理的任务有所不同，适用的工作背景也大不一样。

[0003] 示教编程：工业机械臂的示教主要有2种方式[1]:

[0004] ①在线示教。由技术人员引导,控制机械臂运动,记录机械臂作业的程序点并插入所需的机械臂命令来完成程序的编写。

[0005] ②离线示教。操作者不对实际作业的机械臂直接进行示教,而是在离线编程系统中进行编程或在模拟环境中进行仿真，生成示教程序,通过PC机间接对机械臂控制柜进行示教。

[0006] 对于在线示教，由于人类视觉和控制的限制，示教编程不能实现对特定任务路径的准确跟踪。操作者在拖动机械臂沿目标路径移动过程中，任何微小的抖动都会导致机械臂控制器记录下错误的路径点，导致机械臂末端对目标路径的循迹效果变差。且在拖动示教过程中无法保证机械臂末端与目标路径之间保持精准的间距，特别是在焊接、激光切割等任务中，简单的在线示教难以满足此类工作的高精度要求。

[0007] 对于离线示教，需要对机械臂的工作路径等参数进行提前设置，对于利用机械臂进行单一性、重复性作业来说有一定的优势，而在工作目标时常变化的工作场景中，离线示教就显得相对低效，随着工作目标的变化需要不断更新机械臂的控制程序。

[0008] 现有技术一：一种工业机械臂智能化路径规划装置及方法[2]。该方案设计了一种机械臂智能化路径规划装置，包括加工工件、视觉系统、智能控制系统、机械臂、工作站，各部分的组成模块可以参见图1。具体的控制方法可以参见图2，工业机械臂通过三维视觉自动采集工件的型面，将型面信息分解，应用深度学习的方法判断拍摄图像与输入模型之间的型面差值，当加工型面与设定的基准差距超过阈值时，能够控制机械手电机和关节的动作对加工动作进行修正，不需要人工进行调试，用以保证工件的质量。

[0009] 该技术的应用需要通过深度学习去实现，对于未知的模型型面或曲线是无法使用该方案的，在该方案中，采集的曲线需要与已知模型数据库进行比对，才能获得型面差值，实现路径规划调节；该技术不能实现实时的型面差值计算，深度学习计算往往需要消耗大量时间，很难保证实时的更新误差参数值。

[0010] 现有技术二：一种带电作业机器人机械臂路径规划控制装置及其方法[3]。该方案公开了一种带电作业机器人机械臂路径规划控制装置及其方法，其中控制装置包括状态感知器、动作选择器、评价信号发生器和动态更新机构；方法主要基于正方体空间划分方法、强化学习算法和逆向运动学求解，并依靠控制装置，来生成最优路径。该方案能够准确规划出一条路径，使机器人可以从初始位置到达目标位置，并且绕过周围障碍物，从而实现带电作业机器人的自主作业。具体实现流程参见图3。

[0011] 该技术应用机器学习的方法，自主规划出从初始位置到目标位置的路径，与现有技术方案一类似，学习运算量很大，不能实现实时的规划跟踪；该方案在自定义的学习框架下，规划路径，对于避障有一定的效果，可以得到相对最优的路径，实现距离最短，但是无法精确跟踪一条空间路径；该方案对于出现的突发情况没有自适应能力，若要改变规划的路径，则要重新定义障碍物的位置，没有自适应能力。

[0012] 上述技术方案和技术中，示教编程存在路径规划偏差大且不稳定的问题，技术一存在路径规划耗时长且对未知型面无法处理的问题，以及技术二存在可以规划路径实现避障但无法精确跟踪空间中的期望路径的问题。

[0013] 参考文献

[0014] [1]高党寻,周冰科,姚启明,徐江波.弧焊机器人系统及焊接示教编程技术[J].设备管理与维修,2021(23):111‑113.DOI:10.16621/j.cnki.issn1001‑0599.2021.12.49.[0015] [2]刘湘玲,刘孟祥,旷水章等.一种工业机械臂智能化路径规划装置及方法:中国,112643681.A[P],2021.

[0016] [3]张黎明,杨磊,胡益菲等.一种带电作业机器人机械臂路径规划控制装置及其方法:中国,111645079A[P],2021.

发明内容

[0017] 针对上述问题，本发明提供一种基于人机协作和视觉检测的机械臂精确路径规划方法。

[0018] 本发明的一种基于人机协作和视觉检测的机械臂精确路径规划方法，双目摄像机将拍摄到的包含期望路径的图像通过数据线发送至电脑端，机械臂末端的力传感器将捕获的纠正力数据通过数据线发送至电脑端，机械臂控制器将当前末端执行器空间坐标通过WiFi发送至电脑端；电脑端对图像进行处理后结合纠正力数据和末端执行器空间路径点偏差进行空间迭代学习，解算出新的空间路径坐标，机械臂运行完毕后电脑端将更新完的期望路径坐标通过WiFi发送给机械臂控制器，机械臂开始沿新的路径坐标进行跟踪，直至路径跟踪效果可接受，即获得了空间中工件加工路径的精确坐标，具体步骤为：

[0019] 步骤1：基于视觉辅助算法的待加工工件初始路径获取和实时检测末端执行器跟踪误差计算。

[0020] 双目摄像机将拍摄的图像发送至电脑端，在电脑端选取目标路径区域、提取路径中心像素坐标并进行像素匹配，解算空间点的三维坐标后获得初始路径，并实时对末端执行器进行模板匹配和像素匹配解算末端执行器的三维坐标以检测机械臂的跟踪误差。

[0021] 步骤2：通过底层控制设计获取机械臂运行路径点偏差和交互力数据。

[0022] 在机械臂运行过程中，操作者通过人机交互施加纠正力纠正机械臂的运行轨迹，此时机械臂末端的力传感器捕获得到交互力数据，而机械臂底层控制器根据各关节的姿态解算出末端执行器的空间坐标，即得到末端执行器当前的路径点坐标，进而由末端执行器原计划运行的路径点坐标和受交互力影响后末端执行器实际运行的路径点坐标得机械臂的运行路径点偏差。

[0023] 步骤3：空间迭代学习更新路径。

[0024] 电脑端根据得到的跟踪误差、机械臂运行路径点偏差和交互力数据，按照设定的空间迭代学习算法更新率进行迭代学习，得到新的路径点坐标并保存；待机械臂运行完毕后，若路径跟踪效果可以接受，则机械臂刚执行的路径坐标即为理想的空间加工路径坐标，迭代学习结束；若路径跟踪效果不能接受，机械臂则按刚保存的更新后的路径坐标运行，并循环上述迭代学习过程，直至路径跟踪效果达到理想情况，迭代学习结束，得到机械臂坐标系下工件加工路径的准确空间坐标。

[0025] 步骤4：对于机器视觉拍摄空间路径存在视觉盲区的情况，在机械臂运动至视野范围内空间路径的终点后，由人手拖动机械臂沿盲区内的空间路径行进；此时迭代学习算法中，盲区的视觉实时检测误差为0，盲区内的初始路径为空；第一次跟踪轨迹时由人手拖动示教，示教过程中的路径点即为初始路径，第二次跟踪轨迹时在第一次跟踪时记录的盲区路径点的基础上进行迭代学习，除视觉实时检测误差为0外，其他数据来源和处理方法与视野范围内数据的处理方法相同。

[0026] 进一步的，视觉辅助算法具体为：首先对双目相机的左右相机分别进行标定得到相机内参数，然后获取相机拍摄的图像；在任务起始时对拍摄的图像进行路径区域选取，然后对选取的图像进行滤波、二值化和中心像素提取后得到目标路径中心处的像素图，对左右相机下处理得到的像素路径进行对应像素匹配，得到初始的空间路径坐标；在机械臂运行过程中，对双目相机拍得的进行末端执行器区域选取，然后进行模板匹配，得到末端执行器的像素坐标，经过左右图像像素匹配后得到末端执行器的空间坐标，进而得到跟踪误差。

[0027] 进一步的，底层控制设计中推导得到空间域下闭环系统的动力学模型为：

[0028]

[0029] 其中，Md、Cd、Kd分别为期望的惯性矩阵、阻尼矩阵和刚度矩阵； 是机械臂在‑1运动任务方向上的速度； 表示空间微分运算符； t＝f (s)表示空间域
到时域的转换关系；E(s)为跟踪误差向量；Fh(s)为交互力。

[0030] 进一步的，空间迭代学习中，结合底层控制输出的数据，根据设置的更新率，估测出期望路径，然后再根据处理后的交互力和估测出的期望路径得到更新后的路径点坐标，待机械臂运行完毕后将更新后的整个空间路径的路径点坐标发送至机械臂控制器进行下一次迭代学习，空间迭代学习更新率如下：

[0031]

[0032] 式中， 为空间域下对真实加工路径的估计值，X(s)为参考路径，s为机械臂运行过程中沿加工路径方向的位移，λ为一个正的比例参数，S为加工路径由起点至终点的位移。

[0033] 本发明的有益技术效果为：

[0034] 本发明可以减小机械臂在线示教过程中因示教时人手抖动而引起的工作路径产生较大偏差的影响，也可以避免离线示教编程带来的程序设计难度高、工作量大等问题。也可以解决传统机器视觉方法下无法检测到工件被遮挡部分的加工路径的问题。在部分情况下，纯视觉方法只能测算出待加工工件在视觉范围内被拍摄到的部分加工路径，对于因工件外形而引起的另一部分加工路径被遮挡的情况，本发明技术方案的人机协作和迭代学习部分则可以进行解决，通过在人手拖动纠正过程中对空间路径进行迭代学习，可以对被遮挡部分的路径进行补全。

附图说明

[0035] 图1为现有技术一组成模块示意图。

[0036] 图2为现有技术一控制流程图。

[0037] 图3为现有技术二控制流程图。

[0038] 图4为本发明硬件组成和连接关系。

[0039] 图5为本发明路径规划流程图。

[0040] 图6为视觉辅助算法流程图。

[0041] 图7为相机、物块和机械臂的坐标系关系示意图。

[0042] 图8为像素匹配算法的伪代码。

[0043] 图9为工件及任务坐标系示意图。

[0044] 图10为带视觉辅助系统的空间迭代学习算法执行结构图。

[0045] 图11为空间迭代学习算法仿真结果。

[0046] 图12为跟踪误差和交互力的均值和均方根值及其变化趋势。

[0047] 图13为带有加工路径标记的物块。

[0048] 图14为带视觉辅助系统的空间迭代学习算法实验效果。

具体实施方式

[0049] 下面结合附图和具体实施例对本发明做进一步详细说明。

[0050] 本发明使用的硬件部分主要由机械臂、力传感器、双目相机、电脑，硬件部分组成和连接关系如图4所示，实际中使用的机械臂为7自由度的Sawyer机械臂，双目相机为ZED，力传感器型号为RobotiqFT300。

[0051] 双目摄像机将拍摄到的包含期望路径的图像通过数据线发送至电脑端，机械臂末端的力传感器将捕获的纠正力数据通过数据线发送至电脑端，机械臂控制器将当前末端执行器空间坐标通过WiFi发送至电脑端；电脑端对图像进行处理后结合纠正力数据和末端执行器空间路径点偏差进行空间迭代学习，解算出新的空间路径坐标，机械臂运行完毕后电脑端将更新完的期望路径坐标通过WiFi发送给机械臂控制器，机械臂开始沿新的路径坐标进行跟踪，直至路径跟踪效果可接受，即获得了空间中工件加工路径的准确坐标。

[0052] 本发明的一种基于人机协作和视觉检测的机械臂精确路径规划方法流程图如图5所示，具体步骤为：

[0053] 步骤1：基于视觉辅助算法的待加工工件初始路径获取和实时检测末端执行器跟踪误差计算。

[0054] 双目摄像机将拍摄的图像发送至电脑端，在电脑端选取目标路径区域、提起路径中心像素坐标并进行像素匹配解算空间点的三维坐标后获取初始路径，并实时对末端执行器进行模板匹配和像素匹配解算末端执行器的三维坐标以检测机械臂的跟踪误差。

[0055] 步骤2：通过底层控制设计获取机械臂运行路径点偏差和交互力数据。

[0056] 在机械臂运行过程中，操作者通过人机交互施加纠正力纠正机械臂的运行轨迹，此时机械臂末端的力传感器捕获得到交互力数据，而机械臂底层控制器根据各关节的姿态解算出末端执行器的空间坐标，即得到末端执行器当前的路径点坐标，进而由末端执行器原计划运行的路径点坐标和受交互力影响后末端执行器实际运行的路径点坐标得机械臂的运行路径点偏差。

[0057] 步骤3：空间迭代学习更新路径。

[0058] 电脑端根据得到的跟踪误差、机械臂运行路径点偏差和交互力数据，按照设定的空间迭代学习算法更新率进行迭代学习，得到新的路径点坐标并保存；待机械臂运行完毕后，若路径跟踪效果可以接受，则机械臂刚执行的路径坐标即为理想的空间加工路径坐标，迭代学习结束；若路径跟踪效果不能接受，机械臂则按刚保存的更新后的路径坐标运行，并循环上述迭代学习过程，直至路径跟踪效果达到理想情况，迭代学习结束，得到机械臂坐标系下工件加工路径的准确空间坐标。

[0059] 步骤4：对于机器视觉拍摄空间路径存在视觉盲区的情况，在机械臂运动至视野范围内空间路径的终点后，由人手拖动机械臂沿盲区内的空间路径行进；此时迭代学习算法中，盲区的视觉实时检测误差为0，盲区内的初始路径为空；第一次跟踪轨迹时由人手拖动示教，示教过程中的路径点即为初始路径，第二次跟踪轨迹时在第一次跟踪时记录的盲区路径点的基础上进行迭代学习，除视觉实时检测误差为0外，其他数据来源和处理方法与视野范围内数据的处理方法相同。

[0060] 本发明基于视觉辅助算法的待加工工件初始路径获取和实时检测末端执行器跟踪误差的算法流程如图6所示，首先对双目相机的左右相机分别进行标定得到相机内参数，然后获取相机拍摄的图像；在任务起始时对拍摄的图像进行路径区域选取，然后对选取的图像进行滤波、二值化和中心像素提取后得到目标路径中心处的像素图，对左右相机下处理得到的像素路径进行对应像素匹配，得到初始的空间路径坐标；在机械臂运行过程中，对双目相机拍得的进行末端执行器区域选取，然后进行模板匹配，得到末端执行器的像素坐标，经过左右图像像素匹配后得到末端执行器的空间坐标，进而得到跟踪误差。

[0061] 对于检测初始路径，需要用到双目相机中三维重建的方法。对于空间中的加工路径，首先我们需要做相应的标记，用于图像检测，比如用特定的颜色画出需要跟踪的加工路径。通过图像的基本操作：相机校正、图像采集、区域限定、曲线细化、像素点匹配、三维重建，获得初始路径的空间坐标。以上操作流程中相机标定采用张氏标定法对相机内参进行解算，然后利用得到的相机参数对双目相机拍摄的图像进行矫正。对双目相机拍摄到的图像，通过手动框选出图像中包含空间路径的区域，然后对选中的区域进行图像二值化处理、提取路径中心线，从而可得到空间路径中心线在图像中的像素坐标。之后便是对应像素点进行匹配，解算出空间点的三维坐标。

[0062] 对于检测机械臂跟踪误差，在机械臂运行过程中，对双目相机拍得的图像进行工作区域选取，然后在区域内进行模板匹配，得到末端执行器的像素坐标，经过左右图像像素匹配后得到末端执行器的空间坐标，根据机械臂控制器发送的当前坐标，进而得到跟踪误差。

[0063] 首先根据双目相机三维成像原理以及相机、待加工物块和机械臂之间的坐标系关系，如图7所示坐标系示意图，有像素坐标和与其对应的三维空间点坐标的关系式如下：

[0064]

[0065] 其中，bc为双目相机两相机之间的基线距离，fc为相机焦距，(xl2，yl2)和(xr2，yr2)分别为空间点在左右图像中对应的像素坐标，(xl，yl，zl)为空间点在左相机坐标系下的三维坐标。

[0066] 得到左相机坐标系下空间点的三维坐标后，通过坐标系变换可得到机械臂坐标系(基坐标系)下空间点的三维坐标，如下：

[0067]

[0068]

[0069] 其中， 和 分别为左相机坐标系转换至任务坐标系的旋转矩阵和平移矩阵， 和 分别为任务坐标系转换至机械臂坐标系(基坐标系)的旋转矩阵和平移矩阵。

[0070] 相机焦距、基线距离、旋转矩阵和平移矩阵均可通过实验测得，重点在于如何确定左右图像中路径点像素的对应关系。本发明中设计了一个像素点匹配算法。将左右图像中的像素坐标对应匹配，即可得到由双目视觉方法解算出的空间路径的三维坐标点集。像素匹配算法的伪代码如图8所示。

[0071] 本发明底层控制设计具体为：

[0072] 首先是系统的动力学模型，机械臂在笛卡尔空间下的动力学模型如下：

[0073]

[0074] 其中， 分别表示末端执行器的位置、速度和加速度矢量；Hx∈n×n n n
R 为对称正定质量矩阵； Gx∈R表示科里奥利力和重力；u(t)∈R是机器人n×n
执行器施加的关节扭矩；J∈R 是将关节速度与末端执行器的线速度和角速度联系起来的n
雅可比矩阵，Fh(t)∈R是可以通过力/力矩传感器测量的人手对机械臂的相互作用力。

[0075] 令E(t)＝X(t)‑Xr(t)(Xr(t)为参考轨迹，E(t)为末端执行器的跟踪误差向量)，可得X(t)＝Xr(t)+E(t)， 结合笛卡尔空间下的动力学模型可得：

[0076]‑T

[0077] 上式中He＝Hx，Ce＝Cx， F(t)＝J u(t)。

[0078] 设计机械臂控制器如下：

[0079]

[0080] 式中，Md，Cd，Kd分别为期望的惯性矩阵、阻尼矩阵和刚度矩阵。

[0081] 进而可以得到时间域下闭环动力学系统的表达式：

[0082]

[0083] 至此，虽然已经得到了闭环系统关于交互力Fh(t)和跟踪误差向量E(t)及其各阶导数的时间域下的表达式，但是实际系统运行过程中，受机械臂运行速度存在误差等因素的影响，机械臂执行一次空间加工轨迹跟踪任务的时长是不定的，因此无法定时刻的表达实际系统，但考虑到空间中加工路径由起点至终点的位移是固定的，因此可以将闭环系统由时间域转换至空间域。如图9所示，左图为工作空间下带有加工路径标记的某一工件，A、B分别为加工路径的起点和终点；右图为带有任务坐标系标注的工件示意图。

[0084] 定义：对图9所示的任务坐标系下，从起点A到终点B的方向向量记为任务坐标的xA轴。S是A到B的空间距离。s是机械臂在xA轴方向上多次迭代的位移， 是机械臂在这个方向上的速度。机械臂在xA轴上的速度为vxA＞0，空间路径点之间不存在重叠，因此t和s之‑1间的关系是双射的。函数s＝f(t)是解析的，逆函数t＝f (s)是全局存在。因此，作为一个变‑1
量，v(t)也可以被描述为一个空间函数v(f (s))。

[0085] 在上述定义基础上，推导得到空间域下闭环系统的动力学模型如下：

[0086]

[0087] 式中， t＝f‑1(s)表示空间域到时域的转换关系，E(s)＝X(s)‑Xr(s)，Xr(s)为参考轨迹，E(s)为跟踪误差向量； 表示空间微分运算符。

[0088] 本发明带有视觉辅助的空间迭代学习算法结构图如图10所示，(1)视觉辅助板块：由视觉辅助系统(双目摄像机)结合电脑端的图像处理算法得到初始空间路径坐标并实时检测机械臂末端执行器的跟踪误差；(2)底层控制板块：在机械臂运行过程中，操作者施加交互力使机械臂按期望路径运行，可以由力传感器得到交互力数据，并由机械臂原计划执行的路径点坐标和受交互力影响后实际执行的路径点坐标得到机械臂执行的路径点坐标之差。(3)更新路径板块：结合以上两个版块的数据，根据设置的更新率，估测出期望路径，然后再根据处理后的交互力和估测出的期望路径得到更新后的路径点坐标，待机械臂运行完毕后将更新后的整个空间路径的路径点坐标发送至机械臂控制器进行下一次迭代学习。
(4)对于双目相机拍摄待加工工件的加工路径时存在视觉盲区的情况，当机械臂运行至盲区内的加工路径时，视觉检测误差ξ为0，初始路径为空。机械臂第一次运行至盲区内时，由人手拖动示教完成剩余加工路径的循迹，第二次更新路径时，在第一次示教时机械臂控制器记录的路径坐标的基础上进行，此后，盲区内的加工路径更新均在上一次更新路径的基础上进行。

[0089] 空间迭代学习更新率如下：

[0090]

[0091] 式中， 为空间域下对真实加工路径的估计值，X(s)为参考路径，s为机械臂运行过程中沿加工路径方向的位移，λ为一个正的比例参数，S是空间周期，表示加工路径由起点至终点的位移。

[0092] 根据此学习率可知，机械臂第一次运行时期望路径的估计值 是未知的，需从人手拖动示教过程中获取，而人手拖动示教存在抖动、末端执行器与加工路径之间接触不稳定等问题。为了提高路径学习的效率和准确性，在系统中增加了一个视觉辅助系统，用以获取初始加工路径和实时检测跟踪误差。

[0093] 增加视觉辅助系统后，利用双目相机检测初始路径，并可实时检测机械臂末端执行器的跟踪误差。此时设计新的空间迭代学习更新率如下：

[0094]

[0095] Xinitial(s)是视觉辅助系统测得的初始空间路径坐标，α∈[0，1]是一个防止过度校正的比例因子，ξ(s)为基于视觉测量的路径跟踪误差，加入视觉系统后，空间周期S定义如下：

[0096] S＝||Xinitial(S)‑Xinitial(0)||

[0097] 增加了视觉辅助系统后，提高了路径学习的准确性，提高了空间迭代学习的收敛速度，减轻了由于人类视觉和运动控制噪声而造成的不确定性的影响。

[0098] 在视觉校正过程中，如果机器人末端执行器的位置标记点被阻塞，并且在任务路径工作区定义的目标区域中无法检测到(即进入视觉盲区)，则将ξ设置为0。对应的公式表达如下：

[0099]

[0100] Xvision(s)是视觉辅助系统检测出的末端执行器的空间坐标，Target region是视觉范围内包含加工路径的区域。

[0101] 仿真实验：

[0102] 为了便于表达，以下仿真及实验中，iter代表迭代学习周期，iteri代表第i个周期；desired代表期望的空间加工路径。

[0103] 在仿真中，将任务坐标系中的期望路径设置为Xh＝[xA；yA；zA]m，其中xA∈[0，0.282]m、zA＝0以及yA(如下所示)。

[0104]

[0105] 起点和终点坐标分别为[0.5750；‑‑0.0440；0.1395]m和[0.3597；0.1366；0.1395]m。

[0106] 根据以上数据，空间周期 任务坐标系转换至机械臂坐标系(基坐标系)的旋转矩阵和平移矩阵 和 分别为：

[0107]

[0108]

[0109] 对应的仿真参数和每次迭代学习的时间周期设置如表1所示。机器人在xA轴上的速度为 其中Ti表示第i次迭代学习的时间周期。

[0110] 表1仿真参数

[0111]

[0112] 使用表1参数进行仿真，仿真结果如图11所示，每个迭代学习周期对应的跟踪误差和交互力的均值和均方根值及其变化趋势如图12所示。

[0113] 图11可见，随着空间迭代学习的进行，算法输出的空间路径逐渐逼近期望路径，在6个迭代学习周期之后，算法输出的空间路径与期望路径几乎重合；由图12所示，随着迭代学习周期的增多，每次迭代中的路径跟踪误差和交互力的均值和均方根值呈逐渐减小的趋势，即提出的空间迭代学习算法是收敛的；且表现出了良好的路径学习性能。

[0114] 以图13所示的带有加工路径标记的物块为实验对象，实验中使用的硬件设备、连接方式和设备型号如图4所示，实验采用机器人操作系统(ROS)进行开发，将实时位置、速度和交互力等信息作为ROS中的不同主题进行订阅，并在电脑端显示相关主题，用于学习过程的分析。

[0115] 如图13所示待加工物块，黑色加粗线条为期望加工路径，A点为起始点，B点为终点。实验中使用的控制参数和每个迭代学习周期测得的机械臂运行时间如表2所示：

[0116] 表2实验参数

[0117]

[0118] 使用本发明提出的视觉辅助和空间迭代学习结合的算法，实验了6个迭代学习周期，为了便于观察实验结果，其中迭代学习周期1、周期6对应的路径跟踪结果和期望的路径如图14所示。

[0119] 由图14可见，第一个迭代学习周期中，机械臂末端执行器对空间加工路径的跟踪存在较大偏差且路径点之间波动明显；随着人机协作过程的进行和带视觉辅助的空间迭代学习算法的运行，在6个迭代学习周期后，末端执行器对空间加工路径的跟踪误差已经很小，本发明的算法表现出对陌生工件良好的适应能力和路径学习能力。

[0120] 综上所述，对于放置在工作空间中的待加工工件，在有限次的迭代学习周期内，本发明的算法可以有效确定精度较高的加工路径的空间坐标，用以辅助焊接、激光切割等工作场合。