《具身智能入门教程》

---

1 欢迎来到物理世界

---

2 点亮感官认识世界

---

3 建立身体行动自如

---

4 构建思维开启决策

---

5 低阶实践控制小车

---

6 高光时刻具身微调

---

7 走出仿真面对挑战

---

---

上一节，我们用PID控制让小车跑出了直线。小车只有一个需要纠正的变量——横向偏差。但真实的机器人任务往往复杂得多：机械臂要抓取桌上的水杯，需要同时控制肩、肘、腕好几个关节，最终让夹爪的指尖精确对准目标物体的中心。

这一节，我们要把第二章学到的视觉感知，和第三章学到的关节控制，拧在一起。这项技术叫做视觉伺服——用视觉反馈来引导机器人的运动。你写的程序会让机械臂在仿真里自主完成一次完整的抓取：摄像头看到物体，计算偏差，关节运动修正偏差，夹爪对准，合拢抓起。

这是你第一次把感知和行动串成一个完整的闭环。

什么是视觉伺服？

视觉伺服这个词听起来高深，拆开就是两个字：看，然后走。

你看桌上的水杯，你的眼睛实时把水杯的位置报告给大脑，大脑计算手和杯子之间的差距，然后不断调整手臂的轨迹，直到手指碰到杯壁。你不需要事先量好杯子的坐标，不需要精确规划一条从起点到终点的轨迹。你只需要持续地看、持续地调整、持续地接近。

这就是视觉伺服的核心思想：用一个闭环反馈回路，把视觉感知和运动控制实时耦合在一起。 它不依赖精确的全局坐标，不依赖完美的运动学模型。只要偏差能被看到，系统就能自动修正。

视觉伺服的数学描述非常简洁。设相机看到的目标物体在图像中的位置为 $(u_{target}, v_{target})$，夹爪在图像中的当前位置为 $(u_{gripper}, v_{gripper})$。定义图像空间中的偏差：

$$ e(t) = \begin{bmatrix} u_{target} - u_{gripper} \ v_{target} - v_{gripper} \end{bmatrix} $$

控制目标是让 $e(t) \to 0$。当偏差趋零，夹爪就对准了物体，可以执行抓取。

环境准备：虚拟机械臂与虚拟相机

我们需要一个更完整的仿真环境。本节继续使用 MuJoCo，并加载一个带有视觉传感器的机械臂模型。为了让你专注于视觉伺服的逻辑而非场景搭建，我准备了一个预置好的仿真场景。

首先，确保你已安装所需依赖：

pip install mujoco opencv-python numpy

我们使用一个经典的6轴协作机械臂模型，末端装有一个二指夹爪。场景中会放置一个彩色方块作为目标物体。机械臂的手腕处安装了一个虚拟RGB相机，它会持续输出 $(H \times W \times 3)$ 的图像帧。

以下是我们为本节准备的仿真场景代码，保存为 visual_servo_scene.py：

import mujoco
import numpy as np
import cv2

class VisualServoEnv:
    """视觉伺服仿真环境：机械臂 + 夹爪 + 目标物体 + 腕部相机"""

    def __init__(self, xml_path='visual_servo.xml'):
        """
        初始化仿真环境。
        你需要一个包含机械臂、目标物体和相机传感器的MuJoCo模型文件。
        下文会提供模型的XML结构说明。
        """
        self.model = mujoco.MjModel.from_xml_path(xml_path)
        self.data = mujoco.MjData(self.model)

        # 获取关键元素的ID
        self.target_body_id = mujoco.mj_name2id(
            self.model, mujoco.mjtObj.mjOBJ_BODY, 'target'
        )
        self.camera_id = 0  # 假设第一个相机是腕部相机

        # 仿真参数
        self.timestep = self.model.opt.timestep

        # 图像参数
        self.img_height = 480
        self.img_width = 640

    def get_camera_image(self):
        """从腕部相机获取RGB图像"""
        # 更新场景渲染上下文
        mujoco.mj_forward(self.model, self.data)

        # 创建渲染器并渲染相机视图
        renderer = mujoco.Renderer(self.model, self.img_height, self.img_width)
        renderer.update_scene(self.data, camera=self.camera_id)
        pixels = renderer.render()
        renderer.close()

        # pixels的形状是 (H, W, 3)，RGB格式
        return pixels

    def get_target_pixel_position(self, image):
        """
        通过颜色阈值检测目标物体在图像中的位置。
        假设目标物体是纯红色方块。
        返回目标中心的像素坐标 (u, v)，若未找到则返回 None。
        """
        # 转换到HSV空间以便颜色分割
        hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)

        # 红色的HSV范围（红色在HSV色环两端，需要两个范围）
        lower_red1 = np.array([0, 100, 100])
        upper_red1 = np.array([10, 255, 255])
        lower_red2 = np.array([160, 100, 100])
        upper_red2 = np.array([180, 255, 255])

        mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
        mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
        mask = mask1 | mask2

        # 找到红色区域的质心
        moments = cv2.moments(mask)
        if moments['m00'] > 0:
            u = int(moments['m10'] / moments['m00'])
            v = int(moments['m01'] / moments['m00'])
            return (u, v)
        else:
            return None

    def get_gripper_pixel_position(self, image):
        """
        通过颜色阈值检测夹爪指尖在图像中的位置。
        假设夹爪指尖是亮绿色标记。
        返回夹爪中心的像素坐标 (u, v)，若未找到则返回 None。
        """
        hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)

        # 绿色的HSV范围
        lower_green = np.array([35, 100, 100])
        upper_green = np.array([85, 255, 255])
        mask = cv2.inRange(hsv, lower_green, upper_green)

        moments = cv2.moments(mask)
        if moments['m00'] > 0:
            u = int(moments['m10'] / moments['m00'])
            v = int(moments['m01'] / moments['m00'])
            return (u, v)
        else:
            return None

    def set_joint_velocities(self, velocities):
        """
        设置各关节的目标速度。
        velocities: 长度为 nv 的数组，单位是弧度/秒
        """
        # 对于速度控制模式，将速度指令写入 data.ctrl
        self.data.ctrl[:] = velocities

    def step(self):
        """步进仿真一步"""
        mujoco.mj_step(self.model, self.data)

    def close_gripper(self, force=100):
        """闭合夹爪"""
        # 找到夹爪驱动器的索引（取决于模型定义）
        for i in range(self.model.nu):
            if 'gripper' in self.model.actuator(i).name.lower():
                self.data.ctrl[i] = force

    def open_gripper(self):
        """打开夹爪"""
        for i in range(self.model.nu):
            if 'gripper' in self.model.actuator(i).name.lower():
                self.data.ctrl[i] = 0

