利用矢量法求解并联机器人的轨迹规划

最新推荐文章于 2024-09-12 15:27:22 发布

网易足已

最新推荐文章于 2024-09-12 15:27:22 发布

阅读量457

点赞数 5

文章标签：机器人

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

版权

在开始对文章的轨迹规划，先写一下《机器人建模中和控制》的轨迹概念。

点到点（point to point）的运动，在这种情况下，任务是规划一条从初始位形q（t0）到最终位形q（tf）的轨迹。

废话少说，上干货。

（1）三次多项式轨迹：有4个约束构成，可以控制初始点的位置和速度和终止点的位置和速度。

（2）五次多项式轨迹：有6个约束构成，可以控制初始点的位置、速度、加速度和终止点的位置、速度和加速度。

（3）混有抛物线的直线段（LSPB）轨迹：可以进行优化，比如对最短时间。

一：三次多项式

对其对时间t一次求导得到速度公式和二次求导得到加速度公式，即：

解释一下为啥行列式等于0没有解，因为在线性代数的本质上来说，行列式等于0了是对其进行了一个压缩，也就是降为了，有好多的解，不等于0的意义是：只有一种方法从一个向量变换成另一个向量，而这个变换的过程就是这个行列式不等于0的矩阵。

eg，有2个点，初始点的位置为q0，终止点的位置为qf，并且他们的速度都为0，他们的关系式如下，即带入三次多项式的公式得到：

写成matalab为：

% 定义初始条件和最终条件
q0 = 10;
qf = -20;

% 生成时间向量 t，从 0 到 1，步长为 0.01
t = 0:0.01:1;

% 计算 qt 的值
qt = q0 + 3 * (qf - q0) * t.^2 - 2 * (qf - q0) * t.^3;

% 计算速度 vt 的值
vt = 6 * (qf - q0) * t - 6 * (qf - q0) * t.^2;

% 计算加速度 at 的值
at = 6 * (qf - q0) - 12 * (qf - q0) * t;

% 绘制位置 qt 的图形
subplot(3, 1, 1);
plot(t, qt, 'b-', 'LineWidth', 2);
xlabel('时间 t');
ylabel('位置 q_t');
title('位置轨迹');
grid on;

% 绘制速度 vt 的图形
subplot(3, 1, 2);
plot(t, vt, 'r-', 'LineWidth', 2);
xlabel('时间 t');
ylabel('速度 v_t');
title('速度轨迹');
grid on;

% 绘制加速度 at 的图形
subplot(3, 1, 3);
plot(t, at, 'g-', 'LineWidth', 2);
xlabel('时间 t');
ylabel('加速度 a_t');
title('加速度轨迹');
grid on;

解释一下这个有什么用：我们知道了这个角度、速度、角加速度对于时间的关系，可以在控制中分解时间，比如总时间为1s，利用定时器等分解0.01s，也就是有100个角度信息（末端位置逆解），把这个信息给到PWM等控制电机到达角度。而速度和加速度是根据末端执行器的速度角速度与雅克比的关系得到的，再根据上面的信息逐步给到电机。

eg，有2个点，初始点的位置为q0，终止点的位置为qf，并且初始点的速度为0，终止点的速度不为0，他们的关系式如下，即带入三次多项式的公式得到：

公式推导如下：

matlab程序：

% 给定初始条件和末端条件
theta_0 = 10;      % 起始角度
theta_dot_0 = 0;   % 起始角速度
theta_f = -20;     % 终止角度
theta_dot_f = 1;   % 终止角速度

% 计算三次多项式的系数
a0 = theta_0;
a1 = theta_dot_0;
a3 = 2*(theta_0 - theta_f) + theta_dot_f + theta_dot_0;
a2 = -3*(theta_0 - theta_f) - 2*theta_dot_0 - theta_dot_f;

% 定义时间向量
t = 0:0.01:1;

% 计算角度(theta)、速度(theta_dot)和加速度(theta_ddot)
theta = a3 * t.^3 + a2 * t.^2 + a1 * t + a0;
theta_dot = 3 * a3 * t.^2 + 2 * a2 * t + a1;
theta_ddot = 6 * a3 * t + 2 * a2;

% 绘制角度、速度和加速度的图像
figure;

% 绘制角度
subplot(3,1,1);
plot(t, theta);
title('Angle (\theta) vs. Time');
xlabel('Time (s)');
ylabel('Angle (\theta)');
grid on;

% 绘制速度
subplot(3,1,2);
plot(t, theta_dot);
title('Angular Velocity (\theta\_dot) vs. Time');
xlabel('Time (s)');
ylabel('Angular Velocity (\theta\_dot)');
grid on;

% 绘制加速度
subplot(3,1,3);
plot(t, theta_ddot);
title('Angular Acceleration (\theta\_ddot) vs. Time');
xlabel('Time (s)');
ylabel('Angular Acceleration (\theta\_ddot)');
grid on;

二：五次多项式

五次也是和三次多项式一样。

三：LSPB 混有抛物线的直线段

分为三个阶段：t0->tb 加速阶段，tb->tf-tb匀速阶段，tf-tb->tf减速阶段。

推导书中很详细。

eg，起始角度为0，速度为0，终止角度为40，速度为0（可以改），最大速度为60

matlba程序：

% 参数定义
theta_0 = 0;        % 起始角度
theta_f = 40;       % 终止角度
v_max = 60;         % 最大速度
v_f =0;           % 终止速度
T = 1;              % 总时间
tb = T / 3;         % 加速/减速时间

% 加速度计算
a = (v_max - v_f) / tb;

% 时间向量
t = linspace(0, T, 100);

% 计算角度、速度和加速度
theta = zeros(size(t));
theta_dot = zeros(size(t));
theta_ddot = zeros(size(t)); % 加速度

for i = 1:length(t)
    if t(i) <= tb
        % 加速阶段
        theta(i) = theta_0 + 0.5 * a * t(i)^2;
        theta_dot(i) = a * t(i);
        theta_ddot(i) = a;
    elseif t(i) <= T - tb
        % 匀速阶段
        theta_b = theta_0 + 0.5 * a * tb^2; % 加速阶段结束时的角度
        theta(i) = (theta_f + theta_0 - v_max * T) / 2 + v_max * t(i);
        theta_dot(i) = v_max;
        theta_ddot(i) = 0;
    else
        % 减速阶段
        theta(i) = theta_f - 0.5 * a * T^2 + a * T * t(i) - 0.5 * a * t(i)^2;
        theta_dot(i) = v_max - a * (t(i) - (T - tb));
        theta_ddot(i) = -a;
    end
end

% 绘制角度、速度和加速度图像
figure;

% 角度 vs. 时间
subplot(3,1,1);
plot(t, theta, 'LineWidth', 2);
title('角度 (\theta) vs. 时间');
xlabel('时间 (s)');
ylabel('角度 (\theta)');
grid on;

% 速度 vs. 时间
subplot(3,1,2);
plot(t, theta_dot, 'LineWidth', 2);
title('角速度 (\theta\_dot) vs. 时间');
xlabel('时间 (s)');
ylabel('角速度 (\theta\_dot)');
grid on;

% 加速度 vs. 时间
subplot(3,1,3);
plot(t, theta_ddot, 'LineWidth', 2);
title('角加速度 (\theta\_ddot) vs. 时间');
xlabel('时间 (s)');
ylabel('角加速度 (\theta\_ddot)');
grid on;