1. 引子：凌晨三点，桌子下有个东西在响

那天凌晨三点，我又一次被一阵"咔嗒咔嗒"的声音吵醒。

循着声音找过去，是我工作台下面那只拼了三个月的四足机器人。它没倒下，四条腿稳稳地站着，伺服电机发出微弱的电流声，像一只耐心的机械犬在等我。我蹲下来，和它平视，忽然意识到：三个月的折腾里，真正让我着迷的从来不是"它终于能走了"这个结果，而是每一个想不通的深夜、每一张画烂的草稿、每一次把代码改成四不像之后又推翻重来的过程。

所以我想把这条路完整地走一遍给你看——不是给你一份可以直接复制的"标准答案"，而是把我走过的坑、想通的道理、画过的图、删过的代码，都摊开来讲。这篇文章很长，两万多字，适合泡一杯茶慢慢读。你会看到机械结构怎么选、电路怎么搭、步态怎么想、代码怎么调，也会看到一个人怎么从"完全不懂"走到"能解释清楚每一步"。

我们开始。

2. 为什么是四足？一个值得想清楚的问题

做机器人之前，最容易犯的错误是：还没想清楚"为什么"，就先动手买了一大堆零件。我见过很多人花了不少钱买舵机和开发板，最后堆在角落里吃灰，原因是他们真正想要的其实是一个"会动的玩具"，而不是一个自己设计的机器人。

2.1 轮子、履带和腿的区别

先从直觉说起。轮子最省事，但只能在平坦的地面上高效移动；履带能过复杂地形，但笨重、能耗高、转向不灵活。腿呢？腿的魔力在于：它可以主动选择落点。

想象你在翻越一堆碎石。轮子会被石头卡住，履带会碾过去但很费劲，而腿可以精确地踩在石头之间的空地上。四足机器人相对双足的优势也很明显：静态稳定。四条腿任何时候都有至少三条腿支撑地面，即使没有复杂的平衡算法，也能保持不摔倒——这对初学者是极大的友好。

2.2 四足 vs 六足 vs 双足

• 双足：最像人，但平衡极难，初学者大概率做成"一推就倒"，挫败感极强。
• 六足：稳定容易，但结构复杂、成本翻倍，而且"走路"看起来更像昆虫，少了些机械犬的美感。
• 四足：恰好卡在中间。结构不复杂，静态稳定可做，动态步态（小跑、跳跃）后期也能挑战。是性价比最高的学习平台。

2.3 一次真诚的自我定位

动手前我用一张纸写下了三个问题：

1. 我要做静态稳定（慢走不摔）还是动态平衡（小跑）？
2. 我的预算大概是多少？
3. 我更想学"机械"还是"控制算法"？

答案是：静态稳定 + 中等预算 + 主攻控制算法。这个定位决定了我后面所有选择。如果你和我一样，这篇文章就是为你写的。

3. 总体架构：先把地图画出来

任何系统在动手前，都应该先有一张"总览图"。我把它画成了下面这样。这幅图陪了我三个月，每当陷入细节迷失方向时，我就回来看一眼，提醒自己"我现在在哪个格子里"。

这张图其实回答了最核心的一个问题：四足机器人不是一个"纯软件"项目，也不是"纯硬件"项目，而是一个软硬强耦合的系统。机械决定了物理上限，算法决定了你能多接近这个上限。任何只重其一的做法都会让你在某个环节卡死。

3.1 我踩的第一个坑：顺序错了

我最开始的想法是"先写算法，跑通了再搭硬件"。结果在仿真里走得飞起的步态，一上真机就变成"满地打滚"。原因很简单：仿真里的腿是理想化的，没有摩擦、没有间隙、没有舵机死区。后来我调整了顺序：先搭出一台能站住的机械平台，再在真机上调算法。这个顺序的改变，少走了至少一个月的弯路。

3.2 迭代的节奏：小步快跑

我的开发节奏是：

1. 先用纸板+最便宜的舵机搭一个"能动的概念验证"；
2. 升级到 3D 打印件 + 金属舵机；
3. 最后才是结构优化和动态步态。

记住一句话：第一版的目标是"能走"，不是"完美"。很多人在第一版上花了太多时间打磨，结果迟迟看不到成果，热情就耗尽了。

4. 机械结构：从一张草图说起

机械结构是整台机器人的"骨架"。它决定了腿的长度、关节的布局、重量的分布，也间接决定了后面控制算法的难度。

4.1 腿的构型：每条腿几个关节？

四足机器人最经典的腿构型是每条腿三个关节：

• 髋关节（Hip）：控制腿前后摆动；
• 大腿关节（Thigh）：控制腿上下抬落；
• 小腿关节（Calf/膝关节）：控制小腿屈伸。

三个关节对应三个自由度，让足端能在三维空间里到达任意一点。这是"通用"的方案，也是大多数开源四足（如 Stanford Doggo、Unitree 系列）采用的构型。

4.2 关节布局的两种流派

• 同轴串联：三个关节的旋转轴互相垂直，舵机依次堆叠。结构简单，但腿的"大腿"部分会很长，惯量大。
• 并联/半并联：用连杆机构把两个舵机布置在身体上，通过连杆把运动传递到腿。结构紧凑、惯量小，但设计和加工复杂。

初学者强烈建议选同轴串联，因为直观、好算、好维护。等第一版跑通了，再考虑并联优化。

4.3 一个关键尺寸：腿长怎么定？

腿长不是随便定的。太长，惯量大、力矩需求高；太短，步幅小、越障能力弱。我的经验公式是：

单腿完全伸展长度 ≈ 身体长度 × 0.9 ~ 1.2

我的第一版身体长 200mm，单腿长约 190mm（髋到足端）。这个比例让机器人的"身体"在走路时不容易擦地，同时整体高度合适，重心不会太高。

4.3.1 材料选择的现实考量

