通过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 <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;                                //  中间一串数为2for(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;}elsereturn 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;elseklo=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;//testacc = (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 endreturn 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 testspline.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;}
/*// lumbarspline.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_armspline.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_armspline.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;}// wristspline.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;}// handspline.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();}

这个程序可以计算第一类（已知边界一阶导数）和第二类（已知边界二阶导数）边界条件下的三次样条，机器人运行过程中，我们要求起始和终止的位置速度必须为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;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 testspline.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);

cubspintest.py

import matplotlib.pyplot as plt
import linecachecount = len(open("./1", "r+").readlines())
time = linecache.getline("./1", 1)
times1 = []
for n in list(time.split(",")):if n == "\n":passelse:times1.append(float(n))
time = linecache.getline("./1", 6)
times2 = []
for n in list(time.split(",")):if n == "\n":passelse:times2.append(float(n))pos = linecache.getline("./1", 2)
pos1 = []
for n in list(pos.split(",")):if n == "\n":passelse:pos1.append(float(n))
pos = linecache.getline("./1", 7)
pos2 = []
for n in list(pos.split(",")):if n == "\n":passelse:pos2.append(float(n))vel = linecache.getline("./1", 3)
vel1 = []
for n in list(vel.split(",")):if n == "\n":passelse:vel1.append(float(n))
vel = linecache.getline("./1", 8)
vel2 = []
for n in list(vel.split(",")):if n == "\n":passelse:vel2.append(float(n))acc = linecache.getline("./1", 4)
acc1 = []
for n in list(acc.split(",")):if n == "\n":passelse:acc1.append(float(n))
acc = linecache.getline("./1", 9)
acc2 = []
for n in list(acc.split(",")):if n == "\n":passelse: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()

通过ROS控制真实机械臂(7)---三次样条插补相关推荐

通过ROS控制真实机械臂(9)---上、下位机和PRU程序
上位机的程序redwall_arm_server.cpp 功能是作为ROS的move_group客户端接收ROS规划的机械臂路点信息,进行三次样条插补获得各个关节或自由度的运动PVAT数据,然后通过T ...
通过ROS控制真实机械臂(8)---延时时间精确控制
根据之前的配置,我们已经可以通过move_group发送出机械臂各关节运动的轨迹,并且通过三次样条插补的方法,赋予各个关节在特定角度时的速度和加速度,通过启动程序节点可以看到,本次运动规划使用了LBK ...
通过ROS控制真实机械臂(15) --- 视觉抓取之手眼标定
通过视觉传感器赋予机械臂"眼睛"的功能,配合ATI力和力矩传感器,就可以完成机械臂"手眼"结合的能力,完成视觉抓取过程.目前测试的视觉传感器为 ZED mini ...
通过ROS控制真实机械臂(2)----单轴运动，手柄控制
创建ROS包,包名redwall_arm ,通过自定义的消息,将手柄的数据发布 msg/ joycontrol.msg,内容如下,分别对应罗技手柄的按钮和遥杆轴. int32 button1 int3 ...
ros melodic控制真实机械臂之获取moveit规划插补点
关于该点可查看前辈博客.本文对其中不一致的地方进行记录,但为了查阅方便,该文也记录了完整的操作步骤. 1.demo.launch文件中参数fake_execution的值改为false <arg ...
ros kinetic-moveit驱动ur3机械臂------控制真实机械臂并且能动
ros kinetic-moveit驱动ur3机械臂------控制真实机械臂并且能动本文工作环境配置: ubuntu16.04.6 ros-kinetic ur3 已验证本教程代码在Ubuntu1 ...
ros通过moveit控制真实机械臂
1.demo.launch文件在生成的demo.launch文件中,参数fake_execution的值改为false <include file="$(find tk7arm_mo ...
机器人学习必看系列：如何使用moveit控制真实机械臂？
大家好,我是你们可爱的小鱼.最近关于moveit相关的问题感觉非常多,毕竟机械臂+视觉的应用的确是非常的火爆,小鱼都想直接开课教机械臂运动规划相关的了. 有的同学问小鱼,怎么使用moveit控制真实机 ...
使用ROS控制AUBO机械臂
环境配置: Ubuntu16.04 ROS-kinetic 前提: Ubuntu16.04和 ROS kinetic都需要提前安装好 1.安装依赖 1. sudo apt-get install ro ...

通过ROS控制真实机械臂(7)---三次样条插补

通过ROS控制真实机械臂(7)---三次样条插补相关推荐

最新文章

热门文章