【机器人学】5-3.六自由度机器人轨迹规划-笛卡尔空间- 直线与圆弧规划【附MATLAB代码】

        空间机器人笛卡尔空间路径规划按照路径的性质可以分为笛卡尔点到点的路径规划以及笛卡尔连续路径规划。即通过事先给定空间机器人的目标点或者目标路径，通过运动学的计算得到空间机器人关节层轨迹，进而实现期望路径的跟踪。

        笛卡尔空间的路径规划与其说是一个运动学问题，倒不如说是一个几何问题。比如笛卡尔空间的直线规划，给定起点A和终点B，连接AB两点的向量，然后将这个向量离散化为N个点，对每个点求解逆运动学，即可实现机器人末端TCP完成一段直线轨迹。

               MATLAB代码

                直线规划

function point = my_Linetraj(points, step)
%直线规划
q = zeros(step, 5); %角度
point = zeros(step, 3);
% 角度变化
point(:, 1) = reshape(linspace(points(1, 1), points(2, 1), step), step, 1); %px
point(:, 2) = reshape(linspace(points(1, 2), points(2, 2), step), step, 1); %py
point(:, 3) = reshape(linspace(points(1, 3), points(2, 3), step), step, 1); %pz

                圆弧规划

function pointT = my_Arctraj(points, step)
%圆弧规划
q = zeros(2 * step, 5); %角度
t = zeros(2 * step, 1); %时间
% 求圆心和半径
syms x0 y0 z0
p1 = points(1, :);
p2 = points(2, :);
p3 = points(3, :);
x1 = p1(1); y1 = p1(2); z1 = p1(3);
x2 = p2(1); y2 = p2(2); z2 = p2(3);
x3 = p3(1); y3 = p3(2); z3 = p3(3);
a = (y2 - y1) * (z3 - z1) - (y3 - y1) * (z2 - z1);
b = (x3 - x1) * (z2 - z1) - (x2 - x1) * (z3 - z1);
c = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
d = a * x1 + b * y1 + c * z1;
eq(1) = a * x0 + b * y0 + c * z0 == d;
eq(2) = (x0 - 0.5 * (x1 + x2)) * (x2 - x1) + (y0 - 0.5 * (y1 + y2)) * (y2 - y1) + (z0 - 0.5 * (z1 + z2)) * (z2 - z1) == 0;
eq(3) = (x0 - 0.5 * (x2 + x3)) * (x3 - x2) + (y0 - 0.5 * (y2 + y3)) * (y3 - y2) + (z0 - 0.5 * (z2 + z3)) * (z3 - z2) == 0;
[x0, y0, z0] = solve(eq, x0, y0, z0);
p0 = double([x0, y0, z0]);
R = double(sqrt((x1 - x0).^2 + (y1 - y0).^2 + (z1 - z0).^2));
%求齐次变换矩阵
W = [a, b, c] / sqrt(a^2 + b^2 + c^2);
U = (p1 - p0) / R;
V = cross(W, U);
transT = [U', V', W', p0'; 0, 0, 0, 1];
%计算轨迹并转换
point1 = inv(transT) * [p1, 1]';
point2 = inv(transT) * [p2, 1]';
point3 = inv(transT) * [p3, 1]';
%求圆心角
if point3(2) < 0
    theta13 = atan2(point3(2), point3(1)) + 2 * pi;
else
    theta13 = atan2(point3(2), point3(1));
end
%转换到空间圆弧
j = 1;
for i = 0:theta13 / (2 * step - 1):theta13
    pointT(j, :) = transT * [R * cos(i), R * sin(i), 0, 1]';
    j = j + 1;
end

                测试代码

clc;
clear;
p_start = [200, 60,320];      %圆弧轨迹起始点
p_mid   = [0, 0,0];      %圆弧轨迹中间点
p_final = [230,200,510];      %圆弧轨迹终止点
step = 50;                    %步数
pointC = my_Arctraj([p_start;p_mid;p_final],step);
pointL=my_Linetraj([p_start;p_mid],step);
figure(1);
plot3(pointC(:,1),pointC(:,2),pointC(:,3));
%plot3(pointL(:,1),pointL(:,2),pointL(:,3));

        仿真验证

                ​​​​​​​        

下一章：【机器人学】6-1.六自由度机器人-运动学参数辨识-辨识数学模型的建立​​​​​​​