CN110757455A - 一种速度规划方法 - Google Patents

一种速度规划方法 Download PDF

Info

Publication number
CN110757455A
CN110757455A CN201911021287.5A CN201911021287A CN110757455A CN 110757455 A CN110757455 A CN 110757455A CN 201911021287 A CN201911021287 A CN 201911021287A CN 110757455 A CN110757455 A CN 110757455A
Authority
CN
China
Prior art keywords
planning
speed
point
constraint
path
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201911021287.5A
Other languages
English (en)
Other versions
CN110757455B (zh
Inventor
王效杰
邓洪洁
张慧
林俐
李生
郑彦秋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Step Electric Corp
Original Assignee
Shanghai Step Electric Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Step Electric Corp filed Critical Shanghai Step Electric Corp
Priority to CN201911021287.5A priority Critical patent/CN110757455B/zh
Publication of CN110757455A publication Critical patent/CN110757455A/zh
Application granted granted Critical
Publication of CN110757455B publication Critical patent/CN110757455B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/416—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control of velocity, acceleration or deceleration
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/41—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by interpolation, e.g. the computation of intermediate points between programmed end points to define the path to be followed and the rate of travel along that path
    • G05B19/4103—Digital interpolation
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/34—Director, elements to supervisory
    • G05B2219/34095—Look ahead segment calculation

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Human Computer Interaction (AREA)
  • General Physics & Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • Artificial Intelligence (AREA)
  • Software Systems (AREA)
  • Mathematical Physics (AREA)
  • Fuzzy Systems (AREA)
  • Evolutionary Computation (AREA)
  • Computing Systems (AREA)
  • Theoretical Computer Science (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明实施例涉及机器人控制技术领域，公开了一种速度规划方法。该速度规划方法包括：根据第一约束条件和路径规划参数，获取所述路径上对应点的速度约束，生成速度约束信息；其中，第一约束条件包括：机器人关节速度约束、加速度约束、加加速度约束、雅克比矩阵及其导数；根据第二约束条件和速度约束信息，以预设步长对路径进行分段速度规划，获取规划样条的控制点；其中，第二约束条件包括：运动的总路程、最大加速度、最大加加速度、最大减加速度；根据控制点，以三次B样条曲线为规划曲线，生成第一速度规划结果。该方法使机器人以尽可能快的速度运行，充分发挥机器人关节电机的性能，保证机器人的运动节拍。

Description

一种速度规划方法
技术领域
本发明实施例涉及机器人控制技术领域，特别涉及一种速度规划方法。
背景技术
随着科学技术的发展，工业机器人作为一种自动执行的机器装置在生产制造领域发挥着举足轻重的作用。工业机器人多为多关节串联机械臂，机器人关节空间到操作空间为非线性映射，机器人在不同构型下所能达到的最大速度是不同的。但目前，在进行速度规划时，一段之中往往只能有一个最大速度，如经典的7段S型速度规划。
发明人发现现有技术中至少存在如下问题：传统速度规划方法不能适应多变的速度约束，只能以最低限速进行规划，从而在非限速段速度过低，影响机器人运行节拍。
发明内容
本发明实施方式的目的在于提供一种速度规划方法，使得机器人以尽可能快的速度运行。
为解决上述技术问题，本发明的实施方式提供了一种速度规划方法，包括以下步骤：
根据第一约束条件和路径规划参数，获取路径上对应点的速度约束，生成速度约束信息；其中，第一约束条件包括：机器人关节速度约束、加速度约束、加加速度约束、雅克比矩阵及其导数；根据第二约束条件和速度约束信息，以预设步长对路径进行分段速度规划，获取规划样条的控制点；其中，第二约束条件包括：运动的总路程、最大加速度、最大加加速度、最大减加速度；根据控制点，以三次B样条曲线为规划曲线，生成第一速度规划结果。
本发明的实施方式还提供了一种控制终端，包括：至少一个处理器；以及，与至少一个处理器通信连接的存储器；其中，存储器存储有可被至少一个处理器执行的指令，该指令被至少一个处理器能够执行上述的速度规划方法。
本实施方式还提供了一种计算机可读存储介质，存储有计算机程序，其特征在于，计算机程序被处理器执行时实现上述的速度规划方法。
本发明实施方式相对于现有技术而言，首先通过机器人系统的第一约束，如机器人关节速度约束、加速度约束、加加速度约束、雅克比矩阵及其导数，以及路径规划参数计算出对应路程位置的速度约束，得到速度约束信息以此作为速度规划的第一参考条件，奠定了后续速度规划的准确性；然后根据机器人系统的第二约束条件和上一步得到速度约束信息，以一定的路程长度进行单段的速度规划，获得对应的控制点，在满足约束不超限的条件下，使得每个控制点对应的运行速度尽可能的快；最后根据获取控制点，以凸包性好的三次B样条曲线为规划曲线，生成第一速度规划结果，求解简便，更直观。该方法使机器人以尽可能快的速度运行，充分发挥机器人关节电机的性能，保证机器人的运动节拍。
另外，根据第二约束条件和速度约束信息，以预设步长对路径进行分段速度规划，获取规划样条的控制点，具体包括：选取目标规划点，并获取目标规划点对应的初始信息；其中初始信息包括：目标规划点对应的速度和加速度；根据目标规划点、路径和预设步长确定下一规划点，并根据速度约束信息确定下一规划点对应的速度约束值；根据初始信息和最大加加速度，确定下一规划点的第一速度、第一加速度和第一加加速度；当第一速度小于下一规划点对应的速度约束值，将目标规划点列为规划样条控制点，记录下一规划点的第一加速度和第一加加速度，将下一规划点设置为目标规划点，循环计算至路径终点；当第一速度大于下一规划点对应的速度约束值，则根据初始信息和最大减加速度的近似加加速度，确定满足加加速度约束及加速度约束条件下的下一规划点的第二速度、第二加速度和第二加加速度；根据第二速度与速度约束值的大小比较，确定规划样条控制点。通过按照给定步长从起始点迭代至路径终点，在保证获得的每个控制点，在满足系统约束不超限的条件下，使机器人尽可能快地走完指定路径。
另外，根据第二速度与速度约束值的大小比较，确定所述规划样条控制点，具体包括：
当第二速度小于下一规划点对应的速度约束值，将目标规划点列为规划样条控制点，并利用下一规划点对应的速度约束值更新下一规划点的第二速度；记录下一规划点的第二加速度和第二加加速度，将下一规划点设置为目标规划点，循环计算至路径终点；当第二速度大于下一规划点对应的速度约束值，以给定步长向上回溯至新目标规划点进行速度规划；其中，新目标规划点对应的加加速度大于最大减加速度，所述给定步长为所述预设步长。通过在第二速度小于对应约束值时，将下一规划点的第二速度以速度约束值进行更新，以及在第二速度大于对应约束值时，向上回溯新目标规划点重新规划，保证了规划点准确性的同时，进一步保证机器人每个点处的速度尽可能地快，且满足系统约束不超限。
另外，根据目标规划点、路径和预设步长获取下一规划点，具体包括：判断目标规划点以预设步长确定的规划点是否在所述路径上，获取判断结果；若判断结果为规划点在路径上，则将规划点确认为下一规划点；若判断结果为规划点未在路径上，则根据路径的终点的速度信息确定下一规划点。通过这一步判断，确保所有规划点都位于路径之上，保证规划结果的准确性。
另外，根据路径的终点的速度信息确定下一规划点，具体包括：将路径的终点作为末端规划点，获取末端规划点的末速度和末加速度，判断得到的末速度和末加速度是否满足系统加加速度的约束，获取判断结果；当判断结果为满足时，则将末端规划点列为规划样条控制点，结束速度规划；当判断结果为不满足时，以给定步长，向上回溯至新目标规划点进行速度规划；其中，给定步长为路程总长度减去目标规划点对应的路程长度。当下一规划点超出机器人路径范围外，判断末端速度是否满足机器人系统约束，若不满足时，采用给定步长回溯至新目标规划点重新规划，保证得到的控制点的准确性的同时，保证每个控制点在满足机器人系统约束条件下运行速度尽量快。
另外，根据控制点，以三次B样条曲线为规划曲线，生成第一速度规划结果，具体包括：根据所述控制点形成(P0,P1,…,Pn-1,Pn)的控制点列，其中，n为大于2的自然数；在P0控制点和P1控制点之间插入两个第一过度点，在Pn-1控制点和Pn控制点之间插入两个第二过渡点，生成新点列(P0,P01,P02,P1,P2,…Pn-2,Pn-1,Pn2,Pn1,Pn)；将新点列作为规划样条的控制点；根据预设的节点生成策略确定节点矢量；根据所述控制点及所述节点矢量，得到规划样条曲线，生成第一速度规划结果。采用具有很好凸包性的B样条曲线，且曲线形状受控制点及控制多边形约束，生成规划曲线比较直观，求解简便。
另外，在生成第一速度规划结果之后还包括：根据节点与时间之间的关系策略，确定下一个周期对应的节点；根据规划样条曲线，获取各节点对应的路程；根据路程与速度的关系，求出对应速度，获取对应插补点。通过插补操作，使得到的规划曲线更为平滑，即对应的加速度和加加速度连续，保证机器人动作的连贯性。
附图说明
一个或多个实施例通过与之对应的附图中的图片进行示例性说明，这些示例性说明并不构成对实施例的限定。
图1是根据本发明第一实施方式中的速度规划流程图；
图2是根据本发明第一实施方式中的速度规划结果；
图3是根据本发明第二实施方式中的速度规划流程图；
图4是根据本发明第二实施方式中的速度规划结果；
图5是根据本发明第三实施方式中控制终端的示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的各实施方式进行详细的阐述。然而，本领域的普通技术人员可以理解，在本发明各实施方式中，为了使读者更好地理解本申请而提出了许多技术细节。但是，即使没有这些技术细节和基于以下各实施方式的种种变化和修改，也可以实现本申请所要求保护的技术方案。以下各个实施例的划分是为了描述方便，不应对本发明的具体实现方式构成任何限定，各个实施例在不矛盾的前提下可以相互结合相互引用。
本发明的第一实施方式涉及一种速度规划方法。本实施方式的核心在于根据第一约束条件和路径规划参数，获取路径上对应点的速度约束，生成速度约束信息；其中，第一约束条件包括：机器人关节速度约束、加速度约束、加加速度约束、雅克比矩阵及其导数；根据第二约束条件和速度约束信息，以预设步长对路径进行分段速度规划，获取规划样条的控制点；其中，所述第二约束条件包括：运动的总路程、最大加速度、最大加加速度、最大减加速度；根据控制点，以三次B样条曲线为规划曲线，生成第一速度规划结果。本实施方式使机器人以尽可能快的速度运行，充分发挥机器人关节电机的性能，保证机器人的运动节拍。下面对本实施方式的速度规划方法的实现细节进行具体的说明，以下内容仅为方便理解提供的实现细节，并非实施本方案的必须。
本实施方式中的速度规划方法具体流程如图1所示，具体包括：
步骤101：根据第一约束条件和路径规划参数，获取所述路径上对应点的速度约束，生成速度约束信息。
具体地说：首先根据用户给定点及机械臂运动的路径类型等信息，生成路径参数表达式P(s)；然后，对于路径P(s)上s处，根据机器人系统第一约束条件，可计算得到操作空间对应点出的速度约束，即对路程进行一定间隔的采用，对采样得到的参数进行离散化，分别求得每个采样点对应的速度约束，则可以得到整条路径上的速度约束信息。
在实际应用中，机器人第一约束条件可以是机器人关节速度、加速度、加加速度约束、雅克比矩阵及其导数，即可以通过关节控制键速度、加速度及雅可比矩阵换算得到操作空间加速度、加加速度，以此作为系统约束，如图4所示，通过将关节空间速度及加速度约束转换到操作空间得到的速度约束如‘——’所示。同样，在实施时，也可以采用关节输出力矩等作为第一约束条件。本领域技术人员可以根据实际情况进行选择使用。
步骤102：根据第二约束条件和速度约束信息，以预设步长对路径进行分段速度规划，获取规划样条的控制点。经过本步骤后，得到的控制点，如图2中‘*’所示。
具体地说：选取目标规划点，并获取目标规划点对应的速度和加速度信息；根据目标规划点和预设步长确定下一规划点，并根据速度约束信息确定下一规划点对应的速度约束值；根据目标规划点对应的速度和加速度信息，以及第二约束条件中的最大加加速度，计算得到下一规划点的第一速度、第一加速度和第一加加速度；将得到的第一速度同下一规划点对应的速度约束值进行比较。
当第一速度小于所述下一规划点对应的速度约束值，将目标规划点列为规划样条控制点，同时记录下一规划点的第一加速度和第一加加速度，将下一规划点设置为目标规划点，循环迭代至路径终点。
当第一速度大于下一规划点对应的速度约束值，则根据目标规划点对应的速度和加速度信息，以及以尽可能接近最大减加速度的近似加加速度计算下一规划点的第二速度、第二加速度，并记录该段对应的第二加加速度；其中计算得到的下一规划点对应的第二速度和第二加速度满足加加速度约束和加速度约束。根据第二速度与速度约束值的大小比较，确定规划样条控制点。
在一个例子中，当第二速度小于下一规划点对应的速度约束值，将所述目标规划点列为所述规划样条控制点，并利用下一规划点对应的速度约束值更新下一规划点的第二速度；记录下一规划点的第二加速度和第二加加速度，将下一规划点设置为目标规划点，循环计算至路径终点；而当第二速度大于下一规划点对应的速度约束值，以给定步长，向上回溯至新目标规划点进行速度规划；其中，新目标规划点对应的加加速度大于最大减加速度，给定步长为预设步长。
在实际应用中，以目标规划点、运行路径和预设步长确定的下一规划点必须在机器人的运动路程内，如果以这种方式得到的下一规划点超出了路程范围，则将路径的终点作为末端规划点，获取末端规划点的末速度和末加速度，判断得到的末速度和末加速度是否满足系统加加速度的约束；当判断结果为满足时，则将末端规划点列为规划样条控制点，结束速度规划；当判断结果为不满足时，把路程总长度减去所述目标规划点对应路程的差值作为给定步长，在目标规划点以给定步长为规划量，向上回溯至新目标规划点进行速度规划。
需要说明的是，在本实施方式中，以预设步长或者以路程总长度减去目标规划点对应的路程长度确定的步长，向上回溯至新目标规划点进行速度规划的过程是相同，包括：以给定步长，从目标规划点回溯至上一规划点；若上一规划点已列入控制点，且上一规划点对应的加加速度大于最大减加速度，将上一规划点设置为新目标规划点；根据新目标规划点对应的速度、加速度及减小后的加加速度更新下一规划点对应的第一速度及第一加速度；将下一规划点撤出控制点列，并将下一规划点设置为目标规划点进行速度规划。
步骤103：根据控制点，以三次B样条曲线为规划曲线，生成第一速度规划结果。
具体地说：就是以步骤102中计算得到的控制点，生成路程及速度关于节点的样条曲线作为规划结果。
在实际应用中，首先，对于得到的一个控制点列(P0,P1,…,Pn-1,Pn)，其中n为大于2的自然数，在P0控制点和P1控制点之间插入两个第一过渡点，在Pn-1控制点和Pn控制点之间插入两个第二过渡点，生成新点列(P0,P01,P02,P1,P2,…Pn-2,Pn-1,Pn2,Pn1,Pn)；将新的点列作为规划样条的控制点列。然后，根据预设的节点生成策略确定节点矢量，即根据控制点的个数、B样条曲线的阶次以及节点矢量维数之间的关系可以得到节点矢量维数，并确定具体的节点矢量。最后，根据确定控制点及节点矢量，以三次B样条规划曲线，生成第一速度规划结果，具体如图2所示，‘——’为得到的规划样条曲线，‘—·—·’为得到规划样条控制点顺次相连得到的控制多边形。
需要说明的是，本实施方式中的速度规划方法，不仅应用于工业机械臂，还可以应用于移动机器人、数控机床等领域。
不难发现，本实施方式中首先根据机器人系统的第一约束，如机器人关节速度、加速度、加加速度约束、雅克比矩阵及其导数，以及路径规划参数计算出对应路程位置的速度约束，得到速度约束信息以此作为速度规划的第一参考条件，奠定了后续速度规划的准确性；然后根据机器人系统的第二约束条件和上一步得到速度约束信息，以一定的路程长度进行单段的速度规划，获得对应的控制点，在满足约束不超限的条件下，使得每个控制点对应的运行速度尽可能的快；最后根据获取控制点，以凸包性好的三次B样条曲线为规划曲线，生成第一速度规划结果，求解简便，更直观。该方法使机器人以尽可能快的速度运行，充分发挥机器人关节电机的性能，保证机器人的运动节拍。
本发明的第二实施方式涉及一种速度规划方法。第二实施方式与第一实施方式大致相同，主要改进之处在于：在本发明第二实施方式中，基于第一速度规划结果，根据插补周期计算对应插补点，使获得的速度规划曲线更平滑，进一步保证规划结果速度、加速度连续。具体流程如图3所示：
在本实施方式中，假设，目标规划点对应位置的速度及加速度分别为v0、a0，整个路径的路程长度Len，机器人系统约束：加速度、减速度、加加速度约束分别为Amax、Dmax、Jmax。
步骤201：根据第一约束条件和路径规划参数，获取路径上对应点的速度约束，生成速度约束信息Vlimit(s)。
具体地说，根据用户给定点及路径类型等相关信息，生成路径的参数表达式P(s)。对于路径P(s)上s处，根据机器人关节速度、加速度约束及其雅克比矩阵及其导数，可以计算得到操作空间对应点处的速度约束。对路径参数进行离散化，分别求得每个点处的速度约束，则可以得到整条路径上的速度约束信息Vlimit(s)。
在实际应用中，机器人速度约束可以根据机器人关节速度、加速度、加加速度约束、雅克比矩阵及其导数等转换计算得到，同样，在实施时，也可以根据关节输出力矩等计算得到。本领域技术人员可以根据实际情况进行选择使用。
步骤202：判断s0+Δs是否大于整个路径的路程长度Len。
具体地说，假设目标规划点所在位置s0，在计算过程中采用规划量步长为Δs，下一规划点对应位置为s0+Δs，即选取目标规划点s0，以步长Δs确定下一规划点。当s0+Δs<Len，则下一规划点在机器人运行路径上，则执行步骤203；当s0+Δs>Len，即下一规划点不在机器人运动路径上，则执行步骤204。
步骤203：判断s0是否激活。
具体地说，在迭代规划过程中，是对未列入控制点的路径点进行速度规划，所以在选择目标规划点时，需要判断选择的点是否激活，这里s0激活表示目标规划点已列入控制点列。当判断s0未激活则执行步骤205，当判断s0已激活则执行步骤212。
步骤204：判断剩余路程是否满足降速到末速度。
具体地说，在满足加加速度的情况下，判断在剩余路程长度的情况下是否满足变速到末速度、末加速度。如果满足，则直接跳出迭代规划过程，整个计算(si,vi)的过程结束，执行步骤215；否则，执行步骤214。需要说明的是，本步骤中，剩余路程为路程总长度减去目标规划点对应位置的路程，为方便表示，这里剩余路程依旧用Δs表示，但具体长度表示为：Δs＝Len-s0。
步骤205：以当前s0位置处的速度v0、加速度a0作为初始条件，计算s1＝s0+Δs处的速度v1、加速度a1及该段规划对应的加加速度J0。
具体地说，以目标规划点s0处的速度v0及加速度a0作为初始条件，以尽可能接近Jmax的加加速度计算满足加加速度及加速度约束情况下一规划点s1处的速度v1及加速度a1，并得到该规划段的加加速度j0。
步骤206：判断v1是否超过s1处的速度限制vlimit。
具体地说，根据速度约束信息与路程s之间的关系，通过插值可以得到s1对应的速度约束vlimit＝Vlimit(s1)，当v1<vlimit，则执行步骤207，当v1>vlimit，则执行步骤208。
步骤207：s0点激活；如果s1点激活，则重置为未激活；s0＝s1。
具体地说，激活s0，也就是(s0,v0)列入规划样条控制点列，若下一规划点s1已激活，则重置为未激活，并记录(si,vi)＝(s1,v1)及对应的a1和j0，将s1及相关速度及加速度作为初始条件赋值给s0，并返回步骤202进行下一个点的计算。
步骤208：以尽可能接近-Jmax的加速度进行变速计算s1处的速度v1及加速度a1。
具体地说，以尽可能接近最大减加速度的加加速度计算满足加加速度及加速度约束情况下下一规划点s1处对应的速度v1及加速度a1，并得到该段的加加速度j0。
步骤209：判断速度v1是否超过s1处的速度限制vlimit。
具体地说，本步骤中的速度v1是步骤208中经过变速得到的速度。当v1<vlimit，即下一规划点处变速计算后得到的对应速度未超出下一规划点对应的速度约束值，则执行步骤210；当v1>vlimit，即下一规划点处变速计算后得到的对应速度仍然超出下一规划点对应的速度约束值，则执行步骤211。
步骤210：令v1＝vlimit。
具体地说，当下一规划点处变速计算后得到的对应速度未超出下一规划点对应的速度约束值，为了规划后，机器人以尽可能快的速度运行，本步骤中直接令v1＝vlimit，然后进入步骤207，并返回步骤202进行下一个点的计算。
步骤211：向上回溯，令s0＝s0-Δs。
具体地说，本步骤若接在步骤209之后执行，则Δs为初始给定的规划量步长；若本步骤在步骤212之后执行，则Δs为步骤204中提及的剩余路程长度。
步骤212：判断j0是否为最大减加速度。
具体地说，当向上回溯到的某一目标规划点s0处对应的j0未达到-Jmax时，则执行步骤211，继续向上回溯；当向上回溯到的某一目标规划点s0处对应的j0>-Jmax时，执行步骤213。
步骤213：减小J0重新计算s1处的速度v1及加速度a1。
具体地说，在加加速度约束范围内减小加加速度j0，计算并更新v1和a1。然后，执行步骤207，将回溯得到的新目标规划点s0对应的(s0,v0)列入控制点列，s1对应的(s1,v1)撤出控制点列，并将下一规划点设置为目标规划点重新进行计算规划。
步骤214：向上回溯，另s0＝s0-Δs。
具体地说，当在剩余路程长度的情况下不能变速到末端对应的末速度、末加速度时，则以剩余路程长度作为步长，向上回溯，直到(s0,v0)的j0>-Jmax，重新进行控制点的选择。需要说明的是，本步骤中的Δs仅为步骤204中提及的剩余路程长度。
值得一提的是，在步骤201至步骤214过程中，可以不在步骤201中提前计算出速度约束，而是可以根据条件计算当前规划段Δs中的速度、加速度、减速度等系统约束，然后同理计算得到(si,vi)。
步骤215：计算得到的{(si,vi)}列作为控制点，生成路程s及速度v关于节点u的样条曲线作为规划结果。
具体地说，由于样条曲线具有很好的凸包性，曲线形状受控制点及控制多边形约束，比较直观，且求解简便，因此本步骤中采用三次B样条曲线进行规划。
在完成上述计算后，得到一个点列：(s0,v0),…,(si,vi),…,(sn,vn)，其中s0＝0，sn＝Len。对于端点处(起始点和结束点)，速度和加速度均为零，在(s0,v0)与(s1,v1)之间添加两个点(s01,v01)＝(s0,0)，(s02,v02)＝(s0,w*v1)(其中w∈[0,1])；在(sn-1,vn-1)与(sn,vn)之间按照同样的方法添加两个点(sn1,vn1)＝(sn,0)，(sn2,vn2)＝(sn,w*vn-1)，形成新的点列
(s0,v0),(s01,v01),(s02,v02),(s2,v2),…,(si,vi),…(sn-1,vn-1),(sn2,vn2),(sn1,vn1),(sn,vn)
将该点列作为规划样条的控制点。根据控制点个数(n+5)、阶次(3)及节点矢量维数之间的关系可得几点矢量维数为
nu＝n+9
则规划样条的节点矢量为
U＝(0,0,0,0,u1,…,un+1,1,1,1,1)
其中，u1～un+1可以在[0,1]内均匀取得，也可以通过其他方式取得。控制点和节点矢量确定后，即确定了规划样条曲线：
其中，Bi(u)为第i个基函数节点u处的函数值。
步骤216：基于规划结果根据插补周期计算对应插补点并下发以驱动机器人运动。
具体地说，在进行插补时，需要根据当前时刻t的节点u0及插补周期Δt得到下一个周期t+Δt对应的节点u1。将节点u0代入规划样条曲线，可以得到t时刻对应的路程及速度(s0,v0)，及关于节点u的一阶导数
Figure BDA0002247308480000092
和二阶导数
Figure BDA0002247308480000093
由速度与路程之间的关系可知，如公式1所示：
Figure BDA0002247308480000101
进而有，如公式(2)所示：
Figure BDA0002247308480000102
获得两者之间的导数关系后，可通过泰勒展开式得到相应的节点u，如公式(3)所示:
Figure BDA0002247308480000103
将计算得到的节点u1代入规划样条函数即可得到t1时刻对应的路程s，进而由路径表达式可以求出相应插补点。
在实际应用中，如果插补点为操作空间插补点，则通过运动学逆解得到相应的关节角度。将关节角度下发到伺服驱动关节完成相应的动作。
本实施方式得到的最终速度规划结果如图4所示，从上到下依次为规划样条节点u、路程s、速度v、加速度a及加加速度j随时间的变化关系。在速度随时间变化关系图中，虚线为求得的速度约束，实线为实际插补速度，可以看出速度没有超限。在本实施方式中给定加速度约束为Amax＝30000，减速度为Dmax＝-30000，加加速度约束为Jmax＝300000。从图4可以看出，加速度、减速度及加加速度均未超出约束。
不难发现，本实施方式在满足机器人系统约束的情况下，使机器人以尽可能快的速度运行，充分发挥机器人关节电机的性能，保证机器人的运动节拍；同时根据插补周期计算对应插补点并下发以驱动机器人运动，保证获得的规划曲线更为平滑，使规划结果中对应的速度、加速度连续。
上面各种方法的步骤划分，只是为了描述清楚，实现时可以合并为一个步骤或者对某些步骤进行拆分，分解为多个步骤，只要包括相同的逻辑关系，都在本专利的保护范围内；对算法中或者流程中添加无关紧要的修改或者引入无关紧要的设计，但不改变其算法和流程的核心设计都在该专利的保护范围内。
本发明第三实施方式涉及一种控制终端，如图5所示，至少一个处理器302；以及，与至少一个处理器通信连接的存储器301；其中，存储器301存储有可被至少一个处理器302执行的指令，指令被至少一个处理器302执行，以使至少一个处理器302能够执行上述速度规划方法的实施方式。
其中，存储器和处理器采用总线方式连接，总线可以包括任意数量的互联的总线和桥，总线将一个或多个处理器和存储器的各种电路连接在一起。总线还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其他电路连接在一起，这些都是本领域所公知的，因此，本文不再对其进行进一步描述。总线接口在总线和收发机之间提供接口。收发机可以是一个元件，也可以是多个元件，比如多个接收器和发送器，提供用于在传输介质上与各种其他装置通信的单元。经处理器处理的数据通过天线在无线介质上进行传输，进一步，天线还接收数据并将数据传送给处理器。
处理器负责管理总线和通常的处理，还可以提供各种功能，包括定时，外围接口，电压调节、电源管理以及其他控制功能。而存储器可以被用于存储处理器在执行操作时所使用的数据。
本发明第四实施方式涉及一种计算机可读存储介质，存储有计算机程序。计算机程序被处理器执行时实现上述速度规划方法实施例。
即，本领域技术人员可以理解，实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得一个设备(可以是单片机，芯片等)或处理器(processor)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
本领域的普通技术人员可以理解，上述各实施方式是实现本发明的具体实施例，而在实际应用中，可以在形式上和细节上对其作各种改变，而不偏离本发明的精神和范围。

Claims (10)

1.一种速度规划方法，其特征在于，包括：
根据第一约束条件和路径规划参数，获取所述路径上对应点的速度约束，生成速度约束信息；其中，所述第一约束条件包括：机器人关节速度约束、加速度约束、加加速度约束、雅克比矩阵及其导数；
根据第二约束条件和所述速度约束信息，以预设步长对所述路径进行分段速度规划，获取规划样条的控制点；其中，所述第二约束条件包括：运动的总路程、最大加速度、最大加加速度、最大减加速度；
根据所述控制点，以三次B样条曲线为规划曲线，生成第一速度规划结果。
2.根据权利要求1所述的速度规划方法，其特征在于，所述根据第二约束条件和所述速度约束信息，以预设步长对所述路径进行分段速度规划，获取规划样条的控制点，具体包括：
选取目标规划点，并获取所述目标规划点对应的初始信息；其中所述初始信息包括：所述目标规划点对应的速度和加速度；
根据所述目标规划点、所述路径和所述预设步长确定下一规划点，并根据所述速度约束信息确定所述下一规划点对应的速度约束值；
根据所述初始信息和所述最大加加速度，确定所述下一规划点的第一速度、第一加速度和第一加加速度；
当所述第一速度小于所述下一规划点对应的速度约束值，将所述目标规划点列为所述规划样条控制点，记录所述下一规划点的第一加速度和第一加加速度，将所述下一规划点设置为目标规划点，循环计算至路径终点；
当所述第一速度大于所述下一规划点对应的速度约束值，则根据所述初始信息和所述最大减加速度的近似加加速度，确定满足所述加加速度约束及加速度约束条件下的所述下一规划点的第二速度、第二加速度和第二加加速度；根据所述第二速度与所述速度约束值的大小比较，确定所述规划样条控制点。
3.根据权利要求2所述的速度规划方法，其特征在于，所述根据第二速度与所述速度约束值的大小比较，确定所述规划样条控制点，具体包括：
当所述第二速度小于所述下一规划点对应的速度约束值，将所述目标规划点列为所述规划样条控制点，并利用所述下一规划点对应的速度约束值更新所述下一规划点的第二速度；记录所述下一规划点的第二加速度和第二加加速度，将所述下一规划点设置为目标规划点，循环计算至路径终点；
当所述第二速度大于所述下一规划点对应的速度约束值，以给定步长向上回溯至新目标规划点进行速度规划；其中，所述新目标规划点对应的加加速度大于最大减加速度，所述给定步长为所述预设步长。
4.根据权利要求2所述的速度规划方法，其特征在于，所述根据目标规划点、所述路径和所述预设步长确定下一规划点，具体包括：
判断目标规划点以所述预设步长确定的规划点是否在所述路径上，获取判断结果；
若所述判断结果为所述规划点在所述路径上，则将所述规划点确认为所述下一规划点；
若所述判断结果为所述规划点未在所述路径上，则根据所述路径的终点的速度信息确定所述下一规划点。
5.根据权利要求4所述的速度规划方法，其特征在于，所述则根据所述路径的终点的速度信息确定所述下一规划点，具体包括:
将所述路径的终点作为末端规划点，获取所述末端规划点的末速度和末加速度，判断得到的末速度和末加速度是否满足系统加加速度的约束，获取判断结果；
当判断结果为满足时，则将所述末端规划点列为所述规划样条控制点，结束速度规划；
当判断结果为不满足时，以给定步长向上回溯至新目标规划点进行速度规划；其中，所述给定步长为路程总长度减去所述目标规划点对应的路程长度。
6.根据权利要求3或5所述的任意一项速度规划方法，其特征在于，所述以给定步长向上回溯至新目标规划点进行速度规划，具体包括：
以给定步长，从所述目标规划点回溯至上一规划点；
若所述上一规划点已列入控制点，且所述上一规划点对应的加加速度大于所述最大减加速度，将所述上一规划点设置为新目标规划点；
根据所述新目标规划点对应的速度、加速度及减小的加加速度计算并更新下一规划点对应的第一速度及第一加速度；
将所述下一规划点撤出控制点列，并将所述下一规划点设置为目标规划点进行速度规划。
7.根据权利要求1所述的速度规划方法，其特征在于，所述根据所述控制点，以三次B样条曲线为规划曲线，生成第一速度规划结果，具体包括：
根据所述控制点形成(P0,P1,…,Pn-1,Pn)的控制点列，其中，n为大于2的自然数；
在P0控制点和P1控制点之间插入两个第一过渡点，在Pn-1控制点和Pn控制点之间插入两个第二过渡点，生成新点列(P0,P01,P02,P1,P2,…,Pn-2,Pn-1,Pn2,Pn1,Pn)；
将所述新点列作为所述规划样条的控制点；
根据预设的节点生成策略确定节点矢量；
根据所述控制点及所述节点矢量，得到规划样条曲线，生成第一速度规划结果。
8.根据权利要求7所述的速度规划方法，其特征在于，在所述生成第一速度规划结果之后还包括：
根据所述节点与时间之间的关系策略，确定下一个周期对应的节点；
根据所述规划样条曲线，获取各所述节点对应的路程；
根据路程与速度的关系，求出对应速度，获取对应插补点，生成第二速度规划结果。
9.一种控制终端，其特征在于，包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1至8中任一所述的速度规划方法。
10.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至8中任一项所述的速度规划方法。
CN201911021287.5A 2019-10-25 2019-10-25 一种速度规划方法 Active CN110757455B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911021287.5A CN110757455B (zh) 2019-10-25 2019-10-25 一种速度规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911021287.5A CN110757455B (zh) 2019-10-25 2019-10-25 一种速度规划方法

Publications (2)

Publication Number Publication Date
CN110757455A true CN110757455A (zh) 2020-02-07
CN110757455B CN110757455B (zh) 2022-11-22

Family

ID=69333898

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911021287.5A Active CN110757455B (zh) 2019-10-25 2019-10-25 一种速度规划方法

Country Status (1)

Country Link
CN (1) CN110757455B (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111665851A (zh) * 2020-06-30 2020-09-15 哈工大机器人（合肥）国际创新研究院 一种动态调整机器人运行速度的轨迹规划方法及装置
CN111687822A (zh) * 2020-06-22 2020-09-22 南京航空航天大学 一种双机器人导引轨迹多空间自适应插补方法
CN112276947A (zh) * 2020-10-21 2021-01-29 乐聚(深圳)机器人技术有限公司 一种机器人动作模仿方法、装置、设备及存储介质
CN112498355A (zh) * 2020-11-02 2021-03-16 浙江吉利控股集团有限公司 一种速度规划方法及装置
CN112621735A (zh) * 2020-11-23 2021-04-09 配天机器人技术有限公司 机器人及其工作节拍的优化方法、计算机可读存储介质
CN113370995A (zh) * 2021-07-01 2021-09-10 广州小鹏自动驾驶科技有限公司 速度曲线的处理方法、装置、电动汽车及电子设备
CN113946139A (zh) * 2020-07-17 2022-01-18 配天机器人技术有限公司 数控系统的速度预测、数控系统的控制方法及数控系统
CN114859941A (zh) * 2022-07-05 2022-08-05 上海仙工智能科技有限公司 一种在弯曲光滑路径下的速度规划方法、存储介质
CN114872035A (zh) * 2020-10-16 2022-08-09 深圳市华成工业控制股份有限公司 一种局部减速控制方法、运动速度的规划方法、运动路径的规划方法和数据更新方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105717869A (zh) * 2016-03-15 2016-06-29 珞石（北京）科技有限公司 工业机器人操作空间路径复合限制求解方法
WO2016127826A1 (zh) * 2015-02-11 2016-08-18 深圳配天智能技术研究院有限公司 曲线速度规划方法、装置及其数控加工路径数据处理方法
CN106503373A (zh) * 2016-11-04 2017-03-15 湘潭大学 一种基于b样条曲线的双机器人协调装配的轨迹规划方法
CN106695787A (zh) * 2016-12-17 2017-05-24 上海新时达电气股份有限公司 一种速度规划方法
CN108549328A (zh) * 2018-03-22 2018-09-18 汇川技术（东莞）有限公司 自适应速度规划方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2016127826A1 (zh) * 2015-02-11 2016-08-18 深圳配天智能技术研究院有限公司 曲线速度规划方法、装置及其数控加工路径数据处理方法
CN105717869A (zh) * 2016-03-15 2016-06-29 珞石（北京）科技有限公司 工业机器人操作空间路径复合限制求解方法
CN106503373A (zh) * 2016-11-04 2017-03-15 湘潭大学 一种基于b样条曲线的双机器人协调装配的轨迹规划方法
CN106695787A (zh) * 2016-12-17 2017-05-24 上海新时达电气股份有限公司 一种速度规划方法
CN108549328A (zh) * 2018-03-22 2018-09-18 汇川技术（东莞）有限公司 自适应速度规划方法及系统

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111687822A (zh) * 2020-06-22 2020-09-22 南京航空航天大学 一种双机器人导引轨迹多空间自适应插补方法
CN111687822B (zh) * 2020-06-22 2022-04-01 南京航空航天大学 一种双机器人导引轨迹多空间自适应插补方法
CN111665851A (zh) * 2020-06-30 2020-09-15 哈工大机器人（合肥）国际创新研究院 一种动态调整机器人运行速度的轨迹规划方法及装置
CN111665851B (zh) * 2020-06-30 2022-02-11 哈工大机器人（合肥）国际创新研究院 一种动态调整机器人运行速度的轨迹规划方法及装置
CN113946139A (zh) * 2020-07-17 2022-01-18 配天机器人技术有限公司 数控系统的速度预测、数控系统的控制方法及数控系统
CN114872035A (zh) * 2020-10-16 2022-08-09 深圳市华成工业控制股份有限公司 一种局部减速控制方法、运动速度的规划方法、运动路径的规划方法和数据更新方法
CN114872035B (zh) * 2020-10-16 2023-08-22 深圳市华成工业控制股份有限公司 运动速度和路径的规划方法及减速控制和数据更新的方法
CN112276947A (zh) * 2020-10-21 2021-01-29 乐聚(深圳)机器人技术有限公司 一种机器人动作模仿方法、装置、设备及存储介质
CN112498355A (zh) * 2020-11-02 2021-03-16 浙江吉利控股集团有限公司 一种速度规划方法及装置
CN112621735A (zh) * 2020-11-23 2021-04-09 配天机器人技术有限公司 机器人及其工作节拍的优化方法、计算机可读存储介质
CN112621735B (zh) * 2020-11-23 2022-02-18 配天机器人技术有限公司 机器人及其工作节拍的优化方法、计算机可读存储介质
CN113370995A (zh) * 2021-07-01 2021-09-10 广州小鹏自动驾驶科技有限公司 速度曲线的处理方法、装置、电动汽车及电子设备
CN113370995B (zh) * 2021-07-01 2024-01-09 广州小鹏自动驾驶科技有限公司 速度曲线的处理方法、装置、电动汽车及电子设备
CN114859941A (zh) * 2022-07-05 2022-08-05 上海仙工智能科技有限公司 一种在弯曲光滑路径下的速度规划方法、存储介质
CN114859941B (zh) * 2022-07-05 2022-09-23 上海仙工智能科技有限公司 一种在弯曲光滑路径下的速度规划方法、存储介质

Also Published As

Publication number Publication date
CN110757455B (zh) 2022-11-22

Similar Documents

Publication Publication Date Title
CN110757455B (zh) 一种速度规划方法
CN111897216B (zh) 一种多运动段速度规划和插补方法
CN109782779B (zh) 基于种群超启发式算法的洋流环境下auv路径规划方法
CN109683615B (zh) 机器人连续运动的路径的速度前瞻方法及机器人控制器
CN106313047A (zh) 一种基于Bezier样条的机器人实时拐角过渡方法
CN111791236A (zh) 一种工业机器人笛卡尔空间轨迹过渡方法
EP3486612B1 (en) Method for generating a trajectory
CN113618739B (zh) 一种机器人动态避障方法、装置和机器人
JP2019179518A (ja) 強化学習プログラム、強化学習方法、および強化学習装置
JP2021513466A (ja) １つまたは複数のプロセッサ上に離散化された環境を記憶するロボットのモーションプラニングおよびその改善された動作
CN113618742B (zh) 一种机器人避障方法、装置和机器人
CN112528434B (zh) 信息识别方法、装置、电子设备和存储介质
CN116774648B (zh) 速度规划方法、装置、机床控制系统和存储介质
CN114274147B (zh) 目标跟踪控制方法及装置、机械臂控制设备和存储介质
CN113110423A (zh) 步态轨迹规划方法、装置、计算机可读存储介质及机器人
CN113618741B (zh) 机器人关节位姿优化方法、机器人控制方法和机器人
CN113650011B (zh) 一种机械臂拼接路径规划方法及装置
CN113103240B (zh) 用于实现c2连续的机器人轨迹规划方法、装置及系统
CN113021329B (zh) 一种机器人运动控制方法、装置、可读存储介质及机器人
CN106826814B (zh) 一种机器人的运动控制方法和运动控制系统
WO2018205248A1 (zh) 样条路径的插补方法及相关设备
CN111670093B (zh) 机器人的运动控制方法、控制系统和存储装置
Tangpattanakul et al. Optimal trajectory of robot manipulator using harmony search algorithms
CN115790628A (zh) 一种叉车型agv插入路径生成方法、装置、设备及介质
CN112975986B (zh) 基于径向基函数的机械臂点对点轨迹规划方法及装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant