防撞车辆中的传感器测距和应用

摘要

一种用于使用目标检测数据和车辆动力学数据确定车辆的实际轨迹的方法。目标检测系统识别靠近车辆的点目标和扩展目标，其中点目标在长度和宽度上小于一米。计算更新的车辆姿态，其将在扫描数据中的点目标优化地变换为在先识别的点目标的对象清单。通过迭代地计算姿态进一步改善更新的车辆姿态，其将在扫描数据中的扩展目标优化地变换为在先识别的扩展目标的对象模型，其中迭代被用于同时确定使扫描数据与对象模型相关的概率系数。更新的车辆姿态用于识别车辆的实际轨迹，其与在防撞系统中的规划的路径相比。

说明书

技术领域

本发明总体涉及汽车的增强的轨迹追踪，并且更具体地，涉及一种用于相对于周围静态障碍物和移动对象的相对定位主车辆的方法，其使用目标检测数据和车辆动力学数据以计算主车辆的闭环轨迹，以及在路径规划中使用所述轨迹以便防撞或其他目的。

背景技术

现在很多车辆为了诸如防撞的应用包括可积极控制车辆的路径的系统。这些系统在它们检测到需要改变车辆的路径时将都实时地确定希望的新的路径以便车辆跟随、实施制动和/或转向指令以获得希望的路径、监测车辆相对于希望的路径的实际轨迹、和对跟随希望的路径做出进一步校正。

在这些系统中，必要的是，监测车辆的实际轨迹的可靠的方法是可用的。装备有这些系统的很多车辆使用车辆动力学传感器--包括速度、横摆率和加速度传感器作为计算实际车辆轨迹的一种手段。然而，车辆动力学或“航位推算法”计算易受积累误差。因此，需要车辆轨迹数据的另一来源，优选地不像车辆动力学数据那样积累误差的来源。在车辆路径规划系统中，通常使用全球定位系统（GPS）作为车辆轨迹数据的第二来源。然而，一些车辆没有装备GPS接收器，并且甚至装备有GPS接收器的车辆将有时候没有可用的信号，诸如在隧道、“城市隧道”和具有高大和密集的周围树木覆盖的道路中。

发明内容

根据本发明的教导，公开一种用于使用目标检测数据和车辆动力学数据确定车辆的实际轨迹的方法。目标检测系统识别靠近车辆的点目标和扩展目标，其中点目标在长度和宽度上小于一米。计算更新的车辆姿态，其将在扫描数据中的点目标优化地变换为在先识别的点目标的对象清单。通过迭代地计算姿态进一步改善更新的车辆姿态，其将在扫描数据中的扩展目标优化地变换为在先识别的扩展目标的对象模型，其中迭代被用于同时确定使扫描数据与对象模型相关的概率系数。更新的车辆姿态用于识别车辆的实际轨迹，其与在防撞系统中的规划路径相比较。

本发明还提供如下方案：

1.一种用于基于目标扫描数据估计车辆姿态的方法，所述方法包括：

获得用于靠近主车辆的目标的扫描数据；

提供来自所述主车辆上的车辆动力学系统的主车辆运动数据；

识别在所述扫描数据中的点目标和扩展目标，其中所述点目标具有小于预定临界值的长度和宽度，和所述扩展目标具有等于或超过所述临界值的长度或宽度；

使用微处理器基于所述主车辆运动数据和在所述扫描数据中识别的所述点目标计算所述车辆姿态的第一估计；

使用所述微处理器基于所述车辆姿态的所述第一估计和在所述扫描数据中识别的所述扩展目标计算所述车辆姿态的第二估计；以及

基于所述车辆姿态的所述第一估计和所述车辆姿态的所述第二估计确定所述车辆姿态的总体估计。

2.如方案1所述的方法，其特征在于，所述车辆姿态包括相对于在道路上的局部世界坐标系的二维位置，和相对于所述局部世界坐标系的驶向角。

