基于Matlab的四自由度机械臂轨迹规划仿真与实现

简介基于Matlab实现的四自由度机械臂轨迹规划仿真源码与配套说明面向机器人、自动化、电子信息等专业学习者可作为课程设计、毕业设计或课外拓展的参考资料。压缩包共14个文件主要包含12个.m源码文件、1个txt内容介绍和1个stp三维模型文件整体大小仅998KB方便快速下载与运行。源码覆盖机械臂正逆运动学求解、直线与圆弧插补、三次及五次多项式轨迹规划、工作空间可视化等功能模块内容介绍文件对资源组成和代码用途进行了概要说明能帮助读者理解各脚本的调用关系与参数含义。目前已有562人学习下载。对于想动手实践四自由度机械臂轨迹规划的读者这份资料提供了从运动学建模到轨迹生成的完整参考流程可在其基础上修改参数、增加避障或优化算法适合具备一定Matlab和机器人学基础并希望独立调试代码的学习者。1. 四自由度机械臂轨迹规划为什么我建议先用Matlab把仿真跑起来拿到一套机械臂源码直接上实物调试十有八九会翻车。四自由度机械臂虽然比六自由度少了两个关节但轨迹规划涉及的正逆解、插补、速度平滑、工作空间验证一个都省不掉。如果一开始就把算法逻辑跑在Matlab里你可以在没有硬件的情况下看清每个关节角是怎么变化的也能把直线轨迹、圆弧轨迹的插补误差控制在毫米级甚至更高精度。这套基于Matlab的四自由度机械臂轨迹规划仿真资源包含myfkine.m正运动学、myikine.m和myikine2.m两组逆解、cubic_traj.m和quintic_traj.m多项式轨迹、line_traj.m和arc_traj.m空间插补以及test1.m到test4.m的完整仿真脚本适合刚接触机械臂控制的初学者也适合想把轨迹规划算法快速落地的工程师。先说明一点源码的价值在于参考思路不是拿来就复制粘贴你需要能读懂每段代码背后的数学假设。2. 正运动学与逆运动学从DOBOT模型到myfkine和myikine的实现2.1 DOBOT Magician的结构与DH参数建模资源里附带的DOBOT_Magician-mit-TOOLS_V3DS-190722.stp是三维模型文件用SolidWorks或FreeCAD打开后可以测量各连杆长度。DOBOT Magician是典型的四自由度串联机械臂前三个关节决定末端位置第四个关节决定末端姿态实际构型上最后两个关节轴线平行所以可以简化为四个转动关节的连杆模型。做轨迹规划前第一步必须是建立DH参数表否则后面所有代码都是空中楼阁。常见做法是采用改进DHModified DH建模对资源里的四自由度构型关节角分别记为θ1、θ2、θ3、θ4连杆偏距和杆长从模型实测得到。这里给出一个典型的DH参数表注意实际使用时要把单位统一成米和弧度关节 ia(i-1) / malpha(i-1) / radd(i) / mtheta(i)1000.104θ120.070-pi/20θ230.13500θ340.11200.081θ4这组参数里a(1)是底座到第一关节的水平偏心d(1)是底座高度。你打开test1.m会看到对机械臂对象的初始化其实就是在给这些参数赋值。要注意的是如果模型里末端执行器还带吸盘或夹爪实际杆长会比这个表略大正逆解结果也会差几毫米仿真时可以先忽略但标定时必须重新测量。2.2 myfkine.m中的齐次变换矩阵实现正运动学就是把关节角映射到末端位姿本质上是四个齐次变换矩阵连乘。myfkine.m里一般会用一个循环或者直接展开矩阵乘法我这里给你展示核心逻辑function T myfkine(theta) % theta: [theta1 theta2 theta3 theta4] a [0, 0.070, 0.135, 0.112]; alpha [0, -pi/2, 0, 0]; d [0.104, 0, 0, 0.081]; T eye(4); for i 1:4 % 改进DH的单步变换矩阵 A [cos(theta(i)), -sin(theta(i))*cos(alpha(i)), sin(theta(i))*sin(alpha(i)), a(i)*cos(theta(i)); sin(theta(i)), cos(theta(i))*cos(alpha(i)), -cos(theta(i))*sin(alpha(i)), a(i)*sin(theta(i)); 0, sin(alpha(i)), cos(alpha(i)), d(i); 0, 0, 0, 1]; T T * A; end end这里的A就是相邻坐标系之间的变换矩阵每一行从左到右依次是旋转矩阵的三列和平移向量。循环从底座开始每乘一次就变换到下一个关节坐标系最终得到的T矩阵左上角3×3是末端姿态第4列前三个元素就是末端位置。调试时建议把theta全设为0然后直接看T(1:3,4)如果得到的x、y、z和你在SolidWorks里测到的初始位置一致说明DH参数没写错。2.3 myikine.m与myikine2.m解析解与数值解的取舍逆运动学比正运动学麻烦四自由度机械臂虽然构型简单但末端姿态约束加上关节限位很容易出现多解或者无解。资源里给了两个逆解文件myikine.m通常走解析解路线而myikine2.m是数值迭代解法。解析解的思路是先通过末端位置解出前三个关节角再用姿态偏差解出第四个关节角。以平面三连杆加腕部旋转为例位置逆解过程可以借助几何法取前三个关节在一个平面内那么θ2和θ3可以通过余弦定理求出function theta myikine(p, R, L1, L2) x p(1); y p(2); z p(3); % 先求基座到腕部平面的距离 r sqrt(x^2 y^2); % 关节1直接由反正切得到 theta1 atan2(y, x); % 简化假设腕部在z平面内利用几何关系求θ2和θ3 % 这里省略了肩部偏置的补偿项 cos3 (r^2 (z - L1)^2 - L2^2) / (2 * L1 * L2); % 防止超出[-1,1]范围 cos3 max(min(cos3, 1), -1); theta3 acos(cos3); theta2 atan2(z - L1, r) - atan2(L2*sin(theta3), L1 L2*cos(theta3)); theta4 0; % 由姿态残差补充 theta [theta1 theta2 theta3 theta4]; end这是一个被大幅简化的演示版本实际上DOBOT的肘部结构和基座偏置会让θ1的计算带上补偿量你不能直接把这段代码用在自己的模型上但能看出解析解的基本套路。我的经验是先用myikine.m算一遍初始解再用myikine2.m做迭代细化两段代码互相校验。数值解法里最常用的是Jacobian转置法或者阻尼最小二乘法用当前正解和期望位姿之间的偏差反推关节角增量迭代步长要设得小一些否则容易出现关节角跳变。调试时关注输出theta是否在关节限位内如果解出来的θ2超过±150度说明目标点已经超出了可达范围不是算法错了是路径规划本身不合理。3. 轨迹规划核心cubic_traj.m与quintic_traj.m的边界条件与平滑性3.1 多项式轨迹为什么是机械臂的基础轨迹规划要解决的是“关节怎么从当前角度平滑移动到目标角度”的问题直接给阶跃信号会让机械臂产生巨大冲击所以必须让角度、角速度甚至角加速度满足连续性要求。三次多项式轨迹只需要满足起点和终点的角度与角速度四个约束五次多项式轨迹再加两个角加速度约束。cubic_traj.m和quintic_traj.m分别对应这两种做法它们在速度曲线形态上有明显差异。三次多项式的角速度是抛物线角加速度是直线起止时刻加速度不连续会在电机端产生较大的加速度跳变五次多项式的加速度是抛物线起止时刻加速度从0开始运行过程更平滑但计算量和执行时间也会稍长。实际选型时要看你的执行器如果只是普通舵机或者步进电机三次多项式配合梯形速度规划也够用如果是总线舵机或者伺服电机建议直接上五次多项式。热搜里经常有人问“总线舵机机械臂怎么规划”核心就在这个平滑性上总线舵机可以接收高频率位置指令但你没有平滑的位置序列再好的舵机也会抖动。3.2 cubic_traj和quintic_traj的Matlab实现与参数含义下面是cubic_traj.m里常见的核心代码它接收起始角度、终止角度、起始角速度、终止角速度和总运行时间返回一个从0到1归一化的时间参数s然后再映射到实际角度function [theta, sd, sdd] cubic_traj(theta0, theta1, dtheta0, dtheta1, T, dt) % 构建时间序列 t 0:dt:T; % 三次多项式系数: a0 a1*t a2*t^2 a3*t^3 % 边界条件矩阵求解常被直接写为解析解 a0 theta0; a1 dtheta0; a2 (3*(theta1 - theta0)/T^2) - (2*dtheta0 dtheta1)/T; a3 (2*(theta0 - theta1)/T^3) (dtheta0 dtheta1)/T^2; theta a0 a1*t a2*t.^2 a3*t.^3; sd a1 2*a2*t 3*a3*t.^2; % 角速度 sdd 2*a2 6*a3*t; % 角加速度 end参数说明theta0和theta1是起止关节角单位弧度dtheta0和dtheta1是起止角速度在点到点运动里通常都设为0T是总运动时间不要设得太小因为关节电机有最大角速度限制如果算出来的速度峰值超过电机额定值就得把T拉大dt是仿真步长一般取0.01秒既能保证曲线平滑也不会让t向量过于臃肿。同理quintic_traj.m会在开头多定义一个起始和终止角加速度ddtheta然后多解两个系数最终得到的加速度曲线就没有阶跃点。我在实际调试时习惯先设置T2秒dt0.01把theta画出来看形状如果角速度峰值明显低于电机限幅再把T缩短到1.5秒反复对比找到最短可执行时间。不要只盯着角度曲线看角速度曲线才是判断平滑性的关键。3.3 两组轨迹的对比测试与选择建议这里给你一个可以直接在test2.m里运行的对比脚本theta0 0; theta1 pi/2; T 2; dt 0.01; [cubic_theta, cubic_sd] cubic_traj(theta0, theta1, 0, 0, T, dt); [quintic_theta, quintic_sd, quintic_sdd] quintic_traj(theta0, theta1, 0, 0, 0, 0, T, dt); figure; subplot(2,1,1); plot(0:dt:T, cubic_theta, b, 0:dt:T, quintic_theta, r--); legend(cubic, quintic); ylabel(angle / rad); subplot(2,1,2); plot(0:dt:T, cubic_sd, b, 0:dt:T, quintic_sd, r--); legend(cubic, quintic); ylabel(angular velocity / rad/s);运行后你会看到两条角度曲线几乎重合但角速度曲线差异明显三次多项式的角速度从0迅速拉升到最大值结束前又突然拉回0五次多项式的角速度变化更柔和。由此可以得出结论如果机械臂末端负载不大运行速度也不高cubic_traj就够如果末端有精度要求或者轨迹需要连续经过多个路径点建议使用quintic_traj因为它能让加速度连续减少机构共振。资源里test3.m应该展示了更复杂的组合轨迹你在看源码时多留意它到底调用了哪套轨迹函数。4. 直线与圆弧轨迹line_traj.m和arc_traj.m的插补逻辑4.1 关节空间轨迹不适用于末端走直线前面说的三次、五次多项式轨迹都是在关节空间里规划的末端在笛卡尔空间里走的是一条曲线。如果要让机械臂的末端沿空间直线移动比如在桌面画一条直线就必须在笛卡尔空间做插补再把每个插补点通过逆解转换到关节角。这也是line_traj.m存在的意义。它的基本思想是线性插值但需要注意姿态也需要插值不能只对位置线性插值而让姿态突变。line_traj.m的核心思路通常是这样function pos line_traj(p_start, p_end, T, dt) t 0:dt:T; % 空间直线插补 ratio t / T; pos p_start (p_end - p_start) .* ratio; end但直接套用这个代码会发现轨迹起止速度不连续机械臂在启动和停止瞬间会有冲击。因此更完整的实现里会对ratio先做平滑处理比如用五次多项式把比率从0变到1这样末端在直线路径上也能做到加减速平滑。这里有一个很容易踩的坑如果直线插补N个点每个点都单独调用一次myikine.m而逆解又不唯一前后两帧可能解出完全不同的关节角组合导致机械臂在直线中途突然翻转。我的做法是在逆解时把上一帧的解作为数值迭代的初值或者对解析解加入关节角度连续性约束。4.2 圆弧轨迹插补的平面投影与角度递增策略arc_traj.m是圆弧轨迹规划的关键因为机械臂很多实用动作比如绕着一个物体旋转都需要末端走圆弧。圆弧插补比直线复杂的地方在于需要先在空间里确定圆心和平面。给定起点P1、中间点P2、终点P3三点可以确定一个平面圆心在三个点组成的三角形的外心位置。代码里常见的做法是先通过向量叉积求平面法向量再用几何关系求外心function [center, radius] fit_circle_3d(p1, p2, p3) % 计算平面法向量 v1 p2 - p1; v2 p3 - p1; normal cross(v1, v2); normal normal / norm(normal); % 中垂面求外心 mid1 (p1 p2) / 2; mid2 (p1 p3) / 2; % 在平面内构造两个基向量 u v1 / norm(v1); w cross(normal, u); % 求解圆心坐标 A [dot(u, p1), dot(w, p1); dot(u, p2), dot(w, p2); ...]; % 实际写代码时用线性方程求u和w方向上的投影坐标 % 最终 center 外心坐标; radius norm(p1-center) end这段代码只是个示意真要实现时会把三点的中垂线方程组直接写成矩阵形式求解Matlab里用A\b即可。圆心求出来之后圆弧插补就是计算角度增量把角度从0均匀增加到总圆心角θ_total再映射到空间点delta_theta theta_total / N; % N是插补段数 angle 0:delta_theta:theta_total; pos center radius * (u * cos(angle) w * sin(angle));这里要注意方向问题向量u和w构成平面内的右手系当圆弧的旋转方向与法向量不匹配时末端路径会走反向弧控制上会出大问题。所以调试arc_traj.m时一定要先画图确认圆弧方向如果发现方向反了把angle改成从0递减到-theta_total或者把w取反。4.3 在test4.m中组合直线和圆弧轨迹的完整流程资源里的test4.m多半是把前面这些函数串起来跑一个从p1到p2直线再从p2到p3圆弧的复合轨迹。我建议你自己也搭一个这样的流程% 定义关键路径点 p1 [0.2, 0, 0.1]; p2 [0.25, 0.1, 0.15]; p3 [0.15, 0.2, 0.1]; % 直线插补 line_pos line_traj(p1, p2, 1, 0.01); % 圆弧插补 arc_pos arc_traj(p1, p2, p3, 1, 0.01); % 把笛卡尔点逐个转成关节角 theta_hist []; for i 1:size(line_pos, 1) theta_hist(i, :) myikine(line_pos(i, :), ...); end % 将theta_hist送给cubic_traj做关节平滑后输出关键点是先做笛卡尔插补再对每个插补点做逆解最后对逆解得到关节角序列做一次低通滤波或多项式平滑。如果你只对起点和终点的关节角做三次轨迹那末端走的依然是曲线。很多人问“机械臂直线轨迹规划”怎么实现答案就是上面这套插补逐点逆解的流程而且要处理好逆解连续性问题。5. 从workspace.m到test*.m仿真验证与调试技巧5.1 用workspace.m绘制可达工作空间workspace.m这款脚本用于验证轨迹规划的边界条件最简单的用法是随机采样关节角并计算正解然后把末端点画在三维空间里得到机械臂的可达工作空间。采样时要注意关节限位必须从实际模型读取比如DOBOT的关节2和关节3范围通常在±135度之间如果你用了更大范围画出来的工作空间就是错的。核心代码类似% 随机采样关节角保持循环遍历 theta_range [-pi, pi; -135/180*pi, 135/180*pi; ...]; for i 1:5000 theta rand(1,4) .* (theta_range(:,2) - theta_range(:,1)) theta_range(:,1); T myfkine(theta); pos(i,:) T(1:3,4); end plot3(pos(:,1), pos(:,2), pos(:,3), .);运行后你就能看到哪些区域是末端可以到达的把测试路径点画在这个范围内可以有效避免规划出一个逆解根本不存在的点。检查轨迹时如果myikine报出“角度越界”或者acos输入的绝对值大于1基本可以判定是目标点超出了工作空间。5.2 test1.m到test4.m的运行顺序与报错排查我翻这套源码时习惯按文件名顺序执行。test1.m一般是最基础的正运动学验证直接给一组关节角算出末端位姿test2.m应该是多项式轨迹演示test3.m大概率是关节空间连续轨迹或直线轨迹test4.m则可能是综合仿真并输出三维动画。运行“仿真发散”最常出现在test4.m里现象是末端轨迹在后半段突然飞出去原因通常是圆弧插补的圆心求错或者逆解多解导致关节角跳变。遇到这种问题先把插补点数减少到50个打印出每个插补点的位置和对应的关节角逐行对照很容易定位到第一个发散点。另外解压后不要直接用Matlab打开.stp文件它是STEP三维模型格式需要SolidWorks等软件转换导出Matlab本身只能读取URDF或STL。这个资源里附带STEP模型主要用于测量尺寸和建立DH参数而不是直接导入Simscape仿真如果不能打开不要怀疑资源缺失。5.3 验证轨迹平滑度的三个硬指标最后给一个我每次仿真必做的验证技巧画出关节角、角速度、角加速度三条曲线然后观察角加速度曲线是否连续。对于quintic_traj角加速度应该是一条连续抛物线对cubic_traj角加速度是不连续的折线这是正常现象不是代码bug。另一个指标是末端轨迹误差你可以把规划好的末端路径与理论直线或圆弧逐点求距离直线轨迹误差一般应该控制在插补步长量级如果误差达到毫米以上检查逆解是否有跳解。第三个指标是执行时间仿真总耗时能反映算法复杂度四自由度机械臂每步逆解应该控制在毫秒级如果发现myikine2.m迭代超慢直接换回解析解myikine.m没有必要每个点都做数值迭代。本文还有配套的精品资源点击获取