The Golem Group/University of California at Los Angeles Autonomous Ground Vehicle in the DARPA Grand

本文介绍了Golem集团/加州大学洛杉矶分校参加2005年DARPA挑战赛的情况。 我们描述了Golem 2（赛车）开发背后的主要设计原则。 我们将更详细地讨论障碍物检测，避障和状态估计的子系统。 提供了现场车辆性能的概述，包括成功以及导致故障的原因分析。

1.综述

Golem集团是一个独立的工程师团队，旨在为2004年DARPA Grand Challenge（ DGC）建造一辆车。 对于2005年DARPA挑战赛，Golem集团和加州大学洛杉矶分校Henry Samueli工程与应用科学学院联手打造了第二辆自动驾驶汽车Golem2（见图1）。 表1总结了该车辆的性能亮点。

DGC的一个方面是要求在粗糙的沙漠小径典型的非结构化或半结构化环境中进行高速自动驾驶。全球定位系统（GPS）导航是必要的，但不足以横穿路线，这可能是由于部分区域设置了各种障碍。 为了有很好的机会完成挑战，车辆需要更快地速度驾驶，并且故障率更低，这样才能在没有可预测线索的越野环境中取得过成功。在粗糙的地面上高速行驶会给传感器带来振动问题。

1.1 与以前工作的关系

在DGC之前，无人驾驶的地面车辆在结构化的铺设环境中以非常高的速度行驶（Dickmanns，1997，2004）。其他车辆在非结构化的越野环境中自主运行，但通常不是很高速度。美国国家标准与技术研究所（Coombs，Murphy，Lacaze＆Legowik，2000年）开发了使用激光雷达（激光检测和测距）探测越野环境中障碍物的自主悍马; Hong，Shneier，Rasmussen＆Chang，2002）。美国陆军实验性无人驾驶飞行器（Bornstein＆Shoe-maker，2003）也使用激光雷达探测障碍物，并且能够以超过6 km / h的速度驾驶非结构化的粗糙地面。 Rasmussen（2002）使用了视觉与雷达组合的方式在非结构化道路上来检测障碍物和道路。卡内基梅隆大学（CMU）机器人研究所可能是第一个DGC中最成功的参与者，设计了一个由激光雷达引导的悍马无人车（Urmson，2005; Urmson等，2004）。

Golem 1，我们第一次参加DGC的车辆，使用单个激光扫描仪来避免障碍。 Golem 1在2004年的挑战中行驶了5.1英里（见图2），然后因为对油门的安全限制过于保守而在陡坡上停留。成为2004年DGC比赛的第四; 考虑到Golem 1的总预算为35,000美元，这已经是一个很好的表现。

我们尝试改进以前研究人员的设计，主要集中在简化流线型设计上 - 最初是为了节省成本，同时也是为了通过避免计算瓶颈来实现更快的驾驶。 例如，我们依赖主要是用于避障的激光雷达，但与大多数激光雷达用户不同，我们并没有尝试建立一个世界本身的三维模型。 相反，我们只试图检测最重要的地形特征并跟踪其位置在二维地图。 使用单线雷达扫描仪的Golem 1只能在简单场景上走，Golem 2携带多个Ladar以更好地区分斜坡和山坡。在第3节进一步讨论了Ladar障碍物检测。

作为简化的另一个例子，我们的道路规划过程将卡车的可能轨迹在2D平面中用简单平滑曲线来表示，轨迹的曲率作为驾驶性能的度量，以及到曲线的距离作为碰撞危险的代表。 我们不考虑障碍物的三维空间，更不用说六维。 我们的方法可能不足以导航在迷宫一样的环境，但更重要的是，对于我们的目的，它可以在半结构化的情况下快速获得部分阻塞的土路。 轨迹规划将在第4节中进一步讨论。

除了雷达之外，我们还尝试了一些视觉系统。 Mobileye 视觉科技有限公司为Golem 2提供了一种单眼寻路系统，该系统将在6.1节被讨论。 这可以被视为扩展基于线索的铺设 Dickmanns和Mobileye的环境工作，以及未铺设的环境。 东芝立体视觉系统的实验在6.2节中描述。

2.车辆设计

我们的每辆车都是**商用的皮卡车，配有电动转向和油门，以及气动制动器。**我们觉得机器人更新非常重要，作为人类可驾驶的车辆完全发挥作用.Golem 1可容纳2人，而Golem 2可容纳5人。 一名乘客操作电脑并负责测试，而驾驶员则负责控制车辆并停留 意识到环境。 在测试期间，在自主和人为控制之间来回流畅地过渡是非常方便的。 单独的执行器，制动器，加速器和转向器可以独立启用和禁用，允许隔离一个特定的问题。 有一个人的“安全驱动程序“增加了我们愿意考虑的测试场景的范围。 最后，拥有街道合法车辆大大简化了后顾之忧。

因此，致动背后的中心原则是尽可能地保持原始控制的完整性，以保持车辆道路合法。 转向伺服系统有一个离合器，它由一个可以从驾驶员座椅到达的推杆接合。 制动器由气动缸驱动，该气缸拉动通过防火墙连接的电缆到制动踏板的后部。 电缆足够灵活，允许驾驶员随时施加制动。 可以通过电压控制调节器连续控制气缸中的压力。 伺服系统直接安装在转向柱上。

第二个设计目标是使计算架构尽可能简单。 自动驾驶的核心任务不需要大量的计算能力。 我们努力保持在一台笔记本电脑上运行的软件。没有充满电脑的机架，我们能够保留车辆的工作空间，但更重要的是，任何团队成员都可以使用通用串行总线插入USB总线。 该体系结构的框图如图3所示。

2.1 传感器

安装在Golem 2上用于车辆状态估计的传感器包括Novatel ProPak LB-Plus差分GPS接收器，使用标称14厘米精度OmniStar HP校正，; BEI C-MIGITS惯性测量单元（IMU）;用于测距的差速器上的定制霍尔编码器，精度约为10厘米; 和一个12位编码器用于测量转向角。 车辆状态估计将在第5节中进一步讨论。

