一种时间最优的在线S型加减速规划方法与流程

本发明涉及工业机器人运动控制领域、伺服电机速度控制领域，移动式机器人在线控制特别是涉及速度规划方法与机器人运动规划方面的在线s型加减速规划方法。

背景技术：

传统轨迹规划方法通常不考虑初始运动状态的初始加速度，初始速度不等于零的情况，因为对于初始运动状态不等于零的情况轨迹规划及其困难，并且计算复杂，不能满足实时性的要求。对于高性能机器人控制系统来说，无论当前机器人处于何种运动状态，通过将外部传感器信号作为机器人控制器输入，机器人都应该对输入信号立即做出反应。

技术实现要素：

为了克服传统轨迹规划方法不能根据任意初始运动状态和结束状态进行轨迹规划。本发明提供一种时间最优的在线s型加减速规划方法，该方法能够用于机器人轨迹的自动在线生成。在考虑速度、加速度和加加速度约束条件和机器人运动始末状态的条件下，以时间最短为优化目标进行直接求解。

本发明的目的是通过以下技术方案实现的：

一种时间最优的在线s型加减速规划方法，包括以下步骤：

(1)输入在线运动规划所需要的参数：输入首末运动状态和运动约束条件，首末运动状态包括初始位置ps、初始速度vs、初始加速度as、结束位置ptrgt、结束速度vtrgt，运动约束条件包括最大加加速度jmax、最大加速度amax、最大速度vmax；

(2)不考虑位移计算初始加速度曲线类型typea^*：根据步骤(1)输入的参数，在不考虑位移约束的情况下，满足其它约束条件计算初始加速度曲线类型typea^*；

首先判断在最短时间内由初始速度变化到结束速度过程中，加速度a是否需要达到极限；若不需要达到加速度极限值，则根据首末运动状态计算typea^*，将初始加速度曲线类型typea^*分为四种情况；若加速度需要达到极限值，同样根据首末运动状态计算typea^*也对应四种情况；从上述共计的八种情况之中选出一种作为typea^*；

(3)由步骤(2)返回的typea^*计算初始加加速度js；首先针对步骤(2)返回的typea^*计算typea^*对应的加速度曲线的位移δp^*，考虑计算方便性，将上述八种typea^*分为四类；

(4)确定s型加速度曲线类型typea；若步骤(3)计算的δp^*等于目标位移δp，则typea等于typea^*；否则需要由步骤(3)计算的js计算typea；以规划总时间最短原则，考虑是否存在匀加速区域、是否存在匀速区域、以及加减速过渡次数将s型加速度曲线类型分为八种，通过以下原则从八种s型加速度曲线类型之中选出一种作为typea；分类判断原则如下：

原则a：假定在运动规划过程中，速度恰好达到速度极限且最后达到目标速度，比较此时的位移和实际目标位移大小；

原则b：假定在运动规划过程中，加速度恰好达到极限且最后达到目标速度，比较此时的位移和实际目标位移大小；

原则c：假定在运动规划过程中，加速度已达到极限，速度恰好达到极限且最终达到目标速度，比较此时的位移和实际目标位移大小；

原则d：假定在运动规划过程中，加速度和速度都恰好达到极限，比较此时的速度和实际目标速度的大小；

原则e：假定在运动规划过程中，加速度恰好达到极限，且最后达到目标速度，比较此时的速度和实际目标速度的大小还有此时的位移和实际目标位移大小；

通过以上几种原则的结合使用，最终从以上八种s型加速度曲线类型中选出一种满足总体规划时间最短，并能同时满足运动约束条件；

(5)计算加速度曲线分段参数：由步骤(4)计算得到typea，针对不同加速度曲线列写不同方程组进行求解得到加速度曲线每一段对应的时间；

对于不存在匀速运动的情况，分别将加速部分时间和减速部分时间通过非线性方程组求解；对于存在匀加速或匀减速段的情况，运动过程中一定达到速度约束条件，即中间速度极值vp已知，则与不存在匀加速或匀减速的情况，计算方法相同，此时为线性方程组；从而解出加速时间、减速时间和匀速时间；终得到加速度曲线分段参数[t1a，t1b，t1c，t2a，t2b，t2c，totg]；

