% 机器人逆运动学多解性验证代码 - 修正版
% 基于PUMA560机器人模型
clear all;
close all;
clc;
% 加载机器人模型
mdl_puma560;
robot = p560;
% 设置显示参数
robot.plotopt = {'workspace', [-2 2 -2 2 -0.5 2], 'tilesize', 0.2};
% 1. 生成初始关节角度(考虑关节限制)
qlim = robot.qlim;
q1 = zeros(1,6);
for i = 1:6
q1(i) = qlim(i,1) + rand()*(qlim(i,2)-qlim(i,1));
end
fprintf('初始关节角度q1 (弧度):\n');
disp(q1);
fprintf('对应的角度值(度):\n');
disp(rad2deg(q1));
% 2. 计算末端位姿
T1 = robot.fkine(q1);
fprintf('\n对应的末端位姿T1:\n');
disp(T1);
% 3. 逆运动学求解
tol = 1e-6;
max_iter = 500;
ilimit = 50; % 尝试次数
q_solutions = [];
errors = [];
attempts = [];
fprintf('\n开始搜索不同逆解...\n');
for attempt = 1:ilimit
% 生成随机初始猜测
q_guess = zeros(1,6);
for i = 1:6
q_guess(i) = qlim(i,1) + rand()*(qlim(i,2)-qlim(i,1));
end
% 修正的ikine调用方式
try
% 方法1:使用简化调用方式
q_sol = robot.ikine(T1, 'q0', q_guess, 'tol', tol, 'ilimit', max_iter);
% 验证解的正确性
T_sol = robot.fkine(q_sol);
error = norm(T1.t - T_sol.t); % 仅比较位置
if error < tol
% 检查是否是新解
is_new = true;
for k = 1:size(q_solutions,1)
if norm(q_sol - q_solutions(k,:)) < 0.1
is_new = false;
break;
end
end
if is_new
q_solutions = [q_solutions; q_sol];
errors = [errors; error];
attempts = [attempts; attempt];
fprintf('找到解 #%d (尝试次数: %d)\n', size(q_solutions,1), attempt);
fprintf('关节角度(度): [%s]\n', num2str(rad2deg(q_sol), '%.2f '));
fprintf('位置误差: %.2e\n\n', error);
end
end
catch ME
fprintf('尝试 %d 失败: %s\n', attempt, ME.message);
end
end
% 4. 结果分析
n_solutions = size(q_solutions,1);
if n_solutions == 0
warning('未找到任何有效的逆运动学解!可能原因:');
warning('1. 初始位姿可能超出工作空间');
warning('2. 收敛容差设置过小');
warning('3. 最大迭代次数不足');
return;
end
fprintf('\n共找到 %d 个不同的逆解:\n', n_solutions);
% 5. 可视化结果
figure('Name', '逆解构型比较', 'Position', [100 100 1200 800]);
n_cols = ceil(sqrt(n_solutions));
n_rows = ceil(n_solutions/n_cols);
for i = 1:n_solutions
subplot(n_rows, n_cols, i);
robot.plot(q_solutions(i,:));
title(sprintf('解 %d\n尝试次数: %d', i, attempts(i)));
end
% 6. 性能分析
performance = struct();
for i = 1:n_solutions
% 关节限制裕度
q = q_solutions(i,:);
margin = min([q - qlim(:,1)'; qlim(:,2)' - q]);
performance(i).min_joint_margin = min(margin);
performance(i).avg_joint_margin = mean(margin);
% 操作性能
J = robot.jacob0(q);
performance(i).condition_number = cond(J);
performance(i).manipulability = sqrt(det(J*J'));
% 与初始解距离
performance(i).distance_to_q1 = norm(q - q1);
end
% 显示分析结果
fprintf('\n性能分析:\n');
fprintf('解号\t最小裕度(°)\t平均裕度(°)\t条件数\t\t可操作性\t距离\n');
for i = 1:n_solutions
fprintf('%d\t%.2f\t\t%.2f\t\t%.2f\t%.2e\t%.2f\n', i, ...
rad2deg(performance(i).min_joint_margin), ...
rad2deg(performance(i).avg_joint_margin), ...
performance(i).condition_number, ...
performance(i).manipulability, ...
performance(i).distance_to_q1);
end
% 7. 选择最佳解
[min_margin_norm, avg_margin_norm, cond_norm, manip_norm, dist_norm] = ...
deal(zeros(1,n_solutions));
for i = 1:n_solutions
min_margin_norm(i) = performance(i).min_joint_margin/max([performance.min_joint_margin]);
avg_margin_norm(i) = performance(i).avg_joint_margin/max([performance.avg_joint_margin]);
cond_norm(i) = 1 - performance(i).condition_number/max([performance.condition_number]);
manip_norm(i) = performance(i).manipulability/max([performance.manipulability]);
dist_norm(i) = 1 - performance(i).distance_to_q1/max([performance.distance_to_q1]);
end
weights = [0.3, 0.2, 0.2, 0.2, 0.1];
scores = weights(1)*min_margin_norm + weights(2)*avg_margin_norm + ...
weights(3)*cond_norm + weights(4)*manip_norm + weights(5)*dist_norm;
[best_score, best_idx] = max(scores);
fprintf('\n推荐最佳解: 解 %d (评分: %.2f)\n', best_idx, best_score);
fprintf('关节角度(度): [%s]\n', num2str(rad2deg(q_solutions(best_idx,:)), '%.2f '));
% 8. 可视化最佳解
figure('Name', '最佳解与初始解', 'Position', [100 100 1000 400]);
subplot(1,2,1);
robot.plot(q1);
title('初始构型');
subplot(1,2,2);
robot.plot(q_solutions(best_idx,:));
title('最佳逆解');
% 保存结果
save('ik_solutions.mat', 'q1', 'T1', 'q_solutions', 'performance', 'best_idx');
disp('程序执行完成,结果已保存到 ik_solutions.mat');初始关节角度q1 (弧度):
-2.3263 -0.1578 -3.1099 -0.0094 1.1567 2.8168
对应的角度值(度):
-133.2897 -9.0438 -178.1851 -0.5374 66.2759 161.3899
对应的末端位姿T1:
SE3 - 属性:
t: [3×1 double]
n: [3×1 double]
o: [3×1 double]
a: [3×1 double]
开始搜索不同逆解...
找到解 #1 (尝试次数: 1)
关节角度(度): [-133.29 -9.04 -178.19 -0.54 66.28 161.39]
位置误差: 3.27e-08
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.804856
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 2 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.65099
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 3 失败: q must have 6 columns
找到解 #2 (尝试次数: 4)
关节角度(度): [-133.29 -99.98 3.57 1.18 -24.54 160.10]
位置误差: 4.43e-08
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.948542
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 5 失败: q must have 6 columns
找到解 #3 (尝试次数: 6)
关节角度(度): [0.53 -170.96 3.57 137.67 68.06 151.60]
位置误差: 1.07e-08
找到解 #4 (尝试次数: 7)
关节角度(度): [-133.29 -9.04 -178.19 179.46 -66.28 -18.61]
位置误差: 7.53e-08
找到解 #5 (尝试次数: 8)
关节角度(度): [0.53 -80.02 -178.19 58.47 47.13 -95.15]
位置误差: 2.02e-08
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.731035
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 13 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 1.03344
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 14 失败: q must have 6 columns
找到解 #6 (尝试次数: 16)
关节角度(度): [0.53 -170.96 3.57 -42.33 -68.06 -28.40]
位置误差: 7.26e-08
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 1.13736
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 17 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.841746
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 19 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 1.04038
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 20 失败: q must have 6 columns
找到解 #7 (尝试次数: 23)
关节角度(度): [0.53 -80.02 -178.19 -121.53 -47.13 84.85]
位置误差: 4.61e-08
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.873305
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 24 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.763203
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 25 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.340692
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 26 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.577182
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 29 失败: q must have 6 columns
找到解 #8 (尝试次数: 31)
关节角度(度): [-133.29 -99.98 3.57 -178.82 24.54 -19.90]
位置误差: 3.46e-07
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.537613
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 36 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.866386
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 44 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 1.47284
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 45 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 1.12919
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 47 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.77377
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 48 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.910413
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 49 失败: q must have 6 columns
警告: ikine: rejected-step limit 100 exceeded (pose 1), final err 0.404742
> 位置:SerialLink/ikine (第 244 行)
位置: untitled (第 52 行)
警告: failed to converge: try a different initial value of joint coordinates
> 位置:SerialLink/ikine (第 273 行)
位置: untitled (第 52 行)
尝试 50 失败: q must have 6 columns
共找到 8 个不同的逆解:
警告: floor tiles too small, making them 2.000000 x bigger - change the size or disable them
> 位置:RTBPlot.create_tiled_floor (第 619 行)
位置: RTBPlot.create_floor (第 575 行)
位置: SerialLink/plot (第 250 行)
位置: untitled (第 102 行)
索引超过数组元素的数量。索引不能超过 4。
出错 SerialLink/plot>create_robot (第 468 行)
d = norm( d(4:6)-d(1:3) ) / 72;
^^^^^^
出错 SerialLink/plot (第 251 行)
handle = create_robot(robot, opt);
^^^^^^^^^^^^^^^^^^^^^^^^
出错 untitled (第 102 行)
robot.plot(q_solutions(i,:));
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
>>修改
最新发布