CN111590594A - 基于视觉引导的机器人轨迹跟踪控制方法 - Google Patents

Abstract

本发明涉及一种基于视觉引导的机器人轨迹跟踪控制方法，包括建立机器人视觉伺服控制系统；建立双目视觉统一测量场；利用双目视觉设备观测得到末端执行器坐标系相对于测量坐标系之间的位姿变换关系，并经过双目视觉测量场转换到机器人基坐标系；利用Kalman滤波器对观测到的末端执行器位姿进行平滑估计；计算末端执行器位姿误差；设计基于模糊PID的视觉伺服控制器，对位姿误差进行处理后得到下一时刻的期望位姿，发送给机器人系统控制末端执行器运动。本发明面向航空航天大部件柔性加工领域、机器人高精度加工装备应用需求，通过视觉传感器实时感知末端执行器位姿，形成闭环反馈系统，大幅度提高六自由度串联机器人的轨迹运动精度。

Description

基于视觉引导的机器人轨迹跟踪控制方法

技术领域

本发明属于机器人轨迹跟踪控制技术领域，具体涉及一种基于视觉引导的机器人轨迹跟踪控制方法。

背景技术

近年来，工业机器人凭借其高度柔性化、低成本、高工作空间/占地面积比的优点，在航空航天领域的应用越来越广泛，但航空航天制造业中的飞机大部件加工与装配往往需要很高的精度，目前的机器人根据预设运动轨迹进行加工作业，由于机器人整体精度低，导致加工精度和表面质量难以得到保证。所以需要一种机器人高精度轨迹跟踪控制方法。

公告号为CN106671103A的发明专利公开了一种铣削机器人控制方法及系统，采用激光跟踪仪作为末端执行器位姿测量设备，采集铣削机器人的实时铣削轨迹数据，计算修正参数，发送到铣削机器人对其进行控制。激光跟踪仪的激光头只能发出一束激光，依次扫描末端执行器上安装的靶标球，获取所有靶标球的位置信息，可求解末端执行器的姿态，当末端执行器运动时，需要考虑每个靶标球位置信息的采集时长，各靶标点位置信息的获取存在时差，所以存在原理误差，测量频率受限，实时性差。不仅如此，在复杂的工况中，激光一旦被遮挡就无法继续跟踪靶标球，不能继续测量。

发明内容

本发明所要解决的技术问题是针对上述现有技术的不足，提供一种基于视觉引导的机器人轨迹跟踪控制方法。

为实现上述技术目的，本发明采取的技术方案为：

基于视觉引导的机器人轨迹跟踪控制方法，其中包括如下步骤：

步骤S1：建立视觉伺服控制系统，包括双目视觉测量设备、机器人、末端执行器、视觉伺服控制器；

步骤S2：建立不同坐标系，包括传感器坐标系F_S，机器人基坐标系F_B，末端执行器坐标系F_E，工件坐标系F_W，目标坐标系F_H，然后经坐标系变换建立双目视觉统一测量场；

步骤S3：利用双目视觉测量设备识别末端执行器上的靶标点群，获取末端执行器坐标系F_E相对于传感器坐标系F_S之间的位姿变换关系

并经过双目视觉统一测量场转换到机器人基坐标系；

步骤S4：利用Kalman滤波器对步骤S3中获取的末端执行器位姿进行平滑估计；

步骤S5：对步骤S4中输出的末端执行器当前位姿数据，与预设理论位姿进行比较并计算位姿误差；

步骤S6：将步骤S5中得到的末端执行器位姿误差作为输入，经视觉伺服控制器处理得到下一时刻的期望位姿，发送给机器人系统逆解成所需关节角度，根据计算得出的关节值控制末端执行器到达相应位置。

为优化上述技术方案，采取的具体措施还包括：

进一步地，步骤S2中，将末端执行器的位姿转化到机器人基坐标系下的位姿：

其中，机器人基坐标系F_B与工件坐标系F_W在F_S中分别表示为旋转矩阵

与

目标坐标系F_H在工件坐标系F_W下的描述为

进一步地，步骤S3中，根据末端执行器坐标系的原点在靶标点群几何中心处、末端执行器坐标系通过旋转矩阵Λ旋转一定角度、末端执行器坐标系通过平移向量q平移一定距离、末端执行器坐标系做旋转Λ和平移q复合运动这四种情况，计算末端执行器相对于靶标点群的不同位姿下的测量误差，选择测量误差最小的末端执行器坐标系相对于靶标点群的位姿，优化靶标点群的布局，利用双目视觉测量设备识别末端执行器上的靶标点群，获取末端执行器坐标系F_E相对于双目视觉测量设备的测量坐标系F_S之间的位姿变换关系

