▲ 点击播放查看完整功能演示

MYGO · 开源报告会

MyGo

基于 ROS 2 的半自主移动操作机器人

底盘运动 · 五轴机械臂 · 视觉锁定 · 能量机关全流程自主操作

ROS 2 Jazzy C++ 17 OpenCV 4.x MoveIt 2 嵌入式 2026年5月

MyGo 团队视觉负责人 · Harekasa

🤖

mygo_ros
ROS 2 上位机

📷

mygo_via_maixcdk
MaixCDK 视觉模块

🎯

MygoVison
PC 仿真环境

项目概述

面向 RoboMaster 竞赛场景的半自主移动操作平台，实现底盘运动、机械臂抓取、能量机关收集与提交等复杂任务。

履带式差分驱动底盘 + 5-DOF 总线舵机机械臂 + 双旋杆 + 可抬升小球收集装置
ROS 2 Jazzy 分布式架构，Raspberry Pi 4B + STM32F103ZET6(底盘) + STM32F103C8T6(机械臂/车载)
USB 模拟串口连接，开机自启 + 串口绑定自动识别，类《终末地》派状 UI 操控
独立视觉模块，PC端 OpenCV 仿真 → MaixCDK 交叉编译 → MaixCam2 部署

ROS 2 节点话题拓扑图

🔗 github.com/AndyYang12345/mygo_ros

系统架构设计

五层解耦：手柄 → 输入抽象 → 核心决策 → 硬件驱动 → 嵌入式执行 | 视觉模块独立运行，TCP 网络桥接

输入

🎮 Xbox 手柄

sensor_msgs/Joy

joystick_parser

JoystickIntent
ButtonIntent
ComboIntent

/joy/parsed → /ui/menu_nav →

决策

🧠 state_machine

8 主状态 + 14+ 子状态
FSM · 派状 UI

🖥️ Qt5 UI

局域网可视化控制
状态监测面板

/cmd/chassis/velocity → /cmd/arm/pose_target → /cmd/arm/joint_target → /cmd/gripper/command →

驱动

chassis_serial

差速解算
#0.23,0.33

arm_serial

MoveIt2 路径解析
舵机角度打包

pole_serial

扳机 → 旋转

collector_serial

双旋杆控制

vision_tcp_bridge

TCP ↔ 话题
树莓派中转

USB 模拟串口 (termios) ▸ TCP 网络协议 ▸

下位机

STM32F103ZET6

底盘电机驱动
编码器采集

STM32F103C8T6

机械臂舵机
车载机构控制

视觉

📷 MaixCam2

MaixCDK 固件
色块识别 · 追踪

🖥️ PC 仿真环境

OpenCV TargetSim
遗传算法 PID 训练

ROS 2 节点硬件驱动 & 下位机视觉模块核心决策

🎯 分层解耦

每层独立可测可替换，数据通过标准化接口传递。视觉模块独立运行，通过 TCP 经树莓派中转到主控

🔄 状态驱动

FSM 管理全部模式，状态互斥、转换有序，非法跳转直接拒绝

🛡️ 安全 + 自动化

IDLE 防误触 + EMERGENCY 急停 + 开机自启 + 串口绑定乱序自动识别

🤖

mygo_ros
ROS 2 上位机完整源码

ROS 2 核心模块详解

🧠 有限状态机 (FSM) 设计

State 基类 + 纯虚函数 + 状态指针切换 → 回调中根据当前状态激活正确逻辑

IDLE

默认启动

MENU

派状 UI

CHASSIS

8 子模式

ARM

MoveIt2

POLE

回收杆

VISION

视觉辅助

EMERG

急停

🎮 输入抽象层

JoystickIntent：死区检测 + NORMAL/CENTERED/EDGE
ButtonIntent：按下/释放边缘触发
ComboIntent：双扳机确认（模仿终末地）
TriggerIntent：LT/RT 模拟量 0~1

🔄 状态生命周期

class State {
  virtual OnEnter()  = 0;  // 进入提示、初始化
  virtual OnExit()   = 0;  // 清理、保存状态
  virtual OnUpdate() = 0;  // 每帧逻辑
};

主节点维护 State* 指针 + 所有状态实例，切换时修改指针指向 → 订阅回调自动路由到当前状态

控制数据流：从手柄到电机

🎮 摇杆坐标

→

joystick_parser

语义化 Intent

→

state_machine

按状态路由

→

Twist 速度

→

