自动驾驶汽车合规性安全决策规划系统和方法

本发明涉及自动驾驶，特别涉及一种应用于自动驾驶车辆高速公路场景下的合规性安全决策规划系统和方法。

背景技术：

1、遵守交通法规是道路上人类驾驶员上路行驶的基本要求，随着近年来自动驾驶技术的发展，在可预见的将来人类驾驶车辆与自动驾驶车辆共存的混合交通流将会成为常态，但目前的自动驾驶车辆主要仅把安全和避撞作为决策规划的条件，这会导致人类驾驶员的误解和不信任，甚至引发事故，因此，确保自动驾驶决策规划系统的合规性是有必要的。同时，高速公路由于其路况相对简单，预计会成为自动驾驶最早应用的场景之一，但近年来有关自动驾驶高速公路的事故频发，由于高速公路车速普遍较快，一旦发生事故将会直接造成乘客的生命财产的损失，因此确保高速公路场景下自动驾驶汽车决策规划系统的合规性对于保证自动驾驶的安全性并推广自动驾驶技术的广泛应用至关重要。

2、目前的自动驾驶车辆对于道路法规合规性的研究较少，对于合规性的研究大都仅仅停留在监测领域。如2022年中国科学院软件研究所在专利号为cn115662118a的中国发明专利中公开了一种面向自动驾驶测试的交规合规性自动化判定方法及装置。它由地图获取模块、自动驾驶测试模块、目标交通元素检测模块、是否违规判断模块与合规性评估模块。该发明基于场景信息绘制语义地图，判断车辆在语义地图中投影的合规情况，按照测试对象在自动驾驶测试过程中遵守交通规则的情况统计，旨在于评估测试对象的交规合规性。提升交规合规性判定效率以及判定结果的准确性和可靠性。

3、但是上述现有技术存在以下缺陷：

4、1.适用于容易获取场景信息的仿真测试及离线监测，但很难实现自动驾驶汽车的车端实时应用。

5、2.只是作为违规行为发生后的监测模块，无法参与到自动驾驶决策中约束车辆行为，避免车辆产生违规行为。

技术实现思路

1、本发明针对现有技术的缺陷，提供了一种自动驾驶汽车合规性安全决策规划系统和方法。以高速公路为设计运行域，在决策层基于候选行为集方法，结合提出的双层准入方法对安全性和静态法规的合规性进行处理，在规划层基于人工势场方法对动态法规的合规性进行处理。通过分层解耦处理，该方法能够有效的处理不同类型的法规并进行拓展，基于候选行为集的决策方法相比于当前主流的基于状态机的决策方法更加简单，且能应对的场景更多，计算效率相对较高，基于人工势场的规划方法相比传统二次规划方法算法的结构简单，实时性较强，因此本系统具有可靠性高、拓展性强、计算效率高、实时性强等优点。

2、为了实现以上发明目的，本发明采取的技术方案如下：

3、一种自动驾驶汽车合规性安全决策规划系统，包括：感知模块、导航模块、合规性安全决策规划模块和控制模块，

4、合规性安全决策规划模块包括：决策层和规划层；

5、决策层的作用是基于感知模块获取到的周围环境信息和自车状态信息，对驾驶决策进行高层次的规划。判断并选择合适的行驶策略，以确保车辆在驾驶过程中能够满足交通规则、法律要求和安全性要求。决策层根据当前交通情况、道路条件以及自车状态因素，制定合适的行为规则，如加速、减速、变道、超车等。

6、规划层的作用是根据决策层生成的高层次行驶策略，结合导航模块提供的全局参考路径，进行路径规划和轨迹生成。规划层会考虑多个因素，如车辆动力学特性、周围环境的变化、交通流量等，以生成车辆经过的具体轨迹。规划层还需要考虑合规性和安全性，确保生成的轨迹能够安全地导航汽车通过各种复杂的交通场景。

7、感知模块包括：雷达、摄像头和其他车载传感器，用于获取周围环境中的他车信息、交通标志标线信息以及自车状态信息。

8、所述他车信息包括：他车在全局坐标系中的位置、他车的外形几何，他车类型以及他车的速度和航向角信息；

9、交通标志标线信息包括：交通标志标线的类型、交通标线的坐标和曲率信息等；

