CN113525375B - 一种基于人工势场法的车辆换道方法及装置 - Google Patents

Abstract

本发明属于自动驾驶技术领域，具体涉及一种基于人工势场法的车辆换道方法及装置。该方法首先获取预行驶道路上的各障碍物的信息，所述障碍物的信息包括障碍物的速度和位置；然后在各预行驶道路上沿自车行驶方向的设定检测范围内均设置至少一个检测点，确定各个检测点对应的静态势场和障碍物动态势场，所述静态势场包括道路静态势场和/或障碍物静态势场；接着根据每个预行驶道路的所有检测点对应的静态势场和障碍物动态势场，确定每个预行驶道路的综合势场；最后根据各个预行驶道路的综合势场，进行换道控制。本发明的综合势场能够充分反映行驶环境的连续变化过程，考虑的更加全面，提高了换道决策的合理性和准确性，进而提高了驾驶的安全性。

Description

一种基于人工势场法的车辆换道方法及装置

技术领域

本发明属于自动驾驶技术领域，具体涉及一种基于人工势场法的车辆换道方法及装置。

背景技术

自动驾驶车辆是通过车载传感***感知道路环境后，自动规划行车路线并控制车辆达到预定目的地的智能化车辆。在车辆行驶过程中，需要通过车载装置获得自车信息及外部环境信息，依据交通规则及任务要求，在安全和效率的双重约束下完成换道动作。如若换道不当，极有可能发生交通事故，造成不可预估的损失。

为了进行安全且高效的换道，申请公布号为CN110244713A的中国发明专利申请公开了一种基于人工势场法的智能车辆换道轨迹规划***及方法，当智能车辆(即自车)处于前方障碍车辆的斥力场范围之外时，决策结果为直行，当智能车辆处于前方障碍物车辆的斥力场范围之内时，决策为换道。也即，该方法根据障碍物车辆的速度，计算得到障碍物车辆的动态势场来进行换道判断。但是，影响自车是否换道的因素有很多，除了障碍物动态势场外，还有其他因素，例如预行驶道路自身也存在势场，等等。如若仅根据障碍物动态势场来判断是否进行换道不够准确，换道存在危险。

发明内容

本发明提供了一种基于人工势场法的车辆换道方法及装置，用以解决现有技术仅根据障碍物车辆的势场来判断是否进行换道不够准确的问题。

为解决上述技术问题，本发明的技术方案包括：

本发明提供了一种基于人工势场法的车辆换道方法，包括如下步骤：

1)获取预行驶道路上的各障碍物的信息，所述障碍物的信息包括障碍物的速度和位置；

2)在各预行驶道路上沿自车行驶方向的设定检测范围内均设置至少一个检测点，确定各个检测点对应的静态势场和障碍物动态势场，所述静态势场包括道路静态势场和/或障碍物静态势场；其中：

根据检测点与预行驶道路中心线之间的距离，确定该检测点对应的道路静态势场；

根据检测点与各个障碍物之间的距离，确定该检测点对应的各个障碍物静态势场；

根据障碍物与自车之间的距离、以及各个障碍物的速度，确定各个障碍物动态势场；

3)根据每个预行驶道路的所有检测点对应的静态势场和障碍物动态势场，确定每个预行驶道路的综合势场；

4)根据各个预行驶道路的综合势场，进行换道控制。

上述技术方案的有益效果为：本发明基于人工势场法，考虑障碍物信息和预行驶道路的信息，除了得到障碍物动态势场外，还考虑静态势场，静态势场为道路静态势场、或者障碍物静态势场、或者道路静态势场和障碍物静态势场均考虑在内，从而构建得到每条预行驶道路的综合势场，该综合势场能够充分反映行驶环境的连续变化过程，考虑的更加全面，进而比较每条预行驶道路的综合势场，根据比较结果来判断是否进行换道，或者换道至哪条道路上，提高了换道决策的合理性和准确性，进而提高了换道的安全性以及驾驶的安全性。

作为方法的进一步改进，为了准确确定障碍物动态势场，步骤2)中，所述障碍物动态势场包括障碍物横向动态势场和障碍物纵向动态势场；所述障碍物横向动态势场根据障碍物与自车的横向距离、以及障碍物的横向速度得到；所述障碍物纵向动态势场根据障碍物与自车的纵向距离、以及障碍物的纵向速度得到。

