Unitree Z1 六轴机械臂 SDK 二次开发实战：从接口调用到自动化任务实现

1. 从一台桌面级六轴机械臂说起

第一次把 Unitree Z1 从箱子里拎出来的时候，我脑子里冒出来的第一个念头是：这东西比想象中小太多了。整机不到 5 公斤，六个关节，臂展大概 70 厘米出头，拿在手里像个精密的金属玩具。但通电之后，它关节里那种干净利落的伺服响应，会让你立刻意识到这不是玩具——这是一台真正可以拿来做研究、做原型验证、甚至做轻量级自动化任务的六轴协作机械臂。

Z1 是宇树科技推出的一款轻量化六轴机械臂，官方提供了完整的 SDK 来做二次开发。很多人拿到它之后的第一反应是"我该怎么让它动起来"，第二反应是"我该怎么让它自动地、有逻辑地动起来"。这两个问题之间隔着的，就是从接口调用到自动化任务实现的整条路。这篇文章想聊的就是这条路——不是官方文档的复述，而是我在实际折腾过程中踩过的坑、想明白的道理、以及最后跑通的那套方案。

如果你手上正好有一台 Z1，或者正在评估要不要用它做项目，又或者你用的是别的六轴机械臂但同样面临 SDK 二次开发的问题，那这篇内容应该能给你省下不少时间。我会从 SDK 的基本接口讲起，一路讲到怎么把零散的接口调用组织成一个能自动跑起来的任务流程，中间涉及通信方式的选择、坐标系的理解、轨迹规划的思路、以及状态机设计这些实际开发中绕不开的东西。

需要说明的是，Z1 的 SDK 本身在持续迭代，不同固件版本之间接口可能有细微差异。我下面讲的内容基于我实际使用的版本，如果你发现对不上，优先以你手头版本的官方文档为准。但底层的思路和方法论是通用的，这部分不会因为版本变化而失效。

2. 先搞清楚 SDK 到底给了你什么

2.1 通信层：UDP 还是串口，这是个问题

Z1 的 SDK 底层通信方式主要有两种：一种是基于 UDP 的网络通信，一种是串口通信。官方示例里两种都有，但实际用下来，选择哪种取决于你的部署场景。

UDP 的好处是灵活，你的控制程序可以跑在另一台电脑上，通过网线或者局域网跟机械臂通信，不用物理上贴着机械臂。这对于需要把控制逻辑放在性能更强的工控机或者带 GPU 的机器上的场景很友好。但 UDP 本身是不可靠传输，虽然 Z1 的协议层做了一些处理，但在网络状况不好的时候，你可能会遇到指令丢失或者延迟抖动的问题。

串口的好处是确定性更强，物理连接直接，延迟稳定。缺点是控制程序必须跑在跟机械臂物理连接的设备上，而且串口的带宽有限，高频控制指令的发送会受限。

我自己的选择是：开发调试阶段用 UDP，因为方便；最终部署如果对实时性要求高，切到串口。这个切换成本不高，因为 SDK 把通信层抽象得比较好，上层调用的接口基本一致。

注意：不管你用哪种通信方式，第一次连接之前一定要确认机械臂的固件版本和 SDK 版本匹配。我遇到过因为版本不匹配导致关节角度指令被解析成错误格式的情况，机械臂会做出完全意料之外的动作，非常危险。

2.2 核心接口分类：别被一堆函数名吓到

Z1 SDK 暴露的接口看起来很多，但归类之后其实就几大类：

• 状态获取类：获取当前关节角度、关节速度、末端位姿、IMU 数据等。这类接口是只读的，调用频率可以高一些，用来做状态监控和反馈控制。
• 运动控制类：设置关节角度、设置关节速度、设置末端位姿等。这类接口是写的，调用频率和参数范围需要谨慎控制。
• 模式切换类：在位置模式、速度模式、力矩模式之间切换。不同模式下，同一类控制接口的行为不同。
• 配置类：设置关节限位、设置运动学参数、标定等。这类接口一般在初始化阶段调用，运行中很少动。

理解这个分类很重要，因为它决定了你在写代码时的组织方式。我的习惯是把状态获取封装成一个独立的监控线程，把运动控制封装成另一个线程，两者通过共享内存或者消息队列通信。这样即使控制逻辑写得有问题，监控线程也不会被阻塞，你至少还能看到机械臂当前的真实状态。