• 第一版（概念验证）：纸板 + 热熔胶，成本几乎为零，一天就能搭完，目的是验证"腿能动"。
• 第二版：PLA 3D 打印件，强度够做缓慢行走，能反复修改模型。
• 第三版（可选）：碳纤维板 + 铝合金件，减重、增刚，适合动态步态。

记住：材料是为目标服务的。如果你只想验证代码，纸板和打印件完全够用，不要一上来就上碳纤维。

5. 硬件选型：让"神经系统"跑起来

机械结构是骨骼，硬件电路就是神经系统。这一节我会讲清楚"为什么选这个"，而不是简单列一张购物清单。

5.1 主控：选单片机还是单板机？

类型	代表	优点	缺点
单片机 MCU	STM32、ESP32	实时性好、功耗低、稳定	算力有限、调试不如 Linux 方便
单板机 SBC	树莓派	算力强、生态好、能跑 Linux	实时性弱、启动慢、功耗高

我的选择是 STM32 做主控，因为步态控制对"实时性"要求极高。控制周期是毫秒级甚至更短，树莓派的 Linux 内核调度很难保证这一点（除非上实时补丁）。

5.2 舵机：机器人的"肌肉"

舵机分两类：

• PWM 舵机：便宜、简单，通过 PWM 脉宽控制角度。缺点是没有反馈，你不知道它实际转到哪了；而且死区较大，精度一般。
• 总线舵机（串行舵机）：支持位置反馈、可设置多个参数、精度高，通过 UART 总线控制，比如某个常见型号。缺点是贵。

我第一版用 PWM 舵机，因为它便宜、控制方式简单。这样舵机系统能快速跑起来，把精力留给算法。等步态跑通了，再升级总线舵机以获得力矩和位置反馈。

核心思维：硬件选型要遵循"够用就好"原则。过早追求高级硬件，只会让你在"驱动还没调通"这件事上消耗热情。

5.3 传感器：机器人的"感觉"

第一版需要的最小传感器组合：

1. IMU（惯性测量单元）：提供姿态角（俯仰、横滚、偏航）和角速度，用于判断身体是否倾斜。
2. 足端接触开关：判断哪只脚落地了，对步态触发至关重要。
3. 电池电压检测：防止过放。

第二版可以加：

4. 关节编码器（总线舵机自带）：得到真实关节角度，做闭环。
5. 力传感器：测量足端接触力，实现柔顺控制。

5.4 电源：容易被忽略的命脉

舵机是很"暴力"的负载：堵转时瞬间电流可能高达好几个安培。如果用单路电源同时给主控和舵机供电，舵机一动作，电压骤降，主控就会复位。我的方案是双电源隔离：

• 一路给舵机（比如 7.4V 大电流电池），
• 一路给主控和传感器（通过稳压到 5V/3.3V）。

两地线共地，但电源分开。这个细节能避免大量"莫名重启"的灵异问题。

6. 运动学：让数学把腿"算"出来

这是整篇文章最核心的理论部分。别怕，我会用讲故事的方式把它讲透。

6.1 什么是运动学？

简单说，运动学回答两个相反的问题：

• 正运动学：已知各关节角度，求足端在空间中的位置。
• 逆运动学：已知足端目标位置，求各关节需要转多少度。

对四足机器人来说，我们最常用的是逆运动学。因为步态规划时，我们想的是"把脚放到哪个点"，而不是"关节转几度"。

6.2 建立坐标系

每条腿都可以看作一个独立的"三关节机械臂"，只是它的"基座"长在身体上。我们先研究一条腿，设：

• 髋关节在身体上的安装点为原点 O；
• 大腿长度为 L1，小腿长度为 L2；
• 足端在身体坐标系中的目标位置为 (x, y, z)。

其中 x 方向是前进方向，y 方向是身体侧向，z 方向是向下。

6.3 逆运动学的几何推导

第一步，算髋关节角度 θ1。从俯视图看，足端在水平面上的投影到原点的连线，与身体侧面之间的夹角，就是髋关节需要转的角度：

θ1 = atan2(x, y)

这里用 atan2 而不是 atan，是因为 atan2 能正确处理四个象限，不会出现"脚在身后却算成身前"的错误。

第二步，把三维问题"压缩"成二维。髋关节转完后，剩下的问题就变成了一个平面内的两连杆问题。设足端到髋关节的水平距离为 D：

D = sqrt(x² + y²)

再考虑高度 z，我们可以在"腿所在的竖直平面"内解出大腿角 θ2 和小腿角 θ3。设足端到髋关节的直线距离为 R：

R = sqrt(D² + z²)

在由 L1、L2、R 构成的三角形里，用余弦定理求大腿关节角：

α = acos((L1² + R² - L2²) / (2 × L1 × R))
β = atan2(z, D)
θ2 = α + β

小腿关节角：

γ = acos((L1² + L2² - R²) / (2 × L1 × L2))
θ3 = γ - π   （取决于你的关节零位定义）

这些公式的具体符号取决于你机器人的机械零位定义。不要死记公式，要理解几何关系：本质上就是"两个固定长度连杆，末端够到一个目标点，求两个关节角"。

6.4 核心代码：逆运动学求解

下面是这段推导的 Python 实现。每一条腿只需要调用一个函数，传进去足端目标坐标和腿长，就能得到三个关节的目标角度。

import numpy as np

def inverse_kinematics(foot_pos, L1, L2):
    """
    单腿逆运动学求解
    foot_pos: (x, y, z)，足端在髋关节坐标系下的目标位置
    L1: 大腿长度
    L2: 小腿长度
    返回: (theta1, theta2, theta3) 关节角度(弧度)
    """
    x, y, z = foot_pos

    # 髋关节角度（水平面旋转）
    theta1 = np.arctan2(x, y)

    # 足端到髋关节的水平距离
    D = np.sqrt(x**2 + y**2)

    # 足端到髋关节的直线距离
    R = np.sqrt(D**2 + z**2)

    # 防止超出机械臂可达范围
    if R > (L1 + L2) or R < abs(L1 - L2):
        raise ValueError("目标点超出腿的可达空间！")

    # 大腿关节：由两部分角度组成
    cos_alpha = (L1**2 + R**2 - L2**2) / (2 * L1 * R)
    cos_alpha = np.clip(cos_alpha, -1.0, 1.0)
    alpha = np.arccos(cos_alpha)
    beta = np.arctan2(z, D)
    theta2 = alpha + beta

    # 小腿关节（膝关节）
    cos_gamma = (L1**2 + L2**2 - R**2) / (2 * L1 * L2)
    cos_gamma = np.clip(cos_gamma, -1.0, 1.0)
    gamma = np.arccos(cos_gamma)
    theta3 = gamma - np.pi  # 零位修正，依机械结构而定

    return theta1, theta2, theta3

