利用矢量法求解并联机器人的3-4-5多项式的路径规划

awawaawawa

已于 2024-08-18 21:35:23 修改

阅读量764

点赞数 18

文章标签：机器人

于 2024-08-17 00:12:52 首次发布

本文链接：https://blog.csdn.net/carryoutw/article/details/141270956

版权

那么继续来分析，上一篇恶补了以下三次和五次多项式的公式，那到底怎么使用呢？继续来看。

（外篇：上一篇文章利用几何法进行推导二自由度的并联结构，非常巧妙，可以更好的理解运动学的向量和坐标关系）

轨迹规划涉及两个方面：运动路径规划和运动规律优选。

那么对于本并联机构来说，他的路径规划就是走一个门字形，而运动规律优选是指用什么办法可以让他走的平滑（平滑的意思可以理解为：运动起来不是那么僵硬和符合电机运动，比如不突然的抽抽，丝滑的运行）。

行走路径如上图所示：x’ 与x轴平行，y’ 与y轴重合，距离都是已知的，H，h，hw，hs，bw等等。

那么走这个门字形是有7个点，即p1,p2,p3,p4,p5,p6,p7。其中原点O’ 在oxy中的坐标是（0，hs-H-h）（不难理解吧，换一句话说O’ 的位置在Oxy轴的负y轴上，距离是hs-H-h）

那么其他路径点的坐标是：

上面应该很好理解，根据图就可以知道，但是P3x，P5x，P6y，P2y是不知道的，他们的xy坐标有些是不知道的。

那接下来，如何平滑的走过这7个点呢？这里采用的是3-4-5多项式，主要采用的是五次数多项式进行平滑运动。机械手的运动轨迹由上升段—平移段—下降段构成。设定完成上升段/下降段(即沿 y 轴提升/下降hw ) 所需时间为T1 ，完成平移段(即沿 x 轴平移bw ) 所需时间为T2，（这里需要注意的是：分方向的，就是分x轴和y轴的，这点非常重要），下面就是具体的路径规划策略。

利用五次多项式进行描述：即

公式推导如下：

前提是：初始的位置为0，初始速度为0，初始加速度为0，终止位置为S，终止速度为0，终止加速度也为0，我们可以推导出五次多项式的通式：

继续推导：为了可以进行高速运动（这个是这篇文章的重点，高速！！），根据上面的条件可以控制的是位置时间，和速度和加速度了，而本文采用的是速度和加速度。

即：对a（t）再一次求导，可以知道极值，三次多项式的极值有2，一个极大值，一个极小值，而减去的那个始终是极大值，这个是初中学的一些知识，可以自己看一下，那么，a（t）的极大值是

1/2-根号3/6（加速度的max值），之后带入a（t）表达式，并进行计算。

带入后并进行计算， amax得5.7735S/T^2，这也就是式子中为啥有5.7735这个变量了。

之前说过，此设备是高速的，要进行对速度和加速度的关系，所以在s（t），v（t），a（t）上带有amax的式子，一是可以更好地根据加速度这个变量来控制运动的轨迹，二是实现高速！！高速！！，所以就是式子15的由来，个人感觉特别的神奇，多日后定会在看的。

那么知道了运动路径规划和运动规律优选，那么结合起来。重点哟。

上来几个图片，这个就是五次多项式和路径点的结合，就可以控制丝滑+高速运动了。怎么来的呢，一步步分析。

首先要明白T1和T2是什么，走的那一部分。注意起点是最左右的两个点，即p1或者p7

通过分析知道，总时间是T1+T2（看图就可以知道），之后我们推理得到

我们来分析关系x轴的

当时间小于T1/2时候，最终点的位置在P1和P2的中间，那么x坐标是不变的（如图所示），即是-bw/2。
当时间在中间的时候，（有人会问了为什么是T2+T1/2呢，解释一波，走S距离用的总时间是T2+T1，p1点到p3点用的时间是T1，p3到p5是T2-T1，p5到p6也是T1/2，那么最后一段开始p6到p7是不是T2+T1/2），当所用时间在区间的时候，最终点落在p2到p5这段上的任意位置，假如落在p2到p3的坡上，那么x方向走的距离是不是S（t-T1/2），T1/2是指p1到p2的距离，x方向距离为0，而S（t-T1/2）是指终点落在p2到p3区间在Oxy坐标x轴的坐标，好好想一下，问题应该不是很大，在假如终点落在p4到p5的中间任意位置，那么他走过的x轴是不是也是S（t-T1/2），此时t很大，最终得到的结果是比bw/2大的，那么位置是不是也是S（t-T1/2）-bw/2
最后一段，x方向并没有运动，所以坐标是bw/2。

Y轴的继续分析。

