使用传感器融合来估计车道路径的系统和方法

摘要

本发明涉及使用传感器融合来估计车道路径的系统和方法。一种用于估计车辆在道路上的计划行驶路径的方法，包括：监测多个传感器输入；基于所监测传感器输入来确定车辆前面的道路几何形状；基于所监测传感器输入来确定相对于所述道路几何形状的车辆位置；从所述道路几何形状和车辆位置确定车辆前面的多个质点，所述质点表示可能行驶路径；以及使用所述多个质点中的重复确定的质点来导航车辆，包括删去所述多个质点中的车辆经过的质点。

说明书

相关申请的交叉引用

本申请要求于2009年1月26日提交的美国临时申请No.61/147,461的权益，上述申请作为参考引入本文。

技术领域

本发明涉及车辆中的道路和车道几何形状检测。

背景技术

该部分的内容仅提供与本发明有关的背景信息，且可能不构成现有技术。

车辆可配备有帮助车辆操作者管理车辆操作的各种传感装置和系统。这种传感器可以用来描述车辆的操作环境。一种传感系统旨在识别车道几何形状并估计相对于车道的车辆位置和取向。全球定位装置(GPS)或三维(3D)地图装置用于将车辆置于相对于地图的近似位置。然而，已知GPS装置具有位置误差、低取样速率和响应于车辆动态运动的大的滞后时间。这种系统用于一般导航，但是不是很适合于独立提供用于车辆操作的输入。示例性系统采用传感器(例如，车辆运动性能传感器和照相机系统)以将车辆定位在车道内且可以与GPS装置结合工作。

关于车道几何形状和相对于车道的车辆位置和取向的信息可以从多个源同时生成。将该信息融合成车道和车辆数据的估计值能够通过本领域已知的多种方法来完成。然而，许多这种方法使用历史航迹和地图估计来生成车道几何形状和相对于车道的车辆位置和取向的估计值。此外，许多方法在计算中使用高斯分布，以考虑未知趋势。本领域技术人员将理解，基于历史数据和高斯分布的估计值对输入的快速变化不敏感且通常包括滞后因子。

发明内容

一种用于估计车辆在道路上的计划行驶路径的方法，包括：监测多个传感器输入；基于所监测传感器输入来确定车辆前面的道路几何形状；基于所监测传感器输入来确定相对于所述道路几何形状的车辆位置；从所述道路几何形状和车辆位置确定车辆前面的多个质点(particle point)，所述质点表示可能行驶路径；以及使用所述多个质点中的重复确定的质点来导航车辆，包括删去所述多个质点中的车辆经过的质点。

一种用于估计车辆在道路上的计划行驶路径的方法，所述方法包括：

监测多个传感器输入；

基于所监测传感器输入来确定车辆前面的道路几何形状；

基于所监测传感器输入来确定相对于所述道路几何形状的车辆位置；

从所述道路几何形状和车辆位置确定车辆前面的多个质点，所述质点表示可能行驶路径；以及

使用所述多个质点中的重复确定的质点来导航车辆，包括删去所述多个质点中的车辆经过的质点。

确定车辆前面的多个质点包括：

确定用于多个传感器输入中的每个的多个质点；以及

将用于多个传感器输入中的每个的多个质点融合以确定车辆前面的多个质点。

使用所述多个质点中的重复确定的质点来导航车辆包括：

基于所述多个质点中的重复确定的质点来确定用于车辆导航的计划行驶路径。

基于所监测传感器输入来确定相对于所述道路几何形状的车辆位置包括：

确定相对于道路几何形状的车辆取向；以及

确定相对于道路几何形状的车辆横向位置。

监测多个传感器输入包括：

监测来自于地图装置的数据；以及

监测来自于至少一个附加传感器输入的数据，所述至少一个附加传感器输入选自包括视觉子系统、车辆运动性能传感器、雷达、LIDAR、车道对车辆通信系统和车辆对基础设施通信系统的组。

所述方法还包括：

当监测到来自于所述多个传感器输入中的至少一个的新数据时，重复地更新车辆位置。

车辆前面的道路几何形状包括：

道路曲率；和

在车辆位置处的切线。

道路曲率通过以下步骤确定：

生成样条，所述样条描述邻近车辆的道路曲率；以及

基于所述多个传感器输入来确定样条上的车辆位置。

生成样条包括：

收集来自于地图数据库的地图形状点；以及

用样条拟合地图形状点。

地图形状点与局部坐标系相关联。

监测多个传感器输入包括监测全球定位坐标；

其中，确定样条上的车辆位置包括：

基于所述多个传感器输入来确定样条上的车辆位置的初始估计值；

基于全球定位坐标来校正样条上的车辆位置的初始估计值。

车辆前面的多个质点基于：

基于车辆运动性能数据确定的第一组质点；

基于照相机数据确定的第二组质点；和

