CN112729326B - 运动智能体轨迹规划方法、装置、存储介质和电子设备 - Google Patents

Abstract

本发明提供了一种运动智能体轨迹规划方法、装置、存储介质和电子设备，涉及轨迹规划技术领域。每次取开放队列中期望最小的节点作为当前节点，判断是否满足到达目标；若不满足，对当前节点的空间状态向多个运动方向进行拓展；再计算当前节点的停留允许时间段；进而计算期望到达时间段；以此构建新节点存入开放队列，重复执行，直至到达目标，则根据父节点从最终节点向前回溯找到起始节点，将回溯序列反转，提取对应的空间状态与时刻信息，输出最终轨迹。将时间维度考虑到规划过程中，将静态环境中的规划提升到动态环境中的规划。极大的降低了搜索空间的复杂度，结果更接近最优解。不需要提前在环境中预设节点与轨迹，在不同环境中部署迅速。

Description

运动智能体轨迹规划方法、装置、存储介质和电子设备

技术领域

本发明涉及轨迹规划技术领域，具体涉及一种运动智能体轨迹规划方法、装置、存储介质和电子设备。

背景技术

无人车，无人机等运动智能体的轨迹规划是自动驾驶系统重要的组成部分，关于轨迹规划的研究大多继承自机器人轨迹规划的研究成果。在静态环境中的轨迹规划算法已经相当完备，但在动态环境中的轨迹规划仍没有很好的解决方案。目前通常的作法是全局轨迹规划考虑静态环境，在执行过程中考虑动态环境，通过局部轨迹规划处理动态环境中的变化。但上述方式规划出的最终轨迹往往是非优的，且有可能陷入局部最优甚至死锁。也有将动态障碍物在一段时间内的拖影定义成静态障碍物进行规划的，但此方法有可能无解且解一般都不是最优的。

在自动仓储领域，通常采用预先设定轨迹、铺设地磁线等手段来达到多目标的无冲突规划。

不同于仓库环境，矿区环境变化迅速，预设轨迹的复用率极低，也不具备铺设地磁线的条件。交通管制，时间窗口管理等手段可以一定程度上缓解无人驾驶车辆多车作业的相互冲突问题，但并不能充分利用时间与空间，效率也明显低于人工。亟需一种考虑动态目标的轨迹规划算法实现多目标协同规划。

发明内容

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种运动智能体轨迹规划方法、装置、存储介质和电子设备，解决了现有算法通过预先设定轨迹、铺设地磁线的方法无法适用于矿区的问题。

(二)技术方案

为实现以上目的，本发明通过以下技术方案予以实现：

第一方面，提供了一种运动智能体轨迹规划方法，该方法包括：

S1、获取目标e的空间状态s_e以及包含运动智能体空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示运动智能体初始节点的空间状态；

t₀表示规划开始时间；

null表示初始节点的父节点为空；

S2、基于栅格化的地图，将其他对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map，地图有效时间T_map由若干时间段(t_l,t_r)组成；其中，

所述其它对象为任意几何外形与轨迹可知的物体；

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

t_l和t_r分别表示有效时间段在时间轴上的左边界和右边界；

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标e的耗时估计；

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；

S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)；

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到起始节点X₀，将回溯序列反转，得到从起始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

进一步的，所述S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N包括：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]。

进一步的，所述S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)，包括：

获取运动智能体位于s_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

计算所述集合中地图有效时间的交集，得到交集时间段的集合T_c＝[t_c1,t_c2,...,t_ci,...]；

获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

进一步的，所述S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c，再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列包括：

获取从s_c移动到各个s_n的耗时Δt_n；

计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_stop1+Δt_n，t_desire2＝t_stop2+Δt_n；

计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

第二方面，一种运动智能体轨迹规划装置，该装置包括：

初始化模块，用于获取目标e的空间状态s_e以及包含运动智能体空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示运动智能体初始节点的空间状态；

t₀表示规划开始时间；

null表示初始节点的父节点为空；

地图有效时间计算模块，用于基于栅格化的地图，将其他对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map，用于表示栅格未被占用的若干个有效时间段(t_l,t_r)；其中，

所述其它对象为任意几何外形与轨迹可知的物体；

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

t_l和t_r分别表示有效时间段在时间轴上的左边界和右边界；

