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

  • 内容
  • 评论
  • 相关

    在之前的博客中(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实现的代码如下:
  1. %三次样条测试
  2. %原始数据点
  3. X = [0, 0.422955130, 0.598557636, 0.734591320, 0.850603738, 0.953558869, 1.056514000, 1.159469131, 1.274332912, 1.409208218, 1.585026197, 2];
  4. Y = [0, 0.14881055128822188, 0.2976136037517004, 0.4464166562151788, 0.5952197086786574, 0.7440227611421358, 0.8928258136056142, 1.0416288660690929, 1.1904319185325714, 1.3392349709960498, 1.4880380234595283, 1.6368410759230068];
  5. s = csapi(X,Y);   %三次样条
  6. fnplt(s, 'r');         %绘制样条曲线
  7. hold on
  8. plot(X,Y,'o')       %绘制原始数据点
  9. v=fnder(s,1);     %样条曲线一阶导数得到速度曲线
  10. fnplt(v, 'g');        %绘制速度曲线
  11. hold on
  12. a=fnder(v,1);     %样条曲线二阶导数得到加速度曲线
  13. fnplt(a, 'b');        %绘制加速度曲线
  14. legend('位置样条曲线','原始数据点','速度曲线','加速度曲线')
      运行之后就可以看到效果啦:
3
        看过之后,我们就对三次样条所产生的效果大致明白了,从位置曲线上来看,确实平滑了,速度的上升阶段接近于匀速,在最上边的匀速段稍有抖动,机器速度呈一阶线性变化。
       OK,接下来就是要自己实现了,基于的原理就是《数值分析》上边讲到的,另外参考了网上众多实现的代码。二话不说,先上代码:
       头文件cubicSpline.h:
  1. #ifndef _CUBIC_SPLINE_H_
  2. #define _CUBIC_SPLINE_H_
  3.  
  4. class cubicSpline
  5. {
  6. public:
  7.     typedef enum _BoundType
  8.     {
  9.         BoundType_First_Derivative,
  10.         BoundType_Second_Derivative
  11.     }BoundType;
  12.  
  13. public:
  14.     cubicSpline();
  15.     ~cubicSpline();
  16.  
  17.     void initParam();
  18.     void releaseMem();
  19.  
  20.     bool loadData(double *x_data, double *y_data, int count, double bound1, double bound2, BoundType type);
  21.     bool getYbyX(double &x_in, double &y_out);
  22.  
  23. protected:
  24.     bool spline(BoundType type);
  25.  
  26. protected:
  27.     double *x_sample_, *y_sample_;
  28.     double *M_;
  29.     int sample_count_;
  30.     double bound1_, bound2_;
  31. };
  32.  
  33. #endif /* _CUBIC_SPLINE_H_ */
       源文件cubicSpline.cpp:
  1. #include <string.h>
  2. #include <stddef.h>
  3. #include <stdio.h>
  4. #include "cubicSpline.h"
  5.  
  6. cubicSpline::cubicSpline()
  7. {
  8. }
  9.  
  10. cubicSpline::~cubicSpline()
  11. {
  12.     releaseMem();
  13. }
  14.  
  15. void cubicSpline::initParam()
  16. {
  17.     x_sample_ = y_sample_ = M_ = NULL;
  18.     sample_count_ = 0;
  19.     bound1_ = bound2_ = 0;
  20. }
  21.  
  22. void cubicSpline::releaseMem()
  23. {
  24.     delete x_sample_;
  25.     delete y_sample_;
  26.     delete M_;
  27.  
  28.     initParam();
  29. }
  30.  
  31. bool cubicSpline::loadData(double *x_data, double *y_data, int count, double bound1, double bound2, BoundType type)
  32. {
  33.     if ((NULL == x_data) || (NULL == y_data) || (count < 3)
  34.         || (type > BoundType_Second_Derivative) || (type < BoundType_First_Derivative))
  35.         return false;
  36.  
  37.     initParam();
  38.  
  39.     x_sample_ = new double[count];
  40.     y_sample_ = new double[count];
  41.     M_        = new double[count];
  42.     sample_count_ = count;
  43.  
  44.     memcpy(x_sample_, x_data, sample_count_*sizeof(double));
  45.     memcpy(y_sample_, y_data, sample_count_*sizeof(double));
  46.  
  47.     bound1_ = bound1;
  48.     bound2_ = bound2;
  49.  
  50.     return spline(type);
  51. }
  52.  
  53.  
  54. bool cubicSpline::spline(BoundType type)
  55. {
  56.     if ((type < BoundType_First_Derivative) || (type > BoundType_Second_Derivative))
  57.         return false;
  58.  
  59.     //  追赶法解方程求二阶偏导数
  60.     double f1=bound1_, f2=bound2_;
  61.  
  62.     double *a=new double[sample_count_];                //  a:稀疏矩阵最下边一串数
  63.     double *b=new double[sample_count_];                //  b:稀疏矩阵最中间一串数
  64.     double *c=new double[sample_count_];                //  c:稀疏矩阵最上边一串数
  65.     double *d=new double[sample_count_];
  66.  
  67.     double *f=new double[sample_count_];
  68.  
  69.     double *bt=new double[sample_count_];
  70.     double *gm=new double[sample_count_];
  71.  
  72.     double *h=new double[sample_count_];
  73.  
  74.     for(int i=0;i<sample_count_;i++)
  75.         b[i]=2;                                //  中间一串数为2
  76.     for(int i=0;i<sample_count_-1;i++)
  77.         h[i]=x_sample_[i+1]-x_sample_[i];      // 各段步长
  78.     for(int i=1;i<sample_count_-1;i++)
  79.         a[i]=h[i-1]/(h[i-1]+h[i]);
  80.     a[sample_count_-1]=1;
  81.  
  82.     c[0]=1;
  83.     for(int i=1;i<sample_count_-1;i++)
  84.         c[i]=h[i]/(h[i-1]+h[i]);
  85.  
  86.     for(int i=0;i<sample_count_-1;i++)
  87.         f[i]=(y_sample_[i+1]-y_sample_[i])/(x_sample_[i+1]-x_sample_[i]);
  88.  
  89.     for(int i=1;i<sample_count_-1;i++)
  90.         d[i]=6*(f[i]-f[i-1])/(h[i-1]+h[i]);
  91.  
  92.     //  追赶法求解方程
  93.     if(BoundType_First_Derivative == type)
  94.     {
  95.         d[0]=6*(f[0]-f1)/h[0];
  96.         d[sample_count_-1]=6*(f2-f[sample_count_-2])/h[sample_count_-2];
  97.  
  98.         bt[0]=c[0]/b[0];
  99.         for(int i=1;i<sample_count_-1;i++)
  100.             bt[i]=c[i]/(b[i]-a[i]*bt[i-1]);
  101.  
  102.         gm[0]=d[0]/b[0];
  103.         for(int i=1;i<=sample_count_-1;i++)
  104.             gm[i]=(d[i]-a[i]*gm[i-1])/(b[i]-a[i]*bt[i-1]);
  105.  
  106.         M_[sample_count_-1]=gm[sample_count_-1];
  107.         for(int i=sample_count_-2;i>=0;i--)
  108.             M_[i]=gm[i]-bt[i]*M_[i+1];
  109.     }
  110.     else if(BoundType_Second_Derivative == type)
  111.     {
  112.         d[1]=d[1]-a[1]*f1;
  113.         d[sample_count_-2]=d[sample_count_-2]-c[sample_count_-2]*f2;
  114.  
  115.         bt[1]=c[1]/b[1];
  116.         for(int i=2;i<sample_count_-2;i++)
  117.             bt[i]=c[i]/(b[i]-a[i]*bt[i-1]);
  118.  
  119.         gm[1]=d[1]/b[1];
  120.         for(int i=2;i<=sample_count_-2;i++)
  121.             gm[i]=(d[i]-a[i]*gm[i-1])/(b[i]-a[i]*bt[i-1]);
  122.  
  123.         M_[sample_count_-2]=gm[sample_count_-2];
  124.         for(int i=sample_count_-3;i>=1;i--)
  125.             M_[i]=gm[i]-bt[i]*M_[i+1];
  126.  
  127.         M_[0]=f1;
  128.         M_[sample_count_-1]=f2;
  129.     }
  130.     else
  131.         return false;
  132.  
  133.     delete a;
  134.     delete b;
  135.     delete c;
  136.     delete d;
  137.     delete gm;
  138.     delete bt;
  139.     delete f;
  140.     delete h;
  141.  
  142.     return true;
  143. }
  144.  
  145. bool cubicSpline::getYbyX(double &x_in, double &y_out)
  146. {
  147.     int klo,khi,k;
  148.     klo=0;
  149.     khi=sample_count_-1;
  150.     double hh,bb,aa;
  151.  
  152.     //  二分法查找x所在区间段
  153.     while(khi-klo>1)
  154.     {
  155.         k=(khi+klo)>>1;
  156.         if(x_sample_[k]>x_in)
  157.             khi=k;
  158.         else
  159.             klo=k;
  160.     }
  161.     hh=x_sample_[khi]-x_sample_[klo];
  162.  
  163.     aa=(x_sample_[khi]-x_in)/hh;
  164.     bb=(x_in-x_sample_[klo])/hh;
  165.  
  166.     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;
  167.  
  168.     //////test
  169.     double acc = 0, vel = 0;
  170.     acc = (M_[klo]*(x_sample_[khi]-x_in) + M_[khi]*(x_in - x_sample_[klo])) / hh;
  171.     vel = M_[khi]*(x_in - x_sample_[klo]) * (x_in - x_sample_[klo]) / (2 * hh)
  172.         - M_[klo]*(x_sample_[khi]-x_in) * (x_sample_[khi]-x_in) / (2 * hh)
  173.         + (y_sample_[khi] - y_sample_[klo])/hh
  174.         - hh*(M_[khi] - M_[klo])/6;
  175.     printf("%0.9f, %0.9f, %0.9f\n",y_out, vel, acc);
  176.     //////test end
  177.  
  178.     return true;
  179. }
       测试文件main.cpp:

  1. #include <stdio.h>
  2. #include "cubicSpline.h"
  3.  
  4. #define POINTS_COUNT 12
  5. int main()
  6. {
  7.     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};
  8.     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};
  9.  
  10.     double x_out = 0;
  11.     double y_out = 0;
  12.  
  13.     cubicSpline spline;
  14.     spline.loadData(x_data, y_data, POINTS_COUNT, 0, 0, cubicSpline::BoundType_First_Derivative);
  15.  
  16.     x_out = -0.004;
  17.     for(int i=0; i<=500; i++)
  18.     {
  19.         x_out = x_out + 0.004;
  20.         spline.getYbyX(x_out, y_out);
  21.         //printf("%f, %0.9f \n", x_out, y_out);
  22.     }
  23.  
  24.     return 0;
  25. }

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


原创文章,转载请注明: 转载自古月居

本文链接地址: 机械臂轨迹规划——三次样条对比研究

微信 OR 支付宝 扫描二维码
为本文作者 打个赏
pay_weixinpay_weixin

评论

20条评论
  1. Gravatar 头像

    Y-liberal 回复

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

    • 古月

      古月 回复

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

      • Gravatar 头像

        Y-liberal 回复

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

          • Gravatar 头像

            Y-liberal 回复

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

            • 古月

              古月 回复

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

  2. Gravatar 头像

    李锐 回复

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

    • 古月

      古月 回复

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

  3. Gravatar 头像

    aslm 回复

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

    • 古月

      古月 回复

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

  4. Gravatar 头像

    海漩涡 回复

    月哥
    我发现个问题:这个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;

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

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

    • 古月

      古月 回复

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

  5. Gravatar 头像

    海漩涡 回复

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

  6. Gravatar 头像

    海漩涡 回复

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

    继续学习,还会再来看的

    • 古月

      古月 回复

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

  7. 古月

    古月 回复

    功夫茶里用的是五次样条

  8. Gravatar 头像

    海漩涡 回复

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

发表评论

电子邮件地址不会被公开。 必填项已用*标注