基于地图数据确定的第三组质点。

所述方法还包括：

使用最小二乘估计将第一组质点、第二组质点和第三组质点融合成表示行驶路径的单组质点。

融合基于与所述多个质点中的每个相关联的位置偏差来执行。

确定车辆位置使用卡尔曼滤波器执行。

监测多个传感器输入包括监测描述车辆前面的跟踪车辆轨道的传感器；

所述方法还包括：基于来自于描述车辆前面的跟踪车辆轨道的传感器的输入来确定车辆前面的道路曲率；以及

其中，确定车辆前面的道路几何形状基于车辆前面的道路曲率。

一种用于估计车辆在道路上的计划行驶路径的方法，所述方法包括：

监测多个传感器输入，所述传感器输入描述车辆前面的道路几何形状；

确定用于每个传感器输入的一组质点，包括：

基于相应传感器输入来确定车辆前面的道路几何形状；

基于相应传感器输入来确定相对于道路几何形状的车辆位置；以及

使用道路几何形状和车辆位置来确定相应传感器输入的该组质点，所述质点表示可能行驶路径；

将每个传感器输入的所述组质点融合成一组融合质点；以及

使用该组融合质点中的重复确定的质点来操作车辆，其中，使用重复确定的质点包括删去所述组融合质点中的车辆经过的独立质点。

融合所述组质点包括使用最小二乘估计。

监测多个传感器输入包括：

监测来自于全球定位装置的数据；

监测来自于车辆运动性能传感器的车辆运动性能数据；以及

监测来自于视觉子系统的照相机数据。

融合所述组质点基于与每个传感器输入的每个质点相关联的偏差。

附图说明

现在将通过示例的方式参考附图来描述一个或多个实施例，在附图中：

图1示出了根据本发明的使用传感器来获取车辆前面的道路几何形状数据的示例性车辆；

图2示出了根据本发明的示例性过程，藉此，传感器输入可以用于生成车道几何形状和相对于车道的车辆位置的估计值；

图3示出了根据本发明的示例性过程，其中，来自于地图数据库的信息可以用于建立车辆区域中的道路几何形状模型；

图4以图表形式示出了根据本发明的示例性迭代方法，以寻找相对于估计道路几何形状的车辆近似位置；

图5示出了根据本发明的示例性车辆姿态定位过程；

图6示出了根据本发明的在车辆的横向模型内作出的示例性确定；

图7示出了根据本发明的示例性质点组，包括用于导航车辆的一组融合质点；

图8示出了根据本发明的沿车辆前面的计划车道的质点的示例性使用，以估计车道几何形状；

图9示出了根据本发明的在使用雷达数据确定的公路中的示例性跟踪车辆轨道的使用；

图10是示出了根据本发明的用于确定选定跟踪车辆轨道的曲率的示例性控制方案的流程图；和

图11是根据本发明的用于使用雷达数据确定跟踪车辆轨道的流程图。

具体实施方式

现在参考附图，其中，附图显示仅仅是为了示出某些示例性实施例，而不是为了限制所述实施例，图1示出了根据本发明的使用传感器来获取车辆前面的道路几何形状数据的示例性车辆。示例性车辆包括旨在用于高速公路上的乘用车辆，但是应当理解的是，本文所述的本发明可应用于任何车辆或其它系统，以监测远程车辆和其它目标的位置和轨迹。车辆包括控制系统，所述控制系统包含在各种时刻执行的各种算法和标定值。控制系统优选为总体车辆控制结构的子组，所述总体车辆控制结构提供协调的车辆系统控制。控制系统监测来自于各个传感器的描述车辆操作环境的输入，综合相关信息和输入，并执行算法来控制各种致动器以实现控制目标，包括诸如避免碰撞和适应性巡航控制的参数。车辆控制结构包括多个分布式处理器和装置，包括提供诸如防抱死制动、牵引控制和车辆稳定性的功能的系统控制器。

在图1的示例性实施例中，车辆10被示出且包括视觉子系统16。视觉子系统16使用能够生成车辆前面的区域的数字图像表示的照相机或成像装置。来自于视觉子系统16的数据用于描述车辆前面的状况，且被转换成参考车辆10中心轴线的XY坐标系20。视觉子系统的示例性视场由虚线示出。道路上的行驶车道根据车道标记25A和25B示出，且共同特征可以被视觉检测且用于描述相对于车辆10的车道几何形状。以这种方式，通过本领域技术人员已知的方法，从图像或照相机数据分析获得的信息可以用作相对于车辆10向前行驶的状况。

