CN113721606B - 跟随式自动驾驶物流车辆控制***及方法 - Google Patents

Abstract

本申请公开了跟随式自动驾驶物流车辆控制***及方法，该***由人工驾驶引导车和自动驾驶跟随车组成，采用柔性物理连接和无线连接混合的编队驾驶联网方案，实现近距离编队行驶。采用引导车人工驾驶的模式，跟随车实时模仿引导车驾驶行为，并依据相对轨迹重建进行路径跟随控制，而不完全依赖基于感知***和高精度卫星定位***的自动驾驶和编队驾驶技术，整体方案可靠性优于现有基于卫星定位和纯无线通信的编队驾驶方式，且复杂度大大降低。通过本申请中的技术方案，将高度自动驾驶卡车的运行场景降维为有人监督式跟随自动驾驶场景，实现一种低成本新型引导—跟随式自动驾驶车列***，更加满足物流行业智慧转型需求。

Description

跟随式自动驾驶物流车辆控制***及方法

技术领域

本申请涉及自动驾驶的技术领域，具体而言，涉及跟随式自动驾驶物流车辆控制***及跟随式自动驾驶物流车辆控制方法。

背景技术

随着区域经济的发展以及公路基础设施和车辆的不断改进，中长距离公路运输需求增加，公路货运向快速、长途、重载方向发展。大吨位、重型专用运输车因高速安全、单位运输成本低而成为我国未来公路运输车辆的主力。专用车产品向重型化、专用功能强、技术含量高的方向发展。然而一方面，因资源和能源限制，公路路网里程和货运车辆保有量均不可能等倍数增长；另一方面传统的公路货运物流因为其低效率、劳动强度高、工作环境恶劣，面临驾驶员短缺、成本高、无法适应新需求。现阶段的公路货物运输呈现出堵、缺、危、差的问题，因此公路货物周转量快速增长对于货运物流提出了快、准、安全的新要求。

智能运输***可提高公路交通安全水平，减少交通堵塞，提高公路网的通行能力，降低汽车运输对环境的污染，提高汽车运输生产率和经济效益。随着智能运输***技术的发展，电子技术、信息技术、通信技术和***工程等高科技在公路运输领域将得到广泛应用，物流运输信息管理、运输工具控制技术、运输安全技术等均将产生巨大的飞跃，从而大幅度提高公路网络的通行能力。

然而，现有基于卫星定位和纯无线通信的编队自动驾驶方式依赖于大量高成本传感器，如RTK定位设备、激光雷达、视觉传感器等，且需要复杂的自动驾驶算法加持；

并且，由于物流车辆存在体积大、惯性大、转向难等问题，结合道路交通情况复杂多变，现阶段自动驾驶车辆的感知***与控制***都不能胜任，且存在成本过高的问题，与智能运输***提高汽车运输生产率和经济效益的理念相违背。因此，现阶段实现物流车辆的完全自动驾驶无论是在技术层面、安全性，还是在法规层面都不现实。

发明内容

本申请的目的在于：解决现有基于卫星定位和纯无线通信的物流车辆编队自动驾驶中存在的上述问题中的至少一个。

本申请第一方面的技术方案是：提供了跟随式自动驾驶物流车辆控制***，***适用于物流车辆队列中跟随车的自动驾驶控制，物流车辆队列包括引导车和至少一辆跟随车，***包括：车车相对定位***、车辆状态信息采集***、跟随车规划与控制***，以及跟随车控制执行***；车车相对定位***用于采集车车相对定位信息；车辆状态信息采集***用于采集引导车和跟随车的车辆状态信息；跟随车规划与控制***用于根据车车相对定位信息与车辆状态信息，在跟随车坐标系中，计算引导车的相对位姿，并通过拟合算法，确定跟随车的跟随车参考路径，跟随车规划与控制***还用于根据跟随车参考路径和车辆状态信息，确定跟随车的跟随行驶参数；跟随车控制执行***用于根据跟随行驶参数，控制跟随车进行自动跟随驾驶，其中，跟随行驶参数至少包括跟随车期望踏板开度、跟随车期望转向柱角度。

上述任一项技术方案中，进一步地，跟随车规划与控制***包括引导车相对位姿计算模块，引导车相对位姿计算模块用于计算引导车的相对位姿，引导车的相对位姿的计算过程具体包括：分别根据车辆状态信息中的引导车转向柱角度δ_s1、跟随车转向柱角度δ_s2、引导车纵向车速v_x1、跟随车纵向车速v_x2，计算引导车横摆角速度ω₁与跟随车横摆角速度ω₂；根据引导车横摆角速度ω₁与跟随车横摆角速度ω₂，计算引导车相对位姿估计值，并根据引导车相对位姿估计值

对UWB定位***的引导车相对位姿测量值q_U、车车连接机构的引导车相对位姿q_G进行融合计算，生成引导车的相对位姿q。

上述任一项技术方案中，进一步地，跟随车规划与控制***还包括跟随车参考路径构建模块，跟随车参考路径构建模块用于确定跟随车的跟随车参考路径，具体包括：根据跟随车坐标系的移动与转动量，对上一采样周期的引导车相对路径进行坐标更新和前移；将当前采样周期的引导车相对位姿，作为当前采样周期中的引导车相对路径的初始点；利用拟合算法，对引导车相对路径进行拟合，确定跟随车参考路径。

上述任一项技术方案中，进一步地，跟随车规划与控制***还包括：跟随车纵向控制模块，跟随车横向失稳判定模块，跟随车横向控制模块；跟随车纵向控制模块用于根据跟随车参考路径，构造目标方程x_1RP(a)＝0，并计算目标方程的数值解a_y2，根据数值解a_y2和跟随车参考路径，计算跟随车期望踏板开度；跟随车横向失稳判定模块用于根据数值解a_y2和跟随车参考路径，计算跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，并根据跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，计算轨迹跟随横向失稳程度D_us；跟随车横向控制模块用于根据跟随车横向偏差e_y2和轨迹跟随横向失稳程度D_us，确定跟随车期望转向柱角度

上述任一项技术方案中，进一步地，车车相对定位***中还包括：车车连接机构；引导车与跟随车以及相邻的两辆跟随车之间通过车车连接机构相连，车车连接机构用于有线数据传输。

本申请第二方面的技术方案是：提供了跟随式自动驾驶物流车辆控制方法，方法适用于物流车辆队列中跟随车的自动驾驶控制，物流车辆队列包括引导车和至少一辆跟随车，方法包括：步骤1，根据采集到的车车相对定位信息与车辆状态信息，在跟随车坐标系中，计算引导车的相对位姿；步骤2，将引导车的相对位姿作为当前采样周期的引导车相对路径的初始点，根据上一个采样周期的引导车相对路径和当前采样周期跟随车坐标系的移动与转动量，更新当前采样周期的引导车相对路径，并通过拟合算法，确定跟随车的跟随车参考路径，其中，跟随车坐标系的移动与转动量由跟随车在当前采样周期的跟随车车速、跟随车横摆角速度确定；步骤3，根据跟随车参考路径和车辆状态信息，确定跟随车的跟随行驶参数，跟随行驶参数至少包括跟随车期望踏板开度、跟随车期望转向柱角度。

上述任一项技术方案中，进一步地，引导车与跟随车、以及跟随车与前一辆跟随车之间通过车车连接机构相连，引导车和跟随车上设置有UWB定位***，步骤1中计算引导车的相对位姿q，具体包括：

步骤11，分别根据车辆状态信息中的引导车转向柱角度δ_s1、跟随车转向柱角度δ_s2、引导车纵向车速v_x1、跟随车纵向车速v_x2，计算引导车横摆角速度ω₁与跟随车横摆角速度ω₂；

步骤12，根据引导车横摆角速度ω₁与跟随车横摆角速度ω₂，计算引导车相对位姿估计值，并根据引导车相对位姿估计值

对UWB定位***的引导车相对位姿测量值q_U、车车连接机构的引导车相对位姿q_G进行融合计算，生成引导车的相对位姿q。

上述任一项技术方案中，进一步地，计算跟随车横摆角速度ω₂的方法，具体包括：

根据跟随车转向柱角度δ_s2，换算得到跟随车前轮转角δ_w2，并结合跟随车纵向车速v_x2，计算跟随车横摆角速度计算值ω_{2_KIN}；

根据车辆状态信息中的跟随车横摆角速度采集值ω_{2_IMU}和跟随车横摆角速度计算值ω_{2_KIN}，计算跟随车横摆角速度ω₂，跟随车横摆角速度ω₂的计算公式为：

ω₂＝(1-K_yaw)ω_{2_KIN}+K_yawω_{2_IMU}

式中，K_yaw为横摆角速度融合比例系数，v_low与v_high分别为横摆角速度融合的下限车速与上限车速。

上述任一项技术方案中，进一步地，步骤3中，确定跟随车期望踏板开度的方法，具体包括：

步骤31，根据跟随车参考路径，构造目标方程x_1RP(a)＝0，并计算目标方程的数值解a_y2，根据数值解a_y2和跟随车参考路径，计算引导车与跟随车纵向轨迹距离d_rel，对应的计算公式为：

步骤32，根据引导车与跟随车纵向轨迹距离d_rel，计算跟随车期望踏板开度，对应的计算公式为：

式中，K_ff为前馈比例系数，

为踏板前馈跟随车期望踏板开度，

为运动学反馈跟随车期望踏板开度，K_a纵向加速度反馈系数、K_v车速反馈系数、K_d纵向轨迹距离反馈系数，a_x1为引导车纵向加速度，a_x2为跟随车纵向加速度，v_x1为引导车纵向车速，v_x2为跟随车纵向车速，d_ref为引导车与跟随车的纵向轨迹距离参考值。

上述任一项技术方案中，进一步地，步骤3中，确定跟随车期望转向柱角度的方法，具体包括：

步骤33，根据数值解a_y2和跟随车参考路径，计算跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2；

