本发明公开了一种基于临时避让停车位的狭窄弯道车辆调度方法，包括：将激光雷达和毫米波雷达获取得到的车辆数据进行融合，以实现相会车辆的车辆状态及行驶轨迹预测；通过车辆行驶轨迹的预测计算车辆的预计会车点；基于预计会车点并根据道路情况和临时避让停车位的数量执行不同的车辆避让策略，以实现狭窄弯道会车的车辆避让与调度。本发明将融合处理后的车辆状态数据应用于狭窄弯道避让策略中，实现了基于临时避让停车位的狭窄弯道车辆调度方法。一方面，降低了连续弯道的交通事故发生频率，保证了车上人员的生命财产安全；另一方面，本发明具有成本低、自动化高的特点，推动车路协同向智能化统一管理的方向发展。
1.一种基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，包括：将激光雷达和毫米波雷达获取得到的车辆数据进行融合，以实现相会车辆的车辆状态及行驶轨迹预测；
基于预计会车点并根据道路情况和临时避让停车位的数量执行不同的车辆避让策略，以实现狭窄弯道会车的车辆避让与调度；
所述将激光雷达和毫米波雷达获取得到的车辆数据进行融合，以实现相会车辆的车辆状态及行驶轨迹预测包括：将激光雷达获取的车辆数据使用卡尔曼滤波进行车辆状态预测；
将毫秒波雷达获取的车辆数据使用扩展卡尔曼滤波进行车辆状态的测量更新，使用非线性测量函数 来变化坐标系，设置测量噪声协方差矩阵 ，并计算新的雅可比矩阵 ，以实现测量更新；
在预测阶段，接收到激光雷达采集到的车辆数据 后，通过状态预测公式，计算得到车辆数据的预测值 ；
在更新阶段，滤波器将车辆数据的预测值 与测量值进行比较，并进行数据融合，得到更新的车辆数据，即最优估计值 ；
在 时间后，再次收到激光雷达和毫秒波雷达的测量值，将开始执行下一轮的预测和更新；
其中，是预测下一个状态值的状态转移矩阵， 是上一时刻车辆综合预测和测量得到的最优估计值，是控制矩阵，是控制变量，为噪声项；
设置 ，并将加速度的影响加入到噪声项 中，并用过程噪声矢量 表示；
2.如权利要求1所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，在所述更新阶段中，最优估计值 的计算方式为：其中，是卡尔曼增益，是测量残差矢量。
3.如权利要求1所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，所述车辆避让策略包括无车道线情况下的车辆避让策略，所述无车道线情况下的车辆避让策略包括：判断两辆相遇车辆之间的临时避让停车位数量，如果临时避让停车位数量为1，则执行避让策略Ⅰ，所述避让策略Ⅰ包括：将临时避让停车位前后若干米范围内的道路区域记为期待会车域 ；
如果预计会车点 在期待会车域 内，则进一步判断路面是否为
起伏路面；如果是起伏路面，则由下坡车辆作为避让车辆，上坡车辆作为被避让车辆，避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位；如果不是起伏路面，则由靠近临时避让停车位 一侧的车辆作为避让车辆，另一侧车辆作为被避让车辆，避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
4.如权利要求3所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，所述避让策略Ⅰ还包括：如果预计会车点 在期待会车域 的左侧，则由右侧车辆作为避
让车辆，左侧车辆作为被避让车辆；避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
5.如权利要求3或4所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，所述避让策略Ⅰ还包括：如果预计会车点 在期待会车域 的右侧，则由左侧车辆作为避
让车辆，右侧车辆作为被避让车辆，避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
6.如权利要求3所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，所述无车道线情况下的车辆避让策略还包括：如果临时避让停车位数量为2，则执行避让策略Ⅱ，所述避让策略Ⅱ包括：如果预计会车点 在期待会车域 内，则选择临时避让停车位
作为此次会车避让停车位 ，判断期待会车域 及周围路面是否为起伏路面；
如果预计会车点 在期待会车域 内，则选择临时避让停车位
作为此次会车避让停车位 ，判断期待会车域 及周围路面是否为起伏路面；
如果预计会车点 在期待会车域 的左侧，则选择临时避让停车
位 作为此次会车避让停车位 ，由右侧车辆作为避让车辆，左侧车辆作为被避让车辆，避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
7.如权利要求6所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，所述避让策略Ⅱ还包括：如果预计会车点 在期待会车域 和 的中间，则进
根据相遇车辆的车速，计算得到相遇车辆A车和B车到达临时避让停车位 和 所需要的时间，分别记为 、 、 和 ；
方案①：选择临时避让停车位 作为此次会车避让停车位 ，由左侧车辆作为避让车辆，右侧车辆作为被避让车辆，避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
此时避让时间 指避让车辆进入此次会车避让停车位 后等待被避让车辆通过此次会车避让停车位 所花费的时间：；
方案②：选择临时避让停车位 作为此次会车避让停车位 ，由右侧车辆作为避让车辆，左侧车辆作为被避让车辆，避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
此时避让时间 指避让车辆进入此次会车避让停车位 后等待被避让车辆通过此次会车避让停车位 所花费的时间：如果停车位在车辆的左侧，将驾驶员因停车位位置而产生的额外停车时间记为 ：比较两个方案的避让时间 ，选择避让时间较小的方案作为此次会车的避让策略。
8.如权利要求6或7所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，当判断期待会车域 或 及周围路面是起伏路面时，则由下坡车
辆作为避让车辆，上坡车辆作为被避让车辆，避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
当判断期待会车域 或 及周围路面不是起伏路面时，则由靠近
临时避让停车位 一侧的车辆作为避让车辆，另一侧车辆作为被避让车辆，避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
9.如权利要求1所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，所述车辆避让策略包括有车道线情况下的车辆避让策略，所述有车道线情况下的车辆避让策略包括：判断两辆相遇车辆之间的临时避让停车位数量，如果临时避让停车位数量为1，则执行避让策略Ⅲ，所述避让策略Ⅲ包括：无论预计会车点 在何处，均由靠近临时避让停车位 的车辆作为避让车辆，另一侧车辆作为被避让车辆，避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
10.如权利要求9所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，如果临时避让停车位数量为2，则判断临时避让停车位是否位于道路同侧；
如果位于道路同侧，则执行避让策略Ⅳ，所述避让策略Ⅳ包括：
左侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆，避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 。
11.如权利要求10所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，所述避让策略Ⅳ还包括：如果预计会车点 在期待会车域 右侧，则选择临时避让停车位
作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆，避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 。
12.如权利要求10或11所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，如果临时避让停车位数量为2，且临时避让停车位位于道路异侧，则执行避让策略Ⅴ，所述避让策略Ⅴ包括：如果预计会车点 在期待会车域 内或在期待会车域
左侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆；避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 ；
右侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆；避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 。
13.如权利要求12所述的基于临时避让停车位的狭窄弯道车辆调度方法，其特征在于，所述避让策略Ⅴ还包括：如果预计会车点 在期待会车域 和期待会车域 之
根据相遇车辆的车速，计算得到相遇车辆A车和B车到达临时避让停车位 和 所需要的时间，分别记为 、 、 和 ；
方案①：选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆；避让车辆进入临时避让停车位，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
如果此次会车避让停车位 位于避让车辆的左侧，则避让时间 指被避让车辆等待避让车辆进入此次会车避让停车位 所花费的时间：如果此次会车避让停车位 位于避让车辆的右侧，则避让时间 指避让车辆进入此次会车避让停车位 后等待被避让车辆通过此次会车避让停车位 所花费的时间：方案②：选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆；避让车辆进入临时避让停车位，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
如果此次会车避让停车位 位于避让车辆的左侧，则避让时间 指被避让车辆等待避让车辆进入此次会车避让停车位 所花费的时间；
如果此次会车避让停车位 位于避让车辆的右侧，则避让时间 指避让车辆进入此次会车避让停车位 后等待被避让车辆通过此次会车避让停车位 所花费的时间；
如果停车位在车辆的左侧，将驾驶员因停车位位置而增加的停车时间记为 ；
针对两个方案中的两类情况，比较避让时间 ，选择避让时间较小的方案作为此次会车的避让策略。
一种基于临时避让停车位的狭窄弯道车辆调度方法
技术领域
[0001] 本发明涉及车辆控制技术领域，具体涉及基于临时避让停车位的狭窄弯道车辆调度方法。
背景技术
[0002] 我国地域辽阔，地形地貌复杂多样，山区面积广阔，随着国家经济的高速发展，为了满足山区居民的日常出行以及旅游业发展的需求，山区公路日益增加，但是山区道路弯道多、道路窄等问题使得交通事故频发。虽然国家通过加宽弯道路面、设置临时避让点等方法来保证山区道路的行车安全，但是狭窄弯道的车辆调度问题仍然值得关注。在行车过程中，驾驶员经常因为两车的会车位置过于狭窄而无法进行避让，这种情况下极易发生堵车，甚至在会车时发生交通事故。
[0003] 中国专利202110667345.2公开了一种便道行车调度方法，包括：获取便道车辆行驶信息、道路通行状态及会车位置占用情况并构成状态数据；通过分析处理状态数据得到全线行车情况及会车位置的空闲情况，进而实现车辆调度。
[0004] 中国专利201810188301.X中公开了一种车辆避让控制系统及控制方法，包括：通过获取用户输入的目的地信息规划行车路径，并采集请求避让车辆的紧急状态等级和行车路径上的被请求避让车辆；通过发送装置向行车路径前一定范围内的被请求避让车辆发送避让请求，进而构建一个紧急通道实现请求避让车辆的快速通过。
[0005] 中国专利202110667345.2中的状态数据包含便道车辆行驶信息、道路通行状态及会车位置占用情况，在分析处理状态数据时，没有考虑到山区道路坡度、风向、天气等外界的影响因素。该专利中根据理论会车点和计划会车点制定调度计划，没有考虑到车道是否存在车道线，以及临时避让点的位置对会车车辆的影响。因此，该专利公开的便道行车调度方法仍然存在由坡度、避让点位置等因素导致会车车辆无法顺利避让的问题。
[0006] 中国专利201810188301.X中在城市道路中，通过规划路径并采集车辆数据，确定并通知被请求避让车辆，从而实现请求避让车辆的快速通过。虽然可以实现车辆的顺利避让，但是没有考虑到山区低等级道路无法通过换道等方式进行车辆避让，同时，山区的坡度等道路情况也会很大程度地影响车辆的避让策略。因此，该专利只关注了规整的城市道路的车辆避让问题，仍然没有解决狭窄弯道相遇车辆如何顺利避让的问题。
发明内容
[0007] 本发明的目的在于克服上述现有技术的不足，提供一种基于临时避让停车位的狭窄弯道车辆调度方法，以实现狭窄弯道会车的车辆避让与调度。
[0008] 为实现上述目的，本发明的技术方案是：
[0009] 一种基于临时避让停车位的狭窄弯道车辆调度方法，包括：
[0010] 将激光雷达和毫米波雷达获取得到的车辆数据进行融合，以实现相会车辆的车辆状态及行驶轨迹预测；
[0011] 通过车辆行驶轨迹的预测计算车辆的预计会车点；
[0012] 基于预计会车点并根据道路情况和临时避让停车位的数量执行不同的车辆避让策略，以实现狭窄弯道会车的车辆避让与调度。
[0013] 进一步地，所述将激光雷达和毫米波雷达获取得到的车辆数据进行融合，以实现相会车辆的车辆状态及行驶轨迹预测包括：
[0014] 将激光雷达获取的车辆数据使用卡尔曼滤波进行车辆状态预测；
[0015] 将毫秒波雷达获取的车辆数据使用扩展卡尔曼滤波进行车辆状态的测量更新，使用非线性测量函数 来变化坐标系，设置测量噪声协方差矩阵 ，并计算新的雅可比矩阵，以实现测量更新；
[0016] 在预测阶段，接收到激光雷达采集到的车辆数据 后，通过状态预测公式，计算得到车辆数据的预测值 ；
[0017] 在更新阶段，滤波器将预测的车辆数据 与传感器的测量值 进行比较，并进行数据融合，得到更新的车辆数据，即最优估计值 ；
[0018] 在 时间后，再次收到雷达的测量值，将开始执行下一轮的预测和更新。
[0019] 进一步地，所状态预测公式为：
[0020]
[0021] 其中，是预测下一个状态值的状态转移矩阵， 是上一时刻车辆综合预测和测量得到的最优估计值，是控制矩阵，是控制变量， 为噪声项；
[0022] 设置 ，并将加速度的影响加入到噪声项 中，并用过程噪声矢量 表示；
[0023] 即： 。
[0024] 进一步地，在所述更新阶段中，最优估计值 的计算方式为：
[0025] ；
[0026] 其中，是卡尔曼增益，是测量残差矢量。
[0027] 进一步地，所述车辆避让策略包括无车道线情况下的车辆避让策略和有车道线情况下的车辆避让策略。
[0028] 进一步地，所述无车道线情况下的车辆避让策略包括：
[0029] 判断两辆相遇车辆之间的临时避让停车位数量，如果临时避让停车位数量为1，则执行避让策略Ⅰ，所述避让策略Ⅰ包括：
[0030] 将临时避让停车位前后若干米范围内的道路区域记为期待会车域 。
[0031] 如果预计会车点 在期待会车域 内，则进一步判断路面是否为起伏路面；如果是起伏路面，则由下坡车辆作为避让车辆，上坡车辆作为被避让车辆。
避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；如果不是起伏路面，则由靠近临时避让停车位 一侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
[0032] 如果预计会车点 在期待会车域 的左侧，则由右侧车辆作为避让车辆，左侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
[0033] 如果预计会车点 在期待会车域 的右侧，则由左侧车辆作为避让车辆，右侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0034] 进一步地，如果临时避让停车位数量为2，则执行避让策略Ⅱ，所述避让策略Ⅱ包括：
[0035] 如果预计会车点 在期待会车域 内，则选择临时避让停车位 作为此次会车避让停车位 ，判断期待会车域 及周围路面是否为起伏路面；
[0036] 如果预计会车点 在期待会车域 内，则选择临时避让停车位 作为此次会车避让停车位 ，判断期待会车域 及周围路面是否为起伏路面；
[0037] 如果预计会车点 在期待会车域 的左侧，则选择临时避让停车位 作为此次会车避让停车位 ，由右侧车辆作为避让车辆，左侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
[0038] 如果预计会车点 在期待会车域 和 的中间，则进一步计算避让时间 ：
[0039] 根据相遇车辆的车速，计算得到相遇车辆A车和B车到达临时避让停车位 和所需要的时间，分别记为 、 、 和 ；
[0040] 计算以下两种避让方案所需要的避让时间：
[0041] 方案①：选择临时避让停车位 作为此次会车避让停车位 ，由左侧车辆作为避让车辆，右侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
[0042] 此时避让时间 指避让车辆进入此次会车避让停车位 后等待被避让车辆通过此次会车避让停车位 所花费的时间：
[0043] ；
[0044] 方案②：选择临时避让停车位 作为此次会车避让停车位 ，由右侧车辆作为避让车辆，左侧车辆作为被避让车辆；避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
[0045] 此时避让时间 指避让车辆进入此次会车避让停车位 后等待被避让车辆通过此次会车避让停车位 所花费的时间。
[0046] ；
[0047] 如果停车位在车辆的左侧，则驾驶员需要花费更多的时间才能进入停车位，因此，本发明中将驾驶员因停车位位置而产生的额外停车时间记为 ：
[0048] ；
[0049] 其中， ；
[0050] 比较两个方案的避让时间 ，选择避让时间较小的方案作为此次会车的避让策略。
[0051] 进一步地，当判断期待会车域 或 及周围路面是起伏路面时，则由下坡车辆作为避让车辆，上坡车辆作为被避让车辆。避让车辆进入临时避让停车位，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
[0052] 当判断期待会车域 或 及周围路面不是起伏路面时，则由靠近临时避让停车位 一侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0053] 进一步地，所述有车道线情况下的车辆避让策略包括：
[0054] 判断两辆相遇车辆之间的临时避让停车位数量，如果临时避让停车位数量为1，则执行执行避让策略Ⅲ，所述避让策略Ⅲ包括：
[0055] 无论预计会车点 在何处，均由靠近临时避让停车位 的车辆作为避让车辆，另一侧车辆作为被避让车辆，避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0056] 进一步地，如果临时避让停车位数量为2，则判断临时避让停车位是否位于道路同侧；
[0057] 如果位于道路同侧，则执行避让策略Ⅳ，所述执行避让策略Ⅳ包括：
[0058] 如果预计会车点 在期待会车域 内或在期待会车域左侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆，避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 ；
[0059] 如果预计会车点 在期待会车域 右侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆，避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 ；
[0060] 如果位于道路异侧，则执行避让策略Ⅴ，所述避让策略Ⅴ包括：
[0061] 如果预计会车点 在期待会车域 内或在期待会车域左侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆；避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 ；
[0062] 如果预计会车点 在期待会车域 内或在期待会车域右侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 ；
[0063] 如果预计会车点 在期待会车域 和期待会车域之间，则计算避让时间 ，包括：
[0064] 根据相遇车辆的车速，计算得到相遇车辆A车和B车到达临时避让停车位 和所需要的时间，分别记为 、 、 和 ；
[0065] 计算以下两种避让方案所需要的避让时间。
[0066] 方案①：选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆；避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
[0067] 如果此次会车避让停车位 位于避让车辆的左侧，则避让时间 指被避让车辆等待避让车辆进入此次会车避让停车位 所花费的时间：
[0068]
[0069] 如果此次会车避让停车位 位于避让车辆的右侧，则避让时间 指避让车辆进入此次会车避让停车位 后等待被避让车辆通过此次会车避让停车位 所花费的时间：
[0070]
[0071] 方案②：选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆；避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 ；
[0072] 如果此次会车避让停车位 位于避让车辆的左侧，则避让时间 指被避让车辆等待避让车辆进入此次会车避让停车位 所花费的时间；
[0073]
[0074] 如果此次会车避让停车位 位于避让车辆的右侧，则避让时间 指避让车辆进入此次会车避让停车位 后等待被避让车辆通过此次会车避让停车位 所花费的时间。
[0075]
[0076] 如果停车位在车辆的左侧，将驾驶员因停车位位置而增加的停车时间记为 ；
[0077]
[0078] 其中， ；
[0079] 针对两个方案中两类情况，比较避让时间 ，选择避让时间较小的方案作为此次会车的避让策略。
[0080] 本发明与现有技术相比，其有益效果在于：
[0081] 本发明将融合处理后的车辆状态数据应用于狭窄弯道避让策略中，实现了基于临时避让停车位的狭窄弯道车辆调度方法。一方面，降低了连续弯道的交通事故发生频率，保证了车上人员的生命财产安全；另一方面，本发明具有成本低、自动化高的特点，推动车路协同向智能化统一管理的方向发展。
附图说明
[0082] 图1为基于多传感器数据融合的车辆行驶状态预测流程图；
[0083] 图2为无车道线单停车位情况下车辆避让示意图；
[0084] 图3为无车道线同侧双停车位情况下车辆避让示意图；
[0085] 图4为无车道线异侧双停车位情况下车辆避让示意图；
[0086] 图5为有车道线单停车位情况下车辆避让示意图；
[0087] 图6为有车道线单停车位情况下车辆避让示意图；
[0088] 图7为第一种有车道线异侧双停车位情况下车辆避让示意图；
[0089] 图8为第二种有车道线异侧双停车位情况下车辆避让示意图。
具体实施方式
[0090] 实施例：
[0091] 下面结合附图和实施例对本发明的技术方案做进一步的说明。
[0092] 本发明提供的基于临时避让停车位的狭窄弯道车辆调度方法主要的技术原理为：首先，将激光雷达和毫米波雷达获取得到的车辆数据进行融合，得到车辆行驶状态最优估计，从而实现相会车辆的车辆状态及轨迹预测。其次，通过车辆行驶轨迹的预测计算车辆的预计会车点。然后，根据道路情况和临时避让停车位的数量执行不同的车辆避让策略，从而实现了狭窄弯道会车的车辆避让与调度，在减少交通事故的同时，保证了山区弯道的通行效率。
[0093] 由此可见，本发明提供的一种基于路边停车位的狭窄弯道车辆调度方法与现有技术相比，具有如下技术优势。
[0094] 第一，本发明方法中使用激光雷达和毫秒波雷达来采集车辆数据，并对采集到的车辆状态数据进行融合，一方面利用了激光雷达分辨率高、抗干扰性强、体积小等优势；另一方面，通过融合毫秒波雷达获取的车辆数据，减少了恶劣天气带来的无法识别等不利影响，进而进行更为准确的车辆状态及轨迹预测。
[0095] 第二，本发明根据道路情况和临时避让停车位的数量执行不同的车辆避让策略，在细化现实道路场景时，根据不同的道路场景制定避让策略，进而实现了狭窄弯道会车的高效车辆避让，既减少了交通事故又保证了交通通行效率。
[0096] 下面详细说明本发明方法的实现过程：
[0097] Step1.车辆行驶状态最优估计，如图1所示，具体包括：
[0098] S11.初始化测量值 。
[0099]
[0100] 其中， 和 分别代表初始阶段用笛卡尔坐标表示的车辆位置， 和 分别代表初始阶段用笛卡尔坐标表示的车辆速度。
[0101] S12.基于卡尔曼滤波的车辆状态预测
[0102] （1）状态预测公式
[0103] 考虑到现实中的车辆线性运动模型：
[0104]
[0105] 因此得到用于预测下一个状态值的状态转移矩阵 ，控制矩阵 和控制变量 ，且[0106] 。
[0107] 进而，可以得到如下的状态预测公式：
[0108]
[0109] 即：
[0110] 其中， 是上一时刻（ 时刻）车辆综合预测和测量得到的最优估计值，该估计值包括四个值 ，均基于笛卡尔坐标。 是基于上一时刻（时刻）最优估计值 下预测的 时刻的车辆状态，考虑到车辆运动可能受外界因素影响，因此还需要加上噪声项 。
[0111] 出于山区道路行车安全的考虑，假设车辆在连续弯道处行驶基本处于匀速状态，即采用匀速模型。因为状态预测公式中的 代表加速度对于位置更新的影响，所以本发明中的状态转移公式设置 ，并将加速度的影响加入到噪声项 中，并用过程噪声矢量 表示。
[0112] 即：
[0113] （2）不确定性在各个时刻的传递公式
[0114] 由于将加速度影响加入到噪声项中，因此 可以表示为如下形式：
[0115]
[0116] 其中， 是两个卡尔曼滤波步骤之间的时间差；因为是匀速模型，所以加速度 和的均值都为0，两者的标准差分别为 和 ；同时， 和 互不相关，即 。
[0117] 过程协方差矩阵 定义如下：
[0118]
[0119] 可以发现，随着时间的推移，对车辆位置和速度的不确定性增大，因此过程协方差矩阵 包含了大量 ，也使得状态协方差矩阵 增加了更多的不确定性。
[0120] 不确定性在各个时刻的传递公式为：
[0121]
[0122] S13. 基于扩展卡尔曼滤波的车辆状态更新
[0123] 虽然激光雷达能提供较为准确的车辆数据，但在恶劣天气下会受到很大的影响。因此，基于毫米波雷达具有穿透雾、烟、灰尘的能力，本发明融合毫米波雷达测量值和激光雷达测量值，进而实现车辆位置的精准预测。
[0124] （1）数据处理
[0125] 首先，确定非线性测量函数 ，用于将基于笛卡尔坐标的预测位置和速度转换为基于极坐标的车辆距离、角度和距离变化率。
[0126]
[0127] 其次，基于扩展卡尔曼滤波，使用一阶泰勒展开对非线性函数 进行近似线性化，并得到其分布为高斯分布， 对于 的导数称为雅可比矩阵 。
[0128]
[0129]
[0130] （2）测量更新
[0131] 1）根据需求确定观测矩阵
[0132] ①测量矩阵 包含位置的测量值 ，即 。
[0133] ②测量残差矢量 表示实际测量值和预测估计值之间的残差。
[0134]
[0135] ③测量噪声协方差矩阵 表示实际测量值与测量值之间的误差，即传感器测量中的不确定性，其参数将由传感器制造商提供。
[0136] 2）状态更新
[0137] ①计算卡尔曼增益
[0138] 测量残差协方差矩阵 表示理论估计值与测量值之间的误差。
[0139]
[0140] 卡尔曼增益 用于衡量预测状态协方差与观测状态协方差之间的大小，进而决定如何赋予权重。
[0141]
[0142] ②计算最优估计值
[0143] ③更新噪声协方差矩阵
[0144]
[0145] 也就是说，Step1首先基于激光雷达获取的车辆数据使用卡尔曼滤波进行车辆状态预测；然后，基于毫秒波雷达获取的车辆数据使用扩展卡尔曼滤波进行车辆状态的测量更新，使用非线性测量函数 来变化坐标系，设置测量噪声协方差矩阵 ，并计算新的雅可比矩阵 ，进而实现测量更新。
[0146] 在预测阶段，接收到激光雷达采集到的车辆数据 后，通过S12中的状态预测公式，计算得到车辆数据的预测值 。
[0147] 在更新阶段，滤波器将预测的车辆数据 与传感器的测量值 进行比较，并根据步骤S13进行数据融合，得到更新的车辆数据，即最优估计值 。其中，卡尔曼滤波器将根据每个值的不确定性（卡尔曼增益），决定将更多的权重放在预测车辆数据或测量车辆数据上。
[0148] 在 时间后，再次收到雷达的测量值，算法将开始执行下一轮的预测和更新。
[0149] Step2.狭窄弯道会车避让
[0150] S21.将雷达测量得到的两相遇车辆的车速分别记为 和 ，同时，通过Step1进行车辆轨迹的预测，计算得到两车的预计会车点，并记为 。
[0151] S22.根据道路情况及预计会车点的位置进行会车避让。
[0152] 根据道路情况判断有无车道线，如果无车道线，则转Step3，否则转Step4。
[0153] Step3.无车道线情况下的车辆避让策略
[0154] S31.判断两辆相遇车辆之间的临时避让停车位数量。
[0155] 如果临时避让停车位数量为1，则转S32；
[0156] 如果临时避让停车位数量为2，则转S33。
[0157] S32.无车道线单停车位——执行避让策略Ⅰ
[0158] 如图2所示，将临时避让停车位前后50m范围内的道路区域记为期待会车域。
[0159] （1）如果预计会车点 在期待会车域 内，则进一步判断路面是否为起伏路面。如果是起伏路面，则由下坡车辆作为避让车辆，上坡车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。如果不是起伏路面，则由靠近临时避让停车位 一侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0160] （2）如果预计会车点 在期待会车域 的左侧，则由右侧车辆作为避让车辆，左侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0161] （3）如果预计会车点 在期待会车域 的右侧，则由左侧车辆作为避让车辆，右侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0162] S33.无车道线双停车位——执行避让策略Ⅱ
[0163] （1）如果预计会车点 在期待会车域 内，则选择临时避让停车位 作为此次会车避让停车位 ，判断期待会车域 及周围路面是否为起伏路面。如果是起伏路面，则由下坡车辆作为避让车辆，上坡车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
如果不是起伏路面，则由靠近临时避让停车位 一侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0164] （2）如果预计会车点 在期待会车域 内，则选择临时避让停车位 作为此次会车避让停车位 ，判断期待会车域 及周围路面是否为起伏路面。如果是起伏路面，则由下坡车辆作为避让车辆，上坡车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
如果不是起伏路面，则由靠近临时避让停车位 一侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0165] （3）如果预计会车点 在期待会车域 的左侧，则选择临时避让停车位 作为此次会车避让停车位 ，由右侧车辆作为避让车辆，左侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0166] （4）如果预计会车点 在期待会车域 的右侧，则选择临时避让停车位 作为此次会车避让停车位 ，由左侧车辆作为避让车辆，右侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0167] （5）如果预计会车点 在期待会车域 和 的中间，则进一步计算避让时间 。
[0168] 首先，通过Step1可以得到相遇车辆的车速，进而可以计算得到相遇车辆A车和B车到达临时避让停车位 和 所需要的时间，分别记为 、 、 和 。
[0169] 其次，计算以下两种避让方案所需要的避让时间：
[0170] 方案①：如图3‑4所示，选择临时避让停车位 作为此次会车避让停车位 ，由左侧车辆作为避让车辆，右侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0171] 此时避让时间 指避让车辆A车进入此次会车避让停车位 后等待被避让车辆B车通过此次会车避让停车位 所花费的时间。
[0172]
[0173] 方案②：如图3‑4所示，选择临时避让停车位 作为此次会车避让停车位 ，由右侧车辆作为避让车辆，左侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0174] 此时避让时间 指避让车辆B车进入此次会车避让停车位 后等待被避让车辆A车通过此次会车避让停车位 所花费的时间。
[0175]
[0176] 如果停车位在车辆的左侧，则驾驶员需要花费更多的时间才能进入停车位，因此，本发明中将驾驶员因停车位位置而产生的额外停车时间记为 。
[0177]
[0178] 其中， 。
[0179] 最后，比较两个方案避让时间 ，选择避让时间较小的方案作为此次会车的避让策略。
[0180] Step4.有车道线情况下的车辆避让策略
[0181] S41.判断两辆相遇车辆之间的临时避让停车位数量。
[0182] 如果临时停车避让位数量为1，则转S42；
[0183] （2）如果临时停车避让位数量为2，则判断临时避让停车位是否位于道路同侧。
[0184] 如果位于道路同侧，则转S43；如果位于道路异侧，则转S44。
[0185] S42.有车道线单停车位——执行避让策略Ⅲ
[0186] 如图5所示，无论预计会车点 在何处，均由靠近临时避让停车位 的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0187] S43.有车道线同侧双停车位——执行避让策略Ⅳ，如图6所示：
[0188] （1）如果预计会车点 在期待会车域 内或在期待会车域左侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 。
[0189] （2）如果预计会车点 在期待会车域 右侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 。
[0190] S44.有车道线异侧双停车位——执行避让策略Ⅴ
[0191] （1）如果预计会车点 在期待会车域 内或在期待会车域左侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 。
[0192] （2）如果预计会车点 在期待会车域 内或在期待会车域右侧，则选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入此次会车避让停车位 ，被避让车辆必须在避让车辆进入此次会车避让停车位 后，才能驶入期待会车域 ，避让车辆必须等待被避让车辆通过后再驶出此次会车避让停车位 。
[0193] （3）如果预计会车点 在期待会车域 和期待会车域之间，则计算避让时间 。
[0194] 首先，通过Step1可以得到相遇车辆的车速，进而可以计算得到相遇车辆A车和B车到达临时避让停车位 和 所需要的时间，分别记为 、 、 和 。
[0195] 其次，计算以下两种避让方案所需要的避让时间。
[0196] 方案①：选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0197] 如图7所示，此次会车避让停车位 位于避让车辆的左侧，则避让时间 指被避让车辆A车等待避让车辆B车进入此次会车避让停车位 所花费的时间。
[0198]
[0199] 如图8所示，此次会车避让停车位 位于避让车辆的右侧，则避让时间 指避让车辆A车进入此次会车避让停车位 后等待被避让车辆B车通过此次会车避让停车位 所花费的时间。
[0200]
[0201] 方案②：选择临时避让停车位 作为此次会车避让停车位 ，位于此次会车避让停车位 同侧的车辆作为避让车辆，另一侧车辆作为被避让车辆。避让车辆进入临时避让停车位 ，被避让车辆必须在避让车辆进入临时避让停车位 后，才能驶入期待会车域，避让车辆必须等待被避让车辆通过后再驶出临时避让停车位 。
[0202] 如图7所示，此次会车避让停车位 位于避让车辆的左侧，则避让时间 指被避让车辆B车等待避让车辆A车进入此次会车避让停车位 所花费的时间。
[0203]
[0204] 如图8所示，此次会车避让停车位 位于避让车辆的右侧，则避让时间 指避让车辆B车进入此次会车避让停车位 后等待被避让车辆A车通过此次会车避让停车位 所花费的时间。
[0205]
[0206] 如果停车位在车辆的左侧，则驾驶员需要花费更多的时间才能进入停车位，因此，本发明中将驾驶员因停车位位置而增加的停车时间记为 。
[0207]
[0208] 其中， 。
[0209] 最后，针对两种方案中的两类情况，比较避让时间 ，选择避让时间较小的方案作为此次会车的避让策略。
[0210] 上述实施例只是为了说明本发明的技术构思及特点，其目的是在于让本领域内的普通技术人员能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡是根据本发明内容的实质所做出的等效的变化或修饰，都应涵盖在本发明的保护范围内。