6.4.1 正运动学：反过来算一遍

有了逆运动学，我们还要理解正运动学：已知三个关节角，求足端在哪里。虽然步态控制主要用逆运动学，但正运动学是调试和仿真的利器——它能告诉你"舵机实际转了多少度"应该对应"脚在什么位置"。

直接基于几何关系就能写出来：

import numpy as np

def forward_kinematics(theta1, theta2, theta3, L1, L2):
    """
    单腿正运动学：已知关节角，求足端位置
    theta1: 髋关节角度（绕竖直轴，rad）
    theta2: 大腿关节角度（绕水平轴，rad）
    theta3: 膝关节角度（rad）
    返回: (x, y, z) 足端在髋关节坐标系下的位置
    """
    # 腿在竖直平面内的投影长度
    planar_len = L1 * np.sin(theta2) + L2 * np.sin(theta2 + theta3)

    # 髋关节在水平面内的旋转
    x = planar_len * np.sin(theta1)
    y = planar_len * np.cos(theta1)
    z = L1 * np.cos(theta2) + L2 * np.cos(theta2 + theta3)

    return x, y, z


# 自检：逆运动学 + 正运动学 应该还原原始目标点
def self_check():
    L1, L2 = 0.08, 0.11
    target = (0.05, 0.06, -0.14)  # 随便选一个可达点
    theta1, theta2, theta3 = inverse_kinematics(target, L1, L2)
    recovered = forward_kinematics(theta1, theta2, theta3, L1, L2)
    print("目标点:", target)
    print("还原点:", tuple(round(v, 5) for v in recovered))


self_check()

这段 self_check() 是我每次改完运动学代码后必跑的自检。正运动学是逆运动学的"镜子"——两个函数互相验证，能快速揪出符号错误、零位错误、长度单位不统一这类隐蔽问题。

6.4.2 一条腿的几何参数表

把这一堆变量整理成表，后面看代码就不会晕：

变量	含义	说明
L1	大腿长度	髋关节到膝关节的距离
L2	小腿长度	膝关节到足端的距离
θ1	髋关节水平旋转角	负责前后摆腿
θ2	大腿关节抬落角	负责上下抬腿
θ3	膝关节屈伸角	负责小腿折叠
D	足端到髋关节水平距离	由 sqrt(x²+y²) 算出
R	足端到髋关节直线距离	由 sqrt(D²+z²) 算出

同步再画一张腿部简化关系图，把几何量串起来：

6.5 一个重要的思维：可达空间

在调代码时，最常出现的报错就是"目标点超出腿的可达空间"。这其实是个几何问题：一条腿能伸到的最远距离是 L1 + L2（完全伸直），最近距离是 |L1 - L2|（完全折叠）。步态规划时，每一步的足端目标点都必须落在这个范围内，否则机器人的腿会"够不着"，出现关节锁死或舵机堵转。

这引出了一个高级话题：工作空间分析。你可以在设计阶段就用 Python 把腿的可达空间可视化出来，看看步态轨迹有没有越界。这是"软件反哺机械设计"的典型例子。

6.5.1 用代码画出可达空间

光靠想象很难理解"可达空间"，不如直接画出来。下面的代码用穷举法扫描所有可能的关节角组合，把足端能到达的每个位置画成一个点。你会看到一个扇形或月牙形的二维区域——这就是腿的工作空间：

import matplotlib.pyplot as plt
import numpy as np


def plot_workspace(L1, L2, show_workpoints=None):
    """
    通过穷举关节角，绘制单腿在竖直平面内的可达空间
    show_workpoints: 可选，叠加显示步态轨迹点
    """
    theta2_range = np.linspace(-np.pi/2, np.pi/2, 250)  # 大腿角范围
    theta3_range = np.linspace(-np.pi, 0, 250)          # 膝关节范围

    xs, zs = [], []
    for t2 in theta2_range:
        for t3 in theta3_range:
            x = L1 * np.sin(t2) + L2 * np.sin(t2 + t3)
            z = L1 * np.cos(t2) + L2 * np.cos(t2 + t3)
            xs.append(x)
            zs.append(z)

    plt.figure(figsize=(8, 6))
    plt.scatter(xs, zs, s=1, c='steelblue', alpha=0.3, label='可达空间')
    if show_workpoints is not None:
        wx, wz = zip(*show_workpoints)
        plt.scatter(wx, wz, s=20, c='red', marker='o', label='步态轨迹点')
        plt.legend()
    plt.xlabel('水平距离 x (m)')
    plt.ylabel('竖直高度 z (m)')
    plt.title(f'单腿可达空间 (L1={L1}m, L2={L2}m)')
    plt.grid(True, linestyle='--', alpha=0.5)
    plt.axis('equal')
    plt.show()


# 示例：画出 L1=0.08, L2=0.11 的腿可达空间
if __name__ == "__main__":
    plot_workspace(L1=0.08, L2=0.11)

这个图帮我做了三个重要决策：

1. 验证步态轨迹不会越界：把第 7 章生成的足端轨迹点叠加在可达空间上，如果所有红点都落在蓝色区域内，说明步态安全；
2. 选择站立高度：找到可达空间中"前后范围最宽"的高度区间，让机器人有最大的水平移动余量；
3. 优化腿长比例：改变 L1 和 L2 的比例，观察可达空间形状的变化，反推机械结构设计。