并经过双目视觉测量场转换到机器人坐标系中。

进一步地，步骤S4中，根据双目视觉测量设备的采样周期、动态测量状态下各自由度测量数据的标准差、机器人运动加速度标准差确定Kalman滤波器中的各项矩阵值；

根据双目视觉的采样周期t，确定机器人的状态转移矩阵F：

设机器人运动预测噪声协方差矩阵Q_k：

统计动态测量状态下机床末端各个自由度测量数据的标准差，确定双目视觉的测量噪声协方差矩阵：

其中，状态转移矩阵F、机器人的预测噪声协方差矩阵Q_k和双目视觉的测量噪声协方差矩阵R_k构成Kalman滤波器中的各项矩阵值。

进一步地，步骤S5中，计算末端执行器位姿误差，包括直线轨迹运动和圆弧轨迹运动，对于直线轨迹运动的末端执行器位姿误差为：

ΔPR＝[P_n R_d]^T-[P_c R_c]^T，

P_n＝(k(x₂-x₁)+x₁ k(y₂-y₁)+y₁ k(z₂-z₁)+z₁)

其中，Pn为实际位置Pc到直线上垂足；

对于圆弧轨迹运动的末端执行器位姿误差为ΔP＝P_d-P_c，ΔR＝R_d-R_c。

进一步地，步骤S6中，对机器人末端执行器的6个自由度分别基于模糊PID视觉伺服控制器进行调整控制。

本发明的有益效果：

本发明基于视觉引导的机器人轨迹跟踪控制方法，通过双目视觉设备实时跟踪测量机器人末端执行器的实际位姿，增加闭环回路形成闭环控制系统，由闭环反馈获得的位姿误差通过视觉伺服控制器处理，得出下一时刻的期望位姿并发送给机器人系统逆解成所需关节角度，根据计算得出的关节值控制末端执行器到达相应位置。本发明实现了利用双目视觉测量设备对机器人末端执行器轨迹进行实时的调整，优化了靶标点群的布局，解决了现有技术中机器人本体轨迹精度差以及利用激光跟踪仪对铣削轨迹实时调整存在的原理性测量误差。本发明的有益效果经过空载运行试验验证和铣削试验验证，相比于使用机器人直接铣削，使用本方法进行铣削轨迹跟踪控制后，直线误差降低了92％，圆弧误差降低了91％。

附图说明

图1是本发明的工作流程图；

图2是本发明的机器人视觉伺服控制模型示意图；

图3是本发明的末端执行器相对于靶标点群的4种不同的位姿关系示意图；

图4模糊PID控制器原理图。

具体实施方式

以下结合附图对本发明的实施例作进一步详细描述。

如图1-4所示，本发明为一种基于视觉引导的机器人轨迹跟踪控制方法，其中包括以下步骤：

步骤S1：建立视觉伺服控制系统，包括双目视觉测量设备、机器人、末端执行器、视觉伺服控制器；

具体为，通过双目视觉测量设备实时跟踪测量机器人末端执行器的实际位姿，增加闭环回路形成闭环控制系统，由闭环反馈获得的位姿误差通过视觉伺服控制器处理，得出下一时刻的期望位姿并发送给机器人系统系统逆解成所需关节角度，根据计算得出的关节值控制电机到达相应位置，外部控制系统通过RSI交互接口与机器人系统连接，从外部对机器人进行控制。机器人视觉伺服控制模型如图2所示。

步骤S2：建立不同坐标系，包括传感器坐标系F_S，机器人基坐标系F_B，末端执行器坐标系F_E，工件坐标系F_W，目标坐标系F_H，然后经坐标系变换建立双目视觉统一测量场；

其中，传感器坐标系F_S由双目视觉建立，机器人基坐标系F_B与工件坐标系F_W在F_S下的描述由旋转矩阵表示，记为

与

根据产品加工要求确定目标坐标系F_H，在工件坐标系F_W下的描述为

机器人加工位置即为F_E与F_H重合位置，机器人NC代码指令数值由式(1)计算，即末端在机器人基坐标系下的位姿：

