从仿真到实战：手把手教你用Matlab+Robotics Toolbox搭建视觉伺服控制闭环

从仿真到实战：手把手教你用Matlab+Robotics Toolbox搭建视觉伺服控制闭环

视觉伺服控制是机器人领域的重要研究方向，它通过实时处理视觉反馈信息来调整机器人运动，实现精确的目标跟踪或定位。不同于传统的开环控制，视觉伺服将视觉系统作为闭环反馈的一部分，显著提升了机器人在复杂环境中的适应能力。本文将带你从零开始，使用Matlab的Robotics Toolbox构建一个完整的视觉伺服控制系统，涵盖从基础理论到实际仿真的全流程。

对于工程师和研究者而言，视觉伺服系统的开发往往面临理论与实践脱节的挑战。Robotics Toolbox提供了一套完整的机器人建模、仿真和控制工具链，能够大大降低开发门槛。我们将重点探讨如何将理论算法转化为可运行的代码，并分析不同控制策略的优劣。

1. 环境准备与基础配置

在开始之前，确保你的Matlab安装了Robotics Toolbox和Computer Vision Toolbox。这两个工具箱为我们提供了必要的函数和类来简化开发流程。可以通过以下命令检查安装情况：

ver robotics vision

如果缺少相关工具箱，可以通过Matlab的Add-Ons界面进行安装。建议使用Matlab R2020b或更高版本，以获得最佳的兼容性和功能支持。

接下来，我们需要初始化工作环境。创建一个新的脚本文件，并添加以下基础配置代码：

clear all; clc; close all; addpath(genpath(pwd)); % 添加当前目录及其子目录到路径 rng(0); % 固定随机种子，确保结果可复现

提示：在实际项目中，建议使用独立的文件夹管理不同模块的代码，例如将机器人模型、视觉处理和控制器分别放在不同的子目录中。

2. 机器人模型构建

我们将以UR5机械臂为例，展示如何使用Robotics Toolbox创建机器人模型。UR5是一款常见的6自由度工业机械臂，其参数定义如下：

% UR5 DH参数表 L1 = Link('d', 0.089159, 'a', 0, 'alpha', pi/2); L2 = Link('d', 0, 'a', -0.425, 'alpha', 0); L3 = Link('d', 0, 'a', -0.39225, 'alpha', 0); L4 = Link('d', 0.10915, 'a', 0, 'alpha', pi/2); L5 = Link('d', 0.09465, 'a', 0, 'alpha', -pi/2); L6 = Link('d', 0.0823, 'a', 0, 'alpha', 0); ur5 = SerialLink([L1 L2 L3 L4 L5 L6], 'name', 'UR5'); ur5.teach(); % 交互式查看机器人模型

创建模型后，我们可以通过以下方法验证其正确性：

1. 检查关节限位和运动范围
2. 验证末端执行器的可达空间
3. 测试正向和逆向运动学计算

下表比较了UR5模型的关键参数与实际机器人的规格：

3. 视觉系统集成

视觉伺服的核心在于将视觉信息转化为控制信号。我们首先需要建立相机模型，这里采用经典的针孔相机模型。Robotics Toolbox提供了CentralCamera类来简化这一过程：

% 创建相机模型 cam = CentralCamera('focal', 0.015, 'pixel', 10e-6, ... 'resolution', [1024 1024], 'centre', [512 512], ... 'pose', transl(0.5, 0.1, 0.3)*trotz(pi)); % 可视化相机位置 figure; trplot(eye(4), 'frame', 'W', 'length', 0.2); % 世界坐标系 hold on; trplot(cam.T, 'frame', 'C', 'color', 'r'); % 相机坐标系 axis([-1 1 -1 1 0 1.5]); grid on; xlabel('X'); ylabel('Y'); zlabel('Z');

图像特征提取是视觉伺服的关键步骤。常见的特征包括点、线、区域等。以下代码演示了如何从模拟图像中提取特征点：

% 生成目标场景 P = mkgrid(2, 0.5, 'T', transl(0.8, -0.2, 0.6)); % 投影到图像平面 p = cam.plot(P, 'Tcam', cam.T); % 特征提取（模拟） features = p + 0.5*randn(size(p)); % 添加噪声模拟实际检测

4. 控制器设计与实现

视觉伺服主要分为基于位置的视觉伺服(PBVS)和基于图像的视觉伺服(IBVS)两种策略。我们将分别实现这两种控制器，并比较它们的性能。

4.1 基于图像的视觉伺服(IBVS)

IBVS直接利用图像特征误差生成控制命令，其核心是图像雅可比矩阵（交互矩阵）。以下是一个简单的IBVS控制器实现：