视觉伺服循环：看、算、动、再看

视觉伺服的核心是一个高频循环，每秒运行几十次。每一次循环包含四步：

1. 看：腕部相机拍摄当前画面
2. 算：在画面中找到目标物体的位置，计算偏差
3. 动：根据偏差调整关节运动方向和速度
4. 再看：偏差变小了吗？如果没有，继续调整

我们使用比例控制（P控制）来实现视觉伺服。偏差越大，关节运动速度越快；偏差趋零，运动停止。这种直观的策略在实践中效果出奇地好。

现在，让我们写出主控制循环：

import numpy as np
import cv2
from visual_servo_scene import VisualServoEnv

# 初始化仿真环境
env = VisualServoEnv('visual_servo.xml')

# 视觉伺服的增益参数
Kp = 0.5  # 比例增益，决定响应速度

# 图像中心的坐标（我们希望目标出现在相机视野正中）
center_u = env.img_width // 2   # 320
center_v = env.img_height // 2  # 240

# 对齐阈值（像素）：偏差小于此值时认为已对准
alignment_threshold = 10

# 控制循环
print("视觉伺服抓取开始...")
aligned = False
grasped = False

for step_count in range(1000):
    # 第1步：获取相机图像
    image = env.get_camera_image()

    # 第2步：检测目标物体和夹爪的像素位置
    target_pos = env.get_target_pixel_position(image)
    gripper_pos = env.get_gripper_pixel_position(image)

    if target_pos is not None and gripper_pos is not None:
        # 第3步：计算图像空间中的偏差
        error_u = target_pos[0] - gripper_pos[0]
        error_v = target_pos[1] - gripper_pos[1]
        distance = np.sqrt(error_u**2 + error_v**2)

        # 第4步：判断是否已对准
        if not aligned:
            if distance < alignment_threshold:
                aligned = True
                print(f"已对准目标！偏差像素距离: {distance:.1f}")
            else:
                # 第5步：比例控制，将像素偏差映射为关节速度
                # 水平偏差 -> 机械臂第1关节（基座旋转）的速度
                vel_base = Kp * error_u * 0.01
                # 垂直偏差 -> 机械臂第2关节（肩部俯仰）的速度
                vel_shoulder = -Kp * error_v * 0.01

                # 设置关节速度（简化：只控制前两个关节）
                velocities = np.zeros(env.model.nv)
                velocities[0] = vel_base      # 基座旋转
                velocities[1] = vel_shoulder  # 肩部俯仰
                env.set_joint_velocities(velocities)

        # 第6步：已对准，执行抓取
        if aligned and not grasped:
            env.close_gripper(force=150)
            grasped = True
            print("夹爪闭合，抓取完成！")

        # 定期打印状态
        if step_count % 50 == 0:
            status = "追踪中" if not aligned else ("抓取完成" if grasped else "已对准")
            print(f"步数 {step_count} | 偏差: ({error_u:.1f}, {error_v:.1f}) | 距离: {distance:.1f} | 状态: {status}")

    else:
        # 目标或夹爪不可见
        if target_pos is None:
            if step_count % 100 == 0:
                print(f"步数 {step_count} | 警告: 未检测到目标物体")
        if gripper_pos is None:
            if step_count % 100 == 0:
                print(f"步数 {step_count} | 警告: 未检测到夹爪标记")
        # 停止运动，等待物体出现在视野中
        env.set_joint_velocities(np.zeros(env.model.nv))

    # 步进仿真
    env.step()

    # 如果已抓取完成，保持几帧后退出
    if grasped and step_count > 50:
        # 保持夹爪闭合，等待仿真稳定
        env.close_gripper(force=150)
        if step_count > 200:
            print("视觉伺服抓取演示完成。")
            break

