简介：面向并联机器人学习者与自动化控制开发者的Delta机器人运动学求解C#工程，聚焦正向与逆向运动学的代码实现，适合已具备一定C#基础、希望把运动学理论落到实际控制软件中的读者。压缩包共41个文件、约900KB，以cs源码为核心，配合sln与csproj工程文件、config配置、resx资源及png界面截图，另有exe、dll等编译产物，可直接用Visual Studio打开运行调试。资源已有3928人学习下载，说明其在同类主题中具备一定参考价值。代码围绕关节角度与工作空间XYZ坐标的双向换算展开，正向求解涉及三角函数与坐标变换，逆向求解需处理多解与迭代寻优，读者可据此理解DeltaKinematics等关键类的组织方式、数据结构设计与注释规范，并在此基础上改造或移植到自己的并联机器人控制项目中。
1. Delta 并联机器人运动学：为什么逆解是闭式公式，正解却要解一元三次方程
Delta 机器人看起来像个倒挂的三脚架，三条支链同时驱动动平台，速度快、惯量小，是高速分拣场景的常客。但真正让不少开发者翻车的，不是机械装配，而是运动学求解：逆解（给定动平台位姿求三个主动臂转角）能写成几行闭式公式，正解（给定三个转角求动平台位置）却要解一个一元三次方程，还得在多个根里挑出物理上成立的那个。这份 C# 源码工程把两套求解都实现了，正解走的是解析法而非数值迭代，逆解走的是标准几何投影。搞懂「为什么逆解简单、正解难」这条分界线，比抄一份能跑的代码更有价值——它决定了你在标定、奇异位形判断和实时控制周期里会不会踩坑。
2. 逆运动学：三条支链的几何投影为什么能写成闭式解
2.1 从动平台位姿到主动臂转角的几何链条
Delta 机器人的结构可以拆成三层：固定平台上的三个驱动关节、三根主动臂、三组平行四连杆（从动臂）、动平台。逆解的目标是：已知动平台中心点 P 的坐标 (x, y, z)，求三个主动臂相对固定平台的转角 θ₁、θ₂、θ₃。
关键在于平行四连杆的约束。每组从动臂由两根平行杆组成，它把主动臂末端和动平台上的铰点连起来，并且强制这两点之间的连线方向始终平行于一个固定方向。常见的实现思路是：把每条支链的从动臂等效成一根「虚拟杆」，长度固定为 L，一端在主动臂末端，另一端在动平台上对应的铰点。这样每条支链就退化成一个「固定平台铰点 → 主动臂 → 虚拟杆 → 动平台铰点」的平面几何问题。
设固定平台半径为 R（三个驱动关节分布圆的半径），动平台半径为 r（三个铰点分布圆的半径），主动臂长度为 L₁，从动臂等效长度为 L₂。对第 i 条支链，动平台铰点位置由 P 加上一个随动平台姿态旋转的偏移量得到。当动平台只做平移（姿态固定）时，这个偏移量是常量，逆解就变成：已知主动臂末端必须落在以动平台铰点为球心、L₂ 为半径的球面上，同时主动臂末端又在以固定平台铰点为圆心、L₁ 为半径的圆上，求这个圆与球面的交点对应的转角。
2.2 闭式公式的推导与代码落点
把上面的几何关系写成代数式，对每条支链可以得到一个关于 θᵢ 的三角方程，整理后是 A·sinθ + B·cosθ + C = 0 的形式，用万能公式或直接解二次方程就能得到两个候选角，再根据机械限位选一个。这就是逆解能写成闭式的原因：每条支链独立、约束是球面与圆的交，没有跨支链耦合。
源码里逆解的核心逻辑大致是这样组织的（C# 风格，变量名按工程习惯）：
// 逆解：输入动平台中心坐标 (x, y, z)，输出三个主动臂转角 // R: 固定平台半径, r: 动平台半径, L1: 主动臂长, L2: 从动臂等效长 public double[] InverseKinematics(double x, double y, double z) { double[] theta = new double[3]; for (int i = 0; i < 3; i++) { // 第 i 条支链的固定平台铰点与动平台铰点方位角 double phi = i * 2.0 * Math.PI / 3.0; // 动平台铰点相对中心点的偏移（姿态固定时是常量） double px = x + r * Math.Cos(phi); double py = y + r * Math.Sin(phi); double pz = z; // 固定平台铰点坐标 double fx = R * Math.Cos(phi); double fy = R * Math.Sin(phi); double fz = 0.0; // 主动臂末端到动平台铰点的距离平方必须等于 L2^2 // 主动臂末端 = (fx + L1*cos(theta), fy + L1*sin(theta), 0) 在支链平面内 // 展开后得到 A*sin(theta) + B*cos(theta) + C = 0 double dx = px - fx; double dy = py - fy; double dz = pz - fz; double A = 2.0 * L1 * dz; double B = 2.0 * L1 * (dx * Math.Cos(phi) + dy * Math.Sin(phi)); double C = dx * dx + dy * dy + dz * dz + L1 * L1 - L2 * L2; // 解三角方程，取机械限位内的那个根 theta[i] = SolveTrigEquation(A, B, C); } return theta; }这段代码里最容易被忽略的是SolveTrigEquation的判别式。当 A² + B² < C² 时方程无实数解，物理含义是目标点超出了工作空间，此时不能返回 NaN 让上层去猜，而应该抛出明确的工作空间越界异常。另一个细节是phi的取值：三条支链的方位角相差 120°，但固定平台铰点和动平台铰点的方位角偏移方向必须一致，如果一处用 +phi、一处用 -phi，逆解会在某些象限给出镜像错误的转角，表现为机器人「往反方向够」。这类符号错误在仿真里不一定暴露，上真机才会翻车。
2.3 姿态耦合：为什么逆解公式里藏着一个旋转矩阵
上面的代码假设动平台只平移不旋转。真实 Delta 机器人在很多应用里动平台姿态是固定的（靠平行四连杆被动保持），所以这个假设成立。但如果你的机构允许动平台绕某轴微转，动平台铰点的偏移量就不能用常量r * cos(phi)，而要先用旋转矩阵把偏移向量转到世界坐标系。源码工程里如果保留了姿态参数，逆解函数签名会多一个旋转矩阵或欧拉角输入，内部对每个铰点偏移做一次矩阵乘法。这一步的代价很小，但漏掉它会让逆解在姿态非零时系统性偏差，且偏差随姿态角增大而增大，标定时表现为「中心点准、边缘点飘」。
3. 正运动学：三个转角如何变成一个一元三次方程
3.1 为什么正解不能像逆解那样逐支链独立求
逆解能逐支链独立，是因为动平台位置已知，每条支链的约束只涉及自己的转角。正解反过来：三个转角已知，动平台位置 (x, y, z) 是未知量，而三条支链的约束方程同时包含 x、y、z。三个方程、三个未知数，看起来直接解方程组就行，但每个方程都是二次的（球面约束），联立后消元会得到一个高次方程。
常见的实现思路是：利用 Delta 机构的对称性，把三个球面约束方程两两相减，消去 x²、y²、z² 项，得到两个关于 x、y、z 的线性方程。这样 x 和 y 可以用 z 的线性表达式表示，代回任意一个球面方程，就得到一个只含 z 的一元二次方程——等等，这里有个容易记错的点：不同文献和不同机构参数下，消元后可能是二次也可能是三次，取决于是否把动平台姿态也作为未知量。对于姿态固定的标准 Delta，消元后通常是关于 z 的一元二次方程，取物理上合理的根即可；如果工程里把某些几何约束放松或引入额外自由度，次数会升高。源码工程里如果实现了通用求解器，通常会按最高三次来准备求根逻辑，再用几何约束筛根。
3.2 消元过程与求根代码的结构
正解的实现比逆解长得多，核心分三步：建方程、消元降次、筛根。下面这段示意代码保留了工程里常见的组织方式：
// 正解：输入三个主动臂转角，输出动平台中心坐标 // 返回多个候选解，由调用方按工作空间和连续性筛选 public List<Vector3> ForwardKinematics(double theta1, double theta2, double theta3) { // 第一步：由每个转角算出主动臂末端坐标（在各自支链平面内） Vector3[] elbow = new Vector3[3]; for (int i = 0; i < 3; i++) { double phi = i * 2.0 * Math.PI / 3.0; double t = (i == 0) ? theta1 : (i == 1 ? theta2 : theta3); // 主动臂末端 = 固定铰点 + L1 沿支链平面内旋转 t elbow[i] = new Vector3( R * Math.Cos(phi) + L1 * Math.Cos(t) * Math.Cos(phi), R * Math.Sin(phi) + L1 * Math.Cos(t) * Math.Sin(phi), -L1 * Math.Sin(t)); // 注意 z 方向符号与装配方向一致 } // 第二步：每个动平台铰点满足 |P + offset_i - elbow_i|^2 = L2^2 // 三式两两相减消去二次项，得到两个线性方程 // 第三步：把 x、y 表示为 z 的函数，代回任一球面方程 // 得到 a*z^2 + b*z + c = 0（或三次），解出 z 的候选值 var candidates = SolvePolynomial(BuildCoefficients(elbow)); // 第四步：筛根——z 必须为负（动平台在固定平台下方）， // 且代回三个球面方程残差在容差内 return FilterByWorkspace(candidates); }elbow[i]的 z 分量符号是血泪经验点：主动臂向下倾斜时，末端 z 为负，如果装配方向相反（主动臂朝上），符号要整体翻转，否则正解算出的动平台会跑到固定平台上方，筛根时全被过滤掉，表现为「正解永远返回空列表」。BuildCoefficients里两两相减的顺序也有讲究，用方程 1 减方程 2、方程 1 减方程 3，得到的线性方程组系数矩阵在奇异位形附近条件数会变差，数值实现里最好用双精度并检查行列式量级。
3.3 多解筛选：为什么正解必须返回候选列表而不是单个点
一个给定的转角组合，在数学上可能对应多个动平台位置——这是并联机构正解的固有特性，不是代码 bug。物理上只有一个解落在实际工作空间内，但求解器在消元过程中丢失了几何信息，所以必须把候选解都算出来再筛。筛根的依据通常有三条：z 坐标在机械行程范围内、代回三个球面约束的残差小于容差、与上一控制周期的解在连续性上最接近。第三条在实时控制里尤其重要，因为当两个候选解都满足前两条时（工作空间边缘可能出现），靠连续性选根能避免动平台「跳变」。源码工程里如果只返回单个解，大概率是在筛根时硬编码了「取 z 较小者」之类的规则，这种写法在标准工况下能跑，但换一套机构参数就可能选错根。
4. 避坑与排查：Delta 运动学实现里最容易翻车的几处
4.1 现象：逆解算出的转角代入正解回不来原坐标
原因通常是逆解和正解用了不一致的几何参数约定，比如逆解里动平台铰点偏移用r * cos(phi)，正解里用r * cos(phi + 30°)，或者主动臂零位定义不同（一个以水平为 0°，一个以竖直为 0°）。解决方法是写一个往返测试：随机取工作空间内若干点，逆解再正解，检查残差。残差超过 1e-6 就说明两套公式的参数约定没对齐，逐项核对 R、r、L1、L2 和零位角定义。
4.2 现象：正解在某些转角组合下返回空或抛异常
先看判别式。一元二次方程判别式为负意味着该转角组合在数学上无解，物理上对应三个球面不相交——要么转角超出了机构可达范围，要么几何参数填错了。排查时先把三个转角固定在零位附近（机构名义初始位形），正解应该能稳定返回一个接近设计中心点的坐标；如果零位都无解，基本可以确定是 L1、L2 或 R、r 的量级填反了，比如把毫米当米。
4.3 现象：仿真里轨迹平滑，上真机后靠近工作空间边缘出现抖动
这是奇异位形附近的典型表现。Delta 机器人在工作空间边缘，正解的雅可比矩阵条件数急剧变大，微小的转角误差被放大成较大的位置误差，逆解算出的转角对位置变化极其敏感。代码层面能做的是在逆解里加工作空间边界检查，提前拒绝接近奇异的指令；控制层面则要限制加速度。单纯调 PID 解决不了这个问题，因为根源在运动学映射本身。
4.4 现象：正解筛根后动平台位置在相邻周期跳变
多解筛选只用了「z 在范围内」这一条，没有用连续性。当两个候选解都落在行程内时，硬编码规则可能在不同周期选中不同根。解决方法是把上一周期的解作为参考，选与它欧氏距离最小的候选；同时给候选解打上「是否与上一周期同根」的标记，连续多周期切换根时触发告警，提示机构可能接近了工作空间分界。
4.5 现象：姿态微转时逆解误差随角度线性增长
逆解里漏乘了动平台姿态旋转矩阵，或者旋转矩阵的旋转顺序与机构实际自由度不匹配。排查方法是令姿态角为零，确认逆解与纯平移公式一致；再令姿态角为小角度，检查误差是否与角度成正比。如果是，基本可以锁定是偏移向量没有做旋转变换，补上矩阵乘法即可。
5. 设计取舍：解析解、数值解与工程实现的边界
Delta 运动学的两套解法，本质上是在「闭式表达的确定性」和「消元过程的复杂性」之间做取舍。逆解选闭式，是因为每条支链独立、约束简单，闭式解在实时控制里每周期只做几次三角函数和开方，确定性好、无迭代发散风险。正解选解析消元而不是牛顿迭代，是因为迭代法对初值敏感，在工作空间边缘或多解附近可能不收敛或跳到另一个根，而解析法把所有候选解一次算出来，筛根逻辑显式可控——代价是推导和代码都更长，且对几何参数的约定更敏感。
我一般会这么理解这套源码工程的价值：它把「逆解闭式、正解消元筛根」这条完整链路用 C# 落到了可编译的工程文件里，而不是停留在公式推导。对做并联机器人控制的开发者来说，能直接对照代码检查自己的参数约定、零位定义和筛根策略，比重新推一遍公式省事得多。从那以后我每次接入新的 Delta 机构，都强制先跑一遍「逆解→正解」往返残差测试和零位正解测试，再谈轨迹规划——这两步不过，后面的标定和调参全是空中楼阁。希望帮到你。
本文还有配套的精品资源，点击获取