用于地形感知的传感器包括一个Sick LMS-221激光雷达，它在车辆前方扫描180°弧度 - 测量范围高达80 m，361个样本/扫描，37.5次扫描/秒，样本误差0.5°。 还有四个Sick LMS-291个激光雷达，在大多数方面类似，只是它们各自扫描90°弧度，同时收集181个样本/扫描，75次扫描/秒。 第3节进一步讨论了雷达的布置和功能。用于寻路的Mobileye单眼摄像系统安装在“引擎盖装饰”位置，而东芝的立体视觉系统通过挡风玻璃向外看。

车载诊断总线II提供对车辆数据的有限访问; 它主要用于控制车辆的喇叭。

2.2 软件

为了让多个开发人员并行工作，我们需要一个系统，使我们可以自由地在不同的地方编写代码，例如沙漠和体育场停车场，同时仍然保留修订控制系统的所有功能。 我们在点对点修订版本控制系统darcs （Roundy，2005）中发现了这一点，该系统维护包含源代码和完整变更历史的存储库，并允许开发人员从一个源存储库推送或提取单个补丁到另一个。 尽管我们拥有多样化的工作环境，但使用darcs，我们实现了非常紧凑的开发测试周期。

该软件在Linux笔记本电脑上运行，分为两个主要应用程序。 根据传感器输入，控制执行器和记录事件做出决策的程序称为golem。 它与可视化软件dashboard没有任何关联，除了通过它创建的日志文件，这是人类可读的纯文本。 除了写入磁盘之外，这些日志文件可以直接从golem传输到仪表板，用于实时可视化或稍后通过仪表板离线重放。

可以直接在控制台上输入命令，也可以通过UDP报文端口从dashboard 接收命令。 这些命令设计为简单且易于打字，同时在土路上行驶的车辆中行驶。 虽然这很方便，但是能够使用模拟控制或至少箭头键来实时调整控制参数可能更方便。

2.2.1 Golem

用于数据捕获，规划和控制的主要软件，golem由两个线程组成。 主线程是完全反应的并且是实时的; 它从传感器接收数据，立即处理，并发送命令执行器。 底层驱动程序，状态估计器，雷达和障碍物过滤器，道路/路径跟随器，速度分析器和控制器都在这个线程中运行。第二个路径规划线程花费更多时间来提出全局合理性基于在线程启动时接收的累积传感器数据的快照，车辆的路径。

Golem可以通过实时传感器数据，简单的模拟器或先前记录的日志数据来驱动。 该模拟器对于调试计划程序的高级行为非常有用，但其模型不够精确，无法调整底层控制器。 重放模式允许我们以可重复的方式去除障碍物过滤器和状态估计器，而无需一遍又一遍地驾驶车辆。

2.2.2 仪表板

仪表板是使用C，Gtk + 2和OpenGL是一个可扩展的可视化工具，它允许我们回放我们的日志文件并从他们的内容中学习。 典型的屏幕截图如图4所示。

仪表板的核心由日志文件组成解析库，OpenGL表示，以及控制时间流的重放机制。 仪表板从磁盘，标准输入或UDP网络连接获取日志文件作为输入，并使用配置文件中描述的规则将人类可读的日志行转换为易于其他程序的内部表示要索引和访问的组件。 通过Perl兼容的正则表达式库，日志行一旦可用就会被解析。 通过OpenGL窗口为用户显示内部表示。 用户可以在渲染的世界中播放日志文件，暂停，快退，快进，测量和移动查看感兴趣的传感器可视化。这证明在我们的发展努力中不可或缺。

数据以多种不同方式可视化，因为我们发现没有一种表示适合所有调试情况。 主要部分是世界的图形表示，可从2D自上而下的视角观察也是从3D的角度来看。 它使用OpenGL渲染。 还有一个部分将输入显示为文本数据以获得精确读数。 另一部分包含一组用户可编程图。 由于故意通用的内部表示，向可视化添加新元素，例如来自新传感器的数据，是一个非常简单的过程。

3.雷达障碍物检测

我们认为一个不可穿行的障碍物的特征:（a）代表地面高度的快速明显变化，坡度大于30°，或地面表面不连续; （b）表示地面高度的总变化太大，车辆无法简单翻转; ©如果与地面不连续，则不足以使车辆通过。 这个是对DARPA挑战的“障碍”的充分定义。 由于障碍物是由地面高度的变化引起的，因此最重要的信息来自于空间相邻的地表测量值的比较。

我们没有使用随时间积分的传感器数据来构建世界框架中的绝对地面高程地图，假设这不必要地从感兴趣的真实信息中移除一步。 相反，我们试图直接从瞬时传感器数据中感知或推断身体固定框架中地面高程快速变化的区域，然后在世界框架中保持这些区域的地图。 我们并不关心任何地面坡度或表面粗糙度没有达到使地面不可逆的阈值。

3.1 雷达几何

我们使用Sick LMS-291和LMS-221激光扫描仪作为障碍物检测的主要手段。 有趣的是，许多使用2D Ladars的DGC团队发现了各种各样的方法来安排它们。 这些探测器扫描测距激光束通过平面的扇区，同时平面它可以通过车辆运动或通过致动激光雷达安装座来旋转或平移。 平面的选择，或者更一般地，对任何数字传感器的扫描模式的选择，表示在方位角方向上快速扫描，在不同仰角处的较慢或较稀疏采样或相反的选择。

在垂直平面中的激光雷达扫描具有显着的优点，即通过单次激光扫描可以清楚地扫描地形的可穿越性。 例如，从图5中的单个垂直平面扫描可以很容易地确定地面是连续的，平坦的，并且可以在卡车前几英尺处穿过，直到有一个不可比的垂直表面比车辆的车轮更高的点。

在我们的例子中，单激光扫描需要1/75秒，单个采样点间隔1 / 13,575秒。 在这个时间尺度上，车辆的运动或振动不太可能足以扭曲垂直扫描以改变这种解释，使得可穿越的地面看起来不可逆，反之亦然。 类似地，虽然估计的车辆俯仰中的小误差将导致不可逆障碍物的估计位置中的小误差，但是它们不太可能导致重大误差或阻止在正确的近似位置处感知障碍物。 与这些优点相反，明显的缺点是垂直切片仅在单个窄方向上测量地形。 即使存在多个垂直扫描仪和/或垂直平面可以在不同方向上转动，车辆可能具有对地形的稀疏方位角覆盖的盲目视图，并且可能错过狭窄的障碍物。