chassis_serial

差速映射

→

USB 串口

#0.23,0.33

→

STM32

电机 PWM

CHASSIS 子模式（LB + 右摇杆切换）

HOME→ NORMAL→ CYL_SUBMIT→ CUBE_SUBMIT→ CYL_COLLECT→ CUBE_COLLECT→ UNDER_BRIDGE→ BALL

📦 自定义接口

11 种 Msg + 1 种 Srv

JoystickIntent / ArmPoseTarget / ArmJointTarget / ArmNamedTarget / GripperCommand / PoleCommand / MenuNavigation / RobotState / SetMode 等

🔌 串口抽象基类

SendCommand 统一封装 USB 模拟串口（termios），统一帧格式 + CRC 校验 + 超时重发

USB 线替代杜邦线 → 大幅提升稳定性

机器人模型与运动规划

🤖 机械臂 URDF 建模

5-DOF 连续旋转关节 (joint1~5) + 总线舵机
2-DOF 对称 mimic 夹爪 (end_joint2 = -end_joint1)
STL 网格碰撞模型 (gray/dark_gray/white 材质)
履带式差分驱动底盘

📐 MoveIt2 运动规划

KDL / LMA 逆运动学求解器
笛卡尔路径 / 关节目标 / 命名目标三种模式
SRDF 自碰撞矩阵避免干涉
右肩键(RB)切换预设位姿

⚡ 手动封装硬件接口

不依赖 ros2_control，直接解析 MoveIt 轨迹 → 舵机角度 → 打包下发 USB 串口。总线舵机自带角度闭环，一定程度上保证精度

⚠ 整车模型面数过高，树莓派加载需 1~2 分钟 → 仅保留机械臂 + 底座

RViz2 机械臂仿真可视化

取舍本想整车上 Gazebo 仿真，但模型面数过高，树莓派启动需 1-2 分钟。最终仅保留机械臂，操作手确保从安全位置运动避免实车干涉

视觉模块总览

独立 C++ 项目（Mygo/），仿真层与识别控制层编译为独立库，通过 cv::Mat 接口传递数据。PC端 OpenCV 仿真 → MaixCDK 交叉编译 → MaixCam2 部署，相机固定在机械臂 pitch 轴上

仿真层 lib target_sim

→

识别控制层 lib target_tracker

→

应用层 8 apps

🎯 仿真层

靶子几何建模 → 动态运动生成 → 3D 相机投影 → 激光红点叠加

TargetSim · TrainingFrameGenerator · SimulationCamera · PentagonSimulator

🔍 识别控制层

多色彩空间色块识别 → 状态机追踪 → PID 云台控制 → 串口舵机指令

TargetTracker · TargetTrackingPipeline · GimbalControl · SerialPort

🧬 优化层

遗传算法自动搜索最优 PID 参数，仿真中多轨迹评估 → 实机部署

GeneticAlgorithm · Target3DGenerator

3D 空间内仿真靶面投影效果

🎯

MygoVison
PC 仿真环境 + 追踪算法

📷

mygo_via_maixcdk
MaixCam2 嵌入式部署

仿真层详解

🎨 靶子建模 (TargetSim)

1中心 + 5外围五角星布局，12种预定义 BGR 颜色，暴露 Ground Truth 便于算法评估

🎬 流式帧生成器

4种运动模式：旋转 / 直线 / 随机出现 / 参数方程（圆周、正弦、李萨如）

角速度函数注入：$a \cdot \sin(\omega t) + b$ 模拟能量机关

📷 3D 仿真相机

针孔内参 + 畸变系数 + 6-DOF 外参 + 传感器尺寸毫米级建模

双相机模型：俯视源相机 + 前方观察相机，运行时交互调参

大符运动仿真（能量机关）

小符运动仿真（匀速旋转）

识别算法：多色彩空间融合 + ROI 跟踪

算法演进历程

V1

全图色块两两比较

O(n²) 复杂度
相似颜色误判
中心定位不准

V2

圆形度筛选

仅需 ~6 次确定中心
大幅降低比较次数
5 个周围色块验证

V3

HSV + LAB 融合

三维加权自适应
Softmax 归一化
置信度门控 ≥1.5×

🎨 自适应色彩权重

中心亮色：提高 BGR 权重 → 区分亮度差异

中心暗色：提高 HSV + LAB 权重 → 区分色相和色差

避免 LAB 亮度分量带来的误识别，针对性解决深色/相近色混淆

