CN108616810A - 一种车队自主跟车系统、便携式装置及方法 - Google Patents

Abstract

本发明提供了一种车队自主跟车系统、便携式装置及方法，所述系统包括：数据采集单元中GPS模块用于采集车辆定位信息，惯导模块用于采集车辆的加速度信息和角速度信息；数据处理单元中融合定位模块对GPS信息和惯导信息实时进行数据融合处理，得到自车绝对位姿；跟车控制计算模块根据得到的自车绝对位姿信息、前车绝对位姿信息和头车历史路径，进行绝对路径规划，更新绝对路径表，计算得到车辆目标速度和方向转角用于实现车辆控制。本发明通过GPS模块和INS模块进行组合定位，定位精度高，数据刷新频率高，且不易受光照、天气等环境因素影响，可有效提升自主跟车系统的精度和可靠性。装置方便携带与搭建，成本较低。

Description

一种车队自主跟车系统、便携式装置及方法

技术领域

本发明涉及自动驾驶领域，尤其涉及自主跟车的技术领域，具体地，涉及一种车队自主跟车系统、便携式装置及方法。

背景技术

随着多样化交通需求的产生，自动驾驶技术逐渐得到更广泛的应用。自主跟车技术，即车辆队列中有一辆头车由操作人员驾驶，其他车辆跟随头车的路径自动驾驶的技术，能有效解决多种场景下的问题。例如大量汽车的短途移动，使用自主跟车技术，能够减少驾驶员人数的需求；又例如短途的多人接驳，使用自主跟车技术，能够在数量有限的车辆下提高运载人数，运力灵活。

定位问题是自主跟车技术中一大关键问题。自主跟车对定位精度和可靠性提出了更高的要求。对GPS和INS(惯导)进行数据融合实现组合定位，充分综合各自的技术特点，在提供厘米级精度、100Hz频率的定位数据的同时，增强抗干扰能力,不易受天气和环境遮挡等影响，为自主跟车系统提供准确可靠的定位信息。在准确定位信息的基础上，通过无线网络通信获取各车定位数据，进行路径规划与控制，可实现自主跟车。

经对现有技术的文献检索发现，中国专利申请号201710186870.6，专利名称为：一种自动跟车行驶控制系统及其方法，该专利中通过布置在车上的GPS定位装置、雷达、摄像头及其他多个传感器来采集数据，通过多传感器数据融合来对跟随车执行自动跟车行驶控制。该方案中虽然融合多传感器数据实现跟车，但也因此导致成本较高，难以量产，同时多传感器安装标定比较复杂，模块化集成化困难，一套传感器难以在多辆车上复用，系统部署和操作难度高、工作量大。

检索中还发现，中国专利申请号201710184207.2，专利名称为：一种车队编队驾驶系统及方法，该专利中通过在每辆车上设置定位部、通信部、相邻车道检测部和自动驾驶控制部，定位部采用GPS定位，检测部采用毫米波雷达和超声波雷达，每辆跟车只根据位于其前方且与之相邻的车辆自动跟车行驶。除与上一个发明所述相同问题外，由于每辆跟车只根据位于其前方且与之相邻的车辆自动跟车行驶，会产生累积偏差，如果车队很长的话，在后面的几辆车跟随误差会越来越大，路线会严重偏离。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种车队自主跟车系统、便携式装置及方法，实现可靠的自主跟车驾驶的同时，解决现有技术中成本高、系统过于复杂的问题。

根据本发明的第一方面，提供一种车队自主跟车系统，所述系统位于车队的每辆车上，包括：

数据采集单元，包括GPS模块和惯导模块，所述GPS模块用于采集所在车辆定位信息，所述惯导模块用于采集所在车辆的加速度信息和角速度信息，并将采集的信息传输到自车的数据处理单元；

数据处理单元，包括融合定位模块和跟车控制计算模块，所述融合定位模块对所述数据采集单元的GPS信息和惯导信息实时进行数据融合处理，得到自车绝对位姿，当GPS信号差甚至失效时，通过惯导信息进行航位推算，保持定位数据，所述自车绝对位姿信息和定位数据发送到人机交互单元；所述跟车控制计算模块根据所述融合定位模块得到的自车绝对位姿信息、前车绝对位姿信息和头车历史路径，进行绝对路径规划，更新绝对路径表，计算得到车辆目标速度和方向转角,用于实现车辆控制；

