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

想象一下，你正在控制一个四足机器人（比如波士顿动力的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$，绕Y轴（左肩到右肩连线）旋转${\theta }_2$，并沿大腿长度方向（新
  的X轴）平移$L_1$（大腿长度）：
  $$T_2^1\left[\begin{array}{cccc}\cos {\theta }_2& 0& \sin {\theta }_2& L_1\cos {\theta }_2\\ 0& 1& 0& 0\\ -\sin {\theta }_2& 0& \cos {\theta }_2& -L_1\sin {\theta }_2\\ 0& 0& 0& 1\end{array}\right]$$

• 膝关节俯仰
  大腿末端的坐标系为$O_2$，小腿末端的坐标系为$O_3$，绕Y轴（左肩到右肩连线）旋转${\theta }_3$，并沿小腿长度方向（新
  的X轴）平移$L_2$（大腿长度）：
  $$T_3^2\left[\begin{array}{cccc}\cos {\theta }_3& 0& \sin {\theta }_3& L_2\cos {\theta }_3\\ 0& 1& 0& 0\\ -\sin {\theta }_3& 0& \cos {\theta }_3& -L_2\sin {\theta }_3\\ 0& 0& 0& 1\end{array}\right]$$

• 全局变换矩阵
  足端在基座标系中的位置为：
  $$T_3^0=T_1^0\cdot T_2^1\cdot T_3^2$$
  足端坐标$P_{end}=[x,y,z{]}^T$由$T_3^0$的最后一列前三行给出。

注意事项：

• 旋转顺序的影响：若关节旋转顺序不同（如先俯仰后偏航），结果会完全不同。
• 实际机器人的校准：需测量真实的$L_1$和$L_2$（可能因装配误差而偏离设计值）。
• 姿态计算：足端的姿态（如是否水平着地）由旋转矩阵部分决定，需额外分析。
• 实际的机器人还有踝关节，即图中的（Ankle），因为和膝关节类似，自由度是俯仰方向，且计算方法也类似，所以这里并没有列出。

运动链建模

串联运动链结构（从基座到末端的关节连接）

标准Denavit-Hartenberg（D-H）参数法

D-H参数（Denavit-Hartenberg参数） 是一种用于描述机器人连杆和关节几何关系的标准化方法，由 Jacques Denavit 和 Richard S. Hartenberg 于1955年提出。它是机器人运动学建模的基础工具，尤其适用于串联式机械臂或腿足机器人的运动学分析。

D-H参数的核心思想是机器人每个关节的运动（旋转或平移）会影响末端的位置和姿态，因此D-H参数通过定义相邻连杆之间的相对位姿关系，将复杂的多关节运动分解为一系列简单的坐标系变换，最终通过矩阵乘法得到末端位姿。

D-H参数法
每个关节需要定义 4个参数，用于描述相邻两个连杆之间的几何关系：

参数	符号	意义
连杆长度	$a_i$	从关节$i$到$i+1$沿$X_i$轴的距离
连杆转角	${\alpha }_i$	绕$X_i$轴从$Z_i$旋转到$Z_{i+1}$的角度
连杆偏距	$d_i$	沿$Z_i$轴从$X_{i-1}$到$X_i$的距离
关节角度	${\theta }_i$	绕$Z_i$轴从$X_{i-1}$到$X_i$的旋转角

从坐标系$i-1$到$i$的齐次变换矩阵$T_{i-1}^i$由一下四个参数计算得到：
$$T_{i-1}^i\left[\begin{array}{cccc}\cos {\theta }_i& -\sin {\theta }_i\cos {\alpha }_i& \sin {\theta }_i\sin {\alpha }_i& a_i\cos {\theta }_i\\ \sin {\theta }_i& \cos {\theta }_i\cos {\alpha }_i& -\cos {\theta }_i\sin {\alpha }_i& a_i\sin {\theta }_i\\ 0& \sin {\alpha }_i& \cos {\alpha }_i& d_i\\ 0& 0& 0& 1\end{array}\right]$$

其中：
1.绕$Z_{i-1}$旋转${\theta }_i$:修正关节角度
2.沿$Z_{i-1}$平移$d_i$：处理滑动关节的偏移
3.沿$X_i$平移$a_i$:定义连杆长度
4.绕$X_i$旋转${\alpha }_i$:修正连杆的扭转角度。

这样从基座到末端的全局变换矩阵为各局部变换矩阵的连乘：
$$T_{end}^{base}=T_1\cdot T_2\cdot ...\cdot T_n$$
其中$T_i$是第i个关节的变换矩阵，如上例的$T_3^0$所求解示例。

改进D-H参数法（针对复杂关节轴对齐问题）

标准的D-H参数法坐标系建立在连杆的远端，而改进的D-H参数法，将坐标系建立在连杆的近端，主要是为了更加适应树状结构机器人。

D-H参数法统一了运动学建模的流程，通过链式矩阵乘法即可得到末端位置，适用于大多数串联式机器人（尤其是成熟的机械臂工业机器人），但是对并联或者闭链结构不友好。

多腿协调与基座标系转换