10、自车状态信息包括：自车在全局坐标系中的位置、自车的外形几何以及他车的速度、航向角信息。

11、导航模块用于提供全局参考路径；

12、控制模块用于跟踪控制由合规性安全决策规划模块生成的参考轨迹。

13、进一步地，所述决策层包括：候选行为集生成模块、安全评估模块、静态法规评估模块、评价指标模块和最优决策生成模块；

14、候选行为集生成模块根据感知模块发送的信息生成当前可供选择的候选行为。

15、安全评估模块负责评估候选行为集中每个行为对应的安全性，基于评估的风险和安全阈值将不安全的行为过滤掉，生成安全行为集；

16、静态法规评估模块根据当前场景中道路标线和标志，将违反静态法规的行为滤除，生成初步的通过行为集。

17、评价指标模块基于设计的评价指标，对通过行为集中的动作进行评估，为每一个动作赋一个评价值；

18、最优决策生成模块根据每个动作的评价值选出价值最高的动作，作为决策层输出的决策行为信息。

19、进一步地，所述规划层包括：安全势场构建模块、合规势场构建模块、势场整合模块，规划凸空间生成模块、参考轨迹生成模块和参考轨迹优化模块。

20、规划层中，势场构建模块负责将环境建模为势场，其根据感知模块发送的环境信息，对道路和周围车辆进行势场的构建，包括法规势场的构建和安全势场的构建，之后势场整合模块将法规势场和安全势场整合为最终的环境势场。

21、规划凸空间生成模块根据决策层生成的决策动作在环境势场中开辟出规划凸空间；

22、参考轨迹生成模块在规划凸空间对应的环境势场中寻找势场值最低的一条线作为参考轨迹；

23、参考轨迹优化模块将生成的参考轨迹进行平滑并输出给控制模块进行跟踪控制。

24、本发明还公开了一种自动驾驶汽车合规性安全决策规划方法，该方法是基于上述自动驾驶汽车合规性安全决策规划系统实现的，包括以下步骤：

25、步骤一：通过感知系统获取环境信息。环境信息包括车道信息：车道线坐标xline，yline、车道线类型tline、车道id与车道宽度wl；周围车辆信息：车辆坐标xtgt，ytgt、车速vtgt、航向角长度ltgt和宽度wtgt以及车辆的预测轨迹信息trajpre＝(xpre，ypre，vpre，t)，t≤np，其中np为自车的规划时域；还有自车状态信息其中ey为车辆与参考轨迹的横向偏差，为车辆与参考轨迹的航向角偏差。

26、步骤二：候选行为集构建模块基于环境信息列举出所有的候选行为动作；

27、候选行为集的表示如下：

28、actionset＝[action1；action2；…；actioni；…；actionn]

29、其中n为当前场景下候选行为的候选行为个数，每个可选驾驶动作包含三部分：

30、actioni＝[ai，lanecur，i，lanedes，i，lighti]

31、其中ai代表可选的动作，由于本发明的设计运行域为高速公路，因此可选动作主要有ai∈{lk，r_cl，e_cl，r_cr，e_cr}，其中lk表示保持当前车道行驶，r_cl表示准备向左换道，e_cl表示执行向左换道，r_cr表示准备向右换道，e_cr表示执行向右换道。lanecur，i表示当前行驶的车道id，lanedes，i表示目标车道id，lighti表示与动作ai匹配的车辆转向灯的状态，lighti∈{on_l，off，on_r}，这三种状态量的值是逻辑值，on_l为真时代表左转向灯亮起，off为真代表转向灯关闭，on_r为真代表右转向灯亮起。

32、步骤三：双层准入条件判断。对候选行为集中的动作进行筛选，将不安全的和违反静态法规的动作去除，只保留安全且符合静态法规的动作。首先要进行的是安全评估，安全评估的评估对象是车辆每个动作对应的规划范围内的风险，每一个动作对应一条车道的选择，基于自动驾驶车辆周围车辆的预测轨迹结合自车的规划时域对每条车道的风险进行评估。

33、步骤四：评价指标模块基于评价指标为所有动作赋值。评价指标包括：速度指标fv，空间指标fs和前车车型指标ft。

34、步骤五：最优决策生成模块基于每个行为的评价值选出最优决策动作。基于步骤四的评价指标，每个候选动作都有了一个评价值，基于评价值选出最优的动作作为决策层的决策动作。

