CN115179298A - 一种笛卡尔空间的轨迹规划方法及装置 - Google Patents

Abstract

本发明提供了一种笛卡尔空间的轨迹规划方法及装置，其技术方案包括：获取多轴设备末端的若干个路径节点的位姿，其中，位姿包括笛卡尔位置空间的位置坐标和笛卡尔姿态空间的姿态坐标，其中，姿态坐标用三维的连续欧拉角表示；对路径节点的位姿进行路径拟合，获得多轴设备末端的笛卡尔空间的规划路径；根据规划路径的各路径点的位姿，获得笛卡尔空间的规划轨迹。本发明的实施例的技术方案对姿态坐标采用连续取值的连续欧拉角表示，解决了常规欧拉角在插补时的奇异点问题和四元数在插补时姿态不连续问题，实现了笛卡尔空间基于位姿的连续规划。

Description

一种笛卡尔空间的轨迹规划方法及装置

技术领域

本发明涉及运动控制相关技术领域，尤其涉及一种笛卡尔空间的轨迹规划方法及装置。

背景技术

机器人为一种多轴设备，机械人末端(也称为执行端)的姿态最直观的表示方法是欧拉角，但是在械人末端轨迹规划时因为欧拉角存在奇异点，不适合进行插补，且无法实现其中姿态的规划。

现有技术把欧拉角在限定范围内只用于姿态规划，对于包含多段的姿态规划的轨迹规划，则把欧拉角转换为四元数，再基于四元数进行姿态插补，从而规划机械人末端的姿态，解决了欧拉角奇异点的影响。

但是因为基于四元数的轨迹插补方法本质在一个球面上进行，导致快速变化的节点前后的姿态可能是不连续的。

发明内容

有鉴于此，本发明实施例提供了一种笛卡尔空间的轨迹规划方法及装置，其技术方案包括：获取多轴设备末端的若干个路径节点的位姿，其中，位姿的姿态坐标，其中，所述姿态坐标用三维的连续欧拉角表示；对所述路径节点的所述位姿进行路径拟合，获得多轴设备末端的笛卡尔空间的规划路径；根据所述规划路径的各路径点的所述位姿，获得笛卡尔空间的规划轨迹。本发明的实施例的技术方案对姿态坐标采用连续取值的连续欧拉角表示，解决了常规欧拉角在插补时的奇异点问题和四元数在插补时姿态不连续问题，实现了笛卡尔空间基于位姿的连续规划。

第一方面，本发明实施例提供了一种笛卡尔空间的轨迹规划方法，包括：获取多轴设备末端的若干个路径节点的位姿，所述位姿包括笛卡尔位置空间的位置坐标和笛卡尔姿态空间的姿态坐标，其中，所述姿态坐标用三维的连续欧拉角表示，当多轴设备末端从第i个路径节点运动到第i+1个路径节点时，第i+1个路径节点的第j维的所述连续欧拉角等于第i个路径节点的第j维的所述连续欧拉角与在该运动过程中第j维的欧拉角的旋转角度之和该旋转角度是正向旋转的角度；利用所述位姿对所述路径节点进行平滑拟合，获得多轴设备末端的笛卡尔空间的规划路径；根据所述规划路径的各路径点的所述位姿，获得笛卡尔空间的规划轨迹。

由上，对姿态坐标采用连续取值的连续欧拉角表示，解决了常规欧拉角在插补时的奇异点问题和四元数在插补时姿态不连续问题，实现了笛卡尔空间基于位姿的连续规划。

在第一方面的一种可能实施方式中，所述根据所述各路径点的所述位置长度和所述角度长度，获得笛卡尔空间的规划轨迹，包括：由所述路径点的所述位置长度和所述角度长度组成所述路径点的路程空间坐标，根据所述路程空间坐标对所述路径点进行路程空间轨迹插补，获得所述路程空间的路程轨迹；根据所述路程空间的路程轨迹获得所述规划轨迹。

由上，把各规划路径点在笛卡尔空间的6维度位姿转换为在路程空间的2维度路程坐标，不仅解决欧拉角的奇异点的问题和四元数在插补时姿态不连续问题，而且降低了规划计算的复杂度，提高了规划效果。

在第一方面的一种可能实施方式中，所述根据所述路程空间坐标对所述路径点进行路程空间轨迹插补，获得所述路程空间的路程轨迹，包括：根据多轴设备末端在笛卡尔空间的运动学约束条件，获得多轴设备末端在所述路程空间的运动学约束条件；在所述路程空间的运动学约束条件下，根据所述路程空间坐标对所述路径点进行路程空间轨迹插补，获得所述路程空间的路程轨迹，各路程轨迹点的速度、加速度和加加速度连续。

由上，通过路程轨迹的规划遵从路程空间的运动学约束条件和路程轨迹点的速度、加速度和加加速度连续，实现笛卡尔空间的规划轨迹遵从笛卡尔空间的运动学约束条件和规划轨迹点的速度、加速度和加加速度连续。

在第一方面的一种可能实施方式中，所述根据所述路程空间坐标对所述路径点进行路程空间轨迹插补，具体包括：根据所述路程空间的所述路程空间坐标，对所述路径点的所述路程空间的所述位置长度维度的轨迹和所述位置长度维度的轨迹进行同步插补。

由上，通过路程空间的位置长度维度的轨迹和位置长度维度的轨迹的规划同步，实现笛卡尔空间的规划轨迹在位姿的6个维度上轨迹同步。

