CN102591332A - 用于无人驾驶汽车局部路径规划的装置及方法 - Google Patents

Abstract

本发明一种用于无人驾驶汽车局部路径规划的装置及方法，该装置包括环境感知装置、斥力计算装置、引力计算装置、合力方向角度计算装置和方向盘转角计算装置，其通过环境感知装置探测障碍物，建立道路边界模型和道路中心线模型；斥力计算装置建立斥力点函数和计算斥力；引力计算装置建立引力点函数和计算引力；合力方向角度计算装置计算斥力和引力的合力的方向角度；方向盘转角计算装置根据合力的方向角度和转向***传动比确定方向盘转角。该方法不仅消除了人工势场法中由于斥力和引力在同一个方向时产生的陷入局部极小和路径震荡的问题，而且可对车辆因不确定因素干扰所引起的行驶路径偏离进行实时纠正。

Description

用于无人驾驶汽车局部路径规划的装置及方法

技术领域

本发明属于智能汽车技术领域，具体涉及用于无人驾驶局部路径规划的装置及方法。

背景技术

无人驾驶汽车***(Autonomous Ground Vehicle简称AGV)是一种根据各种传感器获得环境信息以及车辆状态、位置，通过对环境的理解自动控制车辆驾驶行为的智能控制***，主要由传感器，处理器，控制器等装置组成。

局部路径规划是无人驾驶汽车研究的关键技术之一。局部路径规划是指：无人驾驶汽车在不确定的道路环境中，控制***根据环境感知***和车辆状态检测***提供的信息、全局路径规划提供的所要达到的目标等实时规划出车辆当前的行驶路径。

人工势场法是局部路径规划研究中比较成熟和实时性较好的规划方法，它是将车辆行驶的环境信息抽象为引力场函数和斥力场函数，通过合力场函数来规划出一条从起始点到引力点(目标点)的无碰撞路径。

发明内容

本发明的目的在于提供一种用于无人驾驶汽车局部路径规划的装置及方法，其是在不需要建立复杂的环境模型的情况下，根据环境特征计算出无人驾驶汽车的行驶路径。

为达到以上目的，本发明所采用的解决方案是：

一种用于无人驾驶汽车局部路径规划的装置，其包括：

环境感知装置，用于探测障碍物，建立道路边界模型和道路中心线模型；

斥力计算装置，用于建立斥力点函数和计算斥力；

引力计算装置，用于建立引力点函数和计算引力；

合力方向角度计算装置，用于计算斥力和引力的合力的方向角度；

方向盘转角计算装置，用于根据合力的方向角度和转向***传动比确定方向盘转角。

进一步，所述环境感知装置为视觉传感器和雷达，视觉传感器探测道路边界并计算出道路中心线，雷达探测障碍物信息，将视觉传感器给出的道路坐标点信息拟合为多次曲线，建立道路边界模型和道路中心线模型。

所述环境感知装置为雷达，雷达探测路沿，拟合道路边界信息，推算出道路中心线信息，建立道路边界模型和道路中心线模型。

所述斥力计算装置根据环境感知装置给出的环境的实时信息确定车辆将要行驶的两条道路边界来建立斥力点函数；并根据斥力点函数计算随着环境的改变，无人驾驶车斥力点的位置及计算斥力点对无人驾驶汽车的斥力大小。

所述引力计算装置通过偏差计算装置计算以新的边界线为道路的道路中心线与目前车辆所在道路的道路中心线间的横向距离，同时通过高斯隶属组合函数将道路内有无障碍物、障碍物距离无人驾驶汽车远近的信息实时反映在引力点函数上；并根据引力点函数计算随着环境的改变，无人驾驶车引力点的位置，及计算引力点对无人驾驶汽车的引力大小。

一种用于无人驾驶汽车局部路径规划的方法，其包括：

环境感知步骤，探测障碍物，建立道路边界模型和道路中心线模型；

斥力计算步骤，建立斥力点函数和计算斥力；

引力计算步骤，建立引力点函数和计算引力；

合力方向角度计算步骤，计算斥力和引力的合力的方向角度；

方向盘转角计算步骤，根据合力的方向角度和转向***传动比确定方向盘转角。