相反，水平或接近水平的扫描平面将提供良好的方位角覆盖，并且清楚地显示狭窄的障碍物，例如栅栏柱和行人，但是孤立地解释任何单水平扫描都是有问题的。 由于缺乏相邻仰角的测量值，人们无法确定单水平扫描的返回是来自不可逆的陡坡还是来自可横向倾斜的陡坡。因此，必须组合来自多个水平扫描的信息; 但由于现在至少相隔1/75秒的个别测量值之间进行了重要的比较，而不完全估计的运动或振动将使数据失真的可能性更大。俯仰，滚转或高度的小误差 可能导致车辆误解地面轮廓的高度，并导致地形的完全错误分类为可穿越或不可逆。

我们的方法是使用垂直和近水平雷达的互补布置。 在Golem 2车辆上，前保险杠上安装了两个近乎水平的雷达和三个垂直方向的雷达。 这个想法是，在卡车车身固定框架中，而不是世界固定的框架，使用垂直方向的雷达来形成卡车前方的一般地面的轮廓。 我们适合数据的地面模型在卡车的运动方向上是分段线性的，并且在侧向方向上是分段式的。 该模型插入了最新的可用数据。 在超出可用数据的位置，假定地面在车身固定框架中具有恒定的高度

然后计算从几乎水平的雷达相对于地面模型的返回的表观高度，以查看这些返回值：（a）与地面模型的可穿越部分一致;（b）符合一个不可逆的部分地面模型;©与不可逆的部分相一致地面模型; (d)显然，海拔略高于名义地面; 在案例(b)或©中，雷达回归归类为可能出现的障碍; 在从不同有利位置的相同位置接收到若干这些返回之后，确认存在障碍物。 在实践中，诸如停放的汽车之类的物体将被表示为相邻障碍物的集群。

例如，图6示出了来自NQE 比赛的一部分Ladar数据。 来自水平方向的雷达的返回表明在车辆前方有一个长的直线特征，以及两个较小的特征或簇的返回。 三个垂直的雷达在三个方向上测量地面轮廓，并显示卡车正朝着向上倾斜的脚行驶。 “长直线特征”仅仅是向上倾斜的地面的表面轮廓，因此，由于倾斜不是太陡，所以不应该被认为是障碍物。 然而，两个较小的回归群似乎处于地面以上的显着高度，并被解释为不可逆转的障碍。 实际上，这些都是锥桶。

整个过程是Ladar测量数n的O（n），其中大阵列提供对给定位置中的累积障碍物检测的O（1）访问。纬度经度坐标与阵列单元之间的多对一映射是需要的，具有以下特性：（a）阵列单元的每个连接的原像是至少近似相同的物理尺寸，（b）映射到同一阵列单元的任何两个不同的区域相距足够远，它们不会两者都需要在车辆的规划范围内考虑。一种简单的方法，即在极点附近工作，是定义纬度相关的单元尺寸，使得不同的整数个阵列单元对应于每个纬度的固定经度范围。可以接受的是，从赤道进一步需要更少的阵列单元，因此，一些阵列单元将具有比其他阵列单元更多的原像位置。可以调整该方法以通过例外处理极性邻域。

一旦确认，障碍持续存在，除非并且直到一段时间例如1秒通过而没有从该位置检测到雷达返回，在这种情况下障碍物到期。 这使得系统能够从误报检测中恢复，并且还提供了处理移动障碍的基本能力，因为障碍物可以从一个地方消失并在其他地方被重新检测。 然而，我们没有尝试形成移动物体的任何速度估计，并且机器人通常在假设其他物体在世界帧中具有零速度的情况下操作。

为了节省计算时间和记忆，我们不希望跟踪远离DGC比赛边界的障碍物。 偏离障碍应该与车辆的规划问题无关，并定期检查是否已经过期应该是浪费时间。 因此，只要车辆本身保持在路线上，就不会将不可撤离的路面分类为障碍物。 然而，允许指示越野障碍的传感器测量被积累，因此如果车辆由于任何原因离开路线边界，并且被要求将越野障碍视为重要，则这些障碍将非常快地超过确认阈值。

3.2 雷达检测结果

NQE障碍物比赛提供了一个具有明确统一测试环境，其中现在可能存在许多不同的传感器记录，因此在报告我们的雷达分类结果可能是有用的。 车辆无法识别的预定障碍物（停放车辆，轮胎堆垛和油箱陷阱）的范围如下所示：图7。

DARPA使用锥桶作为道路边界 - 车辆依靠自己的GPS定位测量，可能会考虑位于球场边界内部或外部。被认为是球场上的交通锥体在范围内被归类为不可逆转 如图8所示。在低端锥体上的四个异常值中，直到它们在小于20米的范围内才被识别，两个锥体在早期视野中被其他物体遮挡，两个锥体位于倾斜的底部。 并且可能无法检测到，直到车辆爬上斜坡顶部并向下驶去。

有零假阴性。 球道上的每个故意障碍物和锥体都被正确分类，并且一旦被分类，只要它保持在车辆的180°前向传感器弧上，就一直被重新作为障碍物。

然而，有相当多的误报，将可穿越的地形分类为不可逆转。 如果在车辆的路径中出现这些假障碍物，则车辆当然会尝试围绕它们规划新的轨迹，并在必要时降低速度。 然而，随着车辆的接近，虚假障碍并没有持续存在。相反，所有的虚假障碍最终都会过期; 让车辆自由地驶过那个地形。 虚假障碍物检测首次出现然后到期的范围如图9所示。首次出现时的主题范围为30.5米; 到期时的主题范围是20.5米。

一些假阳性与地面上的不规则性或散落在路线上的碎片相吻合。 事后看来，似乎大部分假阳性都是由高速公路轨道边缘的反射标记引起的; 引起激光束的轴外反射并混淆激光雷达范围测量。 我们从误报的位置推断出这一点，但尚未通过深思熟虑的实验来检验这一现象。 在GCE大型挑战赛期间，反光道路标志也出现了类似的问题。

