机械臂轨迹规划——三次样条对比研究

29903
117
2016年12月30日 14时44分

    在之前的博客中(http://www.guyuehome.com/531),展示了目前我们机械臂的控制效果。在技术层面,机械臂的路径规划由ROS MoveIt完成,如果你用过MoveIt,应该知道MoveIt会根据控制指令,发布一个/joint_path_command消息:
    positions: [7.498824743379373e-06, 0.00014502150588668883, 0.00016246525046881288, 0.000576882332097739, -4.5179829612607136e-05, -7.556870696134865e-05]
    velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    accelerations: [1.6636165814011838, 0.0, 0.0, 0.0, 0.0, 0.0]
    time_from_start:
      secs: 0
      nsecs:         0
  –
    positions: [0.14881055128822188, 0.045082294050072864, -0.17503586480931904, 0.00036463185447175315, 0.04101490496878304, -1.833076530072867e-05]
    velocities: [0.5996016442654069, 0.18107466251681814, -0.7059613699939725, -0.0008552629349582365, 0.16545154115596925, 0.00023064018758955565]
    accelerations: [1.655874354768574, 0.5000601529825928, -1.9495999372424513, -0.002361914738103317, 0.45691496442234303, 0.0006369415018473661]
    time_from_start:
      secs: 0
      nsecs: 422955130
  –
    positions: [0.2976136037517004, 0.09001956659425904, -0.3502341948691069, 0.0001523813768457673, 0.08207498976717868, 3.8907176359891316e-05]
    velocities: [0.970627433455754, 0.29312133584605315, -1.1428011901395827, -0.0013844886441320295, 0.26783082783280926, 0.00037335737063574914]
    accelerations: [1.5818667215259667, 0.4777104690862263, -1.8624645355075706, -0.0022563513424359665, 0.436493611188237, 0.0006084740441988044]
    time_from_start:
      secs: 0
      nsecs: 598557636
 
   ……
     这个消息中就包含了路径规划后的所有路点信息,也就是描述机械臂应该以什么样的姿态运动到目标位置。关于MoveIt的运动规划算法,我在ROS探索总结(二十五)——MoveIt基础里边已经大概介绍过,主要是一个pipeline的概念,一步一步约束、修正、添改。这里只提其中一个关键的部分—— AddTimeParameterization。
     在pipeline的前级,运动规划器根据环境计算得出一条空间轨迹,但是这条轨迹只包含位置关系,并没有告诉机器人应该以怎样的速度、加速度运动,于是,就要经过AddTimeParameterization这个模块, 为这条空间轨迹进行速度、加速度约束,并且计算运动到每个路点的时间。可以去看MoveIt中AddTimeParameterization模块的的代码实现(moveit_core\trajectory_processing\src\iterative_time_parameterization.cpp),大概了解模块的运行过程,但是像我这样的算法渣渣,实在看不懂它这么实现的原理,于是就去google这个算法,终于找到了这个算法的作者。
     这个算法的全称是 Time-Optimal Path Parameterization Algorithm,简称TOPP,是不是听上去感觉就很NB。关于这个算法的原理,作者发布了一篇论文  ?A general, fast, and robust implementation of the time-optimal path parameterization algorithm?,算法的原始实现在github上:https://github.com/quangounet/TOPP但是本人数学渣实在看不明白那么多的公式横飞,只知道这个算法十分酷炫,只需要输入一条路径所有路点的位置信息,就可以根据时间最优的原则,输出所有点的速度、加速度、时间信息。当然,这个算法也不是无敌的,其中有一个关键问题就是加速度存在抖动的问题,可以参见github上的讨论:https://github.com/ros-planning/moveit/issues/160
     本文并不是要讨论TOPP算法,我们回到正题。按照我们之前的控制,MoveIt把路径规划后的消息发到下层的控制器,控制器当然要根据所有的路点完成中间状态完成插补运算,如果用多项式插补,在已知所有路点速度、加速度、时间的条件下,需要使用一个五阶多项式来描述(可以参见《机器人学导论》7.3节)。按照这样的插补运算,确实可以完成插补的任务,如下图所示:
1
     有一个很重要的问题就是加速度的抖动,应该就是传说中的“龙格现象”,这种抖动已经远远超过了机器人的加速度限制。于是,我产生了对这种五次多项式插补的怀疑,我们竟然以这样的控制完成了功夫茶的演示,果然是新手。这段时间我就在研究这个插补到底应该怎么做,参考了知乎上各位大神的帖子,发现三次样条插补的算法用的很多,就先从这个入手学习吧。
     样条插值是一种工业设计中常用的、得到平滑曲线的一种插值方法,三次样条又是其中用的较为广泛的一种。三次样条的数学定义如下:
2
       至于三次样条方程组的推导和求解公式这里就不详述了,可以参考《数学分析》(李庆扬)的2.6节和5.3节。重要的是来看一下三次样条插补的效果如何,我们取机器人一轴的路径数据作为实验样本,来看一下三次样条的效果如何。
      Matlab中提供了spline box,其中包含了三次样条的实现,我们先在Matlab里边看一下效果如何。Matlab实现的代码如下:
%三次样条测试
%原始数据点
X = [0, 0.422955130, 0.598557636, 0.734591320, 0.850603738, 0.953558869, 1.056514000, 1.159469131, 1.274332912, 1.409208218, 1.585026197, 2];
Y = [0, 0.14881055128822188, 0.2976136037517004, 0.4464166562151788, 0.5952197086786574, 0.7440227611421358, 0.8928258136056142, 1.0416288660690929, 1.1904319185325714, 1.3392349709960498, 1.4880380234595283, 1.6368410759230068];
s = csapi(X,Y);   %三次样条
fnplt(s, 'r');         %绘制样条曲线
hold on
plot(X,Y,'o')       %绘制原始数据点
v=fnder(s,1);     %样条曲线一阶导数得到速度曲线
fnplt(v, 'g');        %绘制速度曲线
hold on
a=fnder(v,1);     %样条曲线二阶导数得到加速度曲线
fnplt(a, 'b');        %绘制加速度曲线
legend('位置样条曲线','原始数据点','速度曲线','加速度曲线')
      运行之后就可以看到效果啦:
3
        看过之后,我们就对三次样条所产生的效果大致明白了,从位置曲线上来看,确实平滑了,速度的上升阶段接近于匀速,在最上边的匀速段稍有抖动,机器速度呈一阶线性变化。
       OK,接下来就是要自己实现了,基于的原理就是《数值分析》上边讲到的,另外参考了网上众多实现的代码。二话不说,先上代码:
       头文件cubicSpline.h:
#ifndef _CUBIC_SPLINE_H_
#define _CUBIC_SPLINE_H_

class cubicSpline
{
public:
    typedef enum _BoundType
    {
        BoundType_First_Derivative,
        BoundType_Second_Derivative
    }BoundType;

public:
    cubicSpline();
    ~cubicSpline();

    void initParam();
    void releaseMem();

    bool loadData(double *x_data, double *y_data, int count, double bound1, double bound2, BoundType type);
    bool getYbyX(double &x_in, double &y_out);

protected:
    bool spline(BoundType type);

protected:
    double *x_sample_, *y_sample_;
    double *M_;
    int sample_count_;
    double bound1_, bound2_;
};

#endif /* _CUBIC_SPLINE_H_ */
       源文件cubicSpline.cpp:
#include <string.h>
#include <stddef.h>
#include <stdio.h>
#include "cubicSpline.h"

cubicSpline::cubicSpline()
{
}

cubicSpline::~cubicSpline()
{
    releaseMem();
}

void cubicSpline::initParam()
{
    x_sample_ = y_sample_ = M_ = NULL;
    sample_count_ = 0;
    bound1_ = bound2_ = 0;
}

void cubicSpline::releaseMem()
{
    delete x_sample_;
    delete y_sample_;
    delete M_;

    initParam();
}

bool cubicSpline::loadData(double *x_data, double *y_data, int count, double bound1, double bound2, BoundType type)
{
    if ((NULL == x_data) || (NULL == y_data) || (count < 3)
        || (type > BoundType_Second_Derivative) || (type < BoundType_First_Derivative))
        return false;

    initParam();

    x_sample_ = new double[count];
    y_sample_ = new double[count];
    M_        = new double[count];
    sample_count_ = count;

    memcpy(x_sample_, x_data, sample_count_*sizeof(double));
    memcpy(y_sample_, y_data, sample_count_*sizeof(double));

    bound1_ = bound1;
    bound2_ = bound2;

    return spline(type);
}


bool cubicSpline::spline(BoundType type)
{
    if ((type < BoundType_First_Derivative) || (type > BoundType_Second_Derivative))
        return false;

    //  追赶法解方程求二阶偏导数
    double f1=bound1_, f2=bound2_;

    double *a=new double[sample_count_];                //  a:稀疏矩阵最下边一串数
    double *b=new double[sample_count_];                //  b:稀疏矩阵最中间一串数
    double *c=new double[sample_count_];                //  c:稀疏矩阵最上边一串数
    double *d=new double[sample_count_];

    double *f=new double[sample_count_];

    double *bt=new double[sample_count_];
    double *gm=new double[sample_count_];

    double *h=new double[sample_count_];

    for(int i=0;i<sample_count_;i++)
        b[i]=2;                                //  中间一串数为2
    for(int i=0;i<sample_count_-1;i++)
        h[i]=x_sample_[i+1]-x_sample_[i];      // 各段步长
    for(int i=1;i<sample_count_-1;i++)
        a[i]=h[i-1]/(h[i-1]+h[i]);
    a[sample_count_-1]=1;

    c[0]=1;
    for(int i=1;i<sample_count_-1;i++)
        c[i]=h[i]/(h[i-1]+h[i]);

    for(int i=0;i<sample_count_-1;i++)
        f[i]=(y_sample_[i+1]-y_sample_[i])/(x_sample_[i+1]-x_sample_[i]);

    for(int i=1;i<sample_count_-1;i++)
        d[i]=6*(f[i]-f[i-1])/(h[i-1]+h[i]);

    //  追赶法求解方程
    if(BoundType_First_Derivative == type)
    {
        d[0]=6*(f[0]-f1)/h[0];
        d[sample_count_-1]=6*(f2-f[sample_count_-2])/h[sample_count_-2];

        bt[0]=c[0]/b[0];
        for(int i=1;i<sample_count_-1;i++)
            bt[i]=c[i]/(b[i]-a[i]*bt[i-1]);

        gm[0]=d[0]/b[0];
        for(int i=1;i<=sample_count_-1;i++)
            gm[i]=(d[i]-a[i]*gm[i-1])/(b[i]-a[i]*bt[i-1]);

        M_[sample_count_-1]=gm[sample_count_-1];
        for(int i=sample_count_-2;i>=0;i--)
            M_[i]=gm[i]-bt[i]*M_[i+1];
    }
    else if(BoundType_Second_Derivative == type)
    {
        d[1]=d[1]-a[1]*f1;
        d[sample_count_-2]=d[sample_count_-2]-c[sample_count_-2]*f2;

        bt[1]=c[1]/b[1];
        for(int i=2;i<sample_count_-2;i++)
            bt[i]=c[i]/(b[i]-a[i]*bt[i-1]);

        gm[1]=d[1]/b[1];
        for(int i=2;i<=sample_count_-2;i++)
            gm[i]=(d[i]-a[i]*gm[i-1])/(b[i]-a[i]*bt[i-1]);

        M_[sample_count_-2]=gm[sample_count_-2];
        for(int i=sample_count_-3;i>=1;i--)
            M_[i]=gm[i]-bt[i]*M_[i+1];

        M_[0]=f1;
        M_[sample_count_-1]=f2;
    }
    else
        return false;

    delete a;
    delete b;
    delete c;
    delete d;
    delete gm;
    delete bt;
    delete f;
    delete h;

    return true;
}

bool cubicSpline::getYbyX(double &x_in, double &y_out)
{
    int klo,khi,k;
    klo=0;
    khi=sample_count_-1;
    double hh,bb,aa;

    //  二分法查找x所在区间段
    while(khi-klo>1)
    {
        k=(khi+klo)>>1;
        if(x_sample_[k]>x_in)
            khi=k;
        else
            klo=k;
    }
    hh=x_sample_[khi]-x_sample_[klo];

    aa=(x_sample_[khi]-x_in)/hh;
    bb=(x_in-x_sample_[klo])/hh;

    y_out=aa*y_sample_[klo]+bb*y_sample_[khi]+((aa*aa*aa-aa)*M_[klo]+(bb*bb*bb-bb)*M_[khi])*hh*hh/6.0;

    //////test
    double acc = 0, vel = 0;
    acc = (M_[klo]*(x_sample_[khi]-x_in) + M_[khi]*(x_in - x_sample_[klo])) / hh;
    vel = M_[khi]*(x_in - x_sample_[klo]) * (x_in - x_sample_[klo]) / (2 * hh)
        - M_[klo]*(x_sample_[khi]-x_in) * (x_sample_[khi]-x_in) / (2 * hh)
        + (y_sample_[khi] - y_sample_[klo])/hh
        - hh*(M_[khi] - M_[klo])/6;
    printf("%0.9f, %0.9f, %0.9f\n",y_out, vel, acc);
    //////test end

    return true;
}
       测试文件main.cpp:

#include <stdio.h>
#include "cubicSpline.h"

#define POINTS_COUNT 12
int main()
{
    double x_data[POINTS_COUNT] = {0, 0.422955130, 0.598557636, 0.734591320, 0.850603738, 0.953558869, 1.056514000, 1.159469131, 1.274332912, 1.409208218, 1.585026197, 2};
    double y_data[POINTS_COUNT] = {0, 0.14881055128822188, 0.2976136037517004, 0.4464166562151788, 0.5952197086786574, 0.7440227611421358, 0.8928258136056142, 1.0416288660690929, 1.1904319185325714, 1.3392349709960498, 1.4880380234595283, 1.6368410759230068};

    double x_out = 0;
    double y_out = 0;

    cubicSpline spline;
    spline.loadData(x_data, y_data, POINTS_COUNT, 0, 0, cubicSpline::BoundType_First_Derivative);

    x_out = -0.004;
    for(int i=0; i<=500; i++)
    {
        x_out = x_out + 0.004;
        spline.getYbyX(x_out, y_out);
        //printf("%f, %0.9f \n", x_out, y_out);
    }

    return 0;
}

       所有代码就是这样,这个程序可以计算第一类(已知边界一阶导数)和第二类(已知边界二阶导数) 边界条件下的三次样条,机器人运行过程中,我们要求起始和终止的位置速度必须为0,所以采用了第一类边界条件,并且边界的一阶导数为0。运行之后,会print出所有的数据,我在Excel中进行了整理:
4
      乍看上去好像和Matlab画出来的差不多,把数据导入到Matlab中进行一下对比:
5
    数据几乎是完全重叠的,说明我们的算法应该没有问题。至于三次样条相比五阶插补的效果如何,我们还是需要将两者进行对比。还是使用相同的实验输入数据,我们放到五阶插补算法里计算,首先看一下位置曲线的对比:
6
       貌似看不出来多大差别,两者的趋势几乎一致。那么就看一下速度曲线的对比:
7
         哎呦!看到有一些不同了吧,五阶曲线明显抖动更多,再来看一下加速度曲线的对比:
8
      哎呀!这就是传说中的“魔鬼的步伐”吧!五阶算法计算出来的加速度明显抖动的厉害,而且会超出加速度的限制,三次算法算出来的加速度就没那么光滑了。
      OK,今天的对比研究到此告一段落,至于实际放到机械臂上边的效果如何,后边再继续实验。

发表评论

后才能评论

评论列表(117条)

  • iim 2019年12月10日 下午5:43

    我跑上面TOPP的包 Python文件没有TOPPbindings的py文件

    • 古月 回复 iim 2019年12月12日 下午8:01

      好像是没有python的

  • Gang Li 2019年4月10日 下午7:40

    您好,对于一段轨迹,轨迹长度、起点速度、起点加速度、终点速度、终点加速度作为约束条件的话,一共有5个约束条件,3次样条无法完成规划吧。

    • 古月 回复 Gang Li 2019年4月10日 下午9:39

      是的

    • Gang Li 回复 古月 2019年4月11日 下午12:11

      所以如果按照三次样条来规划,就破坏了时间最优规划算法的初衷了

    • 古月 回复 Gang Li 2019年4月15日 下午6:11

      是的

    • lfl 回复 古月 2019年10月21日 下午10:42

      古月老师,请问自己规划得到的关节角度在怎么通过AddTimeParameterization模块的函数进行速度、加速度和时间的规划呀?下载的代码不知道在哪里运行可以得到结果。

  • 阿达 2019年4月3日 下午3:34

    古月老师,如果我想实现机械臂在连续到达几个路径点,用moveit目前只能先到一个点停下再到下一个,如果想连续运动怎么实现呢?

    • 古月 回复 阿达 2019年4月4日 下午10:23

      我在深蓝学院上的线上课正在讲相关的内容,这两天应该会发一篇相关的公众号文章,到时候可以参考

    • 时雨 回复 阿达 2019年5月30日 下午4:01

      请问,你这个问题解决了吗

    • 阿达 回复 时雨 2019年11月5日 下午2:29

      并没有解决

    • miler-hou 回复 阿达 2019年11月7日 下午3:56

      古月私房课 MoveIt中不得不说的“潜规则”,这篇文章里讲到了

  • Wust 2019年2月13日 下午3:37

    古月大神你好,我想问一下怎么样调用这个AddTimeParameterization模块去计算轨迹的速度和加速度?代码实在是看不懂

    • 古月 回复 Wust 2019年2月14日 上午10:16

      这个在moveit里没有封装成接口,还是需要看代码的

    • dso 回复 古月 2020年1月15日 上午11:03

      请教一下,Topp这个算法应该怎么使用啊
      我修改了他test文件里的数据,结果生成的轨迹就不正常了

  • 六铢衣 2019年2月2日 下午9:03

    古月老师好
    /joint_path_command消息:里面有一个时间:
    time_from_start:
    secs: 0
    nsecs: 422955130
    我看这个时间并不是整数,时间间隔也不相等,请问它是TOPP算法得出来的吗?在控制的时候,ros control控制器是怎么对它进行处理的,是固定的时间间隔进行控制吗?还有这个时间间隔是否可以调整?
    非常感谢您的帮助

    • 古月 回复 六铢衣 2019年2月8日 下午3:55

      可以参考contrller的源码,是五次插补的,不是固定时间间隔

  • zjy 2019年1月8日 上午10:56

    请问古月大神,我现在在自己设计一条机械臂的轨迹,即将每个points添加到joint_trajectory,然后机械臂运动时我发现存在抖动现象,应该是因为轨迹不平滑,所以需要用样条来平滑轨迹,那我应该怎样将样条函数添加到points之间,添加到joint_trajectory里呢?

    • zjy 回复 zjy 2019年1月8日 下午9:19

      然后我又遇到了问题,我把我设计的轨迹添加到joint_trajectory,然后发送给机械臂,在仿真环境下机械臂可以动,rviz和gazebo都可以,但是在实际机械臂上测试时机械臂直接锁死,实际机械臂我原来跑过很多用moveit写的代码,都没有问题,但是自己写的joint_trajectory出了问题,程序也没有报错,能运行完,就是机械臂动一下立刻就锁死了,请问古月大神知不知道是什么原因?

    • 古月 回复 zjy 2019年1月10日 上午11:10

      可能是实际机器人有速度或者加速度的限制吧

    • 古月 回复 zjy 2019年1月10日 上午11:09

      添加到joint_trajectory中的points应该是一些关键点,发到机器人后控制器会将每两个点之间的轨迹做插值,在gazebo里仿真的话,这个差值是由配置的ros controller做的

    • zjy 回复 古月 2019年1月10日 上午11:31

      嗯,我修改了速度限制,但是发现机械臂运动时抖动得很厉害,然后我输出了机械臂反馈的速度曲线,非常不平滑,是要在points之间加入样条吗?请问怎样加入呢?

    • zjy 回复 zjy 2019年1月10日 上午11:41

      我的理解是这样,我现在有一系列points,然后我需要将这些points用样条进行平滑处理,然后将处理后的points加入到joint_trajectory里,不知道对不对。

    • 古月 回复 zjy 2019年1月11日 下午11:07

      是的,可以自己加一个样条算法,生成更多点,然后再都放到轨迹里

    • zjy 回复 古月 2019年1月14日 下午5:00

      感谢古月大神的解答,不知道古月大神用过TOPP没有?MoveIt里是根据轨迹用TOPP计算出速度加速度,那么我现在有一条轨迹的position,是否可以用TOPP算出这条轨迹的速度和加速度?

    • 古月 回复 zjy 2019年1月16日 下午5:54

      是可以的

  • 小张 2018年12月15日 下午7:16

    月哥。首先非常感谢您能回答大家的问题,我有个小问题,想问问您,我先用moveit规划出轨迹点,接着用您的程序计算出插补轨迹点,但是怎么让rviz中的机械臂按照插补的轨迹运动呢?

    • 古月 回复 小张 2018年12月16日 下午1:04

      rviz中的模型是根据反馈的joints_state实时显示的,更新这个话题就行了

  • JAY 2018年11月28日 上午10:42

    古月居大神,我目前对于样条曲线的轨迹规划有一些困惑,就我目前掌握的知识来说,样条曲线是一种分段连续的曲线,他的参数u【0,1】并不是关于时间的,因此他的曲线求导和求二阶导并不是关于时间的函数,那么样条曲线的导数为何能用于速度和加速度的控制呢

    • 古月 回复 JAY 2018年11月28日 下午7:51

      为什么不是关于时间的函数?输入是时间,输出是位置、速度、加速度

    • JAY 回复 古月 2018年11月29日 下午4:37

      抱歉,我搞混了三次样条曲线和三次B-样条曲线的概念。三次B样条曲线的那个参数u是分段,且u的范围在【0,1】,我想把三次B样条曲线应用到关节空间轨迹规划,目的是想机械臂可以沿着我给定的任意轨迹走,请问B样条函数有啥办法能变成位置关于时间的函数吗..

    • 古月 回复 JAY 2018年12月1日 上午12:10

      这个不清楚,没试过

    • rui zhng 回复 JAY 2019年6月4日 上午10:11

      他这里的三次样条,是X时间-Y位置 关系。所以在衔接处的一阶导和二阶导是速度和加速度。B样条只是构造空间(三维xyz)坐标下的空间参数曲线。没有速度关系。他们的导数是对参数u求导。没有时间关系,但是可以建立u跟T的关系。插补的时候,计算下一周期的参数u,这时候要根据速度。 其他的不了解,cnc领域一般是 速度规划,然后利用一阶和二阶泰勒展开,根据速度求下一插补周期参数u,然后计算位置。

  • 小飞 2018年11月23日 上午10:54

    老师您能提供一下五次样条的源码吗?我想在matlab里跑一下仿真

    • 古月 回复 小飞 2018年11月24日 下午5:05

      这个从网上找一下吧,我这里也没有了

  • 郭建根_qq1571808481 2018年11月10日 上午11:22

    古月老师,您好,
    在以下链接:
    (http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/chomp_planner/chomp_planner_tutorial.html#using-chomp-with-your-robot)看到这一段(trajectory_initializaiton_method: the type of initialization of the trajectory can be supplied here for CHOMP, this can be quintic-spline, linear, cubic or fillTrajectory.)。这些样条插值算法(quintic-spline, linear, cubic or fillTrajectory)是封装好的吧,是能直接通过chomp_planner规划器调用吗?moveit中有没有机械臂轨迹规划相关算法(例如关节空间的多项式插值、样条插值,笛卡尔空间的直线、圆弧插补)的源码呢, 还有重复上面 “邹 ” 的问题(如何通过代码方式将三次样条插补加入到机械臂的轨迹中呢?或者说如何将自己写的算法应用到机械臂的轨迹规划?)

    • 古月 回复 郭建根_qq1571808481 2018年11月12日 下午8:42

      ROS当中的功能都是开源的,下载moveit代码都可以找到源码的,多项式插值之类的算法也都有。
      要集成自己算法的话,可以参考moveit中已有的算法,需要通过插件的形式集成

    • 郭建根_qq1571808481 回复 古月 2018年11月14日 下午7:24

      感谢古月老师的细心解答,谢谢您!

  • 2018年10月7日 下午7:37

    古月老师你好:
    我最近用moveit做机械臂相关项目时,想用rqt查看机械臂运行时每个关节的实时速度,但不知为何所有关节的速度都几乎时0,查看joint_states时每个关节的速度也是0,机械臂有在动,速度不可能时0呀,请问这是什么原因呢?

    • 古月 回复 2018年10月9日 下午7:13

      这个要检查机器人底层上发的速度值,从源头找起

    • 邹 回复 古月 2018年10月24日 下午10:36

      感谢古月老师的回答,还有两个问题不明白:、
      1.您的博客里写到三次样条插补,意思是在topp算法的基础上加入三次样条插补吗?那如何通过代码方式将三次样条插补加入到机械臂的轨迹中呢?
      2.我看您出的书上写的五次样条插补能保证加速度连续,但是您的博客里说功夫茶项目用五次样条出现加速度抖动,请问这是为什么?不知道是不是我理解的有问题

    • 古月 回复 2018年10月28日 下午8:36

      1. topp规划出来是一系列路径点,没两个点之间还需要细插补,可以用三次或五次
      2. 我说的抖动意思并不是加速度不连续,而是变化幅度过快,和这篇博客写的内容一致

    • 邹 回复 古月 2018年10月29日 上午9:25

      明白了,谢谢古月老师

    • 祺 回复 2019年6月12日 下午8:22

      请问你最后速度曲线有输出来吗?

  • 烟消云散 2018年9月16日 下午7:01

    古月老师你好:
    我想请教一下,比如说moveit里的笛卡尔路径规划,虽然我可以自己设定起始路点和终止路点,但是本质上从起始路点到终止路点所经历的一系列的point还是moveit帮我规划出来的。
    我现在想用ROS+moveit实现这样一个功能,就是自己在空间中设定一条轨迹(而不是moveit规划出来的),这个轨迹上的所有的point都是我自己设定的,然后让机械臂沿着我设定的这条轨迹上的point进行移动,该怎样做呢?希望古月老师能指点我一下。

    • 古月 回复 烟消云散 2018年9月17日 下午3:11

      原理参考《机器人学导论》,相当于自己做规划器,就和moveit没关系了,可以自己把轨迹(路径点、速度、加速度、时间)算出来,封装成ros中的轨迹数据,再发给机器人执行。也就是仿照moveit的输入输出,替换moveit的功能。

    • 烟消云散 回复 古月 2018年9月17日 下午4:47

      谢谢古月老师,我查了一下,机器人学导论同名的有好几种,请问应该参考哪一本呢?

    • 古月 回复 烟消云散 2018年9月21日 下午9:39

      最经典的一本作者是:(美)克来格(Craig,J.J)

    • 烟消云散 回复 古月 2018年9月17日 下午5:22

      还有一个问题,如果自己做规划器的话,那是不是moveit里的逆解插件也用不了了呢?

    • 古月 回复 烟消云散 2018年9月21日 下午9:41

      不会呀,规划和运动学求解是分开的两个功能

    • 烟消云散 回复 古月 2018年9月24日 下午5:17

      明白了,谢谢古月老师

    • hero 回复 烟消云散 2018年11月9日 下午4:26

      你好,我也做机械臂的轨迹规划,能加个好友讨论吗,qq:1571808481

  • lyh 2018年8月31日 上午11:19

    古月大神你好,请问你用的这个三次样条和moveit的TOPP是什么关系?为什么不直接用moveit给出的路点信息呢?是有什么问题吗?

    • 古月 回复 lyh 2018年9月1日 上午10:06

      三次样条是基于路径点计算的,实现周期输出

    • lyh 回复 古月 2018年9月3日 下午6:17

      大神您好!我是想用moveit实现笛卡尔轨迹规划,如果控制器再基于路径点(N个关节角度位置、速度、加速度)进行三次样条插值,那在moveit给出的路径点之间的执行结果就不是直线了,可能对于笛卡尔轨迹规划的概念还不是太清楚,麻烦古月大神指点一下笛卡尔轨迹规划中各部分具体都实现什么任务?

    • 古月 回复 lyh 2018年9月5日 上午10:18

      moveit中有笛卡尔轨迹规划的API,会在一条直线上规划密集的路径点,保证是直线,密集程度可以在API中设置。

  • pengye 2018年8月23日 上午9:57

    老师您好,我最近在看您2018ROS暑期学校的视频,其中第一个视频有一页PPT上写着:
    线性样条:位置连续,速度、加速度不连续
    三次样条:位置和速度连续,加速度不连续
    五次样条:位置、速度、加速度都连续
    我对这一段抱有疑问,根据您这篇文章里的图,三次样条加速度是连续的。从数学的角度上来说,三次样条的二阶导应该也是连续的。请问您可以解答一下嘛?谢谢!

    • 古月 回复 pengye 2018年8月23日 上午10:20

      这里的连续是指变化的圆滑程度,不能出现一次函数的转折点,最少是二次曲线

    • pengye 回复 古月 2018年8月23日 下午1:16

      谢谢您的解答,但我还有一点疑问,如果最少是二次曲线的话,为什么不分别用二次、三次、四次样条插值来实现以上三种情况呢?

    • 古月 回复 pengye 2018年8月29日 下午2:40

      这个没有不可以,也有二次和四次样条,只不过ROS用了常用的这三种

    • 熊波 回复 pengye 2018年9月25日 下午12:03

      古月老师您好,请问一下,如果我在你上面的基础上,还想在每个点上赋予速度大小约束,那是不是需要用5次样条?

    • 古月 回复 熊波 2018年9月28日 下午4:09

      是的,五次可以满足速度和加速度的约束

    • 合工大机器人 回复 pengye 2019年8月5日 下午1:05

      古月老师,我想问一下,点击plan and execute之后move_group规划了一条路径,用rostopic echo follow_joint_trajectory动作之后,数了一下,包含16个路径点,按照你上面的说法,move_group用的是五次插补算法?还有一个问题就是,规划的16个路径点的最后一个点,velocity为什么不是0?
      下面是我一次echo 的一个关节的数据提取:
      position:[0.000000,0.036218,0.072437,0.108655,0.144873,0.181092,0.217310,0.253528,0.289747,0.325965,0.362183,0.398401,0.434620,0.470838,0.507056,0.543275]
      velocity:
      [0.000000,0.131679,0.214509,0.263759,0.301177,0.322455,0.327268,0.327268,0.327268,0.327268,0.327268,0.327268,0.327268,0.327268,0.327268,0.327268]
      accelerations:
      [0.328648,0.328520,0.329367,0.303235,0.273031,0.085672,0.000000,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,-0.000000,-0.000000,0.000000]

    • 古月 回复 合工大机器人 2019年8月6日 下午3:28

      move_group不做插补,ros_controller做五次插补

    • lfl 回复 合工大机器人 2019年10月16日 下午11:51

      您好,您是怎样通过AddTimeParameterization模块将得到的角度序列添加上时间、角速度和角加速度约束的呢,还是只能对直角空间中的轨迹添加约束而不能对关节角度添加约束呢?
      万分感谢。

  • leslie 2018年7月24日 下午3:41

    胡老师,我定义了很多路径点,想让机械臂的末端沿着路径点移动,每个点求逆解的话算不出来路径,请问有什么办法吗

    • 古月 回复 leslie 2018年7月27日 上午10:18

      如果每个点都在工作空间内的话,有可能是求解器的问题,可以把默认的KDL改成fastik或者tracik试试

  • 赵艳超 2018年6月17日 下午3:41

    老师你好,我想问一下你这个MATLAB程序中的五次插值多项式图形生成的代码是什么?可以方便发给我么?

    • 古月 回复 赵艳超 2018年6月17日 下午5:48

      五次插值使用excel画的,程序就是文章中的C++代码

  • 小明 2018年4月16日 下午4:08

    古月大神,我想问一下,利用ros_control计算出的插补曲线,怎样看到呢,就是这条曲线怎样形象的表示出来,或者调出来数据,还有它是怎样周期性输出的呢?

    • 古月 回复 小明 2018年4月16日 下午6:15

      没办法直接看到,或者改代码加一个日志输出,或者只能看机器人的/joints_state

    • 深海独慕舟 回复 小明 2018年5月6日 上午10:44

      同学,我也在做机器人规划的毕设,能方便告知一下qq,想请教一些问题

  • libing403 2018年3月23日 上午8:53

    加速度抖动一般是指加速度曲线不连续,五次多项式轨迹加速度曲线可能没有那么“平滑”,但是不会出现加速度抖动吧,

    • 古月 回复 libing403 2018年3月23日 下午11:14

      就像文中图示,是连续的,是快速大幅的震荡

  • libing403 2018年3月23日 上午8:34

    研究的挺深入呀。我最近也在学习轨迹规划和伺服驱动https://blog.csdn.net/libing403,有机会向你学习学习(我也在深圳)。你最近在做工业机器人?我以为是在做家庭机器人去了。

    • 古月 回复 libing403 2018年3月23日 下午11:11

      没问题,是的,我现在做工业机器人控制系统

  • lijiashushu 2018年3月12日 下午8:48

    大神想知道这个程序三次样条的求解公式在哪里,找了好几个都没对上

    • 古月 回复 lijiashushu 2018年3月12日 下午11:19

      公式可以在数学分析的书上找到,代码实现我也是百度搜的

  • kls 2018年2月3日 下午12:08

    老师你好,现在我想依据给定的末端的起始、中点、终点三点的位置,依据这三点位置,给六关节的机器人规划运动路径,这种情况的话,是不是依据以下思路来做:用样条插值的方法先求出末端的运动轨迹,然后用addtp 的方法你姐出每个关节的位置量,然后驱动关节运动? 这样的思路可能实现吗?
    或者,您有没有可以建议的其他方法?

    • 古月 回复 kls 2018年2月5日 上午9:42

      如果是走直线,就用算出来的两点之间的直线方程求解每个周期的终端位姿,然后逆解驱动关节,重点是要规划好终端运动的速度和加速度。

    • 华科-王志豪 回复 古月 2018年6月24日 下午8:12

      胡师兄,这种走直线的情况,moveit有没有封装好的函数或者库呢?还是只能自己插补直线上的每个点来求逆解呢?

    • 古月 回复 华科-王志豪 2018年6月26日 下午7:39

      moveit里走直线的API有:computeCartesianPath

  • 小白 2018年1月30日 上午10:58

    古月老师您好,我是一个moveit新手,最近用pincher机械臂跑了一个网上的例子,驱动什么的都是从网上下载的,只了解到使机械臂运动调用的是一个movegroupinterface中move()方法,输入末端位子就能动,但机械臂在运行中有时抖动很厉害,想请教一下有什么解决办法吗,还有能调用别的API使机械臂运动吗?

    • 古月 回复 小白 2018年1月30日 下午12:25

      你好,使用moveit做运动控制的接口只有plan()、move()、execute(),这样才能保证上层调用的一致性,运动规划模块使用插件的机制集成(http://moveit.ros.org/documentation/planners/),这个是可以自己修改的,默认是ompl。运动规划出的轨迹需要使用控制器完成对机械臂硬件的周期插补控制,看你的现象,我猜测可能是控制器这一部分的性能不佳,可以调试一下。

    • 小白 回复 古月 2018年1月30日 下午2:45

      非常感谢你的答疑,我去调试一下

  • 小菜 2018年1月23日 下午4:02

    古老师您好
    我想问一下如何在ros中添加自己的规划算法比较方便
    是修改ompl库文件还是直接新建一个结点插入算法,文件的组织形式又是怎么样的呢

    • 古月 回复 小菜 2018年1月23日 下午7:49

      如果是自己的算法,可以使用插件的形式替换ompl,也可以自己创建节点,发布规划的轨迹,只要满足ROS的接口即可。

    • vander 回复 小菜 2019年11月5日 上午9:41

      你好,关于自己算法的验证,你现在有实现吗?

  • 小飞 2017年12月17日 下午7:10

    胡老师,我有两个问题:1.您博客中提到的加速度抖动是使用moveit规划中的速度和加速度信息出现的吗?
    2.那现在的三次样条插补的话只是使用了路点的位置信息,至于moveit规划的每个点的速度和加速度没有使用是吗?

    • 古月 回复 小飞 2017年12月19日 下午2:22

      1. 使用的是MoveIt规划出的轨迹点
      2. 是的,相当于没使用moveit规划的速度和加速度

    • 小飞 回复 古月 2017年12月19日 下午4:14

      在您一开始第一次使用的五次样条插补得时候,使用的轨迹点的位置,速度和加速度这三个条件是吗?

    • 古月 回复 小飞 2017年12月20日 下午3:34

      是的,使用的就是moveit规划出来的位置,速度,加速度,时间

  • Ghost 2017年12月14日 下午11:11

    古大神,我想问下笛卡尔空间运动的轨迹规划步骤是否是:
    1.逆解求解出每个采样点的关节角度+逆雅克比求解出每个采样点的关节速度
    2.用每个关节的关节角度和关节速度在关节空间拟合出平滑的连续曲线
    我在想,是否需要求解笛卡尔空间每个点的加速度,然后逆向求解出关节加速度再加上角度,角速度进行拟合。(机器人学的书都没讲过)。
    工业产品一般是怎么个步骤的?
    谢谢解答。

    • 古月 回复 Ghost 2017年12月15日 上午11:53

      是否需要加速度主要取决于控制需求,如果有加速度参数,控制可以更加平滑,一般工业产品是需要的。

  • 高颜 2017年11月8日 下午9:02

    古月老师,你好,我是在读研究生,有几个问题想请您解答一下,我的目的是快速搭建起来一个实际的机械臂,伺服驱动与电机已经搭建好,对于性能没有太高的要求,就是想通过ROS让伺服电机先运动起来,现在的想法是使用moveit得到运动轨迹,再利用插补算法计算出路径上各点对应各个轴的角度,发送给伺服驱动,完成运动,这样可不可以实现?看了您的微博,想问一下,moveit规划完成后的/joint_path_command消息有六个点的位置、速度等信息,是不是对应的六个轴的位置与速度等信息?如果是插补算法的话,只关心末端的位置啊,怎样利用ros自带的插补算法对最后这个轴末端点进行插补?MoveIt把路径规划后的消息发到下层的控制器,如果控制器想直接用ros提供的,该怎么做呢,下发与控制器这方面没有什么资料,还请古月老师,给多解释下。最后就是我如果通过插补得到各个关节的角度,是不是我直接发布一个话题,把关节角度发布出去,再利用ethercat订阅就可以了?

  • Y-liberal 2017年9月30日 上午10:42

    古月老师,想问你一下,这个轨迹如何与时间变量进行关联?因为在实际控制机器人的过程中轨迹的运动一般与时间有关系。另外在上面处理好轨迹之后,是用publisher-subscriber, service-client,action这三种里面的哪种将数据发送到机器人模型中?

    • 古月 回复 Y-liberal 2017年9月30日 下午2:04

      以上x轴就是时间;我这里是用在周期插补中的,有实时性要求,没用通讯,直接控制机器人关机,如果是仿真的话,ros_control插件已经提供插补的功能了

    • Y-liberal 回复 古月 2017年9月30日 下午2:22

      那你的意思是,你在控制机器人的时候其实和ros是没有关系的,对吧?

    • 古月 回复 Y-liberal 2017年9月30日 下午4:14

      是的,是在机器人控制器里做的,和ros没关系

    • Y-liberal 回复 古月 2017年10月12日 下午2:56

      古月老师,你上面的轨迹可以直接放到ros中用move_group节点驱动吗?还是需要将上面的轨迹经过AddTimeParameterization函数处理之后才能使用?如果需要处理的话,是只用AddTimeParameterization处理,还是需要其他的函数共同参与处理?如果有的话能不能提供一下这些函数?我用你上面的算法写了一个节点驱动7轴机器人运动,结果显示,机器人总是乱动,完全不是按轨迹来的,并且有些位置还算不出来。另外,在学习ROS系统的轨迹规划时,有没有指导性的教材推荐一下,感觉学完了moveit那些基础教程,后面更深入的东西又无从下手了,我用的是kinetic版本的ROS。

    • 古月 回复 Y-liberal 2017年10月13日 上午8:24

      这篇博客本身和ROS或者Moveit没有关系,是我在控制机器人伺服时的插补曲线,用在规划完成的轨迹点之间,可以认为是AddTimeParameterization处理后的轨迹。如果是需要学习轨迹规划的话,确实没有系统性的教材,主要还是需要自己看代码,ROS中的轨迹规划用的是ompl,可以去看下这个开源库的相关资料。

  • 李锐 2017年8月24日 下午2:38

    我想只知道真实机械臂抖动情况如何。我的UR5似乎不需要这样做路径规划吧?研一刚入门机器人项目。

    • 古月 回复 李锐 2017年8月25日 下午7:53

      UR5本身已经有控制器了,这部分工作在控制器中有,可以不需要的

  • aslm 2017年7月6日 下午3:40

    古月,你好,之前在东莞去听你讲课啦。请问如何把自己写的控制路径的速度算法集合进ros里面,类似于你现在的这篇博文。我这边的需求是希望机械臂末端走直线时,大部分是匀速的。你有什么建议或是相关书籍推荐吗?

    • 古月 回复 aslm 2017年7月7日 下午3:50

      可以直接自己写好算法,然后封装成ROS的Trajectory消息发布,主要看一下控制机器人的轨迹消息是什么。

  • 海漩涡 2017年3月8日 下午3:54

    月哥
    我发现个问题:这个AddTimeParameterization生成的时间、速度、加速度不能通过路程公式计算出来一一对应

    这是我的一段数据:
    positions:0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
    velocities:0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
    accelerations:0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
    duration: 0

    positions:0.193030, 0.063562, 0.000005, 0.000003, 0.034119, 0.000006, 0.000002, 0.000000, 0.000001, 0.000003,
    velocities:0.607736, 0.200118, 0.000016, 0.000010, 0.107422, 0.000019, 0.000007, 0.000001, 0.000004, 0.000009,
    accelerations:0.996455, 0.328117, 0.000026, 0.000017, 0.176131, 0.000031, 0.000011, 0.000001, 0.000006, 0.000015,
    duration: 443

    positions:0.386060, 0.127123, 0.000010, 0.000006, 0.068239, 0.000012, 0.000004, 0.000001, 0.000002, 0.000006,
    velocities:0.889884, 0.293025, 0.000023, 0.000015, 0.157294, 0.000028, 0.000010, 0.000001, 0.000005, 0.000013,
    accelerations:0.999741, 0.329199, 0.000026, 0.000017, 0.176712, 0.000031, 0.000011, 0.000001, 0.000006, 0.000015,
    duration: 247

    positions:0.579089, 0.190685, 0.000015, 0.000010, 0.102358, 0.000018, 0.000007, 0.000001, 0.000004, 0.000009,
    velocities:1.000000, 0.329284, 0.000026, 0.000017, 0.176757, 0.000031, 0.000011, 0.000001, 0.000006, 0.000015,
    accelerations:0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
    duration: 193ms

    positions:0.772119, 0.254247, 0.000020, 0.000013, 0.136478, 0.000024, 0.000009, 0.000001, 0.000005, 0.000012,
    velocities:1.000000, 0.329284, 0.000026, 0.000017, 0.176757, 0.000031, 0.000011, 0.000001, 0.000006, 0.000015,
    accelerations:0.000000, -0.000000, -0.000000, -0.000000, -0.000000, 0.000000, -0.000000, -0.000000, 0.000000, -0.000000,
    duration: 193ms

    ——————————————–
    在速度的变化上我不能使用 V = V0 + at来计算下一个速度值

    我查看其源码 :
    v1 = (q2-q1)/dt1;
    v2 = (q3-q2)/dt2; // Needed to ensure continuous velocity for first point
    a = 2.0*(v2-v1)/(dt1+dt2);
    v = (v1+v2)/2;

    由这些源码和我自己的调试理解为:计算出的速度和加速都是在两段路径点的平均速度和平均加速,这样的值不能直接使用路程公式。

    请问你对此有什么看法?这些轨迹上的速度、加速度该怎么使用来控制电机?

    • 古月 回复 海漩涡 2017年3月8日 下午10:00

      如果要严格按照规划的轨迹点运动,需要使用五次曲线做插补

    • lfl 回复 海漩涡 2019年10月17日 上午12:02

      您好,您是通过什么样的方式把这些角度值添加时间和速度约束的呀,有特定的接口吗,谢谢啦。

  • 海漩涡 2017年2月21日 下午6:31

    请问月哥arbotix里是否有一套能控制真实手臂的系统,我看到有插值、PID控制,但是不确定能否用它来实现控制真实的手臂

  • 海漩涡 2017年2月14日 上午11:23

    看了数遍,还是感觉没有理解透
    1、算法代码的实现
    2、五次样条的应用

    继续学习,还会再来看的

    • 古月 回复 海漩涡 2017年2月14日 下午5:39

      我这里写的相关理论比较少,上边代码写的是三次样条的情况,建议可以找一些五次样条的相关理论公式,原理差不多

  • 古月 2017年2月7日 上午9:56

    功夫茶里用的是五次样条

  • 海漩涡 2017年2月5日 上午9:23

    moveit里用的是B样条曲线,你在功夫茶里用的是三次样条吗

    • itfanr 回复 海漩涡 2019年3月28日 下午2:21

      moveit controller用的是五次多项式样条