CN105974917A - 一种基于新型人工势场法的车辆避障路径规划研究方法 - Google Patents

Abstract

本发明公开了一种基于新型人工势场法的车辆避障路径规划研究方法，包括步骤：利用CCD摄像机、毫米波雷达、车载传感器分别实时采集车辆避障路径规划所需的信息，根据车辆避障路径规划所需的信息后，建立基于人工势场法的道路边界斥力势场和障碍物斥力势场模型，由主车在道路边界斥力势场和障碍物斥力势场组成的复合场中受到的力的作用建立平衡方程，求解得到主车在避障过程中要经过的位置点，从而得到避障路径，同时对避障过程中的主车车速进行控制以提高安全性和舒适性。本发明所用方法计算量小、便于实现实时控制，规划出的避障路径比传统方法更加安全、可靠。

Description

一种基于新型人工势场法的车辆避障路径规划研究方法

技术领域

本发明属于机动车辆驾驶安全领域，特别是涉及一种基于新型人工势场法的车辆避障路径规划研究方法。

背景技术

道路交通事故多由车辆与障碍物的碰撞以及碰撞后的次生事故引起。根据公安部交通管理科学研究所发布的我国2010年的交通事故统计数据，分析可知，因驾驶员过失(比如：判断失误，决策失误等)造成的交通事故起数约占90％。如果能够在紧急交通情况下帮助驾驶员采取相应的安全措施，那么交通事故的发生几率将会大幅减小，而车辆的局部避障路径规划正是实现这一目标的重要手段。该方法在碰撞发生前为车辆规划处一条安全路径，使车辆无碰撞的绕过障碍物，这对减少道路交通事故的发生具有十分重要的意义。

车辆局部路经规划是一个比较复杂的过程，驾驶员在行车过程中仅依靠感觉和经验进行避障的判断，很容易发生事故，尤其在高速行驶时。局部路径规划主要分为已知环境信息和未知环境信息两种类型，由于前者所适应的范围非常局限，所以本发明主要针对未知环境信息情形下的车辆局部路径规划。车辆通过车载设备(毫米波雷达、CCD摄像机、各种传感器等)获取道路信息，如路宽、车道数、障碍物的位置，大小等，并将这些信息进行一定的分析处理，然后根据探测到的这些信息规划出一条由起始点到目标点的无碰撞最优路径。

中国专利CN 102520718A公开了一种基于物理建模的机器人避障路径规划方法，该方法通过设立机器人工作区域的引力场栅格和距离信息栅格，建立机器人双重栅格信息图，基于上述双重栅格信息图，采用有向遍历法搜索所有可行路径进行规划。该方法是针对已经环境信息条件下为机器人进行避障路径规划，且当栅格取得很小时，存在计算量大等问题。中国专利CN104317291A公开了一种基于人工势场法的机器人避碰路径规划研究方法，提供了一种复杂形状移动机器人在未知环境下的的避碰路径规划方法。该方法只适用于静态障碍物的场合，且不能克服传统人工势场法的局部极小值问题。

发明内容

针对现有技术的不足，本发明提供了一种基于新型人工势场法的车辆避障路径规划研究方法，解决计算量大、传统人工势场法的局部极小值问题。

本发明是采用如下技术方案，实现上述技术目的。

一种基于新型人工势场法的车辆避障路径规划研究方法，包括以下步骤：

S1.利用CCD摄像机、毫米波雷达、车载设备分别实时采集车辆避障路径规划所需的道路信息、障碍物信息、主车信息；

S2.获取车辆避障路径规划所需的信息后，在毫米波雷达的探测范围建立道路坐标系，用向量表示道路边界点、障碍物、主车的位置，根据道路信息、障碍物信息、主车信息建立基于人工势场法的道路边界斥力势场和障碍物斥力势场模型；具体如下：

S2.1道路坐标系的建立

在平直或近似平直、宽为B的多车道道路上，根据毫米波雷达性能，在毫米波雷达的可探测距离L内，将道路边界分为n等份，每个等份间的距离为L₀＝L/n；以主车所在车道的中心线为x轴，主车质心在路面上的投影点为原点建立道路坐标系，假设在未来每一时刻汽车的质心位于道路边界对应等分点的连线上，则道路左、右边界上每个等分点、障碍物、主车的位置可以用向量表示出来；