2.3 坐标系：最容易出错的地方

六轴机械臂的坐标系问题是个老生常谈的话题，但每次都能坑到人。Z1 涉及到的坐标系至少有这几个：

• 关节空间：六个关节角度组成的六维向量，这是最底层的表示。
• 基坐标系：固定在机械臂底座上的坐标系。
• 末端坐标系：固定在末端法兰盘上的坐标系。
• 工具坐标系：如果你装了夹爪或者其他末端执行器，还需要定义工具坐标系。

SDK 里的正运动学和逆运动学接口，默认是在基坐标系和末端坐标系之间转换。但如果你装了夹爪，末端法兰盘的位置和夹爪实际抓取点的位置之间有一个固定的偏移，这个偏移必须通过工具坐标系来补偿，否则你的抓取位置永远会差那么几厘米。

我踩过的坑是：一开始没定义工具坐标系，直接用末端法兰盘的位姿去做抓取，结果夹爪总是差一点够不到目标。后来量了夹爪的尺寸，在 SDK 里设置了工具坐标系偏移，问题立刻解决。这个偏移量建议用游标卡尺实际测量，不要靠目测或者猜。

3. 从单次调用到连续控制：实时性是怎么保证的

3.1 控制频率的选择与权衡

机械臂控制有一个绕不开的参数：控制频率。你每秒给机械臂发多少次指令，直接影响到运动的平滑度和响应速度。

Z1 官方建议的控制频率范围大概在 100Hz 到 500Hz 之间。频率太低，运动会有明显的顿挫感；频率太高，通信带宽和计算资源都会吃紧，而且如果指令生成的速度跟不上发送速度，反而会出现指令堆积或者丢帧。

我的经验是：对于大多数桌面级的抓取、放置、简单轨迹跟踪任务，200Hz 是一个比较舒服的平衡点。这个频率下，运动足够平滑，同时对控制程序的实时性要求不至于太苛刻。如果你要做力控或者高速轨迹跟踪，可能需要提到 500Hz 甚至更高，但那就需要更认真的实时系统设计了。

这里有个细节：控制频率不等于你主循环的频率。你可以在主循环里以 50Hz 的频率做决策和规划，但用一个独立的定时器以 200Hz 的频率发送控制指令。决策和执行的频率解耦，是保证系统稳定的关键。

3.2 指令插值：让运动看起来像"人"做的

如果你直接以 200Hz 的频率发送一系列离散的目标点，机械臂的运动可能会显得很生硬，尤其是在方向变化的地方。这时候需要做指令插值。

最简单的插值是线性插值：在两个目标点之间，按照时间比例生成中间点。但线性插值在方向变化处会有速度不连续的问题，机械臂会"顿"一下。更好的选择是样条插值或者 S 形速度规划，让速度曲线连续，加速度也连续。

Z1 SDK 本身提供了一些轨迹规划的基础接口，但功能比较基础。如果你需要更复杂的轨迹，比如圆弧、样条曲线，可能需要自己在上层实现，然后把规划好的密集点序列通过 SDK 发送下去。

我自己的做法是：对于简单的点到点运动，用 SDK 自带的规划接口就够了；对于需要经过多个中间点的复杂轨迹，我在上层用 Python 的 scipy 或者自己写的五次多项式插值生成密集点序列，然后以 200Hz 的频率逐点发送。这样既保证了轨迹的平滑性，又不需要在 SDK 层面做太多改动。

3.3 状态反馈的读取与使用

控制不是单向的。你发指令下去，机械臂执行，但执行的结果如何，需要通过状态反馈来确认。

Z1 SDK 的状态反馈接口可以读取当前关节角度、关节速度、关节电流等信息。这些信息可以用来做几件事：

• 闭环控制：如果你在做力控或者阻抗控制，状态反馈是必须的。
• 异常检测：如果某个关节的实际角度跟指令角度偏差过大，可能意味着碰到了障碍物或者发生了堵转。
• 状态估计：通过关节电流可以粗略估计末端受到的力，虽然精度不如力传感器，但在没有力传感器的情况下是个有用的补充。

