机械臂轨迹规划——三次样条对比研究_人工智能

在之前的博客中（https://blog.75271.com/44986.html），展示了目前我们机械臂的控制效果。在技术层面，机械臂的路径规划由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节）。按照这样的插补运算，确实可以完成插补的任务，如下图所示：

有一个很重要的问题就是加速度的抖动，应该就是传说中的“龙格现象”，这种抖动已经远远超过了机器人的加速度限制。于是，我产生了对这种五次多项式插补的怀疑，我们竟然以这样的控制完成了功夫茶的演示，果然是新手。这段时间我就在研究这个插补到底应该怎么做，参考了知乎上各位大神的帖子，发现三次样条插补的算法用的很多，就先从这个入手学习吧。

样条插值是一种工业设计中常用的、得到平滑曲线的一种插值方法，三次样条又是其中用的较为广泛的一种。三次样条的数学定义如下：

至于三次样条方程组的推导和求解公式这里就不详述了，可以参考《数学分析》（李庆扬）的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('位置样条曲线','原始数据点','速度曲线','加速度曲线')

运行之后就可以看到效果啦：

看过之后，我们就对三次样条所产生的效果大致明白了，从位置曲线上来看，确实平滑了，速度的上升阶段接近于匀速，在最上边的匀速段稍有抖动，机器速度呈一阶线性变化。

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中进行了整理：