LeKiwi 工作流程
完整工作循环
┌─────────────────────────────────────────────────────────────────┐
│ 系统启动 │
│ python -m src.main │
└─────────────────────────────────────────────────────────────────┘
│
▼
┌─────────────────────────────────────────────────────────────────┐
│ 1. 初始化 │
│ - init_app() 加载 config.yaml │
│ - init_robot() 创建 LeKiwi 实例 │
│ - robot.connect() 连接硬件，执行校准（如需要） │
│ - get_observation() 获取初始关节角度 │
└─────────────────────────────────────────────────────────────────┘
│
▼
┌─────────────────────────────────────────────────────────────────┐
│ 2. 机械臂归位 │
│ - return_to_start_position() │
│ - 移动到预设位置 (robot_setup.py:5-12) │
└─────────────────────────────────────────────────────────────────┘
│
▼
┌───────────────────┐
│ 主循环 while │
│ True │
└───────────────────┘
│
┌───────────────┼───────────────┐
▼ ▼ ▼
┌───────────┐ ┌───────────┐ ┌───────────┐
│ SEARCH │ │ PICK │ │FIND_BUCKET│
└───────────┘ └───────────┘ └───────────┘
│ │ │
│ │ │
▼ ▼ ▼
┌───────────────────────────────────────────────┐
│ 目标检测 (YOLO) │
│ - yolo_infer() 找球 │
│ - get_black_bucket() 找黑桶 │
│ - get_red_bucket() 找红桶（备选） │
└───────────────────────────────────────────────┘
│
▼
┌───────────────────────────────────────────────┐
│ 底盘移动控制 │
│ - move_controller() 搜索目标 │
│ - move_controller_for_bucket() 跟随桶 │
└───────────────────────────────────────────────┘
│
▼
┌───────────────────────────────────────────────┐
│ 发送控制指令 │
│ - robot.send_action({**arm_action, **move}) │
└───────────────────────────────────────────────┘
状态详解
SEARCH（搜索状态）
职责：寻找球
SEARCH:
├─ 检测: yolo_infer(frame) → 找球
├─ 控制: move_controller(direction, result)
│ ├─ 球在左侧 → 旋转向左
│ ├─ 球在右侧 → 旋转向右
│ ├─ 球太小 → 前进
│ ├─ 球太大 → 后退
│ └─ 球居中且大小合适 → 稳定计数
└─ 稳定10帧 → 切换到 PICK
PICK（捡球状态）
职责：执行捡球动作序列
PICK:
├─ 控制器: p_control_loop(CATCH_ACTION[step])
├─ 动作序列执行:
│ 1. move_to(0.0989, 0.125) # 移动到初始位置
│ 2. shoulder_pan(-12) # 调整肩部角度
│ 3. gripper(60) # 张开夹爪
│ 4. wrist_flex(80) # 弯曲腕部
│ 5. move_to(0.140, 0.1211) # 移动到球上方
│ 6. move_to(0.140, -0.05) # 下降到球位置
│ 7. gap() # 停顿
│ 8. gripper(-60) # 闭合夹爪（夹球）
│ 9. gap() # 停顿
│ 10. shoulder_pan(12) # 肩部归位
│ 11. move_to(-0.1, 0.2) # 举起球
│ 12. wrist_flex(-20) # 调整腕部
└─ 序列完成 → 切换到 FIND_BUCKET
FIND_BUCKET（寻找桶状态）
职责：导航到桶的位置
FIND_BUCKET:
├─ 检测: get_black_bucket_local(frame)
│ └─ 找不到 → get_red_bucket_local(frame)
├─ 检查: gripper_pos > 25 ?
│ ├─ 否（球丢失）→ reset_robot() → SEARCH
│ └─ 是（球还在）→ 继续
├─ 控制: move_controller_for_bucket(direction, result)
│ ├─ 桶在左侧 → 旋转向左
│ ├─ 桶在右侧 → 旋转向右
│ ├─ 桶太小 → 前进
│ ├─ 桶太大 → 后退
│ └─ 桶居中且大小合适 → 稳定计数
└─ 稳定10帧 → 切换到 PUT_BALL
PUT_BALL（放球状态）
职责：执行放球动作序列
PUT_BALL:
├─ 控制器: p_control_loop(PUT_ACTION[step])
├─ 动作序列执行:
│ 1. shoulder_lift(50) # 抬起机械臂
│ 2. gap() # 停顿
│ 3. gripper(60) # 张开夹爪（放球）
│ 4. gap() # 停顿
│ 5. move_to(-0.1, 0.2) # 收回机械臂
│ 6. gripper(-60) # 闭合夹爪
└─ 序列完成 → reset_robot() → SEARCH
捡球动作详解
坐标系统
机械臂工作空间坐标（单位：米）：
y (0.22)
│
│
│
(-0.22)──┼───────────────── x
│
│
│
y (-0.15)
注意：x 范围 -0.22 ~ 0.22，y 范围 -0.15 ~ 0.22
逆运动学控制
文件: arm_inverse_controller.py
def inverse_kinematics(x, y, l1=0.1159, l2=0.1350):
"""
计算二维机械臂逆运动学
x, y: 末端执行器目标坐标
l1: 上臂长度 0.1159m
l2: 下臂长度 0.1350m
返回: (肩部角度, 肘部角度)
"""
P控制参数
MOTION_PARAMS = {
'max_step': 0.01, # 最大步长 1cm/帧
'min_step': 0.001, # 最小步长 1mm/帧
'slow_dist': 0.05, # 5cm 内开始减速
'dead_zone': 0.0005 # 0.5mm 内认为到达
}
夹爪控制
| 动作 | 值 | 说明 |
|---|---|---|
| 张开 | +60 | 夹爪打开到 60° |
| 闭合 | -60 | 夹爪闭合，夹住物体 |
| 检测 | >25 | 夹爪角度大于 25° 认为夹住物体 |
视觉目标检测
检测区域
摄像头画面中央的目标区域：
┌────────────────────────────────────────┐
│ (0,0) │
│ ┌───────────────────────────────┐ │
│ │ │ │
│ │ ┌───────────────┐ │ │
│ │ │ TARGET ZONE │ │ │ ← 球/桶需要在这个区域
│ │ │ │ │ │
│ │ └───────────────┘ │ │
│ │ │ │
│ └───────────────────────────────┘ │
│ (640, 480) │
└────────────────────────────────────────┘
检测参数
TARGET_POSITION = max(target_w, target_h) # 约 160 像素
TARGET_CX = left + target_w // 2 # 约 245 + 75 = 320
TARGET_CY = top + target_h // 2 # 约 160 + 80 = 240
目标大小判断
| 目标在画面中的大小 | 判断 | 机器人动作 |
|---|---|---|
max(w,h) < TARGET_POSITION - 10 | 太远 | 前进 |
max(w,h) > TARGET_POSITION + 10 | 太近 | 后退 |
| 居中且大小合适 | 到位 | 等待稳定 |
异常处理
球丢失检测
# main.py:76-83
if get_robot_status() == RobotStatus.FIND_BUCKET:
gripper_pos = current_obs.get('arm_gripper.pos', 5)
is_gripper_holding = gripper_pos > 25
if not is_gripper_holding:
set_robot_status(RobotStatus.SEARCH)
reset_robot()
当检测到夹爪角度小于 25° 时，认为球已丢失，返回 SEARCH 状态重新寻找。
找不到目标
# move_controller.py:58-66
if not result: # 没有检测到目标
if _last_ball_center_x is not None:
# 向最后看到目标的方向旋转
action = direction.get_action("rotate_left/right")
else:
action = direction.get_action(None) # 停止
机器人会记住最后看到目标的位置，并朝那个方向旋转继续搜索。
完整流程时序图
时间轴
│
▼
SEARCH ────────────────────────────────────────────────────────
│ │
│ [检测到球] │
│ │
▼ │
PICK │ │
│ │
├─ move_to(0.0989, 0.125) [机械臂移动] │
├─ shoulder_pan(-12) [调整角度] │
├─ gripper(60) [张开夹爪] │
├─ move_to(0.140, 0.1211) [下降到球上方] │
├─ move_to(0.140, -0.05) [接触球] │
├─ gripper(-60) [夹住球] │
├─ shoulder_pan(12) [抬起] │
├─ move_to(-0.1, 0.2) [举起球] │
│ │
▼ │
FIND_BUCKET │ │
│ │
│ [找桶导航] │
│ │
▼ │
PUT_BALL │ │
│ │
├─ shoulder_lift(50) [抬起臂] │
├─ gripper(60) [张开放球] │
├─ move_to(-0.1, 0.2) [收回] │
├─ gripper(-60) [闭合夹爪] │
│ │
▼ │
SEARCH ────────────────────────────────────────────────────────