ros moveit机械臂轨迹 三次插补

问题引入

• 马上要进入研究生阶段的学习，跟导师研究机械臂的抓取问题，但本科阶段大多数都在研究一些深度学习算法，所以我准备从零开始搭建一个自己的机械臂，通过实践来掌握机械臂抓取问题的整个体系。在软件层面上我使用ros+moveit作为机械臂开发的主体，moveit可以帮助我们轻松的完成正逆解以及轨迹的规划，是非常好的工具。
• moveit的输出一般是一系列的轨迹点，包含每一个关节的位置position[],速度velocities[]以及加速度accelerations[],通过action接口进行通讯，但是moveit直接输出的轨迹点数量很少并且没有进行插补，为了方便观察我将moveit输出的轨迹保存为.txt文件，使用matplotlib进行绘图如下图所示：

front_tpva.png

通过这张图可以看到moveit输出的轨迹并不圆滑，尤其是加速度曲线，这代表电机的力矩变化很大，会造成机械臂在运动的过程中抖动。

• 所以我打算写一个三次轨迹插补的算法对moveit输出的轨迹进行插补，本篇文章的所有程序都脱离ros和moveit，我将moveit输出的轨迹选取一个关节保存到tractory.txt文件中，文件形式如下图:

  
  tractory.txt.png

第一行是时间，第二行是位置，第三行是速度，第四行是加速度，至于数据具体含义以及怎么使用moveit导出这个轨迹数据我在这篇文章中不去具体阐述。

• 为了方便自己调试也便其他小伙伴复用，本篇文章可以脱离机械臂的具体问题，看作对tractory.txt文件中的数据进行三次插补。
  在介绍具体流程和代码之前我把我的工程目录放在下面。

工程目录.png

实现流程

1.读取tractory.txt文件并转化为double数组

这一部分我感觉写的有一些麻烦，因为我本身c++编程能力并不好，我本科是机械专业，之前编成用python比较多，这里谁有简单实现可以分享以下！

struct String_data
{
    string t_str;
    string pos_str;
    string vel_str; 
    string acc_str;
};

class Deal_data
{
public:


    void load_data(const char * name)
    {
        ifstream openfile;
        openfile.open(name,ios::in);
        if (!openfile.is_open())
        {
            cout << "file can't open!" << endl;
        }
        string buf;
        int count = 0;
        while(getline(openfile,buf))
        {
            if (count == 0)
            {
                str_data.t_str = buf;
                
            }
            if(count==1)
            {
                str_data.pos_str = buf;
            }
            if(count==2)
            {
                str_data.vel_str = buf;
            }   
            if(count==3)
            {
                str_data.acc_str = buf;
            }   
            count ++;   
        }
    }

    void get_point_num()
    {
        int p = 0;
        int num = 0;
        int splite_position[100];
        string t_str_inner = str_data.t_str; 
        while(t_str_inner.find(",",p)!=string::npos)
        {
            p = t_str_inner.find(",",p);
            // int型数组用来储存所有,的位置
            splite_position[num] = p;
            p = p + 1;
            num ++;
        }
        point_num = num;

    }

    double * get_data_t()
    {
        int p = 0;
        int num = 0;
        int splite_position[100];
        string t_str_inner = str_data.t_str; 
        while(t_str_inner.find(",",p)!=string::npos)
        {
            p = t_str_inner.find(",",p);
            splite_position[num] = p;
            p = p + 1;
            num ++;
        }
        char t_char[point_num];
        double * t = new double[point_num];
        for (int i=0;i<point_num;i++)
        {
            int end = splite_position[i];
            if(i==0)
            {
                // 字符串分割
                string str = t_str_inner.substr(0,end);
                // char转double,并存入数组
                t[i] = atof(str.c_str());
                // cout << t[i] << endl;
            }
            else
            {
                int start = splite_position[i-1]+1;
                string str = t_str_inner.substr(start,end - start); 
                t[i] = atof(str.c_str());
                // cout << t[i] << endl;
            }
        }
        return t;
    }

    double * get_data_pos()
    {
        int p = 0;
        int num = 0;
        int splite_position[100];
        string pos_str_inner = str_data.pos_str; 
        while(pos_str_inner.find(",",p)!=string::npos)
        {
            p = pos_str_inner.find(",",p);
            // int型数组用来储存所有,的位置
            splite_position[num] = p;
            p = p + 1;
            num ++;
        }
        char t_char[point_num];
        double * pos = new double[point_num];
        for (int i=0;i<point_num;i++)
        {
            int end = splite_position[i];
            if(i==0)
            {
                string str = pos_str_inner.substr(0,end);
                pos[i] = atof(str.c_str());
            }
            else
            {
                int start = splite_position[i-1]+1;
                string str = pos_str_inner.substr(start,end - start); 
                pos[i] = atof(str.c_str());
            }
        }
        return pos;
    }
    double * get_data_vel()
    {
        int p = 0;
        int num = 0;
        int splite_position[100];
        string vel_str_inner = str_data.vel_str; 
        while(vel_str_inner.find(",",p)!=string::npos)
        {
            p = vel_str_inner.find(",",p);
            // int型数组用来储存所有,的位置
            splite_position[num] = p;
            p = p + 1;
            num ++;
        }
        char t_char[point_num];
        double * vel = new double[point_num];
        for (int i=0;i<point_num;i++)
        {
            int end = splite_position[i];
            if(i==0)
            {
                string str = vel_str_inner.substr(0,end);
                vel[i] = atof(str.c_str());
            }
            else
            {
                int start = splite_position[i-1]+1;
                string str = vel_str_inner.substr(start,end - start); 
                vel[i] = atof(str.c_str());
            }
        }
        return vel;
    }

