简介：双连杆机器人手臂反向运动学Matlab代码面向机器人控制、自动化及数学等专业学生，适用于课程设计、期末大作业与毕业设计，解决已知末端执行器位姿反解各关节角度的核心问题。压缩包共39个文件，约2.46MB，包含m/mlx源码、Simulink模型（slx/slxc）、大量mat数据文件、png图像以及txt/xlsx说明文档，附带可直接运行的案例数据，兼容Matlab 2014、2019a和2024a版本。目前已有40人学习参考。代码采用参数化编程，连杆长度与关节参数均可方便修改，注释明细、思路清晰，从运动学公式推导到仿真验证均有涉及；读者可通过示例数据和可视化图形快速掌握反向运动学求解流程，既可用于实验报告与结课作业，也有助于初学者搭建自己的机器人运动学仿真框架。

1. 双连杆机器人手臂反向运动学：这份 matlab 代码在解什么问题

你手里拿到的是一个压缩包，标题写着“双连杆机器人手臂的反向运动学 matlab代码.rar”。解压之后，里面多半是正向运动学、逆向运动学、画图脚本这么几个模块。所谓反向运动学（Inverse Kinematics，IK），就是给出手臂末端要到达的 (x, y) 坐标，反推肩关节 q1 和肘关节 q2 这两个转角。双连杆机械臂虽然只有两个自由度，但反解并不唯一：同一个末端点，手臂既可以“肘上”姿态到达，也可以“肘下”姿态到达。写 matlab 代码时真正花时间的不是公式本身，而是怎么处理这个双解问题，以及怎么让关节角在连续轨迹里不跳变。这篇文章就按照我从零实现这套代码的顺序来拆，新手能照着跑，熟手可以直接跳到第 5 章看坑。

2. 反向运动学两种求法：几何法先手算，雅可比数值法兜底

2.1 正向运动学先立住坐标：两个关节角怎么变成末端位置

写逆解之前，正解必须烂熟。双连杆手臂的末端位置由两个关节角唯一确定：

x = L1*cos(q1) + L2*cos(q1+q2) y = L1*sin(q1) + L2*sin(q1+q2)

L1 是肩到肘的杆长，L2 是肘到末端的杆长，q2 是相对于第一段杆的转角，所以末端绝对角度是 q1+q2。这个公式是后面所有验证工作的基准，逆解算完，都要用它回代看误差。

常见的做法是把这两行封装成一个函数，我习惯叫fk_2link，参数顺序是杆长、q1、q2。这样在 matlab 命令行里随时可以调，不用每次重写。算完之后顺手用fprintf打印目标点和回代点的坐标差，误差在 1e-14 量级才算正常，这个量级是双精度浮点的极限。

注意单位问题。matlab 的cos、sin、atan2、acos全部默认弧度制，标题里这种 rar 包里的老代码，最容易翻车的地方就是有人把 30 度当 30 弧度传进去，末端位置一下飞到天上去。所以正解函数里不要做任何单位转换，所有接口统一收弧度，只在打印给人类看的时候用rad2deg转成度。

2.2 几何法推导：余弦定理出两个解，夹角公式出 q1

双连杆的逆解用几何法就够，不需要矩阵运算。把末端点到原点的距离记为 r：

r = sqrt(x^2 + y^2)

在三角形 O-E-T 里，三边分别是 L1、L2、r，用余弦定理求 q2。因为 q2 是 L1 和 L2 之间的夹角，而末端距离 r 由 q2 唯一决定：

cos(q2) = (L1^2 + L2^2 - r^2) / (2*L1*L2) q2 = ± acos(cos(q2))

这里就是双解的来源。acos 默认返回 [0, pi] 范围内的角，取正号是肘部向外弯，取负号是肘部向里弯，两种姿态对应同一个末端坐标。代码里常见的做法是用一个branch参数控制正负号。

q1 不能直接用atan2(y, x)了事，因为这个角度是末端相对于基座的极角，不是第一段杆的角度。还要减去（或加上）第一段杆与末端向量之间的夹角 beta：

beta = acos((L1^2 + r^2 - L2^2) / (2*L1*r)) q1 = atan2(y, x) - branch * beta

要特别提醒：beta 和 q2 的符号是配套的。branch=1时 q2 取正、q1 用减号；branch=-1时 q2 取负、q1 用加号。很多初写者只给 q2 乘了 branch，忘给 beta 也乘，结果组合出来的解末端位置完全对不上，这就是第 5 章要展开的坑之一。