print(f"总共执行步数: {step_count}")

逐行解读：这个循环里发生了什么？

上面的主循环是整个视觉伺服的核心。让我们一步步拆解它的逻辑。

偏差计算是整个系统的眼睛。目标物体的像素坐标 $(u_{target}, v_{target})$ 通过红色阈值检测获得，夹爪指尖的像素坐标 $(u_{gripper}, v_{gripper})$ 通过绿色阈值检测获得。两者相减，得到图像空间中的二维偏差向量。这个向量的长度 $d = \sqrt{(u_{target} - u_{gripper})^2 + (v_{target} - v_{gripper})^2}$ 就是我们想要最小化的量。

控制映射是系统的决策部分。这里我们做了最简单的线性映射——水平偏差驱动基座关节旋转，垂直偏差驱动肩部关节俯仰。这种映射基于一个朴素的直觉：如果目标在夹爪的左边，基座就该往左转；如果目标在上方，肩部就该向上抬。它忽略了运动学的非线性，忽略了远心点，忽略了雅可比矩阵的奇异性。但令人惊讶的是，它竟然能工作。 这正是视觉伺服的魅力所在——闭环反馈对模型误差有很强的容忍度。

对齐判断用一个简单的阈值来检测。当目标中心与夹爪中心的像素距离小于10个像素时，我们认为夹爪已经对准了物体，可以执行抓取。这个阈值需要根据实际物体大小来调整——抓硬币和抓篮球对精度的要求显然不同。

夹爪控制在视觉对准后触发。闭合指令发送后，夹爪两个手指向中间合拢，直到碰到物体表面。在仿真中，接触力由物理引擎自动计算，物体会被稳定地夹持在指尖之间。

从P到PI：消除稳态误差的进阶

上面我们只用了比例控制。就像小车跑直线一样，纯P控制可能存在稳态误差——夹爪最终停在目标附近，但差一点点没对准。

解决方案和我们之前学的一样：加上积分项。在偏差计算之后，累积历史偏差，并将其加入控制输出。修改主循环中的控制计算部分：

# 在循环外初始化积分变量
integral_u = 0.0
integral_v = 0.0
Ki = 0.001  # 积分增益

# 在循环内，P控制之后加上积分项
integral_u += error_u
integral_v += error_v

# 积分抗饱和：限制积分值范围，防止过度累积
integral_u = np.clip(integral_u, -100, 100)
integral_v = np.clip(integral_v, -100, 100)

vel_base = Kp * error_u * 0.01 + Ki * integral_u
vel_shoulder = -Kp * error_v * 0.01 - Ki * integral_v

积分项会累积那些微小但持续存在的偏差，最终产生足够的纠正力来消除静差。抗饱和是一个重要的工程细节——如果目标长时间不可见，积分值会无限增长，一旦目标重新出现，巨大的积分项会导致机械臂猛烈摆动。用 np.clip 给积分值设置上下限，是工业实践中通行的做法。

视觉特征的选择：为什么用颜色阈值？

你可能会问：我们用颜色阈值来检测目标和夹爪，这种方法太简单了，现实世界中充满复杂纹理和变化光照，红色阈值根本不管用。

你说得完全正确。在真实应用中，视觉伺服通常使用更鲁棒的视觉特征：AprilTag二维码标记、基于深度学习的物体位姿估计、SIFT特征点匹配等。我们本章用颜色阈值，是为了让视觉伺服的核心逻辑不被复杂的视觉处理淹没。原理是相通的：从图像中提取特征点，在特征空间中定义偏差，然后通过控制回路将偏差归零。

如果你想挑战自己，可以试着用第二章学过的YOLO目标检测来替代颜色阈值。让YOLO输出目标物体的边界框中心坐标，作为视觉伺服的输入。整个控制回路完全不变，只替换视觉前端。这就是模块化设计的优雅之处。

小结：感知与行动的第一次合体

这一节，你完成了一个具身智能系统的完整闭环。

摄像头看到了红色目标，算法计算出了目标与夹爪的偏差，控制器把偏差映射为关节速度指令，机械臂运动，偏差缩小，最终夹爪对准并抓住了物体。感知、决策、行动——三根柱子第一次在你的代码里同时运转。

更重要的是，你体会到了视觉伺服的核心理念：不需要完美的模型，闭环反馈会自然地修正一切。 这个理念贯穿了具身智能的各个层面。从简单的P控制到复杂的强化学习策略，它们共享同一个灵魂——与物理世界持续交互，在试错中不断校准。

在下一节，我们将走向具身智能最前沿的领域。你会第一次接触到具身大模型——一种正在从根本上改变机器人学习范式的技术。我们将调用一个开源VLA（视觉-语言-动作）模型，让它根据自然语言指令，自动完成复杂的操作任务。

---

---