【论文翻译】Robot-Centric Elevation Mapping with Uncertainty Estimates---具有不确定性估计的机器人中心高程图映射

   本文讨论了自主机器人的局部地形映射过程。 基于机载测距传感器和现有机器人姿态估计，我们从机器人为中心的角度制定了一种新颖的高程测绘方法。 该公式可以明确地处理对于许多自主机器人而言发生的机器人姿态估计的漂移。 我们的映射方法完全结合了距离传感器测量不确定度和机器人的六维姿态协方差。 我们引入了一种计算有效的地图融合过程公式，允许以高更新速率绘制地形。 最后，我们的方法在一个走过障碍物的四足机器人上展示。

1、Introduction

    感知和映射环境的能力是允许机器人在非结构化和未知地形中导航的重要步骤。 虽然很多工作都集中在获得全局一致的地图上，但我们的重点在于检索机器人环境的局部地图上。 我们假设全局路径由人类操作员或另一算法给出，并且局部地图用于在有限的时间范围内规划机器人的运动。 鉴于机器人配备了i）距离传感器和ii）状态估计能力，我们的任务是找到考虑到距离测量和状态估计的不确定性的地形表示。

    由于我们关注的是局部规划机器人在障碍物上方或周围的运动，我们将重点放在高程制图技术上，这种技术将地形简化为二维表面。赫伯特等人提出了关于自动腿式机器人高程图生成的早期工作。 al.2和Kweon等。 al.3他们的方法建立在网格地图上，其中每个单元格代表地形的高度，以便近似地形表面。他们使用匹配算法在多次扫描之间找到相应的转换以构建复合高程图，但是它们没有解决匹配后剩余错误导致的错误传播问题。 Cremean等。 al.4开发了一种方法，将具有不确定性的距离测量融合到高度图中。当进行范围测量时，基于先前存储的数据和测量的不确定性更新落入测量区域的单元。未接收测量更新的单元格保持不变。这种方法依赖于GPS的绝对位置测量，因此不适合我们的应用。在Belter等人提出的方法中。例如，使用围绕机器人并随其运动一起移动的局部高程图。虽然我们使用类似的设置，但他们的方法依赖于良好的姿势跟踪算法，并没有解决漂移姿势估计的问题。我们的方法类似于Kleiner等人的工作。等，6，其中高程图基于机器人的运动而恶化。机器人的位置和方向的不确定性通过基于累积的距离和角度线性增长高度估计的方差而反映在地图中。这种方法保守地将姿态不确定性的近似值合并到高度方差中，而不考虑面内不确定性的影响。

    与相关工作相比，其中高程图通常与惯性框架相关，我们从机器人中心角度接近高程映射：我们的主要贡献是制定了一种映射算法，其中高程图与机器人紧密耦合。运动。这导致了重要的结果，即我们的方法能够特定地结合位姿估计的漂移。与依赖于将新扫描与现有地图匹配的方法相反，我们的公式不需要（但可以包含）从地图构建过程到状态估计的反馈循环。在第二节2，我们通过处理传感器测量不确定性和六维姿态协方差来形式化高程映射框架。第3节描述了我们在自主四足机器人上的实现，并在我们在第二节结束之前介绍了实现的结果。 4.虽然我们强调腿式机器人的应用，但所提出的方法适用于任何仅需要自身传感器提供测量信息的机器人，以便规划其运动。

2、Method

    我们从以机器人为中心的视图制定高程映射算法：相对于机器人进行距离测量，当机器人移动时，整个高程图更新有关机器人运动的信息。 在任何时候，以机器人为中心的高程图是系统从局部角度对地形的估计。 机器人前方的区域通常具有最高的精度，因为它通过前视距离传感器的新测量值不断更新。 另一方面，由于机器人的相对姿态估计的漂移，在传感器的视野之外（机器人的下方或后方）的区域已经减少了确定性。

    在下文中，我们描述了获得以机器人为中心的局部高程图的方法，该高程图由二维网格组成，其中每个单元存储高度估计h和方差σh2。

