一种速度规划方法及系统与流程

1.本技术涉及运动规划技术领域，尤其涉及一种速度规划方法及系统。


背景技术：

2.运动规划是移动机器人、自动驾驶车辆或无人机等智能体不可缺少的重要功能。运动规划通常包括路径规划和速度规划。路径规划的主要任务是寻找一条或多条连接起始位置和目标位置的满足约束条件的(如运动学约束)序列点或曲线。速度规划主要任务是根据已知的路径信息规划出一条或多条满足约束条件的(如动力学约束)与时间相关的轨迹序列。现有的速度规划方法可以分为两种：(1)对整段路径进行速度规划方法；(2)基于采样的动态规划方法。
3.对整段路径进行速度规划方法中又包括简单的将每个路径点赋予一个速度值、“梯形”速度曲线、“s”型速度曲线和样条曲线。在机器人和自动驾驶的许多场景中，通常会遇到如下问题：一条路径上的不同位置或区段具有不同的速度限制，如附图1和2所示。目前的对整段路径进行速度规划的方法均无法解决上述问题，即这些方法在进行速度规划时，无法考虑路径上的不同位置或区段具有不同的速度限制的情况。


技术实现要素：

4.有鉴于此，本技术实施例提供一种速度规划方法及系统，至少部分解决现有技术中存在的在进行速度规划时，无法考虑路径上的不同位置或区段具有不同的速度限制的情况的问题。
5.第一方面，本技术实施例提供一种速度规划方法，所述速度规划方法包括：
6.获取路径信息，包括路径点、路径点的坐标和最大速度以及运动主体的最大加速度和最大加加速度；
7.划分路径段，将路径划分为若干个路径段，并获取各路径段的累计长度与各路径段的最大速度的关系，其中，具有相同路径点最大速度的连续路径点为一个路径段，相邻两个路径段之间共有的路径点为衔接点；
8.初始化缩放比例，缩放比例用于对各路径段的最大速度和运动主体的最大加速度进行缩放；
9.第一速度规划，用于对各路径段的累计长度与各路径段的最大速度的关系进行优化，包括根据各路径段的长度、缩放后的各路径段的最大速度和运动主体的最大加速度，计算由于运动主体的最大加速度限制导致的各衔接点的速度信息和各路径段内不同速度类型的时间，并对计算结果进行采样完成第一速度规划的计算；
10.第二速度规划，利用优化算法以及运动主体的最大加速度和最大加加速度对第一速度规划的结果进行优化，得到第二速度规划，利用优化算法进行优化包括确定优化变量、目标函数和约束条件；
11.若第二速度规划失败，需更改缩放比例，重复第一速度规划和第二速度规划的计
算。
12.第二方面，本技术实施例还提供一种速度规划系统，包括：
13.获取模块，用于获取路径信息，包括路径点、路径点的坐标和最大速度以及运动主体的最大加速度和最大加加速度；
14.路径段划分模块，用于划分路径段，将路径划分为若干个路径段，并获取各路径段的累计长度与各路径段的最大速度的关系，其中，具有相同路径点最大速度的连续路径点为一个路径段，相邻两个路径段之间共有的路径点为衔接点；
15.缩放比例输入模块，用于初始化缩放比例，缩放比例用于对各路径段的最大速度和运动主体的最大加速度进行缩放；
16.第一计算模块，用于第一速度规划的计算，用于对各路径段的累计长度与各路径段的最大速度的关系进行优化，包括根据各路径段的长度、缩放后的各路径段的最大速度和运动主体的最大加速度，计算由于运动主体的最大加速度限制导致的各衔接点的速度信息和各路径段内不同速度类型的时间，并对计算结果进行采样完成第一速度规划的计算；
17.第二计算模块，用于第二速度规划的计算，利用优化算法以及运动主体的最大加速度和最大加加速度对第一速度规划的结果进行优化，得到第二速度规划，利用优化算法进行优化包括确定优化变量、目标函数和约束条件；若第二速度规划失败，需更改缩放比例，重复第一速度规划和第二速度规划的计算。
18.第三方面，本技术实施例还提供一种控制终端，包括：
19.至少一个处理器；以及，
20.与所述至少一个处理器通信连接的存储器；其中，
21.所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如前述第一方面中任一实施例所述的速度规划方法。
22.第四方面，本技术实施例还提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时实现前述第一方面中任一实施例所述的速度规划方法。
23.有益效果
24.本技术实施例中的速度规划方法，通过将路径上的具有不同最大速度限制的路径点进行划分为若干个路径段，分别对不同的路径段进行两次速度规划，可以有效解决速度规划时出现的路径上的不同位置或区段具有不同的速度限制的问题，使速度规划的曲线更加平滑，计算结果更加可靠，使车辆或机器人行驶更加平稳。
附图说明
25.为了更清楚地说明本技术实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
26.图1为现有技术的机器人行驶场景；
27.图2为现有技术的自动驾驶车辆行驶场景；
28.图3为根据本发明一实施例的速度规划方法的流程图；
29.图4为根据本发明一实施例的路径点与各路径点的最大速度限制；
30.图5为根据本发明一实施例的路径段划分及累计长度计算结果图；
31.图6为根据本发明一实施例的路径s
‑
v图；
32.图7为根据本发明一实施例的路程
‑
速度图；
33.图8为根据本发明一实施例的时间
‑
路程图；
34.图9为根据本发明一实施例的时间
‑
速度图；
35.图10为根据本发明一实施例的时间
‑
加速度图；
36.图11为根据本发明一实施例的控制终端。
具体实施方式
37.下面结合附图对本技术实施例进行详细描述。
38.以下通过特定的具体实例说明本技术的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本技术的其他优点与功效。显然，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。本技术还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本技术的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
39.要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本技术，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目个方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。
40.还需要说明的是，以下实施例中所提供的图示仅以示意方式说明本技术的基本构想，图式中仅显示与本技术中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
41.另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践所述方面。
42.本技术实施例提供了一种速度规划方法，下面参照附图对本技术的速度规划方法进行详细描述。
43.参照图3
‑
11，本技术的速度规划方法的流程如图3，主要包括以下步骤：
44.s101、获取路径信息，包括路径点、所述路径点的坐标和最大速度以及运动主体的最大加速度和最大加加速度。具体的，路径信息包括运动主体到目标位置的路径，路径上完整的、间隔均匀的连续路径点，路径点的坐标和最大速度，以及运动主体的最大加速度a
m
和最大加加速度j
m
，运动主体可以是车辆、机器人等。为描述方便，下面将以机器人为例进行描述。其中，每个点的最大速度由路径规划部分进行计算，可采用现有路径规划方法，本技术中并不做特殊要求。参照图4所示，图中的连续点为路径点序列，每个点旁边的数字为该
点的最大速度限制。
45.s102、划分路径段，将路径划分为若干个路径段，按照路径点序列先后顺序将具有相同最大速度的连续路径点作为一个路径段的方法进行划分，相邻两个路径段之间的路径点为衔接点，并照路径段先后顺序获取各路径段的累计长度s和最大速度v，可以得到路径的s
‑
v图，s
‑
v图的含义是：以起点开始，沿路径不同长度s位置的最大速度限制。参照图5
‑
6所示，图5为对图4的路径进行了划分，图6为与图5对应的s
‑
v图，图中各路径点的距离间隔为0.1，因此第一个路径段对应的s为0.7，第二路径段s为1.1，第三路径段s为2.0。
46.s103、初始化缩放比例，所述缩放比例用于对所述各路径段的最大速度和运动主体的最大加速度进行缩放。在本实施例中，可将缩放比例r初始值设置为1.0。
47.s104、第一速度规划，根据各所述路径段的长度l
i
、缩放后的各路径段的最大速度v
iph
和所述运动主体的最大加速度a
m
，计算由于所述运动主体的最大加速度a
m
限制导致的各所述衔接点的速度信息和各所述路径段内不同速度类型的时间，并对计算结果进行采样完成所述第一速度规划的计算。
48.在本实施例中，通过前面的步骤共划分了n个路径段，考虑路径的起点和终点，这n个路径段共有n 1个衔接点。令l
i
(i＝0,1,
…
,n
‑
1)表示每个路径段的长度，令l表示路径总长度，令v
ic
(i＝0,1,
…
,n)表示每个衔接点的速度，令v
ip
(i＝0,1,
…
,n
‑
1)表示每个路径段的最大速度，令1,
…
,n
‑
1)为缩放后的每个路径段的最大速度，则
[0049][0050]
即衔接点的速度等于前后两个路径段的最大速度除以缩放比例。
[0051]
令f
ipre
和f
inext
(i＝0,1,
…
,n)分别表示第i个衔接点的速度是否被前一路径段和后一路径段的速度限制的标志位，0表示没有被限制，1表示被限制，若f
ipre
＝1，则表示第i个衔接点被前一路径段的速度限制。
[0052]
各衔接点的速度信息的具体计算包括：
[0053]
s1041、前向计算每个衔接点的最大速度。令a
h
＝a
m
/r为缩放后的最大加速度。对于i＝1,2,
…
,n
‑
1，也即顺着路径点的方向，由前一段路径限制的速度为
[0054][0055]
其中，
[0056]
由后一段路径限制的速度为
[0057]
其中，s
p
＝(v
iph
)2/2/a
h
，
ꢀꢀ
(5)
[0058]
那么，可得当前衔接点的速度为
[0059][0060]
s1042、反向计算由于被后段路径限制的衔接点速度。此时，i＝n
‑
1,
…
,2,1，也即
逆着路径点的方向，被后段路径限制也即f
inext
＝1，则由前一段路径限制的速度可由公式(2)和(3)求出。由后一段路径限制的速度为
[0061][0062]
其中，
[0063]
那么，可得当前衔接点的速度可根据公式(6)进行修正。
[0064]
因此，对于i＝n
‑
1,
…
,2,1，反向计算由于被后段路径限制的衔接点速度，即如果f
inext
＝1，则由前一段路径限制的速度可由公式(2)和(3)求出，由后一段路径限制的速度可由公式(7)和(8)求出，那么，可得当前衔接点的速度可根据公式(6)进行修正。
[0065]
通过步骤s1041和s1042，由于加速度的限制，每个衔接点能够到达的速度均被求出。
[0066]
下面将计算各路径段内不同速度类型的时间，本实施例中，速度类型包括加速、恒速和减速，因此需要计算各路径段内的加速时间、恒速时间和减速时间。
[0067]
s1043、计算各路径段内不同速度类型的时间。令t
a
，t
c
，t
d
是三个集合，t
a
保存了每个路径段的加速时间t
ai
，t
c
保存了每个路径段的恒速时间t
ci
，t
d
保存了每个路径段的减速时间t
di
。对于i＝1,2,
…
,n，
[0068]
如果那么
[0069][0070][0071][0072]
如果t
ci
＝0，对于t
ai
和t
di
需要分情况进行讨论。具体如下，
[0073]
(1)如果并且则
[0074]
(2)如果并且v
m
≤v
ic
，则
[0075]
(3)如果并且v
m
≤v
ic
，则此时，t
ai
、t
ci
和t
di
均已被求解出来，并将t
ai
、t
ci
和t
di
分别放入t
a
，t
c
，t
d
三个集合中。
[0076]
s1044、对t
a
，t
c
，t
d
三个集合进行重组。令t
g
，s
g
，v
g
，a
g
和type分别表示重组之后的时间、路程长度、速度、加速度以及速度类型的集合。其中type等于0，1，2时分别表示加速、恒速和减速的速度类型。
[0077]
对于i＝1,2,
…
,n，初始化idx＝0，
[0078]
(1)处理t
ai
速度段落，如果t
ai
>0，则
[0079]
将t
g_idx
t
ai
放入t
g
集合，将v
g_idx
a
h
×
t
ai
放入v
g
集合，将s
g_idx
(v
g_idx
v
g_idx 1
)t
ai
/2
放入s
g
集合，将0放入type集合，将a
h
放入a
g
集合。idx＝idx 1。
[0080]
(2)处理t
ci
速度段落，如果t
ci
>0，则
[0081]
将t
g_idx
t
ci
放入t
g
集合，将v
g_idx
放入v
g
集合，将s
g_idx
v
g_idx
×
t
di
放入s
g
集合，将1放入type集合，将0放入a
g
集合。idx＝idx 1。
[0082]
(3)处理t
di
速度段落，如果t
di
>0，则
[0083]
将t
g_idx
t
di
放入集合t
g
最后，将v
g_idx
‑
a
h
×
t
di
放入v
g
集合，将s
g_idx
(v
g_idx
v
g_idx 1
)t
di
/2放入s
g
集合，将2放入type集合，将a
h
放入a
g
集合。idx＝idx 1。
[0084]
s1045、对重组后得到的路程、速度和加速度进行采样。根据重组结果，总时间长度t为集合t
g
最后一个元素的值。令采样时间间隔为ts，对时间t＝0，ts,2ts,...,t进行采样，则
[0085]
(1)根据时间t找到其所在t
g
中的索引j；
[0086]
(2)计算路程s
i
，速度v
i
和加速度a
i
，即
[0087]
如果type
j
＝0，可得:
[0088]
如果type
j
＝1，可得:
[0089]
如果type
j
＝2，可得:
[0090]
因此，通过上述步骤完成了第一速度规划，可以得到路程
‑
速度图、时间
‑
路程图、时间速度图和时间
‑
加速度图，参照图7
‑
10所示，图中的实线的启发结果为第一速度规划后的结果。
[0091]
下面将根据第一速度规划的结果进行第二速度规划，通过优化算法对第一速度规划结果进行优化，使得相关的曲线更加平滑。平滑的速度曲线能够使无人驾驶车辆等行驶更加平稳，因而增加乘客的乘坐舒适性。
[0092]
s105、第二速度规划，利用优化算法以及所述运动主体的最大加速度和最大加加速度对所述第一速度规划的结果进行优化，得到所述第二速度规划，利用所述优化算法进行优化包括确定优化变量、目标函数和约束条件。
[0093]
具体的，本实施例中利用优化算法对第一速度规划的结果进行优化时采用三次样条曲线法进行优化拟合。对于路程s，三次样条曲线表示为
[0094][0095]
其中，i＝1,2,
…
,m，即三次样条每一段均为三次多项式，为多项式系数，也是速度、加速度和加加速度，δt
i
为时间间隔。三次样条共有m＝t/ts 1段，共有锚点u＝m 1个。
[0096]
优化算法一般形式为:min f(x)
[0097]
s.t.h(x)
i
＝0,1,2,...,n
h
[0098]
g(x)
i
＝0,1,2,...,n
g
[0099]
x∈d
[0100]
其中，f(x)为目标函数，x为为优化变量，h(x)
i
为等式约束，n
h
为等式约束个数，g(x)
i
为不等式约束，n
g
为不等式约束个数。
[0101]
对于为优化变量x，表达式为：
[0102]
对于目标函数f(x)，其表达式为
[0103][0104]
其中，为参考速度的权重，为加速度权重，为加加速度权重。δt为时间间隔，δt等于上文所描述的采样时间ts。为参考速度，由本发明所描述第一速度规划中的速度再乘以缩放比例r计算所得。
[0105]
下面描述约束条件，包括：
[0106]
(1)优化变量边界范围s
i
,边界范围：其中，i＝0,1,2,...,u
‑
1。
[0107]
(2)不等式约束：其中，j
m
为最大加加速度限制，i＝0,1,2,...,u
‑
2。
[0108]
(3)等式约束，包括：
[0109]
初始状态约束：中止状态约束为：中间状态约束为：
[0110]
确定变量约束、优化目标以及约束条件表示之后，通过优化求解器软件即可求出第二速度规划结果，优化求解器包括但不限于osqp(operator splitting quadratic program，算子分裂二次规划)、ipopt(interior point optimizer，内点优化器)等。
[0111]
s106、若所述第二速度规划失败，需更改缩放比例，重复所述第一速度规划和所述第二速度规划的计算。也就是说，可实现动态调节缩放比例，即如果优化求解失败，增大缩放比例再次求解，直到优化求解成功。例如，如果生成第二速度规划的曲线失败，则增大缩放比例r，然后，重新进行步骤s104和s105，直到规划成功或达到最大计算次数为止，最大计算次数可预先设置。如果第二速度规划成功，则结束计算。
[0112]
在一个更为具体的实施例中，路径的总长度为60m，路径分为4段，每一段的长度分别为25米、5米、10米和20米，每一路径段的速度限制分别为5米/秒、1米/秒、0.5米/秒和4.5米/秒。最大加速度为1米每平方秒，最大加加速度为1米每三次方秒，缩放比例r为1.1。按照本技术提供的速度规划方法进行计算，其结果参照图7
‑
10所示，图中实现的启发结果即为第一速度规划的结果，虚线的优化结果即为第二速度规划结果。可见，通过本技术的速度规划方法，对于路径上的不同位置或区段具有不同的速度限制时，可以很好的进行速度规划，使自动驾驶的运动规划更加安全可靠。
[0113]
第二方面，本发明实施例还提供一种速度规划系统，包括：
[0114]
获取模块，用于获取路径信息，包括路径点、路径点的坐标和最大速度以及运动主体的最大加速度和最大加加速度；
[0115]
路径段划分模块，用于划分路径段，将路径划分为若干个路径段，并获取各路径段的累计长度与各路径段的最大速度的关系，其中,具有相同路径点最大速度的连续路径点为一个路径段，相邻两个路径段之间共有的路径点为衔接点；
[0116]
缩放比例输入模块，用于初始化缩放比例，缩放比例用于对各路径段的最大速度和运动主体的最大加速度进行缩放；
[0117]
第一计算模块，用于第一速度规划的计算，用于对各路径段的累计长度与各路径段的最大速度的关系进行优化，包括根据各路径段的长度、缩放后的各路径段的最大速度和运动主体的最大加速度，计算由于运动主体的最大加速度限制导致的各衔接点的速度信息和各路径段内不同速度类型的时间，并对计算结果进行采样完成第一速度规划的计算；
[0118]
第二计算模块，用于第二速度规划的计算，利用优化算法以及运动主体的最大加速度和最大加加速度对第一速度规划的结果进行优化，得到第二速度规划，利用优化算法进行优化包括确定优化变量、目标函数和约束条件；若第二速度规划失败，需更改缩放比例，重复第一速度规划和第二速度规划的计算。
[0119]
第三方面，本发明实施例还涉及一种控制终端，如图11所示，至少一个处理器302；以及，与至少一个处理器通信连接的存储器301；其中，存储器301存储有可被至少一个处理器302执行的指令，指令被至少一个处理器302执行，以使至少一个处理器302能够执行上述速度规划方法的实施方式。
[0120]
其中，存储器和处理器采用总线方式连接，总线可以包括任意数量的互联的总线和桥，总线将一个或多个处理器和存储器的各种电路连接在一起。总线还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其他电路连接在一起，这些都是本领域所公知的，因此，本文不再对其进行进一步描述。总线接口在总线和收发机之间提供接口。收发机可以是一个元件，也可以是多个元件，比如多个接收器和发送器，提供用于在传输介质上与各种其他装置通信的单元。经处理器处理的数据通过天线在无线介质上进行传输，进一步，天线还接收数据并将数据传送给处理器。
[0121]
处理器负责管理总线和通常的处理，还可以提供各种功能，包括定时，外围接口，电压调节、电源管理以及其他控制功能。而存储器可以被用于存储处理器在执行操作时所使用的数据。
[0122]
第四方面，本发明实施例还涉及一种计算机可读存储介质，存储有计算机程序。计算机程序被处理器执行时实现上述速度规划方法实施例。
[0123]
即，本领域技术人员可以理解，实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得一个设备(可以是单片机，芯片等)或处理器(processor)执行本技术各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read
‑
onlymemory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。
[0124]
以上所述，仅为本技术的具体实施方式，但本技术的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本技术揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本技术的保护范围之内。因此，本技术的保护范围应以权利要求的保护范围为准。