我习惯在控制循环里加一个简单的异常检测：如果连续 N 个周期内，某个关节的实际角度与指令角度的偏差超过阈值，就触发保护逻辑，停止运动并报警。这个逻辑救过我几次，尤其是在调试抓取动作的时候，夹爪还没装好就发了抓取指令，机械臂直接撞到了桌面，幸好异常检测及时触发，没有造成损坏。

4. 自动化任务实现：从脚本到状态机

4.1 为什么需要状态机

当你只是想让机械臂做一个简单的动作，比如"移动到某个位置"，写几行脚本调用接口就够了。但当你需要实现一个完整的自动化任务，比如"识别目标、规划路径、抓取、放置、回到初始位置"，事情就复杂了。

复杂的地方不在于单个动作的实现，而在于动作之间的衔接、异常情况的处理、以及任务状态的跟踪。这时候，一个清晰的状态机设计就非常必要。

状态机的基本思路是：把整个任务分解成若干个状态，每个状态对应一个明确的动作或者等待条件，状态之间通过事件触发转移。比如：

• IDLE：等待任务开始信号。
• APPROACH：移动到目标上方的预备位置。
• DESCEND：下降到抓取位置。
• GRASP：闭合夹爪。
• LIFT：抬起。
• MOVE_TO_PLACE：移动到放置位置上方。
• PLACE：下降并松开夹爪。
• RETURN：回到初始位置。

每个状态里，你只需要关心当前状态要做什么，以及什么条件下转移到下一个状态。这样代码结构清晰，调试也方便——你可以单独测试每个状态，也可以手动触发状态转移来验证逻辑。

4.2 状态机的实现方式

状态机可以用很多种方式实现，从最简单的 switch-case 到复杂的状态机框架。对于机械臂任务这种规模，我推荐用 Python 的字典加函数指针的方式，简单直接，不需要引入额外的框架。

核心结构大概是这样：

