腿足机器人之七- 逆运动学

基本概念

机器人运动控制的本质是将高层指令（“向前走”，“右侧走移动”）转化为关节电机的转动角度，上一篇的正运动学是从关节角度推导出机器人的末端位置，而逆运动学（Inverse Kinematics，IK）是已知机器人末端（如足端）的位置和姿态，反推各关节角度的过程。
腿部自由度越多，解算难度指数级上升（以人类腿部7自由度 vs 工业机械臂6自由度为例），步态规划中需要考虑支撑相和摆动相的切换，以及如何协调双腿的运动。

腿部运动的数学表示

这部分包括世界坐标系、髋关节坐标系以及足端坐标系，此外还涉及D-H参数以及从D-H参数到齐次变换矩阵，在上一篇博客《腿足机器人之六- 前向运动学》已经介绍，这里简略展示计算过程，不清楚可以参考上一篇博客。

坐标系定义以及自由度说明

双足人形机器人髋关节的三个自由度对应绕基座标系的三个轴的旋转：

• 偏航（Yaw）：绕Z轴旋转，角度记为$\psi$
• 俯仰（Pitch）：绕Y轴旋转，角度记为$\theta$
• 横滚（Roll）：绕X轴旋转，角度记为$\varphi$

基座标系（O₀） 设在髋关节中心，X轴向前，Y轴向左，Z轴向上。旋转顺序为ZYX，对应的旋转矩阵为：

$$R_{hip}=R_z(\psi )\cdot R_y(\theta )\cdot R_x(\varphi )$$

正运动学模型

假设大腿长度为$l_1$，小腿长度为$l_2$，膝关节俯仰角为${\theta }_k$，脚的位置$P$在基座标系中的坐标为：
$$P=R_{hip}\cdot ([0,0,l_1]+R_y({\theta }_k)\cdot [0,0,l_2])$$
其中：

• $R_z(\psi )=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$
• $R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$
• $R_x(\varphi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi & \cos \varphi \end{array}\right]$
  上式本质上是坐标系变换的级联过程，包含如下关键步骤：
• 1.大腿的固定长度：髋关节到膝关节的位移 $[0,0,l_1]$;
• 2.膝关节的旋转：绕膝关节局部Y轴的旋转$R_y({\theta }_k)$，影响小腿的延伸方向
• 髋关节的整体旋转：将整个腿部（大腿+小腿）的姿态通过$R_{hip}$转换到基座标系。

注：若踝关节存在，需在公式末尾添加踝关节的旋转和平移，例如：
$$P=R_{hip}\cdot ([0,0,l_1]+R_y({\theta }_k)\cdot [0,0,l_2])+R_{ankle}\cdot △p$$

逆运动学求解

腿部逆运动学主要有三大解法

方法	原理	优点	缺点
几何解析法	利用三角函数分解几何关系	计算速度快	仅适用于简单结构，如平面式机器人
代数法	求解非线性方程组	精确解	可能无解或多解
数值迭代法	雅可比矩阵+梯度下降	通用性强	计算量大，需防发散

几何解法

1.确定脚的目标位置和方向
设目标位置为$P=[x,y,z]$，其单位方向向量为：
$$u=\frac{P}{||P||}=[u_x,u_y,u_z]$$

