用Unitree Go1的Camera SDK和PaddlePaddle，5步搭建一个跟随demo

用Unitree Go1的Camera SDK和PaddlePaddle实现智能跟随机器人

在机器人开发领域，将视觉感知与运动控制相结合一直是令人兴奋的方向。Unitree Go1作为一款高性能四足机器人，其开放的Camera SDK和强大的硬件平台为开发者提供了广阔的创新空间。本文将带你使用PaddlePaddle深度学习框架，在Go1上实现一个完整的视觉跟随系统，从图像采集到运动控制形成闭环。

1. 开发环境快速配置

对于已经完成基础设置的开发者，我们可以精简环境准备流程。首先确保你的Go1主控Nano已连接网络，并通过SSH远程访问：

ssh unitree@你的NanoIP地址

建议使用Python虚拟环境管理项目依赖：

python3 -m venv go1_follow source go1_follow/bin/activate

安装必要的Python包时，使用国内镜像源加速：

pip install -i https://pypi.tuna.tsinghua.edu.cn/simple paddlepaddle-gpu==2.4.2 opencv-python numpy

提示：Go1的Nano采用ARM架构，需安装对应版本的PaddlePaddle，官方提供了Jetson平台的预编译包。

验证Camera SDK是否正常工作：

cd ~/Unitree/sdk/UnitreeCameraSdk ./bins/example_putImagetrans_0

如果看到实时图像输出，说明摄像头驱动已就绪。

2. 实时图像采集与处理

Unitree Camera SDK提供了简洁的Python接口获取图像流。创建一个camera_reader.py文件：

import cv2 from unitree_camera_sdk import CameraSDK camera = CameraSDK(device_num=0) # 使用主摄像头 frame = camera.getFrame() if frame is not None: cv2.imshow("Go1 Camera", frame) cv2.waitKey(1)

为提升处理效率，建议设置适当的分辨率和帧率：

camera.setResolution(640, 480) # 640x480是目标检测的常用分辨率 camera.setFPS(30) # 根据处理能力调整

图像预处理对模型性能影响显著，典型的处理流程包括：

1. 色彩空间转换：BGR转RGB
2. 归一化：像素值缩放到[0,1]范围
3. 通道顺序调整：HWC转CHW
4. 批量维度添加：增加batch维度

def preprocess(img): img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) img = img.astype('float32') / 255.0 img = np.transpose(img, [2, 0, 1]) # HWC -> CHW img = np.expand_dims(img, axis=0) # 添加batch维度 return img

3. 轻量级目标检测模型部署

PaddlePaddle的PaddleDetection项目提供了多种适合边缘设备的预训练模型。我们选择PP-YOLO Tiny作为基础模型：

import paddle from paddle.inference import Config from paddle.inference import create_predictor model_dir = "ppyolo_tiny_650e_coco" # 下载并解压模型文件 config = Config(model_dir+"/model.pdmodel", model_dir+"/model.pdiparams") config.enable_use_gpu(100, 0) # 初始化GPU predictor = create_predictor(config)

模型输入输出处理：

input_names = predictor.get_input_names() input_handle = predictor.get_input_handle(input_names[0]) output_names = predictor.get_output_names() output_handle = predictor.get_output_handle(output_names[0]) def infer(img): input_handle.copy_from_cpu(img) predictor.run() results = output_handle.copy_to_cpu() return results

处理模型输出，提取目标框信息：

def parse_results(results, threshold=0.5): boxes = results[:, :4] # 坐标(x1,y1,x2,y2) scores = results[:, 4] # 置信度 valid = scores > threshold return boxes[valid], scores[valid]

4. 从视觉到运动的控制转换

实现跟随功能的核心是计算目标在图像中的位置偏差，并转换为运动指令。定义简单的控制策略：

def get_control_command(box, img_width): x_center = (box[0] + box[2]) / 2 offset = (x_center - img_width/2) / (img_width/2) # 归一化偏移量[-1,1] if abs(offset) < 0.1: # 死区阈值 return "stop" elif offset > 0: return "right" # 目标在右侧，机器人需右转 else: return "left" # 目标在左侧，机器人需左转

将控制指令发送给Go1的运动控制系统：

