通过ROS控制真实机械臂(7)---三次样条插补
在之前的move_group界面中，当点击plan and execute之后，move_group就会帮我们规划出一条通往指定位姿的轨迹，发布在follow_joint_trajectory上，通过rostopic echo /redwall_arm/follow_joint_trajctory/goal,可以看到其中包含了positions: velocities: accelerations: time_from_start的路径点，称之为p[],v[],a[],T[]，数了一下，一共16个路径点，实际的点不是16，有些没有显示出来！！！
这个消息中就包含了路径规划后的所有路点信息，也就是描述机械臂应该以什么样的姿态运动到目标位置。关于MoveIt的运动规划算法，
运动规划器根据环境计算得出一条空间轨迹，但是这条轨迹只包含位置关系，并没有告诉机器人应该以怎样的速度、加速度运动
于是，就要经过AddTimeParameterization这个模块， 为这条空间轨迹进行速度、加速度约束，并且计算运动到每个路点的时间。
可以去看MoveIt中AddTimeParameterization模块的的代码实.这个算法的全称是 Time-Optimal Path Parameterization Algorithm，简称TOPP。参考链接：http://www.guyuehome.com/752?replytocom=48315
根据时间最优的原则，输出所有点的速度、加速度、时间信息。其中存在的一个关键问题就是加速度存在抖动。MoveIt把路径规划后的消息发到下层的控制器，控制器当然要根据所有的路点完成中间状态完成插补运算，如果用多项式插补，在已知所有路点速度、加速度、时间的条件下，需要使用一个五阶多项式来描述（参见《机器人学导论》或https://blog.csdn.net/qq_26565435/article/details/94545986）
下图所示是用五次多项式插补的一条路径，可以看到速度加速都是平滑的，按照这样的插补运算，确实可以完成插补的任务，但是由于加速度曲线完全拟合，在实际的控制中表现成加速度抖动问题，也就是传说中的”龙格现象“，这种抖动已经远远超过了机器人的加速度限制图片来源：https://blog.csdn.net/libing403/article/details/78715418
所以采用三次样条插补算法： 三次多项式实际是速度规划里面常说的PVT算法。PVT 模 式 是 指 位 置 — 速 度 — 时 间（Position-Velocity-Time）模式。PVT模式是一种简单又有效的运动控制模式，用户只需要给定离散点的位置、速度和时间，运动控制卡的插补算法将会生成一条连续、平滑的运动路径。
三次样条所产生的效果大致：从位置曲线和速度曲线上来看是平滑的，加速度也不存在五次多项式那样的抖动，但是也引入了新的问题就是加速度不连续，对于一些应用的动力学和惯性载荷会产生一些不期待的影响。
图片来源https://blog.csdn.net/libing403/article/details/78698322
接下来看三次样条的具体实现方法：每个轨迹点都有 positions[], velocities[], accelerations[], time_from_start []。古月老师源码要输入的是 两个数组，x时间，y位置。因此需要把轨迹点中每个关节的所有位置取出来放到相应的数组里。时间点的数组可以共用一个。
头文件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_ */
使用上一篇博客的action服务端节点：redwall_arm_server.cpp ，订阅move_group发布的follow_joint_trajectory动作，获取所有的路径点信息，然后经行三次样条插补。
/* ROS action server */
#include
#include
#include
#include
#include
/* 三次样条插补 */
#include
#include
#include
#include "cubicSpline.h"
using namespace std;
/* action 服务端声明 */
typedef actionlib::SimpleActionServer Server;
/* 初始化输入输出速度加速度 */
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=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=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;
printf("[---位置、速度、加速度---]");
printf("%0.9f, %0.9f, %0.9f\n",y_out, vel, acc);
//test end
return true;
}
/* 收到action的goal后调用的回调函数 */
void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) {
/* move_group规划的路径包含的路点个数 */
int point_num = goal->trajectory.points.size();
ROS_INFO("%d",point_num);
/* 各个关节位置 */
double p_lumbar[point_num];
double p_big_arm[point_num];
double p_small_arm[point_num];
double p_wrist[point_num];
double p_hand[point_num];
/* 各个关节速度 */
double v_lumbar[point_num];
double v_big_arm[point_num];
double v_small_arm[point_num];
double v_wrist[point_num];
double v_hand[point_num];
/* 各个关节加速度 */
double a_lumbar[point_num];
double a_big_arm[point_num];
double a_small_arm[point_num];
double a_wrist[point_num];
double a_hand[point_num];
/* 时间数组 */
double time_from_start[point_num];
for (int i = 0; i < point_num; i++) {
p_lumbar[i] = goal->trajectory.points[i].positions[0];
p_big_arm[i] = goal->trajectory.points[i].positions[1];
p_small_arm[i] = goal->trajectory.points[i].positions[2];
p_wrist[i] = goal->trajectory.points[i].positions[3];
p_hand[i] = goal->trajectory.points[i].positions[4];
v_lumbar[i] = goal->trajectory.points[i].velocities[0];
v_big_arm[i] = goal->trajectory.points[i].velocities[1];
v_small_arm[i] = goal->trajectory.points[i].velocities[2];
v_wrist[i] = goal->trajectory.points[i].velocities[3];
v_hand[i] = goal->trajectory.points[i].velocities[4];
a_lumbar[i] = goal->trajectory.points[i].accelerations[0];
a_big_arm[i] = goal->trajectory.points[i].accelerations[1];
a_small_arm[i] = goal->trajectory.points[i].accelerations[2];
a_wrist[i] = goal->trajectory.points[i].accelerations[3];
a_hand[i] = goal->trajectory.points[i].accelerations[4];
time_from_start[i] = goal->trajectory.points[i].time_from_start.toSec();
}
FILE *f;
f=fopen("/home/dyyyyc/catkin_ws/src/redwall_arm_control/src/1","w");
for(int j=0;jpublishFeedback(feedback);
ROS_INFO("Recieve action successful, Now We get all joints P,V,A,T!");
as->setSucceeded();
}
/* 主函数主要用于动作订阅和套接字通信 */
int main(int argc, char** argv)
{
ros::init(argc, argv, "redwall_arm_server");
ros::NodeHandle nh;
// 定义一个服务器
Server server(nh, "redwall_arm/follow_joint_trajectory", boost::bind(&execute, _1, &server), false);
// 服务器开始运行
server.start();
ros::spin();
}
- 这个程序可以计算第一类（已知边界一阶导数）和第二类（已知边界二阶导数） 边界条件下的三次样条， 机器人运行过程中，我们要求起始和终止的位置速度必须为0，所以采用了第一类边界条件，并且边界的一阶导数为0。
- 三次样条插补的话只是使用了路点的位置信息，至于moveit规划的每个点的速度和加速度没有使用，相当于没使用moveit规划的速度和加速度。使用的三次样条：输入是时间，输出是位置、速度、加速度
有如下一段代码，因为我觉得move_group规划的路径点有点少，所以将时间数组放大四倍(也就是相邻轨迹点取更短的时间间隔)，最终规划的路点就是原来的四倍，然后将新的规划的所有pvt信息用新的数组保存。后面代码将原本move_group规划的路点和插补之后的路点都写入文件，然后用python读取之后用matplotlib绘制图像数据。
FILE *f;
f=fopen("/home/dyyyyc/catkin_ws/src/redwall_arm_control/src/1","w");
for(int j=0;jcubspintest.py
import matplotlib.pyplot as plt
import linecache
count = len(open("./1", "r+").readlines())
time = linecache.getline("./1", 1)
times1 = []
for n in list(time.split(",")):
if n == "\n":
pass
else:
times1.append(float(n))
time = linecache.getline("./1", 6)
times2 = []
for n in list(time.split(",")):
if n == "\n":
pass
else:
times2.append(float(n))
pos = linecache.getline("./1", 2)
pos1 = []
for n in list(pos.split(",")):
if n == "\n":
pass
else:
pos1.append(float(n))
pos = linecache.getline("./1", 7)
pos2 = []
for n in list(pos.split(",")):
if n == "\n":
pass
else:
pos2.append(float(n))
vel = linecache.getline("./1", 3)
vel1 = []
for n in list(vel.split(",")):
if n == "\n":
pass
else:
vel1.append(float(n))
vel = linecache.getline("./1", 8)
vel2 = []
for n in list(vel.split(",")):
if n == "\n":
pass
else:
vel2.append(float(n))
acc = linecache.getline("./1", 4)
acc1 = []
for n in list(acc.split(",")):
if n == "\n":
pass
else:
acc1.append(float(n))
acc = linecache.getline("./1", 9)
acc2 = []
for n in list(acc.split(",")):
if n == "\n":
pass
else:
acc2.append(float(n))
plt.title("position", fontsize=24)
plt.xlabel("x", fontsize=14)
plt.ylabel("y", fontsize=14)
plt.tick_params(axis='both', which='major', labelsize=14)
plt.plot(times1, pos1, c='blue', linewidth=5)
plt.plot(times2, pos2, c='red', linewidth=5)
plt.show()
# plt.title("velocity", fontsize=24)
# plt.xlabel("x", fontsize=14)
# plt.ylabel("y", fontsize=14)
# plt.tick_params(axis='both', which='major', labelsize=14)
# plt.plot(times1, vel1, c='blue', linewidth=5)
# plt.plot(times2, vel2, c='red', linewidth=5)
# plt.show()
# plt.title("accelocity", fontsize=24)
# plt.xlabel("x", fontsize=14)
# plt.ylabel("y", fontsize=14)
# plt.tick_params(axis='both', which='major', labelsize=14)
# plt.plot(times1, acc1, c='blue', linewidth=5)
# plt.plot(times2, acc2, c='red',linewidth=5)
# plt.show()
