一种用于L4级自动驾驶车辆的超声波避障方法

摘要

本发明涉及汽车控制技术领域，尤其涉及到一种用于L4级自动驾驶车辆的超声波避障方法，包括以下步骤：S1、自动驾驶汽车超声波雷达采集自动驾驶汽车运行点云信息，对自动驾驶车外环境进行实时检测；S2、设置多个传感器，感应车辆周围障碍物的状态以及自身的运动状态；S3、根据车辆与目标点的距离与吸引力关系定义引力函数；S4、进行超声波测距；S5、构建自动驾驶车辆超声波避障滚动优化模型，包括预测模型，滚动优化和反馈校正；S6、利用引力搜索算法设置车辆行为预测模块，对周围车辆的行为进行预测，并规划避障路径。

说明书

技术领域

本发明涉及到汽车控制技术领域，尤其涉及到一种用于L4级自动驾驶车辆的超声波避障方法。

背景技术

自动驾驶技术是当今世界最热门的研究领域之一，许多成熟的自动驾驶技术已经在不同的环境条件下得到广泛应用。目前智能汽车可以达到L4级高度自动计时级别。随着自动驾驶技术的快速发展，包括自动控制、计算机、电子技术、新材料、传感器、人工智能和机械工程得到飞速发展，伴随着社会发展的需要，自动驾驶技术的应用环境和功能任务越来越复杂，采用了更多新的理论、方法和技术。

超声波避障技术作为一项新兴技术，已从一门新兴的跨学科前沿学科逐渐转变为一门以物理、电子、机械、材料科学为基础的通用技术，并越来越多地应用于自动驾驶技术领域。自动驾驶超声波避障技术主要是利用先进的传感器技术增强汽车对行驶环境的感知，并将传感器系统获得的速度、位置等实时信息反馈给系统。同时根据路况、车流量等综合信息，对安全隐患进行判断分析。在紧急情况下，自动采取报警提示、制动或转向等措施，辅助和控制汽车主动避让障碍物，确保安全、高效、平稳行驶。超声波避障技术可以提高智能车的稳定性和灵活性，优秀的自动避障技术可以使智能车适应不同的工程背景和更多的领域。

超声波避障方法的好坏直接决定了超声波避障的水平，因此研究自动避障方法对提高智能车的自动避障性能，提升智能汽车的应用价值具有十分重要的意义。

超声波测距有多种方法，如相位检测、振幅检测和渡越时间检测。然而，这些方法都存在一些问题和缺陷。相位检测方法通过测量返回波和发射波之间的相位差来判断距离。幅度检测方法通过查看回声的幅度来判断距离；渡越时间检测方法是通过回波的返回延迟来判断距离。虽然相位检测方法具有高精度，但是其检测范围有限。振幅检测方法容易受到反射波的影响，飞行时间法测距时间长，精度低。

现有的超声波避障技术和方法不能很好地满足L4级自动驾驶车辆在复杂道路上的实际需求，因此需要一种更加高效、快速的超声波测距避障方案，以更好地满足L4级自动驾驶汽车的高精度避障需求。

发明内容

针对现有技术的不足，本发明提供一种用于L4级自动驾驶的超声波避障方法，能够精准快速地满足车辆避障预算预测，满足自动驾驶时的安全性、舒适性、可靠性需求。

为解决上述技术问题，本发明所采用的技术方案是：一种用于L4级自动驾驶的超声波避障方法，包括以下步骤：

S1、自动驾驶汽车超声波雷达采集自动驾驶汽车运行点云信息，对自动驾驶车外环境进行实时检测；

S2、设置多个传感器，感应车辆周围障碍物的状态以及自身的运动状态；

S3、根据车辆与目标点的距离与吸引力关系定义引力函数；

S4、进行超声波测距；

S5、构建自动驾驶车辆超声波避障滚动优化模型，包括预测模型，滚动优化和反馈校正；

S6、利用引力搜索算法设置车辆行为预测模块，对周围车辆的行为进行预测，并规划避障路径。

优选的，所述步骤S1包括以下的步骤：

（1）首先采用直通式滤波算法对原始点云进行滤波，准确检测范围，减少点云数量，得到空间尺度为L×W×H的场景点云；

（2）通过二维映射将点云转化为鸟瞰图，对数据进行张量化处理；

