MATLAB机械臂轨迹规划：空间选型、避坑与落地实践

简介面向机械臂/机器人轨迹规划学习者尤其是需要借助 MATLAB 完成运动控制仿真的学生与工程师这份压缩包提供了一套可直接运行的示例与工具。包内共 2 个文件包含一个 .mltbx 机器人工具箱安装包和一个 .m 示例脚本整体大小约 16.57MB便于下载后快速部署仿真环境。内容围绕五次多项式轨迹规划展开同时涉及关节空间、笛卡尔空间及样条曲线规划等常见方法通过工具箱函数完成多项式系数计算与位置、速度、加速度序列生成能够帮助理解平滑连续轨迹的构造过程并对比不同规划方式的特点。资源提供实用脚本与工具箱适合正在学习机器人学、准备课程设计或入门轨迹规划的读者直接参考运行。已有 7738 人浏览学习说明其在相关人群中具有较高的实用参考价值。1. 机械臂轨迹规划的MATLAB落地先搞清楚能做什么别急着写代码拿到机械臂型号的第一天很多人会直接打开MATLAB开始抄traj代码结果发现末端走的轨迹完全不是想要的弧线或者干脆报错说矩阵维度不一致。这类场景我见过太多次了。机械臂轨迹规划在MATLAB里做的事本质上就是把「末端要去哪儿、怎么去」翻译成「每个关节在每个时刻该转多少度」——听起来简单但关节空间插值和笛卡尔空间插值出来的运动形态完全不同选错空间、忽略奇异性、没设置好时间向量都会让轨迹变成废纸。这篇内容面向三类人在MATLAB里做毕设或课题研究的在校生需要快速验证运动方案是否可行的产线工程师以及刚转行做机器人应用开发、被售后现场追问「为什么轨迹抖动」的调试人员。你能从中得到的核心收获是三件事第一搞清楚两种轨迹规划空间的适用边界不盲目套函数第二拿到一份能直接改参数跑通的最小实现从DH建模到可视化一步不差第三提前知道五个高频坑点省掉自己熬夜排查的时间。我的建议很直接先看完第2章的选型逻辑再动手装工具箱。不要跳过原理直接抄代码否则后面排查的时间会翻倍。2. 关节空间还是笛卡尔空间选型直接决定代码怎么写2.1 两种空间的定义与适用场景轨迹规划的第一个岔路口是你在哪个空间里描述这条轨迹。关节空间规划描述的是每个关节角度随时间的变化曲线。它的优势是计算量小、天然避开奇异位形因为规划过程根本不知道笛卡尔空间里发生了什么。但它有个致命的直观性问题末端在两点之间走的路径是弯曲的中间会不会撞到东西完全不可控。适合的场景是点对点搬运、上下料这类只关心起点和终点、不关心中间路径的工作。笛卡尔空间规划描述的是末端执行器在X、Y、Z坐标下的直线或圆弧运动。它直观、路径可控但计算量上了一个量级而且必须在每个插值点做逆运动学解算。机械臂在接近奇异位形时笛卡尔规划会让关节速度飙到离谱的值这是现场抖动和报警的直接来源。我一般这样判断如果工艺要求末端走直线比如涂胶、焊接、点焊必须上笛卡尔空间如果只是从A点抓取放到B点用关节空间就够了省事且不易出幺蛾子。两种空间在MATLAB里的实现路径也不一样。关节空间通常用jtraj或者自写的梯形速度曲线笛卡尔空间用ctraj或者自己用linspace做直线插值后再逐点求逆解。很多人混用这两个函数折腾半天报错本质是对空间概念没拎清。选型时还有个容易被忽略的维度轨迹上有没有必须精确经过的中间点。如果有明确的路点序列要做连续轨迹跟踪比如门字型轨迹那就乖乖用笛卡尔空间配合多点对接关节空间一次jtraj最多只能保证首尾两点精确中间全凭机械臂自己发挥。2.2 Robotics Toolbox的安装与版本坑在MATLAB里做机械臂轨迹规划绕不开Robotics Toolbox。这个工具箱不是MathWorks官方出的而是个人维护的开源项目这暗示了一个重要事实新版本的MATLAB兼容性不一定好装完可能因为缺少编译环境或者依赖库版本冲突而直接启动失败。安装方式最常见的有两种。一种是直接从工具箱官网下载安装包然后在MATLAB里把路径添加进去。另一种是有些发行版会提供tb_install安装脚本如果你用的是较新的MATLAB版本可能还需要安装额外的编译器支持。无论哪种方式装完后的第一件事都是运行ver检查版本是否被正确识别或者尝试例程来确认函数是否全部可用。安装完成后列出的示例工具箱名称不同版本不一致例如有的叫Robotics有的叫Peter Corke Robotics Toolbox此为工具箱常见称谓仅为说明版本差异。我在多个不同的MATLAB版本上都遇到过问题某些极其依赖旧版绘图接口的例程会在新版本里出现图形窗口没有响应的情况而这个报错往往不是你的代码问题单纯就是工具箱版本和MATLAB版本不匹配。遇到这类问题我的处理习惯很固定确认MATLAB大版本然后去工具箱发布历史里找对应维护时间接近的版本如果还报错就把报错信息里涉及的具体函数名拿到文档里查废弃标记。这个工具箱更新频率不快但每次MATLAB大版本升级后它基本都会出适配版本延迟安装比急着上线更靠谱。2.3 建立机械臂模型的最小代码不管你在哪个空间做规划第一步都是把你的机械臂告诉MATLAB。最常用的方式是改进型DHModified Denavit-Hartenberg参数。各厂家手册里给出的DH参数表格式虽然大同小异但你需要特别注意是标准型Standard DH还是改进型Modified DH同一个物理关节在这两种约定下的参数值很可能正负号都不一样。下面是一个六轴关节机械臂的最小建模代码建的是一台虚构六轴臂模型模拟项目X参数用MDH表示。% 机械臂DH参数定义MDH格式 % 每个关节一行: theta偏移, d偏置, a杆长, alpha扭转角 L1 Link(d, 0.333, a, 0, alpha, 0); L2 Link(d, 0, a, 0.649, alpha, -pi/2); L3 Link(d, 0, a, 0, alpha, pi/2); L4 Link(d, 0.106, a, 0, alpha, -pi/2); L5 Link(d, 0.106, a, 0, alpha, pi/2); L6 Link(d, 0.104, a, 0, alpha, 0); robot SerialLink([L1 L2 L3 L4 L5 L6], name, simArm6);这段代码看起来短但每一行都在定义机器人的几何形状。d表示相邻关节坐标系沿Z轴的偏移a是沿X轴的杆长alpha是绕X轴的扭转角。关节角qlim关节限位在建模时如果不设置后面做逆解或者轨迹规划时不会主动帮你检查越界这是很多人在仿真里过关、一上真机就撞限位的原因之一。% 给每个关节设置限位单位: 弧度 L1.qlim [-pi, pi]; L2.qlim [-2.5, 2.5]; L3.qlim [-2.0, 2.0]; L4.qlim [-3.0, 3.0]; L5.qlim [-2.5, 2.5]; L6.qlim [-3.0, 3.0];建模完成后可以用robot.teach()打开交互界面拖动滑块看末端坐标变化这是最快验证DH参数有没有抄错的方法。如果拖动某个关节时末端运动方向跟你预期明显不一致基本可以断定是alpha的正负号写反了。参数设置的另外一个常见误区是把角度单位搞混。MATLAB的Robotics Toolbox约定所有角度都用弧度但真实机械臂的示教器显示的一般都是角度。如果你从示教器上抄了几个关节角回来做逆解验证记得乘以pi/180否则算出来的位姿和实际天差地别。这个问题发生的频率高到我在自己的代码模板里固定写一行deg2rad的转换语句养成肌肉记忆。3. 把两点之间的运动算出来jtraj、ctraj与梯形速度规划3.1 jtraj关节空间插值参数与代码关节空间规划在MATLAB里最常用的函数就是jtraj。它生成从起始关节向量到目标关节向量的平滑轨迹默认采用七次多项式插值保证角度、角速度、角加速度都连续。这比简单的线性插值多了加速度连续性末端执行器在启停时不会出现冲击。直接上代码从当前关节位置走到目标关节位置走2秒插值50个点。% 定义起始和目标关节角单位: 弧度 q0 [0, -pi/4, pi/3, 0, pi/6, 0]; q1 [pi/6, pi/4, -pi/6, pi/12, 0, pi/3]; t linspace(0, 2, 50); % 0到2秒共50个插值点 T jtraj(q0, q1, t); % T是一个50x6的矩阵每行一组关节角每一行对应一个时刻的六个关节角度。使用时把T按行喂给正运动学函数robot.fkine(T(i, :))就能得到每个插值点的末端位姿。linspace里的点数决定了轨迹的平滑度点数太少的话轨迹看起来会是一段段折线点数太多计算会慢一些但现代电脑上50到200之间体感没有区别。这里有一个进阶技巧jtraj默认按整个时间段均匀分配速度但如果你希望起点和终点速度为零、中间匀速段更长直接用jtraj是做不到的。你需要用它的低级实现版本或者自己在关节空间做梯形速度规划。我用jtraj时踩过的最实在的坑是如果起始位姿和目标位姿的关节角差值很大比如某个关节要转两圈以上默认插值会把这段行程均匀塞进整个时间段里导致某些关节速度逼近极限。所以在正式生成轨迹前我习惯先算一下每个关节的角增量再估算一下最大速度需求是否在关节能力范围内。% 查看每个关节的转角差 delta_q q1 - q0; % 按2秒算粗略估计关节平均速度 avg_speed abs(delta_q) / 2;3.2 ctraj笛卡尔空间插值什么时候用它ctraj做的是笛卡尔空间的直线插值。它接收两个齐次变换矩阵作为起始位姿和目标位姿输出一串中间位姿矩阵。配合逆解函数ikine就能得到笛卡尔直线运动对应的关节角序列。% 获取起始和目标位姿矩阵 T0 robot.fkine(q0); T1 robot.fkine(q1); t linspace(0, 2, 50); Tc ctraj(T0, T1, t); % Tc是50x4x4的三维数组每个切片一个位姿 % 逐点求逆解得到关节轨迹 q_route zeros(50, 6); for i 1:50 q_route(i, :) robot.ikine(Tc(:, :, i), mask, [1 1 1 1 1 1]); end注意ikine需要合理的初始猜测值不然后期的插值点可能收敛到错误的关节构型。常见做法是以上一个插值点的解作为下一个点的初始值这通常能保证连续性。代码里如果直接对每个插值点从零开始求解一旦机械臂有对称结构就很容易出现前后两个点解出的关节角构型差了好大一圈的情况在仿真里表现为关节角突变在真机上就是关节反跳。mask参数在这里值得多说一句。默认情况下ikine会尝试同时满足位置和姿态六个约束。但如果你的任务只关心末端位置比如喷涂、搬运就可以把姿态约束关掉减少求解失败的几率。比如喷漆机械臂的末端姿态在多数工艺段不需要严格恒定此时用mask只约束位置三个自由度轨迹成功率会高很多。% 只位置约束姿态不做要求 q_route(i, :) robot.ikine(Tc(:, :, i), mask, [1 1 1 0 0 0]);如果遇到逆解失败的情况ikine返回空或者抛出警告第一步要确认目标点是否在机械臂的工作空间内。画出工作空间最直观——让所有关节在不同限位值之间随机取值正解得到末端点云看目标点是否落在这个点云包络内部。3.3 从点到路径多段轨迹拼接的关键设计真实工艺几乎没有一段直线从头走到尾的情况都是多段轨迹拼接上升 → 平移 → 下降 → 作业 → 抬升 → 返回。每段轨迹单独规划后拼接起来需要在衔接点做过渡处理否则关节速度方向突变产生的冲击会直接影响工艺质量和机械寿命。最常见的做法是每段轨迹末尾加一个减速段或者采用圆弧过渡。MATLAB里没有现成的多段轨迹拼接函数需要自己写。我的通用方案是把多段轨迹的点列拼接成一个大的时间序列但每段之间在时间轴上留出重叠区间用线性插值平滑速度。更简单的落地做法是把每段轨迹的插值点数加多一些让最后几个点和下一段的开头几个点在空间位置上尽量接近再对整条关节轨迹做一次smoothdata滤波。不过这种方法在轨迹动力学要求严格的场合下不够精确只能用于运动学验证阶段。真正要上产线的话我建议在路径点之间使用梯形速度规划的变体——在每段轨迹的末尾留一个速度下沉到零的停顿点然后再启动下一段。这样子虽然牺牲了节拍但换来了确定性调试阶段不容易翻车。% 梯形速度规划示例: 将总时间分成加速、匀速、减速三段 % 给定最大速度和加速度反推各段时间 function [time_vec, q_traj] trapezoid_traj(q0, q1, v_max, acc) dq abs(q1 - q0); % 加速和减速段距离 d_acc v_max^2 / (2 * acc); if d_acc dq / 2 % 能达到最大速度三段式 t_acc v_max / acc; t_const (dq - v_max^2 / acc) / v_max; t_total 2 * t_acc t_const; else % 距离太短达不到最大速度只有加速和减速 t_acc sqrt(dq / acc); t_total 2 * t_acc; end % 按t_total生成时间向量并插值 time_vec linspace(0, t_total, 100); q_traj zeros(100, length(q0)); for i 1:length(q0) q_traj(:, i) interp1([0 t_acc t_total-t_acc t_total], ... [q0(i) q0(i)sign(q1(i)-q0(i))*acc*t_acc^2/2 ... q1(i)-sign(q1(i)-q0(i))*acc*t_acc^2/2 q1(i)], ... time_vec, linear); end end这个手写版本是简化的真正的梯形规划还需要考虑多关节同步问题——每个关节的最大速度不同整条轨迹的实际总时间取决于最慢的那个关节。这也是为什么很多工程师直接用jtraj而不是自己造轮子因为jtraj自动处理了多关节同步。4. MATLAB里的可视化验证别只会plot还要看TCP轨迹4.1 plot和plot3的用法与局限拿到规划的关节角序列后第一件事就是可视化。最直接的是robot.plot(T)它会画出机械臂在每个插值点的姿态动态展示运动过程。但plot有个隐藏的限制它只展示关节角的运动不会直接给你看末端执行器在三维空间里画出的轨迹形状。想看TCPTool Center Point工具中心点轨迹需要自己算末端的笛卡尔坐标。把关节角序列逐行做正解取出平移分量再用plot3画出空间曲线这样能直观看到末端走的到底是直线还是曲线。% 逐点计算末端位置 pos_list zeros(size(T, 1), 3); for i 1:size(T, 1) T_i robot.fkine(T(i, :)); pos_list(i, :) T_i.t; % 齐次矩阵的平移分量就是末端坐标 end figure; plot3(pos_list(:, 1), pos_list(:, 2), pos_list(:, 3), b-, LineWidth, 2); xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); grid on; axis equal;很多人做完这一步就以为万事大吉了但我建议你再做一步把期望轨迹和实际轨迹画在同一个图里对比。期望轨迹是ctraj生成的直线插值坐标实际轨迹是经过逆解再正解回来的坐标。两者之间的偏差反映了逆解数值误差和插值密度带来的影响。如果偏差超过几个毫米就需要加密插值点或者优化逆解初值。4.2 记录末端轨迹并验证连续性验证轨迹连续性看得不是位置——因为位置在仿真里几乎总是连续的——而是速度和加速度。速度是位置对时间的一阶差分加速度是二阶差分。一旦速度曲线出现锯齿或跳变意味着实际的关节运动存在冲击上真机后机构会振动。% 计算末端速度有限差分 vel diff(pos_list) ./ diff(t; % t是时间向量 % 计算速度模长 speed sqrt(sum(vel.^2, 2)); figure; plot(speed); xlabel(插值点序号); ylabel(末端速度 (m/s));理想情况下速度曲线应该是平滑的山丘形。如果出现多个峰值或者在末端附近仍然有较高速度值说明减速段设置不合理或者直接用jtraj在时间上都匀了但空间上路径长度差异大导致的。这里有一个容易被忽略的技术点plot展示的机械臂三维动画和plot3画的末端轨迹可能存在不一致。原因在于robot.plot(T)默认每个插值点的间隔是均匀时间而如果机器人模型比较复杂动画刷新可能丢帧导致视觉上看起来运动不匀速。视觉上的不匀速不等于轨迹有问题以plot3和速度曲线为准不要信动画的观感。4.3 在仿真里检查关节速度与加速度限位机械臂每个关节都有最大速度和最大加速度指标这些指标在数据手册里有。仿真里检查关节速度是否超限跟检查末端速度一样用有限差分只不过对象是关节角序列T。% 关节角速度 q_vel diff(T) ./ diff(t); % 关节角加速度 q_acc diff(q_vel) ./ diff(t); % 对比限位 max_vel [2.0, 2.0, 2.0, 3.0, 3.0, 3.0]; % 单位: rad/s max_acc [5.0, 5.0, 5.0, 8.0, 8.0, 8.0]; % 单位: rad/s^2 for j 1:6 if max(abs(q_vel(:, j))) max_vel(j) fprintf(关节%d速度超限: %.3f rad/s\n, j, max(abs(q_vel(:, j)))); end if max(abs(q_acc(:, j))) max_acc(j) fprintf(关节%d加速度超限: %.3f rad/s^2\n, j, max(abs(q_acc(:, j)))); end end这段检查代码在仿真阶段就能提前暴露问题不用等上真机报警。超限的处理方式有两种拉长时间降低速度需求或者重新规划路径让关节运动分配更平均。如果不想拉长生产节拍就得回到路径规划层面重新设计路点让负载大的关节少走一些路程。5. 轨迹规划避坑清单5个我踩过且常见的问题5.1 奇异性附近的笛卡尔规划导致关节速度爆炸现象笛卡尔空间直线轨迹走到某个位置附近时仿真里的关节速度曲线突然出现一个极高的尖峰有时数值到了几十弧度每秒。原因机械臂在这个位形下某几个关节的轴线接近共线或共面雅可比矩阵近似奇异。为了让末端保持直线运动微小的末端位移需要关节产生极大的转角变化。这就是所谓的奇异位形。解决最可靠的办法是调整路径点让直线轨迹绕开奇异区域。如果你把起点和终点连线画出来发现连线穿过了奇异点附近那就把中间路点偏移一些哪怕末端走的是折线或弧线也比速度爆炸强。在MATLAB里可以用jacob0计算雅可比矩阵的条件数条件数过大的路点就是要绕开的区域。条件数大于某个阈值比如100时就应该对该段轨迹重新规划。5.2 DH参数正负号搞反导致轨迹镜像现象仿真里机械臂运动方向与示教器完全相反比如你让它往X正方向走它实际往X负方向走。更隐蔽的是只有某个关节的运动方向不对。原因DH参数里的alpha或者theta正负号搞反了。不同厂家的DH表格标注习惯不一样有些给的是标准DH有些给的是改进DH。你在建模时没有核对坐标系约定直接照抄了数值。解决建模完成后逐个关节做单独运动的测试。关节1动一下看末端沿哪个轴移动方向对不对。每个关节都单独验证一遍确认全部正确后再做轨迹规划。这个检查过程只花几分钟但能省下后期排查的好几个小时。如果反了改alpha的正负号而不是改theta因为theta是变量改它的正负号会影响后续所有规划代码。5.3 时间向量与插值点数不匹配的隐藏问题现象代码不报错但是算出来的轨迹速度曲线是乱跳的或者末端位置在某个插值点突然跳变回起点。原因linspace生成时间向量时插值点数我设成了N但后面某个地方又用length()取到了不同长度的数组去对应。更常见的是diff计算的差分向量长度比原向量少了一个元素如果直接除以原时间向量会报维度错误如果不注意直接按原长度索引就会取错数据。解决每次做差分计算时明确这个结果的长度是N-1。然后自己构造一个新的时间中点向量t_mid (t(1:end-1) t(2:end)) / 2用这个中点向量去匹配速度数据。这样维度一一对应曲线也不会错位。5.4 逆解函数卡在局部极小值导致关节突变现象某段轨迹的其他点都正常就中间一两个点的关节角突然跳变到完全不同的数值在plot动画里表现为机械臂瞬间闪了一下。原因ikine默认使用迭代数值求解初始猜测值如果离正确的解太远就会收敛到局部极小值。特别是相邻插值点之间关节角变化比较大的时候以上一个点的解作为初值的策略有时会失效。解决给ikine传初始猜测值。在求解第 i 个插值点时使用第 i-1 个点的解通常能保证连续性。如果还不行就把相邻插值点间距减小——加密插值让每次逆解的搜索范围缩小。加密插值点对计算时间的影响可以忽略但对稳定性的提升是立竿见影的。5.5 工具坐标系没有设置导致TCP轨迹偏移现象名义上末端走的是一条直线实际仿真出来是一条斜线。或者说末端位置和预期差了固定的一段距离和角度。原因规划时用的末端坐标系是法兰盘中心但你的工艺工具比如焊枪、吸盘、喷头有长度实际的工具中心点和法兰中心不是同一个点。没有设置工具坐标变换所有轨迹都是以法兰中心为基准规划偏差值等于工具的长度和安装角度。解决在建模时定义工具偏移。常见做法是在最后一个连杆的tool参数上加上平移和旋转。如果工具是一个长度为150毫米的直杆沿Z方向安装代码这样写robot.tool transl(0, 0, 0.15);这一步做完所有的正解、逆解、轨迹规划都会自动把工具长度算进去。很多初学者忘了这一步然后在真机上出现末端位置偏差回头排查半天最后发现是工具坐标系的事。这属于常规低级错误但在项目现场发生的概率一点都不低。6. 进阶玩法把MATLAB轨迹规划的价值延伸到真机落地6.1 把轨迹点导出成机器人程序可读的坐标序列仿真验证做完后下一步通常是导数据给真机。实际生产中很多控制器支持点位列表导入你只需要把插值点导出来按控制器的格式要求整理成点位表。这个过程不复杂但格式转换最容易出错。导出的核心是把关节角序列T保存成CSV或者TXT注意精度控制。关节角导出时我习惯保留6位小数对应微弧度级别因为四舍五入到4位小数可能在串联机构末端放大出几毫米的位置误差。% 导出关节轨迹到CSV % 表头: t, q1, q2, q3, q4, q5, q6 data_out [t, T]; writematrix(data_out, traj_points.csv); % 如果控制器要求角度制先转再导出 data_out_deg [t, T * 180/pi]; writematrix(data_out_deg, traj_points_deg.csv);如果目标控制器支持离线编程脚本你可以把每段轨迹的起点和终点角度提取出来直接生成对应的方法调用序列。这样从仿真到真机程序的路径就通了。导出的点位表在导入前建议在控制器的仿真模式里运行一遍确认没有超出各关节行程再切自动运行。6.2 用MATLAB Coder生成C代码的边界在哪里如果你想把整个轨迹规划算法嵌入到控制器里比如做实时规划MATLAB Coder可以把函数转成C代码。但这有个边界Robotics Toolbox的很多函数不带Coder支持直接转基本失败或者生成的代码无法编译。我实践过的可行路径是把纯数学计算的部分比如自有写的梯形速度规划、插值算法用Coder转换工具箱相关的部分比如fkine、ikine留在MATLAB里仿真用真机上用别的语言重写或调用现成的运动学库。这样就避开了Coder对工具箱函数的支持盲区。% 只对梯形规划函数生成C代码 codegen trapezoid_traj -args {coder.newtype(double,[1,6]), ... coder.newtype(double,[1,6]), 1.0, 1.0}参数里的coder.newtype是为了告诉Coder输入参数的形状。生成的C代码不含任何动态内存分配可以直接嵌入到实时控制循环里。但注意生成的C函数只有数值计算逻辑没有错误处理你得自己在外面包一层参数合法性检查不然输入越界的关节角会让算法静默输出错误结果。6.3 直线加圆弧混合路径的分段规划最后讲一个实战中一定会遇到的进阶需求末端走一条由直线段和圆弧段拼接的路径比如门字轨迹的圆角过渡。方案是做分段规划直线段用笛卡尔直线插值圆弧段用参数方程生成圆弧插值点两段在衔接点处用一段极短的三次样条过渡。在MATLAB里实现的简化版是定义一个包含直线终点和圆弧起点的重叠区间在这个区间内用smoothdata或者自写样条函数平滑过渡。圆弧插值的实现并不复杂关键在于把圆弧定义在正确的平面上。常见做法是取三个不在一条直线上的路点来拟合一段圆弧这段圆弧的圆心、半径、法向量都可以通过三点计算得到相关推导可作为练习验证。% 圆弧插值示例: 给定起点, 中间点, 终点计算圆弧参数 % 简化实现假设圆弧在XY平面内 p0 [0.5, 0.3, 0]; p1 [0.6, 0.5, 0]; p2 [0.4, 0.6, 0]; % 计算圆心数值方法后续可优化为解析解 th linspace(0, 1, 50); arc_pts zeros(50, 3); for i 1:50 t th(i); % 三点圆弧的插值方式有很多种最简单的按角度参数化 angle atan2(p1(2)-p0(2), p1(1)-p0(1)) ... t * (atan2(p2(2)-p1(2), p2(1)-p1(1)) - ... atan2(p1(2)-p0(2), p1(1)-p0(1))); radius norm(p1 - p0); arc_pts(i, :) [p0(1) radius*cos(angle), p0(2) radius*sin(angle), 0]; end上面这段代码是示意实际圆弧规划需要注意圆弧的凹凸方向和过采样角度的连续性否则在角度跨越180度边界时会出现跳变。原则上用atan2计算出来连续角度差后要做一个unwrap操作避免角度回绕。这一点处理不好圆弧末端就直接飞了。我在现场调试时养成的习惯是无论仿真里看起来多顺滑上真机前都做一次全速空跑并把手放在急停上。真机的摩擦、惯量、控制器插补周期都会让理论轨迹发生偏移仿真通过只是第一步实测校准才是闭环。希望这些经验对你有帮助。本文还有配套的精品资源点击获取