问题引入
- 马上要进入研究生阶段的学习，跟导师研究机械臂的抓取问题，但本科阶段大多数都在研究一些深度学习算法，所以我准备从零开始搭建一个自己的机械臂，通过实践来掌握机械臂抓取问题的整个体系。在软件层面上我使用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
感觉整体来看变化不是很大，就是轨迹点多了一些，我这里插值了四倍，还是放到机械臂上跑一下看一下变化，这里我的机械臂还没有搭建完成，搭建完成再测试把！