一次真实经历：我第一版选的 L1=0.06, L2=0.13，画出来发现可达空间又窄又高，支撑时根本没法前后迈步。改成 L1=0.08, L2=0.11 后，空间瞬间变宽。那一刻我真正理解了什么叫"软件反哺机械设计"——几行 Python 帮我省下了一整轮打样。

7. 步态规划：四条腿的"舞蹈编排"

有了逆运动学，我们能控制"脚放到哪"。但四条腿什么时候、按什么顺序抬脚落脚，这就是步态规划，是四足机器人最有意思的部分。

7.1 静态步态：永远有三角形支撑

四足机器人能做静态稳定的核心是：任意时刻至少有三条腿支撑地面，支撑腿构成的三角形要把身体重心投影包在里面。这样即使没有任何主动平衡，机器人也不会翻倒。

最常见的静态步态是 爬行步态（Crawl Gait），抬腿顺序为：

1 → 4 → 2 → 3  （或 1 → 3 → 2 → 4）

其中腿的编号通常为：左前 = 1，右前 = 2，左后 = 3，右后 = 4。

注意抬腿顺序要满足：不能同时抬起同侧或对角的相邻两条腿，否则重心会移出支撑三角形。经典顺序 1-4-2-3 就是先抬左前和右后（对角），再抬右前和左后（另一条对角），这样任何时刻都保持三条腿着地。

7.1.1 支撑三角形：稳定性的秘密

为什么"任何时候至少三条腿着地"就能稳定？原因是：三条腿的落点构成一个三角形，只要机器人重心在地面的投影落在这个三角形内部，重力就无法把机器人翻倒。把这画成图：

这是整篇文章里最重要的一张图。四足机器人的静态稳定，不是靠算法"挣扎着保持平衡"，而是靠机械编排保证任意时刻都有稳固的三角形支撑。双足机器人没有这个"三角形兜底"，所以平衡控制难度完全不同。理解了这一点，你就明白了为什么四足是初学者的最优解。

7.1.2 用代码实现接触检测

接触开关装在足端，落地时输出电平翻转。下面是把接触检测和步态触发结合起来的核心逻辑：

// 读四条腿的接触开关，返回当前着地状态
// 返回值的 bit0~bit3 分别对应 LF, RF, LH, RH
// bit 为 1 表示该腿已经着地
uint8_t read_contact_flags(void)
{
    uint8_t flags = 0;
    // CON1~CON4 是连接足端开关的 GPIO 引脚
    if (HAL_GPIO_ReadPin(CON1_GPIO_Port, CON1_Pin) == GPIO_PIN_SET) flags |= 0x01;
    if (HAL_GPIO_ReadPin(CON2_GPIO_Port, CON2_Pin) == GPIO_PIN_SET) flags |= 0x02;
    if (HAL_GPIO_ReadPin(CON3_GPIO_Port, CON3_Pin) == GPIO_PIN_SET) flags |= 0x04;
    if (HAL_GPIO_ReadPin(CON4_GPIO_Port, CON4_Pin) == GPIO_PIN_SET) flags |= 0x08;
    return flags;
}

// 检查指定腿是否已经落地，决定是否可以结束摆动相
uint8_t is_leg_contacted(uint8_t contact_flags, uint8_t leg_mask)
{
    return (contact_flags & leg_mask) != 0;
}

实际工程中，我会把接触检测做成去抖动 + 最小保持时间。原因有两个：接触开关有机械抖动，瞬间可能通断多次，需要连续多次采样确认；另外，有些腿可能在摆动中途碰到小石子瞬间接触地面，不能一有接触就切换状态，需要设置一个最短着地时间（比如 20ms）才认为"真正落地"。这套细节做得好，机器人过不平整地面的能力会强很多。

7.2 步态的本质：一个相位机

步态规划其实可以抽象成一个有限状态机。每个状态对应"哪条腿在摆动、哪条腿在支撑"，状态之间按固定顺序切换。我把这个想法画成了下面的图：

但"状态"只是宏观描述，真正驱动腿动的是足端轨迹。

7.3 足端轨迹：抬脚不是直上直下

初学者最容易犯的错误是：让脚"直着抬起来、直着放下去"。这会导致两个问题：一是抬脚和落脚瞬间有冲击，二是脚在空中"蹭"到地面。

正确的做法是给足端设计一条光滑的曲线轨迹，比如半椭圆或摆线轨迹。以半椭圆为例：

• 摆动相：足端从后方抬起到前方落下，走一条上拱的弧线；
• 支撑相：足端从前方贴地推到后方，走一条直线。

半椭圆轨迹的参数方程可以这样写（以步频 f、步幅 S、抬腿高度 H 为参数）：

import numpy as np

def foot_trajectory(t, phase, stride_length, step_height, frequency):
    """
    生成单腿足端轨迹
    t: 当前时间(秒)
    phase: 该腿的相位偏移(0~1)
    stride_length: 步幅(米)
    step_height: 抬腿高度(米)
    frequency: 步频(Hz)
    返回: 足端在身体坐标系下的目标位置(x, y, z)
    """
    # 归一化周期位置
    cycle = (t * frequency + phase) % 1.0

    # 摆动相：占周期的 0 ~ 0.35（可调）
    if cycle < 0.35:
        s = cycle / 0.35  # 0 -> 1
        # 前进方向：从 -S/2 到 +S/2
        x = -stride_length/2 + stride_length * s
        # 高度：上拱弧线，落地时高度为0
        z = step_height * np.sin(np.pi * s)
    else:
        # 支撑相：占周期的 0.35 ~ 1.0
        s = (cycle - 0.35) / 0.65  # 0 -> 1
        # 前进方向：从 +S/2 回到 -S/2（相对身体向后推）
        x = stride_length/2 - stride_length * s
        # 贴地
        z = 0.0

    # 侧向偏移 y 在静态步态中通常固定为站立宽度的一半
    y = 0.0
    return x, y, z

这段代码是步态规划的核心。每一条腿都有自己的 phase（相位），四条腿的相位错开，就形成了步态。比如爬行步态的四腿相位可以设为：