人机交互单元，与所述数据处理单元连接，实时显示所有车辆的跟车状态和绝对位姿信息。

进一步的，当所述车辆不是头车时，所述数据处理单元将融合的自车绝对位姿信息通过无线网络通信平台发送到后车的数据处理单元，并接收来自头车和前车的绝对位姿信息。

更进一步的，所述系统还包括数据存储模块，所述数据存储模块存储头车和前车的定位数据。

进一步的，当所述车辆是头车时，则向所有车辆广播自车绝对位姿信息。

进一步的，所述人机交互单元，进一步提供地图显示以及车队组建的设置、跟车的参数设定功能，当车辆发生紧急情况时会进行报警和进行急停和复位处理。

进一步的，所述跟车控制计算模块计算得到的车辆目标速度和方向转角，发送到车辆底层控制器，实现车辆控制。

进一步的，所述系统进一步包括通信网络平台，用于各个部分之间的通信。

根据本发明的第二方面，提供一种车队自主跟车的便携式装置，所述装置安装于车队的每辆车上，包括：处理器、GPS模块、惯导模块以及保护外壳，所述处理器、GPS模块、惯导模块集成在一电路板上，并位于保护外壳内，所述保护外壳通过安装固定座安装于车辆上，其中：

GPS模块，所述GPS模块用于采集所在车辆定位信息，并将采集的信息传输到自车的处理器；

惯导模块，所述惯导模块用于采集所在车辆的加速度信息和角速度信息，并将采集的信息传输到自车的处理器；

处理器，包括融合定位模块和跟车控制计算模块，所述处理器还连接有人机交互部件；所述融合定位模块对所述数据采集单元的GPS信息和惯导信息实时进行数据融合处理，得到自车绝对位姿，当GPS信号差甚至失效时，通过惯导信息进行航位推算，保持定位数据，所述自车绝对位姿信息和定位数据发送到人机交互部件；所述跟车控制计算模块根据所述融合定位模块得到的自车绝对位姿信息、前车绝对位姿信息和头车历史路径，进行绝对路径规划，更新绝对路径表，计算得到车辆目标速度和方向转角,用于实现车辆控制；

所述人机交互部件，用于实时显示所有车辆的跟车状态和绝对位姿信息。

本发明上述整套装置体积小方便携带，可快速安装到车辆上。

根据本发明的第三方面，提供一种车队自主跟车方法，车队每辆车上安装所述车队自主跟车系统，所述方法包括：

S1：数据采集单元中的GPS模块、惯导模块实时采集数据并发送到数据处理单元；

S2：数据处理单元实时地对GPS模块、惯导模块采集的数据进行数据融合，得到车辆精确的绝对位姿，将信息发送到人机交互单元；

S3：通过人机交互单元设置车辆编号、跟车距离范围、最大车速限制参数；

S4：依次将跟随车按顺序开进跟车队伍，然后启动跟车，驾驶头车，若跟车过程中出现异常，人机交互单元会进行报警提醒并紧急停止；

S5：数据处理单元将融合的绝对位姿信息发送到后车的数据处理单元，并接收来自头车和前车的绝对位姿信息，若是头车则向所有车辆广播自车绝对位姿信息；

S6：数据处理单元接收头车和前车发送过来的绝对位姿数据，并存储数据，根据自车绝对位姿信息、前车绝对位姿和头车历史路径，更新自车绝对路径表，计算得到车辆目标速度和方向转角，并传输到车辆底层控制器，实现自主跟车驾驶。

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

本发明所述系统和装置通过惯性模块与GPS定位融合，解决了GPS频率不高且易受环境干扰的问题，从而不需要雷达、摄像头等冗余传感器，即可实现可靠的自主跟车驾驶，解决了背景对比文件中成本高、系统过于复杂的问题；

