精选轨迹时间同步--时间缩放法

test

发布时间 2023.06.12阅读数 2232 评论数 0

轨迹时间同步—时间缩放法

摘要

一般而言，多轴机器人的任务一般是通过控制末端工具的位置和姿态来完成，会涉及位置和姿态两个量，所以在轨迹规划时需要考虑二者的时间同步，即在同一规划时间内机器人末端要满足轨迹的位置和姿态的联动，这就对控制算法提出了更高的要求。由于驱动系统饱和限制或任务要求，对机器人的运动速度和加速度有一定的约束。为了分别满足位置和姿态的速度、加速度幅值约束，往往需要分别对位置和姿态进行时间同步的轨迹规划，保证机器人的位置和姿态同时到达期望值。除了机器人位置和姿态用到时间同步外，多机器人协作，轨迹同步（如电子凸轮，主从运动等）也需要进行时间同步。

时间同步方法

机器人轨迹时间同步方法目前主要有如下四种。

（1）直接规划法。该方法可以在约束条件下，保持给定的加速度，尽可能以最高速度运行，匀速时间较长，可以达到最高的效率。但是对于不同的加减速度算法，时间同步规划的方法不能共用，例如梯形加减速直接同步规划并不复杂，计算效率较高。但是S型等复杂加减速，要实现指定运行时间的同步规划则相当复杂。梯形速度时间同步基本步骤是：梯形时间最优速度规划—>获得最大运行时间做同步时间—>以同步时间重新规划时间小的曲线。基于梯形加减速的时间同步具体实现可以参考这两篇博客《梯形加减速算法》，《梯形时间同步—直接规划法》。直接规划法可以尽可能保证起步速度与末速度的不变，更适用于连续轨迹，速度前瞻控制等场合。这种方法的缺点是如果位置和姿态的规划位移量相差过大，会导致规划时间无法在规定的运动参数内拉长至相等，无法完成时间同步，这时需要对末速度和起步速度进行修改。如下图，时间最优的轨迹为红色曲线，通过时间同步，将其运动时间同步到更大的指定时间，即蓝色曲线。

（2）时间缩放法。该方法会降低加速度，延迟加速度和减速时间，匀速运行时间较短。该方法不管何种减速，对运动时间进行缩放，都可以使用统一的方式实现时间同步，时间缩放法可以用于机器人位姿时间同步，也可以用于对原始轨迹进行修改，例如，在轨迹规划阶段未考虑系统的于东学饱和及动力学饱和限制，可能导致速度，加速度或力超过允许的范围，那么可以使用时间缩放法增加轨迹的时间。时间缩放法的缺点是只适合单段路径的规划，在连续路径中，衔接速度不为零且相邻段的时间缩放因子不相同时，速度会发生跳变。

（3）广义速度法。广义速度法就是把所有需要规划的位移量合成一个位移量进行规划。例如文献[3]，对于位置和姿态的规划，将移动轴x,y,z和姿态轴\alpha,\beta, \gamma合成一个位移量进行加减速规划，将求得的广义速度进行分解。这种方法虽然可以实现了位姿同步，但是同步方法没有明确的物理意义，且同步容易造成某些轴的加速度和加加速度超限。

（4）双样条法。目前有关双 NURBS 的研究文献中，大多采用相同的节点矢量进行路径拟合，而采用相同的曲线参数和完成双 NURBS 插补，但是这方法在同步过程中没有同时考虑位置和姿态的速度和加速度约束。双 NURBS曲线在五轴数控中有较多的研究及应用[4,5,6,7,8]，文献[8]采用如下图的流程实现位置和刀轴姿态的同步插补，实现起来异常复杂。在这个方向上我阅读的文献很少，欢迎大家补充。

双样条法

时间缩放法原理[9]

时间缩放法的初衷是用于修改轨迹，避免轨迹违反系统饱和约束限制的，我们先阐述其原理，然后将其用于实现速度曲线时间同步。

在一些应用场景中需要考虑驱动系统的饱和限制约束，为了保证规划的期望轨迹不违反此类饱和限制，必须对原始的轨迹进行修改。由于那些运动速度、加速度和力矩超过允许范围的运动轨迹在实际中无法执行，因此此类运动轨迹在实际工程中应予以避免。实际中可以将饱和区分为如下两类。
(1)运动学饱和：规划后轨迹的速度或加速度超过了驱动系统所能实现的临界速度或临界加速度。

(2)动力学饱和：此列饱和发生于驱动系统所需的驱动力矩（超过驱动系统所能提供的临界力矩）不可行的情形。特别地，由于多机械系统动力学的非线性耦合性，动力学饱和现象往往出现于此类多轴机械系统（如工业机器人）中。

