基于PatMax 视觉引导的机械臂末端轨迹规划与控制

陈策博，万 琴，苏宏博，刘 波

（1.湖南工程学院 电气与信息工程学院，湘潭 411100；2.湖南中南智能装备有限公司，长沙 410117）

近年来，机器人技术在工业自动化、医疗保健、军事防务等领域得到了广泛的应用。机械臂作为一种典型的机器人，具有灵活性、精度高等优点，已经成为各种工业机器人和服务机器人中不可或缺的部分。机械臂的末端轨迹规划和控制是机械臂控制中的重要问题，对于机械臂的操作精度和稳定性有着重要的影响。

因动力学和摩擦等因素，传统的数学模型机械臂末端轨迹规划和控制难以准确描述机械臂运动特性。基于视觉引导的机械臂末端轨迹规划和控制备受关注。视觉引导方法可实现目标物体的实时识别和跟踪，提高机械臂末端定位和运动控制精度。在制造业中，早期的焊接通常采用机械定位和人工示教，精度低、瑕疵率高、产品部件不匹配，甚至产生结构畸变。当人工示教无法满足工厂的精度要求时，机器视觉系统的定位工作备受关注。因此，本文提出了一种基于PatMax 视觉引导的机械臂末端轨迹规划与控制系统。该系统采用了Cognex 公司旗下的VisionPro 视觉软件，对已有的视觉规划和控制系统进行了二次开发[1-2]，可以更好地实现机械臂末端的轨迹规划和精确定位。

1 视觉定位系统硬件设计

视觉技术硬件组件的设计是其使用的关键之一。光源、镜头、摄像头以及摄像头与计算机之间的接口是视觉定位系统的主要硬件元件。康耐视3DA5120 点云相机是本文在研究中使用的主要设备之一。该相机具有超过150 万个3D 数据点，可检测元件上的细微特征，构建高分辨率的3D 数据点云图像。另外，采用了适应性强的协作式工业机器人UR5，通过编写程序，可以沿着所需的运动轨迹来精确地移动，也可以使用电信号与参与其他机器设备工作时进行通信。在数据处理方面，配备了具有独立显卡RTX2060 的视觉处理工控机。为保证数据传输的稳定性，使用四口网线交换机和五类通讯网线（30 m）。视觉定位系统硬件如图1 所示。

图1 视觉定位系统硬件图Fig.1 Visual positioning system hardware diagram

3D 相机和工控机之间通过BCM 万兆以太网口建立连接，以确保图像传输的速度和质量。机械臂控制柜和工控机之间建立了TCP/IP 通信[3-4]，通过网口进行数据传输。机械臂控制柜在上位机的角色下，向工控机发出拍照指令，实现了机械臂的拍照功能。之后，工业控制计算机指挥相机进行拍照操作，并将捕获的图像数据传递给工业控制计算机内部的程序进行处理。将经过处理的数据转化为信号，然后传输给机械臂，以便机械臂能够执行相应的动作。

2 视觉定位系统软件设计

2.1 图像采集（获取）

本文采用了基于康耐视A5000 点云相机的视觉系统，该系统使用了Gigabit Ethernet 万兆接口，利用VisionPro[5]软件内原有的CogAcqfifotool 工具来进行采集并处理拍摄的图像。在这个软件的视觉系统中，首先创建了一个图像抓取工具用来连接，此工具是维持先进先出队列的Acquisition FiFo 对象，可以设置视频格式、亮度、对比度、曝光、对等参数[6]，从而获取含有深度信息的图像。将采集的深度图像信息传输给输入图像，并且同时进行后续工作的处理。通过这样的设计，可以实现对图像的高效准确采集和处理，并为后续的数据分析提供有效的支持。

图像采集步骤：

（1）通过Cognex GigE Vision Configuration 工具配置相机连接环境；

（2）确认相机是否连接；

（3）为康耐视相机设置视频格式；

（4）利用FIFO 对象，设置相机的参数；

（5）利用Acquire（）方法获取图像；

（6）将获取图像进行保存。

