双足机器人逆运动学解算：从建模推导到工程避坑全解析

简介：一套基于C和C++语言实现的双足机器人逆运动学解算工程，面向机器人方向的研究者、竞赛选手和嵌入式开发者，解决给定脚部位姿后关节角度如何计算的问题，也适用于人形机器人、机器狗等运动控制场景。压缩包共35个文件，体积仅66KB，包含cmake构建脚本、makefile配置、C/C++源文件、编译生成的二进制可执行文件及输出文件，结构清晰，源码与构建脚本分离，便于二次开发和调试。目前已有98人学习下载，适合有一定机器人学基础、想借鉴代码实现或验证数值迭代法等算法的读者。通过该工程可以了解从关节建模、运动学方程构建到求解输出的完整链路，还能结合构建配置快速复现实验，为后续加入动力学约束、神经网络智能解算等扩展提供参考。整体工程规模小巧，适合快速读懂核心逻辑。

1. 双足机器人逆运动学解算：先锁支撑脚，再谈其它

我在第一个双足步态仿真项目X里，卡了将近一周的不是步态规划，而是双足机器人逆运动学解算——躯干位姿明明定好了，摆动脚目标也选定了，可关节角怎么都解不对。后来才想明白，双足逆运动学解算和单臂IK有一个本质差别：双腿、躯干和地面构成一个闭合链，闭链不打破，关节角就没有唯一答案。这里要拆解的就是这个解算方案从建模到避坑的完整落地路径：先把支撑脚固定，把问题降成“髋到踝的两点定位”，再用解析法或雅可比法把髋、膝、踝的关节角算出来。适合正在写足式机器人程控、步态仿真，或者打算把机械臂IK经验迁移到双足构型的开发者。解算本身不复杂，难点在建模约定和几个高频坑。

2. 建模先行：D-H参数、坐标系与闭链约束的取舍

双足IK解出来，拿到仿真里一摆就穿模，十有八九不是算法错，是坐标系和D-H参数没有先立住。我习惯先把坐标系约定写死：世界系原点放在支撑脚踝正下方的地面上，X轴朝机器人前进方向，Z轴垂直向上，Y轴按右手定则指向机器人的左侧。这个约定让躯干位姿、髋位置、摆动脚目标都能用同一组数据表达，后面每一步排查都有据可查。

2.1 坐标基准：为什么把世界系放在支撑脚踝

如果把世界系放在躯干质心，每次躯干俯仰几度，所有坐标跟着转，步态数据可读性会差很多。放在支撑脚踝，支撑脚位置固定为 (0, 0, ankle_height)，躯干位姿直接由支撑腿的正运动学决定，摆动脚目标就是一个简单的 (x, y, z)。支撑脚切换时，世界系不动，只需要改哪条腿是支撑腿，每帧解算前把“支撑腿关节角→躯干位姿”算一次。

左右腿结构镜像的问题，处理方式是给髋偏摆、髋滚转这两类侧向关节在左腿取反号。这在多足步态里是常规做法：侧向关节角度左右腿互为相反数，矢状面关节角度左右共用同一套解算。这样一个 D-H 表、一套解算函数就能覆盖两条腿，省掉一半重复代码。

2.2 D-H参数表：写代码前先落一张表

我采用标准DH约定，每条腿从躯干往下依次是髋偏摆、髋滚转、髋俯仰、膝俯仰、踝俯仰，前三个关节集中在髋部，踝部只保留俯仰。省掉踝滚转是为了让小腿、脚掌始终位于大腿-小腿平面内，第 3 章的解析法才能直接套。D-H参数如下：

这里有个容易看晕的点：D-H 表里的 a 表示“沿当前 X 轴平移多少到达下一个关节轴”，所以大腿长 L1 出现在膝俯仰那一行，小腿长 L2 出现在踝俯仰那一行，而不是出现在髋俯仰行。α 为 -90° 那一行是为了把髋偏摆的旋转轴方向转成髋滚转轴方向。如果你的腿是踝带滚转的构型，这张表要再补一行，解析法也要升级成第 4 章的数值法。

2.3 从两条腿到躯干：闭链约束怎么打破

