%% 机械臂PTP轨迹控制 - 修复版
clear; clc; close all;
% ===========================================
% 1. 机械臂模型定义 (Modified D-H参数)
% ===========================================
% 创建机械臂模型
L1 = 400; % 基座高度 (mm)
L2 = 500; % 上臂长度 (mm)
L3 = 500; % 前臂长度 (mm)
L4 = 100; % 工具长度 (mm)
% 定义D-H参数表 [a, alpha, d, theta]
% 格式: [连杆长度, 连杆扭角, 连杆偏移, 关节角度]
dh_params = [
0, 0, L1, 0; % 关节1
0, -pi/2, 0, 0; % 关节2
L2, 0, 0, 0; % 关节3
L3, -pi/2, 0, 0; % 关节4
0, pi/2, 0, 0; % 关节5
0, -pi/2, L4, 0 % 关节6
];
% 关节限制 (弧度)
joint_limits = [
-pi, pi; % 关节1
-pi/2, pi/2; % 关节2
-pi/2, pi/2; % 关节3
-pi, pi; % 关节4
0, pi; % 关节5
-pi, pi % 关节6
];
% ===========================================
% 主程序 - PTP轨迹控制
% ===========================================
% 选择工作空间中的两点 (基坐标系)
fprintf('选择工作空间中的两点...\n');
P_start = [400; 200; 600]; % 起始点 (mm)
P_end = [-300; 500; 400]; % 终止点 (mm)
% 定义姿态 (固定姿态)
R = [1, 0, 0;
0, -1, 0;
0, 0, -1]; % 简单旋转矩阵
% 创建齐次变换矩阵
T_start = [R, P_start; 0, 0, 0, 1];
T_end = [R, P_end; 0, 0, 0, 1];
% 使用自定义逆运动学求解
fprintf('计算起始点逆运动学...\n');
[q_start_custom, sols_start] = inverse_kinematics(T_start, dh_params, joint_limits);
fprintf('计算终止点逆运动学...\n');
[q_end_custom, sols_end] = inverse_kinematics(T_end, dh_params, joint_limits);
% 使用Robotics Toolbox进行比较 (如果可用)
try
fprintf('使用Robotics Toolbox求解...\n');
% 创建机器人模型
L(1) = Link('a', dh_params(1,1), 'alpha', dh_params(1,2), 'd', dh_params(1,3), 'offset', 0);
L(2) = Link('a', dh_params(2,1), 'alpha', dh_params(2,2), 'd', dh_params(2,3), 'offset', 0);
L(3) = Link('a', dh_params(3,1), 'alpha', dh_params(3,2), 'd', dh_params(3,3), 'offset', 0);
L(4) = Link('a', dh_params(4,1), 'alpha', dh_params(4,2), 'd', dh_params(4,3), 'offset', 0);
L(5) = Link('a', dh_params(5,1), 'alpha', dh_params(5,2), 'd', dh_params(5,3), 'offset', 0);
L(6) = Link('a', dh_params(6,1), 'alpha', dh_params(6,2), 'd', dh_params(6,3), 'offset', 0);
robot = SerialLink(L, 'name', '6-DOF Robot');
% 设置关节限制
for i = 1:6
robot.links(i).qlim = joint_limits(i,:);
end
% 求解逆运动学
q_start_toolbox = robot.ikine(T_start, 'q0', q_start_custom);
q_end_toolbox = robot.ikine(T_end, 'q0', q_end_custom);
% 比较结果
fprintf('\n逆运动学结果比较:\n');
fprintf('起始点 - 自定义解: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f] rad\n', q_start_custom);
fprintf('起始点 - 工具箱解: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f] rad\n', q_start_toolbox);
fprintf('终止点 - 自定义解: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f] rad\n', q_end_custom);
fprintf('终止点 - 工具箱解: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f] rad\n', q_end_toolbox);
% 使用工具箱解进行轨迹规划
q_start = q_start_toolbox;
q_end = q_end_toolbox;
catch ME
fprintf('Robotics Toolbox未安装或出错: %s\n', ME.message);
fprintf('使用自定义解\n');
q_start = q_start_custom;
q_end = q_end_custom;
end
% PTP轨迹规划参数
T_total = 5; % 总时间 (s)
dt = 0.02; % 时间步长 (s)
% 生成轨迹
fprintf('生成PTP轨迹...\n');
[q_traj, qd_traj, qdd_traj, t] = ptp_trajectory(q_start, q_end, T_total, dt);
% ===========================================
% 绘制关节轨迹曲线
% ===========================================
% 创建图形
figure('Name', '关节轨迹曲线', 'Color', 'white', 'Position', [100, 100, 1200, 800]);
% 关节角度
subplot(3,1,1);
hold on;
for i = 1:6
plot(t, rad2deg(q_traj(:,i)), 'LineWidth', 1.5);
end
title('关节角度');
ylabel('角度 (°)');
legend('关节1', '关节2', '关节3', '关节4', '关节5', '关节6', 'Location', 'best');
grid on;
% 关节角速度
subplot(3,1,2);
hold on;
for i = 1:6
plot(t, rad2deg(qd_traj(:,i)), 'LineWidth', 1.5);
end
title('关节角速度');
ylabel('速度 (°/s)');
grid on;
% 关节角加速度
subplot(3,1,3);
hold on;
for i = 1:6
plot(t, rad2deg(qdd_traj(:,i)), 'LineWidth', 1.5);
end
title('关节角加速度');
xlabel('时间 (s)');
ylabel('加速度 (°/s²)');
grid on;
% 添加运动学验证
fprintf('\n轨迹验证:\n');
fprintf('起始点: [%.2f, %.2f, %.2f] mm\n', P_start);
T_start_check = forward_kinematics(q_traj(1,:), dh_params);
fprintf('计算起始点: [%.2f, %.2f, %.2f] mm (误差: %.4f mm)\n', ...
T_start_check(1:3,4), norm(T_start_check(1:3,4)-P_start));
fprintf('终止点: [%.2f, %.2f, %.2f] mm\n', P_end);
T_end_check = forward_kinematics(q_traj(end,:), dh_params);
fprintf('计算终止点: [%.2f, %.2f, %.2f] mm (误差: %.4f mm)\n', ...
T_end_check(1:3,4), norm(T_end_check(1:3,4)-P_end));
% ===========================================
% 函数定义 (必须放在主程序之后)
% ===========================================
% 逆运动学求解函数 (解析法)
function [q, solutions] = inverse_kinematics(T_target, dh_params, joint_limits)
% 提取目标位置和姿态
P_target = T_target(1:3,4);
R_target = T_target(1:3,1:3);
% 提取DH参数
a = dh_params(:,1)';
alpha = dh_params(:,2)';
d = dh_params(:,3)';
% 计算关节1的角度
theta1 = atan2(P_target(2), P_target(1));
% 计算腕部中心位置
Pw = P_target - R_target*[0;0;d(6)];
dist = sqrt(Pw(1)^2 + Pw(2)^2 + (Pw(3)-d(1))^2);
% 计算关节3的角度
cos_theta3 = (dist^2 - a(3)^2 - a(4)^2)/(2*a(3)*a(4));
% 检查可解性
if abs(cos_theta3) > 1
error('目标位置超出工作空间范围! dist=%.2f, a3=%.2f, a4=%.2f', dist, a(3), a(4));
end
sin_theta3 = sqrt(1 - cos_theta3^2); % 取正解 (肘部向上)
theta3 = atan2(sin_theta3, cos_theta3);
% 计算关节2的角度
beta = atan2(Pw(3)-d(1), sqrt(Pw(1)^2 + Pw(2)^2));
gamma = atan2(a(4)*sin_theta3, a(3) + a(4)*cos_theta3);
theta2 = beta - gamma;
% 计算腕部姿态关节 (关节4,5,6)
% 修复: 使用临时变量存储角度向量
angles = [theta1, theta2, theta3];
T03 = eye(4);
for i = 1:3
ct = cos(angles(i));
st = sin(angles(i));
ca = cos(alpha(i));
sa = sin(alpha(i));
Ti = [ct, -st*ca, st*sa, a(i)*ct;
st, ct*ca, -ct*sa, a(i)*st;
0, sa, ca, d(i);
0, 0, 0, 1];
T03 = T03 * Ti;
end
% 计算目标腕部姿态 (使用转置代替逆矩阵)
T03_rot = T03(1:3,1:3);
T36_target = T03_rot' * R_target;
% 提取欧拉角 (ZYZ旋转)
theta5 = atan2(sqrt(T36_target(1,3)^2 + T36_target(2,3)^2), T36_target(3,3));
if abs(theta5) > eps && abs(sin(theta5)) > eps
theta4 = atan2(T36_target(2,3)/sin(theta5), T36_target(1,3)/sin(theta5));
theta6 = atan2(T36_target(3,2)/sin(theta5), -T36_target(3,1)/sin(theta5));
else
% 奇异位形处理
theta4 = 0;
theta6 = atan2(T36_target(1,2), T36_target(1,1));
end
% 组装关节角度
q = [theta1, theta2, theta3, theta4, theta5, theta6];
% 生成多解方案 (4种常见配置)
solutions = zeros(4,6);
% 解1: 默认解 (肘部向上)
solutions(1,:) = q;
% 解2: 肘部向下
solutions(2,:) = [theta1, theta2, -theta3, theta4+pi, -theta5, theta6+pi];
% 解3: 肩部左侧
solutions(3,:) = [theta1+pi, pi-theta2, theta3, theta4, theta5, theta6];
% 解4: 肩部右侧
solutions(4,:) = [theta1+pi, pi-theta2, -theta3, theta4+pi, -theta5, theta6+pi];
% 筛选有效解 (在关节限制内)
valid_solutions = [];
for i = 1:size(solutions,1)
valid = true;
for j = 1:6
if solutions(i,j) < joint_limits(j,1) || solutions(i,j) > joint_limits(j,2)
valid = false;
break;
end
end
if valid
valid_solutions = [valid_solutions; solutions(i,:)];
end
end
% 如果没有有效解，使用最近解
if isempty(valid_solutions)
[~, idx] = min(sum(abs(solutions),2));
q = solutions(idx,:);
warning('无完全有效解，使用最近解');
else
% 选择关节变化最小的解
if size(valid_solutions,1) > 1
joint_diff = sum(abs(diff(valid_solutions,1,1)),2);
[~, min_idx] = min(joint_diff);
q = valid_solutions(min_idx,:);
else
q = valid_solutions(1,:);
end
end
end
% 轨迹规划函数 (5次多项式插值)
function [q_traj, qd_traj, qdd_traj, t] = ptp_trajectory(q_start, q_end, T, dt)
% 参数:
% q_start: 起始关节角度 (1x6)
% q_end: 终止关节角度 (1x6)
% T: 总时间 (s)
% dt: 时间步长 (s)
t = 0:dt:T; % 时间向量
n = length(t);
num_joints = length(q_start);
% 初始化轨迹
q_traj = zeros(n, num_joints);
qd_traj = zeros(n, num_joints);
qdd_traj = zeros(n, num_joints);
for i = 1:num_joints
% 计算多项式系数 (满足边界条件: 位置和速度在起点和终点为0)
a0 = q_start(i);
a1 = 0;
a2 = 0;
a3 = (10*(q_end(i) - q_start(i)))/T^3;
a4 = (-15*(q_end(i) - q_start(i)))/T^4;
a5 = (6*(q_end(i) - q_start(i)))/T^5;
% 计算轨迹
for j = 1:n
tau = t(j);
% 位置
q_traj(j,i) = a0 + a1*tau + a2*tau^2 + a3*tau^3 + a4*tau^4 + a5*tau^5;
% 速度
qd_traj(j,i) = a1 + 2*a2*tau + 3*a3*tau^2 + 4*a4*tau^3 + 5*a5*tau^4;
% 加速度
qdd_traj(j,i) = 2*a2 + 6*a3*tau + 12*a4*tau^2 + 20*a5*tau^3;
end
end
end
% 正运动学函数
function T = forward_kinematics(q, dh_params)
T = eye(4);
for i = 1:size(dh_params,1)
a = dh_params(i,1);
alpha = dh_params(i,2);
d = dh_params(i,3);
theta = q(i) + dh_params(i,4); % 包含初始偏移
ct = cos(theta);
st = sin(theta);
ca = cos(alpha);
sa = sin(alpha);
Ti = [ct, -st*ca, st*sa, a*ct;
st, ct*ca, -ct*sa, a*st;
0, sa, ca, d;
0, 0, 0, 1];
T = T * Ti;
end
end
（误差很大）
最新发布
安装最新版Animate Diff时报错，原因是Animate diff适配了新版webui1.6，1.5版本安装会出问题。解决方法有两个，一是使用webui1.6，二是切换Animate Diff至旧版，可通过秋叶菩萨整合包的版本管理模块操作，如切换到9月15日版本。
扫一扫
8485
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