在第一方面的一种可能实施方式中，所述根据所述路程空间的路程轨迹获笛卡尔空间的规划轨迹，包括：根据所述路程空间的路程轨迹的各路程轨迹点的所述位置长度和所述角度长度，获得所述各路程轨迹点的所述位姿；把所述各路程轨迹点作为笛卡尔空间的规划轨迹点，构成所述规划轨迹。

由上，根据路程轨迹的各路程轨迹点的位置长度和角度长度，结合路程轨迹趋势获得各路程轨迹点的位姿，可以通过搜索法实现路程空间与笛卡尔空间的坐标转换。

在第一方面的一种可能实施方式中，所述利用所述位姿对所述路径节点进行平滑拟合，至少包括下列之一：根据所述位姿利用线段和圆弧对所述路径节点进行平滑拟合；根据所述位姿利用B样条或多项式对所述路径节点进行平滑拟合。在一些实施例中，对规则路径选用线段和圆弧进行规划，降低计算量，对不规则路径采用B样条或多项式进行规划，提高精度。在另一些实施例中，对精度要求高的场景采用B样条或多项式进行规划，在满足精度要求时要平衡计算量的场景选用线段和圆弧进行规划。

由上，通过线段和圆弧对所述路径节点或利用B样条或多项式对所述路径节点进行平滑拟合，实现规划路径的平滑和满足位置精度要求。

在第一方面的一种可能实施方式中，一种笛卡尔空间的轨迹规划方法还包括：根据所述规划轨迹点的位姿，获得各轨迹节点，所述轨迹节点为所述规划轨迹上在笛卡尔空间的曲率大于设定阈值的规划轨迹点；对所述轨迹节点的所述位姿进行运动学逆解，获得关节节点在所述多轴设备的关节空间的关节坐标，所述关节节点为所述轨迹节点在所述关节空间对应的点；在各轴的运动学约束条件下，根据所述节坐标，同步规划所述多轴设备各轴的关节轨迹。

由上，把轨迹节点的位姿转换为多轴设备的关节空间的关节坐标，并据此同步规划多轴设备各轴的关节轨迹，实现多轴设备的各轴运动满足各轴的运动同步，且满足运动学约束条件，同时各关节轨迹的点在关节空间速度、加速度和加加速度，各轴运动连续。

第二方面，本发明实施例提供了一种多轴设备的关节空间轨迹的规划装置，包括：获取模块、路径规划模块和笛卡尔轨迹规划模块；所述获取模块用于获取多轴设备末端的若干个路径节点的位姿，所述位姿包括笛卡尔位置空间的位置坐标和笛卡尔姿态空间的姿态坐标，其中，所述姿态坐标用三维的连续欧拉角表示，当多轴设备末端从第i个路径节点运动到第i+1个路径节点时，第i+1个路径节点的第j维的所述连续欧拉角等于第i个路径节点的第j维的所述连续欧拉角与在该运动过程中第j维的欧拉角的旋转角度之和,该旋转角度是正向旋转的角度；所述路径规划模块用于利用所述位姿对所述路径节点进行平滑拟合，获得多轴设备末端的笛卡尔空间的规划路径；所述笛卡尔轨迹规划模块用于根据所述规划路径的各路径点的所述位姿，获得笛卡尔空间的规划轨迹。

由上，对姿态坐标采用连续取值的连续欧拉角表示，解决了常规欧拉角在插补时的奇异点问题和四元数在插补时姿态不连续问题，实现了笛卡尔空间基于位姿的连续规划。

在第二方面的一种可能实施方式中，所述笛卡尔轨迹规划模块具体用于，包括：由所述路径点的所述位置长度和所述角度长度组成所述路径点的路程空间坐标，根据所述路程空间坐标对所述路径点进行路程空间轨迹插补，获得所述路程空间的路程轨迹；根据所述路程空间的路程轨迹获得所述规划轨迹。

由上，把各规划路径点在笛卡尔空间的6维度位姿转换为在路程空间的2维度路程坐标，不仅解决欧拉角的奇异点的问题和四元数在插补时姿态不连续问题，而且规划计算的复杂度，提高了规划效果。

在第二方面的一种可能实施方式中，所述笛卡尔轨迹规划模块在根据路程空间坐标对路径点进行路程空间轨迹插补获得所述路程空间的路程轨迹时具体用于，包括：根据多轴设备末端在笛卡尔空间的运动学约束条件，获得多轴设备末端在所述路程空间的运动学约束条件；在所述路程空间的运动学约束条件下，根据所述路程空间坐标对所述路径点进行路程空间轨迹插补，获得所述路程空间的路程轨迹，各路程轨迹点的速度、加速度和加加速度连续。

由上，通过路程轨迹的规划遵从路程空间的运动学约束条件和路程轨迹点的速度、加速度和加加速度连续，实现笛卡尔空间的规划轨迹遵从笛卡尔空间的运动学约束条件和规划轨迹点的速度、加速度和加加速度连续。

在第二方面的一种可能实施方式中，所述笛卡尔轨迹规划模块在根据所述路程空间坐标对路径点进行路程空间轨迹插补时具体用于，包括：根据所述路程空间的所述路程空间坐标，对所述路径点的所述路程空间的所述位置长度维度的轨迹和所述位置长度维度的轨迹进行同步插补。

由上，通过路程空间的位置长度维度的轨迹和位置长度维度的轨迹的规划同步，实现笛卡尔空间的规划轨迹在位姿的6个维度上轨迹同步。

