CN107030697B - 一种机器人笛卡尔空间平滑轨迹的规划方法 - Google Patents

Abstract

本发明公开了一种机器人笛卡尔空间平滑轨迹的规划方法，其过程如下：对空间直线、圆弧等单段轨迹的位置和姿态进行规划；对路经进行过渡平滑规划，采用过渡等级控制路经过渡区域的大小，在过渡区域内的路经采用抛物线进行拟合，姿态采用四元数法进行拟合；采用含加速度、加加速度约束的摆线加减速规划方法对空间路径轨迹进行规划，得到平滑的机器人空间位姿轨迹。该方法能够有效解决机器人轨迹停顿、不流畅、抖动、偏离路径、超速等问题，提升机器人运动的平稳性。本发明所涉及的平滑轨迹规划方法具有一定的柔性，既能规划笛卡尔空间的位姿，又能够适用于多种机器人构型，包含常见的6自由度垂直关节机械臂、少自由度机器人及冗余度机器人。

Description

一种机器人笛卡尔空间平滑轨迹的规划方法

技术领域

本发明涉及具有空间运动特征的机器人技术领域，尤其涉及一种机器人笛卡尔空间平滑轨迹的规划方法。

背景技术

机器人笛卡儿空间运动路径多采用直线、圆弧，为提升机器人的执行效率和平稳性，要求路径段具有平滑过渡的特征，即在轨迹运动过程中，不减速或不停止。

目前，轨迹过渡规划的研究中，多采用圆弧、高次曲线来构造过渡段路径。其中圆弧过渡路径应用最为广泛，在相邻两段直线之间具有很好的效果，但对于空间直线与圆弧、空间圆弧与圆弧之间则存在着加速度不连续的情况；采用高次曲线，特别是五次多项式，其过渡路径非常平滑，但是存在着路径随着速度、加速度参数的改变而改变的特征，存在着路径不可控的缺点；中国发明专利“一种工业机器人应用的过渡轨迹规划方法”(申请号201610075346.7)，采用两段抛物线作为过渡路径，以位置规划为主，姿态规划为辅，即可保证路径的平滑性，又能保证速度、加速度的连续性，但是在路径段位置距离过短、姿态变化大等情况下，机器人运动性能还有很大的提升空间。

目前机器人姿态多表示为欧拉角或姿态矩阵，但欧拉角不具有向量的性质，而姿态矩阵插值比较困难。因此，如何提供一种简单高效的机器人笛卡尔空间平滑轨迹规划方法，以进一步提升机器人运动性能是很有必要的。

发明内容

为了克服现有技术存在的缺点与不足，本发明提供一种机器人笛卡尔空间平滑轨迹的规划方法，对以直线、圆弧为主的机器人空间轨迹进行平滑规划，以解决机器人轨迹停顿、不流畅、抖动、偏离路径、超速等问题，进一步提升机器人运动的效率和平稳性。

为解决上述技术问题，本发明提供如下技术方案：一种机器人笛卡尔空间平滑轨迹的规划方法，包括摆线加减速规划方法、空间任意两点位置过渡路径规划方法以及空间任意两点姿态过渡路经规划方法，其中

所述摆线加减速规划方法，其过程分为加速、匀速和减速三个阶段，加速阶段和减速阶段的轨迹形状为正则摆线，具体包括如下步骤：

S11、已知路径长度l、初始速度v_s、期望速度v、期望末速度v_e、关节最大速度和关节最大加速度计算该路径速度、加速度最大值v_m、a_m；

S12、若期望末速度v_e不能到达，需要重新规划期望速度v＝v_e，加速度、匀速段和减速段的运动时间分别为t₁＝|v-v_s|/a_m、t₂＝0、t₃＝|v-v_e|/a_m；

S13、若期望末速度v_e能够到达，进而判断期望速度v能否到达；若期望速度速度v不能到达，需重重新规划期望速度轨迹不含匀速段，即匀速段的运动时间t₂＝0；若期望速度速度v能够到达，匀速段的运动时间

S14、计算机器人运动轨迹，完成摆线加减速规划；

所述空间任意两点位置过渡路径规划方法，已知两过渡点位置矢量p_a、p_b，两过渡点速度方向的单位矢量n_a、n_b，过渡段为两段抛物线，两过渡点所在过渡段的大小分别为λ₁、λ₂，两过渡点的速度大小相等，具体的空间任意两点位置过渡路径规划方法包括如下步骤：

