机器人控制系列教程之运动规划(1)

机械臂的运动

工业机器人的运动，根据其运动轨迹可分为点到点的运动和路径跟踪运动。点到点的运动只关心特点的位置点，而路径跟踪运动关心整个运动路径。

点到点运动

点到点的运动根据目标点的机器人位姿，利用逆向运动学求取机器人各个关节的目标位置，通过控制各个关节的运动，使机器人的末端到达目标位姿。

轨迹跟踪运动

轨迹跟踪运动希望机器人的末端以特定的姿态沿给定的路径运动。为了保证机器人的末端处在给定的路径上，需要计算出路径上各点的位置，以及在各个位置点上机器人所需要达到的姿态。上述计算路径上各点处的机器人位置与姿态的过程，称为机器人笛卡儿空间的路径规划(后续推文将进行讲解)。根据规划出的各个路径点处的机器人位置与姿态，利用逆向运动学求取机器人各个关节的目标位置，通过控制各个关节的运动，便机器人的末端到达各个路径点处的期望位姿。
轨迹跟踪是以点到点运动为基础的，而点到点运动的中间路径是不确定的。因此，轨迹跟踪运动只是在给定的路径点上能够保证机器人末端到达期望位姿，而在各个路径点中间不能保证机器人末端到达期望位姿。

关节空间的轨迹规划

关节空间路径规划，是指给定关节角的约束条件(起点、终点或中间节点的位置、速度、加速度等)，生成各关节变量变化曲线的过程。

当只给定起点、终点时刻的约束条件时，相应的路径规划称为点到点路径规划；若要求关节变量严格按照指定曲线运动时的规划称为连续路径规划；当要求经过多个中间点，而对节点间的轨迹没有严格限制时的路径规划，称为多节点路径规划。实际中，往往是采用介于点到点和连续路径规划之间的多节点路径规划。

可以三次多项式插值、五次多项式插值、采用梯形速度插值、用抛物线拟合的线性插值、三/五次样条、贝塞尔曲线等插值函数进行关节空间的路径规划。高级的有TOPP算法。

三次多项式插值的轨迹规划方法

在这里插入图片描述

五次多项式插值的轨迹规划方法

在这里插入图片描述

抛物线过渡的线性运动轨迹规划方法

己知始点及末端终止点关节角度的机器人，如果想以恒定运动速度来控制机器人各关节，运动段的起点和终点的产生所需的在边界点瞬间速度需要一个无穷大的加速度。为避免上述情况，可以用抛物线来过渡运动段的起点和终止点处，

在这里插入图片描述

MATLAB机器人工具箱`jtraj`函数解析

该函数为MATLAB机器人工具箱中计算关节空间轨迹的函数，该函数中采用五次多项式，使用方式如下：

[Q,QD,QDD] = JTRAJ(Q0, QF, M)：表示是关节坐标从初始位置Q0到终止位置QF的运动，M表示步数，速度和加速度的边界条件默认均为0。可得出关节的角度Q、速度QD、加速度QDD。
[Q,QD,QDD] = JTRAJ(Q0,QF,M,QD0,QDF):如上，但其中还定义了轨迹的边界条件，即：起始关节速度：QD0，终止关节速度：QDF。
[Q,QD,QDD] = JTRAJ(Q0, QF, T): 同1中，但轨迹的步数换为步长T来定义。
[Q,QD,QDD] = JTRAJ(Q0, QF, T, QD0, QDF): 同2中，但轨迹的步数换为步长T来定义。

关节空间轨迹求解

相关代码：

% 导入机器人模型
% 这里输入你的机器人模型
clear,clc,close all;
%% 建立机器人DH参数，初始状态为竖直状态
L1=Link('d',162.50,'a',0,'alpha',0,'modified');
L2=Link('d',0,'a',0,'alpha',pi/2,'offset',pi/2,'modified');
L3=Link('d',0,'a',425,'alpha',0,'modified');
L4=Link('d',126.70+6.6,'a',392.25,'alpha',0,'offset',pi/2,'modified');
L5=Link('d',99.70,'a',0,'alpha',pi/2,'modified');
L6=Link('d',99.60-4.5,'a',0,'alpha',-pi/2,'modified');robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','UR5')t = 0:0.01:2;    %采样时间为2s，采样间隔为0.01s
qz = [0 0 0 0 0 0];    %初始位姿
qn = [pi/2 -pi/2 pi/6 pi/12 0 0];    %末端位姿
[q, qd, qdd] = jtraj(qz, qn, t);    %轨迹规划
plot(robot,q,'trail','b-')    %图形演示
T = fkine(robot, q);    %生成三维矩阵
%取机械臂末端执行器位置所标矩阵
m = squeeze(T(:,4,:));   % 末端执行器坐标的变化曲线
plot(t, m);    %绘制机械臂末端执行器空间轨迹
%生成关节i(i=1, 2, 3, 4, 5, 6)的角位移曲线
figure(2);
subplot(3,1,1);
plot(t,q(:,1),'r','LineWidth',1);
title('关节1位移变化曲线');
%生成关节i(i=1, 2, 3, 4, 5, 6)的角速度曲线
subplot(3,1,2);
plot(t,qd(:,1),'g','LineWidth',1);
title('关节1速度变化曲线');
%生成关节i(i=1, 2, 3, 4, 5, 6)的角加速度曲线
subplot(3,1,3);
plot(t,qdd(:,1),'b','LineWidth',1);
title('关节1加速度变化曲线');

规划动画