第四次作业

设计一个机械臂的运动轨迹，要求使用摆线速度来控制末端位置、速度和加速度。插补周期为10毫秒，最大速度为1000毫米/秒。

求解流程

计算步骤

  1. 定义参数：

    • 连杆长度：\( l_1 = 0.3 \, \text{m} \)，\( l_2 = 0.65 \, \text{m} \)

    • 距离和高度：\( S = b = 0.4 \, \text{m} \)，\( H = 0.6 \, \text{m} \)

    • 最大速度：\( v_{\text{max}} = 1000 \, \text{mm/s} \)

    • 插补周期：\( \Delta t = 0.01 \, \text{s} \)

  2. 生成摆线速度曲线：

    • 使用摆线公式生成速度：\( v(t) = r \cdot (1 - \cos(2 \pi t / T)) \)

    • 计算位置轨迹：通过对速度进行数值积分得到位置 \( x(t) \)

  3. 求解反向运动学：

    • 对每个时间点计算关节角度：

      • 第二关节角度：\(\theta_2 = \text{atan2}(\sqrt{1 - D^2}, D)\)

      • 第一个关节角度：\(\theta_1 = \text{atan2}(y, x) - \text{atan2}(l_2 \cdot \sin(\theta_2), l_1 + l_2 \cdot \cos(\theta_2))\)

  4. 计算速度和加速度：

    • 使用差分法计算速度 \( v_x, v_y \) 和加速度 \( a_x, a_y \)

结果演示

  1. 绘制轨迹：

    • 显示机械臂末端的轨迹、速度和加速度曲线。

  2. 关节角度变化：

    • 绘制关节角度随时间变化的图像。

通过以上步骤，可以实现机械臂的平滑运动，并展示其位置、速度和加速度的变化。

matlab/octave

% Parameters
l1 = 0.3; % Length of the first link
l2 = 0.65; % Length of the second link
b = 0.4; % Length of the line
H = 0.6; % Vertical distance from root

% Time parameters
T = 2; % Total time for the movement
dt = 0.01; % Time step
t = 0:dt:T; % Time vector

% Trajectory (linear for simplicity)
x_start = -b/2;
x_end = b/2;
x_traj = linspace(x_start, x_end, length(t));
y_traj = -H * ones(size(x_traj));

% Preallocate arrays for joint angles, speed, and acceleration
theta1 = zeros(size(t));
theta2 = zeros(size(t));
phi1 = zeros(size(t));
phi2 = zeros(size(t));

% Calculate inverse kinematics
for i = 1:length(t)
    % Position of the end-effector
    x = x_traj(i);
    y = y_traj(i);
    
    % Inverse kinematics for branch A1-B1-E
    D = (x^2 + y^2 - l1^2 - l2^2) / (2 * l1 * l2);
    theta2(i) = atan2(sqrt(1 - D^2), D);
    theta1(i) = atan2(y, x) - atan2(l2 * sin(theta2(i)), l1 + l2 * cos(theta2(i)));
    
    % Inverse kinematics for branch A2-B2-E (mirrored)
    phi2(i) = -theta2(i);
    phi1(i) = atan2(y, x) + atan2(l2 * sin(phi2(i)), l1 + l2 * cos(phi2(i)));
end

% Calculate speed and acceleration
vx = diff(x_traj) / dt;
vy = diff(y_traj) / dt;
ax = diff(vx) / dt;
ay = diff(vy) / dt;

% Plotting
figure;
subplot(4, 1, 1);
plot(t, x_traj, 'r', t, y_traj, 'b');
title('End-Effector Trajectory');
xlabel('Time (s)');
ylabel('Position (m)');
legend('x', 'y');

subplot(4, 1, 2);
plot(t(1:end-1), vx, 'r', t(1:end-1), vy, 'b');
title('End-Effector Speed');
xlabel('Time (s)');
ylabel('Speed (m/s)');
legend('v_x', 'v_y');

subplot(4, 1, 3);
plot(t(1:end-2), ax, 'r', t(1:end-2), ay, 'b');
title('End-Effector Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (m/s^2)');
legend('a_x', 'a_y');

subplot(4, 1, 4);
plot(t, theta1, 'r', t, theta2, 'b');
title('Joint Angles for Branch A1-B1-E');
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('\theta_1', '\theta_2');

figure;
plot(t, phi1, 'r', t, phi2, 'b');
title('Joint Angles for Branch A2-B2-E');
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('\phi_1', '\phi_2');

% Parameters
l1 = 0.3; % Length of the first link
l2 = 0.65; % Length of the second link
b = 0.4; % Length of the line
H = 0.6; % Vertical distance from root

% Time parameters
T = 2; % Total time for the movement
dt = 0.01; % Time step
t = 0:dt:T; % Time vector

% S-curve parameters
t_acc = T/4; % Acceleration time
t_dec = T/4; % Deceleration time
t_const = T - t_acc - t_dec; % Constant speed time

% S-curve velocity profile
v_max = (x_end - x_start) / (t_const + 0.5 * (t_acc + t_dec));
v_traj = zeros(size(t));

for i = 1:length(t)
    if t(i) < t_acc
        v_traj(i) = v_max * (t(i) / t_acc)^2;
    elseif t(i) < (t_acc + t_const)
        v_traj(i) = v_max;
    else
        v_traj(i) = v_max * (1 - ((t(i) - t_acc - t_const) / t_dec)^2);
    end
