那么继续来分析,上一篇恶补了以下三次和五次多项式的公式,那到底怎么使用呢?继续来看。
(外篇:上一篇文章利用几何法进行推导二自由度的并联结构,非常巧妙,可以更好的理解运动学的向量和坐标关系)
轨迹规划涉及两个方面:运动路径规划和运动规律优选。
那么对于本并联机构来说,他的路径规划就是走一个门字形,而运动规律优选是指用什么办法可以让他走的平滑(平滑的意思可以理解为:运动起来不是那么僵硬和符合电机运动,比如不突然的抽抽,丝滑的运行)。
行走路径如上图所示: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多项式进行对比。