CN111708976A - 一种高阶连续的点对点运动轨迹规划方法 - Google Patents

Abstract

本发明公布了一种高阶连续的点对点运动轨迹规划方法，其特征在于：根据点对点运动的特点，利用双曲正切函数设计包含加速和部分匀速段、且变化率可调的光滑连续曲线S₁，及包含部分匀速和减速段、且变化率可调的光滑连续曲线S₂，将曲线S₁和S₂在中点进行衔接，形成包含加速、匀速及减速段的S型曲线，并将S型曲线与跟踪微分器结合，生成速度、加速度和加加速度始终有界，能准确收敛至目标点，且加速、减速阶段的快慢程度、匀速阶段的最大速度可按需调整、高阶连续的点对点运动轨迹，具有结构简单、光滑、冲击强度可控、各阶段状态可调、适用范围广等优点。

Description

一种高阶连续的点对点运动轨迹规划方法

技术领域

本发明涉及一种高阶连续的点对点运动轨迹规划方法，属于自动控制领域。

背景技术

点对点运动广泛存在于机器人、数控装备、起重机械等领域。利用目标位置与当前位置的误差进行反馈控制，是很多点对点运动控制系统普遍采用的算法，这种方法存在着初始控制量大、过程不可控以及振动和冲击较大等缺陷。点对点运动要求启动平稳、停车精准，而其所经历的加、减速阶段势必对其产生冲击，缓慢运行可减少冲击，但低下的效率使人难以接受。为了解决这个问题，通常采用轨迹规划法。合理的点对点轨迹规划应该将最大速度、加速度、加加速度等进行合理约束，并能根据被控对象特点有效调整加、减速过程，且保证系统平稳运行。目前，大多数轨迹规划是根据点对点运动所经历的加速、匀速、减速过程构造梯形、S型速度或加速度分段函数，这些分段函数的衔接点由于连接不光滑，会使其不可微，直接造成柔性冲击和振动。采用高次多项式，或在衔接点采用正弦、余弦、样条或者双曲正切等光滑函数对各段的衔接处进行平滑处理，可以提升轨迹的平滑性和可微的阶次，但是这种方法通常增加了曲线衔接点的个数，使曲线表达形式更加复杂，时间规划更加困难，而且这些曲线参数众多，难以根据某些被控对象要求对启停进行针对性调整，使其效率或精度受到影响。

发明内容

针对上述问题和不足，本文提出了一种高阶连续的点对点运动轨迹规划方法。该法利用两段双曲正切函数构造衔接点在匀速段的非对称S型速度曲线，并将S型曲线与跟踪微分器结合，生成了加、减速阶段的快慢程度、匀速阶段的最大速度可按需调整的高阶连续轨迹。具体地，该方法按照以下步骤实施：

步骤1，设计光滑连续、且幅值和变化率都可调的两个双曲正切函数，有机衔接后形成一条包含加速、减速及匀速三个阶段、且衔接点在匀速段的S型曲线；

以v₁表示实时位置，s_d表示目标点，当v₁∈[0,s_d/2]，即被控对象在起点和中点之间时，利用双曲正切函数设计包含加速和部分匀速段、且变化率可调的光滑连续曲线S₁，其数学表达式为：

f₁(v₁,s_d)＝-k_d·tanh[(v₁+ε)·r₁] (1)

式中，k_d为额定速度，r₁∈R⁺，为加速调节因子，可调节加速阶段的快慢程度，r₁越小，近似线性部分斜率越小，越平缓，ε为小正数，目的是为启动被控对象提供一个初值；

当v₁∈[s_d/2,s_d]，即被控对象在中点和目标点之间时，设计包含部分匀速和减速段、且变化率可调的光滑连续曲线S₂，其数学表达式为：

f₂(v₁,s_d)＝k_d·tanh[(v₁-s_d)·r₂] (2)

式中，r₂∈R⁺，为减速调节因子，可调节减速阶段的快慢程度；

将曲线S₁和S₂在中点进行衔接，形成包含加速、匀速及减速段S型曲线，其数学表达式为：

步骤2，被控对象起动时相当于被施加了一阶跃信号，该信号高阶不可微，阶跃信号经跟踪微分器后，其输出曲线变为一条平滑曲线，可有效消除初始点高阶不可微的问题，因此对0点到v₁-s_d点用跟踪微分器处理，其数学表达式为：