S21、假设过渡段中间点矢量为p_m，等效为三段相邻直线之间的过渡，其中，中间段直线的两个顶点为p_m1＝p_a+λ₁·n_a和p_m2＝p_b-λ₂·n_b，则中间段直线的长度为λ₁₂＝λ₁′+λ₂′＝||p_m1p_m2||，λ₁′和λ₂′分别为p_m1和p_m2到中间点p_m的距离；

S22、求解过渡段中间点速度方向矢量n_m＝(p_m2-p_m1)/λ₁₂；

S23、确定过渡段两段抛物线的运动时间τ₁＝2λ₁/v，τ₂＝2λ₂/v；

S24、由步骤S21至S23求解的参数确定过渡段的运动轨迹；

所述空间任意两点姿态过渡轨迹规划方法，已知过渡段线速度大小为v，两过渡位置长度为l₁、l₂，两过渡点姿态为g_a、g_b，两过渡点姿态速度方向的单位矢量n_a、n_b，过渡段等效为单位球面上的两段抛物线，具体的空间任意两点姿态过渡轨迹规划方法包括如下步骤：

S31、计算两过渡点的姿态角速度大小ω₁、ω₂；

S32、确定两段抛物线的运动时间，由空间任意两点位置过渡规划方法确定，分别为t₁和t₂，t₁＝2l₁/v，t₂＝2l₂/v；

S33、将空间任意两点姿态的过渡等效为单位球面上的三段相邻直线段之间的过渡；计算姿态中间过渡段总长度λ₁₂，λ₁₂表示为单位球面上两姿态变换转动的最小距离，即四元数插值时沿球面旋转的最小路径，其等效转动轴即过渡段中间点方向矢量n_m；

S34、计算过渡段中间点的姿态矢量，g_m＝g_m1·Rot(n_m,λ₁′)，其中，λ₁′为中间过渡段前半段的长度，中间点角速度大小为

S35、由步骤S32得到的运动时间以及由步骤S 34得到的过渡段长度和中间点姿态，确定过渡段姿态运动轨迹。

进一步地，所述步骤S11中计算路径速度、加速度最大值的公式为：

式中，笛卡尔速度矢量雅可比矩阵关节速度向量H(θ)为机器人Hessian矩阵，是一个6层n×n矩阵，前三层为分别对x、y、z轴的移动，后三层分别为对x、y、z轴的转动，为关节加速度，a为笛卡儿加速度。

进一步地，所述步骤S14中计算机器人运动轨迹的公式为：

式中，p为在t时刻的位置，p_s为初始位置，v_s为初始速度，和为中间变量，其中加速段加速度减速段加速度a_m为加速度最大值。

进一步地，所述步骤S24中过渡段的运动轨迹的计算公式为：

式中，p(t)为t时刻机器人在笛卡儿空间中的位置矢量，p_a和p_m分别为过渡段起点和过渡段中间点的位置矢量，v₁和v_m分别为过渡段起点和过渡段中间点的速度矢量，v₁＝n_av，v_m＝n_mv_m，a₁和a₂分别为过渡段起点和过渡段终点的加速度矢量，a₁＝(v_m-v₁)/τ₁，a₂＝(v₂-v_m)/τ₂；τ₁和τ₂分别为过渡段两段抛物线的运动时间。

进一步地，所述步骤S31中计算两过渡点的姿态角速度大小的公式为：

式中，k为当前过渡点角速度大小ω和线速度大小v的比例， 为比例角，由以下公式确定：

式中，n为路径节点速度单位矢量，n_v和n_ω分别为线速度和角速度的单位矢量。

进一步地，所述步骤S33中计算姿态中间过渡段总长度的公式为：

λ₁₂＝|g_m1g_m2|＝slerp(g_m1,g_m2)

式中，slerp()表示单位四元数插值，g_m1和g_m2分别为中间过渡段两个顶点的姿态矢量，g_m1＝g_a·Rot(n_a,λ₁)，g_m2＝g_b·Rot(n_b,-λ₂)，Rot()表示沿单位球面旋转。

进一步地,所述步骤S35中确定过渡段姿态运动轨迹的公式为：

g_t＝g_a·Rot(n_a,σ₁)·Rot(n_m1,σ₂)

式中，g_t表示第一段抛物线在t时刻当前点的姿态，即由初始姿态沿初始方向轴n_a转动σ₁，然后再绕末方向轴n_m1转动σ₂，转动量的计算公式如下：

式中，t为当前时间，t∈[0,τ]，τ为本段抛物线总运动时间，λ₁和λ₁′分别为首过渡段和中间过渡段前半段的长度。

