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

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN112729326A(43)申请公布日2021.04.30(21)申请号202011538659.4(22)申请日2020.12.23(71)申请人北京易控智驾科技有限公司地址100190北京市海淀区中关村东路8号东升大厦AB座七层701B单元(72)发明人冯酉南(74)专利代理机构北京久诚知识产权代理事务所(特殊普通合伙)11542代理人余罡(51)Int.Cl.G01C21/34(2006.01)权利要求书4页说明书18页附图2页(54)发明名称运动智能体轨迹规划方法、装置、存储介质和电子设备(57)摘要本发明提供了一种运动智能体轨迹规划方法、装置、存储介质和电子设备，涉及轨迹规划技术领域。每次取开放队列中期望最小的节点作为当前节点，判断是否满足到达目标；若不满足，对当前节点的空间状态向多个运动方向进行拓展；再计算当前节点的停留允许时间段；进而计算期望到达时间段；以此构建新节点存入开放队列，重复执行，直至到达目标，则根据父节点从最终节点向前回溯找到起始节点，将回溯序列反转，提取对应的空间状态与时刻信息，输出最终轨迹。将时间维度考虑到规划过程中，将静态环境中的规划提升到动态环境中的规划。极大的降低了搜索空间的复杂度，结果更接近最优解。不需要提前在环境中预设节点与轨迹，在不同环境中部署迅速。CN112729326ACN112729326A权利要求书1/4页1.一种运动智能体轨迹规划方法，其特征在于，该方法包括：S1、获取目标e的空间状态se以及包含运动智能体空间状态、节点时刻和父节点的初始节点X0，并将X0存入空的开放队列中；其中，X＝(s,t,Xpre)表示节点，其中：s表示X的空间状态，包含运动智能体的二维坐标(x,y)和朝向t表示节点X的时刻；Xpre表示节点X的父节点；所述初始节点X0＝(s0,t0,null)，其中：s0表示运动智能体初始节点的空间状态；t0表示规划开始时间；null表示初始节点的父节点为空；S2、基于栅格化的地图，将其他对象的轨迹投影到地图内的栅格中，计算地图内的所有栅格的地图有效时间Tmap，所述地图有效时间Tmap由若干时间段(tl,tr)组成；其中，所述其它对象为任意几何外形与轨迹可知的物体；所述轨迹为由多个有序的空间状态与对应速度构成的集合；tl和tr分别表示有效时间段在时间轴上的左边界和右边界；S3、取开放队列中期望最小的节点作为当前节点Xc，判断当前节点Xc是否满足到达se或开放队列为空；若不满足，执行S4～S6；若满足，执行S7；其中，所述期望是从X0到达当前节点Xc的耗时加上从当前节点Xc到达目标e的耗时估计；S4、对当前节点Xc的空间状态sc向多个运动方向进行拓展，得到由多个拓展后的空间状态sn构成的集合SN；S5、基于当前节点Xc的空间状态sc以及运动智能体位于sc时所占栅格对应的若干个地图有效时间Tmap，计算当前节点的停留允许时间段Tstop＝(tstop1,tstop2)；S6、基于停留允许时间段Tstop和从sc移动到sn的耗时Δtn，计算期望到达时间段Tdesire＝(tdesire1,tdesire2)；并基于sn、Tdesire构建新节点Xn，且标记Xn的父节点为Xc；再判断Xn是否在闭合队列中，若不在闭合队列中，则将Xn存入开放队列，将当前节点Xc存入闭合队列；返回S3，以更新当前节点Xc；S7、若当前节点Xc到达se，则根据父节点从最终节点Xf向前回溯，直至找到起始节点X0，将回溯序列反转，得到从起始节点X0到最终节点Xf的有序集合，根据每个节点对应的空间状态与时刻信息输出最终轨迹；若开放队列为空，则输出规划执行失败。2.如权利要求1所述的一种运动智能体轨迹规划方法，其特征在于，所述S4、对当前节点Xc的空间状态sc向多个运动方向进行拓展，得到由多个拓展后的空间状态sn构成的集合SN包括：基于sc分别向前、向后以多个方向盘转角进行的运动递推，并去除与地图边界发生碰撞的空间状态，得到SN＝[sn1,sn2,...]。3.如权利要求1所述的一种运动智能体轨迹规划方法，其特征在于，所述S5、基于当前2CN112729326A权利要求书2/4页节点Xc的空间状态sc以及运动智能体位于sc时所占栅格对应的若干个地图有效时间Tmap，计算当前节点的停留允许时间段Tstop＝(tstop1,tstop2)，包括：获取运动智能体位于sc时所占栅格对应的地图有效时间的集合Tmap＝[tmap1,tmap2,...,tmapi,...]；计算所述集合中地图有效时间的交集，得到交集时间段的集合Tc＝[tc1,tc2,...,tci,...]；获取当前节点Xc的时刻tc作为tstop1，并向时间轴右侧探索到首