进一步，所述环境感知步骤为视觉传感器探测道路边界并计算出道路中心线，雷达探测障碍物信息，将视觉传感器给出的道路坐标点信息拟合为多次曲线，建立道路边界模型和道路中心线模型。

所述环境感知步骤为雷达探测路沿，拟合道路边界信息，推算出道路中心线信息，建立道路边界模型和道路中心线模型。

所述斥力计算步骤根据环境感知装置给出的环境的实时信息确定车辆将要行驶的两条道路边界来建立斥力点函数；并根据斥力点函数计算随着环境的改变，无人驾驶车斥力点的位置及计算斥力点对无人驾驶汽车的斥力大小。

所述引力计算步骤通过偏差计算装置计算以新的边界线为道路的道路中心线与目前车辆所在道路的道路中心线间的横向距离，同时通过高斯隶属组合函数将道路内有无障碍物、障碍物距离无人驾驶汽车远近的信息实时反映在引力点函数上；并根据引力点函数计算随着环境的改变，无人驾驶车引力点的位置，及计算引力点对无人驾驶汽车的引力大小。

由于采用了上述方案，本发明具有以下特点：

1)通过利用高斯组合隶属函数实时反应环境变化的方法，本发明解决了传统人工势场法中由于斥力和引力在同一个方向时产生的陷入局部极小和路径震荡的问题；

2)通过斥力点函数的选择，当车辆因受到不确定性因素的干扰例如侧向风、路面不平度等偏离目标行驶路径时，斥力与引力的合力将引导车辆回到目标行驶路径，提高了车辆跟踪理想目标行驶路径的鲁棒性能；

3)由于不需要建立复杂的环境模型，仅需构建环境特征模型来实现局部路径规划，因此所需传感器数据信息量要求更小、数据信息处理的实时性更好，一方面有助于降低传感器成本，另一方面也有利于满足车载控制***高实时响应性的要求；

4)本发明具有很好的环境适应性，除本发明实施方案中所举两种行驶环境外，本发明对于其他行驶环境，例如交叉路口堵塞但具有可通过路径情况等，可实现多种环境下车辆自动避障；

5)通过高斯组合隶属函数系数的设定，目标行驶路径函数还可满足车辆最小转弯半径，前轮最大转向角速度等车辆运动学和动力学的约束条件，使得被控对象能够很好的跟踪给出的期望路径，从而实现预期的最优控制。

附图说明

图1为局部路径规划装置示意图。

图2为环境特征模型建立装置示意图。

图3为引力点函数建立装置示意图。

图4为σ取不同值的车辆轨迹比较图。

图5为合力方向角度计算装置示意图。

图6为无障碍物环境下无人驾驶车行驶轨迹仿真结果图。

图7为无障碍物环境下无人驾驶车期望转向盘转角图。

图8为障碍物环境下无人驾驶车行驶轨迹仿真结果图。

图9为障碍物环境下无人驾驶车期望转向盘转角图。

具体实施方式

以下结合附图所示实施例对本发明作进一步的说明。

本发明是在人工势场APF(Artificial Potential Field)法的基础上，利用高斯组合隶属函数建立目标行驶路径函数，即人工势场法的引力点函数，将环境的变化实时体现在引力点函数的变化上，并以此来计算引力；根据障碍物信息和道路边沿信息得到斥力点函数，以此来计算斥力。再通过引力和斥力计算出无人驾驶汽车所受合力方向，从而规划出无人驾驶车将行驶的路径。当车辆沿目标行驶路径行驶时，两斥力互相抵消，引力起主导作用，一旦车辆因为受到不确定性因素的干扰例如侧向风，路面不平度等偏离目标行驶路径，斥力与引力的合力将引导车辆回到目标行驶路径。

本发明中一种用于引力计算的装置，包括引力点函数建立装置和引力计算装置，其中该引力点函数装置包括：偏差计算，用于计算以新的边界线为道路的道路中心线与目前车辆所在道路的道路中心线间的横向距离；通过高斯隶属组合函数将道路内有无障碍物、障碍物距离无人驾驶汽车远近等实时反映在引力点函数上；其中该引力计算装置根据在引力点函数上引力点选取来计算引力。

