应用摄像机标定的机械臂轨迹跟踪控制系统

贾 蓉，郑秀娟，张金亮

（1.湖北 汽车工业学院电气与信息工程学院，湖北 十堰 442002；2.武汉科技大学信息科学与工程学院，湖北 武汉 430081）

1 引言

机械臂轨迹跟踪控制系统能够实现机械臂的轨迹跟踪控制，对于提升机械臂的精细化控制程度有很大意义[1]。在提升机械臂控制精度与升级其控制结构的过程中，必须同步进行机械臂轨迹跟踪控制，才能真正实现作业中控制精度的提升。机械臂轨迹跟踪控制涉及控制技术、计算机视觉技术、机器人运动学技术、图像高速处理技术等多种技术学科，控制过程极其复杂[2]。

对于机械臂轨迹跟踪控制系统的研究，相关研究学者己经取得了一定成绩。文献[3]提出基于视觉图像转换技术的机械臂轨迹跟踪控制系统，通过视觉图像转换技术获取目标位置，结合图像分析算法计算目标与机械臂的相对位置，通过OPC传输给机械臂控制模块，实现机械臂的轨迹跟踪控制。该方法的跟踪控制效率较好，但受目标物体质心投影轨迹不清晰的影响而无法进行信号反馈，存在控制时延过长的问题。文献[4]提出基于模型预测控制方法的机械臂轨迹跟踪控制系统。通过欧拉-拉格朗日公式构建机械臂运动动力学模型，应用线性化模型实现机械臂轨迹跟踪控制的模型中。该方法的控制时延较短，但跟踪控制准确性较差。文献[5]提出提出基于改进自适应神经滑模的机械臂轨迹跟踪控制系统。通过状态反馈控制系统自适应学习，输出滑模控制器补偿值，实现机械臂轨迹跟踪控制。该方法信号反馈时效性较好，跟踪控制效率较差。

针对上述方法存在的问题，提出应用摄像机标定的机械臂轨迹跟踪控制系统。在系统硬件部分，通过主控单元、UVC摄像头及压力检测传感器等硬件设计工控模块，基于前馈—反馈控制结构设计反馈模块，实现对机械臂轨迹跟踪信号的采集及反馈。软件部分，采用摄像机标定算法，通过获取轨迹跟踪物体视场的旋转、平移以及摄像机对应内参数，实现机械臂的轨迹跟踪控制；应用摄像机标定，通过机械臂已知运动轨迹，将整个运动过程中的数据密化，完成机械臂运动轨迹重现，实现机械臂运动目标的跟踪控制。

2 机械臂轨迹跟踪控制系统硬件设计

应用摄像机标定的机械臂轨迹跟踪控制系统的硬件构成包括工控模块、反馈模块[6]。通过工控模块实现电机工作状态控制，利用前馈—反馈控制结构设计反馈模块，对机械臂轨迹跟踪信号进行反馈。

2.1 设计工控模块

工控模块由主控单元、UVC摄像头、视频服务器、无线网桥、无刷直流电机、压力检测传感器构成[7]。

其中主控单元负责进行远程控制端通信、运动控制、图像信息采集及网络管理。主要由104/PC-plus工控机构成，选用型号为AMD主板工业嵌入式1815-104工控机[8]。该工控机的具体数据，如表1所示。

表1 工控机的具体数据Tab.1 Specific Data of IPC

视频服务器选用的型号为威视海康SD6704，作为一种新型网络视频服务器，其采用多种标准编码算法，包括MJPEG、MPEG2、MPEG4、H.264。该视频服务器具有强大的兼容性，主要基于Linux操作系统，在FLASH中固定代码，支持各种网络协议。