（3）利用ResNet、FPN和DetNet构建多尺度特征金字塔自动驾驶网络结构；将高分辨率特征图和低分辨率特征图结合起来，输入到子网络中，完成最终的分类任务和回归任务。

优选的，所述步骤（2）中的通过二维映射将点云转化为鸟瞰图，对数据进行张量化处理的具体操作如下：

首先，将超声波滤波器预处理的原始点的云托管离散化得到大小为d

其中L为点云空间的长度； W为点云空间的宽度；H为点云空间的高度； d

优选的，所述步骤S2包括以下步骤：

（1）分别设置感应自身运动状态的第一传感器和感应周围障碍物状态的第二传感器；所述第二传感器安装在车辆自身周围；

（2）若第二传感器感应到周围障碍物的状态为运动状态的情况下，判定分析周围障碍物在接下来的预设时间内的运动动作；若周围障碍物的状态为静止状态的情况下，无需分析周围障碍物在接下来的预设时间内的运动动作；

（3）将自动驾驶车辆的自身运动状态和障碍物运动状态做出对比，分析自动驾驶车辆和障碍物之间是否会发生碰撞；

在分析会有碰撞的情况下，通过控制，自动调节自动驾驶车辆的运动，使自动驾驶车辆和障碍物之间不会发生碰撞，并使得该自身运动动作和障碍物运动动作之间保持一个预设安全距离。

优选的，所述步骤S3根据车辆与目标点的距离与吸引力关系定义引力函数，包括以下步骤：

（1）选择吸引力函数，目标点具有负势能，对自动驾驶车辆具有吸引力，其大小随车辆行驶距离的变化而变化；当车辆接近目标点时，吸引力变大；同样，距离变长，吸引力变小；当达到目标点时，自动驾驶车辆的势能至少为零；因此，吸引力函数定义为:

y( t)表示比例增益系数，f表示自动驾驶车辆的行驶位置，o表示自动驾驶车辆的起始位置的欧氏距离；

（2）自动驾驶车辆在行驶过程中，不仅受到障碍物排斥力场的影响，还会受到目标位置引力场的影响，在实际环境中，路径规划是随机的，障碍物的数量是不确定的；考虑到自动驾驶车辆在多障碍物情况下的潜在受力，基于重力搜索算法的迭代规则如下:

其中cs表示算子的随机数，A表示粒子优化的加速，N表示优化的次数；

（3）由于在寻找最优路径的过程中存在一定的变量，优化参数不可能在一次优化中改变，所以针对不同的维度进行优化计算，表达式为:

其中G 代表寻优过程中的位置坐标，代表车辆的行驶速度，l

优选的，所述步骤S4超声波测距主要包括以下步骤：首先定义一个超声波测距函数，给开发板的Trig高电平足够时间，使超声波模块发出超声波，接着给定时器清零，启动定时器，收到返回的声波后关闭定时器，通过定时器时间测算出距离，实现超声波测距。

优选的，所述步骤S5构建自动驾驶车辆超声波避障滚动优化模型，包括预测模型、滚动优化和反馈校正的具体操作如下：

(1) 根据系统的被控对象和预测的输出状态量，选择合适的预测模型，优选线性误差模型；

(2) 预测中的优化方法是在有限的时间段内在线滚动重复，在每个采样时间内，根据该时间段内的最优性能指标，设计合理的目标函数，求解该时间段内的最优控制序列；并将序列中的第一个元素带入线性误差模型进行计算，真正作用于系统的只是当前的控制量，在下一个采样时间，再次求解最优序列；

(3) 在超声波避障滚动优化的模型预测控制中，检测系统的实际输出并与预测状态量进行比较，得到模型误差，以此结果为基础，对模型的预测结果进行修正，然后进行新的优化。

优选的，所述步骤S5构建自动驾驶车辆超声波避障滚动优化模型，包括预测模型、滚动优化和反馈校正的具体实施过程如下：

选用线性误差模型作为预测模型，设计目标函数进行优化求解，得到最优控制序列，并将序列中的第一个元素带入线性误差模型进行计算；

目标函数采取以下形式:

公式中:X(k+i|k)表示当前时间为k时，k+i对应的给定参考输出,△u(k+j|k)代表控制输入的增量，Q和R是跟踪参考输出和增量控制量的权重矩阵，ρ是权重系数，ε代表松弛因子；