S2.2道路边界斥力势场的建立

通过CCD摄像机识别前方的道路边界线并提取出道路边界线数据，通过边界线数据进行分析处理提取出道路信息，再以此建立道路边界斥力势场；

S2.3障碍物斥力势场的建立

通过毫米波雷达检测主车前方的障碍物信息、车载传感器获取主车的速度信息，再以此建立障碍物斥力势场；

S3.根据主车在道路边界斥力势场和障碍物斥力势场组成的复合场中受到的力的作用建立平衡方程，并利用数学软件matlab对其进行求解得到主车在避障过程中要经过的位置点，从而得到避障路径；

S4.对避障过程中的主车车速进行控制以提高安全性，使主车在避障时将车速降低到一个合适的值，并在绕开障碍物后恢复正常车速行驶。

进一步，所述S1中，道路信息包括路宽及车道数量；障碍物信息，包括障碍物的数目、大小、位置及速度；主车信息包括主车的速度、主车与障碍物的距离、主车与道路边界的距离以及主车相对于障碍物的角度。

进一步，所述S2.3障碍物斥力势场的建立过程中，将主车等效为一个质点，并用一个直径为D的安全圆包裹住障碍物，同时，为了使主车能够提前开始避障，赋予障碍物一个影响范围ρ₀，当车辆进入障碍物影响范围ρ₀时，将只受到沿道路坐标系y方向分布的障碍物斥力势场的作用，同时，对于主车前方运动的障碍物，若障碍物的速度在主车速度方向上有速度分量，则主车还会受到障碍物速度势场的斥力作用。

进一步，所述S3中，借助matlab对建立的平衡方程求解，实际得到的是主车未来某时刻将要驶向的点的纵坐标，从而得到主车在避障过程所要经过的各点，再将这些点进行曲线拟合，从而得到一条能使主车安全绕过障碍物的避障路径；同时，由于障碍物有可能是运动的，所以每隔一段时间Δt，根据新采集的信息再次进行路径规划，以保证实时性。

进一步，所述S4在主车避障过程中，主车进入障碍物影响范围后，对主车车速进行控制，使主车车速能随主车与障碍物距离的减小而减小，当车辆绕过障碍物后，主车车速能随主车与障碍物距离的增大而增大，当主车驶出障碍物的影响范围时，主车车速将不再受该模型的控制。

有益效果：利用改进的人工势场法为车辆自主避障规划出一条安全路径，并在避障过程中对主车车速进行控制以提高避障过程的安全性和舒适性。对于运动的障碍物和静止的障碍物，本方法都能在碰撞发生前为车辆规划出安全无碰撞的路径绕开障碍物，可极大的减少因避障失误所导致的交通事故，并且所用方法计算量小、便于实现实时控制。

附图说明

图1为本发明一种基于新型人工势场法的车辆避障路径规划研究方法的流程图；

图2为本发明一种基于新型人工势场法的车辆避障路径规划研究方法中所述的道路坐标系图；

图3为本发明一种基于新型人工势场法的车辆避障路径规划研究方法中所述的道路边界斥力示意图；

图4为本发明一种基于新型人工势场法的车辆避障路径规划研究方法中所述的道路边界斥力势场分布图；

图5为本发明一种基于新型人工势场法的车辆避障路径规划研究方法中所述的速度方向示意图；

图6为本发明一种基于新型人工势场法的车辆避障路径规划研究方法中所述的主车所受合力图；

图7为本发明一种基于新型人工势场法的车辆避障路径规划研究方法中所述的避障路径图；

图8为本发明一种基于新型人工势场法的车辆避障路径规划研究方法中所述的未引入速度避障路径图；

图9为本发明一种基于新型人工势场法的车辆避障路径规划研究方法中所述的路径跟随图；

图10为本发明一种基于新型人工势场法的车辆避障路径规划研究方法中所述的横摆角速度图；

图11为本发明一种基于新型人工势场法的车辆避障路径规划研究方法中所述的侧向加速度图。

