GPT-6 Astra 接宇树 G1 做厨房整理实战

跑通之后是什么样

先说终点，再倒推过程。整套系统做完，你会得到这样一个工作流：

你站在厨房门口说一句“把台面收拾一下”。相机拍一张台面图，多模态大模型（本文用 GPT-6 Astra 指代这类视觉-语言模型，具体接口、模型名与调用方式以官方文档当前版本为准）看图后输出一份结构化任务清单：有哪些物品、每件物品在画面中的像素坐标、该放到哪个柜子。这份清单经过 JSON 校验和安全校验后，交给运动规划模块算出抓取位姿与轨迹。轨迹在下发前必须穿过一层“安全围栏”：工作空间盒、速度上限、力上限、超时看门狗、人进入区域就减速或停止。最后 G1 以低速完成抓取、搬运、放置。

任何一环出问题——模型输出幻觉物品、坐标算错、网络卡住、有人走进来——系统进入阻尼停止状态，而不是继续挥动手臂。

整体分层：

```text

[RGB-D 相机] ─┐

├─> [视觉感知/大模型规划] ─> [几何+运动规划] ─> [安全围栏] ─> [G1 控制器]

[深度图] ─┘ ^

|

[物理急停 / 安全光栅 / 现场监护人]

```

关键设计原则只有一条：大模型只负责“语义和大致选点”，不负责关节角度。所有低层数值都要经过几何计算和围栏裁剪。

前置条件清单

硬件

• 宇树 G1 或同系列人形机器人，配套官方 SDK 与调试工具，接口以官方文档为准
• RGB-D 相机一台（固定在台面斜上方，视线覆盖整个工作区）
• 二指或多指夹爪，支持力控或电流阈值反馈
• 独立的安全回路：物理急停按钮（必须能在断电状态下生效）、接触式保险杠或安全光栅
• 边缘计算主机一台，带独立显卡；算力需求以你所选模型的官方说明为准
• 地面标线胶带、台面固定夹具、至少一个现场专职监护人

软件

• Python 3.x、NumPy、OpenCV、jsonschema
• 机器人官方 SDK 的 Python 绑定（或 ROS 2 驱动包）
• 大模型 API 的访问凭证（放在环境变量里，不要写进代码）
• 仿真环境（Isaac Sim / MuJoCo / Gazebo 任一，以官方文档为准）

场地

• 空旷房间，台面四周 1.5 米内无杂物
• 刀具、玻璃器皿、明火、热液体、电线在调试阶段全部移出工作区
• 网络使用局域网直连，避免公网抖动导致指令中断

第 1 步：先写安全围栏，再写任何功能

顺序不能反。功能代码可以慢慢补，围栏必须从一开始就存在，否则后面每次调试都在赌运气。

安全围栏分四类：空间围栏（工作空间盒）、运动围栏（速度、加速度上限）、力围栏（夹爪力、腕部力矩）、时间围栏（指令超时、任务总时长）。

配置写成 YAML，方便现场改：

```yaml

safety_limits.yaml

workspace_box:

x: [0.15, 0.75]

y: [-0.45, 0.45]

z: [0.05, 1.30]

velocity:

tcp_max_mps: 0.10

joint_max_radps: 0.6

force:

gripper_max_n: 8.0

wrist_torque_max_nm: 3.0

timing:

cmd_timeout_s: 0.5

max_task_duration_s: 300

human_zone:

stop_distance_m: 1.2

slow_distance_m: 2.5

```

护栏代码：

