在之前的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 <ros/ros.h>
#include <iostream>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <std_msgs/Float32MultiArray.h>
/* 三次样条插补 */
#include <string.h>
#include <stddef.h>
#include <stdio.h>
#include "cubicSpline.h"
using namespace std;
/* action 服务端声明 */
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> 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<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;
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;j<point_num;j++)
{
fprintf(f,"%f,",time_from_start[j]);//1
}
fprintf(f,"\n");
for(int j=0;j<point_num;j++)
{
fprintf(f,"%f,",p_lumbar[j]);//2
}
fprintf(f,"\n");
for(int j=0;j<point_num;j++)
{
fprintf(f,"%f,",v_lumbar[j]);//3
}
fprintf(f,"\n");
for(int j=0;j<point_num;j++)
{
fprintf(f,"%f,",a_lumbar[j]);//4
}
fprintf(f,"\n");
fclose(f);
// 实例化样条
cubicSpline spline;
// lumbar test
spline.loadData(time_from_start, p_lumbar, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
double rate = (time_from_start[point_num-1] - time_from_start[0])/(point_num*4);
double p_lumbar_[point_num*4];
double v_lumbar_[point_num*4];
double a_lumbar_[point_num*4];
double time_from_start_[point_num*4];
for (int k = 0; k <= point_num*4 ; k++) {
spline.getYbyX(x_out, y_out);
time_from_start_[k] = x_out;
p_lumbar_[k] = y_out;
v_lumbar_[k] = vel;
a_lumbar_[k] = acc;
x_out += rate;
}
/*
// lumbar
spline.loadData(time_from_start, p_lumbar, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
for (int k = 0; k < point_num ; k++) {
x_out = time_from_start[k];
spline.getYbyX(x_out, y_out);
v_lumbar[k] = vel;
a_lumbar[k] = acc;
}
// big_arm
spline.loadData(time_from_start, p_big_arm, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
for (int k = 0; k <point_num ; k++) {
x_out = time_from_start[k];
spline.getYbyX(x_out, y_out);
v_big_arm[k] = vel;
a_big_arm[k] = acc;
}
// small_arm
spline.loadData(time_from_start, p_small_arm, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
for (int k = 0; k <point_num ; k++) {
x_out = time_from_start[k];
spline.getYbyX(x_out, y_out);
v_small_arm[k] = vel;
a_small_arm[k] = acc;
}
// wrist
spline.loadData(time_from_start, p_wrist, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
for (int k = 0; k <point_num ; k++) {
x_out = time_from_start[k];
spline.getYbyX(x_out, y_out);
v_wrist[k] = vel;
a_wrist[k] = acc;
}
// hand
spline.loadData(time_from_start, p_hand, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
for (int k = 0; k <point_num ; k++) {
x_out = time_from_start[k];
spline.getYbyX(x_out, y_out);
v_hand[k] = vel;
a_hand[k] = acc;
}
*/
f=fopen("/home/dyyyyc/catkin_ws/src/redwall_arm_control/src/1","a");
fprintf(f,"\n");
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,",p_lumbar_[j]);//7
}
fprintf(f,"\n");
for(int j=0;j<=point_num*4;j++)
{
fprintf(f,"%f,",v_lumbar_[j]);//8
}
fprintf(f,"\n");
for(int j=0;j<=point_num*4;j++)
{
fprintf(f,"%f,",a_lumbar_[j]);//9
}
fprintf(f,"\n");
fclose(f);
//control_msgs::FollowJointTrajectoryFeedback feedback;
//feedback = NULL;
//as->publishFeedback(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();
}
1.这个程序可以计算第一类（已知边界一阶导数）和第二类（已知边界二阶导数） 边界条件下的三次样条， 机器人运行过程中，我们要求起始和终止的位置速度必须为0，所以采用了第一类边界条件，并且边界的一阶导数为0。 2.三次样条插补的话只是使用了路点的位置信息，至于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;j<point_num;j++)
{
fprintf(f,"%f,",time_from_start[j]);//1
}
fprintf(f,"\n");
for(int j=0;j<point_num;j++)
{
fprintf(f,"%f,",p_lumbar[j]);//2
}
fprintf(f,"\n");
for(int j=0;j<point_num;j++)
{
fprintf(f,"%f,",v_lumbar[j]);//3
}
fprintf(f,"\n");
for(int j=0;j<point_num;j++)
{
fprintf(f,"%f,",a_lumbar[j]);//4
}
fprintf(f,"\n");
fclose(f);
// 实例化样条
cubicSpline spline;
// lumbar test
spline.loadData(time_from_start, p_lumbar, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
double rate = (time_from_start[point_num-1] - time_from_start[0])/(point_num*4);
double p_lumbar_[point_num*4];
double v_lumbar_[point_num*4];
double a_lumbar_[point_num*4];
double time_from_start_[point_num*4];
for (int k = 0; k <= point_num*4 ; k++) {
spline.getYbyX(x_out, y_out);
time_from_start_[k] = x_out;
p_lumbar_[k] = y_out;
v_lumbar_[k] = vel;
a_lumbar_[k] = acc;
x_out += rate;
}
f=fopen("/home/dyyyyc/catkin_ws/src/redwall_arm_control/src/1","a");
fprintf(f,"\n");
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,",p_lumbar_[j]);//7
}
fprintf(f,"\n");
for(int j=0;j<=point_num*4;j++)
{
fprintf(f,"%f,",v_lumbar_[j]);//8
}
fprintf(f,"\n");
for(int j=0;j<=point_num*4;j++)
{
fprintf(f,"%f,",a_lumbar_[j]);//9
}
fprintf(f,"\n");
fclose(f);
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明通过ROS控制真实机械臂(7)—三次样条插补！
