1. 拿到Z1之后为什么我建议你从SDK而不是ROS包入手很多人拿到Unitree Z1机械臂的第一反应是去找ROS驱动包觉得跑通一个roslaunch看到rviz里的模型动起来就算入门了。我一开始也是这个思路但实际用下来发现如果你的目标是做自动化任务——比如固定工位上的抓取、分拣、上下料这类需要稳定复现的动作——直接从SDK层切入反而更省事。原因很简单ROS包本质上是对SDK的一层封装它给你的是话题和服务方便你做可视化调试和多传感器融合但如果你只需要控制机械臂本体做确定性的轨迹动作多一层中间件就多一层延迟和不确定性。Z1的SDK提供的是一套基于UDP的底层通信接口直接和机械臂的控制单元对话。它的核心价值在于实时性可控和接口语义清晰。你调用一个关节角度指令它就是把目标角度打包发过去没有话题序列化、没有节点调度链路短。对于自动化任务来说这种确定性比生态丰富重要得多。这篇文章面向的是已经有一台Z1、想把它从能手动示教推进到能自动跑任务的开发者。我会从SDK的接口调用讲起把通信机制、控制模式、轨迹规划、状态反馈这些环节拆开说最后落到一个完整的自动化抓取任务的实现上。中间会穿插我自己踩过的坑比如控制周期设错导致机械臂抖动、坐标系没对齐导致抓偏、急停逻辑没处理好导致任务卡死这些。如果你手上正好有Z1或者在做类似的六轴机械臂二次开发这篇内容应该能帮你少走几天弯路。需要说明的是Z1的SDK在不同固件版本之间接口会有细微差异我下面讲的内容基于较常见的版本具体参数名和函数签名请以你手头SDK文档为准。但底层的控制逻辑和工程思路是通用的。2. Z1 SDK的通信骨架UDP、控制周期与指令打包2.1 为什么是UDP而不是TCPZ1 SDK选择UDP作为传输层这个决定背后有明确的工程考量。机械臂控制是一个周期性高频发送的场景典型的控制周期在1ms到10ms之间也就是每秒100到1000帧指令。如果用TCP每次发送都要经历握手、确认、重传机制一旦网络稍有波动TCP的重传会导致指令延迟到达而延迟到达的旧指令对机械臂来说是灾难——它可能已经执行了新指令突然又收到一个旧目标位置直接造成抖动甚至失控。UDP不保证可靠交付但保证了发送即忘的时序特性。丢一帧指令对机械臂的影响是这一周期维持上一帧的目标而不是收到一个乱序的旧目标。对于控制类应用这种宁可丢帧不可乱序的特性是刚需。实际使用中你需要关注的是发送频率的稳定性。我建议用一个独立的定时线程来驱动指令发送而不是在主循环里顺手发。下面是一个典型的发送节奏控制import time import threading class CommandSender: def __init__(self, arm, period0.002): self.arm arm self.period period # 2ms控制周期对应500Hz self.running False self.thread None def start(self): self.running True self.thread threading.Thread(targetself._loop) self.thread.start() def _loop(self): next_time time.perf_counter() while self.running: self.arm.send_joint_command() next_time self.period sleep_time next_time - time.perf_counter() if sleep_time 0: time.sleep(sleep_time) else: # 说明这一周期已经超时需要记录并调整 next_time time.perf_counter()这里用perf_counter而不是time.time是因为前者是单调时钟不受系统时间调整影响。next_time self.period这种累加方式比每次sleep(period)更稳定因为它补偿了发送本身消耗的时间。2.2 控制周期到底设多少合适这是我最想强调的一个点。Z1的SDK文档通常会建议一个控制周期范围但不会告诉你不同周期对实际表现的影响。我实测下来的经验是控制周期适用场景实际表现1ms (1000Hz)高动态轨迹、力控对网络和主机性能要求高普通工控机容易丢帧2ms (500Hz)常规轨迹跟踪平滑度和稳定性平衡最好推荐5ms (200Hz)点到点运动、低速任务够用但快速运动时能感觉到轻微顿挫10ms (100Hz)仅位置保持、状态监控运动轨迹会有明显阶梯感我一开始图省事用了10ms结果做圆弧轨迹时机械臂走出来的是一段段折线末端执行器在工件表面留下了明显的接痕。后来改成2ms同样的轨迹代码表面质量立刻上来了。这个坑的本质是控制周期决定了轨迹插补的粒度周期太长插补点之间的机械臂只能靠内部伺服去猜猜出来的路径和你的规划路径就有偏差。但也不是越短越好。我试过1ms在普通笔记本上跑CPU占用率直接飙到30%以上而且偶尔会因为操作系统调度延迟导致丢帧反而出现抖动。2ms是一个在普通x86工控机上能稳定跑住的甜点值。2.3 指令打包里容易被忽略的字段Z1的关节控制指令通常包含目标角度、目标速度、目标力矩这几个核心字段。很多人只填角度速度和力矩留默认值结果发现机械臂运动要么太猛要么太软。这里面的逻辑是目标角度是位置环的输入决定去哪里。目标速度是速度前馈告诉伺服大概用多快的速度经过这段路。填0的话伺服完全靠位置误差去算速度响应会滞后。目标力矩是力矩前馈对于需要克服重力或外力的场景很重要。Z1的关节在水平伸展时重力矩很大如果力矩前馈给0位置环需要产生很大的误差才能输出足够的力矩来维持姿态表现为机械臂在目标点附近点头。我的做法是对于已知的轨迹用逆动力学算一下每个路径点的理论力矩作为前馈填进去。如果懒得算至少把速度前馈按轨迹的切向速度填上效果立竿见影。# 简化的前馈填充示例 def build_command(q_target, qd_target, tau_ff): cmd { q: q_target, # 目标关节角度单位rad qd: qd_target, # 目标关节速度单位rad/s tau: tau_ff # 前馈力矩单位N·m } return cmd注意不同固件版本对速度和力矩字段的处理方式可能不同有的版本会忽略这两个字段有的版本会做限幅。建议先用小幅度动作测试观察填入不同值时的实际响应差异。3. 从单点指令到连续轨迹插补与坐标系对齐3.1 关节空间插补和笛卡尔空间插补的选择自动化任务里机械臂很少只走一个点通常是从A点经过一系列中间点到达B点。这就涉及插补方式的选择。关节空间插补是在每个关节的角度序列上做插值优点是计算简单、不会遇到奇异点缺点是末端轨迹不可预测。比如你让关节1从0转到90度关节2从0转到45度同时插补末端走的是一条曲线而不是直线。笛卡尔空间插补是先规划末端在直角坐标系下的直线或圆弧路径再通过逆运动学解算成关节角度。优点是末端轨迹精确可控缺点是要处理奇异点和关节限位。对于抓取任务我强烈建议用笛卡尔空间插补。因为抓取的关键是末端执行器要沿着一个确定的方向接近工件如果末端轨迹是弯的接近方向就不可控抓取成功率会大打折扣。import numpy as np def cartesian_linear_interp(start_pose, end_pose, steps): 在笛卡尔空间做直线插补返回位姿序列 poses [] for i in range(steps 1): t i / steps pos start_pose[:3] t * (end_pose[:3] - start_pose[:3]) # 姿态用四元数球面插值避免欧拉角万向锁 ori slerp(start_pose[3:], end_pose[3:], t) poses.append(np.concatenate([pos, ori])) return poses姿态插值一定要用四元数球面插值slerp不要用欧拉角线性插值。欧拉角在接近90度时会有万向锁问题插值出来的姿态会突然翻转机械臂会做出你完全意想不到的动作。这个坑我在第一次做姿态调整时踩过机械臂手腕突然转了180度差点撞到夹具。3.2 工具坐标系和基坐标系的对齐这是自动化任务里最容易出错的环节。SDK给你的关节角度是相对于机械臂基座的但你的抓取目标是相对于工件坐标系的。如果这两个坐标系没有对齐你算出来的抓取点就是错的。标定的基本思路是在机械臂末端装一个标定针手动示教几个已知在工件坐标系下的点同时记录对应的关节角度通过正运动学算出这些点在基坐标系下的位置然后解算两个坐标系之间的变换矩阵。def compute_base_to_work_transform(work_points, base_points): work_points: 工件坐标系下的标定点坐标列表 base_points: 通过正运动学算出的对应基坐标系坐标 返回4x4变换矩阵 work_points np.array(work_points) base_points np.array(base_points) # 去中心化 work_centroid work_points.mean(axis0) base_centroid base_points.mean(axis0) work_centered work_points - work_centroid base_centered base_points - base_centroid # SVD求旋转 H work_centered.T base_centered U, S, Vt np.linalg.svd(H) R Vt.T U.T if np.linalg.det(R) 0: Vt[-1, :] * -1 R Vt.T U.T t base_centroid - R work_centroid T np.eye(4) T[:3, :3] R T[:3, 3] t return T标定至少需要3个不共线的点我一般用4个点做冗余然后用最小二乘。标定完之后一定要验证拿一个没参与标定的点用变换矩阵算一下看和实际位置的偏差。偏差在0.5mm以内算合格超过1mm就要检查是不是标定点选得不好或者机械臂重复定位精度有问题。提示Z1的重复定位精度在出厂参数里有标称值但实际使用中如果负载偏心或者关节磨损精度会下降。标定前先让机械臂空跑几遍热机之后再标结果更稳定。3.3 逆运动学解算的数值稳定性笛卡尔空间插补出来的每个位姿都要通过逆运动学转成关节角度。Z1是六轴机械臂理论上逆解有解析形式但SDK通常提供的是数值解法。数值解法的好处是通用坏处是在奇异点附近会震荡。奇异点的典型场景是手腕关节和肩部关节共线此时有无穷多组解数值迭代会在这些解之间跳来跳去。表现就是机械臂在某个位置突然抖一下或者关节角度突变。处理办法有两个一是在轨迹规划阶段就避开奇异区域比如不要让机械臂完全伸直二是对逆解结果做连续性检查如果相邻两个路径点的关节角度差超过阈值就插入过渡点或者调整路径。def check_joint_continuity(q_prev, q_next, threshold0.5): 检查相邻关节角度是否连续threshold单位rad diff np.abs(q_next - q_prev) if np.any(diff threshold): return False, diff return True, diff这个阈值我一般设0.5rad约28度。超过这个值基本可以判定是逆解跳变了需要处理。4. 状态反馈与闭环让机械臂知道自己在哪4.1 订阅关节状态和末端位姿光发指令不看反馈那是开环控制机械臂撞了东西你都不知道。Z1的SDK会周期性地回传关节状态包括当前角度、速度、力矩、温度等。你需要起一个接收线程把这些状态存下来供控制逻辑查询。class StateReceiver: def __init__(self, arm): self.arm arm self.state { q: np.zeros(6), qd: np.zeros(6), tau: np.zeros(6), timestamp: 0 } self.lock threading.Lock() self.running False def start(self): self.running True self.thread threading.Thread(targetself._loop) self.thread.start() def _loop(self): while self.running: new_state self.arm.recv_state() with self.lock: self.state new_state def get_state(self): with self.lock: return self.state.copy()这里加锁是必要的因为控制线程和接收线程会并发访问状态。不加锁的话你可能读到一半被接收线程改了拿到一个角度是新的、速度是旧的状态基于这种状态做决策会出问题。4.2 用状态反馈做简单的碰撞检测Z1的关节力矩反馈可以用来做碰撞检测。原理很简单正常情况下关节力矩应该和你的指令前馈加上重力补偿大致吻合。如果实测力矩突然比预期大很多说明遇到了外部阻力大概率是撞了。def detect_collision(state, expected_tau, threshold5.0): threshold单位N·m根据负载调整 tau_error np.abs(state[tau] - expected_tau) if np.any(tau_error threshold): return True, tau_error return False, tau_error这个阈值需要根据你的负载调。空载时设小一点比如3N·m带负载时设大一点比如8N·m。设太小会误报设太大撞了也不停。我一般会在任务开始前让机械臂空跑一遍记录各关节的力矩波动范围然后取波动上限加2N·m作为阈值。注意碰撞检测触发后不要直接切断使能那样机械臂会因为重力直接掉下来。正确的做法是切换到阻尼模式或者位置保持模式让机械臂软停下来然后再做处理。4.3 状态反馈的延迟补偿UDP回传的状态是有延迟的从机械臂采样到你的程序收到中间有网络传输和解析的时间。这个延迟在低速任务里可以忽略但在高速任务里会导致你的控制决策基于过时的状态。补偿的办法是用状态里的时间戳做外推。如果状态里带了采样时刻的关节速度和加速度你可以估算当前时刻的状态def extrapolate_state(state, current_time): dt current_time - state[timestamp] q_est state[q] state[qd] * dt return q_est这个外推只对短时间有效dt超过10ms就不准了。所以如果你的控制周期是2ms状态回传也是2ms那外推个几毫秒是有意义的。如果状态回传是10ms一次那外推就没太大价值不如直接用最新状态。5. 自动化抓取任务的完整实现链路5.1 任务状态机的设计自动化任务不能写成一条直线代码因为中间任何一步都可能失败——没抓到、抓偏了、工件没到位。我习惯用一个状态机来组织任务逻辑每个状态有明确的进入条件、执行动作和退出条件。class GrabTaskState: IDLE idle APPROACH approach DESCEND descend CLOSE_GRIPPER close_gripper LIFT lift MOVE_TO_PLACE move_to_place RELEASE release RETREAT retreat ERROR error状态机的核心是每个状态的超时处理。比如DESCEND状态如果机械臂在5秒内没有到达目标位置说明可能撞到了什么东西应该转到ERROR状态。没有超时处理的状态机一旦某一步卡住整个任务就死在那里了。class TaskExecutor: def __init__(self, arm, state_receiver): self.arm arm self.state_receiver state_receiver self.current_state GrabTaskState.IDLE self.state_start_time 0 self.timeout { GrabTaskState.APPROACH: 5.0, GrabTaskState.DESCEND: 3.0, GrabTaskState.CLOSE_GRIPPER: 2.0, GrabTaskState.LIFT: 3.0, GrabTaskState.MOVE_TO_PLACE: 5.0, GrabTaskState.RELEASE: 2.0, GrabTaskState.RETREAT: 3.0, } def step(self): now time.time() if now - self.state_start_time self.timeout.get(self.current_state, 10.0): self.transition_to(GrabTaskState.ERROR) return # 根据当前状态执行对应逻辑 if self.current_state GrabTaskState.APPROACH: self._do_approach() elif self.current_state GrabTaskState.DESCEND: self._do_descend() # ... 其他状态5.2 接近和下降阶段的力控策略抓取任务里最危险的是下降阶段因为末端执行器要接近工件表面位置稍有偏差就会撞上去。纯位置控制在这里很危险因为位置误差会直接变成碰撞力。我的做法是在下降阶段引入力矩监控。当检测到末端力矩突然增大说明接触到了工件立即停止下降切换到夹爪闭合。这样即使位置标定有1-2mm的误差也不会撞坏工件或机械臂。def _do_descend(self): state self.state_receiver.get_state() # 检查是否接触 if np.any(np.abs(state[tau]) self.contact_threshold): self.transition_to(GrabTaskState.CLOSE_GRIPPER) return # 继续下降 target self.current_target - np.array([0, 0, 0.001, 0, 0, 0]) self.current_target target self.arm.send_cartesian_command(target)这里的contact_threshold要根据夹爪和工件的重量调。轻质工件设2-3N·m重工件设5-8N·m。5.3 夹爪控制与抓取确认夹爪闭合后不能假设一定抓到了。要有确认机制。Z1的夹爪通常有位置反馈如果闭合到位但位置没到预期值说明夹住了东西如果位置到了预期值但力矩很小说明夹空了。def _do_close_gripper(self): self.gripper.close() time.sleep(0.5) # 等夹爪动作完成 gripper_pos self.gripper.get_position() gripper_tau self.gripper.get_torque() if gripper_pos self.gripper_closed_threshold and gripper_tau self.grasp_torque_threshold: # 位置没完全闭合且有力矩说明抓到了 self.transition_to(GrabTaskState.LIFT) else: # 夹空了或者没夹住 self.transition_to(GrabTaskState.ERROR)这个逻辑看起来简单但实际调试时阈值很难一次调对。我的经验是先用标准工件试记录抓取成功时的位置和力矩值然后取一个比成功值稍宽松的范围作为阈值。比如成功时位置是0.8满量程1.0力矩是3N·m那阈值可以设位置0.6且力矩2N·m。5.4 异常恢复与重试逻辑任务失败后不能直接停在那里等人来处理要有自动重试。但重试不能盲目要先回到安全位置再重新开始。def _handle_error(self): # 先抬起到安全高度 safe_pose self.get_safe_pose() self.move_to_pose_slowly(safe_pose) # 打开夹爪释放可能的残留工件 self.gripper.open() time.sleep(0.5) # 重试计数 self.retry_count 1 if self.retry_count self.max_retries: self.transition_to(GrabTaskState.APPROACH) else: # 超过重试次数停机报警 self.arm.stop() self.raise_alarm(抓取任务连续失败请检查)max_retries我一般设3次。超过3次说明不是偶然误差可能是工件位置变了或者夹爪有问题继续重试没意义。6. 调试过程中那些文档不会告诉你的坑6.1 机械臂上电后的热身问题Z1冷机状态和热机状态的关节零位会有微小漂移。我遇到过早上第一遍跑任务抓取很准跑了一个小时后开始偏0.5mm的情况。原因是关节温度升高后机械结构热膨胀导致零位变化。解决办法有两个一是上电后先让机械臂空跑5-10分钟等温度稳定了再开始正式任务二是在任务间隙定期做一次单点标定用标定结果修正坐标系变换。我一般采用第一种简单可靠。如果任务精度要求特别高就两种都用。6.2 网络交换机的选择Z1通过网口和上位机通信如果你用的是一台便宜的桌面交换机可能会遇到偶发的丢包。表现是机械臂偶尔抖一下但看日志又找不到规律。我换过三种交换机最后发现用带QoS的工业级交换机最稳。普通家用交换机在有大流量传输时比如同时传相机图像控制指令的优先级得不到保证延迟会波动。如果预算有限至少把控制网络和图像网络分开用两个网口或者两个交换机。6.3 急停按钮的接线逻辑Z1的急停按钮通常是常闭触点也就是说正常状态下电路是通的按下急停电路断开。这个逻辑在接线时要注意如果你的上位机通过检测电路通断来判断急停状态那断电时比如拔掉电源也会被判定为急停触发这是符合安全设计的。但我在调试时犯过一个错把急停信号接到了上位机的一个普通GPIO上没有做去抖。结果急停按钮的机械触点抖动导致上位机收到了几十次急停触发程序直接卡死在错误处理里。后来加了RC滤波和软件去抖才解决。class DebouncedInput: def __init__(self, pin, debounce_ms50): self.pin pin self.debounce_ms debounce_ms self.last_state None self.last_change_time 0 def read(self): raw self.pin.read() now time.time() * 1000 if raw ! self.last_state: if now - self.last_change_time self.debounce_ms: self.last_state raw self.last_change_time now return self.last_state6.4 轨迹文件的管理自动化任务通常需要保存多条轨迹比如不同工件的抓取路径。我建议把轨迹存成JSON或者YAML文件而不是硬编码在程序里。这样调整轨迹时不用重新编译改文件就行。# trajectories/pick_red_block.yaml name: pick_red_block approach: position: [0.3, 0.1, 0.2] orientation: [0, 0.707, 0, 0.707] descend: distance: 0.05 speed: 0.02 grasp: force: 10.0 lift: height: 0.1 place: position: [0.1, -0.2, 0.15] orientation: [0, 0.707, 0, 0.707]但要注意轨迹文件里的坐标是相对于工件坐标系的加载后要经过坐标系变换才能用。我见过有人直接把示教时的关节角度存下来回放结果换了工件位置就完全不对了。示教数据只能作为参考不能直接当轨迹用。6.5 日志记录的重要性自动化任务跑起来之后出问题是必然的。关键是出问题后能不能快速定位。我的做法是把每个控制周期的关键数据都记下来时间戳、目标位置、实际位置、目标力矩、实际力矩、状态机状态。数据量大的话可以降频记录比如每10个周期记一次。import csv class DataLogger: def __init__(self, filename): self.file open(filename, w, newline) self.writer csv.writer(self.file) self.writer.writerow([timestamp, state, q_target, q_actual, tau_target, tau_actual]) self.counter 0 def log(self, timestamp, state, q_target, q_actual, tau_target, tau_actual): self.counter 1 if self.counter % 10 0: # 降频记录 self.writer.writerow([timestamp, state, *q_target, *q_actual, *tau_target, *tau_actual])有了这些日志机械臂抖了、抓偏了、卡住了回放数据一看就知道是哪个环节的问题。没有日志的话只能靠猜。7. 从单任务到多任务调度把Z1接入产线节拍单次抓取跑通之后下一步通常是让它连续工作配合产线节拍。这时候要考虑的问题就不只是运动控制了还有任务队列、节拍同步、异常上报这些。我的做法是给Z1的控制程序加一个简单的任务队列上位机或者PLC通过一个轻量的TCP接口把任务指令发过来控制程序把任务排进队列按顺序执行。每个任务执行完后回传结果成功或者失败。import socket import json import queue class TaskServer: def __init__(self, executor, port9999): self.executor executor self.task_queue queue.Queue() self.server_socket socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.server_socket.bind((0.0.0.0, port)) self.server_socket.listen(1) def run(self): # 接收任务的线程 recv_thread threading.Thread(targetself._recv_loop) recv_thread.start() # 执行任务的线程 exec_thread threading.Thread(targetself._exec_loop) exec_thread.start() def _recv_loop(self): while True: conn, addr self.server_socket.accept() data conn.recv(4096) task json.loads(data.decode()) self.task_queue.put(task) conn.send(b{status: accepted}) conn.close() def _exec_loop(self): while True: task self.task_queue.get() result self.executor.execute(task) # 回传结果可以通过另一个socket或者写文件 self._report_result(result)这个架构的好处是解耦产线那边只管发任务不用关心机械臂怎么动机械臂这边只管执行不用关心任务从哪来。中间通过队列缓冲产线节拍快的时候任务排队慢的时候机械臂等待。节拍同步的关键是任务执行时间的可预测性。如果你的抓取任务有时候2秒完成有时候5秒完成产线就没法配合。所以要把任务时间尽量固定下来该等的地方就等不要为了快而牺牲稳定性。我一般会在任务末尾加一个固定的延时把总时间凑到一个整数值比如统一3秒一个循环。提示任务队列要有上限不能无限堆积。如果产线发任务的速度持续超过机械臂执行速度队列会越来越长延迟越来越大。设一个上限比如10个任务超过就拒绝并报警让产线知道机械臂跟不上了。8. 一些关于SDK版本和固件兼容性的经验Z1的SDK和固件是配套的版本不匹配会出现各种奇怪的问题。我遇到过SDK里有的接口固件不支持调用后返回一个模糊的错误码也遇到过固件升级后原来的指令格式变了机械臂收到指令后不动。我的建议是升级固件之前先确认SDK版本。如果SDK没有同步更新宁可先不升固件。另外每次升级后都要重新跑一遍基础功能测试单关节运动、笛卡尔运动、状态回传、急停响应。这四个测试过了再跑完整任务。还有一点SDK里的示例代码通常是最可靠的参考。遇到接口不会用先看示例怎么调的比看文档快。示例代码里的参数值也是经过验证的可以直接拿来当初始值再根据实际情况微调。最后说一个关于实时性的问题。如果你用的是Linux系统可以通过设置线程优先级和CPU亲和性来提升控制线程的实时性。把控制线程绑到一个独立的CPU核心上避免和其他线程抢资源。Windows下也有类似的办法但效果不如Linux明显。如果任务对实时性要求极高建议用实时Linux内核。# Linux下设置CPU亲和性示例 taskset -c 2 ./arm_control这个命令把arm_control进程绑到CPU核心2上。如果控制线程和接收线程要分开可以在代码里用pthread_setaffinity_np分别绑定。整个Z1的二次开发从SDK接口调用到自动化任务落地核心就三件事通信要稳、坐标要对、异常要兜。通信稳了机械臂才听话坐标对了抓取才准异常兜住了任务才能连续跑。这三件事做好剩下的就是根据具体场景调参数了。