from unitree_legged_sdk import HighCmd, HighLevelSDK udp = HighLevelSDK() cmd = HighCmd() def send_command(action): if action == "left": cmd.yawSpeed = 0.5 # 左转速度 elif action == "right": cmd.yawSpeed = -0.5 # 右转速度 else: cmd.yawSpeed = 0.0 udp.sendCmd(cmd)

5. 系统集成与性能优化

将各模块整合成完整流程：

def main_loop(): camera = CameraSDK(device_num=0) while True: frame = camera.getFrame() if frame is None: continue img = preprocess(frame) results = infer(img) boxes, scores = parse_results(results) if len(boxes) > 0: action = get_control_command(boxes[0], frame.shape[1]) send_command(action) # 显示结果 vis_frame = visualize(frame, boxes, scores) cv2.imshow("Follow Demo", vis_frame) if cv2.waitKey(1) == 27: # ESC退出 break

性能优化技巧：

1. 模型量化：使用PaddleSlim对模型进行INT8量化，提升推理速度

   pip install paddleslim

2. 多线程处理：将图像采集与模型推理放在不同线程

   from threading import Thread import queue image_queue = queue.Queue(maxsize=1) def capture_thread(): while True: frame = camera.getFrame() if frame is not None: if image_queue.empty(): image_queue.put(frame)

3. 运动平滑：对控制指令进行低通滤波，避免机器人抖动

   class SmoothController: def __init__(self, alpha=0.2): self.alpha = alpha self.last_cmd = None def smooth(self, cmd): if self.last_cmd is None: self.last_cmd = cmd else: self.last_cmd = self.alpha*cmd + (1-self.alpha)*self.last_cmd return self.last_cmd

6. 进阶功能扩展

基础跟随功能实现后，可以考虑以下增强功能：

• 多目标跟踪：为每个检测到的目标分配唯一ID，实现特定目标跟踪

  from collections import defaultdict tracker = defaultdict(lambda: {'age':0, 'position':None}) def update_tracker(boxes, scores): for box, score in zip(boxes, scores): # 简单的基于IOU的跟踪 matched = False for id in tracker: if calculate_iou(box, tracker[id]['position']) > 0.5: tracker[id]['position'] = box tracker[id]['age'] += 1 matched = True break if not matched: new_id = len(tracker) tracker[new_id] = {'age':1, 'position':box} # 清理长时间未更新的目标 for id in list(tracker.keys()): if tracker[id]['age'] > 10: # 10帧未更新 del tracker[id]

• 深度估计：结合目标大小变化估计距离，实现速度控制

  def estimate_distance(box_size, known_width=0.2, focal_length=600): # known_width: 目标的实际物理宽度(米) # focal_length: 相机焦距(像素) return (known_width * focal_length) / box_size

• 异常处理：增加丢失目标后的搜索行为

  class SearchBehavior: def __init__(self): self.search_direction = 1 # 1:右转, -1:左转 self.search_speed = 0.3 self.search_time = 0 def execute(self, cmd): cmd.yawSpeed = self.search_direction * self.search_speed self.search_time += 1 if self.search_time > 30: # 30帧后切换方向 self.search_direction *= -1 self.search_time = 0

7. 实际部署注意事项

在真实机器人上运行时，有几个关键点需要注意：

1. 安全第一：确保测试环境开阔，避免碰撞

   • 设置紧急停止开关
   • 限制最大运动速度

   MAX_YAW_SPEED = 1.0 # rad/s cmd.yawSpeed = np.clip(cmd.yawSpeed, -MAX_YAW_SPEED, MAX_YAW_SPEED)

2. 电源管理：持续运行时的功耗监控

   jtop # 监控GPU和CPU使用率

3. 延迟测试：测量从图像采集到运动执行的总延迟

   import time start_time = time.time() # ...处理流程... latency = time.time() - start_time print(f"Total latency: {latency*1000:.2f}ms")

4. 光照适应：不同光照条件下的鲁棒性处理

   def adaptive_exposure(frame): gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) mean_val = np.mean(gray) if mean_val < 50: # 图像太暗 camera.setExposure(camera.getExposure() + 1) elif mean_val > 200: # 图像过曝 camera.setExposure(camera.getExposure() - 1)

通过这个项目，你不仅能够掌握机器人视觉控制的基本原理，还能深入理解边缘计算中模型部署的优化技巧。Unitree Go1的灵活性和PaddlePaddle的强大功能相结合，为更复杂的机器人应用开发奠定了坚实基础。