轨迹规划模块，用于重复执行步骤S3-S6，直至当前节点X_c满足到达目标s_e或开放队列为空，执行S7；且S3-S7包括：

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标e的耗时估计；

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；

S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)；

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到起始节点X₀，将回溯序列反转，得到从起始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

进一步的，所述轨迹规划模块对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N包括：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]。

进一步的，所述轨迹规划模块基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)包括：

获取运动智能体位于S_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

计算所述集合中地图有效时间的交集，得到交集时间段的集合T_c＝[t_c1,t_c2,...,t_ci,...]；

获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

进一步的，所述轨迹规划模块基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，再判断X_n是否在闭合队列中，若不在闭合队列中，且标记X_n的父节点为X_c，则将X_n存入开放队列包括：

获取从s_c移动到各个s_n的耗时Δt_n；

计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_stop1+Δt_n，t_desire2＝t_stop2+Δt_n；

计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

第三方面，一种计算机可读存储介质，其存储用于运动智能体轨迹规划的计算机程序，其中，所述计算机程序使得计算机执行如下方法：

S1、获取目标e的空间状态s_e以及包含运动智能体空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示运动智能体初始节点的空间状态；

t₀表示规划开始时间；

null表示初始节点的父节点为空；

S2、基于栅格化的地图，将其他对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map，地图有效时间T_map由若干时间段(t_l,t_r)组成；其中，

所述其它对象为任意几何外形与轨迹可知的物体；

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

t_l和t_r分别表示有效时间段在时间轴上的左边界和右边界；

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标e的耗时估计；

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；

S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)；

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到起始节点X₀，将回溯序列反转，得到从起始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

进一步的，所述S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N包括：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]。

进一步的，所述S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)，包括：

获取运动智能体位于s_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

计算所述集合中地图有效时间的交集，得到交集时间段的集合T_c＝[t_c1,t_c2,...,t_ci,...]；

获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

进一步的，所述S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c，再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列包括：

获取从s_c移动到各个s_n的耗时Δt_n；

计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_stop1+Δt_n，t_desire2＝t_stop2+Δt_n；

计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

第四方面，一种电子设备，包括：

一个或多个处理器；

存储器；以及

一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器执行，所述程序包括用于执行如下方法：

S1、获取目标e的空间状态s_e以及包含运动智能体空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示运动智能体初始节点的空间状态；

t₀表示规划开始时间；

null表示初始节点的父节点为空；

S2、基于栅格化的地图，将其他对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map，地图有效时间T_map由若干时间段(t_l,t_r)组成；其中，

所述其它对象为任意几何外形与轨迹可知的物体；

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

t_l和t_r分别表示有效时间段在时间轴上的左边界和右边界；

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标e的耗时估计；

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；

S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)；

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到起始节点X₀，将回溯序列反转，得到从起始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

进一步的，所述S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N包括：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]。

进一步的，所述S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)，包括：

获取运动智能体位于s_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

计算所述集合中地图有效时间的交集，得到交集时间段的集合T_c＝[t_c1,t_c2,...,t_ci,...]；

获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

进一步的，所述S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c，再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列包括：

获取从s_c移动到各个s_n的耗时Δt_n；

计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_stop1+Δt_n，t_desire2＝t_stop2+Δt_n；

计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

(三)有益效果

本发明提供了一种运动智能体轨迹规划方法、装置、存储介质和电子设备。与现有技术相比，具备以下有益效果：

本发明通过计算地图内的所有栅格的地图有效时间，结合构建的开放队列和闭合队列，每次取开放队列中期望最小的节点作为当前节点，判断当前节点是否满足到达；若不满足，对当前节点的空间状态向多个运动方向进行拓展；再计算当前节点的停留允许时间段；进而计算期望到达时间段；以此构建新节点存入开放队列，重复执行，直至到达目标，则根据父节点从最终节点向前回溯找到起始节点，将回溯序列反转，提取对应的空间状态与时刻信息，输出最终轨迹。将时间维度考虑到规划过程中，将静态环境中的规划提升到动态环境中的规划。与将时间列成一个独立的维度并进行采样的方式相比，采用地图有效时间的方式极大的降低了搜索空间的复杂度。在规划过程中可进行“让行”与“停等”等操作，一些场景下可提前避开冲突，结果更接近最优解。对于变化迅速，交通环境结构化低的场景有较好的适应能力，对环境的要求低。不需要提前在环境中预设节点与轨迹，在不同环境中部署迅速。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例的流程图；