本发明所述装置中采用惯导模块体积很小，且数据量小、融合定位算法计算量少，可使用小体积的嵌入式处理器，从而设计成一套便携式装置，集成上述实现自主跟车必需的GPS模块、惯导模块、嵌入式处理器和通信模块，解决了背景对比文件中系统模块化集成化程度低的问题；

本发明所述装置与车辆交互只需与底层控制器进行通信，传感器也无复杂安装和标定的需要，方便安装拆卸，使用非常方便，解决了背景对比文件中系统部署和操作难度高、工作量大的问题；

本发明所述系统和装置中每辆跟车记录头车的绝对定位路径，根据头车的历史路径进行绝对路径规划实现自动驾驶，同时又根据与前车的定位距离调整行驶速度，从而保证了队列中所有车的跟车准确度与安全性，解决了背景对比文件中车队车辆多的情况下，跟随误差累积，跟车路线偏离的问题。

当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明一较优的系统原理框图；

图2为本发明一较优实施例的方法实现流程框图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

参照图1所示，一种车队自主跟车系统的优选实施例示意图，所述系统位于车队的每辆车上，其中包括：数据处理单元、数据采集单元、网络通信平台、人机交互单元，所述数据处理单元与数据采集单元、网络通信平台连接，所述人机交互单元与网络通信平台连接。所述网络通信平台用于各部件之间的通讯。

数据采集单元，包括GPS模块和惯导模块，所述GPS模块用于采集所在车辆定位信息，所述惯导模块用于采集所在车辆的加速度信息和角速度信息，并将采集的信息传输到自车的数据处理单元；

数据处理单元，包括融合定位模块、数据存储模块和跟车控制计算模块，所述融合定位模块对GPS位姿信息和惯导信息实时进行数据融合处理，得到厘米级、100Hz频率的绝对位姿信息，当GPS信号差甚至失效时，通过惯导信息进行航位推算，保持定位数据；所述跟车控制计算模块，根据自身绝对位姿信息、前车绝对位姿和头车历史路径，进行绝对路径规划，更新绝对路径表，计算得到车辆目标速度和方向转角，并发送到车辆底层控制器实现车辆控制；所述数据存储模块存储来自网络通信平台传输的数据，主要包括头车和前车的定位数据；

所述人机交互单元通过无线网络平台与数据处理单元通信，实时显示所有车辆的跟车状态和绝对位姿信息。

作为一优选实施方式，当所述车辆不是头车时，所述数据处理单元将融合的自车绝对位姿信息通过无线网络通信平台发送到后车的数据处理单元，并接收来自头车和前车的绝对位姿信息。更进一步的，所述系统还包括数据存储模块，所述数据存储模块存储头车和前车的定位数据。

作为一优选实施方式，当所述车辆是头车时，则向所有车辆广播自车绝对位姿信息。

作为一优选实施方式，所述跟车控制计算模块计算得到的车辆目标速度和方向转角，发送到车辆底层控制器，实现车辆控制。

作为一优选实施方式，所述GPS模块采用具有RTK功能、可测向的GPS接收机，所述的INS模块采用100Hz或更高频率输出三轴加速度和三轴角速度的INS模块。

作为一优选实施方式，所述数据处理单元采用体积小的嵌入式处理器如树莓派。数据处理单元中：融合定位模块对原始的GPS位姿信息、角速度信息和速度信息实时进行数据融合处理，当接收到INS模块更新数据时进行一次航位推算得到推算解，当收到GPS更新数据时，用GPS测量值与当前的推算解做一次扩展卡尔曼滤波融合得到融合解，用于下一次的推算，最新的推算解输出作为绝对位姿信息。网络通信平台获得头车和前车的绝对位姿信息并记录。

作为一优选实施方式，所述网络通信平台可通过ZigBee网络搭建，进行车辆间的信息传输，头车向所有车辆广播自车绝对位姿信息，跟随车向后车发送自车绝对位姿信息，并接收头车和前车的绝对位姿信息。

作为一优选实施方式，所述人机交互单元可使用平板电脑等智能电子产品，通过ZigBee网络与数据处理单元通信，通过软件界面实时显示所有车辆的跟车状态和绝对位姿信息，并提供地图显示和按键操作供操作人员进行车队组建的设置、跟车的参数设定，当车辆发生紧急情况时界面会进行报警，和/或，进行急停和复位处理。

