基于模拟控制系统的高精度地图路径规划方法
技术领域
本发明涉及无人驾驶汽车技术领域,尤其涉及一种基于模拟控制系统的高精度地图路径规划方法。
背景技术
作为无人驾驶汽车中控制系统的输入源,路径规划输出的参考轨迹合理与否直接决定最终控制效果。然而在现有技术中,路径规划与控制系统是独立设计的,路径规划未能充分考虑控制系统的性能,容易导致控制系统无法很好地跟踪路径规划产生的参考轨迹。如何充分利用高精度地图,并使参考轨迹对于控制系统来说更加容易跟踪,误差更小,对于无人驾驶汽车平稳、安全地行驶具有重要的意义。
发明内容
本发明的一个目的是解决至少上述问题,并提供至少后面将说明的优点。
本发明还有一个目的是提供一种基于模拟控制系统的高精度地图路径规划方法,充分考虑控制系统的模拟轨迹与参考轨迹的偏差,以求得尽可能符合控制系统性能的参考轨迹,并避免不合理参考轨迹的产生。
为实现上述目的和一些其他的目的,本发明采用如下技术方案:
一种基于模拟控制系统的高精度地图路径规划方法,包括以下步骤:
S1、场端或车端基于预存的高精度地图以及车辆的当前状态和目标状态规划出所述车辆的行驶路径;
S2、基于所述行驶路径生成所述车辆的路径参考轨迹;并利用所述车辆的初始状态和当前状态实时更新所述路径参考轨迹;
S3、所述车辆的控制系统按照时间序列利用所述路径参考轨迹以及所述车辆的模型生成所述车辆的路径模拟轨迹;
S4、计算所述路径参考轨迹和路径模拟轨迹间的偏差,并依据所述偏差重新生成路径参考轨迹;
S5、重复S4,直至所述偏差小于预设的偏差阈值或计算所述偏差的次数超出预设的次数阈值时,将最终生成的路径参考路径作为最终路径参考轨迹。
优选的是,所述的基于模拟控制系统的高精度地图路径规划方法中,S1中规划出所述车辆的行驶路径的具体方法为:
S1-1、初级规划:将所述高精度地图抽象为仅含有带有方向的道路信息的第一抽象地图,在所述第一抽象地图上依据所述车辆的当前状态和目标状态标定所述车辆的起点和终点,利用Dijkstra算法得到所述车辆的初级行驶路径;
S1-2、次级规划:在所述高精度地图上提取S1-1得到的所述初级行驶路径包含的车道信息,并依据拓扑关系将所述车道信息抽象为含有方向信息的第二抽象地图,在所述第二抽象地图上利用Dijkstra算法得到所述车辆由起点到终点的车道行驶路径;
S1-3、终极规划:依据所述车道行驶路径得到所述车辆的可行驶区域,并依据所述车辆与障碍物间的距离的大小标定除所述可行驶区域外的其他区域,将所述可行驶区域按照预设的精度划分为网格,最后利用Dijkstra算法得到所述车辆由起点到终点的最终行驶路径。
优选的是,所述的基于模拟控制系统的高精度地图路径规划方法中,所述车辆的当前状态和目标状态均包括:位置、速度、加速度、航向角以及角速度。
优选的是,所述的基于模拟控制系统的高精度地图路径规划方法中,S2中基于行驶路径生成车辆的路径参考轨迹的方法为:
S2-1、将所述行驶路径简化至折线段的形式,并设所述折线段的端点由所述行驶路径的初始点至终止点依次为Pi,i=0,1,2,3…;其中,设定所述车辆在任意Pi点时的状态为Xc;
S2-2、以所述车辆行驶至Pi点时的状态Xc作为路径参考轨迹的起始点Pstart,以Pi+1点作为所述车辆处于终止状态的第一终止点,利用Xc和终止状态间的约束条件生成以时间为序列的基本路径参考轨迹;
S2-3、选取Pi+1点和Pi+2点之间的任一点为所述车辆处于终止状态的第二终止点,利用Xc和终止状态间的约束条件生成次级路径参考轨迹,并在判定所述次级路径参考轨迹与基本路径参考轨迹间的差值是否在预设的范围阈值内,且所述次级路径参考轨迹是否在所述可行驶区域内,是,将所述次级路径参考轨迹作为所述路径参考轨迹;否,重新选取第二终止点;
S2-4、按照预设的频率重复S2-2和S2-3,以实时更新所述路径参考轨迹;
其中,所述终止状态指所述车辆的速度和加速度均为零的状态。
优选的是,所述的基于模拟控制系统的高精度地图路径规划方法中,Xc指位置、速度、加速度、航向角和角速度。
优选的是,所述的基于模拟控制系统的高精度地图路径规划方法中,所述约束条件指Xc中的速度和加速度与终止状态间的差值在预设的控制范围内。
优选的是,所述的基于模拟控制系统的高精度地图路径规划方法中,S2-3中若重新选取第二终止点的次数达到预设的最大阈值,所述次级路径参考轨迹与基本路径参考轨迹间的差值仍超出预设的范围阈值或所述次级路径参考轨迹不完全在所述可行驶区域内时,则将所述基本路径参考轨迹作为所述路径参考轨迹。
优选的是,所述的基于模拟控制系统的高精度地图路径规划方法中,S3中生成所述车辆的路径模拟轨迹的方法为:
S3-1、所述控制系统利用所述路径参考轨迹上的任一时刻ti,i=0,1,2,3…,以及对应于所述时刻的车辆的模拟状态Xi计算得到控制量;
S3-2、所述控制系统将所述控制量应用于预先生成的所述车辆的模型,以得到所述车辆在所述时刻的下一相邻时刻ti+1的模拟状态Xi+1。
优选的是,所述的基于模拟控制系统的高精度地图路径规划方法中,ti由t0逐次选取至tn;其中,t0指所述路径参考轨迹的开始时刻;tn指所述路径参考轨迹的结束时刻。
本发明至少包括以下有益效果:
本发明的基于模拟控制系统的高精度地图路径规划方法中,首先依据基于高精度地图生成的车辆行驶路径生成车辆的路径参考轨迹,然后利用车辆的控制系统结合路径参考轨迹生成车辆的路径模拟轨迹,最后不断通过路径参考轨迹和路径模拟轨迹间的偏差对路径参考轨迹进行修正,以得到适应于车辆控制系统的路径参考轨迹,且更有效的避免了不合理参考轨迹的产生。
本发明的其它优点、目标和特征将部分通过下面的说明体现,部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。
附图说明
图1是本发明提供的基于模拟控制系统的高精度地图路径规划方法的框架结构图。
具体实施方式
下面结合附图对本发明做详细说明,以令本领域普通技术人员参阅本说明书后能够据以实施。
如图1所示,一种基于模拟控制系统的高精度地图路径规划方法,包括以下步骤:
S1、场端或车端基于预存的高精度地图以及车辆的当前状态和目标状态规划出所述车辆的行驶路径。
S2、基于所述行驶路径生成所述车辆的路径参考轨迹;并利用所述车辆的初始状态和当前状态实时更新所述路径参考轨迹。
S3、所述车辆的控制系统按照时间序列利用所述路径参考轨迹以及所述车辆的模型生成所述车辆的路径模拟轨迹。
S4、计算所述路径参考轨迹和路径模拟轨迹间的偏差,并依据所述偏差重新生成路径参考轨迹。
S5、重复S4,直至所述偏差小于预设的偏差阈值或计算所述偏差的次数超出预设的次数阈值时,将最终生成的路径参考路径作为最终路径参考轨迹。
在上述方案中,首先依据基于高精度地图生成的车辆行驶路径生成车辆的路径参考轨迹,然后利用车辆的控制系统结合路径参考轨迹生成车辆的路径模拟轨迹,最后不断通过路径参考轨迹和路径模拟轨迹间的偏差对路径参考轨迹进行修正,以得到适应于车辆控制系统的路径参考轨迹,且更有效的避免了不合理参考轨迹的产生。
利用车辆动力学及运动学模型,充分考虑控制系统的模拟轨迹与参考轨迹的偏差,以求得尽可能符合控制系统性能的参考轨迹,使参考轨迹对于控制系统来说更加容易跟踪,误差更小,即使得无人驾驶汽车行驶更加平稳和安全。
其中,S4中将所述偏差加入到生成参考轨迹时的每个终止点上,以重新计算参考轨迹和模拟轨迹。
一个优选方案中,S1中规划出所述车辆的行驶路径的具体方法为:
S1-1、初级规划:将所述高精度地图抽象为仅含有带有方向的道路信息的第一抽象地图,在所述第一抽象地图上依据所述车辆的当前状态和目标状态标定所述车辆的起点和终点,利用Dijkstra算法得到所述车辆的初级行驶路径;
S1-2、次级规划:在所述高精度地图上提取S1-1得到的所述初级行驶路径包含的车道信息,并依据拓扑关系将所述车道信息抽象为含有方向信息的第二抽象地图,在所述第二抽象地图上利用Dijkstra算法得到所述车辆由起点到终点的车道行驶路径;
S1-3、终极规划:依据所述车道行驶路径得到所述车辆的可行驶区域,并依据所述车辆与障碍物间的距离的大小标定除所述可行驶区域外的其他区域,将所述可行驶区域按照预设的精度划分为网格,最后利用Dijkstra算法得到所述车辆由起点到终点的最终行驶路径。
在上述方案中,利用高精度地图进行了分层路径规划,并产生参考轨迹,提高了轨迹生成的效率和精度。
具体原理为:初级规划为最高层次的规划,称为link级规划,以有向图来代表抽象地图,即该抽象地图为link级别的有向图。Link理解为道路,高精度的地图的link含有很多信息,比如坐标位置、曲率等,这里的抽象地图仅仅是把link的位置及前后的关联关系提取出来而形成的有向图。有向图中的结点node代表link,边界edge代表link之间的连通关系。在这个层级上,给定起始和终止的node,由Dijkstra算法就可得到最优的link路径,其中,为了加快搜索速度,也可用A*算法代替。
次级规划为第二层次的规划,称为lane级别规划,lane可理解为道路上的车道。当由第一层次规划得到link级的路径后,对所有这条路径上关联的lane以有向图来表示,图中的node代表lane,edge代表lane之间的连通关系,由Dijkstra或者A*算法均可得到最优或次优lane路径。
终极规划为第三层次的规划,称为网格级别规划,当由第二层次规划得到lane级别的路径后,计算该路径上的可行驶区域,并在网格图中设置权值0表示该可行驶区域,其余区域依据离障碍物的距离填充大于0且不超过1的权值,然后由Dijkstra或者A*算法就可得到网格级路径,即所述车辆由起点到终点的最终行驶路径。
即在本发明中的分层规划的意思是:首先针对link级的有向图,计算一条比较粗略的路线,这层规划类似于传统导航中的路径规划,生成的路线是link级的,即只能精确到link,即道路;然后在此基础上,根据生成的link路线,从高精度地图中提取出路线中每一条link里的lane,把这些lane依据拓扑关系生成lane级别的有向图并计算路线,该路线精确到lane,即车道;最后根据lane级别的路线,获取可行驶区域,将该区域网格化后,计算得到路线,该路线精确到网格,具体精度依据网格的大小不同而不同。
一个优选方案中,所述车辆的当前状态和目标状态均包括:位置、速度、加速度、航向角以及角速度。
一个优选方案中,S2中基于行驶路径生成车辆的路径参考轨迹的方法为:
S2-1、将所述行驶路径简化至折线段的形式,并设所述折线段的端点由所述行驶路径的初始点至终止点依次为Pi,i=0,1,2,3…;其中,设定所述车辆在任意Pi点时的状态为Xc。
S2-2、以所述车辆行驶至Pi点时的状态Xc作为路径参考轨迹的起始点Pstart,以Pi+1点作为所述车辆处于终止状态的第一终止点,利用Xc和终止状态间的约束条件生成以时间为序列的基本路径参考轨迹。
S2-3、选取Pi+1点和Pi+2点之间的任一点为所述车辆处于终止状态的第二终止点,利用Xc和终止状态间的约束条件生成次级路径参考轨迹,并在判定所述次级路径参考轨迹与基本路径参考轨迹间的差值是否在预设的范围阈值内,且所述次级路径参考轨迹是否在所述可行驶区域内,是,将所述次级路径参考轨迹作为所述路径参考轨迹;否,重新选取第二终止点。
S2-4、按照预设的频率重复S2-2和S2-3,以实时更新所述路径参考轨迹。
其中,所述终止状态指所述车辆的速度和加速度均为零的状态。
在上述方案中,上述方法采用运动轨迹生成方法来得到路径参考轨迹,使得得到的路径参考轨迹更加可靠且平滑。
具体原理为:1、将车辆由起点到终点的最终行驶路径即前述的网格级的路径合并简化为折线段,设折线段端点依次为Pi,i=0,1,2,3…;2、设定车辆在初始点P0的初始状态为X0;3、当车辆经过某点Pi时,以车辆的当前状态Xc设定参考轨迹的起始点Pstart,以Pi+1作为终止点,并以速度、加速度均为零作为终止状态;在一定的约束条件下,生成以时间为序列的参考轨迹做为基本参考轨迹;4、由于前一步会使得车辆在每个端点停住(速度、加速度均为零),影响行驶的连续性,为了使得车辆在端点处以非零的速度通过,因而将终止点设定在Pi+1及Pi+2之间,该点按设定的策略随机选取,并重新计算参考轨迹,判断轨迹是否在Pi+1的范围阈值之内,并且在可行驶区域之内;5、第3步及第4步按一定频率迭代进行,以实现实时更新路径参考轨迹。
一个优选方案中,Xc指位置、速度、加速度、航向角和角速度。
一个优选方案中,所述约束条件指Xc中的速度和加速度与终止状态间的差值在预设的控制范围内。
一个优选方案中,S2-3中若重新选取第二终止点的次数达到预设的最大阈值,所述次级路径参考轨迹与基本路径参考轨迹间的差值仍超出预设的范围阈值或所述次级路径参考轨迹不完全在所述可行驶区域内时,则将所述基本路径参考轨迹作为所述路径参考轨迹。
在上述方案中,如果尝试一定数量的点之后依旧找不到符合要求的参考轨迹,那么就以基本参考轨迹作为最终参考轨迹。
一个优选方案中,S3中生成所述车辆的路径模拟轨迹的方法为:
S3-1、所述控制系统利用所述路径参考轨迹上的任一时刻ti,i=0,1,2,3…,以及对应于所述时刻的车辆的模拟状态Xi计算得到控制量。
S3-2、所述控制系统将所述控制量应用于预先生成的所述车辆的模型,以得到所述车辆在所述时刻的下一相邻时刻ti+1的模拟状态Xi+1。
在上述方案中,控制系统可分为两个部分,一是控制系统本身,该系统可以直接使用车辆的控制算法,以保证贴近真实系统;二是车辆的动力学及运动学模型,该模型可以通过对车辆进行数学建模,并以实验的方式获取相关物理参数的值来构建,也可以运用深度学习等方法,直接根据测试数据来进行端到端的数据拟合。
其中,控制量指的是控制系统的输出,其内容依据实际系统的不同而有所区别。如果控制系统是以对油门/刹车及方向盘施加外力的形式接管车辆控制的话,控制量就是就是对油门/刹车施加的力以及对方向盘施加的力矩。
一个优选方案中,ti由t0逐次选取至tn;其中,t0指所述路径参考轨迹的开始时刻;tn指所述路径参考轨迹的结束时刻。
在上述方案中,ti由t0逐次选取至tn,实现了依据时间顺序对路径模拟轨迹的生成,从而能够获得完整的路径模拟轨迹。
尽管本发明的实施方案已公开如上,但其并不仅仅限于说明书和实施方式中所列运用,它完全可以被适用于各种适合本发明的领域,对于熟悉本领域的人员而言,可容易地实现另外的修改,因此在不背离权利要求及等同范围所限定的一般概念下,本发明并不限于特定的细节和这里所示出与描述的图例。