%% 修复后的机械臂轨迹规划系统
clear; clc; close all;
% ===========================================
% 改进的逆解选择函数
% ===========================================
function selected = select_solution(solutions, reference, joint_limits)
% 参数:
% solutions: N×6矩阵，所有可能的逆解
% reference: 1×6向量，参考关节角度
% joint_limits: 6×2矩阵，关节角度限位
if isempty(solutions)
error('逆运动学无解!');
end
% 步骤1: 筛选满足关节限位的解
valid_mask = true(size(solutions, 1), 1);
for i = 1:size(solutions, 1)
for j = 1:6
if solutions(i, j) < joint_limits(j, 1) || solutions(i, j) > joint_limits(j, 2)
valid_mask(i) = false;
break;
end
end
end
valid_solutions = solutions(valid_mask, :);
% 如果没有满足限位的解，则使用所有解
if isempty(valid_solutions)
warning('所有逆解均超出关节限位，使用最佳近似解');
valid_solutions = solutions;
end
% 步骤2: 计算各解与参考位置的关节位移
displacements = zeros(size(valid_solutions, 1), 1);
for i = 1:size(valid_solutions, 1)
displacements(i) = norm(valid_solutions(i, :) - reference);
end
% 步骤3: 避免奇异构型 (关节5接近0)
singularity_penalty = ones(size(valid_solutions, 1), 1);
for i = 1:size(valid_solutions, 1)
if abs(valid_solutions(i, 5)) < deg2rad(5) % 关节5小于5度
singularity_penalty(i) = 10; % 增加惩罚因子
end
end
% 步骤4: 选择综合最优解
[~, idx] = min(displacements .* singularity_penalty);
selected = valid_solutions(idx, :);
% 步骤5: 检查最终选择
for j = 1:6
if selected(j) < joint_limits(j, 1) || selected(j) > joint_limits(j, 2)
warning('选择的解超出关节%d限位: %.2f° (限位 %.2f°-%.2f°)', ...
j, rad2deg(selected(j)), ...
rad2deg(joint_limits(j, 1)), rad2deg(joint_limits(j, 2)));
end
end
end
% ===========================================
% 主程序修改部分
% ===========================================
% 机械臂参数 (Modified D-H)
a = [0, 0, 500, 500, 0, 0]; % 连杆长度(mm)
alpha = [0, -pi/2, 0, -pi/2, pi/2, -pi/2]; % 连杆扭角(rad)
d = [400, 0, 0, 0, 0, 100]; % 连杆偏移(mm)
joint_limits = [
-pi, pi; % 关节1
-pi/2, pi/2; % 关节2
-pi/2, pi/2; % 关节3
-pi, pi; % 关节4
0, pi; % 关节5
-pi, pi % 关节6
];
% 选择工作空间中的两点
P_start = [600; 200; 500];
P_goal = [300; -300; 600];
R = [1 0 0; 0 -1 0; 0 0 -1]; % 旋转矩阵(末端垂直向下)
T_start = [R, P_start; 0 0 0 1];
T_goal = [R, P_goal; 0 0 0 1];
% 自定义逆运动学求解
q_start_sols = inverse_kinematics(T_start, a, alpha, d, joint_limits);
q_goal_sols = inverse_kinematics(T_goal, a, alpha, d, joint_limits);
% 使用改进的选择函数
current_position = [0, 0, 0, 0, pi/2, 0]; % 假设初始位置
q_start = select_solution(q_start_sols, current_position, joint_limits);
q_goal = select_solution(q_goal_sols, q_start, joint_limits);
% 显示选择结果
fprintf('选择的初始关节角度: [%.2f°, %.2f°, %.2f°, %.2f°, %.2f°, %.2f°]\n', rad2deg(q_start));
fprintf('选择的终止关节角度: [%.2f°, %.2f°, %.2f°, %.2f°, %.2f°, %.2f°]\n', rad2deg(q_goal));
% ===========================================
% 增强的逆运动学函数
% ===========================================
function solutions = inverse_kinematics(T, a, alpha, d, joint_limits)
% 末端位置
P = T(1:3, 4);
x = P(1); y = P(2); z = P(3);
% 末端姿态
R = T(1:3, 1:3);
solutions = [];
% 关节1求解 (2个可能解)
theta1_options = [atan2(y, x), atan2(-y, -x)];
% 关节3参数计算
c3 = (x^2 + y^2 + (z - d(1))^2 - a(2)^2 - a(3)^2 - d(4)^2) / (2 * a(2) * a(3));
% 检查可达性
if abs(c3) > 1
return; % 点不可达
end
s3_options = [sqrt(1 - c3^2), -sqrt(1 - c3^2)];
for theta1 = theta1_options
for s3 = s3_options
theta3 = atan2(s3, c3);
% 关节2求解
A = a(2) + a(3)*c3;
B = a(3)*s3;
k1 = A*cos(theta1) + B*sin(theta1);
k2 = A*sin(theta1) - B*cos(theta1);
k3 = z - d(1);
% 解三角函数方程
denom = k1^2 + k2^2;
if abs(denom) < 1e-6
continue; % 避免除零
end
sin_theta2 = (k1*k3 + k2*sqrt(denom - k3^2)) / denom;
cos_theta2 = (k1 - k2*sin_theta2) / k1;
theta2 = atan2(sin_theta2, cos_theta2);
% 计算T03变换
T01 = dh_transform(a(1), alpha(1), d(1), theta1);
T12 = dh_transform(a(2), alpha(2), d(2), theta2);
T23 = dh_transform(a(3), alpha(3), d(3), theta3);
T03 = T01 * T12 * T23;
% 计算腕部旋转R36
R03 = T03(1:3, 1:3);
R36 = R03' * R;
% ZYZ欧拉角分解 (2组解)
[theta4, theta5, theta6] = zyz_euler(R36);
% 保存所有有效解
for k = 1:2
sol = [theta1, theta2, theta3, theta4(k), theta5(k), theta6(k)];
% 简单限位检查
valid = true;
for j = 1:6
if sol(j) < joint_limits(j, 1) || sol(j) > joint_limits(j, 2)
valid = false;
break;
end
end
if valid
solutions = [solutions; sol];
end
end
end
end
% 如果没有找到解，尝试数值优化
if isempty(solutions)
fprintf('解析法无解，尝试数值优化...\n');
options = optimoptions('fmincon', 'Display', 'off', ...
'Algorithm', 'sqp');
q0 = mean(joint_limits, 2)'; % 关节限位中值作为初始猜测
% 定义优化目标函数
objective = @(q) norm(forward_kinematics(q, a, alpha, d) - T, 'fro');
% 定义约束函数
constraint = @(q) deal([], ...
[q - joint_limits(:, 1); % 下限约束
joint_limits(:, 2) - q]); % 上限约束
% 执行优化
[q_opt, ~, exitflag] = fmincon(objective, q0, [], [], [], [], ...
joint_limits(:, 1), joint_limits(:, 2), ...
constraint, options);
if exitflag > 0
solutions = [solutions; q_opt];
end
end
end
% ===========================================
% 辅助函数
% ===========================================
% 单个D-H变换
function T = dh_transform(a, alpha, d, theta)
ct = cos(theta);
st = sin(theta);
ca = cos(alpha);
sa = sin(alpha);
T = [ct, -st*ca, st*sa, a*ct;
st, ct*ca, -ct*sa, a*st;
0, sa, ca, d;
0, 0, 0, 1];
end
% 完整正运动学
function T = forward_kinematics(q, a, alpha, d)
T = eye(4);
for i = 1:length(q)
T = T * dh_transform(a(i), alpha(i), d(i), q(i));
end
end
% ZYZ欧拉角分解 (改进版)
function [theta4, theta5, theta6] = zyz_euler(R)
% 处理数值精度问题
R = round(R, 10);
% 求解theta5
theta5 = atan2(sqrt(R(1,3)^2 + R(2,3)^2), R(3,3));
% 两种分支解
theta5_alt = -theta5;
% 求解theta4和theta6
if abs(theta5) > 1e-6
theta4 = atan2(R(2,3)/sin(theta5), R(1,3)/sin(theta5));
theta6 = atan2(R(3,2)/sin(theta5), -R(3,1)/sin(theta5));
theta4_alt = atan2(R(2,3)/sin(theta5_alt), R(1,3)/sin(theta5_alt));
theta6_alt = atan2(R(3,2)/sin(theta5_alt), -R(3,1)/sin(theta5_alt));
else % 奇异位置处理
theta4 = 0;
theta6 = atan2(-R(2,1), R(1,1));
theta4_alt = theta4;
theta6_alt = theta6;
end
% 返回两组解
theta4 = [theta4, theta4_alt];
theta5 = [theta5, theta5_alt];
theta6 = [theta6, theta6_alt];
end
% 轨迹规划函数 (与之前相同)
% ... [保持不变] ...
% 绘图函数 (与之前相同)
% ... [保持不变] ...
格式化输出日期与浮点数
本文介绍如何使用格式化字符串将int型日期转换为2位数显示形式，例如：2010年09月05日，并展示如何将float型变量输出为保留两位小数的形式。
扫一扫
1937
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