每个处理器可采用任何合适的形式，包括一个或多个专用集成电路(ASIC)、电子电路、执行一个或多个软件或固件程序的中央处理单元(优选为微处理器)以及相关存储器和储存器(只读、可编程只读、随机存取、硬盘驱动器等)、组合逻辑电路、输入/输出电路和装置、合适的信号调节和缓冲电路、以及提供所述功能的其它合适部件的各种组合。控制模块具有一组处理算法，所述处理算法包括存储在存储器中且被执行提供期望功能的常驻软件程序指令和标定值。所述算法优选在预定循环期间被执行。所述算法例如由中央处理单元执行，并且可操作以监测来自感测装置和其它网络控制模块的输入以及执行控制和诊断程序从而控制致动器的操作。在持续进行的发动机和车辆操作期间，循环以规则间隔例如每3.125、6.25、12.5、25和100毫秒被执行。替代性地，算法可响应于事件发生而被执行。

由车辆10使用的传感器，例如视觉子系统16或其它雷达或测距装置，优选定位在车辆10内相对于车辆前面的视场的相对不受阻碍的位置。也应当理解的是，这些传感器中的每个提供道路的实际细节或车辆前面的道路上的目标的估计值。应当理解的是，这些估计值不是准确的位置，且每个估计值的偏差标准是可能的。还应当理解的是，这些传感器的特征是互补的，因为一些传感器比其它传感器在估计某些参数方面更可靠。常规传感器具有不同的操作范围和角视界，且能够估计在其操作范围内的不同参数。例如，雷达传感器通常可以估计目标的距离、距离变化率和方位位置，但是在估计检测目标的范围方面通常不稳固。具有视觉处理器的照相机在估计目标的形状和方位位置方面更稳固，但是在估计目标的距离和距离变化率方面效率较低。扫描式LIDAR关于估计距离和方位位置方面有效且准确地执行，但是通常不能估计距离变化率，因而关于新目标获取/识别方面不准确。超声传感器能够估计距离，但是通常不能估计或计算距离变化率和方位位置。描述车辆运动性能(例如速度和偏航速率)的传感器不准确，尤其是已知在跟踪车辆运动的较小变化方面不好。此外，应当理解的是，每个传感器技术的性能受到不同环境状况的影响。因而，常规传感器存在参数差异，其可操作叠加为传感器融合提供机会。

描述车辆前面的操作环境的传感器和子系统通常提供输出(即传感器数据)，包括距离R、基于时间的距离变化R_dot、和角度Θ(优选相对于车辆的纵轴线)，所述输出可写成测量矢量(°)。示例性短程雷达子系统具有160度的视场(FOV)和三十米的最大量程。示例性长程雷达子系统具有17度的视场和220米的最大量程。示例性前视子系统具有45度的视场和五十(50)米的最大量程。对于每个子系统，视场优选围绕车辆10的纵轴线取向。车辆优选相对于坐标系(称为XY坐标系20)取向，其中，车辆10的纵轴线建立X轴，中心位于便于车辆和信号处理的点处，Y轴通过在水平面内垂直于车辆10的纵轴线的轴线建立，因而平行于道路表面。

优选控制模块包括控制器，其中，算法和相关标定值被存储且配置成从可用传感器接收估计数据并将数据聚类为车辆前面的状况的有用估计，并将聚类的观测结果融合以确定所需车道几何形状和相对车辆位置估计。应当理解的是，使用不同传感系统和技术来融合数据得到稳固的结果。同样，应当理解的是，在该技术中可以使用任何数量的传感器。

给出了一种生成和保持系统内的道路和车道几何形状估计的方法，其中，历史测量值用于评估或预测随后的航迹数据。示例性系统基于在时间T时的函数来作出估计以描述时间T+1时的系统状态。通常，为了支持实时估计，使用表示高斯分布的信息阵列来估计未知误差的影响。这种系统允许收集和融合车辆前面的道路状况的估计值。

用以生成和保持车辆前面的状况的估计值的上述方法用于快速而有效地生成可用于需要近似车辆位置的方案中的估计值。然而，本领域技术人员应当理解的是，使用历史数据和高斯分布的估计值包括基于平均和正态分布假设的内在误差。例如，在车道几何形状估计操作中，在建立车辆经过的估计安全计划行驶路径时，车辆后面的直车道对车辆前面的道路的急弯没有实际减轻影响。关于车辆前面的车道的数据的散度不必通过借助于高斯分布应用随机矢量以求解散度而改进。使用历史平均和标准化或高斯分布的方法，例如依赖于卡尔曼滤波器的方法，通常包括导致对道路几何形状的变化或过渡的时间滞后的误差因子。

公开了一种方法，该方法通过融合来自于多个数据输入的当前测量值来生成车道几何形状和车辆位置(包括相对于车道的取向)的估计值，而不会招致基于历史数据或标准化分布的误差。示例性实施例包括GPS数据、视觉照相机子系统和车辆运动性能的输入。应当理解的是，车道几何形状和相对于车道几何形状的车辆位置可以基于同一组数据，或者可以使用不同数据源来确定车道几何形状和车辆位置。