function [uv, hist] = ibvs_controller(cam, P, uv_star, lambda) % 初始化 uv = cam.plot(P); hist = struct('uv', [], 'vel', [], 'cam', []); % IBVS主循环 for k = 1:100 % 计算图像误差 e = uv - uv_star; % 计算交互矩阵 J = cam.visjac_p(uv, 1); % 计算速度命令 v = -lambda * pinv(J) * e(:); % 更新相机位姿 Td = trnorm(delta2tr(v*0.1) * cam.T); cam.T = Td; % 记录数据 hist.uv = [hist.uv; uv(:)']; hist.vel = [hist.vel; v']; hist.cam = [hist.cam; transl(Td)']; % 更新特征点 uv = cam.plot(P); % 检查收敛 if norm(e) < 0.01 break; end end end

4.2 基于位置的视觉伺服(PBVS)

PBVS首先从图像特征估计目标位姿，然后在三维空间中进行控制。以下是PBVS的实现示例：

function [T, hist] = pbvs_controller(cam, P, T_star, lambda) % 初始化 T = cam.T; hist = struct('T', [], 'vel', []); % PBVS主循环 for k = 1:100 % 估计当前位姿（这里假设已知） T_est = cam.T; % 计算位姿误差 e = tr2delta(T_est, T_star); % 计算速度命令 v = -lambda * e; % 更新相机位姿 Td = trnorm(delta2tr(v*0.1) * cam.T); cam.T = Td; % 记录数据 hist.T = [hist.T; transl(Td)']; hist.vel = [hist.vel; v']; % 检查收敛 if norm(e) < 0.01 break; end end end

4.3 性能比较与分析

下表对比了IBVS和PBVS的主要特性：

在实际项目中，选择哪种策略取决于具体应用场景和系统要求。通常，IBVS更适合对精度要求不高但需要鲁棒性的场合，而PBVS则适用于需要精确控制末端位姿的任务。

5. 系统集成与仿真验证

将上述模块整合为一个完整的视觉伺服系统，我们可以进行全面的仿真验证。以下代码展示了如何设置仿真场景并运行控制器：

% 初始化场景 setup_visual_servoing; % 设置目标位置 P = mkgrid(2, 0.5, 'T', transl(0.8, -0.2, 0.6)); % 设置期望图像特征 uv_star = bsxfun(@plus, cam.pp', [100 -100; 100 100; -100 100; -100 -100]'); % 运行IBVS控制器 cam_ibvs = cam.copy(); [uv_ibvs, hist_ibvs] = ibvs_controller(cam_ibvs, P, uv_star, 0.1); % 运行PBVS控制器 cam_pbvs = cam.copy(); T_star = transl(0.5, 0.1, 0.3)*trotz(pi); [T_pbvs, hist_pbvs] = pbvs_controller(cam_pbvs, P, T_star, 0.1); % 可视化结果 plot_visual_servoing_results(hist_ibvs, hist_pbvs, P, uv_star);

仿真结果通常包括以下几个方面的分析：

1. 特征点轨迹：观察图像平面中特征点的运动路径
2. 相机运动：分析相机在三维空间中的运动轨迹
3. 误差收敛：检查图像误差或位姿误差随时间的变化
4. 控制命令：评估生成的速度命令是否平滑合理

在实际应用中，还需要考虑以下优化方向：

• 加入滤波器平滑视觉测量
• 实现更鲁棒的特征提取和匹配
• 考虑机器人动力学约束
• 处理遮挡和特征丢失的情况

6. 实际应用中的挑战与解决方案

将视觉伺服系统从仿真迁移到实际机器人平台时，会遇到各种挑战。以下是一些常见问题及其解决方案：

相机-机械臂标定误差：

• 问题：手眼标定不准确导致系统性能下降
• 解决方案：实现在线标定算法，定期更新变换矩阵

% 手眼标定示例 H_cam2tool = eye(4); % 需要实际标定

视觉处理延迟：

• 问题：图像处理耗时导致控制环路延迟
• 解决方案：使用预测滤波器补偿延迟

% 简单的预测滤波器 alpha = 0.5; % 滤波系数 filtered_uv = alpha*current_uv + (1-alpha)*previous_uv;

特征跟踪稳定性：

• 问题：动态环境中特征容易丢失
• 解决方案：结合多种特征类型提高鲁棒性

机器人动力学限制：

• 问题：机械臂无法瞬时响应速度命令
• 解决方案：在控制器中加入动力学模型

% 考虑关节速度限制 max_joint_vel = [2.0 2.0 2.0 2.0 2.0 2.0]; % rad/s qdot = min(max(qdot, -max_joint_vel), max_joint_vel);

在实验室环境中，我们使用UR5机械臂配合Eye-in-Hand相机配置测试了上述系统。实际运行中发现，IBVS在目标初始位置偏离较大时容易失败，而PBVS对相机标定误差更为敏感。最终采用了两阶段策略：先用IBVS进行粗定位，再用PBVS进行精调。