Marathongo：人形机器人马拉松导航系统的结构、原理与落地路径

转载自公众号：敢敢AUTOHUB

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

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

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

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

仓库地址：https://github.com/landitbot/marathongo

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

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

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

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

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 在这里采取的做法不是追求花哨的统一控制理论，而是用一套可调、可剪裁、可解释的运动学包络把控制量限制在机器人能够承受的物理边界内。

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 分支”分开解码后再做合并。这说明项目方关心的不只是训练出一个模型，而是如何把融合后的类别体系稳定地送进后处理链路。

valid_dict_person = {0: 0, 2: 1, 5: 1, 7: 1}
boxes_list, masks_list = self.postprocess_(
    img, prep_img, (boxes, scores, masks, protos),
    valid_dict_person, num_classes=80,
)

valid_dict_robot = {0: 2}
boxes2, masks2 = self.postprocess_(
    img, prep_img, (boxes, scores, masks, protos),
    valid_dict_robot, num_classes=3,
)

names = {0: "person", 1: "car", 2: "robot"}
merged_boxes, merged_masks = self.suppress_person_overlapping_robot(
    merged_boxes, merged_masks
)
return [Results(img, path="", names=names, boxes=merged_boxes, masks=merged_masks)]

而在导出与部署侧，仓库 README 里把训练、融合导出和 TensorRT 引擎构建分成了几步独立流程，这也更符合真实工程而不是“一键魔法脚本”的写法。

python train.py
python fusion_export/robotbase/fusion_robotbase_export.py
python fusion_export/fusion10/fusion_branch10_export.py
bash scripts/build_engine.sh \
  /path/to/fusion_branch10_960x544.onnx \
  engines/weights/fusion_branch10_960x544_fp16.engine

需要强调的是，仓库作者也明确提醒：视觉与导航的最终在线融合，仍然依赖目标机器人上的标定与系统集成。这句话看似保守，实际上非常重要。因为视觉模块再强，也只有在时间同步、相机外参、推理延迟和控制频率都被约束住之后，才会从“离线识别效果不错”变成“在线避障真的有用”。

8. 接入自有机器人的实现步骤

第一步不是跑代码，而是先把时间同步和外参标定做对。无论是 glio_mapping 还是后续的视觉融合，项目都默认你的 LiDAR、IMU、GNSS，乃至相机之间存在可用的时间关系和坐标关系。对于马拉松这种长距离、高速、持续振动的场景来说，时间偏差和外参误差不会只带来一点点噪声，而是会一路放大成定位漂移、障碍错位和控制延迟。因此，真正的接入顺序应该从“定义传感器关系”开始，而不是从 roslaunch 开始。

第二步应当先选择一条轻量路线把闭环跑通，再决定要不要切到完整比赛路线。Marathongo 保留 tangent_arc_navigation 的意义就在这里：如果你的机器人刚完成基础软件环境搭建，最合理的策略不是一上来就啃复杂的 marathontracking。而在仓库里，更接近“仿真验证 / 轻量桥接闭环”的一类实现，则是 sim_interface_node.cpp 这样的桥接层代码，它直接把 Gazebo 侧的 /gazebo/model_states、/lidar_points 接成导航侧更关心的 /high_frequency_odometry、/rslidar_points 和 /cmd_vel。这比抽象地说“先做仿真”更具体，因为它明确告诉你要先对齐哪些 topic。

suber_model_status_ =
    nh_->subscribe("/gazebo/model_states", 1,
                   &sim_interface_node::handler_model_status, this);
suber_lidar_ = nh_->subscribe("/lidar_points", 1,
                              &sim_interface_node::handler_lidar, this);
suber_tracking_cmd_vel_ = nh_->subscribe(
    "/fuzzy_cmd_vel", 1, &sim_interface_node::handler_track_cmd_vel, this);

puber_robot_odom_ =
    nh_->advertise<nav_msgs::Odometry>("/high_frequency_odometry", 1);
puber_lidar_ =
    nh_->advertise<sensor_msgs::PointCloud2>("/rslidar_points", 1);
puber_cmd_vel_ = nh_->advertise<geometry_msgs::Twist>("/cmd_vel", 1);

std::this_thread::sleep_for(std::chrono::milliseconds(500));
node.loadFreePath("./free_path.txt");

第三步是控制接口适配，而不是继续沉迷于规划算法。源码已经很明确地显示，marathontracking 的自动控制输出最终会被送到 /fuzzy_cmd_vel，然后由 joy_node.py 路由到 /final_stampd_cmd_vel，再交给发送节点和机器人接口。也就是说，如果你的底层执行器、步态控制器或者板载通信链路和这里假设的不一致，那么真正需要修改的地方往往不是局部规划本身，而是命令映射层、模式切换层和发送链路。很多团队在这一环节掉了大量时间，因为他们一直在调规划参数，却没有先把控制接口的边界定义清楚。

