一种基于有限状态机的车辆自组织与协同控制方法

技术领域

本发明涉及协同自动驾驶技术领域，尤其是涉及一种基于有限状态机的网联自动驾驶车辆自组织与协同控制方法。

背景技术

近年来，协同自动驾驶系统(Cooperative-Automated Driving System，C-ADS)及相关算法技术得到广泛研究。相较于单车自动驾驶，协同自动驾驶可以借助通信技术，实现V2V、V2I等全方位协同配合，从而有效解决单车自动驾驶面临的冲突死锁、交互保守等问题。

当前关于协同自动驾驶主要包含求解运动控制量与求解通行顺序两种思路，前者通常采用集中式优化方法控制车辆运动，提升效率安全舒适性，后者计算多辆车通过冲突点的先后顺序，保证车辆无冲突行驶。但在现实应用中，上述方法往往存在计算开销大、缺乏实时性、控制精度差、发生碰撞等问题。

此外，在现实世界中，交通场景复杂多变，交通流密度变化不一，人机混驾环境都导致的高度不确定性，这些因素导致网联自动驾驶车需要根据不同环境采取不同措施以应对可能面临的风险。

发明内容

本发明的目的是提出一种基于有限状态机的网联自动驾驶车辆自组织与协同控制方法，本车收集周边车辆状态信息，寻找本车潜在协同对象，通过信息交流形成组织，组织内车辆通过集中协同优化确定每辆车应采取的控制量，实现复杂道路环境下多辆CAV的自组织与协同。

本发明的目的可以通过以下技术方案来实现：

一种基于有限状态机的车辆自组织与协同控制方法，涉及构建两个车辆状态模型：单车状态模型M1与组织状态模型M2，以及这两个模型的状态切换和演变。

单车状态模型M1的单车状态是指CAV本车不需与其他车辆进行通信，不需要协同。本车可采用自由行驶或避障行驶的行驶模式。单车状态下CAV车辆执行如下步骤：

S11 CAV本车收集周边车辆信息。

S12 CAV本车执行组织寻找动作。

S13 CAV本车执行信息检查动作。

S14 CAV本车执行风险检查动作。

S15 CAV本车执行行驶动作。

S16 CAV本车执行状态转换。

组织状态模型M2的组织状态是指CAV本车与其他CAV进行通信，多辆CAV形成组织。车辆可以自由行驶、编队行驶或是协同避撞。组织状态下CAV车辆执行如下步骤：

S21组织内CAV执行共享信息动作。

S22组织内CAV执行信息检查动作。

S23组织内CAV执行风险检查动作。

S24组织内CAV执行行驶动作。

S25组织内CAV执行驶离组织动作。

S26云端控制器执行组织分裂动作。

S27组织内CAV执行状态转换。

与现有技术相比，本发明具有以下优点

(1)本发明提出的基于有限状态机的网联自动驾驶车辆自组织与协同控制方法，上层通过车辆自组织实现协同对象选取，下层通过优化实现协同控制，双层模型实现了对协同问题的拆分与求解，有效处理高密度交通流复杂交互场景下网联自动驾驶车辆之间的协同决策与控制问题。

(2)本发明提出的车辆自组织有限状态机，各个交通参与者可以自主发现冲突问题，并于协同对象建立联系，而无需指定协同对象，极大地拓展了协同自动驾驶算法的使用场景。

(3)本发明实现了网联自动驾驶车的分层协同控制，相比集中式控制，该方法不具有中心节点，更具有鲁棒性，同时拆解优化问题，提高求解效率；相比于分布式控制，该方法仅对冲突严重车辆进行集中控制，求解更具有优越性。

附图说明

图1为本发明所述的整体框架图；

图2为本发明所述的单车状态及动作集的执行流程图；

图3为本发明所述的组织状态及动作集的执行流程图；

图4为本发明实施例的场景图示与演示图示。

具体实施方式

一种基于有限状态机的车辆自组织与协同控制方法，涉及构建两个车辆状态模型：单车状态模型M1与组织状态模型M2，以及两个模型的状态切换和演变。

单车状态模型M1的单车状态是指CAV本车不需与其他车辆进行通信，不需要协同。本车可采用自由行驶或避障行驶的行驶模式。单车状态下CAV车辆执行如下步骤：

S11 CAV本车收集周边车辆信息。

S12 CAV本车执行组织寻找动作。

S13 CAV本车执行信息检查动作。

S14 CAV本车执行风险检查动作。

S15 CAV本车执行行驶动作。

S16 CAV本车执行状态转换。

组织状态模型M2的组织状态是指CAV本车与其他CAV进行通信，多辆CAV形成组织。车辆可以自由行驶、编队行驶或是协同避撞。组织状态下CAV车辆执行如下步骤：