步骤S3：利用双目视觉测量设备识别末端执行器上的靶标点群，获取末端执行器坐标系F_E相对于传感器坐标系F_S之间的位姿变换关系

并经过双目视觉统一测量场转换到机器人基坐标系；

具体为，获取末端执行器位姿信息至少需要3个不共线的靶标点位置信息。末端执行器坐标系F_E相对于双目视觉测量设备的测量坐标系F_S之间的位姿求解问题可以由以下方式描述：空间中有n(n>3)个视觉靶标点，并与一个末端执行器坐标系F_E固连，将靶标点在F_E坐标系下的坐标值m_i构成矩阵M＝[m₁ m₂ … m_n]；同理，在双目视觉坐标系F_S下，靶标点的坐标值s_i构成矩阵S＝[s₁ s₂ … s_n]，靶标点在两个坐标系下坐标值的变换如下：

Rm_i+t＝s_i (2)

其中R是F_E变换到F_S的旋转矩阵，t是F_E变换到F_S的平移向量。由于双目视觉测量的误差，无法求得满足所有m_i变换到s_i的旋转矩阵R和平移向量t，为求解旋转矩阵R与平移向量t的最优估计，即求式(3)的最小值。

令

构建矩阵M_c和S_c：M_c＝[m_c1 m_c2 … m_cn]，S_c＝[s_c1 s_c2 … s_cn]，令

对P做奇异值分解：P＝U∑V^T，可得最优旋转矩阵R和最优平移向量t：

末端执行器坐标系F_E相对于双目视觉测量坐标系F_S之间的位姿变换关系

如式(5)所示：

根据最优旋转矩阵R和平移向量t的表达式可知，末端执行器坐标系F_E相对于测量坐标系F_S之间的位姿变换关系

的测量精度与末端执行器坐标系相对于视觉靶标点群的位姿有关。针对同一靶标群，末端执行器相对于靶标点群的位姿关系有4种，如图3所示。F_E0坐标系原点在靶标点群几何中心处，在测量坐标系F_S下位置为t，姿态与测量坐标系相同；F_E1坐标系由F_E0坐标系通过旋转矩阵Λ变换得到；F_E2坐标系由F_E0坐标系平移q得到；F_E3由坐标系F_E0坐标系的平移q加旋转Λ。由于建系位姿不改变靶标点相对于测量坐标系F_S的位置，故有S＝S₀＝S₁＝S₂＝S₃。

计算不同建系位姿对测量精度的影响：a.F_E0测量精度

根据矩阵变换关系，容易得出F_E0在测量坐标系F_S下所测得的旋转与平移为：

R₀＝EI，t₀＝t+e (6)

其中E是一个表示微小偏差的误差正交矩阵，由靶标点测量噪声所引起的微小误差决定，e是一个表示微小偏差的三维列向量。E和e共同表示C-Track位姿测量误差，是测量系统的固有属性。

b.F_E1测量精度

对于末端执行器坐标系F_E1，靶标点坐标在末端执行器坐标系F_E1和F_E0下存在关系：m_1,i＝Λ^Tm_0,i，计算末端执行器坐标系F_E1在测量坐标系F_S下所测得的旋转与平移为：

R₁＝ΛR₀＝ΛEI，t₁＝t+e (7)

对比式(6)与式(7)可以看出建系姿态不影响最优平移向量t₁的误差。

c.F_E2测量精度

对于末端执行器坐标系F_E2，靶标点坐标在末端执行器坐标系F_E2和F_E0下存在关系：m_2,i＝m_0,i+q，计算末端执行器坐标系F_E2在测量坐标系F_S下所测得的旋转与平移为：

R₂＝EI，t₂＝-Eq+t+e (8)

对比式(6)与式(8)可以看出当末端执行器坐标系偏移q时，最优平移向量误差多了一项-Eq，当q的值越大时，误差也就越大。

d.F_E3测量精度

对于末端执行器坐标系F_E3，靶标点坐标在末端执行器坐标系F_E3和F_E0下存在关系：m_1,i＝Λ^Tm_0,i+q，计算末端执行器坐标系F_E3在测量坐标系F_S下所测得的旋转与平移为：

R₃＝ΛEI，t₃＝-ΛEq+t+e (9)

对比式(6)与式(9)可以看出，当末端执行器坐标系偏移q时，最优平移向量误差多了一项-ΛEq，当q的值越大时，误差也就越大。