3.如方案1所述的方法，其特征在于，获得扫描数据包括从合成孔径雷达目标检测系统获得扫描数据。

4.如方案1所述的方法，其特征在于，计算所述车辆姿态的第一估计包括将在所述扫描数据中的所述点目标与在先识别的点目标的对象清单比较，以及使用最小二乘计算以寻找所述在先识别的点目标的变换，其最小化在所述扫描数据中的所述点目标和所述在先识别的点目标之间的差。

5.如方案4所述的方法，其特征在于，在先识别的点目标的所述对象清单通过使命中计数增量和变换在所述扫描数据中存在匹配的在所述对象清单中的点目标的位置来更新。

6.如方案1所述的方法，其特征在于，计算所述车辆姿态的第二估计包括将在所述扫描数据中的所述扩展目标与在先识别的扩展目标的对象模型比较，以及使用迭代的最小二乘计算以同时寻找概率系数和所述在先识别的扩展目标的变换，其最小化在所述扫描数据中的所述扩展目标和所述在先识别的扩展目标之间的差，其中概率系数表示在所述扫描数据中的所述扩展目标中的具体一个与所述在先识别的扩展目标中的具体一个相匹配的概率。

7.如方案6所述的方法，其特征在于，将在先识别的扩展目标的对象模型模拟为高斯混合模型分布，并且所述对象模型通过使命中计数增量和变换在所述扫描数据中存在匹配的在所述对象模型中的扩展目标的位置来更新。

8.如方案1所述的方法，其特征在于，确定所述车辆姿态的总体估计包括在所述扫描数据包括占主导的点目标时使用所述车辆姿态的所述第一估计作为所述总体估计，以及在所述扫描数据包括占主导的扩展目标时使用所述车辆姿态的所述第二估计作为所述总体估计。

9.如方案1所述的方法，其特征在于，确定所述车辆姿态的总体估计包括执行同时优化所述车辆姿态的所述第一估计和所述车辆姿态的所述第二估计的融合计算。

10.如方案1所述的方法，其特征在于，还包括使用所述车辆姿态的所述总体估计以便确定在车辆上的防撞系统中的实际车辆轨迹。

11.一种用于使用车辆动力学和目标检测数据的车辆路径规划和具有闭环控制的轨迹跟踪的方法，所述方法包括：

获得用于靠近主车辆的目标的扫描数据；

提供来自所述主车辆上的车辆动力学系统的主车辆运动数据；

忽视在所述扫描数据中的移动的目标，其中所述移动目标通过与主车辆运动数据相比较来确定；

识别在所述扫描数据中的点目标和扩展目标，其中所述点目标具有小于预定临界值的长度和宽度，和扩展目标具有等于或超过所述临界值的长度或宽度；

使用微处理器基于所述主车辆运动数据和在所述扫描数据中被识别的所述点目标计算所述车辆姿态的第一估计，其中所述车辆姿态包括相对于在道路上的局部世界坐标系的二维位置，和相对于所述局部世界坐标系的驶向角；

使用所述微处理器基于所述车辆姿态的所述第一估计和在所述扫描数据中被识别的所述扩展目标计算所述车辆姿态的第二估计；

基于所述车辆姿态的所述第一估计和所述车辆姿态的所述第二估计确定所述车辆姿态的总体估计；以及

使用所述车辆姿态的所述总体估计以确定由车辆跟随的实际轨迹，其中所述实际轨迹和规划的路径并入车辆上的防撞系统中。

12.如方案11所述的方法，其特征在于，计算所述车辆姿态的第一估计包括将在所述扫描数据中的所述点目标与在先识别的点目标的对象清单比较，以及使用最小二乘计算以寻找所述在先识别的点目标的变换，其最小化在所述扫描数据中的所述点目标和所述在先识别的点目标之间的差，和其中计算所述车辆姿态的第二估计包括将在所述扫描数据中的所述扩展目标与在先识别的扩展目标的对象模型比较，以及使用迭代的最小二乘计算以同时寻找概率系数和所述在先识别的扩展目标的变换，其最小化在所述扫描数据中的所述扩展目标和所述在先识别的扩展目标之间的差，其中概率系数表示在所述扫描数据中的所述扩展目标中的具体一个与所述在先识别的扩展目标中的具体一个相匹配的概率。