目标色块实时追踪效果

🎯 ROI 加速

锁定后仅处理靶子周围 ±padding 区域，大幅降低后续帧处理开销

📡 Kalman 预测

卡尔曼滤波预测靶子运动状态，丢失计数器触发后退回全图搜索

🔴 激光红点

HSV 双色调 0°~12°+168°~179° 覆盖红色环绕，形态学去噪 + 圆形度评分

控制层：从像素到舵机角度

角度解算：pitch_error = -(y - cy) / fy | yaw_error = (x - cx) / fx

追踪管线状态机

Waiting

等待指令

Searching

三角波扫描

Locked

连续锁定N帧

Tracking

PID闭环

追踪策略对比

版本	策略	问题
V1	像素误差 → PID 直接控制	识别精度要求极高，效果不佳
V2	单目测距 → 开环圆形轨迹 + PID 修正	依赖理想条件，实机表现不理想
最终	ROI + Kalman 预测 + PID 闭环	仿真效果好，实机受限于物理约束

双轴 PID 控制细节

独立 Kp/Ki/Kd · 积分限幅 30° · 输出限幅 180°/s · 微分低通滤波 · 死区 8px

调参经验：Ki 宜小 → 优先降 Kd 减小超调 → 收紧输出步进抑制振荡

明亮环境下对目标色块的实机击打

遗传算法 PID 自动整定

参数	默认值	说明
种群大小	50	每代个体数
变异率	0.5	基因突变概率
变异幅度	σ=0.3	高斯噪声标准差
最大代数	300	迭代次数上限
精英数	2	每代保留最优

编码：$(K_p, K_i, K_d)$ 实数三元组 | 交叉：线性混合父代 | 变异：高斯噪声

遗传算法在仿真中训练 PID 参数

📊 适应度函数（多轨迹综合评估）

随机跳跃 × N + 正弦追踪 × N + 圆周追踪 × N + 李萨如轨迹 × N

评分维度：时间效率 $w_t$ + 跟踪误差 $w_e$ + 控制平滑度 $w_s$

⚠ 仿真仅需数秒，实机迭代需半小时以上。仿真PID仅供参考，最终采用人工调参

技术栈总览

🤖 机器人框架

ROS 2 Jazzy · rclcpp · ament_cmake · colcon · launch_ros

📐 运动规划

MoveIt 2 · ros2_control · KDL/LMA · SRDF · tf2

🎮 仿真可视化

RViz2 · Xacro/URDF · STL 网格

👁️ 视觉算法

OpenCV 4.x · 自研遗传算法 · 3D 投影模型 · PID 控制器 · 多色彩空间融合

🔌 底层通信

USB模拟串口 (termios) · STM32F103ZET6/STM32F103C8T6 · MaixCam2 · TCP 网络通信 · sensor_msgs/Joy

🖥️ 工程化

C++ 17 · Python 3 · CMake · Qt5 Widgets · Git · VS Code

完整仿真 → 追踪 → 云台控制全链路演示

项目亮点与创新

ROS 2 主控系统

4 层解耦架构，各层独立可测可替换
8 主状态 + 14+ 子状态 + 派状 UI 操控
手柄操控 + MoveIt2 预设轨迹双模融合
USB 模拟串口 + 开机自启 + 乱序自动识别
11 种语义化消息 + 1 种服务

视觉模块

仿真驱动开发：靶子建模 → 3D投影 → 算法验证，不依赖外部引擎
多色彩空间自适应融合 + 圆形度筛选 + Kalman 滤波
PC仿真 → MaixCDK 交叉编译 → MaixCam2 嵌入式部署
遗传算法探索 PID 自整定（仿真参考）

💡 经验与反思

⚠ 实机挑战

追踪方案

相机固连机械臂（外参难标定）、环境光照复杂、机械臂运动导致滞后。仿真效果好但实机表现不理想

⚡ 工程取舍

仿真 vs 现实

整车模型面数过高，树莓派无法部署，仅保留机械臂。云台因空间限制取消。遗传算法仿真仅需数秒，实机需半小时

✅ 核心收获

分层解耦 + 仿真驱动

分层架构让各模块独立可测可替换；仿真驱动开发让算法在PC端充分验证后再迁移至嵌入式平台

"一份代码，PC 仿真验证 → MaixCDK 交叉编译 → MaixCam2 嵌入式部署。"

🔗 项目已开源

🤖