步骤34，根据跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，计算轨迹跟随横向失稳程度D_us，对应的计算公式为：

D_us＝D_{us_y2}(e_y2)+D_{us_θ2}(e_θ2)

式中，D_{us_y2}(e_y2)与D_{us_θ2}(e_θ2)分别为横向偏差失稳程度与朝向偏差失稳程度，e_{y2_eff}、e_{θ2_eff}分别为跟随车横向生效偏差与跟随车朝向生效偏差；K_ey、K_eθ分别为横向偏差失稳增长系数与朝向偏差失稳增长系数；

步骤35，根据跟随车横向偏差e_y2和轨迹跟随横向失稳程度D_us，确定跟随车期望转向柱角度

对应的计算公式为：

式中，K_s为转向比例系数，

为跟随车期望前轮转角，

为稳定状态跟随车期望前轮转角，失稳状态跟随车期望前轮转角

为横向偏差反馈部分，

为相对路径预瞄部分转角，K_{P_ey}、K_{I_ey}、K_{D_ey}分别为横向偏差反馈的比例系数、积分系数与微分系数。

本申请的有益效果是：

本申请中的技术方案由人工驾驶引导车和自动驾驶跟随车组成，采用柔性物理连接和无线连接混合的编队驾驶联网方案，实现近距离编队行驶。采用引导车人工驾驶的模式，跟随车实时模仿引导车驾驶行为，并依据相对轨迹重建，进行路径跟随控制，而不完全依赖基于感知***和高精度卫星定位***的自动驾驶和编队驾驶技术，整体方案可靠性优于现有基于卫星定位和纯无线通信的编队驾驶方式，且复杂度大大降低。将高度自动驾驶卡车的运行场景，降维为有人监督式跟随自动驾驶场景，实现一种低成本新型引导—跟随式自动驾驶车列***，更加满足物流行业智慧转型需求。其重大意义在于：

1、解决货运物流驾驶员短缺与高成本：引导车人工驾驶，跟随车自动驾驶，减少一半驾驶员，进而降低司机驾驶强度与人力成本。

2、提升公路货运物流转运效率：引导车驾驶员和跟随车安全员交替驾驶，提高驾驶效率；调度***对编队驾驶车辆整体调度，提高整体效率；队列行驶有利于减小风阻和油耗。

3、增强货运驾驶员长途驾驶安全：车辆状态全程实时监控，跟随车自动驾驶***可靠性高于人类驾驶员。

附图说明

本申请的上述和/或附加方面的优点在结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是根据本申请的一个实施例的跟随式自动驾驶物流车辆的示意图；

图2是根据本申请的一个实施例的跟随式自动驾驶物流车辆控制***的示意框图；

图3是根据本申请的一个实施例的UWB定位***采集参数示意图；

图4是根据本申请的一个实施例的车辆运动学模型中横摆角速度关系示意图；

图5是根据本申请的一个实施例的引导车相对位姿示意图；

图6是根据本申请的一个实施例的引导车相对路径重建示意图；

图7是根据本申请的一个实施例的引导车相对路径坐标更新示意图；

图8是根据本申请的一个实施例的跟随车纵向运动控制算法的示意框图；

图9是根据本申请的一个实施例的跟随车横向运动控制算法的示意框图；

图10是根据本申请的一个实施例的跟随车横向运动控制的示意图。

具体实施方式

为了能够更清楚地理解本申请的上述目的、特征和优点，下面结合附图和具体实施方式对本申请进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互结合。

在下面的描述中，阐述了很多具体细节以便于充分理解本申请，但是，本申请还可以采用其他不同于在此描述的其他方式来实施，因此，本申请的保护范围并不受下面公开的具体实施例的限制。

实施例一：

如图1和图2所示，本实施例提供了跟随式自动驾驶物流车辆控制***100，该***100适用于物流车辆队列中跟随车的自动驾驶控制(自动跟随)，物流车辆队列中包括引导车1和至少一辆跟随车2，跟随车2通过车车连接机构33与引导车1或者前一辆跟随车2依次首尾相连，其中，引导车1由人类驾驶员驾驶，跟随车2在控制***控制下进行自动跟随。该***100包括：车车相对定位***3、车辆状态信息采集***4、跟随车规划与控制***5，以及跟随车控制执行***6。

在本实施例中，车车相对定位***3包括UWB定位***31、引导车车头与货箱夹角测量装置32、以及车车连接机构33。车车相对定位***3用于采集车车相对定位信息。

UWB定位***31为超宽带定位***，设置在引导车1货箱尾部上方以及跟随车2车头头部上方，以实现车车间相对定位功能。UWB定位***31采集的车车相对定位信息包括：UWB定位***相对距离d_U、UWB定位***方位角θ_UO和UWB定位***姿态角θ_UA。

需要说明的是，当物流车辆队列中包括多辆跟随车时，第一辆物流车辆为引导车，第二辆至最后一辆物流车为队列中的跟随车，第三辆物流车的跟随控制根据第二辆物流车与第三辆物流车的车车相对定位信息与车辆状态信息确定，此时第二辆物流车相当于第三辆物流车的引导车。

为了便于理解，本实施例中设定仅包括一辆引导车和一辆跟随车。

具体的，如图3所示，设引导车货箱尾部左侧和右侧的UWB定位单元分别为①号和②号，跟随车车头头部的UWB定位单元分别为③号和④号；则UWB定位***相对距离d_U为①号、②号中点与③号、④号中点连线长度，UWB定位***方位角θ_UO为上述①号、②号中点与③号、④号中点连线与③号、④号中垂线夹角，UWB定位***姿态角θ_UA为①号、②号中垂线与③号、④号中垂线夹角。

引导车车头与货箱夹角测量装置32用于采集引导车与跟随车纵轴线水平夹角θ_1P。

引导车1与跟随车2以及相邻的两辆跟随车之间通过车车连接机构33相连，车车连接机构33可以实现车车间有线数据传输，以及车车连接线缆与所连接车辆的纵轴线水平夹角，包括车车通讯连接线(车车连接机构33)与引导车货箱中轴线的水平角度θ_G1、车车连接机构33与跟随车车头中轴线的水平角度θ_G2的测量功能。车车连接机构33采集的信息包括：车车通讯连接线长度d_G、车车通讯连接线与引导车货箱中轴线的水平角度θ_G1及其与跟随车车头中轴线的水平角度θ_G2；

在本实施例中，车辆状态信息采集***4用于采集引导车和跟随车的车辆状态信息，车辆状态信息采集***4包括油门与制动踏板开度传感器41、转向柱转角传感器42、运动学信息组合传感器43。

车车连接机构33采集车车通讯连接线长度d_G、车车通讯连接线与引导车货箱中轴线的水平角度θ_G1及其与跟随车车头中轴线的水平角度θ_G2；

油门与制动踏板开度传感器41采集引导车油门踏板开度p_acc1、引导车制动踏板开度p_brk1、跟随车油门踏板开度p_acc2、跟随车制动踏板开度p_brk2；

转向柱转角传感器42采集引导车转向柱角度δ_s1、跟随车转向柱角度δ_s2；

运动学信息组合传感器43采集引导车纵向加速度a_{x1_IMU}、引导车横摆角速度采集值ω_{1_IMU}、跟随车纵向加速度a_{x2_IMU}、跟随车横摆角速度采集值ω_{2_IMU}；

车辆状态信息采集***4还能够从引导车1和跟随车2CAN总线上分别直接读取的引导车纵向车速v_x1、跟随车纵向车速v_x2。

需要说明的是，本实施例对上述信息的采集方式并不限定。

该***中还包括：跟随车规划与控制***5。跟随车规划与控制***5用于根据车车相对定位信息与车辆状态信息，在跟随车坐标系中，计算引导车的相对位姿，并通过拟合算法，确定跟随车的跟随车参考路径，跟随车规划与控制***5还用于根据跟随车参考路径和车辆状态信息，确定跟随车的跟随行驶参数；

进一步的，跟随车规划与控制***5包括引导车相对位姿计算模块51，引导车相对位姿计算模块51用于计算引导车的相对位姿，引导车的相对位姿的计算过程具体包括：

分别根据车辆状态信息中的引导车转向柱角度δ_s1、跟随车转向柱角度δ_s2、引导车纵向车速v_x1、跟随车纵向车速v_x2，计算引导车横摆角速度ω₁与跟随车横摆角速度ω₂；

需要说明的是，引导车横摆角速度ω₁与跟随车横摆角速度ω₂的计算方式相同，因此，本实施例以跟随车横摆角速度ω₂为例进行说明。

本实施例中，计算跟随车横摆角速度ω₂的方法具体包括：

首先，根据跟随车转向柱角度δ_s2，换算得到跟随车前轮转角δ_w2，并结合跟随车纵向车速v_x2，计算跟随车横摆角速度计算值ω_{2_KIN}。

如图4所示，在二自由度车辆运动学模型中，可以根据跟随车前轮转角δ_w2、跟随车纵向车速v_x2推导得到跟随车横摆角速度计算值ω_{2_KIN}，其公式为：

ω_{2_KIN}＝v_x2tanδ_w2/l₂

式中，l₂为跟随车与引导车之间车头部分的轴距，δ_w2为跟随车前轮转角，v_x2为跟随车纵向车速，其中，跟随车前轮转角δ_w2由跟随车转向柱角度δ_s2经过换算得到，换算关系为：

δ_w2＝K_sδ_s2

式中，K_s为转向比例系数。

其次，根据车辆状态信息中的跟随车横摆角速度采集值ω_{2_IMU}和跟随车横摆角速度计算值ω_{2_KIN}，计算跟随车横摆角速度ω₂，跟随车横摆角速度ω₂的计算公式为：

ω₂＝(1-K_yaw)ω_{2_KIN}+K_yawω_{2_IMU}

式中，K_yaw为横摆角速度融合比例系数，v_low与v_high分别为横摆角速度融合的下限车速与上限车速。

