在之前的博客中(https://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,今天的对比研究到此告一段落,至于实际放到机械臂上边的效果如何,后边再继续实验。