在目标函数中加入松弛因子，防止在运行过程中无解；考虑到控制过程中车辆本身的控制极限和控制增量约束，在速度增量约束上增加速度跟踪偏差约束，约束表达式如下:

目标函数中的控制变量是控制量的形式，约束表达式需要转换如下:

其中:V代表克罗内克乘积，u(k-1)为最后时刻的控制量；

将目标函数转化为标准二次型如下：

式中：

通过求解目标函数和约束条件，得到控制时域中控制变量的增量序列:

控制变量增量序列中的第一个元素作为当前时刻的实际控制变量作用于系统，即:

该控制量只作用于当前时刻，进入下一时刻后，重复上述求解过程，即可实现系统的滚动最优控制。

优选的，所述步骤S6利用引力搜索算法设置车辆行为预测模块，对周围车辆的行为进行预测，并规划避障路径，包括以下步骤：

（1）自动驾驶车辆在复杂的道路上行驶，考虑到车辆的实际情况，增加了路径规划的约束条件，包括道路边界约束、动静态障碍物约束和车辆运动学约束；

根据斥力势场，限定车辆的行驶区域，当行驶道路上没有其他障碍物时，要求车辆保持在右侧车道道路的中央行驶，道路边界的斥力函数是:

F表示道路边界的排斥系数，A表示左右边界点的坐标向量，r和uy表示车辆离道路边界点的距离参数，c表示道路宽度；

（2）利用引力搜索算法规划出车辆所需要到达的下一个坐标，以根据车辆上一时刻的坐标位置确定车辆所需要的转向角；

（3）若所规划的转向半径小于当前速度下的最小转向半径，根据最小转向半径计算下一点的坐标；

（4）计算坐标点与下一目标点坐标之间的关系，若两个目标点相同则路径规划结束，若不相同，则重新进行计算，直到寻找到最优点。

本发明的有益技术效果:

本发明方案成本低，便于大规模使用，可以在复杂环境下实现L4级车辆的高精度地超声波避障；根据本发明的技术方案，通过设置多个传感器实现对车辆点云信息及周围障碍物和自身运动状态的实时检测；根据车辆行为预测模块，用于对周围车辆的行为进行预测；根据超声波模块及定时器实现超声波测距。本发明专利将超声波避障与滚动优化模型及引力搜索算法相结合，利用滚动优化和引力搜索算法，提高了车辆的避障能力。

附图说明

图1是本发明实例例提供的用于L4级自动驾驶的超声波避障方法流程图；

图2是本发明实施例提供的用于L4级自动驾驶车辆的步骤S1的超声波雷达检测算法结构流程图；

图3是本发明实例提供的用于L4级自动驾驶车辆的避障方法的步骤S2的流程图；

图4是本发明实施例提供的用于L4级自动驾驶车辆的步骤S3的超声波避障滚动优化模型总体框图；

图5是本发明实例提供的用于L4级自动驾驶车辆的步骤S4的超声波测距程序流程图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将根据附图和实施例对本发明作进一步的详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

本发明提供一种用于L4级自动驾驶车辆的超声波避障方法，包括以下步骤，如图1所示：

S1、自动驾驶汽车超声波雷达采集自动驾驶汽车运行点云信息，对自动驾驶车外环境进行实时检测；

S2、设置多个传感器，感应车辆周围障碍物的状态以及自身的运动状态；

S3、根据车辆与目标点的距离与吸引力关系定义引力函数；

S4、进行超声波测距；

S5、构建自动驾驶车辆超声波避障滚动优化模型，包括预测模型，滚动优化和反馈校正；

S6、利用引力搜索算法设置车辆行为预测模块，对周围车辆的行为进行预测，并规划避障路径。

所述步骤S1自动驾驶汽车超声波雷达采集自动驾驶汽车运行点云信息，对自动驾驶车外环境进行实时检测；包括以下的步骤，如图2所示：

（1）首先采用直通式滤波算法对原始点云进行滤波，准确检测范围，减少点云数量，得到空间尺度为L×W×H的场景点云；

（2）通过二维映射将点云转化为鸟瞰图，对数据进行张量化处理；

