机器人学导论-轨迹生成(1)三次多项式插值

轨迹指的是每个自由度的位置、速度、加速度的时间历程。
路径更新速率：计算轨迹点的位置、速度、加速度的速率，一般在60Hz到2000Hz之间。

1.综述

操作臂的运动看作是工具坐标系{T}相对于工作台坐标系{S}的运动，它可以将运动的描述与任何具体的机器人、末端执行器或工件分离。
基本问题是将操作臂从初始位置{$T_{initial}$}移动，经过中间点{$T_{ti}$}，最后到达最终期望位置{{$T_{final}$}。一般运动包括工具相对于工作台的姿态变化和位置变化。

路径点包括所有的中间点以及初始点和最终点。

2.关节空间的规划方法

关节空间的规划方法：以关节转角的函数来描述轨迹(在空间和时间)的轨迹生成方法。
流程：已知笛卡尔空间下的路径点，通过逆运动学，求解每个路径点对应的期望关节角；通过关节轨迹规划获得各个关节的平滑函数，通过正运动学获得笛卡尔空间下的轨迹。最后检查轨迹可行性。

3.三次多项式

编程实例：给定每个点的位置和速度信息
单个关节在10s内从初始点A，第3s经中间点B，最后到达目标点C

clear;clc;close all;
q_array=[25,70,50];%起始点，中间点，目标点
t_array=[0,3,10];
v_array=[0,20,30];
t=t_array(1);q=q_array(1);v=v_array(1);%起始点的位置；速度；
for i=1:1:length(q_array)-1%多一个中间点就多循环一次
    T=t_array(i+1)-t_array(i);%计算单段三次多项式插值的时间
    a0=q_array(i);
    a1=v_array(i);
    a2=(q_array(i+1)-q_array(i))*3/(T^2)-(2*v_array(i)+v_array(i+1))/T;
    a3=(q_array(i)-q_array(i+1))*2/(T^3)+(v_array(i)+v_array(i+1))/(T^2);
    if i==1
        a=2*a2;
    else 
        a=[a(1:end-1),aa(i-1)];
    end%if语句的作用为使得a向量的列数和t向量列数相同，便于画图；
    ti=t_array(i):0.02:t_array(i+1);
    qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3;
    vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2;
    ai=2*a2+6*a3*(ti-t_array(i));
    aa(i)=2*a2+6*a3*(t_array(i+1)-t_array(i));
    t=[t,ti(2:end)];q=[q,qi(2:end)];v=[v,vi(2:end)];a=[a,ai(2:end)];%多段三次多项式进行拼接
end
subplot(3,1,1),plot(t,q,'r'),xlabel('t'),ylabel('position');hold on;plot(t_array,q_array,'*','color','r'),grid on;
subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');hold on;plot(t_array,v_array,'o','color','g'),grid on;
subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');grid on;

结果：

结果分析：
从图中可以看到，位置曲线是“平滑”的，速度曲线是连续的，加速度曲线是可变的，但是不连续。总的来说：三次多项式插值能够保证位置曲线和速度曲线是连续的，但加速度曲线不一定连续。

4.笛卡尔空间的规划方法

笛卡尔空间的规划方法：用笛卡尔位置和姿态关于时间的函数来描述路径形状。
流程：已知笛卡尔空间下的路径点，通过笛卡尔空间的规划方法获得描述笛卡尔变量的时间的函数，通过逆运动学获得关节空间下各个关节的轨迹，发给控制器执行，最后检查轨迹可行性。

5.参考

[1]约翰 J. 克雷格(John J. Craig)著的机器人学导论，第四版；
[2]台湾大学-机器人学之运动学—林沛群；