简介：Delta机器人作为典型的并联机器人，广泛用于高速拾放场景。这份C#源代码工程针对正向运动学与逆向运动学求解展开，面向机器人开发者、自动化工程师及运动控制方向学习者，帮助将理论公式落地为可运行的算法程序。

压缩包共41个文件，约900KB，以6个cs源码文件为核心，配合sln、csproj工程配置、Form界面资源及png图表说明，构成完整VS工程结构；exe、DLL等编译产物便于直接查看运行效果。

目前已有3916人学习下载。通过阅读DeltaKinematics等关键类，读者可掌握三臂坐标变换、雅可比矩阵迭代求解的实现思路，也能借鉴界面交互与数据结构设计，为后续并联机器人控制开发提供可复用的算法模板。

1. delta机器人运动学算法正向逆向求解：C#源码与工程文件怎么落到自己项目里

做delta机器人控制的人，多半都经历过这样一幕：网上找到一个“delta机器人运动学算法正向逆向求解C#源代码vs工程文件.rar”，下载、解压、用Visual Studio打开，然后卡在编译或调试环节。运动学本身并不难，难在坐标系定义、角度约定和工程配置。这篇笔记从一个可用的几何模型讲起，把逆解和正解分别落到C#实现，再讲清楚源代码和工程文件两种形态怎么选、常见坑在哪。适合正在写C#上位机、准备把delta接到实时控制里的开发者，也适合刚入门并联机器人、不想在公式推导里耗太久的同学。

2. 从机构模型到坐标系：先想清楚delta的几何约束再去写公式

2.1 delta机器人的核心几何：上三角形、下三角形与三条平行四边形闭环

delta机器人是典型的并联机构。基座上固定着三个主动臂铰点，它们分布在一个大三角形上；动平台上有三个从动臂铰点，分布在一个小三角形上。三条支链各包含一根主动臂和一根从动臂，主动臂由伺服电机驱动，从动臂通常是两端带球铰的平行四边形结构。平行四边形结构在运动学里有一个关键作用：它保证动平台相对基座不发生旋转，末端姿态恒为水平。于是我们计算末端位置时，只需要关心空间中的平移量(x, y, z)，不需要管姿态角。

这一点让delta的运动学比六轴串联机器人简单不少，也是网上代码虽然风格各异、公式写法不同，却都能对得上的底层原因。平行四边形结构在理想模型里可以等效为一根长度为Lb的连杆，两端都是球铰，这就把每条支链简化为：基座铰点→主动臂→从动臂→动平台铰点。整条链只有主动臂转角θi是变量，其他几何量都是常量。

整个机构输出的自由度是三个平移量，输入是三个主动臂转角，所以逆解是从(x, y, z)求(θ1, θ2, θ3)，正解反过来。这个“三进三出”的对应关系是后续所有公式的骨架。

2.2 关键参数与坐标系约定

在写任何代码之前，先把参数表和坐标系钉死。我一般统一用毫米做长度单位，弧度做角度单位，坐标系原点和方向按下表约定。

坐标系的约定：基座中心为原点，Z轴向上，动平台末端点位于Z轴的负半轴，也就是基座下方。三条支链的方位角αi均匀分布，我习惯取0°、120°、240°，方位角从X轴正方向起算。主动臂角度θi定义为主动臂相对水平面的仰角：θi=0时主动臂水平外伸，θi>0时向上抬，θi<0时向下摆。这个定义直接决定后面公式的符号，换一套定义公式就要整体换符号。

把参数换成自己机器人之前，按这个顺序核对：先用卷尺量出Rb和Rp，注意量的是铰点中心所在圆的半径，不是外壳半径；再量La和Lb，也是铰点中心距；然后把机器人手动摆到主动臂水平的位置，确认此时θi=0，以及Z轴正方向是不是朝着基座上方。这四步做完，公式才能直接套用。

2.3 为什么并联机构不适合用D-H参数表硬套