（3）利用ResNet、FPN和DetNet构建多尺度特征金字塔自动驾驶网络结构；将高分辨率特征图和低分辨率特征图结合起来，输入到子网络中，完成最终的分类任务和回归任务。

所述步骤（2）中的通过二维映射将点云转化为鸟瞰图，对数据进行张量化处理的具体操作如下：

首先，将超声波滤波器预处理的原始点的云托管离散化得到大小为d

其中L为点云空间的长度； W为点云空间的宽度；H为点云空间的高度； d

所述步骤（3）中利用ResNet、FPN和DetNet构建多尺度特征金字塔自动驾驶网络结构的具体的操作如下：

网络中前四级由残差模块组成，每级残差瓶颈模块个数分别为(3，6，7，3)，形成51层残差网络，每级第一个残差瓶颈模块共进行四次降采样，将图片大小压缩到原大小的1/6；阶段5和阶段6形成用于空卷积的特征提取网络，每个阶段由三个空的剩余瓶颈模块组成，并且不执行下一个采样操作。最后加入一个自上而下的融合网络，通过融合各个阶段提取的特征，完成整个自动驾驶网络的特征提取。

对于步骤S2、设置多个传感器，感应车辆周围障碍物的状态以及自身的运动状态的具体的操作如下：

步骤1，预设多个传感器件，感应周围障碍物的状态以及自身的运动状态；

步骤2，分析得到自身运动状态，并将自身运动和障碍物运动做出对比，分析自身和障碍物之间是否会发生碰撞；

步骤3，在分析会有碰撞的情况下，控制自动调节自身运动动作，并使得该自身运动和障碍物运动之间不会发生碰撞。

通过上述技术方案，本发明的自动驾驶避障方法克服了现有技术中的避障汽车无法预知的弊端，可以预先测试是否会成为障碍物，因此，预先进行预警，并在有限的情况下实现自身运行轨迹的改变，从而避免出现碰撞，从而可以保证驾驶过程中可以越过所出现的障碍物。在步骤2中对障碍物的动作有一个预先判定，并且保证不会碰撞。在步骤3中，控制自动调节自身运动动作，并使得该自身运动动作和障碍物运动动作之间一直保持预设安全距离。通过上述的方式，可以保证驾驶过程中的自身和障碍物一直保持相应的距离，使其不会对自身造成影响。

在步骤1和步骤2中，分别设置感应自身的运动状态的第一传感器和感应周围障碍物状态的第二传感器；所述第二传感器布置于自身的周围。通过上述的方式，可以对自身和障碍物周围的所有数据进行采集，尽可能的避免障碍物对本身有任何的影响。

感应周围障碍物的状态包括：

在感应周围障碍物的状态为运动状态的情况下，判定分析周围障碍物在接下来的预设时间内的运动动作；通过上述的方式，可以减少运算，尽可能的避免复杂运算给系统带来压力，可以提高所需判断的准确度。

在该种实施方式中，为了实现周围障碍物的感应，所述第二传感器的数量为多个，且均匀地布置于所述自身的周围。

在该种实施方式中，为了避免对自身造成影响，所述预设安全距离为15m。

对于步骤S4超声波测距主要包括以下步骤：首先定义一个超声波测距函数，给开发板的Trig高电平足够时间，使超声波模块发出超声波，接着给定时器清零，启动定时器，收到返回的声波后关闭定时器，通过定时器时间测算出距离，实现超声波测距，参见图5。

对于步骤S5构建自动驾驶车辆超声波避障滚动优化模型，包括预测模型、滚动优化和反馈校正的具体操作如下：

(1) 根据系统的被控对象和预测的输出状态量，选择合适的预测模型，优选线性误差模型；

(2) 预测中的优化方法是在有限的时间段内在线滚动重复，在每个采样时间内，根据该时间段内的最优性能指标，设计合理的目标函数，求解该时间段内的最优控制序列；并将序列中的第一个元素带入线性误差模型进行计算，真正作用于系统的只是当前的控制量，在下一个采样时间，再次求解最优序列；

(3) 在超声波避障滚动优化的模型预测控制中，检测系统的实际输出并与预测状态量进行比较，得到模型误差，以此结果为基础，对模型的预测结果进行修正，然后进行新的优化。