(6)根据加速度曲线分段参数生成运动轨迹：由加速度曲线类型typea和步骤(5)计算的加速度曲线分段参数代入s型加减速曲线公式即可生成运动轨迹。

进一步的，步骤(2)中，若不需要达到加速度极限值，则根据首末运动状态判断速度变化趋势将初始加速度曲线类型typea^*分为四种情况，对应的四种情况分别为：a、初始加速度曲线先加加速再加减速，记为+postri；b、初始加速度曲线先加减速再减减速，记为-negtri；c、初始加速度曲线先减减速，再加加速然后减加速，记为-postri；d、初始加速度曲线先减加速，再加减速然后减减速，记为+negtri；

对于加速度需要达到极限值的情况，根据首末运动状态判断速度变化趋势，对应四种情况为：e、初始加速度曲线先加加速，再匀加速最后减加速，记为+postrap；f、初始加速度曲线先加减速，再匀减速最后减减速，记为-negtrap；g、初始加速度曲线先减减速，再加加速然后匀加速，最后减加速，记为-postrap；h、初始加速度曲线先减加速，再加减速然后匀减速，最后减减速，记为+negtrap。

进一步的，步骤(3)中，在计算对应的加速度曲线的位移δp^*方面，将八种typea^*分为四类，其中+postri、-negtri一类，-postri、+negtri一类，+postrap、-negtrap一类和-postrap、+negtrap一类；

如果目标位移δp等于δp^*，则typea等于typea^*；如果目标位移δp不等于δp^*，则typea^*无效；若目标位移δp大于δp^*，则js等于jmax；若目标位移δp小于δp^*，则js等于-jmax；对于第二类和第四类减加速、减减速区域可能存在速度超过速度最大值约束，此时规定js＝0，即输入参数错误，无法在运动约束条件下规划。

进一步的，步骤(4)中考虑规划总时间最短原则，将加速度曲线类型typea分为八种包括：tritri、traptri、tritrap、traptrap、trizerotri、trapzerotri、trizerotrap、trapzerotrap；其中tri表示先加加速再减加速或先加减速再减减速；trap表示存在匀加速或匀减速的情况，即先加加速，再匀加速减加速过程；zero表示加速度等于零，即匀速段过程。

与现有技术相比，本发明的技术方案所带来的有益效果是：

1.本发明方法能根据满足运动约束条件(jmax，amax，vmax)的任意初始运动状态(as，vs，ps)和考虑结束加速度为零的运动状态(atrgt，vtrgt，ptrgt)进行s型加减速规划，生成满足运动约束条件且时间最优的唯一s型加减速速度轨迹。

2.本发明详细考虑了s型加减速曲线的各种情况，极大简化了在任意初始状态下s型加减速曲线的求解算法，能够在微秒级时间内计算s型加减速轨迹参数并生成运动轨迹，实时性较高，适用于在线实时规划轨迹。由于规划的轨迹在时间上最优，故能提高工作效率。

3.本发明可以根据传感器返回信号实时规划运动轨迹。为在未知环境下机器人自动规划运动路径提供了一种方法。有助于实现机器人的智能化和灵活化。

附图说明

图1表示时间最优的在线s型加减速规划方法原理示意图。

图2表示不考虑目标位移情况下的八种初始加速度曲线类型typea^*示意图。其中横轴为时间t，纵轴为加速度a；图中1、+postri，2、-negtri，3、-postri，4、+negtri，5、+postrap，6、-negtrap，7、-postrap，8、+postrap。

图3表示八种s型加速度曲线类型typea示意图。(as∈[-amax，amax]，js＝±jmax)；其中横轴为时间t，纵轴为加速度a，图中1、tritri，2、traptri，3、tritrap，4、traptrap，5、trizerotri，6、trapzerotri，7、trizerotrap，8、trapzerotrap。

图4(a)至图4(f)表示一些实验参数下的结果图。其中横坐标为时间t，图4(a)至图4(f)的上、中、下子图的纵坐标分别为加速度，速度和位移。

具体实施方式

以下结合附图和具体实施例子对本发明作进一步详细说明。应当理解，此处所描述的具体实施例子仅仅用以解释本发明，并不用于限定本发明。

