3、Delta机器人运动学模型：正解、逆解与几何约束
各位同学，今天我们来啃Delta机器人最核心的一块骨头——运动学模型。说实话，我刚入行那会儿，觉得运动学就是套公式，没什么了不起。直到有一次在产线上调试，机器人死活抓不到指定位置的工件，我才意识到：正解算不对，逆解算不准，这机器人就是个“瞎子”。
Delta机器人的运动学，说白了就是解决两个问题：“腿动多少，手到哪”（正解）和“手要到哪，腿该动多少”（逆解）。中间还得靠几何约束方程把主动臂、从动臂和动平台串起来。今天我们就一步步拆开来看。
核心要点：Delta机器人的三条链是并联的，每条链由主动臂（近架）、从动臂（平行四边形连杆）和动平台组成。正解是已知三个关节角θ₁、θ₂、θ₃，求末端中心点P的坐标(x, y, z)；逆解则反过来。
3.1 几何约束方程的建立
先搭框架。我习惯把Delta机器人简化成三个“R-U-S”链（旋转副-万向副-球副），但实际建模时，从动臂的平行四边形结构可以等效成一个虚拟连杆。为什么？因为平行四边形保证了动平台始终与静平台平行，不会倾斜。嗯，这里要注意：这个假设成立的前提是四个铰点构成严格的平行四边形，加工精度不够的话，后面逆解会出偏差。
我们定义几个关键参数：
- R：静平台半径（固定基座外接圆半径）
- r：动平台半径（末端执行器外接圆半径）
- L₁：主动臂长度
- L₂：从动臂等效长度
- θᵢ：第i条链的主动臂关节角（i=1,2,3）
对于第i条链，主动臂末端（即从动臂起点）的位置可以写成：
Aᵢ = [R·cos(φᵢ) + L₁·cos(θᵢ)·cos(φᵢ),
R·sin(φᵢ) + L₁·cos(θᵢ)·sin(φᵢ),
-L₁·sin(θᵢ)]其中φᵢ是第i条链在静平台上的方位角，通常取0°、120°、240°。
从动臂的另一端连接动平台，动平台中心点P到Aᵢ的距离必须等于L₂。这就得到了几何约束方程：
||P + r·[cos(φᵢ), sin(φᵢ), 0] - Aᵢ||² = L₂²三条链，三个方程，联立起来就是整个Delta机器人的运动学约束。我曾经在项目里直接用这个方程去解正解，结果发现数值稳定性很差——因为方程是非线性的，迭代求解时容易发散。后来我换了个思路，用解析法。
我的经验：几何约束方程虽然直观，但直接用于数值求解效率低。建议先化简成关于P坐标的二次方程组，再用消元法。具体做法是把三个方程两两相减，消去平方项，得到两个线性方程，再回代求解。
3.2 运动学正解：从关节角到末端位置
正解问题，说白了就是：你告诉我三个电机转了多少度，我告诉你抓手在哪儿。这在实时控制中非常关键——比如你给机器人发了个“走到点A”的指令，控制器得先知道当前末端在哪，才能规划路径。
正解的推导思路是这样的：
- 根据三个关节角θ₁、θ₂、θ₃，分别计算出三条链上A₁、A₂、A₃的坐标。
- 利用几何约束方程，得到三个球面方程。每个球面的球心是Aᵢ偏移后的点，半径是L₂。
- 三个球面的交点就是动平台中心点P。
具体计算时，我推荐用Be careful方法（其实是我自己起的名字，意思是“小心处理符号”）：
# 伪代码示例：Delta机器人正解
def forward_kinematics(theta1, theta2, theta3):
# 1. 计算三个球心位置
A1 = compute_A(theta1, phi1)
A2 = compute_A(theta2, phi2)
A3 = compute_A(theta3, phi3)
# 2. 球心偏移（考虑动平台半径r）
C1 = A1 - r * [cos(phi1), sin(phi1), 0]
C2 = A2 - r * [cos(phi2), sin(phi2), 0]
C3 = A3 - r * [cos(phi3), sin(phi3), 0]
# 3. 两两相减得到线性方程
# (C2 - C1)·P = (||C2||² - ||C1||²)/2
# (C3 - C1)·P = (||C3||² - ||C1||²)/2
# 4. 解线性方程组得到P的x,y
# 5. 代入任意球面方程求z（注意取负值，因为Delta工作空间在下方）
return P这里有个坑：球面相交可能有两个解（上下对称），但Delta机器人的工作空间只在静平台下方，所以z坐标取负值。我曾经见过有新手忘了这茬，结果正解算出来的点在天上飘。
避坑指南：正解计算中，如果三个球面不相交（比如关节角组合超出了工作空间），程序会报错。一定要在代码里加异常处理，或者先判断一下三个球心之间的距离是否都小于2L₂。
3.3 运动学逆解：从末端位置到关节角
逆解更常用。你告诉机器人“去抓那个点”，控制器得算出三个电机各转多少度。逆解的核心思路是：已知P点坐标，反推每条链的主动臂角度θᵢ。
对于第i条链，我们可以把几何约束方程改写成关于θᵢ的方程。展开后得到：
Eᵢ·cos(θᵢ) + Fᵢ·sin(θᵢ) = Gᵢ其中Eᵢ、Fᵢ、Gᵢ是由P坐标和机器人几何参数决定的已知量。这是一个标准的三角函数方程，解为：
θᵢ = atan2(Fᵢ, Eᵢ) ± acos(Gᵢ / sqrt(Eᵢ² + Fᵢ²))注意，这里有个±号，意味着每条链有两个可能的解（手臂朝上或朝下）。实际中我们只取“朝下”的那个解（即主动臂与静平台夹角为负的那个分支）。
我个人的习惯是，在代码里先算出两个候选解，然后根据工作空间约束筛选：
def inverse_kinematics(P):
theta = [0, 0, 0]
for i in range(3):
E, F, G = compute_coeffs(P, i)
delta = E**2 + F**2 - G**2
if delta < 0:
raise ValueError("目标点不可达")
sqrt_delta = sqrt(delta)
theta1 = atan2(F, E) + acos(G / sqrt(E**2 + F**2))
theta2 = atan2(F, E) - acos(G / sqrt(E**2 + F**2))
# 选择在[-π/2, 0]范围内的解（主动臂向下）
theta[i] = select_valid(theta1, theta2)
return theta小技巧：逆解时，如果delta非常接近0（比如小于1e-6），说明目标点刚好在边界上，此时两个解重合。这种情况在轨迹规划中要特别处理，因为边界点附近速度会突变。
3.4 知识体系总览
为了让大家更直观地理解这三部分的关系，我画了一张图：
从这张图可以看得很清楚：几何约束方程是地基，正解和逆解是上面的两栋楼。正解用于验证和标定（比如你装好机器人后，用手推动末端，读电机角度，反算位置对不对），逆解用于实时控制（轨迹规划时每个插补点都要算一遍）。
3.5 一些实用建议
最后，分享几个我在实际项目中踩过的坑：
- 参数标定很重要：理论模型算得再漂亮，如果R、r、L₁、L₂的实际值和图纸对不上，正解逆解全是错的。我建议装完机器人后，先用激光跟踪仪标定一下关键尺寸。
- 奇异位形要避开：当从动臂与主动臂共线时，逆解会出现无穷多解或解突变。轨迹规划时尽量绕开这些区域。
- 数值精度别忽视：正解中三球面相交，如果三个球心几乎共线，求解会非常不稳定。可以用SVD分解来提高鲁棒性。
好了，运动学模型这部分就讲到这里。记住：正解和逆解就像硬币的两面，缺一不可。下一章我们会用Python把这些公式变成可运行的代码，到时候大家可以亲手试试。