本实施例中，跟随车横摆角速度采集值ω_{2_IMU}可以由传感器如运动组合传感器直接测量得到，数据较为准确，但包含较大噪声；由车辆运动学模型计算得到的跟随车横摆角速度计算值ω_{2_KIN}噪声较小，但受转向传动机构间隙、轮胎侧偏角等影响存在一定误差。所以为了提高跟随车横摆角速度计算的准确性，采用加权算法，对跟随车横摆角速度采集值ω_{2_IMU}、跟随车横摆角速度计算值ω_{2_KIN}进行加权运算。

因此，根据跟随车纵向车速v_x2的取值，确定两者权重的取值，在运动学模型较为准确的车辆低速阶段，加大ω_{2_KIN}的权值，而在传感器噪声影响较小的车辆高速阶段加大ω_{2_IMU}的权值。当v_x2<v_low或v_x2≥v_high时，不进行数据融合，分别直接采用ω_{2_KIN}或ω_{2_IMU}作为最终的横摆角速度ω₂，以保证跟随车横摆角速度的真实性，进而提高跟随车跟随控制的准确性。

根据引导车横摆角速度ω₁与跟随车横摆角速度ω₂，计算引导车相对位姿估计值，并根据引导车相对位姿估计值

对UWB定位***的引导车相对位姿测量值q_U、车车连接机构的引导车相对位姿q_G进行融合计算，生成引导车的相对位姿q；

如图5(a)所示，首先定义引导车坐标系x₁O₁y₁和跟随车坐标系与x₂O₂y₂。引导车坐标系x₁O₁y₁固连于引导车车头，其中坐标原点O₁位于引导车车头后轴中点，x₁轴沿引导车车头纵轴线向前，y₁轴指向引导车车头纵轴线左方；跟随车坐标系x₂O₂y₂固连于跟随车车头，其中坐标原点O₂位于跟随车车头后轴中点，x₂轴沿跟随车车头纵轴线向前，y₂轴指向跟随车车头纵轴线左方。

本实施例中，引导车相对位姿指在跟随车坐标系x₂O₂y₂中，引导车坐标系原点O₁的横纵坐标x_1R、y_1R，以及x₁轴与x₂轴的夹角θ_1R；用向量q表示引导车相对位姿，即q＝(x_1R,y_1R,θ_1R)^T。

引导车相对位姿可描述引导车车头相对于跟随车车头的平面相对位置与相对车头朝向角，可用于后续的引导车相对路径重建，但为了降低物流车辆自动驾驶的实现成本，本实施例中不存在绝对位置传感器，所以，引导车的相对位姿并不能直接获取。

此外，由于引导车由车头与货箱两部分组成，引导车相对位姿也不能通过单一传感器获取，而需要多个传感器的测量值组成测量链计算而来。

为降低多个传感器测量所带来的累积误差，本实施例通过设置两种定位***获取引导车相对位姿测量值，分别是基于UWB***的引导车相对位姿测量值q_U，以及基于车车连接机构的引导车相对位姿测量值q_G，并根据引导车相对位姿的估计值变化量

进行融合计算。

具体的，在采样周期ΔT内，分别计算获得引导车与跟随车在各自坐标系内的位移估计值：

具体公式如下：

计算获得采样周期ΔT内，引导车相对位姿的估计值变化量

具体公式为：

式中，

分别为当前采样周期内引导车横纵坐标x_1R、y_1R以及夹角θ_1R的变化量，x′_1R、y′_1R、θ′_1R分别为上一采样周期内引导车横纵坐标x_1R、y_1R以及夹角θ_1R的取值。

因此，计算获得引导车相对位姿的估计值

具体公式为：

式中，q′＝(x′_1R,y′_1R,θ′_1R)^T为上一采样周期的引导车相对位姿。

根据附图5(b)，基于UWB定位***的引导车相对位姿测量值q_U＝(x_U1R,y_U1R,θ_U1R)^T的计算公式为：

式中，l_2UO为UWB定位***③号、④号中点到跟随车坐标系原点O₂的水平距离；l_1PU为引导车牵引座转轴到UWB定位***①号、②号中点的水平距离；l_1PO为引导车牵引座转轴到引导车坐标系原点O₁的水平距离。

根据附图5(c)，基于车车连接机构的引导车相对位姿q_G＝(x_G1R,y_G1R,θ_G1R)^T的计算公式为：

式中，l_2GO为车车连接机构与跟随车车头连接点到跟随车坐标系原点O₂的水平距离；l_1PG为引导车牵引座转轴到车车连接机构与引导车车尾连接点的水平距离。

在进行融合计算时，引导车的相对位姿q的计算公式为：

q＝K_qq_U+(1-K_q)q_G

式中，K_q为引导车相对位姿融合比例系数，其计算公式为：

K_q＝(K_qx+K_qy+K_qθ)/3

也就是说，本实施例在进行引导车相对位姿的融合计算时，根据UWB定位***的引导车相对位姿测量值q_U、车车连接机构的引导车相对位姿q_G与引导车相对位姿估计值

的偏差，确定这两个值在引导车的相对位姿q中所占的比例，偏差越小，在融合中占比越大，计算出的引导车的相对位姿q越接近真实值。

本实施例中，跟随车规划与控制***5还包括跟随车参考路径构建模块52，跟随车参考路径构建模块52用于确定跟随车的跟随车参考路径，具体包括：根据跟随车坐标系的移动与转动量，对上一采样周期的引导车相对路径进行坐标更新和前移；将当前采样周期的引导车相对位姿，作为当前采样周期中的引导车相对路径的初始点；利用拟合算法，对引导车相对路径进行拟合，确定跟随车参考路径。

具体的，跟随车参考路径的规划建立在引导车相对路径重建的基础上。如图6所示，引导车相对路径{A_i|i＝0,1,2,...,n+1}指在当前跟随车坐标系x₂O₂y₂中，引导车历史相对位姿的集合；由于采样存在时间间隔，实际采集到的引导车相对路径由离散的采样点组成，用A_ii＝0,1,2...表示，A_i＝(x_1Ri,y_1Ri,θ_1Ri)^T含有三个元素，分别是采样时刻引导车坐标系原点O₁在当前跟随车坐标系x₂O₂y₂中横纵坐标x_1Ri、y_1Ri，以及采样时刻x₁轴与当前时刻x₂轴的夹角θ_1Ri。

设采样周期为ΔT，A₀＝(x_1R,y_1R,θ_1R)^T为当前时刻的采样点，等价于当前时刻引导车的相对位姿q；A_i＝(x_1Ri,y_1Ri,θ_1Ri)^T为iΔT即i次采样之前的采样点在当前跟随车坐标系x₂O₂y₂中的位姿。

A_n＝(x_1Rn,y_1Rn,θ_1Rn)^T为nΔT即n次采样之前的采样点在当前跟随车坐标系x₂O₂y₂中的位姿；其中n的确定方式为：A_n为位于y₂轴右侧且距y₂轴最近的采样点，即在x_1Ri>0的采样点中x_1Ri最小的一个。

因此，在确定跟随车的跟随车参考路径过程中，上一采样周期的引导车相对路径为测量值。

如图6(a)所示，若存在位于y₂轴左侧的采样点，即存在x_1Ri<0的采样点，A_n确定为位于y₂轴右侧且距y₂轴最近的采样点，A_n+1为A_n前一个采样点，即位于y₂轴左侧且距y₂轴最近的采样点；如图6(b)所示，若不存在位于y₂轴上或左侧的采样点，即不存在x_1Ri≤0的采样点，A_n确定为最左侧的采样点，此时A_n+1定义为当前跟随车坐标系原点O₂。

每个采样周期ΔT内，完成一次跟随车参考路径规划，引导车相对路径{A_i|i＝0,1,2,...,n+1}的采样点A_i只保留A₀、A₁、……A_n、A_n+1，舍去i>n+1的历史采样点。

如图7所示，本实施例中，跟随车坐标系当前采样周期内的移动与转动量由跟随车在当前采样周期的跟随车车速、跟随车横摆角速度确定，跟随车坐标系当前采样周期内的移动与转动量的计算公式为：

式中，ΔO₂为跟随车坐标系当前采样周期内的移动与转动量，Δx₂、Δy₂、θ₂为跟随车坐标系x₂O₂y₂在当前采样周期内沿x₂轴、y₂轴的移动量以及整体的转动量,₁为引导车横摆角速度，ω₂为跟随车横摆角速度，ΔT为采样周期，图7中F₂为跟随车车头前轴中点O₂F₂长度，即为跟随车与引导车之间车头部分的轴距l₂。

本实施例示出一种跟随车的跟随车参考路径的计算方法，该方法具体包括：

根据跟随车坐标系的移动与转动量，对上一采样周期的引导车相对路径{A′_i|i＝0,1,2,...,n+1}进行坐标更新和前移，具体公式为：

A_i+1＝A′_i-ΔO₂

相当于将上一周期全部采样点平移-ΔO₂，并将采样编号加1。

将当前采样周期的引导车相对位姿q，作为当前采样周期中的引导车相对路径的初始点，即最新的引导车相对路径采样点A₀，A₀＝q；至此获得了当前采样周期内的引导车相对路径{A_i|i＝0,1,2,...,n+1}。

利用拟合算法，如贝塞尔拟合，对引导车相对路径{A_i|i＝0,1,2,...,n+1}进行n+111阶拟合，获取连续的跟随车参考路径P(a)＝[x_1RP(a),y_1RP(a),θ_1RP(a)]^T，其中a∈[0,1]，具体的计算公式为：

A_i+1＝A′_i-ΔO₂,A₀＝q

i＝0,1,...,n,n+1

式中，{A_i|i＝0,1,2,...,n+1}为当前采样周期的引导车相对路径，{A′_i|i＝0,1,2,...,n+1}为上一个采样周期的引导车相对路径，ΔO₂为跟随车坐标系的移动与转动量，q为引导车的相对位姿作，a为路径参数，x_1RP(a)、y_1RP(a)、θ_1RP(a)为跟随车参考路径P(a)在跟随车坐标系x₂O₂y₂中的横纵坐标以及朝向角，它们都是路径参数a的函数。