2.分解髋关节旋转矩阵
根据正运动学，旋转后的z轴应对齐于$u$，即：
$$R_{hip}\cdot [0,0,1{]}^T=u$$
展开后得到三个方程：
$$\{\begin{array}{rlr}\cos \psi \sin \theta \cos \varphi +\sin \psi \sin \varphi & =& u_x\\ \sin \psi \sin \theta \cos \varphi -\cos \psi \sin \varphi & =& u_y\\ \cos \theta \cos \varphi & =& u_z\end{array}$$

3.计算俯仰角$\theta$和横滚脚$\varphi$
从第三个方程：
$\cos \theta \cos \varphi =u_z\Rightarrow \varphi =\arccos (\frac{u_z}{\cos \theta })$
代入前两个方程，结合几何约束求解$\theta$和$\varphi$，例如当$\varphi =0$时，简化为：
$$\theta =\arccos (u_z),\psi =\arctan 2(u_y{,}_x)$$

4.计算膝关节角度${\theta }_k$
根据余弦定理，解算膝关节角度：
$${\theta }_k=\arccos (\frac{||p|{|}^2-l_1^2-l_2^2}{2l_1{l}_2})$$

注意事项

• 多解性：可能存在多组解，需根据关节限制选择。
• 奇异点：当大腿与目标方向共线时，需特殊处理。
• 姿态调整：踝关节需进一步调整以满足脚的朝向。

数值迭代法

雅可比矩阵在机器人学中用于描述末端执行器速度与关节速度之间的关系。逆运动学问题中，雅可比矩阵的逆或伪逆被用来迭代调整关节角度，使末端执行器逐步接近目标位置。对于双足机器人来说，特别是在动态运动如小跑和走路时，雅可比方法能处理实时调整的需求，因为步态需要连续更新关节角度以保持平衡和步态周期。

步态规划中需要考虑支撑相和摆动相的切换，以及如何协调双腿的运动。可能需要结合具体的数学模型，例如线性倒立摆模型（LIPM）或零力矩点（ZMP）准则来确保动态稳定性。雅可比方法在这里可能用于实时调整关节角度以满足ZMP的要求。

雅可比矩阵法基础

1.雅可比矩定义
雅可比矩阵描述了机器人末端执行器速度（线速度$\dot{\mathbf{p}}$和角速度$\mathbf{w}$）与关节速度$\dot{\mathbf{q}}$之间的关系：
$$\left[\begin{array}{c}\dot{\mathbf{p}}\\ \mathbf{w}\end{array}\right]=\mathbf{J}(\mathbf{q})\cdot \dot{\mathbf{q}}$$
其中，$\mathbf{J}$是6 x n的雅可比矩阵（n为关节数）。

2.逆运动学迭代求解
目标是通过末端位置误差$\mathbf{e}={\mathbf{p}}_{target}-{\mathbf{p}}_{current}$，调整关节角度$\mathbf{q}$:
$$△\mathbf{q}={\mathbf{J}}^{+}\cdot \mathbf{e}$$
其中${\mathbf{J}}^{+}$是雅可比矩阵的伪逆（Moore-Penrose 逆），计算为：
$${\mathbf{J}}^{+}={\mathbf{J}}^{\top }（\mathbf{J}{\mathbf{J}}^{\top }{）}^{-1}(当\mathbf{J}行满秩时)$$
或使用阻尼最小二乘法避免奇异：
$${\mathbf{J}}^{+}={\mathbf{J}}^{\top }（\mathbf{J}{\mathbf{J}}^{\top }+{\lambda }^2\mathbf{I}{）}^{-1}$$
$\lambda$为代入阻尼因子，通常取0.001~0.1。

3.迭代步骤
a.初始化：设定初始关节角度${\mathbf{q}}_0$，容差$ϵ$，步长$\alpha$。
b.计算误差：${\mathbf{e}}_i={\mathbf{p}}_{target}-f_{kin}({\mathbf{q}}_i)$，其中$f_{kin}$为正运动学函数。
c.更新关节角：
$${\mathbf{q}}_{i+1}={\mathbf{q}}_i+\alpha \cdot {\mathbf{J}}^{+}({\mathbf{q}}_i)\cdot {\mathbf{e}}_i$$
d.终止条件：当$||e_i||<ϵ$或达到最大迭代次数时停止。

双足机器人步态规划中的雅可比法应用

对于多足机器人，涉及到多个腿的并行运动，需要对步态进行规划，基于周期性步态（如四足机器人的对角步态）或非周期性步态（如动态奔跑），结合时间参数生成关节轨迹，常用方法包括零力矩点（ZMP）理论：用于双足机器人，确保质心投影在支撑多边形内和贝塞尔曲线或样条插值：生成平滑的足端轨迹。

1. 步态特征

• 步行（Walking）：交替支撑相（单腿支撑）和摆动相，重心轨迹平滑，ZMP（零力矩点）始终在支撑多边形内。
• 小跑（Trot）：对角腿同步运动，动态平衡需求更高，存在双支撑相和腾空相。

2. 单腿逆运动学模型
   以右腿为例，关节配置为髋关节（横滚$\varphi$、俯仰$\theta$、偏航$\psi$）和膝关节${\theta }_k$，踝关节${\theta }_a$。
   构造雅可比矩阵：
   对于腿部位置$\mathbf{p}=[x,y,z{]}^T$，雅可比矩阵的列为各关节对末端速度的贡献：
   $$\mathbf{J}=\left[\begin{array}{cccc}\frac{\partial x}{\partial \varphi }& \frac{\partial x}{\partial \theta }& \frac{\partial x}{\partial \psi }& \frac{\partial x}{\partial {\theta }_k}\\ \frac{\partial y}{\partial \varphi }& \frac{\partial y}{\partial \theta }& \frac{\partial y}{\partial \psi }& \frac{\partial y}{\partial {\theta }_k}\\ \frac{\partial z}{\partial \varphi }& \frac{\partial z}{\partial \theta }& \frac{\partial z}{\partial \psi }& \frac{\partial z}{\partial {\theta }_k}\end{array}\right]$$

偏导数计算示例（髋关节俯仰角$\theta$）：
$$\frac{\partial \mathbf{p}}{\partial \theta }=R_z(\psi )\cdot (\frac{\partial {\mathbf{R}}_y(\theta )}{\partial \theta })\cdot {\mathbf{R}}_x(\varphi )\cdot {\mathbf{p}}_{link}$$

3. 步行步态求解
   支撑相（以右腿支撑为例）：

• 目标：调整左腿摆动轨迹，同时保持躯干平衡。
• 躯干控制：通过髋关节角度补偿重心偏移，雅可比矩阵需包含躯干姿态的影响。
• 迭代过程：
  1. 规划左脚摆动轨迹${\mathbf{p}}_{foot}(t)$
  2. 计算当前左脚位置误差$e={\mathbf{p}}_{foot}(t)-{\mathbf{p}}_{current}(t)$
  3. 用雅可比逆矩阵更新左腿关节角$△q={\mathbf{J}}^{+}\cdot \mathbf{e}$
  4. 同步调整右腿髋关节横滚$\varphi$以维持 ZMP 在右脚支撑区内

摆动相轨迹规划：
常用三次多项式插值，设摆动时间$T$，起点${\mathbf{p}}_0$，终点${\mathbf{p}}_f$，则：
$$\mathbf{p}(t)={\mathbf{p}}_0+(3(\frac{t}{T}{)}^2-2(\frac{t}{T}{)}^3)({\mathbf{p}}_f-{\mathbf{p}}_0)$$

4.小跑步态求解
动态特性：双足腾空相存在，需控制落地冲击。

• 腾空相：腿部收缩以减少转动惯量。
• 支撑相：对角腿（如右腿和左腿）同步支撑，雅可比矩阵需同时处理双腿的耦合运动。

协调控制步骤：

1. 规划躯干质心（CoM）轨迹，使其符合倒立摆动力学。
2. 对角腿的脚部轨迹${\mathbf{p}}_{foot,left}(t)$和${\mathbf{p}}_{foot,right}(t)$相位差$180^o$。
3. 对每条腿独立应用雅可比逆矩阵法，但需引入耦合项确保躯干平衡：
   $$△{\mathbf{q}}_{left}={\mathbf{J}}_{left}^{+}\cdot {\mathbf{e}}_{left}+K\cdot ({\mathbf{q}}_{right}-{\mathbf{q}}_{nominal})$$

K为细条增益矩阵。

5.关键问题与优化

• 奇异性处理，当腿完全伸展${\theta }_k=0$时雅可比矩阵秩亏，需增加阻尼项或约束关节速度。
• 多目标优化
  同时优化脚部位置、躯干姿态和关节限制：
  $${\min }_{△q}(||\mathbf{J}△\mathbf{q}-e|{|}^2+\lambda ||△\mathbf{q}|{|}^2)$$
  其解为：
  $$△\mathbf{q}=({\mathbf{J}}^{\top }\mathbf{J}+\lambda \mathbf{I}{)}^{-1}{\mathbf{J}}^{\top }\mathbf{e}$$
• 实时性保障预计算常见步态的雅可比矩阵，或使用高效数值库（如 Eigen, ROS可调用eigen）进行伪逆计算。

工程挑战与解决方案

实际应用中的工具和算法

IKFast（OpenRAVE）

• 原理：自动生成解析解代码，适用于固定结构的机器人。

FABRIK（Forward and Backward Reaching IK）

• 特点：基于几何的迭代算法，高效且无需矩阵运算。

ROS中的IK求解器（如TRAC-IK、KDL）

• 功能：提供数值解接口，支持关节限制和实时计算。

多解问题

选择最优解：根据关节限位、能耗或平滑性选择解。
多解选择策略
能量最优原则：选择关节移动总距离最小的解
避障约束：排除导致机械干涉的解
硬件限制：关节角度限位、速度饱和（示例：舵机最大转角180°）

高自由度机器人（如Atlas的28自由度）

雅可比典型问题与解决方案
奇异位形崩溃：当腿部完全伸直时雅可比矩阵不可逆
解决方案：在轨迹规划阶段避开危险区域
动态扰动影响：奔跑时地面反作用力导致解算失效
应对措施：结合IMU数据实时修正期望位姿