简介一套面向机器人学习与研究者的机械臂轨迹规划资料包聚焦MATLAB机器人工具箱中的直线轨迹与圆弧轨迹生成方法适用于需要掌握关节空间规划、理解逆运动学与插值原理的入门及进阶用户。包内包含完整SolidWorks三维模型、MATLAB轨迹规划程序及运动学分析报告可辅助从模型到代码的对照学习。资源共36个文件涵盖28个sldprt零件、4个sldasm装配体、3个.m脚本及1个docx报告包体大小10.22MB模型用于结构展示脚本提供可直接运行的规划示例报告梳理理论推导与参数设置。目前已有9548人学习下载适合以实际机械臂模型为参考系统走通直线与圆弧轨迹规划全流程的用户。借助完整模型、程序与文档读者能从正/逆运动学、速度约束到轨迹插值逐步展开分析快速形成可复用、可扩展的实验思路。1. matlab机器人工具箱做机械臂轨迹规划直线与圆弧别等画出来再改机械臂直线轨迹和圆弧轨迹规划难点不在正逆解而在“把末端位置约束在一条直线/一段圆弧上”。很多人第一次做直线规划时顺手用关节空间插值函数动画里看末端走的是大弧线折腾半天才发现轨迹规划要回到笛卡尔空间重做。用 matlab机器人工具箱做这件事套路其实很固定先在任务空间生成带加减速的路径点再逐点做逆运动学拿到关节序列后驱动模型验证SW模型用来核对几何外观和关节坐标系朝向matlab程序负责把轨迹可复现地算出来报告则把每一步的参数和偏差记录留档。这篇文章从直线轨迹讲到圆弧轨迹再把 SW模型导入、逆解不收敛这类翻车点一起说清楚适合正在做机器人课程设计或机械臂应用方案的开发者。2. 直线轨迹规划从起点到终点的目标位移落到机械臂关节角2.1 为什么末端直线必须在笛卡尔空间插值关节空间和任务空间的本质区别直线轨迹规划里最常见的错误是在关节空间做插值。关节空间里每个关节从初始角平滑运动到目标角看起来每个关节都很顺滑但正运动学是一种非线性映射多关节同时转动的合成结果末端在三维空间里画出来的是一条复杂曲线不是直线。换句话说关节角的变化很漂亮不代表末端轨迹满足工艺要求。焊接、涂胶、码垛这类场景要求的是末端路径严格走直线所以约束必须定义在笛卡尔空间也就是任务空间。任务空间做直线轨迹规划的流程分三步先确定起点位姿和目标位姿接着在两点之间按时间生成一系列中间位姿最后对每个中间位姿做逆运动学求关节角。中间位姿的生成要解决两个问题一是位置插值保证平移部分沿直线走二是速度轮廓避免起步和结束的瞬间速度突变。常见做法是用梯形速度曲线或S型速度曲线生成归一化位移序列再把这个序列传给位姿插值函数这样末端速度就是先匀加速、再匀速、最后匀减速机械臂动作不会前后一顿一顿。这也就是为什么不能直接用均匀分布的位移点。均匀等距听起来简单但每个点之间的位移差恒定相当于全程匀速起点和终点的加速度理论上无限大真实机械臂根本跟不住。用 matlab机器人工具箱做这件事一般先把位移比例序列算出来再逐点构造位姿矩阵最后统一逆解。整个流程里最容易被忽略的是逆解连续性相邻路径点上的关节角必须平滑过渡如果某个点逆解跳到一个完全不同的构型动画里就是关节猛甩一下。所以后续代码里我用上一时刻的关节角作为当前逆解的初始猜测值这是让关节序列连续的一个很实用的习惯。2.2 直线轨迹生成的Matlab代码梯形速度加SE(3)插值逐点逆解下面这段代码是最小可复现版本机器人对象假设已经用工具箱里的 SerialLink 类创建好命名为 myrobotD-H参数和关节限位都已设置完毕。轨迹起始位姿和终止位姿用工具箱的 transl 和 rpy2tr 构造前者写平移量后者写旋转量。% 直线轨迹规划任务空间插值 逐点逆解 steps 80; % 轨迹点数越大路径越密 dt 0.01; % 时间步长用于后续速度计算 T0 transl(0.30, 0, 0.25) * rpy2tr(0, pi/2, 0); % 起始位姿 T1 transl(0.55, 0.20, 0.40) * rpy2tr(0, pi/2, pi/4); % 终止位姿 s lspb(0, 1, steps); % 梯形速度曲线返回归一化位移 Ttraj zeros(4, 4, steps); % 存放中间位姿矩阵序列 for i 1:steps Ttraj(:,:,i) trinterp(T0, T1, s(i)); % SE(3)线性插值 end q zeros(steps, 6); % 关节角矩阵6自由度机械臂 q(1,:) myrobot.ikine(T0, mask, [1 1 1 1 1 1]); % 起点逆解 for i 2:steps q(i,:) myrobot.ikine(Ttraj(:,:,i), ... q0, q(i-1,:), mask, [1 1 1 1 1 1]); % 用上一时刻角做初始猜测 end逻辑说明lspb 生成从 0 到 1 的位移比例序列起止段速度为零中间段匀速这是梯形速度曲线的标准用法。trinterp 在 4×4 齐次变换矩阵之间做插值平移部分线性插值旋转部分使用球面线性插值所以末端位置严格走在两点连线上旋转姿态也是平滑过渡。ikine 是工具箱的数值逆解函数第 2 行到第 8 行是为了避开奇异位形而做的逐点求解。参数说明steps 决定路径密度直线轨迹 50 到 100 点足够点数越多速度曲线越平滑但逆解次数线性上涨实时性差的电脑会开始卡顿。mask 控制末端哪些自由度参与逆解6 自由度机械臂全开写成六个 1如果机械臂末端只有 5 个自由度必须把对应轴位置改成 0否则逆解会在一个不存在自由度的方向上反复迭代收敛得很慢。q0 是当前路径点的初始关节角猜测值传上一时刻的关节角可以让相邻两个路径点的逆解结果保持连续避免关节角跳变。这里有一个细节如果起始位姿本身就接近奇异ikine 第一帧就可能失败需要先用 myrobot.ikine(T0) 单独验证一次再进入循环。2.3 直线轨迹验证直线度和速度连续性用什么数字盯住轨迹算完不等于做完验证环节至少要盯两个数末端实际轨迹和理论直线的最大偏差以及关节速度曲线的光滑程度。末端实际轨迹不能直接看动画要通过正运动学把关节角序列映射回末端位置再对比。% 验证末端是否走直线并计算直线度偏差 t_theory squeeze(transl(Ttraj)); % 理论路径位置3×steps 矩阵 t_actual myrobot.fkine(q); % 正运动学返回4×4×steps t_actual transl(t_actual); % 提取实际末端位置 % 逐点取偏差向量的模再取最大偏差 delta t_actual - t_theory; max_dev max(vecnorm(delta, 2, 1)); disp([最大直线度偏差: , num2str(max_dev), m]); % 关节速度连续性检查 qd diff(q) / dt; % 数值差分近似关节速度 max_joint_vel max(abs(qd(:))); figure; plot(qd); grid on; % 画所有关节速度曲线这里我没有在第一行给 Ttraj 做转置处理是因为 transl 返回的是 3×steps 的矩阵每列对应一个路径点方向正确。max_dev 通常是逆解的数值误差累计理想情况下在 1e-6 量级如果超过 1e-3说明逆解迭代精度不够或者目标位姿已经越过了奇异区域这时需要调 ikine 的容差参数比如 tol 改小到 1e-10或者把 steps 提高让路径点更密。关节速度曲线如果出现明显的尖刺基本就是某一帧逆解跳到了别的构型回到上一段代码里检查 q 序列相邻两行的差值差值特别大的那一行就是问题帧。3. 圆弧轨迹规划三点定弧从几何计算到关节空间的完整落地方案3.1 圆弧轨迹的几何基础圆心、半径、方向和三点定圆的稳定算法圆弧轨迹和直线轨迹最大的差别是路径不再由两个端点唯一确定需要三个不共线的点。三个点定义一段空间圆弧此时必须先把圆心和半径算出来才能在圆弧上密集生成路径点。三个不共线的点能确定一个平面圆心在这个平面上并且到三个点的距离相等也就是同时落在三条边的中垂面上。常见的数值做法是把这个几何约束写成线性方程组一次解出。三个点里如果有两点过于靠近或者三点接近共线圆心会飘到很远轨迹计算直接失真所以做圆弧轨迹前要先判断三点是否满足基本几何条件。判断方式很简单法向量长度小于某个阈值就说明接近共线直接报错返回。下面这段代码给出圆心求解的稳定写法用的是最小二乘解而不是手工推导的解析公式处理任意空间姿态的三点圆弧都不需要额外旋转坐标平面。% 三点定圆求空间圆弧的圆心 C 和半径 r P1 [0.40; 0.10; 0.30]; % 圆弧起点 P2 [0.45; 0.20; 0.35]; % 圆弧中间点 P3 [0.35; 0.25; 0.32]; % 圆弧终点 n cross(P2 - P1, P3 - P1); % 圆弧平面法向量判别三点是否共线 if norm(n) 1e-6 error(三点接近共线无法确定空间圆弧); end A [2*(P2-P1); 2*(P3-P1); n]; % 两个中垂面方程 三点所在平面方程 b [norm(P2)^2 - norm(P1)^2; ... norm(P3)^2 - norm(P1)^2; ... n * P1]; C A \ b; % 圆心坐标3×1向量 r norm(P1 - C); % 圆弧半径说明一下这个方程组的来源。第一个方程来自 |P2 - C|^2 |P1 - C|^2展开后就是 2*(P2-P1)·C |P2|^2 - |P1|^2第二个方程同理。第三个方程是平面约束 n·(C - P1) 0保证圆心落在三点所在的平面上。用 A\b 不是 inv(A)*b数值上更稳定是解线性方程组的通用习惯。这里的 n 没有归一化也没关系方程组右侧第三行已经用 n*P1 做了对应缩放不影响方程组的一致性。计算 C 之后圆弧半径 r 取 P1 到 C 的距离就够不需要对三个点都算一遍取均值因为方程组已经强制三个点到 C 的距离相等了。这个步骤完成后圆心和半径是后续所有路径点生成的基础如果 SW模型里圆弧特征对应的几何尺寸能和这里的 r 对上说明输入数据没给错。3.2 圆弧路径点生成代码局部坐标系下做角度插值避免三维旋转混乱圆心算出来之后下一步是在圆弧上按角度生成密集路径点。实现上有两种常见做法直接在全局坐标系里用atan2算角度或者在圆弧平面建立局部坐标系把三维圆弧降成二维圆再插值。我一般用局部坐标系原因是在全局坐标系里对三个点求角度很容易被坐标轴方向的投影干扰尤其是当圆弧平面不平行于任何坐标平面时角度计算容易出符号错误。局部坐标系的构建方式是以 P1-C 为 x 轴以平面法向量为 z 轴y 轴由叉积补全。这个坐标系把圆弧变成 xOy 平面上的一个标准圆角度插值就退化成二维问题最后再把生成的路径点转换回全局坐标即可。这里有一个需要小心的知识点圆弧有顺时针和逆时针两种方向还有优弧和劣弧的区别。判断方向不能只看起点和终点的角度差而要看中间点 P2 落在哪一侧这就是很多人写出来的圆弧轨迹会突然画一个大圈的原因。% 在局部坐标系中生成圆弧路径点 u (P1 - C) / r; % 局部x轴基向量 w n / norm(n); % 局部z轴基向量即法向量归一化 v cross(w, u); % 局部y轴基向量 Rl [u; v; w]; % 世界-局部 的旋转矩阵3×3 p1 Rl * (P1 - C); % 三点变换到局部坐标系的xOy平面 p2 Rl * (P2 - C); p3 Rl * (P3 - C); th1 atan2(p1(2), p1(1)); % 起点角度 th2 atan2(p2(2), p2(1)); % 中间点角度用于判断圆弧走向 th3 atan2(p3(2), p3(1)); % 终点角度 % 根据中间点确定旋转方向并处理跨 ±π 的情况 if th2 th1 if th3 th2 th_end th3; % 逆时针终点角直接使用 else th_end th3 - 2*pi; % 逆时针跨过 π角度减去一整圈 end else if th3 th2 th_end th3; % 顺时针 else th_end th3 2*pi; % 顺时针跨过 -π end end steps 60; theta linspace(th1, th_end, steps); % 角度等差序列 arc_pts zeros(3, steps); % 存放圆弧路径点世界坐标系 for i 1:steps local_pt [r * cos(theta(i)); r * sin(theta(i)); 0]; arc_pts(:,i) C Rl * local_pt; % 局部转世界 end逻辑说明Rl 的每一行是局部坐标系的基向量所以 Rl * (P1-C) 把全局坐标转换到局部坐标。Rl 是正交矩阵从局部回到世界用 Rl 乘以局部坐标向量即可不需要单独写逆矩阵。th1、th2、th3 分别对应三个点在局部坐标系里的极角判断顺序用的是 th2 和 th1 的相对大小而不是 th3 和 th1 的相对大小这是为了确定真实旋转方向。如果 th2 大于 th1 说明从起点到中间点是逆时针走此时终点角度如果比中间点小说明弧线跨过了 π 边界th_end 要减 2π 再插值。这个跨边界的处理是圆弧轨迹里最容易写错的地方不加这一步跨边界时 linspace 会从 th1 反向一路插过去画出来的直接是补弧。参数说明steps 这里设置成 60圆弧一般比直线更需要密一点因为在相同角度间隔下半径越大实际弧长越长末端线速度在每个区间里并不完全均匀。如果需要时间同步或速度可调还要额外把 theta 序列换成等弧长序列或者先算弧长再按弧长规划速度这个属于圆弧轨迹的进阶玩法在避坑章里再展开。3.3 圆弧位姿序列与逆解姿态固定和姿态随动的两条实现路线圆弧上的每个点都只是一个位置要把路径点变成机械臂能执行的轨迹还必须给每个路径点配一个姿态。姿态怎么给取决于工艺要求常见的是两种姿态全程固定或者姿态随圆弧切线方向旋转。姿态固定适合点胶、喷涂这类要求工具方向始终垂直于工件表面的场景代码实现最简单直接把一个恒定旋转矩阵右乘到每个路径点上。姿态固定做法的代码段如下R0 是基准姿态矩阵按你的工具安装方向设定。如果机械臂末端法兰坐标系和工具坐标系之间有安装转角需要在 R0 前面左乘工具安装补偿矩阵这个细节漏掉的话末端看起来会整个歪着走。% 圆弧路径生成位姿序列姿态全程固定 R_fix rpy2tr(0, pi/2, 0); % 固定姿态按实际需要修改 T_arc zeros(4, 4, steps); for i 1:steps T_arc(:,:,i) transl(arc_pts(:,i)) * R_fix; end % 逐点逆解q0 用上一帧关节角保证连续 q_arc zeros(steps, 6); q_arc(1,:) myrobot.ikine(T_arc(:,:,1), q0, myrobot.getpos(), mask, ones(1,6)); for i 2:steps q_arc(i,:) myrobot.ikine(T_arc(:,:,i), q0, q_arc(i-1,:), mask, ones(1,6)); end第一种实现的关键参数是 R_fix 本身它除了包含姿态还决定了圆弧路径点在世界坐标里的朝向一致性。如果 R_fix 和 arc_pts 的几何位置方向差得太多ikine 很容易在某一段圆弧上不收敛因为姿态要求和平移要求同时压在同一个运动学链上。第二种姿态随动做法是逐点计算弧线上的切线方向把切线方向作为工具的 x 轴或 z 轴再用法向量作为约束补全旋转矩阵。这种做法的特点是末端在圆弧上每个点的姿态都不一样姿态平滑变化适合焊缝跟踪这类需要工具始终对齐焊缝法线的任务。为了不把篇幅拖长这里只给思路对 arc_pts 逐列做一阶差分得到切线方向切线与法向量 n 叉积得到第三个基向量三个基向量组成旋转矩阵再拼成齐次矩阵。需要注意切线的方向要统一否则姿态会在某个点附近突然翻转 180°那也是圆弧规划里很经典的一个坑。4. 机械臂轨迹规划避坑指南SW模型导入、逆解翻车与圆弧方向反了的排查思路4.1 SW模型导入后机械臂尺寸差了1000倍轨迹全飞了现象SW模型导入仿真环境后机械臂看起来异常庞大或者末端轨迹点完全不在模型表面附近轨迹线在空间里飘着。此时不管怎么调整轨迹参数机械臂都碰不到目标位置。原因SW建模时默认长度单位是毫米而 matlab机器人工具箱做运动学计算时一律按米走。模型几何尺寸如果是毫米数值导入后相当于整个机械臂放大了1000倍D-H参数按米设置两者自然对不上轨迹和模型表面相差十万八千里。解决常见做法是导入SW模型文件时先把几何顶点坐标统一除以 1000。例如读取STL网格数据后把 vertices 字段整体除以 1000再重新构造模型显示对象。另一种做法是在SW里导出模型之前把文档单位直接改成米但很多装配体是从元件库拖出来的源文件单位不一定统一改起来容易漏件。我自己的习惯是在导入后统一处理几何顶点不动原始模型文件这样做可重复性好换一台机械臂的SW模型时不会重复踩同样的单位坑。4.2 ikine逆解中途不收敛关节角序列突然乱跳现象直线轨迹和圆弧轨迹的前几十个路径点都正常突然在某一帧报出逆解失败警告或者不报错但关节角序列出现一个很大的突变动画里机械臂像被甩了一下。原因多数情况是目标位姿穿越了机械臂的奇异位形比如腕部中心接近某个关节轴延长线附近雅可比矩阵退化为奇异数值迭代失去稳定。另一个常见原因是逆解的初始猜测值给得离真实解太远迭代收敛到了另一个分支。解决第一步把上一时刻的关节角作为 q0 传入当前位置的逆解这是消除跳变最直接的手段。第二步检查奇异。用雅可比矩阵的条件数做预判在求解循环里对每个路径点计算 jacob0奇异值最小值低于阈值时跳过该点或者调整姿态。第三步检查 mask 设置如果机械臂实际自由度少于6mask 里多余的1必须改成0否则逆解会在不存在的自由度方向上空转误差一直压不下去。4.3 圆弧方向画反了短弧走成了长弧现象三个点定义了一个90度的圆弧但机械臂末端画出来的轨迹是270度的大半圈而且方向是反的。原因圆弧方向判断只用了起点和终点的角度差没有看中间点 P2 的位置。linspace(th1, th3, steps) 默认沿着角度数值增大的方向插值如果真实圆弧需要经过角度减小的方向或者需要跨过±π边界这种写法就会把补弧当成目标弧。解决用中间点 th2 判断方向规则是若 th2 大于 th1 则为逆时针否则为顺时针。跨±π时先对终点角度加或减一整圈再进入 linspace。这个判断逻辑在 3.2 节的代码里已经完整写了。排查圆弧轨迹问题最有效的办法是把生成的三个原始点、圆心和实际轨迹画在同一个三维图里直接看有没有经过 P2。轨迹没有经过 P2 而是绕了另一端基本可以断定是角度方向判断出了问题。4.4 圆弧轨迹末端抖动速度曲线像锯齿现象仿真动画里机械臂在圆弧末端附近来回小幅度晃动关节速度曲线不是平滑过渡而是类似于锯齿的波动末端实际轨迹也不光滑。原因圆弧路径点之间是等角度间隔的圆周上的线速度等于角速度乘以半径等角度间隔不意味着等弧长末端线速度其实是波动的。同时如果逆解容差设置得太大每个路径点上的位姿误差不统一叠加起来就会表现出末端抖动。解决把直线和圆弧路径点进一步细化steps 提高到100以上让相邻点之间角度变化更小。然后把 ikine 的容差参数调小比如 tol 设置到 1e-10 或 1e-12换取更一致的位姿精度。如果抖动集中在末端附近还要检查是否因为终点姿态和路径切线方向不一致工具坐标系在末端被强行扭了一下。5. 轨迹规划的结果怎么验收把理论轨迹和实际轨迹贴在一起看运动学仿真动画只是给人看的真正要确认轨迹能不能用要把理论路径和实际末端路径叠加画在一张图上逐项检查偏差和速度曲线。这个阶段我习惯做三个验证末端路径偏差检查、关节速度连续性检查、逆解成功率统计。第一项是末端路径偏差调用正运动学把关节序列转成末端位置序列再和理论直线或圆弧逐点做差取最大模长。这里建议把偏差值写进报告里因为它是轨迹规划算法精度的直接证据。第二项是关节速度连续性对关节角序列做差分除以时间步长得到近似速度观察曲线有没有明显尖刺。尖刺意味着某一帧的关节角跳变说明逆解在那个点附近发生了分支切换。第三项是逆解成功率也就是收敛帧数占总帧数的比例理想状态是100%只要出现一帧失败整个轨迹就不能直接输出给下位机。我把验证数据和可接受范围汇总成一张表方便写报告时直接抄验证项计算方式参考阈值末端直线/圆弧最大偏差max(vecnorm(实际位置-理论位置)) 1e-3 m关节速度尖刺max(abs(diff(q)/dt)) 出现孤立大值不超过均值的10倍逆解成功率收敛帧数 / 总帧数100%圆弧半径误差实际路径点与圆心距离减理论半径 1e-3 m上述四个数字在轨迹规划报告里是必须出现的比截图更有说服力。我在做圆弧轨迹时还有一个习惯就是不只在动画里看末端走弧线而是把 arc_pts 点集导出来和理论圆弧直接对比这样能确认几何计算环节无误后续逆解导致的偏差才能单独分离出来排查。做机械臂轨迹规划最怕的就是几何计算和运动学求解混在一起出问题分不清是哪一个环节导致末端偏移分步验证是这类问题最好的后悔药。我早期做第一个直线轨迹Demo时完全没做偏差验证直接上仿真模型结果机械臂末端在终点附近抖得厉害还以为逆解算法有问题查了很久才发现是关节空间和任务空间混用了。后来养成习惯无论直线还是圆弧先画理论轨迹再把实际轨迹叠上去两个轨迹贴在一起才有信心继续往下做。做轨迹规划很多时候不是公式拦住人而是验证顺序没做对。希望帮到你。本文还有配套的精品资源点击获取