y轴我们发现，p1点到p3点，y轴方向上均有位置变换，所以节点是T1，还记得上面说的么p3到p5的时间是T2-T1，那么终点落在中间时候，其实已经走过了时间T1，所以到达最远点p5的时间是T2，而最后一段是到T2+T1，应该没有问题的。详细分析：

当时间小于T1时刻，在p1和p2中间很好理解，s（t）走过的路程（y轴）加上hs 就等于最低点到终点的y轴方向的位置，那么对于Oxy坐标系来说，是不是就等于s（t）+hs-（H+h）了，其中H+h是指Oxy坐标系到最低端的距离。终点落在p2和p3之间也是同样的道理。
那么当t大于T1，小于T2时刻，即p3和p5之间，我们发现y方向上并没有变化，此时的坐标是hs+hw-(H+h)。
当最后一段时刻，当终点落在p5和p6中间时，s（t-T2），t一定大于T2的，所以需要减去s（T2）的y距离，得到终点在p5的位置，那么终点在Oxy中是不是用hs+hw-(H+h)-s(t-T2)组成的？

是不是以上的推导很有趣，也很佩服推理出的这些人。哈哈我也是。

那么最后这些点的坐标是怎么求出来的：

分析：P2y和P6y在y轴坐标（以上和以下的x轴坐标和y轴坐标都是基于Oxy坐标系的）是一样的，

我们根据设定的位置p2和p6处于hw的一半，即坐标是hw/2+hs-（H+h）

P3x 是把p2点作为初始出发点，（因为p1到p2的时间等于p2到p3的，p1到p2的x轴距离为0），在x轴上所走的距离是s(T1/2)，在Oxy中的坐标就是s(T1/2)-bw/2。

P5x是，从p1点出发点，有人会问了，上面是p2为啥这里是p1，哈哈，大家可以自己想一想（提示：x轴啊！！）从p1到p5总时间是T1/2+T1/2+T2-T1 = T2，那么实际x方向的距离其实是p2到p5的x轴方向的距离，减去bw/2，是不是就是正数p5的x轴坐标。

以下是matlab函数的图像：

设：最大的加速度为6.1，保证了可以高速运行和安全。

x轴：

% 参数设置
T1 = 0.5; % T1 的值
T2 = 0.7; % T2 的值
T = T2 + T1; % 总时间
bw = 1; % 宽度
s = 1; % 距离
amax = 6.1;

% 自定义时间向量 t
t = linspace(0, T, 1000); % 时间向量

% 初始化距离、速度和加速度函数
distance = zeros(size(t));
velocity = zeros(size(t));
acceleration = zeros(size(t));

% 计算距离、速度和加速度函数
for i = 1:length(t)
    if t(i) <= T1 / 2
        distance(i) = -bw / 2; % 前半段，保持在 -bw/2
        velocity(i) = 0; % 前半段，速度为 0
        acceleration(i) = 0; % 前半段，加速度为 0
    elseif t(i) > T1 / 2 && t(i) <= T2 + T1 / 2
        c = (t(i) - T1 / 2) / T;
        distance(i) = (1 / 5.7735) * amax * T^2 * (10 * c^3 - 15 * c^4 + 6 * c^5) - bw / 2;
        velocity(i) = (1 / 5.7735) * amax * T * (30 * c^2 - 60 * c^3 + 30 * c^4);
        acceleration(i) = (1 / 5.7735) * amax * (60 * c - 180 * c^2 + 120 * c^3);
    else
        distance(i) = bw / 2; % 后半段，保持在 bw/2
        velocity(i) = 0; % 后半段，速度为 0
        acceleration(i) = 0; % 后半段，加速度为 0
    end
end

% 绘制距离函数
figure;
plot(t, distance, 'LineWidth', 1.5)
xlabel('时间 (t)')
ylabel('距离')
title('距离随时间变化图')
grid on

% 绘制速度函数
figure;
plot(t, velocity, 'LineWidth', 1.5)
xlabel('时间 (t)')
ylabel('速度')
title('速度随时间变化图')
grid on

% 绘制加速度函数
figure;
plot(t, acceleration, 'LineWidth', 1.5)
xlabel('时间 (t)')
ylabel('加速度')
title('加速度随时间变化图')
grid on

Y轴的：

% 参数设置
clc;clear;
T1 = 0.5; % T1 的值
T2 = 0.7; % T2 的值
T = T2 + T1; % 总时间
bw = 1; % 宽度
s = 1; % 距离
amax = 6.1;
hw = 0.77; % hw 的值
hs = 0.5; % hs 的值
h = 1.2;
H = 1.6; % H 的值

% 自定义时间向量 t
t = linspace(0, T, 1000); % 时间向量

% 初始化距离、速度和加速度函数
distance = zeros(size(t));
velocity = zeros(size(t));
acceleration = zeros(size(t));