采用上述技术方案后，本发明至少具有如下有益效果：

1、本发明的摆线加减速规划方法，使用简化的正弦曲线或余弦曲线加减速模型，其完整的加减速过程分为加加速、减加速、匀速、加减速、减减速五个阶段，相对S曲线加减速，该方法具有加加速度连续、计算简单等优点；

2、本发明的空间任意两点位置过渡路径规划方法，适用于空间任意直线、圆弧之间的过渡路径规划，由过渡等级控制空间路径过渡区域的大小，空间任意两点间的过渡路径段具有连续抛物线特征，路经段之间的位置速度、加速度、加加速度保证连续；

3、本发明的空间任意两点姿态过渡路径规划方法，使用姿态采用四元数表示，两点之间的姿态运动表示为沿单位球表面最短距离的旋转；姿态规划以该旋转角度为路径的线性规划，与位置规划保证同步；任意路径段之间的姿态过渡保证姿态角速度、角加速度、角加加速度的连续；

4、本发明适用于多种构型机器人，解决了轨迹停顿、不流畅、偏离规定路径等问题，保证机器人位置、姿态规划的连续性，特别是姿态过渡点角速度无需为零，即姿态运动不停止，进一步提升了机器人运动效率及平稳性。

附图说明

图1为本发明一种机器人笛卡尔空间平滑轨迹的规划方法的实施流程图；

图2(a)是本发明一种机器人笛卡儿空间平滑轨迹的规划方法的过渡路径示例图；

图2(b)是本发明一种机器人笛卡儿空间平滑轨迹的规划方法的过渡路径示例图；

图3是本发明一种机器人笛卡儿空间平滑轨迹的规划方法的路径过渡情况判断流程图；

图4是本发明一种机器人笛卡儿空间平滑轨迹的规划方法的摆线加减速曲线图；

图5是本发明一种机器人笛卡儿空间平滑轨迹的规划方法的任意两点之间的位置过渡示意图；

图6是本发明一种机器人笛卡儿空间平滑轨迹的规划方法的任意两点之间的姿态平滑过渡示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互结合，下面结合附图和具体实施例对本申请作进一步详细说明。

如图1所示，本发明提供一种机器人笛卡尔空间平滑轨迹的规划方法，其主要实施内容包括摆线加减速规划、空间任意两点位置过渡轨迹规划以及空间任意两点姿态过渡轨迹规划，其中

利用所述的摆线加减速规划方法，对笛卡儿空间直线、圆弧等单段路径进行划，主要步骤为：

S11、计算该路径速度、加速度的最大值；

S12、判断期望末速度能否到达，若不能到达，则重新规划末速度；

S13、若期望末速度能够到达，进而判断期望速度能否到达，若期望速度速度v不能到达，需重新规划；

S14、计算机器人运动轨迹，完成摆线加减速规划。

利用所述空间任意两点位置过渡轨迹规划方法，对相邻两段空间路径进行位置过渡规划，主要步骤为：

S21、将过渡段等效为三段相邻直线之间的过渡，计算中间段直线的两个顶点位置，以及中间段直线的长度；

S22、求解过渡段中间点速度和方向矢量；

S23、确定过渡段两段抛物线的运动时间；

S24、确定过渡段的运动轨迹。

利用所述的空间任意两点姿态过渡轨迹规划方法，对相邻两段空间路径进行姿态过渡规划，主要步骤为：

S31、计算两过渡点的姿态角速度大小；

S32、确定两段抛物线的运动时间；

S33、将空间任意两点姿态的过渡等效为单位球面上的三段相邻直线段之间的过渡，计算姿态中间过渡段总长度；

S34、计算过渡段中间点的姿态矢量；

S35、确定过渡段姿态运动轨迹。

实施例

本发明的实施例具体流程：判断过渡区域——非过渡段规划——过渡段位置规划——过渡段姿态规划。

如图2(a)、图2(b)所示，本发明所涉及的路径过渡区域大小由过渡等级设定，当机器人运动到过渡区域时，将飞越当前路径点朝下一个路径点运动，如图中的p1点，若下一个路径点仍在过渡区域内，则直接越过下一个路径点，如图2(a)中的p2点。

如图3所示为路经过渡情况判断的具体流程，首先根据过渡等级确定各程序段的过渡距离，直线距离是其长度，圆弧距离则由圆弧的弧度确定。过渡情况判断结果分为完全过渡(如图2(a)中的路径段p₀p₁)、部分过渡(如图2(b)中的路径段p₁p₂)和全部过渡三种情况(如图2(a)中的路径段p₁p₂)。由此可将路径分为非过渡段和过渡段两种情况。