图2为本发明实施例的地图有效时间的示意图；

图3为本发明实施例的停留允许时间段的计算示意图；

图4为本发明实施例的生成的新节点X_n的示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本申请实施例通过提供一种运动智能体轨迹规划方法、装置、存储介质和电子设备，解决了现有算法通过预先设定轨迹、铺设地磁线的方法无法适用于矿区的问题。

本申请实施例中的技术方案为解决上述技术问题，总体思路如下：

通过计算地图内的所有栅格的地图有效时间，结合构建的开放队列和闭合队列，每次取开放队列中期望最小的节点作为当前节点，判断当前节点是否满足到达；若不满足，对当前节点的空间状态向多个运动方向进行拓展；再计算当前节点的停留允许时间段；进而计算期望到达时间段；以此构建新节点存入开放队列，重复执行，直至到达目标，则根据父节点从最终节点向前回溯找到起始节点，将回溯序列反转，提取对应的空间状态与时刻信息，输出最终轨迹。将时间维度考虑到规划过程中，将静态环境中的规划提升到动态环境中的规划。与将时间列成一个独立的维度并进行采样的方式相比，采用地图有效时间的方式极大的降低了搜索空间的复杂度。在规划过程中可进行“让行”与“停等”等操作，一些场景下可提前避开冲突，结果更接近最优解。对于变化迅速，交通环境结构化低的场景有较好的适应能力，对环境的要求低。不需要提前在环境中预设节点与轨迹，在不同环境中部署迅速。

为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。

实施例1：

如图1所示，以运动智能体为用于矿场的无人驾驶车辆为例，本发明提供了一种运动智能体轨迹规划方法，其运动智能体也可为对于其它运动智能体同样适用，例如用于无人机，该方法由计算机执行，该方法包括：

S1、获取目标e的空间状态s_e以及包含运动智能体空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示运动智能体初始节点的空间状态；

t₀表示规划开始时间；

null表示初始节点的父节点为空；

S2、基于栅格化的地图，将其他对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map，地图有效时间T_map由若干时间段(t_l,t_r)组成；其中，

所述其它对象为任意几何外形与轨迹可知的物体；

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

t_l和t_r分别表示有效时间段在时间轴上的左边界和右边界；

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标e的耗时估计；

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；

S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)；

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到起始节点X₀，将回溯序列反转，得到从起始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

本实施例的有益效果为：

①将时间维度考虑到规划过程中，将静态环境中的规划提升到动态环境中的规划。

②与将时间列成一个独立的维度并进行采样的方式相比，采用地图有效时间的方式极大的降低了搜索空间的复杂度。

③在规划过程中可进行“让行”与“停等”等操作，一些场景下可提前避开冲突，结果更接近最优解。

④对于变化迅速，交通环境结构化低的场景有较好的适应能力，对环境的要求低。

⑤不需要提前在环境中预设节点与轨迹，在不同环境中部署迅速。

进一步的，所述S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N包括：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]。

进一步的，所述S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)，包括：

获取运动智能体位于s_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

计算所述集合中地图有效时间的交集，得到交集时间段的集合T_c＝[t_c1,t_c2,...,t_ci,...]；

获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

进一步的，所述S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c，再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列包括：

获取从s_c移动到各个s_n的耗时Δt_n；

计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_stop1+Δt_n，t_desire2＝t_stop2+Δt_n；

计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

下面对本发明实施例的实现过程进行详细说明；如图1所示：

S1、获取目标e的空间状态s_e以及包含车辆空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示节点X的空间状态，包含车辆的二维坐标(x,y)和朝向

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示车辆初始节点的空间状态；

t₀表示规划开始时间，即时间轴的原点；

null表示初始节点的父节点为空；

对于任意两个节点X中，空间状态s和与之对应的时刻t任一变量不同，则认为节点是不同的。空间状态s可以独立存在，但时刻t必须有与之对应的空间状态s；是否相同的判断还需考虑一定的误差，当所有变量的差异都在误差范围内时则认为两者相同。

上述目标s_e仅指空间状态，时刻t不作限制。即只限制了目标的空间状态，对何时到达未作要求。在此遵循快速原则，即尽可能早的到达目标，在存入初始节点X₀前，开放队列是一个空队列。