在第二方面的一种可能实施方式中，所述笛卡尔轨迹规划模块在根据路程空间的路程轨迹获得笛卡尔空间的规划轨迹时，具体用于，包括：根据所述路程空间的路程轨迹的各路程轨迹点的所述位置长度和所述角度长度，获得所述各路程轨迹点的所述位姿；把所述各路程轨迹点作为笛卡尔空间的规划轨迹点，构成所述规划轨迹。

由上，根据路程轨迹的各路程轨迹点的位置长度和角度长度，结合路程轨迹趋势获得各路程轨迹点的位姿，可以通过搜索法实现路程空间与笛卡尔空间的坐标转换。

在第二方面的一种可能实施方式中，所述路径规划模块在利用所述位姿对所述路径节点进行平滑拟合时，至少包括下列之一：根据所述位姿利用线段和圆弧对所述路径节点进行平滑拟合；根据所述位姿利用B样条或多项式对所述路径节点进行平滑拟合。在一些实施例中，对规则路径选用线段和圆弧进行规划，降低计算量，对不规则路径采用B样条或多项式进行规划，提高精度。在另一些实施例中，对精度要求高的场景采用B样条或多项式进行规划，在满足精度要求时要平衡计算量的场景选用线段和圆弧进行规划。由上，通过线段和圆弧对所述路径节点或利用B样条或多项式对所述路径节点进行平滑拟合，实现规划路径的平滑和满足位置精度要求。

在第二方面的一种可能实施方式中，一种笛卡尔空间的轨迹规划装置还包括：运动逆解模块，用于根据所述规划轨迹点的位姿，获得各轨迹节点，所述轨迹节点为所述规划轨迹上在笛卡尔空间的曲率大于设定阈值的规划轨迹点，并对所述轨迹节点的所述位姿进行运动学逆解，获得关节节点在所述多轴设备的关节空间的关节坐标，所述关节节点为所述轨迹节点在所述关节空间对应的点；关节轨迹规划模块，用于在各轴的运动学约束条件下，根据所述节坐标同步规划所述多轴设备各轴的关节轨迹。

由上，把轨迹节点的位姿转换为多轴设备的关节空间的关节坐标，并据此同步规划多轴设备各轴的关节轨迹，实现多轴设备的各轴运动满足各轴的运动同步，且满足运动学约束条件，同时各关节轨迹的点在关节空间速度、加速度和加加速度，各轴运动连续。

第三方面，本发明实施例提供了一种计算设备，包括，

总线；

通信接口，其与所述总线连接；

至少一个处理器，其与所述总线连接；以及

至少一个存储器，其与所述总线连接并存储有程序指令，所述程序指令当被所述至少一个处理器执行时使得所述至少一个处理器执行本发明第一方面任一所述实施方式。

第四方面，本发明实施例提供了一种计算机可读存储介质，其上存储有程序指令，所述程序指令当被计算机执行时使得所述计算机执行申请第一方面任一所述实施方式。

附图说明

图1为本发明的一种笛卡尔空间的轨迹规划方法实施例一的流程示意图；

图2为本发明的一种笛卡尔空间的轨迹规划方法实施例二的流程示意图；

图3A为本发明的一种笛卡尔空间的轨迹规划方法实施例三的流程示意图；

图3B为本发明的一种笛卡尔空间的轨迹规划方法实施例三的关节空间规划方法的流程示意图；

图4为本发明的一种笛卡尔空间的轨迹规划装置实施例一的结构示意图；

图5为本发明的一种笛卡尔空间的轨迹规划装置实施例二的结构示意图；

图6为本发明的一种笛卡尔空间的轨迹规划装置实施例三的结构示意图；

图7为本发明各实施例的一种计算设备的结构示意图。

具体实施方式

在以下的描述中，涉及到“一些实施例”，其描述了所有可能实施例的子集，但是可以理解，“一些实施例”可以是所有可能实施例的相同子集或不同子集，并且可以在不冲突的情况下相互结合。

在以下的描述中，所涉及的术语“第一\第二\第三等”或模块A、模块B、模块C等，仅用于区别类似的对象，或用于区别不同的实施例，不代表针对对象的特定排序，可以理解地，在允许的情况下可以互换特定的顺序或先后次序，以使这里描述的本发明实施例能够以除了在这里图示或描述的以外的顺序实施。

在以下的描述中，所涉及的表示步骤的标号，如S110、S120……等，并不表示一定会按此步骤执行，在允许的情况下可以互换前后步骤的顺序，或同时执行。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述本发明实施例的目的，不是旨在限制本发明。

笛卡尔空间、笛卡尔空间坐标：笛卡尔空间为直角坐标空间，笛卡尔空间的坐标为位姿，包括位置坐标和姿态坐标，位置坐标为三维直角坐标，姿态坐标为物体当前位置相对于三维直角坐标的旋转角度。

速度规划器：运动物体的轨迹和速度规划的装置，通过轨迹插补法规划物体的速度和轨迹，在速度规划时保持各点的速度连续和加速度连续，且服从运动学约束条件。速度规划器既可以对笛卡尔空间的轨迹进行规划，也可以对其他空间的轨迹进行规划。规划时把速度描述为时间的表达式，例如以时间为基的多项式。

本发明的各实施例对姿态坐标采用连续取值的连续欧拉角表示，解决了常规欧拉角在插补时的奇异点问题和四元数在插补时姿态不连续问题，实现了笛卡尔空间基于位姿的连续规划。