```python

safety_guard.py

import time

import yaml

class SafetyGuard:

"""所有动作指令的唯一出口。任何模块都不允许绕过它直接调用机器人。"""

def __init__(self, robot, cfg_path="safety_limits.yaml", reset_token="现场复位口令"):

self.robot = robot

self.cfg = yaml.safe_load(open(cfg_path, encoding="utf-8"))

self.reset_token = reset_token

self.estopped = False

self.last_cmd_ts = time.time()

self.task_start_ts = None

def estop(self, reason: str):

if not self.estopped:

print(f"[ESTOP] {reason}")

self.estopped = True

self.robot.damp() # 进入阻尼/软停，实际接口以官方 SDK 为准

def clear_estop(self, token: str):

急停复位只能由现场人员手动完成，程序不得自动复位

if token != self.reset_token:

raise PermissionError("急停复位需要现场人工确认")

self.estopped = False

self.last_cmd_ts = time.time()

def check_pose(self, xyz):

box = self.cfg["workspace_box"]

for axis, val in zip("xyz", xyz):

lo, hi = box[axis]

if not (lo <= val <= hi):

raise ValueError(f"目标点 {axis}={val:.3f} 超出工作空间 [{lo}, {hi}]")

return True

def clamp_speed(self, v_mps: float) -> float:

return min(abs(v_mps), self.cfg["velocity"]["tcp_max_mps"])

def check_force(self, measured_n: float):

if measured_n > self.cfg["force"]["gripper_max_n"]:

raise RuntimeError(f"夹爪受力 {measured_n:.1f}N 超限，疑似夹到硬物")

def check_human(self, distance_m: float):

hz = self.cfg["human_zone"]

if distance_m <= hz["stop_distance_m"]:

self.estop("人员进入停止区")

return "stop"

if distance_m <= hz["slow_distance_m"]:

return "slow"

return "run"

def watchdog(self):

if time.time() - self.last_cmd_ts > self.cfg["timing"]["cmd_timeout_s"]:

self.estop("指令超时，疑似通讯中断")

def move(self, pose, speed_mps):

if self.estopped:

raise RuntimeError("急停状态下拒绝执行动作")

self.check_pose(pose["xyz"])

self.last_cmd_ts = time.time()

self.robot.move_to(pose, self.clamp_speed(speed_mps))

```

一句话记住：急停只能人工复位，程序没有自动复位的权限。

第 2 步：打通机器人 SDK，先只做小幅动作

不要一上来就抓杯子。先让机器人站起来、读取关节状态、在围栏限制下做小幅摆臂，确认通讯链路稳定。

```python

g1_bringup.py

import time

from safety_guard import SafetyGuard

class RobotIO:

"""适配层：把官方 SDK 的方法包成统一接口。

真实函数名、初始化参数、使能流程以官方文档为准。"""

def __init__(self):

self.status = "idle"

def connect(self):

这里对接官方 SDK：初始化通道、握手、查询固件状态

self.status = "connected"

def damp(self):

self.status = "damped"

def read_joint_state(self):

返回关节位置/速度/力矩列表

return {"q": [], "dq": [], "tau": []}

def move_to(self, pose, speed_mps):

关节空间或笛卡尔空间运动，内部必须调用官方限速接口

pass

if __name__ == "__main__":

bot = RobotIO()

bot.connect()

guard = SafetyGuard(bot)

print("关节状态:", bot.read_joint_state())

低速小幅动作，验证通讯与控制

test_pose = {"xyz": [0.35, 0.0, 1.0], "quat": [0, 0, 0, 1]}

guard.move(test_pose, speed_mps=0.05)

time.sleep(1.0)

guard.estop("联调结束，主动停止")

```

联调检查项：拔掉网线看是否触发超时停止；按下物理急停看是否切断动力；松开急停后是否必须手动复位才能继续。

第 3 步：相机标定与手眼标定

这一步不做，后面的坐标全是错的。

内参标定用棋盘格或 ArUco 板，得到相机内参矩阵 K。外参标定（eye-to-hand）求相机坐标系到机器人基座坐标系的变换矩阵 T_base_cam。标定板固定在机器人末端、或者固定在工作台上让机器人末端去触碰若干已知点，两种方式都行，取多点做最小二乘。

