CN113848809A - 基于数值滤波的轨迹规划方法 - Google Patents

Abstract

本发明涉及一种基于数值滤波的轨迹规划方法，包括：S1、给定路径约束条件；S2、计算最大步长，设置滤波器阶数f和辅助点Cn；S3、构造离散点序列；S4、对离散点序列进行低通滤波计算得到一条平滑的轨迹；S5、计算滤波后的轨迹与给定控制点约束条件的偏差，并依此调整构造的离散点序列；S6、判断误差是否在容许范围，若是则进入S7，若否则重复S3‑S5；S7、判断是否满足最大加速度约束条件，若是则输出轨迹规划结构，若否则重复S3‑S7。本发明的轨迹规划方法绕过函数拟合计算过程，直接从数值计算角度出发，通过低通滤波计算使轨迹的位移、速度和加速度达到高阶平滑；适用数控机床，工业机器人，移动机器人等多控制电机联动或单个控制电机的轨迹跟踪控制工况。

Description

基于数值滤波的轨迹规划方法

技术领域

本发明涉及自动化控制技术领域，具体涉及一种轨迹规划方法。

背景技术

在工业机器人、数据机床等需要多个电机联动控制的领域，都需要对其控制终端的运动轨迹进行规划计算。通常这里的运动规划可分为路径规划和轨迹规划两个层次。路径规划的目的是为工业机器人终端或移动机器人确定一条符合工作要求的无碰撞的空间几何路径。轨迹规划的任务是确定一组沿着该几何路径运动时的位置，姿态关于时间的离散序列。轨迹规划的结果经过适当的转换计算后最终将作为机器人执行电机的控制目标曲线。轨迹规划需要考虑机器人运动过程中的一些约束条件，主要有两个方面：一方面是作业要求的约束，例如焊接，喷涂等对位姿，速度甚至加速度都有严格的要求作业；另一方面是执行机构的性能约束，例如执行电机能输出的最大速度，转矩等。轨迹规划方案在满足上面的约束条件下，还应考虑在运动平稳性，时间和能量消耗等方面的综合性能。

轨迹规划常用的方法是采用适当的优化函数将路径规划的结果以拟合函数方程的形式描述，并对拟合函数插值处理后得到一组离散的坐标点序列。常用的拟合函数方法有多次样条，贝塞尔，B样条等。拟合函数的次数越高，对应轨迹规划结果的越平滑，而高次拟合函数方程的求解比较复杂，另外控制点的速度和加速度约束条件也会增加拟合函数求解的复杂程度。

发明内容

本发明要解决的技术问题在于针对高次拟合函数求解复杂的问题，提供一种基于数值滤波的轨迹规划方法，该方法绕过函数拟合计算过程，直接从数值计算角度出发，通过低通滤波计算使轨迹的位移、速度和加速度达到更高阶的平滑，计算过程简洁并能适用于控制点有运动约束的情况。

本发明为解决上述提出的技术问题所采用的技术方案为：

一种基于数值滤波的轨迹规划方法，该方法包括以下步骤：

S1、设定轨迹规划的约束条件，包括轨迹所依次经过的起点S、控制点序列P_n和终点E的坐标值以及在每个坐标点处的速度和加速度条件，轨迹的最大速度V_max、最大加速度a_max约束条件，轨迹离散的时间周期T；

S2、由最大速度约束条件和时间周期计算出离散点的最大步长Step_max；设置滤波器阶数的初始值f，并设计数字滤波器；设置一组辅助点C_n，其初始坐标值与控制点P_n相等；

S3、从起点S开始依次经过n个辅助点C_n到终点E形成一条折线，在折线的每条线段上，按最大步长条件以及每个控制点的速度和加速度约束条件，构造一组离散的坐标点构成离散序列I_k；

S4、通过低通数字滤波器对离散序列I_k每个坐标轴的数值组成的序列I_k(x)，I_k(y)，I_k(z)分别进行滤波计算，组合成离散点序列为R_k；

S5、在滤波后的轨迹R_k上，找到距离P_n最近的一组点记为Q_n，以点Q_i和P_i之间的距离矢量为误差

调节辅助点C_i的坐标，得到一组新的辅助点C′_n；

S6、以新的辅助点C′_n代替C_n，重复步骤S3，S4，S5的操作计算R_k，直到Q_n和P_n之间距离偏差e在允许的误差范围内；

S7、计算序列R_k的最大加速度，并判断是否满足最大加速度约束条件，若满足则S6得到的序列R_k即为轨迹规划结果；若不满足，则调整滤波器阶数f的值，重复步骤S3～S7，直到满足最大加速度约束条件，得到的序列R_k即为轨迹规划结果。