因此，P(0)＝A₀，为当前采样点，P(n+1)＝A_n+1为所保留最早的采样点；当a从0到1增加时，P(a)从A₀向A_n+1平滑过渡。

上述过程每个采样周期执行一次，获得实时动态更新的跟随车参考路径P(a)＝[x_1RP(a),y_1RP(a),θ_1RP(a)]^T，用于跟随车的纵向与横向控制，以实现物流车辆队列中跟随车的自动跟随驾驶。

进一步的，跟随车规划与控制***5还包括：跟随车纵向控制模块53，跟随车横向失稳判定模块54，跟随车横向控制模块55；

跟随车纵向控制模块53用于根据跟随车参考路径，构造目标方程x_1RP(a)＝0，并计算目标方程的数值解a_y2，根据数值解a_y2和跟随车参考路径，计算跟随车期望踏板开度；

本实施例中跟随车辆自动跟随驾驶过程中的跟随行驶参数至少包括跟随车期望踏板开度、跟随车期望转向柱角度，即跟随车辆自动跟随驾驶过程分为横向控制和纵向控制。

在跟随车纵向控制过程中，对应的输入量至少包括：引导车油门踏板开度p_acc1、引导车制动踏板开度p_brk1、引导车纵向加速度a_{x1_IMU}、引导车纵向车速v_x1、跟随车纵向加速度a_{x2_IMU}、跟随车纵向车速v_x2，以及引导车与跟随车纵向轨迹距离d_rel，对应的输出为跟随车期望踏板开度

其中，

表示控制制动踏板，

表示控制油门踏板，

表示对应踏板期望开度，由跟随车的油门与制动踏板执行机构执行。

具体的，本实施例中定义引导车与跟随车纵向轨迹距离d_rel为跟随车参考路径P(a)在跟随车坐标系x₂O₂y₂中x_1RP(a)≥0部分的长度。

确定跟随车期望踏板开度的方法，具体包括：

根据跟随车参考路径，构造目标方程x_1RP(a)＝0，并计算目标方程的数值解a_y2，即使得P(a_y2)点落在y₂轴上。

之后，根据数值解a_y2和跟随车参考路径，计算引导车与跟随车纵向轨迹距离d_rel，对应的计算公式为：

如图8所示，跟随车期望踏板开度包括两部分，分为踏板前馈部分与运动学状态量反馈部分，这两部分按照一定比例关系合成最终的跟随车期望踏板开度。

根据引导车与跟随车纵向轨迹距离d_rel，计算跟随车期望踏板开度，对应的计算公式为：

式中，K_ff为前馈比例系数，

为踏板前馈跟随车期望踏板开度，

为运动学反馈跟随车期望踏板开度，K_a纵向加速度反馈系数、K_v车速反馈系数、K_d纵向轨迹距离反馈系数，a_{x1_IMU}为引导车纵向加速度，a_{x2_IMU}为跟随车纵向加速度，v_x1为引导车纵向车速，v_x2为跟随车纵向车速，d_ref为引导车与跟随车的纵向轨迹距离参考值。

其中，纵向加速度反馈系数K_a、车速反馈系数K_v、纵向轨迹距离反馈系数K_d的计算公式为：

式中，Δd_rel为纵向轨迹距离跟随误差，d_ref为引导车与跟随车的纵向轨迹距离参考值，D_Δd为纵向轨迹距离跟随偏离系数，r_d为纵向轨迹距离跟随偏差增长系数，v_{x2_low}、v_{x2_high}分别为反馈系数调节车速下限与反馈系数调节车速上限；K_{a_vl}、K_{v_vl}、K_{d_vl}分别为低速纵向加速度反馈系数、低速车速反馈系数、低速纵向轨迹距离反馈系数，K_{a_vh}、K_{v_vh}、K_{d_vh}分别为高速纵向加速度反馈系数、高速车速反馈系数、高速纵向轨迹距离反馈系数。

纵向加速度反馈系数K_a、车速反馈系数K_v、纵向轨迹距离反馈系数K_d的取值受到跟随车车速v_x2与纵向轨迹距离跟随误差Δd_rel的影响，在此基础上，继续引入纵向轨迹距离跟随偏离系数D_Δd，取值范围为D_Δd≥1，用于描述引导车与跟随车的纵向轨迹距离d_rel偏离参考值d_ref的程度，Δd_{rel_eff}为纵向轨迹距离跟随误差触发门限；当|d_rel|超过Δd_{rel_eff}时，D_Δd开始增大；r_d为纵向轨迹距离跟随偏差增长系数，r_d>0且r_d越大，D_Δd随|d_rel|增大的速率越快。

通过上述设置，使得K_a、K_v、K_d随着跟随车车速v_x2变化，以便分别适应低速工况与高速工况并平滑过渡；当纵向轨迹距离跟随误差的绝对值|d_rel|增大时，纵向轨迹距离跟随偏离系数D_Δd增大，使得K_a、K_v减小，K_d等比例增大，从而加强距离反馈，削弱速度与加速度反馈，以便迅速降低纵向轨迹距离跟随误差。

同样的，踏板前馈跟随车期望踏板开度

的计算公式为：

式中，p_acc1为引导车油门踏板开度，p_brk1为引导车制动踏板开度K_pa、K_pb分别为加速踏板前馈系数与制动踏板前馈系数，p_{acc_eff}、p_{brk_eff}分别为加速踏板前馈生效开度与制动踏板前馈生效开度，p_{brk_eff}、p_{brk_sat}分别为加速踏板前馈饱和开度与制动踏板前馈饱和开度。

加速踏板前馈系数K_pa与制动踏板前馈系数K_pb，是对应踏板开度的函数，在一定范围内踏板开度增大时，对应的踏板前馈系数也随之增大，以对引导车急加速、急减速情况作出迅速响应。

在跟随车横向控制过程中，跟随车对跟随车参考路径进行轨迹跟随时，需要时刻判定横向控制是否失稳，以便及时对横向控制进行调整。因此，为了将横向失稳情况进行量化，定义轨迹跟随横向失稳程度D_us，以保证跟随车横向控制，即跟随车期望转向柱角度计算的准确性和可靠性。

跟随车横向失稳判定模块54用于根据数值解a_y2和跟随车参考路径，计算跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，并根据跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，计算轨迹跟随横向失稳程度D_us；

具体的，根据数值解a_y2和跟随车参考路径，计算跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，对应的计算公式为：

式中，数值解a_y2是方程x_1RP(a)＝0的数值解，即当前时刻跟随车参考路径P(a)＝[x_1RP(a),y_1RP(a),θ_1RP(a)]^T与y₂轴交点P(a_y2)的自变量取值。根据该数值解a_y2和跟随车参考路径，可以计算出跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，跟随车横向偏差e_y2指当前跟随车与引导车历史路径的横向距离，即跟随车坐标系x₂O₂y₂中跟随车参考路径P(a)与y₂轴交点的纵坐标；跟随车朝向偏差e_θ2指当前跟随车位姿与引导车在同一横向位置的历史位姿中车头朝向的夹角。

根据跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，计算轨迹跟随横向失稳程度D_us，对应的计算公式为：

D_us＝D_{us_yz}(e_yz)+D_{us_θ2}(e_θz)

式中，D_{us_y2}(e_y2)与D_{us_θ2}(e_θ2)分别为横向偏差失稳程度与朝向偏差失稳程度，e_{y2_eff}、e_{θ2_eff}分别为跟随车横向生效偏差与跟随车朝向生效偏差；K_ey、K_eθ分别为横向偏差失稳增长系数与朝向偏差失稳增长系数；

需要说明的是，当跟随车横向偏差e_y2、跟随车朝向偏差e_θ2在对应的生效偏差e_{y2_eff}、e_{θ2_eff}以内时，轨迹跟随横向失稳程度D_us为0，认为跟随车横向没有发生失稳；当跟随车横向偏差e_y2或跟随车朝向偏差e_θ2超过对应的生效偏差e_{y2_eff}、e_{θ2_}后，轨迹跟随横向失稳程度D_us随之增加，表示跟随车横向失稳程度趋于严重；当轨迹跟随横向失稳程度D_us达到上限值1时，则认为跟随车横向控制已经完全失稳。

跟随车横向控制模块55用于根据跟随车横向偏差e_y2和轨迹跟随横向失稳程度D_us，确定跟随车期望转向柱角度

具体的，如图9所示，跟随车横向控制过程中，输入量至少包括：跟随车横向偏差e_y2、跟随车纵向车速v_x2、跟随车参考路径P(a)以及引导车当前位置A₀(x_1R,y_1R)；其中A₀(x_1R,y_1R)由当前引导车相对位姿q＝(x_1R,y_1R,θ_1R)^T的前两项构成，对应的输出为跟随车期望转向柱角度

由跟随车的电控转向机构执行。

跟随车横向控制分为两部分：稳定控制部分与失稳控制部分，并按照一定比例关系合成跟随车期望前轮转角

其中，随轨迹跟随横向失稳程度D_us的增大，跟随车期望前轮转角

中失稳状态跟随车期望前轮转角

所占比例也随之增大。

特别的，对于稳定状态跟随车期望转向柱角度

而言，其由横向偏差反馈部分

和相对路径预瞄部分转角

两部分叠加而成。

跟随车期望转向柱角度

由跟随车期望前轮转角

比例放大而来，对应的计算公式为：

式中，K_s为转向比例系数，

为跟随车期望前轮转角，

为稳定状态跟随车期望前轮转角，失稳状态跟随车期望前轮转角

为横向偏差反馈部分，

为相对路径预瞄部分转角，K_{P_ey}、K_{I_ey}、K_{D_ey}分别为横向偏差反馈的比例系数、积分系数与微分系数。

