机器人关节空间轨迹规划（matlab）

1.引言

机器人关节空间轨迹规划是研究如何在关节空间中为机器人规划一条从初始状态到目标状态的最优路径。关节空间是一个高维空间，每个关节对应一个维度，轨迹规划需要考虑机械结构约束、环境动态和传感器反馈等因素。本文采用三次多项式轨迹规划。

2.轨迹规划方法

1.直线插值法

线性插值：在关节空间或笛卡尔空间中，通过简单的线性插值来生成轨迹。这种方法快速且易于实现，适合于简单的运动。

优点：

实现简单，计算量小。

对于简单的任务（如直线移动），效率高，响应快。

缺点：

运动缺乏平滑性，可能导致加速度和速度的突变。

不适用于复杂路径或需要精确控制的情况。

2. 多项式插值法

多项式插值：通过多项式（如三次多项式）来生成平滑的运动轨迹。三次多项式可以控制起始和结束位置的速度和加速度，使得运动更加平滑。

三次多项式轨迹：用于连接起始和目标位置，考虑位置、速度和加速度的连续性。优点：通过控制起始和结束的速度、加速度，能够生成平滑的轨迹。可调性好，适合大多数简单的运动任务。

优点：

通过控制起始和结束的速度、加速度，能够生成平滑的轨迹。

可调性好，适合大多数简单的运动任务。

缺点：

多项式次数过高可能导致振荡现象。对于复杂路径，可能需要多个多项式段连接，处理较为复杂。

3. S曲线轨迹

S曲线轨迹：通过控制加速度和减速度，生成平滑的运动轨迹，通常分为加速阶段、匀速阶段和减速阶段。S曲线有效减少了运动中的冲击和振动。

优点：

提供平滑的加速和减速，减少冲击和振动。

在工业机器人和自动化设备中广泛应用，效果显著。

缺点：

计算相对复杂，特别是在复杂环境中。

需要准确的参数调节，否则可能导致过长的运动时间。

4. 贝塞尔曲线

Bezier曲线：使用控制点定义的曲线，可以生成复杂的曲线轨迹。适用于需要精确控制路径形状的应用。

优点：

适合控制复杂形状的路径，灵活性高。

可以通过调整控制点来精准控制曲线形状。

缺点：

仍需要多次计算以生成整个轨迹，计算成本可能较高。

对于动态环境中的实时应用，响应时间可能较慢。

5. B样条（B-spline）

B样条：是一种通用的曲线表示方法，可以灵活地控制曲线的形状，适合于生成复杂的轨迹。B样条具有局部控制特性，便于调整。

优点：

具有局部控制特性，便于调整。

可以生成平滑且复杂的曲线，适合高维空间。

缺点：

算法实现复杂，调试较为困难。

对计算资源的需求可能较高。

6. 动态规划

动态规划：通过状态空间的离散化，利用最优控制理论来求解最优轨迹，适用于复杂环境中的路径规划。

优点：

可以处理复杂的环境和约束条件，保证找到最优解。

适用于时间和空间复杂度高的问题。

缺点：

计算复杂度高，特别是在高维空间中，计算资源消耗大。

可能需要长时间来求解最优解，不适合实时系统。

7. 基于采样的方法

快速扩展随机树（RRT）：通过随机采样构建树状路径，适合于高维空间中的路径规划。

概率道路图（PRM）：通过在配置空间中随机采样生成道路图，适用于复杂环境中的路径规划。

优点：

适合高维空间和复杂环境，能够有效寻找可行路径。

更加灵活，能够处理动态障碍物和非线性约束。

缺点：

生成的路径不一定平滑，后续需要平滑化处理。

可能无法找到最优路径，只能找到可行路径。

8. 运动学和动力学规划

基于运动学的规划：考虑机器人的运动学模型，通过逆运动学求解关节轨迹。

基于动力学的规划：考虑机器人的动力学特性，生成符合机器人动力学约束的轨迹。

优点：

可以得到符合机器人实际运动的轨迹，考虑了实际的机械限制。

动力学规划能够处理复杂的物理交互问题。

缺点：

计算过程复杂，需要精确的模型和参数，增加了实现难度。

对于动态环境的适应性较差。

9. 优化方法

