Ros中的轨迹规划模块

MOONICK

已于 2023-09-18 09:18:24 修改

阅读量3.3k

点赞数 7

分类专栏： ROS学习文章标签：人工智能

于 2023-01-30 16:49:43 首次发布

本文链接：https://blog.csdn.net/yuanshenqiang/article/details/128716684

版权

ROS学习专栏收录该内容

3 篇文章 0 订阅

订阅专栏

一、Moveit!运动规划模块

Ros中的运动规划是由Moveit!支撑的，Moveit内部分为路径规划模块和轨迹生成模块，路径规划默认使用的是open motion plan library(OMPL)方法，轨迹生成部分实现了Ruckig时间最优轨迹生成算法。(想了解Ruckig算法原理的朋友，也可以去看看原作者提供的算法实现，开源项目仓库地址为https://github.com/pantor/ruckig，对应的论文为https://arxiv.org/abs/2105.04830。)其中OMPL主要解决路径搜索和避障，相关资料较多，此处不再讨论。下面主要讨论Ruckig的轨迹生成方法。

Ruckig解决了非零急动度(Jerk)约束下有限加速度、速度的时间最优在线轨迹生成问题。它将起点到终点之间的运动划分为7个阶段，讨论了7个阶段所有可能的配置组合方案，从合理的组合中选出了时间最优方案，从而给定了起点到终点之间任意时刻的运动学状态 $\text{[math]}$ 的数值计算方法,在最优时间内，各轴恰好到达终点。

Ruckig算法鲁棒性分析：

随机生成1000000000组起始点和终止点运动学状态[( $\text{[math]}$ ; $\text{[math]}$ ),(),(),...]，其中：

$\text{[math]}$

$\text{[math]}$ (注：Normal为正太分布，Gamma为伽马分布)

$\text{[math]}$

去掉终点加速度不合理的情况，实现了百分百的轨迹生成。具体来说，终点加速度由于受到急动度约束，应该满足下列不等式：

$\text{[math]}$

当生成的1000000000组运动学状态中有不满足上述不等式的情况，则直接丢弃。

需要说明的是，Ruckig对动作持续时间较为敏感，如果从起点运行到终点，需要很长的时间，则由于积分误差，可能导致Ruckig计算得到轨迹终点和设定的轨迹终点之间的误差会超过阈值，此时认为轨迹生成失败。根据论文设定的阈值推算，动作持续时间不能超过16分钟。

此外，Ruckig对各个自由度表示的物理含义是无感的，意味着该轨迹生成方法即适用于轴关节空间，也适用于笛卡尔工具空间。当使用笛卡尔工具空间时，六个自由度通常是 $\text{[math]}$ (位置+姿态，其中姿态用一组欧拉角表示)。如果期望得到笛卡尔空间的直线轨迹，则要求起始点和终止点的位置、位置速度和位置加速度都满足相应的轨迹方程。

假如空间直线轨迹由下面的两个平面方程确定：

$\text{[math]}$

则速度需要满足：

$\text{[math]}$

加速度需要满足：

$\text{[math]}$

至于姿态的插值，如果直接对RPY角或欧拉角进行插值，可能存在万向节死锁的问题，所以通常会用四元数表示姿态，然后在四元数上进行插值，一般使用正则化线性插值或球面线性插值。

关于圆弧轨迹，由于Ruckig是以时间最优进行优化的，所以无法在两点之间插出指定的圆弧轨迹。鉴于此，提供一个通用的空间圆弧插值方法(理论部分参考https://blog.csdn.net/Kalenee/article/details/86698110)。给定任意三点，如果不共线则得到圆弧轨迹，否则将得到直线轨迹：


#include <iostream>
#include <vector>
#include <Eigen>
#include <math.h>
#include <fstream>
#define Pi 3.14159265358979323846

struct CircleInterploateCoffecients
{
    double nx, ny, nz;
    double ox, oy, oz;
    double theta;
};

bool CalculateCircleCenterRadius(float x_0, float y_0, float z_0, float x_1, float y_1, float z_1, float x_2, float y_2, float z_2, float& c_x, float& c_y, float& c_z, float& r)
{
    Eigen::Vector3d x0, x1, x2;
    x0 << x_0, y_0, z_0;
    x1 << x_1, y_1, z_1;
    x2 << x_2, y_2, z_2;
    Eigen::Vector3d v1 = x1 - x0;
    Eigen::Vector3d v2 = x2 - x0;
    if (v1.norm() < 1e-5 || v2.norm() < 1e-5)
        return false;//三个点不能重复
    Eigen::Vector3d v1norm = v1 / v1.norm();
    Eigen::Vector3d v2norm = v2 / v2.norm();
    Eigen::Vector3d co = v1norm.cross(v2norm);
    if ((co.x() > -1e-5 && co.x() < 1e-5) &&
        (co.y() > -1e-5 && co.y() < 1e-5) &&
        (co.z() > -1e-5 && co.z() < 1e-5)/*三点共线*/ || 
        (abs(co.x()) + abs(co.y()) + abs(co.z()) < 1e-5/*三点趋近于直线*/))
    {
        v2.normalize();
        c_x = v2.x();
        c_y = v2.y();
        c_z = v2.z();
        r = -1.0;
        return true;
    }
    Eigen::Vector3d u = v1norm;
    Eigen::Vector3d w = v2.cross(v1);
    w.normalize();
    Eigen::Vector3d v = w.cross(u);
    double bx = v1.dot(u);
    double cx = v2.dot(u);
    double cy = v2.dot(v);
    double h = (pow((cx - 0.5*bx), 2) + pow(cy, 2) - pow(0.5*bx, 2)) / (2 * cy);
    Eigen::Vector3d center = x0 + 0.5*bx * u + h * v;
    c_x = center.x();
    c_y = center.y();
    c_z = center.z();
    r = sqrt(pow(c_x - x0.x(), 2) + pow(c_y - x0.y(), 2) + pow(c_z - x0.z(), 2));
    return true;
}

CircleInterploateCoffecients GetCircleInterploateParameters(float x_0, float y_0, float z_0, float x_1, float y_1, float z_1, float x_2, float y_2, float z_2, float c_x,float c_y,float c_z,float radius)
{
    Eigen::Vector3d p0, p1, p2, pc;
    p0 << x_0, y_0, z_0;
    p1 << x_1, y_1, z_1;
    p2 << x_2, y_2, z_2;
    pc << c_x, c_y, c_z;
    double A = (y_1 - y_0) * (z_2 - z_1) - (z_1 - z_0) * (y_2 - y_1);
    double B = (z_1 - z_0) * (x_2 - x_1) - (x_1 - x_0) * (z_2 - z_1);
    double C = (x_1 - x_0) * (y_2 - y_1) - (y_1 - y_0) * (x_2 - x_1);
    double K = sqrt(pow(A,2)+pow(B,2)+pow(C,2));
    Eigen::Vector3d co = p0 - pc;
    double nx, ny, nz;
    double ox, oy, oz;
    double ax, ay, az;
    nx = (x_0 - c_x) / co.norm(), ny = (y_0 - c_y) / co.norm(), nz = (z_0 - c_z) / co.norm();
    ax = A / K, ay = B / K, az = C / K;
    ox = ay * nz - az * ny, oy = az * nx - ax * nz, oz = ax * ny - ay * nx;
    Eigen::Matrix4d T;
    T << nx, ox, ax, c_x, ny, oy, ay, c_y, nz, oz, az, c_z, 0.0, 0.0, 0.0, 1.0;
    Eigen::Matrix<double,4,3> Q;
    Q << x_0, x_1, x_2, y_0, y_1, y_2, z_0, z_1, z_2, 1.0, 1.0, 1.0;
    Eigen::Matrix4d invT = T.inverse();
    Eigen::MatrixXd q1 = invT * Q.col(0);
    Eigen::MatrixXd q2 = invT * Q.col(1);
    Eigen::MatrixXd q3 = invT * Q.col(2);
    double thetaRange13 = 0.0, thetaRange12 = 0.0;
    if (q3(1, 0) < 0)
        thetaRange13 = atan2(q3(1, 0), q3(0, 0)) + 2 * Pi;
    else
        thetaRange13 = atan2(q3(1, 0), q3(0, 0));
    if (q2(1, 0) < 0)
        thetaRange12 = atan2(q2(1, 0), q2(0, 0)) + 2 * Pi;
    else
        thetaRange12 = atan2(q2(1, 0), q2(0, 0));
    CircleInterploateCoffecients coffe;
    coffe.theta = thetaRange13;
    coffe.nx = nx, coffe.ny = ny,  coffe.nz = nz;
    coffe.ox = ox, coffe.oy = oy, coffe.oz = oz;
    return coffe;
}

void GetStrightLinePointCase1(float x_start, float y_start,float z_start, float delta,float x0,float y0,float z0, float& x_end,float& y_end,float& z_end)
{
    x_end = x_start;
    y_end = y_start;
    z_end = z_start + delta;
}

void GetStrightLinePointCase2(float x_start, float y_start, float z_start, float delta, float x0, float y0, float z0, float& x_end, float& y_end, float& z_end)
{
    x_end = x_start;
    y_end = y_start + (y0/z0)* delta;
    z_end = z_start+delta;
}

void GetStrightLinePointCase3(float x_start, float y_start, float z_start, float delta, float x0, float y0, float z0, float& x_end, float& y_end, float& z_end)
{
    x_end = x_start;
    y_end = y_start+delta;
    z_end = z_start;
}

void GetStrightLinePointCase4(float x_start, float y_start, float z_start, float delta, float x0, float y0, float z0, float& x_end, float& y_end, float& z_end)
{
    x_end = x_start+delta;
    y_end = y_start;
    z_end = z_start;
}

void GetStrightLinePointCase5(float x_start, float y_start, float z_start, float delta, float x0, float y0, float z0, float& x_end, float& y_end, float& z_end)
{
    x_end = x_start + (x0/z0)*delta;
    y_end = y_start;
    z_end = z_start+delta;
}

void GetStrightLinePointCase6(float x_start, float y_start, float z_start, float delta, float x0, float y0, float z0, float& x_end, float& y_end, float& z_end)
{
    x_end = x_start + delta;
    y_end = y_start + (y0/x0)*delta;
    z_end = z_start + (z0/x0)*delta;
}

void GetStrightLinePointCase7(float x_start, float y_start, float z_start, float delta, float x0, float y0, float z0, float& x_end, float& y_end, float& z_end)
{
    x_end = x_start + (x0/y0)*delta;
    y_end = y_start + delta;
    z_end = z_start;
}

typedef void(*InterploateLineFunc)(float,float,float,float, float, float, float, float&,float&,float&);
InterploateLineFunc GetLineInterploateFunc(float x0,float y0,float z0)
{
    // [x0,y0,z0] 为直线的方向向量
    if ((x0<1e-5 && x0>-1e-5) && (y0<1e-5 && y0>-1e-5) && (z0 > 1e-5 || z0 < -1e-5))//m==0,n==0,p!=0
    {
        // x=p1[0], y=p1[1], z = p1[2] + delta
        return GetStrightLinePointCase1;
    }
    else if ((x0<1e-5 && x0>-1e-5) && (y0 > 1e-5 || y0 < -1e-5) && (z0 > 1e-5 || z0 < -1e-5))//m==0,n!=0,p!=0
    {
        // x = p1[0], y=p1[1]+(y0/z0)*(z-p1[2]), z = p1[2] + delta
        return GetStrightLinePointCase2;
    }
    else if ((x0<1e-5 && x0>-1e-5) && (y0 > 1e-5 || y0 < -1e-5) && (z0 < 1e-5 && z0 > -1e-5))//m==0,n!=0,p==0
    {
        // x = p1[0], y = p1[1] + delta, z = p1[2]
        return GetStrightLinePointCase3;
    }
    else if ((x0 > 1e-5 || x0 < -1e-5) && (y0 < 1e-5 && y0 > -1e-5) && (z0 < 1e-5 && z0 > -1e-5))//m!=0,n==0,p==0
    {
        //x = p1[0] + delta, y = p1[1], z = p1[2]
        return GetStrightLinePointCase4;
    }
    else if ((x0 > 1e-5 || x0 < -1e-5) && (y0 < 1e-5 && y0 > -1e-5) && (z0 > 1e-5 || z0 < -1e-5))//m!=0,n==0,p!=0
    {
        // x = p1[0]+(x0/z0)*(z-p1[2]), y = p1[1], z = p1[2] + delta
        return GetStrightLinePointCase5;
    }
    else if ((x0 > 1e-5 || x0 < -1e-5) && (y0 > 1e-5 || y0 < -1e-5) && (z0 > 1e-5 || z0 < -1e-5))//m!=0,n!=0,p!=0
    {
        //x = p1[0]+delta, y = p1[1] + (y0/x0)*(x-p1[0]), z = p1[2] + (z0/x0)(x-p1[0])
        return GetStrightLinePointCase6;
    }
    else if ((x0 > 1e-5 || x0 < -1e-5) && (y0 > 1e-5 || y0 < -1e-5) && (z0 < 1e-5 && z0 > -1e-5))//m!=0,n!=0,p==0
    {
        // x = p1[0]+(x0/y0)(y-p1[1]), y = p1[1] + delta, z = p1[2]
        return GetStrightLinePointCase7;
    }
    else
        return nullptr;
}

int main()
{ 
    float x0=-0.1, y0=-0.1, z0=-0.1,radius;
    //float p1[] = { 0.3790,0.4368,-0.81581 };
    //float p2[] = { -0.3990,0.8164,-0.41741 };
    //float p3[] = { -0.6984,0.7155,-0.01701 };

    float p1[] = { 0.0,0.0,0.0 };
    float p2[] = { 1.0,1.0,1.0 };
    float p3[] = { 2.0,2.0,2.0 };

    bool res = CalculateCircleCenterRadius(p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],p3[0],p3[1],p3[2], x0, y0, z0, radius);
    if (!res)
        return -1;
    std::ofstream in;
    in.open("zz-com.txt", std::ios::trunc); //ios::trunc表示在打开文件前将文件清空,由于是写入,文件不存在则创建
    std::vector<std::vector<float>> pathPoints;
    double interploateRange = 0.0;
    if (radius > 0)
    {
        CircleInterploateCoffecients param = GetCircleInterploateParameters(p1[0], p1[1], p1[2], p2[0], p2[1], p2[2], p3[0], p3[1], p3[2], x0, y0, z0, radius);
        
        interploateRange = param.theta;
        double step = interploateRange / 200;
        int cnt = 0;
        double accumalate = 0.0;
        for (; abs(accumlate-interploateRange)>1e-5; )
        {
            double coeff1, coeff2;
            coeff1 = cos(accumalate);
            coeff2 = sin(accumalate);
            accumalate += step;
            float xt, yt, zt;
            xt = param.nx * radius * coeff1 + param.ox * radius * coeff2 + x0;
            yt = param.ny * radius * coeff1 + param.oy * radius * coeff2 + y0;
            zt = param.nz * radius * coeff1 + param.oz * radius * coeff2 + z0;
            pathPoints.push_back({ xt,yt,zt });
            in << cnt++ << "," << xt << "," << yt << "," << zt << "\n";
        }
    }
    else
    {
        InterploateLineFunc method = GetLineInterploateFunc(x0, y0, z0);
        if (method != nullptr)
        {
            float xt, yt, zt;
            if (method == GetStrightLinePointCase1)
                interploateRange = p3[2] - p1[2];
            else if (method == GetStrightLinePointCase2)
                interploateRange = p3[2] - p1[2];
            else if (method == GetStrightLinePointCase3)
                interploateRange = p3[1] - p1[1];
            else if (method == GetStrightLinePointCase4)
                interploateRange = p3[0] - p1[0];
            else  if (method == GetStrightLinePointCase5)
                interploateRange = p3[2] - p1[2];
            else if(method == GetStrightLinePointCase6)
                interploateRange = p3[0] - p1[0];
            else if(method == GetStrightLinePointCase7)
                interploateRange = p3[1] - p1[1];
            float accumlate = 0.0;
            float step = interploateRange /200;
            int cnt = 0;
            for (; abs(accumlate-interploateRange)>1e-5;)
            {
                method(p1[0], p1[1], p1[2], accumlate, x0,y0, z0, xt, yt, zt);
                accumlate += step;
                in << cnt++ << "," << xt << "," << yt << "," << zt << "\n";
                pathPoints.push_back({ xt,yt,zt });
            }
        }
    }
    in.close();
    return 0;
}

需要注意的是，该插值完全没有考虑速度条件，默认轨迹是匀速的，轨迹点数量默认是200，可以根据需要进行调整。