很多从串联机器人转过来的人第一反应是把D-H参数表建出来。这个思路在六轴、SCARA这类串联链上没问题，但delta是闭环机构，三条支链共享同一个末端点，硬套D-H会写出大量冗余变量和额外约束，求解反而更乱。常见做法是直接从几何约束出发，把每条支链写成独立的长度约束方程，再用代数方法消元。

正解和逆解的差别也在这里：逆解每条支链可以独立求解，因为末端点坐标已知，每条支链只有一个未知角度；正解则是三条支链的约束方程耦合在一起，需要联立求解。理解了这个底层差异，后面写代码时就会清楚为什么逆解简单、正解要专门处理双解。

3. 逆解：从末端坐标反推三个主动臂角度，C#代码与参数说明

3.1 逆解推导：把每条支链写成三角方程

逆解的几何路径很直接。对第i条支链，基座铰点P_i是固定的，动平台铰点Q_i可以由末端点E和方位角直接算出来：

P_i = (Rb·cosαi, Rb·sinαi, 0) Q_i = (x + Rp·cosαi, y + Rp·sinαi, z)

主动臂末端U_i则由θi决定，注意这里θi=0时主动臂水平外伸：

U_i = P_i + (La·cosθi·cosαi, La·cosθi·sinαi, La·sinθi)

从动臂长度约束是|U_i - Q_i| = Lb。把式子展开，令D_i = P_i - Q_i，得到关于θi的三角方程：

A·cosθi + B·sinθi = C

其中：

A = D_i在主动臂摆动平面水平方向的分量，即(Dx·cosαi + Dy·sinαi) B = D_i的竖直分量Dz C = (Lb² - La² - |D_i|²) / (2·La)

这个方程的标准解法是令R = sqrt(A² + B²)，则原方程化为R·sin(θ + φ) = C，其中φ = atan2(A, B)。于是两组解分别是：

θ = asin(C/R) - φ θ = π - asin(C/R) - φ

两组解对应主动臂“上肘”和“下肘”两种位形。实际机构里机械限位、基座干涉会滤掉其中一组，但数学上两组都存在，代码里必须显式处理选解。

3.2 逆解C#实现

下面的类把逆解封装成两个重载方法，一个不带上一时刻角度，一个带。带上一时刻角度是为连续轨迹规划准备的，可以避免角度在两个解之间来回跳。

