CN113703433A - 机器人运动轨迹的速度规划方法及装置 - Google Patents
机器人运动轨迹的速度规划方法及装置 Download PDFInfo
- Publication number
- CN113703433A CN113703433A CN202010436903.XA CN202010436903A CN113703433A CN 113703433 A CN113703433 A CN 113703433A CN 202010436903 A CN202010436903 A CN 202010436903A CN 113703433 A CN113703433 A CN 113703433A
- Authority
- CN
- China
- Prior art keywords
- speed
- preset
- acceleration
- planning
- starting point
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 230000001133 acceleration Effects 0.000 claims description 347
- 238000006073 displacement reaction Methods 0.000 claims description 167
- 230000036461 convulsion Effects 0.000 claims description 63
- 238000004590 computer program Methods 0.000 claims description 8
- 230000009467 reduction Effects 0.000 claims description 4
- 230000008859 change Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 5
- 230000006870 function Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
本申请公开了一种机器人运动轨迹的速度规划方法及装置,该方法包括:获取机器人速度规划的预设规划参数;根据预设规划参数计算机器人的起点速度调整比例,以及机器人的终点速度调整比例;根据起点速度调整比例和终点速度调整比例规划机器人运动轨迹的速度。通过上述方式,本申请能够对输入的预设规划参数进行调整,根据调整后的参数进行速度规划,提高速度规划的适应性。
Description
技术领域
本申请涉及机器人技术领域,具体涉及一种机器人运动轨迹的速度规划方法及装置。
背景技术
在机器人系统中,为保证机器人在起动与停止时不产生冲击、失步、超程或振荡,需要采用专门的加减速控制算法,使得系统可以在各种情况下平滑而又准确地停在指定位置,因而需要对运行轨迹做速度规划,规划方式多为T型或S型,即加速度为常数规划和加加速度为常数规划;规划的重点是计算从起点到终点之间的连续位置、速度和/或加速度的信息,速度与加速度受到最大值的约束。
在传统的S型或T型速度规划中,起点和终点的状态作为输入条件必须满足,但是在使用时,有可能会给终点速度过大的输入值,使得位移从起点变化到终点的过程中,速度达不到终点的速度,此时传统的S型或T型速度规划会返回速度规划失败,但是在某些应用下,仅需要在位移到达预设位移时,其速度尽可能地趋近于终点速度即可,并不需求与终点速度完全一致。
发明内容
本申请提供一种机器人运动轨迹的速度规划方法及装置,能够对输入的预设规划参数进行调整,根据调整后的参数进行速度规划,提高速度规划的适应性。
为解决上述技术问题,本申请采用的技术方案是:提供一种机器人运动轨迹的速度规划方法,该方法包括:获取机器人速度规划的预设规划参数;根据预设规划参数计算机器人的起点速度调整比例,以及机器人的终点速度调整比例;根据起点速度调整比例和终点速度调整比例规划机器人运动轨迹的速度。
基于本申请第一方面,本申请第一方面的第一种实现方式中,预设规划参数包括预设起点速度、预设起点加速度、预设终点速度、预设终点加速度、预设最大速度、预设最大加速度以及预设最大加加速度;该方法还包括:根据预设起点速度、预设起点加速度、预设最大加加速度与预设最大速度,得到起点速度调整比例;根据预设终点速度、预设终点加速度、预设最大加加速度与预设最大速度,得到终点速度调整比例。
通过对起点的速度与加速度、终点的速度与加速度、预设最大速度以及预设最大加加速度进行处理,可得到相应的起点速度调整比例与终点速度调整比例,以便对规划参数进行更新。
基于本申请第一方面至第一方面的第一种实现方式,本申请第一方面的第二种实现方式包括:
当预设起点加速度大于零时,采用公式(1-1)计算出起点速度调整比例:
其中,ks为起点速度调整比例,vs为预设起点速度,Jmax为预设最大加加速度,as为预设起点加速度;
当预设起点加速度小于零时,采用公式(1-2)计算出起点速度调整比例:
其中,vmax为预设最大速度。
利用起点速度调整比例、起点速度、起点加速度以及预设最大加加速度之间的函数关系,可以计算出起点速度调整比例;还可利用起点速度调整比例、起点速度、起点加速度、预设最大加加速度以及预设最大速度之间的函数关系,计算出起点速度调整比例。
基于本申请第一方面至第一方面的第一种实现方式至第二种实现方式,本申请第一方面的第三种实现方式包括:
当预设终点加速度大于零时,采用公式(1-3)计算出终点速度调整比例:
其中,ke为终点速度调整比例,ve为预设终点速度,ae为预设终点加速度,Jmax为预设最大加加速度;
当预设终点加速度小于零时,采用公式(1-4)计算出终点速度调整比例:
其中,vmax为预设最大速度。
利用终点速度调整比例、终点速度、终点加速度以及预设最大加加速度之间的函数关系,可以计算出终点速度调整比例;还可利用终点速度调整比例、终点速度、终点加速度、预设最大加加速度以及预设最大速度之间的函数关系,计算出终点速度调整比例。
基于本申请第一方面至第一方面的第一种实现方式至第三种实现方式,本申请第一方面的第四种实现方式包括:将起点速度调整比例与预设起点速度相乘,得到第一起点速度,以及将起点速度调整比例的平方与预设起点加速度相乘,得到第一起点加速度;将终点速度调整比例与预设终点速度相乘,得到第一终点速度,以及将终点速度调整比例的平方与预设终点加速度相乘,得到第一终点加速度。
通过将调整比例与速度相乘,实现对起点速度与终点速度的更新,并将调整比例的平方与加速度相乘,实现对起点加速度与终点加速度的更新。
基于本申请第一方面至第一方面的第一种实现方式至第四种实现方式,本申请第一方面的第五种实现方式中预设规划参数还包括预设期望速度、预设起点位置以及预设终点位置,利用起点速度调整比例和终点速度调整比例分别对预设规划参数进行更新,得到更新的规划参数;利用更新的规划参数进行三段式速度规划;判断利用三段式速度规划得到的位移是否小于预设期望位移,其中,预设期望位移为从预设起点位置到预设终点位置的位移;若利用三段式速度规划得到的位移大于预设期望位移,则调整预设起点速度、预设起点加速度、预设终点速度以及预设终点加速度,并利用调整后的参数进行速度规划;若利用三段式速度规划得到的位移小于预设期望位移,则利用二分法在预设速度搜索区间中查找满足最大速度等于预设期望位移的六段式速度规划,其中,预设速度搜索区间的下限值为0,预设速度搜索区间的上限值为预设期望速度;判断利用六段式速度规划得到的位移是否超过预设期望位移;若利用六段式速度规划得到的位移超过预设期望位移,则利用二分法在预设速度搜索区间中查找满足最大速度等于预设期望速度,且规划得到的位移等于预设期望位移的六段式速度规划;若利用六段式速度规划得到的位移未超过预设期望位移,则根据位移差计算匀速段的轨迹时长,并进行七段式速度规划,其中,位移差为利用六段式速度规划得到的位移与预设期望位移之间的差值。
通过对更新的规划参数进行处理,判断需要采取哪一种规划方式,以便得到最佳的规划结果。
基于本申请第一方面至第一方面的第一种实现方式至第五种实现方式,本申请第一方面的第六种实现方式包括:根据第一终点加速度与第一起点加速度之间的差值以及预设最大加加速度,计算出加加速度和运动时间;利用加加速度和运动时间进行规划,判断规划得到的最大速度是否超过预设期望速度;若规划得到的最大速度超过预设期望速度,则对加加速度进行处理,得到处理后的加加速度,进行从预设起点位置到预设终点位置的两段式速度规划;若规划得到的最大速度未超过预设期望速度,则执行进行从预设起点位置到预设终点位置的两段式速度规划的步骤;判断利用两段式速度规划得到的最大加速度是否超过预设最大加速度;若利用两段式速度规划得到的最大加速度超过预设最大加速度,则利用预设最大加速度进行两段式速度规划,计算出速度差,并利用速度差、预设起点速度、预设终点速度、预设最大加速度以及预设最大加加速度,计算匀加速段/匀减速段的轨迹时长,进行三段式速度规划;若利用两段式速度规划得到的最大加速度未超过预设最大加速度,则输出利用两段式速度规划得到的轨迹时长和最大加加速度。
通过速度规划,可得到轨迹时长与最大速度,根据最大速度可判断是进行两段式速度规划还是三段式速度规划,以便进行更恰当地规划。
基于本申请第一方面至第一方面的第一种实现方式至第六种实现方式,本申请第一方面的第七种实现方式包括:
采用公式(1-5)计算出进行两段式速度规划得到的最大加速度:
采用公式(1-6)计算出速度差:
采用公式(1-7)计算出匀加速段/匀减速段的轨迹时长:
其中,a0为利用两段式速度规划得到的最大加速度,Dv为速度差,t为匀加速段/匀加速段的轨迹时长。
通过两段式速度规划的公式,可得到进行两段式速度规划得到的最大加速度以及进行三段式速度规划得到的匀加速段/匀加速段的轨迹时长。
基于本申请第一方面至第一方面的第一种实现方式至第七种实现方式,本申请第一方面的第八种实现方式包括:将第一起点速度减小至第二起点速度,并第一终点速度减小至第二预设终点速度,其中,第二起点速度对应的起点加速度为0,第二终点速度对应的终点加速度为0;判断第二起点速度是否大于第二终点速度;若第二起点速度大于第二终点速度,则计算起点速度的下降比例,并利用起点速度的下降比例对预设起点速度与预设起点加速度进行更新;若第二起点速度不大于第二终点速度,则判断第二终点速度是否大于第二起点速度;若第二终点速度大于第二起点速度,则计算终点速度的下降比例,并利用终点速度的下降比例对预设终点速度与预设终点加速度进行更新;利用更新的速度与更新的加速度进行三段式速度规划;判断利用三段式速度规划得到的位移是否等于预设期望位移;若利用三段式速度规划得到的位移不等于预设期望位移,则判断利用三段式速度规划得到的位移是否小于预设期望位移;若利用三段式速度规划得到的位移小于预设期望位移,则利用超加加速度进行速度规划;若利用三段式速度规划得到的位移大于预设期望位移,则利用二分法在预设速度搜索区间内进行搜索规划,以使得规划后的位移等于预设期望位移,且规划出来的最大速度等于预设期望速度。
在三段式速度规划不符合要求时,对起点的速度与加速度以及终点的速度与加速度进行下拉,并利用下拉后的参数进行速度规划,以得到符合要求的规划方式与输出值。
基于本申请第一方面至第一方面的第一种实现方式至第八种实现方式,本申请第一方面的第九种实现方式包括:将预设最大加加速度乘以预设倍数,得到上限加加速度;在预设最大加加速度与上限加加速度之间进行搜索规划,以使得规划得到的位移等于预设期望位移。
通过对最大加加速度进行放大,以进行速度规划。
基于本申请第一方面至第一方面的第一种实现方式至第九种实现方式,本申请第一方面的第十种实现方式包括:
采用公式(1-8)计算出第二起点速度:
采用公式(1-9)计算出第二终点速度:
其中,vs2为第二起点速度,vs1为第一起点速度,as1为第一起点加速度,ve2为第二终点速度,ve1为第一终点速度,ae1为第一终点加速度。
基于本申请第一方面至第一方面的第一种实现方式至第十种实现方式,本申请第一方面的第十一种实现方式包括:判断起点速度调整比例是否小于第一预设值;若起点速度调整比例小于第一预设值,则输出起点速度调整比例;判断终点速度调整比例是否小于第二预设值;若终点速度调整比例小于第二预设值,则输出终点速度调整比例。
在计算出调整比例之后,通过对利用公式计算出来的起点速度调整比例与第一预设值之间的大小关系进行判断,选择恰当的起点速度调整比例;通过对利用公式计算出来的终点速度调整比例与第二预设值之间的大小关系进行判断,选择恰当的终点速度调整比例。
基于本申请第一方面至第一方面的第一种实现方式至第十一种实现方式,本申请第一方面的第十二种实现方式包括:判断预设规划参数是否满足预设条件;若预设规划参数满足预设条件,则判断预设起点位置是否小于预设终点位置;若预设起点位置大于预设终点位置,则将预设起点位置与预设终点位置交换。
在进行规划之前,先判断规划参数的合法性,以便更好地进行规划。
基于本申请第一方面至第一方面的第一种实现方式至第十二种实现方式,本申请第一方面的第十三种实现方式包括:判断预设起点速度、预设终点速度以及预设期望速度是否大于预设最大速度,并判断预设起点加速度与预设终点加速度是否大于预设最大加速度。
为了判断输入的参数是否合法,将起点速度、终点速度以及预设期望速度与预设最大速度进行比较,同时将预设终点加速度、预设终点加速度与预设最大加速度进行比较,在输入合法时,才进行速度规划。
基于本申请第一方面至第一方面的第一种实现方式至第十三种实现方式,本申请第一方面的第十四种实现方式中,预设规划参数还包括起点速度标识值与终点速度的标识值,该方法还包括:判断起点速度的标识值是否为有效值;以及判断终点速度的标识值是否为有效值。
本申请第二方面提供一种机器人运动轨迹的速度规划装置,该速度规划装置包括互相连接的存储器和处理器,其中,存储器用于存储计算机程序,计算机程序在被处理器执行时,用于实现上述实施例中的机器人运动轨迹的速度规划方法。
本申请第三方面提供一种计算机存储介质,该计算机存储介质用于存储计算机程序,计算机程序在被处理器执行时,用于实现上述实施例中的机器人运动轨迹的速度规划方法。
通过上述方案,本申请的有益效果是:在根据预设规划参数进行规划无法得到满足预设规划参数的速度时,能够利用起点速度调整比例与终点速度调整比例对输入的预设规划参数进行调整,然后根据调整后的参数进行速度规划,使得规划后的速度尽可能与预设规划参数中的速度接近,可以解决传统S型或T型在速度不可达时返回规划失败的问题,能够在输入的预设规划参数不可达时,尽可能地趋近于输入的参数,提高速度规划的适应性。
附图说明
为了更清楚地说明本申请实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。其中:
图1是T型曲线加减速规划的加速度、速度以及位移的示意图;
图2是S型曲线加减速规划的加速度、速度以及位移的示意图;
图3是本申请提供的机器人运动轨迹的速度规划方法一实施例的流程示意图;
图4是本申请提供的机器人运动轨迹的速度规划方法另一实施例的流程示意图;
图5是图4所示的实施例中步骤402的流程示意图;
图6是图4所示的实施例中步骤404的流程示意图;
图7是图4所示的实施例中步骤406的流程示意图;
图8是本申请提供的机器人运动轨迹的速度规划装置一实施例的结构示意图;
图9是本申请提供的机器人运动轨迹的速度规划装置另一实施例的结构示意图;
图10是本申请提供的计算机存储介质一实施例的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请一部分实施例,而不是全部实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性的劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
为了对机器人进行控制,使得机器人按照设定的速度进行运动,最终停在指定的位置,需要机器人的运动轨迹进行速度规划,目前常用的速度规划方式包括T型速度规划与S型速度规划,具体如下:
T型曲线加减速控制是让速度曲线线性且连续变化,如图1所示,一共分为三个时间段:匀加速阶段、匀速阶段和匀减速阶段,这三个时间段的时间长度分别记作T1~T3,每个时间段对应的位移分别为l1~l3,每个时间段的末点时间记作t1~t3,最大加速度amax及最大减速度dmax可由机床电机参数决定,用户可指定初速度fs、目标速度f、终点速度fe以及总位移L;根据这些条件计算t1~t3的值,即可完成T型曲线加减速规划,于是有:
f-fs=aT1
f-fe=dT3
l2=fT2
l1+l2+l3=L
其中,0<a≤amax,0<d≤dmax,为保证效率,一般取a=amax,d=dmax,可以计算得到T1~T3,如下式所示:
对速度关于时间进行积分可以得到每段的位移,如下式所示:
S型曲线加减速控制是让轨迹的加速度呈线性且连续变化,如图2所示,对加速度进行积分可得到速度与时间的方程,对速度进行积分可得到位移与时间的方程;S型曲线加减速规划一共分为七个时间段:加加速、匀加速、减加速、匀速、减加速、匀减速和减减速,这七个时间段的时间长度分别记作T1~T7,每个时间段的末点时间记作t1~t7,加速度的导数称为加加速度,其由电机自身的参数决定,最大加速度A和最大减速度D也由电机给出,初速度为fs,目标速度为f,终点速度为fe,总位移为L,根据这些条件,计算T1~T7的值,即可完成S型曲线加减速规划。
一般情况下,设定T1(加加速时间长度)=T3(减加速时间长度),T5(减加速时间长度)=T7(减减速时间长度),得到如下公式:
其中,Ja与Jd分别为加加速度的最大值与最小值,由以上公式可推出速度关于时间的函数,进行积分可得到距离关于时间的函数,实现了对轨迹的速度规划。
本申请所提供的速度规划记作A型速度规划,由于本申请的速度规划设计为节拍优化服务,在节拍优化的计算中考虑了负载力矩,因此加速和减速能力有可能不同,A型速度规划支持加速能力与减速能力分开的速度规划。
请参阅图3,图3是本申请提供的机器人运动轨迹的速度规划方法一实施例的流程示意图,该方法包括:
步骤31:获取机器人速度规划的预设规划参数。
可先获取对机器人的速度进行规划时所需的预设规划参数,该预设规划参数可为用户输入的参数,其可为初始参数,在根据该参数进行规划,无法得到符合条件的运动轨迹时,对其进行自适应调整。
步骤32:根据预设规划参数计算机器人的起点速度调整比例,以及机器人的终点速度调整比例。
可在获取到预设规划参数后,对该预设规划参数进行处理,从而得到起点速度的调整比例和终点速度的调整比例。
步骤33:根据起点速度调整比例和终点速度调整比例规划机器人运动轨迹的速度。
可对速度与加速度下降的方式进行规定,由于速度和加速度均要下降,因而速度与加速度可一起下降,在计算出起点速度调整比例后,将起点速度调整比例与起点的速度与加速度相乘,得到更新的起点速度与起点加速度;将终点速度调整比例与终点的速度与加速度相乘,得到更新的终点速度与终点加速度。
然后利用更新的规划参数进行速度规划,得到机器人的运动轨迹对应的参数,该运动轨迹可由多段轨迹组成,每段轨迹对应不同的属性,对于某一段轨迹来说,其对应的速度、加速度或加加速度可保持不变。
每段轨迹对应的参数包括轨迹时长,在对预设规划参数进行更新之后,可利用更新的参数进行速度规划,比如,可进行三段式速度规划或S型速度规划;由于S型速度规划使用分段函数,具有速度、加速度连续变化以及加加速度为常数且可以控制的优点,为了稳定性,本申请中速度和加速度均不跳变,采用加加速度为常数的分段式速度规划。
本申请的方案的应用场景为:现有技术中的速度规划方法无法利用预设规划参数进行速度规划,该速度规划方法能够对输入的预设规划参数进行调整,根据调整后的参数进行速度规划,可以解决传统S型或T型在速度不可达时无法规划的问题,能够在输入的参数不可达时,尽可能地趋近于输入的参数,拓展速度规划的使用范围。
请参阅图4,图4是本申请提供的机器人运动轨迹的速度规划方法另一实施例的流程示意图,该方法包括:
步骤401:获取机器人速度规划的预设规划参数。
该预设规划参数包括预设起点速度、预设起点加速度、起点速度的标识值、预设终点速度、预设终点加速度、终点速度的标识值、预设最大速度、预设最大加速度、预设最大加加速度、预设起点位置、预设终点位置与预设期望速度。
在一些情况下速度是可以降的,但是有些情况速度无法下降,比如前一条轨迹已经进入到插补器了,此时起点速度就无法下降了,因此需要开放两个布尔量描述起点和终点是否可以变化,即设置起点速度的标识值与终点速度的标识值。
起点速度的标识值用于表示预设起点速度与预设起点加速度是否允许调整,终点速度的标识值用于表示预设终点速度与预设终点加速度是否允许调整;用户可预先设置速度的标识值(包括起点速度的标识值与终点速度的标识值),如果速度的标识值为有效值,则表示能够对速度进行调整;如果速度的标识值为无效值,则表示禁止对速度进行调整;具体地,可将有效值设置为1,无效值设置为0。
在接收到预设规划参数后,在开始规划时,可判断预设规划参数是否满足预设条件;若预设规划参数不满足预设条件,则直接返回输入错误,提醒用户输入不合法,以使得用户重新输入参数;若预设规划参数满足预设条件,则判断预设起点位置与预设终点位置是否相同;如果预设起点位置与预设终点位置相同,则无法进行规划,提醒用户重新输入参数;如果预设终点位置小于预设起点位置,由于在规划中速度不能为负,因而如果起点大于终点则规划失败,此时可将预设起点位置与预设终点位置交换,将此次规划看作是终点向起点移动。
进一步地,判断预设起点速度、预设终点速度以及预设期望速度是否大于预设最大速度,并判断预设起点加速度与预设终点加速度是否大于预设最大加速度;若预设起点速度、预设终点速度以及预设期望速度中的至少一个大于预设最大速度,或者预设终点加速度与预设终点加速度中的至少一个大于预设最大加速度,则输入不合法;若预设起点速度、预设终点速度以及预设期望速度均小于预设最大速度,且预设终点加速度与预设终点加速度均小于预设最大加速度,则输入合法。
在验证完预设规划参数的合法性后,可验证预设起点与预设终点的速度、加速度的关系,由于速度受速度范围(0到预设最大速度的闭区间)的约束,加速度在变化的过程中,速度跟随变化,因此正的加速度使得速度上升,可能会超过预设最大速度;负的加速度使得速度下降,导致速度小于零,为了避免出现这种情况,先验证速度和加速度的关系。
步骤402:根据预设起点速度、预设起点加速度、预设最大加加速度与预设最大速度,得到起点速度调整比例,并根据预设终点速度、预设终点加速度、预设最大加加速度与预设最大速度,得到终点速度调整比例。
如果根据起点速度的标识值与终点速度的标识值判断出起点速度和/或终点速度允许调整,则可对预设规划参数进行处理,从而得到起点速度的调整比例和/或终点速度的调整比例。
在一具体的实施例中,如图5所示,可采用如下步骤,得到起点速度调整比例与终点速度调整比例:
步骤501:判断预设规划参数中是否具有预设起点加速度。
若预设规划参数中不具有预设起点加速度,则判断预设规划参数中是否具有预设终点加速度,即执行步骤509。
步骤502:若预设规划参数中具有预设起点加速度,则判断预设起点加速度是否大于零。
步骤503:若预设起点加速度大于零,则利用起点速度调整比例与预设起点速度、预设起点加速度以及预设最大加加速度,得到起点速度调整比例。
由于加速度大于零时应确保速度不超预设最大速度,加速度小于零时应保证速度不会降至零以下,因此计算公式不同,在预设起点加速度大于零时,可采用公式(1-1)计算出起点速度调整比例:
其中,ks为起点速度调整比例,vs为预设起点速度,Jmax为预设最大加加速度,as为预设起点加速度;在计算出起点速度调整比例之后,可执行步骤506。
步骤504:若预设起点加速度小于或等于零,则判断预设起点加速度是否小于零。
步骤505:若预设起点加速度小于零,则利用起点速度调整比例与预设起点速度、预设起点加速度、预设最大加加速度以及预设最大速度,得到起点速度调整比例。
采用公式(1-2)计算出起点速度调整比例:
其中,vmax为预设最大速度。
步骤506:判断起点速度调整比例是否小于第一预设值。
该第一预设值可以为1,如果计算出来的起点速度调整比例大于1,则可将起点速度调整比例设置为1,即不对起点速度进行调整。
步骤507:若起点速度调整比例小于第一预设值,则判断起点速度是否允许调整。
判断起点速度的标识值是否为有效值;若起点速度的标识值为有效值,则表示起点的速度可变;若起点速度的标识值为无效值,则表示起点的速度不可变,此时可返回调整失败。
步骤508:若起点速度允许调整,则输出起点速度调整比例。
在起点速度允许调整时,可直接输出起点速度调整比例;在起点速度不允许调整时,返回调整失败。
步骤509:若起点速度调整比例大于或等于第一预设值,则判断预设规划参数中是否具有预设终点加速度。
若预设规划参数中不具有预设终点加速度,则无法得到调整比例。
步骤510:若预设规划参数中具有预设终点加速度,则判断预设终点加速度是否大于零。
步骤511:若预设终点加速度大于零,则利用终点速度调整比例与预设终点速度、预设终点加速度以及预设最大加加速度,得到终点速度调整比例。
采用公式(1-3)计算出终点速度调整比例:
其中,ke为终点速度调整比例,ve为预设终点速度,ae为预设终点加速度。
步骤512:若预设终点加速度小于或等于零,则判断预设终点加速度是否小于零。
步骤513:若预设终点加速度小于零,则利用终点速度调整比例与预设终点速度、预设终点加速度、预设最大加加速度以及预设最大速度,得到终点速度调整比例。
采用公式(1-4)计算出终点速度调整比例:
步骤514:判断终点速度调整比例是否小于第二预设值。
该第二预设值可以为1,如果计算出来的终点速度调整比例大于1,则可将终点速度调整比例设置为1,即不对终点速度进行调整。
步骤515:若终点速度调整比例小于第二预设值,则判断终点速度是否允许调整。
判断终点速度的标识值是否为有效值;若终点速度的标识值为有效值,则表示终点的速度可变;若终点速度的标识值为无效值,则表示终点速度不可变,此时可返回调整失败。
步骤516:若终点速度允许调整,则输出终点速度调整比例。
步骤403:利用起点速度调整比例和终点速度调整比例分别对预设规划参数进行更新,得到更新的规划参数。
由于加速度与速度之间存在一阶导的关系,它们下降的倍率同样赋予一阶导的关系,比如,设置速度下降k倍,加速度下降k2倍,因而可将起点速度调整比例与预设起点速度相乘,得到第一起点速度,并将起点速度调整比例的平方与预设起点加速度相乘,得到第一起点加速度;将终点速度调整比例与预设终点速度相乘,得到第一终点速度,并将终点速度调整比例的平方与预设终点加速度相乘,得到第一起点加速度。
步骤404:利用更新的规划参数进行三段式速度规划。
在对起点速度和终点速度进行了调整之后,如图6所示,可采用步骤601-步骤607进行三段式速度规划:
步骤601:根据第一终点加速度与第一起点加速度之间的差值以及预设最大加加速度,计算出加加速度和运动时间。
将第一终点加速度减去第一起点加速度,得到加速度差值,如果加速度差值为正值,则加加速度等于最大加加速度;如果加速度差值为负值,则加加速度等于最大加加速度乘以-1,运动时间为加速度差值与加加速度的比值。
步骤602:利用加加速度和运动时间进行规划,判断规划得到的最大速度是否超过预设期望速度。
在计算出加加速度与运动时间之后,可对加加速度关于时间进行二次积分,并将第一起点速度与积分得到的值叠加,得到规划出来的最大速度,并判断规划出来的最大速度与预设期望速度之间的大小关系,若规划得到的最大速度超过预设期望速度,则执行步骤604。
步骤603:若规划得到的最大速度超过预设期望速度,则对加加速度进行处理,得到处理后的加加速度。
将加加速度乘以-1,以向预设终点位置减速运行。
步骤604:进行从预设起点位置到预设终点位置的两段式速度规划。
采用公式(1-5)计算出进行两段式速度规划得到的最大加速度:
其中,a0为利用两段式速度规划得到的最大加速度,v1为第一起点速度,v2为第一终点速度,a1为第一起点加速度,a2为第一终点加速度,Jmax为最大加加速度。
利用公式(1-10)与(1-11)可计算出每段轨迹对应的轨迹时长,如下所示:
其中,t1为第一段轨迹对应的轨迹时长,t2为第二段轨迹对应的轨迹时长。
步骤605:判断利用两段式速度规划得到的最大加速度是否超过预设最大加速度。
在计算出两段式速度规划得到的最大加速度之后,可判断该最大加速度与预设最大加速度之间的大小关系。
步骤606:若利用两段式速度规划得到的最大加速度超过预设最大加速度,则利用预设最大加速度进行两段式速度规划,计算出速度差,并利用速度差、预设起点速度、预设终点速度、预设最大加速度以及预设最大加加速度,计算匀加速段/匀减速段的轨迹时长,进行三段式速度规划。
采用公式(1-6)计算出速度差:
其中,Dv为速度差。
采用公式(1-7)计算出匀加速段/匀加速段的轨迹时长:
其中,t为匀加速段/匀加减段的轨迹时长。
利用公式(1-12)与(1-13)可计算出每段轨迹对应的轨迹时长:
其中,t1为第一段轨迹对应的轨迹时长,t2为第三段轨迹对应的轨迹时长,利用每段轨迹的时长与加加速度可以计算出从预设起点位置开始的位移。
步骤607:若利用两段式速度规划得到的最大加速度未超过预设最大加速度,则输出利用两段式速度规划得到的轨迹时长和最大加加速度。
如果利用两段式速度规划得到的最大加速度未超过预设最大加速度,则直接输出步骤604计算出的参数。
步骤405:判断利用三段式速度规划得到的位移是否小于预设期望位移。
预设期望位移为从预设起点位置到预设终点位置的位移,在确保了预设规划参数均没问题后,可开始速度规划,利用三段式速度规划可得到从第一起点速度、第一起点加速度规划到第一终点速度、第一终点加速度所产生的位移。
步骤406:若利用三段式速度规划得到的位移大于预设期望位移,则调整预设起点速度、预设起点加速度、预设终点速度以及预设终点加速度,并利用调整后的参数进行速度规划。
如果判断出三段式速度规划得到的位移大于预设期望位移,则表明在从第一起点速度、第一起点加速度变化到第一终点速度、第一终点加速度的过程中,位移已经超过预设期望位移,此时可对第一起点速度、第一终点速度、第一起点加速度以及第一终点加速度进行下拉,即进行降低操作,具体如图7所示:
步骤701:判断起点速度或终点速度是否允许调整。
步骤702:若起点速度与终点速度均不允许调整,则利用超加加速度进行速度规划。
如果起点速度与终点速度均不允许调整,可使得加加速度超过最大加加速度,从而使得速度、加速度变化得更快,达到规划需求;具体地,将预设最大加加速度乘以预设倍数,得到上限加加速度;在预设最大加加速度与上限加加速度之间进行搜索规划,以使得规划得到的位移等于预设期望位移;例如,预设倍数为10,将最大加加速度*10,然后进行规划,规划出来的加加速度为[最大加加速度,最大加加速度*10]中的值。
步骤703:若起点速度或终点速度允许调整,则将第一起点速度减小至第二起点速度,并第一终点速度减小至第二预设终点速度。
在计算下降比例的过程中,目标是计算产生位移的最小值,如果起点与终点都没有加速度,可以把比较大的速度值下拉到比较小的速度值,这样位移为0,但是不可能所有情况中起点与终点都没有加速度,因此需要考虑加速度对于速度的影响,由于本申请中加速度可连续变化,因此位移最小值应该需要先把加速度降为0,在降为0的过程中又会产生速度变化,因此计算加速度为0时速度的值;在具体实现时为了不引入加速度趋近于零的误差,可对加速度是否为零进行判断以分别处理。
第二起点速度对应的起点加速度为0,第二终点速度对应的终点加速度为0,采用公式(1-8)计算出第二起点速度:
采用公式(1-9)计算出第二终点速度:
其中,vs2为第二起点速度,vs1为第一起点速度,as1为第一起点加速度,ve2为第二终点速度,ve1为第一终点速度,ae1为第一终点加速度。
步骤704:判断第二起点速度是否大于第二终点速度。
步骤705:若第二起点速度大于第二终点速度,则判断起点速度是否允许调整。
步骤706:若起点速度允许调整,则计算起点速度的下降比例,并利用起点速度的下降比例对预设起点速度与预设起点加速度进行更新。
步骤707:若起点速度不允许调整,则将终点速度上调第一预设倍数,并利用第一预设倍数对终点速度进行更新。
第一预设倍数可以为1.05,可将第一预设倍数与第二起点速度相乘,将第一预设倍数的平方与第二起点加速度相乘,以进行更新。
步骤708:若第二起点速度不大于第二终点速度,则判断第二终点速度是否大于第二起点速度。
步骤709:若第二终点速度大于第二起点速度,则判断终点速度是否允许调整。
步骤710:若终点速度允许调整,则计算终点速度的下降比例,并利用终点速度的下降比例对预设终点速度与预设终点加速度进行更新。
步骤711:若终点速度不允许调整,则将预设起点参数上调第二预设倍数,并利用第二预设倍数对预设终点速度与预设终点加速度进行更新。
第二预设倍数可以为1.05,可将第二预设倍数与第二终点速度相乘,将第二预设倍数的平方与第二终点加速度相乘,以进行更新。
步骤712:利用更新的速度与更新的加速度进行三段式速度规划。
根据更新的起点速度、起点加速度、终点速度以及终点加速度进行规划,判断规划出来的位移与预设期望位移之间的大小关系。
步骤713:判断利用三段式速度规划得到的位移是否等于预设期望位移。
如果利用三段式速度规划得到的位移等于预设期望位移,则正好得到预设期望位移,不需要再搜索,速度规划成功。
步骤714:若利用三段式速度规划得到的位移不等于预设期望位移,则判断利用三段式速度规划得到的位移是否小于预设期望位移。
若利用三段式速度规划得到的位移小于预设期望位移,则执行步骤702。
步骤715:若利用三段式速度规划得到的位移大于预设期望位移,则利用二分法在预设速度搜索区间内进行搜索规划,以使得规划后的位移等于预设期望位移,且规划出来的最大速度等于预设期望速度。
预设速度搜索区间的下限值为0,预设速度搜索区间的上限值为预设期望速度;为了增加规划的成功率,还可以将上限值进行上调,比如允许预设期望速度上调5%。
步骤407:若利用三段式速度规划得到的位移小于预设期望位移,则利用二分法在预设速度搜索区间中查找满足最大速度等于预设期望位移的六段式速度规划。
如果利用三段式速度规划得到的位移小于预设期望位移,则表明可以完成七段式速度规划。
步骤408:判断利用六段式速度规划得到的位移是否超过预设期望位移。
步骤409:若利用六段式速度规划得到的位移超过预设期望位移,则利用二分法在预设速度搜索区间中查找满足最大速度等于预设期望速度,且规划得到的位移等于预设期望位移的六段式速度规划。
步骤410:若利用六段式速度规划得到的位移未超过预设期望位移,则根据位移差计算匀速段的轨迹时长,并进行七段式速度规划。
位移差为利用六段式速度规划得到的位移与预设期望位移之间的差值,将位移差与预设期望速度相除,得到匀速段的轨迹时长,并在[0,预设期望速度]之间查找满足预设期望位移的速度。
为了满足起点或者终点的速度与加速度能够变化,本实施例提供可一种区别于传统S型的速度规划,可寻找与起点速度以及终点速度最接近的速度规划,经过速度规划可得到每段轨迹的轨迹时长、起点速度的调整比例以及终点速度的调整比例,A型速度规划可规划最多七段曲线的参数,计算出每个时间点所对应的位置、速度以及加速度,且在一定约束内允许输入的参数变化。
请参阅图8,图8是本申请提供的机器人运动轨迹的速度规划装置一实施例的结构示意图,机器人运动轨迹的速度规划装置80包括互相连接的存储器81和处理器82,存储器81用于存储计算机程序,计算机程序在被处理器82执行时,用于实现上述实施例中的速度规划方法。
速度规划装置80能够在速度、加速度以及加加速度满足约束的前提下,满足预设期望位移,在起点或终点的速度/加速度不可达时,通过对加速度和/或速度进行下调,使得可以从起点速度变化到终点速度。
请参阅图9,图9是本申请提供的机器人运动轨迹的速度规划装置另一实施例的结构示意图,机器人运动轨迹的速度规划装置90包括互相连接的获取模块91和处理模块92。
获取模块91用于获取机器人速度规划的预设规划参数;根据预设规划参数计算机器人的起点速度调整比例,以及机器人的终点速度调整比例;根据起点速度调整比例和终点速度调整比例规划机器人运动轨迹的速度。
处理模块92还用于根据预设起点速度、预设起点加速度、预设最大加加速度与预设最大速度,得到起点速度调整比例;根据预设终点速度、预设终点加速度、预设最大加加速度与预设最大速度,得到终点速度调整比例,其中,预设规划参数包括预设起点速度、预设起点加速度、预设终点速度、预设终点加速度、预设最大速度、预设最大加速度以及预设最大加加速度。
处理模块92还用于将起点速度调整比例与预设起点速度相乘,得到第一起点速度,以及将起点速度调整比例的平方与预设起点加速度相乘,得到第一起点加速度;将终点速度调整比例与预设终点速度相乘,得到第一终点速度,以及将终点速度调整比例的平方与预设终点加速度相乘,得到第一终点加速度。
处理模块92还用于将起点速度调整比例与预设起点速度相乘,得到第一起点速度,并将起点速度调整比例的平方与预设起点加速度相乘,得到第一起点加速度;将终点速度调整比例与预设终点速度相乘,得到第一终点速度,并将终点速度调整比例的平方与预设终点加速度相乘,得到第一起点加速度。
处理模块92还用于在预设起点加速度大于零时,采用公式(1-1)计算出起点速度调整比例:
其中,ks为起点速度调整比例,vs为预设起点速度,Jmax为预设最大加加速度,as为预设起点加速度;
在预设起点加速度小于零时,采用公式(1-2)计算出起点速度调整比例:
其中,vmax为预设最大速度。
处理模块92还用于在预设起点加速度大于零时,采用公式(1-3)计算出终点速度调整比例:
其中,ke为终点速度调整比例,ve为预设终点速度,ae为预设终点加速度,Jmax为预设最大加加速度;
在预设起点加速度小于零时,采用公式(1-4)计算出终点速度调整比例:
其中,vmax为预设最大速度。
处理模块92还用于利用起点速度调整比例和终点速度调整比例对预设规划参数进行更新,得到更新的规划参数;利用更新的规划参数进行三段式速度规划;判断利用三段式速度规划得到的位移是否小于预设期望位移,其中,预设期望位移为从预设起点位置到预设终点位置的位移;若利用三段式速度规划得到的位移大于预设期望位移,则调整预设起点速度、预设起点加速度、预设终点速度以及预设终点加速度,并利用调整后的参数进行速度规划;若利用三段式速度规划得到的位移小于预设期望位移,则利用二分法在预设速度搜索区间中查找满足最大速度等于预设期望位移的六段式速度规划,其中,预设速度搜索区间的下限值为0,预设速度搜索区间的上限值为预设期望速度;判断利用六段式速度规划得到的位移是否超过预设期望位移;若利用六段式速度规划得到的位移超过预设期望位移,则利用二分法在预设速度搜索区间中查找满足最大速度等于预设期望速度,且规划得到的位移等于预设期望位移的六段式速度规划;若利用六段式速度规划得到的位移未超过预设期望位移,则根据位移差计算匀速段的轨迹时长,并进行七段式速度规划,其中,位移差为利用六段式速度规划得到的位移与预设期望位移之间的差值,预设规划参数还包括预设期望速度,运动轨迹包括三段轨迹。
处理模块92还用于判断预设起点加速度是否大于零;若预设起点加速度大于零,则采用公式(1-1)计算出起点速度调整比例;若预设起点加速度小于或等于零,则判断预设起点加速度是否小于零;若预设起点加速度小于零,则采用公式(1-2)计算出起点速度调整比例;判断预设终点加速度是否大于零;若预设终点加速度大于零,则采用公式(1-3)计算出终点速度调整比例;若预设终点加速度小于或等于零,则判断预设终点加速度是否小于零;若预设终点加速度小于零,则采用公式(1-4)计算出终点速度调整比例。
处理模块92还用于判断起点速度调整比例是否小于第一预设值;若起点速度调整比例小于第一预设值,则输出起点速度调整比例;判断终点速度调整比例是否小于第二预设值;若终点速度调整比例小于第二预设值,则输出终点速度调整比例。
处理模块92还用于根据第一终点加速度与第一起点加速度之间的差值以及预设最大加加速度,计算出加加速度和运动时间;利用加加速度和运动时间进行规划,判断规划得到的最大速度是否超过预设期望速度;若规划得到的最大速度超过预设期望速度,则对加加速度进行处理,得到处理后的加加速度,进行从预设起点位置到预设终点位置的两段式速度规划;若规划得到的最大速度未超过预设期望速度,则执行进行从预设起点位置到预设终点位置的两段式速度规划的步骤;判断利用两段式速度规划得到的最大加速度是否超过预设最大加速度;若利用两段式速度规划得到的最大加速度超过预设最大加速度,则利用预设最大加速度进行两段式速度规划,计算出速度差,并利用速度差、预设起点速度、预设终点速度、预设最大加速度以及预设最大加加速度,计算匀加速段/匀减速段的轨迹时长,进行三段式速度规划;若利用两段式速度规划得到的最大加速度未超过预设最大加速度,则输出利用两段式速度规划得到的轨迹时长和最大加加速度。
处理模块92还用于采用公式(1-5)计算出进行两段式速度规划得到的最大加速度:
采用公式(1-6)计算出速度差:
采用公式(1-7)计算出匀加速段/匀减速段的轨迹时长:
其中,a0为利用两段式速度规划得到的最大加速度,Dv为速度差,t为匀加速段/匀加速段的轨迹时长。
处理模块92还用于将第一起点速度减小至第二起点速度,并第一终点速度减小至第二预设终点速度,其中,第二起点速度对应的起点加速度为0,第二终点速度对应的终点加速度为0;判断第二起点速度是否大于第二终点速度;若第二起点速度大于第二终点速度,则计算起点速度的下降比例,并利用起点速度的下降比例对预设起点速度与预设起点加速度进行更新;若第二起点速度不大于第二终点速度,则判断第二终点速度是否大于第二起点速度;若第二终点速度大于第二起点速度,则计算终点速度的下降比例,并利用终点速度的下降比例对预设终点速度与预设终点加速度进行更新;利用更新的速度与更新的加速度进行三段式速度规划;判断利用三段式速度规划得到的位移是否等于预设期望位移;若利用三段式速度规划得到的位移不等于预设期望位移,则判断利用三段式速度规划得到的位移是否小于预设期望位移;若利用三段式速度规划得到的位移小于预设期望位移,则利用超加加速度进行速度规划;若利用三段式速度规划得到的位移大于预设期望位移,则利用二分法在预设速度搜索区间内进行搜索规划,以使得规划后的位移等于预设期望位移,且规划出来的最大速度等于预设期望速度。
处理模块92还用于将预设最大加加速度乘以预设倍数,得到上限加加速度;在预设最大加加速度与上限加加速度之间进行搜索规划,以使得规划得到的位移等于预设期望位移。
处理模块92还用于采用公式(1-8)计算出第二起点速度:
采用公式(1-9)计算出第二终点速度:
其中,vs2为第二起点速度,vs1为第一起点速度,as1为第一起点加速度,ve2为第二终点速度,ve1为第一终点速度,ae1为第一终点加速度。
处理模块92还用于判断预设规划参数是否满足预设条件;若预设规划参数满足预设条件,则判断预设起点位置是否小于预设终点位置;若预设起点位置大于预设终点位置,则将预设起点位置与预设终点位置交换。
处理模块92还用于判断起点速度的标识值是否为有效值;以及判断终点速度的标识值是否为有效值,其中,预设规划参数还包括起点速度的标识值与终点速度的标识值。
请参阅图10,图10是本申请提供的计算机存储介质一实施例的结构示意图,计算机存储介质100用于存储计算机程序101,计算机程序101在被处理器执行时,用于实现上述实施例中的速度规划方法。
计算机存储介质100可以是服务端、U盘、移动硬盘、只读存储器(ROM,Read-OnlyMemory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
在本申请所提供的几个实施方式中,应该理解到,所揭露的方法以及设备,可以通过其它的方式实现。例如,以上所描述的设备实施方式仅仅是示意性的,例如,模块或单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。
作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施方式方案的目的。
另外,在本申请各个实施方式中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
以上仅为本申请的实施例,并非因此限制本申请的专利范围,凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换,或直接或间接运用在其他相关的技术领域,均同理包括在本申请的专利保护范围内。
Claims (12)
1.一种机器人运动轨迹的速度规划方法,其特征在于,包括:
获取机器人速度规划的预设规划参数;
根据所述预设规划参数计算所述机器人的起点速度调整比例,以及所述机器人的终点速度调整比例;
根据所述起点速度调整比例和所述终点速度调整比例规划机器人运动轨迹的速度。
2.根据权利要求1所述的方法,其特征在于,所述预设规划参数包括预设起点速度、预设起点加速度、预设终点速度、预设终点加速度、预设最大速度、预设最大加速度以及预设最大加加速度,所述根据所述预设规划参数计算所述机器人的起点速度调整比例,以及所述机器人的终点速度调整比例,包括:
根据所述预设起点速度、所述预设起点加速度、所述预设最大加加速度与所述预设最大速度,得到所述起点速度调整比例;
根据所述预设终点速度、所述预设终点加速度、所述预设最大加加速度与所述预设最大速度,得到所述终点速度调整比例。
5.根据权利要求2至4任一项所述的方法,其特征在于,所述根据所述起点速度调整比例和所述终点速度调整比例规划机器人运动轨迹的速度,包括:
将所述起点速度调整比例与所述预设起点速度相乘,得到第一起点速度,以及将所述起点速度调整比例的平方与所述预设起点加速度相乘,得到第一起点加速度;
将所述终点速度调整比例与所述预设终点速度相乘,得到第一终点速度,以及将所述终点速度调整比例的平方与所述预设终点加速度相乘,得到第一终点加速度。
6.根据权利要求5所述的方法,其特征在于,所述预设规划参数还包括预设期望速度、预设起点位置以及预设终点位置,所述根据所述起点速度调整比例和所述终点速度调整比例规划机器人运动轨迹的速度,包括:
利用所述起点速度调整比例和所述终点速度调整比例分别对所述预设规划参数进行更新,得到更新的规划参数;
利用所述更新的规划参数进行三段式速度规划;
判断利用所述三段式速度规划得到的位移是否小于预设期望位移,其中,所述预设期望位移为从所述预设起点位置到所述预设终点位置的位移;
若利用所述三段式速度规划得到的位移大于所述预设期望位移,则调整所述预设起点速度、所述预设起点加速度、所述预设终点速度以及所述预设终点加速度,并利用调整后的参数进行速度规划;
若利用所述三段式速度规划得到的位移小于所述预设期望位移,则利用二分法在预设速度搜索区间中查找满足最大速度等于所述预设期望位移的六段式速度规划,其中,所述预设速度搜索区间的下限值为0,所述预设速度搜索区间的上限值为所述预设期望速度;
判断利用所述六段式速度规划得到的位移是否超过所述预设期望位移;
若利用所述六段式速度规划得到的位移超过所述预设期望位移,则利用二分法在所述预设速度搜索区间中查找满足最大速度等于所述预设期望速度,且规划得到的位移等于所述预设期望位移的六段式速度规划;
若利用所述六段式速度规划得到的位移未超过所述预设期望位移,则根据位移差计算匀速段的轨迹时长,并进行七段式速度规划,其中,所述位移差为利用所述六段式速度规划得到的位移与所述预设期望位移之间的差值。
7.根据权利要求6所述的方法,其特征在于,所述利用所述更新的规划参数进行三段式速度规划,包括:
根据所述第一终点加速度与所述第一起点加速度之间的差值以及所述预设最大加加速度,计算出加加速度和运动时间;
利用所述加加速度和所述运动时间进行规划,判断规划得到的最大速度是否超过所述预设期望速度;
若规划得到的最大速度超过所述预设期望速度,则对所述加加速度进行处理,得到处理后的加加速度,进行从所述预设起点位置到所述预设终点位置的两段式速度规划;
若规划得到的最大速度未超过所述预设期望速度,则执行所述进行从所述预设起点位置到所述预设终点位置的两段式速度规划的步骤;
判断利用所述两段式速度规划得到的最大加速度是否超过所述预设最大加速度;
若利用所述两段式速度规划得到的最大加速度超过所述预设最大加速度,则利用所述预设最大加速度进行两段式速度规划,计算出速度差,并利用所述速度差、所述预设起点速度、所述预设终点速度、所述预设最大加速度以及所述预设最大加加速度,计算匀加速段/匀减速段的轨迹时长,进行三段式速度规划;
若利用所述两段式速度规划得到的最大加速度未超过所述预设最大加速度,则输出利用所述两段式速度规划得到的轨迹时长和最大加加速度。
9.根据权利要求6所述的方法,其特征在于,所述调整所述预设起点速度、所述预设起点加速度、所述预设终点速度以及所述预设终点加速度,并利用调整后的参数进行速度规划,包括:
将所述第一起点速度减小至第二起点速度,并所述第一终点速度减小至第二预设终点速度,其中,所述第二起点速度对应的起点加速度为0,所述第二终点速度对应的终点加速度为0;
判断所述第二起点速度是否大于所述第二终点速度;
若所述第二起点速度大于所述第二终点速度,则计算起点速度的下降比例,并利用所述起点速度的下降比例对所述预设起点速度与所述预设起点加速度进行更新;
若所述第二起点速度不大于所述第二终点速度,则判断所述第二终点速度是否大于所述第二起点速度;若所述第二终点速度大于所述第二起点速度,则计算终点速度的下降比例,并利用所述终点速度的下降比例对所述预设终点速度与所述预设终点加速度进行更新;
利用更新的速度与更新的加速度进行三段式速度规划;
判断利用所述三段式速度规划得到的位移是否等于所述预设期望位移;
若利用所述三段式速度规划得到的位移不等于所述预设期望位移,则判断利用所述三段式速度规划得到的位移是否小于所述预设期望位移;
若利用所述三段式速度规划得到的位移小于所述预设期望位移,则利用超加加速度进行速度规划;
若利用所述三段式速度规划得到的位移大于所述预设期望位移,则利用二分法在所述预设速度搜索区间内进行搜索规划,以使得规划后的位移等于所述预设期望位移,且规划出来的最大速度等于所述预设期望速度。
10.根据权利要求9所述的方法,其特征在于,所述利用超加加速度进行速度规划,包括:
将所述预设最大加加速度乘以预设倍数,得到上限加加速度;
在所述预设最大加加速度与所述上限加加速度之间进行搜索规划,以使得规划得到的位移等于所述预设期望位移。
12.一种机器人运动轨迹的速度规划装置,其特征在于,包括互相连接的存储器和处理器,其中,所述存储器用于存储计算机程序,所述计算机程序在被所述处理器执行时,用于实现权利要求1-11中任一项所述的机器人运动轨迹的速度规划方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010436903.XA CN113703433B (zh) | 2020-05-21 | 2020-05-21 | 机器人运动轨迹的速度规划方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010436903.XA CN113703433B (zh) | 2020-05-21 | 2020-05-21 | 机器人运动轨迹的速度规划方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113703433A true CN113703433A (zh) | 2021-11-26 |
CN113703433B CN113703433B (zh) | 2024-05-14 |
Family
ID=78646216
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010436903.XA Active CN113703433B (zh) | 2020-05-21 | 2020-05-21 | 机器人运动轨迹的速度规划方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113703433B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115946121A (zh) * | 2023-01-31 | 2023-04-11 | 北京立迈胜控制技术有限责任公司 | 一种用于机械臂基于动态规划的传送带跟踪方法 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103970073A (zh) * | 2013-01-24 | 2014-08-06 | 北京配天大富精密机械有限公司 | 一种用于数控系统的加减速规划方法、装置及数控机床 |
CN104076742A (zh) * | 2014-06-20 | 2014-10-01 | 北京航空航天大学 | 一种连续多类型曲线段混合插补三次多项式进给速度前瞻规划方法 |
CN105867376A (zh) * | 2016-04-12 | 2016-08-17 | 南京工程学院 | 基于三角函数的工业机器人轨迹规划升降速控制方法 |
WO2017219639A1 (zh) * | 2016-06-20 | 2017-12-28 | 广州视源电子科技股份有限公司 | 一种机械臂的运动轨迹规划方法、装置及机器人 |
CN107561933A (zh) * | 2017-08-21 | 2018-01-09 | 深圳市同川科技有限公司 | 速度调节方法、装置、计算机设备和存储介质 |
JP2018030220A (ja) * | 2016-08-26 | 2018-03-01 | 株式会社デンソーウェーブ | ロボット制御装置、及びロボット制御プログラム |
CN109188915A (zh) * | 2018-11-05 | 2019-01-11 | 南开大学 | 内嵌运动性能调节机制的速度规划方法 |
-
2020
- 2020-05-21 CN CN202010436903.XA patent/CN113703433B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103970073A (zh) * | 2013-01-24 | 2014-08-06 | 北京配天大富精密机械有限公司 | 一种用于数控系统的加减速规划方法、装置及数控机床 |
CN104076742A (zh) * | 2014-06-20 | 2014-10-01 | 北京航空航天大学 | 一种连续多类型曲线段混合插补三次多项式进给速度前瞻规划方法 |
CN105867376A (zh) * | 2016-04-12 | 2016-08-17 | 南京工程学院 | 基于三角函数的工业机器人轨迹规划升降速控制方法 |
WO2017219639A1 (zh) * | 2016-06-20 | 2017-12-28 | 广州视源电子科技股份有限公司 | 一种机械臂的运动轨迹规划方法、装置及机器人 |
JP2018030220A (ja) * | 2016-08-26 | 2018-03-01 | 株式会社デンソーウェーブ | ロボット制御装置、及びロボット制御プログラム |
CN107561933A (zh) * | 2017-08-21 | 2018-01-09 | 深圳市同川科技有限公司 | 速度调节方法、装置、计算机设备和存储介质 |
CN109188915A (zh) * | 2018-11-05 | 2019-01-11 | 南开大学 | 内嵌运动性能调节机制的速度规划方法 |
Non-Patent Citations (1)
Title |
---|
潘海鸿;等: "全类型非对称七段式S型曲线加减速控制算法研究", 机械科学与技术, vol. 37, no. 12, pages 1928 - 1935 * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115946121A (zh) * | 2023-01-31 | 2023-04-11 | 北京立迈胜控制技术有限责任公司 | 一种用于机械臂基于动态规划的传送带跟踪方法 |
CN115946121B (zh) * | 2023-01-31 | 2023-06-27 | 北京立迈胜控制技术有限责任公司 | 一种用于机械臂基于动态规划的传送带跟踪方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113703433B (zh) | 2024-05-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108549328B (zh) | 自适应速度规划方法及系统 | |
CN108569336B (zh) | 在动力学约束下基于车辆运动学模型转向控制方法 | |
CN101849215B (zh) | 用于满足运动控制系统中的结束条件的方法和系统 | |
CN109415089A (zh) | 控制车辆的控制器和方法以及非暂时性计算机可读存储器 | |
CN111158318B (zh) | 一种非对称性四次曲线柔性加减速规划方法 | |
CN104678899A (zh) | 曲线速度规划方法、装置及其数控加工路径数据处理方法 | |
CN112965501A (zh) | 一种自动驾驶速度规划方法及装置 | |
CN107844058A (zh) | 一种运动曲线离散动态规划方法 | |
Lu et al. | An offline predictive feedrate scheduling method for parametric interpolation considering the constraints in trajectory and drive systems | |
CN113703433A (zh) | 机器人运动轨迹的速度规划方法及装置 | |
CN111739342A (zh) | 用于避让侧前方车辆的方法、装置、介质以及车辆 | |
CN113253678A (zh) | 精密运动平台的运动轨迹规划方法 | |
CN109656200B (zh) | 机台的柔性加减速控制方法及系统 | |
US10216168B2 (en) | Operation aware control of processing machine with redundant actuators | |
CN115303256A (zh) | 自动泊车的路径跟踪控制方法、装置和计算机设备 | |
US8924013B2 (en) | Method and system for path planning and controlling | |
CN115149873B (zh) | 基于神经网络模型的伺服电机组控制方法及相关设备 | |
CN112082555A (zh) | 狭窄通道环境下基于自适应动态窗口法的曲率一致性路径规划算法 | |
CN114237157B (zh) | 数据驱动的机床进给伺服系统机器学习建模方法及系统 | |
CN113296515A (zh) | 一种双独立电驱动车辆显式模型预测路径跟踪方法 | |
CN113050648A (zh) | 一种机器人避障方法和系统 | |
Mi et al. | Path planning of indoor mobile robot based on improved A* algorithm incorporating RRT and JPS | |
CN109634280A (zh) | 一种悬浮脚踏器以及适用其的控制方法 | |
CN102236338B (zh) | 四阶定值发生器的实现方法 | |
CN117873136B (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 |