end

% Position trajectory using S-curve velocity profile
x_traj = x_start + cumtrapz(t, v_traj);
y_traj = -H * ones(size(x_traj));

% Preallocate arrays for joint angles
theta1 = zeros(size(t));
theta2 = zeros(size(t));
phi1 = zeros(size(t));
phi2 = zeros(size(t));

% Calculate inverse kinematics
for i = 1:length(t)
    % Position of the end-effector
    x = x_traj(i);
    y = y_traj(i);
    
    % Inverse kinematics for branch A1-B1-E
    D = (x^2 + y^2 - l1^2 - l2^2) / (2 * l1 * l2);
    theta2(i) = atan2(sqrt(1 - D^2), D);
    theta1(i) = atan2(y, x) - atan2(l2 * sin(theta2(i)), l1 + l2 * cos(theta2(i)));
    
    % Inverse kinematics for branch A2-B2-E (mirrored)
    phi2(i) = -theta2(i);
    phi1(i) = atan2(y, x) + atan2(l2 * sin(phi2(i)), l1 + l2 * cos(phi2(i)));
end

% Calculate speed and acceleration
vx = diff(x_traj) / dt;
vy = diff(y_traj) / dt;
ax = diff(vx) / dt;
ay = diff(vy) / dt;

% Plotting
figure;
subplot(4, 1, 1);
plot(t, x_traj, 'r', t, y_traj, 'b');
title('End-Effector Trajectory');
xlabel('Time (s)');
ylabel('Position (m)');
legend('x', 'y');

subplot(4, 1, 2);
plot(t(1:end-1), vx, 'r', t(1:end-1), vy, 'b');
title('End-Effector Speed');
xlabel('Time (s)');
ylabel('Speed (m/s)');
legend('v_x', 'v_y');

subplot(4, 1, 3);
plot(t(1:end-2), ax, 'r', t(1:end-2), ay, 'b');
title('End-Effector Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (m/s^2)');
legend('a_x', 'a_y');

subplot(4, 1, 4);
plot(t, theta1, 'r', t, theta2, 'b');
title('Joint Angles for Branch A1-B1-E');
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('\theta_1', '\theta_2');

figure;
plot(t, phi1, 'r', t, phi2, 'b');
title('Joint Angles for Branch A2-B2-E');
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('\phi_1', '\phi_2');

% Parameters
l1 = 0.3; % Length of the first link
l2 = 0.65; % Length of the second link
b = 0.4; % Length of the line
H = 0.6; % Vertical distance from root

% Time parameters
T = 2; % Total time for the movement
dt = 0.01; % Time step
t = 0:dt:T; % Time vector

% Cycloid parameters
r = b / (2 * pi); % Radius to match the total distance
a = 1; % Ratio for normal cycloid

% Cycloid velocity profile
v_traj = r * (1 - cos(2 * pi * t / T));

% Position trajectory using cycloid velocity profile
x_traj = x_start + cumtrapz(t, v_traj);
y_traj = -H * ones(size(x_traj));

% Preallocate arrays for joint angles
theta1 = zeros(size(t));
theta2 = zeros(size(t));
phi1 = zeros(size(t));
phi2 = zeros(size(t));

% Calculate inverse kinematics
for i = 1:length(t)
    % Position of the end-effector
    x = x_traj(i);
    y = y_traj(i);
    
    % Inverse kinematics for branch A1-B1-E
    D = (x^2 + y^2 - l1^2 - l2^2) / (2 * l1 * l2);
    theta2(i) = atan2(sqrt(1 - D^2), D);
    theta1(i) = atan2(y, x) - atan2(l2 * sin(theta2(i)), l1 + l2 * cos(theta2(i)));
    
    % Inverse kinematics for branch A2-B2-E (mirrored)
    phi2(i) = -theta2(i);
    phi1(i) = atan2(y, x) + atan2(l2 * sin(phi2(i)), l1 + l2 * cos(phi2(i)));
end

% Calculate speed and acceleration
vx = diff(x_traj) / dt;
vy = diff(y_traj) / dt;
ax = diff(vx) / dt;
ay = diff(vy) / dt;

% Plotting
figure;
subplot(4, 1, 1);
plot(t, x_traj, 'r', t, y_traj, 'b');
title('End-Effector Trajectory');
xlabel('Time (s)');
ylabel('Position (m)');
legend('x', 'y');

subplot(4, 1, 2);
plot(t(1:end-1), vx, 'r', t(1:end-1), vy, 'b');
title('End-Effector Speed');
xlabel('Time (s)');
ylabel('Speed (m/s)');
legend('v_x', 'v_y');

subplot(4, 1, 3);
plot(t(1:end-2), ax, 'r', t(1:end-2), ay, 'b');
title('End-Effector Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (m/s^2)');
legend('a_x', 'a_y');

subplot(4, 1, 4);
plot(t, theta1, 'r', t, theta2, 'b');
title('Joint Angles for Branch A1-B1-E');
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('\theta_1', '\theta_2');

figure;
plot(t, phi1, 'r', t, phi2, 'b');
title('Joint Angles for Branch A2-B2-E');
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('\phi_1', '\phi_2');