失稳状态下，跟随车期望前轮转角

为直接预瞄引导车当前位置A₀(x_1R,y_1R)而来，具体公式为：

如图10所示，相对路径预瞄部分转角

被定义为在当前跟随车参考路径P(a)上选取一点T₂进行预瞄，使跟随车恰好经过T₂点时的前轮转角，相对路径预瞄部分转角

的计算方法，具体包括：

根据跟随车纵向车速v_x2，计算跟随车预瞄距离d_PV(v_x2)，对应的计算公式为：

式中，d_{PV_low}为低速预瞄距离，v_{x2_pV}为预瞄距离增加起始车速，K_PV为预瞄系数；

根据跟随车预瞄距离d_PV(v_x2)和跟随车参考路径，构造预瞄距离方程，计算跟随车在跟随车参考路径上的预瞄点T₂的坐标(x_T2,y_T2)，对应的计算公式为：

式中，a_T2为预瞄距离方程的数值解；

根据预瞄点T₂的纵向坐标y_T2和跟随车预瞄距离d_PV(v_x2)，计算相对路径预瞄部分转角

对应的计算公式为：

式中，l₂为跟随车与引导车之间车头部分的轴距。

本实施例中，跟随车横向控制处于稳定状态时，倾向于在跟随车参考路径P(a)上的较近位置确定预瞄点并实时修正轨迹跟随的横向误差，驱使跟随车沿引导车的历史路径行驶；当跟随车横向控制处于失稳状态时，意味着跟随车参考路径重建出现问题或路径跟随已经比较困难，这时倾向于直接预瞄引导车当前位置，使得跟随车直接跟随引导车而非引导车的历史路径，增强横向控制稳定性，逐渐改出横向控制的失稳状态。

该***100还包括：跟随车控制执行***6，跟随车控制执行***6用于根据跟随行驶参数，控制跟随车进行自动跟随驾驶，其中，跟随行驶参数至少包括跟随车期望踏板开度、跟随车期望转向柱角度。

需要说明的是，本实施例对跟随车控制执行***6的实现方式并不限定。

实施例二：

在上述实施例的基础上，本实施例提供了跟随式自动驾驶物流车辆控制方法，该方法适用于物流车辆队列中跟随车的自动驾驶控制，物流车辆队列包括引导车和至少一辆跟随车，引导车与跟随车、以及跟随车与前一辆跟随车之间通过车车连接机构相连，引导车和跟随车上设置有UWB定位***，方法包括：

步骤1，根据采集到的车车相对定位信息与车辆状态信息，在跟随车坐标系中，计算引导车的相对位姿，其中，车车相对定位信息由车车相对定位***确定，车车相对定位***中包括UWB定位***、引导车车头与货厢夹角测量装置、车车连接机构等；

本实施例对车车相对定位***的实现并不限定。

车车相对定位信息包括：UWB定位***相对距离d_U、UWB定位***方位角θ_UO和UWB定位***姿态角θ_UA、引导车与挂车纵轴线水平夹角θ_1P、车车通讯连接线长度d_G、车车通讯连接线与引导车货箱中轴线的水平角度θ_G1及其与跟随车车头中轴线的水平角度θ_G2。

需要说明的是，当物流车辆队列中包括多辆跟随车时，第一辆物流车辆为引导车，第二辆至最后一辆物流车为队列中的跟随车，第三辆物流车的跟随控制根据第二辆物流车与第三辆物流车的车车相对定位信息与车辆状态信息确定，此时第二辆物流车相当于第三辆物流车的引导车。

为了便于理解，本实施例中设定仅包括一辆引导车和一辆跟随车。

具体的，如图3所示，设引导车货箱尾部左侧和右侧的UWB定位单元分别为①号和②号，跟随车车头头部的UWB定位单元分别为③号和④号；则UWB定位***相对距离d_U为①号、②号中点与③号、④号中点连线长度，UWB定位***方位角θ_UO为上述①号、②号中点与③号、④号中点连线与③号、④号中垂线夹角，UWB定位***姿态角θ_UA为①号、②号中垂线与③号、④号中垂线夹角。

车辆状态信息包括：引导车油门踏板开度p_acc1、引导车制动踏板开度p_brk1、跟随车油门踏板开度p_acc2、跟随车制动踏板开度p_brk2、引导车转向柱角度δ_s1、跟随车转向柱角度δ_s2、引导车纵向加速度a_{x1_IMU}、引导车横摆角速度采集值ω_{1_IMU}、跟随车纵向加速度a_{x2_IMU}、跟随车横摆角速度采集值ω_{2_IMU}、引导车纵向车速v_x1、跟随车纵向车速v_x2。

需要说明的是，本实施例对上述信息的采集方式并不限定。

本实施例示出一种计算引导车的相对位姿q的计算方法，该方法具体包括：

步骤11，分别根据车辆状态信息中的引导车转向柱角度δ_s1、跟随车转向柱角度δ_s2、引导车纵向车速v_x1、跟随车纵向车速v_x2，计算引导车横摆角速度ω₁与跟随车横摆角速度ω₂；

需要说明的是，引导车横摆角速度ω₁与跟随车横摆角速度ω₂的计算方式相同，因此，本实施例以跟随车横摆角速度ω₂为例进行说明。

本实施例中，计算跟随车横摆角速度ω₂的方法具体包括：

首先，根据跟随车转向柱角度δ_s2，换算得到跟随车前轮转角δ_w2，并结合跟随车纵向车速v_x2，计算跟随车横摆角速度计算值ω_{2_KIN}。

如图4所示，在二自由度车辆运动学模型中，可以根据跟随车前轮转角δ_w2、跟随车纵向车速v_x2推导得到跟随车横摆角速度计算值ω_{2_KIN}，其公式为：

ω_{2_KIN}＝v_x2tanδ_w2/l₂

式中，l₂为跟随车与引导车之间车头部分的轴距，δ_w2为跟随车前轮转角，v_x2为跟随车纵向车速，其中，跟随车前轮转角δ_w2由跟随车转向柱角度δ_s2经过换算得到，换算关系为：

δ_w2＝K_sδ_s2

式中，K_s为转向比例系数。

其次，根据车辆状态信息中的跟随车横摆角速度采集值ω_{2_IMU}和跟随车横摆角速度计算值ω_{2_KIN}，计算跟随车横摆角速度ω₂，跟随车横摆角速度ω₂的计算公式为：

ω₂＝(1-K_yaw)ω_{2_KIN}+K_yawω_{2_IMU}

式中，K_yaw为横摆角速度融合比例系数，v_low与v_high分别为横摆角速度融合的下限车速与上限车速。

本实施例中，跟随车横摆角速度采集值ω_{2_IMU}可以由传感器(如运动组合传感器)直接测量得到，数据较为准确，但包含较大噪声；由车辆运动学模型计算得到的跟随车横摆角速度计算值ω_{2_KIN}噪声较小，但受转向传动机构间隙、轮胎侧偏角等影响存在一定误差。所以为了提高跟随车横摆角速度计算的准确性，采用加权算法，对跟随车横摆角速度采集值ω_{2_IMU}、跟随车横摆角速度计算值ω_{2_KIN}进行加权运算。

因此，根据跟随车纵向车速v_x2的取值，确定两者权重的取值，在运动学模型较为准确的车辆低速阶段，加大ω_{2_KIN}的权值，而在传感器噪声影响较小的车辆高速阶段加大ω_{2_IMU}的权值。当v_x2<v_low或v_x2≥v_high时，不进行数据融合，分别直接采用ω_{2_KIN}或ω_{2_IMU}作为最终的横摆角速度ω₂，以保证跟随车横摆角速度的真实性，进而提高跟随车跟随控制的准确性。

步骤12，根据引导车横摆角速度ω₁与跟随车横摆角速度ω₂，计算引导车相对位姿估计值，并根据引导车相对位姿估计值

对UWB定位***的引导车相对位姿测量值q_U、车车连接机构的引导车相对位姿q_G进行融合计算，生成引导车的相对位姿q；

如图5(a)所示，首先定义引导车坐标系x₁O₁y₁和跟随车坐标系与x₂O₂y₂。引导车坐标系x₁O₁y₁固连于引导车车头，其中坐标原点O₁位于引导车车头后轴中点，x₁轴沿引导车车头纵轴线向前，y₁轴指向引导车车头纵轴线左方；跟随车坐标系x₂O₂y₂固连于跟随车车头，其中坐标原点O₂位于跟随车车头后轴中点，x₂轴沿跟随车车头纵轴线向前，y₂轴指向跟随车车头纵轴线左方。

本实施例中，引导车相对位姿指在跟随车坐标系x₂O₂y₂中，引导车坐标系原点O₁的横纵坐标x_1R、y_1R，以及x₁轴与x₂轴的夹角θ_1R；用向量q表示引导车相对位姿，即q＝(x_1R,y_1R,θ_1R)^T。

引导车相对位姿可描述引导车车头相对于跟随车车头的平面相对位置与相对车头朝向角，可用于后续的引导车相对路径重建，但为了降低物流车辆自动驾驶的实现成本，本实施例中不存在绝对位置传感器，所以，引导车的相对位姿并不能直接获取。

此外，由于引导车由车头与货箱两部分组成，引导车相对位姿也不能通过单一传感器获取，而需要多个传感器的测量值组成测量链计算而来。

为降低多个传感器测量所带来的累积误差，本实施例通过设置两种定位***获取引导车相对位姿测量值，分别是基于UWB***的引导车相对位姿测量值q_U，以及基于车车连接机构的引导车相对位姿测量值q_G，并根据引导车相对位姿的估计值变化量

进行融合计算。

具体的，在采样周期ΔT内，分别计算获得引导车与跟随车在各自坐标系内的位移估计值：

具体公式如下：

计算获得采样周期ΔT内，引导车相对位姿的估计值变化量

具体公式为：

式中，

