简介：一份发明专利公开文件，主题为四足机器人爬楼梯控制方法及系统，面向机器人控制领域的研究人员、工程师以及高校相关专业学生。该发明针对现有四足机器人爬楼梯控制方法复杂、成本偏高的问题，提出一套简洁高效的控制方案：先识别台阶参数，包括台阶边缘到各个腿足端的距离与高度；再依据这些参数调整机器人的相对位置和姿态；随后综合计算前后腿足端的落点位置和身体位置，实现平稳爬楼梯。资源包内共1个PDF文件，大小412KB，含完整的发明专利申请文本，包括权利要求书、说明书和附图，公开了方法的具体步骤及第一识别模块、第二识别模块、调整模块等系统结构，还涵盖计算设备与可读存储介质相关权利要求。目前已有84人学习或下载，对于从事足式机器人运动控制研究、专利技术调研或专利申请文本撰写的读者，具有直接的技术参考与范例价值。
1. 爬楼梯为什么是四足机器人工程化的试金石
一份《一种四足机器人爬楼梯控制方法及系统.pdf》的技术文档，看起来像是专利或技术交底书，但它的价值并不仅仅在纸面上。四足机器人能跑、能跳、能越障，这些能力在演示视频里很常见，可一旦放到楼梯这个场景，很多算法会集体翻车。楼梯意味着连续的、有规律的、带有几何约束的垂直落差，机器人不仅要感知每一级台阶，还要在窄小的支撑面上完成重心转移和落足。这个标题锁定的问题，正是从实验室样机走向小区巡检、楼道配送、应急救援时绕不开的一道坎。
这篇文章会先把爬楼梯的难点拆成感知、规划、执行三个层面，然后给出一套可落地的控制方案：从台阶识别到步态编排，再到步态参数整定和系统联动。最后还会把我在模拟环境里跑断腿、在真机上调到怀疑人生的几个坑摆出来，并给出排查路径。适合正在做四足落地应用、或者在读相关控制与机器人方向论文的从业者。给新手一条能照着搭的路线，给熟手一批可以直接拿去试的参数边界和失效判据，比空谈仿生步态有用得多。
2. 爬楼梯问题的本质：为什么四足会在这件事上卡住
2.1 楼梯场景的约束拆解
楼梯不是普通的障碍物，它是一组连续且互相耦合的几何约束。每一级台阶存在一个竖直面和水平面，机器人需要准确区分哪个平面能踩、哪个平面会打滑，而且台阶宽度往往只比脚掌大一点。把这个约束量化出来，就是三个核心参数：台阶高度 h、台阶深度 d、台阶宽度 w。不同场景的值域变化很大，室内楼梯高度在 150～180 mm，深度在 250～300 mm，而户外台阶可能在 200 mm 以上，深度反而不规则。控制策略如果只针对一组固定尺寸，换到另一栋楼就会失效。
其次是运动学约束。四足机器人的腿部工作空间是有限的，髋关节、膝关节、踝关节的角度范围决定了它能抬多高、往前够多远。爬楼梯时，后腿从上一级台阶跨到下一级，前腿需要先探到更高的平面，这就让每条腿的轨迹规划变成一个带边界条件的逆解问题。更麻烦的是，楼梯空间里没有足够的余量让机器人像平地上那样随意摆动躯干，重心投影必须一直落在多边形支撑区域内。
还有一个容易被忽视的因素是感知的不确定性。视觉或深度传感器从正前方看楼梯时，台阶边缘会形成周期性的深度断层，但如果光线角度刁钻、或者台阶是浅色且无反光，边缘点云会消失。这时候如果控制器还按理想几何模型执行，一脚踩空就成了必然。
2.2 稳定判据的迁移：从静态判据到动态判据
平地上常用的零力矩点（ZMP）判据在爬楼梯场景里依然有效，但它很保守。ZMP 要求机器人重心投影始终在支撑多边形内，这在低速爬楼时没问题，可一旦想提高速度，ZMP 约束会显著限制步幅和步频。实际工程中我一般会把 ZMP 和重心角动量结合起来看，引入一个动态稳定裕度。核心思路是：允许重心短暂偏离支撑区，但限制角动量变化率，让系统在下一个落足时刻重新回到稳定域。
楼梯场景对稳定判据还有一个特殊要求——每一级台阶的支撑面高度不一样。这意味着传统的水平面 ZMP 公式要改成三维形式，把重力在竖直方向的分量、惯性力在水平方向的分量投影到当前支撑平面所在的倾斜坐标系里。很多团队在仿真里能用 ZMP 控制器跑通平地，一上楼梯就频繁触发防摔倒保护，原因往往是他们把 ZMP 约束写成平面的，没有考虑台阶垂直落差带来的支撑面切换瞬间的冲击力。
从控制周期来看，爬楼梯时躯干的俯仰角变化比平地行走大得多，反馈控制器需要更高的带宽。我常用的做法是把姿态环放在 1 kHz 频率，而步态规划放在 200 Hz，两者之间用一个插值器衔接，避免规划层和驱动层之间出现步态抖动。
2.3 步态选择的底层逻辑
四足爬楼梯不是只有一种步态。常见的方案有三种：静态爬行步态、对角小跑步态、以及带跳跃的越障步态。静态爬行步态每次只移动一条腿，另外三条腿保持支撑，躯干姿态稳定，适合台阶高度大、表面不平的场景，但速度慢，每一步要花 1～2 秒。对角小跑则是同时移动对角的两条腿，速度快，但对台阶深度的要求更高，因为对角腿落足点的距离要足够容纳一个对角步幅。跳跃式适合跨度大的台阶，但落地冲击大，对结构强度要求高，一般不作为常规控制策略。
我在实际方案里倾向于把静态爬行作为默认步态，再用一条状态机切换逻辑响应不同楼梯参数。切换条件不是简单的台阶高度阈值，而是综合台阶深度、当前机器人俯仰角、以及上一次落足偏差的置信度。比如当台阶深度小于 230 mm 且宽度大于 200 mm 时，静态爬行是安全的；反之如果深度超过 300 mm，静态爬行的跨步距离可能够不到，就要切到对角小跑。这个切换逻辑是整个系统的灵魂，比单一步态的调参更关键。
3. 系统设计与感知端：把楼梯变成可计算的几何模型
3.1 系统总体架构
一个完整的爬楼梯控制系统分为四层：感知层、认知层、规划层、执行层。感知层负责采集深度图像、点云和惯性数据；认知层把原始数据转成几何特征，比如台阶平面、边缘线、高度差；规划层基于几何模型生成重心轨迹和足端轨迹；执行层把轨迹转换成关节扭矩指令，并通过低电平驱动器控制电机。
我一般会把这四层拆成两个独立进程，感知、认知和规划跑在上位机里，执行层跑在实时控制器里。上下位机之间的通信用共享内存或以太网总线，数据周期控制在 10 ms 以内。为什么要这么拆？因为规划层的算法经常要调试，如果和实时控制耦合在一起，每次改参数都要重启整个系统，浪费时间且容易引入状态不一致。拆开后，规划层可以随时热更新，执行层保持固定周期运行。
系统状态机至少要包含四个状态：检测楼梯、对准楼梯、攀爬中、完成退出。检测楼梯状态负责识别台阶序列，并计算机器人相对第一级台阶的位姿；对准楼梯状态会微调机器人朝向，让它正面面对楼梯，避免斜着上台阶导致落足点偏移；攀爬中状态按每级台阶逐步执行步态编排；完成退出状态负责最后一级台阶到平台时的过渡动作。这个状态机不是拍脑袋定的，每个状态都对应一组明确的感知输入和输出条件，状态切换时有缓冲逻辑，防止传感器抖动造成频繁切换。
3.2 基于点云切割的阶梯识别方法
识别楼梯最直接的方式是用深度相机获取点云，然后做平面分割。不过在楼梯场景里，平面分割不能用一份全局阈值，因为楼梯平面之间互相平行且高度不同，直接用 RANSAC 拟合容易把多个台阶拟合成同一个平面。我习惯的做法是先做区域生长分割，按法向量一致性把点云切碎成碎片，再按法向量与地面法向量的夹角归并，凡是在正负 15 度以内的平面都标记为候选台阶面。
候选面之间通过高度连续性排序，生成一条楼梯骨架线。具体过程是先对每个候选面计算质心高度，然后按深度值排序，再检查相邻面的高度差是否在合理阈值内（比如 120 mm 到 250 mm）。如果一段点云里能连续找到 3 个以上满足高度差约束的面，就判定为楼梯。这个逻辑能避免把斜坡或散乱障碍物误判成楼梯。
代码层面可以这样落：
import numpy as np from sklearn.cluster import DBSCAN def detect_stair_planes(points, normals, ground_height): # 输入：Nx3 点云、Nx3 法向量、地面参考高度 # 1. 用法向量过滤，保留与地面法向量夹角小于 15° 的平面点 ground_normal = np.array([0.0, 0.0, 1.0]) dot = np.clip(np.abs(normals @ ground_normal), 0.0, 1.0) angle = np.arccos(dot) plane_mask = angle < np.deg2rad(15) plane_points = points[plane_mask] if len(plane_points) < 20: return None # 2. 按高度分桶，统计每个高度层的点云密度 heights = plane_points[:, 2] h_min = heights.min() h_max = heights.max() bin_size = 0.03 # 3cm 高度桶 bins = np.arange(h_min, h_max + bin_size, bin_size) density, edges = np.histogram(heights, bins=bins) # 3. 寻找连续的密度峰，每个峰对应一级台阶 peak_threshold = density.mean() * 0.5 peaks = [] for i in range(1, len(density) - 1): if density[i] > peak_threshold and density[i] > density[i-1] and density[i] > density[i+1]: peaks.append((edges[i] + edges[i+1]) / 2.0) # 4. 按高度差筛选连续台阶 stair_heights = [] for p in peaks: if len(stair_heights) == 0 or p - stair_heights[-1] > 0.10: stair_heights.append(p) return stair_heights这段代码做了三件事：先用法向量把非水平面滤掉，然后按高度分桶，把点云密度峰当作候选台阶面，最后按高度差筛选连续序列。参数bin_size=0.03决定了高度分辨率，如果台阶表面有杂物或噪声，可以适当调大到 0.04。peak_threshold用的是全局均值的一半，这个阈值在点云密度不均时会导致漏检，更稳的方式是先做一次直方图平滑，再取局部极值。
3.3 姿态对齐与台阶边缘定位
检测到台阶面还不够，机器人需要知道台阶边缘的精确位置，因为落足点必须落在台阶内侧，不能踩在边缘的悬空带上。台阶边缘就是相邻两级台阶的交线，它对应点云在水平方向上的突变位置。一种可靠的做法是，对每个台阶面提取一个矩形轮廓，然后用相邻两个矩形轮廓的相邻边拟合出边缘线。
实际场景里，点云边缘往往带噪声，直接拟合出的直线会抖动。我会在直线拟合前加一步中值滤波，把边缘附近的点沿水平方向排序，滑动窗口取中值，消除离群点。这个操作成本很低，但能让落足点稳定性显著提升。边缘定位的另一个关键点是判断机器人与楼梯中轴线的夹角，如果夹角过大，需要先调整机身的偏航角，否则后腿跨步时很容易碰到台阶侧壁。
感知模块输出给规划层的不是一堆点云，而是紧凑的几何描述子：每级台阶的前缘线方程、高度、深度，以及楼梯的总阶数。规划层只需要读这些参数就能生成步态序列，不需要重新处理点云。这个抽象层非常重要，它把感知和控制解耦，也让仿真环境和真机之间的迁移变得顺畅——在仿真里可以直接用地面真值生成同样的描述子，控制逻辑不用改一行代码。
4. 步态编排与重心规划：让每一步踩得稳又准
4.1 步态周期与相位分配
爬楼梯的步态编排本质上是一个周期性的时间规划问题。每级台阶的攀爬过程可以分成四个阶段：前腿抬起、前腿落足、后腿抬起、后腿落足。静态爬行步态里，这四个阶段严格串行，每个阶段开始时重心先移到剩余三条腿围成的支撑区中心，然后才允许移动目标腿。这个顺序不能乱，一旦重心没到位就抬腿，机器人会瞬间失去稳定。
单级台阶攀爬的典型时间分配是：重心调整 0.35 秒，抬腿摆动 0.55 秒，落地缓冲 0.25 秒，总周期约 1.15 秒。步幅大小取决于台阶深度和机器人腿长，一般设定为台阶深度的 0.8 倍，留出 20% 的余量给落足误差。若台阶深度只有 220 mm，步幅就是 176 mm，这时前腿的摆腿高度要降低，避免踢到上一级台阶的竖直面。
相位分配还有一个容易犯的错：把四条腿的抬腿顺序固定成右前、左后、左前、右后，这在平地没问题，但在楼梯上因为前后腿所处高度不同，重心调整的幅度不一样。如果固定顺序，经常出现后腿还没完全抬起，前腿已经开始摆动，导致躯干扭转。我一般会让顺序跟着感知到的楼梯坡度自适应：坡度大时先抬前腿，坡度小时先抬后腿。
4.2 重心轨迹生成的简化模型
爬楼梯的重心轨迹不需要用复杂度很高的全身动力学优化，一个带约束的倒立摆模型就能跑得很好。把机器人简化成躯干质心和四条腿的支撑点，重心轨迹的目标是保证每个时刻的重心投影落在当前三条支撑腿构成的三角形内，同时躯干保持接近水平或轻微前倾。
我常用的做法是把重心轨迹规划问题转成一个二次规划（QP），目标函数是追踪一条参考轨迹，同时约束重心位置、速度和加速度的边界。参考轨迹怎么定义？可以先用一个简单的斜坡函数预测重心在竖直方向的变化——上楼梯时重心整体上移，但每一级台阶之间会有小的上下波动，波动幅度控制在 3 mm 以内。这样机器人看起来爬得又稳又顺，不会像丧尸一样一顿一顿。
落地时还有一个关键参数：落地速度。前腿落到下一级台阶的瞬间，垂直速度要控制到接近零，否则冲击力会超过执行机构的能力边界。我会在轨迹上做个梯形速度曲线，最后 100 ms 内把垂直速度从峰值降到 0.05 m/s 以下。代码上可以这样实现：
def generate_foot_trajectory(start_pos, end_pos, height, duration, dt): """生成足端 5 次多项式轨迹，保证落地速度为零""" t = np.arange(0, duration, dt) t_norm = t / duration # 5 次多项式系数，边界条件：初末位置、初末速度、初末加速度 # 位置：0 -> 1，速度：0 -> 0，加速度：0 -> 0 coeff = np.array([ [1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 2, 0, 0, 0], [1, 1, 1, 1, 1, 1], [0, 1, 2, 3, 4, 5], [0, 0, 2, 6, 12, 20] ]) b = np.array([0, 0, 0, 1, 0, 0]) a_params = np.linalg.solve(coeff, b) # 标量轨迹，映射到三维空间 pos_x = start_pos[0] + (end_pos[0] - start_pos[0]) * np.polyval(a_params, t_norm) pos_y = start_pos[1] + (end_pos[1] - start_pos[1]) * np.polyval(a_params, t_norm) pos_z = start_pos[2] + height * np.sin(np.pi * t_norm) # 生成轨迹数组 trajectory = np.vstack([pos_x, pos_y, pos_z]).T return trajectory这里用 5 次多项式保证速度和加速度边界条件，垂直方向叠加一个半正弦波来模拟抬腿高度。注意height是抬腿峰值，不是台阶高度，通常设成台阶高度的 1.2 倍，留出越过边缘的余量。dt要和控制周期匹配，若是 200 Hz 控制则 dt=0.005。
4.3 摆动腿与支撑腿的协调控制
在攀爬过程中，支撑腿需要做两件事：维持躯干高度、主动吸收冲击。常见的控制律是位置-力混合控制，支撑腿期望位置按躯干高度参考值计算，同时把接触力限制在一个范围内。如果接触力超过上限，就让该腿的虚拟弹簧变软，避免硬冲击。
摆动腿方面，最重要的是防止摆腿中途碰到台阶竖壁。解决思路是让摆动轨迹有一个安全平面，该平面由当前支撑高度和下一级台阶高度之间的一条斜线组成。具体做法是在轨迹生成后做一次碰撞检查，把轨迹上的每个点与楼梯几何模型求交，如果交点存在，就提高抬腿高度重新生成。
当然，协调控制逃不开一个工程现实：腿与躯干之间有柔性形变，导致实际位置和运动学计算位置有偏差。我会在支撑腿的控制环里加一个估算的等效连杆长度补偿量，通过每次落地瞬间的接触力反馈在线更新。这个补偿量的初始值设为 0，经过几次落地后收敛到稳定值，系统鲁棒性会有明显提升。
4.4 状态机里的异常分支
步态状态机不能只处理正常情况。我在状态机里一共设计了六个异常分支：落足超时、躯干倾角超限、支撑腿滑移、重心偏移过大、感知丢失、执行器饱和。每个分支触发后，系统要做的不是原地崩溃，而是执行一套安全序列。比如落足超时，系统会暂停当前步态，保持当前支撑腿不动，重新执行一次感知，尝试修正落足位置；如果第二次依然失败，就降低躯干高度，退回上一级台阶。
这套异常分支在仿真里不会触发，但真机调试时几乎每轮测试都会碰到至少一种。所以状态机的实现必须保证每个分支的退出条件明确，不能出现死锁。我通常会在状态机里加入一个 watchdog 计时器，任何状态超过设定的最长时间都会被强制切换回安全状态。这个设计看似简单，却能避免机器人卡在中间姿态导致电机过热。
5. 爬楼梯控制系统的参数整定与避坑指南
5.1 必调的五个核心参数及其影响
第一个是躯干俯仰角。爬楼梯过程中躯干保持水平还是前倾，对后腿步距影响明显。我推荐前倾 5～8 度，这样后腿的髋关节自然前移，增加了跨步距离，但同时要降低重心高度，否则前倾会减小稳定裕度。第二个是重心前移比例，定义为重心在支撑多边形上的投影点沿前进方向的比例位置。起步时设为 0.5，前腿摆动前要移到 0.6 左右，后腿摆动前要移回 0.4。这个参数错了，机器人会摔倒或步态僵硬。
第三个是支撑腿阻抗参数。虚拟刚度拟设为 k=250 N/m，阻尼系数设为 15 N·s/m。刚度太大会导致躯干震动，太小则在倾斜面上支撑不住。第四个是摆动腿的抬腿高度，它必须大于台阶物理高度，同时又要小于台阶顶端到机器人躯干底面之间的距离。这个边界需要实测，不同型号机器人差别很大。第五个是落足点安全冗余，即目标落足点离台阶前缘的距离，室内台阶 20 mm，户外台阶 40 mm。冗余太大容易踩到台阶后缘的不稳定区域，太小则容易踩空。
5.2 真机调试的典型失败循环
真机上的失败不是一次性的，它会形成循环。最常见的是：机器人前腿碰到台阶竖壁，导致躯干后仰，然后后腿在下级台阶边缘打滑，整个身子歪向一侧，触发摔倒保护。这个循环的根源往往不是控制参数，而是感知给到的台阶深度偏小。因为深度相机视角是斜向下的，台阶竖壁会遮挡一部分水平面，点云拟合出来的台阶深度比实际小 30 mm 左右。解决方法是做一个感知补偿表，按当前相机俯仰角查表加上深度补偿值，或者直接换用正视深度相机加 IMU 融合的方案。
另一个常见失败循环是机器人越爬越歪。原因是对准楼梯这一步做得太粗，初始偏航角误差超过 3 度，且后续步态没有修正偏航的能力。我的解法是在每一步落足后，用腿部的力传感器估算躯干的偏航力矩，把这个量反馈到下一步的重心横向调节中。这种修正方式比单纯靠视觉更鲁棒，因为它不受光照影响。
5.3 排查路径与关键日志索引
如果爬楼中途摔倒，我会按下面的顺序排查：先看状态机日志，确认触发摔倒保护的状态名；再看 IMU 记录的躯干俯仰角曲线，如果角速度变化异常，多半是步态切换时支撑腿数目不够；然后看各腿接触力，找出哪只腿先失去支撑；最后反查感知模块的台阶序列，看台阶高度是否出现了跳变。
日志索引很关键。我一般在每步执行时记录一个step_log，包含步序号、目标落足点坐标、实际落足点坐标、偏差量、躯干倾角、支撑腿接触力均值、状态机停留时间。有了这个数据，回放任何一次失败都能判断是规划问题、感知问题还是执行问题。实际调试中，70% 的摔倒源于感知延迟，15% 源于状态机逻辑缺陷，剩下的才是控制参数问题。
5.4 防摔倒策略：安全优先于性能
爬楼梯控制系统的最后一道防线是紧急防摔倒策略。它不参与正常步态，而是持续监控稳定裕度，一旦低于阈值就触发。触发后的动作序列是：立刻锁住所有支撑腿的位置，让躯干以当前姿态静止，然后降低躯干高度 10 mm，增加支撑面积，最后反向摆动身体试图回到稳定域。这个策略不能保证所有情况下都救得回来，但能把摔倒概率降低一个数量级。
防摔倒策略的难点在于阈值的设定。阈值太高，正常爬行时也会频繁误触发，导致机器人动不动就停住；阈值太低，真摔了才触发，失去了保护意义。我的经验是先统计正常爬行 20 次过程中的最小稳定裕度，取它的 0.7 倍作为触发阈值。这样做既不会误触发，又能覆盖一定程度的扰动。
6. 从单级台阶到连续楼梯：进阶技巧与验证方法
6.1 连续楼梯中的步态衔接
当从第二阶段走到第三阶段时，前腿已经踩在较高一级，后腿还在较低一级，整个机身处于倾斜状态。这时候如果直接按平地步态推进，会出现一个明显的顿挫。解决技巧是引入「过渡步态」：在前腿落足的瞬间，后腿不急着抬，而是先做一个微小的后蹬动作来拉直躯干。这个动作让机身重新回正，再进入下一级循环。过渡步态持续约 0.2 秒，成本很低，但对整体流畅度贡献极大。
6.2 在仿真环境里批量整定参数的推荐流程
仿真器的最大价值不是写 demo，而是可以做参数扫描。我把前面说的五个核心参数放到一个网格空间里，每个参数选 4 档，共 4^5 = 1024 组组合，在仿真楼里自动跑。每组给一个累计分数：完成整段楼梯得基础分，每摔倒一次扣分，每顿挫一次扣分。筛选出分数最高的前 20 组，再拿到真机上去微调。这个方法能大幅压缩现场调参的时间。需要注意，仿真和真机的摩擦系数、关节响应时间不同，仿真整定的参数不能直接照搬，但能提供非常有价值的初始点。
6.3 验证爬楼梯性能的三个指标
评价一套爬楼梯控制方法好不好，不要只看爬不爬得上去。我会看三个指标：成功爬完整段楼梯的连续测试次数；单级台阶的平均耗时；以及爬楼过程中的躯干最大俯仰角变化率。第一个指标衡量鲁棒性，第二个衡量效率，第三个衡量稳定性。性能好的系统，连续测试 20 次成功率达到 80% 以上，单级耗时 1.1 秒左右，最大俯仰角变化率小于 60 度每秒。如果某个指标相差很大，直接按对应方向去调参，不会是无头苍蝇。
另一个经常被忽略的验证方向是「失败后的恢复能力」。让机器人在爬楼过程中被人为踢一脚，看它需要多久才能回到稳定步态。这个测试能暴露控制器的适用范围边界，比单纯刷成功率更能说明工程成熟度。我在一次内部测试中，用这个方法发现整定好的参数在面对侧向扰动时恢复时间过长，后来在重心横向比例里加入一个积分修正项，恢复时间从 0.8 秒缩短到 0.35 秒。
6.4 一点经验之谈
爬楼梯控制方法和系统，最终考验的并不是哪一个华丽算法，而是感知、规划、执行之间的一致性。感知给到的台阶高度是 180 mm，规划走了 190 mm 的抬腿，执行时又因为腿部柔性丢失了 10 mm，这个链条上任何一个环节出差，都会在落足瞬间暴露出来。我在迭代过程中吃过不少亏，印象最深的一次是花了整整两天调一个前腿弹跳问题，最后发现是感知的时戳同步差了 60 ms，导致步态规划一直在用旧数据。那次以后，我所有模块的时间戳都统一用控制器主时钟，任何传感数据都必须带时间戳才能进入规划器。希望这套方法能帮你在做四足落地时少走几步弯路，也希望你在真机上遇到问题时，先怀疑时间同步和滤波器参数，而不是上来就重写步态算法。
本文还有配套的精品资源，点击获取