2.2 手眼标定

一般来说，相机与机械臂之间的位置关系有两种：一种是“眼在手外”（eye-to-hand），即相机摄像头的位置在机械臂的外边，并与机器人的基座的位置（即世界坐标系）保持固定，不随着其位置的改变而发生移动，这种模式适用于需要稳定的视野，或是需要在机械臂的工作空间之外进行视觉检测和控制的情况；另一种是“眼在手上”（eye-in-hand），即相机摄像头的位置安装在机械臂上，并会随着机械臂位置的变化而发生移动，需要进行复杂的姿态计算和控制，这种模式适用于需要机械臂在其工作空间内对目标物体进行精确定位和操作的情况，手眼标定示意图如图2 所示。

图2 手眼标定示意图Fig.2 Hand-eye calibration diagram

本文采用的是眼在手外的手眼标定系统，在机械臂外的架子上固定住Cognex3D 点云相机，并固定机械臂的基座，使其不会随着机械臂位置的变化而移动。这样就保证了相机和机械臂底座的位置相对固定。坐标变换的示意图如图3 所示。

图3 坐标变换示意图Fig.3 Coordinate transformation diagram

图3 中：机械臂基座坐标系（W）；相机坐标系（C）；机械臂末端坐标系（B）；工件坐标系（A）。

4 个坐标系对应3 个变换矩阵：

（1）机械臂基座坐标系（W）—机械臂末端坐标系（B），T（W-B）该变换矩阵可以直接从机械臂坐标系得到。

（2）相机坐标系（C）—机械臂基座坐标系（W），T（C-W）该变换矩阵即为待求取的位置固定矩阵—手眼标定矩阵。

（3）工件坐标系（A）—相机坐标系（C），T（A-C）该变换矩阵是一个相对固定的矩阵，通过视觉定位求取。

3D 相机不需要单独标定，但是需要和机械臂标定。待求量是机械臂底座坐标系（W）和相机坐标系（C）之间的位姿关系：T（C-W）。

定义坐标系A-B 的变换矩阵为T（A-B），根据坐标系之间变换关系：

在标定的过程中，固定住了标定板坐标系（A）和机械臂的末端坐标系（B），所以在机械臂不同姿态下的T（A-B）均不会发生变化，这里令X=T（C-W），E=T（A-C），F=T（W-B），所以机械臂姿态下的X 值不会发生变化，但Ei、Fi 不同，E 与机械臂相关，F与相机相关。

变换后，手眼矩阵为

用平移向量T 和旋转矩阵R 来表示矩阵方程，可以得到：

式中：T 是大小为3×1 的平移矢量；R 是大小为3×3的旋转矩阵。手眼标定的基本方程可以由式（6）得到：

进行多次变换机械臂末端位姿，对手眼标定方程式（4）求解，即可得到手眼转化矩阵X 的值。为了提高实验的精准度，标定时共拍摄6 组照片。

2.3 图像定位

图像定位前需要首先对图像进行预处理，首先使用3DPatMaxTool 工具对目标物进行寻找，以获取物体的坐标信息，并将其存储到数据库中。然后通过在目标图像中寻找相近的形状来确定目标物体的坐标信息，该方法不受特定图像灰度级别的限制，提高了精度和抗干扰性。此外，通过设定参数特征阈值，该算法能够旋转、缩放目标，具有较高的灵活性和适应性。定位流程如图4 所示。

图4 PatMax 定位流程Fig.4 PatMax positioning flow chart

在使用3DPatMaxTool 工具时，首先需要拍摄一张所需的3D 照片，并框选出所需的具体几何特征。在选框的中心设定好训练模板原点，用来定位Mark点的位置，这样可以实现最高精度的定位。经过训练后，PatMax[7]利用模板对Mark 点进行图像模板3D匹配，并返回该图像的Mark 点的中心坐标，以实现精确定位，模板定位如图5 所示。

图5 PatMax 特征的模板定位Fig.5 PatMax feature template localization