总体道路几何形状是通过使用GPS装置和3D地图可容易获得的信息。给定来自于GPS装置的近似位置，局部化道路几何形状可以表示道路形状点列表。类似地，包括全球纬度测量值和全球经度测量值的GPS坐标可通过GPS装置获得。附加的地图实施例可包括配置成传输地图数据和道路几何形状和从基于地面的监测系统和其它车辆接收地图数据和道路几何形状的车辆对基础设施和车辆对车辆通信系统。至少包括车辆速度和偏航速率的车辆运动性能可通过监测车辆操作和/或监测加速计读数的传感器获得。照相机数据可用于将车辆定位到实际行驶车道。通过照相机数据限定车道传感系数(即，y＝a+bx+cx²，其中，x是车道纵向偏移，且y是距车道中心的横向偏移)。通过该数据，前向车道估计模块可估计车道的曲率、距车道中心的横向偏移、和相对于车道切线的车辆取向。

使用传感器数据的方法可以用于确定道路几何形状和相对于道路几何形状的车辆位置。基于这些被确定的值，可以生成车辆的导航指令，以控制道路表面上的车辆。这种导航指令可包括多个示例性实施例。图2示出了根据本发明的示例性过程，藉此，传感器输入可以用于生成车道几何形状和相对于车道的车辆位置的估计值。示例性过程包括地图几何形状模型模块202、车辆姿态定位模块204、曲率估计模块206和车辆横向跟踪模块208。地图几何形状模型模块202输入地图路点(由本领域已知的方法确定)，包括确定从起始或当前点到目的地或者通过地图数据库中的点的总体路径；并输出车辆区域中的车道几何形状。该车道几何形状可以描述为包括该区域中的道路的几何形状表示的弧线。车辆姿态定位模块204输入来自于地图几何形状模型模块的车道几何形状、来自于GPS装置的GPS坐标、和来自于视觉子系统的照相机数据，并输出相对于车辆区域中的车道几何形状的估计车辆位置。相对于车道几何形状或弧线的该车辆位置可以描述为弧长参数(s_m)。曲率估计模块206输入照相机数据、来自于车辆传感器的车辆运动性能数据(例如车辆速度和偏航速率)以及s_m，并输出曲率(K)或在车辆位置处的道路曲线度量。最后，车辆横向跟踪模块208输入照相机数据、车辆运动性能数据和K，并输出关于相对于当前车道中心的车辆位置和相对于车道当前前进方向的车辆角度取向的数据。以这种方式，关于当前位置和车辆行驶的当前输入可以用于生成与车辆区域中的车道几何形状以及相对于车道的车辆位置和取向有关的数据。

如上所述，地图几何形状模型模块输入地图路点并输出车辆区域中的车道几何形状。具体地，地图几何形状模型监测地图数据库中所述的地图形状点的输入，并建立表示形状点的几何形状模型。图3示出了根据本发明的示例性过程，其中，来自于地图数据库的信息可以用于建立车辆区域中的道路几何形状模型。示例性过程包括收集来自于地图数据库的描述道路几何形状的地图形状点(302)。地图数据库以全球坐标的形式提供地图形状点，通常依照纬度位置、经度位置和高度或海拔来描述位置。全球坐标然后转换为局部坐标系，通常识别邻近车辆位置的点作为静态参考点并将任何其它位置描述为距参考点的南北位移(即，纬度位移)和距参考点的东西位移(即，经度位移)(304)。接下来，使用样条来拟合地图形状点，以生成几何形状或近似所表示的道路几何形状的弧线(306)。最后，在车辆的估计位置确定拟合样条的切线和曲率(308和310)。

描述了地图几何形状模型内的示例性确定。假定{(lat_i，lon_i)|i＝1，…，N}为形状点。选出一个点作为参考点，可以将形状点转换为局部坐标{(e_i，n_i)|i＝1，…，N}，表示距参考点的东西位移和南北位移。定义序列{(s_i，e_i，n_i)|i＝1，…，N}，其中，s₁＝0，$s_i={\Sigma }_{k=2}^i\sqrt{e_k^2+n_k^2}i\ge 2,$获得二维三次样条函数以拟合形状点，如下所述：

$\left(\begin{array}{c}e\\ n\end{array}\right)=f\left(s\right)---\left[1\right]$

其中，s是弧长参数，e和n分别是位移的东西和南北分量。于是，在s时的梯度矢量计算如下：

$\left(\begin{array}{c}{e}^t\\ n^t\end{array}\right)=f^t\left(s\right)---\left[2\right]$

且，取向角度ξ计算如下：ξ＝atan2(n^t，e^t)        [3]

最后，在s时的曲率κ可以计算如下：

$\kappa =\frac{e^t{n}^{\mathrm{tt}}-n^t{e}^{\mathrm{tt}}}{{(e^{t2}+n^{t2})}^{3/2}}---\left[4\right]$