当车辆在连续的无标记路面上行驶时很少发生误报，例如NQE处的沥青轨道或GCE中的泥路。 假阳性的主要NQE性能影响是导致车辆在穿过沥青标记边缘的过渡处犹豫不决。 然而，车辆仍然设法保持高平均速度并在竞争时期完成障碍训练。 见（表I.）

4.避障

我们使用通常的递归算法完成了车辆轨迹规划。为了与团队的一般哲学方法保持一致，我们尝试设计最小的计算足迹，利用DARPA提供的启发式规则和知识的战略优势路线定义数据文件（RDDF）。 该方法与概率路线图的产生相似，（Kavraki，Svestka，Latombe和Overmars，1996）或者是一个快速探索的随机树（Frazzoli，Dahleh＆Feron，2002; LaValle，1998年）我们生成了一个通过平滑轨迹连接的车辆配置图，而是为了在扩展图的边界上生成新配置，我们递归地生成轨迹中点以搜索连接起始配置和结束配置的最佳可行路径。

在我们寻求高效率的过程中，在实践中，我们丧失了对我们解决方案路径的全局最优性的任何保证，甚至可以保证例行程序的可预测收敛条件。 然而，我们发现在绝大多数情况下，可穿越的路径是在没有给车辆的处理资源带来负担的情况下找不到。

计划算法的阶段是在车辆的初始全局坐标附近的笛卡尔2D地图，其中填充有多个离散障碍物。 障碍物表示为必须通过特定半径避免的点危险，或者可以仅在一个方向上交叉的线段。 点障碍集中在飞机上被雷达系统识别为不可逆的位置。 围绕每个点障碍物的缓冲半径包括车辆的物理宽度，并且另外，基于从车辆到危险的距离而变化。诸如墙壁或汽车之类的真实物体由这样的点的集群表示，每个点都是 与其相关的半径。 线段障碍来自RDDF，其指定车辆的走廊边界。车辆轨迹是描述车架在平面中的运动的单个曲线。

我们的任务是找到从初始配置到目标配置的可驱动路径，该路径不会侵犯任何障碍物。 2目的地被视为RDDF走廊的中心，距离我们的传感器地平线一定距离。最简单且可能是最常发生的场景将要求车辆简单地从/向前行驶而不执行任何规避操纵。我们用包含两个节点和单个连接边缘的有向无环图来表示该场景。这种数据结构的选择以后允许我们利用地形排序的排序，这使得图的最低成本遍历能够在线性时间内完成。图中的每个节点与车辆的可能配置相关联，并且每个边缘与将起始配置连接到目标配置的定向路径相关联。在任何时候，都要注意保持图中节点与本地映射中位置之间的拓扑对应关系。

初始化后，我们进入算法的递归循环，它包含三个阶段：
1.图评估
2.路径验证
3.图扩张

重复执行该循环，直到从/到找到满意的路径，或直到满足看门狗终止条件。 通常，如果在几百毫秒后找不到路径，我们将终止规划器。

图10-12提供了路径规划过程的可视化。 节点用亮紫色箭头表示，而考虑的轨迹用紫色线表示。

在图形评估中，计算图形中所有新边缘的轨迹段和成本因子。从连接的端点节点配置计算每个轨迹段，然后检查可能的障碍物碰撞。可能存在大量不同的轨迹段以进行检查并且当前正在跟踪大量障碍物，记住单个真实物体任何显着的水平范围将被追踪为一个较小的障碍云，它们靠得很近。为了有效地进行碰撞检查，我们使用波前传播方法来创建一个地图，指示从每个点到最近的障碍物的近似距离。生成此贴图后，可以针对障碍物碰撞快速检查轨迹。如果没有障碍物相交，则沿着轨迹的瞬时曲率的积分产生遍历的成本。根据我们的经验，路径的动态可行性可以通过这个单一值充分表征。


循环继续路径验证。 通过参考节点的拓扑排序，可以快速找到整个探索图的最佳路径。 如果成本令人满意，则循环结束并开始细化过程。 该细化过程包括许多启发式修改，这些修改将节点和边添加到图中以“平滑”计算的轨迹。 作为最后一步，沿着解决方案计算速度曲线，以便车辆的速度与路径的曲率以及任何DARPA施加的速度限制一致。计划线程然后更新用于转向和速度反馈的期望轨迹。 控制系统。

如果最佳图遍历的总成本太高，我们进入第三阶段
算法：图形扩展。 在此阶段，与障碍物相交的边缘在发生碰撞的点处被分开。 对于根据启发式避免条件选择的配置，将多个新节点添加到图形中。 未评估的边缘将这些新节点连接到违规轨迹段的端点。 然后，当使用图形评估再次开始递归循环时，计算这些新边缘的轨迹段。

在启发式技术中应用于选择良好目标配置的障碍避免，其位置成分目标配置通常是预测的从障碍物垂直向外插入障碍物位置和障碍物之间RDDF边界。 我们发现车辆驶向同样最好指定为两者的函数在碰撞点和RDDF定义的航向走廊方向。

5.车辆状态估计

Golem 2的路径规划和控制子系统需要对车辆的纬度，经度，航向和速度进行良好估计，其精确度超出车载传感器提供的精度，例如C-MIGITS。 实施了两个不同的状态估计器来执行此任务，作为提供分析冗余的手段。 第一个状态估计器使用类似于自行车的模型，并且主要依赖于状态的历史。 第二个估算器基于六自由度刚体模型，具有在车辆的速度上添加“软”非完整约束作为虚拟航向测量。

5.1 自行车估计

在本节中，我们将描述第一估计器的工作，因此称为自行车估计器。 自行车估计器是一个离散的时间扩展卡尔曼滤波器，（1974年Gelb;卡尔曼，1960; 卡尔曼和布西，1961年; Welch＆Bishop，1995年），具有以下输入：

