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

166
0
2021年1月27日 09时06分

在之前的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,有些没有显示出来!!!

 

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

 

这个消息中就包含了路径规划后的所有路点信息,也就是描述机械臂应该以什么样的姿态运动到目标位置。关于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

 

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

 

所以采用三次样条插补算法: 三次多项式实际是速度规划里面常说的PVT算法。PVT 模 式 是 指 位 置 — 速 度 — 时 间(Position-Velocity-Time)模式。PVT模式是一种简单又有效的运动控制模式,用户只需要给定离散点的位置、速度和时间,运动控制卡的插补算法将会生成一条连续、平滑的运动路径。

 

三次样条所产生的效果大致:从位置曲线和速度曲线上来看是平滑的,加速度也不存在五次多项式那样的抖动,但是也引入了新的问题就是加速度不连续,对于一些应用的动力学和惯性载荷会产生一些不期待的影响

 

图片来源https://blog.csdn.net/libing403/article/details/78698322

 

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

 

接下来看三次样条的具体实现方法:每个轨迹点都有 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)—三次样条插补插图(3)

 

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

 

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

 

发表评论

后才能评论