python工业机器人轨迹规划_机械臂轨迹规划——三次样条对比研究

在之前的博客中(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。关于这个算法的原理，作者发布了一篇论文当然，这个算法也不是无敌的，其中有一个关键问题就是加速度存在抖动的问题，可以参见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

#include

#include

#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

b[i]=2; // 中间一串数为2

for(int i=0;i

h[i]=x_sample_[i+1]-x_sample_[i]; // 各段步长

for(int i=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

c[i]=h[i]/(h[i-1]+h[i]);

for(int i=0;i

f[i]=(y_sample_[i+1]-y_sample_[i])/(x_sample_[i+1]-x_sample_[i]);

for(int i=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

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

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

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

乍看上去好像和Matlab画出来的差不多，把数据导入到Matlab中进行一下对比：

数据几乎是完全重叠的，说明我们的算法应该没有问题。至于三次样条相比五阶插补的效果如何，我们还是需要将两者进行对比。还是使用相同的实验输入数据，我们放到五阶插补算法里计算，首先看一下位置曲线的对比：

貌似看不出来多大差别，两者的趋势几乎一致。那么就看一下速度曲线的对比：

哎呦！看到有一些不同了吧，五阶曲线明显抖动更多，再来看一下加速度曲线的对比：

哎呀！这就是传说中的“魔鬼的步伐”吧！五阶算法计算出来的加速度明显抖动的厉害，而且会超出加速度的限制，三次算法算出来的加速度就没那么光滑了。

OK，今天的对比研究到此告一段落，至于实际放到机械臂上边的效果如何，后边再继续实验。