1. 人形机器人导航概述

大家好，欢迎来到这门实战课。我是老周，在机器人导航这个领域摸爬滚打了快十年。今天咱们先聊聊人形机器人导航到底是怎么回事。

很多人一听到「人形机器人导航」，第一反应就是——这不就是给机器人装个激光雷达，跑个SLAM吗？嗯，如果你真这么想，后面调试的时候肯定会摔跟头。我刚开始带人形项目时也这么以为，结果第一次让双足机器人自主导航穿过走廊……它直接原地劈叉了。

所以这一章，我们先打好基础。搞清楚人形机器人和轮式机器人导航到底差在哪，整个系统架构长什么样，每个核心组件是干什么的。

1.1 人形机器人 vs 轮式机器人导航差异

先问大家一个问题：为什么轮式机器人导航已经挺成熟了，人形机器人还要单独开一门课？

说白了，轮式机器人是「二维生物」。它在一个平面上运动，前后左右，顶多转个弯。它的运动模型简单到可以用差速驱动模型来描述：

# 轮式机器人运动模型（差速驱动）
v = (v_left + v_right) / 2
ω = (v_right - v_left) / L

但人形机器人呢？它是「三维生物」。它有髋关节、膝关节、踝关节，每一步都是动态平衡的过程。它的运动模型长这样：

# 人形机器人简化运动模型（线性倒立摆）
ẍ = (g / z_c) * x + (1 / (m * z_c)) * τ

你看，光是数学表达就复杂了一个数量级。我在项目中遇到过最典型的问题：轮式机器人导航的路径规划，输出的是(x, y, θ)；但人形机器人需要的是(x, y, z, roll, pitch, yaw, 步态相位)。多出来的维度，全是坑。

具体差异我整理了一张表，大家对照着看：

对比维度	轮式机器人	人形机器人
运动模型	差速/阿克曼，连续平滑	步态周期，离散落脚点
定位约束	2D位姿 (x, y, θ)	6D位姿 (x, y, z, roll, pitch, yaw)
地图表示	2D栅格/占据网格	2.5D高程图 / 3D体素
规划维度	路径 + 速度	路径 + 步态 + 落脚点序列
控制频率	10-50 Hz	100-1000 Hz（关节级）
稳定性要求	静态稳定（不会倒）	动态稳定（ZMP约束）

核心差异一句话总结：轮式机器人导航是「平面几何问题」，人形机器人导航是「时空动力学问题」。你不仅要管它去哪，还要管它每一步踩得稳不稳。

1.2 导航系统架构

好，知道了差异，我们来看看整个系统怎么搭。人形机器人导航架构，我习惯把它分成四层：

这个架构图我建议你保存下来。每次调参遇到问题，先回来看一眼——问题出在哪一层？是感知不准，还是规划出来的落脚点不合理？我自己的经验是，80%的导航失败案例，根源都在「层间接口」上。

举个例子，我曾经遇到一个情况：全局路径规划明明给出了路径，但机器人就是不走。查了两天，最后发现是规划层输出的落脚点序列，控制层解析时把左右脚搞反了。嗯，这种低级错误，架构清晰了就能快速定位。

1.3 核心组件介绍

架构搭好了，我们看看每个组件具体干什么。我挑四个最关键的讲：定位、建图、规划、控制。

1.3.1 定位

定位就是回答「我在哪」。轮式机器人用2D激光雷达+轮式里程计就够了。但人形机器人不行——它的脚会打滑，IMU会漂，关节编码器累积误差大。

我建议的定位方案是：

全局定位：基于3D激光雷达的ICP匹配，或者视觉词袋重定位
局部定位：IMU预积分 + 关节运动学 + 足底力触发的零速修正
融合方式：因子图优化（GTSAM / g2o），而不是传统EKF

实战小技巧：人形机器人的IMU安装位置很关键。我习惯把IMU放在骨盆中心，而不是头部。为什么？因为头部在行走时晃动太大，骨盆相对稳定，IMU数据质量高一个量级。

1.3.2 建图

建图就是回答「周围长什么样」。轮式机器人用2D占据网格就够了。但人形机器人需要知道地面高度、坡度、有没有台阶、有没有障碍物会绊到脚。

所以人形机器人建图，我推荐用2.5D高程图。它本质上是一个2D栅格，但每个格子存一个高度值和置信度。这样既能处理楼梯、斜坡，又不会像3D体素地图那样计算量爆炸。