S2、基于栅格化的地图，将其他对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map；

其中，所述其它对象为任意几何外形与轨迹可知的物体；例如自动驾驶车辆未来一段时间内的运动轨迹可通过调度系统获得，或通过V2V通信获得，也可以根据目标车辆的速度、加速度、转向趋势等可观测条件预测所得，未来一段时间内的运动是可知的，且几何外形可知。

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

所述空间栅格是正方形的一片区域，与之对应的地图有效时间描述了其在哪些时间段是未被占用的。对于地图范围内的任一空间状态至少有一个栅格与之对应，所述一个空间栅格可与若干个有效时间段对应。t_l和t_r分别表示有效时间段(t_l，t_r)在时间轴上的左边界和右边界，分别表示开始时刻与结束时刻。

举例说明，如图2所示，某栅格在t₁～t₂期间被占用，则地图有效时间T_map中包含两个有效时间段，分别为(t₀,t₁)和(t₂,t_∞)；此外，如地图内某栅格在规划的有限时间内都未被占用，则其地图有效时间为T_map＝(t₀,t_∞)。如某栅格始终被占用则没有地图有效时间与之对应。

其它对象轨迹所达的全部空间栅格都将被占用，结合轨迹的速度可推导出其占用相应栅格的时间信息，便可得到与被占用栅格对应的地图有效时间。在计算栅格对应的地图有效时间时，可以将时间的边界压缩，保留一定的安全余量，以免车辆无法准确跟踪轨迹时发生危险。

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标s_e的耗时估计；即其物理意义为通过该节点的轨迹的预计总耗时。

当前节点X_c到达目标s_e指当前节点X_c的空间状态s_c与目标的空间状态s_e已足够接近。且从s_c到达s_e的过程中与地图以及其它对象没有冲突。

上述从s_c到达s_e的耗时估计不得大于车辆s_c到达s_e的真实耗时。可以假设车辆将以均速完成剩余路程，则耗时预估与两状态的几何距离成正比。上述几何距离可以是欧式距离，也可以是曼哈顿距离、或切比雪夫距离。

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；具体为：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，例如方向包括车辆的左前方、正前方、右前方、左后方、正后方和右后方，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]。

S5、基于当前节点X_c的空间状态s_c以及车辆位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)，包括：

S51、获取车辆位于s_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

S52、计算所述集合中地图有效时间的交集，得到交集时间段的集合T_cn＝[t_c1,t_c2,...,t_ci,...]；

S53、获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

上述当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)与地图有效时间T_map不同。此处停留允许时间段的物理意义表示车辆可以在T_stop＝(t_stop1,t_stop2)时段内在当前空间状态s_c进行停等。

举例说明，如图3所示，假设当前空间状态s_c占用三个栅格，三个栅格对应三条时间轴，

第一个栅格对应的地图有效时间T_map1＝[t_map11,t_map12]，且t_map11＝(t₀,t₁)，t_map12＝(t₂,t_∞)；

第二个栅格对应的地图有效时间T_map2＝[t_map21,t_map22]，且t_map21＝(t₀,t₃)，t_map22＝(t₄,t_∞)；

第三个栅格对应的地图有效时间T_map3＝[t_map31]，且t_map31＝(t₀,t_∞)。

则三个栅格对应的地图有效时间取交集，得到交集时间段的集合：

T_cn＝[t_c1＝(t₀，t₁),t_c2＝(t₂，t₃),t_c3＝(t₄，t_∞)]；

从t_stop1＝t_c向时间轴右侧探索到首个交集时间段为t_c2＝(t₂，t₃)，右边界为t₃，即t_stop2＝t₃，得到当前节点的停留允许时间段T_stop＝(t_c,t₃)。

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；包括：

S61、获取从s_c移动到各个s_n的耗时Δt_n；上述Δt_n考虑到车辆的转向、前进方向以及是否存在换向等问题，不同的转向，前进方向以及换向状态对应的Δt_n是不同的。可以是预先设定好的Δt_n，针对不同的情况选用不同的Δt_n；

S62、计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_c+Δt_n，t_desire2＝t_c’+Δt_n；表示车辆预计可以在t_desire1～t_desire2时段内到达s_n。

S63、计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，可能不止一个，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，即判断该节点是否被探索过，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

举例说明，如图4所示，假设s_n对应的交集时间段分别为：t_c1＝(t_l1,t_r1)，t_c2＝(t_l2,t_r2)；