本发明提供的线速度规划方法。其输入参数包括运动约束条件，首末运动状态，和插补时间dt。运动约束条件包括最大加加速度，最大加速度，和最大速度，记为(jmax，amax，vmax)。初始运动状态包括初始加速度，初始速度，初始位移，记为(as，vs，ps)。初始运动状态应满足实际条件，否则运动规划可能会超出运动约束条件。

结束运动状态包括目标加速度为零，目标速度，目标位移，记为(atrgt，vtrgt，ptrgt)。其输出参数包括规划最优时间totg，按照插补时间生成的实时加加速度j(t)，加速度a(t)，速度v(t)，和位移p(t)。

在线s型加减速规划方法技术方案具体如图1所示，以下是对图1的详细说明。：

步骤1，输入在线运动规划所需要的参数：

首先以数组的形式输入首末运动状态和运动约束条件，首末运动状态包括初始位置ps、初始速度vs、初始加速度as、结束位置ptrgt、结束速度vtrgt，约束条件包括最大加加速度jmax、最大加速度amax、最大速度值vmax。记为input＝[ps，vs，as，ptrgt，vtrgt，vmax，amax，jmax]。

步骤2，不考虑位移计算初始加速度曲线类型typea^*：

根据输入参数，在不考虑位移的情况下，计算满足其它约束条件的初始加速度曲线类型typea^*。

判断在最短时间内速度由初始速度变化到结束速度过程中，加速度a是否需要达到加速度极限。若不需要达到加速度极限值，则根据运动首末状态来计算typea^*。

不考虑位移的初始加速度曲线类型typea^*分为四种情况，如图2所示。其中1、速度曲线先加加速再加减速，记为+postri；2、速度曲线先加减速再减减速，记为-negtri；3、速度曲线先减减速，再加加速然后减加速，记为-postri；4、速度曲线先减加速，再加减速然后减减速，记为+negtri。

当首末速度之差δv与初始加速度as符号不同，且|δv|大于初始加速度降到零时对应的速度变化量时则为+postri或-negtri，否则为-postri或+negtri。

对于加速度需要达到极限值的情况，同样根据运动首末状态来计算typea^*，同样也对应四种情况见图2。5、速度曲线先加加速，再匀加速最后减加速，记为+postrap；6、速度曲线先加减速，再匀减速最后减减速，记为-negtrap；7、速度曲线先减减速，再加加速然后匀加速，最后减加速，记为-postrap；8、速度曲线先减加速，再加减速然后匀减速，最后减减速，记为+negtrap。

同上，当首末速度之差δv与初始加速度as符号不同，且|δv|大于初始加速度降到零时对应的速度变化量时则为+postrap或-negtrap，否则为-postrap或+negtrap。这样就可以从图2的八种情况中选出1种初始加速度曲线类型typea^*。具体见步骤3说明。

步骤3，由步骤2返回的typea^*计算初始加加速度js：

根据typea^*计算typea^*对应的加速度曲线的位移δp^*。上面八种初始加速度曲线类型在计算位移方面可以分为四类，即+postri、-negtri一类，-postri、+negtri一类，+postrap、-negtrap一类，-postrap、+negtrap一类。

根据步骤2计算的typea^*，计算typea^*对应的加速度曲线的位移δp^*。如果目标位移δp等于δp^*，则typea等于typea^*；如果目标位移δp不等于δpmin，则typea^*无效；若目标位移δp大于δp^*，则js等于jmax；若目标位移δp小于δp^*，则js等于-jmax。对于第2类和第4类中间速度可能会超过速度最大值约束，此时规定js＝0，即输入参数错误，无法在运动约束条件下规划。

具体的，本实施例所述的加速度曲线与时间轴的交点的个数是不考虑初始加速度等于零的特殊情况。如图2所示，其中第一幅图可以表示为+postri或-negtri，包括初始加速度大于零，初始加加速度大于零、初始加速度小于零，初始加加速度小于零的情况，其特点是加速度最大值未达加速度上限值，且加速度曲线与时间轴交点只有一个。第二幅图可以表示为-postri或+negtri，包括初始加速度小于零，初始加加速度大于零、初始加速度大于零，初始加加速度小于零的情况，其特点是加速度曲线与时间轴的交点有两个且最大加速度小于加速度上限值。第三幅图可以表示为+postrap或-negtrap，包括初始速度和初始加速度符号相同且中间某时刻加速度已经达到加速度上限值，加速度曲线与时间轴只有一个交点。第四幅图可以表示为-postrap或+postrap，包括初始加速度与初始加加速度符号不同，中间某时刻加速度已达加速度上限且加速度曲线与时间轴交点有两个。