其中，该偏差计算装置根据雷达(激光或毫米波雷达)获得的障碍物边界点信息以及道路中心线计算。

其中，该引力点函数根据视觉***给出道路中心线，根据实时环境的变化通过求解道路中心线偏差发生的最大变化计算出偏差值，并利用高斯隶属组合函数实时体现障碍物的信息，从而得到引力点函数。

其中，该高斯隶属组合函数的系数将使得目标行驶路径满足车辆的运动学和动力学约束条件。

其中，该引力点选取将根据道路的曲率和车辆的速度而定。

一种用于斥力计算的装置，包括斥力点函数建立装置和斥力计算装置，其中斥力点函数根据视觉传感器和雷达给出的环境的实时信息确定车辆将要行驶的两条道路边界来建立；斥力计算装置用于根据斥力点计算斥力大小。

本发明还包括将合力转化为转向盘转角装置，包括如下步骤：确立车体坐标系，根据引力大小和在车体坐标系下的方向将引力分解为正交坐标轴的分力，根据斥力大小和在车体坐标系下的方向将斥力分解为正交坐标轴的分力，从而计算出前轮转角，再根据转向***传动比计算出转向盘转角。

本发明拟使用视觉传感器探测道路边界并计算出道路中心线，使用雷达(激光或毫米波雷达)探测障碍物信息，将视觉***给出的道路坐标点信息拟合为多次曲线(考虑到车辆的动力学约束，一股为三次及以上曲线)建立道路边界模型和道路中心线模型(也可仅用激光雷达作为环境感知***，利用激光雷达探测路沿，拟合道路边界信息，推算出道路中心线信息)。基于传感器得到环境特征模型，引力点函数中的高斯隶属组合函数项将实时体现道路内环境的变化，即是否出现障碍物、障碍物与无人驾驶车间的距离，并根据实时得到的引力点函数计算引力；根据环境改变建立斥力点函数，并根据斥力点函数计算斥力大小。局部路径规划装置示意图如图1。

1、环境感知：

根据视觉***，在车体坐标系上建立道路两边界函数模型

$\left\{\begin{array}{c}{y}_1=a_1{x}^3+b_1{x}^2+c_{1}x+d_1\\ y_2=a_1{x}^3+b_1{x}^2+c_{1}x+d_2\end{array}\right.---\left(1\right)$

其中a₁，b₁，c₁，d₁，d₂为三次曲线系数。

由式(1)推出道路中心线函数模型

y_centre＝a₁x³+b₁x²+c₁x+(d₁+d₂)/2                             (2)

当道路内有障碍物时，通过雷达扫描与视觉***给出的道路线信息数据进行融合，得到当前道路内同一坐标系下的障碍物最危险边界点实时坐标(X_ob，Y_ob)，即得到可通过道路的两边界的数据信息。通过以上原始数据得到环境特征的基本模型。环境特征模型建立装置示意图如图2。

2、引力点函数建立

车辆在结构化道路上行驶，必然具有道路边界约束，并且可能存在障碍物，存在障碍物的情况具有两种可能，一种为障碍物与道路边界作为边界线时为最宽可通过路径，一种为两障碍物作为边界时为最宽可通过路径。引力点函数建立装置如图3。

1)道路内没有障碍物

此时，车辆受到两条道路边界线的约束不能行驶到道路以外，因此，两道路边界线即为斥力点函数，引力点函数即为道路中心线函数。

2)当道路内有障碍物时

当道路内有障碍物时，首先选择最宽可通过的道路，从而确定道路边界点以及偏差系数。

道路内有障碍物的情况下的引力点函数，分为两种情况，一种情况障碍物做为一侧边界线，道路边界线作为另一侧边界线，即斥力点函数分别为道路边界线和障碍物最危险边界点，另一种情况为两障碍物作为边界线，现以第一种情况加以详细说明。

这种情况下斥力点函数为道路边界线和障碍物最危险点的实时坐标点(X_ob，Y_ob)。

障碍物和一边道路线组成可通过路径的边界，产生新的道路中心线，则原道路中心线函数所需最大偏移量

Δs＝|(a₁x_ob ³+b₁x_ob ²+c₁x_ob+(d₁+d₂)/2)-(y_ob+a₂x_ob ³+b₂x_ob ²+c₂x_ob+d₂)/2|    (3)

