本发明属于车辆网联式自动驾驶领域，尤其涉及一种车辆协作式编队行驶系统。
背景技术：
智能网联汽车是指搭载先进的车载传感器、控制器、执行器等装置，并融合现代通信与网络技术，实现车与x(人、车、路、云端等)智能信息交换、共享，具备复杂环境感知、智能决策、协同控制等功能，可实现“安全、高效、舒适、节能”行驶，并最终可实现替代人来操作的新一代汽车。汽车智能化可分为驾驶辅助(da)、部分自动驾驶(pa)、有条件自动驾驶(ca)、高度自动驾驶(ha)、完全自动驾驶(fa)五个等级。
网联化可分为网联辅助信息交互、网联协同感知、网联协同决策与控制三个等级。随着车辆通信和计算技术愈来愈成熟，智能车路系统车队控制技术研究成为热点。智能车路系统将车辆组成一列或多列具有共同速度，小车间距的车队，有效地减少了由人为因素所致的交通事故，加强交通安全性；同时，可减少汽车尾气排放和环境污染。
当前国际主流的车队协同驾驶系统结构包含网络层、链接层、协调层、控制层和物理层，协同驾驶策略主要包括编队车辆汇聚、跟驰、退出、停车和协作式换道，由于道路环境和通信环境的不确定性，根据其发送的状态信息选择跟驰策略，编队模型鲁棒性不强。
综上所述，现有技术存在的问题是：
目前的网联式车辆编队算法只能在结构化道路上配合高精度地图使用，且以引导车辆(头车)为中心，根据其发送的状态信息选择跟驰策略，编队模型鲁棒性不强；没有结合基于控制稳定理论提供一种去中心化的自动驾驶多车协作式编队行驶的方法。
技术实现要素：
针对现有技术存在的问题，本发明提供了一种车辆协作式编队行驶系统。
本发明是这样实现的，一种车辆协作式编队行驶系统，所述车辆协作式编队行驶系统从上到下包括网络层、链接层、协调层、控制层和物理层；
所述网络层包括中心服务单元csu、路侧单元rsu、车载单元obu；车载单元obu通过无线网络与链接层的无线路由器通信；所述无线路由器通过以太网分别与中心服务单元csu、路侧单元rsu通信；中心服务单元csu与路侧单元rsu采用zeromq的通信；
所述协调层包括：编队人机交互界面hmi，用于接收人机交互系统中驾驶员设置的起点和终点，完成自动驾驶行为的全局路径规划；并结合全局路径规划规划的行驶路线和车辆定位模块的定位信息；在高精度地图上，规划满足交通规则和安全性要求的局部行驶路线。
所述控制层主要功能是对传感器和网络融合信息进行分析，实现车辆的自动驾驶决策和控制，包括：环境感知、纵横向行驶速度规划、发送车辆控制指令；所述环境感知，由自身附加安装的毫米波雷达感知车辆行驶的外部环境；所述纵横向速度规划，主要利用gps/ins组合定位方式，运用迈普时空的mp-pos320和司南的gnss组合装置获取车辆姿态和位置状态信息。
所述物理层包括：转向控制、油门控制、制动控制，主要作用是执行控制层轨迹规划和速度规划的指令，通过计算各执行器的给定量，油门、转向和制动器接收到命令后执行相应操作，从而完成轨迹跟踪、车辆加减速、换道等行驶动作。
进一步，所述车辆协作式编队行驶系统加装有车载传感器；所述车载传感器包括感知模块，所述感知模块为视觉传感器和毫米波雷达传感器；所述视觉传感器采用单目相机和立体相机；所述毫米波雷达传感器包含前向长距毫米波雷达传感器和四角中距毫米波雷达传感器；
所述车辆协作式编队行驶系统还加装有包含gps\惯导和高精度地图的定位模块，所述gps\惯导用于接收gps信号和车载传感器信号，解算出自车的绝对位置信息；并将绝对位置信息通过以太网送入控制层自动驾驶控制器及车载单元obu；高精度地图用于存储车辆行驶道路的道路边沿、车道中心线、隧道、路段坡道、路段曲率、匝道出入口、服务区出入口的相关信息、交通标志位置和地标性建筑的绝对位置信息；
所述车辆协作式编队行驶系统还加装无线路由器，所述车载控制分别通过can总线和以太网获取感知模块和定位模块的通信数据，并与网络层进行交互，同时获取路侧系统提供的交通信息，形成链接层。
进一步，所述编队人机交互界面hmi，完成自动驾驶行为的全局路径规划中，人机交互模块hmi创建编队；创建编队完成后向控制层下发编队指令；根据车辆汇聚、车辆跟驰、退出编队、编队停车、协作式换道的编队状态填充控制字段实现实时控制；
控制层的控制器根据车载传感器获得的前车状态信息，进行速度控制，完成车辆汇聚；车辆汇聚达到目标距离时进入车辆跟驰状态；车辆中途退出编队时，进入编队车辆退出状态；编队到达全局规划重点前，进入编队车辆停车状态；
所述车辆汇聚包括：
车辆的编队状态模型描述如下：
τi是发动机时间常数，
δi＝xi-1-xi-hi；
其中hi＝δdtar为目标车间距；引入一个期望空间误差具体描述为：
tgo表示从当前时刻保持tgo时间所达到的期望间距；通过应用lyapunov直接法，构造一个原函数为正，导数严格递减的lyapunov函数，具体定义为：
联立方程后得到控制输出模型：
车载计算单元将控制输出信号
进一步，车辆跟驰时，设定目标跟驰距离δdtar，车载计算单元根据当前车辆状态信息，计算δd＝xi-1-xi，为目前实际编队车辆距离；当|δdtar-δd|≤1m时令vi＝vi-1，向底层执行单元透传速度控制目标为前车速度，车辆进入跟驰状态，否则进入车辆汇聚状态。
进一步，车辆退出编队时，中途退出车辆向引导车发送请求；准许退出时，车辆通过摄像头及车载侧向毫波雷达探测相邻车道内有无动静态障碍物，交通标识，经过信息融合决策判断当前道路环境是否可以变道；车载单元obu经计算可以变道后，车辆切换至单车自动驾驶模式，并向控制器填充换道指令字段；车辆换道完成后向引导车辆发送退出成功消息，并重新规划路径，引导车辆更新编队车辆数目及状态信息，车队进入汇聚状态，重新锁定前车。
进一步，编队停车时，判断全局路径规划脚本终点与引导车辆当前最近行驶路径点距离，当此距离小于100m时，进入编队车辆停车模式；引导车辆根据自身速度设置跟驰时距，即令δdtar＝th*vl+δdsafe，参数th设定后则编队车辆减距离随着速度降低而降低，最终停车时保持在安全距离δdsafe，当δd＝xi-1-xi小于δdsafe时刹停车辆。
进一步，所述协作式换道用于当相邻道路车辆发现正在行驶的车队时，向引导车发送加入车队的请求信标，同时启动定时器1，引导车辆根据车队最大车辆数目以及当前车队状态，限制接受\拒绝加入请求，在定时器失效前不断并更新加入车辆信息id；当车辆被允许加入车队后，取消定时器1，引导车辆设置加入点，并向目标车辆纵向控制器设置纵向速度及加入时间；判断加入车辆当前点是否为目标跟随车辆，若非跟随车辆，则引导车向该非跟随车辆发送扩大间距gap_dis的请求；目标跟驰距离dis_req设为2δdtar；若前方车辆是加入车辆的目标跟随车辆，并且gap_dis≥dis_req时，更新车队状态信息，启动车辆汇聚算法，请求加入车辆完成协同换道，编队再次更新变道后id,编队数目，信息标志位置于ack；当变道指令发送期间，系统正忙，令信标busy＝true,同时启动定时器3，接收到协作式变道完成信标后，取消定时器3，更新车辆编队数目，车辆列表，令信标busy＝false，完成此次周期内的车辆协作式换道动作。
本发明的优点及积极效果为：
本发明针对有条件自动驾驶(ca)的工况开发，符合目前网联式车辆编队行驶的趋势；系统架构清晰，高度模块化，方便车队功能扩展；去中心化，编队车辆不必在跟驰过程中时刻掌握引导车(头车)状态。
本发明使用5g方式实现各模块的连接及模块间直接通信，同时加装视觉和毫米波雷达等低成本车载传感器，将感知融合、路径规划、决策控制集成到同一软件架构下，整合车辆道路环境信息，通过高精度控制器实现可靠的车辆编队行驶功能。系统符合网联式车辆编队行驶的发展趋势，高度模块化，易于功能拓展，去中心化的跟驰策略可以减少网络通信不确定性的影响。
附图说明
图1是本发明实施例提供的车辆协作式编队行驶方法流程图。
图2是本发明实施例提供的车队协作式换道的流程图。
图3是本发明实施例提供的车辆协作式编队行驶系统的网络设计框图。
图4是本发明实施例提供的车辆协作式编队行驶系统图。
图中：1、网络层；2、链接层；3、协调层；4、控制层；5、物理层。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明的软件基于linux(ubuntu)操作系统开发，搭配高性能计算机，中心服务单元(csu)软件内部用lcm接口(公共开源)，外部通信用zeromq(公共开源)，开发语言c、c++。
下面结合附图及具体实施例对本发明的应用原理作进一步描述。
如图1所示，本发明实施例提供的车辆协作式编队行驶方法，包括：
步骤一：使用专用实验网络，搭建网络层架构，采用5g方式实现各模块的连接，这些模块主要包括中心服务单元(csu)、路侧单元(rsu)、车载单元(obu)，各模块直接通信。
步骤二：加装车载传感器，感知模块主要包括视觉和毫米波雷达等低成本传感器(视觉传感器采用单目相机和立体相机，毫米波雷达包含前向长距毫米波雷达和四角中距毫米波雷达)，定位模块，包含gps\惯导和高精度地图信息，gps\惯导接收gps信号和车载传感器信号，解算出自车的绝对位置信息。车载控制器分别通过can总线和以太网获取感知模块和定位模块的通信数据，并与网络层进行交互，同时获取路侧系统提供的交通信息，形成链接层。
步骤三：在协调层，通过人机交互界面(hmi)选择引导车辆(头车)编队行驶目的地，上报至网络层后，中心服务单元(csu)作全局路径规划，并向引导车辆下发高精度地图脚本定义路径(包含起点、关键途经点、终点、经纬度信息)，车载单元(obu)获得路径规划关键点列表后，以当前起点作重规划，约每隔1m增加生成一条道路平滑中心线，最终得到包含关键点在内的局部路径规划，由车辆的控制单元调用。
步骤四：协调层启动创建编队，引导车辆在人机交互界面(hmi)选择需要加入编队的车辆id(车牌号)，通过网络通信发送创建编队请求，同一车道线内的车辆接受请求后，通过车载单元(obu)解算自身位置信息，通过单目相机、立体相机、毫米波雷达等传感器获取的车辆行驶环境信息，检测识别交通标志、道路标线和动静态障碍物，进行多源信息融合处理后，请求或拒绝加入编队。引导车辆在固定周期内确认加入编队车辆数目、位置等信息、并向编队车辆下发路径规划信息，编队创建完成。
步骤五：协调层创建编队完成后向控制层下发编队指令。为实现实时控制，必须根据编队状态填充控制字段。本发明定义编队状态包括车辆汇聚、车辆跟驰、退出编队、编队停车、协作式换道。
步骤六：编队车辆汇聚时，采用如下控制输入模型
通过gps\惯导获得本车相对位置xi，本车速度vi，本车加速度ai，借助车载传感器探测解算前车相对位置xi-1，前车速度vi-1，前车加速度ai-1前车控制目标加速度
步骤七：编队车辆跟驰时，设定目标跟驰距离δdtar(为一定值)，设δd＝xi-1-xi，为目前实际编队车辆距离，当|δdtar-δd|≤1m时令vi＝vi-1，车辆保持跟驰状态，否则进入步骤六；
步骤八：编队车辆退出，车辆中途退出编队时向引导车(头车)发送请求，准许退出时，车辆通过摄像头及车载侧向毫米波雷达探测相邻车道内有无动静态障碍物，交通标识，经过信息融合决策判断当前道路环境是否可以变道；车载单元(obu)经计算可以变道后，车辆切换至单车自动驾驶模式，并向控制器填充换道指令字段，车辆换道完成并向引导车辆(头车)发送退出成功消息，引导车辆(头车)更新编队车辆数目及状态信息，回到步骤六。
步骤九：编队车辆停车，判断全局路径规划脚本终点与引导车辆(头车)当前近行驶路径点距离，当此距离小于100m时，进入编队车辆停车模式。引导车辆根据自身速度设置跟驰时距，即令δdtar＝th*vl+δdsafe，参数th(时距)设定后则编队车辆减距离随着速度降低而降低，最终停车时保持在安全距离δdsafe。
如图2所示，本发明实施例提供的车队协作式换道，当相邻道路车辆发现正在行驶的车队时，向引导车(头车)发送加入请求，引导车辆根据车队最大车辆数目及编队状态限制接受\拒绝加入请求。当车辆被允许加入车队后，计算加入点，同时向头车发送加入点前后车辆的id(车牌号)，以加入点的前车为跟驰目标，向加入车辆控制器设置纵向速度，逐渐向加入点靠近。当到达加入点后，向引导车辆发送换道请求，加入车辆此时调用车队汇聚算法，加入点后方车辆此时将目标跟驰距离设为2δdtar，为换道车俩拉开与前车的距离；当到达目标距离后，引导车辆向加入车辆发送换道指令，换道成功后，加入点后方车辆将跟驰目标换为加入车辆，重回车队汇聚状态，引导车(头车)更新车队车辆数目，状态信息。
下面结合具体的系统和网络设计对本发明的应用原理作进一步描述。
如图4所示，本发明实施例提供的车辆协作式编队行驶系统，包括网络层1、链接层2、协调层3、控制层4和物理层5。
所述网络层1主要模块。包括中心服务单元(csu)、路侧单元(rsu)、车载单元(obu)。采用vpn的方式实现各模块间的直接通信，为方便布网，将csu和rsu放到一个局域网内，用无线路由器连接(参考图1)。由于obu、rsu和csu都采用若干独立进程的软件设计构架，进程间通信采用lcm模式，为了保持总体构架不变，因此把通信总体分成两类：一类为网元内通信，即局域网内通信，由各个实体单元采用lcm方式来实现；另一类为网元间通信，即不同网络实体间的通信，由于lcm只能工作在局域网内，不能跨网段工作，因此采用zeromq的通信方式来实现，网络设计框图如图3所示。
所述车辆定位模块，包含gps\惯导和高精度地图信息，gps\惯导接收gps信号和车载传感器信号，解算出自车的绝对位置信息，并将该位置信息通过以太网送入自动驾驶控制器及车载单元(obu)；高精度地图存储车辆行驶道路的道路边沿、车道中心线、隧道、路段坡道、路段曲率、匝道出入口、服务区出入口等相关信息，交通标志位置和地标性建筑等的绝对位置信息。
所述编队人机交互界面(hmi)，接收人机交互系统中驾驶员设置的起点和终点，完成自动驾驶行为的全局路径规划；并结合全局路径规划规划的行驶路线和车辆融合定位模块的定位信息，在多传感器信息融合模块建立的车辆行驶局部环境地图上，规划满足交通规则和安全性要求的局部行驶路线。
所述编队车辆汇聚控制模型，车辆状态模型描述如下：
τi是发动机时间常数，是一个跟汽车相关的参数，
δi＝xi-1-xi-hi
其中hi＝δdtar为目标车间距。本发明引入一个期望空间误差具体描述为：
这里tgo表示从当前时刻保持tgo时间所达到的期望间距；本发明通过应用lyapunov直接法，可构造一个原函数为正，导数严格递减的lyapunov函数，具体定义为：
联立方程后可以得到控制输出模型：
车载计算单元将控制输出信号
编队车辆跟驰时，设定目标跟驰距离δdtar，车载计算单元根据当前车辆状态信息，计算δd＝xi-1-xi，为目前实际编队车辆距离；当|δdtar-δd|≤1m时令vi＝vi-1，向底层执行单元透传速度控制目标为前车速度，车辆进入跟驰状态，否则进入车辆汇聚状态。
编队车辆退出时，中途退出车辆向引导车(头车)发送请求(不允许头车退出编队，不允许单车道退出编队)。准许退出时，车辆通过摄像头及车载侧向毫波雷达探测相邻车道内有无动静态障碍物，交通标识，经过信息融合决策判断当前道路环境是否可以变道；车载单元(obu)经计算可以变道后，车辆切换至单车自动驾驶模式，并向控制器填充换道指令字段。车辆换道完成后向引导车辆(头车)发送退出成功消息，并重新规划路径，引导车辆(头车)更新编队车辆数目及状态信息，车队进入汇聚状态，重新锁定前车。
编队车辆停车时，判断全局路径规划脚本终点与引导车辆(头车)当前最近行驶路径点距离，当此距离小于100m时，进入编队车辆停车模式。引导车辆根据自身速度设置跟驰时距，即令δdtar＝th*vl+δdsafe，参数th设定后则编队车辆减距离随着速度降低而降低，最终停车时保持在安全距离δdsafe，当δd＝xi-1-xi小于δdsafe时刹停车辆。
编队车辆汇聚、跟驰、退出、停车等车队控制方法流程图如图1所示。
路侧单元(rsu)按照1hz下发编队信息(位置、速度、角度、加速度、锁定关系)给车载单元(obu)，车载单元(obu)透传给人机交互界面(hmi)，hmi显示编队信息，有事件实时上报(包含刹车)。
编队车辆协作式换道：当相邻道路车辆发现正在行驶的车队时，向引导车(头车)发送加入车队的请求信标，同时启动定时器1，引导车辆根据车队最大车辆数目以及当前车队状态(系统是否正忙busy)，限制接受\拒绝加入请求，在定时器1失效前不断并更新加入车辆信息(车辆id,车队数目)。当车辆被允许加入车队后，取消定时器1，引导车辆(头车)设置加入点，并向目标车辆纵向控制器设置纵向速度及加入时间。判断加入车辆当前点是否为目标跟随车辆，若非跟随车辆，则引导车(头车)向该非跟随车辆发送扩大间距gap_dis的请求，这里目标跟驰距离dis_req设为2δdtar；若前方车辆是加入车辆的目标跟随车辆，并且gap_dis≥dis_req时，更新车队状态信息，启动车辆汇聚算法，请求加入车辆完成协同换道，编队再次更新变道后id,编队数目，信息标志位置于ack(表明所有信息已经接收到了)。当变道指令发送期间，系统正忙，令信标busy＝true,同时启动定时器3，接收到协作式变道完成信标后，取消定时器3，更新车辆编队数目，车辆列表，令信标busy＝false，此次周期内的车辆协作式换道动作完成。
编队车辆协作式换道控制方法流程图如图2所示。
本发明的软件基于linux(ubuntu)操作系统开发，开发语言c、c++，搭配高性能计算机，中心服务单元(csu)的部署位置可按照网络负载平衡的原则选择合适的位置，通常为区域中心。每个区域中心的csu通过有线方式连接，组成连通图，实现全局交通控制。中心服务单元(csu)与路侧单元(rsu)通过以太网有线连接。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。