本发明公开了一种基于分层状态机的混合式驾驶方法，先利用传感器感知环境信息，再利用分层状态机认定具体场景和规划驾驶动作，然后利用滚动时域优化的局部路径规划方法进行具体驾驶路线及指令的规划，最后对规划的驾驶路线进行合成，合成后的轨迹点所对应的加速度等信息即为智能车决策系统的指令规划，那么智能车按照合成轨迹就可以进行安全驾驶。
1.一种基于分层状态机的混合式驾驶方法，其特征在于，包括以下步骤：(1)、利用传感器感知环境信息
在车联网方式下，利用传感器获取智能车道和周围两边车道前方和后方最近车辆的距离、速度信息，以及智能车的速度信息以及GPS导航和路网文件；
(2)、利用分层状态机的顶层状态机认定具体场景顶层状态机M顶＝(Q顶,Σ顶,δ顶,q0顶,F顶)，其中，Q顶为位于不同场景之间的特定状态；Σ顶为GPS坐标、地图、任务文件构成的对场景认定的特定约束，作为输入事件；q0∈Q为系统的初始状态； 在时刻i，系统处于状态qi顶时，qi顶∈Q顶，接受输入事件ai后，δ顶为状态迁移到新状态的概率分布； 是系统任务完成的终止状态；
(3)、利用分层状态机的底层状态机规划驾驶动作底层状态机M底＝(Q底,Σ底,δ底,q0底,F底)，Q底为决策过程的各子阶段状态，Σ底为决策过程中产生的各种换道逻辑，δ底采用全概率转移，q0底为决策开始的初始阶段状态，F底为车道保持和换道决策结果对应的状态集合；
(4)、基于步骤(3)规划的驾驶动作，利用滚动时域优化的局部路径规划方法进行具体驾驶路线及指令的规划(4.1)、在Frenet坐标系下，初始时刻t0，智能车横、纵向方向的位置、速度、加速度状态分别为(l0,l′0,l″0),(s0,s′0,s″0)，规划路径的末时刻为t1，其状态为(l1,l′1,l″1),(s1,s′1,s″1)，智能车轨迹的制动周期为T＝t1-t0；
(4.2.1)、设置目标配置集合[l1,l′1,l″1,T]＝[d,0,0,t]，d为横向位置，t为决策时间；
(4.2.2)、设置横向控制的代价函数：CL＝kjmax(|at0-at|)+ktT+kd|l1-llabel|其中，max(|at0-at|)表示决策时间t内最大加速度的变化值，保证智能车舒适性，T为制动时间，要求智能车能短时间内制动，|l1-llabel|为末状态的横向位移与车道线中心的距离，保证车辆不会偏离目标车道中心线，中心线位置为llabel；kj、kt和kd为惩罚性系数；
(4.2.3)、基于代价函数，通过配置d和t来进行轨迹采样，从而在备选轨迹集合中选取代价函数最小的最优的轨迹；
(4.3.1)、跟车：t1时刻车辆速度为前车速度，位置为前车位置减去安全距离，加速度为
(4.3.2)、车速保持：设置目标配置集合：[s′1,s1″,T]＝[s',0,t]
基于代价函数，通过配置s'和t来进行轨迹采样，从而在备选轨迹集合中选取代价函数最小的最优的轨迹；
[s1,s′1,s″1,T]＝[d*,0,0,t]其中，d*为纵向位置，t为决策时间；
基于代价函数，通过配置d*和t来进行轨迹采样，从而在备选轨迹集合中选取代价函数最小的最优的轨迹；
其中，kL横行控制系数，kS纵向控制系数；τ＝1,2，当τ＝1时，取车速保持的代价函数当τ＝2时，取车速保持的代价函数(6)、智能车按照合成轨迹进行安全驾驶。
2.根据权利要求1所述的基于分层状态机的混合式驾驶方法，其特征在于，所述底层状态机在决策过程中产生的各种换道逻辑为：(2.1)、产生换道意图
利用车头时距去衡量是否离前方车辆足够近，即：TH＝d/vhost
其中，d为前后车辆车头间距，vhost为后车的速度；
当TH值大于预设阈值，则不存在换道动机，进行车道保持；否则，存在空间层面的换道动机，并进行后续步骤判断；
在实际交通规则的约束下，根据实际车况路况，选择邻近的一条具体可行的车道，优先考虑临界左车道；若当前路段不允许变道，则进行车道保持，否则进行后续步骤判断；
(2.3.1)、计算智能车与目标车道后方跟随车的风险系数P1：其中，τ∈(0,t)，反应了加速度对纵向位移的影响，d0为在换道初始阶段智能车的车尾部和跟随车的车头部的横向距离，as-host和vs-host为智能车的加速度和速度，as-follow和vs-follow为后方跟随车辆的加速度和速度，dtb为智能车与跟随车的实际距离；
(2.3.2)、计算智能车与目标车道前方前导车的风险系数P2：其中，ksafe为驾驶员的敏感系数，L1为前导车车辆长度，L2为冗余距离；
判断步骤(2.3.1)和(2.3.2)计算得到的风险系数均小于预设阈值，则执行换道策略，否则，进行车道保持。
一种基于分层状态机的混合式驾驶方法
技术领域
[0001] 本发明属于自动驾驶技术领域，更为具体地讲，涉及一种基于分层状态机的混合式驾驶方法。
背景技术
[0002] 车辆不恰当的换道行为是道路交通事故和交通拥堵的主要因素。城区环境下，交通要素复杂多变，其状态难以预测。在换道过程中，驾驶员不仅需要仔细观察车辆周围的交通条件，还需要短时间内，根据当前车辆与目标车辆的空间条件、时间条件、车辆状况和驾驶员的主管意愿等，进行判断，决策是否换道、如何换道。换道行为作为日常驾驶中的一种常见行为，也是一种比较复杂的驾驶行为。
[0003] 传统的经验规则决策算法对智能车及环境模型的依赖较高，在复杂动态的交通环境下，车辆的换道空间具有明显的差异性，换道过程中的大量动态性、不确定信息也为自动驾驶车辆的驾驶决策带来了巨大挑战。
[0004] 先验规则型驾驶行为建模过程通常不涉及数据采样和训练，而是人为解析驾驶场景抽取有用信息，利用先验规则，综合考虑安全性、舒适性、目的便利性等因素，建立驾驶场景到安全驾驶行为的映射。
发明内容
[0005] 本发明的目的在于克服现有技术的不足，提供一种基于分层状态机的混合式驾驶方法，针对多车道换道场景，实现无人驾驶过程中的有效决策。
[0006] 为实现上述发明目的，本发明一种基于分层状态机的混合式驾驶方法，其特征在于，包括以下步骤：
[0007] (1)、利用传感器感知环境信息
[0008] 在车联网方式下，利用传感器获取本车道和周围两边车道前方和后方最近车辆的距离、速度信息，以及本车的速度信息以及GPS导航和路网文件；
[0009] (2)、利用分层状态机的顶层状态机认定具体场景
[0010] 顶层状态机M顶＝(Q顶,Σ顶,δ顶,q0顶,F顶)，其中，Q顶为位于不同场景之间的特定状态；Σ顶多为GPS坐标、地图、任务文件构成的对场景认定的特定约束，作为输入事件；q0∈Q为系统的初始状态，； 在时刻i，系统处于状态qi顶时，qi顶∈Q顶，接受输入事件ai后，δ顶为状态迁移到新状态的概率分布； 是系统任务完成的终止状态；
[0011] (3)、利用分层状态机的底层状态机规划驾驶动作
[0012] 底层状态机M底＝(Q底,Σ底,δ底,q0底,F底)，Q底为决策过程的各子阶段状态，Σ底为决策过程中产生的各种换道逻辑，δ底采用全概率转移，q0底为决策开始的初始阶段状态，F底为车道保持和换道决策结果对应的状态集合；
[0013] (4)、基于步骤(3)规划的驾驶动作，利用滚动时域优化的局部路径规划方法进行具体驾驶路线及指令的规划
[0014] (4.1)、在Frenet坐标系下，初始时刻t0，智能车横、纵向方向的位置、速度、加速度状态分别为(l0,l′0,l″0),(s0,s′0,s″0)，规划路径的末时刻为t1，其状态为(l1,l′1,l″1),(s1,s′1,s″1)，智能车轨迹的制动周期为T＝t1-t0；
[0015] (4.2)、智能车横向控制
[0016] (4.2.1)、设置目标配置集合[l1,l′1,l″1,T]＝[d,0,0,t]，d为横向位置，t为决策时间；
[0017] (4.2.2)、设置横向控制的代价函数：CL＝kjmax(|at0-at|)+ktT+kd|l1-llabel|[0018] 其中，max(|at0-at|)表示决策时间t内最大加速度的变化值，保证智能车舒适性，T为制动时间，要求智能车能短时间内制动，|l1-llabel|为末状态的横向位移与车道线中心的距离，保证车辆不会偏离目标车道中心线，中心线位置为llabel；kj、kt和kd为惩罚性系数；
[0019] (4.2.3)、基于代价函数，通过配置d和t来进行轨迹采样，从而在备选轨迹集合中选取代价函数最小的最优的轨迹；
[0020] (4.3)、智能车纵向控制
[0021] (4.3.1)、跟车：t1时刻车辆速度应该为前车速度，位置应该前车位置减去安全距离，加速度为0；
[0022] (4.3.2)、车速保持：设置目标配置集合：
[0023] [s′1,s″1,T]＝[s′,0,t]
[0024] 其中，s′为纵向速度，t为决策时间；
[0025] 制定车速保持的代价函数：
[0026]
[0027] 其中，s′label为需要保持的纵向速度；
[0028] 基于代价函数，通过配置s′和t来进行轨迹采样，从而在备选轨迹集合中选取代价函数最小的最优的轨迹；
[0029] (4.3.3)、停车：设置目标配置集合：
[0030] [s1,s′1,s″1,T]＝[d*,0,0,t]
[0031] 其中，d*为纵向位置，t为决策时间；
[0032] 制定停车的代价函数：
[0033]
[0034] 基于代价函数，通过配置d*和t来进行轨迹采样，从而在备选轨迹集合中选取代价函数最小的最优的轨迹；
[0035] (5)、轨迹二维合成
[0036]
[0037] 其中，kL横行控制系数，kS纵向控制系数；τ＝1,2，当τ＝1时，取车速保持的代价函数 当τ＝2时，取车速保持的代价函数
[0038] (6)、智能车按照合成轨迹进行安全驾驶。
[0039] 本发明的发明目的是这样实现的：
[0040] 本发明基于分层状态机的混合式驾驶方法，先利用传感器感知环境信息，再利用分层状态机认定具体场景和规划驾驶动作，然后利用滚动时域优化的局部路径规划方法进行具体驾驶路线及指令的规划，最后对规划的驾驶路线进行合成，合成后的轨迹点所对应的加速度等信息即为智能车决策系统的指令规划，那么智能车按照合成轨迹就可以进行安全驾驶。
[0041] 同时，本发明基于分层状态机的混合式驾驶方法还具有以下有益效果：
[0042] (1)、采用混合式规划决策体系结构，既符合逐层递阶体系的特点：系统结构层次清晰，每个模块各司其职，问题求解精度较高；也满足动作反应体系的特点：结合了诸如变道超车的动作分解，各子行为相对独立，提高了系统鲁棒性。
[0043] (2)、系统中的行为规划层加上滚动时域优化的局部路径规划，综合传感器感知的周围环境，规划出一条无碰撞路径，进一步保障了系统的安全性。
[0044] (3)滚动时域优化的局部路径规划能保障系统的实时性以及考虑驾驶过程的舒适性作出决策。
附图说明
[0045] 图1是本发明基于分层状态机的混合式驾驶方法流程图；
[0046] 图2是智能车感知信息示意图；
[0047] 图3是顶层状态机的决策机制结构图；
[0048] 图4是换道状态机的决策机制结构图；
[0049] 图5是路径规划的决策机制结构图；
[0050] 图6是横向位移路径规划仿真图；
[0051] 图7是跟车模型纵向规划仿真图；
[0052] 图8是速度保持模型纵向速率规划仿真图；
[0053] 图9是停车模型纵向速率规划仿真图；
[0054] 图10是中轨迹的二维合成仿真图。
具体实施方式
[0055] 下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。
[0056] 实施例
[0057] 图1是本发明基于分层状态机的混合式驾驶方法流程图。
[0058] 在本实施例中，如图1所示，本发明一种基于分层状态机的混合式驾驶方法，包括以下步骤：
[0059] S1、利用传感器感知环境信息
[0060] 如图2所示，在车联网方式下，通过雷达等传感器获取本车道和周围两边车道前方和后方最近车辆的距离、速度信息，以及本车的速度信息以及GPS导航和路网文件；
[0061] S2、利用分层状态机的顶层状态机认定具体场景
[0062] 顶层状态机M顶＝(Q顶,Σ顶,δ顶,q0顶,F顶)，其中，Q顶为位于不同场景之间的特定状态；Σ顶多为GPS坐标、地图、任务文件构成的对场景认定的特定约束，作为输入事件；q0∈Q为系统的初始状态，； 在时刻i，系统处于状态qi顶时，qi顶∈Q顶，接受输入事件ai后，δ顶为状态迁移到新状态的概率分布； 是系统任务完成的终止状态；
[0063] 在本实施例中，如图3，采用GPS导航和路网文件等完成如乡村道路、城区道路、高速道路场景的认定及路上、匝道等具体子场景的细化。
[0064] S3、利用分层状态机的底层状态机规划驾驶动作
[0065] 如图4所示，底层状态机负责具体驾驶子动作的决策，不但承接了上层状态机对驾驶环境的认定，更是对下层实际行车的轨迹规划模块服务，能够将复杂的决策逻辑清晰条理化，排错容易。
[0066] 底层状态机M底＝(Q底,Σ底,δ底,q0底,F底)，Q底为决策过程的各子阶段状态，Σ底为决策过程中产生的各种换道逻辑，δ底采用全概率转移，q0底为决策开始的初始阶段状态，F底为车道保持和换道决策结果对应的状态集合；
[0067] 其中，针对多车道换道场景，底层状态机在决策过程中产生的各种换道逻辑为：
[0068] S3.1、产生换道意图
[0069] 在多车道道路中，在路上状态下智能车需要根据实际道路环境，采取车道保持和换道两种驾驶行为。我们常用车头时距去衡量是否离前方车辆足够近，即：
[0070] TH＝d/vhost
[0071] 其中，d为前后车辆车头间距，vhost为后车的速度；
[0072] 车头时距可以反应当前车刹车时，后车驾驶员的最大反应时，可间接用于评价车辆通行时空间层面的安全性因素。后车速度一定的情况下，当TH值大于预设阈值，则不存在换道动机，进行车道保持；否则，存在空间层面的换道动机，并进行后续步骤判断；
[0073] S3.2、交通规则下的车道选择
[0074] 在实际交通规则的约束下，根据实际车况路况，选择邻近的一条具体可行的车道，优先考虑临界左车道；若当前路段不允许变道，则进行车道保持，否则进行后续步骤判断；
[0075] S3.3、计算换道的风险系数
[0076] 在选定好具体目标车道后，分别考虑目标车道的前导车和跟随车的因素，计算换道的风险系数，如果任意的一个风险系数较大，则由换道状态机内部状态迁移到车道保持状态机，如果满足上述条件，则执行换道策略，向相应方向横向移动车辆，在未完成换道时，不断进行风险系数的计算，确认整个换道过程更加安全可靠。下面我们对具体过程进行说明。
[0077] S3.3.1、计算智能车与目标车道后方跟随车的风险系数P1：
[0078]
[0079] 其中，τ∈(0,t)，反应了加速度对纵向位移的影响，d0为在换道初始阶段，智能车的车尾部和跟随车的车头部的横向距离，as-host和vs-host为智能车的加速度和速度，as-follow和vs-follow为后方跟随车辆的加速度和速度，dtb为智能车与跟随车的实际距离；
[0080] S3.3.2、计算智能车与目标车道前方前导车的风险系数P2：
[0081]
[0082] 其中，ksafe为驾驶员的敏感系数，L1为前导车车辆长度，L2为冗余距离；
[0083] S3.3.3、综合判断
[0084] 判断步骤S3.3.1和S3.3.2计算得到的风险系数均小于预设阈值，则执行换道策略，否则，进行车道保持。
[0085] S4、基于步骤S3规划的驾驶动作，利用滚动时域优化的局部路径规划方法进行具体驾驶路线及指令的规划
[0086] 基于滚动时域优化的路径规划算法依靠智能车的传感器实时对周围局部环境进行探测，随着滚动优化在线实时规划，更新最优路径，通过启发式的方法生成最优子目标，在当前时域范围内规划最优路径并实施，时间域信息往前滚动推进，环境信息有可能发生动态变化，此时进行轨迹优化及在线更新。该算法能确保智能车在实时动态跟新的换道环境以确保安全性。
[0087] 如图5，Frenet坐标系很方便的将智能车的二维移动问题解耦成横向和纵向相对独立的优化问题，针对两个方向的目标状态分别求其轨迹的备选集合，针对不同的优化场景和任务诉求分别制定代价函数，结合两者代价函数，选取一条代价最低的轨迹进行物理限制和碰撞检测，如果挑选出的轨迹不满足检测要求，则剔除，接着挑选代价次低的轨迹，直到找到满足条件的代价最小的轨迹。最后进行二维合成，用于进行智能车位置的更新。其中，规划轨迹所对应的的速度、加速度、位置即为智能车决策系统的输出。
[0088] S4.1、在Frenet坐标系下，初始时刻t0，智能车横、纵向方向的位置、速度、加速度状态分别为(l0,l′0,l″0),(s0,s′0,s″0)，规划路径的末时刻为t1，其状态为(l1,l′1,l″1),(s1,s′1,s″1)，智能车轨迹的制动周期为T＝t1-t0；
[0089] S4.2、智能车横向控制
[0090] S4.2.1、设置目标配置集合[l1,l′1,l″1,T]＝[d,0,0,t]，d为横向位置，t为决策时间；
[0091] S4.2.2、设置横向控制的代价函数：CL＝kjmax(|at0-at|)+ktT+kd|l1-llabel|[0092] 其中，max(|at0-at|)表示决策时间t内最大加速度的变化值，保证智能车舒适性，T为制动时间，要求智能车能短时间内制动，|l1-llabel|为末状态的横向位移与车道线中心的距离，保证车辆不会偏离目标车道中心线，中心线位置为llabel；kj、kt和kd为惩罚性系数；
[0093] S4.2.3、基于代价函数，通过配置d和t来进行轨迹采样，从而在备选轨迹集合中选取代价函数最小的最优的轨迹；
[0094] 在本实施例中，初始状态假设为：初始时刻l0＝3，l′0＝0.3，l″0＝0，道路的中心线为l＝0，时间滚动窗口为3s，智能车的横向控制目的为使智能车能沿道路中心线方向平稳行驶，则存在约束：l′1＝0，l″1＝0。其仿真结果如图6所示，虚线为所有备选轨迹曲线，实线为根据上述损失函数选出的备选集合中选择最优轨迹曲线。
[0095] S4.3、智能车纵向控制
[0096] 对于智能车的纵向控制，不同运动情形其优化目标配置集合不尽相同，其场景大致可分为跟车、停车、车速保持，下面我们分别进行讨论。
[0097] S4.3.1、跟车：t1时刻车辆速度应该为前车速度，位置应该前车位置减去安全距离，加速度为0；
[0098] 在本实施例中，初始状态假设为：s0＝0，s′0＝10m/s，s″0＝10，为满足安全驾驶约束，t1时刻车辆速度应该为前车速度，位置应该前车位置减去安全距离，加速度为0。其位置、速度、加速度轨迹可以表示为如图7所示。
[0099] S4.3.2、车速保持：设置目标配置集合：
[0100] [s′1,s1″,T]＝[s′,0,t]
[0101] 其中，s′为纵向速度，t为决策时间；
[0102] 制定车速保持的代价函数：
[0103]
[0104] 其中，s′label为需要保持的纵向速度；
[0105] 基于代价函数，通过配置s′和t来进行轨迹采样，从而在备选轨迹集合中选取代价函数最小的最优的轨迹；
[0106] 在本实施例中，假设需要保持的纵向速度为5m/s，其仿真结果图8如下所示。
[0107] S4.3.3、停车：设置目标配置集合：
[0108] [s1,s′1,s″1,T]＝[d*,0,0,t]
[0109] 其中，d*为纵向位置，t为决策时间；
[0110] 制定停车的代价函数：
[0111]
[0112] 基于代价函数，通过配置d*和t来进行轨迹采样，从而在备选轨迹集合中选取代价函数最小的最优的轨迹；
[0113] 在本实施例中，初始状态假设为：s0＝0，s′0＝3m/s，s″0＝0，停车点为slabel＝12，时间滚动窗口为3s，要求智能车能够以较舒适的方式在停车点停车，且存在约束：s′1＝0，s″1＝0，其仿真结果如图9所示。
[0114] S5、轨迹二维合成
[0115] 为了能够轨迹线的物理和碰撞检测，需要进行轨迹线和代价函数的二维合成：
[0116]
[0117] 其中，kL横行控制系数，kS纵向控制系数；τ＝1,2，当τ＝1时，取车速保持的代价函数 当τ＝2时，取车速保持的代价函数
[0118] 轨迹二维合成结果如图10所示，在合成轨迹曲线上，各轨迹点所对应的加速度等信息即为智能车决策系统的指令规划。其中，如图10所示，虚线为备选轨迹集合，每一个轨迹对应着按照算得的代价值，实线为代价值最小的最优轨迹。我们将代价值由低到高依次进行物理检测和碰撞检测，直到找到一条最优轨迹满足以下所有条件：
[0119] ①轨迹线与其他障碍物无任何碰撞；
[0120] ②纵向S方向的速度、加速度不超过最大限定值；
[0121] ③轨迹线曲率不超过最大曲率。
[0122] S6、智能车按照合成轨迹进行安全驾驶。
[0123] 尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