作为方法的进一步改进，步骤2)中，所述检测点对应的道路静态势场为：

式中，u_ci为第i个检测点对应的道路静态势场，ρ_ci为第i个检测点与当前道路中心线之间的距离，U_c为当前道路中心线的势场强度，L_c为当前道路引力场的影响范围宽度，k_c为道路静态势场对应的系数。

作为方法的进一步改进，步骤2)中，所述检测点对应的各个障碍物静态势场为：

式中，u_omi为第i个检测点对应的第m个障碍物的障碍物静态势场，U_sm为第m个障碍物的安全范围内的势场强度，ρ_omi为为第i个检测点与第m个障碍物之间的距离，k_o为障碍物静态势场对应的系数，L_obsm为第m个障碍物的影响范围宽度，L_s为设定的安全距离。

作为方法的进一步改进，障碍物纵向动态势场为：

式中，u_lom为第m个障碍物的障碍物纵向动态势场，sign()为符号函数，v_Ty为设定的纵向速度限值，v_ym为第m个障碍物的纵向速度，k_lo为障碍物纵向动态势场对应的系数，L_lom为第m个障碍物纵向动态势场影响长度，y_minm为第m个障碍物与自车的最近纵向距离，L_s为设定的安全距离，ρ_ym为第m个障碍物与自车之间的纵向距离，y_maxm为第m个障碍物与自车的最远纵向距离。

作为方法的进一步改进，障碍物横向动态势场为：

式中，u_latm为第m个障碍物的障碍物横向动态势场，sign()为符号函数，v_Tx为设定的横向速度限值，v_xm为第m个障碍物的横向速度，k_lat为障碍物横向动态势场对应的系数，L_latm为第m个障碍物的横向动态势场影响长度，x_minm为第m个障碍物与自车的最近横向距离，L_s为设定的安全距离，ρ_xm为第m个障碍物与自车之间的横向距离，x_maxm为第m个障碍物与自车的最远横向距离。

作为方法的进一步改进，步骤4)中，将各个预行驶道路的综合势场与对应的道路系数相乘，得到各个预行驶道路的换道概率，比较各个预行驶道路的换道概率，进行换道控制。

作为方法的进一步改进，为了准确得到预行驶道路的综合势场，所述静态势场包括道路静态势场和障碍物静态势场，则预行驶道路的综合势场为：

式中，U_j为第个预行驶道路的综合势场，u_ci为第i个检测点对应的道路静态势场，u_omi为第i个检测点对应的第m个障碍物的障碍物静态势场，u_lom为第m个障碍物的障碍物纵向动态势场，u_latm为第m个障碍物的障碍物横向动态势场，len为预行驶道路上沿自车行驶方向的设定检测范围内检测点的个数，n_obs为预行驶道路上的障碍物的个数，w_i、w_o、w_lo和w_lat分别为道路静态势场、障碍物静态势场、障碍物纵向动态势场和障碍物横向动态势场对应的系数。

作为方法的进一步改进，步骤4)中，为了提高换道的安全性，进行换道控制时，控制换道至综合势场最小的预行驶道路。

本发明还提供了一种基于人工势场法的车辆换道装置，包括存储器和处理器，所述处理器用于执行存储在存储器中的指令以实现上述介绍的基于人工势场法的辆换道方法，并达到与方法相同的效果。

附图说明

图1是本发明的方法实施例中基于人工势场法的车辆换道方法的流程框图；

图2是本发明的障碍物静态势场和障碍物动态势场的示意图；

图3是本发明的道路静态势场的示意图；

图4是本发明的道路的综合势场的示意图；

图5是本发明的装置实施例中基于人工势场法的车辆换道装置的结构图。

具体实施方式

本发明基于人工势场法，考虑障碍物信息和预行驶道路的信息，得到道路静态势场、障碍物静态势场和障碍物动态势场，障碍物动态势场包括障碍物横向动态势场和障碍物纵向动态势场，从而根据这些势场信息，得到每个预行驶道路的综合势场，根据各预行驶道路的综合势场，进行换道控制，确定是否换道以及如何换道。下面先对道路静态势场、障碍物静态势场和障碍物动态势场进行介绍。