13.如方案12所述的方法，其特征在于，确定所述车辆姿态的总体估计包括执行同时优化所述车辆姿态的所述第一估计和所述车辆姿态的所述第二估计的融合计算。

14.一种车辆姿态估计系统，所述系统包括：

目标检测模块，所述目标检测模块获得用于靠近主车辆的目标的扫描数据；

车辆动力学模块，所述车辆动力学模块提供主车辆运动数据，其包括纵向速度和横向速度、纵向加速度和横向加速度和横摆率，和

与所述目标检测模块和所述车辆动力学模块通信的车辆姿态估计模块，所述车辆姿态估计包括存储模块和处理器，其构造成：

识别在所述扫描数据中的点目标和扩展目标，其中所述点目标具有小于预定临界值的长度和宽度，和扩展目标具有等于或超过所述临界值的长度或宽度；

基于所述主车辆运动数据和在所述扫描数据中被识别的所述点目标计算所述车辆姿态的第一估计，其中所述车辆姿态包括相对于在道路上的局部世界坐标系的二维位置，和相对于所述局部世界坐标系的驶向角；

基于所述车辆姿态的所述第一估计和在所述扫描数据中被识别的所述扩展目标计算所述车辆姿态的第二估计；以及

基于所述车辆姿态的所述第一估计和所述车辆姿态的所述第二估计确定所述车辆姿态的总体估计。

15.如方案14所述的方法，其特征在于，所述车辆姿态估计模块通过以下计算所述车辆姿态的第一估计，将在所述扫描数据中的所述点目标与在先识别的点目标的对象清单比较，以及使用最小二乘计算以寻找所述在先识别的点目标的变换，其最小化在所述扫描数据中的所述点目标和所述在先识别的点目标之间的差异。

16.如方案15所述的方法，其特征在于，在先识别的点目标的所述对象清单通过使命中计数增量和变换在所述扫描数据中存在匹配的在所述对象清单中的点目标的位置来更新。

17.如方案14所述的方法，其特征在于，所述车辆姿态估计模块通过以下计算所述车辆姿态的第二估计，将在所述扫描数据中的所述扩展目标与在先识别的扩展目标的对象模型比较，以及使用迭代的最小二乘计算以同时寻找概率系数和所述在先识别的扩展目标的变换，其最小化在所述扫描数据中的所述扩展目标和所述在先识别的扩展目标之间的差异，其中概率系数表示在所述扫描数据中的所述扩展目标中的具体一个与所述在先识别的扩展目标中的具体一个相匹配的概率。

18.如方案17所述的方法，其特征在于，将在先识别的扩展目标的对象模型模拟为高斯混合模型分布，并且所述对象模型通过使命中计数增量和变换在所述扫描数据中存在匹配的在所述对象模型中的扩展目标的位置来更新。

19.如方案14所述的方法，其特征在于，通过在所述扫描数据包括占主导的点目标时使用所述车辆姿态的所述第一估计作为所述总体估计，以及在所述扫描数据包括占主导的扩展目标时使用所述车辆姿态的所述第二估计作为所述总体估计，所述车辆姿态估计模块确定所述车辆姿态的总体估计。

20.如方案14所述的方法，其特征在于，通过执行同时优化所述车辆姿态的所述第一估计作和所述车辆姿态的所述第二估计的融合计算，所述车辆姿态估计模块确定所述车辆姿态的总体估计。

本发明的额外的特征将从下面的描述和随附的权利要求并结合附图变得清楚。

附图说明

图1是车辆的示意图，具有使用车辆动力学和目标检测数据作为输入的路径规划和闭环轨迹控制；