1.纬度和精度来自Novatel GPS 传感器 20HZ
2.来自后轴霍尔速度传感器系统30 Hz。 一组16个磁铁安装在后轴上。 每当一个磁铁经过它们时，两个探测器连接到车架并通过电压脉冲。 启用的两个传感器具有正交编码器，即我们能够区分正向和反向运动。 离散时间双态卡尔曼滤波器使用电压脉冲作为输入并估计旋转速率，其通过齿轮比和轮距来缩放以推断车辆的速度。
3.来自绝对编码器的转向角度20赫兹。

公式

5.2 六自由度模拟器

在本节中，我们描述了6DOF估计器的设计。 与前面讨论的自行车估计器一样，6DOF估计器被实现为离散时间扩展卡尔曼滤波器。 估计器采用相当标准的技术设计用于捷联惯性导航系统。 由于无法获得车辆动力学的详细模型，因此滤波器主要依赖于刚体运动方程。 然而，由于没有磁力计或其他方法来测量车辆的方向，并且需要能够确保非线性滤波器的收敛而不需要初始校准程序，因此以虚拟的形式利用车辆动力学的知识。 从惯性速度数据进行航向测量。

公式

5.3 模型系统噪音

每次GPS信号到达时，状态都会传播和更新。 没有与状态传播方程共有的噪声。 假设存在与自行车模型测量和测量的输入共同相关的加性高斯白噪声，以及六自由度模型的z a和z g。 为了有效地跟踪状态变量S S，␴bias，␥␥中的常数，假设在它的传播中存在加性高斯白噪声项。 还假设GPS测量共有纬度和经度兲具有与之相关的一些噪声。 调整分配给上述噪声过程的方差，以确保创新3不相关，并且估计器是稳定的，并且收敛理由很快。 通过调整方差，我们可以改变与车辆状态历史或GPS测量相关的“信任”。

5.3.1 确定适当的GPS测量噪声方差

从GPS接收的数据包括纬度，经度和水平稀释精度（HDOP）值。 HDOP是GPS测量的品质因数，它与GPS天线可见的卫星数量直接相关.HDOP值与每次测量中的噪声有关。 然而，我们将HDOP值映射到实际方差的尝试是徒劳的，因为我们没有观察到单调关系。 注意到，超过5的HDOP通常对应于GPS中的多径接收，因此，GPS在该情况下具有异常巨大的变化。 如果HDOP与噪声方差之间没有任何特殊关系，如果HDOP小于5且存在巨大差异，我们选择保持与GPS噪声相关的恒定方差除此以外。 此外，我们注意到GPS噪声高度相关而非白色。

5.3.2 创新部分

自行车估算器基于非常直观的车辆模型，这促使我们在相关的物理参考系中考虑GPS创新，而不是任何任意的参考系。 将创新投射到其中是有洞察力的车辆的局部框架：平行于航向并与之垂直。 平行和垂直的主体固定轴如图13所示。

对于理想的估算器，这些创新将彼此相互关联。 但是，我们调整了估算器，以实现零均值的创新。 在调整估算器时，考虑平行和垂直创新是非常有用的。 例如，并行创新中的直流偏差意味着我们没有跟踪滑动因子（S）足够。 因此，为了确保零区域并行创新，应该增加与滑移因子传播相关的方差。

5.3.3 自适应塑造创新

GPS数据中的噪声高度相关，并且对方差的先验知识非常少。 通常，特别是当车辆靠近墙壁或靠近隧道时，由于多径反射，GPS测量中会出现高度不稳定的跳跃。 在这种情况下，如果没有任何关于方差的先验知识，状态估计会反弹很多。 这是不合需要的，因为它会妨碍主程序中的路径规划器和障碍物检测子程序。

为了对抗这些“非物理”的跳跃，一旦这些推动者融合，创新就会被削减到一定的最大绝对值。例如，GPS测量对应于0.05m的2m的垂直创新 直行是非物质的，因此在我们的案例中，垂直创新应该被削减到名义值，即6英寸。这阻止了状态估计的大幅跳跃，但是有一个严重的缺点。据观察，如果创新被削减到固定的范围，然后在某些情况下，状态估计将远远落后于“良好”的GPS测量集，并且需要很长时间才能收敛。为了防止这种情况发生，创新的限幅限制自适应地确定为固定极限的最小值，或最后2 s中创新的平均值，其数值因子略大于1。该创新的平行和垂直组件分别用不同的数值常数进行修剪。

5.3.4 应对时间延迟

总线上传感器数据的出现与控制程序处理它们的时间之间存在一些有限的延迟。 通常，这种延迟是名义上的共计（50-200us)，但它偶尔会非常大。 GPS数据的大延迟表现在大的负向并行创新中。然而，通过前面描述的方法有效地削减了大的创新，因此不影响状态估计。 将来，我们计划实现一个例程，该例程可以同步所有串行/ USB数据源之间的时间。

在桥下进行状态估计器的性能如图14所示。“*”表示在本地笛卡尔坐标系中接收的GPS数据。 “·”表示卡车的估计位置。在这个图中，Golem 2从左向右移动。请注意，GPS数据存在巨大差异由于多路径。另请注意，GPS噪声高度相关;在这种情况下，方差是巨大的，状态估计的性能不受GPS数据中大的非物理跳跃的影响。在长度⬃0.3m的偶然跳跃中观察到估计状态的连续更新。这些对应于接收的GPS数据的延迟和大的负并行创新，如本节所讨论的。与平行方向相比垂直于运动的方向。从图中可以看出，状态估计器的性能是对GPS数据中大量非物质跳跃的影响。在连续更新中观察到长度为0.3 m的偶然跳跃。估计状态。这些对应于接收的GPS数据的延迟和大的负并行创新，如本文所讨论的部分。

5.3.5 特殊操作模式

有几种情况需要特殊处理说明：
当车辆静止时，观察到GPS信号显着漂移。 这不仅是非物质的，而且对路径规划也是有害的。 因此，当车辆行驶相当缓慢或处于静止状态时，分配给GPS测量值的方差显着增加。 在这种情况下，自行车估计器基本上用作积分器而不是滤波器。