最优化轨迹规划：通过设定目标函数（例如最小化能量消耗、时间或路径长度）和约束条件，利用优化算法（如遗传算法、粒子群优化等）来生成最优轨迹。

优点：

能够生成最优解，考虑了多个目标和约束。

适用于复杂问题，能够适应动态环境。

缺点：

求解过程可能需要大量计算时间。

依赖于初始条件，可能陷入局部最优解。

10. 机器学习方法

强化学习：利用强化学习算法，通过与环境的交互学习生成轨迹，适用于复杂动态环境中的路径规划。

优点：

能够处理复杂的动态环境，具有自适应能力。

随着经验的积累，能逐步提高性能。

缺点：

训练时间长且数据需求高，需大量的训练样本。

不确定性较高，实际应用效果依赖于训练质量。

3.机器人轨迹规划基本概念

4.关节空间路径规划（三次多项式插值轨迹规划）

5.PUMA560机器人工作原理

6.matlab代码及实验

clc;clear;close all;
N=6;angle=180/pi;% N 表示关节数（6个关节），angle 用于将弧度转为角度
mdl_puma560;%加载自带的puma560机器人
T1=transl(0.3,0,0.3)*rpy2tr(45,10,20);%根据给定起始点，得到起始点位姿,将平移矩阵与旋转矩阵相乘，得到 T1
T2=transl(0.6,0,0.6)*rpy2tr(45,20,25);%根据给定终止点，得到终止点位姿
%ikine6s 是 PUMA 560 机械臂的逆运动学求解函数，它根据给定的位姿 T1 和 T2 计算出对应的关节角度 q0 和 qf
q0=p560.ikine6s(T1);%根据起始点位姿，得到起始点关节角
qf=p560.ikine6s(T2);%根据目标点位姿，得到目标点关节角
tf=50;%定义终点时刻
%% 三次多项式轨迹规划
figure(1)
grid on
% tf 是时间步数，N 是机械臂的自由度（比如关节数量），那么 q 矩阵的每一行表示在一个特定时间步下，每个关节的位置。
q=zeros(tf,N);%初始化机械臂的位置
qd=zeros(tf,N);%初始化机械臂的角速度
qdd=zeros(tf,N);%初始化机械臂的角加速度
a0=q0;%三次多项式的第1个系数
a1=zeros(1,6);%三次多项式的第2个系数
a2=3/(tf^2)*(qf-q0);%三次多项式的第3个系数
a3=-2/(tf^3)*(qf-q0);%三次多项式的第四个系数
for t=1:tf
    q(t,:)=a0+a1*t+a2*(t^2)+a3*(t^3);%速度
    qd(t,:)=a1+2*a2*t+3*a3*(t^2);%角速度
    qdd(t,:)=2*a2+6*a3*t;%角加速度
end
p560.plot(q,'trail','b-');%动画演示
%% 抛物线轨迹规划
figure(2);
grid on
q1=zeros(tf,N);%初始化机械臂的位置
qd1=zeros(tf,N);%初始化机械臂的角加速度
qdd1=zeros(tf,N);%初始化机械臂的角加速度
for i=1:N%这个循环会遍历每个关节，lspb 是一个常见的函数，用于生成线性平滑轨迹（线性路径插值），通常用于控制轨迹的生成
    [q1(:,i),qd1(:,i),qdd1(:,i)]=lspb(q0(i),qf(i),tf);%lspb(q0(i), 
    % qf(i), tf) 的作用是为每个关节从初始位置 q0(i) 到最终位置 qf(i) 生成一条平滑的轨迹，时间总步数为 tf