```python

pixel_to_base.py

import numpy as np

def pixel_to_base(u, v, depth_m, K, T_base_cam):

"""把像素坐标 + 深度还原成机器人基座坐标系下的三维点。"""

fx, fy = K[0, 0], K[1, 1]

cx, cy = K[0, 2], K[1, 2]

x_c = (u - cx) * depth_m / fx

y_c = (v - cy) * depth_m / fy

z_c = depth_m

p_cam = np.array([x_c, y_c, z_c, 1.0])

p_base = T_base_cam @ p_cam

return p_base[:3]

```

验证方法：把一个物块依次放在工作区三个不同位置，用视觉算出坐标，再用示教方式让末端点过去，比对偏差。偏差稳定在 1 厘米内再往下走；不稳定就回头检查深度图有效范围和标定点的分布。

第 4 步：视觉-动作接口——让模型只吐结构化 JSON

这是整套系统的核心接口设计。四条规则：

1. 模型只输出 JSON，不输出任何解释文字；

2. 模型只给像素坐标和语义标签，不给关节角、不给轨迹；

3. 输出必须通过 JSON Schema 校验才能进入下一步；

4. 刀具、玻璃、明火、液体、电线类物品一律要求人工确认。

提示词示例：

```text

你是厨房整理任务的规划器。输入是一张俯视 RGB 图和对应的深度图。

只输出 JSON，不要输出任何解释文字。

输出格式：

{"task": "...", "items": [{"id": "...", "label": "...", "pixel": [u, v],

"depth_m": 0.0, "grasp_hint": "...", "confidence": 0.0}],

"targets": [{"item_id": "...", "destination": "..."}],

"uncertain": [], "need_human_confirm": false}

规则：

1. 只报告你确实看到、且该像素处深度值有效的物品；

2. pixel 是该物品抓取点在图像中的像素坐标；

3. confidence 低于 0.6 的物品放进 uncertain 列表；

4. 不要输出关节角度、不要输出轨迹、不要输出速度；

5. 遇到刀具、玻璃器皿、明火、液体、电线类物品，need_human_confirm 置为 true；

6. 不确定的物品宁可放进 uncertain，也不要猜。

```

解析与校验：

```python

vlm_planner.py

import json

import os

import jsonschema

SCHEMA = {

"type": "object",

"required": ["task", "items", "targets"],

"properties": {

"task": {"type": "string"},

"items": {

"type": "array",

"items": {

"type": "object",

"required": ["id", "label", "pixel", "depth_m", "confidence"],

"properties": {

"id": {"type": "string"},

"label": {"type": "string"},

"pixel": {"type": "array", "items": {"type": "number"},

"minItems": 2, "maxItems": 2},

"depth_m": {"type": "number", "exclusiveMinimum": 0},

"grasp_hint": {"type": "string"},

"confidence": {"type": "number", "minimum": 0, "maximum": 1},

},

},

},

"targets": {"type": "array"},

"uncertain": {"type": "array"},

"need_human_confirm": {"type": "boolean"},

},

}

BLOCKLIST = {"刀", "菜刀", "水果刀", "玻璃杯", "热水壶", "电饭煲电源线"}

def plan_from_image(image_b64: str, prompt: str) -> dict:

调用多模态模型接口，具体请求格式、字段名、鉴权方式以官方文档为准

raw = call_vlm_api(

model=os.environ["VLM_MODEL"],

image=image_b64,

prompt=prompt,

)

data = json.loads(raw)

jsonschema.validate(data, SCHEMA)

safe_items = []

for item in data["items"]:

if item["confidence"] < 0.6:

continue

if any(word in item["label"] for word in BLOCKLIST):

data["need_human_confirm"] = True

continue

safe_items.append(item)

data["items"] = safe_items

return data

```

call_vlm_api 需要你自己按官方文档实现，包含超时、重试上限和失败降级（降级到规则表或直接报错停下，不要静默继续）。

第 5 步：从抓取点到动作序列

运动规划用现成的 IK 求解器，不要手写。每条抓取任务拆成固定的动作序列：

```text

预抓取位姿（物品上方 12cm）

→ 直线下降到抓取位姿

→ 闭合夹爪（力阈值监控）

→ 抬起 12cm

→ 移动到目标容器上方

→ 下降放置

→ 松开

→ 撤退到安全位

```

