1. 人形机器人半程马拉松的任务背景

2025 年 4 月 19 日，北京亦庄举行了全球首个人形机器人半程马拉松。按照北京经开区公开报道，这场比赛与人类跑者同场进行，赛程同样是 21.0975 公里，但机器人拥有独立赛道，并且赛事规则不是只看“能不能迈腿”，还把设备稳定性、换电策略、运维组织能力一起纳入考核。换句话说，这不是一场单纯的运动控制展示，而是一次把本体、电池、感知、导航、控制、通信、现场保障全部拉到真实城市道路里接受压力测试的综合考试。

更关键的是，2025 年的官方报道也明确写到，赛事允许工程师跟随奔跑，也允许采用遥控操作方式，并配有专属保障车辆跟随。这说明当时行业的主流状态并不是“机器人已经会自主跑完全程”，而是“机器人已经能在严格保护下完成高强度公开测试”。把这个背景放在前面，才会理解 Marathongo 的价值所在：它试图解决的不是让机器人在实验室里走几步，而是减少人对机器人全程决策的持续接管，把导航、避障和控制从“示范可用”推进到“长距离可复现”。

2026 年 4 月 19 日，北京亦庄又举办了第二届人形机器人半程马拉松。官方公开信息显示，这一届比赛首次规模化启用自主导航模式，超百支赛队参赛，其中自主导航类别约占四成；赛道还部署了北斗厘米级高精度定位、多频段冗余通信、7 个标准化补给与应急处置点，并加入坡道、弯道、起伏路面、公园生态路段等复杂场景。 这意味着赛事本身已经从“允许人形机器人参赛”升级为“鼓励机器人在开放环境中自己跑”，而 Marathongo 恰好出现在这个产业节点上。

2. Marathongo 的开源范围与模块组成

Marathongo 官方将自己定义为“面向人形机器人马拉松比赛场景的导航全栈开源方案”，这个定义的重点不在“导航”两个字，而在“全栈”和“马拉松场景”。在仓库总 README 中，项目方反复强调它不是一个单点算法 Demo，而是一套经过多平台验证、可扩展、可裁剪、尽量接近开箱即用的实战框架。它同时开放了定位、规划、控制、视觉训练、部署相关内容，并明确告诉使用者：这里提供的不是一篇论文的附录，而是一堆带着真实调试痕迹的工程资产。

从仓库结构来看，Marathongo 不是把所有能力硬塞进一个统一的程序里，而是拆成了四条彼此协作、又能按需选用的技术路线。这样的组织方式很有现实意义，因为人形机器人平台之间的差异远比很多人想象得大，传感器外参不同，底层控制接口不同，算力预算不同，步态能力也不同。一个团队最需要的并不是“世界上最先进的一套算法”，而是一条能根据自己硬件条件裁剪、替换、验证、最终真正跑起来的工程路径。

模块	核心职责	面向问题
glio_mapping	GNSS + IMU + LiDAR 融合定位与建图	长距离室外定位一致性、抗漂移、抗遮挡
tangent_arc_navigation	极简巡线与基础避障路线	快速跑通导航闭环、低复杂度验证
marathontracking	完整局部规划、避障与控制集成	比赛级动态环境、复杂障碍与模式切换
vision_part	视觉障碍识别训练、融合与部署	动态目标识别、边缘部署、TensorRT 推理

3. Marathongo 的数据链路与系统分层

如果把 Marathongo 当成一个黑盒，很容易误解它只是“多传感器融合 + 规划控制”的普通组合。但从源码和子模块说明来看，它更像一条明确分层的数据管线：底层传感器首先解决机器人“我在什么位置、朝什么方向、周围有什么”的问题；中间层把全局路径、局部点云、障碍边界和速度约束拼装成一个可决策的局部世界；最上层再把规划结果转换成机器人控制接口真正能理解的速度指令。对人形机器人来说，这三层不是线性串起来就结束了，而是必须一直在抖动、延迟、算力受限和通信波动中闭环工作。