1、道路静态势场

道路静态势场引导车辆沿同向车道行驶，是对道路的引力进行建立。如图3所示，车辆在行驶过程中，同向行驶道路对自车具有一定引力，诱导自车保持当前方向道路行驶，逆向车道对自车具有斥力作用，避免自车逆向行驶，从而可通过对当前道路建立引力场和斥力场以建立道路静态势场。考虑到车辆沿道路行驶时保持车道中心行驶，将当前道路静态势场中心设为道路中心线，势场强度随远离中心线方向消弱，当前道路引力场的影响范围宽度为L_c，当前道路中心线势场强度U_c。

在预行驶道路上沿自车行驶方向的设定检测范围内设置多个检测点后，可根据第i个检测点与当前道路中心线之间的距离，确定第i个检测点对应的道路静态势场u_ci，称下式为公式(1)：

式中，u_ci为第i个检测点对应的道路静态势场，ρ_ci为第i个检测点与预行驶道路中心线之间的距离，k_c为道路静态势场对应的系数，U_c为预行驶道路中心线的势场强度，L_c为当前道路引力场的影响范围宽度。

2、障碍物静态势场

障碍物静态势场是对障碍物周边一定安全距离范围内的斥力进行建立。如图2所示，障碍物在道路行驶时，作为交通参与者，对周边行驶车辆存在一定斥力，需要与周边障碍物保持一定的安全距离，障碍物周边建立一定安全范围内的静态势场，避免换道过程距离障碍物过近。设定的安全距离为L_s，安全范围内采用固定势场U_sm，安全范围外随远离障碍物方向消弱，斥力场影响范围宽度L_obsm。

在预行驶道路上沿自车行驶方向的设定检测范围内设置多个检测点后，可根据第i个检测点与各障碍物之间的距离，确定第i个检测点对应的各个障碍物静态势场u_omi，称下式为公式(2)：

式中，u_omi为第i个检测点对应的第m个障碍物的障碍物静态势场，U_sm为第m个障碍物的势场强度，ρ_omi为第i个检测点与第m个障碍物之间的距离，k_o为障碍物静态势场对应的系数，L_obsm为第m个障碍物的影响范围宽度，L_s为设定的安全距离。

3、障碍物动态势场

如图2所示，运动障碍物除一定安全距离范围内的静态势场，障碍物的运动导致障碍物周边势能重新进行分配，某一方向上的速度越大，该方向上势能影响范围越大，势场能量越强，反向势场强度随着距离障碍物越远，势场能量消弱越明显，这便是障碍物动态势场。障碍物沿道路行驶，根据其运动行为(换道、车道巡航等)建立不同动态势场。本发明针对自车换道模型进行势场建立，因此障碍物运动行为仅关注自车坐标系下障碍物运动信息，根据障碍物横纵向速度v_xm、v_ym确定横向动态势场影响长度L_latm和纵向动态势场影响长度L_lom，进而确定障碍物动态势场区间。且障碍物沿某一方向运动速度越大，势场影响范围越广。

车辆换道判断时，除根据障碍物速度确定势场影响区间，同时需要考虑障碍物相对本车运动方向以及速度大小。当障碍物纵向速度v_ym大于设定的纵向速度限值v_Ty行驶，其对后方车辆产生一定引力，吸引车辆跟车行驶；反之，对后方车辆产生斥力，强迫车辆换道。同理，当障碍物横向速度v_xm大于设定的横向速度限值v_Tx行驶，其对同向行驶车辆产生一定引力，反之产生斥力。

在预行驶道路上沿自车行驶方向的设定检测范围内设置多个检测点后，可根据障碍物与自车的纵向距离、以及障碍物的纵向速度，得到障碍物纵向动态势场u_lom，称下式为公式(3)：

式中，u_lom为第m个障碍物的障碍物纵向动态势场，sign()为符号函数，v_Ty为设定的纵向速度限值，v_ym为第m个障碍物的纵向速度，k_lo为障碍物纵向动态势场对应的系数，L_lom为第m个障碍物纵向动态势场影响长度，y_minm为第m个障碍物与自车的最近纵向距离，L_s为设定的安全距离，ρ_ym为第m个障碍物与自车之间的纵向距离，y_maxm为第m个障碍物与自车的最远纵向距离。