步骤4，确定s型加速度曲线类型typea：

若步骤(3)计算的δp^*等于目标位移δp，则typea等于typea^*；否则需要由步骤3计算的js计算typea。考虑规划总时间最短原则，将typea分为八种tritri、traptri、tritrap、traptrap、trizerotri、trapzerotri、trizerotrap、trapzerotrap，分别如图3所示。

图3所示的八种s型加速度曲线类型同时包含了图2所示的几种情况。其中第一类曲线可以用tritri表示，其特点是初始加速度与初始加加速度符号相同，最大加速度小于加速度上限值，加速度曲线与时间轴交点有两个且不存在加速度恒等于零的时间区间。第二类曲线可以用traptri表示，其特点是初始加速度与初始加加速度符号相同，加速区域某时刻最大加速度已达加速度上限值，加速度曲线与时间轴交点有两个且不存在加速度恒等于零的时间区间。第三类曲线可以用tritrap表示，其特点是初始加速度与初始加加速度符号相同，减速区域某时刻最大加速度已达加速度上限值，加速度曲线与时间轴交点有两个且不存在加速度恒等于零的时间区间。第四类曲线可以用traptrap表示，其特点是初始加速度与初始加加速度符号相同，加速和减速区域分别在某时刻最大加速度已达加速度上限值，加速度曲线与时间轴交点有两个且不存在加速度恒等于零的时间区间。第五类加速度曲线可以表示为trizerotri，相对于第一类加速度曲线其特征在于，其存在加速度等于零的时间区间，即存在匀速运动区间。第六类加速度曲线表示为trapzerotri，其相对于第二种加速度曲线的明显特征在于其包括匀速运动区间。第七种加速度曲线可以表示为trizerotrap，其区别于第三种加速度曲线的明显特征在于中间段存在匀速运动区间。第八种加速度曲线可以表示为trapzerotrap，其区别于第四种加速度曲线的明显特征在于中间段存在匀速区间。

选取加速度曲线类型的分类判断原则：

原则a：假定在运动规划过程中，速度恰好达到速度极限且最后达到目标速度，比较此时的位移和实际目标位移大小。

原则b：假定在运动规划过程中，加速度恰好达到极限且最后达到目标速度，比较此时的位移和实际目标位移大小。

原则c：假定在运动规划过程中，加速度已达到极限，速度恰好达到极限且最终达到目标速度，比较此时的位移和实际目标大小。

原则d：假定在运动规划过程中，加速度和速度都恰好达到极限，比较此时的速度和实际目标速度的大小。

原则e：假定在运动规划过程中，加速度恰好达到极限，且最后达到目标速度，比较此时的速度和实际目标速度的大小还有此时的位移和实际目标位移大小。

通过以上多种原则的结合使用，最终可以从以上8种加速度曲线中选出一种曲线满足总体规划时间最短，并且能同时满足运动约束条件。

以初始加加速度js＞0且加速度恰好达到最大加速度然后加速度降为零的过程的速度大于速度最大值为例。

判断1：如果初始速度小于最终目标速度，则进入判断2。

判断2：由原则a判断，如果加速度曲线为tritri，比较此时的位移和实际目标位移大小。若此时的位移小于实际目标位移则typea等于trizerotri。否则为tritri。如果不符合判断1条件则进入判断3。

判断3：如果减速部分最小加速度达到加速度下限值且最终速度达到目标速度，则判断匀速段速度是否小于速度极限值。若判断3小于速度极限值，则进入判断4。

判断4：由原则b判断，假设加速度曲线类型为tritri，比较最终位移和实际目标位移大小。若此时的位移大于实际目标位移则typea等于tritri，否则进入判断5。

判断5：由原则d判断最终速度是否大于目标速度。若小于目标速度，则typea等于trizerotri，否则进入判断6。