仓库里的配置和启动方式把这条链路写得很直接。以 glio_mapping 为例，默认配置里会显式指定雷达、IMU 和 GNSS 话题，启动文件则把对应节点和 RViz 可视化一起拉起来。对于机器人系统开发来说，这种“先把管线接通，再谈性能上限”的风格非常重要，因为大量项目失败并不是算法不够先进，而是系统边界从一开始就没有定义清楚。

slam:
  common:
    lid_topic: "rslidar_points"
    imu_topic: "imu/data"
    gnss_topic: "gnss_ksxt"
    time_sync_en: false
  publish:
    path_en: true
    scan_bodyframe_pub_en: true

imu_processing:
  extrinsic_T: [0.065, 0.0, 0.07]
  gnss_heading_need_init: true

gps_processing:
  extrinsic_T_gnss: [0.07, 0.27, 0.12]

<arg name="rviz" default="true" />
<arg name="config_files" default="$(find glio_mapping)/config/robosense.yaml" />
<node pkg="glio_mapping" type="glio_mapping_node"
      name="laserMapping" output="screen" args="$(arg config_files)"/>
<group if="$(arg rviz)">
  <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz"
        args="-d $(find glio_mapping)/rviz_cfg/loam_livox.rviz" />
</group>

roslaunch glio_mapping mapping_robosense.launch

Gemini 绘图提示词：Marathongo 系统数据链路总览
请绘制一张 16:9 中文科技信息图，主题为“Marathongo 人形机器人马拉松导航数据链路”。画面采用浅灰蓝背景、冷白文字、青绿色与橙色作为强调色，风格克制、专业、工程感强。整体为横向三层架构：左侧是传感器层，包含 GNSS、IMU、LiDAR、Camera 图标和对应 topic 标签 gnss_ksxt、imu/data、rslidar_points；中间是算法层，依次展示 glio_mapping、local_planner、seg_fusion / TensorRT 三个模块卡片，并用箭头标出“定位估计”“局部避障”“视觉语义”三条信息流；右侧是控制执行层，展示 joy_node.py、/final_stampd_cmd_vel、send_node、Robot Controller。底部单独画一条回环监控链路，标出 watchdog、manual takeover、UDP bridge。要求版式简洁、层次清楚、标签可读、避免卡通感，适合技术文章插图。

4. GLIO-Mapping 在系统中的定位作用

在人形机器人马拉松这个任务里，定位问题比很多人想象得更难。汽车自动驾驶通常受益于更稳定的车体姿态、更充足的供电、更规范的安装空间和更高的传感器高度；人形机器人则恰好相反，双足运动带来的周期性震动会持续污染 IMU 和点云，机身高度有限导致遮挡更频繁，户外赛道又包含树荫、桥梁、坡道、弯道、开阔区与局部退化区。只靠单一传感器，很难在 21 公里级别的开放环境里既不漂移，又不断线，更不可能稳定支撑高速奔跑时的局部决策。

glio_mapping 的核心思路是把 GNSS、IMU 和 LiDAR 放到一条紧耦合融合链路中处理。根据该子项目 README 的表述，它强调两层一致性：前端里程计负责高频姿态连续性，后端图优化负责长距离全局一致性；同时又把 GNSS 约束引入进来，抵消纯 LIO 在超长路线中的累计偏移。项目文档甚至直接写明，这个模块已经应用于 2026 年人形机器人马拉松竞赛，实现了 21 公里自主导航，并以“厘米级定位精度”作为目标效果。这里需要说明，这一性能表述来自项目方文档，而不是独立第三方测评，但它至少解释了整个仓库为什么首先把定位模块单独拎出来。