同理，根据障碍物与自车的横向距离、以及障碍物的横向速度，便可障碍物横向动态势场u_latm，下式称为公式(4)：

式中，u_latm为第m个障碍物的障碍物横向动态势场，sign()为符号函数，v_Tx为设定的横向速度限值，v_xm为第m个障碍物的横向速度，k_lat为障碍物横向动态势场对应的系数，L_latm为第m个障碍物的横向动态势场影响长度，x_minm为第m个障碍物与自车的最近横向距离，L_s为设定的安全距离，ρ_xm为第m个障碍物与自车之间的横向距离，x_maxm为第m个障碍物与自车的最远横向距离。

其中，公式(3)中的第m个障碍物与自车的最近纵向距离y_minm是指障碍物的车尾与自车的车头之间的纵向距离，第m个障碍物与自车的最远纵向距离y_maxm为障碍物的车头与自车的车尾之间的纵向距离；当障碍物在自车的左边或左前方，第m个障碍物与自车的最近横向距离x_minm为障碍物的车体右侧与自车的车体左侧之间的横向距离，第m个障碍物与自车的最远横向距离x_maxm为障碍物的车体左侧与自车的车体右侧之间的横向距离；当障碍物在自车的右边或右前方，第m个障碍物与自车的最近横向距离x_minm为障碍物的车体左侧与自车的车体右侧之间的横向距离，第m个障碍物与自车的最远横向距离x_maxm为障碍物的车体右侧与自车的车体左侧之间的横向距离。

在进行上述分析后，便可实现本发明的一种基于人工势场法的车辆换道方法。下面结合图1，以预行驶道路包括当前道路和右侧的旁侧道路为例，来具体说明该方法。

方法实施例：

步骤一，进行环境信息的获取，包括预行驶道路上的各障碍物的信息。根据车身定位和高清地图确定当前车辆行驶道路以及当前道路信息，包括车道条数，本车当前所处位置信息等，利用车身激光雷达对车身周边一定范围内障碍物进行扫描，确定障碍物相对车***置和速度信息。

步骤二，根据自车的纵向速度和反应时间等信息，确定各预行驶道路上沿自车行驶方向的设定检测范围，并在该设定检测范围内设置多个检测点。检测点并不是真的在实际道路上设置，是为了计算综合势场需要而添加的虚拟的点。

步骤三，根据检测点与预行驶道路中心线之间的距离以及公式(1)，确定第i个检测点对应的道路静态势u_ci。根据检测点与各个障碍物之间的距离以及公式(2)，确定第i个检测点对应的第m个障碍物的障碍物静态势场u_omi。根据障碍物与自车的纵向距离、障碍物的纵向速度、以及公式(3)，确定第m个障碍物的障碍物纵向动态势场u_lom。根据障碍物与自车的横向距离、障碍物的横向速度、以及公式(4)，确定第m个障碍物的障碍物横向动态势场u_latm。至此，完成了自车运行环境下势场分配，为进一步确定各预行驶道路的综合势场打下基础。

步骤四，确定预行驶道路的综合势场。如图4所示，预行驶道路的综合势场是周边环境共同作用的下的效果，道路静态势场、障碍物静态势场和障碍物动态势场对道路上不同位置产生不同的势场。具体过程如下：

1)先将第i个检测点对应的障碍物静态势场、障碍物纵向动态势场和障碍物横向动态势场进行加权求和，以得到将第i个检测点对应的障碍物势场u_zi：

式中，w_o、w_lo和w_lat分别为障碍物静态势场、障碍物纵向动态势场和障碍物横向动态势场对应的加权系数，根据U_c、U_sm的大小标定得到。

2)将第i个检测点对应的障碍物势场u_zi和第i个检测点对应的道路静态势场相加，得到第i个检测点对应的综合势场u_i：

u_i＝u_ci+u_zi

3)将各个检测点对应的综合势场进行加权求和，得到第j个预行驶道路的综合势场U_j：

式中，w_i为各个检测点对应的加权系数，各个加权系数根据检测点相对自车车头沿行驶方向距离确定，当然也可采用相同加权系数，根据对不同距离的关注程度确定。