LF: 0.00
LH: 0.50
RF: 0.50
RH: 0.00

或者按 1-4-2-3 顺序：

LF: 0.00
RH: 0.25
RF: 0.50
LH: 0.75

7.4 步态参数与身体姿态的联动

步态生成了"脚在身体坐标系下的目标位置"，但机器人身体本身也会动（前倾、侧倾）。所以完整的步态规划流程是：

1. 根据步态参数，计算每条腿足端在身体坐标系下的目标位置；
2. 结合 IMU 测得的身体姿态，把目标位置变换到世界坐标系（或地面坐标系）；
3. 固定身体在世界系中的期望高度和姿态，反解出每条腿需要的关节角度；
4. 把角度发给舵机执行。

这个流程可以画成：

8. 动力学与控制：让动作"听话"

运动学解决"脚去哪"，控制解决"脚怎么去"。这一节讲两个层次：舵机自身的控制，以及身体姿态的闭环控制。

8.1 舵机控制：从开环到闭环

PWM 舵机本质是开环角度控制：你给它一个脉宽，它内部有电位器闭环，会努力转到对应角度。但它的闭环是内部的、不可见的，而且响应慢、精度低。

对第一版来说，直接发目标角度就够了。但要注意两个问题：

• 角度平滑：如果相邻控制周期的目标角度跳变太大，舵机会"抽搐"。需要在轨迹生成时就保证轨迹光滑，或者在控制端做插值。
• 速度限制：给舵机发一个阶跃角度，它会以最大速度冲过去，冲击很大。可以在代码里做简单的斜坡限制或 S 曲线。

8.1.1 PWM 舵机角度换算

PWM 舵机通过脉宽控制角度。典型舵机的控制信号是：周期 20ms，脉宽 0.5ms~2.5ms 对应 0°~180°。在 STM32 上，这段换算很容易写：

#define SERVO_MIN_PULSE     500     // 0.5ms = 500us，对应 0°
#define SERVO_MAX_PULSE     2500    // 2.5ms = 2500us，对应 180°
#define SERVO_ANGLE_RANGE   180.0f  // 总角度范围

// 把目标角度（弧度）转换为定时器比较值
uint32_t servo_angle_to_pulse(float angle_rad)
{
    float angle_deg = angle_rad * 180.0f / 3.141592653589793f;

    // 限制角度范围，防止舵机超范围损坏
    if (angle_deg < 0.0f) angle_deg = 0.0f;
    if (angle_deg > SERVO_ANGLE_RANGE) angle_deg = SERVO_ANGLE_RANGE;

    float pulse_us = SERVO_MIN_PULSE +
        (SERVO_MAX_PULSE - SERVO_MIN_PULSE) * angle_deg / SERVO_ANGLE_RANGE;

    // 假设定时器 1MHz 计数，1us = 1 个计数值
    return (uint32_t)pulse_us;
}

但这段代码只做"角度→脉宽"换算，没有做运动平滑。 如果把第 7 章生成的轨迹直接发给舵机，相邻控制周期的角度跳变会让舵机"抽搐"。所以还要加一段斜坡逼近，让角度缓慢靠近目标，而不是一步到齐：

// 简单的斜坡逼近器：每步最多变化 max_step 弧度
float ramp_approach(float current, float target, float max_step)
{
    float diff = target - current;
    if (diff > max_step) {
        return current + max_step;        // 限制最大增量
    } else if (diff < -max_step) {
        return current - max_step;
    } else {
        return target;                     // 已经够近，直接到位
    }
}

把这些拼起来，控制一条腿的完整链路就是：

每一层都很薄，但合在一起，腿就会"柔"起来，不再像廉价玩具那样硬邦邦的。

8.2 PID 控制：一个必须懂的算法

虽然第一版舵机内部自带闭环，但理解 PID 对后续升级到总线舵机、做力矩控制至关重要。PID 的数学表达：

输出 = Kp × 误差 + Ki × 误差积分 + Kd × 误差微分

通俗地讲：

• P（比例）：差多少补多少。误差越大，输出越大。但只有 P 会震荡或存在稳态误差。
• I（积分）：把过去的误差累加起来，消除稳态误差。但 I 太大容易超调。
• D（微分）：预测误差变化趋势，加"阻尼"，抑制震荡。但 D 对噪声敏感。

一个离散形式的 PID 控制器：

class PIDController:
    def __init__(self, kp, ki, kd, dt):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.dt = dt
        self.integral = 0.0
        self.prev_error = 0.0

    def update(self, setpoint, measurement):
        error = setpoint - measurement

        # 比例
        p_term = self.kp * error

        # 积分（带限幅防饱和）
        self.integral += error * self.dt
        i_term = self.ki * self.integral

        # 微分
        derivative = (error - self.prev_error) / self.dt
        d_term = self.kd * derivative

        # 更新历史
        self.prev_error = error

        return p_term + i_term + d_term

    def reset(self):
        self.integral = 0.0
        self.prev_error = 0.0

在四足机器人里，PID 常用于两个地方：

1. 姿态控制：把身体俯仰角和横滚角控制在目标值（通常是 0）附近。IMU 提供测量值，PID 输出的是"身体需要调整的偏移量"，叠加到足端目标位置上，形成闭环。
2. 身高控制：通过 IMU 或腿部运动学估算身体高度，与目标高度比较，PID 输出身体上下调整量。

8.3 姿态闭环：让身体保持水平

这是四足机器人最关键的闭环之一。思路是：

• 如果 IMU 测到身体向前倾（俯仰角为正），说明前腿"陷"下去了或后腿"翘"起来了；
• 我们让前腿的足端朝前上方移动、后腿的足端朝后下方移动（或者等效地调整身体高度指令），产生一个纠正力矩，让身体回正。

一个简化的实现：

