UR5e 机器人采用全部8种逆运动学解执行直线运动。逆运动学解的最优选择可能取决于关节限制、奇点、避障需求或动态性能。
对于许多在工业和研究中使用UR系列机器人的从业者而言，逆运动学仍是一项挑战。本文将展示如何基于IK-Geo编写高性能逆运动学求解器，其具备以下优势：
-
在我们的测试中，它是速度最快、精度最高的逆运动学求解器，性能已通过独立验证：比IKFast 快40倍以上；若仅返回一个解，速度约为IKFast的200倍；误差达到机器精度级别。 -
可返回全部解，能基于关节限制、奇点、障碍物和动态性能选择最优解。 -
可返回奇异解，通过避免切换至关节级控制实现机器人平稳运动。 -
易于实现、调试和修改。 -
支持MATLAB、C++、Rust和Python语言。
该求解器的速度、精度和可靠性，为实时控制、路径规划、仿真和分析领域带来了新的可能。
1. 引言
UR5及更新款的UR5e均属于Universal Robots公司的UR系列协作机械臂。Standard Bots、Techman Robot/欧姆龙（Omron）、大疆创新（Dobot）等公司也生产具有类似运动学特性的机器人。UR5这类UR系列机器人拥有3个平行关节轴（2、3、4轴）和两组相交关节轴（1与2轴、5与6轴）。
UR5协作机械臂示意图：它是科研和工业领域应用最广泛的6自由度机械臂之一，也是本教程的重点研究对象。
在求解逆运动学问题之前，我们首先采用指数积 （Product of Exponentials, POE）法求解正运动学问题。正运动学问题相对简单，可用于建立机器人运动学模型、验证模型与UR机器人仿真器的一致性，并确认逆运动学解的正确性。
1.1 正运动学问题
已知关节角度向量q，求解末端执行器坐标系相对于基坐标系的位姿(R0T,p0T)
接下来，我们将通过子问题分解法 求解逆运动学问题。利用关节轴的相交和平行特性，可将逆运动学方程转化为三个典型几何子问题的闭式解。
1.2 逆运动学问题
已知末端执行器位姿(R0T,p0T)，求解关节角度向量q的所有可能值。
最后，我们将对求解器的性能（执行时间、精度、奇点鲁棒性）进行评估，并与IKFast等其他方法进行对比。
本教程将带领读者从正运动学出发，通过子问题分解法求解逆运动学，最终与工业界常用方法进行性能基准测试。
2. 运动学参数与正运动学
在求解逆运动学问题之前，需先建立机器人模型，并确保正运动学计算结果与机器人仿真器一致。
我们采用指数积（POE）法描述机器人运动学。尽管丹纳-哈滕伯格（Denavit–Hartenberg, DH）法更为常用，但POE法推导的方程更为简洁。
机器人的运动是沿关节和连杆的一系列旋转与平移运动。可通过多个坐标系对其进行建模：基坐标系（0系）固定在机器人基座，1-6系分别固定在各关节处，工具坐标系（T系）固定在末端执行器上。关节角度向量q表示机械臂各关节相对于零位形（q=0）的角度。
（关节坐标系原点调整前后的运动学参数示意图：使1与2系、5与6系的原点重合，可简化逆运动学推导过程）
对于6自由度机器人，其运动学参数包括关节轴向量 H =[h1h2h3h4h5h6]和连杆位移向量 P=[p01p12p23p34p45p56p6T]。
其中，hi是机器人处于零位形时，第i个关节旋转轴在基坐标系下的单位向量；pij是机器人处于零位形时，从i系原点到j系原点的位移在基坐标系下的向量。
最后一个运动学参数是R6T，即末端执行器（工具坐标系T）相对于最后一个关节坐标系（6系）的旋转矩阵，其值由机器人处于零位形时工具坐标系的姿态决定。
若已知运动学参数H、P、R6T和关节向量q，则可计算末端执行器的旋转矩阵和位置向量：
旋转矩阵Rij表示j系相对于i系的姿态，可通过各关节旋转矩阵相乘得到。例如：
求解正、逆运动学需要运动学参数的数值。部分机器人可通过产品文档中的示意图获取参数；对于UR5e，可利用Universal Robots提供的DH参数，将其转换为POE参数。我们将使用IK-Geo自带的自动逆运动学函数，完成DH参数到POE参数的转换，并检测和显示关节轴的相交与平行情况。
2.1 UR5e机器人DH参数转POE参数及关节轴相交/平行检测
alpha_vec = [sym(pi)/2 0 0 sym(pi)/2 -sym(pi)/2 0]; % 单位：弧度（rad）
a_vec = [0 -0.425 -0.3922 0 0 0]; % 单位：米（m）
d_vec = [0.1625 0 0 0.1333 0.0997 0.0996]; % 单位：米（m）
kin = dh_to_kin(alpha_vec, a_vec, d_vec);
[is_int, is_int_nonconsecutive, is_parallel, is_spherical] =...
detect_intersecting_parallel_axes(kin);
print_intersecting_parallel_axes(...
is_int, is_int_nonconsecutive, is_parallel, is_spherical);
disp("P=");disp(vpa(kin.P)) % 以小数形式显示，而非分数
disp("H=");disp(kin.H)
相交关节轴：(1, 2) (4, 5) (5, 6)
非连续相交关节轴：无
平行关节轴：(2, 3) (3, 4)
球关节：无
P=
[0, 0, -0.425, -0.3922, 0, 0, 0]
[0, 0, 0, 0, -0.1333, 0, -0.0996]
[0, 0.1625, 0, 0, 0, -0.0997, 0]
H=
[0, 0, 0, 0, 0, 0]
[0, -1, -1, -1, 0, -1]
[1, 0, 0, 0, -1, 0]
为简化逆运动学求解，我们可利用以下特性：1与2轴相交、5与6轴相交、2、3、4轴平行。其中，平行关节轴意味着 h2=h3=h4， 相交关节轴则允许我们调整运动学参数，使 p12=p56=0。
在POE法中，每个关节坐标系的原点可沿对应关节轴任意选取。为使1系和2系原点重合，可将1系原点沿1轴滑动，并对p01进行反向调整；同理，可将5系原点沿5轴滑动，使其与6系原点重合，同时更新p45和p56。本文选择使5系与6系原点重合（而非4系与5系）。
2.2 调整后使1-2系、5-6系原点重合的运动学参数
zv = [0;0;0];
ex = [1;0;0];
ey = [0;1;0];
ez = [0;0;1];
kin.H = [ez ey ey ey -ez ey];
kin.P = [0.1625*ez zv -0.425*ex -0.3922*ex...
-0.1333*ey-0.0997*ez zv -0.0996*ey];
kin.joint_type = zeros([6 1]);
我们通过对比正运动学计算结果与UR机器人仿真器结果，验证POE运动学参数的正确性。选取包括零位形在内的三种测试位形，计算结果与仿真器完全一致，证明模型准确。
（UR机器人仿真器中的三种位形，用于验证POE运动学模型。MATLAB正运动学计算结果与仿真器一致）
2.3 三种不同关节位形下正运动学测试的MATLAB代码（末端执行器位姿与MATLAB计算结果一致）
q_test_0 = zeros([6 1]);
q_test_1 = deg2rad(90)*ones([6 1]);
q_test_2 = deg2rad([10 20 30 40 50 60]);
[R_06_0, p_0T_0] = fwdkin(kin, q_test_0);
[R_06_1, p_0T_1] = fwdkin(kin, q_test_1);
[R_06_2, p_0T_2] = fwdkin(kin, q_test_2);
p_test_0 = [-817.2 -232.9 62.8]'/1000;
R_test_0 = rot(ex, deg2rad(90)); % 零位形，与R_6T相同
p_test_1 = [133.3 292.5 -162.9]'/1000;
R_test_1 = rot(ez, deg2rad(90));
p_test_2 = [-509.12 -290.14 -359.6]'/1000;
rot_vec = deg2rad([55.76 -153.20 58.44]);
R_test_2 = rot(rot_vec/norm(rot_vec), norm(rot_vec));
disp("[p_test_0 p_0T_0] ="); disp([p_test_0 p_0T_0])
disp("[R_test_0 R_0T_0] ="); disp([R_test_0 R_06_0*R_6T])
disp("[p_test_1 p_0T_1] ="); disp([p_test_1 p_0T_1])
disp("[R_test_1 R_0T_1] ="); disp([R_test_1 R_06_1*R_6T])
disp("[p_test_2 p_0T_2] ="); disp([p_test_2 p_0T_2])
disp("[R_test_2 R_0T_2] ="); disp([R_test_2 R_06_2*R_6T])
[p_test_0 p_0T_0] =
-0.8172 -0.8172
-0.2329 -0.2329
0.0628 0.0628
[R_test_0 R_0T_0] =
1.0000 0 0 1.0000 0 0
0 0.0000 -1.0000 0 0.0000 -1.0000
0 1.0000 0.0000 0 1.0000 0.0000
[p_test_1 p_0T_1] =
0.1333 0.1333
0.2925 0.2925
-0.1629 -0.1629
[R_test_1 R_0T_1] =
0.0000 -1.0000 0 0.0000 -1.0000 0.0000
1.0000 0.0000 0 1.0000 0.0000 0.0000
0 0 1.0000 -0.0000 -0.0000 1.0000
[p_test_2 p_0T_2] =
-0.5091 -0.5091
-0.2901 -0.2901
-0.3596 -0.3596
[R_test_2 R_0T_2] =
-0.7864 -0.6076 0.1116 -0.7864 -0.6076 0.1116
-0.5276 0.5665 -0.6330 -0.5276 0.5665 -0.6330
0.3214 -0.5567 -0.7660 0.3214 -0.5567 -0.7660
建立并验证正运动学模型后，下一步将介绍用于求解UR5逆运动学问题的子问题。
3. 三个典型几何子问题的解
在《IK-Geo》 论文中，我们讨论了6个几何子问题的解，可用于求解任意6自由度机器人的逆运动学。对于UR5，仅需用到其中3个：子问题1、子问题3和子问题4。
以下是这三个子问题的代数与几何描述。每个子问题均需求解旋转角度，使向量旋转后与点、球面或平面相交；若不存在相交解，则返回最小二乘解。
3.1 子问题1：圆与点
已知向量p1,p2和单位向量k，求解
) p1-p2 。若存在精确解，则
满足方程 R (k,
) p1=p2。该角度
使点 p1 绕轴 k 旋转后与点 p2 重合。
（子问题1示意图：k为旋转轴，θ为旋转角度，p1为初始点，p2为目标点）
3.2 子问题3：圆与球面
（子问题3示意图：k为旋转轴，θ为旋转角度，p1为初始点，p2为球心，d为球的半径）
3.3 子问题4：圆与平面
（子问题4示意图：k为旋转轴，θ为旋转角度，p为初始点，h为平面法向量， 为平面到原点的距离）
以下是各子问题的MATLAB实现代码，通过is_LS标志表示是否为最小二乘解。代码中仅使用atan2()作为反三角函数，其数值稳定性优于acos()、asin()或atan()。
3.3.1 子问题1：圆与点
function [theta, is_LS] = sp_1(p1, p2, k)
KxP = cross(k, p1);
A = [KxP -cross(k,KxP)];
x = A'*p2;
theta = atan2(x(1),x(2));
is_LS = abs(norm(p1) - norm(p2)) > 1e-8 ...
|| abs(dot(k,p1) - dot(k,p2)) > 1e-8;
end
3.3.2 子问题3：圆与球面
function [theta, is_LS] = sp_3(p1, p2, k, d)
[theta, is_LS] = sp_4(p2, p1, k, 1/2 * (dot(p1,p1)+dot(p2,p2)-d^2));
end
3.3.3 子问题4：圆与平面
function [theta, is_LS] = sp_4(h, p, k, d)
A_11 = cross(k,p);
A_1 = [A_11 -cross(k,A_11)];
A = h'*A_1;
b = d - h'*k*(k'*p);
norm_A_2 = dot(A,A);
x_ls_tilde = A_1'*(h*b);
if norm_A_2 > b^2
xi = sqrt(norm_A_2-b^2);
x_N_prime_tilde = [A(2); -A(1)];
sc_1 = x_ls_tilde + xi*x_N_prime_tilde;
sc_2 = x_ls_tilde - xi*x_N_prime_tilde;
theta = [atan2(sc_1(1), sc_1(2)) atan2(sc_2(1), sc_2(2))];
is_LS = false;
else
theta = atan2(x_ls_tilde(1), x_ls_tilde(2));
is_LS = true;
end
end
解决子问题1、3、4后，我们已具备推导UR5完整逆运动学解的工具。
4. 逆运动学
首先，通过将正运动学方程改写为 (R06,po6) 的形式，并假设 po1=0,
简化方程 (该方法适用于所有机器人，而非仅 UR5)。由此可直接写出:
若 po1≠0 (如本文推导的运动学参数), 可从 po6 中减去 po1 进行修正。
接下来利用 UR5 的几何简化特性：由于 h2=h3=h3=h4, 这三个关节节轴的累积旋转可表示为绕 h2 轴、角度为 θ14 的单一旋转；同时，沿 h2 方向投影可大幅简化方程，因为
第二个简化特性 p12=p56=0 可使部分项直接消去 (实际上，只需其中一项消去，即可将问题分解为子问题 1、3、4)。此时可将正运动学方程改写为：
.沿 h2 方向投影可消去变量 q2、q3、q4, 得到
应用几何简化后，可通过子问题逐步求解关节角度:
- 1. 利用子问题 4 和沿 h2 投影的位置方程，求解 q1:
- 2. 利用子问题 4 和沿h 2 投影的旋转方程，求解 q5:
- 3. 沿 h6 或 h2 方向投影旋转方程，利用子问题 1 求解
- 4. 调整位置方程各项并对两边取模, 利用子问题 3 求解 q3:
- 5. 利用子问题 1 和位置方程，求解 q2:
- 6. 通过减法求解 Q4, 并将结果归一化到 [一 π,π] 范围内:
至此，已求解出所有关节角度 Q1,92,Q3,Q4,Q5,Q6, 构成关节角度向向量。求解子问题 3 或 4时，最多可能得到 2 个解，因此同一末端执行器位姿最多对应 8 种关节配置。
若某一子问题无精确解，可采用最小二乘解（由is_LS标志标识）得到近似连续解。这种近似逆运动学解使算法在末端执行器期望位姿处于或略超出工作空间时，仍能稳健地求解。
以下是UR5逆运动学求解的MATLAB实现代码：
4.1 适用于所有UR系列机器人的逆运动学代码
function [Q, is_LS_vec] = IK_UR(R_06, p_0T, kin)
% 已知条件：h_2 = h_3 = h_4 且 p_12 = p_56 = 0
P = kin.P;
H = kin.H;
Q = [];
is_LS_vec = [];
p_06 = p_0T - P(:,1) - R_06*P(:,7);
% 利用子问题4求解q1
[theta1, theta1_is_ls] = sp_4(...
H(:,2), p_06, -H(:,1), H(:,2)'*sum(P(:,2:5), 2));
for i_t1 = 1:length(theta1)
q_1 = theta1(i_t1);
R_01 = rot(H(:,1), q_1);
% 利用子问题4求解q5
[theta5, theta5_is_ls] = sp_4(H(:,2),H(:,6),H(:,5), ...
H(:,2)' * R_01' * R_06 * H(:,6));
for i_t5 = 1:length(theta5)
q_5 = theta5(i_t5);
R_45 = rot(H(:,5), q_5);
% 利用子问题1求解R_14（对应theta_14）
[theta_14, theta_14_is_LS] = sp_1(...
R_45*H(:,6),R_01'*R_06*H(:,6), H(:,2));
% 利用子问题1求解q6
[q_6, q_6_is_LS] = sp_1(R_45'*H(:,2), R_06'*R_01*H(:,2), -H(:,6));
% 利用子问题3求解q3
d_inner = R_01'*p_06-P(:,2) - rot(H(:,2), theta_14)*P(:,5);
d = norm(d_inner);
[theta_3, theta_3_is_LS] = sp_3(-P(:,4), P(:,3), H(:,2), d);
for i_q3 = 1:length(theta_3)
q_3 = theta_3(i_q3);
% 利用子问题1求解q2
[q_2, q_2_is_LS] = sp_1(...
P(:,3) + rot(H(:,2), q_3)*P(:,4), d_inner, H(:,2));
% 通过减法求解q4
q_4 = wrapToPi(theta_14 - q_2 - q_3);
q_i = [q_1; q_2; q_3; q_4; q_5; q_6];
Q = [Q q_i];
is_LS_vec = [is_LS_vec theta1_is_ls||theta5_is_ls...
||theta_14_is_LS||theta_3_is_LS||q_2_is_LS||q_6_is_LS];
end
end
end
end
实现UR5逆运动学求解器的MATLAB代码后，下一步将对其性能进行评估。
5. 性能评估
我们通过测量执行时间、求解精度，并测试算法在工作空间边界及略超出边界时的求解能力，评估该逆运动学方法的性能。
5.1 求解精度测量
生成10000组随机关节角度，通过正运动学计算末端执行器位姿；然后计算原始关节角度向量与最接近的逆运动学解之间的差异，并测量末端执行器位姿在姿态和位置上的误差。
（逆运动学求解精度图：黑色曲线为关节空间误差，红色和蓝色曲线分别为任务空间姿态误差和位置误差）
关节误差、位置误差和姿态误差的中值分别为 9.15×10-16 弧度 (rad)、3.30×10-16米 (m) 和 1.24×10-16 弧度 (rad), 略优于慕尼黑工业大学 (TUMurnich)《EAIK》论文中报道的 IK-Geo 任务空间精度 (10-15 rad 和 10-15m)。双精度浮点的机器精度 (使2.22×10-16, 反映了 1 附近可表示数值的间隔。IK-Geo1+e>1 的最小 E) 为 2返回的逆运动学解精度已接近双精度算术的实际极限。
同时，在10000组随机位姿测试中，IK-Geo逆运动学算法的求解成功率为100%。
5.2 执行时间测量
参考《MATLAB性能测量白皮书》的方法，经MEX编译器编译为C++代码后，该逆运动学算法的执行时间为每个末端执行器位姿2.93微秒（μs），与《IK-Geo》论文中报道的2.85 μs/位姿接近，且快于《EAIK》论文图表中报道的约3.9 μs/位姿。
若仅返回一个解，MATLAB环境下的执行时间可缩短至0.64 μs/位姿，速度提升4.7倍。
5.3 奇点鲁棒性测试
IK-Geo逆运动学算法的一个重要优势是：对于无精确解的分支，可返回连续近似解，因此即使末端执行器期望位姿处于工作空间边界，算法仍能正常工作；对于因数值误差或遥操作等应用场景中略超出工作空间的位姿，也能返回连续近似解。
在丹纳-哈滕伯格（DH）法定义的零位形下，机器人处于奇点状态（所有关节轴平行于同一平面）。因此，可利用 对应的末端执行器位姿测试算法的奇点鲁棒性。以下代码示例验证了该逆运动学算法可恢复 的逆运动学解。
5.3.1 利用最小二乘子问题解求解奇异逆运动学解的代码
% 生成奇异位姿
q_0 = zeros([6 1]);
[R_06, p_0T] = fwdkin(kin, q_0);
% 验证奇点（计算雅可比矩阵最小奇异值）
J = robotjacobian(kin, q_0);
disp("最小奇异值: " + min(svd(J)))
% 验证奇点处逆运动学求解功能
[Q, is_LS] = hardcoded_ur5_IK(R_06, p_0T)
最小奇异值: 5.7183e-17
Q =
0 0 -2.8182 -2.8182 -2.8182
0.0000 -0.0000 2.9023 -3.1416 3.1416
-0.0000 0.0000 0 -0.0000 0.0000
0.0000 -0.0000 0.2393 -3.1416 3.1416
0 0 2.8182 -2.8182 -2.8182
0 0 3.1416 0 0
is_LS =
1 1 1 0 0
在该示例中，原本的8个解合并为5个，且前2个解接近合并。只需对逆运动学算法稍作修改（重复合并的解），即可确保始终返回8个解。
尽管超出本教程范围，但当机器人处于存在连续逆运动学解的内部奇点时，仍可通过该算法求解全部解。
IK-Geo不仅能求解边界奇点处的逆运动学解，还能求解存在自运动的内部奇点处的解。内部奇点冗余解析超出本教程范围，此处以ABB IRB 6640机械臂为例进行演示。
该求解器精度接近机器精度、执行时间达微秒级、且在奇点处具有鲁棒性，接下来将其与IKFast等其他逆运动学方法进行对比。
6. 与其他逆运动学方法的对比
对于UR5这类机器人，最常用的逆运动学方法或许是IKFast。它是一个基于COLLADA XML描述文件生成C++代码的Python脚本，属于OpenRAVE项目的一部分，也被用于MoveIt、Tesseract等ROS规划器中。
6.1 IKFast的局限性
IKFast的代码生成高度依赖COLLADA文件中关节原点的位置，即便正运动学模型相同也是如此：
-
若COLLADA文件基于DH参数，且无重合关节轴，则IKFast无法生成逆运动学代码； -
若COLLADA文件包含1-2个重合关节原点，生成的C++文件有932行代码，耗时57秒； -
若机械臂初始位姿略有不同，生成的C++文件可达118312行代码，耗时40分36秒； -
已有研究表明，IKFast生成的代码可能存在无法编译或返回错误逆运动学解的问题。
6.2 性能对比
-
执行时间：本文之前的测试结果显示，UR5机器人的IKFast执行时间为121.71 μs/位姿；《EAIK》论文中的结果显示其速度较快，约为54.7 μs/位姿。这种差异可能源于COLLADA文件不同导致的代码生成不一致。 -
求解精度：《EAIK》论文显示，IKFast的位置误差中值为 m，比IK-Geo低3-4个数量级。 -
工作空间适应性：与许多其他逆运动学方法类似，当末端执行器期望位姿处于或接近工作空间边界时，IKFast无法求解。
6.3 其他方法对比
-
基于运动学雅可比矩阵的迭代法：这类方法仅能返回接近初始猜测值的一个解，而非全部解；《EAIK》论文的时序结果显示，其执行时间比IK-Geo慢1-3个数量级，且执行时间波动性更大、精度低11个数量级；此外，迭代法需仔细调整参数，在接近奇点时难以收敛。 -
基于机器学习的逆运动学方法：与迭代法存在类似的性能问题，且针对不同机器人需重新训练模型。
7. 结论
本教程展示了如何通过子问题分解法，为UR5机械臂推导速度快、精度高且鲁棒性强的逆运动学求解器。
由于MATLAB代码未经过深度优化，其性能仍有提升空间。
尽管本教程以UR5为研究对象，但类似的逆运动学方法可应用于所有具有三组相交或平行关节轴的6自由度旋转关节机器人。UR5需用到子问题1、3、4，其他机器人则可能根据运动学结构需要子问题2、5或6。通过添加一维搜索，该方法可求解所有至少具有一对相交或平行关节轴的6自由度旋转关节机器人的逆运动学问题，涵盖所有工业用6自由度旋转关节机器人。
Airking Robots
北京艾科伯特科技有限公司，是专注协作机器人和移动机器人的技术公司，Airking Robots——艾科伯特立足于航空/航天，专注于机器人智能制造方向，Airking Robots是优傲机器人，Robotiq等协作机器人方向中国区域金牌提供商
商务联系：
更多案例请关注公众号：