基于实时运动的笛卡尔空间轨迹规划方法和系统
阅读说明：本技术 基于实时运动的笛卡尔空间轨迹规划方法和系统 (Real-time motion-based Cartesian space trajectory planning method and system ) 是由 廖志祥 郭震 于 2021-09-10 设计创作，主要内容包括：本发明提供了一种基于实时运动的笛卡尔空间轨迹规划方法和系统,包括：步骤1：获取笛卡尔空间中的路径点；步骤2：根据笛卡尔空间中路径点的个数规划,生成第一段轨迹规划器；步骤3：根据运动周期,通过第一段轨迹规划器生成带有时刻的位置信息,并下发至被控对象；步骤4：获取当前轨迹规划器的规划时间,除以运动周期后取整再乘以运动周期,得到时间基座,若当前轨迹不是最后一段轨迹且运动时间比时间基座少一个运动周期时,生成下一段轨迹规划器；步骤5：判断当前轨迹为最后一段轨迹,且运动时间等于轨迹规划器的规划时间时,完成轨迹规划。本发明采用圆弧过渡匀速规划的方式,避免了被控对象出现速度突跳现象,保证被控对象运动顺滑性。(The invention provides a Cartesian space trajectory planning method and a Cartesian space trajectory planning system based on real-time motion, which comprise the following steps of: step 1: acquiring path points in a Cartesian space; step 2: planning according to the number of path points in the Cartesian space to generate a first section of track planner; and step 3: generating position information with time through a first section of track planner according to the motion period, and issuing the position information to a controlled object; and 4, step 4: acquiring the planning time of the current trajectory planner, dividing the planning time by the movement period, rounding, and multiplying by the movement period to obtain a time base, and if the current trajectory is not the last trajectory and the movement time is less than the movement period of the time base, generating a next trajectory planner; and 5: and when the current track is judged to be the last section of track and the movement time is equal to the planning time of the track planner, finishing the track planning. The invention adopts a mode of circular arc transition constant speed planning, avoids the phenomenon of sudden speed jump of the controlled object and ensures the smoothness of the movement of the controlled object.)
技术领域
本发明涉及轨迹规划技术领域，具体地，涉及一种基于实时运动的笛卡尔空间轨迹规划方法和系统。
背景技术
大多数情况下，笛卡尔空间轨迹规划均是以直线路径为主，机械臂末端在不同路径点之间均以直线插补，机械臂末端运动至某位置时已减速为零，再加速运动至下一位置，并保证运动至下一位置时也减速为零，再不断重复上述动作。但对于路径点较多，且相邻路径点距离不大的情况，该轨迹规划方法存在较大问题，频繁的启停没有充分利用机械臂的工作能力，降低了机械臂工作效率，并且严重影响机械臂传动系统的寿命。
部分情况下，笛卡尔空间轨迹规划中的直线路径会加上圆弧过渡，并且将直线与圆弧的距离之和作为轨迹规划的总距离，再辅以运动学约束进行轨迹规划，由于机械臂在圆弧过渡时受到加速度约束影响较大，导致机械臂无法达到较大速度，浪费了机械臂的工作性能，降低其工作效率。
专利文献CN106313047B(申请号：CN201610860966.1)公开了一种基于Bezier样条的机器人实时拐角过渡方法，包括以下步骤：记录关节空间起始点，过渡点的坐标，笛卡尔空间目标点，过渡点的坐标，笛卡尔空间的过渡半径R及最大误差约束；计算笛卡尔空间过渡曲线起点和终点的坐标和关节空间过渡曲线起点的坐标；计算笛卡尔空间过渡曲线起点和终点速度方向单位矢量；计算笛卡尔卡尔空间过渡曲线中间控制点的坐标，并求解Bezier样条曲线的构造函数；最后进行速度规划和插补。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于实时运动的笛卡尔空间轨迹规划方法和系统。
根据本发明提供的基于实时运动的笛卡尔空间轨迹规划方法，包括：
步骤1：通过用户输入或自动识别，获取笛卡尔空间中的路径点；
步骤2：根据笛卡尔空间中路径点的个数进行直线规划和圆弧过渡匀速规划，生成第一段轨迹规划器；
步骤3：根据运动周期，通过第一段轨迹规划器生成带有时刻的位置信息，并下发至被控对象；
步骤4：获取当前轨迹规划器的规划时间，除以运动周期后取整再乘以运动周期，得到时间基座，若当前轨迹不是最后一段轨迹且运动时间比时间基座少一个运动周期时，生成下一段轨迹规划器；
步骤5：判断当前轨迹为最后一段轨迹，且运动时间等于轨迹规划器的规划时间时，完成轨迹规划。
优选的，所述步骤1包括：若相邻两个路径点的距离不在预设范围内，则返回无法规划信息；若相邻两个路径点的距离在预设范围内，则保存所有笛卡尔空间中路径点。
优选的，仅有两个笛卡尔空间路径点A点和B点时，采用直线规划，计算两路径点的直线距离，再设定笛卡尔空间最大线速度vmax和最大线加速度amax，利用梯形速度轨迹得到这两个路径点的全部轨迹，规划从A点运动至B点的轨迹P公式为：
其中，A点和B点速度均为零，OP为任意时刻下的轨迹点，OA为A点坐标，AB为A点和B点的线段向量，u为梯形速度轨迹规划器规划值。
优选的，笛卡尔空间路径点个数大于两个时，以第二个路径点为圆心作球面，求得球面与第一、二个路径点构成线段AB的交点，以及球面与第二、三个路径点构成线段BC的交点，根据圆弧段与线段AB和线段BC相切的条件计算得到圆弧的半径与B’C’圆弧的角度，其中，圆弧过渡采用匀速规划的方式，圆弧过渡的线速度为：
其中，r为球半径，点A和B’为第一段轨迹规划器的路径点，A点时速度为零，B’点速度为vlim。
优选的，所述步骤4包括：
若下一段路径为圆弧，则求得圆弧半径、角度和圆弧过渡的限制速度，以上一段轨迹规划器中规划时间为时间基座时的位置为下一段路径轨迹规划器的起始位置；
若下一段路径为直线，则获取下一段轨迹的终止点和限制速度，再生成笛卡尔空间直线轨迹规划器。
根据本发明提供的基于实时运动的笛卡尔空间轨迹规划系统，包括：
模块M1：通过用户输入或自动识别，获取笛卡尔空间中的路径点；
模块M2：根据笛卡尔空间中路径点的个数进行直线规划和圆弧过渡匀速规划，生成第一段轨迹规划器；
模块M3：根据运动周期，通过第一段轨迹规划器生成带有时刻的位置信息，并下发至被控对象；
模块M4：获取当前轨迹规划器的规划时间，除以运动周期后取整再乘以运动周期，得到时间基座，若当前轨迹不是最后一段轨迹且运动时间比时间基座少一个运动周期时，生成下一段轨迹规划器；
模块M5：判断当前轨迹为最后一段轨迹，且运动时间等于轨迹规划器的规划时间时，完成轨迹规划。
优选的，所述模块M1包括：若相邻两个路径点的距离不在预设范围内，则返回无法规划信息；若相邻两个路径点的距离在预设范围内，则保存所有笛卡尔空间中路径点。
优选的，仅有两个笛卡尔空间路径点A点和B点时，采用直线规划，计算两路径点的直线距离，再设定笛卡尔空间最大线速度vmax和最大线加速度amax，利用梯形速度轨迹得到这两个路径点的全部轨迹，规划从A点运动至B点的轨迹P公式为：
其中，A点和B点速度均为零，OP为任意时刻下的轨迹点，OA为A点坐标，AB为A点和B点的线段向量，u为梯形速度轨迹规划器规划值。
优选的，笛卡尔空间路径点个数大于两个时，以第二个路径点为圆心作球面，求得球面与第一、二个路径点构成线段AB的交点，以及球面与第二、三个路径点构成线段BC的交点，根据圆弧段与线段AB和线段BC相切的条件计算得到圆弧的半径与B’C’圆弧的角度，其中，圆弧过渡采用匀速规划的方式，圆弧过渡的线速度为：
其中，r为球半径，点A和B’为第一段轨迹规划器的路径点，A点时速度为零，B’点速度为vlim。
优选的，所述模块M4包括：
若下一段路径为圆弧，则求得圆弧半径、角度和圆弧过渡的限制速度，以上一段轨迹规划器中规划时间为时间基座时的位置为下一段路径轨迹规划器的起始位置；
若下一段路径为直线，则获取下一段轨迹的终止点和限制速度，再生成笛卡尔空间直线轨迹规划器。
与现有技术相比，本发明具有如下的有益效果：
(1)本发明避免了被控对象的频繁启停，提高了运动流畅性，提高了被控对象工作寿命；
(2)本发明充分发挥被控对象速度性能，提高了工作效率；
(3)本发明采用圆弧过渡匀速规划的方式，避免了被控对象出现速度突跳现象，保证被控对象运动顺滑性。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为笛卡尔空间直线轨迹规划图；
图2为圆弧过渡中间点计算示意图；
图3为两路径点的轨迹规划效果图；
图4为两路径点的轨迹规划速度曲线图；
图5为四个路径点的轨迹规划效果图；
图6为四个路径点的轨迹规划速度曲线图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
实施例：
根据本发明提供的基于实时运动的笛卡尔空间轨迹规划方法，包括如下步骤：
步骤1：获取笛卡尔空间路径点；
步骤2：生成第一段轨迹规划器；
步骤3：生成规划位置信息；
步骤4：生成下一段轨迹规划器；
步骤5：完成轨迹规划。
步骤1的详细内容为：通过用户输入，或系统自动识别的方法获取了笛卡尔空间中的路径点，若出现相邻两路径点的距离过短情况，则返回无法规划信息；若未出现相邻路径点的距离过短情况，则保存所有笛卡尔空间路径点并开始执行步骤2。
步骤2可分为两种情况：
情况1：仅有两个笛卡尔空间路径点，此时无需考虑圆弧过渡，直接用直线规划即可，如图1。计算两路径点的直线距离，再设定笛卡尔空间最大线速度vmax和最大线加速度amax，设定A点和B点速度均为零，利用梯形速度轨迹即可得到这两个路径点的全部轨迹。规划从A点运动至B点的轨迹P公式为：
其中，OP为任意时刻下的轨迹点，OA为A点坐标，AB为线段向量，u为梯形速度轨迹规划器规划值。图一为笛卡尔空间直线轨迹规划示意图。
情况2：笛卡尔空间路径点大于2，此时最少包含一个圆弧过渡路径。设定以第二个路径点为圆心，5mm为半径的球形，可求得该球面与第一、二个路径点构成线段的交点，可求得该球面与第二、三个路径点构成线段的交点，如图2。
根据圆弧段与线段AB和线段BC相切的条件可计算得到圆弧的半径与B’C’圆弧的角度，其中，圆弧过渡采用匀速规划的方式，为满足线加速度约束条件，此时可计算得到圆弧过渡的线速度为：
此时以点A和B’为第一段轨迹规划器的路径点，A点时速度为零，B’点速度为vlim，利用上述的笛卡尔空间直线轨迹规划方法可生成第一段轨迹规划器。
步骤3的详细内容为：根据实时运动控制系统的运行周期，随时间变化，轨迹规划器生成该时刻的位置信息，并下发至被控对象。
步骤4的详细内容为：获取当前轨迹规划器的规划时间，除以实时运动控制系统的运行周期后取整再乘以实时运动控制系统的运行周期，将运算后得到的时间基座tf_int。当满足当前轨迹不是最后一段轨迹且运动时间比tf_int少一个实时运动控制系统的运行周期时，开始生成下一段轨迹规划器，根据下一段路径的不同，又可分为两种情况：
情况1：下一段路径为圆弧，利用步骤2的方法可求得圆弧半径与角度，还有圆弧过渡的限制速度。以上一段轨迹规划器中规划时间为tf_int时的位置为下一段路径轨迹规划器的起始位置。
情况2：下一段路径为直线，利用步骤2的方法可获得下一段轨迹的终止点，限制速度，再生成笛卡尔空间直线轨迹规划器。
步骤5的详细内容：当满足当前轨迹是最后一段轨迹，且运动时间等于轨迹规划器的规划时间时，表明轨迹规划完成。
具体地，两个路径点的轨迹规划：
给定任意两个笛卡尔空间路径点，A＝[1 0.5 1]T，B＝[-0.3 1.4 0]T，设定最大线速度为0.4m/s，最大线加速度为0.9m/s/s，满足步骤2中的情况1，此时笛卡尔空间直线轨迹规划效果如图3所示，运动过程中的速度曲线如图4所示
具体地，四个路径点的轨迹规划：
给定任意四个笛卡尔空间路径点，A＝[1 0.5 1]T，B＝[-0.3 1.4 0]T，C＝[-1.30.2 -0.6]T，D＝[0.1 -0.4 0.8]T，同样的，设定最大线速度为0.4m/s，最大线加速度为0.9m/s，满足步骤2中的情况2，此时笛卡尔空间轨迹规划效果如图5所示，运动过程中的速度曲线如图6所示。
根据本发明提供的基于实时运动的笛卡尔空间轨迹规划系统，包括：模块M1：通过用户输入或自动识别，获取笛卡尔空间中的路径点；模块M2：根据笛卡尔空间中路径点的个数进行直线规划和圆弧过渡匀速规划，生成第一段轨迹规划器；模块M3：根据运动周期，通过第一段轨迹规划器生成带有时刻的位置信息，并下发至被控对象；模块M4：获取当前轨迹规划器的规划时间，除以运动周期后取整再乘以运动周期，得到时间基座，若当前轨迹不是最后一段轨迹且运动时间比时间基座少一个运动周期时，生成下一段轨迹规划器；模块M5：判断当前轨迹为最后一段轨迹，且运动时间等于轨迹规划器的规划时间时，完成轨迹规划。
所述模块M1包括：若相邻两个路径点的距离不在预设范围内，则返回无法规划信息；若相邻两个路径点的距离在预设范围内，则保存所有笛卡尔空间中路径点。仅有两个笛卡尔空间路径点A点和B点时，采用直线规划，计算两路径点的直线距离，再设定笛卡尔空间最大线速度vmax和最大线加速度amax，利用梯形速度轨迹得到这两个路径点的全部轨迹，规划从A点运动至B点的轨迹P公式为：
其中，A点和B点速度均为零，OP为任意时刻下的轨迹点，OA为A点坐标，AB为A点和B点的线段向量，u为梯形速度轨迹规划器规划值。
笛卡尔空间路径点个数大于两个时，以第二个路径点为圆心作球面，求得球面与第一、二个路径点构成线段AB的交点，以及球面与第二、三个路径点构成线段BC的交点，根据圆弧段与线段AB和线段BC相切的条件计算得到圆弧的半径与B’C’圆弧的角度，其中，圆弧过渡采用匀速规划的方式，圆弧过渡的线速度为：
其中，r为球半径，点A和B’为第一段轨迹规划器的路径点，A点时速度为零，B’点速度为vlim。
所述模块M4包括：若下一段路径为圆弧，则求得圆弧半径、角度和圆弧过渡的限制速度，以上一段轨迹规划器中规划时间为时间基座时的位置为下一段路径轨迹规划器的起始位置；若下一段路径为直线，则获取下一段轨迹的终止点和限制速度，再生成笛卡尔空间直线轨迹规划器。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。