基于上述跟车系统，本发明还提供一种车队自主跟车的便携式装置的较优实施例结构，其中所述装置安装于车队的每辆车上，包括：处理器、GPS模块、惯导模块以及保护外壳，所述处理器、GPS模块、惯导模块集成在一电路板上，并位于保护外壳内，所述保护外壳通过安装固定座安装于车辆上，其中：

GPS模块，所述GPS模块用于采集所在车辆定位信息，并将采集的信息传输到自车的处理器；

惯导模块，所述惯导模块用于采集所在车辆的加速度信息和角速度信息，并将采集的信息传输到自车的处理器；

处理器，包括融合定位模块和跟车控制计算模块，所述融合定位模块对所述数据采集单元的GPS信息和惯导信息实时进行数据融合处理，得到自车绝对位姿，当GPS信号差甚至失效时，通过惯导信息进行航位推算，保持定位数据，所述自车绝对位姿信息和定位数据发送到人机交互部件；所述跟车控制计算模块根据所述融合定位模块得到的自车绝对位姿信息、前车绝对位姿信息和头车历史路径，进行绝对路径规划，更新绝对路径表，计算得到车辆目标速度和方向转角，用于实现车辆控制；

所述处理器还连接有人机交互部件，用于实时显示所有车辆的跟车状态和绝对位姿信息。

所述自主跟车车队具有一辆头车和若干辆跟车，其中头车为操作人员手动驾驶，跟车跟随前车的路径自动驾驶。所述装置安装的车队车辆进行过底盘改造，所述底盘设有车辆速度和转向控制接口，可接收所述跟车控制计算模块传输的车辆目标速度和方向转角对车辆进行控制，此外无特定要求。

当所述车辆不是头车时，所述处理器将融合的自车绝对位姿信息发送到后车的数据处理单元，并接收来自头车和前车的绝对位姿信息；当所述车辆是头车时，则向所有车辆广播自车绝对位姿信息。

上述实施例中的所述装置实体包括将处理器、GPS模块、INS模块集成在一起的电路板，并设置在保护外壳内。整套装置体积小方便携带，可快速安装到车辆上。所述装置还设有GPS天线。

上述实施例中的处理器采用体积小的嵌入式处理器，融合定位模块、数据存储模块和跟车控制计算模块在某些实施例中可以通过软件实现，融合定位模块对GPS位姿信息和惯导信息实时进行数据融合处理，得到厘米级、100Hz频率的绝对位姿信息。

作为一优选实施方式，所述装置安装的车队车辆提供底层控制器接口，进行过底盘改造，提供车辆速度和转向控制接口，可接收处理器传输的速度和转向控制目标对车辆进行控制，所述处理器计算得到的目标速度和方向转角发送到底层控制器，底层控制器通过底层CAN协议对车辆进行控制。

基于以上系统，提供一种自主跟车方法，车队每辆车上安装所述的车队自主跟车系统，所述方法包括：

S1：数据采集单元中的GPS模块、惯导模块实时采集数据并发送到数据处理单元；

S2：数据处理单元实时地对GPS模块、惯导模块采集的数据进行数据融合，得到车辆精确的绝对位姿，将信息发送到人机交互单元；

S3：通过人机交互单元设置车辆编号、跟车距离范围、最大车速限制参数；

S4：依次将跟随车按顺序开进跟车队伍，然后启动跟车，驾驶头车，若跟车过程中出现异常，人机交互单元会进行报警提醒并紧急停止；

S5：数据处理单元将融合的绝对位姿信息发送到后车的数据处理单元，并接收来自头车和前车的绝对位姿信息，若是头车则向所有车辆广播自车绝对位姿信息；

S6：数据处理单元接收头车和前车发送过来的绝对位姿数据，并存储数据，根据自车绝对位姿信息、前车绝对位姿和头车历史路径，更新自车绝对路径表，计算得到车辆目标速度和方向转角，并传输到车辆底层控制器，实现自主跟车驾驶。

参照图2所示，为一种自主跟车方法的较优实施例，根据该图，包括如下操作步骤：

