启元 Q1 腿部运动学逆解：Python 算站立下蹲关节角

七夜zippoe2026-10-23 14:14

摘要

本文面向没有启元 PrimeBot Q1 真机的开发者，基于 q1_v3.urdf 的大腿 0.108046m 与小腿 0.170652m 两段连杆，用纯 Python 实现正运动学与闭式逆运动学，反解站立、半蹲、深蹲三档姿态的髋膝角，并对照 hip_pitch 负175度到90度、膝0度到140度限位做合规校验。文章先从关节轴与机体坐标系推出角度符号约定，厘清下蹲前屈应取负角，再以脚本实算给出三档姿态与可达工作空间：站立高 27.87 厘米，半蹲 43.94/70 度高 23.11 厘米，深蹲 101.68/140 度高 11.20 厘米，膝 140 度封顶深度。全文脚本可直接复现，等价于一份无真机可达性预检。

📌 版本声明：本文基于 PrimeBot Q1 SDK（aimdk_msgs / primebot-q1-sdk），配套官方文档 dev.primebot.com，无真机离线分析。所有连杆长度与关节限位均取自 q1_v3.urdf，几何计算以文内脚本输出为准。

文章目录

一、为什么要在"无真机"状态下算腿

很多开发者第一次接触启元 PrimeBot Q1，第一反应是先把机器开机、连上 SDK、跑一段站立 demo。这条路径本身没错，但它有一个绕不开的前提：你得有一台真机、一台装好 Ubuntu 22.04 + ROS 2 Humble 的机器，以及一份能下发的运动模式授权。官方明确不支持在 Mac / Windows 上做开发编译，环境门槛先把一大批人挡在门外。

更关键的是，即便手边有真机，"先开机再调参"也并不是理解运动学最高效的方式。关节角解算本质是几何问题，只依赖连杆长度与关节限位这两类静态参数，而这两类参数都白纸黑字写在 q1_v3.urdf 里。只要把 URDF 读明白，在家用一台笔记本就能把"站多高、蹲多深、该给哪些角度"算得清清楚楚。

本系列前面的文章已经讲过 SDK 环境搭建、接口地图、MJCF 对比和指令预检器，本文换一个更"硬核"的切入点：不演示如何连真机，而是直接写一个几何逆解脚本，把站立、半蹲、深蹲三档姿态的关节角算出来，并验证这些解是否真的落在 URDF 限位之内。

需要说明的是，05 篇的预检器回答的是"给定一条指令合不合法"，本文回答的是更上游的问题："这条指令该不该这么给"。前者是裁判，后者是教练------先把几何算清楚，指令自然不必等到校验环节才被拦下来。两篇文章共用同一份 URDF 限位数据，结论可以互相印证。

这样做还有一个额外收益：当你把"解算结果"和"限位区间"摆在一起看，一个反直觉的事实会浮出水面------决定下蹲深度的往往是膝的 140° 上限，而不是髋的俯仰限位。更微妙的是，这个判断完全取决于角度符号约定：约定差一个负号，"瓶颈关节"就会从膝翻到髋。所以本文动手算角之前，先花一整节把符号钉死，再谈几何、逆解与可达性。

这也是本系列"无真机复现"思路的自然延伸：不在真机上点点按按，而是把机器人拆成可离线分析的文件------URDF 描述结构、MJCF 描述仿真、Python 脚本做计算。运动学逆解恰好是这条链路里最干净的一环：不依赖传感器、不依赖控制频率、不依赖网络，只要几何常量正确，结果就是确定的。你今天在笔记本上算出的 11.20 厘米，和明天在真机上测到的落点，在理想刚体假设下应当一致。

二、先把符号钉死：URDF 角度到底谁正谁负

为什么先讲符号？因为两杆 IK 的公式到处都长一个样，差别全在"哪个方向算正"。URDF 里 left_hip_pitch_joint 与 left_knee_joint 的轴都是 <axis xyz="0 1 0">，即绕机体 Y 轴旋转；机体坐标系遵循 URDF 标准的 z-up 约定，而 api.md 在描述 /aima/mc/locomotion/velocity 时注明"前进速度为 X 轴"------机体 +X 是前进方向，+Y 是左侧，+Z 竖直向上。

有了这三个事实，就能用右手定则做一次符号自检：把站立时竖直向下的大腿向量 (0,0,−1) 依次绕 +Y 旋转 +90° 与 −90°，看它被推向哪。结果前者落到 (−1,0,0)，后者落到 (+1,0,0)。翻译成工程语言：hip_pitch 取正角，腿被推向 −X，也就是向后摆；取负角才推向 +X，向前屈。下蹲是前屈动作，所以 URDF 口径下深蹲必须给负角------这是全文最重要的一个负号。

膝侧则没有歧义：knee 轴同为 (0,1,0)，正角使小腿相对大腿向后上方回摆（蹲下时膝盖前顶、脚向后撤），与"屈曲为正"的解剖直觉一致，直接取正即可。于是得到模型角与 URDF 角的映射：模型用 θ1 表示髋前屈角（正=前屈）、θ2 表示膝屈曲角（正=屈膝），则 hip_pitch = −θ1、knee = +θ2；限位也跟着镜像，URDF 的 −175°, +90° 换成模型口径就是"前屈预算 175°、后伸预算 90°"。

还有两个佐证值得记。其一，左腿各关节位于机体 +Y 侧、右腿位于 −Y 侧，但 hip_pitch 与 knee 的轴都是 (0,1,0)、限位完全一致，矢状面分析对左右腿同一套公式，差别只在翻滚/偏航限位互为相反数。其二，别用 MJCF 变体里的近似区间做这个推导：q1_v3.xml 是显示用文件，range 用的是 −2.95~1.47 这类近似值，只有 q1_v3.urdf 与 q1_v3_mc.xml 的精确区间才配当分析基准。

别小看这次自检。如果跳过它、想当然把"前屈为正"代进 URDF，就会算出"深蹲需要髋 101.7°、超过 90° 上界"的错误结论，把瓶颈误判到髋上；而按 URDF 轴推导，同样的 101.68° 前屈在 URDF 里是 −101.68°，距 −175° 下限还有 73.32° 余量，髋根本不是约束。符号镜像的后果第十节会用两组实算数值现场演示，错得有多离谱一看便知。

代码 1 先把 URDF 里的常数与限位全部现场解析出来，不手填任何数字------这是后续所有计算的地基。

python 复制代码 
import math
import xml.etree.ElementTree as ET

URDF = "q1_v3.urdf"

def read_urdf(path):
    """解析 URDF，返回 {关节名: 属性dict}，长度与限位全部现场读取。"""
    root = ET.parse(path).getroot()
    joints = {}
    for jn in root.findall("joint"):
        og = jn.find("origin")
        lim = jn.find("limit")
        joints[jn.get("name")] = {
            "parent": jn.find("parent").get("link"),
            "child":  jn.find("child").get("link"),
            "xyz": tuple(float(v) for v in og.get("xyz", "0 0 0").split()),
            "axis": tuple(float(v) for v in jn.find("axis").get("xyz").split()),
            "lower": float(lim.get("lower")) if lim is not None else None,
            "upper": float(lim.get("upper")) if lim is not None else None,
        }
    return joints

joints = read_urdf(URDF)

# 大腿/小腿长 = 子关节原点相对父连杆偏移的模长（含 x/y/z 全部分量）
L1 = math.dist((0, 0, 0), joints["left_knee_joint"]["xyz"])
L2 = math.dist((0, 0, 0), joints["left_ankle_pitch_joint"]["xyz"])

# 限位：URDF 存弧度，统一转角度
hp_lo = math.degrees(joints["left_hip_pitch_joint"]["lower"])
hp_hi = math.degrees(joints["left_hip_pitch_joint"]["upper"])
kn_lo = math.degrees(joints["left_knee_joint"]["lower"])
kn_hi = math.degrees(joints["left_knee_joint"]["upper"])