即当车辆处于一边为障碍物边界的边界约束条件下的引力点函数为

y_goal＝a₁x³+b₁x²+c₁x+(d₁+d₂)/2+Δs                                       (4)

但直接加一个偏移量会造成路径曲率的不连续，不满足车辆的运动学约束条件，综上分析及考虑车辆约束条件，采用高斯隶属函数来光滑过度两个目标函数，因此修正引力点函数为

y_goal＝a₁x³+b₁x²+c₁x+(d₁+d₂)/2+Δs*exp(-(X-X_ob)²/2*σ²)                (5)

其中σ是与高斯隶属函数曲线的曲率相关的系数，在引力的引力点函数中，通过调节其值可满足车辆的动力学约束即路径曲率有界和路径曲率倒数有界，如图4σ取不同值的道路轨迹。从以上分析可以看出道路内没有障碍物的情况是道路内有障碍物情况的特例，即在没有障碍物的情况下Δs*exp(-(X-X_ob)²/2*σ²将趋于零。

综上，确立引力点函数即为

y_goal＝a₁x³+b₁x²+c₁x+(d₁+d₂)/2+Δs*exp(-(X-X_ob)²/2*σ²)    (6)

3、斥力点函数建立

斥力点函数

1)当没有障碍物时，斥力点函数即为道路边界即式(1)

2)当道路内有障碍物时，根据最宽可通过道路边界情况将引入障碍物作为一侧边界

4、引力计算

在本方法中将无人驾驶汽车(被控对象)简化为一质点，其所在空间为二维欧式空间。被控对象在空间中的位置X＝[xy]^T，在本发明中，因为是在车体坐标系下，所以X＝[00]^T。被控对象在X所受的引力场函数U_att(X)定义为与目标位置X_g＝[x_gy_g]^T相关的函数：

$U_{\mathrm{att}}\left(X\right)=\frac{1}{2}\mathrm{\κ}{(X-X_g)}^2---\left(7\right)$

式中：κ为引力场增益。相应的引力F_att(X)为引力场的负梯度：

$F_{\mathrm{att}}\left(X\right)=-\▿U_{\mathrm{att}}\left(X\right)=-\mathrm{\κ}{\mathrm{\ρ}}_g{a}_G---\left(8\right)$

式中：a_G为被控对象指向目标的单位向量；ρ_g＝||X-X_g||为被控对象与引力点之间的距离。

5、斥力计算

斥力场函数为