% 计算距离、速度和加速度函数
for i = 1:length(t)
    if t(i) <= T1
         c = t(i);
         s_T = 1/5.7735*amax*T^2 *(10 * c^3 - 15 * c^4 + 6 * c^5) ;
        distance(i) = s_T + hs - (H + h); % 前半段，距离等于 s(t) + hs - (H + h)
        velocity(i) = (1 / 5.7735) * amax * T * (30 * c^2 - 60 * c^3 + 30 * c^4);
        acceleration(i) = (1 / 5.7735) * amax * (60 * c - 180 * c^2 + 120 * c^3);
  
    elseif t(i) > T1 && t(i) <= T2
        distance(i) = hs + hw - (H + h); % 中间段，距离等于 hs + hw - (H + h)
        velocity(i) = 0; % 前半段，速度为 0
        acceleration(i) = 0; % 前半段，加速度为 0
  
    else
         c = t(i)-T2;
         s_T = 1/5.7735*amax*T^2 *(10 * c^3 - 15 * c^4 + 6 * c^5) ;
        distance(i) = hs + hw - (H + h) - s_T; % 后半段，距离等于 hs + hw - (H + h) - s(t - T2)
        velocity(i) = (1 / 5.7735) * amax * T * (30 * c^2 - 60 * c^3 + 30 * c^4);
        acceleration(i) = (1 / 5.7735) * amax * (60 * c - 180 * c^2 + 120 * c^3);
  
  
    end
end

% 绘制距离函数
figure;
plot(t, distance, 'LineWidth', 1.5)
xlabel('时间 (t)')
ylabel('距离')
title('距离随时间变化图')
grid on

% 绘制速度函数
figure;
plot(t, velocity, 'LineWidth', 1.5)
xlabel('时间 (t)')
ylabel('速度')
title('速度随时间变化图')
grid on

% 绘制加速度函数
figure;
plot(t, acceleration, 'LineWidth', 1.5)
xlabel('时间 (t)')
ylabel('加速度')
title('加速度随时间变化图')
grid on

通过上面的计算，我们知道了在同一时刻的xy值，也就就是对应的某坐标系的xy坐标，通过运动学逆解进行求出此时刻的theta1角度值。

坐标的计算：

工作空间：

% 参数设置
l1 = 10; % 第一个连杆的长度
l2 = 10; % 第二个连杆的长度
e = 4;

% 定义工作空间的范围
theta1_range = linspace(0, 2*pi, 100);
theta2_range = linspace(0, 2*pi, 100);

% 初始化工作空间坐标
X = [];
Y = [];

% 计算工作空间
for theta1 = theta1_range
    for theta2 = theta2_range
        % 计算末端执行器的坐标
        
C = 2 * e + l1 * (cos(theta1) + cos(theta2));
D = l1 * (sin(theta1) - sin(theta2));
K = -e * l1 * (cos(theta1) - cos(theta2));
M = 1 + (C^2 / D^2);
N = C * K / D^2 + e + l1 * cos(theta2) + l1 * C / D * sin(theta2);
H = K^2 / D^2 + e^2 + l1^2 - l2^2 + 2 * l1 * (K / D * sin(theta2) + e * cos(theta2));

x = (-N + sqrt(N^2 - M * H)) / M;
y = -(K + C * ((-N + sqrt(N^2 - M * H)) / M)) / D;
%         x = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
%         y = L1 * sin(theta1) + L2 * sin(theta1 + theta2);
        
        % 添加到工作空间坐标
        X = [X, x];
        Y = [Y, y];
    end
end

% 绘制工作空间
figure;
plot(X, Y, '.');
xlabel('X');
ylabel('Y');
title('二自由度并联结构的工作空间');
axis equal;
grid on;

我们发现那几个坐标都在重色的区域内，是可以运动到的。

到此！关于关于并联机构的操作空间的轨迹规划已全部完成！

下篇来看关于它的用 5 次非均匀 B 样条曲线构造关节空间的轨迹规划，并和3-4-5多项式进行对比。

awawaawawa

关注

18
点赞
踩
6

收藏

觉得还不错? 一键收藏
0
评论
利用矢量法求解并联机器人的3-4-5多项式的路径规划

之前说过，此设备是高速的，要进行对速度和加速度的关系，所以在s（t），v（t），a（t）上带有amax的式子，一是可以更好地根据加速度这个变量来控制运动的轨迹，二是实现高速！y轴我们发现，p1点到p3点，y轴方向上均有位置变换，所以节点是T1，还记得上面说的么p3到p5的时间是T2-T1，那么终点落在中间时候，其实已经走过了时间T1，所以到达最远点p5的时间是T2，而最后一段是到T2+T1，应该没有问题的。，根据上面的条件可以控制的是位置时间，和速度和加速度了，而本文采用的是速度和加速度。
复制链接

扫一扫