下面结合附图介绍本发明的一种笛卡尔空间的轨迹规划方法实施例、装置实施例和其他相关实施例。

本发明中多轴设备具有多个轴，可以为机械人、机床。其轨迹节点为多轴设备末端在笛卡尔空间的若干点；多轴设备末端可以为多轴设备对外操作部位，如机械人的手指、机床的刀具等；多轴设备的关节节点为多轴设备末端到达轨迹节点时多轴设备的各轴在其关节空间中对应的点。

下面首先结合附图1至图3B介绍一种笛卡尔空间的轨迹规划方法各规划方法实施例。

一种笛卡尔空间的轨迹规划方法实施例一

图1示出了一种笛卡尔空间的轨迹规划方法实施例一的流程，包括步骤S110至S130。

S110：获取多轴设备末端的若干个路径节点的位姿，所述位姿包括笛卡尔空间的三维位置坐标和三维姿态坐标。

其中，路径节点为多轴设备末端的移动过程中关键点。

其中，所述位姿包括笛卡尔空间的位置坐标和姿态坐标，姿态坐标用三维的连续欧拉角表示。

其中以第i个路径节点i的第j维连续欧拉角为例说明连续欧拉角的定义，第一个路径节点的所述连续欧拉角等于第一个路径节点的欧拉角，当多轴设备末端从第i个路径节点运动到第i+1个路径节点时，第i+1个路径节点的第j维的连续欧拉角等于第i个路径节点的第j维的连续欧拉角与在该运动过程中第j维的欧拉角的旋转角度之和。该旋转角度始终是正向旋转的角度，普通的欧拉角的第一个角和第三个角顺时针方向为正向，第二个角逆时针方向为正向。

其中，普通的欧拉角的三个角范围分别是：第一个角为0到2π，第二个角为0到π，第三个角为0到2π。每个欧拉角增大到最大值时，如果继续增大则跳变到最小值再继续增大；每个欧拉角减小到最小值时，如果继续减小则跳变到最大值再继续减小。例如，以欧拉角的第一种取值范围中第二个角为例，当其初始位置为

时，再顺时针旋转π，传统的方法其从

变化到π，然后跳变到0再变化到

采用连续欧拉角坐标的方法其从

直接变化到

取值连续。

S120：根据各路径节点的位姿对各路径节点进行平滑拟合，获得多轴设备末端的笛卡尔空间的规划路径。

其中，路径的平滑拟合不包括速度规划，为实现路径节点在笛卡尔空间中规划路径几何平滑，确定各路径点在笛卡尔空空间的位置与姿态。

其中，在路径规划过程中可以采用经过点与不经过点的两种路径拟合算法。不经过点的路径规划的较好方法为过渡点采用直线线段+圆弧的方法规划，经过点的较好方法采用B样条函数或多项式的方法规划。在一些实施例中，对规则路径选用线段和圆弧进行规划，降低计算量，对不规则路径采用B样条或多项式进行规划，提高精度。在另一些实施例中，对精度要求高的场景采用B样条或多项式进行规划，在满足精度要求时要平衡计算量的场景选用线段和圆弧进行规划。其中，路径规划获得的各路径点的位姿是对多轴设备末端的运动位置进行栅格化，可以认为是对多轴设备末端的运动位置进行全局规划，无论是通过不经过点或经过点都实现了多轴设备末端的规划路径满足平滑与精度的要求。

由上，采用连续欧拉角坐标的方式解决了传统欧拉角不连续的问题，便于进行拟合，在路径的平滑拟合通过插入路径点的方法实现姿态取值的连续。

S130：根据各规划路径点的位姿，获得多轴设备末端在笛卡尔空间的规划轨迹。

其中，规划轨迹上各点的位姿与步骤S120中规划路径上相应路径点的位姿相同，但本步骤通过速度规划获得了各路径点的位姿与时间的关系，也就是各路径点增加了时间属性，为了区别，把增加了时间属性的规划路径称为规划轨迹。

其中，在一些实施例中，根据各规划路径点的位姿，利用速度规划器分别且同步规划多轴设备末端在笛卡尔位置空间的位置轨迹和在笛卡尔位置空间的姿态轨迹，该位置轨迹与姿态轨迹组成多轴设备末端在笛卡尔空间的规划轨迹。

由上，采用连续欧拉角坐标的方式解决了传统欧拉角不连续的问题，便于在笛卡尔姿态空间对姿态进行插补，解决欧拉角的奇异点的问题和四元数在插补时姿态不连续问题。

其中，在另一些实施例中，把各规划路径点的位姿转换为路程空间的坐标，根据路程空间的坐标规划多轴设备末端在路程空间的路程轨迹，再把路程轨迹转换为笛卡尔空间的规划轨迹。

由上，采用连续欧拉角坐标的方式解决了传统欧拉角不连续的问题，便于计算笛卡尔姿态空间的长度，该长度可用于姿态规划，解决欧拉角的奇异点的问题和四元数在插补时姿态不连续问题。

综上，一种笛卡尔空间的轨迹规划方法实施例一获取多轴设备末端的若干个路径节点的位姿，对其中姿态坐标采用连续取值的连续欧拉角表示；对路径节点的位姿进行路径拟合，获得多轴设备末端的笛卡尔空间的规划路径；根据规划路径的各路径点的位姿，获得笛卡尔空间的规划轨迹。本方法实施例通过对姿态坐标采用连续取值的连续欧拉角表示，解决了常规欧拉角在插补时的奇异点问题和四元数在插补时姿态不连续问题，实现了笛卡尔空间基于位姿的连续规划。