def posture_control(imu_pitch, imu_roll, kp_pitch, kp_roll):
    """
    根据姿态误差，计算身体需要做的补偿
    返回: (pitch_offset_x, roll_offset_y)，加到足端目标位置
    """
    # 目标姿态是水平，即 0
    pitch_error = 0.0 - imu_pitch
    roll_error = 0.0 - imu_roll

    # 线性补偿（可用 PID，这里简化为 P）
    pitch_correction = kp_pitch * pitch_error
    roll_correction = kp_roll * roll_error

    return pitch_correction, roll_correction

把 pitch_correction 加到足端的前后方向上，这样身体前倾时，前脚往前方多伸一点、后脚往后方多推一点，身体就被"抬回来"了。

关键思维：四足机器人的姿态控制，本质上是通过调整足端相对身体的位置，来改变支撑多边形与重心的关系。理解了这一点，你就理解了四足控制的核心。

9. 状态估计：让机器人"感觉到自己"

控制需要反馈，反馈需要测量，而测量往往不完美。这一节讲怎么从传感器数据里"估计"出可靠的状态。

9.1 IMU 的困境：漂移与噪声

IMU 里的陀螺仪能测角速度，加速度计能测重力加速度方向。理论上：

• 角速度积分 → 角度；
• 加速度方向 → 角度。

但两者都有问题：陀螺仪有零偏漂移，积分久了误差越来越大；加速度计在机器人运动时会测到动态加速度，不只有重力分量，噪声很大。所以单独用哪个都不行。

9.2 互补滤波：简单但有效

互补滤波的思想一句话：陀螺仪在高频段可靠，加速度计在低频段可靠，两者取长补短。

class ComplementaryFilter:
    def __init__(self, alpha, dt):
        self.alpha = alpha  # 0~1，越大越信任陀螺仪
        self.dt = dt
        self.angle = 0.0

    def update(self, gyro_rate, accel_angle):
        """
        gyro_rate: 陀螺仪测得的角速度(rad/s)
        accel_angle: 加速度计推算的角度(rad)
        """
        # 陀螺仪积分
        gyro_angle = self.angle + gyro_rate * self.dt

        # 互补：低通滤波加速度计 + 高通滤波陀螺仪
        self.angle = self.alpha * gyro_angle + (1 - self.alpha) * accel_angle
        return self.angle

alpha 通常取 0.95~0.98，表示"更相信陀螺仪的瞬时变化，但用加速度计慢慢纠正长期漂移"。这个算法简单到只有几行，但在低速四足机器人上效果非常好。

9.3 加速度计如何算角度

静止时，加速度计测到的是重力在三个轴上的分量。对于俯仰角 θ（绕横滚轴转）：

θ = atan2(ax, sqrt(ay² + az²))

对于横滚角 φ（绕俯仰轴转）：

φ = atan2(ay, az)   （取决于传感器安装方向）

9.4 足端接触检测

步态切换需要知道"脚有没有落地"。最可靠的方式是接触开关，结构简单、响应快。把它装在足端，落地时触电闭合，给一个数字信号。

接触开关的意义在于：它把步态从一个"开环时间表"变成了"闭环触发"。如果没有触地反馈，摆动相结束时间只能靠估计，地面不平就会早落或晚落；有了触地反馈，脚一落地就立即切换状态，适应地形能力强得多。

10. 软件架构：一个健壮的状态机

当机械、电路、算法都就位后，软件架构决定了整个系统的可维护性和可扩展性。

10.1 为什么需要状态机？

四足机器人的运行状态不止"走路"一种，它有：

• 待机（Idle）：四条腿站立不动；
• 爬行（Crawl）：慢速静态步态；
• 小跑（Trot）：对角动态步态；
• 起立（Stand up）：从趴下到站立；
• 蹲下（Squat）：从站立到趴下；
• 摔倒保护（Fall）：检测到摔倒后停机。

这些状态之间需要有序切换，而且有些转换是不允许的（比如从"小跑"直接切成"趴下"）。状态机让这些规则显式化。

10.1.1 用枚举和 switch 实现状态机

前面那张状态机图，翻译成 C 代码就是一个枚举加上 switch 分支：

// 机器人运行状态枚举
typedef enum {
    STATE_IDLE,       // 待机：四条腿站立不动
    STATE_STAND_UP,   // 起立：从趴下到站立
    STATE_CRAWL,      // 爬行：慢速静态步态
    STATE_TROT,       // 小跑：对角动态步态
    STATE_SQUAT,      // 蹲下：从站立到趴下
    STATE_FALL,       // 摔倒保护
    STATE_COUNT
} RobotState;

// 状态转换函数：检查当前状态和指令，决定下一个状态
RobotState state_transition(RobotState current, RobotCommand cmd)
{
    switch (current) {
        case STATE_IDLE:
            if (cmd == CMD_STAND_UP) return STATE_STAND_UP;
            if (cmd == CMD_SQUAT) return STATE_SQUAT;
            break;

        case STATE_STAND_UP:
            if (cmd == CMD_STAND_DONE) return STATE_IDLE;
            break;

        case STATE_CRAWL:
            if (cmd == CMD_STOP) return STATE_IDLE;
            if (cmd == CMD_TROT) return STATE_TROT;
            if (cmd == CMD_FALL_DETECTED) return STATE_FALL;
            break;

        case STATE_TROT:
            if (cmd == CMD_CRAWL) return STATE_CRAWL;
            if (cmd == CMD_FALL_DETECTED) return STATE_FALL;
            break;

        case STATE_FALL:
            if (cmd == CMD_RECOVER) return STATE_IDLE;
            break;

        default:
            break;
    }
    return current;  // 没有匹配的转换，保持当前状态
}

这个实现里最重要的设计是：不允许的转换直接被忽略。比如机器人正在小跑，你突然发来"蹲下"指令，状态机不会执行，而是继续保持小跑——因为它知道"小跑中途蹲下"一定会摔。这种安全性不是靠程序员"记得别发错误指令"，而是靠状态机在结构上杜绝了非法路径。