其中$\left(\begin{array}{c}{e}^{\mathrm{tt}}\\ n^{\mathrm{tt}}\end{array}\right)=f^{\mathrm{tt}}\left(s\right).$

如上所述，车辆姿态定位模块204输入来自于地图几何形状模型模块202的车道几何形状、来自于GPS装置的GPS坐标和照相机数据，并输出相对于车辆区域中的车道几何形状的估计车辆位置。本领域技术人员将理解，一个问题可以描述地图到监测GPS数据的定位。地图几何形状由样条函数表示，例如方程1所述的函数。该样条描述离散位置，其中，假定存在道路车道。GPS数据测量的点以示例性形式$P=\left(\begin{array}{c}x\\ y\end{array}\right)$返回。在GPS装置中一些偏差的不准确性和不精确性是正常的。在样条函数中误差也是固有的。P与地图几何形状样条相对精确地相符。样条函数描述车道中的点(例如车道中心)，且实际车辆位置将通常以可测量的量偏离车道中心。车辆在地图上的近似位置必须基于P和区域中的估计道路几何形状来确定。一种校正P和道路几何形状表示之间的偏差的示例性方案在于寻找最接近的[e_m，n_m]^T＝f(s_m)，使得$s_m=\underset{s}{\arg \min }\left|\right|P-f\left(s\right)\left|\right|.$该示例性过程用于近似s_m且可迭代地应用以寻找道路曲线中的车辆位置并在监测数据变化时改进估计位置。

图4以图表形式示出了根据本发明的示例性迭代方法，以寻找车辆相对于估计道路几何形状的近似位置。假定s₀为s_m的初始猜测值。弧长参数的校正可以写成如下：

$\mathrm{\Delta s}=\frac{{(P-P_m)}^T{P}_m^t}{\left|\right|P_m^t\left|\right|}---\left[5\right]$

其中P_m＝f(s₀)，$P_m^t=f^t\left(s_0\right).$换句话说，校正值Δs为猜测位置s₀处的梯度的单位矢量上的投影。

本领域技术人员将理解，GPS测量值通常不如车辆传感器读数那么频繁地更新。对于大多数车载GPS接收器而言，1Hz的示例性更新速率是常见的。此外，更新不总是被接收且可能在卫星信号视场被阻碍的城市区域或其它区域中有噪音。可以使用滤波技术来补偿GPS信号更新的缓慢速率。

示例性车辆姿态定位模块使用卡尔曼滤波器。车辆姿态建模为矢量且由东西位移(e)、南北位移(n)、相对于车道的取向(Φ)、和弧长(s)构成。由于惯性，车辆姿态并不突然变化。因而，假设以下恒定转向模型：

e^t＝e+vcos(φ+ξ)ΔT+w₁

n^t＝n+vsin(φ+ξ)ΔT+w₂

φ^t＝φ+ωΔT＝κΔT+w₃

s^t＝s+vΔT                            [6]

其中，v是车辆速度；ω是车辆偏航速率；ΔT是从先前循环的时间差；ξ是道路的当前取向(参见(2))；κ是基于地图曲线的道路当前曲率；w₁，w₂和w₃是表示未建模扰动的过程噪音项。

图5示出了根据本发明的车辆姿态定位模块204的示例性控制方案。控制方案在图5中示出且在本文描述为包括分立元件。这种图示是为了便于描述，应当认识到，由这些元件执行的功能可组合在一个或多个装置中，例如在软件、硬件和/或专用集成电路中实施。如上所述，车辆姿态定位模块204输入来自于地图几何形状模型模块202的车道几何形状、来自于GPS装置的GPS坐标和来自于视觉子系统的照相机数据，并输出相对于车辆区域中的车道几何形状的估计车辆位置。

只要分别在步骤502、542和582监测来自于GPS、车辆运动性能传感器或照相机装置的新数据，就重复地更新车辆位置。在监测新数据之后，计算时间差ΔT，其描述在步骤504、544和584车辆位置更新之间的时间变化。时间差ΔT从当前和先前循环之间的时标的差异进行计算。不同数据源的示例性循环时间包括对于GPS数据的1秒，对于运动性能数据的20ms，对于照相机数据的50ms。然后，使用方程5来计算预测车辆姿态。当GPS数据可用时，车辆位置使用卡尔曼滤波器更新(508)。车辆姿态的测量值更新使用以下GPS测量方程来确定：

e_gps＝e+k₁            [7]

n_gps＝n+k₂            [8]

其中，(e_gps，n_gps)是车辆的GPS测量位置；k₁和k₂是测量噪音。在使用GPS测量值更新车辆姿态之后，使用方程5来计算正确的弧长参数(s)。该步骤对于通过去除由方程6中的推算导航过程引起的累计误差来获得正确的K和ξ值是重要的。