print(f"L1 = {L1:.6f} m, L2 = {L2:.6f} m")
print(f"L1+L2 = {(L1+L2)*100:.2f} cm, |L1-L2| = {abs(L1-L2)*100:.2f} cm")
print(f"hip_pitch: [{hp_lo:.2f}, {hp_hi:.2f}] deg, "
      f"axis={joints['left_hip_pitch_joint']['axis']}")
print(f"knee:      [{kn_lo:.2f}, {kn_hi:.2f}] deg, "
      f"axis={joints['left_knee_joint']['axis']}")

代码 1 说明（URDF 常量解析） ：read_urdf 用标准库 xml.etree 遍历所有 joint，把原点偏移、旋转轴、上下限位一并收进字典，lim is not None 的判断是为了跳过 3 个 fixed 关节（它们没有 limit 标签，直接取值会抛 AttributeError）。连杆长度取子关节 origin 的模长而非单一 z 分量，是为了不丢几何信息------left_knee_joint 的偏移 (−0.015, 0, −0.107) 里含 1.5 厘米后移量，模长 0.108046m 比 0.107m 严谨。两个易错点：URDF 的 limit 是弧度，必须 math.degrees 转换；axis 字段要一起打印出来，因为符号约定全靠它。运行结果应与文中常量表逐项一致。

三、两段连杆几何模型与正运动学

给腿建立几何模型的第一步，是承认"纯蹲起"只发生在矢状面内。从髋到踝，真正起作用的只有两段：大腿（髋偏航关节到膝关节）与小腿（膝关节到踝俯仰关节）。踝俯仰主要补偿足面贴地，髋翻滚负责侧倾，髋偏航负责转向，在"脚尖朝前、原地蹲起"的设定里贡献可以暂时剥离，只留髋俯仰与膝屈曲两个自由度。先降维到两杆，是为了用余弦定理拿闭式解；等这个基座打牢，再叠加其余自由度做全身 IK，才是稳妥的工程顺序。

反过来说，如果不先降维会怎样：22 个可动关节的全身 IK 是带冗余的非线性优化，初值敏感、多解泛滥，离线脚本很难给出确定答案。两杆模型是唯一能在笔记本上"秒算且可验证"的切入点，也是理解全身 IK 的必经台阶。

两段长度不是拍脑袋：left_knee_joint 相对父连杆的偏移是 (−0.015, 0, −0.107)，模长得 L1 = 0.108046m；left_ankle_pitch_joint 的偏移 (0.015015, 0, −0.16999) 给出 L2 = 0.170652m。还有个容易忽略的细节：髋俯仰关节的 rpy 是 −20°，髋翻滚关节的 rpy 是 +20°，两者相互抵消，因此矢状面分析不需要再叠任何静态旋转，零位姿下大腿方向就是竖直向下。髋翻滚、偏航两关节的结构偏移并入髋部机构、不计入腿长，这是两杆模型的第一处近似，第九节会说明它的量级。模型对左右腿完全对称：hip_pitch 与 knee 的轴、限位左右一致，一套公式两边通用；真正不对称的是 roll 与 yaw 的限位，而那是侧倾与转向的事，与原地蹲起无关。

常量 数值 URDF 来源
大腿长 L1 0.108046 m left_knee_joint origin (-0.015, 0, -0.107) 模长
小腿长 L2 0.170652 m left_ankle_pitch_joint origin (0.015015, 0, -0.16999) 模长
腿全长 L1+L2 27.87 cm 站立垂落髋-踝距
最短髋踝距 r_min 11.20 cm 膝 140° 屈曲（余弦定理实算）
理论全屈 |L1−L2| 6.26 cm 无膝限位时才可达
hip_pitch 限位 −175°, 90° -3.0543 ~ 1.5708 rad，axis (0,1,0)
knee 限位 0°, 140° 0 ~ 2.4435 rad，axis (0,1,0)

正运动学把 (θ1, θ2) 映射到足端相对髋的坐标。以髋为原点、前向为 +x、竖直下垂为 +h，大腿向量与竖直方向夹角 θ1，小腿绝对角为 θ1−θ2（膝屈曲使小腿回摆），于是：

text 复制代码 
x = L1·sin(θ1) + L2·sin(θ1 − θ2)
h = L1·cos(θ1) + L2·cos(θ1 − θ2)

这是竖直角参考写法。你若在别处见过 h = L1·sin θ1* + L2·sin(θ1*+θ2) 的水平角参考形式（研究简报即用此式），那只是把参考零位从竖直挪到水平：令 θ1* = 90° − θ1，两种形式完全等价------脚本对 (43.94°, 70°) 抽查，两种写法都给 h = 23.1103cm，一个字节都不差。公式里小腿用 θ1−θ2 而不是 θ1+θ2，正是因为膝屈曲让小腿相对大腿向后回摆，这个减号正是"膝盖前顶、脚向后撤"的几何刻画。

下面这张类图把"数据"与"行为"固化成模型结构，后续所有脚本都围绕它展开。
#mermaid-svg-hP9JYECGdOxqS4e8{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-hP9JYECGdOxqS4e8 .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-hP9JYECGdOxqS4e8 .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-hP9JYECGdOxqS4e8 .error-icon{fill:#552222;}#mermaid-svg-hP9JYECGdOxqS4e8 .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-hP9JYECGdOxqS4e8 .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-hP9JYECGdOxqS4e8 .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-hP9JYECGdOxqS4e8 .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-hP9JYECGdOxqS4e8 .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-hP9JYECGdOxqS4e8 .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-hP9JYECGdOxqS4e8 .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-hP9JYECGdOxqS4e8 .marker{fill:#333333;stroke:#333333;}#mermaid-svg-hP9JYECGdOxqS4e8 .marker.cross{stroke:#333333;}#mermaid-svg-hP9JYECGdOxqS4e8 svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-hP9JYECGdOxqS4e8 p{margin:0;}#mermaid-svg-hP9JYECGdOxqS4e8 g.classGroup text{fill:#9370DB;stroke:none;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:10px;}#mermaid-svg-hP9JYECGdOxqS4e8 g.classGroup text .title{font-weight:bolder;}#mermaid-svg-hP9JYECGdOxqS4e8 .cluster-label text{fill:#333;}#mermaid-svg-hP9JYECGdOxqS4e8 .cluster-label span{color:#333;}#mermaid-svg-hP9JYECGdOxqS4e8 .cluster-label span p{background-color:transparent;}#mermaid-svg-hP9JYECGdOxqS4e8 .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-hP9JYECGdOxqS4e8 .cluster text{fill:#333;}#mermaid-svg-hP9JYECGdOxqS4e8 .cluster span{color:#333;}#mermaid-svg-hP9JYECGdOxqS4e8 .nodeLabel,#mermaid-svg-hP9JYECGdOxqS4e8 .edgeLabel{color:#131300;}#mermaid-svg-hP9JYECGdOxqS4e8 .edgeLabel .label rect{fill:#ECECFF;}#mermaid-svg-hP9JYECGdOxqS4e8 .label text{fill:#131300;}#mermaid-svg-hP9JYECGdOxqS4e8 .labelBkg{background:#ECECFF;}#mermaid-svg-hP9JYECGdOxqS4e8 .edgeLabel .label span{background:#ECECFF;}#mermaid-svg-hP9JYECGdOxqS4e8 .classTitle{font-weight:bolder;}#mermaid-svg-hP9JYECGdOxqS4e8 .node rect,#mermaid-svg-hP9JYECGdOxqS4e8 .node circle,#mermaid-svg-hP9JYECGdOxqS4e8 .node ellipse,#mermaid-svg-hP9JYECGdOxqS4e8 .node polygon,#mermaid-svg-hP9JYECGdOxqS4e8 .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-hP9JYECGdOxqS4e8 .divider{stroke:#9370DB;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 g.clickable{cursor:pointer;}#mermaid-svg-hP9JYECGdOxqS4e8 g.classGroup rect{fill:#ECECFF;stroke:#9370DB;}#mermaid-svg-hP9JYECGdOxqS4e8 g.classGroup line{stroke:#9370DB;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 .classLabel .box{stroke:none;stroke-width:0;fill:#ECECFF;opacity:0.5;}#mermaid-svg-hP9JYECGdOxqS4e8 .classLabel .label{fill:#9370DB;font-size:10px;}#mermaid-svg-hP9JYECGdOxqS4e8 .relation{stroke:#333333;stroke-width:1;fill:none;}#mermaid-svg-hP9JYECGdOxqS4e8 .dashed-line{stroke-dasharray:3;}#mermaid-svg-hP9JYECGdOxqS4e8 .dotted-line{stroke-dasharray:1 2;}#mermaid-svg-hP9JYECGdOxqS4e8 #compositionStart,#mermaid-svg-hP9JYECGdOxqS4e8 .composition{fill:#333333!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 #compositionEnd,#mermaid-svg-hP9JYECGdOxqS4e8 .composition{fill:#333333!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 #dependencyStart,#mermaid-svg-hP9JYECGdOxqS4e8 .dependency{fill:#333333!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 #dependencyStart,#mermaid-svg-hP9JYECGdOxqS4e8 .dependency{fill:#333333!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 #extensionStart,#mermaid-svg-hP9JYECGdOxqS4e8 .extension{fill:transparent!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 #extensionEnd,#mermaid-svg-hP9JYECGdOxqS4e8 .extension{fill:transparent!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 #aggregationStart,#mermaid-svg-hP9JYECGdOxqS4e8 .aggregation{fill:transparent!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 #aggregationEnd,#mermaid-svg-hP9JYECGdOxqS4e8 .aggregation{fill:transparent!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 #lollipopStart,#mermaid-svg-hP9JYECGdOxqS4e8 .lollipop{fill:#ECECFF!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 #lollipopEnd,#mermaid-svg-hP9JYECGdOxqS4e8 .lollipop{fill:#ECECFF!important;stroke:#333333!important;stroke-width:1;}#mermaid-svg-hP9JYECGdOxqS4e8 .edgeTerminals{font-size:11px;line-height:initial;}#mermaid-svg-hP9JYECGdOxqS4e8 .classTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-hP9JYECGdOxqS4e8 .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-hP9JYECGdOxqS4e8 .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-hP9JYECGdOxqS4e8 :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 包含大腿与小腿
含髋俯仰与膝屈曲
限位约束解算结果
1
1
2
2
LegLink
+string name
+float length_m
+bool is_thigh
Joint
+string name
+float lower_deg
+float upper_deg
+in_range(angle_deg) : bool
LegKinematics
+float L1
+float L2
+fk(hip_flex, knee_flex) : tuple
+ik_foot_under_hip(h) : tuple
+check_limits(hip_flex, knee_flex) : dict
+reachable(x, h) : bool