综合以上分析，测量噪声主要取决于末端执行器坐标系的偏移量q，偏移量q与测量噪声成正相关。因此，在建立末端执行器坐标系的时候，原点位置需要尽可能靠近靶标点几何中心。在实际使用过程中，末端执行器坐标系的原点通常根据需求设定，因此在实际的使用过程中，粘贴靶标点时需要将靶标点群中心尽可能的靠近刀尖点位置，尽可能减小从位姿测量原理上所导致的位姿测量噪声。

步骤S4：利用Kalman滤波器对步骤S3中获取的末端执行器位姿进行平滑估计；

具体为，根据机器人的运动性能以及双目视觉测量设备的测量性能来确定Kalman滤波器中的各项矩阵值。首先确定机器人的状态转移矩阵F，根据双目视觉测量设备的采样周期t确定：

协方差矩阵Q_k是机器人的运动预测噪声协方差矩阵，设预测噪声协方差矩阵Q_k如式(11)：

对于双目视觉的测量噪声协方差矩阵R_k，其主要测量噪声来源于设备本身，根据步骤S3中的分析，噪声还与末端执行器坐标系相对于靶标点群的位姿相关。C-Track的测量数据符合高斯分布假设，进一步，考虑被测物体的不同速度的运动可能会导致相机在拍摄的时候产生不同程度的拖影，进而导致测量噪声，使用C-Track测量机床末端执行器在直线轨迹运动下的位姿，因为机床具有很高的运动精度，可以认为实际运动轨迹就是理论直线轨迹。通过理论轨迹来计算测量误差，对实际的机床末端执行器跟踪测量，统计各个自由度的标准差，确定双目视觉的测量噪声协方差矩阵：

其中，状态转移矩阵F、机器人的预测噪声协方差矩阵Q_k和双目视觉的测量噪声协方差矩阵R_k构成Kalman滤波器中的各项矩阵值。

步骤S5：对步骤S4中输出的末端执行器当前位姿数据，与预设理论位姿进行比较并计算位姿误差。

具体为，计算末端执行器位姿误差，包括直线轨迹运动和圆弧轨迹运动，对于直线轨迹运动的末端执行器位姿误差为：

ΔPR＝[P_n R_d]^T-[P_c R_c]^T，

P_n＝(k(x₂-x₁)+x₁ k(y₂-y₁)+y₁ k(z₂-z₁)+z₁)

其中，Pn为实际位置Pc到直线上垂足；

对于圆弧轨迹运动的末端执行器位姿误差为ΔP＝P_d-P_c，ΔR＝R_d-R_c。

步骤S6：将步骤S5中得到的末端执行器位姿误差作为输入，经视觉伺服控制器处理得到下一时刻的期望位姿，发送给机器人系统逆解成所需关节角度，根据计算得出的关节值控制末端执行器到达相应位置。

具体为，以机器人基坐标系x轴方向为例设计模糊PID控制器，模糊PID控制器原理图如图4。工业应用中常规PID公式如式(13)：

在实际的控制过程中，外部系统是以固定周期对机器人系统发送脉冲信号，需要对式(13)离散化。将误差的积分近似转化为误差的累加和，误差的微分近似转化为上一时刻到当前时刻误差的变化率，离散化后的PID公式如式(14):

将末端执行器误差及其变化率取绝对值，将|e|和|ec|作为模糊控制器的输入，PID控制器Kp、Ki、Kd的变化量作为输出，选取具有平滑输出的面积重心法解模糊；将{ZO,S,M,B}设置为输入变量|e|与|ec|、输出变量ΔKp、ΔKi、ΔKd的模糊子集，描述变量的大小程度，分别代表零、小、中、大，模糊论域取区间[0，3]。三角形函数在论域范围内分布均匀，且灵敏度较高，将其选作系统的隶属度函数；结合仿真及机器人操作实际经验，得出如表1所示的模糊控制规则表。

表1模糊控制规则表

为了将模糊推理机的与实际控制系统联系起来，需要计算量化因子和比例因子，完成模糊论域与物理论域的转换。|e|与|ec|的物理论域分别为[0，1]和[0，2]，模糊论域均为[0，3]，输出变量ΔKp、ΔKi、ΔKd的物理论域分别为[0，0.05]，[0，0.01]，[0，0.01]，模糊论域均为[0，3]，由式(15)与式(16)计算可得量化因子与比例因子。

基于试凑法，确定PID初值为：