式中，r∈R⁺，为速度调节因子，可调节被控对象的运行速度；

步骤3，对式(3)、式(4)进行综合，得到速度、加速度和加加速度始终有界，能准确收敛至目标点，且高阶连续的点对点光滑运动轨迹，其数学表达式为：

步骤4，点对点光滑运动轨迹在中点处存在衔接点，为使中点处高阶可微，调整速度调节因子r，使速度至少在s_d/2邻域的2N个计算歩长内都以最大速度匀速运行，N表示可微的阶数；最大速度根据目标点距离s_d，加、减速调节因子r₁、r₂，额定速度k_d，速度调节因子r，通过采用龙哥库塔法，由计算机仿真确定。

本发明有益效果是：高阶连续的点对点运动轨迹能根据需要调整加、减速阶段的快慢程度、匀速阶段的最大速度且高阶连续，具有结构简单、光滑、冲击强度可控、各阶段的快慢程度可调、适用范围广等优点。

附图说明

图1为高阶连续的点对点运动轨迹规划方法的实现流程；

图2为参数r对高阶连续点对点运动轨迹的影响；

图3为参数r₁对高阶连续点对点运动轨迹的影响；

图4为参数r₂对高阶连续点对点运动轨迹的影响；

图5为轨迹T₁、T₂、T₃的位移、加速度、加加速度、加加加速度仿真结果，图中自上至下分别为T₁、T₂、T₃的位移、加速度、加加速度、加加加速度随时间的变化曲线。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合附图与具体实施方式，对本发明进行详细描述。

本发明的一种高阶连续的点对点运动轨迹规划方法，其基本思路是：根据点对点运动的特点，利用两段双曲正切函数构造衔接点在匀速段的S型速度曲线，并将S型曲线与跟踪微分器结合，生成了加、减速阶段的快慢程度、匀速阶段的最大速度可按需调整且高阶连续的点对点运动轨迹，其实现流程。

一种高阶连续的点对点运动轨迹规划方法的实现流程如图1所示，具体包括以下步骤：

步骤A，设计光滑连续、且幅值和变化率都可调的两个双曲正切函数，有机衔接后形成一个包含加、减及匀速三个阶段、且衔接点在匀速段的S型曲线。

S型曲线包含加速、减速及匀速三个阶段，符合点对点运动的特点，因此，先构造各阶段状态都可按需调整的S型曲线。为了避免S型曲线在加速、匀速、减速段的衔接点出现断点，寻找一个具有加速、匀速、减速特点，且能光滑过渡的函数就很关键。以v₁表示实时位置，s_d表示目标点，当v₁∈[0,s_d/2]，即被控对象在起点和中点之间时，利用双曲正切函数设计包含加速和部分匀速段、且变化率可调的光滑连续曲线S₁，其数学表达式为：

f₁(v₁,s_d)＝-k_d·tanh[(v₁+ε)·r₁] (1)

式中，k_d为额定速度，r₁∈R⁺，为加速调节因子，可调节加速阶段的快慢程度，r₁越小，近似线性部分斜率越小，越平缓，ε为小正数，目的是为启动被控对象提供一个初值；

当v₁∈[s_d/2,s_d]，即被控对象在中点和目标点之间时，设计包含部分匀速和减速段、且变化率可调的光滑连续曲线S₂，其数学表达式为：

f₂(v₁,s_d)＝k_d·tanh[(v₁-s_d)·r₂] (2)

式中，r₂∈R⁺，为减速调节因子，可调节减速阶段的快慢程度；

将曲线S₁和S₂在中点进行衔接，形成包含加速、匀速及减速段S型曲线，其数学表达式为：

步骤B，被控对象起动时相当于被施加了一阶跃信号，该信号高阶不可微，会对被控对象造成严重冲击，阶跃信号经过跟踪微分器后，其输出曲线变为一条平滑曲线，可有效消除初始点高阶不可微的问题，因此对0点到v₁-s_d点用跟踪微分器处理，其数学表达式为：

式中，r∈R⁺，为速度调节因子，可调节被控对象的运行速度；

步骤C，对式(3)、式(4)进行综合，得到速度、加速度和加加速度始终有界，能准确收敛至目标点，且高阶连续的点对点光滑运动轨迹，其数学表达式为：