一种笛卡尔空间的轨迹规划方法实施例二把各规划路径点的位姿转换为路程空间的坐标，根据路程空间的坐标规划多轴设备末端在路程空间的路程轨迹，再把路程轨迹转换为笛卡尔空间的规划轨迹。本方法实施例把各规划路径点在笛卡尔空间的6维度位姿转换为在路程空间的2维度路程坐标，不仅解决欧拉角的奇异点的问题和四元数在插补时姿态不连续问题，而且规划计算的复杂度，提高了规划效果。

图2示出了一种笛卡尔空间的轨迹规划方法实施例二的流程，包括步骤S210至S250。

S210：获取多轴设备末端的若干个路径节点的位姿，所述位姿包括笛卡尔空间的三维位置坐标和三维姿态坐标。

其中，本步骤的具体方法和优点请参照方法实施例一的步骤S110。

S220：根据各路径节点的位姿对各路径节点进行平滑拟合，获得多轴设备末端的笛卡尔空间的规划路径。

其中，本步骤的具体方法和优点请参照方法实施例一的步骤S120。

S230：根据笛卡尔空间的规划路径的位姿，获得规划路径上各路径点的位置长度和角度长度，并组成各路径点的路程空间坐标。

其中，路径点包括路径节点和步骤S220中的插入的路径点。以第i个路径点为例说明各路径点的位置长度和角度长度的定义，第i个路径点的位置长度为从第一个路径点顺序运动到第i个路径点所经过的规划路径在笛卡尔位置空间中的长度，第i个路径点的所述角度长度为从第一个路径点顺序运动到第i个路径点所经过的规划路径在笛卡尔姿态空间中的长度，可以得到第i+1个路径点与第i个路径点之间的位置长度为笛卡尔位置空间中第i+1个路径点与第i个路径点之间的距离，第i+1个路径点与第i个路径点之间的角度长度为笛卡尔姿态空间中第i+1个路径点与第i个路径点之间的距离。

其中，路程空间包括平动坐标和转动坐标，一个路径点的平动坐标为该路径点的位置长度，一个路径点的转动坐标为该路径点的角度长度。

由上，因为使用了连续欧拉角表示姿态，计算的角度长度即转动坐标也是连续的，可用于转动坐标连续插补。

S240：根据各路径点的路程空间坐标，利用速度规划器对各路径点进行路程空间轨迹插补，获得路程空间的路程轨迹。

其中，在利用速度规划器对各路径点进行路程空间轨迹插补时，根据多轴设备末端在笛卡尔空间的运动学约束条件，获得多轴设备末端在路程空间的运动学约束条件，并在该路程空间的运动学约束条件下，进行对各路径点进行路程空间轨迹插补。

其中，在利用速度规划器对各路径点进行路程空间轨迹插补时，根据路程空间的路程空间坐标，对路径点的在路程空间中的平动维度的轨迹和转动维度的轨迹进行同步插补。该同步为到达一个路径点在路程空间中对应的平动维度的轨迹点和转动维度的轨迹点的时间相同。

其中，各插补点的速度通过速度规划器进行前瞻速度规划，且路程轨迹点在路程空间中各路程坐标维度的速度、加速度和加加速度的连续。

其中，速度规划器在轨迹插补时对路程空间的平动坐标和转动坐标同步插补，采用速度规划器的速度曲线包括S形曲线或其他形状的曲线，速度与时间的关系包括以时间为基函数的多项式或以时间余弦为基函数的表达式。

由上，因为路程空间的转动坐标也为连续值，在进行路程空间的轨迹规划时，实现了转动坐标的连续规划，相当于实现笛卡尔空间的姿态连续规划，解决了欧拉角存在奇异点而不适合进行插补的问题，避免了基于四元数的轨迹插补方法的姿态不连续问题。

S250：把路程空间的各路程轨迹点的坐标转换为各路程轨迹点在笛卡尔空间的位姿，并把各路程轨迹点作为笛卡尔空间的规划轨迹点，构成笛卡尔空间的规划轨迹。

其中，根据路程轨迹的运动趋势和路程空间的各路程轨迹点的坐标，采用搜索的方法把路程空间的各路程轨迹点的坐标转换为各路程轨迹点在笛卡尔空间的位姿。

其中，路程空间的路程轨迹点与笛卡尔空间的规划轨迹点一一对应。在利用速度规划器对各路径点进行路程空间轨迹插补时，路程轨迹点满足多轴设备末端路程空间的运动学约束条件,本质上就是笛卡尔空间的规划轨迹点满足在笛卡尔空间的运动学约束条件，路程轨迹点满足路程空间的速度、加速度和加加速度的连续本质上就是笛卡尔空间的规划轨迹点满足在笛卡尔空间的速度、加速度和加加速度的连续，路程轨迹在平动维度的轨迹点和转动维度的轨迹点上同步就是规划轨迹在位姿的6个维度上同步。

综上，一种笛卡尔空间的轨迹规划方法实施例二获取多轴设备末端的若干个路径节点的位姿，其中，姿态坐标用三维的连续欧拉角表示；对各路径节点的位姿进行路径拟合，获得多轴设备末端的笛卡尔空间的规划路径；根据规划路径的各路径点的位姿，获得各路径点的位置长度和角度长度；并据此获得笛卡尔空间的规划轨迹。本方法实施例把各规划路径点在笛卡尔空间的6维度位姿转换为在路程空间的2维度路程坐标，不仅解决欧拉角的奇异点的问题和四元数在插补时姿态不连续问题，而且规划计算的复杂度，提高了规划效果。