如果在轨迹规划阶段并未提前考虑上述运动学饱和及动力学饱和限制，那么有必要在系统跟踪运动轨迹之前验证运动轨迹的可行性，并采取必要措施（如增加轨迹的时间长度）来防止违反上述饱和限制。

首先，给定一条轨迹：

q=q(t)

接下来引入一个新的与时间t有关的时间变量t^\prime 可使得上面轨迹变快或变慢，或者更一般地，可以修改轨迹的速度和加速度等，其中 t和 t^\prime之间具有如下严格的函数关系：

t=\sigma(t^\prime)

因此有

\tilde q(t^\prime)=(q\circ\sigma)(t^\prime)=q(\sigma(t^\prime))

其速度和加速度可表示为

\dot {\tilde q}(t^\prime)=\frac{dq(\sigma)}{d\sigma}\frac{d(\sigma (t^\prime))}{dt^\prime}

\ddot {\tilde q}(t^\prime)=\frac{dq(\sigma)}{d\sigma}\frac{d^2\sigma(t^\prime)}{dt^{\prime2}}+\frac{d^2q(\sigma)}{d\sigma^2}\left (\frac{d\sigma(t^\prime)}{dt^\prime}\right)^2

因此，通过合理定义函数\sigma, 我们就可以改变\tilde q(t^\prime)关于时间的导数。本质上，轨迹q与函数 \sigma具有无穷多种的组合形式。特别地，如下线性形式是\sigma的一种常用特例：

t=\sigma (t)=\lambda t^\prime \Rightarrow t^\prime=\frac{t}{\lambda}

为此，可以得到如下关系

\dot {\tilde q}(t^\prime)=\frac{dq(\sigma)}{d\sigma}\lambda\\ \ddot {\tilde q}(t^\prime)=\frac{d^2q(\sigma)}{d\sigma^2}\lambda^2\\ {\tilde q}^{(3)}(t^\prime)=\frac{d^3q(\sigma)}{d\sigma^3}\lambda^3\\ \vdots\\ {\tilde q}^{(n)}(t^\prime)=\frac{d^nq(\sigma)}{d\sigma^n}\lambda^n\\

为了简便起见，上述关系可以重新写成如下形式：

\dot {\tilde q}(t^\prime)=\lambda\dot q(t)\\ \ddot {\tilde q}(t^\prime)=\lambda^2\ddot q(t)\\ {\tilde q}^{(3)}(t^\prime)=\lambda^3q^{(3)}(t)\\ \vdots\\ {\tilde q}^{(n)}(t^\prime)=\lambda^nq^{(n)}(t)\\

因此，通过对时间变量t以及常数1/\lambda进行缩放得到新的时间变量t^\prime，进而重新参数化轨迹的各阶导数以\lambda各次幂为系数进行缩放。通过合理选择参数\lambda，我们可以减小或者增加轨迹的速度、加速度和加加速度等，进而可以使调整后的轨迹满足给定的各类约束。例如，当\lambda按如下关系选取是，可以保证速度、加速度和加加速度不超过其各自给定的最大值：

\lambda =\min \{ \frac{v_{max}}{|\dot q(t)|_{max}},\sqrt\frac{a_{max}}{|\ddot q(t)|_{max}}, \sqrt[3]{\frac{j_{max}}{| q^{(3)}(t)|_{max}}\}}

实例：梯形加减速轨迹时间缩放

基于上面的原理，不管是q(t)是何种加减速算法生成，都可以使用上面的时间缩放法进行时间同步，下面以梯形减速为例进行说明,流程如下图所示。

可以考虑这样的场景：在笛卡尔空间，按照位置和姿态的速度和加速度参数，分别对机器人的位置和姿态进行梯形速度规划，得到位置轨迹为 q_p(t),t\in[0,t_p] ，姿态轨迹为 q_o(t),t\in[0,t_o]，同步时间为较大者，即

t_s=\max(t_p,t_o)

那么缩放因子

\lambda_p=t_p/t_s, \lambda_o=t_o/t_s

得到缩放后的轨迹位移

q_{ps}(t_s)=q_p(t\lambda_p),t\in[0,t_s]\\ q_{os}(t_s)=q_o(t\lambda_o),t\in[0,t_s]

速度和加速度参考前面公式计算。

时间最优规划请参考这篇博客《梯形加减速算法》，从流程图中可以看出，只需在插补环节对时间较小的轨迹进行时间缩放即可，整个流程计算量很少。