具体实施方式

下面将结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

如图1所示，一种基于改进型人工势场法和速度控制的车辆避障路径规划研究方法流程图，包括步骤：

S1.利用CCD摄像机、毫米波雷达、车载设备(车载传感器、全球定位系统GPS)分别实时采集车辆避障路径规划所需的道路信息、障碍物信息、主车信息；CCD摄像机安装在主车前挡风玻璃上，需保证摄像头能够“直视”前方；毫米波雷达安装在车头前部、车辆的纵向轴线上，与地面的距离最少为35cm，最大为65cm；车速传感器安装在变速器壳内，主车内置有GPS。

其中道路信息包括路宽及车道数量；障碍物信息，包括障碍物的数目、大小、位置及速度；主车信息包括主车的速度、主车与障碍物的距离、主车与道路边界的距离以及主车相对于障碍物的角度。

S2.获取车辆避障路径规划所需的信息后，在毫米波雷达的探测范围建立道路坐标系，用向量表示道路边界点、障碍物、主车的位置，根据道路信息、障碍物信息、主车信息建立基于人工势场法的道路边界斥力势场和障碍物斥力势场模型；具体如下：

S2.1道路坐标系的建立

在平直或近似平直、宽为B的多车道道路上，根据毫米波雷达性能，在毫米波雷达的可探测距离L内，将道路边界分为n等份，每个等份间的距离为L₀＝L/n，如图2所示；以主车所在车道的中心线为x轴，主车质心在路面上的投影点为原点建立道路坐标系，假设在未来每一时刻汽车的质心位于道路边界对应等分点的连线上，则道路左、右边界上每个等分点、障碍物、主车的位置可以用向量表示出来。

S2.2道路边界斥力势场的建立

通过CCD摄像机识别前方的道路边界线并提取出道路边界线数据，通过边界线数据进行分析处理提取出道路信息，再以此建立道路边界斥力势场；道路边界斥力势场对处在其中的车辆产生斥力作用,如图3所示，以此来限制车辆的行驶区域。

根据人工势场模型在机器人领域运用的经验，建立如下式所示的道路边界斥力势场的数学模型：

式中的U_road,L、U_road,R分别为道路左右边界斥力势场，k_roadL、k_roadR分别为道路左、右边界危险斥力场施加斥力的比例常数，分别为汽车质心、道路左、右边界上对应点的坐标向量，W_车为车宽。

由数学模型(1)、(2)及图4可知，汽车与道路边界的距离越小，它的势能值越大，汽车受到的斥力也就越大，当车辆与道路边界的距离趋于零时，道路边界斥力将趋于无穷大，以此来限制车辆的行驶区域，斥力可由对势场模型进行梯度运算得到。

为了使车辆在道路中没有障碍物或无需采取避障措施时，能够沿着车道行驶，即实现车道保持功能，我们通过调整势场的比例常数k_road,L、k_road,R来使车辆在道路边界斥力势场的作用下，其受力平衡点始终保持在车道中间，但是，考虑到交通规则和驾驶员的驾驶习惯，我们将尽量使主车在无需避障的情况下能够沿着右车道的中心线行驶，本发明取主车行驶车道为双车道，由势场数学模型可知：

U_road,L＝U_road,R (3)

在没有障碍物时，主车距离车道右边界距离为主车距离车道左边界距离为

即

$\frac{1}{2}\·k_{road,L}{\left(\frac{1}{3/2B}\right)}^2=\frac{1}{2}\·k_{road,R}{\left(\frac{1}{1/2B}\right)}^2---\left(4\right)$

从而得到如下关系：

$\frac{k_{road,L}}{k_{road,R}}=\frac{{\left(\frac{1}{1/4B}\right)}^2}{{\left(\frac{1}{3/4B}\right)}^2}=9---\left(5\right)$

S2.3障碍物斥力势场的建立

通过毫米波雷达检测主车前方的障碍物信息、车载传感器获取主车的速度信息，再以此建立障碍物斥力势场。

