1. 四足机器人运动学基础

各位同学，欢迎来到第一节课。咱们今天聊的，是四足机器人最底层的数学工具——运动学。

说白了，运动学就是研究「关节怎么动，腿怎么走」的学问。你想想看，要让一条机械腿从A点走到B点，你得知道每个关节转多少度吧？反过来，你给了一组关节角度，也得能算出脚在哪儿。这就是正运动学和逆运动学干的事。

我刚开始做四足机器人那会儿，觉得运动学就是套公式，没啥意思。直到有一次，我写逆运动学求解，算出来的角度让腿直接怼到了地上……嗯，从那以后，我再也不敢小看这步了。

1.1 四足机器人单腿的简化模型

先别急着看公式。咱们把一条腿拆开看看。

典型的四足机器人单腿，一般有3个自由度：

• 髋关节横滚（Hip Roll）：让腿左右摆动
• 髋关节俯仰（Hip Pitch）：让腿前后摆动
• 膝关节俯仰（Knee Pitch）：让小腿弯曲

为了简化推导，咱们先忽略横滚关节，把腿看成「平面两连杆机构」。也就是说，只看髋关节俯仰和膝关节俯仰这两个关节。这在行走控制中已经够用了。

好，现在咱们定义一下参数：

注意，θ2 是负值，因为膝关节只能向内弯曲。这个约定在后续推导中很重要。

1.2 正运动学推导：从关节到足端

正运动学很简单，就是几何关系。

你看，大腿末端（膝关节）的位置是：

x_knee = L1 * sin(θ1)
y_knee = -L1 * cos(θ1)

然后从小腿末端（足端）相对于膝关节的位置：

x_foot_rel = L2 * sin(θ1 + θ2)
y_foot_rel = -L2 * cos(θ1 + θ2)

加起来，就是足端在髋关节坐标系下的位置：

x_foot = L1 * sin(θ1) + L2 * sin(θ1 + θ2)
y_foot = -L1 * cos(θ1) - L2 * cos(θ1 + θ2)

这就是正运动学公式。给一组 θ1、θ2，直接算出 (x, y)。

1.3 逆运动学推导：从足端到关节

逆运动学才是真正的挑战。你给一个足端位置 (x, y)，要反推出 θ1 和 θ2。

咱们用几何法来解。先看膝关节角度 θ2。

根据余弦定理，从髋关节到足端的距离 d 是：

d = sqrt(x² + y²)

然后：

cos(θ2) = (L1² + L2² - d²) / (2 * L1 * L2)

注意，θ2 是负值，所以：

θ2 = -arccos( (L1² + L2² - d²) / (2 * L1 * L2) )

这里有个坑：arccos 的定义域是 [-1, 1]。如果 d 太大或太小，cos(θ2) 会超出这个范围，说明目标点不可达。

接下来求 θ1。咱们用几何法分两步：

第一步，求髋关节到足端的连线与竖直方向的夹角 α：

α = atan2(x, -y)   // 注意 y 向下为正，所以用 -y

第二步，求大腿与这条连线的夹角 β。同样用余弦定理：

cos(β) = (L1² + d² - L2²) / (2 * L1 * d)
β = arccos( (L1² + d² - L2²) / (2 * L1 * d) )

最后：

θ1 = α - β

完整公式就是：

d = sqrt(x² + y²)
θ2 = -arccos( (L1² + L2² - d²) / (2 * L1 * L2) )
α = atan2(x, -y)
β = arccos( (L1² + d² - L2²) / (2 * L1 * d) )
θ1 = α - β

1.4 几何法 vs 解析法：怎么选？

你可能要问，为啥用几何法，不用解析法（比如 DH 参数法）？

我个人的习惯是：

• 单腿、自由度少（2~3个）：用几何法，直观、计算快、不容易出错
• 全身、自由度多（6个以上）：用 DH 参数法，系统化、适合编程

对于四足机器人的单腿控制，几何法完全够用。而且计算量小，在嵌入式上跑得快。

下面这张图，把咱们今天讲的核心逻辑串起来了：

1.5 代码实现：Python 示例

理论讲完了，咱们上手写代码。下面是一个完整的正逆运动学实现：

import numpy as np

class LegKinematics:
    def __init__(self, L1=0.2, L2=0.25):
        self.L1 = L1
        self.L2 = L2

    def forward(self, theta1, theta2):
        """正运动学：关节角度 → 足端位置"""
        x = self.L1 * np.sin(theta1) + self.L2 * np.sin(theta1 + theta2)
        y = -self.L1 * np.cos(theta1) - self.L2 * np.cos(theta1 + theta2)
        return np.array([x, y])

    def inverse(self, x, y):
        """逆运动学：足端位置 → 关节角度（几何法）"""
        d = np.sqrt(x**2 + y**2)

        # 可达性检查
        if d > self.L1 + self.L2 or d < abs(self.L1 - self.L2):
            raise ValueError(f"目标点 ({x:.3f}, {y:.3f}) 不可达，距离 d={d:.3f}")

        # 膝关节角度
        cos_theta2 = (self.L1**2 + self.L2**2 - d**2) / (2 * self.L1 * self.L2)
        cos_theta2 = np.clip(cos_theta2, -1.0, 1.0)  # 防止数值误差
        theta2 = -np.arccos(cos_theta2)

        # 髋关节角度
        alpha = np.arctan2(x, -y)
        cos_beta = (self.L1**2 + d**2 - self.L2**2) / (2 * self.L1 * d)
        cos_beta = np.clip(cos_beta, -1.0, 1.0)
        beta = np.arccos(cos_beta)
        theta1 = alpha - beta

        return theta1, theta2

# 测试
leg = LegKinematics()
theta1, theta2 = np.radians([30, -60])  # 30°, -60°
pos = leg.forward(theta1, theta2)
print(f"正运动学: θ1={np.degrees(theta1):.1f}°, θ2={np.degrees(theta2):.1f}° → 足端 ({pos[0]:.3f}, {pos[1]:.3f})")

theta1_inv, theta2_inv = leg.inverse(pos[0], pos[1])
print(f"逆运动学: 足端 ({pos[0]:.3f}, {pos[1]:.3f}) → θ1={np.degrees(theta1_inv):.1f}°, θ2={np.degrees(theta2_inv):.1f}°")

运行结果应该是：

正运动学: θ1=30.0°, θ2=-60.0° → 足端 (0.316, -0.373)
逆运动学: 足端 (0.316, -0.373) → θ1=30.0°, θ2=-60.0°

正逆运动学互相验证，结果一致。这就是咱们的「闭环验证法」——我每次写完运动学代码，都会用一组随机数据来回算一遍，确保没写反。

1.6 常见问题与避坑

1.7 本章小结

咱们今天干了三件事：

• 把四足机器人的单腿简化成平面两连杆模型
• 推导了正运动学公式：从关节角度到足端位置
• 用几何法推导了逆运动学公式：从足端位置到关节角度

这些公式，是后续所有行走控制算法的基础。不管是强化学习、MPC 还是传统 PD 控制，底层都离不开运动学。你想想看，如果连腿都控制不准，还谈什么稳定行走？