上述方案中，所述约束条件和轨迹由包含空间三维坐标和一维时间坐标的坐标系描述。

上述方案中，步骤S2中数字滤波器设计方法为：数字滤波器为低通有限数字滤波器，采用窗函数方法设计，滤波器包含阶数，截止频率，窗函数类型三个参数，其中，数字滤波器的阶数选为偶数，其大小根据轨迹的最大加速度约束确定，滤波器的阶数越大，规划结果轨迹的平滑性越好；截止频率的取值应保证滤波器的左半侧是单调递增的；窗函数本身应具有高阶可导的特性。

上述方案中，步骤S3中的离散序列I_k构造方法因路径控制点约束条件不同，包括以下三种情况：

(1)控制点没有速度和加速度约束条件：设置轨迹的边界条件为起点和终点的速度及加速度等于0；按最大的步长条件对折线上的每条线段取等间距的插值点，连同f个重复的起点和终点坐标一起构成I_k，保证滤波计算后的轨迹经过起点和终点，且起点和终点的速度及加速度满足设置的边界条件；特别的，如果起点坐标在原点处，且初始速度和加速度为0，则不需要增加f个重复的起点坐标，滤波计算后同样可满足起点的约束条件；

(2)控制点有速度矢量和加速度矢量约束条件：构造一组离散序列代替原来的控制点，保证这组离散序列经过滤波计算后经过原来的控制点，且在控制点处满足速度和加速度约束条件，构造方法为，将速度矢量和加速度矢量分解到x,y,z轴向，得到速度矢量和加速矢量在x,y,z轴上的分量v_x,v_y,v_z和a_x,a_y,a_z，构造如下函数

其中，x(t),y(t),z(t)表示构造离散序列的x,y,z坐标值，t取区间[-f/2,f/2]上的整数值，p_x,p_y,p_z是当前控制点的坐标值，f为滤波器阶数，T是轨迹离散的时间周期；

由上式可计算得到一组包含f+1个坐标点的序列，该序列经过原控制点，且满足控制点处的速度和加速度矢量约束条件，以这组插值点构成的线段代替原来的一个控制点坐标，并将所有的线段依次连接起来，在连线上按照最大的步长条件取等间距的插值点，构成离散序列I_k；

(3)控制点的速度和加速度只有大小或只有方向约束条件：只有大小没有方向约束，则根据与控制点相邻的两点的几何关系，计算一个方向并作为约束条件，构造完整的矢量约束条件；只有方向没有大小的约束，按最大速度或加速度条件作为其约束条件，构造完整的矢量约束条件；然后按(2)的方法构造I_k。

上述方案中，步骤S5中距离P_n最近的点Q_n的确定方法为：根据数字滤波器的计算特征，将辅助点C_n在离散序列I_k中对应的位置序号加上f/2后，对应R_k中的点的坐标作为误差计算的点Q_n。

上述方案中，步骤S5中辅助点C_i的坐标调节方法为：借鉴反馈控制的思想调节辅助点C_n的坐标，辅助点C_n的调整的公式是

其中k取为略大于1的常系数。

上述方案中，步骤S7中，滤波器阶数的调整方法为：滤波器阶数按公式f_new＝f·a_max/a_{R_max}计算，其中a_{R_max}为序列R_k的最大加速度，a_max为最大加速度约束条件，f为步骤S2中设置的滤波器阶数初始值，并将计算结果往上取为偶整数作为新的滤波器阶数。

本发明的有益效果在于：

1、本发明绕过路径的函数拟合计算过程，直接从数值计算的角度出发，计算过程简洁灵活，能适用复杂的约束条件，计算的结果能达到比传统函数拟合方法更高阶的平滑，且在总的轨迹距离和时间消耗上更优。相比函数拟合方法，本发明具体有以下突出优点：

1)本发明方法能得到更加高阶平滑的轨迹规划结果。本发明的方法能去除规划轨迹中的高频信号成分，从本质上保障轨迹的平滑性，而函数拟合方法只能保证轨迹在一条连续的多次方程上。

2)滤波器参数可以在适当范围内自由选择，从而得到多组符合条件的结果，而函数拟合只能得到唯一解，相比而言本发明的方法更灵活。

3)本发明的方法可适用于各种不同路径约束条件下的轨迹规划。

4)使用发明得到的规划轨迹更加贴合控制点的连线，并且在控制点之外的部分接近于最大速度，使规划的轨迹在总的路径距离和时间消耗上更优。