当新的运动性能数据可用时，预测车辆位置可使用多种方法中的一种来确定(546)。第一方法包括基于时间差和车辆轨迹来插值未来位置。用于预测车辆位置的第二方法包括分析近似地图点和车辆位置。假定P_m为地图曲线上距当前车辆位置的距离最近的点，表示为P＝(e，n)。假定矢量m表示在P_m处地图曲线的法线。那么，垂直距离d可以表示为d＝(P-P_m)^Tm，其中，法线m计算为：

$m=\left(\begin{array}{cc}0& -1\\ 1& 0\end{array}\right)\left(\begin{array}{c}{e}^t\\ n^t\end{array}\right).$

车辆位置然后使用卡尔曼滤波器更新(548)。

当新的照相机数据可用时，使用卡尔曼滤波器确定预测车辆位置(586)且更新车辆位置(588)。预测车辆位置使用以下测量方程和卡尔曼滤波器确定：

a＝d+k₃                     [9]

b＝φ+k₄                    [10]

其中，a和b是照相机车道传感参数；d是当前车辆位置距行驶车道中心的垂直距离；φ表示相对于行驶车道的车辆取向；k₃和k₄是未建模测量噪音。

如上所述，曲率估计模块206输入照相机数据、来自于车辆传感器的车辆运动性能数据(例如车辆速度和偏航速率)以及弧长参数s_m，并输出描述道路的曲线度量的融合曲率(K_fus)。一旦车辆定位在由s表示的地图曲线中，那么可以通过方程4寻找相应的地图曲率K_map。

应当注意，存在三个信息源来估计道路曲率：地图曲率(K_map)、照相机曲率(K_cam＝2c)、基于偏航速率的曲率(K_yaw＝ω/v)。所使用的来自于一个信息源的曲率数据可以用于估计车辆的计划行驶路径的质点。例如，地图曲率数据可以用于估计车辆的第一计划行驶路径。每个独立信息源(例如，照相机曲率和基于偏航速率的曲率)可以被独立地使用以估计车辆的附加计划行驶路径。计划行驶路径中的质点可以融合在一起以生成用于车辆导航的单个计划行驶路径。

设想用于融合质点的多种方法。第一方法包括使用最小二乘估计分析来融合质点。在本文描述一种示例性方法以将上述曲率融合在一起。假定k_fus表示融合曲率，其中偏差为σ_fus²。假定σ_map²，σ_yaw²和σ_cam²分别表示地图曲率、基于偏航速率的曲率和照相机曲率的偏差。我们得出以下更新方程。

当地图曲率估计值可用时，那么

${\kappa }_{\mathrm{fus}}=\frac{{\sigma }_{\mathrm{map}}^2{\kappa }_{\mathrm{fus}}+{\sigma }_{\mathrm{fus}}^2{\kappa }_{\mathrm{map}}}{{\sigma }_{\mathrm{map}}^2+{\sigma }_{\mathrm{fus}}^2}---\left[11\right]$

且

${\kappa }_{\mathrm{fus}}=\frac{{\sigma }_{\mathrm{map}}^2{\sigma }_{\mathrm{fus}}^2}{{\sigma }_{\mathrm{map}}^2+{\sigma }_{\mathrm{fus}}^2}---\left[12\right]$

当偏航速率曲率估计值可用时，那么

${\kappa }_{\mathrm{fus}}=\frac{{\sigma }_{\mathrm{yaw}}^2{\kappa }_{\mathrm{fus}}+{\sigma }_{\mathrm{fus}}^2{\kappa }_{\mathrm{yaw}}}{{\sigma }_{\mathrm{yaw}}^2+{\sigma }_{\mathrm{fus}}^2}---\left[13\right]$

且

${\kappa }_{\mathrm{fus}}=\frac{{\sigma }_{\mathrm{yaw}}^2{\sigma }_{\mathrm{fus}}^2}{{\sigma }_{\mathrm{yaw}}^2+{\sigma }_{\mathrm{fus}}^2}---\left[14\right]$

当照相机曲率估计值可用时，那么

${\kappa }_{\mathrm{fus}}=\frac{{\sigma }_{\mathrm{cam}}^2{\kappa }_{\mathrm{fus}}+{\sigma }_{\mathrm{fus}}^2{\kappa }_{\mathrm{cam}}}{{\sigma }_{\mathrm{cam}}^2+{\sigma }_{\mathrm{fus}}^2}---\left[15\right]$

且

${\kappa }_{\mathrm{fus}}=\frac{{\sigma }_{\mathrm{cam}}^2{\sigma }_{\mathrm{fus}}^2}{{\sigma }_{\mathrm{cam}}^2+{\sigma }_{\mathrm{fus}}^2}---\left[16\right]$

在雷达曲率估计值σ_radar²可用的附加实施例中，融合曲率可通过以下方程确定：

