# 毕设(5)—笛卡尔空间轨迹规划（直线、圆弧）

## 直线轨迹规划

$L=\sqrt{(x_1-x_0)^2+(y_1-y_0)^2+(z_1-z_0)^2}$

$d=v\cdot t$

d为一个插补周期内移动的距离，t为插补的时间间隔

## 圆弧轨迹规划

1. 求出$$P_{123}$$$$P_{12}$$$$P_{23}$$的方程，通过三个平面方程获得圆心$$O_1$$坐标和圆弧半径
2. $$P_{123}$$平面建立新坐标系，计算两个坐标系之间的齐次变换矩阵
3. $$P_{123}$$平面计算平面圆弧的轨迹，通过变换矩阵转换为空间圆弧轨迹
4. 圆弧轨迹通过运动学逆解转换为各关节角度变化序列

## Matlab代码验证

test4.m

clear, clc, close all;

L(1) = Link([0 0 0 0 0 0], 'modified');
L(2) = Link([0 0 0 -pi / 2 0 0], 'modified');
L(3) = Link([0 0 135 0 0 0], 'modified');
L(4) = Link([0 0 147 0 0 0], 'modified');
L(5) = Link([0 131 61 -pi / 2 0 0], 'modified');

angle1 = [-pi / 3, -pi / 9, pi / 3, -2 * pi / 9, 0];
angle2 = [-pi / 9, -pi / 3, pi / 2, -pi / 6, pi / 9];
angle3 = [7 * pi / 36, -pi / 4, 5 * pi / 12, -pi / 6, 2 * pi / 9];
angle4 = [4 * pi / 9, -pi / 9, 7 * pi / 18, -5 * pi / 18, pi / 3];
angleT = [angle1; angle2; angle3];

for i = 1:size(angleT, 1)
fk = myfkine(angleT(i, :));
points(i, :) = [fk(1, 4), fk(2, 4), fk(3, 4)];
theta5(i) = angleT(i, 5);
end
[q, t] = line_traj(points, theta5, [0, 5], 100);
%[q, t] = arc_traj(points, theta5, [0, 5, 10], 40);

figure(1);
T = zeros(4, 4, size(q, 1));
for i = 1:size(q, 1)
T(:, :, i) = myfkine(q(i, :));
end
plot3(squeeze(T(1, 4, :)), squeeze(T(2, 4, :)), squeeze(T(3, 4, :)), 'r-', 'LineWidth', 2);
title('轨迹图');
hold on;
plot3(points(:, 1), points(:, 2), points(:, 3), 'bo', 'MarkerSize', 7, 'LineWidth', 2);
hold on;
robot.plot(q);
grid on;
hold on;

figure(2);
plot(t, qdeg);
title('角度图');
legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5');
xlabel('t(s)');
ylabel('deg(°)');
grid on;
hold on;


 直线轨迹图 直线角度图

 圆弧轨迹图 圆弧角度图

for i = 0:theta13 / (2 * step - 1):theta13 %这个地方我直接减1了，原来我写的函数是没有这个的
pointT(j, :) = transT * [R * cos(i), R * sin(i), 0, 1]';
j = j + 1;
end


posted @ 2022-06-05 17:03  Dragonet-Z  阅读(1932)  评论(0编辑  收藏  举报