据观察，GPS信号偶尔会离散地跳跃。 这些跳跃通常对应于附近存在的输电线路。 困难在于GPS在跳跃后需要一段时间才能重新收敛。 这些非物理跳跃很容易从平行和垂直创新的跳跃中检测到。在检测到这种跳跃之后，GPS方差增加直到它再次值得信任，即直到创新再次在一定限度内。

5.3.6 IMU初始化

该模型的一个优点是它在直行时非常快速地收敛，通常在大约5秒内。 一旦自行车模型收敛，则使用航向估计来初始化IMU。 直行时，自行车估计器的性能非常好，因为航向估计值在IMU计算航向的0.5°范围内。但是，在急转弯时，航向估计值与IMU计算的航向上达到⬃3°。 因此，IMU计算的航向更加重要，特别是当GPS信号不好时。 在未来，我们计划扩展自行车模型，包括角度旋转和线性位移来自IMU的数据。

6.视觉系统

对于计算机视觉而言，沙漠路径对铺设的道路提出了不同的挑战，因为环境结构要差得多，并且在构建算法时可以利用较少的先验信息。 我们的方法是整合现有的视觉技术，这些技术已被证明可以在公路上工作，但有可能转移到越野领域。 这些包括基于学习的路径探测和双目立体重建。

6.1 Mobileye视觉系统

Golem 2配备了由Mobileye 视觉公司创建的复杂视觉系统，由单个摄像头和专用处理单元组成。 在路上，Mobileye系统可以找到车道边界并检测其他车辆和他们的实时位置。 该系统由Mobileye和耶路撒冷希伯来大学为的越野环境开发的。

Mobileye系统结合了基于区域和基于边界的方法来找到相对于车辆的路径位置和方向。 这两种方法相辅相成; 从而允许在各种情况下进行可靠的路径检测。 具体来说，我们使用各种纹理过滤器和自适应增强学习器Adaboost（Freund＆Schapire，1996）分类引擎以形成路径和非路径图像块的初始图像分割。 同时，我们使用相同的过滤器来定义候选纹理边界和在可能的间距空间上的投影扭曲搜索偏航参数，以便选择与纹理渐变和几何模型一致的一对边界线。 然后组合基于基础的模型和基于边界的模型（通过它们的置信度值加权）以形成每个帧的最终路径模型。

6.1.1 基于区域的路径检测

灰度图像被分成部分重叠的块。将滤波器组应用于所有图像像素，并且每个块生成描述符矢量。描述符包含16个过滤器中每个过滤器对块的过滤器响应的平均值和标准偏差。纹理描述符中的每个条目都可以被视为**“弱”学习**者，因为它形成了类别区别。迭代Ada-boost算法将弱学习者组合成一个强大的分类引擎，**根据训练数据为每个块分配路径或路径标签。**训练数据来自200张图像，这些图像是在2004年挑战路线的各个部分收集的。块分类本身不足以用于自动车辆路径规划，因为预计大约10％的块被错误分类。过滤方法用于清除一些错误分类的块，然后检测路径边界。路径边界是通过路径每一侧的最小误差分隔线导出的。系统置信度是根据分离质量和感应范围计算的，（距离我们可靠地识别为最远点的距离为路径）。图15显示了算法的每个主要部分的结果。

6.1.2 基于边界的道路检测

基于边界的技术不依赖于先前学习的纹理信息。 相反，它假设路径纹理属性不同于周围的不可行驶区域。为了使这个提示可靠，我们必须将解决方案约束为严格的几何模型，其中路径边界位于直的平行边缘上。这使我们能够减少找到四个自由度的可行驶路径的问题：消失点的（x，y）位置，以及到路径边缘的左右距离。假设平面世界，透视相机和平行路径边界在世界范围内产生的几何约束表明每帧的下拉投影扭曲方案：给定相机的俯仰角和偏航角的假设，在世界坐标视图中形成鸟瞰图。在扭曲图像中，如果确实俯仰角和偏航角是正确的，则路径边界应该是平行的垂直线。将图像纹理边缘投影到地平线上将产生一维轮廓，其峰值对应于变形图像中的垂直纹理边缘。我们在1D轮廓中寻找一对主导峰值，并生成得分值，然后通过迭代刚刚描述的投影扭曲过程，通过搜索俯仰和偏航角来最大化该得分值。搜索从前一帧的俯仰和偏航角估计开始，然后使用光流和小运动模型进行增量俯仰和偏航估计：

其中（u，v）是该点的流（位移）（x，y）和w x，w y是俯仰角和偏航角。该扭曲图像被分成重叠的10×10块，每个像素形成块中心。 使用与基于区域的方法相同的滤波器组，我们估计垂直线的似然性e-。穿过块中心形成纹理梯度，其中⌬是块的两个相应半部的纹理矢量描述符之间的L 1距离。 为了检查假设（对于俯仰和偏航），我们将水平纹理渐变垂直投影到x轴上，并在此投影中查找峰值。该投影的示例结果如图16（d）所示。图像中的路径边界和其他垂直元素在投影中创建高区域，而低区域很可能是由不连续的垂直纹理渐变引起的，并且由灌木，岩石等创建。当消失点假设正确并且路径边缘（以及可能的其他平行特征）对齐时，投影最大化。 通过找到这些假设的最高峰，我们的系统能够找到左右的横向位置边图16共e兲显示了“裁剪后的”1维投影轮廓和相关的峰值对对应于路径边界线。

6.1.3 表现

该系统采用Power-PC PPC7467 1 GHZ，以每秒20帧的速度运行。摄像机安装在连接前保险杠的杆上（图17），以实现最大视野。我们尝试了45°和80°视场镜头，发现后者更适合自动驾驶，车辆不一定以道路为中心。对于我们的应用，最有意义的整体系统性能测量是计算系统产生正确路径边缘位置的频率（帧的几分之一）以及适当的航向角度。此外，系统知道何时知道它至关重要无法准确地确定路径，因此车辆可以减速并更多地依赖来自其他传感器的信息。我们的结果被不同的地形类型分解。对于每一个，选择1000帧的代表性挑战性剪辑，并且由人类观察者在这些序列上对系统性能进行评分。通过观察车辆前方约6米处的行驶标记的位置来计算路径边缘距离精度。如果路径边缘，则框架标记为不正确该位置的标记似乎距离实际路径边界大于30厘米共18英尺像素。对于直线路径，路径的感知消失点也被标记，并且我们的算法的航向指示符与该点的边界位置进行比较。