# 2.5D高程图数据结构示例
struct ElevationCell {
    float height;        // 地面高度
    float variance;      // 高度不确定性
    uint8_t traversability;  // 可通行性评分 (0-255)
};

这里有个坑要注意：人形机器人的传感器安装高度通常比轮式机器人高，激光雷达扫描到地面时会有盲区。我踩过的坑是——机器人走到一个下坡前，因为盲区没扫到坡底，直接一脚踩空。后来我在建图模块里加了地形预测，用相邻格子的高度梯度外推盲区的地形。

1.3.3 规划

规划层是导航的大脑。它要回答「怎么走过去」。人形机器人的规划比轮式复杂得多，因为它要同时规划：

身体路径：从A点到B点的全局路径（A* / Hybrid A*）
步态参数：步长、步高、步频、单脚支撑时间
落脚点序列：每一步左脚踩哪、右脚踩哪

这三者是耦合的。你不能先规划路径再算步态——路径上的某个点可能根本放不下脚。我习惯用两阶段规划：

第一阶段：用低分辨率高程图做全局路径搜索，只考虑可通行性
第二阶段：在全局路径的局部窗口内，用落脚点优化器生成具体的步态序列

注意：落脚点规划一定要考虑机器人的运动学可达范围。我曾经见过一个团队规划的落脚点间距1.2米，但机器人最大跨步只有0.8米——结果就是每一步都摔。检查一下你的机器人髋关节活动范围，把这个约束写进规划器。

1.3.4 控制

控制层是最后执行者。它要把规划层的落脚点序列，变成实实在在的关节运动。

人形机器人的控制有两个流派：

控制方法	原理	优点	缺点
基于ZMP	维持零力矩点在支撑多边形内	理论成熟，稳定性可证明	对地形平坦度要求高
基于MPC	模型预测控制，滚动优化	适应复杂地形，鲁棒性强	计算量大，调参复杂

我个人更倾向于MPC方案。虽然调参确实痛苦——我调第一个MPC控制器时，光权重矩阵就试了200多组参数。但一旦调好，机器人能在碎石路、斜坡、甚至轻微推搡下保持稳定。这是ZMP方案做不到的。

控制层的核心代码框架大概长这样：

// MPC控制器简化伪代码
void MPCController::update(const State& current_state, 
                           const FootstepPlan& plan) {
    // 1. 构建优化问题
    OptimizationProblem problem;
    
    // 2. 设置预测时域（通常0.5-1.0秒）
    problem.setHorizon(20);  // 20个步长，每个50ms
    
    // 3. 添加约束：关节限位、速度限位、ZMP约束
    problem.addConstraint(joint_limits);
    problem.addConstraint(zmp_constraint);
    
    // 4. 设置代价函数：跟踪误差 + 能耗 + 平滑性
    problem.setCost(tracking_weight * tracking_error 
                    + energy_weight * joint_torques);
    
    // 5. 求解QP问题
    Solution sol = solver.solve(problem);
    
    // 6. 发送第一个控制指令
    sendJointCommands(sol.joint_positions[0]);
}

注意看第2步的预测时域。我踩过的坑是：时域设太短（<0.3s），机器人反应过激，走一步晃三下；设太长（>2s），计算跟不上，控制频率掉到50Hz以下，机器人直接宕机。经验值是0.5-1.0秒，具体看你用的求解器性能。

1.4 本章小结

这一章我们聊了人形机器人导航和轮式机器人的本质差异，梳理了四层系统架构，也把定位、建图、规划、控制四个核心组件过了一遍。

你可能会觉得内容有点多。没关系，这一章是总纲，后面每一章都会展开讲具体的调参方法和避坑经验。记住一句话：人形机器人导航，难的不是算法本身，而是所有组件要协同工作。任何一个环节的参数没调好，机器人都会用最直接的方式告诉你——摔给你看。

好，这一章就到这里。后面我们开始动手，先从定位模块的调参讲起。

📚 课程目录

1. 人形机器人导航概述

1.1 人形机器人 vs 轮式机器人导航差异

1.2 导航系统架构

1.3 核心组件介绍

1.3.1 定位

1.3.2 建图

1.3.3 规划

1.3.4 控制

1.4 本章小结