public class DeltaKinematics { public double Rb = 200.0; // 基座铰点分布圆半径，mm public double Rp = 60.0; // 动平台铰点分布圆半径，mm public double La = 250.0; // 主动臂长度，mm public double Lb = 650.0; // 从动臂等效长度，mm private static readonly double[] Alpha = { 0.0, 2.0 * Math.PI / 3.0, 4.0 * Math.PI / 3.0 }; // 不带上一时刻角度的逆解 public bool Inverse(double x, double y, double z, out double[] theta) { return Inverse(x, y, z, null, out theta); } // 带上一时刻角度的逆解，previous为null时自动选小角度解 public bool Inverse(double x, double y, double z, double[] previous, out double[] theta) { theta = new double[3]; double[] sol1 = new double[3]; double[] sol2 = new double[3]; for (int i = 0; i < 3; i++) { double ca = Math.Cos(Alpha[i]); double sa = Math.Sin(Alpha[i]); // 基座铰点 P_i double px = Rb * ca, py = Rb * sa; // 动平台铰点 Q_i，末端点( x, y, z )默认 z 为负 double qx = x + Rp * ca, qy = y + Rp * sa; // D = P - Q double dx = px - qx; double dy = py - qy; double dz = -z; // 基座平面在 z=0，末端通常在下方 // 三角方程 A*cos(theta) + B*sin(theta) = C 的系数 double a = dx * ca + dy * sa; // 摆动平面内水平分量 double b = dz; // 竖直分量 double d2 = dx * dx + dy * dy + dz * dz; double c = (Lb * Lb - La * La - d2) / (2.0 * La); double r = Math.Sqrt(a * a + b * b); // 临界点附近先做一次 clamp，避免浮点误差让 Math.Asin 产生 NaN double ratio = c / r; if (ratio < -1.0 || ratio > 1.0) { if (Math.Abs(ratio) - 1.0 > 1e-9) return false; // 目标点超出该支链可达范围 ratio = Math.Clamp(ratio, -1.0, 1.0); } double phi = Math.Atan2(a, b); double asinVal = Math.Asin(ratio); sol1[i] = WrapAngle(asinVal - phi); sol2[i] = WrapAngle(Math.PI - asinVal - phi); } if (previous == null) { // 没有上一时刻角度时，默认取绝对值更小的那组解。 // 多数 delta 机械限位在 -30°~65° 之间，小角度解对应主动臂接近水平。 double d1 = Math.Abs(sol1[0]) + Math.Abs(sol1[1]) + Math.Abs(sol1[2]); double d2 = Math.Abs(sol2[0]) + Math.Abs(sol2[1]) + Math.Abs(sol2[2]); theta = d1 <= d2 ? sol1 : sol2; } else { // 有上一时刻角度时，选“离上一时刻更近”的同族解，避免角度跳变 double d1 = 0.0, d2 = 0.0; for (int i = 0; i < 3; i++) { d1 += Math.Abs(ShortestAngleDiff(sol1[i], previous[i])); d2 += Math.Abs(ShortestAngleDiff(sol2[i], previous[i])); } theta = d1 <= d2 ? sol1 : sol2; } return true; } private static double WrapAngle(double a) { while (a > Math.PI) a -= 2.0 * Math.PI; while (a < -Math.PI) a += 2.0 * Math.PI; return a; } private static double ShortestAngleDiff(double a, double b) { double d = a - b; while (d > Math.PI) d -= 2.0 * Math.PI; while (d < -Math.PI) d += 2.0 * Math.PI; return d; } }

代码里几个地方值得展开说。a和b是D向量在主动臂摆动平面内的水平、竖直分量，它们的几何意义比公式本身更重要：主动臂只能在过基座铰点和中心轴线的竖直平面内摆动，所以D的水平分量必须投影到这个平面上。c的表达式来自从动臂长度约束展开，符号取决于θi的定义，如果自己的机器人主动臂水平时朝圆心方向，而不是向外，要把ca、sa换成-ca、-sa。

选解逻辑我分两种情况处理：没有上一时刻角度时就选绝对值小的那组，适合静态点动；有上一时刻角度时选角度差总和小的那组，适合连续轨迹。这里还有一个工程细节：真要对接运动控制器时，最好把机械限位也加进去，角度超出限位直接过滤掉，比单纯按角度差选更可靠。

3.3 逆解的边界：无解、双解与奇异位形

|C/R| > 1时无解，说明末端点不在这个支链的可达范围内。delta的可达工作空间是三个球面相交区域的包络，近似一个倒置的圆台，边界上速度放大效应很明显。|C/R|接近1时，asin的结果对误差极其敏感，末端的微小抖动会造成角度的大幅变化，轨迹规划时最好留出10到20毫米的安全余量。

双解问题值得单独说：代数上每条支链有两组解，三条支链组合起来理论上甚至有8种组合，但机械限位和基座干涉会滤掉大多数。实际运行中真正稳定的位形通常只有“上肘”和“下肘”两种连续位形。如果程序里出现角度来回跳变，先检查是不是选解时忘了加限位，或者上一时刻角度传成了别的时间戳的旧值。

4. 正解：从三个主动臂角度回到末端坐标，解析法落在C#里

4.1 正解为什么比逆解难

逆解每条支链独立，一个方程一个未知数，直接解三角方程就行。正解是三条支链共享一个末端点E，方程组是耦合的非线性方程组。常见解法有三类：数值迭代法、消除法、几何解析法。

数值迭代法（牛顿法、Levenberg-Marquardt）通用，但需要给初值，初值给不好就收敛到错误解，而且迭代次数不确定，不适合硬实时环境。几何解析法的思路是三个球面求交：每条从动臂的约束方程都是一个球面方程，三个球面的交点就是末端位置。球面方程两两相减会得到两个线性平面方程，两个平面的交线是一条直线，把直线代回任一个球面方程，就得到一个一元二次方程。整个过程没有迭代，计算量非常小，我用这个方案。

4.2 解析法推导：三个球面求交

先由主动臂角度直接算出主动臂末端U_i：

U_i = (Rb·cosαi + La·cosθi·cosαi, Rb·sinαi + La·cosθi·sinαi, La·sinθi)

注意这里不能直接把U_i当球心。动平台铰点相对末端中心有一个Rp的偏移，所以真正的球心是：

c_i = U_i - (Rp·cosαi, Rp·sinαi, 0)

三条从动臂约束分别是|E - c_i| = Lb，i=0,1,2。把第1、第2条方程分别减去第0条，得到两个线性方程：

(c1 - c0)·E = (|c1|² - |c0|²) / 2 (c2 - c0)·E = (|c2|² - |c0|²) / 2

这两个方程解出来的E在一条直线上。把E写成E = base + v·z的形式，代回第0个球面方程，就得到关于z的一元二次方程。判别式大于0时有两个解，对应末端在基座上方和下方两种位置；实际delta的末端在基座下方，所以选z更小的那个。

4.3 C#正解实现与选解规则

public bool Forward(double[] theta, out double ex, out double ey, out double ez) { ex = ey = ez = 0.0; if (theta == null || theta.Length != 3) return false; double[] cx = new double[3], cy = new double[3], cz = new double[3]; for (int i = 0; i < 3; i++) { double ca = Math.Cos(Alpha[i]); double sa = Math.Sin(Alpha[i]); double ct = Math.Cos(theta[i]); double st = Math.Sin(theta[i]); // 主动臂末端 U double ux = Rb * ca + La * ct * ca; double uy = Rb * sa + La * ct * sa; double uz = La * st; // 球心 c = U - 动平台铰点相对末端中心的偏移 cx[i] = ux - Rp * ca; cy[i] = uy - Rp * sa; cz[i] = uz; } // 两个线性平面方程的系数 double a11 = cx[1] - cx[0], a12 = cy[1] - cy[0], a13 = cz[1] - cz[0]; double a21 = cx[2] - cx[0], a22 = cy[2] - cy[0], a23 = cz[2] - cz[0]; double n0 = cx[0] * cx[0] + cy[0] * cy[0] + cz[0] * cz[0]; double n1 = cx[1] * cx[1] + cy[1] * cy[1] + cz[1] * cz[1]; double n2 = cx[2] * cx[2] + cy[2] * cy[2] + cz[2] * cz[2]; double b1 = (n1 - n0) / 2.0; double b2 = (n2 - n0) / 2.0; double det = a11 * a22 - a12 * a21; if (Math.Abs(det) < 1e-9) return false; // 三个球心几乎共线，解析法失效，改用数值迭代兜底 // 把 x、y 表示成 z 的函数：x = bx + vx*z，y = by + vy*z double bx = (b1 * a22 - a12 * b2) / det; double by = (a11 * b2 - b1 * a21) / det; double vx = (a12 * a23 - a13 * a22) / det; double vy = (a13 * a21 - a11 * a23) / det; // 代入球面0：|E - c0|^2 = Lb^2，得到关于z的一元二次方程 double mx = bx - cx[0]; double my = by - cy[0]; double mz = -cz[0]; double aSq = vx * vx + vy * vy + 1.0; double bSq = 2.0 * (mx * vx + my * vy + mz); double cSq = mx * mx + my * my + mz * mz - Lb * Lb; double disc = bSq * bSq - 4.0 * aSq * cSq; if (disc < 0) return false; // 三个角度组合不出有效末端位置 double sqrtDisc = Math.Sqrt(disc); double z1 = (-bSq + sqrtDisc) / (2.0 * aSq); double z2 = (-bSq - sqrtDisc) / (2.0 * aSq); double x1 = bx + vx * z1, y1 = by + vy * z1; double x2 = bx + vx * z2, y2 = by + vy * z2; // delta 末端在基座下方，z 通常为负，两个解里选 z 更小的那个 bool useFirst = z1 <= z2; ex = useFirst ? x1 : x2; ey = useFirst ? y1 : y2; ez = useFirst ? z1 : z2; return true; }

这段代码里最容易抄错的是球心c_i的计算。很多人会把U_i直接当球心，忽略Rp的偏移，导致正解算出来的点在机构半径大时误差不明显、半径小时偏差很大。动平台半径Rp必须在这里扣掉，因为球面方程描述的是“从动臂末端到动平台铰点”，而不是“从动臂末端到动平台中心”。

det阈值和disc阈值要注意量纲。代码按毫米单位写，det的量级通常是几千到几万，1e-9这个阈值基本不会误触发，只在三个球心几乎共线的奇异位形才会接近0。如果你把单位换成米，这个阈值也要对应调整。disc出现极小负值时，多数是末端点贴近工作空间边界，浮点误差导致，我这里直接返回false，工程上可以在外层补一个迭代法兜底。

5. 源代码 vs 工程文件：两种交付形态、工程配置与运行问题排查

5.1 源代码和工程文件分别解决什么问题

标题里“源代码 vs 工程文件”其实是压缩包里的两种常见形态。源代码通常是单个或多个.cs文件，方便移植到自己的上位机项目里；工程文件则是完整的.sln解决方案，带着WinForms或WPF界面，甚至可能有串口通讯、虚拟演示模块，方便直接跑起来看效果。

我的建议是：只想验证算法、把运动学塞进自己现有项目，优先看源代码文件，把DeltaKinematics这个类拷进去就行。想理解作者怎么调电机、怎么联接视觉或者怎么看轨迹，优先跑工程文件。但不管先看哪个，都要先确认对方的θi角度定义跟自己的一致，这是最容易翻车的地方，后面细说。

5.2 用C#工程跑通运动学的最小步骤

拿到.rar之后，我一般按这个顺序操作，能省掉大半编译报错：

1. 解压后先看有没有.sln，有就先打开.sln，不要只打开.cs文件。
2. 检查解决方案里项目的目标框架。C#上位机在现场环境很常见的是.NET Framework 4.7.2或4.8，新一些的工程可能是.NET 6/8。目标框架跟本机安装不一致，编译会直接报错。
3. 在Visual Studio里执行一次NuGet还原，很多工程依赖第三方库，不还原会报一堆找不到类型。
4. 全局搜索“class DeltaKinematics”或“Inverse(”定位运动学代码，先把参数改成自己机器人的实测值。
5. 用一小段调用代码验证逆解输出角度是不是符合直觉，再考虑接电机。

var ik = new DeltaKinematics { Rb = 200.0, Rp = 60.0, La = 250.0, Lb = 650.0 }; double[] angles; if (ik.Inverse(0.0, 0.0, -300.0, out angles)) { for (int i = 0; i < 3; i++) { Console.WriteLine($"电机{i + 1}角度：{angles[i] * 180.0 / Math.PI:F2}°"); } } else { Console.WriteLine("目标点在可达工作空间之外"); }

这段代码把弧度转成度再输出，因为界面上通常显示角度，而Math.Sin和Math.Cos只吃弧度。输出结果里三个角度应该大致相等且为正值，表示末端在正下方时三条支链对称上抬。如果出现一正一负之类的情况，先怀疑坐标系定义或主动臂朝向，而不是公式本身。

5.3 四个最容易翻车的运行问题排查

解压或打开阶段报错

现象：rar解压后双击.sln没反应，或者提示“此项目的目标框架需要某某版本，但本机未安装”。原因：压缩包从网络下载后被系统打上了“来自Internet”标记，或者目标框架版本不对。解决：右键工程文件，选“属性”，在“解除锁定”那里勾掉安全锁；然后确认本机安装的.NET版本，必要时用Visual Studio Installer补装对应开发组件。

编译报错“未能加载文件或程序集”

现象：编译通过，运行时抛异常，提示找不到某个DLL或依赖项。原因：多数是NuGet包没还原，或者平台目标不匹配，比如工程是x64的，本机调试环境是x86。解决：先执行“工具→NuGet包管理器→还原NuGet包”；再检查“项目属性→生成→平台目标”，工业相机SDK和控制器DLL经常只在x64下可用，统一改成x64。

正逆解互锁对不上，末端点偏差几十毫米

现象：用逆解算角度，再用正解算回去，结果和原始输入差了很大一截。原因：常见原因有三个——θi的零位定义不对（比如主动臂水平时不是朝外而是朝内）、Rp偏移在正解里没减掉、z轴方向反了。解决：先手动把θi设为0，在纸上算出U_i的坐标，再和实物比对；确认正解里c_i的计算扣掉了Rp的偏移；把逆解里的dz = -z改成dz = z试试，哪个结果符合实物就按哪个。

连续运动中角度突然跳变

现象：轨迹插补时，某个电机角度突然从30°跳到-120°，然后又跳回来。原因：逆解选解策略在两条解之间来回切换，通常是没传上一时刻角度，或者传了但角度差计算没做环绕处理。解决：调用带previous参数的重载方法；检查ShortestAngleDiff是否把差值归一化到[-π, π]，否则角度从179°到-179°会被误判为跳变。更稳的做法是再加一道机械限位过滤。

5.4 运动学计算的上位机线程与性能预算

C#上位机里，运动学计算最常被误放的坑是UI线程。WinForms或WPF的一帧渲染和事件处理本身就占时间，如果鼠标拖拽末端、实时刷新画面、同时做逆解，界面卡顿几乎是必然的。运动学本身其实非常轻：一次逆解大约几十微秒，一次正解也差不多，1毫秒的控制周期里计算占用不到百分之十。真正拖垮性能的是UI绘制、视觉识别和通讯等待。

我一般会把运动学放进独立线程，和UI线程用信号量或Channel传递数据。采集线程负责读编码器或视觉坐标，运动学线程做正逆解，发送线程把角度指令发给伺服。这样即使算法有瞬时波动，也不会让界面卡成黑匣子。实时性要求高的场合，建议在运动学线程里用Stopwatch做固定周期循环，或者用Windows的多媒体定时器，避免Thread.Sleep的节拍抖动。

6. 验证与收尾：先做正逆互锁自检，再谈收敛与选解策略

运动学代码写完后，第一件事不是接电机，而是做正逆互锁测试。随机在工作空间内取样，逆解出角度，再正解回来，看误差能不能控制在0.05毫米以内。这个做法能一次性暴露坐标符号、Rp偏移、选解策略几乎所有逻辑错误。

bool SelfTest(DeltaKinematics ik, int samples = 10000) { var rnd = new Random(20250601); int ok = 0; for (int i = 0; i < samples; i++) { // 在工作空间内随机采样：半径0~180mm，高度-20~-250mm double r = 180.0 * Math.Sqrt(rnd.NextDouble()); double a = 2.0 * Math.PI * rnd.NextDouble(); double x = r * Math.Cos(a); double y = r * Math.Sin(a); double z = -20.0 - 230.0 * rnd.NextDouble(); if (!ik.Inverse(x, y, z, out double[] theta)) continue; if (!ik.Forward(theta, out double px, out double py, out double pz)) return false; double err = Math.Abs(px - x) + Math.Abs(py - y) + Math.Abs(pz - z); if (err > 0.05) { Console.WriteLine($"样例{i}: 期望({x:F3},{y:F3},{z:F3}) 实际({px:F3},{py:F3},{pz:F3})"); return false; } ok++; } Console.WriteLine($"通过：{ok}/{samples} 个可达点互锁误差小于0.05mm"); return true; }

这个方法的价值在于回归测试。每次换参数、改坐标系、调整选解逻辑，先跑一遍随机互锁，确认没有低级错误，再考虑上控制器。我见过太多同行在实物上调试时发现正解反了，最后查出来是Rp漏了减，或者z轴符号取反了——这些用互锁测试几分钟就能暴露。

如果你打算用数值迭代法替代解析法，也有一个收敛策略：初值用上一帧的末端位置，迭代上限控制在10次以内，超出就强制返回上一帧结果。解析法虽然没有初值问题，但双解选择仍然要匹配机械限位。我现在每写一套运动学算法，都会先把SelfTest放进去，参数一改就跑一遍，这个习惯帮我少翻几次车。希望帮到你。

本文还有配套的精品资源，点击获取