图2是车辆路径规划系统的框图，具有使用车辆动力学和目标检测数据作为输入的闭环轨迹控制；

图3是车辆确定其相对于包括点目标和扩展目标的被检测目标的说明；和

图4是使用车辆动力学和目标检测数据作为输入的、用于车辆路径规划和具有闭环控制的轨迹跟踪的方法的流程图。

具体实施方式

本发明的涉及用于使用目标检测数据和车辆动力学数据确定车辆的实际轨迹的方法的实施例的下面描述在本质上仅仅是示例性的，并且决不意于限制本发明或其应用或用途。

一些车辆现装备有防撞系统，其在车辆碰撞呈现为即将来临的情况下可控制车辆转向和制动。防撞系统确定用于车辆采取的希望的路径以便避免碰撞、测量实际的车辆轨迹和实时地做出转向/制动校正。在这些系统中使用车辆动力学数据和GPS数据的组合典型地确定实际的车辆轨迹。然而，在没有装备GPS接收器的车辆中，和在GPS数据由于天空能见度障碍而不可用的情况下，需要车辆位置数据的另一个来源。

图1是车辆10的示意图，其具有使用车辆动力学和目标检测数据作为输入的路径规划和闭环轨迹控制。车辆10包括车辆动力学模块20，其使用来自多个传感器22的输入基于车辆动力学计算来估计车辆运动。传感器22（示出一个）可包括车辆速度传感器、横摆率传感器、横向和纵向加速度传感器和可能其他的传感器。车辆10还包括目标范围数据模块30，其提供来自一个或多个目标检测传感器32的目标范围数据。

目标检测传感器32可使用任何合适的目标检测技术，包括近程雷达、远程雷达、LIDAR和/或相机图像。特别地，合成孔径雷达系统现在正成为可使用的，其中雷达天线是薄片，其可放置在车身面板或保险杠罩上或内侧，因此使得基于雷达的目标检测更有成本效益。使用目标检测传感器32的任何希望组合，目标范围数据模块30提供在时间间隔诸如100毫秒（ms）被更新的目标范围数据。车辆10还包括下面进一步讨论的车辆姿态估计模块40、路径规划模块50和车辆控制模块60。

图2是车辆路径规划系统70的框图，其具有使用车辆动力学和目标检测数据作为输入的闭环轨迹控制。图2的路径规划系统70示出模块20-60是怎样相互关联的。如上面论述的，车辆动力学模块20将车辆运动数据提供到车辆姿态估计模块40，同时目标范围数据模块30将目标范围数据提供到车辆姿态估计模块40。车辆姿态估计模块40使用车辆运动数据和目标范围数据，以在每个时间步估计更新的车辆姿态，或位置和定向。路径规划模块50提供希望的路径以便车辆10跟随，例如以便防撞。来自车辆姿态估计模块40的实际的车辆轨迹数据和来自路径规划模块50的希望的车辆路径数据提供到求和结点55，其中差被提供到车辆控制模块60，其对转向和制动指令做出进一步调整以便最小化实际的车辆轨迹和希望的车辆路径之间的差异。

应该理解，车辆动力学模块20、目标范围数据模块30、车辆姿态估计模块40、路径规划模块50和车辆控制模块60由至少处理器和存储模块组成，其中处理器构造有软件，其设计成基于描述的输入计算希望的和实际的车辆位置和发布车辆控制指令。特别地，下面将详细描述在车辆姿态估计模块40中使用的逻辑和计算。

还应该理解，车辆姿态估计模块40、路径规划模块50和车辆控制模块60的特征和计算可不同于在此描述的被分配而不偏离所公开发明的精神。例如，尽管模块40、50和60的功能贯穿本公开被描述为不同的，但是它们在实际上可都在同样的处理器上编程。即，全部的车辆位置规划、轨迹估计和控制功能可在具有一个或多个处理器的单个物理装置中运行。