一种笛卡尔空间的轨迹规划方法实施例三利用一种笛卡尔空间的轨迹规划方法实施例二获得的笛卡尔空间的规划轨迹，获得多轴设备末端轨迹节点在关节空间的坐标，并据此在多轴设备的各轴的运动学约束条件同步规划多轴设备的在关节空间的关节轨迹。

下面结合附图3A和图3B介绍一种笛卡尔空间的轨迹规划方法实施例三。

图3A示出了一种笛卡尔空间的轨迹规划方法实施例三的流程，包括步骤S310至S350。

S310：获取多轴设备末端的若干个路径节点的位姿，所述位姿包括笛卡尔空间的三维位置坐标和三维姿态坐标。

其中，本步骤的具体方法和优点请参照方法实施例一的步骤S110。

S320：根据各路径节点的位姿对各路径节点进行平滑拟合，获得多轴设备末端的笛卡尔空间的规划路径。

其中，本步骤的具体方法和优点请参照方法实施例一的步骤S120。

S330：根据笛卡尔空间的规划路径的位姿，获得笛卡尔空间的规划轨迹。

其中，本步骤执行了方法实施例二的步骤S230、S240和S250，其主要方法和优点请参照方法实施例二的步骤S230、S240和S250。

S340：根据笛卡尔空间的规划轨迹获得多轴设备末端轨迹节点对应的关节节点在关节空间的关节坐标。

其中，根据笛卡尔空间的规划轨迹点的位姿，获得多轴设备末端在笛卡尔空间的轨迹节点，所述轨迹节点为在笛卡尔空间的规划轨迹上曲率大于设定阈值的规划轨迹点，这些点为规划轨迹上的关键点，决定规划轨迹的趋势。

其中，对轨迹节点的位姿进行运动学逆解，获得轨迹节点在多轴设备的关节节点的关节坐标。其中，关节坐标为关节空间的坐标，运动学逆解方法包括：解析法，以解析的方式求轨迹节点在各轴的关节空间对应的关节节点的坐标角度；迭代法，通过数值迭代法数值迭代求出方程的特解，获得轨迹节点在各轴的关节空间对应的关节节点的坐标；几何法，对一些结构比较简单的机器人通过几何关系获得轨迹节点在各轴的关节空间对应的关节节点的坐标。本发明对运动学逆解方法不予限制。

S350：在多轴设备各轴的运动学约束条件下，根据各关节节点的关节坐标，利用速度规划器同步规划多轴设备各轴的关节轨迹。

其中，每个轴可以看成是一个关节，对应一个关节空间的一维，每个轴可以平移轴或旋转轴。

其中，速度规划器在规划各轴在关节空间的关节轨迹时，采用速度规划器的速度曲线包括S形曲线或其他形状的曲线，速度与时间的关系包括以时间为基函数的多项式或以时间余弦为基函数的表达式。

其中，所述同步为多轴设备的各轴在关节空间中在相同时间到达每个关节节点。

其中，每个轴在关节空间的关节轨迹点满足该轴的运动学约束条件，且在关节空间实现其关节轨迹点的速度、加速度和加加速度连续。

其中，本步骤的具体方法请参照一种笛卡尔空间的轨迹规划方法实施例的关节空间轨迹规划方法的流程。

图3B示出了一种笛卡尔空间的轨迹规划方法实施例的关节空间轨迹规划方法的流程，包括：步骤S3510至S3540。

S3510：根据关节节点的关节坐标，利用速度规划器分别规划每个轴在关节空间的第一轨迹，获得每个轴到达各关节节点的规划时间。

其中，本步骤的速度规划器为一种笛卡尔空间的轨迹规划方法实施例二的步骤S350中描述的速度规划器，且各轴均在运动学约束条件下进行规划。

S3520：根据每个轴到达各关节节点的规划时间，获得每个轴到达各关节节点的同步时间。

其中，在一个关节节点对应的同步时间每个轴同步到达该轨迹节点的对应的关节节点。

其中，本步骤的一种可能的实施方式包括：

1)根据每个轴的各关节节点的规划时间，获得每个轴在各相邻关节节点之间运动的运行时长。

2)对于任一对相邻关节节点，把各轴在该对相邻关节节点之间的运行时长中最长的运行时长作为该对相邻关节节点之间运动的同步时长。

3)根据每对相邻关节节点的同步时长，依次确定多轴设备每个轴到达每个关节节点的同步时间。

S3530：根据每个轴到达每个关节节点的同步时间和每个关节节点的关节坐标，利用速度规划器分别规划每个轴在关节空间的第二轨迹。

其中，本步骤的速度规划器为一种笛卡尔空间的轨迹规划方法实施例二的步骤S350中描述的速度规划器，且在关节空间实现其关节轨迹点的速度、加速度和加加速度连续。

S3540：当一个轴的第二轨迹上存在速度、加速度或加加速度跳变的点时，对该点的速度、加速度或加加速度进行滤波，并根滤波结果优化相应的第二轨迹，获得各关节空间的关节轨迹。

综上，一种笛卡尔空间的轨迹规划方法实施例三利用一种笛卡尔空间的轨迹规划方法实施例二获得的笛卡尔空间的规划轨迹，获得多轴设备末端轨迹节点在关节空间的坐标，并据此在多轴设备的各轴的运动学约束条件同步规划多轴设备的各轴的关节轨迹，从而不仅实现了多轴设备的末端规划轨迹的平滑和精度，且满足多轴设备的末端的运动学约束条件，还满足了多轴设备各轴的运动学约束，且实现在各轴的速度、加速度和加加速度连续。