通过手眼标定得到了相机坐标系下机械臂底座的位姿H（C-W），模板匹配定位是确定了工件坐标系相对于相机的位姿H（A-C），通过让机械臂直接读取H（W-B），可以获得机械臂末端坐标系相对于机械臂基坐标系的位姿。

工件坐标系相对于机械臂末端坐标系的位姿为

3 实验

在本实验中，机械臂末端需要在2 块焊板之间移动，利用基于VisionPro 软件完成的视觉定位技术，对机械臂末端进行精确定位。而后，通过对机械臂控制系统的编程来控制机械臂末端按照预先设定的轨迹进行运动，实现对焊板的自动焊接。

3.1 实验平台

本试验在Win10 系统下，采用C# 的语言环境，利用VisionPro 开发软件进行开发，手眼标定的目标物体三维特征明显，视觉系统操作界面如图6 所示。

图6 视觉系统操作界面Fig.6 Visual system operating interface

视觉引导的机械臂末端轨迹规划与控制的目的之一是获取机械臂底座坐标系上目标物的坐标，通过相机拍摄的标定，对图像上的坐标与机械臂的底座坐标系进行匹配，定位到目标物便可以获取其位置。

3.2 2D 坐标与3D 坐标转换

为了找到所需的目标物，采用了VisionPro 开发软件中的3DPatMaxTool 工具，该工具通过内置的模板匹配技术来辨识目标物，3DPatMaxTool 工具的输入图像是摄像机标定后的图像，目标物的位置可以在工控机上直接输出机械臂底座坐标系上的坐标。接着继续使用以下工具Cog3D Range Grey Splitter Script3D 范围灰色拆分器脚本，目的是将高度和灰度图像（16 位Range）分离为高度图像（16 位Range）和灰度图像（16 位Grey），以便于软件进行模板匹配工作。

康耐视3D 点云相机拍摄一次照片会同时得到2D 成像照片和3D 点云图两种数据。然而在视觉定位软件系统中，仅能直接使用2D 成像照片数据，而无法直接读取3D 点云图数据。因此，需要通过代码编写的方式，将2D 成像照片中的坐标点转换为3D点云的坐标，增加一个坐标轴的坐标信息，以便于实现3D 点云图像的使用。

文中所拍摄的6 帧点云图片同时记录了机械臂末端执行器在机器人坐标系下的位置，如表1 所示。表1 中X，Y，Z，UX，UY，UZ 分别代表六轴机械臂执行器在机械臂基座坐标系的坐标。

表1 机器人六轴坐标（相对于机械臂基座坐标系）Tab.1 Robot six-axis coordinates（relative to arm base coordinates）

3.3 实验结果

完成所有训练之后，进行机械臂末端移动实验，改变工件的位置并使用相机进行拍照定位，识别出的目标物与模板进行匹配，以坐标的形式发送到工控机，保存在设置好的路径，然后传输给机械臂，机械臂末端可到达所需的位姿。标定时共拍摄6 组照片，如图7 所示。

图7 点云标定图Fig.7 Point cloud calibration map

将训练好的1 组图像与其他的5 组图像使用3DPatMax 能快速匹配出标定物的点云特征，按照式（5）可以得到5 个F 矩阵。通过对机械臂基座的位置信息和相机的外参矩阵，对EX=XF 进行求解，最终得到手眼关系矩阵X：

再移动工件的位置，进行多次重复试验，定位准确率高达99%，3DPatMax 图像定位得分为0.98。结果表明，每次都能定位到工件所需要到达的位置。

4 结语

本文采用旋转矩阵和平移向量以及3DPat-MaxTool 工具对眼在手外的手眼标定系统康耐视3D点云相机进行了图像匹配定位和标定，随后基于成熟的可视化开发软件，采用标定好的相机进行对视觉系统的二次开发，以确保其可靠性。实验结果表明，采用PatMax 视觉引导的机械臂末端轨迹规划与控制系统能够快速地进行应用程序开发，并准确地定位训练并识别特征物品的位置，在实现工业自动化，特别是机器人的精确控制方面具有重要实际应用意义。