2、本发明适用数控机床，工业机器人，移动机器人等多控制电机联动或单个控制电机的轨迹跟踪控制工况。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明基于数值滤波的轨迹规划方法的轨迹规划流程图；

图2是本发明实施例中路径控制点初始条件示意图；

图3是本发明实施例中构造的离散序列I_k示意图；

图4是本发明实施例中滤波计算前后轨迹的x轴分量示意图；

图5是本发明实施例中滤波计算前后轨迹的y轴分量示意图；

图6是本发明实施例中滤波计算前后的轨迹示意图；

图7是本发明实施例中规划轨迹的加速度曲线图；

图8是本发明实施例中轨迹规划结果示意图。

具体实施方式

为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。

如图1所示，本发明提供了一种基于数值滤波的轨迹规划方法，包括以下步骤：

S1、设定轨迹规划的约束条件，包括轨迹所依次经过的起点S、控制点序列P_n和终点E的坐标值以及在每个坐标点处的速度和加速度条件，轨迹的最大速度V_max、最大加速度a_max约束条件，轨迹离散的时间周期T。

所述约束条件和轨迹由包含空间三维坐标和一维时间坐标的坐标系描述。

S2、由最大速度约束条件和时间周期计算出离散点的最大步长Step_max；设置滤波器阶数的初始值f，并设计数字滤波器；设置一组辅助点C_n，其初始坐标值与控制点P_n相等。

数字滤波器设计方法为：数字滤波器为低通有限数字滤波器，采用窗函数方法设计，滤波器包含阶数，截止频率，窗函数类型三个参数，其中，数字滤波器的阶数选为偶数，其大小根据轨迹的最大加速度约束确定，通常可取较大的整数值，滤波器的阶数越大，规划结果轨迹的平滑性越好；截止频率的取值应保证滤波器的左半侧是单调递增的；窗函数本身应具有高阶可导的特性。

S3、从起点S开始依次经过n个辅助点C_n到终点E形成一条折线，在折线的每条线段上，按最大步长条件以及每个控制点的速度和加速度约束条件，构造一组离散的坐标点构成离散序列I_k。

离散序列I_k构造方法因路径控制点约束条件不同，包括以下三种情况：

(1)控制点没有速度和加速度约束条件：设置轨迹的边界条件为起点和终点的速度及加速度等于0；按最大的步长条件对折线上的每条线段取等间距的插值点，连同f个重复的起点和终点坐标一起构成I_k，保证滤波计算后的轨迹经过起点和终点，且起点和终点的速度及加速度满足设置的边界条件；特别的，如果起点坐标在原点处，且初始速度和加速度为0，则不需要增加f个重复的起点坐标，滤波计算后同样可满足起点的约束条件。

(2)控制点有速度矢量和加速度矢量约束条件：构造一组离散序列代替原来的控制点，保证这组离散序列经过滤波计算后经过原来的控制点，且在控制点处满足速度和加速度约束条件。将速度矢量和加速度矢量分解到x,y,z轴向，得到速度矢量和加速矢量在x,y,z轴上的分量v_x,v_y,v_z和a_x,a_y,a_z，构造如下函数

其中，x(t),y(t),z(t)表示构造离散序列的x,y,z坐标值，t取区间[-f/2,f/2]上的整数值，p_x,p_y,p_z是当前控制点的坐标值，f为滤波器阶数，T是轨迹离散的时间周期；

由上式可计算得到一组包含f+1个坐标点的序列，该序列经过原控制点，且满足控制点处的速度和加速度矢量约束条件，以这组插值点构成的线段代替原来的一个控制点坐标，并将所有的线段依次连接起来，在连线上按照最大的步长条件取等间距的插值点，构成离散序列I_k。

(3)控制点的速度和加速度只有大小或只有方向约束条件：只有大小没有方向约束，则根据与控制点相邻的两点的几何关系，计算一个方向并作为约束条件，构造完整的矢量约束条件；只有方向没有大小的约束，按最大速度或加速度条件作为其约束条件，构造完整的矢量约束条件；然后按(2)的方法构造I_k。

在速度和加速度约束条件下，经过一次滤波计算即可得到满足约束条件的轨迹。

S4、通过低通数字滤波器对离散序列I_k每个坐标轴的数值组成的序列I_k(x)，I_k(y)，I_k(z)分别进行滤波计算，组合成离散点序列为R_k。步骤S4中采用了低通数字滤波计算过程使规划的轨迹达到高阶平滑的效果。

S5、在滤波后的轨迹R_k上，找到距离P_n最近的一组点记为Q_n，以点Q_i和P_i之间的距离矢量为误差

调节辅助点C_i的坐标，得到一组新的辅助点C′_n。