分别为当前采样周期内引导车横纵坐标x_1R、y_1R以及夹角θ_1R的变化量，x′_1R、y′_1R、θ′_1R分别为上一采样周期内引导车横纵坐标x_1R、y_1R以及夹角θ_1R的取值。

因此，计算获得引导车相对位姿的估计值

具体公式为：

式中，q′＝(x′_1R,y′_1R,θ′1_R)^T为上一采样周期的引导车相对位姿。

根据附图5(b)，基于UWB定位***的引导车相对位姿测量值q_U＝(x_U1R,y_U1R,θ_U1R)^T的计算公式为：

式中，l_2UO为UWB定位***③号、④号中点到跟随车坐标系原点O₂的水平距离；l_1PU为引导车牵引座转轴到UWB定位***①号、②号中点的水平距离；l_1PO为引导车牵引座转轴到引导车坐标系原点O₁的水平距离。

根据附图5(c)，基于车车连接机构的引导车相对位姿q_G＝(x_G1R,y_G1R,θ_G1R)^T的计算公式为：

式中，l_2GO为车车连接机构与跟随车车头连接点到跟随车坐标系原点O₂的水平距离；l_1PG为引导车牵引座转轴到车车连接机构与引导车车尾连接点的水平距离。

在进行融合计算时，引导车的相对位姿q的计算公式为：

q＝K_qq_u+(1-K_q)q_G

式中，K_q为引导车相对位姿融合比例系数，其计算公式为：

k_g＝(K_qx+K_qy+K_qθ)/3

也就是说，本实施例在进行引导车相对位姿的融合计算时，根据UWB定位***的引导车相对位姿测量值q_U、车车连接机构的引导车相对位姿q_G与引导车相对位姿估计值

的偏差，确定这两个值在引导车的相对位姿q中所占的比例，偏差越小，在融合中占比越大，计算出的引导车的相对位姿q越接近真实值。

步骤2，将引导车的相对位姿作为当前采样周期的引导车相对路径的初始点，根据上一个采样周期的引导车相对路径和当前采样周期跟随车坐标系的移动与转动量，更新当前采样周期的引导车相对路径，并通过拟合算法，确定跟随车的跟随车参考路径，

本实施例中，跟随车参考路径的规划建立在引导车相对路径重建的基础上。如图6所示，引导车相对路径{A_i|i＝0,1,2,...,n+1}指在当前跟随车坐标系x₂O₂y₂中，引导车历史相对位姿的集合；由于采样存在时间间隔，实际采集到的引导车相对路径由离散的采样点组成，用A_i(i＝0,1,2...)表示，A_i＝(x_1Ri,y_1Ri,θ_1Ri)^T含有三个元素，分别是采样时刻引导车坐标系原点O₁在当前跟随车坐标系x₂O₂y₂中横纵坐标x_1ri、y_1Ri，以及采样时刻x₁轴与当前时刻x₂轴的夹角θ_1Ri。

设采样周期为ΔT，A₀＝(x_1R,y_1R,θ_1R)^T为当前时刻的采样点，等价于当前时刻引导车的相对位姿q；A_i＝(x_1Ri,y_1Ri,θ_1Ri)^T为iΔT即i次采样之前的采样点在当前跟随车坐标系x₂O₂y₂中的位姿。

A_n＝(x_1Rn,y_1Rn,θ_1Rn)^T为nΔT即n次采样之前的采样点在当前跟随车坐标系x₂O₂y₂中的位姿；其中n的确定方式为：A_n为位于y₂轴右侧且距y₂轴最近的采样点，即在x_1Ri>0的采样点中x_1Ri最小的一个。

因此，在确定跟随车的跟随车参考路径过程中，上一采样周期的引导车相对路径为测量值。

如图6(a)所示，若存在位于y₂轴左侧的采样点，即存在x_1Ri<0的采样点，A_n确定为位于y₂轴右侧且距y₂轴最近的采样点，A_n+1为A_n前一个采样点，即位于y₂轴左侧且距y₂轴最近的采样点；如图6(b)所示，若不存在位于y₂轴上或左侧的采样点，即不存在x_1Ri≤0的采样点，A_n确定为最左侧的采样点，此时A_n+1定义为当前跟随车坐标系原点O₂。

每个采样周期ΔT内，完成一次跟随车参考路径规划，引导车相对路径{A_i|i＝0,1,2,...,n+1}的采样点A_i只保留A₀、A₁、……A_n、A_n+1，舍去i>n+1的历史采样点。

如图7所示，本实施例中，跟随车坐标系当前采样周期内的移动与转动量由跟随车在当前采样周期的跟随车车速、跟随车横摆角速度确定，跟随车坐标系当前采样周期内的移动与转动量的计算公式为：

式中，ΔO₂为跟随车坐标系当前采样周期内的移动与转动量，Δx₂、Δy₂、Δθ₂为跟随车坐标系x₂O₂y₂在当前采样周期内沿x₂轴、y₂轴的移动量以及整体的转动量,₁为引导车横摆角速度，ω₂为跟随车横摆角速度，ΔT为采样周期，图7中F₂为跟随车车头前轴中点O₂F₂长度，即为跟随车与引导车之间车头部分的轴距l₂。

本实施例示出一种跟随车的跟随车参考路径的计算方法，该方法具体包括：

根据跟随车坐标系的移动与转动量，对上一采样周期的引导车相对路径{A′_i|i＝0,1,2,...,n+1}进行坐标更新和前移，具体公式为：

A_i+1＝A′_i-ΔO₂

相当于将上一周期全部采样点平移-ΔO₂，并将采样编号加1。

将当前采样周期的引导车相对位姿q，作为当前采样周期中的引导车相对路径的初始点，即最新的引导车相对路径采样点A₀，A₀＝q；至此获得了当前采样周期内的引导车相对路径{A_i|i＝0,1,2,...,n+1}。

利用拟合算法，如贝塞尔拟合，对引导车相对路径{A_i|i＝0,1,2,...,n+1}进行n+1阶拟合，获取连续的跟随车参考路径P(a)＝[x_1RP(a),y_1RP(a),θ_1RP(a)]^T，其中a∈[0,1]，具体的计算公式为：

A_i+1＝A′_i-ΔO₂,A₀＝

i＝0,1,...,n,n+1

式中，{A_i|i＝0,1,2,...,n+1}为当前采样周期的引导车相对路径，{A′_i|i＝0,1,2,...,n+1}为上一个采样周期的引导车相对路径，ΔO₂为跟随车坐标系的移动与转动量，q为引导车的相对位姿作，a为路径参数，x_1RP(a)、y_1RP(a)、θ_1RP(a)为跟随车参考路径P(a)在跟随车坐标系x₂O₂y₂中的横纵坐标以及朝向角，它们都是路径参数a的函数。

因此，P(0)＝A₀，为当前采样点，P(n+1)＝A_n+1为所保留最早的采样点；当a从0到1增加时，P(a)从A₀向A_n+1平滑过渡。

上述过程每个采样周期执行一次，获得实时动态更新的跟随车参考路径P(a)＝[x_1RP(a),y_1RP(a),θ_1RP(a)]^T，用于跟随车的纵向与横向控制，以实现物流车辆队列中跟随车的自动跟随驾驶。

步骤3，根据跟随车参考路径和车辆状态信息，确定跟随车的跟随行驶参数，跟随行驶参数至少包括跟随车期望踏板开度、跟随车期望转向柱角度。

本实施例中跟随车辆自动跟随驾驶过程中的跟随行驶参数至少包括跟随车期望踏板开度、跟随车期望转向柱角度，即跟随车辆自动跟随驾驶过程分为横向控制和纵向控制。

在跟随车纵向控制过程中，对应的输入量至少包括：引导车油门踏板开度p_acc1、引导车制动踏板开度p_brk1、引导车纵向加速度a_{x1_IMU}、引导车纵向车速v_x1、跟随车纵向加速度a_{x2_IMU}、跟随车纵向车速v_x2，以及引导车与跟随车纵向轨迹距离d_rel，对应的输出为跟随车期望踏板开度

其中，

表示控制制动踏板，

表示控制油门踏板，

表示对应踏板期望开度，由跟随车的油门与制动踏板执行机构执行。

具体的，本实施例中定义引导车与跟随车纵向轨迹距离d_rel为跟随车参考路径P(a)在跟随车坐标系x₂O₂y₂中x_1RP(a)≥0部分的长度。

确定跟随车期望踏板开度的方法，具体包括：

步骤31，根据跟随车参考路径，构造目标方程x_1RP(a)＝0，并计算目标方程的数值解a_y2，即使得P(a_y2)点落在y₂轴上。

之后，根据数值解a_y2和跟随车参考路径，计算引导车与跟随车纵向轨迹距离d_rel，对应的计算公式为：

如图8所示，跟随车期望踏板开度包括两部分，分为踏板前馈部分与运动学状态量反馈部分，这两部分按照一定比例关系合成最终的跟随车期望踏板开度。

步骤32，根据引导车与跟随车纵向轨迹距离d_rel，计算跟随车期望踏板开度，对应的计算公式为：

式中，K_ff为前馈比例系数，

为踏板前馈跟随车期望踏板开度，

为运动学反馈跟随车期望踏板开度，K_a纵向加速度反馈系数、K_v车速反馈系数、K_d纵向轨迹距离反馈系数，a_{x1_}为引导车纵向加速度，a_{x2_IMU}为跟随车纵向加速度，v_x1为引导车纵向车速，v_x2为跟随车纵向车速，d_ref为引导车与跟随车的纵向轨迹距离参考值。

其中，纵向加速度反馈系数K_a、车速反馈系数K_v、纵向轨迹距离反馈系数K_d的计算公式为：

Δd_rel＝d_ref-d_rel