下面给出不同参数下的几个算例，实现代码在本文末尾。

算例1

有匀速段，位置与姿态位移量相差不大。

%位置梯形参数
vs=10;%起步速度
vmax=50;%最大速度
ve=20;%末速度
amax=500;%加速度
dmax=400;%减速度
L=10;%位移
Ts=0.001;%插补周期
%姿态参数
vs=5;
vmax=40;
ve=15;
amax=300;
dmax=200;
L=8;
Ts=0.001;

算例2

位置与姿态位移量相差不大，均没有匀速段。

%位置梯形参数
vs=10;%起步速度
vmax=50;%最大速度
ve=20;%末速度
amax=500;%加速度
dmax=400;%减速度
L=5;%位移
Ts=0.001;%插补周期
%姿态参数
vs=5;
vmax=40;
ve=15;
amax=300;
dmax=200;
L=2;
Ts=0.001;

算例3

位置与姿态位移量相差很大,位移是姿态的10倍，位移有匀速段，姿态没有匀速段。

%位置梯形参数
vs=10;%起步速度
vmax=50;%最大速度
ve=20;%末速度
amax=500;%加速度
dmax=400;%减速度
L=20;%位移
Ts=0.001;%插补周期
%姿态参数
vs=5;
vmax=40;
ve=15;
amax=300;
dmax=200;
L=2;
Ts=0.001;

算例4

位置位移量大于0，姿态位移量为0，同步前姿态运行时间就是0，所以其轨迹就是一个点，在左图中看不到。时间同步后，姿态运行时间是与位置相同，只是速度和加速度始终是0。

%位置梯形参数
vs=10;%起步速度
vmax=50;%最大速度
ve=20;%末速度
amax=500;%加速度
dmax=400;%减速度
L=20;%位移
Ts=0.001;%插补周期
%姿态参数
vs=5;
vmax=40;
ve=15;
amax=300;
dmax=200;
L=0;
Ts=0.001;

算例5

连续两段轨迹，同步前衔接速度是相等没有跳变的。同步后衔接速度发生了跳变，不连续。这是因为两段轨迹的缩放因子\lambda不相等，导致衔接处速度发生了跳变，这是时间缩放法的缺点，因此，时间缩放法不适用于衔接速度非零的连续轨迹时间同步。轨迹参数如下

%第一段轨迹参数
%位置梯形参数
vs=10;%起步速度
vmax=50;%最大速度
ve=20;%末速度
amax=500;%加速度
dmax=400;%减速度
Lp=5;%位移
Ts=0.001;%插补周期
%姿态参数
vs=5;
vmax=40;
ve=15;
amax=300;
dmax=200;
Lo=15;
Ts=0.001;
%%
%第二段轨迹参数
%位置梯形参数
vs=20;%起步速度
vmax=50;%最大速度
ve=10;%末速度
amax=500;%加速度
dmax=400;%减速度
L=20;%位移
Ts=0.001;%插补周期
%姿态参数
vs=15;
vmax=40;
ve=15;
amax=300;
dmax=200;
L=15;
Ts=0.001;

总结

本文主要介绍了轨迹同步的背景及必要性；轨迹同步的主要方法及其优缺点；轨迹时间缩放的基本原理；以梯形加减速轨迹为例，介绍了时间缩放法实现轨迹时间同步的流程，同时给出了典型的5个算例。并通过一个算例指出时间缩放法不适用于衔接速度非零的连续轨迹时间同步。最后提供了相关参考资料和本文的代码。

参考资料

[1]博客《梯形加减速算法》

[2]博客《梯形时间同步—直接规划法》

[3]史中权,叶文华.多轴联动条件下插补速度实时可调的前瞻控制算法[J].航空学报, 2014, 35(2).DOI:10.7527/S1000-6893.2013.0386.

[3]吕逸倩. 五轴加工双NUBRS刀具路径进给速度规划研究[D].天津大学,2021.DOI:10.27356/d.cnki.gtjdu.2021.001874.

[4]李航标. 五轴联动双NURBS插补技术研究[D].南昌航空大学,2019.