距离P_n最近的点Q_n的确定方法为：根据数字滤波器的计算特征，将辅助点C_n在离散序列I_k中对应的位置序号加上f/2后，对应R_k中的点的坐标作为误差计算的点Q_n。

辅助点C_i的坐标调节方法为：借鉴反馈控制的思想调节辅助点C_n的坐标，辅助点C_n的调整的公式是

其中k为略大于1的常系数。当k＝1时，迭代计算过程中，规划的轨迹上的点Q_n与路径控制点P_n之间的距离将单调收敛于0，是一致收敛过程，为了加快收敛速度并防止振荡，k可取为略大于1的常系数。

步骤S5借鉴反馈控制的思想调节辅助的位置，使规划的轨迹最终经过路径规划的控制点。

S6、以新的辅助点C′_n代替C_n，重复步骤S3，S4，S5的操作计算R_k，直到Q_n和P_n之间距离偏差e在允许的误差范围内。

步骤S6中通过修正和迭代使规划的轨迹符合控制点处的位置约束条件。关于误差容许范围的限定在实际的控制精度范围之内。

S7、计算序列R_k的最大加速度，并判断是否满足最大加速度约束条件，若满足则S6得到的序列R_k即为轨迹规划结果；若不满足，则调整滤波器阶数f的值，重复步骤S3～S7，直到满足最大加速度约束条件，得到的序列R_k即为轨迹规划结果。

滤波器阶数的调整方法为：滤波器阶数按公式f_new＝f·a_max/a_{R_max}计算，其中a_{R_max}为序列R_k的最大加速度，a_max为最大加速度约束条件，f为步骤S2中设置的滤波器阶数初始值，并将计算结果往上取为偶整数作为新的滤波器阶数。

下面以平面轨迹规划(Z＝0)为例，点S和E分别是轨迹规划的起点和终点，P₁,P₂...P₅是轨迹规划经过的控制点，实施本发明的轨迹规划方法步骤如下：

S1、设起点S和终点E的速度和加速度均为0，P₁点处速度大小为0.3m/s，方向为45°，其它控制点无速度和加速度约束。路径最大速度为0.5m/s，最大加速度为0.5m/s²，系统控制周期T＝0.1s，如图2所示。

S2、计算最大步长Step_max＝v_max*T＝5mm，取滤波器阶数f为30，设置一组辅助点C_n，其坐标值与控制点P_n相等。

S3、控制点P₁有速度约束，在P₁点按其约束条件构造一段序列，再与其它点通过直线连接，并在直线段上以最大步长Step_max为条件等间距插值，即间距不能大于最大步长。由于起点在原点处，只需要增加f个重复的终点坐标E。构成离散序列I_k，如图3所示。

S4、把离散序列I_k按坐标轴分解为序列I_k(x)，I_k(y)，如图4，5所示；并分别进行滤波计算，组合得到离散点序列R_k，如图6所示。

S5、找到距离P_n最近的一组点Q_n，计算新的辅助点C′_n，如图6所示。

S6、以新的辅助点C′_n代替C_n，经过10次迭代计算后，Q_n和P_n之间距离偏差e已控制在0.05mm以下。

S7、计算得到R_k的最大加速度值为0.80m/s²，而路径的最大允许加速度为0.5m/s²，因此不满足最大加速度约束条件，需要调整滤波器阶数f。可计算滤波的阶数为30*0.80/0.5＝48.0，可取f＝50，重复步骤S3-S6，重新计算后得到的轨迹最大加速度为0.49m/s²，满足最大加速度约束条件，轨迹的加速度曲线以及轨迹规划结果分别如图7，8所示。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

Claims (7)

1.一种基于数值滤波的轨迹规划方法，其特征在于，该方法包括以下步骤：

S1、设定轨迹规划的约束条件，包括轨迹所依次经过的起点S、控制点序列P_n和终点E的坐标值以及在每个坐标点处的速度和加速度条件，轨迹的最大速度V_max、最大加速度a_max约束条件，轨迹离散的时间周期T；

S2、由最大速度约束条件和时间周期计算出离散点的最大步长Step_max；设置滤波器阶数的初始值f，并设计数字滤波器；设置一组辅助点C_n，其初始坐标值与控制点P_n相等；

S3、从起点S开始依次经过n个辅助点C_n到终点E形成一条折线，在折线的每条线段上，按最大步长条件以及每个控制点的速度和加速度约束条件，构造一组离散的坐标点构成离散序列I_k；

S4、通过低通数字滤波器对离散序列I_k每个坐标轴的数值组成的序列I_k(x)，I_k(y)，I_k(z)分别进行滤波计算，组合成离散点序列为R_k；