步骤1：操作人员在车队中每辆车上安装自主跟车装置，连接电源和底层控制模块然后启动装置，作为一优选实施方式，装置安装在车顶，两个测向天线连成的直线在车辆的前进方向上；

步骤2：数据采集单元中的GPS模块、INS模块实时采集数据并发送到数据处理单元，作为一优选实施方式，数据处理单元分别以两个中断处理两个传感器模块的数据；

步骤3：数据处理单元实时地对传感器数据进行数据融合，得到车辆精确的绝对位姿，通过网络通信平台，将信息发送到人机交互单元，作为一优选实施方式，这个过程放在100Hz或更高频率时间中断中运行，以保证定位数据更新速度不变；

步骤4：操作人员通过人机交互单元在软件界面定位地图上点击相应车辆，设置车辆编号(1号为头车，按编号顺序跟车，其中头车为人开)、跟车距离范围、最大车速限制等参数，作为一优选实施方式，车辆数目不大于10辆，跟车距离不大于15m，最大车速不超过30km/h；

步骤5：操作人员依次将跟随车按顺序开进跟车队伍，然后通过人机交互单元启动按键启动跟车，驾驶头车；

步骤6：数据处理单元将融合的绝对位姿结果通过无线网络通信平台发送到后车的数据处理单元，并接收来自头车和前车的绝对位姿信息，若是头车则向所有车辆广播自车绝对位姿信息；

步骤7：数据处理单元接收头车和前车发送过来的绝对位姿数据，并存储数据；

步骤8：数据处理单元根据自车绝对位姿信息、前车绝对位姿和头车历史路径，更新自车绝对路径表，计算得到车辆目标速度和方向转角，并传输到车辆底层控制器，实现自主跟车驾驶，作为一优选实施方式，数据处理单元的处理器和底层控制器可通过网线连接组建通信网络；

步骤9：到达目的地后通过人机交互单元关闭跟车，关闭其他车辆自主跟车装置并拆下，然后将各车停到合适位置。

通过上述实施例，本发明所述系统、装置以及方法能实现准确性和可靠性更高的自主跟车队列。

本发明利用GPS模块的位姿信息、INS模块的加速度和角速度信息实时进行数据融合，得到车辆精确的绝对位姿，通过车间网络通信平台进行车辆间的通信。根据自车和前车和头车的绝对位姿信息进行跟车路径规划和速度转向控制。

本发明通过GPS模块和INS模块进行组合定位，充分结合各自的优点，定位精度高，数据刷新频率高，且不易受光照、天气等环境因素影响，可有效提升自主跟车系统的精度和可靠性。装置方便携带与搭建，成本较低。

需要说明的是，本发明提供的所述自主跟车方法中的步骤，可以利用所述自主跟车方法系统中对应的模块、装置、单元等予以实现，本领域技术人员可以参照所述系统的技术方案实现所述方法的步骤流程，即，所述系统中的实施例可理解为实现所述方法的优选例，在此不予赘述。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。

Claims (10)

1.一种车队自主跟车系统，其特征在于：所述系统位于车队的每辆车上，包括：

数据采集单元，包括GPS模块和惯导模块，所述GPS模块用于采集所在车辆定位信息，所述惯导模块用于采集所在车辆的加速度信息和角速度信息，并将采集的信息传输到自车的数据处理单元；

数据处理单元，包括融合定位模块和跟车控制计算模块，所述融合定位模块对所述数据采集单元的GPS信息和惯导信息实时进行数据融合处理，得到自车绝对位姿，当GPS信号差甚至失效时，通过惯导信息进行航位推算，保持定位数据，所述自车绝对位姿信息和定位数据发送到人机交互单元；所述跟车控制计算模块根据所述融合定位模块得到的自车绝对位姿信息、前车绝对位姿信息和头车历史路径，进行绝对路径规划，更新绝对路径表，计算得到车辆目标速度和方向转角，用于实现车辆控制；

人机交互单元，与所述数据处理单元连接，实时显示所有车辆的跟车状态和绝对位姿信息。