压力检测传感器选用的型号为微型高精度A301压力传感器，其尺寸为（25.4×14×0.208）mm，能够保持（±3）%之内的线性误差，测量范围为（0～540）N。在MCP6004运放中内置了共4个运算放大器[10]，支持的工作电压为（1.9～6.2）V，输出输入均为轨至轨形式，工作静态电流仅为100μA，带宽2MHz。压力检测中等效压力的具体计算公式如下式：

2.2 设计反馈模块

基于前馈—反馈控制结构设计反馈模块，反馈模块的主要作用是对机械臂轨迹跟踪信号进行反馈。反馈模块的具体结构为反馈控制器、执行器、前馈控制器。反馈模块的具体运行流程如下：首先向反馈控制器输入目标信号，接着反馈控制器向执行器发送信号，并向前馈控制器发送信号，经过测量与变送，该信号与执行器输出信号汇集为被控变量，通过被控变量对目标进行跟踪，获取跟踪信号，并对其进行测量与变送，向系统进行反馈，实现轨迹跟踪信号的反馈。

3 应用摄像机标定的机械臂轨迹跟踪控制系统软件设计

3.1 轨迹跟踪

通过摄像机标定算法实现机械臂的轨迹跟踪。通过摄像机标定进行机械臂轨迹跟踪的具体流程如下：

用下式表示空间中的三维点到成像平面的对应映射单应性关系式：

式中：fx、fy—摄像机对应内参数f的左右坐标；cx、cy—摄像机对应内参数c的左右坐标。

对一个平面进行定义使Z等于0。对旋转矩阵进行分解，将其分解为三个大小为3×1的向量，则可省略其中一个向量。分解后的旋转矩阵具体如下式：

对于机械臂轨迹跟踪物体，可以利用上式对多幅图像进行计算，从而获取轨迹跟踪物体视场的旋转、平移以及摄像机对应内参数。

3.2 基于直线插补算法的机械臂运动轨迹跟踪控制

在机械臂运动轨迹跟踪控制过程中，每个运动坐标点都需要和给定的数据进行比对，进而决定下一步该怎么走。

直线插补算法能够通过已知坐标点确定运动轨迹，已知曲线上的某些数据，推导出中间点的方法，也称为“数据点的密化”。

通过直线插补算法对之间的空间进行数据密化，从而形成要求的轮廓轨迹，通过直线插补算法实现机械臂的运动控制。直线插补算法的具体步骤如下：

（1）进行插补准备，具体计算公式如下：

式中：K—插补周期中给定的进给速率数；

ΔL—一个插补周期中的对应进给线段长度；

L—直线段长度；

Δx、Δy、Δz—x、y、z三个坐标轴的位移增量；

xe、ye、ze—终点的坐标轴坐标。

（2）插补计算，具体公式如下：

式中：xi+1、yi+1、zi+1—插补结果坐标点的坐标值；

xi、yi、zi—插补点的坐标值。

据此得出了插补点的坐标值，还原了整体运动轨迹。通过反馈模块将运动轨迹信息传输至工控模块，实现机械臂轨迹跟踪控制。

4 实验测试

4.1 实验设计

为证明设计的应用摄像机标定的机械臂轨迹跟踪控制系统的性能，对其进行实验测试。实验中采用的机械臂为直线式九自由度机械臂，其立体空间中的工作范围为（800×800×800)mm，运行速度的最大值为100mm/s。

直线式九自由度机械臂结构，如图1所示。

图1 实验所用机械臂Fig.1 Manipulator Used in Experiment

4.2 实验方法与步骤

利用应用摄像机标定的机械臂轨迹跟踪控制系统对实验机械臂进行轨迹跟踪控制，在MATLAB软件搭建实验平台，使用上文提到的九自由度机械臂进行圆弧轨迹跟踪实验，获取在机械臂位置增量为（10.52～30.52）mm 的范围内的控制时延数据以及曲线跟踪仿真结果作为实验数据。采用原有的前馈—反馈控制方法、文献[4-5]方法作为实验对比方法，验证机械臂位置增量为（10.52～30.52）mm的范围内的控制时延及轨迹跟踪控制准确性，实验流程图，如图2所示。