几何法的优势是快、解析、无迭代，双连杆这种两自由度平面臂用这个方案完全够。劣势是当机构变复杂、带球腕或者杆长不满足三角形条件时，解析式会迅速变得不可维护，那时才需要换数值法。

2.3 数值法什么时候用：雅可比迭代与 fsolve 的取舍

数值法是另一条路线，常见的是雅可比迭代或者直接扔给fsolve。雅可比矩阵把末端速度映射到关节角速度，双连杆的 J 是 2×2 矩阵：

J = [-L1*sin(q1) - L2*sin(q1+q2), -L2*sin(q1+q2); L1*cos(q1) + L2*cos(q1+q2), L2*cos(q1+q2)]

迭代格式是dq = J^(-1) * dx，每步把末端误差缩小一点。它的通用性强，加关节限位、加权重、避奇异都方便，但有两个明显缺点：一是初值给不好就不收敛；二是每步都在算伪逆，比几何法慢一两个数量级。在实时仿真里，如果你要做 200 赫兹的控制循环，几何法怎么跑都够，数值法就吃力。

我给一个选择标准：机构自由度不超过 3、目标是平面定位的，一律几何法；带姿态约束、带冗余自由度、或者工作空间里存在大量奇异位形的，才用fsolve兜底。下面的表把两种方案的边界列清楚。

fsolve的问题在于它是个黑匣子，报错信息对新手不友好。同一个目标点，初值落在不同吸引域会收敛到不同解，而且你不知道它收敛到的是不是你要的那个姿态。所以我建议fsolve只用来做交叉验证：几何法算出一组 q，再用 fsolve 从这组 q 附近起算，看能不能收敛到同一个点。

3. 从零搭出 IK 的 matlab 代码：主脚本、IK 函数与正解验证

3.1 主脚本：参数放最上面，目标点一次一改

我习惯把一套 IK 代码按三个文件组织，主脚本只干三件事：定义参数、调用 IK、打印正解回代结果。这样别人拿到手里的.m文件，打开第一个脚本就能看懂数据流，不用翻三个文件才找到杆长在哪。

% demo_ik_2link.m clear; close all; clc; % 连杆长度，单位随意，只要两段统一就行 L1 = 1.0; L2 = 0.7; % 目标末端位置 x_target = 1.0; y_target = 0.5; % branch 取 1 或 -1，对应两组解 branch = 1; % 调用几何法逆解 [q1, q2] = ik_2link(L1, L2, x_target, y_target, branch); % 用正解回代，验证逆解是否正确 [x_ret, y_ret] = fk_2link(L1, L2, q1, q2); fprintf('目标点: (%.3f, %.3f)\n', x_target, y_target); fprintf('逆解: q1 = %.3f rad (%.2f deg), q2 = %.3f rad (%.2f deg)\n', ... q1, rad2deg(q1), q2, rad2deg(q2)); fprintf('回代误差: %.3e\n', norm([x_target - x_ret, y_target - y_ret]));

这段代码的逻辑很直白：先定杆长和目标点，调用逆解函数拿到 q1、q2，再用正解函数原路返回，最后把误差打到命令行。branch参数放在主脚本里而不是写死在函数里，是为了快速对比两组解的姿态，这是调试阶段最频繁的操作。实际运行的时候，回代误差应该显示在 1e-15 或 1e-16 量级，如果出现 1e-3 这种数字，说明逆解公式里某个符号反了，直接进入排查流程。

3.2 几何法 IK 函数：atan2 与 branch 参数的写法

核心函数ik_2link输入杆长、目标坐标和 branch，输出两个关节角。最容易被忽视的是可达性检查，必须先做，否则余弦定理的输入超出定义域，acos会直接吐 NaN。

function [q1, q2] = ik_2link(L1, L2, x, y, branch) % 双连杆平面臂几何法逆解 % 输入: L1, L2 杆长; x, y 末端坐标; branch 1或-1 % 输出: q1 肩关节角, q2 肘关节角, 单位弧度 r = sqrt(x.^2 + y.^2); % 可达性检查: 目标点必须在内外半径之间 if r > (L1 + L2) || r < abs(L1 - L2) error('Target unreachable: r=%.3f out of [%.3f, %.3f]', ... r, abs(L1 - L2), L1 + L2); end % 肘关节角 q2, 余弦定理 cos_q2 = (L1^2 + L2^2 - r^2) / (2 * L1 * L2); cos_q2 = max(-1, min(1, cos_q2)); % 防止浮点越界 q2 = branch * acos(cos_q2); % 肩关节角 q1, 末端极角减(加)第一段与末端向量的夹角 beta = acos((L1^2 + r^2 - L2^2) / (2 * L1 * r)); q1 = atan2(y, x) - branch * beta; end