2.1Definitions

    为了描述该框架，我们引入了三个坐标系，即惯性坐标系I，传感器坐标系S和地图坐标系M（图1）。惯性坐标系I固定在环境中，我们假设真实地形相对于该坐标系是静止的。传感器坐标系S附接到距离传感器并且通过平移rIS和旋转CIS与惯性坐标系I相关。惯性坐标系I和传感器坐标系S之间的变换是通过车载状态估计获得的，并且由六维姿态协方差矩阵ΣP表征。最后，通过与传感器坐标系S的关系定义高程图坐标系M，其具有由用户指定的平移rSM和旋转CSM。我们选择旋转CSM，使得地图坐标系M的z轴和惯性坐标系I始终保持对齐（eI z = eM z）。选择最终自由度，其对应于惯性坐标系I和地图坐标系M之间的偏航角ψ（围绕eI z的旋转）以匹配I和S之间的偏航角           

 

2.2Measurement Update

    来自距离传感器的新测量值被处理为空间中的点并映射到高程图。 这导致高度图中的单元（x，y）的新高度测量结果。 在高程图框M中，高度测量值通过高斯概率分布近似为p~N（p，σp2），具有平均值p和方差σp2。 作为传感器框架S中的位置SrSP给出的单个测量可以被转换为相应的高度测量值p

    其中地图坐标系和传感器坐标系CSM之间的旋转由单位四元数q参数化。 投影矩阵P = [0 0 1]将三维测量值映射到标量高度测量值p（在地图帧M中）。 为了获得高度测量值σp2的方差，我们推导出传感器测量JS的雅可比行列式和来自（1）的传感器坐标系旋转J q的雅各比行列式

其中上标×用于表示向量的斜对称矩阵。 方差σp2的误差传播给出为

    其中ΣS表示距离传感器模型的协方差矩阵，ΣP，q表示传感器旋转的协方差矩阵。 值得注意的是，由于我们选择了高程图坐标系M（第2.1节），因此在此步骤中不需要考虑传感器ΣP，r的位置的不确定性。 此外，由于使用投影矩阵P和我们对地图坐标系M的定义，排除了传感器ψ的偏转（围绕eI z的旋转）的不确定性对测量的影响。

高度测量（p，σp2）通过卡尔曼滤波器与现有的高程图估计（h，σh2）融合：

    如果更新已经发生，那么在更新之前的估计用a-上标表示，已经更新的用a+上标表示。

    如果具有不同高度的多个测量落入相同的单元中（就像在垂直墙的情况下那样），我们采用类似于Kleiner等人提出的更新规则。 al.6根据马哈拉诺比斯距离，该规则融合了最高海拔的测量值，并使测量值低于当前估计值的某个距离。

2.3Model Update 

    由于高程图坐标系M是根据传感器/机器人的运动来定义的（第2.1节），每当机器人相对于惯性坐标系I的运动已经发生时，需要更新高程图数据。这是必要的，因为真实地形在惯性坐标系I中是静止的，并且我们想要近似移动地图坐标系M中的地形。

    根据机器人姿势估计的变化更新均值h和方差值σ2h。理想地，根据位姿估计不确定性的增量以及周围单元的估计值来更新每个单元的方差和均值。然而，对地图的每个单元执行这样的更新在计算量上是相当大的。相反，我们扩展高程图结构，其中包含有关每个单元格中x和y水平方向的方差σx2和σy2的信息。因此，我们可以收集高程图的每个点的完整三维不确定性，并将大量的融合计算推迟（第2.4节）到用户或者其它算法需要调用地图的时候。

    我们的目标是导出高程图方差[σx2, σy2, σh2]的误差传播，并将其作为传感器姿态协方差矩阵ΣP从时间k-1到k的变化的结果。 为此，我们分析了姿态不确定性对从惯性坐标系IrIP中的固定点到高程图中的表示的映射的影响，MrMP k在时间步长k：