代码 2 先复现第二节的符号自检，再封装 forward_kinematics，并输出三档姿态的足端位置。

python 复制代码 
def rot_y(v, deg):
    """绕 Y 轴旋转 deg 度（右手定则），用于自检 URDF 关节轴语义。"""
    t = math.radians(deg)
    x, y, z = v
    return (x*math.cos(t) + z*math.sin(t), y, -x*math.sin(t) + z*math.cos(t))

# 机体坐标系：+X 前（api.md：前进速度为 X 轴）、+Y 左、+Z 上
# hip_pitch / knee 的 axis 均为 (0,1,0)
print("R_y(+90)*(0,0,-1) =", tuple(round(c, 4) for c in rot_y((0, 0, -1), 90)))
print("R_y(-90)*(0,0,-1) =", tuple(round(c, 4) for c in rot_y((0, 0, -1), -90)))
# => 正角把腿推向 -X（向后摆）；负角推向 +X（前屈，下蹲方向）

def forward_kinematics(t1_deg, t2_deg):
    """正运动学。t1=髋前屈角(正=前屈)，t2=膝屈曲角(正=屈膝)。
    返回足端相对髋的 (x 前向, h 下垂深度)，单位 m。"""
    t1, t2 = math.radians(t1_deg), math.radians(t2_deg)
    x = L1*math.sin(t1) + L2*math.sin(t1 - t2)
    h = L1*math.cos(t1) + L2*math.cos(t1 - t2)
    return x, h

# URDF 映射：hip_pitch = -t1（前屈取负），knee = +t2（屈曲取正）
for name, t1, t2 in [("站立", 0, 0), ("半蹲", 43.94, 70.0),
                     ("深蹲", 101.68, 140.0)]:
    x, h = forward_kinematics(t1, t2)
    print(f"{name}: 模型角=({t1:.2f},{t2:.2f}) URDF=({-t1:.2f},{t2:.2f}) "
          f"-> x={x*100:+.2f}cm h={h*100:.2f}cm")

代码 2 说明（符号自检与正运动学） ：rot_y 实现绕 Y 轴的标准旋转矩阵，输入 (0,0,-1) 分别旋 ±90°，打印结果 (−1,0,0) 与 (+1,0,0)，把第二节的右手定则推导变成可复现的实验------这是全文唯一不能跳过的一步。forward_kinematics 沿用代码 1 解析出的 L1、L2，输入模型角返回 (x, h)；循环里同时打印模型角与 URDF 角两套值，提醒读者下发时要做 hip_pitch = −θ1 的取负映射。冒烟测试：站立档必须输出 x=+0.00cm、h=27.87cm，否则常量或公式抄错。

图1：启元 Q1 腿部两段连杆几何模型与核心常量示意

四、逆运动学：闭式解、多解分支与往返验证

正运动学解决"给角度、求位置"，控制时更常遇到反问题：想让足端落到某个高度，髋膝各给多少度？在两杆平面模型里，逆解有闭式表达，靠余弦定理，不用迭代。目标姿态约定为"足端落在髋正下方"（x=0）：单足支撑时重心投影最稳，也最贴近真实站立与下蹲。

给定下垂高度 h，髋-踝直线距离 r = h。先判可达区间：两杆能到的最近距离由膝上限决定，r_min = √(L1²+L2²+2·L1·L2·cos140°) = 11.20cm；最远就是腿全长 27.87cm。目标落在区间外直接返回 None，因为数学上无解。区间内先用 c2 = (r²−L1²−L2²)/(2·L1·L2) 反解膝角 θ2 = arccos(c2)，再用第二段余弦定理反解髋角 θ1 = arccos((L1²+r²−L2²)/(2·L1·r))。

必须讨论多解。平面两杆 IK 数学上有两支：膝前屈支与膝后屈支，前者对应人类蹲起，后者对应折叠收储（Q1 的 STORE_DEFAULT 即此类构型）。URDF 给 knee 的下限是 0°，明确禁止过伸，因此膝后屈支在真机上非法，足下分支解唯一。这不是遗憾，反而是工程上的简化：不用自己选支，闭式解直接给。第七节的可达性采样同样只认这一支，正是因为膝下限把第二支整个砍掉了。

若目标点不限足下（给定任意 (x, h)），两支解写成 θ1 = φ ± α：其中 φ = atan2(x, h) 是目标方向角，α = arccos((L1²+r²−L2²)/(2·L1·r)) 是大腿与目标方向的夹角，膝前屈支取 +α、膝后屈支取 −α。后者被 knee ≥ 0° 直接判非法，所以对任意目标点，合法解同样唯一。第七节的 reachable 函数用的正是这个通用式。