对于笛卡儿空间非过渡段，采用摆线加减速规划方法对直线、圆弧等路径进行单段轨迹规划，将单段路径分解成加速段、匀速段和减速段，并根据如图4所示的摆线加减速的加加速度、加速度、速度和位置曲线，对其轨迹进行求解。

本发明提供的空间任意两点位置过渡轨迹规划方法，可对空间任意两个具有不同速度方向的位置矢量进行平滑过渡规划，如图5所示，已知两过渡点位置矢量以及速度方向的单位矢量，将过渡段等效为两段抛物线，由已知的两过渡点所在过渡段的大小和速度大小，首先可计算中间段直线的两个顶点p_m1和p_m2，然后求解过渡段中间点速度v_m，再计算两段抛物线的运动时间，最终确定过渡段的运动轨迹。

姿态轨迹为单位球面上的两段抛物线。对于过渡段的首末节点，其速度方向已知，位置和姿态速度大小的比率已知。由于姿态保持与位置同步，若位置轨迹已定，则姿态过渡段的两段抛物线的运动时间可事先确定，然后可确定整个姿态过渡段的轨迹。如图6所示，已知两过渡点姿态g_a、g_b，以及两过渡点姿态速度方向的单位矢量n_a、n_b，同理，将过渡段等效为单位球面上的两段抛物线，两过渡点的姿态角速度和两段抛物线的运动时间可事先求得。利用本发明提供的空间任意两点姿态过渡规划方法，首先计算中间段直线的两个顶点g_m1和g_m2，然后计算过渡段中间点矢量g_m，最终确定过渡段姿态的运动轨迹。

本发明所提供的机器人笛卡尔空间平滑轨迹规划方法，适用于笛卡尔空间任意两点之间的平滑过渡，同时保证位置速度、加速度以及姿态角速度、角加速度等连续平滑过渡，可进一步提升机器人运动的效率和平稳性。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解的是，在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种等效的变化、修改、替换和变型，本发明的范围由所附权利要求及其等同范围限定。

Claims (7)

1.一种机器人笛卡尔空间平滑轨迹的规划方法，其特征在于，包括摆线加减速规划方法、空间任意两点位置过渡路径规划方法以及空间任意两点姿态过渡路经规划方法，其中

所述摆线加减速规划方法，其过程分为加速、匀速和减速三个阶段，加速阶段和减速阶段的轨迹形状为正则摆线，具体包括如下步骤：

S11、已知路径长度l、初始速度v_s、期望速度v、期望末速度v_e、关节最大速度和关节最大加速度计算该路径速度、加速度最大值v_m、a_m；

S12、若期望末速度v_e不能到达，需要重新规划期望速度v＝v_e，加速度、匀速段和减速段的运动时间分别为t₁＝|v-v_s|/a_m、t₂＝0、t₃＝|v-v_e|/a_m；

S13、若期望末速度v_e能够到达，进而判断期望速度v能否到达；若期望速度速度v不能到达，需重重新规划期望速度轨迹不含匀速段，即匀速段的运动时间t₂＝0；若期望速度速度v能够到达，匀速段的运动时间

S14、计算机器人运动轨迹，完成摆线加减速规划；

所述空间任意两点位置过渡路径规划方法，已知两过渡点位置矢量p_a、p_b，两过渡点速度方向的单位矢量n_a、n_b，过渡段为两段抛物线，两过渡点所在过渡段的大小分别为λ₁、λ₂，两过渡点的速度大小相等，具体的空间任意两点位置过渡路径规划方法包括如下步骤：

S21、假设过渡段中间点矢量为p_m，等效为三段相邻直线之间的过渡，其中，中间段直线的两个顶点为p_m1＝p_a+λ₁·n_a和p_m2＝p_b-λ₂·n_b，则中间段直线的长度为λ₁₂＝λ′₁+λ′₂＝||p_m1p_m2||，λ′₁和λ′₂分别为p_m1和p_m2到中间点p_m的距离；

S22、求解过渡段中间点速度方向矢量n_m＝(p_m2-p_m1)/λ₁₂；

S23、确定过渡段两段抛物线的运动时间τ₁＝2λ₁/v，τ₂＝2λ₂/v；

S24、由步骤S21至S23求解的参数确定过渡段的运动轨迹；