end
p560.plot(q1,'trail','b-');%动画演示
%% 关节角度
figure(3);
%subplot(m, n, p) 用于在当前图形窗口中创建一个 m 行 n 列的子图，并激活第 p 个子图
subplot(6,1,1);plot(q(:,1)*angle);hold on;plot(q1(:,1)*angle);
title('关节1角度');grid on;legend('三次多项式','抛物线');
subplot(6,1,2);plot(q(:,2)*angle);hold on;plot(q1(:,2)*angle);
title('关节2角度');grid on;
subplot(6,1,3);plot(q(:,3)*angle);hold on;plot(q1(:,3)*angle);
title('关节3角度');grid on;
subplot(6,1,4);plot(q(:,4)*angle);hold on;plot(q1(:,4)*angle);
title('关节4角度');grid on;
subplot(6,1,5);plot(q(:,5)*angle);hold on;plot(q1(:,5)*angle);
title('关节5角度');grid on;
subplot(6,1,6);plot(q(:,6)*angle);hold on;plot(q1(:,6)*angle);
title('关节6角度');grid on;
%% 关节速度
figure(4);
subplot(6,1,1);plot(qd(:,1)*angle);hold on;plot(qd1(:,1)*angle);
title('关节1速度');grid on;
subplot(6,1,2);plot(qd(:,2)*angle);hold on;plot(qd1(:,2)*angle);
title('关节2速度');grid on;
subplot(6,1,3);plot(qd(:,3)*angle);hold on;plot(qd1(:,3)*angle);
title('关节3速度');grid on;
subplot(6,1,4);plot(qd(:,4)*angle);hold on;plot(qd1(:,4)*angle);
title('关节4速度');grid on;
subplot(6,1,5);plot(qd(:,5)*angle);hold on;plot(qd1(:,5)*angle);
title('关节5速度');grid on;
subplot(6,1,6);plot(qd(:,6)*angle);hold on;plot(qd1(:,6)*angle);
title('关节6速度');grid on;
%% 关节加速度
figure(5);
subplot(6,1,1);plot(qdd(:,1)*angle);hold on;plot(qdd1(:,1)*angle);
title('关节1角加速度');grid on;legend('三次多项式','抛物线');
subplot(6,1,2);plot(qdd(:,2)*angle);hold on;plot(qdd1(:,2)*angle);
title('关节1角加速度');grid on;
subplot(6,1,3);plot(qdd(:,3)*angle);hold on;plot(qdd1(:,3)*angle);
title('关节1角加速度');grid on;
subplot(6,1,4);plot(qdd(:,4)*angle);hold on;plot(qdd1(:,4)*angle);
title('关节1角加速度');grid on;
subplot(6,1,5);plot(qdd(:,5)*angle);hold on;plot(qdd1(:,5)*angle);
title('关节1角加速度');grid on;
subplot(6,1,6);plot(qdd(:,6)*angle);hold on;plot(qdd1(:,6)*angle);
title('关节1角加速度');grid on;
%% 末端点轨迹
figure('name','末端点轨迹线');
T = p560.fkine(q);%得到笛卡尔轨迹
T1=p560.fkine(q1);
p=transl(T);
p1=transl(T1);
plot3(p(:,1),p(:,2),p(:,3),'LineWidth',1.5);grid on;
hold on;
plot3(p(:,1),p(:,2),p1(:,3),'LineWidth',1.5);grid on;
legend('三次多项式','抛物线');
xlabel('X轴(mm)');ylabel('Y轴(mm)');zlabel('Z轴(mm)');

function [s,sd,sdd]=lspb(q0,q1,t)
    if isscalar(t)%如果 t 是一个标量，则创建一个从 0 到 t-1 的列向量
        t=(0:t-1)';
    else
        t=t(:);%如果 t 已经是一个向量，则将其转换为列向量
    end
    tf=max(t(:));
    V=(q1-q0)/tf*1.5;
    if q0==q1
        s=ones(size(t))*q0;
        sd=zeros(size(t));
        sdd=zeros(size(t));
        return
    end
    tb=(q0-q1+V*tf)/V;
    a=V/tb;
    p=zeros(length(t),1);
    pd=p;
    pdd=p;
        for i=1:length(t)
            tt=t(i);
            if tt<=tb
                %initial blend
                p(i)=q0+a/2*tt^2;
                pd(i)=a*tt;
                pdd(i)=a;
            elseif tt<=(tf-tb)
                %linear motion
                p(i)=(q1+q0-V*tf)/2+V*tt;
                pd(i)=V;
                pdd(i)=0;
            else
                %final blend
                p(i)=q1-a/2*tf^2+a*tf*tt-a/2*tt^2;
                pd(i)=a*tf-a*tt;
                pdd(i)=-a;
            end
        end
        s=p;
        sd=pd;
        sdd=pdd;
end