边界行为值得单独验：把 h = 0.0626m（|L1−L2|，理论全屈才够得着）喂进去返回 None，因为那要求膝屈曲约 180°，既超 140° 又过伸；h = 0.30m 也返回 None，超过腿全长；甚至字面的 h = 0.2787m 都会被拒------它比真实腿全长 0.278698m 大了 2 微米。最后一条是浮点教训：边界比较必须留 epsilon 或先钳制，否则"刚好站立"会莫名其妙不可达。IK 的解代回 FK，往返误差在 1e-17m 量级，只说明公式自洽，不代表真机精度。
#mermaid-svg-cDwCcHp6xF11qeXX{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-cDwCcHp6xF11qeXX .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-cDwCcHp6xF11qeXX .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-cDwCcHp6xF11qeXX .error-icon{fill:#552222;}#mermaid-svg-cDwCcHp6xF11qeXX .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-cDwCcHp6xF11qeXX .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-cDwCcHp6xF11qeXX .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-cDwCcHp6xF11qeXX .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-cDwCcHp6xF11qeXX .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-cDwCcHp6xF11qeXX .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-cDwCcHp6xF11qeXX .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-cDwCcHp6xF11qeXX .marker{fill:#333333;stroke:#333333;}#mermaid-svg-cDwCcHp6xF11qeXX .marker.cross{stroke:#333333;}#mermaid-svg-cDwCcHp6xF11qeXX svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-cDwCcHp6xF11qeXX p{margin:0;}#mermaid-svg-cDwCcHp6xF11qeXX .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-cDwCcHp6xF11qeXX .cluster-label text{fill:#333;}#mermaid-svg-cDwCcHp6xF11qeXX .cluster-label span{color:#333;}#mermaid-svg-cDwCcHp6xF11qeXX .cluster-label span p{background-color:transparent;}#mermaid-svg-cDwCcHp6xF11qeXX .label text,#mermaid-svg-cDwCcHp6xF11qeXX span{fill:#333;color:#333;}#mermaid-svg-cDwCcHp6xF11qeXX .node rect,#mermaid-svg-cDwCcHp6xF11qeXX .node circle,#mermaid-svg-cDwCcHp6xF11qeXX .node ellipse,#mermaid-svg-cDwCcHp6xF11qeXX .node polygon,#mermaid-svg-cDwCcHp6xF11qeXX .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-cDwCcHp6xF11qeXX .rough-node .label text,#mermaid-svg-cDwCcHp6xF11qeXX .node .label text,#mermaid-svg-cDwCcHp6xF11qeXX .image-shape .label,#mermaid-svg-cDwCcHp6xF11qeXX .icon-shape .label{text-anchor:middle;}#mermaid-svg-cDwCcHp6xF11qeXX .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-cDwCcHp6xF11qeXX .rough-node .label,#mermaid-svg-cDwCcHp6xF11qeXX .node .label,#mermaid-svg-cDwCcHp6xF11qeXX .image-shape .label,#mermaid-svg-cDwCcHp6xF11qeXX .icon-shape .label{text-align:center;}#mermaid-svg-cDwCcHp6xF11qeXX .node.clickable{cursor:pointer;}#mermaid-svg-cDwCcHp6xF11qeXX .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-cDwCcHp6xF11qeXX .arrowheadPath{fill:#333333;}#mermaid-svg-cDwCcHp6xF11qeXX .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-cDwCcHp6xF11qeXX .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-cDwCcHp6xF11qeXX .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-cDwCcHp6xF11qeXX .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-cDwCcHp6xF11qeXX .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-cDwCcHp6xF11qeXX .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-cDwCcHp6xF11qeXX .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-cDwCcHp6xF11qeXX .cluster text{fill:#333;}#mermaid-svg-cDwCcHp6xF11qeXX .cluster span{color:#333;}#mermaid-svg-cDwCcHp6xF11qeXX div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-cDwCcHp6xF11qeXX .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-cDwCcHp6xF11qeXX rect.text{fill:none;stroke-width:0;}#mermaid-svg-cDwCcHp6xF11qeXX .icon-shape,#mermaid-svg-cDwCcHp6xF11qeXX .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-cDwCcHp6xF11qeXX .icon-shape p,#mermaid-svg-cDwCcHp6xF11qeXX .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-cDwCcHp6xF11qeXX .icon-shape .label rect,#mermaid-svg-cDwCcHp6xF11qeXX .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-cDwCcHp6xF11qeXX .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-cDwCcHp6xF11qeXX .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-cDwCcHp6xF11qeXX :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 否
是
NG
WARN
OK
输入目标足端高度 h
h 在 11.20~27.87 cm?
返回 None 数学上无解
余弦定理解膝屈曲角
余弦定理解髋前屈角
限位校验
拒绝下发
贴边告警 建议留1到2度
输出 hip_pitch 与前屈取负映射

代码 3 实现 ik_foot_under_hip，并把上述边界逐条跑成断言式的输出。

python 复制代码 
def ik_foot_under_hip(h_target):
    """足下分支闭式逆解：足端落在髋正下方(x=0)，给定 h 反解 (t1, t2)。
    h 超出 [r_min, L1+L2] 返回 None。r_min 由膝上限 kn_hi 决定。"""
    r = h_target
    r_min = math.sqrt(L1*L1 + L2*L2 + 2*L1*L2*math.cos(math.radians(kn_hi)))
    if not (r_min <= r <= L1 + L2):
        return None
    c2 = max(-1.0, min(1.0, (r*r - L1*L1 - L2*L2) / (2*L1*L2)))
    t2 = math.degrees(math.acos(c2))          # 膝不能过伸，支唯一
    ratio = max(-1.0, min(1.0, (L1*L1 + r*r - L2*L2) / (2*L1*r)))
    t1 = math.degrees(math.acos(ratio))       # 髋前屈角
    return t1, t2

# 往返验证：IK 的解代回 FK 应还原目标高度
for label, ht in [("站立", 0.278698), ("半蹲", 0.2311), ("深蹲", 0.1120)]:
    sol = ik_foot_under_hip(ht)
    if sol is None:
        print(f"{label}: h={ht*100:.2f}cm 不可达")
        continue
    t1, t2 = sol
    x, h = forward_kinematics(t1, t2)
    print(f"{label}: h={ht*100:.2f}cm -> 前屈={t1:.2f}deg 膝={t2:.2f}deg "
          f"反算h={h*100:.2f}cm 误差={abs(h-ht):.1e}m")

# 边界：低于 11.20cm 或高于 27.87cm 一律 None
print(ik_foot_under_hip(0.0626))   # None: 需膝约180度，超限且过伸
print(ik_foot_under_hip(0.30))     # None: 超过腿全长
print(ik_foot_under_hip(0.2787))   # None: 比真实腿全长 0.278698 大 2 微米

代码 3 说明（逆运动学与往返验证） ：ik_foot_under_hip 的 r_min 不是写死的 0.1120，而是用代码 1 读到的 kn_hi（140.0022°）现算，换 URDF 变体（如 full_range 版）时限位变了结果自动跟着变，这是"常量不硬编码"的收益。c2 与 ratio 都经过 max(-1, min(1, ·)) 钳制，防止浮点误差把 acos 输入推出 −1,1 抛 ValueError------这是验证脚本最初踩过的坑。往返验证把 IK 输出代回 FK，误差 1e-17m；三个 None 用例分别对应"低于 11.20cm、高于 27.87cm、字面站立值超腿全长 2 微米"，建议直接留在代码里当回归测试。

五、三档姿态实战：站立 / 半蹲 / 深蹲

把正解、逆解、限位校验拼成流水线，就能批量产出三档代表性姿态。三档定义全部可复现：站立垂落 (0°, 0°)；半蹲固定膝 70°，髋角由"足下"条件反推；深蹲膝打满 140°，同样反推髋角。半蹲与深蹲的髋角不是估的，而是解 tan(θ1) = L2·sinθ2 / (L1 + L2·cosθ2) 得到的足下精确解，43.94° 与 101.68° 都经 FK 复核 x = 0。

python 复制代码 
def hip_for_foot_under(t2_deg):
    """足下(x=0)时由膝角反解髋前屈角:
    tan(t1) = L2*sin(t2) / (L1 + L2*cos(t2))"""
    t2 = math.radians(t2_deg)
    return math.degrees(math.atan2(L2*math.sin(t2), L1 + L2*math.cos(t2)))

