CN111897216B - 一种多运动段速度规划和插补方法 - Google Patents

一种多运动段速度规划和插补方法 Download PDF

Info

Publication number
CN111897216B
CN111897216B CN202010686665.8A CN202010686665A CN111897216B CN 111897216 B CN111897216 B CN 111897216B CN 202010686665 A CN202010686665 A CN 202010686665A CN 111897216 B CN111897216 B CN 111897216B
Authority
CN
China
Prior art keywords
speed
curve
segment
displacement
interpolation
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.)
Active
Application number
CN202010686665.8A
Other languages
English (en)
Other versions
CN111897216A (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.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
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 Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN202010686665.8A priority Critical patent/CN111897216B/zh
Publication of CN111897216A publication Critical patent/CN111897216A/zh
Application granted granted Critical
Publication of CN111897216B publication Critical patent/CN111897216B/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
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • 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
    • 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/1628—Programme controls characterised by the control loop
    • B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
    • 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
    • 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
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/047—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators the criterion being a time optimal performance criterion
    • 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
    • G05B17/00—Systems involving the use of models or simulators of said systems
    • G05B17/02—Systems involving the use of models or simulators of said systems electric

Abstract

本发明公开了一种多运动段速度规划和插补方法，本发明添加过渡段后，连续运动过程中的速度规划问题。为提高运行效率，以单段位置和姿态速度规划为基础，研究连续运动段的轨迹速度传递问题及过渡段的速度优化问题，同时为提高末端轨迹的平稳性，采用S型加减速优化起始段速度规划；最后提出通过基于截距法的插补方法对样条曲线进行实时插补的方法。

Description

一种多运动段速度规划和插补方法
技术领域
本发明涉及机器人技术领域，尤其涉及一种多运动段速度规划和插补方法。
背景技术
在机器人轨迹规划过渡研究中，一般关注笛卡尔空间规划、关节空间轨迹规划方法以及多运动段速度规划三个方面的内容。轨迹规划过渡技术适应于复杂的作业工况，通过在不同类型的运动段之间插入过渡段保证机器人末端轨迹的连续性，能够避免运动过程中出现的冲击；速度规划为保证运行效率需要满足时间最优原则，并且考虑位置和姿态的同步问题，本发明针对机器人速度规划和插补问题，首先实现了单运动段的速度规划和同步方法，然后提出了多运动段速度规划方法来解决速度平滑性的问题，最后通过截距法对样条曲线进行实时插补。
发明内容
本发明的目的就在于为了解决上述问题而提供一种多运动段速度规划和插补方法。
本发明通过以下技术方案来实现上述目的：
本发明多运动段速度规划方法包括多运动段轨迹起始点速度设计、曲率约束的最大速度控制方法优化、S型加减速的运动段起始速度优化；
所述多运动段轨迹起始点速度设计：需要先预读所有的运动段，然后从后往前推导每个运动段的终速度，并且本段的终点速度等于下一段的起点速度；所述多运动段轨迹起始点速度限制条件包含了在加速度a、本段的位移s和末速度ve的限制下，起点速度vb需要满足：
Figure GDA0003067235180000011
在相同运动段类型中位移的值可以直接转换，但是在关节运动段和笛卡尔空间运动段之间需要考虑关节空间和笛卡尔空间的互换；
关节坐标转换为笛卡尔坐标采用截距法，在运动关节范围内选取多个点，转换为正解坐标后，求和每相邻点之间的距离得到预估距离
Figure GDA00030672351800000210
易知预估距离
Figure GDA00030672351800000211
小于实际距离s，所以求初始速度的限制如下：
Figure GDA0003067235180000021
笛卡尔坐标转关节坐标通过运动反解，通过将运动段的起点和终点反解为在挂接坐标下表示的起点和终点，可以计算每个关节的位移，按公式(4)表示的方法计算角速度限制；
所述曲率约束的最大速度控制方法优化：空间平滑曲线方程由参数方程x＝ x(t),y＝y(t),z＝z(t)组成，向量和加速度向量为：
Figure GDA0003067235180000022
根据空间曲率计算方法计算出曲率半径：
Figure GDA0003067235180000023
化简计算曲率半径为：
Figure GDA0003067235180000024
针对由四个控制点P0、P1、P2和P3构成的三阶贝塞尔曲线P，一阶贝塞尔函数和二阶贝塞尔函数为：
Figure GDA0003067235180000025
对于任意一点t，代入上式中得到
Figure GDA0003067235180000026
和
Figure GDA0003067235180000027
将x1，x2，y1，y2，z1和z2代入曲率半径计算公式中即可得到曲率半径；
在确认曲率半径的计算方法后，需要计算曲线段上最小曲率半径所在位置；常用方法为在曲线段上均匀采样多个点，然后通过比较选择最小值对应点，然而此种方法精度不高，会对最优位置的计算存在偏差；本小节通过遗传寻优算法中的一种粒子群算法来解决寻找最优曲率点的问题；
粒子群算法从随机值出发，不断迭代寻找最优值，主要在于构造适应度函数；适应度函数与优化问题有关，根据优化模型来构造，对于最小曲率半径寻优问题选择式(8)作为适应度函数，评价每个点补偿值的好坏，选择四个控制点，构造贝塞尔曲线通过仿真得到适应度值的变化示意图；
超过6次迭代计算后，适应度值不变，所以当参数因子等于0.607315时，最小曲率半径等于17.0705mm；在保证其他条件不变的前提下，改变最大迭代次数，对比不同数据，通过多次实验，针对最小曲率半径寻优问题设置粒子数量50，精度为0.01，以及最大迭代次数为10时，计算速度较快，能够避免PSO 算法的复杂迭代，能够高效的得到最优解决；
在最小曲率半径点需要满足
Figure GDA0003067235180000031
如果按照规划完整运动所采用的方法，样条各点的速度都会不大于最小曲率半径点规定的速度，这会限制机器人末端速度，同时不满足时间最短规划，所以针对已经求出最小曲率半径的样条速度规划，需要针对最小曲率半径点做出优化；
优化方法具体过程为：以最小曲率半径点作为参考将运动段分为前后两段，在考虑最小曲率半径点速度限制的前提下，采用单段最短时间运动规划方法规划前后两段位置速度和姿态速度；选择位置规划速度和姿态规划速度中的最长时间，并且通过等比例放大时间通过给定时间规划同步位置和姿态；通过分段规划然后同步的方法能够有效避免因为曲率半径限制导致整个运动段运行时间过长的问题；
所述S型加减速的运动段起始速度优化：
建立S型曲线位移方程P(t)需要已知梯形加减速加速段的初速度为vb，终速度为ve，加速时间为T；在建立参数方程时，还需要考虑如下限制条件：在 t＝0时刻，曲线的位移为0；曲线终止时，S型曲线的位移等于梯形曲线的位移 (vb+ve)T/2；在曲线开始和终止时刻，曲线对应的速度即一阶导数需要等于梯形加减速的初速度和终止速度vb和ve；并且在开始和终止时刻，为了避免冲击，曲线P(t)的加速度和加加速度必须保证为0；所以曲线P(t)在满足以上8个条件的情况下，需要建立7次曲线方程如下：
P(t)＝At7+Bt6+Ct5+Dt4+Et3+Ft2+Gt+H (10)
将限制条件以公式表示为：
Figure GDA0003067235180000041
联立以上两式解得：
Figure GDA0003067235180000042
通过以上求解的参数，能够建立S型曲线位移，优化梯形加减速；
过渡段插补方法包括弧长节点集合构造方法、过渡段样条插补方法，所述弧长节点集合构造方法：对样条曲线进行插补前需要计算样条曲线的总位移，通过截距法均匀等分插补参数，在样条曲线上取点，累加曲线上的直线位移，以直代曲作为曲线的总位移，构造弧长节点Si集合的步骤为：首先在[0,1]区间内均匀选择n个点，第i个点满足ui＝i/n，再根据样条方程Pi＝F(ui)计算对应的 P1,P2,…Pn；然后构造弧长节点Si集合，Si表示起点P1到Pi的预估长度，Si 通过累加Si＝Si-1+|PiPi-1|计算；
所述过渡段样条插补方法：样条插补根据插补时间计算插补点，然后反算成关节值；在弧长节点集合与速度信息图的基础上，将插补时间映射成插补点的位姿；设当前的插补时间为t，已完成位移为s，首先根据插补时间确定对应的速度v，从而计算已完成位移s＝s+vT；然后将位移映射成样条曲线参数方程输入值，在弧长节点集合中寻找当前位移所在区间i，第i个节点段满足Si<s<Si+1，根据线性比例得到样条曲线参数方程输入值u；最后根据u和参数方程P＝F(u) 计算插补点，再反算成对应多轴的关节值；循环以上过程，直至到达插补周期，在插补开始时初始时间和位移为0；在样条曲线插补得过程中，需要得到插补点在弧长节点集合中的位置，并且弧长节点集记录的各分段样条节点的位移呈升序排列，因此适应于二分法对其进行快速定位；将分段样条集合中间两点的位移与查找位移进行比较，如果查找的位移位于两点之间，则查找成功；否则利用两点将原集合分成前、后两个集合，如果两点大于查找的位移，则进一步查找前一集合，否则进一步查找后一集合，直至成功。
本发明的有益效果在于：
本发明是一种多运动段速度规划和插补方法，本发明添加过渡段后，连续运动过程中的速度规划问题。为提高运行效率，以单段位置和姿态速度规划为基础，研究连续运动段的轨迹速度传递问题及过渡段的速度优化问题，同时为提高末端轨迹的平稳性，采用S型加减速优化起始段速度规划；最后提出通过基于截距法的插补方法对样条曲线进行实时插补的方法。
附图说明
图1是梯形规划速度和加速度与时间关系图；
图2是时间最优的梯形速度规划；
图3是时间同步速度规划流程图；
图4是给定三段运行时间的梯形速度规划；
图5是给定总运行时间同步速度规划图；
图6是速度预估流程图；
图7是PSO算法流程图；
图8是最小曲率变化寻优示意图；
图9是样条位姿同步规划示意图；
图10是S型和梯形速度对比图；
图11是截距法取点示意图；
图12是样条插补示意图；
图13是样条插补流程图；
图14是基于二分查找的位移搜寻方法；
图15是位姿速度时间变化图；
图16是过渡段速度优化示意图；
图17是S型和梯形速度对比图。
具体实施方式
下面结合附图对本发明作进一步说明：
单段位姿速度规划方法：
在位置、速度和加速度等条件的限制下，过渡段速度包含两种方法。时间最优规划在机器人速度和加速度等限制条件下，达到时间最优；时间同步速度规划使得多轴关节或位姿的时间同步。
时间最优速度规划：
时间最优速度规划采用梯形加减速规划，包含前后两加速段以及中间匀速段，如图1所示。
通过图1可以计算已完成位移S和速度V与时间t的关系。
机器人时间最优速度规划在最大速度vo、最大加速度a、初速度vb和终点速度ve等约束条件下，以最短时间完成总的位移Sp。
首先计算位移临界值S1和S2，S1表示以最大加速度a先匀加速再匀减速的等效位移，S2表示以最大加速度a保持匀加速的等效位移：
Figure GDA0003067235180000071
然后根据Sp与S1和S2之间的大小关系，进行梯形速度规划。
如果Sp>S2，如图2(a)，以最大加速度a和最大速度vo做匀加速-匀速-匀减速规划；如果S1＜Sp≤S2，如图2(b)，重新计算最大速度vm，以最大加速度a和最大速度vm做匀加速-匀减速规划；如果Sp≤S1，如图2(c)则需要增加加速度，匀速规划；如果Sp<10-4，如图2(d)，表示位移为0，不需规划。
通过时间最优的梯形速度规划可以看出每个运动段的运动时间，以及等效位移、速度、加速度和时间之间的关系。
时间同步速度规划：
针对以时间最优速度规划对机器人同一运动段的位置和姿态规划过程中，存在的运行时间不一致问题，本发明提出时间同步速度规划方法。时间同步速度规划选择位置和姿态速度规划中两者最大的运行时间作为总运行时间，重新对位置或者姿态进行规划，保证位置和姿态规划时间都等于总运行时间。
设梯形速度规划三段的时间分别为t1、t2和t3，总运行时间为Tm，时间同步速度规划对运行时间的规定可以分为两种：第一种规定t1、t2和t3的值；第二种规定Tm的值。给定三段运行时间的方法条件显然比给定总时间规划严格，所以在同步时可以先采用给定三段运行时间，规划失败后选用给定总时间规划。基于这两种规划时间同步速度规划的流程如图3所示。
图3中规划成功条件在给定三段时间速度同步规划中详细说明。给定时间规划指机器人以最大速度vo、最大加速度a、初速度vb和终点速度ve的约束下，能在规定时间内情况下完成总的位移Sp。
给定三段时间速度同步规划
先计算临界位移S1
Figure GDA0003067235180000081
如果Sp<S1，速度规划的加速度会超出规定的最大加速度，如图4(b)，不能满足要求，返回规划失败；否则按图4(a)进行规划。
给定总时间速度同步规划：
给定总时间速度同步规划在给定的时间内，以最大加速度a，在时间Tm内完成位移Sp。首先计算两个位移临界值S1、S2和S3，S1表示匀减速位移，S2表示第三段匀减速，S3表示加速段临界位移，计算公式如下：
Figure GDA0003067235180000082
如果Sp<10-4，表示位移为0，不需规划。如果Sp<S1，加大加速度，若加速度大于系统限定最大值，规划失败，如图5(a)；如果Sp<S2，第一段做匀减速，第二段匀速，第三段匀减速，如图5(b)；经过时间最优规划保证了S3的位移必定大于Sp，所以在其余情况做匀加速-匀速-匀减速规划，如图5(c)。
通过以上规划，能够实现机器人单运动位姿时间相同的同步速度规划。对于多关节的速度规划，将多个关节的关节转角作为等效位移，与上述步骤相似，对每个关节做时间最优规划，再对多个关节进行同步规划。
多运动段轨迹速度设计与优化：
添加过渡段后，连续轨迹保证了G1连续性，避免了起始速度需要降为0的问题，因此多运动段速度规划过程只需在单段速度规划的基础上考虑每个运动段起始点的速度大小。对于当前运动段可以通过计算后续运动段的位移预估本段终点的最大允许速度，并将终点速度传递给下一运动段的起点速度。
针对过渡段，构造曲线为贝塞尔样条曲线或者非均匀样条曲线，在速度规划时，需要考虑曲率限制对速度的影响，常用做法为通过最小曲率半径点计算限制速度vr，将其作为过渡段的最大速度限制之一，但在过渡段的其他位置不能充分利用机器人的电机性能。针对上述问题，本发明采用遗传算法准确寻找到最小曲率半径点，并以此点将过渡段分成前后两段分开规划，从而充分利用机器人的电机性能。
多运动段轨迹起始点速度设计：
多运动段为了提高运行时的速度连续性，并防止因后续运动段位移不足而导致产生过大的加速度，需要提前预估每个运动段的终点速度，并将前一运动段的末速度转化为后一运动段的初始速度，并且对于不同的运动类型的过渡段，需要进行关节速度和线速度的转换。速度预估流程如图6所示；
根据图6所示的速度预估方法，需要先预读所有的运动段，然后从后往前推导每个运动段的终速度，并且本段的终点速度等于下一段的起点速度。
速度预估方法中限制条件包含了在加速度a、本段的位移s和末速度ve的限制下，起点速度vb需要满足：
Figure GDA0003067235180000091
在相同运动段类型中位移的值可以直接转换，但是在关节运动段和笛卡尔空间运动段之间需要考虑关节空间和笛卡尔空间的互换。
关节坐标转换为笛卡尔坐标采用截距法，在运动关节范围内选取多个点，转换为正解坐标后，求和每相邻点之间的距离得到预估距离
Figure GDA00030672351800001010
易知预估距离
Figure GDA00030672351800001011
小于实际距离s，所以求初始速度的限制如下：
Figure GDA0003067235180000101
笛卡尔坐标转关节坐标通过运动反解，通过将运动段的起点和终点反解为在挂接坐标下表示的起点和终点，可以计算每个关节的位移，按(4)表示的方法计算角速度限制。
基于曲率约束的最大速度控制方法优化
在速度规划中，为保证不超出加速度的限制，需要计算样条的速度限制点即曲率最大点，优化速度规划。空间平滑曲线方程由参数方程x＝x(t),y＝y(t), z＝z(t)组成，向量和加速度向量为：
Figure GDA0003067235180000102
根据空间曲率计算方法计算出曲率半径：
Figure GDA0003067235180000103
化简计算曲率半径为：
Figure GDA0003067235180000104
针对由四个控制点P0、P1、P2和P3构成的三阶贝塞尔曲线P，一阶贝塞尔函数和二阶贝塞尔函数为：
Figure GDA0003067235180000105
对于任意一点t，代入上式中得到
Figure GDA0003067235180000106
和
Figure GDA0003067235180000107
将x1，x2，y1，y2，z1和z2代入曲率半径计算公式中即可得到曲率半径。
在确认曲率半径的计算方法后，需要计算曲线段上最小曲率半径所在位置。常用方法为在曲线段上均匀采样多个点，然后通过比较选择最小值对应点，然而此种方法精度不高，会对最优位置的计算存在偏差。本小节通过遗传寻优算法中的一种PSO算法(粒子群算法)来解决寻找最优曲率点的问题。
PSO算法从随机值出发，不断迭代寻找最优值，主要在于构造适应度函数。适应度函数与优化问题有关，根据优化模型来构造，对于最小曲率半径寻优问题选择式(8)作为适应度函数，评价每个点补偿值的好坏，具体流程如图7所示。
选择四个控制点，构造贝塞尔曲线通过仿真得到适应度值的变化示意图8。
超过6次迭代计算后，适应度值不变，所以当参数因子等于0.607315时，最小曲率半径等于17.0705mm。在保证其他条件不变的前提下，改变最大迭代次数，对比不同数据，通过多次实验，针对最小曲率半径寻优问题设置粒子数量 50，精度为0.01，以及最大迭代次数为10时，计算速度较快，能够避免PSO算法的复杂迭代，能够高效的得到最优解决。
在最小曲率半径点需要满足
Figure GDA0003067235180000111
如果按照规划完整运动所采用的方法，样条各点的速度都会不大于最小曲率半径点规定的速度，这会限制机器人末端速度，同时不满足时间最短规划，所以针对已经求出最小曲率半径的样条速度规划，需要针对最小曲率半径点做出优化。
优化方法具体过程为：以最小曲率半径点作为参考将运动段分为前后两段，在考虑最小曲率半径点速度限制的前提下，采用单段最短时间运动规划方法规划前后两段位置速度和姿态速度；选择位置规划速度和姿态规划速度中的最长时间，并且通过等比例放大时间通过给定时间规划同步位置和姿态。
通过分段规划然后同步的方法能够有效避免因为曲率半径限制导致整个运动段运行时间过长的问题。
基于S型加减速的运动段起始速度优化
梯形加减速速度曲线的一阶导数不连续，也就是加速度存在突变，在机器人末端进行高速运动时，由于加速度的突变，会对机器人产生冲击。本小节通过对梯形加减速前后段进行S型曲线优化，使用满足加速度和加加速度均连续的高次速度曲线P(t)，避免梯形加减速产生冲击。
建立S型曲线位移方程P(t)需要已知梯形加减速加速段的初速度为vb，终速度为ve，加速时间为T。在建立参数方程时，还需要考虑如下限制条件：在 t＝0时刻，曲线的位移为0；曲线终止时，S型曲线的位移等于梯形曲线的位移 (vb+ve)T/2；在曲线开始和终止时刻，曲线对应的速度即一阶导数需要等于梯形加减速的初速度和终止速度vb和ve；并且在开始和终止时刻，为了避免冲击，曲线P(t)的加速度和加加速度必须保证为0。所以曲线P(t)在满足以上8个条件的情况下，需要建立7次曲线方程如下：
P(t)＝At7+Bt6+Ct5+Dt4+Et3+Ft2+Gt+H (10)
将限制条件以公式表示为：
Figure GDA0003067235180000121
联立以上两式解得 ：
Figure GDA0003067235180000122
通过以上求解的参数，能够建立S型曲线位移，优化梯形加减速。对比图如图10。
通过对比两种速度图，可以直观的看出S型速度段在开始和结束段与匀速段的衔接加速度为0，减少了角速度突变而产生的运动冲击。
基于截距法的过渡段插补方法：
过渡段由样条曲线构造，在样条曲线的插补过程中，首先依据插补时间和速度规划信息计算已完成位移，然后将已完成位移转换为参数方程的输入值，最后计算得到样条上的插补点。其中完成位移与参数方程的输入难以转换，本发明基于截距法对样条曲线进行插补，构造弧长节点集，建立插补距离和插补位置之间的关系，简化转换过程，再通过二分法快速对位置进行插补。
弧长节点集合构造方法：
对样条曲线进行插补前需要计算样条曲线的总位移，通过截距法均匀等分插补参数，在样条曲线上取点，累加曲线上的直线位移，以直代曲作为曲线的总位移，构造方法如图11所示。
构造弧长节点Si集合的步骤为：首先在[0,1]区间内均匀选择n个点，第i 个点满足ui＝i/n，再根据样条方程Pi＝F(ui)计算对应的P1,P2,…Pn；然后构造弧长节点Si集合，Si表示起点P1到Pi的预估长度，Si通过累加Si＝Si-1+ |PiPi-1|计算。
过渡段样条插补方法：
样条插补根据插补时间计算插补点，然后反算成关节值。本发明在弧长节点集合与速度信息图的基础上，将插补时间映射成插补点的位姿。
如图12所示：设当前的插补时间为t，已完成位移为s，首先根据插补时间确定对应的速度v，从而计算已完成位移s＝s+vT；然后将位移映射成样条曲线参数方程输入值，在弧长节点集合中寻找当前位移所在区间i，第i个节点段满足Si<s<Si+1，根据线性比例得到样条曲线参数方程输入值u；最后根据u和参数方程P＝F(u)计算插补点，再反算成对应多轴的关节值。循环以上过程，直至到达插补周期，在插补开始时初始时间和位移为0。插补流程图如图13所示。
在样条曲线插补得过程中，需要得到插补点在弧长节点集合中的位置，并且弧长节点集记录的各分段样条节点的位移呈升序排列，因此适应于二分法对其进行快速定位。将分段样条集合中间两点的位移与查找位移进行比较，如果查找的位移位于两点之间，则查找成功；否则利用两点将原集合分成前、后两个集合，如果两点大于查找的位移，则进一步查找前一集合，否则进一步查找后一集合，直至成功。
为提高查找效率，避免每次查找遍历所有弧长节点集合，查找过程中需要记录上一次查找的集合号，在下次查询开始之前，往后依次查询，如果本段和下一段不能满足条件，则开始二分查找，其具体的流程如图14所示。
多运动段速度设计与优化方法仿真与分析：
本实验对本文位姿速度设计方法、轨迹过渡速度优化方法以及S型曲线优化速度规划方法进行验证。设计参数采用HRT默认手动模式；当前关节最大速度：20°/s；当前关节加速度：500°/s2；当前平动最大速度：100mm/s(指令)；平动最大加速度：50mm/s2。
位姿速度设计方法和同步方法仿真：
通过采集插补器输出位姿数据，计算每个时刻机器人的位置，得到机器人位置和姿态速度和角速度图如图15所示。
通过图形可知机器人运动由5段运动段组成，5-15(a)表示位置速度曲线， 5-15(b)姿态轴角速度曲线，在每个运动段，速度图为梯形，表明本发明的速度规划有效。
在最优时间规划下，位置的最大规划速度为100mm/s，姿态的最大规划角速度为20°/s，图15(b)速度图上的最大速度相同，第2、3和4段都到达了最大速度，同时相对应的15(a)速度图的第2、3和4段运动段的最大速度不同且小于100mm/s，但每个运动段的运行时间与图15(b)的时间相同，这是由于采用了同步规划，插补器选用了位置和姿态每个运动段的最大时间，同时对时间较短的段使用了给定时间规划，进而验证了同步规划，同理第1和5段情况类似。
多运动段轨迹过渡速度优化仿真：
如图16所示构建过渡段，采集过渡段的速度信息，绘制优化后速度对比图形。在最小曲率点计算最小速度为27.3mm/s，通过基于曲率限制的过渡段优化方法，避免了全程速度小于27.3mm/s导致运行时间过长的问题，能够满足最短时间规划，提高了运动效率。
对于同一运动段分别采用梯形和S型加减速，进行规划，得到速度对比图如图17。
通过对比两种速度图，可以直观的看出S型速度段在开始和结束段与匀速段的衔接加加速度为0，减少了加速度突变而产生的运动冲击。
本发明对多运动段速度规划方法进行了研究和仿真实验，首先基于梯形速度规划，对单运动段时间最优规划和给定时间两类规划进行了研究；其次针对过渡段的特点进行了基于曲率限制的速度优化，以及针对起始段进行了优化；最后通过仿真验证了多运动段速度规划的可行性以及两种优化方法对时间效率的提升。
以上显示和描述了本发明的基本原理和主要特征及本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (1)

1.一种多运动段速度规划和插补方法，其特征在于：多运动段速度规划方法包括多运动段轨迹起始点速度设计、曲率约束的最大速度控制方法优化、S型加减速的运动段起始速度优化；
所述多运动段轨迹起始点速度设计：需要先预读所有的运动段，然后从后往前推导每个运动段的终速度，并且本段的终点速度等于下一段的起点速度；所述多运动段轨迹起始点速度限制条件包含了在加速度a、本段的位移s和末速度ve的限制下，起点速度vb需要满足：
Figure FDA0003067235170000011
在相同运动段类型中位移的值可以直接转换，但是在关节运动段和笛卡尔空间运动段之间需要考虑关节空间和笛卡尔空间的互换；
关节坐标转换为笛卡尔坐标采用截距法，在运动关节范围内选取多个点，转换为正解坐标后，求和每相邻点之间的距离得到预估距离
Figure FDA0003067235170000015
易知预估距离
Figure FDA0003067235170000016
小于实际距离s，所以求初始速度的限制如下：、
Figure FDA0003067235170000012
笛卡尔坐标转关节坐标通过运动反解，通过将运动段的起点和终点反解为在挂接坐标下表示的起点和终点，可以计算每个关节的位移，按公式(4)表示的方法计算角速度限制；
所述曲率约束的最大速度控制方法优化：空间平滑曲线方程由参数方程x＝x(t),y＝y(t),z＝z(t)组成，向量和加速度向量为：
Figure FDA0003067235170000013
根据空间曲率计算方法计算出曲率半径：
Figure FDA0003067235170000014
化简计算曲率半径为：
Figure FDA0003067235170000021
针对由四个控制点P0、P1、P2和P3构成的三阶贝塞尔曲线P，一阶贝塞尔函数和二阶贝塞尔函数为：
Figure FDA0003067235170000022
对于任意一点t，代入上式中得到
Figure FDA0003067235170000023
和
Figure FDA0003067235170000024
将x1，x2，y1，y2，z1和z2代入曲率半径计算公式中即可得到曲率半径；
在确认曲率半径的计算方法后，需要计算曲线段上最小曲率半径所在位置；通过遗传寻优算法中的一种粒子群算法来解决寻找最优曲率点的问题；
粒子群算法从随机值出发，不断迭代寻找最优值，主要在于构造适应度函数；适应度函数与优化问题有关，根据优化模型来构造，对于最小曲率半径寻优问题选择式(8)作为适应度函数，评价每个点补偿值的好坏，选择四个控制点，构造贝塞尔曲线通过仿真得到适应度值的变化示意图；
超过6次迭代计算后，适应度值不变，所以当参数因子等于0.607315时，最小曲率半径等于17.0705mm；在保证其他条件不变的前提下，改变最大迭代次数，对比不同数据，通过多次实验，针对最小曲率半径寻优问题设置粒子数量50，精度为0.01，以及最大迭代次数为10时，计算速度较快，能够避免PSO算法的复杂迭代，能够高效的得到最优解决；
在最小曲率半径点需要满足
Figure FDA0003067235170000025
如果按照规划完整运动所采用的方法，样条各点的速度都会不大于最小曲率半径点规定的速度，这会限制机器人末端速度，同时不满足时间最短规划，所以针对已经求出最小曲率半径的样条速度规划，需要针对最小曲率半径点做出优化；
优化方法具体过程为：以最小曲率半径点作为参考将运动段分为前后两段，在考虑最小曲率半径点速度限制的前提下，采用单段最短时间运动规划方法规划前后两段位置速度和姿态速度；选择位置规划速度和姿态规划速度中的最长时间，并且通过等比例放大时间和给定时间规划同步位置和姿态；通过分段规划然后同步的方法能够有效避免因为曲率半径限制导致整个运动段运行时间过长的问题；
所述S型加减速的运动段起始速度优化：
建立S型曲线位移方程P(t)需要已知梯形加减速加速段的初速度为vb，终速度为ve，加速时间为T；在建立参数方程时，还需要考虑如下限制条件：在t＝0时刻，曲线的位移为0；曲线终止时，S型曲线的位移等于梯形曲线的位移(vb+ve)T/2；在曲线开始和终止时刻，曲线对应的速度即一阶导数需要等于梯形加减速的初速度和终止速度vb和ve；并且在开始和终止时刻，为了避免冲击，曲线P(t)的加速度和加加速度必须保证为0；所以曲线P(t)在满足以上8个条件的情况下，需要建立7次曲线方程如下：
P(t)＝At7+Bt6+Ct5+Dt4+Et3+Ft2+Gt+H (10)
将限制条件以公式表示为：
Figure FDA0003067235170000031
联立以上两式解得 ：
Figure FDA0003067235170000032
通过以上求解的参数，能够建立S型曲线位移，优化梯形加减速；
过渡段插补方法包括弧长节点集合构造方法、过渡段样条插补方法，所述弧长节点集合构造方法：对样条曲线进行插补前需要计算样条曲线的总位移，通过截距法均匀等分插补参数，在样条曲线上取点，累加曲线上的直线位移，以直代曲作为曲线的总位移，构造弧长节点Si集合的步骤为：首先在[0,1]区间内均匀选择n个点，第i个点满足ui＝i/n，再根据样条方程Pi＝F(ui)计算对应的P1,P2,…Pn；然后构造弧长节点Si集合，Si表示起点P1到Pi的预估长度，Si通过累加Si＝Si-1+|PiPi-1|计算；
所述过渡段样条插补方法：样条插补根据插补时间计算插补点，然后反算成关节值；在弧长节点集合与速度信息图的基础上，将插补时间映射成插补点的位姿；设当前的插补时间为t，已完成位移为s，首先根据插补时间确定对应的速度v，从而计算已完成位移s＝s+vT；然后将位移映射成样条曲线参数方程输入值，在弧长节点集合中寻找当前位移所在区间i，第i个节点段满足Si<s<Si+1，根据线性比例得到样条曲线参数方程输入值u；最后根据u和参数方程P＝F(u)计算插补点，再反算成对应多轴的关节值；循环以上过程，直至到达插补周期，在插补开始时初始时间和位移为0；在样条曲线插补得过程中，需要得到插补点在弧长节点集合中的位置，并且弧长节点集记录的各分段样条节点的位移呈升序排列，因此适应于二分法对其进行快速定位；将分段样条集合中间两点的位移与查找位移进行比较，如果查找的位移位于两点之间，则查找成功；否则利用两点将原集合分成前、后两个集合，如果两点大于查找的位移，则进一步查找前一集合，否则进一步查找后一集合，直至成功。
CN202010686665.8A 2020-07-16 2020-07-16 一种多运动段速度规划和插补方法 Active CN111897216B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010686665.8A CN111897216B (zh) 2020-07-16 2020-07-16 一种多运动段速度规划和插补方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010686665.8A CN111897216B (zh) 2020-07-16 2020-07-16 一种多运动段速度规划和插补方法

Publications (2)

Publication Number Publication Date
CN111897216A CN111897216A (zh) 2020-11-06
CN111897216B true CN111897216B (zh) 2021-07-02

Family

ID=73191046

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010686665.8A Active CN111897216B (zh) 2020-07-16 2020-07-16 一种多运动段速度规划和插补方法

Country Status (1)

Country Link
CN (1) CN111897216B (zh)

Families Citing this family (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112327756B (zh) * 2020-11-19 2021-08-17 杭州爱科科技股份有限公司 一种柔性材料轨迹数据处理方法、装置、设备及存储介质
CN112777488B (zh) * 2020-12-21 2022-04-12 湖南天桥嘉成智能科技有限公司 基于运行轨迹控制的行车精准定位方法
CN112706166B (zh) * 2020-12-22 2022-12-27 江苏集萃智能制造技术研究所有限公司 一种基于s型与三角函数的多关节同步轨迹规划方法
CN112659126B (zh) * 2020-12-23 2023-05-16 广东工业大学 一种基于非均匀b样条曲线的工业机器人轨迹段过渡方法
CN112363515B (zh) * 2021-01-14 2021-05-04 上海交大智邦科技有限公司 一种基于视觉定位的麦克纳姆轮式agv停位方法
CN112947558B (zh) * 2021-02-07 2023-04-07 北京理工大学 一种时空同步达到协同轨迹规划方法
CN113021356B (zh) * 2021-04-01 2022-05-03 南开大学 一种面向修锭过程的机器人轨迹规划方法及系统
CN113341884B (zh) * 2021-06-28 2022-07-08 广州中科博约医疗科技有限公司 运动目标曲线控制数据构造方法
CN113442140B (zh) * 2021-06-30 2022-05-24 同济人工智能研究院(苏州)有限公司 一种基于Bezier寻优的笛卡尔空间避障规划方法
CN113835447B (zh) * 2021-09-08 2024-04-05 中国人民解放军91776部队 一种基于贝塞尔曲线的控制攻击角度及时间的二维协同制导方法
CN114089742B (zh) * 2021-10-25 2023-09-26 广东嘉腾机器人自动化有限公司 基于路径曲率的agv行驶车速控制方法、装置及介质
CN114296398B (zh) * 2021-11-16 2024-04-05 中南大学 一种用于激光切割的高速高精度插补方法
CN113894796B (zh) * 2021-11-24 2022-12-13 伯朗特机器人股份有限公司 一种基于梯形规划的机械臂多关节轨迹时间同步方法
CN114488797A (zh) * 2022-01-07 2022-05-13 南京工程学院 一种自动开关门运动曲线规划方法及控制器和控制方法
CN114603558B (zh) * 2022-03-21 2023-12-05 合肥哈工图南智控机器人有限公司 混合空间过渡轨迹规划方法及装置
CN115922687B (zh) * 2022-05-23 2023-11-17 以诺康医疗科技（苏州）有限公司 一种基于s形速度曲线的运动轨迹规划方法及装置
CN115837669A (zh) * 2022-10-14 2023-03-24 港珠澳大桥管理局 五自由度机械臂笛卡尔空间直线轨迹规划方法及装置
CN116577994B (zh) * 2023-06-25 2024-02-27 英孚康（浙江）工业技术有限公司 冗余容错式非对称s型柔性速度曲线自适应规划方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20120098152A (ko) * 2011-02-28 2012-09-05 한국과학기술연구원 이동 로봇의 경로 생성 시스템
CN104062902A (zh) * 2014-05-15 2014-09-24 江南大学 Delta机器人时间最优轨迹规划方法
CN105573315A (zh) * 2015-12-01 2016-05-11 珞石（北京）科技有限公司 用于工业机器人的笛卡尔空间轨迹的几何平滑方法
CN106313047A (zh) * 2016-09-28 2017-01-11 华中科技大学 一种基于Bezier样条的机器人实时拐角过渡方法
CN106346478A (zh) * 2016-11-09 2017-01-25 广州视源电子科技股份有限公司 机械臂的控制方法和装置
WO2017223061A1 (en) * 2016-06-20 2017-12-28 Hypertherm, Inc. Systems and methods for planning paths to guide robots
CN110497411A (zh) * 2019-08-23 2019-11-26 华中科技大学 一种工业机器人协同运动控制方法
CN110900612A (zh) * 2019-12-17 2020-03-24 东莞市三姆森光电科技有限公司 一种位姿同步的六轴工业机器人轨迹平顺方法
CN111267098A (zh) * 2020-02-19 2020-06-12 清华大学 机器人关节层控制方法及系统

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5566288A (en) * 1994-09-02 1996-10-15 Caterpillar Inc. System and method for automatically fitting a B-spline curve to a set of data points
CN105302147B (zh) * 2015-11-06 2019-05-10 太原科技大学 一种串联机构轨迹规划方法
CN105511266B (zh) * 2016-01-07 2018-09-11 无锡信捷电气股份有限公司 基于引力搜索粒子群算法的Delta机器人轨迹规划方法
CN105773620B (zh) * 2016-04-26 2017-09-12 南京工程学院 基于倍四元数的工业机器人自由曲线的轨迹规划控制方法
CN106863306B (zh) * 2017-03-31 2020-08-18 华南理工大学 一种机器人关节空间平滑轨迹规划方法
CN109991915A (zh) * 2017-12-29 2019-07-09 深圳市优必选科技有限公司 一种机器人的运动控制方法、装置及机器人
CN108568817B (zh) * 2018-03-13 2021-05-11 同济大学 一种基于贝塞尔曲线的Delta机器人轨迹连接控制方法
CN111208807A (zh) * 2018-11-06 2020-05-29 苏州艾吉威机器人有限公司 一种基于b样条曲线的agv运动控制方法
CN109375632B (zh) * 2018-12-17 2020-03-20 清华大学 自动驾驶车辆实时轨迹规划方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20120098152A (ko) * 2011-02-28 2012-09-05 한국과학기술연구원 이동 로봇의 경로 생성 시스템
CN104062902A (zh) * 2014-05-15 2014-09-24 江南大学 Delta机器人时间最优轨迹规划方法
CN105573315A (zh) * 2015-12-01 2016-05-11 珞石（北京）科技有限公司 用于工业机器人的笛卡尔空间轨迹的几何平滑方法
WO2017223061A1 (en) * 2016-06-20 2017-12-28 Hypertherm, Inc. Systems and methods for planning paths to guide robots
CN106313047A (zh) * 2016-09-28 2017-01-11 华中科技大学 一种基于Bezier样条的机器人实时拐角过渡方法
CN106346478A (zh) * 2016-11-09 2017-01-25 广州视源电子科技股份有限公司 机械臂的控制方法和装置
CN110497411A (zh) * 2019-08-23 2019-11-26 华中科技大学 一种工业机器人协同运动控制方法
CN110900612A (zh) * 2019-12-17 2020-03-24 东莞市三姆森光电科技有限公司 一种位姿同步的六轴工业机器人轨迹平顺方法
CN111267098A (zh) * 2020-02-19 2020-06-12 清华大学 机器人关节层控制方法及系统

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
design and implementation of programming-free robot system based on plcopen;Mei Chen，等;《2019IEEE 3rd advanced information management，communicates，electronic and automation control conference》;20191013;1539-1544 *
Stacking robot interpolation algorithm research and application for irregular cigarette packs;Zhang Yi，等;《2017 IEEE 2nd advanced information technology，electronic and automation control conference（IAEAC）》;20171231;1134-1138 *
Time-optimal path tracking for articulated robot along spline curves based on master-slave strategy;Xiaojing Yu，等;《2016 International conference on control，automation and information sciences（ICCAIS）》;20161029;149-154 *
关节型工业机器人轨迹规划研究综述;李黎，等;《计算机工程与应用》;20180531;第54卷(第5期);36-50 *
基于贝塞尔曲线的机器人非时间轨迹跟踪方法;余伶俐，等;《仪器仪表学报》;20160731;第37卷(第7期);1564-1572 *
改进B样条曲线应用于6R机器人轨迹优化;董甲甲，等;《中国机械工程》;20180131;第29卷(第2期);193-200 *
粒子群算法在机械手臂B样条曲线轨迹规划中的应用;郭彤颖，等;《组合机床与自动化加工技术》;20190630;第29卷(第6期);71-77 *

Also Published As

Publication number Publication date
CN111897216A (zh) 2020-11-06

Similar Documents

Publication Publication Date Title
CN111897216B (zh) 一种多运动段速度规划和插补方法
CN112757306B (zh) 一种机械臂逆解多解选择和时间最优轨迹规划算法
CN111791236B (zh) 一种工业机器人笛卡尔空间轨迹过渡方法
CN110757455B (zh) 一种速度规划方法
CN108568817B (zh) 一种基于贝塞尔曲线的Delta机器人轨迹连接控制方法
CN109623810B (zh) 一种机器人平滑的时间最优轨迹规划的方法
CN109397292B (zh) 一种基于解析解的7自由度机械臂控制方法与系统
CN111580453B (zh) 基于plc的提高二维圆弧插补轨迹精度的实现方法
CN109683615B (zh) 机器人连续运动的路径的速度前瞻方法及机器人控制器
CN112486101B (zh) Nurbs曲线自适应前瞻插补方法
CN112975992B (zh) 一种误差可控的机器人轨迹同步优化方法
CN112083727B (zh) 基于速度障碍物的多自主体系统分布式避碰编队控制方法
CN111390902A (zh) 轨迹规划方法、轨迹规划装置、设备和存储介质
CN111283683B (zh) 一种机器人视觉特征规划轨迹的伺服跟踪加速收敛方法
CN113189938A (zh) 一种连续加工路径的速度曲线规划方法
CN113478489A (zh) 一种机械臂轨迹规划方法
CN111633668A (zh) 一种用于机器人加工三维自由曲面的运动控制方法
CN114815743A (zh) 一种数控机床的曲线插补方法、系统及存储介质
CN113442170A (zh) 一种对机械臂路径冗余节点的反向拆分计算的方法及系统
CN113650011B (zh) 一种机械臂拼接路径规划方法及装置
CN113276116B (zh) 一种误差可控的机器人轨迹同步过渡方法
Wang et al. Jerk-optimal trajectory planning for stewart platform in joint space
CN115302498A (zh) 工程机械作业装置连续轨迹位姿的规划方法、规划装置、电子设备及存储介质
CN113124875A (zh) 一种路径导航方法
CN115958597B (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