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

鱼会上树HY

已于 2024-07-21 11:46:50 修改

阅读量2k

点赞数 13

分类专栏：机器人学文章标签：机器人 matlab 开发语言

于 2024-07-17 20:00:08 首次发布

本文链接：https://blog.youkuaiyun.com/y12345655/article/details/140503957

版权

机器人学专栏收录该内容

23 篇文章

订阅专栏

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

笛卡尔空间的路径规划与其说是一个运动学问题，倒不如说是一个几何问题。比如笛卡尔空间的直线规划，给定起点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));

仿真验证