还要注意，上面只做了状态转移逻辑，没有把动作放进去。实际代码中，每次切换状态时还要触发"进入动作"和"退出动作"：比如进入 STATE_FALL 时立即关闭舵机力矩、退出 STATE_IDLE 时锁定关节。这些细节才是状态机的真正价值，也是它比散落的 if-else 强得多的原因。

10.2 主循环与控制周期

主控程序的主循环必须保证严格的控制周期。一个典型的架构是：

初始化 → 传感器校准 → 进入主循环：
    每隔 dt 毫秒：
        1. 读取传感器（IMU、接触开关）
        2. 运行状态机，更新当前状态
        3. 根据状态生成足端目标位置（步态）
        4. 姿态闭环修正
        5. 逆运动学求解关节角
        6. 发送舵机指令

在 STM32 上，这个循环可以由定时器中断触发，保证控制周期稳定（比如 5ms 或 10ms）。控制周期的稳定性比"代码跑得快"重要得多——因为 PID、互补滤波这些算法都假设你每隔固定时间采样一次。

// STM32 中断服务程序（伪代码，示意控制节奏）
void TIM2_IRQHandler(void) {
    if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) {
        TIM_ClearITPendingBit(TIM2, TIM_IT_Update);

        // 每 5ms 执行一次控制循环
        read_sensors();       // 读 IMU + 接触开关
        run_state_machine();  // 状态机
        generate_gait();      // 步态目标
        posture_control();    // 姿态闭环
        inverse_kinematics(); // 逆解
        servo_output();       // 舵机输出
    }
}

10.3 参数化的艺术

把所有可调参数集中到一个配置结构里，而不是散落在代码各处。比如：

typedef struct {
    float stride_length;   // 步幅
    float step_height;     // 抬腿高度
    float frequency;       // 步频
    float body_height;     // 身体目标高度
    float kp_pitch;        // 俯仰 P
    float kp_roll;         // 横滚 P
    float swing_ratio;     // 摆动相占空比
} RobotConfig;

这样做的好处是：调参时不用改代码逻辑，只需要改一个数字，而且可以很容易地通过串口或上位机实时调整。

11. 核心代码实战：把步态跑起来

这一节把前面零散的知识串起来，给你一个能跑的核心框架。代码以 Python 写示意，方便理解；实际部署可以移植到 C。

11.1 一个完整的步态生成器

import numpy as np

class QuadrupedGait:
    def __init__(self, L1, L2, stride_length, step_height, frequency,
                 body_height, leg_phases):
        """
        L1, L2: 大腿、小腿长度
        stride_length: 步幅
        step_height: 抬腿高度
        frequency: 步频(Hz)
        body_height: 身体目标高度（正值向下）
        leg_phases: 四腿相位列表 [LF, RF, LH, RH]
        """
        self.L1 = L1
        self.L2 = L2
        self.stride_length = stride_length
        self.step_height = step_height
        self.frequency = frequency
        self.body_height = body_height
        self.leg_phases = leg_phases

        # 四条腿的编号
        self.legs = ['LF', 'RF', 'LH', 'RH']

        # 站立时髋关节在身体坐标系中的位置（x, y）
        self.hip_positions = {
            'LF': ( 0.12,  0.06),
            'RF': ( 0.12, -0.06),
            'LH': (-0.12,  0.06),
            'RH': (-0.12, -0.06),
        }

    def foot_trajectory(self, t, phase):
        """生成单腿足端在身体坐标系下的目标位置"""
        cycle = (t * self.frequency + phase) % 1.0

        if cycle < 0.35:  # 摆动相
            s = cycle / 0.35
            x = -self.stride_length/2 + self.stride_length * s
            z = self.step_height * np.sin(np.pi * s)
        else:             # 支撑相
            s = (cycle - 0.35) / 0.65
            x = self.stride_length/2 - self.stride_length * s
            z = 0.0

        return x, 0.0, z  # y 暂为 0，实际用髋关节 y 偏移

    def compute_joint_angles(self, t, posture_offset):
        """
        计算当前时刻所有腿的关节角度
        posture_offset: (pitch_off_x, roll_off_y) 姿态补偿
        """
        pitch_off, roll_off = posture_offset
        result = {}

        for i, leg in enumerate(self.legs):
            hip_x, hip_y = self.hip_positions[leg]
            phase = self.leg_phases[i]

            # 足端轨迹（身体坐标）
            foot_x, foot_y, foot_z = self.foot_trajectory(t, phase)

            # 加入姿态补偿
            foot_x += pitch_off
            foot_y += roll_off

            # 转换到该腿髋关节坐标系
            rel_x = foot_x - hip_x
            rel_y = foot_y - hip_y
            rel_z = self.body_height + foot_z  # 身体高度 + 抬腿

            # 逆运动学
            theta1, theta2, theta3 = self.inverse_kinematics(
                (rel_x, rel_y, rel_z), self.L1, self.L2
            )
            result[leg] = (theta1, theta2, theta3)

        return result

    def inverse_kinematics(self, foot_pos, L1, L2):
        """同第 6.4 节实现"""
        x, y, z = foot_pos
        theta1 = np.arctan2(x, y)
        D = np.sqrt(x**2 + y**2)
        R = np.sqrt(D**2 + z**2)
        R = np.clip(R, abs(L1 - L2) + 1e-6, L1 + L2 - 1e-6)

        cos_alpha = (L1**2 + R**2 - L2**2) / (2 * L1 * R)
        cos_alpha = np.clip(cos_alpha, -1.0, 1.0)
        alpha = np.arccos(cos_alpha)
        beta = np.arctan2(z, D)
        theta2 = alpha + beta

        cos_gamma = (L1**2 + L2**2 - R**2) / (2 * L1 * L2)
        cos_gamma = np.clip(cos_gamma, -1.0, 1.0)
        gamma = np.arccos(cos_gamma)
        theta3 = gamma - np.pi

        return theta1, theta2, theta3