状态机骨架：

```python

pick_place.py

from safety_guard import SafetyGuard

from pixel_to_base import pixel_to_base

STEPS = ["approach", "descend", "grasp", "lift",

"transport", "place", "release", "retreat"]

def execute_pick(guard, robot, ik_solver, item, K, T_base_cam, target_xyz):

u, v = item["pixel"]

obj_xyz = pixel_to_base(u, v, item["depth_m"], K, T_base_cam)

poses = {

"approach": {"xyz": obj_xyz + [0, 0, 0.12]},

"descend": {"xyz": obj_xyz},

"grasp": {"xyz": obj_xyz},

"lift": {"xyz": obj_xyz + [0, 0, 0.12]},

"transport":{"xyz": target_xyz + [0, 0, 0.15]},

"place": {"xyz": target_xyz + [0, 0, 0.02]},

"release": {"xyz": target_xyz + [0, 0, 0.02]},

"retreat": {"xyz": target_xyz + [0, 0, 0.20]},

}

for step in STEPS:

guard.watchdog()

guard.check_pose(poses[step]["xyz"])

robot.move_to(poses[step], speed_mps=0.08)

if step == "grasp":

f = robot.read_gripper_force()

guard.check_force(f) # 受力异常直接抛错，进急停

return True

```

注意每个动作之后都要调用 watchdog()。抓取失败不要原地重试超过两次，直接汇报给人，硬试是事故的常见来源。

第 6 步：端到端串起来

主循环里，人和机器人的分工是：人给自然语言指令、人确认一次计划、人守着急停；机器人负责执行。

```python

kitchen_loop.py

from safety_guard import SafetyGuard

from vlm_planner import plan_from_image

from pick_place import execute_pick

def main():

robot = RobotIO(); robot.connect()

guard = SafetyGuard(robot)

K, T_base_cam = load_calibration()

ik_solver = build_ik_solver()

while True:

human_distance = read_human_distance() # 来自光栅或视觉检测

state = guard.check_human(human_distance)

if state == "stop":

break

image = capture_rgbd()

plan = plan_from_image(image, PROMPT)

if plan.get("need_human_confirm"):

if not ask_operator("存在需要人工确认的物品，是否继续？"):

continue

print("计划：", plan)

if not ask_operator("确认执行？"):

continue

for item in plan["items"]:

dest = lookup_destination(plan["targets"], item["id"])

execute_pick(guard, robot, ik_solver, item, K, T_base_cam,

destination_xyz(dest))

if __name__ == "__main__":

main()

```

急停联调清单：断网是否停；按急停是否立即进阻尼；复位后是否必须人工确认；人员在停止区内机器人是否拒绝执行；模型 API 超时后机器人是否停在安全位而不是停在半空。

常见坑与排错

下一步建议

• 从单件到批量：加任务队列和优先级，先做“单件抓放”稳定性，再扩到多件。
• 加语音入口：语音转文字后仍走同一套计划与校验流程，不要给语音指令开后门。
• 记录与回放：把每次的 RGB-D 图、模型原始输出、执行轨迹、异常事件存下来，这是后续调参和复盘的基础。
• 引入力控与柔顺控制：接触式任务（插、推、擦）只靠位置控制很难做稳。
• 多模态反馈闭环：执行后再拍一张图，让模型判断任务是否完成，形成“感知—执行—复核”。
• 人机共处安全：涉及人员进出的场景，参考当地劳动安全规范与厂商安全手册，必要时引入安全 PLC 与认证光栅。
• 把围栏做成通用件：同一套 SafetyGuard 换个配置文件，就能复用到机械臂、移动底盘等其它场景。

最后提醒一句：涉及人形机器人与人共处的工作，安全责任在部署方。做任何实机调试前，先读一遍厂商的安全手册与当地法规，涉及参数与接口的部分，以官方文档当前版本为准。