[5何文杰. 五轴双NURBS刀具路径拟合及其插补算法研究[D].合肥工业大学,2018.

[6]黄亮. 五轴刀具路径双NURBS优化研究[D].哈尔滨工业大学,2013.

[7]王群. 五轴联动双NURBS曲线插补算法研究[D].湖南大学,2012.

[8]Yuen A, Zhang K, Altintas Y. Smooth trajectory generation for five-axis machine tools[J]. International Journal of Machine Tools and Manufacture, 2013: 11–19.

[9] Biagiotti L , Melchiorri C .Trajectory Planning for Automatic Machines and Robots[M].Springer Berlin Heidelberg,2009.

代码附录

算例的测试程序：

clc
clear
close('all')
%位置梯形参数
vs=10;%起步速度
vmax=50;%最大速度
ve=20;%末速度
amax=500;%加速度
dmax=400;%减速度
L=20;%位移
Ts=0.001;%插补周期
qp=trapProfile(vs,vmax,ve,amax,dmax,L);%梯形时间最优速度规划
%姿态参数
vs=5;
vmax=40;
ve=15;
amax=300;
dmax=200;
L=0;
Ts=0.001;
qo=trapProfile(vs,vmax,ve,amax,dmax,L);
%计算时间缩放因子
ts=max(qp.t,qo.t);
lambdap=qp.t/ts;
lambdao=qo.t/ts;
%按周期计算插补点
%原始轨迹，用于绘图比较
%位置
k=1;
for t=0:Ts:qp.t
   tp(k,1)=(k-1)*Ts;
   q1(k,1)= trapProfilePos(qp,t);
   dq1(k,1)= trapProfileVel(qp,t);
   ddq1(k,1)= trapProfileAcc(qp,t);
   k=k+1;
end
k=1;
%姿态
for t=0:Ts:qo.t
   to(k,1)=(k-1)*Ts;
   q2(k,1)= trapProfilePos(qo,t);
   dq2(k,1)= trapProfileVel(qo,t);
   ddq2(k,1)= trapProfileAcc(qo,t);
   k=k+1;
end
%缩放轨迹
k=1;
for t=0:Ts:ts
   ts(k,1)=(k-1)*Ts;
   %位置
   qps(k,1)= trapProfilePos(qp,t*lambdap);
   dqps(k,1)= lambdap* trapProfileVel(qp,t*lambdap);
   ddqps(k,1)= lambdap^2* trapProfileAcc(qp,t*lambdap);
   %姿态
   qos(k,1)=  trapProfilePos(qo,t*lambdao);
   dqos(k,1)= lambdao* trapProfileVel(qo,t*lambdao);
   ddqos(k,1)= lambdao^2* trapProfileAcc(qo,t*lambdao);
   k=k+1;
end
%绘图
figure
set(gcf,'color','w');
fig1=subplot(3,1,1);
title('时间同步前')
ylabel('position')
grid on;
hold on;
fig2=subplot(3,1,2);
ylabel('velocity')
grid on;
hold on;
fig3=subplot(3,1,3);
ylabel('acceleration')
grid on;
hold on;
%绘制缩放前的轨迹
c1=plot(fig1,tp,q1,'--r');
c2=plot(fig1,to,q2,'-r');
legend([c1 c2],'位置','姿态')
c1=plot(fig2,tp,dq1,'--g');
c2=plot(fig2,to,dq2,'-g');
legend([c1 c2],'位置','姿态')
c1=plot(fig3,tp,ddq1,'--b');
c2=plot(fig3,to,ddq2,'-b');
legend([c1 c2],'位置','姿态')
%绘制缩放后的轨迹
figure
set(gcf,'color','w');
fig1=subplot(3,1,1);
title('时间同步后')
ylabel('position')
grid on;
hold on;
fig2=subplot(3,1,2);
ylabel('velocity')
grid on;
hold on;
fig3=subplot(3,1,3);
ylabel('acceleration')
grid on;
hold on;
c1=plot(fig1,ts,qps,'--r');
c2=plot(fig1,ts,qos,'-r');
legend([c1 c2],'位置','姿态')
c1=plot(fig2,ts,dqps,'--g');
c2=plot(fig2,ts,dqos,'-g');
legend([c1 c2],'位置','姿态')
c1=plot(fig3,ts,ddqps,'--b');
c2=plot(fig3,ts,ddqos,'-b');
legend([c1 c2],'位置','姿态')

梯形速度时间最优规划：

function tp = trapProfile(vs, vmax, ve, amax,dmax, L)
%梯形速度时间最优规划
    acc=amax;
    dec=-dmax;
    %起步速度和终止速度不能大于匀速速度
    if (vs > vmax)
        vs=vmax;
    end
    if (ve > vmax)
        ve = vmax;
    end
    %4种情形
    vf = sqrt((-2.0 * acc * dec * L - vs * vs * dec + ve * ve * acc) / (acc - dec));
    if (vf > vmax)%有匀速段        
        vc = vmax;
        t1 = (vc - vs) / acc;
        t3 = (ve - vc) / dec;
        L1 = vs * t1 + 0.5 * acc * t1 * t1;
        L3 = vc * t3 + 0.5 * dec * t3 * t3;
        L2 = L - L1 - L3;
        t2 = L2 / vc;
        t = t1 + t2 + t3;

    else%没有匀速段

        if (vs < vf && vf < ve)%只有加速段

            ve = sqrt(vs * vs + 2 * acc * L);
            vc = ve;
            t1 = (ve - vs) / acc;
            t2 = 0;
            t3 = 0;
            L1 = vs * t1 + 0.5 * acc * t1 * t1;
            L2 = 0;
            L3 = 0;
            t = t1;

        elseif (ve < vf && vf < vs)%只有减速段

            ve = sqrt(vs * vs + 2 * dec * L);
            vc = vs;

            vc = vs;

            t1 = 0;
            t2 = 0;
            t3 = (ve - vs) / dec;
            L1 = 0;
            L2 = 0;
            L3 = vc * t3 + 0.5 * dec * t3 * t3;
            t = t3;

         else%存在加速段和减速段

            vc = vf;
            t1 = (vc - vs) / acc;
            t2 = 0;
            t3 = (ve - vc) / dec;
            L1 = vs * t1 + 0.5 * acc * t1 * t1;
            L2 = 0;
            L3 = vc * t3 + 0.5 * dec * t3 * t3;
            t = t1 + t3;
         end
    end
    tp.vs=vs;
    tp.vc=vc;
    tp.ve=ve;
    tp.acc=amax;
    tp.dec=-dmax;
    tp.t1=t1;
    tp.t2=t2;
    tp.t3=t3;
    tp.L1=L1;
    tp.L2=L2;
    tp.L3=L3;
    tp.L=L;
    tp.t=t;
end

计算梯形速度的位置:

function Lt= trapProfilePos(tp,t)
    tmp = 0.0;
    Lt = 0.0;
    if (tp.L < 1.0E-5)

        Lt = 0.0;

    elseif (t < tp.t1)

        Lt = tp.vs * t + 0.5 * tp.acc * t * t;

    elseif (t < tp.t1 + tp.t2)

        Lt = tp.L1 + tp.vc * (t - tp.t1);

    else

        tmp = t - tp.t1 - tp.t2;
        Lt = tp.L1 + tp.L2 + tp.vc * tmp + 0.5 * tp.dec * tmp * tmp;

    end
end

计算梯形速度曲线的速度：

function  vt= trapProfileVel(tp, t)
    vt = 0.0;
    if (tp.L < 1.0E-5)

        vt = 0.0;

    elseif (t < tp.t1)

        vt = tp.vs + tp.acc * t;

    elseif (t < tp.t1 + tp.t2)

        vt = tp.vc;

    else

        vt = tp.vc + tp.dec * (t - tp.t1 - tp.t2);

    end

end

计算梯形曲线的加速度：

function acc= trapProfileAcc(tp,t)
    if (t < 0.0)
        acc= 0.0;
    elseif (t < tp.t1)
        acc=tp.acc;
    elseif (t < tp.t1 + tp.t2)
        acc=0.0;
    else
        acc=tp.dec;
end

运动控制轨迹规划时间同步位姿同步梯形加减速

原创文章作者：test。如若转载，请注明出处：古月居 https://www.guyuehome.com/43425

打赏 0

精选轨迹时间同步--时间缩放法

test

轨迹时间同步—时间缩放法

摘要

时间同步方法

时间缩放法原理[9]

实例：梯形加减速轨迹时间缩放

算例1

算例2

算例3

算例4

算例5

总结

参考资料

代码附录

为你推荐

非完整约束轮式机器人WMR运动控制系统：笔记2

周期同步位置模式(CSP)，轮廓位置模式(PPM)，位置模式(PM)

②蜘蛛机械人--组装测试与软件安装、运动步态分析

PID控制（五）PI控制器参数计算流程

TRAC-IK机器人运动学求解器

自研机器人控制系统，挑战波士顿动力机器人

评论（0）

关于作者

test

1

0

0

2

精选轨迹时间同步--时间缩放法

相关推荐

[Astar_algorithm04]A_star类与Astar_algorithm等函数

自动驾驶——LQR控制器

精选地平线上的Go1机器狗--第一期-前期准备

Moco Arm 四足机器人 足臂协调控制教程1：机械臂介绍与组装

积分分离PIDsimulink仿真

多机器人协同控制①——仿真平台搭建

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

Moco Arm 四足机器人足臂协调控制教程1：机械臂介绍与组装

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板