式中，Δd_rel为纵向轨迹距离跟随误差，d_ref为引导车与跟随车的纵向轨迹距离参考值，D_Δd为纵向轨迹距离跟随偏离系数，r_d为纵向轨迹距离跟随偏差增长系数，v_{x2_low}、v_{x2_high}分别为反馈系数调节车速下限与反馈系数调节车速上限；K_{a_vl}、K_{v_vl}、K_{d_vl}分别为低速纵向加速度反馈系数、低速车速反馈系数、低速纵向轨迹距离反馈系数，K_{a_vh}、K_{v_vh}、K_{d_vh}分别为高速纵向加速度反馈系数、高速车速反馈系数、高速纵向轨迹距离反馈系数。

纵向加速度反馈系数K_a、车速反馈系数K_v、纵向轨迹距离反馈系数K_d的取值受到跟随车车速v_x2与纵向轨迹距离跟随误差Δd_rel的影响，在此基础上，继续引入纵向轨迹距离跟随偏离系数D_Δd，取值范围为D_Δd≥1，用于描述引导车与跟随车的纵向轨迹距离d_rel偏离参考值d_ref的程度，Δd_{rel_eff}为纵向轨迹距离跟随误差触发门限；当|Δd_rel|超过Δd_{rel_eff}时，D_Δd开始增大；r_d为纵向轨迹距离跟随偏差增长系数，r_d>0且r_d越大，D_Δd随|Δd_rel|增大的速率越快。

通过上述设置，使得K_a、K_v、K_d随着跟随车车速v_x2变化，以便分别适应低速工况与高速工况并平滑过渡；当纵向轨迹距离跟随误差的绝对值|Δd_rel|增大时，纵向轨迹距离跟随偏离系数D_Δd增大，使得K_a、K_v减小，K_d等比例增大，从而加强距离反馈，削弱速度与加速度反馈，以便迅速降低纵向轨迹距离跟随误差。

同样的，踏板前馈跟随车期望踏板开度

的计算公式为：

式中，p_acc1为引导车油门踏板开度，p_brk1为引导车制动踏板开度K_pa、K_pb分别为加速踏板前馈系数与制动踏板前馈系数，p_{acc_eff}、p_{brk_eff}分别为加速踏板前馈生效开度与制动踏板前馈生效开度，p_{brk_eff}、p_{brk_sat}分别为加速踏板前馈饱和开度与制动踏板前馈饱和开度。

加速踏板前馈系数K_pa与制动踏板前馈系数K_pb，是对应踏板开度的函数，在一定范围内踏板开度增大时，对应的踏板前馈系数也随之增大，以对引导车急加速、急减速情况作出迅速响应。

在跟随车横向控制过程中，跟随车对跟随车参考路径进行轨迹跟随时，需要时刻判定横向控制是否失稳，以便及时对横向控制进行调整。因此，为了将横向失稳情况进行量化，定义轨迹跟随横向失稳程度D_us，以保证跟随车横向控制，即跟随车期望转向柱角度计算的准确性和可靠性。

本实施例示出一种确定跟随车期望转向柱角度的方法，具体包括：

步骤33，根据数值解a_y2和跟随车参考路径，计算跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，对应的计算公式为：

式中，数值解a_y2是方程x_1RP(a)＝0的数值解，即当前时刻跟随车参考路径P(a)＝[x_1RP(a),y_1RP(a),θ_1RP(a)]^T与y₂轴交点P(a_y2)的自变量取值。根据该数值解a_y2和跟随车参考路径，可以计算出跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，跟随车横向偏差e_y2指当前跟随车与引导车历史路径的横向距离，即跟随车坐标系x₂O₂y₂中跟随车参考路径P(a)与y₂轴交点的纵坐标；跟随车朝向偏差e_θ2指当前跟随车位姿与引导车在同一横向位置的历史位姿中车头朝向的夹角。

步骤34，根据跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，计算轨迹跟随横向失稳程度D_us，对应的计算公式为：

D_us＝D_{us_y2}(e_y2)+D_{us_θ2}(e_θ2)

式中，D_{us_y2}(e_y2)与D_{us_θ2}(e_θ2)分别为横向偏差失稳程度与朝向偏差失稳程度，e_{y2_eff}、e_{θ2_eff}分别为跟随车横向生效偏差与跟随车朝向生效偏差；K_ey、K_eθ分别为横向偏差失稳增长系数与朝向偏差失稳增长系数；

具体的，当跟随车横向偏差e_y2、跟随车朝向偏差e_θ2在对应的生效偏差e_{y2_eff}、e_{θ2_eff}以内时，轨迹跟随横向失稳程度D_us为0，认为跟随车横向没有发生失稳；当跟随车横向偏差e_y2或跟随车朝向偏差e_θ2超过对应的生效偏差e_{y2_eff}、e_{θ2_eff}后，轨迹跟随横向失稳程度D_us随之增加，表示跟随车横向失稳程度趋于严重；当轨迹跟随横向失稳程度D_us达到上限值1时，则认为跟随车横向控制已经完全失稳。

步骤35，根据跟随车横向偏差e_y2和轨迹跟随横向失稳程度D_us，确定跟随车期望转向柱角度

具体的，如图9所示，跟随车横向控制过程中，输入量至少包括：跟随车横向偏差e_y2、跟随车纵向车速v_x2、跟随车参考路径P(a)以及引导车当前位置A₀(x_1R,y_1R)；其中A₀(x_1R,y_1R)由当前引导车相对位姿q＝(x_1R,Y_1R,θ_1R)^T的前两项构成，对应的输出为跟随车期望转向柱角度

由跟随车的电控转向机构执行。

跟随车横向控制分为两部分：稳定控制部分与失稳控制部分，并按照一定比例关系合成跟随车期望前轮转角

其中，随轨迹跟随横向失稳程度D_us的增大，跟随车期望前轮转角

中失稳状态跟随车期望前轮转角

所占比例也随之增大。

特别的，对于稳定状态跟随车期望转向柱角度

而言，其由横向偏差反馈部分

和相对路径预瞄部分转角

两部分叠加而成。

跟随车期望转向柱角度

由跟随车期望前轮转角

比例放大而来，对应的计算公式为：

式中，K_s为转向比例系数，

为跟随车期望前轮转角，

为稳定状态跟随车期望前轮转角，失稳状态跟随车期望前轮转角

为横向偏差反馈部分，

为相对路径预瞄部分转角，K_{P_ey}、K_{I_ey}、K_{D_ey}分别为横向偏差反馈的比例系数、积分系数与微分系数。

失稳状态下，跟随车期望前轮转角

为直接预瞄引导车当前位置A₀(x_1R,y_1R)而来，具体公式为：

如图10所示，相对路径预瞄部分转角

被定义为在当前跟随车参考路径P(a)上选取一点T₂进行预瞄，使跟随车恰好经过T₂点时的前轮转角，相对路径预瞄部分转角

的计算方法，具体包括：

根据跟随车纵向车速v_x2，计算跟随车预瞄距离d_PV(v_x2)，对应的计算公式为：

式中，d_{PV_low}为低速预瞄距离，v_{x2_PV}为预瞄距离增加起始车速，K_PV为预瞄系数；

根据跟随车预瞄距离d_PV(v_x2)和跟随车参考路径，构造预瞄距离方程，计算跟随车在跟随车参考路径上的预瞄点T₂的坐标(x_T2,y_T2)，对应的计算公式为：

式中，a_T2为预瞄距离方程的数值解；

根据预瞄点T₂的纵向坐标y_T2和跟随车预瞄距离d_PV(v_x2)，计算相对路径预瞄部分转角

对应的计算公式为：

式中，l₂为跟随车与引导车之间车头部分的轴距。

本实施例中，跟随车横向控制处于稳定状态时，倾向于在跟随车参考路径P(a)上的较近位置确定预瞄点并实时修正轨迹跟随的横向误差，驱使跟随车沿引导车的历史路径行驶；当跟随车横向控制处于失稳状态时，意味着跟随车参考路径重建出现问题或路径跟随已经比较困难，这时倾向于直接预瞄引导车当前位置，使得跟随车直接跟随引导车而非引导车的历史路径，增强横向控制稳定性，逐渐改出横向控制的失稳状态。

以上结合附图详细说明了本申请的技术方案，本申请提出了跟随式自动驾驶物流车辆控制***及方法，***适用于物流车辆队列中跟随车的自动驾驶控制，物流车辆队列包括引导车和至少一辆跟随车，***包括：车车相对定位***、车辆状态信息采集***、跟随车规划与控制***，以及跟随车控制执行***；车车相对定位***用于采集车车相对定位信息；车辆状态信息采集***用于采集引导车和跟随车的车辆状态信息；跟随车规划与控制***用于根据车车相对定位信息与车辆状态信息，在跟随车坐标系中，计算引导车的相对位姿，并通过拟合算法，确定跟随车的跟随车参考路径，跟随车规划与控制***还用于根据跟随车参考路径和车辆状态信息，确定跟随车的跟随行驶参数；跟随车控制执行***用于根据跟随行驶参数，控制跟随车进行自动跟随驾驶，其中，跟随行驶参数至少包括跟随车期望踏板开度、跟随车期望转向柱角度。通过本申请中的技术方案，将高度自动驾驶卡车的运行场景降维为有人监督式跟随自动驾驶场景，实现一种低成本新型引导—跟随式自动驾驶车列***，更加满足物流行业智慧转型需求。

本申请中的步骤可根据实际需求进行顺序调整、合并和删减。

本申请装置中的单元可根据实际需求进行合并、划分和删减。

尽管参考附图详地公开了本申请，但应理解的是，这些描述仅仅是示例性的，并非用来限制本申请的应用。本申请的保护范围由附加权利要求限定，并可包括在不脱离本申请保护范围和精神的情况下针对发明所作的各种变型、改型及等效方案。

Claims (7)

1.跟随式自动驾驶物流车辆控制***，其特征在于，所述***适用于物流车辆队列中跟随车的自动驾驶控制，所述物流车辆队列包括引导车和至少一辆所述跟随车，所述***包括：车车相对定位***(3)、车辆状态信息采集***(4)、跟随车规划与控制***(5)，以及跟随车控制执行***(6)；