从配置文件也能看出这个模块的工程化特点。robosense.yaml 不只是让用户填几个 topic 名称，它还涉及时间同步开关、LiDAR 到 IMU 的外参、GNSS 外参、点云预处理参数和 PCD 保存策略。这些内容看起来琐碎，却恰好决定了一套室外导航系统是不是能真的跑起来。对不了解机器人系统的人来说，可以把它理解成这样一句话：在 Marathongo 里，“知道自己在哪儿”不是调用一个定位库就结束了，而是一套必须围绕时间、坐标和噪声反复校准的基础设施。

如果只看配置还不够，那么 glio_mapping 的核心源码大致可以分成三层：第一层是 LaserMapping::init() 里把预处理、IMU、GNSS 和地图结构接成一条工作链；第二层是 ImuProcess::UndistortPcl() 里按时间顺序推进 IMU 与 GNSS 观测，并把一帧激光点云去畸变；第三层则是 h_share_model() 中把点云匹配结果构造成 EKF 的残差和 Jacobian。下面这三段代码，比单看参数更能说明 Marathongo 的定位底座到底在做什么。

首先是系统初始化。这里最关键的不是创建了多少对象，而是 Preprocess、ImuProcess、GNSS 处理器、位姿图和 kf.init_dyn_share(...) 被放进同一个入口里，意味着后续每一帧激光数据都可以走到统一的状态估计链路中。

ikdtree_ptr = std::make_shared<KD_TREE<PointType>>();
config_ptr = std::make_shared<ModuleBase>(config_path_, "slam", "mapping");
gnss_processor_ptr_ = std::make_shared<GPSProcessor>(config_path_);
pose_graph_manager_ptr_ = std::make_shared<PoseGraphManager>(config_path_);
p_pre_ = std::make_shared<Preprocess>(config_path_);
p_imu_ptr_ = std::make_shared<ImuProcess>(config_path_);

p_imu_ptr_->SetHeadingInitCallback([&](bool val) {
    pose_graph_manager_ptr_->setGnssHeadingNeedInit(val);
});

kf.init_dyn_share(get_f, df_dx, df_dw,
    [](state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data) {
      if (opt_with_gnss()) {
        p_imu_ptr_->glio_estimaor_ptr_->h_share_model_gnss(s, ekfom_data);
        return;
      }
      if (opt_with_wheel()) {
        p_imu_ptr_->glio_estimaor_ptr_->h_share_model_wheel(s, ekfom_data);
        return;
      }
      if (opt_with_angle()) {
        p_imu_ptr_->glio_estimaor_ptr_->h_share_model_angle(s, ekfom_data);
        return;
      }
      h_share_model(s, ekfom_data);
    }, NUM_MAX_ITERATIONS, epsi);

第二层是 IMU 主导的时序推进与点云去畸变。UndistortPcl() 不是简单地把 IMU 数据“附加”到 LiDAR 上，而是把一帧激光采样期间穿插的 IMU、GNSS 观测按时间顺序送入估计器，然后把每个点补偿到帧末坐标系。

for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++) {
  auto &&head = *(it_imu);
  auto &&tail = *(it_imu + 1);

  angvel_avr << 0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
                0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
                0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
  acc_avr    << 0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
                0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
                0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);

  glio_estimaor_ptr_->addIMUData(tail->header.stamp.toSec(), acc_avr, angvel_avr);
  glio_estimaor_ptr_->process(kf_state, Q);

  if (gnss_front_ && !curr_gnss.empty()) {
    double gnss_time = curr_gnss.front()->header.stamp.toSec();
    if (gnss_time < tail->header.stamp.toSec() && !gnss_heading_need_init_) {
      glio_estimaor_ptr_->addGNSSData(curr_gnss.front());
      glio_estimaor_ptr_->process(kf_state, Q);
      curr_gnss.pop_front();
    }
  }
}

common::V3D P_compensate =
    imu_state.offset_R_L_I.conjugate() *
    (imu_state.rot.conjugate() *
     (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) -
     imu_state.offset_T_L_I);