图3是车辆10确定其相对于包括点目标（诸如灯杆和树干）和扩展目标（诸如车辆和具体的障碍物）的被检测目标的位置的图示。建立具有原点O和X-Y轴线的局部世界坐标系100。车辆10相对于局部世界坐标系100定位于位置处和具有驶向角。下面详述基于被检测目标来连续更新车辆姿态（由和构成）的方法。

在下面的讨论中描述的计算在车辆姿态估计模块40中运行，除非相反注释。在每个时间步处，从目标范围数据模块30接收扫描映射S，并基于已知的在先姿态将S投射到局部世界坐标系100。扫描映射S然后被成组成集群清单，并且每个集群分类为点目标（其可被定义为具有长度和宽度小于1米的那些目标）或扩展目标（如果更大的话）。本方法的目的是从观察的扫描目标的相对运动来预测新的主车辆姿态。

预测的新的车辆姿态可基于车辆动力学和在先姿态P来计算，如下：

其中是车辆纵向速度，是车辆横向速度，是车辆横摆率和△T是时间步增量。贯穿下面的讨论，重要的是区分P'和其他变量诸如P^*和P''，P'是基于车辆动力学的初步预测的新的车辆姿态，P^*和P''是基于目标扫描数据由计算产生的新的车辆姿态。

新的车辆姿态的第一估计，产生值P^*，将基于在图3中由括弧110所示的点目标将做出。对于扫描数据中的每个点目标（由圆112表示），计算质量中心，并且点目标与在点对象清单中的目前已注册的对象（由星114表示）相匹配，其中在中的每个对象由其位置表示。通过使用最小二乘优化可以找到新的姿态P^*，如下：

其中是与点目标相关联的对应的注册的对象，是由车辆位置确定的刚性变换P'下的的投影，和是对于对应的对象的测量误差协方差。

对于每个注册的对象追踪命中计数（hitcount）。对于被来自扫描数据的点目标匹配的每个注册的对象使命中计数增加一。如果大于预定的临界值，诸如四，那么对象被标记为完成。如果不与映射中的任何已注册的对象相匹配，则新的对象将产生，=1。

使用同时定位和映射方法可更新点对象清单，如下：

其中，是新的对象位置（x,y），是P^*的逆变换，和是来自目前扫描的与已注册的对象匹配的点目标。对于对应的对象的测量误差协方差更新为：

其中，是与相关联的点目标。同样，如上面提及的，通过增加=+1更新命中计数。

来自方程（2）的新的车辆姿态P^*是来自上面描述的点目标的分析的输出。

新的车辆姿态的第二估计然后可基于在图3中由括弧120所示的扩展目标执行。扩展目标计算的目的是寻找从延伸对象（点云）的现有模型M到对象（级联集群）的目前扫描S的最佳变换。由于在将S中的个体扫描点（由圆122表示）关联到M中的模型点m_k（由星124表示）中的不确定性，所以扩展目标计算在本质上是迭代的。

该迭代由假设姿态P⁽⁰⁾的初始值等于来自点目标的分析的方程（2）的新的车辆姿态P^*开始。如果在扫描数据中没有可用的点目标，则P⁽⁰⁾可被设定为等于来自车辆动力学数据的P'。扩展目标车辆姿态计算然后迭代地被计算直到收敛，如下：

其中，P⁽ⁿ⁺¹⁾是车辆姿态的下一个迭代值，和分别是扩展目标扫描点和模型点，是将刚性变换P应用到点x的运算符，和由期望最大化（EM）算法计算的系数矩阵是是的测量值的概率且。

在方程（5）中，是带宽参数，其是预定义的正常数。的实现可以是：

其中，c是归一化因子。在收敛或某个指定数量的迭代后，基于扩展目标的变换P''从方程（5）被限定为P''=P⁽ⁿ⁺¹⁾。