    double * get_data_acc()
    {
        int p = 0;
        int num = 0;
        int splite_position[100];
        string acc_str_inner = str_data.acc_str; 
        while(acc_str_inner.find(",",p)!=string::npos)
        {
            p = acc_str_inner.find(",",p);
            // int型数组用来储存所有,的位置
            splite_position[num] = p;
            p = p + 1;
            num ++;
        }
        char t_char[point_num];
        double * acc = new double[point_num];
        for (int i=0;i<point_num;i++)
        {
            int end = splite_position[i];
            if(i==0)
            {
                string str = acc_str_inner.substr(0,end);
                acc[i] = atof(str.c_str());
            }
            else
            {
                int start = splite_position[i-1]+1;
                string str = acc_str_inner.substr(start,end - start); 
                acc[i] = atof(str.c_str());
            }
        }
        return acc;
    }
    int point_num;
private:
    struct String_data str_data;

};

1.进行轨迹三次插补

插补的具体实现我参照了古月老师的方法。边界条件都为0。
cublicSpline.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_ */

cublicSpine.cpp源文件

/* 三次样条插补 */
#include <string.h>
#include <stddef.h>
#include <stdio.h>
#include "cubicSpline.h"

using namespace std;
/* 初始化输入输出速度加速度 */
double acc = 0, vel = 0;
double x_out = 0, y_out = 0;

/* 三次样条无参构造 */
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
    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;
    //test end
 
    return true;
}

3.main.cpp以及CMakeLists.txt

别忘了把读取tractory.txt数据的程序，放到主函数的上面，我在上面写过了就不放上去了。

#include<iostream>
#include<stdio.h>
#include<stddef.h>
#include<string.h>
#include<fstream>
#include<vector>
#include "cubicSpline.h"
double acc = 0, vel = 0;
double x_out = 0, y_out = 0;
int main()
{
    setlocale(LC_ALL,"");
    char file_name[] = "/home/zhw/slam/learn_c++/TRY_SPLINE/src/tractory.txt";
    Deal_data opf;
    opf.load_data(file_name);
    opf.get_point_num();
    int point_num = opf.point_num;
    cout << opf.point_num << endl;
    double * time = opf.get_data_t();
    double * pos = opf.get_data_pos();
    double * vel_ = opf.get_data_vel();
    double * acc_ = opf.get_data_acc();

    cubicSpline spline;
    // lumbar test
    spline.loadData(time,pos, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
    double rate = (time[point_num-1] - time[0])/(point_num*4);
    double deal_pos[point_num*4];
    double deal_vel[point_num*4];
    double deal_acc[point_num*4];
    double time_from_start_[point_num*4];
    for (int k = 0; k <= point_num*4 ; k++) {
        printf("[---位置、速度、加速度---]");
        printf("%0.9f, %0.9f, %0.9f\n",y_out, vel, acc);
        spline.getYbyX(x_out, y_out);
        time_from_start_[k] = x_out;
        deal_pos[k] = y_out;
        deal_vel[k] = vel;
        deal_acc[k] = acc;
        x_out += rate;
    }
    FILE *f;
    f = fopen("/home/zhw/slam/learn_c++/TRY_SPLINE/deal_tractory.txt","a");
    for(int j=0;j<=point_num*4;j++)
    {
        fprintf(f,"%f,",time_from_start_[j]);//6
    }
    fprintf(f,"\n");
    for(int j=0;j<=point_num*4;j++)
    {
        fprintf(f,"%f,",deal_pos[j]);//7
    }
    fprintf(f,"\n");
    for(int j=0;j<=point_num*4;j++)
    {
        fprintf(f,"%f,",deal_vel[j]);//8
    }
    fprintf(f,"\n");
    for(int j=0;j<=point_num*4;j++)
    {
        fprintf(f,"%f,",deal_acc[j]);//9
    }
    fprintf(f,"\n");
    fclose(f);

    return 0;
}

这段程序中包含了将插值后的数据保存到deal_tractory.txt文件中。

cmake_minimum_required(VERSION 3.0.2)
include_directories(${CMAKE_SOURCE_DIR}/include)
add_library(cubicSpline SHARED cubicSpline.cpp)
add_executable(main main.cpp)
target_link_libraries(main cubicSpline)

结果

最后把插值前插值后的对比图show一下！！！

deal_tpva.png

感觉整体来看变化不是很大，就是轨迹点多了一些，我这里插值了四倍，还是放到机械臂上跑一下看一下变化，这里我的机械臂还没有搭建完成，搭建完成再测试把！