步骤D，点对点光滑运动轨迹在中点处存在衔接点，为使中点处高阶可微，调整速度调节因子r，使速度至少在s_d/2邻域的2N个计算歩长内都以最大速度匀速运行，N表示可微的阶数；最大速度根据目标点距离s_d，加、减速调节因子r₁、r₂，额定速度k_d，速度调节因子r，通过采用龙哥库塔法，由计算机仿真确定。

步骤E、对高阶连续点对点运动轨迹的如下性质进行严格的数学证明：a、收敛至目标点s_d处；b、高阶连续点对点运动轨迹轨迹速度始终有界，且r∈[0,2]时，0≤v₂≤k_d；c、高阶连续点对点运动轨迹轨迹加速度和加加速度始终有界。

定理1高阶连续的点对点运动轨迹收敛至目标位置s_d处。

考虑到启动加速度调节因子ε的数值很小，为使证明过程简洁明晰，将其忽略不计。

构造如下函数h(v₁):

其中，Q＝ln(cosh(r₁s_d/2))。当v₁≤s_d/2时，ln(cosh(r₁v₁))≤ln(cosh(r₁s_d/2))，又ln(cosh(r₁v₁))≥0，且k_d、r、r₁均为正数，因此式(6)≥0。

对式(6)求导可得：

构造如下Lyapunov候选函数V：

根据式(6)可知，V为恒大于等于0的函数，将V对时间求导，可得：

由式(9)≤0可知，高阶连续点对点运动轨迹是Lyapunov意义下稳定的。

定义集合Γ为包含所有满足

的点，由式(6)可知，在该集合中，V为常数，且由

可知：v₂＝0，相应的有：

综合式(3)、式(5)可知，当t→∞时，f(v₁,s_d)＝0，因此可得：

定理2高阶连续点对点运动轨迹速度始终大于0且有界，且r∈[0,2]时，0≤v₂≤k_d。

证明：构造新函数：

令G(v₁)＝g(v₁)+f(v₁,s_d)，接下来证明G(v₁)≥0。

当0≤v₁<1/(2r₁)时：

G(v₁)＝k_d(2r₁v₁-tanh(r₁v₁)) (12)

对式(12)求导，可得：

因此，在区间[0,0.5/r₁]，G(v₁)为单调递增函数，又因为G(0)＝0，因此在区间[0,0.5/r₁]，G(v₁)≥0；

当1/(2r₁)≤v₁<(2s_d-1)/(2r₂)时，很显然G(v₁)≥0；

当(s_d-0.5)/r₂≤v₁<s_d/r₂时：

G(v₁)＝k_d tanh(r₂(v₁-s_d))-2k_d(r₂v₁+s_d) (14)

对式(14)求导，得：

又G(s_d)＝0，因此在区间[(s_d-0.5)/r₂，s_d/r₂]内，G(v₁)≥0，综上证明可得，在区间[0,s_d/r₂]：

-f(v₁,s_d)≤g(v₁) (16)

接下来构造一个新微分方程：

假定到达目标位置的时间为t，当v₁＝0.5/r₁时为t₁，当v₁＝(s_d-0.5)/r₂时为t₂，当v₁＝s_d时为t₃，对上式中的

求定积分：

当0≤t≤t₁时，

求解该方程可得v₁的表达式为：

其中，

对式

求定积分可得：

其中，c₁、c₂为待定参数(根据初始状态确定)，k_d为额定速度，显然式(20)是有界的。

由于t＝0时，v₁＝0，将其代入式(19)，可得如下关系：

c₁＝-c₂ (21)

由v₁≥0可知，c₁为正数。

对式(19)求导，并结合式(21)可得：

由于ψ₁₁>ψ₁₂，因此当0≤t≤t₁时v₂(t)≥0，

当t₁≤t≤t₂时，

求解该方程可得v₁的表达式为：

对式(24)求导，得速度v₂：

对式(25)求导，并将t₁代入，得：

由

可知，c₄>0，因此，v₂≤rk_d/2。

又v₂(t)>v₂(t₁)，因此v₂(t)>0

当t₂≤t≤t₃时，

将

代入式(27)，并整理得：

这是一个典型的二阶系统，其阶跃响应可以无超调地跟踪设定值s_d，因而其位移是有界的，当然速度也是有界的。又由于无超调的阶跃响是单调上升的，因此当t₂≤t≤t₃时，v₂>0且有界。