${\kappa }_{\mathrm{fus}}=\frac{{\sigma }_{\mathrm{radar}}^2{\kappa }_{\mathrm{fus}}+{\sigma }_{\mathrm{fus}}^2{\kappa }_{\mathrm{radar}}}{{\sigma }_{\mathrm{radar}}^2+{\sigma }_{\mathrm{fus}}^2}---\left[17\right]$

且具有偏差σ_fus²的融合曲率可使用以下方程确定：

${\kappa }_{\mathrm{fus}}=\frac{{\sigma }_{\mathrm{radar}}^2{\sigma }_{\mathrm{fus}}^2}{{\sigma }_{\mathrm{radar}}^2+{\sigma }_{\mathrm{fus}}^2}---\left[18\right]$

在上述方程中，偏差σ_map²，σ_yaw²和σ_cam²分别表示来自于不同源(地图、车辆传感器和照相机)的曲率信息的置信度。信息源的偏差越高，该源对融合曲率的贡献越少。可以采用一些启发式规则来选择三个源的不同权重。例如，当偏航速率高时，我们选择小σ_yaw²来推导融合曲率。以这种方式，可以生成用于车辆导航的融合质点。

相对于道路几何形状的车辆位置可以被改进，以包括在行驶车道内的车辆取向和横向位置，以便控制在道路表面上的车辆。如上所述，车辆横向跟踪模块输入照相机数据、车辆运动性能数据和K，并输出关于相对于当前车道中心的车辆位置和相对于车道当前前进方向的车辆角度取向的数据。图6示出了根据本发明的在车辆的横向模型内作出的示例性确定。车辆横向跟踪模块监测车辆运动性能的输入(车轮速度v和偏航速率ω)和车道传感参数的输入。可以使用卡尔曼滤波器来整合来自于车辆运动性能和车道传感装置的数据。如图6所示，横向偏移y_L是距车道中心的位移。K_road是道路的估计曲率。K_yaw是由瞬时车辆路径估计的曲率，即K_yaw＝ω/v。卡尔曼滤波器的测量方程表示为：b＝Φ，a＝y_L。如果更新误差大于阈值，那么实施门逻辑。换句话说，如果预测和实际测量值之间的差大于阈值，那么在当前时间瞬时忽略实际测量值。

上述方法可以用于描述道路几何形状和相对于道路几何形状的车辆位置。这种导航指令可以包括多个示例性实施例。图7示出了根据本发明的示例性质点组，包括用于导航车辆的一组融合质点。每组质点表示由特定方法确定的车辆可能行驶路径，在该示例中，包括车辆运动性能、照相机数据和地图数据。环绕图7所示的每个质点的圆表示与每种方法相关联的质点的行驶路径上的定位偏差。所述圆表示质点的估计偏差。第一组质点基于车辆运动性能确定且在该图中由三角形图标表示。第二组质点基于来自于视觉子系统的照相机数据确定且在该图中由方形图标表示。第三组质点基于来自于地图数据库的地图数据确定且在该图中由菱形图标表示。该组融合质点基于第一、第二和第三组质点使用上文所述或者本领域已知的方法确定，包括根据其偏差修正每个质点的影响。应当理解的是，该组融合质点表示车辆的计划行驶路径。

图8示出了根据本发明的沿车辆前面的计划车道的质点的示例性使用，以估计车道几何形状。在连续时间间隔时的质点的重复生成(质点隔开短的距离累计量)可以用于增强车辆前面的估计车道几何形状。质点的多次重复生成增加了从多个可能行驶路径生成的计划行驶路径的置信度，这通过包括基于使用多个车辆视角生成的质点、通过增强多次计算并通过允许滤波质点失常来实现。当车辆经过质点时，所述质点可以被废弃且仅仅使用与车辆未经过的计划行驶路线相对应的质点。以这种方式，可以使用沿估计路径的车辆前面的质点投影来估计车辆可能行驶通过的车道几何形状。

应当理解的是，如参考图7和8所述，车辆的可能行驶路径包括描述车辆行驶的可能安全通路的多个质点。可能行驶路径可以组合或融合在多个不同组合中的一种以确定车辆的计划行驶路径。在一个实施例中，可能行驶路径可使用权重组合以确定车辆的计划行驶路径。例如，在预定情况下，使用全球定位/数字地图数据确定的车辆的可能行驶路径可给予比使用车辆运动性能确定的可能行驶路径更大的权重。

上述方法使用车道几何形状确定和车辆位置确定的示例性方法，包括基于车辆运动性能、照相机或视觉系统数据和全球定位/数字地图数据的传感器输入。在附加实施例中，雷达数据可与上述传感器输入组合或者替代地使用。

图9示出了根据本发明的在使用雷达数据确定的公路中的示例性跟踪车辆轨道的使用。跟踪车辆轨道使用上文所述的雷达曲率估计值来确定，且可使用下文所述的示例性控制方案来确定。图9示出了主车辆900以及第一车辆905和第二车辆906。在第一车辆905和第二车辆906之后示出了估计跟踪车辆轨道A和B。