双足的闭链约束表现在：躯干位姿既由支撑腿的正运动学决定，又反过来约束摆动腿的逆解。我一般的处理顺序是：先固定支撑腿的关节角，用正运动学求躯干、进而求摆动侧髋关节在世界系下的位置；再取摆动脚目标位姿，得到踝关节位置；最后把髋位置、踝位置代入第 3 章的几何解算。闭链一打破，问题就变成“给定髋和踝两个点，求中间三个关节角”的常规链路。

下面是支撑腿正运动学求髋位置的函数，是整个双足 IK 工程可以起步的最小代码骨架。

import numpy as np def dh_transform(theta, d, a, alpha): """标准DH：绕Z转theta，沿Z平移d，沿X平移a，绕X转alpha""" ct, st = np.cos(theta), np.sin(theta) ca, sa = np.cos(alpha), np.sin(alpha) return np.array([ [ct, -st*ca, st*sa, a*ct], [st, ct*ca, -ct*sa, a*st], [0, sa, ca, d], [0, 0, 0, 1] ]) def fk_hip(leg_q, hip_offset): """支撑腿正运动学：由髋部三关节角求髋关节在躯干系的位置 leg_q: [q1, q2, q3] 髋偏摆、髋滚转、髋俯仰 hip_offset: 躯干中心到髋关节的固定平移 [x, y, z] """ T = np.eye(4) T[:3, 3] = hip_offset for theta, d, a, alpha in [ (leg_q[0], 0, 0, 0), # 髋偏摆 (leg_q[1], 0, 0, -np.pi/2), # 髋滚转 (leg_q[2], 0, 0, 0), # 髋俯仰，a=0得到髋关节中心 ]: T = T @ dh_transform(theta, d, a, alpha) return T[:3, 3] # 髋关节在躯干系下的位置

代码逻辑不复杂：每乘一个 D-H 变换矩阵，就把坐标系沿关节链推进一级。这里只叠乘髋部三关节，a 列置 0，返回的原点就是髋俯仰旋转轴的中心，也正是髋关节中心。要算膝、踝位置时，再把 D-H 表里对应行的 a=L1、a=L2 叠乘进去。两个参数容易改错：hip_offset 用的是“躯干中心到髋关节”的结构尺寸，不是 D-H 表里的 a；alpha 写成弧度，别把角度直接扔进来。算出的髋位置是相对躯干系的，后面要再叠加躯干在世界系下的位姿矩阵，才能得到世界系的髋位置。

3. 解析法解算：把腿拆成两段，用余弦定理求出髋、膝、踝

建模立住之后，进入解算。解析法适合没有踝滚转、腿部平面清晰的构型，也是我在双足仿真里用得最多、调试最快的方法。

3.1 两条腿的IK可以退化成平面两连杆

当踝关节只有俯仰，脚掌和小腿在同一个平面内，问题就降到二维：大腿 L1、小腿 L2、髋点 H、踝点 A。已知 H 和 A 的坐标，求髋俯仰 q3、膝俯仰 q4、踝俯仰 q5。这比解 6 自由度机械臂简单太多——先算髋到踝的直线距离 d，通过余弦定理一次性求出膝角，再反推髋角，最后用脚掌姿态约束补齐踝角。

需要注意，d 是髋到踝的直线距离，受步态规划里踝高度、髋高度的限制。当 d 大于 L1+L2 或小于 |L1-L2| 时，几何解不存在，这就是后面第 5 章要专门做的工作空间判定。另外，这一步假设髋偏摆和髋滚转已经由步态规划预先给定，实际调用求解器之前，要把世界系的踝坐标先变换到腿部平面内，再做平面解算。

3.2 髋-膝-踝三角几何推导与多解选择

设腿部平面内踝相对髋的坐标为 (dx, dz)，dx 是水平向前距离，dz 是垂直方向分量，通常踝低于髋，dz 为负。用余弦定理解三角形 H-K-A，三条边分别是 d、L1、L2：

• 膝俯仰：q4 = arccos((d² − L1² − L2²) / (2·L1·L2))，腿伸直时为 0，完全折叠时趋近 π
• 髋俯仰：连线角与大腿夹角之和，q3 = atan2(dz, dx) + arccos((L1² + d² − L2²) / (2·L1·d))
• 踝俯仰：脚掌姿态补齐，q5 = foot_pitch − q3 + q4

这里髋俯仰的推导要特别说明：三角形里大腿与髋-踝连线之间的夹角用余弦定理求出，大腿在该连线的上方，所以是相加而不是相减。多解问题在双足里比机械臂简单：膝关节朝前是唯一符合步态直观的解，因此直接取 q4 在 0 到 π 的主值即可，不需要像机械臂那样遍历腕部姿态。真正要小心的是零位约定，不同样机对踝关节零位的定义可能差 90 度，代码里的 foot_pitch 是脚掌相对水平面的目标角，不是踝关节角本身。

3.3 落到代码：5自由度腿的解析解算函数

def solve_planar_leg(hip_pos, ankle_pos, L1, L2, foot_pitch=0.0): """给定髋与踝(世界系)，解髋/膝/踝三个俯仰角 参数: hip_pos, ankle_pos: 世界系下位置 [x, y, z] L1, L2: 大腿、小腿长度 foot_pitch: 脚掌目标俯仰角(期望的脚掌姿态) 返回: [q3, q4, q5]，单位为弧度 """ dx = ankle_pos[0] - hip_pos[0] dz = ankle_pos[2] - hip_pos[2] d = np.hypot(dx, dz) # 工作空间检查：几何解存在才有意义 if d < 1e-6 or d > L1 + L2 or d < abs(L1 - L2): return None # 膝俯仰：由边长反解，伸直为0，完全折叠为pi c4 = (d*d - L1*L1 - L2*L2) / (2.0 * L1 * L2) q4 = np.arccos(np.clip(c4, -1.0, 1.0)) # 髋俯仰：连线角加大腿与连线的夹角 pitch_line = np.arctan2(dz, dx) beta = np.arccos(np.clip( (L1*L1 + d*d - L2*L2) / (2.0 * L1 * d), -1.0, 1.0)) q3 = pitch_line + beta # 脚掌姿态补齐踝俯仰，零位按样机标定 q5 = foot_pitch - q3 + q4 return [q3, q4, q5]

代码逻辑：先检查髋踝距离是否在可达范围，几何解不存在时返回 None，让上层决定是调整落脚点还是调整躯干高度；然后按膝、髋、踝的顺序解算，这个顺序不能颠倒，因为踝角依赖前两个角的结果。参数里有两个坑：一个是 atan2 的返回值范围是 (−π, π]，当连线角接近 ±π 时输出会发生跳变，需要配合第 5 章的角度连续性处理；另一个是 foot_pitch 的符号约定，不同步态规划器对“脚掌抬起为正”的定义可能相反，接入前先跑一个回读脚本核对。

如果机器人的腿带髋偏摆和髋滚转的预设角度，调用前要先做“去偏置”：把世界系的踝坐标旋转回腿部平面。常见做法是，用第 2 章的坐标变换算出踝在髋坐标系下、经过 yaw/roll 反旋后的坐标，再调用这个求解器。这个步骤漏掉，解出来的 q3 会整体偏移一个角度，表现就是机器人走路时躯干明显前倾或后仰。

4. 当解析法不够用：雅可比迭代与阻尼最小二乘的取舍

双足并不是所有构型都适合纯几何解。这一章讲的数值法，是我在换构型时最常用的一条后路。

4.1 哪些构型必须放弃纯几何解

如果你的腿部结构带踝滚转、髋部有横向偏距、或者小腿有倾斜设计，解析法的平面假设就容易翻车。另一种情况是自由度冗余：比如带踝滚转的 6 自由度腿，髋和踝定好后，踝滚转还能在多个解之间切换，几何法只能解出其中一个，还不一定是最自然的那个。遇到这类构型，我直接切换到数值法：构造雅可比矩阵，用阻尼最小二乘迭代求解。

数值法的可移植性比解析法好：换个机器人，只改 FK 函数和初始猜测，主迭代逻辑不用动。代价是对初值敏感。双足步态里相邻帧的关节角变化很小，拿上一帧的解当下一帧初值，迭代 5 到 10 次就能收敛，实时性通常不是问题。

4.2 雅可比矩阵怎么构建：数值差分足够用

雅可比矩阵把关节角增量映射到脚踝位置增量。解析推导对符号约定要求高，工程里数值差分更抗造。核心思想是：对每个关节角加一个小扰动，用正运动学算一遍脚踝位置，位置差除以扰动就是该列。这里的 fk_func 就是第 2 章 dh_transform 的完整叠乘，按 D-H 表五行顺序乘完，取 [:3, 3] 就是脚踝的世界位置。

def build_jacobian(leg_q, fk_func, delta=1e-6): """数值差分构建 3xN 雅可比矩阵 fk_func: 关节角->脚踝世界位置 [x,y,z] """ p0 = fk_func(leg_q) J = np.zeros((3, len(leg_q))) for i in range(len(leg_q)): q_plus = leg_q.copy() q_plus[i] += delta p_plus = fk_func(q_plus) J[:, i] = (p_plus - p0) / delta return J

delta 的取值是关键：太大，雅可比方向被非线性性污染；太小，浮点误差盖过差分信号。我这里用 1e-6 弧度，对毫米级腿长、微秒级仿真步长来说够用。脚踝位置对侧向关节（yaw/roll）的映射系数天然偏小，遇到侧向与俯仰串联的构型，建议先给侧向关节加一个机械增益再迭代，否则侧向误差收敛得很慢。

4.3 迭代求解主流程与阻尼项的作用

数值法的主流程是一个带阻尼的最小二乘迭代。目标是当前脚踝位置与目标位置之间的误差 Δp，求解关节增量 Δq。直接用伪逆 Δq = J⁺ Δp 在腿伸直附近会爆炸，所以给 J·Jᵀ 加一个阻尼项 λ²I，这就是阻尼最小二乘（DLS），它在误差收敛和奇异抑制之间取平衡。

def ik_dls(leg_q0, target_pos, fk_func, alpha=0.8, lam=0.01, tol=1e-4, max_iter=50): """阻尼最小二乘逆运动学 alpha: 迭代步长，0~1，过大易震荡 lam: 阻尼系数，腿越接近奇异越需要调大 """ q = leg_q0.copy() for _ in range(max_iter): err = target_pos - fk_func(q) if np.linalg.norm(err) < tol: break J = build_jacobian(q, fk_func) dq = J.T @ np.linalg.solve(J @ J.T + lam*lam*np.eye(3), err) q += alpha * dq return q

参数设置建议：alpha 取 0.6 到 0.9，仿真里取 0.8 收敛快而且稳；lam 初始取 0.01，腿接近伸直时拉到 0.1；tol 取 1e-4 米，相当于 0.1 毫米，视觉上已经完全够用。收敛失败时优先看 err 是震荡还是缓慢下降：震荡就把 alpha 调小，缓慢下降就把 lam 调小。还有一个容易忽略的点：初值 leg_q0 最好来自解析法或上一帧解，而不是全零，全零初值在冗余构型里会收敛到一个关节角分布很不合理的解，虽然位置对了，姿态却没法看。

5. 双足IK避坑：限位、奇异、跳变与收敛失败的五种现场

这一章是我在项目里踩过、也帮人排查过最频繁的五个问题。每条都按现象、原因、解决的顺序来写。

5.1 膝关节反向后弯：余弦定理符号用反了

现象：解算结果让机器人膝关节向后弯，大腿和小腿反弓，躯干直接趴下。原因：余弦定理里 c4 的符号约定写反，把伸直状态算成 π，弯曲状态算成 0，膝盖就反了。解决：按 q4 = arccos((d² − L1² − L2²) / (2·L1·L2)) 计算，腿伸直时 q4 为 0，完全折叠时趋近 π；再通过大腿与小腿的叉积方向做一次符号校验，把 q4 的限位下限设成 0，彻底阻断反关节。

5.2 髋、踝俯仰在某一步骤跳变180度：atan2返回值跨π导致

现象：步态数据在某一帧忽然跳变，前后帧躯干姿态完全翻转，回放时机器人像被抽了一鞭子。原因：atan2 的返回值在 ±π 边界不连续，髋角从接近 π 跳到接近 −π，视觉上就是翻转。解决：对解算输出做角度连续性处理，用角度差取模到 (−π, π] 再做累加，确保相邻帧角度差不被 π 的边界放大。这个后处理代码很短，通常两行就能解决，但必须在每个输出关节角上都做，不能只处理髋俯仰。

5.3 腿伸直时数值法震荡不收敛：雅可比接近奇异

现象：初值接近腿伸直，ik_dls 迭代几十次，误差一直下不去。原因：腿伸直时髋到踝距离接近 L1+L2，雅可比各列近似线性相关，J·Jᵀ 接近奇异，伪逆解被放大。解决：加阻尼项 lam，或者检测 d 与 L1+L2 的差值，差值小于阈值时切换步态轨迹，先抬髋再迈步，绕开奇异区。我在仿真里把阈值设为 1 厘米，也就是 d > L1+L2−0.01 时，不再调用数值法。

5.4 算出的踝roll与规划值对不上：D-H约定与姿态顺序不一致

现象：同一个踝滚转角，在姿态回读里和规划目标差 0.1 到 0.3 弧度。原因：D-H 表里 α 为 −90° 那一行的方向，和步态规划用的欧拉角顺序不一致，侧向轴方向理解反了。解决：统一姿态表达为“先 yaw 再 roll 再 pitch”，并写一个回读比对脚本，每次改动 D-H 表后自动检查。这个检查脚本就是第 6 章要展开的验证函数，别省。

5.5 关节角都正常但仿真里脚掌穿地：工作空间判定缺失

现象：IK 返回的每个角都在关节限位范围内，但脚掌陷进地面里。原因：髋踝距离超出工作空间，或者踝目标 z 坐标低于地面，IK 硬解出一个姿态，关节角被限位截断后，脚就被拉进地里。解决：在解算入口就检查踝目标 z 坐标与脚掌厚度的关系；同时把 d 与 L1+L2 的偏差作为是否调整落脚点的判定依据。这两个检查都属于工作空间判定，放在最前面，比任何算法优化都重要。

6. 解完不算完：用位姿回读和离线仿真验证 IK 的边界

解算代码写完只是第一步，真正让双足 IK 可用的是一套验证习惯。我的做法是三步：先位姿回读，再离线仿真，最后做连续步态一致性检查。

6.1 位姿回读：把解算结果反向投影回目标点

位姿回读就是把解算出的关节角再代入正运动学，拿脚踝位置和原始目标比对。理论上用同一个正运动学函数，回读偏差一定为零；真正的问题是关节限位截断了 IK 结果，回读时偏差会出现在某个关节的限位点上。为了排查，我把回读函数做得能逐个关节检查。

def verify_ik(q_leg, target_pos, fk_func, tol=1e-3): """回读校验IK结果，返回偏差与超限关节""" pos = fk_func(q_leg) err = np.linalg.norm(pos - target_pos) overflow = [[i, q] for i, q in enumerate(q_leg) if q > joint_limits[i][1] or q < joint_limits[i][0]] return err, overflow

超限列表里的每一项都直接指向问题关节，省去逐个排查的时间。

6.2 连续步态下的一致性检查：专查跳变和奇异

双足 IK 比单帧 IK 多一层要求：连续步态中同一关节的角度应当平滑变化。我会跑一段 2 到 3 秒的步态轨迹，对每个关节的相邻帧角度差分取绝对值，超过 0.3 弧度就报警。这种检查能抓出 atan2 跳变和限位截断引发的抖动，是纯静态验证看不到的边界问题。

把这三步固化成一个“解算—回读—仿真—检查”的习惯后，我换新构型机器人的调试时间从几天压缩到几个小时。某个项目里我因为漏掉一致性检查，步态跑到第三秒脚掌就抖起来，最后定位到限位截断与髋偏摆符号约定冲突——从那以后，回读脚本和一致性检查就成了我解算代码的标配。换构型、改尺寸、加自由度，跑一遍这套流程就敢把 IK 结果喂给步态控制器。希望这篇双足机器人逆运动学解算笔记能帮你在第一步就绕开我走过的这些坑。

本文还有配套的精品资源，点击获取