下面结合图4至图6介绍本发明的一种笛卡尔空间的轨迹规划装置实施例。

图4示出了一种笛卡尔空间的轨迹规划装置实施例一的结构，包括：获取模块410、路径规划模块420和笛卡尔轨迹规划模块430。

获取模块410用于获取多轴设备末端的若干个路径节点的位姿，所述位姿包括笛卡尔空间的三维位置坐标和三维姿态坐标。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例一的步骤S110。

路径规划模块420用于根据各路径节点的位姿对各路径节点进行平滑拟合，获得多轴设备末端的笛卡尔空间的规划路径。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例一的步骤S120。

笛卡尔轨迹规划模块430用于根据笛卡尔空间的规划路径的位姿，获得笛卡尔空间的规划轨迹。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例一的步骤S130。

图5示出了一种笛卡尔空间的轨迹规划装置实施例二的结构，包括：获取模块510、路径规划模块520和笛卡尔轨迹规划模块530。

获取模块510用于获取多轴设备末端的若干个路径节点的位姿，所述位姿包括笛卡尔空间的三维位置坐标和三维姿态坐标。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例二的步骤S210。

路径规划模块520用于根据各路径节点的位姿对各路径节点进行平滑拟合，获得多轴设备末端的笛卡尔空间的规划路径。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例二的步骤S220。

笛卡尔轨迹规划模块530用于根据笛卡尔空间的规划路径的位姿，获得规划路径上各路径点的位置长度和角度长度，并组成各路径点的路程空间坐标。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例二的步骤S230。

笛卡尔轨迹规划模块530还用于根据各路径点的路程空间坐标，利用速度规划器对各路径点进行路程空间轨迹插补，获得路程空间的路程轨迹。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例二的步骤S240。

笛卡尔轨迹规划模块530还用于把路程空间的各路程轨迹点的坐标转换为各路程轨迹点在笛卡尔空间的位姿，并把各路程轨迹点作为笛卡尔空间的规划轨迹点，构成笛卡尔空间的规划轨迹。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例二的步骤S250。

图6示出了一种笛卡尔空间的轨迹规划装置实施例三的结构，包括：获取模块610、路径规划模块620、笛卡尔轨迹规划模块630、运动学逆解模块640和关节轨迹规划模块650。

获取模块610用于获取多轴设备末端的若干个路径节点的位姿，所述位姿包括笛卡尔空间的三维位置坐标和三维姿态坐标。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例三的步骤S310。

路径规划模块620用于根据各路径节点的位姿对各路径节点进行平滑拟合，获得多轴设备末端的笛卡尔空间的规划路径。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例三的步骤S320。

笛卡尔轨迹规划模块630用于根据笛卡尔空间的规划路径的位姿，获得笛卡尔空间的规划轨迹。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例三的步骤S330。

运动学逆解模块640用于根据笛卡尔空间的规划轨迹获得多轴设备末端轨迹节点对应的关节节点在关节空间的关节坐标。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例三的步骤S340。

关节轨迹规划模块650用于在多轴设备各轴的运动学约束条件下，根据各关节节点的关节坐标，利用速度规划器同步规划多轴设备各轴的关节轨迹。其方法和优点请参照一种笛卡尔空间的轨迹规划方法实施例三的步骤S350。

本发明实施例还提供了一种计算设备，下面结合图7详细介绍。

该计算设备700包括，处理器710、存储器720、通信接口730、总线740。

应理解，该图所示的计算设备700中的通信接口730可以用于与其他设备之间进行通信。

其中，该处理器710可以与存储器720连接。该存储器720可以用于存储该程序代码和数据。因此，该存储器720可以是处理器710内部的存储单元，也可以是与处理器710独立的外部存储单元，还可以是包括处理器710内部的存储单元和与处理器710独立的外部存储单元的部件。

可选的，计算设备700还可以包括总线740。其中，存储器720、通信接口730可以通过总线740与处理器710连接。总线740可以是外设部件互连标准(Peripheral ComponentInterconnect，PCI)总线或扩展工业标准结构(EFStended Industry StandardArchitecture，EISA)总线等。所述总线740可以分为地址总线、数据总线、控制总线等。为便于表示，该图中仅用一条线表示，但并不表示仅有一根总线或一种类型的总线。

应理解，在本发明实施例中，该处理器710可以采用中央处理单元(centralprocessing unit，CPU)。该处理器还可以是其它通用处理器、数字信号处理器(digitalsignal processor，DSP)、专用集成电路(application specific integrated circuit，ASIC)、现成可编程门阵列(field programmable gate Array，FPGA)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。或者该处理器710采用一个或多个集成电路，用于执行相关程序，以实现本发明实施例所提供的技术方案。

该存储器720可以包括只读存储器和随机存取存储器，并向处理器710提供指令和数据。处理器710的一部分还可以包括非易失性随机存取存储器。例如，处理器710还可以存储设备类型的信息。

在计算设备700运行时，所述处理器710执行所述存储器720中的计算机执行指令执行各方法实施例的操作步骤。