35、步骤六：势场构建模块基于环境信息构建法规势场和安全势场。基于感知模块的环境信息，进行环境势场的构建，包括安全势场的构建和法规势场的构建。安全势场是保证车辆行驶中不发生碰撞的底线，自车不应该进入周围车辆的安全势场范围，安全势场通过使用自车相对于障碍物车辆距离的双曲函数进行构建；

36、法规势场在高速公路场景下考虑的有车道线法规势场和周围车辆法规场，对于车道线法规势场，当自车在道路上行驶时，除非需要变道，否则车辆应该沿道路中心线行驶且最好不要骑压车道线，并且禁止撞击道路隔离带或越过车道实线，否则会造成危险甚至导致事故。法规势场的车道线通过二次函数定义

37、对于周围车辆的法规势场，规定当车辆在道路上行驶时应当与周围车辆保持必要的安全距离。周围车辆合规势场通过指数函数的形势构建为可跨越势场，车辆的合规势场构建包括两种情况，一是自车与同车道前车的跟车限距的法规，二是换道超车时与邻侧车道车辆保持合适距离的法规。

38、步骤七：势场整合模块将法规势场与安全势场整合为环境势场。

39、步骤八：规划凸空间生成模块基于最优动作将环境势场分割。基于步骤五中的最优决策动作对应的车道，对当前环境势场中以车道为单位的空间进行分割，将最优决策动作对应车道和当前车道的空间作为后续的规划凸空间pfplanningpfplanning＝pflane_cur+pflane_actionbest

40、其中pflane_cur指自车当前车道区域对应的环境势场，pflane_actionbest指自车决策最优动作所选的车道区域对应的环境势场。

41、步骤九：参考轨迹生成模块从凸空间中搜寻最低点作为参考轨迹。基于上一步的规划凸空间和自车当前位置，通过寻找势场值最低的点作为参考轨迹：

42、referenceline(actionbest)＝min(pfplanning(actionbest))

43、参考轨迹中主要包含了期望纵向位置xref，期望横向位置yref，期望车速vref。

44、步骤十：参考轨迹优化模块对参考轨迹进行平滑并输出。采用mpc进行轨迹优化和跟踪，车辆动力学模型采用常用的三自由度单车模型，考虑轨迹的曲率问题，mpc引入了轨迹跟踪的误差，系统状态空间方程如下：

45、

46、y＝cx

47、其中u＝[fxt，δf]t，

48、经过mpc优化，生成了一条安全合规且平滑的轨迹。

49、进一步地，步骤三双层准入条件判断的具体子步骤如下：

50、自动驾驶车辆的行驶范围由格点表示，点与点之间的间隔为规划步长δt，行驶范围长度为规划区域pl，pl＝ve·t，宽度为车道的宽度，每个动作代表一条车道的选择，格点表示为ω(j，t)，每个格点包含坐标信息和车辆速度信息，即ω(j，t)＝(xe，ye，ve)，其中安全评估中做出匀速行驶假设，以车辆初始评估时刻的速度为所有格点的车速，则每个状态格点表示为

51、

52、其中lmin表示车道下边界，δl代表横向采样间隔，j表示横向格点的下标，j＝1：(lmax-lmin)/δl。预测的车辆轨迹也被分解为与自动驾驶车辆在不同时间的行驶范围相关的位置点，用三角形表示。

53、安全评估将碰撞时间ttc和车头时距th相结合来建立安全评估函数：

54、

55、其中，d(t)为时间t时刻自车格点与前方车辆的距离，vtgt表示前方车辆的车速，ltgt为前车长度；

56、通过横向风险因子γ表示周围车辆对自车横向范围的影响，γ通过他车预测轨迹点与自车规划时域内格点的横向距离表示：

57、

58、其中，n代表每条车道内他车的数量，δl代表车道宽度，δl代表格点横向采样间隔，a表示横向风险因子的取值范围。

59、通过计算每个时刻预测车辆轨迹序列点和格子中状态点之间的ttc和th，获得自动驾驶车辆每条车道的环境风险：

60、

61、其中i是规划范围内周围车辆的数量，ωth，ωttc是权重因子；定义时间损失因子η。

62、定义安全阈值safemax，第一层安全准入设计如下：