如果模型点由扫描点（比如）测量，则对于的命中计数被增加一。当扫描点不与任何模型点相匹配时，扫描点被添加到模型M并且其命中计数被设定为等于一。当模型点不被S中的任何扫描点匹配时，其命中计数减少一。具有命中计数小于一的模型点周期性地被删除。

扩展目标对象模型M可模拟为高斯混合模型（GMM）分布：

其中

并且，其是在2-D卡迪尔坐标系中的点。

在上面的方程中，参数具有正态分布，

其中，是平均值，是方差，以及和是模型M的参数。对于和的更新规则是：

和

其中，且。

总结以上的讨论，车辆姿态P^*从车辆动力学数据和在扫描数据中的点目标计算，和车辆姿态P''从车辆姿态P^*和在扫描数据中的扩展目标计算。对于使用车辆姿态P^*或车辆姿态P''或两者的组合，可利用不同的算法。例如，如果点目标在扫描数据中是占主导的，那么为了轨迹追踪目的可使用车辆姿态P^*作为更新的车辆位置。相反地，如果扩展目标在扫描数据中是占主导的，那么为了轨迹追踪目的可使用车辆姿态P''作为更新的车辆位置。作为另一替代，融合计算可使用点目标和扩展目标来执行，如下：

其中，如在方程（2）中示出第一求和是对于基于点的目标，和如在方程（5）中示出第二求和是对于扩展目标。如在先讨论的，由于在将扩展目标扫描点关联到模型点中的不确定性，所以方程（12）迭代地求解。在收敛或某个指定数量的迭代后，总体的融合的基于扫描目标的变换P^**由方程（12）产生。

图4是使用车辆动力学和目标检测数据作为输入的、用于车辆路径规划和具有闭环控制的轨迹跟踪的方法的流程图150。如在先描述的，流程图150的方法由在车辆姿态估计模块40中的算法执行。在框152处，新的扫描数据从目标范围数据模块30获得。在框154处，主车辆运动数据由车辆动力学模块20提供。使用主车辆运动数据和目标扫描数据，可在框156处估计扫描的对象的地面速度。在决策菱形158处，可以确定移动的目标是否出现在扫描数据中。如果确定仅移动的目标出现在扫描数据中，那么流程返回到框152以接收扫描数据的下一次增量，因为移动的目标对于上面讨论的轨迹追踪目的可能是无用的。如果移动的和静止的目标被识别，则移动的目标可被忽视。

在决策菱形160处，确定点目标、扩展目标或两者是否出现在扫描数据中。在框162处，在扫描数据中的点目标被用于使用方程（2）计算新的车辆姿态P^*。如在先详细讨论的，在框162处的点目标计算也使用来自框154的车辆动力学数据。在框164处，在扫描数据中的扩展目标被用于使用方程（5）计算新的车辆姿态P''。如在先详细讨论的，在框164处的扩展目标计算也使用来自框162的基于点目标的姿态估计P^*。

在方框166处，使用来自框162和164的姿态估计作为输入，从扫描的目标数据确定总体的车辆姿态估计。如上面讨论的，如果点目标或扩展目标在扫描数据中是占主导的，则总体的车辆姿态估计可简单地使用在框162或164处计算的值。替代地，融合计算可使用方程（12）在框166处执行。总体的车辆姿态估计是来自框166的输出，其中其由车辆控制模块60使用以便车辆轨迹控制。流程然后返回到框152以接收扫描数据的下一次增量。

使用在此公开的方法，闭环车辆轨迹控制可使用目标扫描数据作为输入来实施。通过消除将GPS数据用于车辆轨迹追踪反馈的需要，可降低系统成本，和避免GPS信号的暂时不可用的问题。而且，处理点目标和扩展目标的能力提供稳健的解决方案，同时在车辆上的实时处理器中依然是可实施的。

在先的讨论仅仅公开和描述本发明的示例性实施例。从该讨论和从附图和权利要求，本领域技术人员将容易认识到其中可以做出各种变化、修改和变型而不偏离由在下面的权利要求中限定的本发明的精神和范围。