2022-4-10 轨迹规划 梯形速度规划 calctrapezoid

台大机器人学

maxv = 0.2, 0.4, 2
maxa = 0.5

maxv = 0.2
maxa = 0.1, 0.5, 2

maxv = 0.1, 0.2, 0.4
maxa = 0.1

自动调节整段运动的整体时间
加速度最大最小突变
速度连续，梯形变化

    void calctrapezoid(int len, double cp, double ct, double cv,double tp, double tt, double tv, double maxv, double maxa,vector<double> &op, vector<double> &ov, vector<double> &oa);

len 轨迹计算精度（分段数）
cp 初始位置
tp 目标位置
cv 初始速度
tv 目标速度
ct 初始时间
tt 预计 目标时间
maxv 最大速度（绝对值）
maxa 最大加速度（绝对值）
op 输出位置序列
ov 输出速度序列
oa 输出加速度序列

运动分段规划：

	// 位置变化量
    double deltap = tp-cp; 
    // 预计运动时间
    double deltat = tt-ct;
    if(cp>tp)
    {
    	// 根据运动方向确定速度正负和初始加速度正负
        cout << "move back , v and a < 0" << endl;
        maxv=-maxv;
        maxa=-maxa;
        cout << "maxv: " << maxv<< " . maxa : " << maxa<<endl;
    }
    cout << "cv : " << cv<< " . tv : " << tv << endl; 
    // 加速时间
    double ta1 = (maxv-cv)/maxa;
    // 减速时间
    double ta2 = (maxv-tv)/maxa;
	//加减速 两部分总路程
    double ap = (maxv+cv)*ta1/2 + (maxv+tv)*ta2/2;
    // 加速或者减速一次路程
    double ctvp = (cv+tv)*(tv-cv)/(maxa*2);

    double a=0;
    double v=cv;
    double p=cp;
	// 轨迹计算时间步长
    double dt = deltat/len;

	// 判断需要分段数量
    if(abs(deltap)<=abs(ctvp)) //一段
	else if(abs(deltap)<=abs(ap)) // 两段
	else if(abs(deltap)>abs(ap)) // 三段

完整三段轨迹：

	//匀速运动需要的时间
        double mt = (deltap-ap)/maxv;
        //加速转匀速位置
        double p1 = cp+ap/2;
        //匀速转减速位置
        double p2 = tp-ap/2;
        //时间系数
        double k = (ta1+ta2+mt)/deltat;
//        len = (k*(double)len);
		//时间精度放缩，保证总段数不变
        dt = dt*k;
        cout << "time: ta1:"<< ta1<<". mt :"<< mt<<". ta2 :"<< ta2 << ". dt :"<< dt <<endl;
        cout << "cp:"<< cp<< " . p1:"<<p1<<" . p2:"<< p2<< "tp:"<< tp<<endl;
        for(int i=0;i<len;++i)
        {
        	// 控制各个时刻加速度
        	// 增量式计算速度位置
            double t = dt*i;
            if(t<ta1)
            {
                a=maxa;
            }
            if(t>=ta1 && t<=(ta1+mt))
            {
                a=0;
            }
            if(t>(ta1+mt))
            {
                a=-maxa;
            }
            v=v+a*dt;
            p=p+v*dt;
            op.push_back(p);
            ov.push_back(v);
            oa.push_back(a);
        }

完整函数：

void MainWindow::calctrapezoid(int len, double cp, double ct, double cv,
                                            double tp, double tt, double tv, double maxv, double maxa,
                                            vector<double> &op, vector<double> &ov, vector<double> &oa)
{
    double deltap = tp-cp;
    double deltat = tt-ct;
    if(cp>tp)
    {
        cout << "move back , v and a < 0" << endl;
        maxv=-maxv;
        maxa=-maxa;
        cout << "maxv: " << maxv<< " . maxa : " << maxa<<endl;
    }
    cout << "cv : " << cv<< " . tv : " << tv << endl;
    double ta1 = (maxv-cv)/maxa;
    double ta2 = (maxv-tv)/maxa;

    double ap = (maxv+cv)*ta1/2 + (maxv+tv)*ta2/2;
    double ctvp = (cv+tv)*(tv-cv)/(maxa*2);

    double a=0;
    double v=cv;
    double p=cp;

    double dt = deltat/len;

    if(abs(deltap)<=abs(ctvp))
    {
        double k = ((tv-cv)/maxa)/deltat;
//        len = (int)(k*(double)len);
        dt = dt*k;
        for(int i=0;i<len;++i)
        {

            a = maxa;
            v = v + a*dt;
            p = p + v*dt;

            op.push_back(p);
            ov.push_back(v);
            oa.push_back(a);
        }
    }
    else if(abs(deltap)<=abs(ap))
    {
        double vm = sqrt((deltap*maxa)+(cv*cv+tv*tv)/2);
        if(deltap<0)vm=-vm;

        double t1 = (vm-cv)/maxa;
        double t2 = (vm-tv)/maxa;
        double k = (t1+t2)/deltat;
//        len = (int)(k*(double)len);
        dt = dt*k;

        double pm = cp+(cv+vm)*t1/2;

        for(int i=0;i<len;++i)
        {
            double t = dt*i;
            if(t<=t1)
            {
                a=maxa;
            }
            else
            {
                a=-maxa;
            }
            v=v+a*dt;
            p=p+v*dt;
            op.push_back(p);
            ov.push_back(v);
            oa.push_back(a);
        }

    }
    else if(abs(deltap)>abs(ap))
    {
        double mt = (deltap-ap)/maxv;
        double p1 = cp+ap/2;
        double p2 = tp-ap/2;
        double k = (ta1+ta2+mt)/deltat;
//        len = (k*(double)len);
        dt = dt*k;
        cout << "time: ta1:"<< ta1<<". mt :"<< mt<<". ta2 :"<< ta2 << ". dt :"<< dt <<endl;
        cout << "cp:"<< cp<< " . p1:"<<p1<<" . p2:"<< p2<< "tp:"<< tp<<endl;
        for(int i=0;i<len;++i)
        {
            double t = dt*i;
            if(t<ta1)
            {
                a=maxa;
            }
            if(t>=ta1 && t<=(ta1+mt))
            {
                a=0;
            }
            if(t>(ta1+mt))
            {
                a=-maxa;
            }
            v=v+a*dt;
            p=p+v*dt;
            op.push_back(p);
            ov.push_back(v);
            oa.push_back(a);
        }


    }


}