障碍物斥力势场对行驶中的车辆产生斥力作用，使车辆远离障碍物。汽车在行驶过程中，前方的道路交通情况随时都可能发生变化，可能没有障碍物，可能突然出现障碍物，障碍物可能是静止的，也可能运动的；因此，车载雷达必须以一定的频率不断探测前方的道路情况，包括障碍物的数目、大小、位置以及速度。影响障碍物斥力大小的因素主要有：障碍物相对于本车的位置和速度，障碍物的大小(按标准车)。

在路径规划过程中，将主车等效为一个质点，这与实际情况不符，为此用一个直径为D的安全圆包裹住障碍物，这个安全圆的尺寸必须将主车和障碍物的尺寸都考虑进去，为此本发明暂定直径D为两倍车宽。同时，为了是主车能够提前开始避障，我们赋予障碍物一个影响范围ρ₀，ρ₀取值为车辆的制动安全距离，当车辆进入障碍物影响范围ρ₀时，将受到障碍物斥力势场的作用，且该斥力势场只沿y方向分布。同时，对于主车前方运动的障碍物，规定障碍物速度方向为β，主车速度的方向为θ，如图5所示，则主车速度方向与障碍物速度方向之间的夹角α＝θ-β；若即障碍物的速度在主车速度上有一个分量v₀cosα，则主车还会受到障碍物速度势场的斥力作用；若即障碍物的速度在主车速度方向上没有速度分量，则主车不受速度斥力势场的作用。避障过程只考虑主车前方的障碍物，故α∈(0,π)。

情况一，当时，障碍物的斥力势场为：

情况二，当时，障碍物的斥力势场为：

情况三，当时，此时，主车不受障碍物斥力势场的作用，即

U_req＝0 (8)

综上所述，障碍物斥力势场为

式中k_ob为障碍物斥力势场的比例系数，为汽车质心对应点的坐标向量，W_车为车宽，为障碍物的位置向量，D为安全圆的直径，ρ₀为障碍物的影响范围，v为主车的当前运动速度，v₀为动态障碍物的当前运动速度，θ为移动车体的当前运动方向；φ为动态障碍物的当前运动方向。

从数学模型(9)及图4的势能值分布图可知，主车在进入障碍物的影响范围之后，随着主车与障碍物距离减小，主车受到的斥力增大，当主车与障碍物的距离趋近于零时，障碍物的斥力将区域无穷大，以此来使主车不予障碍物相撞。同时，对于运动的障碍物，障碍物的速度在本车速度方向上的分量越大，本车受到的斥力也就越大。

S3.根据主车在道路边界斥力势场和障碍物斥力势场组成的复合场中受到的力的作用建立平衡方程，并利用matlab对其进行求解得到主车在避障过程中要经过的位置点，从而得到避障路径；

在道路边界斥力势场和障碍物斥力势场组成的复合场中,主车受到复合场力的作用，并最终达到平衡状态，如图6所示，即：

$U_{total}=U_{L_i}+U_{R_i}+\underset{j=1}{\overset{m}{\Σ}}{U}_{req}=0---\left(10\right)$

$F_{total}=-\▿U_{tatal}=F_{L_i}+F_{R_i}+\underset{j=1}{\overset{m}{\Σ}}{F}_{req}=0---\left(11\right)$

借助数学软件matlab对其求解，由于假设主车为未来任意时刻的横坐标都对应道路边界某一等分点，即主车的横坐标已知(x_i＝(i-1)L₀)，对平衡方程进行求解，实际得到的是主车未来某时刻将要驶向的点的纵坐标，从而得到主车在避障过程所要经过的各点，然后用一条平滑的曲线将这些点连接起来，就得到一条能使主车安全绕过障碍物的避障路径。同时，由于障碍物有可能是运动的，所以每隔一段时间Δt，根据新采集的信息再次进行路径规划，以保证实时性。

主程序如下：

通过matlab的运算，可以得到避障路径的坐标，根据l的取值，共有120组数据，由于篇幅的限制，这里只列出20组，如表1所示：

表1避障过程的20组坐标