步骤五，在得到各个预行驶道路的综合势场后，将各个预行驶道路的综合势场与对应的道路系数相乘，得到各个预行驶道路的换道概率。当前道路和旁侧道路对应的换道概率分别为：

P_curr＝U_curr*W_curr

P_neighbor＝U_neighbor*W_neighbor

式中，P_curr为当前道路的换道概率，P_neighbor为旁侧道路的换道概率，U_curr为当前道路的综合势场，U_neighbor为旁侧道路的综合势场，W_curr为当前道路的道路系数，W_neighbor为旁侧道路对应的道路系数，用于区分可行驶道路为当前道路还是旁侧道路，不可行驶道路该系数可设置的较大，以表示对道路进行禁行处理。

维修，突然发生车坡。

步骤六，比较各个预行驶道路的换道概率，构建得到如下所示的换道模型，以确定是否换道：

至此，便可根据上述结果来控制自车是否换道。

需说明的是，该实施例中出现的多个系数，公式(1)中的道路静态势场对应的系数k_c、公式(2)中的障碍物静态势场对应的系数k_o、公式(3)中的障碍物纵向动态势场对应的系数k_lo、公式(4)中的障碍物横向动态势场对应的系数k_lat，根据设定的初始势场U_c、U_sm的大小标定，可以认为是增益常量。

在本实施中，预行驶道路的综合势场考虑了三种势场，即除了考虑障碍物动态势场外，还考虑了静态势场中的道路静态势场和障碍物静态势场，以全面考虑自车周边环境，得到的综合势场较为准确。作为其他实施方式，静态势场可只考虑道路静态势场和障碍物静态势场中的一种势场。

在本实施例中，是以预行驶道路包括当前道路和右侧的旁侧道路为例来进行说明。作为其他实施方式，预行驶道路可包括当前道路、左侧的旁侧道路、以及右侧的旁侧道路，则需要计算这三个预行驶道路的综合势场，并比较这三个预行驶道路的换道概率，从中挑选出换道概率最低的道路进行行驶。

在本实施例中，在计算得到各个预行驶道路的综合势场后，将各个预行驶道路与对应的道路系数相乘，从而得到各个预行驶道路的换道概率，根据换道概率确定是否换道。该种处理方法可应用于一些出现不可行驶道路的状况，例如，某一条道路出现了车祸，则设置该道路对应的道路系数较大一些，以表示对该道路进行禁行处理。当然，作为其他实施方式，若每条道路均没有出现这种完全不能行驶的状况，此时得到各个预行驶道路的综合势场后，直接将各个预行驶道路的综合势场进行比较，根据比较结果换道即可，无需再将各个预行驶道路的综合势场乘以对应的道路系数，并根据换道概率来确定是否换道。

装置实施例：

该实施例提供了一种基于人工势场法的车辆换道装置，如图5所示，包括存储器、处理器和内部总线，处理器、存储器之间通过内部总线完成相互间的通信。

处理器可以为微处理器MCU、可编程逻辑器件FPGA等处理装置。

存储器可为利用电能方式存储信息的各式存储器，RAM、ROM等；利用磁能方式存储信息的各式存储器，例如硬盘、软盘、磁带、磁芯存储器、磁泡存储器、U盘等；利用光学方式存储信息的各式存储器，例如CD、DVD等。当然，还有其他方式的存储器，例如量子存储器、石墨烯存储器等。

处理器可以调用存储器中的逻辑指令，以实现一种基于人工势场法的车辆换道方法。在方法实施例中对该方法做了详细介绍。

Claims (9)

1.一种基于人工势场法的车辆换道方法，其特征在于，包括如下步骤：

1)获取预行驶道路上的各障碍物的信息，所述障碍物的信息包括障碍物的速度和位置；

2)在各预行驶道路上沿自车行驶方向的设定检测范围内均设置至少一个检测点，确定各个检测点对应的静态势场和障碍物动态势场，所述静态势场包括道路静态势场和/或障碍物静态势场；其中：

根据检测点与预行驶道路中心线之间的距离，确定该检测点对应的道路静态势场；所述检测点对应的道路静态势场为：