第三层是激光约束真正进入滤波器的地方。h_share_model() 会先在地图里搜索近邻点、估计局部平面，再把点到面的残差写成观测方程。这一段很能说明它不是“GNSS 定个大概位置 + LiDAR 做个修正”这么简单，而是把每个有效点都变成了状态更新的一部分。

ikdtree_ptr->Nearest_Search(point_world, NUM_MATCH_POINTS,
                            points_near, pointSearchSqDis);

if (common::esti_plane(pabcd, points_near, 0.1f)) {
  float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y +
              pabcd(2) * point_world.z + pabcd(3);
  float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());
  if (s > 0.9) {
    normvec->points[i].x = pabcd(0);
    normvec->points[i].y = pabcd(1);
    normvec->points[i].z = pabcd(2);
    normvec->points[i].intensity = pd2;
  }
}

common::V3D C(s.rot.conjugate() * norm_vec);
common::V3D A(point_crossmat * C);
common::V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C);
ekfom_data.h_x.block<1, 12>(i, 0)
    << norm_p.x, norm_p.y, norm_p.z,
       VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C);
ekfom_data.h(i) = -norm_p.intensity;

把这三层放在一起看，就能理解 glio_mapping 为什么适合作为马拉松导航系统的底座。它不是把 GNSS、IMU、LiDAR 生硬堆在一起，而是把初始化、时序传播、点云去畸变、局部几何约束和后续位姿图优化组织成了一条连续的数据路径。对于长距离户外任务来说，这种结构比单独强调某一个算法名词更重要。

5. 局部规划的模式设计与速度边界

如果说定位模块负责“别迷路”，那么 marathontracking 的核心任务就是“在不迷路的前提下，决定现在该怎么跑”。这个子模块 README 用一句话概括了它的行为逻辑：系统分为全速寻线、全速避障、低速恢复三个阶段。表面上看这像是一个很朴素的状态机描述，但源码展开以后会发现，这三种模式对应的是不同的速度边界、不同的规划策略、不同的控制器裁剪方式，以及不同的安全触发条件。

local_planner_node.cpp 开头就把模式枚举写得很清楚：TRACKING、AVOIDANCE、RECOVERY。同时它在机器人周围构建了一个 20 米 × 20 米 × 3 米、分辨率为 0.2 米的局部栅格世界，用环形体素地图管理障碍，再用 Dijkstra 做恢复路径规划。更有意思的是，它不是只算一条路径，而是从 ./pathes 目录中加载一组采样轨迹，对这些候选路径分别计算跟踪代价、安全分数、能量代价和平行性指标，再根据当前模式挑选最终路径。换句话说，Marathongo 的局部规划并不是“看见障碍就拐一下”，而是在有限时间里做带有约束的候选路径评估。

enum class PlannerMode : int {
  TRACKING = 0,
  AVOIDANCE,
  RECOVERY,
};

double kAvoidanceModeObstacleDistance = 7.0;
double kMinScoreSafety = 3.0;
double kFatalMinScoreSafety = 1.5;

local_planner_node(ros::NodeHandle* nh) : nh_(nh) {
  elevation_map.setGeometry(20.0, 20.0, 3.0, 0.2);
  auto local_map_size = elevation_map.getSize();
  obstacle_map.setGeometry(local_map_size.x, local_map_size.y, 1, 0.2);
  planner_ = std::make_unique<slamchain::Dijkstra>(
      local_map_size.x, local_map_size.y, 1);
}

void start() {
  suber_lidar_ = nh_->subscribe(
      "/current_scan_body", 1, &local_planner_node::handler_lidar, this);
  suber_lidar_odom_ = nh_->subscribe(
      "/odometry", 1, &local_planner_node::handler_lidar_odom, this);
  puber_cmd_vel_ =
      nh_->advertise<geometry_msgs::Twist>("/fuzzy_cmd_vel", 1, false);

  init_sampler();
  init_kinematic_envelope();
}