图4是本发明实施例提供的用于避障的超声波滚动优化模型的具体实施的整体框图。根据线性误差模型可以实现对给定轨迹的跟踪和避障，但在跟踪过程中不评估跟踪效果。需要设计合理的目标函数进行优化求解，得到最优控制序列，并将序列中的第一个元素带入线性误差模型进行计算。考虑综合目标函数采取以下形式:

公式中:X(k+i|k)表示当前时间为k时，k+i对应的给定参考输出；△u(k +j|k)代表控制输入的增量，Q和R是跟踪参考输出和增量控制量的权重矩阵，ρ是权重系数，ε代表松弛因子。在目标函数中加入松弛因子的目的是防止在运行过程中无解。考虑到控制过程中车辆本身的控制极限和控制增量约束，在速度增量约束上增加了速度跟踪偏差约束。约束表达式如下:

目标函数中的控制变量是控制量的形式，约束表达式需要转换如下:

其中:V代表克罗内克乘积，u(k-1)为最后时刻的控制量。

将目标函数转化为标准二次型如下：

式中：H

G

通过求解目标函数和约束条件，可以得到控制时域中控制变量的增量序列:

控制变量增量序列中的第一个元素作为当前时刻的实际控制变量作用于系统，即:

该控制量只作用于当前时刻，进入下一时刻后，重复上述求解过程，即可实现系统的滚动最优控制。

根据本发明使用的L4级自动驾驶汽车的参数和跟踪数据，约束条件的数值设置如下:

其中:

在设定自主车辆避障路径约束条件的基础上，规划车辆的避障路径。

对于步骤S3引力函数的定义，主要包括以下步骤：

在空间区域的任意位置，作用在车辆上力的方向和大小都是确定的，并且都是单值。

选择吸引函数，目标点具有负势能，对自动驾驶车辆具有吸引力，其大小随车辆行驶距离的变化而变化。当车辆接近目标点时，吸引力变大。同样，距离变长，吸引力变小。当达到目标点时，自动驾驶车辆的势能至少为零。因此，吸引力函数定义为:

y( t)表示比例增益系数，f表示智能车的行驶位置，o表示智能车起始位置的欧氏距离。

自动驾驶车辆在行驶过程中，不仅受到障碍物排斥力场的影响，还会受到目标位置引力场的影响。在实际环境中，路径规划是随机的，障碍物的数量是不确定的。考虑到智能车在多障碍物情况下的潜在受力，基于重力搜索算法的迭代规则如下:

其中cs表示算子的随机数，A表示粒子优化的加速，N表示优化的次数。

由于在寻找最优路径的过程中存在一定的变量，优化参数不可能在一次优化中改变，所以针对不同的维度进行优化计算，表达式为:

其中G 代表寻优过程中的位置坐标，代表车辆的行驶速度，l

对于步骤S6，利用引力搜索算法设置车辆行为预测模块对周围车辆的行为进行预测并规划避障路径，包括以下步骤：

（1）自动驾驶车辆在复杂的道路上行驶，考虑到车辆的实际情况，增加了路径规划的约束条件，包括道路边界约束、动静态障碍物约束和车辆运动学约束；

根据斥力势场，限定车辆的行驶区域，当行驶道路上没有其他障碍物时，要求车辆保持在右侧车道道路的中央行驶，道路边界的斥力函数是:

F表示道路边界的排斥系数，A表示左右边界点的坐标向量，r和uy表示车辆离道路边界点的距离参数，c表示道路宽度；

（2）利用引力搜索算法规划出车辆所需要到达的下一个坐标，以根据车辆上一时刻的坐标位置确定车辆所需要的转向角；

（3）若所规划的转向半径小于当前速度下的最小转向半径，根据最小转向半径计算下一点的坐标；

（4）计算坐标点与下一目标点坐标之间的关系，若两个目标点相同则路径规划结束，若不相同，则重新进行计算，直到寻找到最优点。

本发明方案成本低，便于大规模使用，可以在复杂环境下实现L4级车辆的高精度地超声波避障；根据本发明的技术方案，通过设置多个传感器实现对车辆点云信息及周围障碍物和自身运动状态的实时检测；根据车辆行为预测模块，用于对周围车辆的行为进行预测；根据超声波模块及定时器实现超声波测距。本发明实施例将超声波避障与滚动优化模型及引力搜索算法相结合，利用滚动优化和引力搜索算法，提高了车辆的避障能力。