从表1数据和图7可以看出，改进后的方法规划出来的路径在避障过程中主车与障碍物的距离大于改进前的距离至少20cm,这可以有效的提高避障时的安全性，避免与障碍物发生“擦肩而过”的现象。从图8中可以看出，对于运动的障碍物，改进前的方法规划出的路径和障碍物有重叠，这说明主车按此路径行驶会与障碍物相撞，而改进后的方法却能很好的规避运动的障碍物。

S4.对避障过程中的主车车速进行控制以提高安全性，使主车在避障时将车速降低到一个合适的值，并在绕开障碍物后恢复正常车速行驶。

车辆在避障过程中为了保证安全，必须与障碍物保持一定的距离，且在避障转向时，如果仍以之前的速度行驶(通常＞40km/h)，则会严重影响避障的安全性和乘坐舒适性，尤其是高速行驶时，为此本文提出了对避障过程车辆速度的控制方法，使车辆在避障时将车速降低到一个合适的值，并在绕开障碍物后恢复正常车速行驶。速度的控制模型为：

$v=\left\{\begin{array}{cc}\mathrm{\λ}\·\frac{d_{co}}{v_c\·t_d+\frac{{v_c}^2}{2a_{max}}+d_0}{v}_c,& d_{co}\≤{\mathrm{\ρ}}_0\\ v_c,& d_{co}>{\mathrm{\ρ}}_0\end{array}\right.---\left(12\right)$

式中d_co代表主车与障碍物间的距离，d₀为预留的安全距离，v_c是主车避障前的速度，a_max为最大减速度，λ为放大系数，取λ＝0.6～0.7，ρ₀代表障碍物的影响距离，t_d为车辆从开始制动到其停止的时间。

从速度控制模型可以看出，主车在进入障碍物影响范围内后，速度能够随与障碍物距离的减小而减小，当车辆绕过障碍物时，速度随车辆与障碍物距离的增大而增大，当主车驶出障碍物的影响范围时，车速将不再受从模型的控制。

将matlab计算出的避障路径导入车辆动力学仿真软件carsim中，可以得到如图9和图10所示的仿真曲线；其中图9是路径跟随曲线，图中带圈曲线是仿真软件中主车的真实行驶路径，不带圈曲线是目标路径，为求解得到的路径；从图中可以看出，两条曲线基本吻合，说明在carsim仿真软件中，主车是沿计算得到的避障路径行驶，同时这也可以说明仿真过程的主车各种动力学信息与现实中主车避障过程的状态信息相符。仿真过程中各模型参数见表2。

表2仿真模型参数

变量	数值	单位
			KL	900	--
KR	100	--
			Koj	1500	--
λ	2.5	--
			l0	1.25	m
ρ0	10	m
			L	180	m
B	7	m
			D	4	m

图10和图11是主车避障过程中的横摆角速度和侧向加速度图。可以看出，两图的变化趋势大致相同，这说明主车的横摆角速度和侧向加速度随主车避障过程的变化而变化，只是在避障开始阶段，侧向加速度的变化速度大于横摆角速度。

两个图的变化趋势反映了主车的整个避障过程，开始时，主车在极短的时间加速到预设车速，横摆角速度和侧向加速度都出现了较大的值，随着车速提高到预设值，横摆角速度和侧向加速度只在很小的范围内变化，直到第6秒左右，主车接近前方障碍物，开始缓慢转向，在8.4秒左右，主车横摆角速度和侧向加速度都达到了最大值，说明主车已到达障碍物正侧方，即将就要绕过障碍物，之后，横摆角速度和侧向加速度的值变小，主车逐渐绕过障碍物并回到原车道。

在图10和图11中，虚线为避障过程对速度进行控制后得到的主车横摆角速度和侧向加速度图，实线为未对车速进行控制得到的主车横摆角速度和侧向加速度图。主车在未进入障碍物的影响范围内时，图中两条曲线基本吻合，在进入障碍物的影响范围内后，可以明显看出，避障过程对车速进行控制后，其横摆角速度和侧向加速度的值都小于未对车速进行控制的值，且侧向加速度的值小于0.4g,这说明在避障过程对车速进行控制可以有效提高舒适性。

以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围，应当理解，本发明并不限于这里所描述的实现方案，这些实现方案描述的目的在于帮助本领域中的技术人员实践本发明。任何本领域中的技术人员很容易在不脱离本发明精神和范围的情况下进行进一步的改进和完善，因此本发明只受到本发明权利要求的内容和范围的限制，其意图涵盖所有包括在由所附权利要求所限定的本发明精神和范围内的备选方案和等同方案。

Claims (5)

1.一种基于新型人工势场法的车辆避障路径规划研究方法，其特征在于，包括以下步骤：

S1.利用CCD摄像机、毫米波雷达、车载设备分别实时采集车辆避障路径规划所需的道路信息、障碍物信息、主车信息；

S2.获取车辆避障路径规划所需的信息后，在毫米波雷达的探测范围建立道路坐标系，用向量表示道路边界点、障碍物、主车的位置，根据道路信息、障碍物信息、主车信息建立基于人工势场法的道路边界斥力势场和障碍物斥力势场模型；具体如下：

S2.1道路坐标系的建立

在平直或近似平直、宽为B的多车道道路上，根据毫米波雷达性能，在毫米波雷达的可探测距离L内，将道路边界分为n等份，每个等份间的距离为L₀＝L/n；以主车所在车道的中心线为x轴，主车质心在路面上的投影点为原点建立道路坐标系，假设在未来每一时刻汽车的质心位于道路边界对应等分点的连线上，则道路左、右边界上每个等分点、障碍物、主车的位置可以用向量表示出来；

S2.2道路边界斥力势场的建立

通过CCD摄像机识别前方的道路边界线并提取出道路边界线数据，通过边界线数据进行分析处理提取出道路信息，再以此建立道路边界斥力势场；

S2.3障碍物斥力势场的建立

通过毫米波雷达检测主车前方的障碍物信息、车载传感器获取主车的速度信息，再以此建立障碍物斥力势场；

S3.根据主车在道路边界斥力势场和障碍物斥力势场组成的复合场中受到的力的作用建立平衡方程，并利用数学软件matlab对其进行求解得到主车在避障过程中要经过的位置点，从而得到避障路径；

S4.对避障过程中的主车车速进行控制以提高安全性，使主车在避障时将车速降低到一个合适的值，并在绕开障碍物后恢复正常车速行驶。

2.根据权利要求1所述的一种基于新型人工势场法的车辆避障路径规划研究方法，其特征在于，所述S1中，道路信息包括路宽及车道数量；障碍物信息包括障碍物的数目、大小、位置及速度；主车信息包括主车的速度、主车与障碍物的距离、主车与道路边界的距离以及主车相对于障碍物的角度。

3.根据权利要求1所述的一种基于新型人工势场法的的车辆避障路径规划研究方法，其特征在于，所述S2.3障碍物斥力势场的建立过程中，将主车等效为一个质点，并用一个直径为D的安全圆包裹住障碍物，同时，为了使主车能够提前开始避障，赋予障碍物一个影响范围ρ₀，当车辆进入障碍物影响范围ρ₀时，将只受到沿道路坐标系y方向分布的障碍物斥力势场的作用，同时，对于主车前方运动的障碍物，若障碍物的速度在主车速度方向上有速度分量，则主车还会受到障碍物速度势场的斥力作用。

4.根据权利要求1所述的一种基于新型人工势场法的的车辆避障路径规划研究方法，其特征在于，所述S3中，借助matlab对建立的平衡方程求解，实际得到的是主车未来某时刻将要驶向的点的纵坐标，从而得到主车在避障过程所要经过的各点，再将这些点进行曲线拟合，从而得到一条能使主车安全绕过障碍物的避障路径；同时，由于障碍物有可能是运动的，所以每隔一段时间Δt，根据新采集的信息再次进行路径规划，以保证实时性。

5.根据权利要求1所述的一种基于新型人工势场法的的车辆避障路径规划研究方法，其特征在于，所述S4在主车避障过程中，主车进入障碍物影响范围后，对主车车速进行控制，使主车车速能随主车与障碍物距离的减小而减小，当车辆绕过障碍物后，主车车速能随主车与障碍物距离的增大而增大，当主车驶出障碍物的影响范围时，主车车速将不再受该模型的控制。