这些阈值背后有非常强的工程含义。源码里，kAvoidanceModeObstacleDistance 被设置为 7 米，意味着系统在相对远距离时就开始评估是否切入避障模式；kFatalMinScoreSafety 则是 1.5 米，当最优安全分数低于这个阈值时，系统会直接判定“无法通过”，触发 crash() 逻辑，把机器人带入恢复模式，而不是硬着头皮往前冲。恢复过程也不是原地乱试，而是重新规划一条路径，检查距离自由路径的偏移量、恢复路径长度、机器人朝向与路径朝向的夹角，并用看门狗机制判断何时可以退出恢复。这种写法非常朴素，但正因为朴素，它才更像一套真正为比赛和实地运行准备的系统。

下面这一段更能看出它如何把采样轨迹、打分和模式切换写成实际决策逻辑。

std::string folder_path = "./pathes";
for (const auto& entry : std::filesystem::directory_iterator(folder_path)) {
  if (entry.is_regular_file() && entry.path().extension() == ".path") {
    path_files.push_back(entry.path());
  }
}

double score_safety = compute_score_safety(path, beg_idx, end_idx,
                                           draw_collision_range_points);
double score_tracking =
    compute_score_tracking(path, cropped_path_, dyn_ahead_idx);
double score_energy =
    compute_score_energy(robot_pos, robot_rot, path, dyn_ahead_idx);

if (safe_rate < 0.8) {
  need_to_avoid_obstacles = true;
}
if (max_score_safety < kFatalMinScoreSafety) {
  final_path_idx = -1;
  this->crash();
}

从源码中的运动学包络器还可以看出，三种模式对应的速度边界被严格分开。TRACKING 模式允许更高的直线速度，AVOIDANCE 模式缩小速度和角速度范围，RECOVERY 模式则进一步把线速度压到低速区间。这一点对双足机器人尤其重要，因为腿式系统不像轮式底盘那样能随时“高速大转弯”，速度、转向和步态稳定性之间存在强耦合。Marathongo 在这里采取的做法不是追求花哨的统一控制理论，而是用一套可调、可剪裁、可解释的运动学包络把控制量限制在机器人能够承受的物理边界内。

模式	线速度包络	角速度包络	作用
TRACKING	约 2.5 到 3.5 m/s	约 ±2.3	无明显障碍时保持高速巡线
AVOIDANCE	约 1.6 到 2.3 m/s	约 ±1.5	有风险但仍可通过时快速绕障
RECOVERY	约 0 到 0.6 m/s	约 ±0.8	停车、重规划、低速回到主路径

6. 控制链路中的模式切换与安全机制

很多关于机器人自主导航的宣传都会刻意回避一个问题：当自动控制暂时失效时，系统如何优雅地退回人工接管，而不是直接摔倒、撞人或冲出赛道。Marathongo 在这件事上非常现实。joy_node.py 一边订阅自动控制器输出的 /fuzzy_cmd_vel，一边保留手柄或 SBUS 遥控输入，再把最终结果统一发布到 /final_stampd_cmd_vel，交给发送节点转发给机器人本体。也就是说，在它的系统设计里，自动和人工并不是彼此否定的关系，而是可以被一个统一的路由层安全切换。

更重要的是，这个路由层带了明确的看门狗逻辑。如果自动控制的话题在多个周期内不再更新，节点会主动把线速度和角速度清零；如果从自动切回手动，它还会调用 smooth_stop() 做缓停，而不是直接掐掉速度。这些代码并不华丽，却是严肃机器人系统和“实验室演示代码”之间最容易被忽视的分水岭。2025 年官方赛事允许工程师跟跑与遥控，本质上就是承认现实世界中的机器人系统必须有兜底方案；Marathongo 并没有假装这件事不存在，而是把它正式写进了控制链路里。