在具有舒适宽路径的相对直的段上，我们的系统在100％的时间内报告可用性（高系统置信度），同时在99.5％的时间内产生准确的路径边界位置。 从人类标记的消失点开始的航向角的平均角度偏差是1.7°。

第二个测试片段是具有高度变化的更不均匀的地形的示例。 在这里，车辆穿过干涸的河沟[图18（b）]，其中路径纹理和场景几何都很难。 当我们的车辆到达山顶时，可以看到只有一小段道路。 在这种情况下，系统报告不可用性低于8％的时间。 但是，如果可用，边界位置的准确度为98％。

最后一个夹子包含一个蜿蜒的山口[图18（g）]; 由于路径曲率以及纹理变化而难以实现。 尽管如此，我们的系统在整个剪辑中都可用，并且在检测路径边界时实现了96％的准确度。

6.1.4 结合

学习和几何方法的结合产生了高质量的结果，并且可靠度估计适合集成到我们的控制系统中。 Mobileye系统的输出（包括路径边界，方向和俯仰）被集成到Golem路径规划器中。 路径边界被标记为线障碍物，并使用Mobileye相机，GPS天线和路面之间的已知变换投影到世界坐标系上。 图19显示了检测到的相对于车辆的轨迹边界和轨迹中心以及车辆的实际轨迹。

6.2 东芝视觉系统

Golem / UCLA团队尝试了各种立体视觉系统，用于障碍物检测，地平面配准和路径寻找。 固定摄像机立体视觉是一个研究得很好的问题，并且有许多不同质量的学术和商业系统。 远距离传感对于在高速汽车中使用至关重要，因为时间至关重要; 这是不切实际的和Golem / UCLA团队尝试了各种立体视觉系统，用于障碍物检测，地平面配准和路径寻找。固定摄像机立体声是一个研究得很好的问题，并且有许多不同质量的学术和商业系统。远距离传感对于在高速汽车中使用至关重要，因为时间至关重要;这是不切实际的，有时不可能停下来分析附近的障碍。这个要求转化为分离立体相机的宽基线，因为系统的最大范围由该距离决定。进一步的性能限制是处理时间，因为延迟会带来危险路径规划和控制中的错误。东芝研究公司在日本川崎开发了一种高速公路驾驶的立体声系统，我们安装在Golem 2上。由于时间不够，我们没有在GCE之前将立体声系统集成到车辆的控制系统中但毫无疑问，它具有成功实现自动驾驶的巨大潜力。在本节中，我们将介绍硬件配置和实现细节。

6.2.1 硬件配置

图20显示了我们的立体摄像机的设置。两个金属板夹层并严格固定摄像机，以便它们能够承受越野驾驶引起的强烈振动。 两个相机之间的距离是1.2米，每个相机都是地平面上方1.5米。 我们使用收费耦合设备（CCD）相机，7.5毫米镜头，图像分辨率为320×240像素。

我们的立体声系统基于一个名为Visconti的多VLIW（超长指令字兲）处理器（Hattori＆Takeda，2005; Tanabe，2003）。处理器架构旨在确保一般图像处理操作的高效性能，同时满足汽车应用的若干要求，例如，工作温度范围
-40- + 85°C，功耗⬍1W@ 150 MHz。图21显示了使用Visconti的图像处理单元的原型。它有三个视频输入通道和一个视频图形阵列视频输出显示处理结果。图22显示了Visconti的框图。处理器包括一个图像变换模块和三个并行操作的处理模块。三个处理模块中的每一个都包含一个精简指令集计算机处理器内核和一个VLIW协处理器。指令支持立体声计算所需的几种单指令多数据共SIMD操作，包括卷积，累加和像素移位。协处理器的集合。每个处理模块还有一个暂存存储器和一个直接存储器访问控制器，因此，双缓冲数据转换会隐藏内存访问延迟。

6.3 实施细节

我们采用绝对差之和（SAD）作为7 x7窗口内的匹配标准，因为SAD在计算上的成本低于其他度量，例如平方差和归一化互相关之和。 我们还使用递归技术来有效估计SAD测量（Faugeras等，1993; Hattori＆Takeda，2005）。 为了补偿由于立体相机的不同设置而可能出现的灰度变化，通过减去每个像素处的匹配窗口内的强度的平均值来归一化输入立体图像。 此外，在参考图像上计算每个点处的强度方差，以识别那些具有不足的强度变化的区域，以建立可靠的对应关系。

由于Visconti有一个图像转换模块和三个并行操作的处理模块，因此这些模块的任务分配对于实时操作至关重要。 例如，立体声校正是转换输入的必不可少的过程立体图像使得极线与图像扫描线对齐。 图像变换模块执行立体声校正，由于不规则的存储器访问，SIMD操作难以加速。 此外，我们将一对图像分成三个水平带，这三个水平带分配给这三个处理模块。 每个三个区域具有大约相同数量的像素，因此计算成本在三个模块中均匀分布。

由于SAD计算，那些任务分配和SIMD指令的递归技术，我们的立体声系统能够以大约30帧/秒的速率进行视差估计，具有多达30个视差水平和320×240像素的图像输入大小。 这种表现高于带有SIMD指令的2.0 GHz处理器。 图23示出了视差图像的若干示例。 更多红色强度表示更大的视差值，这意味着更近的区域，而黑色表示无纹理区域。 输入图像的底部被排除在立体计算之外，因为它对应于车辆的发动机罩的一部分。 这些输入立体图像是在莫哈韦沙漠中拍摄的。

7.结果

Golem 2的资格赛在NQE障碍赛上进行当然，如表I所示，该领域是最好的领域之一，尽管由于第7.1节讨论的原因，我们也在两次运行中失败了。

Golem 2在2005年GCE的第七杆位置赛跑出了滑道。 我们知道Golem 2能够高速行驶。 我们的速度策略是，只要低于这个值，车辆就会以最大允许速度行驶每小时25英里 如果建议的速度大于25英里/小时（暗示最大允许速度为50英里/小时），那么傀儡2将超过建议的速度，首先是少量，但更多随着比赛的继续，更激进，直到它始终以最大法定速度行驶，当然，除非在转动期间调节其速度。