63、

64、双层准入的第二层是静态法规的合规性评估，考虑了道路标志标线限制和行为转移的合规性限制，合规动作的筛选基于当前动作通过预定义的合规转移列表进行查找下一个合规动作，合规转移列表定义如下表1，其中t表示合规，f表示违规禁止：

65、表1静态法规行为转移合规性列表

66、

67、基于上述合规动作转移列表，第二层静态合规准入设计如下：

68、

69、结合第一层的安全准入判断和第二层的静态合规性准入判断，经过双层准入筛选后的通过行为集为：

70、actionsetpass＝logsafe.logstatic.actionset

71、进一步地，步骤四中对于速度评价指标fv，定义为：

72、

73、其中vlimit为第l条车道的限速，n为车辆规划范围内车道l的车辆数量，vi，l为第i辆车的速度。

74、对于空间评价指标fs，定义为：

75、

76、其中sf，l指车道l前车的位置，sego指自车的位置，pl指规划时域内道路的长度。

77、对于前车车型指标ft，定义为：

78、

79、

80、其中κj是车道l第j辆车的权重系数，距离自车越近，权重越大，tj，l是第j辆车的类型，type1，2，3分别代表乘用车、中型客车和重型卡车，sj，l是车道l内第j辆车的位置。

81、基于速度指标fv，空间指标fs和前车车型指标ft，最终的候选动作评价指标如下：

82、feval，l＝wvfv，l+wsfs，l+wtft，l

83、其中wv，ws，wt分别代表相应的权重值。

84、进一步地，步骤六中安全势场的构建如下：

85、

86、

87、其中ai，bi是周围第i个车辆势场的形状和强度参数，si是自车和周围第i个车辆的安全距离，dx，dy分别为自车与周围车辆的纵向距离和横向距离，ltgt，i，wtgt，i为周围车辆的长度和宽度，xsafe，i，ysafe，i是自车距离第i个车辆的横向和纵向安全距离，tsafe是安全时间间隔，vego，vtgt，i代表自车和周围第i个车辆的速度，是车辆之间的航向角，δvxi，δvyi是车辆之间的横向与纵向接近速度，an代表舒适加速度。

88、所述法规势场的车道线通过二次函数，如下式：

89、

90、其中，asolid，adotted分别代表车道实线和车道虚线处的势场强度参数，sr，q(x，y)指车辆质心到第q条车道线的距离，dsafe为车辆质心到所在车道的车道线的最小安全距离，tline，q是第q条车道线的类型，solid，dotted分别代指实线和虚线。

91、车辆的合规势场构建，如下：

92、

93、

94、

95、其中aj，bj是周围第i个车辆法规势场的形状和强度参数，dx，dy分别为自车与周围车辆的纵向距离和横向距离，lane(ego)id指自车所在车道的id，lane(tgtj)id指第j辆车所在车道的id，xfront，yfront指自车与同车道前车的合规距离，xaround，yaround指自车与邻侧车道周围车辆的合规距离，ttcx为合规时距。

96、进一步的，步骤十中mpc的代价函数如下：

97、

98、其中np，nc分别代表预测时域和控制时域，t+k，t代表在t时刻后第k步的预测值，yref代表输入的参考值。q是输入参考的权重矩阵，其中w1，ω2，w3，w4，w5分别为参考横向位置、参考航向角、参考车速、参考横向跟踪误差和参考航向角误差的权重参数，r是控制量的权重矩阵，其中wf，wδ分别是纵向力和前轮转角的权重参数。

99、其由步骤九中的referenceline提供，其中为

100、

101、与现有技术相比，本发明的优点在于：

102、1.本发明通过将静态法规和动态法规分层处理，能够有效的处理不同类型的法规并进行拓展。

103、2.本发明基于候选行为集的决策方法相比于当前主流的基于状态机的决策方法更加简单，且能应对的场景更多，计算效率相对较高，基于人工势场的规划方法相比传统二次规划方法算法的结构简单，实时性较强。

104、3.本发明在规划层中构建环境势场时安全势场采用不可跨越势场，保证了自动驾驶车辆行驶中的安全性，而法规势场通过可跨越势场进行构建，能够有效地保证车辆在安全时能够合规行驶，危险请情况下也能够突破法规以保证安全，安全性和鲁棒性高。