2.根据权利要求1所述的车队自主跟车系统，其特征在于：当所述车辆不是头车时，所述数据处理单元将融合的自车绝对位姿信息发送到后车的数据处理单元，并接收来自头车和前车的绝对位姿信息。

3.根据权利要求2所述的车队自主跟车系统，其特征在于：所述系统还包括数据存储模块，所述数据存储模块存储头车和前车的定位数据。

4.根据权利要求1所述的车队自主跟车系统，其特征在于：当所述车辆是头车时，则向所有车辆广播自车绝对位姿信息。

5.根据权利要求1-4任一项所述的车队自主跟车系统，其特征在于：所述人机交互单元，进一步提供地图显示以及车队组建的设置、跟车的参数设定功能，当车辆发生紧急情况时会进行报警和进行急停和复位处理。

6.根据权利要求1-4任一项所述的车队自主跟车系统，其特征在于：所述跟车控制计算模块计算得到的车辆目标速度和方向转角，发送到车辆底层控制器,实现车辆控制。

7.一种车队自主跟车的便携式装置，其特征在于：所述装置安装于车队的每辆车上，包括：处理器、GPS模块、惯导模块以及保护外壳，所述处理器、GPS模块、惯导模块集成在一电路板上，并位于保护外壳内，所述保护外壳通过安装固定座安装于车辆上，其中：

GPS模块，所述GPS模块用于采集所在车辆定位信息，并将采集的信息传输到自车的处理器；

惯导模块，所述惯导模块用于采集所在车辆的加速度信息和角速度信息，并将采集的信息传输到自车的处理器；

处理器，包括融合定位模块和跟车控制计算模块，所述处理器还连接有人机交互部件；所述融合定位模块对所述数据采集单元的GPS信息和惯导信息实时进行数据融合处理，得到自车绝对位姿，当GPS信号差甚至失效时，通过惯导信息进行航位推算，保持定位数据，所述自车绝对位姿信息和定位数据发送到人机交互部件；所述跟车控制计算模块根据所述融合定位模块得到的自车绝对位姿信息、前车绝对位姿信息和头车历史路径，进行绝对路径规划，更新绝对路径表，计算得到车辆目标速度和方向转角，用于实现车辆控制；

所述人机交互部件，用于实时显示所有车辆的跟车状态和绝对位姿信息。

8.根据权利要求7所述的车队自主跟车的便携式装置，其特征在于：自主跟车车队具有一辆头车和若干辆跟车，其中头车为手动驾驶，跟车跟随前车的路径自动驾驶，所述车队车辆的底盘设有车辆速度和转向控制接口，接收所述跟车控制计算模块传输的车辆目标速度和方向转角对车辆进行控制。

9.根据权利要求7所述的车队自主跟车的便携式装置，其特征在于：当所述车辆不是头车时，所述处理器将融合的自车绝对位姿信息发送到后车的数据处理单元，并接收来自头车和前车的绝对位姿信息；当所述车辆是头车时，则向所有车辆广播自车绝对位姿信息。

10.一种车队自主跟车方法，其特征在于：车队每辆车上安装权利要求1-6任一项所述的车队自主跟车系统，所述方法包括：

S1：数据采集单元中的GPS模块、惯导模块实时采集数据并发送到数据处理单元；

S2：数据处理单元实时地对GPS模块、惯导模块采集的数据进行数据融合，得到车辆精确的绝对位姿，将信息发送到人机交互单元；

S3：通过人机交互单元设置车辆编号、跟车距离范围、最大车速限制参数；

S4：依次将跟随车按顺序开进跟车队伍，然后启动跟车，驾驶头车，若跟车过程中出现异常，人机交互单元会进行报警提醒并紧急停止；

S5：数据处理单元将融合的绝对位姿信息发送到后车的数据处理单元，并接收来自头车和前车的绝对位姿信息，若是头车则向所有车辆广播自车绝对位姿信息；

S6：数据处理单元接收头车和前车发送过来的绝对位姿数据，并存储数据，根据自车绝对位姿信息、前车绝对位姿和头车历史路径，更新自车绝对路径表，计算得到车辆目标速度和方向转角，并传输到车辆底层控制器，实现自主跟车驾驶。