所述车车相对定位***(3)用于采集车车相对定位信息；

所述车辆状态信息采集***(4)用于采集所述引导车和所述跟随车的车辆状态信息；

所述跟随车规划与控制***(5)用于根据所述车车相对定位信息与所述车辆状态信息，在跟随车坐标系中，计算所述引导车的相对位姿，并通过拟合算法，确定所述跟随车的跟随车参考路径，所述跟随车规划与控制***(5)还用于根据所述跟随车参考路径和所述车辆状态信息，确定所述跟随车的跟随行驶参数；

所述跟随车控制执行***(6)用于根据所述跟随行驶参数，控制所述跟随车进行自动跟随驾驶，其中，所述跟随行驶参数至少包括跟随车期望踏板开度、跟随车期望转向柱角度；

所述跟随车规划与控制***(5)包括引导车相对位姿计算模块(51)，所述引导车相对位姿计算模块(51)用于计算所述引导车的相对位姿，所述引导车的相对位姿的计算过程具体包括：

分别根据所述车辆状态信息中的引导车转向柱角度δ_s1、跟随车转向柱角度δ_s2、引导车纵向车速v_x1、跟随车纵向车速v_x2，计算引导车横摆角速度ω₁与跟随车横摆角速度ω₂；

根据所述引导车横摆角速度ω₁与所述跟随车横摆角速度ω₂，计算引导车相对位姿估计值，并根据所述引导车相对位姿估计值

对UWB定位***的引导车相对位姿测量值q_U、车车连接机构的引导车相对位姿q_G进行融合计算，生成所述引导车的相对位姿q；

根据跟随车转向柱角度δ_s2，换算得到跟随车前轮转角δ_w2，并结合跟随车纵向车速v_z2，计算跟随车横摆角速度计算值ω_{2_KIN}；

根据跟随车前轮转角δ_w2、跟随车纵向车速v_x2推导得到跟随车横摆角速度计算值ω_{2_KIN}，其公式为：

ω_{2_KIN}＝v_x2tanδ_w2/l₂

式中，l₂为跟随车与引导车之间车头部分的轴距，δ_w2为跟随车前轮转角，v_x2为跟随车纵向车速，其中，跟随车前轮转角δ_w2由跟随车转向柱角度δ_s2经过换算得到，换算关系为：

δ_w2＝K_sδ_s2

式中，K_s为转向比例系数；

根据车辆状态信息中的跟随车横摆角速度采集值ω_{2_IMU}和跟随车横摆角速度计算值ω_{2_KIN}，计算跟随车横摆角速度ω₂，跟随车横摆角速度ω₂的计算公式为：

ω₂＝(1-K_yaw)ω_{2_KIN}+K_yawω_{2_IMU}

式中，K_yaw为横摆角速度融合比例系数，v_low与v_high分别为横摆角速度融合的下限车速与上限车速。

2.如权利要求1所述的跟随式自动驾驶物流车辆控制***，其特征在于，所述跟随车规划与控制***(5)还包括跟随车参考路径构建模块(52)，所述跟随车参考路径构建模块(52)用于确定所述跟随车的跟随车参考路径，具体包括：

根据跟随车坐标系的移动与转动量，对上一采样周期的引导车相对路径进行坐标更新和前移；

将当前采样周期的引导车相对位姿，作为当前采样周期中的引导车相对路径的初始点；

利用拟合算法，对引导车相对路径进行拟合，确定所述跟随车参考路径。

3.如权利要求1所述的跟随式自动驾驶物流车辆控制***，其特征在于，所述跟随车规划与控制***(5)还包括：跟随车纵向控制模块(53)，跟随车横向失稳判定模块(54)，跟随车横向控制模块(55)；

所述跟随车纵向控制模块(53)用于根据所述跟随车参考路径，构造目标方程x_1RP(a)＝0，并计算所述目标方程的数值解a_y2，根据所述数值解a_y2和所述跟随车参考路径，计算所述跟随车期望踏板开度；

所述跟随车横向失稳判定模块(54)用于根据所述数值解a_y2和所述跟随车参考路径，计算跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2，并根据所述跟随车横向偏差e_y2、以及所述跟随车朝向偏差e_θ2，计算轨迹跟随横向失稳程度D_us；

所述跟随车横向控制模块(55)用于根据所述跟随车横向偏差e_y2和所述轨迹跟随横向失稳程度D_us，确定所述跟随车期望转向柱角度

4.如权利要求1至3中任一项所述的跟随式自动驾驶物流车辆控制***，其特征在于，所述车车相对定位***(3)中还包括：车车连接机构(33)；

所述引导车与所述跟随车以及相邻的两辆所述跟随车之间通过所述车车连接机构(33)相连，所述车车连接机构(33)用于有线数据传输。

5.跟随式自动驾驶物流车辆控制方法，其特征在于，所述方法适用于物流车辆队列中跟随车的自动驾驶控制，所述物流车辆队列包括引导车和至少一辆所述跟随车，所述方法包括：

步骤1，根据采集到的车车相对定位信息与车辆状态信息，在跟随车坐标系中，计算所述引导车的相对位姿；

所述引导车与所述跟随车、以及所述跟随车与前一辆跟随车之间通过车车连接机构相连，所述引导车和所述跟随车上设置有UWB定位***，所述步骤1中计算所述引导车的相对位姿q，具体包括：

步骤11，分别根据所述车辆状态信息中的引导车转向柱角度δ_s1、跟随车转向柱角度δ_s2、引导车纵向车速v_x1、跟随车纵向车速v_x2，计算引导车横摆角速度ω₁与跟随车横摆角速度ω₂；

步骤12，根据所述引导车横摆角速度ω₁与所述跟随车横摆角速度ω₂，计算引导车相对位姿估计值，并根据所述引导车相对位姿估计值

对所述UWB定位***的引导车相对位姿测量值q_U、所述车车连接机构的引导车相对位姿q_G进行融合计算，生成所述引导车的相对位姿q；计算所述跟随车横摆角速度ω₂的方法，具体包括：

根据所述跟随车转向柱角度δ_s2，换算得到跟随车前轮转角δ_w2，并结合所述跟随车纵向车速v_x2，计算跟随车横摆角速度计算值ω_{2_KIN}；

根据所述车辆状态信息中的跟随车横摆角速度采集值ω_{2_IMU}和所述跟随车横摆角速度计算值ω_{2_KIN}，计算所述跟随车横摆角速度ω₂，所述跟随车横摆角速度ω₂的计算公式为：

ω₂＝(1-K_yaw)ω_{2_KIN}+K_yawω_{2_IMU}

式中，K_yaw为横摆角速度融合比例系数，v_low与v_high分别为横摆角速度融合的下限车速与上限车速；步骤2，将所述引导车的相对位姿作为当前采样周期的引导车相对路径的初始点，根据上一个采样周期的引导车相对路径和当前采样周期所述跟随车坐标系的移动与转动量，更新所述当前采样周期的引导车相对路径，并通过拟合算法，确定所述跟随车的跟随车参考路径，

其中，所述跟随车坐标系的移动与转动量由所述跟随车在当前采样周期的跟随车车速、跟随车横摆角速度确定；

步骤3，根据所述跟随车参考路径和所述车辆状态信息，确定所述跟随车的跟随行驶参数，所述跟随行驶参数至少包括跟随车期望踏板开度、跟随车期望转向柱角度。

6.如权利要求5所述的跟随式自动驾驶物流车辆控制方法，其特征在于，步骤3中，确定所述跟随车期望踏板开度的方法，具体包括：

步骤31，根据所述跟随车参考路径，构造目标方程x_1RP(a)＝0，并计算所述目标方程的数值解a_y2，根据所述数值解a_y2和所述跟随车参考路径，计算引导车与跟随车纵向轨迹距离d_rel，对应的计算公式为：

步骤32，根据所述引导车与跟随车纵向轨迹距离d_rel，计算所述跟随车期望踏板开度，对应的计算公式为：

式中，K_ff为前馈比例系数，

为踏板前馈跟随车期望踏板开度，

为运动学反馈跟随车期望踏板开度，K_a纵向加速度反馈系数、K_v车速反馈系数、K_d纵向轨迹距离反馈系数，a_x1为引导车纵向加速度，a_x2为跟随车纵向加速度，v_x1为引导车纵向车速，v_x2为跟随车纵向车速，d_ref为引导车与跟随车的纵向轨迹距离参考值。

7.如权利要求6所述的跟随式自动驾驶物流车辆控制方法，其特征在于，所述步骤3中，确定所述跟随车期望转向柱角度的方法，具体包括：

步骤33，根据所述数值解a_y2和所述跟随车参考路径，计算跟随车横向偏差e_y2、以及跟随车朝向偏差e_θ2；

步骤34，根据所述跟随车横向偏差e_y2、以及所述跟随车朝向偏差e_θ2，计算轨迹跟随横向失稳程度D_us，对应的计算公式为：

D_us＝D_{us_y2}(e_y2)+D_{us_θ2}(e_θ2)

式中，D_{us_y2}(e_y2)与D_{us_θ2}(e_θ2)分别为横向偏差失稳程度与朝向偏差失稳程度，e_{y2_eff}、e_{θ2_eff}分别为跟随车横向生效偏差与跟随车朝向生效偏差；K_ey、K_eθ分别为横向偏差失稳增长系数与朝向偏差失稳增长系数；

步骤35，根据所述跟随车横向偏差e_y2和所述轨迹跟随横向失稳程度D_us，确定所述跟随车期望转向柱角度

对应的计算公式为：

式中，K_s为转向比例系数，

为跟随车期望前轮转角，

为稳定状态跟随车期望前轮转角，失稳状态跟随车期望前轮转角

为横向偏差反馈部分，

为相对路径预瞄部分转角，K_{P_ey}、K_{I_ey}、K_{D_ey}分别为横向偏差反馈的比例系数、积分系数与微分系数。

Images