max(-1, min(1, cos_q2))这行是关键。目标点明明在工作空间内，但因浮点舍入，cos_q2可能算出 1.0000000001，直接丢给acos就是 NaN。这里的 clamp 顺序不能写反，先压上界再提下界，结果一定落在 [-1, 1]。报错文案我写成英文，是为了避免中文编码在部分 matlab 版本里显示乱码，你完全可以改成中文。

atan2(y, x)的参数顺序是 y 在前、x 在后，写反是新手翻车率最高的点。branch * beta这个耦合也要注意，branch=1 和 branch=-1 必须同时改变 q2 的符号和 q1 的加减方向，只改一处时正解回代一定对不上。

3.3 用正运动学反查 IK：一条命令验证关节角对不对

正解函数是 IK 的照妖镜。任何疑似错误的逆解，回代一次就知道。

function [x, y] = fk_2link(L1, L2, q1, q2) % 双连杆平面臂正解 % 输入: 杆长和两个关节角(弧度) % 输出: 末端位置 x, y x = L1 * cos(q1) + L2 * cos(q1 + q2); y = L1 * sin(q1) + L2 * sin(q1 + q2); end

这段只有两行运算，没有分支。写完后第一步就是用第 3.1 节的主脚本跑一遍，确认回代误差是 1e-15 量级。我见过有人把正解公式里的 q1+q2 写成 q1-q2，逆解怎么调都对不上，最后才发现是正解本身错了。所以顺序很重要：先单独验证正解在几个特殊角度下的输出，比如 q1=0、q2=0 时末端在 (L1+L2, 0)，再拿它去验证逆解。

三个文件的分工在这张表里，方便以后维护：

4. 让机械臂在 figure 里动起来：动画脚本与末端轨迹记录

4.1 动画最小框架：plot 两段连杆加 drawnow 刷新

逆解算出来是一堆角度，看不出姿态对不对，必须画出来。动画的最小框架是：开局创建两个 plot 句柄，循环里只更新坐标数据，用drawnow强制刷新。

% animate_ik_2link.m clear; close all; clc; L1 = 1.0; L2 = 0.7; figure('Color', 'w'); hold on; axis equal; grid on; xlabel('x (m)'); ylabel('y (m)'); xlim([-1.8 1.8]); ylim([-1.8 1.8]); % 两段杆和末端点的句柄 h_link1 = plot([0 0], [0 0], 'b-o', 'LineWidth', 3, 'MarkerSize', 6); h_link2 = plot([0 0], [0 0], 'r-o', 'LineWidth', 3, 'MarkerSize', 6); h_end = plot(0, 0, 'k*', 'MarkerSize', 10); % 让末端沿一个圆走 t = linspace(0, 1, 100); x_traj = 1.0 + 0.3 * cos(2 * pi * t); y_traj = 0.3 + 0.3 * sin(2 * pi * t); plot(x_traj, y_traj, 'k--', 'LineWidth', 1); branch = 1; q_prev = [0.3; 0.3]; % 上一时刻关节角, 后面做连续性选择用 for i = 1:length(t) [q1, q2] = ik_2link(L1, L2, x_traj(i), y_traj(i), branch); q_prev = [q1; q2]; % 画出当前姿态 x1 = L1 * cos(q1); y1 = L1 * sin(q1); x2 = x1 + L2 * cos(q1 + q2); y2 = y1 + L2 * sin(q1 + q2); set(h_link1, 'XData', [0 x1], 'YData', [0 y1]); set(h_link2, 'XData', [x1 x2], 'YData', [y1 y2]); set(h_end, 'XData', x2, 'YData', y2); drawnow; pause(0.02); end

关键点全在句柄操作上：循环里只用set更新已有的 plot，绝不用plot重新画线，否则旧线条会留在图上，速度也会越跑越慢。axis equal不能省，没有它两段杆会在视觉上被拉伸，姿态判断会被误导。pause(0.02)控制帧间隔，数字越小动画越流畅，但如果你的机器卡，先看是不是循环里误建了新的 figure，而不是急着加大 pause。

4.2 轨迹跟踪：让末端沿直线走一遍

圆轨迹适合看全貌，直线轨迹适合暴露问题。让末端从 (0.5, 0.3) 匀速走到 (1.3, 0.9)，如果中途手臂姿态突然跳一下，说明多解切换没有处理好，这会直接引出第 5 章的第一个坑。