S21组织内CAV执行共享信息动作。

S22组织内CAV执行信息检查动作。

S23组织内CAV执行风险检查动作。

S24组织内CAV执行行驶动作。

S25组织内CAV执行驶离组织动作。

S26云端控制器执行组织分裂动作。

S27组织内CAV执行状态转换。

状态的定义：根据有限状态机理论，CAV车辆被分为“单车”与“组织”，两种状态下的CAV车辆分别执行不同的动作。根据动作执行结果，两种状态可以互相转换。

动作的定义：根据有限状态机理论，针对“单车”与“组织”状态，分别设定了“信息检查”、“风险检查”、“行驶”等动作，车辆在运动过程中会顺序的执行相关动作的程序。

感知范围与邻居范围：感知范围是指CAV上传感器所能监测到的范围，车辆可以获得该范围内的障碍物信息。邻居范围是自定义的一个距离阈值，用于CAV聚集编队，邻居范围小于感知范围。

行驶模式：在“行驶”动作中，不同状态的CAV依照不同的公式得出加速度控制量，即对应不同的行驶模式。行驶模式共包括“单车-自由行驶”、“单车-避障行驶”、“组织-自由行驶”、“组织-协同行驶”、“组织-编队行驶”。

以下结合附图和实施例对本发明技术方案做进一步介绍。本实施例技术方案适用于无信号环形交叉口、匝道汇入等场景。本发明方法通过CAV车载感知设备收集周边车辆信息，由云端控制器进行信息处理，并在需要协同时统一控制车辆。在技术上具有可行性。

实施例

一种基于有限状态机的车辆自组织与协同控制方法涉及构建两个车辆状态模型：单车状态模型M1与组织状态模型M2，以及两个模型的状态切换和演变。

过程如下：

初始状态：车辆运动初始状态应为单车状态。每回合开始时，车辆根据自身状态决定执行S1步骤或S2步骤。状态切换如图1所示。

S1：如图2所示，本车状态为单车状态时通过以下步骤，完成运动控制。

S11：CAV本车获取信息。记本车为Veh0，本车在行驶过程中，获取自身车辆信息，包括位置(x0,y0)、速度v0、航向角heading

S12：CAV本车执行组织寻找动作。搜索本车邻居范围车辆，若存在其他CAV，则向该车发送组织请求。

S13：本车执行信息检查动作。检查本车是否收到其他车辆组织请求并进行回复；查看自身请求是否得到同意回复。若将形成组织，则本车在下一回合状态转变为“组织”状态。

S14：本车执行风险检查动作。计算与本车感知范围内的其他车辆的风险指标，包括交通安全评价指标：TTC(碰撞时间)与PET(后侵占时间)。

在TTC计算公式中，Dist代表本车与目标车的间距，v

在PET的计算公式中，Dist

若TTC

S15：本车进行行驶动作。在S14中，若不存在冲突车辆，则本车采用“单车-自由行驶”模式。否则本车采用“单车-避障行驶”模式行驶。

进一步的，在S15中，“单车-自由行驶”的计算公式如下：

本车加速度通过IDM模型确定。输入车辆参数与模型参数，获得本车在当前时刻应该采取的加速度；其中，车辆参数包括本车期望速度v

公式(3)中车间最小间隙s*(v)的计算公式如下：

公式(3)与(4)中，s

进一步的，在S15中，“单车-避障行驶”的计算公式如下：

此实例中采用滚动优化方法举例进行优化求解加速度。车辆动力学模型采用质点模型：

x

y

v

t＝0,1,2,…,9 (9)

其中，a

此实例中，需要对障碍物进行轨迹预测。输入障碍物当前运动学状态信息，输出障碍物在未来一段内车辆位置坐标，保证本车在未来行驶过程的每个时间步和目标车保持安全距离。在本实例中，采用恒定加速度方法举例：

a

v

x

y

t＝0,1,2,…,9(14)

其中，x

此实例中，优化求解公式采用滚动优化方法举例：

其中θ

θ

θ

θ

应满足以下约束：加速度范围约束、速度范围约束、车辆间距安全约束，分别为：

a

v