图2 实验方法流程图Fig.2 Flow Chart of Experimental Method

4.3 实验结果分析

在机械臂工作范围内选取（200×200×200）mm的立体空间，让其进行圆弧轨迹实验，机械臂在范围为（200×200×200）mm 的立体空间内作随机圆弧轨迹运动，A为运动路径的起始点，B为运动终点。其运动路径，如图3所示。所设计应用摄像机标定的机械臂轨迹跟踪控制系统是在原有前馈—反馈控制系统的基础上进行改进，所以将应用摄像机标定的机械臂轨迹跟踪控制系统和前馈—反馈控制系统、文献[4-5]系统进行对比实验，得到轨迹跟踪路线图，如图4所示。

图3 机械臂运行轨迹Fig.3 Trajectory of Manipulator

图4 轨迹跟踪控制路径Fig.4 Trajectory Tracking Control Path

由图4可知，在九自由度机械臂圆弧轨迹跟踪实验过程中，原有的前馈—反馈控制系统在跟踪前期得到的跟踪路径与实际路径较为一致，在跟踪后期由于未能及时获取误差坐标点，出现了跟踪偏差较大的问题；而所提方法得到的轨迹与机械臂的实际运行轨迹较为一致，解决了原有方法存在的问题，体现了其良好的跟踪准确性。而文献[4]方法得到的轨迹具有一定的误差，与设计圆弧轨迹相比有所偏离，文献[5]方法在轨迹跟踪过程中存在缺失，没有得到一个完整的圆弧轨迹，且与设定轨迹的偏离误差较大。

收集轨迹跟踪路径数据，得到轨迹跟踪控制准确性，如图5所示。

图5 轨迹跟踪控制准确性对比实验结果Fig.5 Experimental Results of Trajectory Tracking Control Accuracy Comparison

根据图5可知，在机械臂35min运动时间内，原有的前馈—反馈控制系统对机械臂运动轨迹跟踪控制的准确度平均值为83%，文献[4]方法对机械臂运动轨迹跟踪控制的准确度平均值为69%，文献[5]方法对机械臂运动轨迹跟踪控制的准确度平均值为63%，而所设计系统对机械臂运动轨迹跟踪控制的准确度平均值为93%。所设计系统应用摄像机标定准确计算出了机械臂运动轨迹坐标点，并通过反馈系统传递给控制模块，因此能实现机械臂运动目标的准确跟踪控制。

在机械臂位置增量为（10.52～20.52）mm的范围内，应用摄像机标定的机械臂轨迹跟踪控制系统对比系统的控制时延对比实验结果，如表2所示。

表2（10.52～20.52）mm范围内控制时延对比实验结果Tab.2 Comparative Experimental Results of Control Delay in the Range of（10.52～20.52）mm

根据表2结果可知，应用摄像机标定的机械臂轨迹跟踪控制系统的控制时延远低于文献对比系统及原有的前馈—反馈控制系统。这是因为所设计系统在前馈—反馈控制结构的基础上引入了直线插补算法，能够快速求解运动控制插补点坐标值并传输至工控模块，有效降低了控制时延，提升了控制效率。

5 结束语

为提升机械臂轨迹跟踪控制的准确性及效率，提出应用摄像机标定的机械臂轨迹跟踪控制系统设计，设计工控模块由主控单元、UVC 摄像头、视频服务器压力检测传感器构成；引入前馈-反馈控制结构实现控制系统的快速反馈。软件部分通过摄像机标定算法实现机械臂的轨迹跟踪，引入直线插补算法得出目标物体准确坐标值，实现机械臂的运动控制。实验结果表明，应用摄像机标定的机械臂轨迹跟踪控制系统能够有效提升机械臂轨迹跟踪控制的准确性，通过直线插补算法实现了控制时延的降低。