class Node():
    def __init__(self):
        self._puber_cmd_vel = rospy.Publisher(
            "/final_stampd_cmd_vel", TwistStamped, latch=False, queue_size=1)
        self.suber_fuzzy_cmd_vel = rospy.Subscriber(
            "/fuzzy_cmd_vel", Twist, self.handler_fuzzy_cmd_vel, queue_size=1)
        self.timer_watch_dog = rospy.Timer(
            rospy.Duration(0.1), self.handler_timer_watch_dog)
        self.process_timer = rospy.Timer(
            rospy.Duration(0.02), self.process_state)

        self.joy_interface = SBUSJoy("/dev/tty_elrs")
        self.control_mode = 'manual'
        self.fuzzy_cmd_vel = None
        self.cmd_vel_feed_count = 0
        self.state = None

    def handler_fuzzy_cmd_vel(self, msg):
        self.fuzzy_cmd_vel = msg
        self.cmd_vel_feed_count = 0

    def handler_timer_watch_dog(self, event):
        if self.fuzzy_cmd_vel is not None:
            self.cmd_vel_feed_count = self.cmd_vel_feed_count + 1
            if self.cmd_vel_feed_count > 5:
                self.cmd_vel_feed_count = 9999
                self.fuzzy_cmd_vel.linear.x = 0
                self.fuzzy_cmd_vel.angular.z = 0

    def process_state(self, event):
        state = self.state
        if state is None:
            return
        if state.btn_manual_control and self.control_mode != 'manual':
            self.control_mode = 'manual'
            self.call_rc1_mode()
            self.smooth_stop()
        elif state.btn_auto_control and self.control_mode != 'auto':
            self.control_mode = 'auto'
            self.call_rc1_mode()

    def call_rc1_mode(self):
        rospy.wait_for_service("/cmd_server/rc1_mode", timeout=2)
        client = rospy.ServiceProxy("/cmd_server/rc1_mode", Trigger)
        client(TriggerRequest())

如果从系统架构的角度重新看这段代码，会发现 Marathongo 其实在回答一个更深的问题：真正可用的自主导航，不是“永远不需要人工”，而是“在绝大多数时间里能自己完成任务，在极少数异常时刻也不会把系统带入不可控状态”。对于户外高速奔跑的人形机器人来说，这种设计比“全自动”三个字本身更值得信任。

7. 视觉模块的训练与部署链路

许多人第一次看到 vision_part 时，会以为它只是仓库里顺手附带的一个视觉实验目录。实际上，这部分内容非常关键，因为户外赛道不是一个静态世界。人群、车辆、其他机器人、临时路障都会不断进入视野，而纯激光和纯几何规则并不总能给出足够稳定的语义信息。Marathongo 把视觉能力拆成训练、融合、ONNX 导出和 TensorRT 部署几层，说明项目方真正关心的是“怎样把视觉结果送进实时系统”，而不是“怎样单独训练出一个看上去很强的模型”。

seg_fusion 子项目最有价值的地方，是它没有把视觉训练说成一个一键完成的黑箱。README 直接承认它面对的是部分标注数据：自定义数据集中只标了 robot，但同一画面里可能还有 person 和 car。如果处理不当，未标注目标会被当成背景，最后把模型的通用识别能力一起破坏掉。为了解决这个问题，项目采用了两阶段思路：先冻结 backbone 做 robot 目标域适配，再把面向自定义数据训练得到的权重与保留通用类别能力的权重做融合。这种写法不“炫”，却非常像真实项目里会出现的折中方案。

from ultralytics import YOLO

model = YOLO("yolo11s-seg.pt")
model.load()

results = model.train(
    data="data_3/dataset.yaml",
    epochs=100,
    imgsz=(640, 640),
    device=[1],
    freeze=10,
)

更能体现仓库个性的，是它在 ONNX 推理阶段没有简单复用单一类别表，而是把“通用类别分支”和“robot 分支”分开解码后再做合并。这说明项目方关心的不只是训练出一个模型，而是如何把融合后的类别体系稳定地送进后处理链路。

…详情请参照古月居