S5、在滤波后的轨迹R_k上，找到距离P_n最近的一组点记为Q_n，以点Q_i和P_i之间的距离矢量为误差

调节辅助点C_i的坐标，得到一组新的辅助点C′_n；

S6、以新的辅助点C′_n代替C_n，重复步骤S3，S4，S5的操作计算R_k，直到Q_n和P_n之间距离偏差e在允许的误差范围内；

S7、计算序列R_k的最大加速度，并判断是否满足最大加速度约束条件，若满足则S6得到的序列R_k即为轨迹规划结果；若不满足，则调整滤波器阶数f的值，重复步骤S3～S7，直到满足最大加速度约束条件，得到的序列R_k即为轨迹规划结果。

2.根据权利要求1所述的基于数值滤波的轨迹规划方法，其特征在于，所述约束条件和轨迹由包含空间三维坐标和一维时间坐标的坐标系描述。

3.根据权利要求1所述的基于数值滤波的轨迹规划方法，其特征在于，步骤S2中数字滤波器设计方法为：数字滤波器为低通有限数字滤波器，采用窗函数方法设计，滤波器包含阶数，截止频率，窗函数类型三个参数，其中，数字滤波器的阶数选为偶数，其大小根据轨迹的最大加速度约束确定，滤波器的阶数越大，规划结果轨迹的平滑性越好；截止频率的取值应保证滤波器的左半侧是单调递增的；窗函数本身应具有高阶可导的特性。

4.根据权利要求1所述的基于数值滤波的轨迹规划方法，其特征在于，步骤S3中的离散序列I_k构造方法因路径控制点约束条件不同，包括以下三种情况：

(1)控制点没有速度和加速度约束条件：设置轨迹的边界条件为起点和终点的速度及加速度等于0；按最大的步长条件对折线上的每条线段取等间距的插值点，连同f个重复的起点和终点坐标一起构成I_k，保证滤波计算后的轨迹经过起点和终点，且起点和终点的速度及加速度满足设置的边界条件；特别的，如果起点坐标在原点处，且初始速度和加速度为0，则不需要增加f个重复的起点坐标，滤波计算后同样可满足起点的约束条件；

(2)控制点有速度矢量和加速度矢量约束条件：构造一组离散序列代替原来的控制点，保证这组离散序列经过滤波计算后经过原来的控制点，且在控制点处满足速度和加速度约束条件，构造方法为，将速度矢量和加速度矢量分解到x,y,z轴向，得到速度矢量和加速矢量在x,y,z轴上的分量v_x,v_y,v_z和a_x,a_y,a_z，构造如下函数

其中，x(t),y(t),z(t)表示构造离散序列的x,y,z坐标值，t取区间[-f/2,f/2]上的整数值，p_x,p_y,p_z是当前控制点的坐标值，f为滤波器阶数，T是轨迹离散的时间周期；

由上式可计算得到一组包含f+1个坐标点的序列，该序列经过原控制点，且满足控制点处的速度和加速度矢量约束条件，以这组插值点构成的线段代替原来的一个控制点坐标，并将所有的线段依次连接起来，在连线上按照最大的步长条件取等间距的插值点，构成离散序列I_k；

(3)控制点的速度和加速度只有大小或只有方向约束条件：只有大小没有方向约束，则根据与控制点相邻的两点的几何关系，计算一个方向并作为约束条件，构造完整的矢量约束条件；只有方向没有大小的约束，按最大速度或加速度条件作为其约束条件，构造完整的矢量约束条件；然后按(2)的方法构造I_k。

5.根据权利要求1所述的基于数值滤波的轨迹规划方法，其特征在于，步骤S5中距离P_n最近的点Q_n的确定方法为：根据数字滤波器的计算特征，将辅助点C_n在离散序列I_k中对应的位置序号加上f/2后，对应R_k中的点的坐标作为误差计算的点Q_n。

6.根据权利要求5所述的基于数值滤波的轨迹规划方法，其特征在于，步骤S5中辅助点C_i的坐标调节方法为：借鉴反馈控制的思想调节辅助点C_n的坐标，辅助点C_n的调整的公式是

其中k取为略大于1的常系数。

7.根据权利要求1所述的基于数值滤波的轨迹规划方法，其特征在于，步骤S7中，滤波器阶数的调整方法为：滤波器阶数按公式f_new＝f·a_max/a_{R_max}计算，其中a_{R_max}为序列R_k的最大加速度，a_max为最大加速度约束条件，f为步骤S2中设置的滤波器阶数初始值，并将计算结果往上取为偶整数作为新的滤波器阶数。

Images