Delta机器人的运动学正反解几乎是我每次要在新项目里引入并联机械臂时都要重新梳理一遍的东西。正解和反解本身的数学难度并不高但坐标系约定、支链布局角、主动臂零位定义这些细节只要有一个对不上写出来的代码就和真机完全对不上。这篇文章把我常用的推导过程、Python实现以及调试中踩过的坑整理一遍给正在做Delta机器人本体、控制系统或运动学算法移植的同学一个可以照着走的参考。1. Delta机器人的运动学本质三条虚拟支链的合成1.1 先搞清楚Delta为什么是“位置三自由度”很多第一次接触Delta的人会把它当成一个普通的三轴机械手但实际上它的运动学特性和串联三轴完全不同。Delta的结构是典型的并联构型静平台上按120度分布安装三个驱动电机每个电机带动一根主动臂主动臂末端再接一组平行四边形结构的从动臂从动臂另一端连接动平台。三条支链并联支撑动平台电机转动时三条支链共同决定动平台的位置。平行四边形从动臂是理解Delta运动学最关键的地方。由于平行四边形结构的存在动平台在运动过程中始终与静平台保持平行不会发生绕X、Y、Z轴的旋转。也就是说姿态被机械结构硬性锁死了剩下的自由度只有X、Y、Z三个方向的平动。所以Delta的运动学问题被大幅简化我们不需要解欧拉角不需要处理姿态矩阵只需要解末端位置P(Px, Py, Pz)。这是整个正反解分析的起点。正因为只有三个位置自由度正反解的目标就非常清晰反解是从目标坐标映射到三个主动臂转角正解是从三个电机编码器读数映射回末端坐标。搞清楚这个前提后面的公式推导才不会跑偏。1.2 本文的坐标系、符号与几何参数约定做运动学推导最怕符号不统一。不同文献里主动臂角度的定义方式五花八门有的从竖直向上方向量有的从水平面向上量有的还出现过从水平面向下量。写代码之前先把约定固化下来后面能省掉大量排查时间。我这套推导采用的约定如下表符号含义说明R静平台铰链分布半径静平台中心到主动臂旋转轴线的水平距离r动平台球铰分布半径动平台中心到从动臂连接球铰的水平距离L主动臂长度主动臂旋转轴到从动臂上球铰中心距离l从动臂长度平行四边形杆两端球铰中心距离θi第i支链主动臂角0度对应主动臂水平逆时针向上为正φi第i支链布局角本文取0度、120度、240度P动平台中心位置即运动学输出/输入的末端坐标Ei第i主动臂末端点主动臂与从动臂连接点Ci动平台上第i球铰中心从动臂与动平台连接点ei第i支链方向单位向量ei (cosφi, sinφi, 0)坐标系我使用右手系静平台中心为原点Z轴竖直向上X轴指向第一个主动臂铰链点方向。需要注意的是有些Delta机型的三个铰链并不在0度、120度、240度这些整数角度上而是整体旋转了一个偏移角比如第一臂指向30度方向。这种差别不影响算法本身但代码里要留一个布局角偏置参数方便按照实际机械图纸调整。还有一个细节必须在建模时先确认静平台铰链分布角和动平台球铰分布角必须使用同一套φ定义也就是基座第i个铰链和动平台第i个球铰要一一对应。如果两者之间有额外相错角不能直接套用下面推导的简化公式否则正反解回代一定对不上。1.3 为什么正解难、反解相对直观Delta的运动学有个有趣的反差反解很简单正解反而要绕一点。原因是三条支链在反解时是彼此独立的——给定末端位置每个主动臂只需要单独满足自己的几何约束就能求出转角互不干扰。而正解时三个转角共同决定一个末端位置三条支链的约束方程必须联立求解相当于在三维空间里找三个球面的交点。从控制系统的角度看两者的用途也不一样。反解用于轨迹规划上位机给出笛卡尔空间的目标点控制器必须在一个控制周期内把它换算成三个电机的角度指令。正解用于位置反馈读取编码器角度后换算成末端实际位置供监控、示教和闭环补偿使用。工业控制器里反解通常每个周期都要算正解可以放在稍低频率的监控任务里。这也意味着工程上反解代码的实时性要求更高正解代码的数值稳定性要求更高。我见过有人在低成本MCU上跑正解时因为浮点运算处理不当在工作空间边缘出现抖动后面第五章会专门讲这类坑。2. 位置正解从三个电机角度解出末端坐标2.1 三球交会法的几何建模正解的几何建模思路我推荐用三球交会法也叫等效球心法。先写出主动臂末端Ei的坐标。由于主动臂在一个包含Z轴的径向垂直平面内摆动它的水平投影始终沿ei方向所以Eix (R L·cosθi)·cosφi Eiy (R L·cosθi)·sinφi Eiz L·sinθi这里θi从水平面向上为正。当θi等于0度时主动臂水平向外伸展θi等于90度时主动臂竖直向上。这个定义方式在实际标定时很直观——用水平尺就能找到零位后续对零会方便很多。接着看从动臂约束。动平台上的球铰点Ci满足Ci P r·ei就是从动臂末端相对动平台中心有一个固定偏移。而从动臂本身长度为l它连接了Ei和Ci所以天然满足|Ei - Ci| l。把Ci P r·ei代入这个距离约束整理一下就得到一个很漂亮的形式|P - (Ei - r·ei)|² l²这就意味着如果定义一个等效球心Oi Ei - r·ei那么末端位置P到Oi的距离恒等于l。三条支链分别得到三个球面方程P就是这三个球面的交点。Oi的坐标表达式可以合并成Oi ((R - r L·cosθi)·cosφi, (R - r L·cosθi)·sinφi, L·sinθi)大家看这个式子会感觉到等效球心法本质上把动平台半径r“挪”到了基座上把问题变成了一个纯粹的三球求交问题。这是我自己最喜欢用的正解建模方式几何意义清楚不容易出错。2.2 建立球方程组并消元三球交会的方程写出来是|P - O1|² l² |P - O2|² l² |P - O3|² l²直接展开是三个二次方程看着有点复杂但用两个方程两两相减二次项会被消掉得到两个线性方程2·P·(O2 - O1) |O2|² - |O1|² 2·P·(O3 - O1) |O3|² - |O1|²这两个线性方程在几何上表示两个平面它们的交线是一条直线。末端位置P必须落在这条直线上。再把直线参数代回任意一个球面方程就能得到一个关于参数的一元二次方程最多两个解。对Delta而言两个解分别对应动平台在交线上方还是下方。实际机构中动平台都在静平台下方工作所以通常取z较小或z较大的根取决于你的坐标系Z轴方向。接下来的数值实现我推荐直接用三维几何的方法也就是GPS三边定位算法里那套正交基分解办法。它的好处是不需要手工推导复杂的二次方程系数代码通用性强而且数值稳定性比直接解解析表达式好得多。2.3 正解代码实现Python下面给出一个可以直接跑通的正解函数。整体流程是由三个主动臂角算出Ei进而算出等效球心Oi然后调用三球交会函数求P。import numpy as np DELTA_PHI np.deg2rad([0.0, 120.0, 240.0]) def delta_forward_kinematics(theta_deg, R, r, L, l): theta np.deg2rad(theta_deg) E np.empty((3, 3)) for i in range(3): E[i, 0] (R L * np.cos(theta[i])) * np.cos(DELTA_PHI[i]) E[i, 1] (R L * np.cos(theta[i])) * np.sin(DELTA_PHI[i]) E[i, 2] L * np.sin(theta[i]) # 等效球心 O_i E_i - r * e_i O E.copy() O[:, 0] - r * np.cos(DELTA_PHI) O[:, 1] - r * np.sin(DELTA_PHI) return _trilateration(O[0], O[1], O[2], l, l, l) def _trilateration(P1, P2, P3, r1, r2, r3): # 三球交会取z正解 ex (P2 - P1) / np.linalg.norm(P2 - P1) i_tmp np.dot(ex, P3 - P1) ey (P3 - P1 - i_tmp * ex) / np.linalg.norm(P3 - P1 - i_tmp * ex) ez np.cross(ex, ey) d np.linalg.norm(P2 - P1) j np.dot(ey, P3 - P1) x (r1 * r1 - r2 * r2 d * d) / (2.0 * d) y (r1 * r1 - r3 * r3 - x * x i_tmp * i_tmp j * j) / (2.0 * j) z2 r1 * r1 - x * x - y * y if z2 0.0: if z2 -1e-9: z2 0.0 else: raise ValueError(目标点不在工作空间内) z np.sqrt(z2) return P1 x * ex y * ey z * ez注意_trilateration里我取了z的正根。如果你的Delta是倒装结构或者Z轴定义成向下为正这个符号要相应调整。另一个细节是三个球心不能共线否则正交基构建会失败。实际Delta结构里三个O点必然构成一个正三角形分布不会退化所以可以放心用。2.4 正解的自检方法写完正解第一件事不是上真机而是做回代验证。我的习惯是这样随机生成1000组在机械限位范围内的θ角分别用正解算出P再把P丢进反解函数算出θ比较|θ - θ|的最大误差。如果是double精度误差应该压在1e-9这个量级。如果误差很大基本可以断定是布局角定义、θ正负号或坐标系方向出了问题。还可以做一个对称性测试令三个θ角完全相同即θ1θ2θ3此时正解算出的P点应该落在Z轴上也就是Px和Py接近0。如果不为0说明三个支链的布局角相位关系不对大概率是φi定义或者动平台和静平台的对应关系错了。3. 位置反解从目标坐标解出三个电机角度3.1 独立支链的几何分解反解的建模思路是把它拆成三个独立的平面几何问题。对第i条支链目标点P是已知的所以动平台球铰点Ci的位置也是已知的Ci P r·ei。现在只看这一个支链所在的平面。这个平面由ei方向和Z轴张成主动臂末端Ei在这个平面内绕基座铰链转动从动臂长度l把Ei和Ci连接起来。已知Ci、固定端A_i、杆长L和l求主动臂转角θi这完全是一个平面上的几何题。把Ci在这个平面里做正交分解。沿着ei方向的分量是P·ei r竖直方向的分量是Pz。主动臂末端Ei的径向分量是R L·cosθi竖直分量是L·sinθi。于是水平距离差和竖直距离差都变成θi的函数再用距离公式套从动臂长度约束即可。3.2 单支链闭环方程推导记水平距离差s (R L·cosθi) - (P·ei r) R - r - P·ei L·cosθi竖直距离差t L·sinθi - Pz从动臂长度约束给出s² t² l²这个方程里既有sinθi又有cosθi直接用反三角函数不太好解。我习惯做一步变量替换令m R - r - P·ein Pz则方程变为(m L·cosθi)² (L·sinθi - n)² l²展开后整理L² m² n² - l² 2L·(m·cosθi - n·sinθi) 0再用辅助角公式。令K sqrt(m² n²)α atan2(n, m)那么m·cosθi - n·sinθi K·cos(θi α)于是得到cos(θi α) (l² - L² - m² - n²) / (2·L·K)右边这个值如果落在[-1, 1]之外说明目标点超出了该支链的可达范围。只要落在区间内反解就有两个可能的角度θi -α ± arccos(rhs)这两个解对应主动臂“向上装”和“向下装”两种装配模式具体选哪个由机构决定。3.3 反解代码实现Pythondef delta_inverse_kinematics(P, R, r, L, l, elbow_upTrue): x, y, z P theta np.empty(3) for i in range(3): m R - r - (x * np.cos(DELTA_PHI[i]) y * np.sin(DELTA_PHI[i])) n z K np.hypot(m, n) alpha np.arctan2(n, m) rhs (l * l - L * L - m * m - n * n) / (2.0 * L * K) if rhs -1.0 or rhs 1.0: raise ValueError(f目标点不可达: 支链{i}) delta np.arccos(np.clip(rhs, -1.0, 1.0)) theta[i] -alpha delta if elbow_up else -alpha - delta return np.rad2deg(theta)这里elbow_upTrue表示主动臂向上偏转也就是θ角的解析解取较大的一支。如果你的机构是倒挂安装机械上主动臂只能朝下摆就传False。实际工程中更稳妥的做法是在选解之后再做一次机械限位判断把不在限位范围内的解直接丢弃。比如θ设计范围是0度到90度解析解算出-30度那就说明当前目标点在机构上不可达。3.4 多解处理与装配模式选择反解每条支链最多两个解三条支链组合起来理论上最多有八组关节角但机械上通常只有一组符合实际装配形态。最省事的做法就是根据主动臂的安装方向固定选解分支比如全部取elbow_upTrue。还有一种需要特别注意的情况当机构接近工作空间边界时rhs接近±1此时arccos的导数趋于无穷大θ角对末端坐标极其敏感微小扰动会被放大。这时候即使解在数学上存在实际控制效果也会很差。所以轨迹规划时一定要留出安全余量不要把目标点压在工作空间边界上。我实际调试中还遇到过一个问题走连续轨迹时某个轴偶尔会出现瞬间反转180度的跳变。根因就是某一帧的rhs在边界上抖动导致arccos输出跨过分支切换点。解决办法是给反解加一个连续性约束在八组可行解里选择与上一帧θ最接近的那组而不是固定用某个elbow_up。这个细节在第五章会展开。4. 配套的工程计算工作空间、雅可比与零点标定4.1 工作空间边界判定Delta的工作空间形状有点像倒扣的穹顶中心区域饱满四周和顶部快速收窄。工程上判断一个点是否可达不需要单独写复杂的几何求交直接用反解函数即可反解时如果任何一个支链的rhs越界就说明这个点不在工作空间内。基于这个思路做工作空间扫描非常简单。在某个固定高度z上对x和y做二维网格扫描调用反解函数能够成功解出θ就标记为可达抛出异常就标记为不可达。网格分辨率取2到5毫米扫描一圈就能得到该高度下的XY截面轮廓。把不同高度的截面叠起来就能看到完整的工作空间形状。用这种方法还能顺带发现一个常见问题有些点的三个支链都能独立满足距离约束但组合出来的三轴转角互相矛盾比如某个轴的解析解超出电机限位。所以扫描时别忘了把机械限位判断也加进去否则算出的“可达区域”会比实际大一圈。这个差异在规划拾取路径时非常关键尤其是贴着工作空间边缘跑的轨迹。4.2 速度雅可比与奇异位形位置正反解解决的是“角度到位置”的静态映射但真正做高速运动控制时还需要速度层面的关系也就是雅可比矩阵J它把关节角速度映射到末端笛卡尔速度Ṗ J·θ̇。对Delta的速度雅可比可以用约束方程求导来推。对每条支链都有|Ei - Ci|² l²两边对时间求导令wi Ei - Ci得到wi · (Ẹi - Ċi) 0其中Ċi Ṗ因为Ci相对P的偏移是常数Ẹi vi·θ̇ivi是主动臂末端速度方向。整理后得到(wi · vi)·θ̇i wi · Ṗ把三条支链写在一起就得到对角矩阵左乘关节速度等于矩阵W乘以末端速度其中W的行向量是wi。于是J W⁻¹ · diag(wi · vi)奇异位形出现在两类情况一类是det(W)接近0三条从动臂的力线方向趋于共面相关机构在这个方向失去约束另一类是某个(wi · vi)接近0意味着从动臂方向几乎和主动臂末端速度方向垂直该支链在这个瞬间无法有效驱动末端运动。最小条件数出现在工作空间中心附近越靠近边界条件数越大。实操层面的建议是在每个控制周期顺手算一下(wi·vi)的最小值或W的条件数如果低于阈值就降速或报警。我在高速分拣设备上就是这样做的效果比单纯依赖位置反解的可达性判断可靠得多。4.3 零点标定模型约定与机械零位对齐运动学模型里θi0度对应主动臂水平这个零位必须和真机机械零位严格对齐。整机装配完成后电机的编码器零位基本不可能刚好在水平位置所以标定是绕不开的一步。我的做法是先断开伺服使能手动把三个主动臂分别转到水平位置。判断水平可以用精度高一点的数显水平尺贴在主动臂的加工基准面上。然后读取编码器原始值把这三个读数存成零点偏移。代码里每次读取角度时先做(编码器读数 - 零点偏移)再根据安装方向乘以正负号和减速比得到模型所用的θ。这里有两个容易踩的细节。第一个如果编码器装在电机端而减速器装在输出端角度要除以减速比。真实系统的减速比可能有齿轮间隙正反转时零点偏移会有微小差异高速往返运动时这个差异会直接表现为末端轨迹误差。第二个方向符号必须在通电后实际验证手动把主动臂抬高几度观察换算后的θ是否变大。如果变小把符号取反。不要想当然按电机接线顺序推断我在这上面吃过大亏。4.4 从代码仿真到控制器部署正反解在Python里验证通过后移植到C/C控制器时主要注意三件事。第一浮点精度。尽量用double。float在小数点后有效位数不够经过反解余弦、arccos等运算后末端坐标误差会被放大。第二角度规范化。电机角度是连续增长的换算成θ后要限制在[-π, π]或[0, 2π)否则三角函数会出现周期性跳跃连续性约束判断也会失灵。第三实时性。一次反解包含三个支链的三角函数计算在带FPU的Cortex-M4或M7上几百纳秒到几微秒级别完全跑得动1000Hz控制周期。如果芯片不带FPU可以提前算好sin/cos查表或者把所有角度转成定点数但精度要重新评估。代码结构上我建议把正解和反解拆成独立的纯函数不依赖任何全局变量这样单元测试好写也方便以后做几何参数标定时反复调用。5. 调试实录运动学代码最容易踩的几个坑5.1 坐标系方向不一致导致轨迹扭曲症状很典型走正方形轨迹变成了平行四边形走圆轨迹变成了歪斜的椭圆。这时候先别怀疑机械精度大概率是运动学代码里的坐标系方向和实际安装对不上。排查方法是单轴动作测试。只给第一个电机发一个正弦角度指令让另外两个轴保持在零位观察末端实际轨迹。如果代码正确末端应该在一个包含X轴和Z轴的径向垂直平面内移动也就是Px变化最大Py基本不动。如果实测发现末端在别的平面里动说明第一个支链的布局角φ1或电机方向符号不对。逐个轴测一遍很快就能锁定问题在哪条支链。我碰到的真实案例是第2轴安装支架角度偏差了将近2度激光跟踪仪测出末端轨迹整体绕Z轴偏转一开始我怀疑代码不对查了一圈下来发现是机械加工件精度问题。所以这个问题还有一层警示先确认机械本体的安装精度达标再调运动学参数否则会陷入互相甩锅的循环。5.2 静平台铰链与动平台球铰相位不一致如果正解反解回代误差很大但单轴测试又是正常的那要重点检查静平台和动平台的布局相位。我在1.2节强调过基座第i个铰链和动平台第i个球铰必须使用同一套φ角。但也有机器设计时两边的铰接连杆位置是相错60度安装的比如静平台三个铰链在0、120、240度而动平台三个球铰在60、180、300度。这种情况下等效球心Oi的推导不能直接套用。解决办法有两种一是把布局角拆成两组Φ_base和Φ_moving在推导时保留两个不同的e向量公式会复杂不少二是干脆把动平台坐标系旋转60度后重新定义让两组φ角重新对齐。我在代码里更倾向第二种因为改动最小、不易出错。检测这个问题的土办法很简单把动平台拆下来三个主动臂手动摆到完全相同的角度比如都水平此时理论上正解算出的Px和Py应该为0。如果不为0基本就是两组φ相位对不上。5.3 反解多解跳变导致电机瞬间反转这是我在高速拾取项目里踩过最隐蔽的坑。现象是设备跑一段时间后某个轴突然“咔哒”一声电机瞬间反转随后又恢复正常。最初以为是驱动器参数问题后来抓了波形才发现是反解选解不稳定。原因其实在3.4节提过固定elbow_up选解时某些靠近工作空间边界的点会让rhs在±1附近抖动导致相邻两个控制周期里选出的角度分支不同。真机上的表现就是角度指令瞬变电机带着机械负载直接反转。解决办法是在反解函数里加入上一帧角度作为输入计算每个可行解与上一帧角度的差值取最小的那个分支。如果差值大于某个阈值比如90度同时另一个分支差值很小就优先取小差值的解。配合机械限位判断基本能根除这个问题。这个连续性处理是Delta控制器里必须做的一步但很多教程里的伪代码都省略了。5.4 数值与测量误差先测参数再调算法最后说一个容易被忽视的现实问题仿真跑得再完美真机精度还是由几何参数的实测精度兜底。R、r、L、l这四个参数如果完全照抄CAD模型而不实测0.1毫米级别的误差就可能导致末端几毫米的定位偏差。我建议装配完后用卡尺或激光测距仪实测R是三个铰链轴线到静平台中心的平均距离r是动平台三个球窝中心到动平台中心的平均距离L是主动臂两端销轴中心距l是从动臂两端球窝中心距。注意从动臂是平行四边形两杆长度必须严格一致否则动平台会倾斜。如果整机精度要求很高还可以做一次简单的几何参数标定让机器人走一个已知尺寸的矩形或圆记录多个位置的编码器值和末端实测坐标用最小二乘法反推R、r、L、l。这一步能明显提升绝对定位精度尤其是对视觉引导抓取类应用。从我实际经手的几个项目来看运动学正反解推导本身从来没有成为瓶颈真正决定整机表现的往往是坐标系约定、机械装配精度和零点标定这些“周边”工程细节。把前面五章的内容按顺序走一遍正解反解互验、工作空间扫描、零点标定、选解连续性这些环节都做到位Delta机器人的运动学控制基本就稳了。后面如果大家感兴趣我还可以把几何参数标定的最小二乘实现单独写一篇那个话题展开讲也挺多内容。