图10是示出了根据本发明的用于确定选定跟踪车辆轨道的曲率的示例性控制方案600的流程图。控制方案600在图10中示出且在本文描述为包括分立元件。这种图示是为了便于描述且应当认识到，由这些元件执行的功能可组合在一个或多个装置中，例如在软件、硬件和/或专用集成电路中实施。在由主车辆获得新的雷达数据(602)时，使用控制方案600。新的雷达数据用于更新每个跟踪前进车辆轨道的车辆轨道(604)，如下文参考图11所述。使用每个跟踪车辆轨道来估计车道几何形状曲线拟合(606)。

假定跟踪车辆轨道由N个点{(x₁，y₁)，(x₂，y₂)，…，(x_n，y_n)}表示，可以使用抛物线模型(例如ax²+bx+c＝y)来拟合所述N个点，可以通过使得过约束线性方程Aα＝b的残差最小化来导出，其中：

$A=\left(\begin{array}{ccc}{x}_1^2& x_1& 1\\ x_2^2& x_2& 1\\ ·& ·& ·\\ ·& ·& ·\\ ·& ·& ·\\ x_N^2& x_N& 1\end{array}\right),$$b=\left(\begin{array}{c}{y}_1\\ y_2\\ ·\\ ·\\ ·\\ y_N\end{array}\right)$且$\alpha =\left(\begin{array}{c}a\\ b\\ c\end{array}\right)---\left[19\right]$

求解为α＝(A^TA)^-1A^Tb，其中，跟踪车辆轨道的估计曲率为2a。

在估计车道几何形状之后，每个跟踪车辆轨道与预定阈值进行比较(608)。基于一致相关的车道曲率选择大于预定阈值的预定数量或百分比的跟踪车辆轨道(例如70％)(610和612)。如果预定百分比的跟踪车辆轨道可用，那么控制方案600以高的置信度输出所有跟踪车辆轨道的取样平均值曲率(614)。如果预定百分比的跟踪车辆轨道不可用，那么控制方案600以低的置信度输出所有跟踪车辆轨道的取样平均值曲率(616)。以这种方式，一组跟踪车辆轨道可以被分析且用于生成曲率，以在本文所述的方法中使用。

图11是根据本发明的用于使用雷达数据确定跟踪车辆轨道的流程图。跟踪车辆轨道可在步骤604使用上文所述的示例性控制方案600确定。步骤604在图11中示出，且在本文描述为包括分立元件。这种图示是为了便于描述，应当认识到，由这些元件执行的功能可组合在一个或多个装置中，例如在软件、硬件和/或专用集成电路中实施。车辆雷达装置的视场中的目标被识别和收集，且车辆位置和取向使用雷达数据进行估计(702)。每个识别目标被分配至匹配的车辆轨道(704)。如果没有车辆轨道匹配，可生成新的车辆轨道。识别的目标位置转换为全球坐标系(例如全球位置系统坐标)，且添加到跟踪车辆轨道(706)。从航迹去除没有识别目标的跟踪车辆轨道(708)。

上文所述的方法使用车道几何形状确定和车辆位置确定的示例性方法，包括基于车辆运动性能、照相机或视觉系统数据和全球定位/数字地图数据的传感器输入。然而应当理解的是，可以使用传感器输入的不同或附加组合，例如，附加地或替代地包括雷达数据、LIDAR数据、超声数据、车辆对车辆通信、或车辆对基础设施通信。车辆对车辆通信可以用于描述诸如另一车辆相对于主车辆的当前位置或者另一车辆在公路上行驶的路径的信息。车辆对基础设施通信可以用于传输诸如引导交通图案、道路几何形状信息和局部化偏移信息的信息，从而改进全球定位数据对道路几何形状的准确性。这种组合可以基于多个因素进行选择，包括环境状况(例如，天气或光线水平)和针对当前状况优化的传感器输入。附加地或替代地，传感器输入可以基于在例如上述偏差中体现的确定步骤中所确定的置信度来选择或者运动增加。

关于车道几何形状和相对于车道的车辆位置和取向的实时和可靠信息可以用于多种应用或车辆控制方案。如上所述，所述信息可以用于车辆在公路上的导航控制。在其它示例中，这种信息可以用于车辆操作应用，包括车道保持、前灯调整、改进导航辅助、和雾天警示。然而，本领域技术人员将理解，大量的应用可以使用这种信息，且本发明不旨在限于本文所述的特定实施例。

本发明已经描述了某些优选实施例及其变型。在阅读和理解说明书之后，本领域技术人员可想到其它的变型和修改。因而，本发明不旨在限于作为用于实施本发明的最佳模式公开的特定实施例，而本发明将包括落入所附权利要求范围内的所有实施例。