腿足机器人之六- 前向运动学

想象一下，你正在控制一个四足机器人（比如波士顿动力的Spot）在复杂地形中行走。要让它的“脚”准确踩到目标位置，首先需要知道一个问题：
“如果机器人的关节转动某个角度，它的脚会移动到哪里？”
这就是**前向运动学（Forward Kinematics）**解决的问题。
它是机器人控制的基石，也是理解步态规划、避障等高级功能的第一步。

机器人在运动的时候，还需要根据未来的姿态动作，倒推计算关节的控制信息，这被称为逆运动学，下一篇博客揭晓，欢迎关注，前向运动学的基础是刚体变换和坐标系转换，这部分和普通机械臂类似（如成熟的工业机械臂），用到的核心知识是D-H参数，但腿足机器人的结构更加多变，比如关节的串联和并联以及多个自由度。

腿足机器人通常有两或四个腿，需要考虑基坐标系到每个腿的基座，再到脚端的位置，这里涉及多个运动链，比如四足机器人每条腿独立，但身体作为基座。需要确定每个连杆的坐标系，然后通过变换矩阵相乘得到末端位置。

在第二篇 腿足机器人之二- 运动控制概览我们提到前向运动学是中间层，运动的规划（步态规划）则是MPC/RL等算法实现，前向运动学主要是关注单腿的运动学，而不是步态规划。但前向运动学需要考虑基坐标系的变化，比如当机器人身体移动时，各腿末端的位置会相对世界坐标系变化，可能需要将身体坐标系到世界坐标系的转换也包括进去。

前向运动学知识点主要包括：刚体运动学基础（坐标系、旋转矩阵、齐次变换）、关节类型（旋转和移动关节）、运动链建模（串联结构）、正运动学公式推导（变换矩阵连乘）、多腿协调中的基座变换（比如身体移动对脚端位置的影响）、以及可能的并联机构（如果腿有并联结构的话）。

刚体运动学基础

在腿足机器人中，坐标系定义是运动学建模的核心，经典运动学中主要包括三种坐标系，分别是基坐标系，关节坐标系、末端坐标系，这是描述机器人运动控制、步态规划，注意和环境感知、定位导航的坐标系（如Camera传感器上的坐标、地图坐标等等）区别。

坐标系定义

基坐标系（base frame）：通常固定在机器人躯干或髋关节处(如上图中间的圆圈位置，在髋关节的旋转中心)，通常以髋关节的初始位置为原点，Z轴（Yaw）垂直于躯干平面（竖直方向，人体直立时从脚到头的连线），X轴（Roll）指向机器人前进方向(平行于地面)，Y轴（Pitch，从左肩到右肩的连线）。
关节坐标系：每个关节的局部坐标系，用于描述相邻连杆的相对运

• 髋关节坐标系：与基坐标系重合（初始状态），当髋关节偏航（Yaw）时，绕基坐标系的Z轴旋转。
• 膝关节坐标系：位于大腿末端，绕髋关节俯仰轴（Y轴）旋转。
• 踝关节坐标系：位于小腿末端，绕膝关节俯仰轴（Y轴）旋转。
• 肩关节坐标系：位于肩部，提供三个方向运动。
• 肘关节坐标系：位于手肘不，沿X轴（Roll）平移运动。
  末端坐标系：固定在足端或执行器（如足底、手掌）的坐标系。

旋转矩阵与欧拉角

在腿足机器人中，每个关节的旋转会改变后续连杆的方向，以髋关节的偏航$Yaw$为例：
假设髋关节绕基坐标系的Z轴旋转角度为$\theta$，则其旋转矩阵为：
$$R_z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$

矩阵元素	物理意义
$R_{11}=\cos \theta$	表示新坐标系Z轴在原坐标系X轴上的投影比例
$R_{12}=-\sin \theta$$	表示新坐标系Y轴在原坐标系X轴上的投影比例（因旋转后Y轴向左偏移）
$R_{21}=\sin \theta$	表示新坐标系X轴在原坐标系Y轴上的投影比例（X轴向右前方倾斜）
$R_{22}=\cos \theta$	表示新坐标系Y轴在原坐标系Y轴上的投影比例
$R_{33}=1$$	表示新坐标系Z轴和原坐标系Z轴始终重合（绕Z轴旋转不影响竖直方向）

其他轴（X/Y）的旋转矩阵类似，需注意旋转顺序（如Z-Y-Z欧拉角）

齐次变换矩阵（平移+旋转的统一表示）

齐次变换矩阵用于描述从一个坐标系到另一个坐标系的平移和旋转，是串联关节运动的核心工具，还是以上面的髋关节俯仰（绕Y轴旋转）为例，其变换矩阵为：
将平移向量$\mathbf{p}=[x,y,z{]}^T$和旋转矩阵$R$合并为4x4矩阵：
$$T=\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]$$
用于统一描述刚体的位姿变换。

• 髋关节偏航（Yaw）
  假设基座标系为（$O_0$），髋关节旋转后的坐标系为$O_1$，则变换矩阵$T_1^0$包含绕Z轴（脚到头连线）旋转${\theta }_1$，无平移（假设关节在原点）：
  $$T_1^0\left[\begin{array}{cccc}\cos {\theta }_1& -\sin {\theta }_1& 0& 0\\ \sin {\theta }_1& \cos {\theta }_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

• 髋关节俯仰
  髋关节旋转后的坐标系为$O_1$，大腿末端的坐标系为 O 2 O_2