上面的例子都是单腿，但是对于韧性的双足或者机器狗的四足，在步态的运动中，就涉及到两足或者四足之间的协调，以实现动态环境下的步态规划，其中一个重要点是通过将各腿运动学链与基座变换结合实现全局坐标统一，在此基础上更容易规划步态。

基座标系（机器人本体）到世界坐标系的变换

若机器人躯干（基座）在三维空间世界中运动，坐标系中运动（平移$T_b=[x_b,y_b,z_b{]}^T$和旋转$R_b\in SO(3)$描述，则腿末端(足端)在基座标系位置为$T_{end}^{base}$，则其在世界坐标系（全局坐标系）为：
$$T_{end}^{world}=T_b^{world}+R_b\cdot T_{end}^{base}$$
其中：

• $T_b$是基座原点在世界坐标系中的位置。表示从世界坐标系原点（如地面上的固定点）到机器人基座中心的位移。$x_b$是沿世界坐标系x轴的位移（前后方向，可参考一开始的图）。
  假设机器人的足端在基座标系中的位置为 $P_{base}=[1,0,0{]}^T$，若基座向右移动了2米，即$T_b=[0,2,0{]}^T$，则足端的世界坐标为：
  $$P_{world}=T_b+P_{base}=[1,2,0{]}^T$$

• $R_b$是基座姿态的旋转矩阵，由欧拉角（横滚$\varphi$，俯仰$\theta$和偏航$\psi$）定义：
  $$R_b=R_z(\psi )\cdot R_y(\theta )\cdot R_x(\varphi )$$
  $R_x$，$R_y$，$R_z$是绕各轴的旋转矩阵。

• SO(3) 是 特殊正交群（Special Orthogonal Group） 的缩写，表示所有三维旋转矩阵的集合。满足正交性（即旋转不改变向量长度和夹角，$R^{T}R=I$）和纯旋转，无镜像反射（即$det(R)=1$）。SO(3)中的矩阵除了能唯一描述物理上可能的旋转，还方便了逆向计算（$R^{-1}=R^T$）。

基座到世界坐标系的齐次变换矩阵为：
$$H_b^{world}==\left[\begin{array}{cc}{R}_b& T_b\\ 0& 1\end{array}\right]$$

足端位置变换为：
$$\left[\begin{array}{c}{P}_{end}^{world}\\ 1\end{array}\right]=H_b^{world}\cdot \left[\begin{array}{c}{P}_{base}^{world}\\ 1\end{array}\right]$$

在动态步态中，基座位姿随时间变化（如行走时躯干起伏），需实时更新$T_b$和$R_b$，可通过IMU测量姿态角，编码器或视觉里程计估计位置，通常要求更新频率 ≥ 100Hz（如ROS（机器人操作系统）中的tf库）。

如控制右前腿在摆动相中沿抛物线轨迹运动，同时基座移动，支撑腿左腿需调整关节角度以保持足端静止（全局坐标），这可以用逆运动学（下一篇博客）计算基座运动导致的足端位移，调整关节角度抵消。

多腿独立运动学链的全局坐标统一

在多腿运动学模型中，每条腿为串联结构（如3自由度），使用D-H参数建模，所有腿共享同一基座坐标系，但运动独立（并联驱动），当基座运动时，各腿的末端全局位置需统一到世界坐标系，以协调步态。

全局坐标统一步骤主要分为三步：
1.单腿前向运动学：计算每条腿末端在基座标系中的位置$T_{end}^{base}$（基于关节角度）；
2.坐标变换到世界系：应用基座标系的$H_b^{world}$变换，得到全局坐标系；
3.步态协调：根据全局坐标规划各腿的摆动/支撑相位，保持机器人平衡，这部分放到后面MPC/RL部分内容展开。

• 基座标系变换 是多腿协调的核心，需实时融合传感器数据更新位姿。
• 全局坐标统一 通过将各腿运动学链与基座变换结合，实现动态环境下的步态规划。
• 实际挑战 包括实时性保障、多解协调和动态补偿，需结合控制理论与工程优化。
• 当各退独立求解时可能导致冲突（如跳跃时双腿同时摆动），可以通过状态机控制各腿的摆动/支撑相位和基于ZMP（零力矩点）调整基座位姿和各腿力分配。

真实机器人中的挑战

• 误差校准：实际关节存在齿轮间隙、制造公差（实际连杆长度与理论值偏差）、电机误差、连杆柔性变形。
• 传感器反馈：使用IMU、编码器实时修正位置。
• 制动态控制：前向运动学是静态模型，实际需结合动力学（如惯性、摩擦力）。
• 多腿协调运动控制

仿真工具推荐

• PyBullet：轻量级物理引擎，适合验证运动学算法。
• ROS + Gazebo：工业级机器人仿真平台。
• Webots：用户友好的多机器人仿真环境。
• ROS MoveIt、Robotics Toolbox for Python.

工程上如果只是使用运动学和动力学，而不深究和改进其实现，那么ROS机器人操作系统是个不错的入手环境，该操作系统集成了运动学和动力学计算工具orocos-kdl -， 这个工具还集成了bullet3D物理引擎，用于模拟碰撞检测、刚体和软体动力学。