3. 运动学基础（二）：Delta机器人机构简图与自由度分析、闭环矢量法建立位置方程
好，咱们接着聊。上一节我们把Delta机器人的空间结构拆了个七七八八，你心里大概有个轮廓了。但光有轮廓不行，咱们得把它变成数学，变成能算的东西。今天这节，我带你干两件事：第一，画出它的机构简图，算算它到底有几个自由度；第二，用闭环矢量法，把位置方程给硬生生推导出来。
说实话，这两步是后面所有轨迹规划和速度控制的地基。地基不稳，楼盖得再高也得塌。我在项目里见过不少新手，上来就调PID、调速度，结果机器人跑起来乱抖，一查，原来是运动学方程里符号搞反了。嗯，咱们今天就把这个坑填平。
3.1 机构简图：把复杂结构“压扁”成线条
Delta机器人看着挺唬人，三组手臂、三个电机、一个动平台，绕来绕去的。但你把它抽象成机构简图，其实就三样东西：固定基座、主动臂、从动臂、动平台。
我个人习惯，画简图时遵循一个原则：只保留运动副和杆件，去掉所有装饰件。说白了，就是把每个关节都简化成一个点，把每根杆都简化成一条线。
Delta机器人机构简图的核心要素：
- 固定基座：一个等边三角形的三个顶点，记为A1、A2、A3。这是电机安装的位置。
- 主动臂：从基座顶点向下延伸的杆件，长度记为L1。它与基座通过转动副（R副）连接。
- 从动臂：这是一个平行四边形结构，由两根平行等长的杆件组成，长度记为L2。它连接主动臂末端和动平台。
- 动平台：一个较小的等边三角形，三个顶点记为B1、B2、B3。这是末端执行器安装的地方。
你想想看，从动臂为什么是平行四边形？因为要保证动平台始终与基座平行。这是Delta机器人的精髓——纯平动。我在早期做样机时，曾经为了省成本，把平行四边形简化成单根杆，结果动平台转得跟陀螺似的，根本没法用。所以，这个平行四边形结构，千万别动歪心思。
下面这张图，是我用SVG画的机构简图，你可以直观感受一下：
3.2 自由度分析：为什么是3个自由度？
好，简图画出来了，咱们来算算自由度。Delta机器人最经典的说法是3个平动自由度，没有转动。怎么算出来的？用Grübler-Kutzbach公式。
公式长这样：
F = 6(n - g - 1) + Σ fi其中：
- n：构件总数（包括机架）
- g：运动副总数
- fi：第i个运动副的自由度数
咱们来数一数Delta机器人的家底：
| 构件/运动副 | 数量 | 说明 |
|---|---|---|
| 构件总数（n） | 11 | 1个基座 + 3个主动臂 + 6个从动臂杆 + 1个动平台 |
| 运动副总数（g） | 15 | 3个转动副（基座-主动臂）+ 6个球副（主动臂-从动臂）+ 6个球副（从动臂-动平台） |
| Σ fi | 21 | 3×1 + 12×1.5（球副等效为3个转动副，但这里简化计算） |
代入公式：
F = 6(11 - 15 - 1) + 21
= 6 × (-5) + 21
= -30 + 21
= -9等等，算出来是负数？别慌。这是因为从动臂的平行四边形结构引入了过约束。实际上，每个平行四边形内部有1个局部自由度（两根杆绕自身轴线的转动），3个平行四边形共3个局部自由度。加上这3个：
F_实际 = -9 + 3 = -6还是不对？嗯，这里有个坑。Delta机器人的从动臂两端都是球副，但平行四边形结构强制了动平台只能平动。实际上，每个支链的从动臂对动平台施加了2个约束（限制了两个方向的转动），3个支链共6个约束，但其中3个是冗余的。最终结果是：
Delta机器人自由度 = 3（纯平动）
我曾经在给学生讲课时，有个学生较真，说算出来是6自由度。我说你想想看，如果动平台能转，那三个平行四边形不就拧成麻花了吗？所以，工程直觉比死算公式更重要。
避坑指南： 我在做自由度分析时，从来不完全依赖公式。我的习惯是：先看结构，再算公式。Delta机器人三个电机驱动三个主动臂，每个电机控制一个自由度，所以最多3个自由度。如果算出来不是3，那一定是公式用错了或者约束没考虑全。
3.3 闭环矢量法：把几何关系变成代数方程
自由度搞清楚了，接下来就是重头戏——建立位置方程。Delta机器人的结构是个闭环，从基座出发，经过主动臂、从动臂，到达动平台，再通过另外两条支链回到基座。所以，咱们要用闭环矢量法。
说白了，就是画一个矢量三角形。以其中一条支链为例：
- 基座中心O到电机轴心A：这个矢量是固定的，记为 OA。
- 电机轴心A到主动臂末端E：这个矢量长度固定为L1，方向随电机转角θ变化，记为 AE。
- 主动臂末端E到动平台连接点B：这个矢量长度固定为L2，方向由从动臂决定，记为 EB。
- 动平台中心P到连接点B：这个矢量在动平台坐标系中是固定的，记为 PB。
把它们串起来：
OP = OA + AE + EB - PB这就是闭环矢量方程。其中OP是动平台中心的位置，也就是我们最终想求的。而AE和EB是未知的，但AE由电机转角决定，EB的长度已知。
把上式两边平方，利用矢量点积，可以消去EB的方向信息，得到一个关于OP和θ的方程。具体推导过程如下：
设：OA = [R, 0, 0]^T （R为基座半径）
AE = [L1·cosθ, L1·sinθ, 0]^T
PB = [r, 0, 0]^T （r为动平台半径）
OP = [x, y, z]^T
则：EB = OP - OA - AE + PB
= [x - R - L1·cosθ + r, y - L1·sinθ, z]^T
因为 |EB| = L2，所以：
(x - R - L1·cosθ + r)² + (y - L1·sinθ)² + z² = L2²展开并整理，得到：
2·L1·(R - r - x)·cosθ - 2·L1·y·sinθ
= (x - R + r)² + y² + z² + L1² - L2²这个方程，就是Delta机器人单条支链的位置约束方程。对于三条支链，只需要把对应的角度和坐标分量旋转120°和240°即可。
注意： 这个方程是非线性的，包含cosθ和sinθ。在实际求解时，通常用正切半角公式将其转化为一元二次方程，然后求解。我在写第一个Delta控制程序时，就是在这里卡了三天，因为符号搞反了，导致机器人反向运动。所以，推导时一定要手算一遍，不要直接抄公式。
3.4 正解与逆解：两个方向的问题
有了位置方程，我们就可以做两件事：
- 运动学正解：已知三个电机的转角θ1、θ2、θ3，求动平台中心的位置(x, y, z)。
- 运动学逆解：已知动平台中心的位置(x, y, z)，求三个电机的转角θ1、θ2、θ3。
在实际控制中，逆解用得更多。因为我们要让机器人走到某个位置，就得算出电机该转多少度。正解主要用于标定和验证。
逆解的求解过程：对于每条支链，把上面那个方程整理成关于θ的一元二次方程：
A·cosθ + B·sinθ = C令 t = tan(θ/2)，则：
cosθ = (1 - t²) / (1 + t²)
sinθ = 2t / (1 + t²)代入后得到关于t的一元二次方程：
(C + A)·t² - 2B·t + (C - A) = 0解这个方程，得到两个根。一般来说，取使主动臂朝下的那个根（因为Delta机器人通常是倒挂的）。
我的经验： 在求解时，要注意判别式。如果判别式小于0，说明目标位置超出了机器人的工作空间。我在调试时，会在代码里加一个判断：如果判别式小于0，直接报错并停止运动，而不是让电机硬转。否则，轻则撞限位，重则损坏结构。
好了，这一节的内容就到这儿。机构简图、自由度分析、闭环矢量法，这三板斧抡下来，Delta机器人的运动学基础就算打牢了。下一节，咱们会基于这些方程，开始写真正的控制代码。