应理解，根据本发明实施例的计算设备700可以对应于执行根据本发明各实施例的方法中的相应主体，并且计算设备700中的各个模块的上述和其它操作和/或功能分别为了实现本方法实施例各方法的相应流程，为了简洁，在此不再赘述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本发明所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本方法实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述译码方法的全部或部分步骤。而前述的存储介质包括，U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时用于执行各方法实施例的操作步骤。

本发明实施例的计算机存储介质，可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是，但不限于，电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括，具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。

计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括、但不限于无线、电线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络，包括局域网(LAN)或广域网(WAN)，连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，均属于本发明保护范畴。

Claims (10)

1.一种笛卡尔空间的轨迹规划方法，其特征在于，包括：

获取多轴设备末端的若干个路径节点的位姿，所述位姿包括笛卡尔位置空间的位置坐标和笛卡尔姿态空间的姿态坐标，其中，所述姿态坐标用三维的连续欧拉角表示，当多轴设备末端从第i个路径节点运动到第i+1个路径节点时，第i+1个路径节点的第j维的所述连续欧拉角等于第i个路径节点的第j维的所述连续欧拉角与在该运动过程中第j维的欧拉角的正向旋转角度之和；

利用所述位姿对所述路径节点进行平滑拟合，获得多轴设备末端的笛卡尔空间的规划路径；

根据所述规划路径的各路径点的所述位姿，获得笛卡尔空间的规划轨迹。

2.根据权利要求1所述方法，其特征在于，所述根据所述规划路径的各路径点的所述位姿，获得笛卡尔空间的规划轨迹，包括：

根据所述规划路径的各路径点的所述位姿，获得所述各路径点的位置长度和角度长度，一个路径点的所述位置长度为在笛卡尔位置空间中从第一个路径点到该路径点的所述规划路径的长度，一个路径点的所述角度长度为在笛卡尔姿态空间中从第一个路径点到该路径点的所述规划路径的长度；

由所述位置长度和所述角度长度组成路程空间的坐标，根据所述路程空间的坐标对所述路径点进行路程空间轨迹插补，获得所述路程空间的路程轨迹；

根据所述路程轨迹获得所述规划轨迹。

3.根据权利要求2所述方法，其特征在于，所述根据所述路程空间的坐标对所述路径点进行路程空间轨迹插补，获得所述路程空间的路程轨迹，包括：

根据多轴设备末端在笛卡尔空间的运动学约束条件，获得多轴设备末端在所述路程空间的运动学约束条件；

在所述路程空间的运动学约束条件下，根据所述路程空间的坐标对所述路径点进行路程空间轨迹插补，获得所述路程轨迹，各路程轨迹点的速度、加速度和加加速度连续。

4.根据权利要求3所述方法，其特征在于，所述根据所述路程空间的坐标对所述路径点进行路程空间轨迹插补，具体包括：

根据所述路程空间的坐标，对所述路径点在所述路程空间的所述位置长度维度的轨迹和所述姿态长度维度的轨迹进行同步插补。

5.根据权利要求2所述方法，其特征在于，所述根据所述路程轨迹获得所述规划轨迹，包括：

根据各路程轨迹点的所述位置长度和所述角度长度，获得所述各路程轨迹点的所述位姿；

把所述各路程轨迹点作为笛卡尔空间的规划轨迹点，构成所述规划轨迹。

6.根据权利要求1所述方法，其特征在于，所述利用所述位姿对所述路径节点进行平滑拟合，至少包括下列之一：

根据所述位姿利用线段和圆弧对所述路径节点进行平滑拟合；

根据所述位姿利用B样条或多项式对所述路径节点进行平滑拟合。

7.根据权利要求1所述方法，其特征在于，还包括：

根据所述规划轨迹点的位姿，获得各轨迹节点，所述轨迹节点为所述规划轨迹上在笛卡尔空间的曲率大于设定阈值的规划轨迹点；

对所述轨迹节点的所述位姿进行运动学逆解，获得关节节点在所述多轴设备的关节空间的关节坐标，所述关节节点为所述轨迹节点在所述关节空间对应的点；

在所述多轴设备各轴的运动学约束条件下，根据所述节坐标，同步规划所述多轴设备各轴的关节轨迹。

8.一种多轴设备的关节空间轨迹的规划装置，其特征在于，包括：获取模块、路径规划模块和笛卡尔轨迹规划模块；

所述获取模块用于获取多轴设备末端的若干个路径节点的位姿，所述位姿包括笛卡尔位置空间的位置坐标和笛卡尔姿态空间的姿态坐标，其中，所述姿态坐标用三维的连续欧拉角表示，当多轴设备末端从第i个路径节点运动到第i+1个路径节点时，第i+1个路径节点的第j维的所述连续欧拉角等于第i个路径节点的第j维的所述连续欧拉角与在该运动过程中第j维的欧拉角的旋转角度之和；

所述路径规划模块用于利用所述位姿对所述路径节点进行平滑拟合，获得多轴设备末端的笛卡尔空间的规划路径；

所述笛卡尔轨迹规划模块用于根据所述规划路径的各路径点的所述位姿，获得笛卡尔空间的规划轨迹。

9.一种计算设备，其特征在于，包括：

总线；

通信接口，其与所述总线连接；

至少一个处理器，其与所述总线连接；以及

至少一个存储器，其与所述总线连接并存储有程序指令，所述程序指令当被所述至少一个处理器执行时使得所述至少一个处理器执行权利要求1至7任一所述译码方法。

10.一种计算机可读存储介质，其上存储有程序指令，其特征在于，所述程序指令当被计算机执行时使得所述计算机执行权利要求1至7任一所述译码方法。

Images