class ArmStateMachine: def __init__(self): self.state = 'IDLE' self.transitions = { 'IDLE': {'start': 'APPROACH'}, 'APPROACH': {'reached': 'DESCEND', 'error': 'ERROR'}, 'DESCEND': {'reached': 'GRASP', 'error': 'ERROR'}, 'GRASP': {'grasped': 'LIFT', 'error': 'ERROR'}, 'LIFT': {'reached': 'MOVE_TO_PLACE', 'error': 'ERROR'}, 'MOVE_TO_PLACE': {'reached': 'PLACE', 'error': 'ERROR'}, 'PLACE': {'released': 'RETURN', 'error': 'ERROR'}, 'RETURN': {'reached': 'IDLE', 'error': 'ERROR'}, 'ERROR': {'reset': 'IDLE'} } self.handlers = { 'IDLE': self.handle_idle, 'APPROACH': self.handle_approach, # ... 其他状态的处理函数 } def step(self): handler = self.handlers.get(self.state) if handler: event = handler() if event and event in self.transitions.get(self.state, {}): self.state = self.transitions[self.state][event]

这个结构的好处是，状态转移逻辑集中在一处，一目了然。每个状态的处理函数只负责执行动作和返回事件，不关心下一步是什么。这样当你需要调整任务流程时，只需要改转移表，不需要动各个状态的处理逻辑。

4.3 异常处理与恢复策略

自动化任务最怕的就是异常。机械臂在无人值守的情况下跑，一旦出错，轻则任务失败，重则设备损坏。所以异常处理必须做在前面。

我的策略是分三层：

第一层是预防。在发送指令之前，检查目标位置是否在工作空间内，检查关节角度是否在限位范围内，检查速度是否超过安全阈值。这些检查在软件层面做，成本很低，但能避免大部分低级错误。

第二层是检测。在运动过程中，持续监控关节角度偏差、电流、温度等指标。一旦发现异常，立即停止运动，进入错误状态。

第三层是恢复。错误状态不是终点，而是一个等待人工干预或者自动恢复的中间状态。根据错误的类型，可以设计不同的恢复策略：如果是通信超时，可以尝试重连；如果是关节偏差过大，可以尝试回到安全位置；如果是硬件故障，那就只能报警等待处理。

这里有个经验：错误状态一定要设计一个明确的退出条件，否则机械臂会一直卡在错误状态里，既不能继续任务，也不能接受新指令。我一般会设计一个"复位"事件，触发后机械臂先回到一个安全的初始位置，然后回到 IDLE 状态。

5. 实操过程中的那些坑

5.1 初始化顺序不能乱

Z1 的初始化有一套固定的流程：先上电，等待自检完成，然后建立通信连接，然后使能关节，然后设置工作模式，最后才能发送运动指令。这个顺序不能乱，乱了就会出问题。

我遇到过的情况是：通信连接建立之后，没等自检完成就发了使能指令，结果机械臂没反应，但也不报错。后来查了半天才发现是自检还没结束。所以初始化的时候，一定要在每一步之间加足够的等待时间，或者通过状态查询接口确认上一步确实完成了再进入下一步。

5.2 关节限位不是摆设

Z1 的每个关节都有机械限位和软件限位。机械限位是物理上的硬限位，撞上去会损坏机械结构；软件限位是 SDK 里设置的，可以在到达机械限位之前就阻止运动。

我的建议是：软件限位一定要设置，而且要比机械限位留出足够的安全余量。我一般会留 5 到 10 度的余量。这样即使控制程序出了 bug，机械臂也不会撞到硬限位。

另外，不同关节的限位范围不一样，设置的时候要逐个确认。Z1 的关节 1 和关节 6 的限位范围比较大，关节 2 和关节 3 的限位范围相对小一些，具体数值查官方文档。

5.3 通信超时与重连

UDP 通信在网络状况不好的时候会出现超时。如果控制程序没有处理超时，可能会一直等待，导致整个任务卡住。

我的做法是：在通信层加一个超时计数器，每次发送指令后等待反馈，如果超过一定时间没有收到反馈，就认为超时。连续超时达到阈值，就触发重连逻辑。重连成功后，先查询机械臂当前状态，确认安全后再继续任务。

这里有个细节：重连之后不要直接继续之前的任务，因为机械臂的实际位置可能已经跟预期不一样了。正确的做法是回到一个已知的安全状态，然后重新开始任务。

5.4 常见问题速查表

6. 把零散接口组织成可复用的控制框架

6.1 分层设计：让代码好维护

当你把上面这些东西都跑通之后，会发现代码里有很多重复的逻辑：连接、初始化、发送指令、读取状态、异常处理。如果每个任务都重新写一遍，不仅效率低，而且容易出错。

我的做法是把控制代码分成三层：

• 通信层：封装 SDK 的底层接口，处理连接、发送、接收、超时重连。这一层不涉及任何业务逻辑，只负责把数据从 A 传到 B。
• 控制层：封装运动控制的基本操作，比如移动到某个关节角度、移动到某个末端位姿、设置速度、读取状态。这一层处理单位转换、限位检查、插值等。
• 任务层：实现具体的自动化任务，用状态机组织控制层的调用。这一层只关心任务逻辑，不关心底层怎么实现。

这样分层之后，换一个机械臂或者换一个通信方式，只需要改通信层；调整控制参数，只需要改控制层；修改任务流程，只需要改任务层。各层之间通过清晰的接口通信，互不干扰。

6.2 参数配置化：别把数值写死在代码里

控制参数、限位范围、工具坐标系偏移、状态机转移条件，这些东西都不应该写死在代码里。我的做法是全部放到一个 YAML 或者 JSON 配置文件里，代码启动时读取。

这样做的好处是：调整参数不需要改代码，不需要重新编译，甚至不需要重启程序（如果做了热加载）。对于需要反复调试的场景，这个效率提升非常明显。

配置文件的结构大概是这样：

robot: connection: type: "udp" ip: "192.168.1.100" port: 8080 control: frequency: 200 max_velocity: 0.5 max_acceleration: 1.0 limits: joint_1: [-150, 150] joint_2: [-90, 90] # ... tool: offset: [0.0, 0.0, 0.12] orientation: [0.0, 0.0, 0.0] task: approach_height: 0.15 grasp_height: 0.02 place_height: 0.10

6.3 日志与可视化：调试的好帮手

机械臂调试过程中，最有用的是两样东西：日志和可视化。

日志要记录每一次状态转移、每一次指令发送、每一次异常触发。日志的格式要结构化，方便后续分析。我一般用 Python 的 logging 模块，输出到文件和控制台，级别可以配置。

可视化方面，如果条件允许，用一个简单的 3D 可视化工具实时显示机械臂的位姿和轨迹，会大大加快调试速度。ROS 的 RViz 是一个选择，但如果你不想引入 ROS 的复杂性，也可以用 matplotlib 的 3D 绘图做一个简易的可视化。虽然简陋，但足够看清机械臂在干什么。

我自己的调试流程是：先在可视化界面里确认轨迹规划的结果是对的，然后再实际发送指令给机械臂。这样可以把软件 bug 和硬件问题分开，排查起来快很多。

7. 一些实际任务中的经验

7.1 抓取任务：视觉与运动的配合

如果你的自动化任务涉及抓取，那大概率需要视觉系统的配合。视觉负责识别目标的位置和姿态，机械臂负责移动到目标位置并执行抓取。

这里的关键是坐标转换：视觉系统给出的目标位置通常是在相机坐标系下的，需要转换到机械臂的基坐标系。这个转换涉及相机标定和手眼标定，是另一个大话题，这里不展开。但我要强调的是：标定的精度直接决定了抓取的成败。标定没做好，后面怎么调都是白搭。

我的经验是：标定完成后，一定要做验证。拿一个已知位置的目标，让机械臂去抓，看实际抓取位置和预期位置的偏差。如果偏差在可接受范围内，标定就算通过了。如果偏差大，先检查标定流程，再检查坐标系转换的代码。

7.2 轨迹跟踪：平滑比精确更重要

有些任务需要机械臂沿着一条轨迹运动，比如画圆、画直线、或者跟踪一个移动的目标。这时候，轨迹的平滑性往往比精确性更重要。

原因很简单：机械臂的动力学特性决定了它无法瞬间改变速度方向。如果你给的轨迹有尖锐的拐角，机械臂要么跟不上，要么会产生很大的振动。所以轨迹规划的时候，一定要做平滑处理，让速度和加速度连续。

我一般用五次多项式或者 B 样条来做轨迹平滑。五次多项式可以保证位置、速度、加速度都连续，适合点到点的轨迹。B 样条更灵活，适合需要经过多个中间点的复杂轨迹。

7.3 多任务调度：别让机械臂闲着

如果你的自动化系统需要处理多个任务，比如从传送带上抓取不同位置的物体，那任务调度就很重要。机械臂的运动时间往往是瓶颈，所以调度策略的目标是让机械臂尽可能少地等待。

我的做法是：把任务分解成"必须串行"和"可以并行"的部分。比如，视觉识别可以和机械臂运动并行，因为相机拍照不需要机械臂停下来。抓取和放置必须串行，因为涉及机械臂的物理动作。

在代码层面，我用一个任务队列来管理待执行的任务，用一个调度器来决定下一个执行哪个任务。调度器的策略可以根据实际场景调整，比如优先执行距离最近的任务，或者优先执行截止时间最近的任务。

8. 关于 SDK 二次开发的一些思考

Z1 的 SDK 二次开发，表面上是学一堆接口怎么调用，实际上是在学怎么把一个物理设备的行为抽象成软件可以控制的模型。这个抽象过程涉及到通信、控制、状态管理、异常处理等多个方面，每一方面都有它的门道。

我自己的体会是：不要一上来就想着做复杂的任务。先把最简单的"让机械臂移动到指定位置"跑通，然后加上状态反馈，然后加上异常处理，然后加上状态机，一步一步来。每一步都确认稳定了，再进入下一步。这样虽然看起来慢，但实际上是最快的路径，因为每一步的问题都局限在很小的范围内，排查起来容易。

另外，官方文档和示例代码一定要仔细看。Z1 的 SDK 文档虽然不算特别详细，但关键信息都有。很多问题其实文档里已经写了，只是没注意到。我遇到过好几次，折腾半天的问题，回头翻文档发现第一页就写了注意事项。

最后，安全永远是第一位的。机械臂是物理设备，它的运动会对周围的人和物产生实际影响。在调试阶段，一定要确保工作空间内没有障碍物，机械臂的限位设置正确，异常检测逻辑有效。不要因为赶进度就跳过安全检查，一次事故的代价远大于省下来的时间。

如果你也在做 Z1 或者其他机械臂的二次开发，欢迎交流。这个领域的东西很杂，一个人踩坑不如大家一起踩，踩完了分享出来，后来的人就能少走弯路。