直线插值写法很简单：

x_start = 0.5; y_start = 0.3; x_end = 1.3; y_end = 0.9; t = linspace(0, 1, 150); x_traj = x_start + (x_end - x_start) * t; y_traj = y_start + (y_end - y_start) * t;

把这两行替换掉第 4.1 节里的圆轨迹生成代码即可，动画循环不用动。跑完之后观察两段杆的形态变化：关节角应该是平滑过渡的，如果某一帧 q1 或 q2 突然跳了 90 度以上，那就是解选错了。

直线轨迹还有一个额外收获：能看到工作空间的边界在哪。目标点靠近半径 L1+L2 的外边界时，手臂会趋于完全伸直，动画上表现为第二段杆和第一段杆几乎共线。如果你设定的路径超出了边界，ik_2link会直接报错，动画中断在那一帧，这种明显反馈比看数值日志直观得多。

4.3 用 VideoWriter 把仿真录成 mp4

动画在屏幕上看完就没了，课程设计提交报告时需要一段录屏或者视频文件。matlab 自带的VideoWriter就能做，不用额外装工具。

v = VideoWriter('trajectory_2link.avi'); open(v); for i = 1:length(t) % ... 这里放第 4.1 节的更新代码 ... frame = getframe(gcf); writeVideo(v, frame); end close(v);

VideoWriter的默认格式是 avi，想输出 mp4 的话，构造时加第二个参数，写成VideoWriter('trajectory_2link.mp4', 'MPEG-4')。要注意的是编码器依赖系统环境，有些精简版 matlab 不带 mp4 编码器，用 avi 最稳。getframe抓的是当前 figure 的内容，所以动画里的轴范围、网格、曲线样式都会进视频，录制前先把布局调好。

录制时长等于帧数乘帧间隔：上面例子里 100 帧乘 0.02 秒，视频约 2 秒，想延长就加帧数或加大 pause 值。pause的时长在录制时会真实消耗，所以录 1000 帧的路径要有耐心，不是 matlab 卡了，是在等墙钟时间走完。

5. 反向运动学实现中常见的 5 个坑：现象、原因与解决

5.1 姿态突跳：多解没做连续性选择

现象：末端沿直线轨迹运动时，手臂原本一直是肘外弯姿态，走到中间某一帧突然变成肘内弯，关节角数据出现一个很大的阶跃。原因：同一个末端点有两组解，固定branch=1只取了其中一组，当轨迹走到两组解在关节空间里“交叉”的位置附近，继续沿当前 branch 走就会出现大角度变化，而换到另一组解反而更平滑。解决：连续轨迹里不锁死 branch，每帧算两个解，选离上一帧关节角最近的那组。距离用带角度回绕处理的差值：

function [q1, q2] = ik_2link_near(L1, L2, x, y, q_prev) [q1a, q2a] = ik_2link(L1, L2, x, y, 1); [q1b, q2b] = ik_2link(L1, L2, x, y, -1); d_a = abs(atan2(sin(q1a - q_prev(1)), cos(q1a - q_prev(1)))) + ... abs(atan2(sin(q2a - q_prev(2)), cos(q2a - q_prev(2)))); d_b = abs(atan2(sin(q1b - q_prev(1)), cos(q1b - q_prev(1)))) + ... abs(atan2(sin(q2b - q_prev(2)), cos(q2b - q_prev(2)))); if d_a <= d_b q1 = q1a; q2 = q2a; else q1 = q1b; q2 = q2b; end end

这里用atan2(sin(delta), cos(delta))处理角度回绕，是为了避免 179 度和 -179 度被算成相差 358 度，实际只差 2 度。我见过有人直接求绝对值之差，结果轨迹每到 180 度边界就突跳一次，属于典型的符号处理翻车。

5.2 acos 出现 NaN：浮点越界没有 clamp

现象：目标点手算明明在工作空间内，acos却返回 NaN，整条轨迹断掉。原因：浮点舍入导致cos_q2略微超出 [-1, 1]，比如算出 1.0000000001，acos对定义域外的输入直接返回 NaN。解决：在调用acos之前强制 clamp，这行代码我在第 3.2 节里已经写进了ik_2link函数。注意只能 clamp 余弦值本身，不能 clamp 后面的角度，否则会引入真实误差。

5.3 角度制混淆：sind/cosd 和 rad2deg 混用

