文章目录
- @[toc]
- 一、什么是逆运动学（Inverse Kinematics, IK）？
- 1️⃣ 先看“正运动学”（FK）好理解一点
- 2️⃣ 逆运动学（IK）正好反过来
- 二、先从“最简单的一条腿”开始（二维）
- 模型（非常重要）
- 三、逆运动学的几何本质（不是魔法）
- 1️⃣ 核心思想：**三角形**
- 2️⃣ 膝关节角（最简单）
- 3️⃣ 髋关节角（分两步）
- 步骤 A：脚的方向角
- 步骤 B：三角形内部补角
- 髋关节角：
- 四、用 Python 写一个“最小可运行 IK”
- 1️⃣ 纯数学 IK（二维单腿）
- 2️⃣ 试一下（和你站立高度接近）
- 五、你代码里的 IK 是“进阶版”
- 六、一句话总结（非常重要）
- 这一步在做什么：从 (x, y) 到 (r, θ)
- Step 1：先有一个点 P(x, y)
- Step 2：把“到原点的距离”画出来，这就是 r
- Step 3：用投影形成直角三角形，解释 r 为什么是 √(x²+y²)
- Step 4：θ 是“从 x 轴转到 OP 的角度”，用 atan2 最稳
- 总结：cartesian_to_polar() 的核心就是这两行
文章目录
- @[toc]
- 一、什么是逆运动学（Inverse Kinematics, IK）？
- 1️⃣ 先看“正运动学”（FK）好理解一点
- 2️⃣ 逆运动学（IK）正好反过来
- 二、先从“最简单的一条腿”开始（二维）
- 模型（非常重要）
- 三、逆运动学的几何本质（不是魔法）
- 1️⃣ 核心思想：**三角形**
- 2️⃣ 膝关节角（最简单）
- 3️⃣ 髋关节角（分两步）
- 步骤 A：脚的方向角
- 步骤 B：三角形内部补角
- 髋关节角：
- 四、用 Python 写一个“最小可运行 IK”
- 1️⃣ 纯数学 IK（二维单腿）
- 2️⃣ 试一下（和你站立高度接近）
- 五、你代码里的 IK 是“进阶版”
- 六、一句话总结（非常重要）
- 这一步在做什么：从 (x, y) 到 (r, θ)
- Step 1：先有一个点 P(x, y)
- Step 2：把“到原点的距离”画出来，这就是 r
- Step 3：用投影形成直角三角形，解释 r 为什么是 √(x²+y²)
- Step 4：θ 是“从 x 轴转到 OP 的角度”，用 atan2 最稳
- 总结：cartesian_to_polar() 的核心就是这两行
一、什么是逆运动学（Inverse Kinematics, IK）？
1️⃣ 先看“正运动学”（FK）好理解一点
正运动学（FK）：
👉已知关节角度 → 算末端在哪里
比如：
- 髋关节 30°
- 膝关节 60°
问：脚现在在空间的什么位置 (x, z)？
这就是 FK。
2️⃣ 逆运动学（IK）正好反过来
逆运动学（IK）：
👉已知脚的位置 → 算关节角度
比如你在程序里写：
set_site(leg,x=60,y=0,z=-50)你真正想问的是：
“如果脚要到这个点，
髋关节要转多少度？
膝关节要转多少度？”
这一步，就是逆运动学。
二、先从“最简单的一条腿”开始（二维）
我们先不管四足、不管 yaw，只看一条腿在一个平面里动。
模型（非常重要）
髋关节 o \ 大腿 L1 \ o 膝关节 \ \ 小腿 L2 \ x 足端- 大腿长度：
L1 - 小腿长度：
L2 - 脚目标位置：
(x, z)（注意 z 往下是负）
三、逆运动学的几何本质（不是魔法）
1️⃣ 核心思想：三角形
当脚的位置(x, z)已知时：
从髋关节到脚 → 构成一个三角形边长分别是：
- 一条边：
r = sqrt(x² + z²) - 另外两条边：
L1、L2
于是你得到一个已知三边的三角形
👉可以用余弦定理！
2️⃣ 膝关节角（最简单）
用余弦定理：
[
\cos(\theta_{knee}) = \frac{L1^2 + L2^2 - r^2}{2 L1 L2}
]
注意：
- 这是“膝关节内角”
- 舵机角度往往要再转换（你代码里已经做了）
3️⃣ 髋关节角（分两步）
步骤 A：脚的方向角
phi=atan2(z,x)步骤 B：三角形内部补角
[
\psi = \arccos\left(\frac{L1^2 + r^2 - L2^2}{2 L1 r}\right)
]
髋关节角：
hip = phi + psi⚠️ 有些机器人用
phi - psi，取决于你角度定义
你现在用的代码就是其中一种约定
四、用 Python 写一个“最小可运行 IK”
1️⃣ 纯数学 IK（二维单腿）
importmathdefleg_ik_2d(x,z,L1,L2):""" 二维平面两连杆逆运动学 输入： x, z : 足端坐标 L1 : 大腿长度 L2 : 小腿长度 输出： hip_angle, knee_angle (单位：度) """# 到脚的距离r=math.sqrt(x*x+z*z)# ---- 膝关节角（余弦定理）----cos_knee=(L1*L1+L2*L2-r*r)/(2*L1*L2)cos_knee=max(-1.0,min(1.0,cos_knee))# 防止数值炸掉knee=math.acos(cos_knee)# ---- 髋关节角 ----phi=math.atan2(z,x)cos_psi=(L1*L1+r*r-L2*L2)/(2*L1*r)cos_psi=max(-1.0,min(1.0,cos_psi))psi=math.acos(cos_psi)hip=phi+psi# 转成角度hip_deg=math.degrees(hip)knee_deg=math.degrees(knee)returnhip_deg,knee_deg2️⃣ 试一下（和你站立高度接近）
L1=55.0L2=77.5x=62z=-50hip,knee=leg_ik_2d(x,z,L1,L2)print("hip =",hip,"knee =",knee)这一步得到的角度，就是 IK 的核心结果。
五、你代码里的 IK 是“进阶版”
你现在用的这个函数：
cartesian_to_polar(x,y,z)本质上做了三件事：
把 (x, y) 合成水平距离 w
减去髋关节横向偏移 length_c
在 3D 空间里算：
- α：髋俯仰
- β：膝关节
- γ：髋 yaw（左右摆）
也就是说：
你已经在用完整的三维逆运动学了
只是还没“从几何角度彻底看懂它”
六、一句话总结（非常重要）
🔹逆运动学不是“控制舵机”
🔹它只是回答一个问题：👉 “脚要去这里，关节应该转多少？”
你现在的代码结构已经是工业级四足机器人的典型写法了：
步态规划（set_site） ↓ 插值（servo_service） ↓ 逆运动学（IK） ↓ 舵机映射cartesian_to_polar(x, y)（把直角坐标转成极坐标）用图形一步步拆开讲清楚。为了直观，用示例点P = (3, 2)来演示（换成任意 x,y 逻辑都一样）。
这一步在做什么：从 (x, y) 到 (r, θ)
Step 1：先有一个点 P(x, y)
- 直角坐标就是“从原点往右 x、往上 y”的位置。
（图里就是把点 P 标出来）
Step 2：把“到原点的距离”画出来，这就是 r
- 极坐标的r是从原点 O 到点 P 的距离。
- 用勾股定理：
[
r = \sqrt{x^2 + y^2}
]
示例：
[
r = \sqrt{3^2 + 2^2} = \sqrt{13} \approx 3.606
]
（图里就是从 O 连到 P 的那条线段）
Step 3：用投影形成直角三角形，解释 r 为什么是 √(x²+y²)
把 P 垂直投影到 x 轴，形成一个直角三角形：
- 底边长度 = x
- 高 = y
- 斜边 = r
所以 r 就是三角形斜边。
（图里有 x、y、r 三条边）
Step 4：θ 是“从 x 轴转到 OP 的角度”，用 atan2 最稳
- 极坐标的θ是从正 x 轴逆时针转到OP的角。
- 用：
[
\theta = \text{atan2}(y, x)
]
为什么不用atan(y/x)？ atan2能正确区分象限（比如 x<0 的时候角度不一样），也能处理 x=0 的情况。
示例：
[
\theta = \text{atan2}(2,3) \approx 0.588\text{ rad} \approx 33.69^\circ
]
（图里有一个小弧线表示 θ）
总结：cartesian_to_polar() 的核心就是这两行
[
r=\sqrt{x2+y2},\quad \theta=\text{atan2}(y,x)
]
)