K_p＝0.05 K_i＝0.01 K_d＝0.01 (17)

本发明的有益效果通过测试试验得以验证，描述如下。

空载试验验证：

搭建机器人轨迹跟踪试验平台，平台硬件部分主要包含KR500-3机器人、末端执行器、C-Track、PLC、PC工作站，软件部分主要包含集成控制软件、PLC控制软件、机器人控制软件。在机器人工作区域内规划一条长度为1000mm的期望直线轨迹，通过C-Track测量出无跟踪和基于视觉伺服的轨迹跟踪控制方法这两种情况下末端执行器轴尖处的位姿；在机器人工作区域内规划一条半径为500mm的期望半圆轨迹，通过C-Track测量出无跟踪和基于视觉伺服的轨迹跟踪控制方法这两种情况下末端执行器轴尖处的位姿。试验结束后，进行数据处理。

铣削试验验证：

搭建机器人轨迹跟踪试验平台，平台硬件部分主要包含KR500-3机器人、末端执行器、C-Track、PLC、PC工作站，软件部分主要包含集成控制软件、PLC控制软件、机器人控制软件。在150mm×250mm的钣金件上规划左右各5条直线轨迹，分别采用机器人自身控制系统以及本文所阐述的机器人视觉伺服控制方法，切削深度为0.02mm，速度为10mm/s，对钣金件进行直线轨迹铣削，进行对比试验；在150mm×250mm的钣金件上规划上下各6条圆弧轨迹，分别采用机器人自身控制系统以及本文所阐述的机器人视觉伺服控制方法，切削深度为0.05mm，速度为10mm/s，对钣金件进行圆弧轨迹顺时针铣削，进行对比试验。试验结束后，进行数据处理。

空载试验结果分析：

直线轨迹空载试验数据统计表如表2所示，从统计数据可以看出，本发明所述的视觉伺服控制模型能够极高的提高机器人的直线轨迹精度，受控制的直线轨迹的位置误差控制在0.05mm以内，姿态误差控制在0.02°以内，各项RMSE值也显著降低。

表2直线轨迹空载试验统计数据值

圆弧轨迹空载试验数据统计表如表3所示，从统计数据可以看出，本发明所述的视觉伺服控制模型能够极高的提高机器人的圆弧轨迹精度，受控制的圆弧轨迹的位置误差控制在0.07mm以内，姿态误差控制在0.02°以内，各项RMSE值也显著降低。

表3圆弧轨迹空载试验统计数据值

铣削试验结果分析：

直线轨迹铣削试验数据统计表如表4所示，从统计数据可以看出，无跟踪的最大直线误差达到了1.43mm，使用视觉伺服跟踪控制后的最大直线误差0.12mm，降低了92％；

表4直线铣削位置误差统计数据

直线编号	是否补偿	平均/mm	最大/mm	最小/mm
					1	否	1.07	1.11	1.02
2	否	1.28	1.32	1.21
					3	否	1.12	1.16	1.08
4	否	1.35	1.43	1.26
					5	否	1.11	1.16	1.05
6	是	0.03	0.11	0.01
					7	是	0.04	0.10	0.01
8	是	0.03	0.12	0.01
					9	是	0.03	0.09	0.01
10	是	0.03	0.12	0.00

圆弧轨迹铣削试验数据统计表如表5所示，其中无数据单元格表示该次试验中铣刀切深减小过大导致未正常切除材料。从统计数据可以看出，无跟踪的最大圆弧误差达1.55mm、最大拟合圆心位置误差达1.34mm、最大拟合圆半径误差达-0.26mm，使用视觉伺服跟踪控制后的最大圆弧误差达0.14mm、最大拟合圆心位置误差达0.10mm、最大拟合圆半径误差达0.05mm，分别降低了91％、93％和81％。

表5圆弧铣削位置误差统计数据

圆弧编号	是否补偿	平均/mm	最大/mm	最小/mm	拟合圆心位置/mm	拟合圆半径/mm
							1	否	0.61	1.09	0.20	1.20	-0.13
2	否	/	/	/	/	/
							3	否	0.96	1.55	0.35	1.34	0.00
4	否	0.82	1.10	0.23	0.97	-0.13
							5	否	0.54	0.98	0.20	0.73	-0.26
6	否	1.02	1.12	0.82	0.88	-0.21
							7	是	0.04	0.14	0.01	0.08	0.04