$U_{\mathrm{rep}}\left(X\right)=\left\{\begin{array}{cc}\frac{1}{2}\mathrm{\η}{(\frac{1}{{\mathrm{\ρ}}_{\mathrm{ob}}}-\frac{1}{{\mathrm{\ρ}}_0})}^2{{\mathrm{\ρ}}_g}^n& {\mathrm{\ρ}}_{\mathrm{ob}}\≤{\mathrm{\ρ}}_0\\ 0& {\mathrm{\ρ}}_{\mathrm{ob}}>{\mathrm{\ρ}}_0\end{array}\right.---\left(9\right)$

式中n是一个大于零的任意实数。

当被控对象不在引力点时，则相应的斥力为：

$F_{\mathrm{rep}}\left(X\right)=\left\{\begin{array}{cc}(F_{\mathrm{rep}1}+F_{\mathrm{rep}2})a_o& {\mathrm{\ρ}}_{\mathrm{ob}}\≤{\mathrm{\ρ}}_0\\ 0& {\mathrm{\ρ}}_{\mathrm{ob}}>{\mathrm{\ρ}}_0\end{array}\right.---\left(10\right)$

其中：

$F_{\mathrm{rep}1}=\mathrm{\η}(\frac{1}{{\mathrm{\ρ}}_{\mathrm{ob}}}-\frac{1}{{\mathrm{\ρ}}_0})\frac{{{\mathrm{\ρ}}_g}^n}{{{\mathrm{\ρ}}_{\mathrm{ob}}}^2}$

$F_{\mathrm{rep}2}=\frac{n}{2}\mathrm{\η}{(\frac{1}{{\mathrm{\ρ}}_{\mathrm{ob}}}-\frac{1}{{\mathrm{\ρ}}_0})}^2{{\mathrm{\ρ}}_g}^{n-1}$

式中：η为斥力场函数增益，ρ_ob＝||X-X_ob||为被控对象与障碍物的最短距离，常数ρ₀为根据车速设定的障碍物的影响距离。

其中a_O为障碍物指向被控对象的单位向量。

6、合力方向计算

合力方向即决定了被控对象的运动方向。在车体坐标系下，将引力和斥力分别分解为两坐标轴上的分力。合力方向角度计算装置示意图如图5。

在以车体坐标系为坐标轴建立的引力点函数上，选取引力点X_g＝[x_g，y_g]，则无人驾驶车与引力点之间的夹角

α＝arctan(y_g/x_g)                            (11)

引力在横、纵坐标上的分力为

$\left\{\begin{array}{c}{F}_{\mathrm{att}}\left(x_g\right)=F_{\mathrm{att}}*\cos \left(\mathrm{\α}\right)\\ F_{\mathrm{att}}\left(y_g\right)=F_{\mathrm{att}}*\sin \left(\mathrm{\α}\right)\end{array}\right.---\left(12\right)$

在车体坐标系上，斥力点X_ob(i)＝[x_ob(i)，y_ob(i)]，则无人驾驶车与障碍物之间的夹角

β_i＝arctan(y_ob(i)/x_ob(i))                   (13)

斥力在横、纵坐标上的分力为

$\left\{\begin{array}{c}{F}_{\mathrm{rep}}\left(x_{\mathrm{ob}}\left(i\right)\right)=F_{\mathrm{rep}}\left(i\right)*\cos \left({\mathrm{\β}}_i\right)\\ F_{\mathrm{rep}}\left(y_{\mathrm{ob}}\left(i\right)\right)=F_{\mathrm{rep}}\left(i\right)*\sin \left({\mathrm{\β}}_i\right)\end{array}\right.---\left(14\right)$

则无人驾驶汽车与合力的夹角，即期望的航向角

θ＝arctan((F_att(y)+F_rep(y(i)))/(F_att(x)+F_rep(x(i))))    (15)

转向盘转角

δ_sw＝θ*i_s                                              (16)

其中i_s为转向***传动比。

对以上发明在不同环境下进行仿真验证，***各参数采用如下值：

κ＝6，ρ₀＝10，η＝0.7，n＝2，σ＝4.5，车速v＝18km/h

当道路内无障碍物或障碍物不在有效距离范围内(即ρ_ob＞ρ₀)，利用改进人工势场法以道路边界线作为斥力，以道路中心线作为引力，此时修正项为零。图6为无人驾驶汽车行驶轨迹，从行驶轨迹可以看出，无人驾驶汽车能够很好的跟随道路中心线。图7为转向盘转角，从给出的转向盘转角命令可以看出，期望转角光滑且满足车辆的运动学和动力学约束(例如，受转向执行机构限制，无人驾驶汽车的转向盘转角的约束为|δ_sw|＜＝500°，转向盘角速度的约束为|V_δ|＜＝200°/s。从图7可以看出，在无障碍物环境下，利用本发明无人驾驶车辆在具有一定曲率的道路内行驶时，转向盘转角可保持在200度以内，转向盘角速度也可保持在200度/秒的约束范围内)。

当前方道路内出现障碍物，且无人驾驶车与障碍物距离在有效范围内(ρ_ob≤ρ₀)，以障碍物和一侧道路边界线作为斥力点函数，此时修正项与最大偏移量有关，以障碍物边界线和道路边界线的中心线为引力。图8为无人驾驶汽车行驶轨迹，从行驶轨迹可以看出，当道路内有障碍物时，无人驾驶汽车能够平滑的绕过障碍物，并在绕过障碍物后，平滑的回到道路中心线。图9为转向盘转角，从给出的转向盘转角可以看出，期望转角光滑且满足车辆动力学约束(在有障碍物环境下，利用本发明转向盘转角可保持在120度以内，转向盘角速度也可保持在200度/秒的约束范围内)。

上述实施例的描述是为便于该技术领域的普通技术人员能理解和应用本发明。熟悉本领域技术的人员显然可以容易地对这些实施例做出各种修改，并把在此说明的一股原理应用到其他实施例中而不必经过创造性的劳动。因此，本发明不限于这里的实施例，本领域技术人员根据本发明的揭示，对于本发明做出的改进和修改都应该在本发明的保护范围之内。

Claims (10)

1.一种用于无人驾驶汽车局部路径规划的装置，其特征在于：其包括：

环境感知装置，用于探测障碍物，建立道路边界模型和道路中心线模型；

斥力计算装置，用于建立斥力点函数和计算斥力；

引力计算装置，用于建立引力点函数和计算引力；

合力方向角度计算装置，用于计算斥力和引力的合力的方向角度；

方向盘转角计算装置，用于根据合力的方向角度和转向***传动比确定方向盘转角。

2.如权利要求1所述的用于无人驾驶汽车局部路径规划的装置，其特征在于：所述环境感知装置为视觉传感器和雷达，视觉传感器探测道路边界并计算出道路中心线，雷达探测障碍物信息，将视觉传感器给出的道路坐标点信息拟合为多次曲线，建立道路边界模型和道路中心线模型。

3.如权利要求1所述的用于无人驾驶汽车局部路径规划的装置，其特征在于：所述环境感知装置为雷达，雷达探测路沿，拟合道路边界信息，推算出道路中心线信息，建立道路边界模型和道路中心线模型。

4.如权利要求1所述的用于无人驾驶汽车局部路径规划的装置，其特征在于：所述斥力计算装置根据环境感知装置给出的环境的实时信息确定车辆将要行驶的两条道路边界来建立斥力点函数；并根据斥力点函数计算随着环境的改变，无人驾驶车斥力点的位置及计算斥力点对无人驾驶汽车的斥力大小。

5.如权利要求1所述的用于无人驾驶汽车局部路径规划的装置，其特征在于：所述引力计算装置通过偏差计算装置计算以新的边界线为道路的道路中心线与目前车辆所在道路的道路中心线间的横向距离，同时通过高斯隶属组合函数将道路内有无障碍物、障碍物距离无人驾驶汽车远近的信息实时反映在引力点函数上；并根据引力点函数计算随着环境的改变，无人驾驶车引力点的位置，及计算引力点对无人驾驶汽车的引力大小。

6.一种用于无人驾驶汽车局部路径规划的方法，其特征在于：其包括：

环境感知步骤，探测障碍物，建立道路边界模型和道路中心线模型；

斥力计算步骤，建立斥力点函数和计算斥力；

引力计算步骤，建立引力点函数和计算引力；

合力方向角度计算步骤，计算斥力和引力的合力的方向角度；

方向盘转角计算步骤，根据合力的方向角度和转向***传动比确定方向盘转角。

7.如权利要求6所述的用于无人驾驶汽车局部路径规划的方法，其特征在于：所述环境感知步骤为视觉传感器探测道路边界并计算出道路中心线，雷达探测障碍物信息，将视觉传感器给出的道路坐标点信息拟合为多次曲线，建立道路边界模型和道路中心线模型。

8.如权利要求6所述的用于无人驾驶汽车局部路径规划的方法，其特征在于：所述环境感知步骤为雷达探测路沿，拟合道路边界信息，推算出道路中心线信息，建立道路边界模型和道路中心线模型。

9.如权利要求6所述的用于无人驾驶汽车局部路径规划的方法，其特征在于：所述斥力计算步骤根据环境感知装置给出的环境的实时信息确定车辆将要行驶的两条道路边界来建立斥力点函数；并根据斥力点函数计算随着环境的改变，无人驾驶车斥力点的位置及计算斥力点对无人驾驶汽车的斥力大小。

10.如权利要求6所述的用于无人驾驶汽车局部路径规划的方法，其特征在于：所述引力计算步骤通过偏差计算装置计算以新的边界线为道路的道路中心线与目前车辆所在道路的道路中心线间的横向距离，同时通过高斯隶属组合函数将道路内有无障碍物、障碍物距离无人驾驶汽车远近的信息实时反映在引力点函数上；并根据引力点函数计算随着环境的改变，无人驾驶车引力点的位置，及计算引力点对无人驾驶汽车的引力大小。

Images