正如预期的那样，Golem 2在污垢上做得很快
道路和干涸的湖床。 在一座铺砌的桥上，Golem 2的激光传感器错误地认识到道路中央的反光“Botts’点”作为障碍物，随着车辆越来越近，它们似乎像海市蜃楼一样消失。 （参见图24.）这导致车辆在桥上来回编织，使追击车辆中的DARPA观察员感到震惊。 但是我们的车继续行驶，一旦再次到达泥路，它就会整理并以每小时超过30英里的速度恢复进度。

DARPA观察员认为Golem 2最初是“怯懦”并将其与青少年司机进行比较，但表示一旦离开铺好的道路并进入沙漠，他们就会对其表现印象深刻，并相信他们手中有胜利者。

不幸的是，在不到1小时的车程中驾驶22英里后，由于记忆管理错误，计算机崩溃了。 不受控制的车辆高速离开航线边界，穿过植被。 DARPA“暂停”按钮不再起作用，因为没有运行任何软件，并且DARPA观察员没有按下“禁用”按钮以防车辆恢复。 在从崎岖的地形冲击之后，Golem 2在离球场超过半英里之前冲了过去，最终震动了连接器从其保险丝箱中脱离出来，杀死了发动机。

7.1 失败的原因

Golem 2在三个重要场合坠毁：NQE试验期间两次，GCE期间一次。我们认为所有这些失败都应该被视为“错误”，而不是设计中的根本缺陷。 然而，回顾这些失败的原因可能会很有趣。

在NQE课程的第一次尝试中，Golem 2立即转向右侧并撞向一个用于模拟牛道口的大门的一侧。 它撞倒了大门旁边的栅栏，停了下来。 造成这种失败的主要原因是其中一个垂直雷达被重新定位并且错误校准（由于缺少小数点）。课程开始条件的处理是一个促成因素。

事件序列如图所示25.在2005年的NQE中，车辆是从远离指定航线边界的起始滑槽发射出来的。 我们应该为这种特殊情况做好准备，正确的程序是考虑课程边界向后延伸到起始位置。 然而，相反，Golem 2会做出反应，因为它会通过向外放松路线边界限制来对远离路线作出一般性反应。在图25（a）中，车辆以直线轨迹运动，该轨迹几乎不受航线边界的限制。 在图25（b）中，车辆已经移回到RDDF航线上，并且还检测到前方的门，其中橙色圆圈表示障碍物。 规划边界约束已向内收缩，并且车辆已经规划了非常接近正确的轨迹，即，穿过第一门的轨迹。 不幸的是，边界约束没有得到足够的收紧，并且轨迹绕过RDDF路线的右边缘。在图25（c）中，由于错误校准的激光雷达，车辆错误地感知到了实际上不存在障碍物的幻影障碍云，并且计划向右转弯。 在图25（d）中，车辆已经察觉到它的新轨迹与真正的障碍物 - 围栏碰撞，但是它找不到似乎不会导致碰撞的计划。在图25（e）中，即将发生碰撞，车辆迟迟会试图制动。 在图25（f）中，围栏已经被撞倒并且车辆已经停止。 虽然车辆在物理上能够在压碎的围栏上前进，但是没有“重新启动”逻辑使得车辆能够再次开始移动。


当然，我们对这种失败的反应是修正垂直雷达的校准，正确处理在航线边界外启动的特殊情况，以改善速度控制器对障碍物的反应，并防止车辆进入 如果没有暂停，永久停止。 我们能够执行这些修复只是因为图25和其他地方显示了非常有用的仪表板可视化工具，使我们能够仔细检查失败的运行结果并模拟软件更改会带来什么结果。

经过NQE课程的两次非常成功的完成，Golem 2在第四次尝试时再次坠毁。 这次，错误发生在路径规划器中，无法正确验证所有可能的候选轨迹，最终选择了包含两个尖锐180°转弯的退化轨迹。在所需轨迹中不可能紧密的环路导致车辆突然猛然撞入混凝土屏障。 这一事件促进了对候选轨迹的评估，以及对Golem 2转向执行器的修复和加固。

由于静态内存过度分配，Golem 2的最终和最令人痛苦的失败发生在GCE期间。 在开始时留出大量随机存取存储器用于记录障碍物，轨迹和传感器历史。 事实上，记忆被过度分配，但直到大型传感器历史积累才变得明显，由于地理坐标和传感器观测阵列的元素之间的映射，只有在覆盖了大量地形时才会发生这种情况。。 Golem 2进行了10英里左右的实验性自主运行，但从未在GCE的规模上进行过连续的陆路旅行。 此外，耐力试验包括在赛道周围长时间驾驶可能不会发现这个错误。 只有当Golem 2驾驶过22时数英里的新地形使记忆错误显现出来并使控制程序崩溃。

虽然Golem 2软件在其方法中具有固有的内存密集性，但它应该能够在1 GB以下的随机存取存储器下运行，因此原则上完全可以避免这种故障。

8.总结和未来的工作

尽管偶尔出现问题，但Golem车辆已经展示了高水平的高速驾驶性能，我们认为我们的设计方法具有前景。 该系统设法使用相对少量的速度以速度协商障碍物计算能力（单个2.2 GHz笔记本电脑）和相对稀疏的激光范围数据。

这种经济上有希望的性能的关键驱动因素包括简化的计算架构; 使用水平和垂直方向的Ladars组合来可靠地感知主要障碍，同时无视地形的不必要的细节; 快速启发式规划器，可在典型的驾驶情况下快速找到解决方案; 和车辆状态估计使用IMU和关于车辆约束的物理推理。

雷达系统有时宣布的“虚假”障碍并非完全虚假，但通常代表道路标记或轻微的可穿越障碍。 理想情况下，这些既不会被忽视也不会被误认为是不可逆转的障碍，而是放在第三类并由规划软件适当处理。 传感软件的未来发展方向是改进这些功能的处理，并改善移动障碍物的处理。