s_n对应的期望到达时间段T_desire＝(t_desire1,t_desire2)

t_c1∩T_desire为空，表示车辆不能在期望到达时间段T_desire内到达s_n。

t_c2∩T_desire＝(t_l2,t_desire2)，则在该情况下取其交集的左边界时刻t_l2＝t_n。其中时段(t_desire1,t_r2)车辆在s_c进行停等。

此时，可构成新节点X_n＝(s_n,t_n,X_c)。

所述闭合队列存放一系列节点，在闭合队列中元素无需排序，只需能够快速验证某个节点是否已探索过。

返回S3，重复执行S3-S6，直至已经探索到目标空间状态或开放队列为空时停止。当开放队列为空时，规划执行失败。

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到起始节点X₀，将回溯序列反转，得到从起始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

上述最终节点X_f对应的空间状态为s_e。根据每个节点对应的空间状态与时刻信息输出轨迹的过程即根据空间状态和对应时间计算轨迹的过程。由于其它对象所占用栅格对应的地图有效时间是经过压缩的，此处可消除部分由于惯性以及误差的影响。

实施例2：

第二方面，提供了一种运动智能体轨迹规划装置，该装置包括：

初始化模块，用于获取目标e的空间状态s_e以及包含运动智能体空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示运动智能体初始节点的空间状态；

t₀表示规划开始时间；

null表示初始节点的父节点为空；

地图有效时间计算模块，用于基于栅格化的地图，将其他对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map，用于表示栅格未被占用的若干个有效时间段(t_l,t_r)；其中，

所述其它对象为任意几何外形与轨迹可知的物体；

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

t_l和t_r分别表示有效时间段在时间轴上的左边界和右边界；

轨迹规划模块，用于重复执行步骤S3-S6，直至当前节点X_c满足到达目标s_e或开放队列为空，执行S7；且S3-S7包括：

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标e的耗时估计；

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；

S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)；

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到起始节点X₀，将回溯序列反转，得到从起始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

进一步的，所述轨迹规划模块对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N包括：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]。

进一步的，所述轨迹规划模块基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)包括：

获取运动智能体位于S_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

计算所述集合中地图有效时间的交集，得到交集时间段的集合T_c＝[t_c1,t_c2,...,t_ci,...]；

获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

进一步的，所述轨迹规划模块基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，再判断X_n是否在闭合队列中，若不在闭合队列中，且标记X_n的父节点为X_c，则将X_n存入开放队列包括：

获取从s_c移动到各个s_n的耗时Δt_n；

计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_stop1+Δt_n，t_desire2＝t_stop2+Δt_n；

计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

可理解的是，本发明实施例提供的运动智能体轨迹规划装置与上述运动智能体轨迹规划方法相对应，其有关内容的解释、举例、有益效果等部分可以参考运动智能体轨迹规划方法中的相应内容，此处不再赘述。

实施例3：

提供了一种计算机可读存储介质，其存储用于运动智能体轨迹规划的计算机程序，其中，所述计算机程序使得计算机执行如下方法：

S1、获取目标e的空间状态s_e以及包含运动智能体空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示运动智能体初始节点的空间状态；

t₀表示规划开始时间；

null表示初始节点的父节点为空；

S2、基于栅格化的地图，将其他对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map，地图有效时间T_map由若干时间段(t_l,t_r)组成；其中，

所述其它对象为任意几何外形与轨迹可知的物体；

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

t_l和t_r分别表示有效时间段在时间轴上的左边界和右边界；

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标e的耗时估计；

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；

S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)；

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到起始节点X₀，将回溯序列反转，得到从起始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

进一步的，所述S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N包括：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]。

进一步的，所述S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)，包括：

获取运动智能体位于s_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

计算所述集合中地图有效时间的交集，得到交集时间段的集合T_c＝[t_c1,t_c2,...,t_ci,...]；

获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

进一步的，所述S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c，再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列包括：

获取从s_c移动到各个s_n的耗时Δt_n；

计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_stop1+Δt_n，t_desire2＝t_stop2+Δt_n；

计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

实施例4：

提供了一种电子设备，包括：

一个或多个处理器；

存储器；以及

一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器执行，所述程序包括用于执行如下方法：

S1、获取目标e的空间状态s_e以及包含运动智能体空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示运动智能体初始节点的空间状态；