现象：逆解算出的 q1、q2 放在正解里回代，末端坐标差了一大截，但单独看每个角度又“感觉没问题”。原因：有人用sind求正解，用acosd求逆解，中间某个量被rad2deg转了一次又忘了转回来，单位链断裂。解决：整个工程统一弧度制，所有三角函数都用cos、sin、atan2、acos，只在fprintf打印时用rad2deg给人看。如果你实在喜欢角度制，那么sind、cosd、atan2d、acosd必须全套一起用，绝不能混。

5.4 末端接近边界时抖动：工作空间边缘的数值脆弱

现象：目标点轨迹靠近外边界 r = L1+L2 或内边界 r = |L1-L2| 时，关节角在连续几帧内剧烈变化，动画看起来像抽搐。原因：在工作空间边界上，目标点对应的两组解趋于同一个姿态，“解空间”变窄，同时acos的导数在边界附近趋于无穷，关节角对末端误差的敏感度大增。解决：轨迹规划阶段不要贴着边界走，留出 3% 到 5% 的余量，比如给目标点半径上限设成 0.97 倍的最大可达距离。如果应用必须贴近边界工作，几何法算出的角度要用第 5.1 节的连续性选择函数过滤，并且接受边界附近关节角变化率偏大的现实。

5.5 atan 与 atan2 混用：象限判断丢失导致关节角错位

现象：目标点出现在第三象限（x 和 y 都为负）时，q1 整个偏差约 180 度。原因：atan(y/x)无法区分 (x,y) = (-1,-1) 和 (x,y) = (1,1)，因为比值都是 1，只有atan2(y,x)才能保留象限信息。另一个隐匿版本是把atan2的参数顺序写成(x,y)，matlab 不报错，但角度偏 90 度，回代误差肉眼可见。解决：项目代码里全局搜索atan(，凡是要算关节角的一律换成atan2，并检查参数顺序是 y 在前、x 在后。我自己的习惯是写完 IK 函数后第一件事就是用第三象限的目标点做一次回代测试，专治这种象限问题。

6. 进阶：从单点解到连续轨迹，IK 的边界感怎么找

第 5.1 节给出的连续性选择函数，是单点 IK 走向连续轨迹的关键一步，但它只能解决解的选择，还没解决关节限位。真实机械臂的关节角有物理上下限，加限位的方式很简单：在选出解之后做一次检查，如果超限就切换到另一组解，另一组也超限就报错或者做线性插值回退。下面这段是我常用的带限位版本，核心思路是先选最近解，再验证是否越界。

function [q1, q2, feasible] = ik_2link_limited(L1, L2, x, y, q_prev, q1_max, q2_max) q_min = [-q1_max; -q2_max]; q_max = [q1_max; q2_max]; [q1n, q2n] = ik_2link_near(L1, L2, x, y, q_prev); c1 = (q1n >= q_min(1)) && (q1n <= q_max(1)); c2 = (q2n >= q_min(2)) && (q2n <= q_max(2)); if c1 && c2 q1 = q1n; q2 = q2n; feasible = true; else [q1o, q2o] = ik_2link(L1, L2, x, y, -sign(q_prev(1))); if q1o >= q_min(1) && q1o <= q_max(1) && q2o >= q_min(2) && q2o <= q_max(2) q1 = q1o; q2 = q2o; feasible = true; else q1 = q_prev(1); q2 = q_prev(2); feasible = false; end end end

feasible=false时保留上一帧关节角，而不是给一组非法角度，这个设计是为了避免下游控制逻辑拿到越界值。实际做轨迹规划时，还要预先检查整条路径是否都在工作空间内，不要等问题发生在动画中间才报错。

还有一个验证习惯值得固化下来：每一帧 IK 解出来之后，立刻用正解回代，并把误差写进一个数组，轨迹结束后看最大误差和均方根误差。我第一次做双连杆 IK 时，动画看起来非常顺滑，结果把关节角数据导出来才发现 q2 有一小段符号反了，回代误差只有 2 毫米，视觉上根本看不出来。这之后我所有 IK 代码都会带一个误差数组，并且在轨迹结束用一行命令检查最大值：

assert(max(err_array) < 1e-10, 'IK error too large');

数值 IK 的边界感比几何法更难找，因为初值不好时，fsolve可能收敛到完全错误的姿态，但回代误差依然很小，所以数值法的验证不能只看末端误差，还要看关节角变化率和上一帧的连续性。我现在的习惯是：能用几何法就不用数值法，必须用数值法时，几何法的解永远作为迭代初值，这比随便给一组零初始角稳定得多。希望帮到你。

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