由于所规划轨迹的最大速度在匀速段，而匀速段所在区间为[t₁,t₂]，该区间内的最大速度为rk_d/2，因而整个区间的速度都小于rk_d/2。

综合以上分析，可得期望轨迹速度始终大于0且有界，且r∈[0,2]时，0≤v₂≤k_d。

定理3高阶连续点对点运动轨迹加速度和加加速度都有界。

根据式(23)、(26)及(27)，显然加速度有界。

对式(5)针对时间求导得：

由于加速度有界，且tanh函数为有界函数，因此式(29)有界，即加加速度有界。

步骤F、通过仿真说明高阶连续点对点运动轨迹的可调参数可有效调整轨迹的各阶段状态和冲击强度。

对不同被控对象，在加速、匀速、减速阶段有不同要求。譬如：为提高效率，起重机械要求较快加速到最大运行速度后匀速运行，为保证安全，其最大运行速度也要根据环境风速进行调整，减速阶段则要求缓慢地停靠在目标点，以实现精准定位。因此，有必要研究可调参数对高阶连续点对点运动轨迹各阶段状态和冲击强度的影响规律，使其在不同的应用场合发挥最大效能。

为研究最大速度调节因子r对高阶连续点对点运动轨迹的影响，令s_d＝30m，k_d＝2m/s，ε＝0.01，r＝2、r₂＝0.4，当r分别取2、1.61和1.24时，位移、速度、加速度随时间的变化如图2所示，从图中可以看出，当r等于2时，轨迹的最大匀速运行速度等于额定速度。r越小，最大匀速运行速度越小，在作业距离一定时，耗时也越长。因此，作业距离越远，应使r越大，以尽可能提高作业效率，但r不能大于2；近距离作业时，则应使r较小，但太小会降低作业效率，因此以在s_d/2邻域的2N个计算歩长内都以最大速度匀速运行为前提。对在室外环境中作业的起重机械，为保证安全，应根据风速等级，选择合适的r值。

为研究加速调节因子r₁对高阶连续点对点运动轨迹的影响，令s_d＝30m，k_d＝2m/s，ε＝0.01，r＝2、r₂＝0.4，当r₁分别取0.65、1和1.8时，高阶连续点对点运动轨迹的位移、速度、加速度随时间的变化如图3所示，从图中可以看出，r₁越小，轨迹初始阶段越平缓，到达最大速度的时间变长，运行确定距离所耗费的时间也越多，随着r₁的增加，加速度变大，冲击强度增加，到达最大速度的时间变短，匀速段和减速段提前，但最大速度大小和减速阶段状态不受影响。因此，对于需要慢启动的精密机械，应取较小的r₁值，以减小冲击，而对于行程较长的起重机械，在不超过最大加速度的前提下，则宜使用较大r₁值，以提高效率。

为研究减速调节因子r₂对高阶连续点对点运动轨迹的影响，令s_d＝30m，k_d＝2m/s，ε＝0.01，r＝1.24、r₁＝1，当r₂分别取0.2、0.28和0.36时，高阶连续点对点运动轨迹的位移、速度、加速度随时间的变化如图4所示，从图中可以看出，在进入减速段前，整个轨迹曲线保持不变，但r₂越小，进入减速段的时间越早，使匀速段的时间变短，减速段的时间变长，完成整个作业过程的时间增多，但变化趋势越平缓，接近目标点时的速度也越慢，对于需要在目标点精确对位的精密装备，如岸桥起重机，宜选择较小的r₂值。

步骤G、通过和典型轨迹进行对比，证明高阶连续点对点运动轨迹的优越性。

为了检验高阶连续点对点运动轨迹的效果，取s_d＝20m，k_d＝5m/s，ε＝0.01，r＝1.28、r₁＝0.5、r₂＝0.2，计算步长取0.05，进行仿真，得仿真结果T₁。同时，在相同目标位置和计算步长下，将S型速度曲线的仿真结果记为T₂，S型位移曲线的仿真结果记为T₃。

S型速度曲线采用如下表达式：

其中，y_c为该轨迹的最大速度，t₁为加速阶段时间，t₂为匀速阶段时间，T为总时间，具体参数为：y_c＝3.065m/s，t₁＝2.345s，t₂＝12s、T＝18.69s。

S型位移曲线轨迹采用如下表达式：