def build_pose_table():
    """三档姿态批量计算：正解几何量 + 限位状态，全部脚本实算。"""
    tiers = [("站立垂落", 0.0, 0.0), ("半蹲", None, 70.0), ("深蹲", None, 140.0)]
    rows = []
    for name, t1, t2 in tiers:
        if t1 is None:
            t1 = hip_for_foot_under(t2)   # 由膝角反推髋角，保证 x=0
        x, h = forward_kinematics(t1, t2)
        r = math.hypot(x, h)
        chk = check_limits(t1, t2)        # 见代码 5
        rows.append((name, t1, t2, x*100, h*100, r*100,
                     chk["hip"]["status"], chk["knee"]["status"]))
    return rows

for name, t1, t2, x, h, r, hs, ks in build_pose_table():
    print(f"{name:>6}: 前屈={t1:6.2f}deg 膝={t2:6.2f}deg URDF=({-t1:+.2f},{t2:.2f}) "
          f"| h={h:6.2f}cm r={r:6.2f}cm x={x:+5.2f}cm | 髋{hs} 膝{ks}")

代码 4 说明（三档姿态批量计算） ：hip_for_foot_under 是足下条件的隐式解显式化：令 FK 的 x=0，可推出 tan(θ1) 的解析式，比每次跑数值搜索快得多。build_pose_table 里半蹲、深蹲只给膝角（t1=None），髋角由该函数反推，从机制上保证三档全部满足 x=0；check_limits 引用代码 5 的函数，返回两关节状态。输出同时打模型角与 URDF 角两套值，再次强调下发前的取负映射。这张表可直接当离线调参的速查基准。

姿态 髋前屈（模型角） 膝屈曲 URDF hip_pitch URDF knee 足端高度 h 髋踝距 r 前向偏移 x
站立垂落 0.00° 0.00° 0.00° 0.00° 27.87 cm 27.87 cm 0.00 cm
半蹲 43.94° 70.00° −43.94° 70.00° 23.11 cm 23.11 cm 0.00 cm
深蹲 101.68° 140.00° −101.68° 140.00° 11.20 cm 11.20 cm 0.00 cm

读表抓三点。其一，足端高度随膝角非线性下降：站立到半蹲只降 4.76cm，半蹲到深蹲骤降 11.91cm，全因为连杆是三角函数耦合，轨迹规划必须走曲线插值而非直线。其二，深蹲档髋-踝距 11.20cm 正是两杆在 140° 屈曲时的最短直线距离，几何上已到底。其三，三档全部满足 x = 0，足端始终在髋正下方，这是"足下分支"的定义使然，也保证重心投影最稳。

还有一个反直觉的细节：站立档膝角 0° 恰恰贴在限位下限上（余量 0°，状态 WARN）。这不是故障，而是说明"站立本身就是膝的零位工作点"------真机上任何让膝过伸的负角指令都会被底层拦截，0° 是允许的极限。半蹲档两关节余量都超过 70°，是最舒服的工作区；深蹲档髋余量仍有 73.32°，膝余量归零，瓶颈归属一目了然。

再补一个规划视角：三档之间如果按直线插值下发关节角，中段会出现足端先下沉再回升的抖动，因为 h 对膝角是凸函数；正确做法是按第八节曲线采样后做样条，或者干脆每 5° 解一次 IK 查表，用查表换实时性。

图2：站立、半蹲、深蹲三档姿态的关节角与足端高度对比

六、限位合规校验：算得出不等于做得到

逆解只保证几何自洽，URDF 的硬限位才是真机的边界，越界指令会被底层直接拦截（本系列 05 篇的预检器正是干这个）。因此任何离线姿态都要再过一道限位校验，才算"可下发"。

校验设计成三态：OK（余量大于阈值）、WARN（距边界 ≤0.2°，真机极易触发告警）、NG（已越界，拒绝）。0.2° 不是拍脑袋------简报记录 left_knee_joint = 2.44 rad 时余量仅 0.2° 即触发 WARN，与预检器实测一致。阈值应当随场景收紧：搬运重载时留 2° 都不过分，这也是把 tol 暴露成参数的原因。

python 复制代码 
# 模型角口径限位：前屈 [-90, 175]（= URDF [-175, 90] 取负），膝屈 [0, 140]
LIMITS = {"hip": (-90.0, 175.0), "knee": (0.0, 140.0)}

def check_limits(t1, t2, tol=0.2):
    """三态限位校验：OK(余量>tol) / WARN(贴边<=tol) / NG(越界)。"""
    out = {}
    for jname, (lo, hi) in LIMITS.items():
        v = t1 if jname == "hip" else t2
        margin = min(v - lo, hi - v)
        status = "NG" if margin < 0 else ("WARN" if margin <= tol else "OK")
        out[jname] = {"value": v, "range": (lo, hi),
                      "margin": margin, "status": status}
    return out

if __name__ == "__main__":
    for name, t1, t2 in [("站立", 0, 0), ("半蹲", 43.94, 70),
                         ("深蹲", 101.68, 140)]:
        c = check_limits(t1, t2)
        print(f"{name}: 髋前屈 {c['hip']['value']:6.2f} 余量 {c['hip']['margin']:6.2f} "
              f"{c['hip']['status']} | 膝 {c['knee']['value']:6.2f} 余量 "
              f"{c['knee']['margin']:5.2f} {c['knee']['status']}")