# 使用示例
if __name__ == "__main__":
    gait = QuadrupedGait(
        L1=0.08, L2=0.11,
        stride_length=0.06,
        step_height=0.03,
        frequency=1.0,
        body_height=0.14,
        leg_phases=[0.0, 0.5, 0.5, 0.0]  # 爬行步态相位
    )
    # 模拟 2 秒，控制周期 0.01s
    for i in range(200):
        t = i * 0.01
        angles = gait.compute_joint_angles(t, (0.0, 0.0))
        # 打印或发送到舵机
        if i % 20 == 0:
            print(f"t={t:.2f}s LF={angles['LF']}")

11.2 代码的核心思想

看完这段代码，我想强调三个关键点：

1. 分层解耦：轨迹生成、逆运动学、姿态补偿是三个独立的职责，互不干扰。这样哪一层出了问题，你可以单独调试。
2. 一切都是时间函数：步态生成器是"给定时间 t，输出目标角度"。这让它天然适合实时循环调用。
3. 参数化：步幅、步频、抬腿高度都是参数，你可以实时调整而不改代码。

11.3 从仿真到真机的鸿沟

仿真里这段代码能走出漂亮的步态，但上了真机，你会遇到：

• 舵机响应延迟：发指令后，舵机要过一会儿才转到目标位，所以实际轨迹和理论轨迹有相位差；
• 机械间隙：关节松动导致足端实际位置和理论位置差几毫米；
• 地面摩擦：不同地面摩擦系数不同，支撑相可能打滑；
• 电池电压下降：电量低时舵机力量变小，动作变慢。

这些就是"真机调试"的意义所在——仿真是理想化模型，真机是复杂现实，而真正的工程师是把复杂现实逐步改造成理想模型的人。

12. 调试与调参：从满地打滚到稳稳行走

这一节是整篇文章最"接地气"的部分，因为这里全是血泪教训。

12.1 分阶段调试法

不要一上来就调整个步态。我推荐这样拆：

1. 单腿测试：把一条腿接到控制板，单独跑逆运动学，验证"脚能精确到达目标点"。
2. 四腿站立：四条腿固定支撑，验证机器人在无控制时能物理站稳。
3. 开环步态：不读传感器，只让四条腿按时间表摆动，观察基本步态。
4. 姿态闭环：加入 IMU 反馈，让身体保持水平。
5. 触地触发：加入接触开关，让摆动相结束由触地决定。
6. 调参优化：微调步幅、步频、抬腿高、PID 参数。

每一阶段只验证一件事，问题就很容易定位。

12.2 常见问题的诊断

现象	可能原因	排查方向
机器人前后摇晃	步幅太大或步频太快	减小步幅、降低步频
机器人侧翻	支撑三角形太窄	增大站立宽度
脚拖地	抬腿高度不够或摆动相太短	增大抬腿高，增大摆动相占比
关节抖动	PID 的 D 过大或信号噪声	降低 Kd，给信号滤波
舵机过热	堵转或负载过大	检查可达空间，减小腿长
走几步就摔	姿态闭环参数不对	从 P 开始调，逐步加 I、D

12.3 调参的顺序与心法

调 PID 有个经典顺序：

1. 先把 I 和 D 设为 0，只调 P，直到系统"接近目标但略有震荡"；
2. 加入 D，抑制震荡，让系统稳定；
3. 最后加入 I，消除稳态误差。

心法：每次只改一个参数，改完观察再继续。很多人一上来同时改三个参数，结果完全不知道哪个起了作用。

12.4 记录日志：别相信自己的记忆

我每次调参后都会记录：参数值、现象、视频。三个月下来，积累了上百条记录。这些记录在项目后期帮了大忙——当我遇到重复问题时，直接翻记录就能找到上次的解法。

13. 进阶方向：机器人还没做完

当你的四足机器人能稳定地爬行时，真正的探索才刚刚开始。

13.1 动态步态：Trot 小跑

爬行是静态稳定，小跑（对角腿同时抬起）则需要动态平衡。这是质的飞跃。动态步态通常引入模型预测控制（MPC）或全身控制（WBC），通过实时求解优化问题来决定足端力。

13.2 强化学习

近几年，用强化学习（RL）训练四足机器人策略成为热门方向。核心思路是：在仿真里让机器人"自己试"，用奖励函数引导它学会走路，再迁移到真机。很多开源四足（如 Unitree 的强化学习控制）都走这条路。

13.3 感知与自主

加上摄像头、激光雷达，机器人就能做 SLAM、建图导航、目标识别。这是"机器人"变成"智能体"的必经之路。

14. 总结：我学到了什么

三个月，从纸板到能稳稳行走的四足机器人，我总结出几条真正重要的事：

1. 先想清楚"为什么"，再动手"怎么做"。需求定义决定了后面所有选择。
2. 分层解耦是复杂系统的生存法则。机械、运动学、步态、控制、状态估计，每层独立可测。
3. 第一版的目标是"能走"，不是"完美"。小步快跑，快速得到反馈，热情才能持续。
4. 真机是终极裁判。仿真只能验证想法，真机才暴露问题。
5. 大胆动手，小心调参。不要怕犯错，但每次要记录，让错误也变得有价值。

我那只桌子底下的四足机器人现在正安静地站着。它不完美：走路有点慢，姿态偶尔晃，但它是我的。它身上每一颗螺丝、每一行代码、每一个深夜的困惑，都构成了"我自己做出来"这件事的全部意义。

如果你也想做一只属于自己的四足机器人，别犹豫，从一张纸、一支笔、一个最便宜的舵机开始。等你第一次看到它自己站起来的时候，所有熬夜都值得。

---

附：初学者推荐路线图

1. 用纸板 + 4 个最便宜的 PWM 舵机搭一个四条腿的静态模型；
2. 写单腿逆运动学，验证"脚能到指定点"；
3. 四腿站立，保证物理稳定；
4. 加 IMU，实现姿态水平调节；
5. 实现爬行步态，让它慢慢走起来；
6. 升级 3D 打印结构 + 总线舵机，加触地反馈；
7. 挑战 Trot 小跑，进入动态控制的世界。

祝你好运，未来的机器人工匠。