从 marago.sh 往下继续看，真正决定接口边界的是发送节点本身。server.cpp 直接订阅 /final_stampd_cmd_vel，再把控制量封装成 UDP 协议包，同时暴露 /cmd_server/init_udp_receiver、/cmd_server/rc1_mode 这类服务接口。相比只看启动脚本，这一层更能说明“底层适配”到底要改什么。

add_executable(send_node src/server.cpp)

std::string topic_name_stamped = "/final_stampd_cmd_vel";
ros::Subscriber sub_cmd_vel_stamp =
    nh.subscribe(topic_name_stamped, 1, send_cmd_vel_stamped);

auto srv_init_robot_recevier_2 = nh.advertiseService(
    "/cmd_server/init_udp_receiver", srv_handler_init_udp_receiver);
auto srv_third_rc1_mode = nh.advertiseService(
    "/cmd_server/rc1_mode", srv_handler_rc1_mode_receiver);

void send_cmd_vel_stamped(const geometry_msgs::TwistStamped::ConstPtr& msg) {
  buffer[8] = MSGTYPE_CMD_VEL;
  float vel_cmd[6] = {
      msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z,
      msg->twist.angular.x, msg->twist.angular.y, msg->twist.angular.z};
  memcpy(buffer + 9, vel_cmd, sizeof(vel_cmd));
  uint32_t crc = calculate_crc32(buffer + 8, data_size);
}

第四步才轮到视觉融合与边缘部署。只有当定位、规划、控制闭环已经稳定以后，视觉模块才有资格进入主链路。此时要做的工作包括：确认你的数据集类别定义是否与仓库一致，检查 robot 的类别索引，完成模型导出后的 ONNX 输出顺序核对，并根据目标平台选择 FP16 还是 INT8 的 TensorRT Engine。否则，一个训练指标不错的分割模型，很可能在真实系统里只会额外引入延迟和不确定性，而不会带来避障收益。

第五步一定是空场验证、限速验证和长距离路测，而不是“代码一编过就上赛道”。这一点仓库 README 写得非常坦率：不同平台在传感器型号、控制接口、运动学约束、算力预算和安全策略上差异巨大，开源代码并不等价于零修改适配全部机器人。项目还专门提醒用户在真实部署前完成必要的仿真、空场测试、限速验证和应急策略设计。对任何认真做人形机器人的团队来说，这段免责声明不该被当作客套话，而应该被当作接入流程的一部分。

9. Marathongo 的工程意义与复现价值

如果只从比赛成绩看问题，很容易把 Marathongo 理解成一个为了半程马拉松而生的窄场景项目。但从 2025 到 2026 年北京亦庄赛事规则和参赛结构的变化来看，行业真正要解决的问题从来不是“有没有机器人能跑”，而是“能不能把自主导航、人机协同和长距离稳定运行做成可复现、可迁移、可继续迭代的系统能力”。2025 年官方赛事仍允许大量人工辅助；到 2026 年，赛事已经开始用规模化自主导航和更严格的规则引导技术演进。Marathongo 的价值，恰好在于它给出了一个可以拿来研究、拆分、裁剪和验证的工程底座。

更难得的是，仓库并没有把自己包装成一份“完美代码”。总 README 反而主动说明：目录里保留了阶段性实验特征，不同模块代码风格并不统一，一些实现优先满足“真实可跑、便于验证和快速适配”，而不是产品级抽象。这种态度反而提升了它的可信度，因为真实机器人系统本来就不是靠漂亮的抽象先长出来的，而是在一次次路测、一次次摔倒、一次次传感器时序修正和一次次控制接口重写中慢慢磨出来的。Marathongo 最值得看的地方，不是它是否优雅，而是它是否把一条复杂但真实的工程路径摊开给后来者看。

10. 结语

如果要用一句话概括 Marathongo，我会把它定义为：一套围绕“长距离室外高速自主运行”组织起来的人形机器人导航工程框架。它的核心不是某个单独的算法名词，而是把定位、感知、局部规划、控制路由、人工接管、视觉训练和边缘部署这些本来容易分裂的环节重新串成了一条连续的实现链。对于不了解这类系统的人，理解这条链，就理解了人形机器人为什么还离不开工程；对于真正要做系统的人，理解这条链，也就知道了应该先从哪里动手。