本文介绍了一个基于 ESP32-P4 高性能 MCU 的可独立控制机械臂项目，涵盖完整的运动学计算、视觉检测与远程控制方案，展示了 ESP32-P4 在工业级机械臂应用中的巨大潜力。
随着近年来 AI 技术的快速发展，机器人领域正逐步迈入具身智能 (Embodied Intelligence) 时代，机器人开始具备环境感知与自主学习能力。ESP32-P4 凭借强大的算力与丰富的外设接口，为构建智能机械臂控制系统提供了理想的平台。
本文所介绍的机械臂方案具备以下核心优势：
- 板载运动学计算 ：通过基于 D-H 参数的迭代算法实现正向与逆向运动学，无需依赖外部计算平台
- 板载视觉能力 ：支持 USB 摄像头与颜色检测模型，结合视觉标定矩阵实现精准抓取；同时借助 esp-dl，可运行 YOLO11n、esp-detection 等轻量模型，实现自定义目标识别
- 远程控制能力：通过一套同构的小型机械臂采集关节数据，并基于 ESP-NOW（ESP32-P4-Function-EV-Board 上的 ESP32-C6）无线发送，实现远程操控
本文将详细讲解主从机械臂的机械装配、电机控制、视觉标定与系统集成全过程，覆盖机器人与视觉应用的基础原理。同时，依托 ESP32-P4 的高性能，本方案还可进一步扩展至多种应用场景。
安全提示
为防止机械臂在运行过程中因意外动作造成人身伤害或财产损失，请在正式运行前仔细核对所有运动参数，并确保紧急停止按钮始终处于可触达状态，以便在紧急情况下立即断电。
主机械臂 (Leader Robotic Arm，TRLC DK1)
主机械臂仅用于远程控制从机械臂 ，本身不涉及运动学计算。从机械臂可在板载运动学与视觉能力支持下独立运行。如果你不需要远程控制功能，可直接从 从机械臂 (Ragtime Panthera)章节开始阅读。
主机械臂基于 trlc-dk1 项目中的 leader 部分。需要注意的是，目前 leader 结构已更新至 v0.2.0，而本文使用的是初始版本结构 。可在此处下载初版 3D 文件。
完整的主机械臂项目代码位于此处，主要实现 XL330_M077 总线舵机的位置控制与力矩切换。
XL330_M077 总线舵机配置
在正式装配前，需要使用 DYNAMIXEL Protocol 2.0 对 7 个 XL330_M077 总线舵机进行参数配置。请提前准备总线舵机驱动板（用于将标准串口信号转换为总线舵机 DATA 信号），并参考下图或官方调试电路进行连接。
使用上述电路时，请将 USB-TTL 的 TX 接驱动板 TX、RX 接 RX，注意不要接反。
连接完成后，打开 DYNAMIXEL Protocol 2.0 工具，在菜单栏点击 Scan 扫描总线上的舵机。
为保证与主机械臂工程参数一致，请按如下要求配置舵机：
- ID：1 ～ 7
- 波特率：115200
- 工作模式：位置控制
- 初始角度：180° (2045)
选择 180° 作为初始角度的原因是：若以 0° 作为起始点，上电时有可能被识别为 360°，从而影响后续角度读取与控制。
结构装配
打印完成所有结构件后，请在所有舵机均处于 180° 初始角度 的状态下进行装配。所需螺丝均包含在XL330_M077 舵机包装盒中。
ESP32-C3 驱动板安装与调试
ESP32-C3 驱动板将 ESP32-C3 与总线舵机电路集成在同一模块中，支持外部 5 V / ≥1 A 电源同时为舵机与ESP32-C3 供电。通过板载 Micro-USB 接口可直接烧录和调试固件。点击此处下载用于 PCB 打样的 Gerber 文件。
在开始前，请安装并配置 ESP-IDF v5.5 环境。
随后克隆 esp-iot-solution，并进入 leader 工程目录进行编译与烧录：
git clone git@github.com:espressif/esp-iot-solution.git
cd esp-iot-solution/examples/robot/ragtime_panthera/leader
idf.py set-target esp32c3
idf.py -p /dev/ttyACM0 build flash monitor如需进行更多配置，请运行 `idf.py menuconfig`，并进入 (Top) → Example Configuration：
硬件配置 (Hardware Configuration) 包含 XL330_M077 总线舵机的基础参数设置，包括波特率、TX/RX 引脚、初始位置以及角度容差：
硬件配置
关键说明：
- XL330_M077 配置说明：如需修改 UART 设置或通信引脚，请调整 XL330_M077 communication speed、XL330_M077 RXD pin number 以及 XL330_M077 init position；
- 初始角度定义：程序中零位 = 当前角度 − 配置的初始角度；
- 角度容差：上电后舵机会多次回到初始位置，当目标角度与当前角度差值小于该阈值时，认为到位。
ESP-NOW 配置
- Channel：默认 1
- Primary Master Key：默认 bot12345678901234
- Slave MAC Address：需修改为从机械臂 ESP32-C6 的 MAC 地址
ESP-NOW 是乐鑫定义的低功耗、低时延点对点无线通信协议，无需路由器，广泛应用于智能家居、遥控、传感器等场景。
从机械臂 (Follower Robotic Arm，Ragtime Panthera)
从机械臂基于 Ragtime_Panthera 项目，采用 DM 系列关节电机驱动，并提供完整的 ROS 与 Python 支持。同时，其结构与主机械臂完全一致，可直接进行遥操作控制。
为避免因原项目更新导致结构差异，请使用本文所基于的 fork 版本。
运动学建模与验证
D-H 参数建模
由于 MCU 平台不支持 URDF 与成熟运动学库，本文在 ESP32-P4 上使用 Denavit--Hartenberg (D-H) 参数描述机械臂结构，并采用迭代法实现逆运动学求解。
在移植前，推荐使用 robotics-toolbox-python 对 D-H 参数进行预验证：
pip3 install roboticstoolbox-python随后可在仿真环境中查看机械臂模型，并测试正向与逆向运动学。
import numpy as np
from roboticstoolbox import DHRobot, RevoluteDH
robot = DHRobot(
[
RevoluteDH(a=0.0, d=0.1005, alpha=-np.pi / 2, offset=0.0),
RevoluteDH(a=0.18, d=0.0, alpha=0.0, offset=np.deg2rad(180)),
RevoluteDH(a=0.188809, d=0.0, alpha=0.0, offset=np.deg2rad(162.429)),
RevoluteDH(a=0.08, d=0.0, alpha=-np.pi / 2, offset=np.deg2rad(17.5715)),
RevoluteDH(a=0.0, d=0.0, alpha=np.pi / 2, offset=np.deg2rad(90)),
RevoluteDH(a=0.0, d=0.184, alpha=np.pi / 2, offset=np.deg2rad(-90)),
],
name="Ragtime_Panthera"
)
robot.teach(np.array([0, 0, 0, 0, 0, 0]))
正向运动学 (Forward Kinematics)
正向运动学是指：根据输入的各关节角度，计算机械臂末端执行器的位置与姿态 。
可使用以下代码对正向运动学进行测试：
import matplotlib.pyplot as plt
import numpy as np
from roboticstoolbox import DHRobot, RevoluteDH
robot = DHRobot(
[
RevoluteDH(a=0.0, d=0.1005, alpha=-np.pi / 2, offset=0.0),
RevoluteDH(a=0.18, d=0.0, alpha=0.0, offset=np.deg2rad(180)),
RevoluteDH(a=0.188809, d=0.0, alpha=0.0, offset=np.deg2rad(162.429)),
RevoluteDH(a=0.08, d=0.0, alpha=-np.pi / 2, offset=np.deg2rad(17.5715)),
RevoluteDH(a=0.0, d=0.0, alpha=np.pi / 2, offset=np.deg2rad(90)),
RevoluteDH(a=0.0, d=0.184, alpha=np.pi / 2, offset=np.deg2rad(-90)),
],
name="Ragtime_Panthera"
)
state = np.deg2rad([0, 30, -36, 65, 0, 0])
T1 = robot.fkine(state)
print("T1:\n{}".format(T1))
robot.plot(state, jointaxes=True, eeframe=True, block=True)
plt.show()
运行结果如下：
T1:
0.8572 0.515 0 0.1531
0 0 1 0
0.515 -0.8572 0 0.03971
0 0 0 1 通过调用 fkine 函数，可以快速计算出在当前关节角度下，末端执行器对应的空间位姿（位置与方向）。
逆向运动学（Inverse Kinematics）
逆向运动学是指：根据给定的末端执行器位置与姿态，反求每个关节所需的旋转角度 。
可使用以下代码对逆向运动学进行测试：
import matplotlib.pyplot as plt
import numpy as np
from roboticstoolbox import DHRobot, RevoluteDH
robot = DHRobot(
[
RevoluteDH(a=0.0, d=0.1005, alpha=-np.pi / 2, offset=0.0),
RevoluteDH(a=0.18, d=0.0, alpha=0.0, offset=np.deg2rad(180)),
RevoluteDH(a=0.188809, d=0.0, alpha=0.0, offset=np.deg2rad(162.429)),
RevoluteDH(a=0.08, d=0.0, alpha=-np.pi / 2, offset=np.deg2rad(17.5715)),
RevoluteDH(a=0.0, d=0.0, alpha=np.pi / 2, offset=np.deg2rad(90)),
RevoluteDH(a=0.0, d=0.184, alpha=np.pi / 2, offset=np.deg2rad(-90)),
],
name="Ragtime_Panthera"
)
state = np.deg2rad([0, 30, -36, 65, 0, 0])
T1 = robot.fkine(state)
print("T1:\n{}".format(T1))
T2 = np.array(T1)
T2[0, 3] = T2[0, 3] + 0.1
print("T2:\n{}".format(T2))
sol = robot.ikine_LM(T2, q0=state, ilimit=100, mask=[1, 1, 1, 1, 1, 0], joint_limits=True)
print(sol)
T3 = robot.fkine(sol.q)
print("T3:\n{}".format(T3))
robot.plot(sol.q, jointaxes=True, eeframe=True, block=True)
plt.show()
T1:
0.8572 0.515 0 0.1531
0 0 1 0
0.515 -0.8572 0 0.03971
0 0 0 1
T2:
[[ 8.57171795e-01 5.15030595e-01 1.04973270e-16 2.53139272e-01]
[-1.54001208e-16 5.24866348e-17 1.00000000e+00 1.49891524e-17]
[ 5.15030595e-01 -8.57171795e-01 1.24305397e-16 3.97085648e-02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
IKSolution: q=[0, 1.161, -0.8568, 0.725, 0, 0], success=True, iterations=4, searches=1, residual=4.59e-10
T3:
0.8572 0.515 0 0.2531
0 0 1 0
0.515 -0.8572 0 0.03971
0 0 0 1 在该示例中，我们以初始位姿为基础，尝试将末端执行器沿 X 轴正方向移动 0.1 m，并在当前位姿附近求解对应的关节角度 sol.q。随后，通过再次执行正向运动学对结果进行验证。
输出结果如下：
T1:
0.8572 0.515 0 0.1531
0 0 1 0
0.515 -0.8572 0 0.03971
0 0 0 1
T2:
[[ 8.57171795e-01 5.15030595e-01 1.04973270e-16 2.53139272e-01]
[-1.54001208e-16 5.24866348e-17 1.00000000e+00 1.49891524e-17]
[ 5.15030595e-01 -8.57171795e-01 1.24305397e-16 3.97085648e-02]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
IKSolution: q=[0, 1.161, -0.8568, 0.725, 0, 0], success=True, iterations=4, searches=1, residual=4.59e-10
T3:
0.8572 0.515 0 0.2531
0 0 1 0
0.515 -0.8572 0 0.03971
0 0 0 1从测试结果可以看出，逆运动学求解是有效的，且由逆解计算得到的正向运动学结果与期望的末端位姿完全一致，验证了运动学模型的正确性。
工作空间验证
由于当前项目不支持姿态估计，因此在固定末端姿态的前提下，通过遍历末端位移范围验证逆运动学是否可解，从而确定机械臂安全工作空间。
import numpy as np
from roboticstoolbox import DHRobot, RevoluteDH
robot = DHRobot(
[
RevoluteDH(a=0.0, d=0.1005, alpha=-np.pi / 2, offset=0.0),
RevoluteDH(a=0.18, d=0.0, alpha=0.0, offset=np.deg2rad(180)),
RevoluteDH(a=0.188809, d=0.0, alpha=0.0, offset=np.deg2rad(162.429)),
RevoluteDH(a=0.08, d=0.0, alpha=-np.pi / 2, offset=np.deg2rad(17.5715)),
RevoluteDH(a=0.0, d=0.0, alpha=np.pi / 2, offset=np.deg2rad(90)),
RevoluteDH(a=0.0, d=0.184, alpha=np.pi / 2, offset=np.deg2rad(-90)),
],
name="Ragtime_Panthera"
)
state = np.deg2rad([0, 30, -36, 65, 0, 0])
T1 = robot.fkine(state)
print("T1:\n{}".format(T1))
x_range = np.linspace(0, 23, 30)
y_range = np.linspace(-15, 15, 30)
for dx in x_range:
for dy in y_range:
T_test = np.array(T1).copy()
T_test[0, 3] += dx / 100
T_test[1, 3] += dy / 100
sol = robot.ikine_LM(T_test, q0=state)
if sol.success:
pass
else:
print(f"Failed in dx: {dx:.4f} cm, dy: {dy:.4f} cm")
print("Test Done")
测试结果表明，在 23 cm × 30 cm 区域内均可成功求解逆运动学。
ESP32-P4 板载运动学测试
为了使 ESP32-P4 能够独立控制机械臂，板载运动学能力是必不可少的。受 Matrix_and_Robotics_on_STM32 项目的启发，我们尝试将其移植并部署到 ESP32 平台上。运动学模块可以作为一个独立的组件引入到单独的工程中进行测试。需要注意的是，该项目目前仅移植了运动学 (Kinematics) 部分，尚不支持动力学 (Dynamics)。
以逆运动学 (Inverse Kinematics) 为例，在完成 ESP-IDF v5.5 环境搭建后，可在examples/robot/ragtime_panthera/follower 工程中进行测试。该工程已经将 kinematic 作为组件加入：
#include <stdio.h>
#include <iostream>
#include "kinematic.h"
extern "C" void app_main(void)
{
Kinematic kinematic;
Joint j1 = Joint(0.0, DEG2RAD(30.0), DEG2RAD(-36.0), DEG2RAD(65.0), 0.0, 0.0);
TransformMatrix t1;
kinematic.solve_forward_kinematics(j1, t1);
std::cout << "t1: " << std::endl;
t1.print();
TransformMatrix t2 = t1;
t2(0, 3) += 0.1f;
std::cout << "t2: " << std::endl;
t2.print();
Joint j2 = j1;
kinematic.solve_inverse_kinematics(t2, j2);
std::cout << "j2: " << std::endl;
for (int i = 0; i < 6; i++) {
std::cout << j2[i] << " ";
}
std::cout << std::endl;
TransformMatrix t3;
kinematic.solve_forward_kinematics(j2, t3);
std::cout << "t3: " << std::endl;
t3.print();
}输出结果如下：
t1:
Transform Matrix:
[ 0.8572, 0.5150, -0.0000, 0.1531]
[ 0.0000, -0.0000, 1.0000, -0.0000]
[ 0.5150, -0.8572, -0.0000, 0.0397]
[ 0.0000, 0.0000, 0.0000, 1.0000]
t2:
Transform Matrix:
[ 0.8572, 0.5150, -0.0000, 0.2531]
[ 0.0000, -0.0000, 1.0000, -0.0000]
[ 0.5150, -0.8572, -0.0000, 0.0397]
[ 0.0000, 0.0000, 0.0000, 1.0000]
j2:
-4.71993e-13 1.1615 -0.856673 0.724918 -2.53619e-13 -4.4205e-13
t3:
Transform Matrix:
[ 0.8572, 0.5150, -0.0000, 0.2531]
[ 0.0000, -0.0000, 1.0000, -0.0000]
[ 0.5150, -0.8572, -0.0000, 0.0397]
[ 0.0000, 0.0000, 0.0000, 1.0000]从测试结果可以看出，ESP32-P4 板载运动学的计算结果与 逆运动学测试结果保持一致，验证了该实现的正确性。
URDFly （可选）
URDF ( **Unified Robot Description Format，统一机器人描述格式)**是一种基于 XML 的标准，用于描述机器人的运动学结构及基本动力学属性。它可用于定义连杆（link）、关节（joint）、惯性参数、关节限制，以及碰撞模型和可视化几何模型。URDF 在机器人领域被广泛采用，是 ROS 生态中的通用模型格式，并被Gazebo、RViz、MoveIt 等主流仿真与运动规划工具原生支持。
然而，在 MCU 平台上直接使用 URDF 相对复杂，更适合采用 D-H 参数 来描述机械臂并进行正、逆运动学计算。可使用 URDFly 或其他类似工具，将 URDF 解析并转换为 D-H 参数。
以 Ragtime_Panthera 为例，在克隆 URDFly 并按照 README 安装完成所有依赖后，按照以下步骤对Ragtime_Panthera 的 URDF 进行可视化与转换：
- 将 URDF 文件中的 package://panther_description 替换为 ../
-
打开 URDFly 并加载修改后的 URDF 文件
(urdfly) PS D:\project\github\URDFly> python .\main.py
拖动右侧滑块以驱动各个关节运动，点击左侧按钮可获取 MDH 参数及其他相关信息。
关节电机调试 (Joint Motor Debugging)
在正式开始调试 DM 电机之前，请先参考 DM Motor Getting Started Guide，了解 DM 电机的控制流程。可在此下载 DM Debug Tool v1.6.8.6。
以 DM4310-24V 为例，打开 DM 电机套件后，包含以下组件：
DM Motor kit
按照下图所示的接线方式连接套件中的所有组件，为电源适配板提供 24 V 电源，并通过 USB Type-C 数据线将 PC 连接至 USB-to-CAN 模块：
DM Motor Connect
随后打开 DM 调试工具进行测试，选择正确的串口，并在 Parameter Settings 页签中尝试读取参数：
DM Debug Tool
当参数读取成功时，说明电机连接正常。随后可在 Debug 页签中尝试位置模式测试：
DM Position Control
机械装配 (Mechanical Assembly)
在对上述运动学和 DM 电机有初步了解后，可按照以下步骤组装 follower 机械臂。在此之前，请准备以下材料：
- ESP32-P4-Function-EV-Board：由 ESP32-P4 开发板和一块 7 英寸 MIPI DSI 电容触摸屏（1024 × 600）组成
- CAN 模块：用于连接 ESP32-P4 与 DM 电机
- USB 摄像头：本项目选用分辨率为 640 × 480，请确认所选 USB 摄像头支持该分辨率
- XT30 2+2 双端线缆：用于连接各电机，至少准备 6 根
- 各类 CNC 与钣金结构件：将在结构装配章节中详细说明
- 24 V 电源：用于为 DM 电机供电
- DM 电机：4 × DM4340、2 × DM4310、1 × DMH3510
注意：在原始 Ragtime_Panthera 项目中，采用的是 3 × DM4310、3 × DM4340 与 1 × DMH3510 的组合，其中DM4310 用于 base_link 旋转。但在实际运行中，由于机械臂完全伸展时惯量较大，最终将 base_link 的DM4310 更换为 DM4340，且无需进行额外的结构修改。
DM 电机配置 (DM Motor Configuration)
在将电机安装到机械结构之前，请使用 DM 调试工具为每个电机预先设置 Master ID 和 CAN ID：
ID Setting
结构装配
在装配前，请根据 BOM 提前准备所需材料，如螺丝、螺母、固定板、滑块、导轨、线缆等。由于涉及材料种类较多，需要采用不同的加工方式进行制造：
钣金加工
- Joint 1--2 connector：加工孔需攻丝，见此处，材质选用不锈钢 304
- Joint 2--3 link：加工孔需攻丝，见此处，材质选用不锈钢 304
- Joint 3--4 link：加工孔需攻丝，见此处，材质选用不锈钢 304
- Joint 4--5 connector：无需攻丝，材质选用铝合金 5052
- Joint 5--6 connector：无需攻丝，材质选用铝合金 5052
CNC 加工
- Base plate：无需攻丝，材质选用铝合金 6061
可使用 JLC 等在线加工服务商进行加工，其余材料可通过 3D 打印完成。
ESP32-P4-Function-EV-Board 安装与调试
由于 follower 项目使用的是 ESP32-P4-Function-EV-Board 套件，请提前将 LCD 屏幕安装到 ESP32-P4-Function-EV 上。并参考用户指南完成基础硬件与软件测试：
| LCD Adapter Board | ESP32-P4-Function-EV |
|---|---|
| J3 header | MIPI DSI connector |
| J6 的 RST_LCD | J1 的 GPIO27 |
| J6 的 PWM | J1 的 GPIO26 |
| J6 的 5 V | J1 的 5 V |
| J6 的 GND | J1 的 GND |
硬件连接与工程编译
克隆 follower 工程后，使用 idf.py menuconfig 进入
(Top) → Panthera Follower Configuration
对工程进行配置，例如 CAN TX/RX 引脚、C6 固件烧录引脚等。
git clone git@github.com:espressif/esp-iot-solution.git
cd esp-iot-solution/examples/robot/ragtime_panthera/follower
idf.py set-target esp32p4
idf.py menuconfig
idf.py build
Follower Menuconfig
配置项说明：
- Direct control robot arm via console：启用后，可在使用 idf.py monitor 时通过终端命令直接控制 follower
- Hardware Configuration：配置 ESP32-P4 与 CAN 模块的连接，默认 TX 为 GPIO24，RX 为 GPIO25
- Leader and Follower Gripper Mapping：配置 leader 与 follower 机械臂夹爪角度的映射关系，默认单位为弧度并乘以 100
- Leader and Follower Angle Inversion：配置 leader 与 follower 之间的关节角度是否需要取反。由于机械结构安装方式不同，各关节的正向旋转方向可能不同，请根据右手定则判断并配置是否取反
- Receiver Serial Flash Config ：用于配置 follower 的 ESP-NOW 接收端。由于当前 esp-hosted-mcu 不支持ESP-NOW，项目在 ESP32-C6 侧独立实现了 ESP-NOW 数据包接收，并使用 esp-serial-flasher 将固件下载至 ESP32-C6。默认连接如下：
| ESP32-P4 引脚 | 连接说明 |
|---|---|
| GPIO24 | 连接 CAN 模块 TX |
| GPIO25 | 连接 CAN 模块 RX |
| GPIO6 | 连接 ESP32-C6 U0RXD |
| GPIO5 | 连接 ESP32-C6 U0TXD |
| GPIO54 | 连接 ESP32-C6 EN |
| GPIO53 | 连接 ESP32-C6 BOOT |
重要提示 ：首次编译固件时，请启用 Direct control robot arm via console 和 Enable update C6 Flash 选项，并按照默认配置完成接线。
烧录与运行
以 Linux 为例，完成编译后将 ESP32-P4 连接至 PC，系统会自动生成串口设备节点（如 /dev/ttyUSB0），用于固件下载与串口日志调试：
idf.py -p /dev/ttyUSB0 flash monitor烧录完成后，可在终端中测试以下控制台命令：
| 命令 | 描述 | 用法 |
|---|---|---|
| panthera_enable | 启用或禁用所有电机 | panthera_enable <on|off> |
| panthera_goto_zero | 所有关节回零 | panthera_goto_zero |
| panthera_set_zero | 将当前位置设为零位 | panthera_set_zero |
| panthera_goto_position | 将末端移动至指定笛卡尔坐标 | panthera_goto_position -x -y -z |
| panthera_set_vision_matrix | 设置视觉标定矩阵 | panthera_set_vision_matrix -1 ... -9 |
| panthera_get_vision_matrix | 读取当前标定矩阵 | panthera_get_vision_matrix |
| panthera_read_position | 读取所有关节位置 | panthera_read_position |
首次烧录后，请先执行 panthera_read_position，确认电机通信是否正常。正常情况下，应能读取所有已添加电机的角度信息。执行 panthera_enable on 时，所有电机应亮起绿色 LED；执行 panthera_enable off 时，电机应恢复为红色 LED。随后将 follower 机械臂调整至结构零位，并执行 panthera_set_zero，使结构零位与电机零位对齐。
可直接在屏幕上启动所有电机、执行抓取动作（需已完成视觉标定）、回零及设置零位。
对于 DMH3510 电机，请在安装夹持结构后、夹爪闭合状态下设置 DMH3510 的零点。此外，在为 DMH3510 上电前，请确保其角度范围位于 0--360° 之间；若在断电状态下转动角度超过 360°，将导致 DMH3510 零点发生变化。因此，上电前应确保 DMH3510 保持在接近零位的位置。
视觉标定
本文采用 eye-to-hand 视觉配置方式，即相机与机械臂位置固定，相比 eye-in-hand 更为简单。在正式标定前，将机械臂移动至若干指定位置，并记录对应的像素坐标。为简化流程，可制作一块独立的标定板，并在指定位置放置 AprilTag，通过 PC 识别后直接生成标定矩阵：
参考上图，将机械臂放置在 300 mm × 300 mm 平板上，相机固定在 300 mm × 600 mm 平板一侧，并使用以下脚本进行标定：
import numpy as np
A = np.array([[170, 250, 331, 332, 246, 247],
[232, 227, 228, 375, 377, 291]])
B = np.array([[0.4431, 0.4431, 0.4431, 0.17, 0.17, 0.3277],
[0.15, 0, -0.15, -0.15, 0, -0.1],
[0.0397, 0.0397, 0.0397, 0.0397, 0.0397, 0.0397]])
A_hom = np.vstack([A, np.ones(A.shape[1])])
M = np.zeros((3, 3))
for i in range(3):
m_i, _, _, _ = np.linalg.lstsq(A_hom.T, B[i, :], rcond=None)
M[i, :] = m_i
print(repr(M))
flat = M.flatten()
print_str = 'panthera_set_vision_matrix'
for idx, val in enumerate(flat, start=1):
print_str += f' -{idx} {val:.6f}'
print(print_str)
for i in range(A.shape[1]):
a = np.array([0, 0, 1])
a[0] = A[0][i]
a[1] = A[1][i]
b = M @ a
print(repr(b))上述脚本的输出结果不能直接作为项目的最终标定结果，因为其依赖于相机的安装位置与角度。请使用panthera_set_vision_matrix 命令，将机械臂末端依次移动至 B 中的各个点，记录其像素坐标并替换 A；或在这些点放置 AprilTag，以便 PC 自动识别像素位置。
标定完成后，将输出命令直接输入至 idf monitor 终端，标定数据将自动保存至 NVS。
目标检测
本文默认使用 color_detect 模型，对绿色方块进行检测，作为机械臂的抓取目标。
此外，esp-dl 还提供了多种 AI 模型，可前往 esp-dl 获取更多板载 AI 模型。
目标抓取
完成标定、参数正确保存至 NVS，并将 USB 摄像头插入 ESP32-P4-Function-EV-Board 的高速 USB 接口后，LCD 屏幕将显示摄像头画面。放置绿色方块，确认其在屏幕上被正确识别后，点击 Grasp 按钮尝试抓取。
注意：当前 follower 项目仅集成了绿色颜色检测，后续维护中将尝试支持更多目标的识别。
主从机械臂同步
主从机械臂通过 ESP-NOW 进行通信（目前仅支持 leader → follower 的单向通信）。leader 机械臂通过 ESP-NOW 将关节角度信息同步至 follower，实现远程控制。在屏幕上点击 Sync 和 Enable 开关以启用同步。
leader 端舵机角度数据包结构如下：
| 字段 | 大小（字节） | 描述 | 说明 / 编码 |
|---|---|---|---|
| Header | 2 | 数据包起始标志 | 固定为 0xFF 0xFF |
| Joint Data | 14 (7×2) | 7 个舵机关节角度 | 弧度 × 100，转为 uint16，低字节在前 |
| CRC | 2 | CRC 校验 | CRC16，小端序，初始值 UINT16_MAX |
如遇同步异常，请检查以下问题：
- ESP32-C6 固件问题 ：确认 follower 的 ESP32-C6 固件已下载。在 follower 工程中运行 idf.py menuconfig，进入
(Top) → Panthera Follower Configuration → Receiver Serial Flash Config，启用 Enable update C6 Flash 完成首次烧录；烧录完成后关闭该选项以减少启动时间 - ESP-NOW 通信问题：确认 leader 工程中配置的接收端 MAC 地址是否正确，应填写 ESP32-P4-Function-EV-Board 上 ESP32-C6 的 MAC 地址
- Leader and Follower Synchronization
- Leader and Follower Synchronization
总结
本文介绍了在 ESP32-P4 上实现机械臂项目的完整流程，涵盖运动学基础、视觉标定以及远程通信，验证了ESP32-P4 在工业机械臂应用中的可行性。基于本文内容，可进一步探索 ESP32-P4 的应用边界。
欢迎尝试这些示例，构建属于你自己的应用，并分享你的实践经验！