关于传感器坐标系平移和旋转的雅可比行列式Jr和Jq可以进行评估

现在，基于位姿协方差矩阵ΣP，r和ΣP，q从时间k-1到k的差异来更新时间k的映射。

    平均高度h的估计值保持不变，因为它们仍然代表平均值的最佳估计值。 注意，当全部传感器位置协方差矩阵 ΣP,r 的变化在高程图方差上传播时，只有来自传感器旋转协方差矩阵ΣP，q（围绕eI z = eM z的旋转）的偏航 - 旋转ψ的方差是被传播的。 这是对测量更新（第2.2节）的补充，其中传感器位置协方差和传感器偏航 - 旋转方差从更新步骤中排除。

 

2.4Map Fusion

    当需要时，我们就可以将高程图数据结构（h，σh2，σx2，σy2）转换为相应的原始表示（h，σh2）。 这需要基于来自所有周围单元的数据推断出均值h和方差σh2。 

    我们根据多个种群的标准偏差组合规则制定地图融合：

通过考虑单元n位于感兴趣的单元区域中的概率，导出感兴趣的单元区域附近的单元n的权重wn。

其中Φx和Φy表示具有协方差σx和σy的累积正态分布，并且dx和dy表示单元n与正被更新的单元的距离。 变量r表示单元边的长度。

 

3、Results 

    我们已经在四足机器人StarlETH上实现了所提出的高程地图的映射方法。该系统的状态估计是基于Bloesch等人提出的运动学和惯性测量的融合。 al.8已经表明，围绕重力轴的位置和旋转（偏航角）通常是不可观察的并且因此易于漂移。我们在机器人的前部安装了一个向下的PrimeSense Carmine 结构光传感器作为距离传感器。我们使用由Nguyen al.9等人引入的传感器测量模型。例如，将深度和横向噪声模拟为测量深度的函数。实验装置显示在图2的左侧。我们添加不同高度（7-150 mm）的障碍物并手动控制机器人以步行步态以~0.05 m / s的速度在测试区域上行走。高程图设置为2.5×2.5米的大小，单元边长为1厘米。使用测量和20 Hz的模型估计更新地图。对于地图融合，该过程需要~0.3秒来融合整个高程图（Intel Core i3,2.60GHz），但通常只需要一个子地图。

    我们的设置的估计高程图显示在图2a的右侧。我们得出结论，与真实设置相比，高度信息保存得很好。 地图中包含很少的孔，这是传感器的高分辨率和高程更新的高频率的结果。 可以观察到，机器人前方的区域被清晰地捕捉到了边缘，而远离机器人的区域往往会因不确定性增加而平滑。 我们在图3中说明了这种效果，图3显示了图2中标记的区域的特写。图3的三个图像描绘了随着时间的推移（从左到右）随着机器人扫描该区域然后远离它。 可以观察到，即使在机器人的姿势不确定性增加的情况下，大的平坦区域仍保持相对低的标准偏差。 这是一个重要的功能，可用于运动规划算法中的立足点选择。

 

4、Summary and Future Work

    我们已经提出了一种用于高程映射的新方法，该方法解决了在许多自主机器人上出现的位姿估计漂移问题。 所提出的方法从以机器人为中心的感知方式来逐步展现高程图，意味着以距离传感器的精度和机器人的俯仰角和俯仰角的估计将新测量引入到地图中。 通过机器人的运动，基于机器人的位姿估计的不确定性估计来更新地图中的数据。 这为机器人在任何时间点从地方角度估计地形。 我们通过将方法分成数据收集（测量和模型更新）和地图融合步骤来降低映射过程的计算负担。 这实现了具有高更新频率的实时高程映射。我们目前正在对不同平台上呈现的映射方法进行更详细的评估和比较。