式中，u_ci为第i个检测点对应的道路静态势场，ρ_ci为第i个检测点与预行驶道路中心线之间的距离，k_c为道路静态势场对应的系数，U_c为预行驶道路中心线的势场强度，L_c为当前道路引力场的影响范围宽度；

根据检测点与各个障碍物之间的距离，确定该检测点对应的各个障碍物静态势场；

根据障碍物与自车之间的距离、以及各个障碍物的速度，确定各个障碍物动态势场；

3)根据每个预行驶道路的所有检测点对应的静态势场和障碍物动态势场，确定每个预行驶道路的综合势场；

4)根据各个预行驶道路的综合势场，进行换道控制。

2.根据权利要求1所述的基于人工势场法的车辆换道方法，其特征在于，步骤2)中，所述障碍物动态势场包括障碍物横向动态势场和障碍物纵向动态势场；所述障碍物纵向动态势场根据障碍物与自车的纵向距离、以及障碍物的纵向速度得到；所述障碍物横向动态势场根据障碍物与自车的横向距离、以及障碍物的横向速度得到。

3.根据权利要求1所述的基于人工势场法的车辆换道方法，其特征在于，步骤2)中，所述检测点对应的各个障碍物静态势场为：

式中，u_omi为第i个检测点对应的第m个障碍物的障碍物静态势场，U_sm为第m个障碍物的安全范围内的势场强度，ρ_omi为为第i个检测点与第m个障碍物之间的距离，k_o为障碍物静态势场对应的系数，L_obsm为第m个障碍物的影响范围宽度，L_s为设定的安全距离。

4.根据权利要求2所述的基于人工势场法的车辆换道方法，其特征在于，障碍物纵向动态势场为：

式中，u_lom为第m个障碍物的障碍物纵向动态势场，sign()为符号函数，v_Ty为设定的纵向速度限值，v_ym为第m个障碍物的纵向速度，k_lo为障碍物纵向动态势场对应的系数，L_lom为第m个障碍物纵向动态势场影响长度，y_minm为第m个障碍物与自车的最近纵向距离，L_s为设定的安全距离，ρ_ym为第m个障碍物与自车之间的纵向距离，y_maxm为第m个障碍物与自车的最远纵向距离。

5.根据权利要求2所述的基于人工势场法的车辆换道方法，其特征在于，障碍物横向动态势场为：

式中，u_latm为第m个障碍物的障碍物横向动态势场，sign()为符号函数，v_Tx为设定的横向速度限值，v_xm为第m个障碍物的横向速度，k_lat为障碍物横向动态势场对应的系数，L_latm为第m个障碍物的横向动态势场影响长度，x_minm为第m个障碍物与自车的最近横向距离，L_s为设定的安全距离，ρ_xm为第m个障碍物与自车之间的横向距离，x_maxm为第m个障碍物与自车的最远横向距离。

6.根据权利要求1所述的基于人工势场法的车辆换道方法，其特征在于，步骤4)中，将各个预行驶道路的综合势场与对应的道路系数相乘，得到各个预行驶道路的换道概率，比较各个预行驶道路的换道概率，进行换道控制。

7.根据权利要求2所述的基于人工势场法的车辆换道方法，其特征在于，所述静态势场包括道路静态势场和障碍物静态势场，则预行驶道路的综合势场为：

式中，U_j为第j个预行驶道路的综合势场，u_ci为第i个检测点对应的道路静态势场，u_omi为第i个检测点对应的第m个障碍物的障碍物静态势场，u_lom为第m个障碍物的障碍物纵向动态势场，u_latm为第m个障碍物的障碍物横向动态势场，len为预行驶道路上沿自车行驶方向的设定检测范围内检测点的个数，n_obs为预行驶道路上的障碍物的个数，w_o、w_lo和w_lat分别为道路静态势场、障碍物静态势场、障碍物纵向动态势场和障碍物横向动态势场对应的加权系数，w_i为各个检测点对应的加权系数。

8.根据权利要求1～7任一项所述的基于人工势场法的车辆换道方法，其特征在于，步骤4)中，进行换道控制时，控制换道至综合势场最小的预行驶道路。

9.一种基于人工势场法的车辆换道装置，其特征在于，包括存储器和处理器，所述处理器用于执行存储在存储器中的指令以实现如权利要求1～8任一项所述的基于人工势场法的车辆换道方法。