dist(veh

其中dist()计算两车间距离，公式如下：

其中，x

S16：本车执行状态转换。根据上述动作执行结果，本车将在下回合采取单车状态或组织状态，并决定执行S1的步骤或S2的步骤。

S2：如图3所示，本车状态为组织状态时通过以下步骤，完成运动控制。

S21：组织内部所有车辆共享信息。包括位置{(x0,y0),(x1,y1),(x2,y2),…,(xn,yn)}、速度{v0,v1,v2,…,vn}，航向角{heading

S22：组织内部所有车辆执行信息检查动作。检查本车是否收到组织外车辆的组织请求并进行回复；查看自身请求是否得到同意回复。若形成组织，则外部车辆加入组织或进行组织合并。

S23：组织内部所有车辆执行风险检查动作。组织内部车辆计算与自身周边车辆的风险指标TTC与PET，具体计算公式同S14。若指标超过阈值，则认为与该车存在潜在冲突。具体的，Veh0的潜在冲突可分为三类，并采用不同策略。

(1)与组织内部的CAV存在冲突。此时云端控制器记录冲突(Veh0，Vehi)，其中Vehi为冲突车辆，该冲突将由云端控制器集中协同。

(2)与组织外部的CAV存在冲突。此时CAV本车向该车发送组织请求，云端控制器记录冲突(Veh0，Vehi)，在S25动作中，该车被视为动态障碍物。

(3)与组织外部其他车辆存在冲突。云端控制器记录冲突(Veh0，Vehi)，该车无法协同，将被视为动态障碍物。

根据获得所有冲突(Veh i，Veh j),云端控制器将其视为无向图，根据顶点链接关系提取其中的不连通的子图，进而获得协同对象群。如图4所示，实例中{ego车、2号车、3号车}可视为一个协同对象群。针对每一个协同对象群，云端控制器将进行并行化的集中协同优化控制。

S24：组织内部所有车辆执行行驶动作。组织状态有三种行驶模式，“组织-协同行驶”模式采用集中式优化确定各车运动控制量；“组织-编队行驶”模式采用反馈控制完成车辆聚集；“组织-自由行驶”模式采用IDM模型计算控制量。

进一步的，在S24中，若组织内车辆不属于任何协同对象群，且无前方车辆，本车可采用“组织-自由行驶”模式，见公式(3)(4)。

进一步的，在S24中，若组织内车辆不属于任何协同对象群，但存在前方车辆，本车采用“组织-编队行驶”模式，控制量由车辆编队模型计算得到。此实例中采用线性反馈律进行举例，计算公式如下：

e＝x

其中：i为本车，i-1为前车，e代表车间距与期望距离之差，v

进一步的，在S24中，对协同对象群内的车辆，采用“组织-协同行驶”模式。云端控制器集中优化后向各车发送控制指令。协同行驶模式中，组织内车辆i在时刻t时的加速度通过以下公式求得。优化目标见公式(25).

其中，VEH为组织内所有协同的车辆，h为优化时域，J

满足以下约束：

X

v

a

其中，公式(26)表示车辆动力学公式，描述了车辆在时刻t时的动力学状态X

公式(27)-公式(29)分别表示速度范围，加速度范围与安全距离约束，同公式(19)-公式(21)。

优化目标的具体公式如下：

与“单车-避障行驶”不同的是，在“组织-协同行驶”中，受控对象由一辆车变为多辆车，优化目标为多辆车目标之和。执行效果如图4片段3所示，ego车与2车3车为组织内同一协同对象群车辆，三辆车的运动由云端控制器计算获得，结果为ego加速先行，3车减速让行。

S25：组织内部所有车辆执行驶离组织判定。当本车与周边车辆不存在冲突时，本车根据本车及周边车位置信息、目标地信息进行判断，若车辆间距大于阈值，则本车驶离当前组织，本车状态转变为单车状态。

进一步的，在S25中，采用规则方法作为驶离组织判定依据举例。具体公式如下：

t

其中，t

S26：组织云端控制器执行组织分裂判定。云端控制器集中获取所有车辆的位置信息及目标地信息，采用聚类方法将车辆组织划分为小型组织。其中，若网联自动驾驶车与其他车辆存在冲突，则该车不被允许离开组织，待该车与其它车无冲突后将离开组织。

进一步的，在S26中，采用规则与聚类方法相结合作为组织分裂依据。其中聚类方法为DBSCAN方法。具体公式如下：

new group＝DBSCAN(eps,min_samples)(26)

其中eps为扫描半径，此实例中采用车辆空间距离d1；min_samples为最小包含点数，此实例中设定为1。

若在一次分裂中，仅有一辆车分裂离开组织，则该车在下一回合将转变为单车状态。若在一次分裂中，多辆临近车同时分离组织，则这些车离开原组织形成新组织，在下一回合将转变为组织状态，但新组织需重新分配云端控制器。

S27：组织内部所有车辆执行状态转换。根据上述动作执行结果，车辆将在下回合采取单车状态或组织状态，并决定执行S1流程或S2流程。

上述描述仅是对本申请较佳实施例的描述，并非是对本申请范围的任何限定。任何熟悉该领域的普通技术人员根据上述揭示的技术内容做出的任何变更或修饰均应当视为等同的有效实施例，均属于本申请技术方案保护的范围。