8	是	0.03	0.13	0.02	0.06	0.05
							9	是	0.03	0.10	0.01	0.08	-0.04
10	是	0.05	0.11	0.02	0.09	-0.04
							11	是	0.04	0.10	0.00	0.10	0.03
12	是	0.04	0.12	0.00	0.10	-0.03

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (6)

1.基于视觉引导的机器人轨迹跟踪控制方法，其特征在于，包括如下步骤：

步骤S1：建立视觉伺服控制系统，包括双目视觉测量设备、机器人、末端执行器、视觉伺服控制器；

步骤S2：建立不同坐标系，包括传感器坐标系F_S，机器人基坐标系F_B，末端执行器坐标系F_E，工件坐标系F_W，目标坐标系F_H，然后经坐标系变换建立双目视觉统一测量场；

步骤S3：利用双目视觉测量设备识别末端执行器上的靶标点群，获取末端执行器坐标系F_E相对于传感器坐标系F_S之间的位姿变换关系

并经过双目视觉统一测量场转换到机器人基坐标系；

步骤S4：利用Kalman滤波器对步骤S3中获取的末端执行器位姿进行平滑估计；

步骤S5：对步骤S4中输出的末端执行器当前位姿数据，与预设理论位姿进行比较并计算位姿误差；

步骤S6：将步骤S5中得到的末端执行器位姿误差作为输入，经视觉伺服控制器处理得到下一时刻的期望位姿，发送给机器人系统逆解成所需关节角度，根据计算得出的关节值控制末端执行器到达相应位置。

2.根据权利要求1所述的一种基于视觉引导的机器人轨迹跟踪控制方法，其特征在于：所述步骤S2中，将末端执行器的位姿转化到机器人基坐标系下的位姿：

其中，机器人基坐标系F_B与工件坐标系F_W在F_S中分别表示为旋转矩阵

与

目标坐标系F_H在工件坐标系F_W下的描述为

3.根据权利要求2所述的一种基于视觉引导的机器人轨迹跟踪控制方法，其特征在于：所述步骤S3中，根据末端执行器坐标系的原点在靶标点群几何中心处、末端执行器坐标系通过旋转矩阵Λ旋转一定角度、末端执行器坐标系通过平移向量q平移一定距离、末端执行器坐标系做旋转Λ和平移q复合运动这四种情况，计算末端执行器相对于靶标点群的不同位姿下的测量误差，选择测量误差最小的末端执行器坐标系相对于靶标点群的位姿，利用双目视觉测量设备识别末端执行器上的靶标点群，获取末端执行器坐标系F_E相对于双目视觉测量设备的测量坐标系F_S之间的位姿变换关系

并经过双目视觉测量场转换到机器人坐标系中。

4.根据权利要求2所述的一种基于视觉引导的机器人轨迹跟踪控制方法，其特征在于：所述步骤S4中，根据双目视觉测量设备的采样周期、动态测量状态下各自由度测量数据的标准差、机器人运动加速度标准差确定Kalman滤波器中的各项矩阵值；

根据双目视觉的采样周期t，确定机器人的状态转移矩阵F：

设机器人运动预测噪声协方差矩阵Q_k：

统计动态测量状态下机床末端各个自由度测量数据的标准差，确定双目视觉的测量噪声协方差矩阵：

其中，状态转移矩阵F、机器人的预测噪声协方差矩阵Q_k和双目视觉的测量噪声协方差矩阵R_k构成Kalman滤波器中的各项矩阵值。

5.根据权利要求4所述的一种基于视觉引导的机器人轨迹跟踪控制方法，其特征在于：所述步骤S5中，计算末端执行器位姿误差，包括直线轨迹运动和圆弧轨迹运动，对于直线轨迹运动的末端执行器位姿误差为：

ΔPR＝[P_n R_d]^T-[P_c R_c]^T，

P_n＝(k(x₂-x₁)+x₁ k(y₂-y₁)+y₁ k(z₂-z₁)+z₁)

其中，Pn为实际位置Pc到直线上垂足；

对于圆弧轨迹运动的末端执行器位姿误差为ΔP＝P_d-P_c，ΔR＝R_d-R_c。

6.根据权利要求5所述的一种基于视觉引导的机器人轨迹跟踪控制方法，其特征在于：所述步骤S6中，对机器人末端执行器的6个自由度分别基于模糊PID视觉伺服控制器进行调整控制。

Images