代码 5 说明（限位三态校验） ：LIMITS 用模型角口径书写，前屈区间 −90°, 175° 就是 URDF −175°, 90° 取负镜像，落地时也可以用 hip_pitch = −t1 换成 URDF 口径，两者等价。margin 取"到最近边界的距离"，负数即越界；tol=0.2 与 05 篇预检器口径对齐。函数返回带数值、区间、余量、状态的字典，方便上层做"任一 NG 即整体拒绝"的聚合。注意站立档膝余量 0.00° 会判 WARN------这是零位工作点的固有属性，不是 bug，别为了消 WARN 把站立膝角改成小正角。
#mermaid-svg-Wrj9MAIntdSpBsJl{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-Wrj9MAIntdSpBsJl .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-Wrj9MAIntdSpBsJl .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-Wrj9MAIntdSpBsJl .error-icon{fill:#552222;}#mermaid-svg-Wrj9MAIntdSpBsJl .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-Wrj9MAIntdSpBsJl .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-Wrj9MAIntdSpBsJl .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-Wrj9MAIntdSpBsJl .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-Wrj9MAIntdSpBsJl .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-Wrj9MAIntdSpBsJl .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-Wrj9MAIntdSpBsJl .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-Wrj9MAIntdSpBsJl .marker{fill:#333333;stroke:#333333;}#mermaid-svg-Wrj9MAIntdSpBsJl .marker.cross{stroke:#333333;}#mermaid-svg-Wrj9MAIntdSpBsJl svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-Wrj9MAIntdSpBsJl p{margin:0;}#mermaid-svg-Wrj9MAIntdSpBsJl defs #statediagram-barbEnd{fill:#333333;stroke:#333333;}#mermaid-svg-Wrj9MAIntdSpBsJl g.stateGroup text{fill:#9370DB;stroke:none;font-size:10px;}#mermaid-svg-Wrj9MAIntdSpBsJl g.stateGroup text{fill:#333;stroke:none;font-size:10px;}#mermaid-svg-Wrj9MAIntdSpBsJl g.stateGroup .state-title{font-weight:bolder;fill:#131300;}#mermaid-svg-Wrj9MAIntdSpBsJl g.stateGroup rect{fill:#ECECFF;stroke:#9370DB;}#mermaid-svg-Wrj9MAIntdSpBsJl g.stateGroup line{stroke:#333333;stroke-width:1;}#mermaid-svg-Wrj9MAIntdSpBsJl .transition{stroke:#333333;stroke-width:1;fill:none;}#mermaid-svg-Wrj9MAIntdSpBsJl .stateGroup .composit{fill:white;border-bottom:1px;}#mermaid-svg-Wrj9MAIntdSpBsJl .stateGroup .alt-composit{fill:#e0e0e0;border-bottom:1px;}#mermaid-svg-Wrj9MAIntdSpBsJl .state-note{stroke:#aaaa33;fill:#fff5ad;}#mermaid-svg-Wrj9MAIntdSpBsJl .state-note text{fill:black;stroke:none;font-size:10px;}#mermaid-svg-Wrj9MAIntdSpBsJl .stateLabel .box{stroke:none;stroke-width:0;fill:#ECECFF;opacity:0.5;}#mermaid-svg-Wrj9MAIntdSpBsJl .edgeLabel .label rect{fill:#ECECFF;opacity:0.5;}#mermaid-svg-Wrj9MAIntdSpBsJl .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-Wrj9MAIntdSpBsJl .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-Wrj9MAIntdSpBsJl .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-Wrj9MAIntdSpBsJl .edgeLabel .label text{fill:#333;}#mermaid-svg-Wrj9MAIntdSpBsJl .label div .edgeLabel{color:#333;}#mermaid-svg-Wrj9MAIntdSpBsJl .stateLabel text{fill:#131300;font-size:10px;font-weight:bold;}#mermaid-svg-Wrj9MAIntdSpBsJl .node circle.state-start{fill:#333333;stroke:#333333;}#mermaid-svg-Wrj9MAIntdSpBsJl .node .fork-join{fill:#333333;stroke:#333333;}#mermaid-svg-Wrj9MAIntdSpBsJl .node circle.state-end{fill:#9370DB;stroke:white;stroke-width:1.5;}#mermaid-svg-Wrj9MAIntdSpBsJl .end-state-inner{fill:white;stroke-width:1.5;}#mermaid-svg-Wrj9MAIntdSpBsJl .node rect{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-Wrj9MAIntdSpBsJl .node polygon{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-Wrj9MAIntdSpBsJl #statediagram-barbEnd{fill:#333333;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-cluster rect{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-Wrj9MAIntdSpBsJl .cluster-label,#mermaid-svg-Wrj9MAIntdSpBsJl .nodeLabel{color:#131300;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-cluster rect.outer{rx:5px;ry:5px;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-state .divider{stroke:#9370DB;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-state .title-state{rx:5px;ry:5px;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-cluster.statediagram-cluster .inner{fill:white;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-cluster.statediagram-cluster-alt .inner{fill:#f0f0f0;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-cluster .inner{rx:0;ry:0;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-state rect.basic{rx:5px;ry:5px;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-state rect.divider{stroke-dasharray:10,10;fill:#f0f0f0;}#mermaid-svg-Wrj9MAIntdSpBsJl .note-edge{stroke-dasharray:5;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-note rect{fill:#fff5ad;stroke:#aaaa33;stroke-width:1px;rx:0;ry:0;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-note rect{fill:#fff5ad;stroke:#aaaa33;stroke-width:1px;rx:0;ry:0;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-note text{fill:black;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram-note .nodeLabel{color:black;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagram .edgeLabel{color:red;}#mermaid-svg-Wrj9MAIntdSpBsJl #dependencyStart,#mermaid-svg-Wrj9MAIntdSpBsJl #dependencyEnd{fill:#333333;stroke:#333333;stroke-width:1;}#mermaid-svg-Wrj9MAIntdSpBsJl .statediagramTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-Wrj9MAIntdSpBsJl :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 余量收缩到0.2度以内
再越界一步
回退一到两度
修正后回到区间内
留足余量再下发
校验通过
贴边告警
越界拒绝
可下发
拒绝下发

姿态 髋前屈值 髋余量 髋状态 膝值 膝余量 膝状态 整体判定
站立垂落 0.00° 90.00° OK 0.00° 0.00° WARN WARN
半蹲 43.94° 131.06° OK 70.00° 70.00° OK OK
深蹲 101.68° 73.32° OK 140.00° 0.00° WARN WARN

表 3 把三档姿态的余量并排摊开，再和第二节的符号结论叠起来，瓶颈判断就不再靠直觉：髋的总摆幅 265°，前屈预算 175° 对深蹲所需的 101.68° 来说绰绰有余；膝从 0° 到 140° 一路合法，但 140° 就是终点，过伸非法。封顶的是膝，不是髋。

工程落地时，建议把三态判定包成一层聚合函数：任一关节 NG 即整体拒绝，任一 WARN 即降速并记录，全 OK 才放行。这个逻辑和 05 篇预检器完全同构，区别只在输入来源------预检器接的是上位机指令，本文接的是 IK 输出。两层护栏叠起来，才同时覆盖"算错"和"传错"两种失败。

WARN 态要多说一句：它不是"不能动"，而是"贴着边界"。深蹲贴在膝 140° 上，真机持续压边界会加速关节磨损甚至触发保护停机。工程上建议主动留 1°~2°：膝退到 138°，脚本实算前屈 99.34°、足端高度 11.57cm，只比 140° 损失 0.37cm 深度，却换来完整的 OK 状态。用几何量化安全余量，正是离线分析最实用的产出。

图3：hip_pitch 与 knee 限位区间及越界拦截示意

七、可达性分析：为什么 140° 是膝上限

前面都在问"给定高度能不能解"，这一节反过来问："所有角度组合起来，足端到底能到哪些点？"把关节网格化，θ1 从 −90° 到 175°、θ2 从 0° 到 140°，每 5° 采样，1566 个构型的足端在 (x, h) 平面画出可达工作空间。目标点普查则换成极坐标：半径 11.5~27.5cm、方向角每 5°，共 648 个点。