所述空间任意两点姿态过渡轨迹规划方法，已知过渡段线速度大小为v，两过渡位置长度为l₁、l₂，两过渡点姿态为g_a、g_b，两过渡点姿态速度方向的单位矢量n_a、n_b，过渡段等效为单位球面上的两段抛物线，具体的空间任意两点姿态过渡轨迹规划方法包括如下步骤：

S31、计算两过渡点的姿态角速度大小ω₁、ω₂；

S32、确定两段抛物线的运动时间，由空间任意两点位置过渡规划方法确定，分别为t₁和t₂，t₁＝2l₁/v，t₂＝2l₂/v；

S33、将空间任意两点姿态的过渡等效为单位球面上的三段相邻直线段之间的过渡；计算姿态中间过渡段总长度λ₁₂，λ₁₂表示为单位球面上两姿态变换转动的最小距离，即四元数插值时沿球面旋转的最小路径，其等效转动轴即过渡段中间点方向矢量n_m；

S34、计算过渡段中间点的姿态矢量，g_m＝g_m1·Rot(n_m,λ′₁)，其中，λ′₁为中间过渡段前半段的长度，中间点角速度大小为g_m1＝g_a·Rot(n_a,λ₁)，Rot()表示沿单位球面旋转；

S35、由步骤S32得到的运动时间以及由步骤S34得到的过渡段长度和中间点姿态，确定过渡段姿态运动轨迹。

2.根据权利要求1所述的一种机器人笛卡尔空间平滑轨迹的规划方法，其特征在于，所述步骤S11中计算路径速度、加速度最大值的公式为：

式中，笛卡尔速度矢量雅可比矩阵关节速度向量H(θ)为机器人Hessian矩阵，是一个6层n×n矩阵，前三层为分别对x、y、z轴的移动，后三层分别为对x、y、z轴的转动，为关节加速度，a为笛卡儿加速度。

3.根据权利要求1所述的一种机器人笛卡尔空间平滑轨迹的规划方法，其特征在于，所述步骤S14中计算机器人运动轨迹的公式为：

式中，p为在t时刻的位置，p_s为初始位置，v_s为初始速度，和为中间变量，其中加速段加速度减速段加速度a_m为加速度最大值。

4.根据权利要求1所述的一种机器人笛卡尔空间平滑轨迹的规划方法，其特征在于，所述步骤S24中过渡段的运动轨迹的计算公式为：

式中，p(t)为t时刻机器人在笛卡儿空间中的位置矢量，p_a和p_m分别为过渡段起点和过渡段中间点的位置矢量，v₁和v_m分别为过渡段起点和过渡段中间点的速度矢量，v₁＝n_av，v_m＝n_mv_m，a₁和a₂分别为过渡段起点和过渡段终点的加速度矢量，a₁＝(v_m-v₁)/τ₁，a₂＝(v₂-v_m)/τ₂；τ₁和τ₂分别为过渡段两段抛物线的运动时间。

5.根据权利要求4所述的一种机器人笛卡尔空间平滑轨迹的规划方法，其特征在于，所述步骤S31中计算两过渡点的姿态角速度大小的公式为：

式中，k为当前过渡点角速度大小ω和线速度大小v的比例，θ_vω为比例角，θ_vω由以下公式确定：

式中，n为路径节点速度单位矢量，n_v和n_ω分别为线速度和角速度的单位矢量。

6.根据权利要求1所述的一种机器人笛卡尔空间平滑轨迹的规划方法，其特征在于，所述步骤S33中计算姿态中间过渡段总长度的公式为：

λ₁₂＝|g_m1g_m2|＝slerp(g_m1,g_m2)

式中，slerp()表示单位四元数插值，g_m1和g_m2分别为中间过渡段两个顶点的姿态矢量，g_m1＝g_a·Rot(n_a,λ₁)，g_m2＝g_b·Rot(n_b,-λ₂)，Rot()表示沿单位球面旋转。

7.根据权利要求1所述的一种机器人笛卡尔空间平滑轨迹的规划方法，其特征在于,所述步骤S35中确定过渡段姿态运动轨迹的公式为：

g_t＝g_a·Rot(n_a,σ₁)·Rot(n_m1,σ₂)

式中，g_t表示第一段抛物线在t时刻当前点的姿态，即由初始姿态沿初始方向轴n_a转动σ₁，然后再绕末方向轴n_m1转动σ₂，转动量的计算公式如下：

式中，t为当前时间，t∈[0,τ]，τ为本段抛物线总运动时间，λ₁和λ′₁分别为首过渡段和中间过渡段前半段的长度。