t₀表示规划开始时间；

null表示初始节点的父节点为空；

S2、基于栅格化的地图，将其他对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map，地图有效时间T_map由若干时间段(t_l,t_r)组成；其中，

所述其它对象为任意几何外形与轨迹可知的物体；

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

t_l和t_r分别表示有效时间段在时间轴上的左边界和右边界；

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标e的耗时估计；

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；

S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)；

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到起始节点X₀，将回溯序列反转，得到从起始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

进一步的，所述S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N包括：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]。

进一步的，所述S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)，包括：

获取运动智能体位于s_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

计算所述集合中地图有效时间的交集，得到交集时间段的集合T_c＝[t_c1,t_c2,...,t_ci,...]；

获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

进一步的，所述S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c，再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列包括：

获取从s_c移动到各个s_n的耗时Δt_n；

计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_stop1+Δt_n，t_desire2＝t_stop2+Δt_n；

计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

综上所述，与现有技术相比，本发明具备以下有益效果：

①将时间维度考虑到规划过程中，将静态环境中的规划提升到动态环境中的规划。

②与将时间列成一个独立的维度并进行采样的方式相比，采用地图有效时间的方式极大的降低了搜索空间的复杂度。

③在规划过程中可进行“让行”与“停等”等操作，一些场景下可提前避开冲突，结果更接近最优解。

④对于变化迅速，交通环境结构化低的场景有较好的适应能力，对环境的要求低。

⑤不需要提前在环境中预设节点与轨迹，在不同环境中部署迅速。

需要说明的是，通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种运动智能体轨迹规划方法，其特征在于，该方法包括：

S1、获取目标e的空间状态s_e以及包含运动智能体空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向φ；

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示运动智能体初始节点的空间状态；

t₀表示规划开始时间；

null表示初始节点的父节点为空；

S2、基于栅格化的地图，将其它对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map，所述地图有效时间T_map由若干时间段(t_l,t_r)组成；其中，

所述其它对象为任意几何外形与轨迹可知的物体；

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

t_l和t_r分别表示有效时间段在时间轴上的左边界和右边界；

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标e的耗时估计；

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；

S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)；

其中，t_stop1表示时间段T_stop的左边界，t_stop2表示时间段T_stop的右边界；

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；

其中，t_desire1表示时间段T_desire的左边界，t_desire2表示时间段T_desire的右边界；

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到初始节点X₀，将回溯序列反转，得到从初始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

2.如权利要求1所述的一种运动智能体轨迹规划方法，其特征在于，所述S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N包括：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]；

其中，s_n1表示第1个拓展后的空间状态s_n；s_n2表示第2个拓展后的空间状态s_n。

3.如权利要求1所述的一种运动智能体轨迹规划方法，其特征在于，所述S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)，包括：

获取运动智能体位于s_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

其中，t_map1、t_map2、t_mapi分别表示栅格对应的地图有效时间中的第1个地图有效时间段、第2个地图有效时间段、第i个地图有效时间段；

计算所述集合中地图有效时间的交集，得到交集时间段的集合T_c＝[t_c1,t_c2,...,t_ci,...]；

其中，t_c1、t_c2、t_ci分别表示第1个交集时间段、第2个交集时间段、第i个交集时间段；

获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

4.如权利要求3所述的一种运动智能体轨迹规划方法，其特征在于，所述S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c，再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列包括：

获取从s_c移动到各个s_n的耗时Δt_n；

计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_stop1+Δt_n，t_desire2＝t_stop2+Δt_n；

计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

5.一种运动智能体轨迹规划装置，其特征在于，该装置包括：

初始化模块，用于获取目标e的空间状态s_e以及包含运动智能体空间状态、节点时刻和父节点的初始节点X₀，并将X₀存入空的开放队列中；其中，

X＝(s,t,X_pre)表示节点，其中：

s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向φ；

t表示节点X的时刻；

X_pre表示节点X的父节点；

所述初始节点X₀＝(s₀,t₀,null)，其中：

s₀表示运动智能体初始节点的空间状态；

t₀表示规划开始时间；

null表示初始节点的父节点为空；

地图有效时间计算模块，用于基于栅格化的地图，将其它对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间T_map，用于表示栅格未被占用的若干个有效时间段(t_l,t_r)；其中，

