問題引入
- 馬上要進入研究生階段的學習螟炫，跟導師研究機械臂的抓取問題，但本科階段大多數(shù)都在研究一些深度學習算法惭婿，所以我準備從零開始搭建一個自己的機械臂不恭，通過實踐來掌握機械臂抓取問題的整個體系。在軟件層面上我使用ros+moveit作為機械臂開發(fā)的主體财饥，moveit可以幫助我們輕松的完成正逆解以及軌跡的規(guī)劃换吧，是非常好的工具。
- moveit的輸出一般是一系列的軌跡點钥星，包含每一個關節(jié)的位置position[],速度velocities[]以及加速度accelerations[],通過action接口進行通訊沾瓦，但是moveit直接輸出的軌跡點數(shù)量很少并且沒有進行插補，為了方便觀察我將moveit輸出的軌跡保存為.txt文件，使用matplotlib進行繪圖如下圖所示：
front_tpva.png
通過這張圖可以看到moveit輸出的軌跡并不圓滑贯莺，尤其是加速度曲線风喇，這代表電機的力矩變化很大，會造成機械臂在運動的過程中抖動缕探。
-
所以我打算寫一個三次軌跡插補的算法對moveit輸出的軌跡進行插補魂莫，本篇文章的所有程序都脫離ros和moveit，我將moveit輸出的軌跡選取一個關節(jié)保存到tractory.txt文件中爹耗，文件形式如下圖:
tractory.txt.png
第一行是時間耙考，第二行是位置，第三行是速度潭兽，第四行是加速度倦始，至于數(shù)據(jù)具體含義以及怎么使用moveit導出這個軌跡數(shù)據(jù)我在這篇文章中不去具體闡述。
- 為了方便自己調試也便其他小伙伴復用山卦，本篇文章可以脫離機械臂的具體問題鞋邑，看作對tractory.txt文件中的數(shù)據(jù)進行三次插補。
在介紹具體流程和代碼之前我把我的工程目錄放在下面账蓉。
工程目錄.png
實現(xiàn)流程
1.讀取tractory.txt文件并轉化為double數(shù)組
這一部分我感覺寫的有一些麻煩枚碗，因為我本身c++編程能力并不好，我本科是機械專業(yè)剔猿，之前編成用python比較多视译，這里誰有簡單實現(xiàn)可以分享以下！
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型數(shù)組用來儲存所有,的位置
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,并存入數(shù)組
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型數(shù)組用來儲存所有,的位置
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型數(shù)組用來儲存所有,的位置
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型數(shù)組用來儲存所有,的位置
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.進行軌跡三次插補
插補的具體實現(xiàn)我參照了古月老師的方法归敬。邊界條件都為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();
}
/* 初始化參數(shù) */
void cubicSpline::initParam()
{
x_sample_ = y_sample_ = M_ = NULL;
sample_count_ = 0;
bound1_ = bound2_ = 0;
}
/* 釋放參數(shù) */
void cubicSpline::releaseMem()
{
delete x_sample_;
delete y_sample_;
delete M_;
initParam();
}
/* 加載關節(jié)位置數(shù)組等信息 */
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;
}
// 追趕法解方程求二階偏導數(shù)
double f1=bound1_, f2=bound2_;
double *a=new double[sample_count_]; // a:稀疏矩陣最下邊一串數(shù)
double *b=new double[sample_count_]; // b:稀疏矩陣最中間一串數(shù)
double *c=new double[sample_count_]; // c:稀疏矩陣最上邊一串數(shù)
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; // 中間一串數(shù)為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;
}
/* 得到速度和加速度數(shù)組 */
bool cubicSpline::getYbyX(double &x_in, double &y_out)
{
int klo,khi,k;
klo=0;
khi=sample_count_-1;
double hh,bb,aa;
// 二分法查找x所在區(qū)間段
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數(shù)據(jù)的程序，放到主函數(shù)的上面汪茧，我在上面寫過了就不放上去了椅亚。
#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;
}
這段程序中包含了將插值后的數(shù)據(jù)保存到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
感覺整體來看變化不是很大，就是軌跡點多了一些珠插，我這里插值了四倍惧磺，還是放到機械臂上跑一下看一下變化，這里我的機械臂還沒有搭建完成捻撑，搭建完成再測試把磨隘！