判断6：同样由原则a比较规划位移与目标位移大小，若规划位移大于目标位移，则typea等于tritrap，否则为trizerotrap。

若判断3大于等于速度极限值，则进入判断7。

判断7：由原则a，减速部分为tri型，比较此时的位移和实际目标位移大小，若大于目标位移，则typea等于tritri，否则为trizerotri。

步骤5，计算加速度曲线分段参数：

由步骤4计算得到typea，针对不同加速度曲线列写不同方程组进行求解得到加速度曲线每一段对应的时间。

对于不存在匀加速或匀减速的情况，分别将加速部分时间和减速部分时间用t10，和t21表示，这样可以将以上八种加速度曲线简化为5个通用方程和2个区别方程，其中有7个未知量。根据方程求解t10和t21。

以typea等于tritri为例，引入中间变量s1、s2、δp1a、δp1b、vp、tb。其中vp为中间速度极值。

tb＝-|as/j|

通用方程

s1＝vp-vs

s2＝vtrgt-vp

δp1a＝(jstb³/6+tbvb)+(t10-tb)(vb+vp)/2

δp1b＝δp-t21(vp+vtrgt)/2

δp1a-δp1b＝0

区别方程

对于存在匀加速或匀减速段的情况，运动过程中一定达到速度约束条件，即中间速度极值vp已知，则与不存则匀加速或匀减速的情况计算方法相同，此时可以直接求解加速部分时间t10和减速时间t21和匀加(减)速时间tm。根据t10，t21，tm可以求得加速度曲线分段参数

[t1a，t1b，t1c，t2a，t2b，t2c，totg]

其中t1a表示加加速段结尾时刻，t1b表示匀加速段结尾时刻，t1c为减加速段结尾时刻，t2a为匀速段结尾时刻，t2b为加减速段结尾时刻，t2c为匀减速段结尾时刻，totg为减减速段结尾时刻。

步骤6，根据加速度曲线分段参数生成运动轨迹：

然后由加速度曲线类型typea和步骤5计算的加速度曲线分段参数代入s型加减速轨迹公式生成轨迹j(t)，a(t)，v(t)，p(t)。

s型加减速曲线公式如下：

具体轨迹如下：

第一段0≤t≤t1a，

第二段t1a＜t≤t1b，其中p1a、v1a、a1a分别为关节第一段末端的位移，速度，加速度，

第三段t1b＜t≤t1c，其中p1b、v1b、a1b分别为关节第二段末端的位移，速度，加速度

第四段t1c＜t≤t2a，其中p1c、v1c、a1c分别为关节第三段末端的位移，速度，加速度

p(t)＝p1c+v1c(t-t1c)

第五段t2a＜t≤t2b，其中p2a、v2a、a2a分别为关节第四段末端的位移，速度，加速度

第六段t2b＜t≤t2c，其中p2b、v2b、a2b分别为关节第五段末端的位移，速度，加速度

第七段t2c＜t≤totg，其中p2c、v2c、a2c分别为关节第六段末端的位移，速度，加速度

j(t)，a(t)，v(t)分别等于p(t)对t求三阶二阶和一阶导函数。

以下是测试参数：

input＝[ps，vs，as，ptrgt，vtrgt，vmax，amax，jmax]

input1＝[0，100，50，500，-100，250，100，100]

input2＝[0，100，50，500，-100，200，100，100]

input3＝[0，100，50，500，-100，250，150，100]

input4＝[0，100，50，500，-100，250，200，100]

input5＝[0，100，50，500，-100，200，150，100]

input6＝[0，100，50，500，-100，200，200，100]

图4(a)至图4(f)显示了本方法规划的时间最优s型加减速轨迹规划结果，以上6组输入参数对应的轨迹曲线分别如图4(a)、图4(b)、图4(c)、图4(d)、图4(e)和图4(f)所示。

本发明并不限于上文描述的实施方式。以上对具体实施方式的描述旨在描述和说明本发明的技术方案，上述的具体实施方式仅仅是示意性的，并不是限制性的。在不脱离本发明宗旨和权利要求所保护的范围情况下，本领域的普通技术人员在本发明的启示下还可做出很多形式的具体变换，这些均属于本发明的保护范围之内。