其中，v_e为最大速度，a_e为最大加速度，κ∈R⁺，用于调整初始加速度，κ＝1.4，v_e＝3.056m/s，a_e＝2.2m/s²。

将仿真时间步长设置为0.01s，仿真结果如图5，图中自上至下分别为T₁、T₂、T₃的位移、加速度，加加速度，加加加速度随时间t的变化曲线，从图中可以看出：三种轨迹几乎同时到达指定位置，也就是在合适条件下它们的运行效率基本相同。轨迹T₃的加速度在初始阶段出现了突变，说明利用轨迹T₃进行跟踪控制时，在起动时需要很大驱动力，这势必提高对执行器的功率要求，而且会对系统产生严重冲击，T₁、T₂的加速度都是连续的，但轨迹T₁的最大加速度更小，说明所需的最大驱动力更小，对执行器的最大功率要求也就更低，有利于降低成本，而且T₂在各段(加速、匀速、减速)衔接处出现了明显转折，而轨迹T₁的加速度在整个过程中都比较平滑，因而利用T₁进行轨迹跟踪控制，系统运行更平稳。T₃初始阶段的加加速度也出现了突变，会对系统产生很大的柔性冲击，使其大幅振动，T₂则在各段衔接处出现了最大值为2.73m/s³的跃变，会对系统产生明显的冲击和振动，而T₁加加速度的最大值为1.24m/s³，远小于T₂、T₃的加加速度最小值，并且T₂的加加速度平滑和连续，不会对系统产生大冲击和振动。轨迹T₂、T₃的加加加速度在各段衔接处都出现了突变，而轨迹T₁的加加加速度光滑且连续，且最大值只有2.75m/s⁴，即使进行更高阶微分，都将是平滑且连续的，因此特别适用于需要进行高阶微分的场合(图中↑表示在该时刻，纵坐标值为无穷大值)。

如上所述，结合附图和说明所给出的方案内容，可以衍生出类似的技术方案。但凡是依据本发明的技术实质所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (1)

1.一种高阶连续的点对点运动轨迹规划方法，其特征在于，按照以下步骤实施：

第1，设计光滑连续、且幅值和变化率都可调的两个双曲正切函数，有机衔接后形成一条包含加速、减速及匀速三个阶段、且衔接点在匀速段的S型曲线；

以v₁表示实时位置，s_d表示目标点，当v₁∈[0,s_d/2]，即被控对象在起点和中点之间时，利用双曲正切函数设计包含加速和部分匀速段、且变化率可调的光滑连续曲线S₁，其数学表达式为：

f₁(v₁,s_d)＝-k_d·tanh[(v₁+ε)·r₁] (1)

式中，k_d为额定速度，r₁∈R⁺，为加速调节因子，可调节加速阶段的快慢程度，r₁越小，近似线性部分斜率越小，越平缓，ε为小正数，目的是为启动被控对象提供一个初值；

当v₁∈[s_d/2,s_d]，即被控对象在中点和目标点之间时，设计包含部分匀速和减速段、且变化率可调的光滑连续曲线S₂，其数学表达式为：

f₂(v₁,s_d)＝k_d·tanh[(v₁-s_d)·r₂] (2)

式中，r₂∈R⁺，为减速调节因子，可调节减速阶段的快慢程度；

将曲线S₁和S₂在中点进行衔接，形成包含加速、匀速及减速段S型曲线，其数学表达式为：

第2，被控对象起动时相当于被施加了一阶跃信号，该信号高阶不可微，阶跃信号经过跟踪微分器后，其输出曲线变为一条平滑曲线，可有效消除初始点高阶不可微的问题，因此对0点到v₁-s_d点用跟踪微分器处理，其数学表达式为：

式中，r∈R⁺，为速度调节因子，可调节被控对象的运行速度；

第3，对式(3)、式(4)进行综合，得到速度、加速度和加加速度始终有界，能准确收敛至目标点，且高阶连续的点对点光滑运动轨迹，其数学表达式为：

第4，点对点光滑运动轨迹在中点处存在衔接点，为使中点处高阶可微，调整速度调节因子r，使速度至少在s_d/2邻域的2N个计算歩长内都以最大速度匀速运行，N表示可微的阶数；最大速度根据目标点距离s_d，加、减速调节因子r₁、r₂，额定速度k_d，速度调节因子r，通过采用龙哥库塔法，由计算机仿真确定。

Images