python 复制代码 
def reachable(x_t, h_t):
    """目标点 (x_t, h_t) 是否在限位内可达（任意方向，不限足下）。"""
    r = math.hypot(x_t, h_t)
    r_min = math.sqrt(L1*L1 + L2*L2 + 2*L1*L2*math.cos(math.radians(kn_hi)))
    if not (r_min <= r <= L1 + L2):
        return False                      # 直线距离就够不着
    c2 = max(-1.0, min(1.0, (r*r - L1*L1 - L2*L2) / (2*L1*L2)))
    t2 = math.degrees(math.acos(c2))     # 膝不能过伸，支唯一
    phi = math.degrees(math.atan2(x_t, h_t))     # 目标方向角（0=正下）
    alpha = math.degrees(math.acos(max(-1.0, min(1.0,
                 (L1*L1 + r*r - L2*L2) / (2*L1*r))))
    t1 = phi + alpha                     # 候选髋前屈角
    # 髋角可做 360 度等价环绕，任一落在 [-90, 175] 即可达
    return any(-90.0 <= t1 + k <= 175.0 for k in (-360.0, 0.0, 360.0))

tot = ok = 0
r = 0.115
while r <= 0.275 + 1e-9:
    phi = -180.0
    while phi < 180.0:
        tot += 1
        ok += reachable(r*math.sin(math.radians(phi)),
                        r*math.cos(math.radians(phi)))
        phi += 5.0
    r += 0.02
print(f"目标网格 {tot} 点，可达 {ok}（{ok/tot*100:.1f}%）")
print("足端正上方(x=0,h=-0.27)可达?", reachable(0.0, -0.27))

代码 6 说明（可达工作空间采样） ：reachable 先按 r_min, L1+L2 卡直线距离，再用余弦定理反解膝角与大腿方向角 alpha，候选髋角 θ1 = φ + α。关键细节是 any(... for k in (−360, 0, 360))：髋前屈角超出 −90°, 175° 时，加或减一整圈是同一构型，必须环绕判断，否则会把"髋前屈 200° 等效为后伸 160°"这类点误判为不可达------这是初版脚本少算一成可达率的 bug。输出 73.6% 可达率与足端正上方不可达两个结论，后者源于前屈 180° 超 175° 限位 5°。

三个数字概括这片空间。半径方向：髋-踝直线距 r ∈ 11.20, 27.87cm，内边界由膝 140° 决定，外边界是腿全长。竖直方向：h ∈ −27.76, +27.87cm------足端最低只到髋下方 27.87cm（站立），最高能翻到髋上方 27.76cm（极限前屈 175° 且腿伸直），但到不了理论值 27.87cm，因为 175° 与 180° 差的 5° 在顶端啃出约 0.11cm 的缺口。水平方向：x ∈ −27.87, +27.87cm。

为什么 140° 是膝上限、并且封顶 11.20cm？把余弦定理展开看：髋-踝距随膝屈曲单调缩短，140° 时压到 11.20cm；若没有这条限位，理论全屈（膝 180°）可以到 |L1−L2| = 6.26cm。也就是说，140° 的上限"吃掉"了 4.94cm 的折叠深度。深蹲想低于 11.20cm，在纯矢状面两杆模型内无解------膝已打满且不许过伸，髋还有 73.32° 余量却使不上劲。唯一出路是动用本模型剥离掉的自由度：踝背屈、躯干前倾、屈髋行程，这些恰恰要留到全身 IK 里才能安排。

方向上的不可达区同样有章可循。648 个目标点里 73.6% 可达；不可达点几乎全部集中在正上方附近，张成一个约 95° 的楔形。这个角度不是偶然：髋的总摆幅是 265°（前屈 175° + 后伸 90°），360° 减它正好 95°。把不可达点按方向角每 10° 分档统计，分布非常干净：紧贴正上方（±170° 附近）最密，向两侧递减，到 ±80° 以外完全消失------它是一个以正上方为顶点、向下张开的楔形，而不是零散盲点。足端要放到髋正上方需要前屈 180°，超限 5°，够不着；足端后摆到水平线以下则轻松可达。别小看这块楔形------它意味着任何"把脚抬到髋正上方"的动作，在 Q1 上不存在纯矢状面解。

图4：给定足端高度反解关节角的离线速查卡片

八、曲线、导出与文本速写

数字之外，曲线更能说明问题。足下分支的"足端高度 vs 膝角"可以直接用 Mermaid 的 xychart-beta 画出来：从 27.87cm 一路降到 11.20cm，全程合法，终点由膝 140° 截断------注意截断位置与髋无关，这正是修正符号约定后结论发生的变化。

读这条曲线有个技巧：膝角从 0° 到 40° 曲线很平（只降 1.59cm），从 100° 到 140° 很陡（降 7.35cm）。下蹲前半程几乎不损失高度，后半程高度暴跌------做蹲起控制时，后半段的关节速度要预留更多裕量，否则膝会"砸"向限位。
#mermaid-svg-eLIdlEH7rc1NaHnz{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-eLIdlEH7rc1NaHnz .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-eLIdlEH7rc1NaHnz .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-eLIdlEH7rc1NaHnz .error-icon{fill:#552222;}#mermaid-svg-eLIdlEH7rc1NaHnz .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-eLIdlEH7rc1NaHnz .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-eLIdlEH7rc1NaHnz .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-eLIdlEH7rc1NaHnz .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-eLIdlEH7rc1NaHnz .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-eLIdlEH7rc1NaHnz .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-eLIdlEH7rc1NaHnz .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-eLIdlEH7rc1NaHnz .marker{fill:#333333;stroke:#333333;}#mermaid-svg-eLIdlEH7rc1NaHnz .marker.cross{stroke:#333333;}#mermaid-svg-eLIdlEH7rc1NaHnz svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-eLIdlEH7rc1NaHnz p{margin:0;}#mermaid-svg-eLIdlEH7rc1NaHnz :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} 足下分支：足端高度 vs 膝屈曲角（脚本实算） 0 20 40 60 80 100 120 140 30 28 26 24 22 20 18 16 14 12 10 8 6 4 2 0 足端高度 (cm)

代码 7 用 matplotlib 把这条曲线落成 PNG。特意设 matplotlib.use("Agg")：离线环境通常没有显示器，非交互后端才能保证脚本在任何服务器上跑通并出图。

python 复制代码 
import matplotlib
matplotlib.use("Agg")                    # 无显示环境也能落盘
import matplotlib.pyplot as plt

def plot_height_vs_knee(out_png="height_vs_knee.png"):
    knees, heights = [], []
    for kk in range(0, 141):              # 膝 0~140 度全程合法
        r2 = math.sqrt(L1*L1 + L2*L2
                       + 2*L1*L2*math.cos(math.radians(kk)))
        t1 = hip_for_foot_under(kk)
        knees.append(kk)
        heights.append(r2 * 100)          # 足下分支 x=0，故 h 即 r
    fig, ax = plt.subplots(figsize=(8, 4.5))
    ax.plot(knees, heights, color="#1f77b4", label="足端高度 h（足下分支）")
    ax.set_xlabel("膝屈曲角 knee (°)"); ax.set_ylabel("足端高度 h (cm)")
    ax.set_xlim(0, 140); ax.set_ylim(0, 30)
    ax.axvline(140, color="#d62728", ls="--", label="膝限 140 度截断")
    ax.legend(); ax.grid(True, alpha=0.3)
    fig.tight_layout(); fig.savefig(out_png, dpi=120)
    return out_png

print(plot_height_vs_knee())

代码 7 说明（曲线绘制） ：循环里对每个膝角用余弦定理还原足下直线距 r2（足下分支 x=0，故 h 就等于 r2），同时算出所需髋前屈角备用。红色 axvline(140) 标出膝限截断位置------截断发生在膝而不在髋，这正是符号约定修正后结论发生的变化，看图时别被旧结论带偏。dpi=120 出图可直接插进技术文档或 PPT。若想看未经限位过滤的理论曲线，把上限改成 180 即可，但记得在图例注明那是非法分支。

脚本的价值不止于终端打印。代码 8 给两个零依赖小工具：export_csv 把三档姿态连同限位状态写成 UTF-8 CSV，表格里每个数都能回溯到几何；ascii_pose_sketch 用纯字符画侧视轮廓，适合塞进日志或 CI 输出，没有 GUI 也能一眼看出深蹲时大腿前倾、小腿回摆的趋势。

python 复制代码 
def export_csv(path="pose_table.csv"):
    import csv
    with open(path, "w", newline="", encoding="utf-8") as f:
        w = csv.writer(f)
        w.writerow(["姿态", "髋前屈(°)", "膝屈曲(°)", "URDF hip_pitch",
                    "URDF knee", "足端高度(cm)", "髋踝距(cm)",
                    "前向偏移(cm)", "髋状态", "膝状态"])
        for name, t1, t2, x, h, r_, hs, ks in build_pose_table():
            w.writerow([name, f"{t1:.2f}", f"{t2:.2f}", f"{-t1:+.2f}",
                        f"{t2:.2f}", f"{h:.2f}", f"{r_:.2f}",
                        f"{x:+.2f}", hs, ks])
    return path

def ascii_pose_sketch():
    """纯文本侧视轮廓：H=髋 O=膝 A=踝，无依赖快速预览三档姿态。"""
    return "\n".join([
        "侧视轮廓（H=髋 O=膝 A=踝，| 为髋正下方参考线）",
        "站立   H|        半蹲    H\\       深蹲    H\\",
        "       |          \\          \\",
        "       O          O          O",
        "       |          |          |\\",
        "       A          A          A",
    ])

print(export_csv()); print(ascii_pose_sketch())

代码 8 说明（导出与文本速写） ：export_csv 复用 build_pose_table 的结构化结果，用标准库 csv 写入，全程 encoding="utf-8" 防中文乱码；列里同时保留模型角与 URDF 角两套值，下游脚本不用再推映射。ascii_pose_sketch 不依赖任何第三方库，用字符折线画出髋、膝、踝三点，深蹲档小腿明显回摆的趋势一眼可辨，适合放进单元测试的失败信息或服务器日志。两者都不需要真机、不需要联网，是"离线优先"工作流的标配小工具。

九、边界与误差：这份离线画像缺了什么

必须诚实说明这份离线画像缺了什么，否则就是误导。第一，模型只覆盖矢状面的髋俯仰与膝屈曲，没有踝俯仰/滚转、髋偏航/翻滚、躯干前倾与质心平衡；下蹲能不能站得稳，取决于全身质量分布（总质量 16.9173kg，pelvis 为质量中枢）与平衡控制器，几何管不着。第二，模型角与 URDF 角之间隔着一个零位映射（hip_pitch = −θ1），落地真机前要用工况标定把公式坐实，本文给的映射本身就是标定的起点。

第三，刚体假设：没有连杆柔性、关节间隙与足端滑动，1e-17m 的往返误差只证明公式自洽，不证明真机精度。第四，BIPED_STAND_DEFAULT 位控站立本身无平衡调节，离线算出的角度若直接下发仍需外部固定防摔。第五，浮点边界要留 epsilon，前面 0.2787 被拒就是活例。第六，髋部机构偏移（髋翻滚关节横向 3.68cm 等）并入髋部未计入腿长，量级约 1~2cm，做精细足端规划时要补回。

交叉验证也能做。研究目录里的 com_zero_pose.py 按零位姿实测：双踝间距 181.1mm、总质量 16.9173kg、质心落在骨盆上方 33.67mm 处。把这些实测值与 URDF 解析值对照，可以确认解析出的连杆长度与质量分布没有在传输或单位换算中走样------离线分析的可信度，正是靠这样一层层交叉验出来的。

想进一步逼近真机，可以把本文输出喂给 q1_v3_mc.xml 做动力学验证：那份 MJCF 带 22 个 motor 执行器、timestep=0.001 与 armature=0.004，最适合确认"这个关节角序列会不会让电机饱和、会不会失稳"。离线几何负责"能不能到"，仿真负责"到了稳不稳"，分工清晰，谁也替代不了谁。

十、常见误区：别把"数学解"当"可执行解"

误区一："膝能到 140° 就能蹲到最低。"前半句对，后半句要修正：140° 对应足端 11.20cm，这就是矢状面两杆模型的最低点，再低无解。把"膝打满"当成"蹲到底"，会漏掉踝、躯干这些真正的后续自由度。

误区二："深蹲瓶颈在髋 90°。"这是符号搞反的典型后果。把 URDF 的 hip_pitch 直接当"前屈为正"填 +90°、knee 填 +140°，按正确映射反推，实际构型是足端高于髋 10.97cm 的折叠姿态，不是深蹲；填 0° + 140° 则是足端后移 10.97cm、还高出 2.27cm 的怪姿势。两组数都是脚本实算，现场演示符号差一个负号会错得多离谱。

误区三："r 就是 h。"足下分支里恰好相等，别推广：极限前屈 175° 时 r = 27.87cm 而 h = −27.76cm。谈高度必须分清直线距离与竖直分量，混用会在轨迹规划里埋雷。

误区四："URDF 里存的是角度。"恰恰是弧度：−3.0543 rad 才是 −175°，2.4435 rad 才是 140°。脚本里 math.degrees 转换就是为了不被单位坑，这也是 05 篇预检器反复强调的陷阱。

误区五："左右对称，算一遍就够。"结构镜像，但 roll、yaw 的限位左右互为相反数；pitch 与 knee 虽左右一致，也应分别调用校验，别把一侧的 OK 复制到另一侧。

误区六："高度随膝角线性下降。"前 40° 膝角只降 1.59cm（27.87 到 26.28），后 40°（100° 到 140°）降 7.35cm。规划蹲起轨迹必须用曲线，直线插值会在中段过冲、末端欠伸。

误区七："WARN 就是不能动。"恰恰相反，WARN 只表示贴着边界。站立档本身就常年贴着膝 0° 下限工作，把它判成故障会让人无所适从；正确用法是把 WARN 当预警------规划时主动留 1°~2°，让关键姿态落回 OK 区，而不是见 WARN 就停。

十一、无真机也能算：离线可达性分析的价值

这套脚本合起来就是一份离线可达性预检：给定任意目标高度，先判直线可达，再解角，再校验限位，最后输出"能不能做、给什么角"。不需要 Q1、不需要 ROS 2、不需要网络，有 Python 和 URDF 就能跑。

三个场景最受益。方案预研：先确认目标高度落在可达区再立项，避免拿到真机才发现蹲不到。指令护栏：把 check_limits 编译进上位机，任何要下发的关节角先过一遍，等价于把 05 篇的预检器前置到运动规划阶段。教学与文档：三档表、限位余量表与高度曲线直接进内部 wiki，让团队对"蹲多深、用什么角"形成统一认知------尤其是"膝 140° 封顶 11.20cm"这条，能省掉反复试错的真机时间。

最后提醒一笔成本账：本文全部脚本加起来不到两百行，跑一次不到一秒；相比之下，为同样的问题开一次真机、搭一次 ROS 2 环境，成本高出两个数量级。把便宜的验证做在前面，正是这类离线方法最朴素的性价比。

总结

本文用一段不到两百行的纯 Python，把启元 PrimeBot Q1 的腿部站立与下蹲姿态彻底算清。地基是 q1_v3.urdf 的两段连杆：大腿 L1 = 0.108046m、小腿 L2 = 0.170652m，站立垂落髋-踝 27.87cm，膝 140° 屈曲压缩到 11.20cm。动手算角之前，先从关节轴 (0,1,0) 与机体 +X 朝前推出符号约定：hip_pitch 为正腿向后摆，下蹲前屈须取负角，前屈预算 175°、后伸 90°------这一个负号决定了瓶颈判断的方向。闭式逆解给出三档姿态：站立 0°/0° 高 27.87cm，半蹲 43.94°/70° 高 23.11cm，深蹲 101.68°/140° 高 11.20cm；限位校验显示髋全程余量充足（深蹲仍余 73.32°），膝从站立贴 0° 下限走到深蹲贴 140° 上限。结论因此明确：封顶下蹲深度的是膝 140° 而非髋 90°，想低于 11.20cm 必须动用踝、躯干等矢状面之外的自由度；工程上膝退到 138° 只损失 0.37cm 深度即可换得完整合规。可达性普查进一步给出方向边界：73.6% 目标可达，正上方一块约 95° 楔形不可达，足端最高只能翻到髋上方 27.76cm。整套方法只依赖几何与限位，无需真机即可在方案阶段规避越界指令；它的边界同样清晰------未覆盖平衡控制与其余自由度，落地前还需标定零位映射并在 MJCF 中验证动力学。把"算得出"和"做得到"分开审视，正是无真机复现这条路线最务实的纪律。

参考资料

相关推荐
熊猫钓鱼>_>1 小时前
精准碰一碰近场分享与拖拽投送实战：把文件“碰“进另一台设备【鸿蒙心迹】
开发语言·华为·harmonyos·鸿蒙·openharmony
第404号工位1 小时前
插上充电宝真能偷光相册？我拆解了一台改装充电宝，实测“恶意充电”的真实攻击链
linux·python·架构
Axis tech1 小时前
Xsens&Manus全身数据采集解决方案，用精确数据训练您的具身智能
人工智能·科技·机器人
技术卷1 小时前
Windows 下自定义 MCP 踩坑：stdin 默认继承导致 CPython 启动阻塞
python·mcp·zcode
小oo呆1 小时前
【学习心得】Python多进程
python·操作系统
CodeGL1 小时前
9、Python基础之IO编程
开发语言·数据库·python
HySpark2 小时前
ERes2Net声纹识别短音频匹配不稳定：从音频时长到有效音素的排查实践
python·语音识别·声纹识别·vad·eres2net
TomEval2 小时前
【测AI】第17篇：大模型评测方法论 —— LLM 怎么测？
人工智能·python·aigc·测试用例
fanruitian2 小时前
python 进程 线程
java·前端·python
热门推荐
01国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）02如何新建文件夹？ 电脑新建文件夹的4种方法03ai安全工具:CyberStrikeAI安装部署与使用04GitHub 镜像站点05历年考研数学一、数学二、数学三真题试卷及答案PDF06几个好用的ip纯净度检测网站07昇腾910B系列芯片完全指南：四款型号对比、服务器选型与大模型部署实战08Windows 10/11 如何退出微软账户并改用本地账户登录（含找不到选项的解决方法）09苹果iPA游戏软件资源下载网站10乙二醇水溶液物性参数