所述其它对象为任意几何外形与轨迹可知的物体；

所述轨迹为由多个有序的空间状态与对应速度构成的集合；

t_l和t_r分别表示有效时间段在时间轴上的左边界和右边界；

轨迹规划模块，用于重复执行步骤S3-S6，直至当前节点X_c满足到达目标s_e或开放队列为空，执行S7；且S3-S7包括：

S3、取开放队列中期望最小的节点作为当前节点X_c，判断当前节点X_c是否满足到达s_e或开放队列为空；

若不满足，执行S4～S6；若满足，执行S7；

其中，所述期望是从X₀到达当前节点X_c的耗时加上从当前节点X_c到达目标e的耗时估计；

S4、对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N；

S5、基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的若干个地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)；

其中，t_stop1表示时间段T_stop的左边界，t_stop2表示时间段T_stop的右边界；

S6、基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，且标记X_n的父节点为X_c；再判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列；返回S3，以更新当前节点X_c；

其中，t_desire1表示时间段T_desire的左边界，t_desire2表示时间段T_desire的右边界；

S7、若当前节点X_c到达s_e，则根据父节点从最终节点X_f向前回溯，直至找到初始节点X₀，将回溯序列反转，得到从初始节点X₀到最终节点X_f的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；

若开放队列为空，则输出规划执行失败。

6.如权利要求5所述的一种运动智能体轨迹规划装置，其特征在于，所述轨迹规划模块对当前节点X_c的空间状态s_c向多个运动方向进行拓展，得到由多个拓展后的空间状态s_n构成的集合S_N包括：

基于s_c分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到S_N＝[s_n1,s_n2,...]；

其中，s_n1表示第1个拓展后的空间状态s_n；s_n2表示第2个拓展后的空间状态s_n。

7.如权利要求5所述的一种运动智能体轨迹规划装置，其特征在于，所述轨迹规划模块基于当前节点X_c的空间状态s_c以及运动智能体位于s_c时所占栅格对应的地图有效时间T_map，计算当前节点的停留允许时间段T_stop＝(t_stop1,t_stop2)包括：

获取运动智能体位于S_c时所占栅格对应的地图有效时间的集合T_map＝[t_map1,t_map2,...,t_mapi,...]；

其中，t_map1、t_map2、t_mapi分别表示栅格对应的地图有效时间中的第1个地图有效时间段、第2个地图有效时间段、第i个地图有效时间段；

计算所述集合中地图有效时间的交集，得到交集时间段的集合T_c＝[t_c1,t_c2,...,t_ci,...]；

其中，t_c1、t_c2、t_ci分别表示第1个交集时间段、第2个交集时间段、第i个交集时间段；

获取当前节点X_c的时刻t_c作为t_stop1，并向时间轴右侧探索到首个交集时间段的右边界作为t_stop2；得到停留允许时间段T_stop＝(t_stop1,t_stop2)。

8.如权利要求7所述的一种运动智能体轨迹规划装置，其特征在于，所述轨迹规划模块基于停留允许时间段T_stop和从s_c移动到s_n的耗时Δt_n，计算期望到达时间段T_desire＝(t_desire1,t_desire2)；并基于s_n、T_desire构建新节点X_n，再判断X_n是否在闭合队列中，若不在闭合队列中，且标记X_n的父节点为X_c，则将X_n存入开放队列包括：

获取从s_c移动到各个s_n的耗时Δt_n；

计算期望到达时间段T_desire＝(t_desire1,t_desire2)，其中，t_desire1＝t_stop1+Δt_n，t_desire2＝t_stop2+Δt_n；

计算各个s_n对应的期望到达时间段T_desire与各个交集时间段t_ci的交集的左边界，取第一个左边界为t_n；若t_n存在，则记为新节点X_n＝(s_n,t_n,X_c)，并判断X_n是否在闭合队列中，若不在闭合队列中，则将X_n存入开放队列，将当前节点X_c存入闭合队列。

9.一种计算机可读存储介质，其特征在于，其存储用于运动智能体轨迹规划的计算机程序，其中，所述计算机程序使得计算机执行如权利要求1-4任一项所述的运动智能体轨迹规划方法。

10.一种电子设备，其特征在于，包括：

一个或多个处理器；

存储器；以及

一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器执行，所述程序包括用于执行如权利要求1-4任一项所述的运动智能体轨迹规划方法。