编者按：自动驾驶车辆的研究可分为感知、定位、决策、规划、控制等层面。路径规划是自动驾驶汽车系统中至关重要的一部分，它通过感知车辆周围的环境和交通状况，结合车辆当前位置和目标设定，利用路径规划算法确定一条最佳的行驶路径，并在车辆行驶中实时更新路径以应对新出现的障碍或交通状况变化，确保安全、高效地完成行驶任务。越野路面的路径规划与普通自动驾驶汽车的路径规划存在着诸多不同之处。其挑战主要源于复杂多变的地形和环境、缺乏精准的地图数据、动态障碍物和不确定性、车辆特性和稳定性考虑以及通信和定位问题。这些因素使得在越野环境中进行路径规划变得更加复杂和困难，需要系统能够动态地适应环境变化，以确保车辆的安全、稳定和高效行驶。
《Adaptive Motion Planning Based on Vehicle Characteristics and Regulations for Off-Road UGVs》
IEEE TRANSACTIONS ON INDUSTRIAL INFORMATICS, VOL. 15, NO. 1, JANUARY 2019
Ji Yonghoon1, Tanaka Yusuke2, Tamura Yusuke3, Kimura Mai4, Umemura Atsushi5, Kaneshima Yoshiharu4, Murakami Hiroki4, Yamashita Atsushi3, Asama Hajime4
摘要：本文提出了一种基于三维(3-D)地形图信息的新型越野无人地面车辆运动规划方法。以往关于车辆在崎岖地形上行驶的运动规划研究仅涉及相对较小的环境。此外，还没有考虑到车辆的独特特性，也不可能将保持行驶速度和抑制姿态变化等规定纳入其中。所提出的方法能让车辆在地形崎岖的大范围环境中，通过考虑车辆特性和规定自适应地生成路径。基于三维环境模型的随机抽样方案被应用于全局路径规划。实验结果表明，所提出的越野运动规划器可以生成适当的路径，满足车辆特性和预定义规则的要求。
关键词：自主导航，野外机器人，运动规划，无人地面车辆
近年来，自主移动机器人和无人地面车辆(UGV)吸引了众多研究人员的关注，并逐渐具备了应对各种环境的能力。安全可靠的运动规划是此类无人机器人最重要的要求之一。对于与人类互动的室内服务机器人，解决运动规划问题的大量研究已经深入开展，并建立了可靠的理论[1]-[4]。然而，尽管不可否认的事实是，在崎岖地形（如灾害现场等危险因素禁止人类进入的地方）上运行无人机器人是一项不可或缺的要求，但有关越野环境下室外运动规划方法的研究却寥寥无几。
当UGV在地形崎岖的越野环境中自主导航时，避免碰撞和翻车等事故对于安全导航的重要性无论如何强调都不为过。因此，UGV需要避免此类风险，并在可穿越区域内选择路线。为使UGV在崎岖地形上安全导航，有必要利用传感器数据估计崎岖地形的可穿越性，并执行适当的运动规划。作为在崎岖地形上估计可穿越性的一种方法，Hata和Wolf[5]采用了一种基于支持向量机(SVM)的方法，定义了一个两类分类问题。他们利用激光测距仪(LRF)获取的距离和强度数据，从目标区域提取地形特征作为SVM分类器的输入值。因此，在很难进行正确标注的情况下，可能无法产生足够的性能。在这方面，Tanaka等人[6]提出了一种无需训练数据集即可在粗糙地形上进行地形可穿越性分析(TTA)的方法。他们提取了移动机器人周围区域的三种特征，包括不平度和倾斜度信息。然后，通过这些特征值构成的成员函数，基于模糊推理计算可穿越性。Lalonde等人[7]和Bellone等人[8]提出的方案将主成分分析(PCA)应用于与周围环境相对应的点云数据，以执行地形分类任务。根据包含点云几何形状、倾斜度和不平度的PCA结果，可以分析周围环境的可穿越性。然而，这些方法的目的只是估计粗糙地形的可穿越性。因此，尚未提出基于TTA结果的UGV运动规划方法。为了解决UGV在崎岖地形上的运动规划问题，应考虑基于TTA结果的适当运动平移方案。下文将介绍前人针对UGV在崎岖地形导航方面的运动规划问题所做的研究。关于UGV的运动规划方案，由于过去几十年来计算能力的飞速提高，基于随机抽样的方法，如快速探索随机树(RRT)[9]、概率路标法(PRM)[10]和路径定向细分树(PDST)[11]，目前应用最为广泛。
在十年前举行的美国国防部高级研究项目局(DARPA)大型挑战赛上，主要问题之一是无人机车在地形崎岖、高度受控的沙漠地区自主导航[12]、[13]。许多团队应用了基于随机抽样的运动规划方法，从而在越野UGV方面取得了一些突破。在这项挑战中，事先给出了一条全局路径，每辆车在识别路况和障碍物的同时执行局部运动规划。然而，在灾难现场等实际情况下，几乎不可能预先给出整个全局路径。此外，与崎岖地形导航相关的大多数其他研究都存在前面所述的局限性。首先，没有研究考虑到每辆车的独特性。其次，以前的研究没有根据不同情况纳入UGV运行所需的规则，如保持行驶速度和抑制姿态变化。第三部分将详细介绍车辆特性和本文定义的规则。最后，由于计算负荷的限制，以往的研究仅涉及相对较小的环境。
为了克服前面提到的局限性，我们提出了一种全局运动规划的自适应方法，即在预先给定的包括崎岖地形在内的相对较大的环境地图中，考虑车辆姿态的所有自由度(DoFs)。这里的"自适应"指的是所提出的方法能够执行适当的规划，以满足根据车辆特性和规则定义的不同条件。为了根据每辆车的独特特性实现自适应运动规划，我们提出了一种新方法，即在使用树形结构进行运动规划时，定义配置障碍，将车辆的运动模型和地形信息考虑在内。此外，我们还提出了一种使用偏置采样生成控制速度的新方法，通过考虑规则来执行自适应运动规划，这也是本研究的关键目标之一。最后，利用我们提出的运动规划器，我们可以在可行的处理时间内找到解决方案，即使是在与以往研究覆盖的环境相比规模相对较大的环境中也是如此。
本文的其余部分安排如下。在第二节讨论了相关研究之后，我们在第三节明确阐述了问题和目的。第四节介绍了本研究的重点——车辆特征和规则的定义。第五节介绍了拟议运动规划器的整体结构。第六节简要概述了基于随机抽样的运动规划器。第七节介绍了随机抽样过程中的树形扩展方案，第八节详细介绍了配置障碍物设计。然后，在第九节中，通过第四节讨论的实验结果评估了针对越野UGV提出的运动规划器的有效性。最后，第十节总结本文并讨论未来工作。
有几项研究解决了车辆从初始位置安全导航到目的地时的运动规划或路径规划问题。其中，关于在崎岖地形上行驶的车辆，很少有方法能解决行星漫游车或UGV的运动规划问题。
Ono等人[14]将基于随机抽样的路径规划方法应用于行星漫游车。在该方法中，根据安装在车辆上的立体摄像机拍摄的图像分析了车辆周围粗糙地形的可穿越性，并将可穿越性的估计结果映射到数字高程模型中。在规划算法方面，他们将A*搜索算法与快速探索随机图(RRG)[15]相结合，后者是基于随机抽样的方法之一。因此，可以在估计的可穿越区域中生成一条安全路径。Lee等人[16]采用了一种基于RRT的方法，用于移动机器人在崎岖地形上行进。该方法采用二维(2-D)网格图作为环境模型，并为每个网格分配一个由TTA估算的成本值。为了生成由随机节点组成的树形结构，本研究提出了一种基于启发式的节点选择方法，该方法使用了成本值。Kuwata等人[17]也将基于RRT的方法应用于UGV的路径规划。在这种方法中，在生成随机节点时，会根据车辆的情况对采样区域进行偏置；因此，可以防止在显然无法提前成为路径解决方案的区域生成随机节点。例如，在移动机器人向右转弯的情况下，随机节点的生成集中在车辆右侧，而对包括车辆左侧在内的不必要区域的采样则被抑制，这样车辆就能有效地确定右转的路径。Richter等人[18]提出了一种在线运动规划算法方法，利用传感器测量数据对UGV的目标环境同时进行自定位和地图构建。通过这种方法，未知区域被赋予了高风险值，由于遮挡等各种因素，该区域的环境地图尚未绘制。关于未知区域风险值的分配，他们通过在模拟环境中的初步学习，获得了针对各种情况的启发式方法。因此，可以抑制车辆在不确定区域（如闭塞区域）的运动。Fassbender等人[19]开发了一种用于UGV的系统，该系统可同时进行适当的可穿越性估计和运动规划，即使在森林等复杂环境中也是如此。该系统还能独立执行自定位和映射，类似于Richter等人的建议，将可穿越性估计的结果作为搜索解决方案的条件。在考虑到车速的情况下，可以生成可行的运动。不过，上述由Ono等人、Lee等人、Kuwata等人、Richter等人和Fassbender等人提出的建议也有类似的局限性。例如，路径搜索是在假设车辆的运动是在二维平面内定义的前提下进行的，因此无法管理车辆的三维信息（即高差或姿态信息），而在崎岖地形中必须考虑这些信息。此外，它也无法生成反映车辆特性的自适应路径，如行驶速度和爬坡性能（即车辆的最大允许倾斜角，因为这些因素都没有考虑在内）。此外，它也无法根据不同情况进行规划，如保持行驶速度和抑制姿态变化。
Ishigami等人[20]提出了一种生成多个候选路径解决方案的方法，以便为行星漫游车生成安全运动。根据评估标准，从候选解决方案中选择合适的路径。他们使用Dijkstra算法进行路径搜索，生成多个候选路径。在这种方法中，在执行路径搜索时，从环境地图中计算出三类评价标准，如不平整度、倾斜度和路径长度信息，并通过改变评价标准的权重参数生成多条路径。最后，根据车辆的物理模型，对每个候选路径方案进行导航模拟，以选择能耗最小的最优路径方案。这种方法实现了路径规划，考虑了车辆高度方向和姿态的变化。不过，Ishigami等人提出的方案中的目标环境面积相对较小，仅为8平方米，在这一区域内输出路径方案所需的计算时间约为47分钟。需要注意的是，Dijkstra算法的计算复杂度与节点数有关，定义为，并且与环境的大小密切相关；因此，当以相对较大的环境地图为目标时，可靠的路径规划无法在可行的处理时间内执行，前提是路径解搜索所需的计算时间随着节点数的增加而以平方阶增加。
如引言所述，实现安全可靠地自主行驶到目的地是越野UGV在崎岖地形环境中最重要的要求之一。因此，要满足这些要求，在崎岖地形上估计可穿越性和进行适当的运动规划是非常重要的任务。在这方面，我们的目标是为UGV提出一种新的运动规划方法，以便在包括崎岖地形在内的复杂环境中安全地导航到目的地。通过总结前文所述的车辆运动规划研究，我们认为本研究应解决以下问题。因此，在设计新型运动规划器时，我们需要考虑以下几点。
1)所有DoFs，即车辆姿态（位置和方向）的6DoFs，包括高度方向、滚动角和俯仰角[见图3(a)]。
2)每辆车的独特特征，如车辆大小、最小转弯半径或可行驶的最大倾斜角，具体取决于行驶速度（详见第四节A部分）。
3)不同情况下车辆运行所需的规定，如保持行驶速度和抑制姿态变化（详见第四B部分）。
4)即使在相对较大规模的环境中，也能在合理的处理时间内确定解决方案。
如前所述，之前的研究还没有报告针对所有四个问题的解决方案。因此，本研究的目的是为越野UGV建立一种新型运动规划器，以解决上述所有问题。具体来说，当用户根据事先提供的由三维点云组成的环境地图指定UGV的初始姿态和目标姿态时，运动规划器应解决离线生成连接这两种状态的路径的问题。关于环境地图的比例尺，我们在本研究中将几百米的比例尺视为大比例尺，所提出的运动规划器也适用于这一比例尺的环境地图。当然，运动规划器必须在一个共同的环境中工作，其中包括井然有序的道路和越野环境，从而考虑到车辆的特性和预定义的规则。
A.车辆特性
必须考虑根据驾驶方案或机构设计等各种因素设定车辆特性值。越野UGV的车辆特性可分为两类：与车辆几何形状有关的特性和与车辆操作限制有关的特性。前者是定义车辆整体尺寸的数值，如总长度、宽度和高度。另一方面，后者包括由车辆设计者或用户设定的最小转弯半径或最大倾斜角度。首先，由于我们需要确定车辆可以实际行驶的路径，因此应考虑最小转弯半径，以生成可行的车辆运动。其次，关于最大倾角，随着车辆行驶速度的增加，相对于滚动和俯仰方向的最大倾角一般会减小。另一方面，当车辆低速行驶时，最大倾角会变大。这意味着，仅仅生成包含位置数据的几何路径是不够的；因此，当车辆在生成的路径上行驶时，必须同时考虑行驶速度和姿态信息，以防止翻车事故的发生。换句话说，在执行运动规划器时，必须检查路径上各点的车辆姿态是否超过最大倾斜角。综上所述，在崎岖地形上行驶的越野UGV必须在考虑车辆特性的基础上进行运动规划，以防止事故发生并安全导航。
B.车辆操作规则
此外，在解决越野UGV在崎岖地形上行驶的运动规划问题时，我们应根据目标环境和UGV运行目的考虑各种要求。这就意味着，自主驾驶系统的运行除了要产生满足上一节所述车辆特性的求解路径外，还必须遵守多项规定。例如，在某些情况下，必须通过最短的路线生成车辆到目的地的运动，同时允许剧烈的姿势变化。另一方面，在某些情况下，从确保导航安全的角度出发，必须在不允许剧烈姿势变化的情况下产生运动，而不是放弃最短路径。图1是一个概念图，显示了在崎岖地形环境中，根据前面提到的不同规则设置的运动规划器。越野UGV的运行规定不仅包括姿势变化和最短路线的要求，还包括在减少速度波动的同时尽可能保持行驶速度的要求。在后一种情况下，考虑到上一节所述的车辆特性，输出路径应在不超过与设定的理想行驶速度相对应的最大倾斜角的条件下生成。如前所述，要在各种情况下操作越野UGV，必须事先根据用户指定的一系列规定执行运动规划。
图1基于不同规则的粗糙地形运动规划概念图。
拟议运动规划器的整体流程如图2所示。车辆的初始姿态和目标姿态是运动规划器的输入数据，条件是预先给出由三维点云组成的环境地图。如果存在解决方案，规划器将输出其中一个满足预设条件（即车辆特性、规则等）的解决方案。输出数据是连接初始位置和目标位置的车辆位置列表。当然，如果初始位置和目标位置都无效（例如与障碍物发生碰撞），规划器将无法工作，因为无法搜索到解决方案。图3(a)显示了本研究中采用的车辆局部坐标系。此外，我们的运动规划器还能生成一个输出列表，其中包括车辆控制输入数据和姿态数据。这里，表示车辆在状态下的控制输入数据[即控制速度，如图3(b)所示]。在本研究中，由于允许的倾斜角和转弯半径会随车辆速度的变化而变化，如前所述，控制速度会影响根据最大倾斜角和最大转弯半径对地形上特定姿势的有效性检查。
图2概述提议的运动规划器。
图3定义与车辆相关的局部坐标系和状态。(a)取向态。(b)线速度和角速度。
基于八度的地图转换过程涉及减少三维点云数据量。如果我们使用三维点云中的原始数据，计算负担将非常大，这使得在相对大规模的环境中进行全局运动规划的问题变得非常困难。因此，有必要使用高效的三维环境模型数据结构来减少数据量。目前已开发出多种表示三维环境模型的方法[21]-[24]。其中，OctoMap库被广泛用于管理密集的三维环境模型，并被用来转换三维点云以解决这一问题[24]。OctoMap数据结构由不规则体素组成，基于八叉树结构，与基于点云的结构相比，内存管理效率非常高。使用OctoMap的一大优势是，车辆与三维环境模型之间的干扰检查问题变成了树状搜索问题，具有搜索时间长、时间复杂度低的优点。因此，在将三维点云数据转换为OctoMap格式时，可以大大减少搜索求解所需的处理时间；这样，即使在包括大量数据在内的相对大规模环境中，我们也可以在处理时间变得可行的情况下解决运动规划问题。
在基于随机抽样的规划过程中，会生成一个由三维位置和三维方位状态组成的六维配置空间。然后，执行运动规划以找到从初始配置连接到目标配置的解决方案。在这里，基于随机抽样的规划器是一种广泛使用的方法，可以有效地搜索高维配置空间。我们可以从各种基于随机抽样的规划器（如RRT、PRM、PDST等）中选择合适的算法。在基于随机抽样的算法中扩展树形结构时，我们应考虑车辆的运动模型和三维地形信息（可通过OctoMap计算），以便检查配置障碍物的干扰情况。干扰检查模块确定生成的随机节点的配置是否包含在配置障碍中。在检查随机节点的状态时，我们不仅考虑了车辆与OctoMap之间的物理干扰，还考虑了车辆的特性，这样就有可能规划出满足车辆特性和用户定义规则的运动。此外，由于基于随机抽样的算法已经考虑了运动学模型和实际车辆的特性，因此我们提出的运动规划方案不需要任何后处理，如平滑算法来处理路径解决方案，以便实际车辆可以跟随。
为了实现考虑越野UGV的6-DoF状态的实际运动规划，我们在所提出的方法中引入了配置空间的概念。配置空间由六个维度组成，包括三个位置变量和三个方向变量。执行运动规划的目的是在整个配置空间内找到一条从初始配置状态到目标配置状态的路径。配置空间中的规划方法可分为两类：离散目标空间中的规划方案和连续目标空间中的规划方案。其中，Dijkstra算法或A*搜索算法属于前者，而后者包括基于势场的方法[25]或基于随机抽样的方法，如RRT[9]、PRM[10]、PDST[11]等。前一种方法将目标空间离散化，必须将车辆每种可能状态的整个空间完全映射到配置空间，因此一般需要较长的计算时间。尤其是本研究针对的是相对大规模的环境，与计算时间相关的问题就成了一个致命的缺陷；因此，在本研究中，前一种方法无法应用于需要处理此类环境的运动规划问题。从这个意义上说，我们采用了基于随机抽样的方案来解决运动规划问题，这种方案被归类为将目标空间视为连续空间的后一种方法，因为它不需要任何预处理，即使在多维空间中也能执行高速搜索。这里的"随机抽样"是指在整个配置空间内随机生成与某个配置相对应的状态。
本节简要总结了各种随机抽样方法的共同处理过程。每种方法都有自己的特点和优势。更多详细信息请参见参考文献[9]-[11]。随机抽样在配置空间中的概念图如图4所示。在本研究中，配置对应于6-DoF车辆姿态，配置障碍代表无法实现的车辆姿态映射到配置空间的区域。运动规划问题中的路径搜索可以表述为在范围内寻找从初始配置到目标配置的路径解的问题。指的是车辆可能配置的区域集，由车辆的形状和工作区的地形决定。一般来说，这很难明确描述，因为它们在高维空间中的相互作用非常复杂。另一方面，如果给定了一个具体的配置，那么检查它是否属于或就相对容易；因此，通过有效实施验证过程，即使在高维空间中也能实现高速运动规划。第八节将详细介绍验证过程。如图4(b)所示，基于随机采样的运动规划器通过随机数生成等方法，仅在无自由空间（即不属于配置障碍的区域）内消耗与从初始配置到目标配置相对应的随机节点。此外，它还会在检查是否与配置障碍相干扰的同时，将它们串联起来。这样，就能在扩展后的树形结构中找到不干扰配置障碍物的路径解决方案，如图4(c)所示。下一节将介绍基于随机抽样的运动规划器中的树形扩展方案。
图4随机抽样过程从开始到目标配置在配置空间。
建议的方法考虑了车辆的运动模型和OctoMap中的地形信息，同时通过在配置空间中生成随机节点，在前几步的树形结构基础上扩展出新的树形结构。图5是拟议方法的树形扩展方法概览。从先前节点生成新节点的过程分为两次转换，分别根据车辆的运动模型和地形信息（即来自OctoMap的高度和倾角信息）进行。换句话说，鉴于车辆在地面上的导航控制一般在3-DoF空间内进行，因此随机抽样过程是在3-DoF配置空间内进行的，以应用车辆运动模型。其余的状态变量则利用地形信息直接计算和分配，因为OctoMap包含足够的地形高度和倾斜度信息。由随机节点组成的树状结构的每个消耗过程的细节将在后面介绍。
图5通过每个随机节点的树展开方法概述。
A.基于车辆运动模型的状态转换
如果根据车辆的移动能力生成随机节点，在配置空间中扩展树形结构进行搜索时，就能生成更符合实际的运动作为解决方案。图4(b)显示了在配置空间中扩展的树形结构。仅在车辆能够到达的位置生成节点，并将这些节点连接起来。换句话说，考虑到车辆的移动能力，可以避免在现实中不可能到达的位置随机生成节点。
为此，首先要进行随机取样，生成车辆的控制输入速度。之所以要对控制输入速度进行采样，而不是直接对节点状态进行采样，是为了将第IV-B节中描述的用户自定义规则纳入计划结果中。如果在状态采样后再计算状态之间的速度，我们就无法保证速度是否符合规定。在此，我们定义了适当的概率分布p(v)，以及各控制速度的上下限，用于生成随机数，以便在考虑规定的情况下执行运动规划。我们可以在设定的范围内进行均匀分布，作为生成控制速度的随机数生成的最简单方法。然而，如果控制速度是根据无偏差的分布（如均匀分布）生成的，则生成速度的离散性会非常大，从而导致车辆运动不可行或不稳定。特别是，如果在这种情况下每个节点的线性控制速度变成一个完全随机的值，我们就无法满足本研究的一个要求，具体来说，就是无法产生符合规定的运动，例如抑制速度的大幅变化（即保持行驶速度）。
为解决这一问题，我们提出了一种偏置采样方案，在生成线性控制速度时纳入此类规定。我们采用概率分布函数，对每个节点的线性控制速度进行偏置采样，具体如下：
这里，和分别服从均值分布，方差和均匀分布。因此，本研究根据高斯分布和均匀分布的混合概率分布进行有偏采样，如(1)-(3)所示。此外，下限速度和上限速度是根据待运行车辆的性能设定的。当范围为或时，概率变为0，这样就有可能产生不超出车辆性能的控制速度。对于角速度采样，我们采用均匀概率分布。速度上限和速度下限也是根据车辆的性能设定的，与线速度的情况相同。图6(a)显示了如何分别为线速度和角速度设置采样范围。在此范围内进行随机采样，以便为节点分配控制速度。图6(b)显示了最终概率分布的可视化，其范围[见图6(a)]在(1)中设定。
图6控制速度生成的有偏抽样方案。(a)抽样范围。(b)产生控制速度的最终概率分布。
使用图6(b)所示的概率分布进行偏置采样的优势在于：在这种方法中，最常产生接近平均值的控制速度，并将其分配给随机节点；因此，通过调整和（高斯分布(2)的参数），可以产生适当的运动，这分别反映了第三节B中所述的规定，如保持特定速度和改变其波动。另一方面，如果所有控制速度都在附近采样，则可能会因地形条件而无法确定解决方案。即使在这种情况下，建议的方法也能找到能够实现目标的解决方案，因为短时间偏离的控制值也可以通过结合均匀分布以较低的概率生成；因此，可以根据各种地形条件更灵活地搜索解决方案。用户可以通过操作均匀分布(3)来定义取样与的允许偏差。因此，新节点的状态是由运动模型根据控制速度（即线速度和角速度）从节点生成的，而控制速度是由前面所述的随机采样过程生成的。因此，在这一转换步骤中，每个节点的配置将根据车辆的运动模型进行转换，该运动模型是针对位置和偏航角的状态定义的，具体如下：
其中，是节点索引，是控制输入的时间周期。和分别表示线速度和角速度的控制输入，它们由前面所述的偏置采样生成。根据(4)-(6)，由节点额外生成一个新节点，并将其添加到树形结构中。这里，(6)中的上标"-"表示偏航角状态，即地形信息更新前的状态。需要注意的是，使用线速度和角速度作为控制输入数据的运动模型是最通用的模型。它很容易适用于各种类型的车辆（如阿克曼转向、差速转向等），因为我们只需将控制输入数据从转换为与车辆控制实例中的车辆类型相对应的其他输入数据，即可让车辆沿着生成的路径行驶。
B.基于OctoMap地形信息的状态更新
根据运动模型过渡后，将根据地形信息更新过渡节点的高度以及滚动角和俯仰角的配置。首先，该转换步骤利用OctoMap提供的高度信息更新新节点的高度状态。具体过程如下。选取在上一个过渡步骤中新生成的节点位置附近构成OctoMap的一些体素。如图7(a)所示，车辆附近区域的定义是根据车辆的大小确定的，即车辆周围的矩形区域。因此，选择覆盖节点位置周围矩形区域的体素。然后，根据下式更新新节点的高度信息，在选定的体素中获取最大高度信息
式中，和表示用于区分属于矩形区域的每个体素的索引及其总数。图7(b)显示了节点的高度状态是如何根据(7)进行分配的，以获取所选体素中的最大高度信息。
图7基于OctoMap的地形信息更新高度状态的过程。(a)属于矩形区域的体素选择。(b)更新高度状态。
图8基于OctoMap的地形信息更新方向状态的过程。(a)基于拟合平面的旋转相位。(b)车辆模型描述的更新方向状态。
接下来，新节点的方位状态也会根据OctoMap提供的倾角信息进行更新。图8显示了更新方位状态的整个过程。如图8(a)所示，该更新过程与高度信息更新步骤相同，选择属于矩形区域的体素，对每个体素的中心点执行PCA，具体如下：
其中，表示每个象素中心点的坐标。通过对协方差矩阵进行特征值分析，可分别计算出特征值和特征向量, 和。其中，表示相对于每个象素位置方差最大的方向，与正交()。因此，我们可以拟合地形平面，其法向量为，如图8(a)所示。根据这种方法，节点的方位状态将通过基于该拟合平面的旋转矩阵来计算。在第一阶段，分别计算垂直单位向量与近似平面法向量之间的乘积和角度：
在第二阶段，如图8(a)所示，连续进行两次旋转：以旋转轴为基础旋转角，以及以旋转轴为基础旋转角。这里，是偏航角，由(6)所表示的运动模型计算得出。该旋转过程可表示为两个四元数表达式的乘积，如下所示：
最后，为了更新随机节点的方位状态，我们将四元数符号转换为滚转-偏航符号如下：
如图8(b)所示，通过前面介绍的步骤，新节点的方位状态将根据OctoMap中的地形信息完全更新。
因此，每个节点对应的车辆模型都与地形紧密结合。需要注意的是，本节介绍的实现状态更新的处理步骤是一种近似处理方法，因为将车辆建模为刚体表示为矩形平行六面体并不能保证完全与非平面地形相切。一般来说，由于车辆通过橡胶轮胎与地面接触，因此更新后的状态是与地形接触时最稳定的姿态。因此，这种近似过程是有效的。
本节将详细介绍在配置空间中定义的配置障碍的设计方法。在生成随机节点以扩展配置空间中的树形结构时，必须验证节点的有效性，即节点的相应配置是否包含在配置障碍的区域内。只有那些与车辆配置相关的有效性得到保证的节点才能相互连接以扩展树形结构。因此，在本研究中，用于验证节点有效性的评价标准的设计等同于配置障碍的设计。从避免物理障碍和考虑车辆特性的角度出发，我们为配置障碍的设计设计了两个评估标准：与OctoMap的干扰检查和车辆特性检查。下文将详细介绍这两个标准。
A.使用OctoMap检查干扰
首先，评估标准根据在配置空间生成的每个随机节点的姿态，检查假定存在于节点上的车辆本身是否干扰OctoMap。如图8(b)所示，我们使用第三节A中提到的整体尺寸参数（即长、宽、高）定义了一个代表车辆几何形状的三维模型。与随机节点相对应的车辆三维模型与OctoMap之间的干扰检查可以写成如下形式：
其中，表示与节点相对应的属于车辆三维模型的任意三维位置。表示一组具有规则六面体结构的体素，构成OctoMap。在本研究中，我们利用灵活碰撞库[26]来实现车辆三维模型与构成OctoMap的体素集之间的推理检查。
B.车辆特性检查
第二个评估标准通过考虑节点的6-DoF状态和控制速度，检查生成的随机节点是否满足车辆特性所定义的条件。检查过程基于两个标准：最大倾斜角和最小转弯半径。
图9线速度和相应的最大倾角之间关系的例子。
1)最大倾角：车辆线速度与相应最大倾角之间关系的典型示例如图9所示。在这种情况下，这两者之间的关系被模拟为线性单调递减函数，条件是当车辆以更高的速度行驶时，与滚动角和俯仰角相对应的最大允许倾斜角会变小。需要注意的是，这种关系应根据车辆性能或用户意图等不同情况进行不同设置。这些评估标准可以写成以下内容：
其中，和分别表示节点的滚动角和俯仰角，它们是通过第六节B中描述的方法计算得出的；是节点对应的线速度。和表示由图9所示图形得出的最大允许倾斜角。
2)最大转弯半径：最后，设计中还应考虑最小转弯半径，以评估节点是否属于配置障碍。车辆的转弯半径定义为。因此，最小转弯半径的评估标准可定义如下：
其中，表示与节点相对应的转弯半径。表示最小转弯半径，应由用户预先确定。它的设置应考虑各种条件，如车辆的性能。
因此，只有满足(18)、(20)和(22)所有条件的随机节点才会被添加到新的树结构中。另一方面，只满足(19)、(21)和(23)中一个条件的节点会被删除。
在实验中，假定越野UGV为SuzukiJimny，它以极高的越野性能而广为人知。图10显示的是为在崎岖地形上完全自动驾驶而定制的SuzukiJimny。此外，线速度与最大倾角之间关系的函数模型如下：
这里，角度和线速度的单位分别是deg和km/h。图9是(24)和(25)的示意图。我们使用(24)和(25)来确定根据最大倾斜角生成的随机节点的有效性，同时考虑到车辆速度。换句话说，这些方程用于计算(20)和(21)评估标准中的和。请注意，这些单调递减函数的系数是根据图10所示UGV的性能和安全性设置的。相比之下，以下参数则根据规则或用户意图为每个实验设置了不同的参数，即控制输入的时间周期、线速度的上限和下限以及用于生成随机数的高斯分布参数。使用配备英特尔酷睿i7-3612QMCPU和12.0GB内存的笔记本电脑来执行拟议的运动规划器。
图10SuzukiJimny为在崎岖地形上完全自动驾驶而定制，实验中假设为越野UGV。
因此，每个节点的延伸长度取决于概率密度函数(1)所采样的控制速度。此外，除了用于生成随机节点的过程（如第七节A所述）外，目标偏置节点和相应的控制速度也会以5%的概率在朝向目标点的方向上生成。因此，有可能实现一个偏向于目的地的规划器。当然，对所有生成节点（包括目标偏置节点）的有效性检查是根据第八节所述的评估标准进行的。因此，只保留有效节点。
A.仿真环境
在模拟实验中，我们生成了人工越野环境的三维点云，如图11所示，以验证所提出的越野运动规划器的有效性。整个信息地图的大小约为200×200m2，其点云包含数百万个点。因此，与之前的研究相比，可以认为这是一个相对较大的环境。此外，该环境中部有一座高山，还有大量崎岖地形；因此，规划者需要根据车辆性能或预定义的规定来决定是避开高山还是直接通过高山。在此，我们对同一环境下的两种速度调节模式进行了实验：基于10km/h的速度采样（模式1）和基于30km/h的速度采样（模式2）。在模式1的实验中，由于采样的线速度较低，约为10km/h，因此计划程序在通过崎岖地形时应允许剧烈的姿态（即翻滚角和俯仰角）变化。与模式1形成鲜明对比的是，模式2的运动规划器应抑制剧烈的姿态波动；因此，由于随机生成了约30km/h的高线速度，它应产生一种能避开陡峭地形的运动。表1总结了根据这些规定，每个实验条件下的一组可变参数。在模拟环境实验中，初始位置和目标位置分别设置在环境的左下方和右上方，以便在尽可能宽的区域内产生运动。选择PDST[11]计划器作为运动计划器。模式1和2的实验结果如图11所示。在这两种情况下，找到解决方案所需的时间都在5秒以内，这是可行的处理时间，而生成的构成树的节点总数分别为7873个和6259个。其中，输出路径上的节点数分别为251和87。它们的长度分别为366.0米和346.3米。
表1各实验条件下的参数
图11仿真环境下的实验结果。(a)在模式1的情况下，所有生成的节点，输出路径作为解，每个节点在生成的运动上的几个变量的变化。(b)在模式2的情况下，所有生成的节点，输出路径作为解，每个节点在生成的运动上的几个变量的变化。
根据前面提到的预定规则，在模式1的情况下，在地图中部有起伏山脉的区域生成了许多随机节点；因此，如图11(a)所示，生成了一条穿过起伏环境的解算路径，因为在线速度约为10km/h时，滚动和俯仰方向的允许倾斜角相对较大，如图9、(24)和(25)所示。另一方面，与模式1相反，使用模式2参数的运动规划器无法在地图中心部分生成随机节点，因此会绕道避开起伏的山体，如图11(b)所示，这是因为在线速度约为30km/h时，滚动和俯仰方向的允许倾斜角相对较小。图11中的图表说明了产生运动时每个节点的变化，即高度方向、线速度、滚动角和俯仰角的变化。需要注意的是，所有节点都没有超过根据(24)和(25)计算出的允许倾斜角和。
图12显示了基于模式1和模式2各参数的重复实验结果。我们将每个实验重复了10次。模式1的大部分结果都穿过了中心部分起伏的山体。另一方面，模式2的所有结果都绕过了该区域。产生解决方案所需的时间大约在3-6秒之间。此外，重复实验结果所需的时间和路径长度的标准偏差都不大。此外，我们还成功地在一个更大的越野环境(1000×1000m2)中进行了额外的实验，实验条件是需要相当长的路径作为解决方案。识别解决方案所需的时间都在200秒以内，这是一个相对可行的处理时间。当然，计算时间取决于环境的大小。不过，与第二节所述的Dijkstra算法相比，我们的方法不会导致计算时间随节点数的平方而增加。因此，上述结果表明，我们可以设计一种新型运动规划器，它可以在一个相对较大的越野环境中，通过考虑所有的DoFs、车辆特性和预定义的规则，在可行的处理时间内生成运动。
图12模式1(红色路径)和模式2(蓝色路径)使用每个参数集重复实验结果。
B.真实环境
为了验证其在典型的有组织户外环境中的适用性，我们在一个既有未铺设路面区域又有铺设路面的真实环境中进行了额外的实验，如图13所示。我们使用移动测绘系统绘制了环境信息地图（即三维点云数据），如图10所示，该系统事先配备了全球定位系统和Velodyne激光成像探测和测距(LiDAR)HDL-32E传感器。激光雷达传感器安装在车辆坐标框架Z轴上2.4米的高度。HDL-32E的规格如表2所示。所建地图信息的整体大小为208×184m2，其点云包含4,615,845个点。我们在各种条件下进行了大量实验，如改变初始姿态和目标姿态、车辆参数、规定和规划器选择等。其中，图14和图15显示了在不同初始位置和目标位置下的实验结果，分别应用RRT[9]和PDST[11]规划器对表一所列的模式1参数进行了计算。结果详情如下。关于RRT规划器[见图14]，确定解决方案所需的时间分别约为1秒和3秒。代表整套树的节点总数分别为707个和1980个。输出路径解的总数分别为60和50。路径长度分别为82.5米和70.2米。代表整套树的节点总数分别为1355和1287。输出路径解的总数分别为58和49。最后，路径长度分别为79.2米和67.1米。在这些实验中，使用的是真实数据，由于人行道和未铺设路面之间有低土堆，因此大部分解路径都是沿着人行道生成的。不过，也有一些节点是在人行道和未铺设路面的区域生成的，这些区域足够平坦，车辆可以行驶。结果表明，一些土堆足够低，允许车辆应用模式1的条件进行突破。
表2velodyne激光雷达传感器hdl-32e的规格
图13实际实验环境包括铺装区和未铺装区。
图14将RRT[9]应用于真实环境的实验结果。(a)生成节点。(b)作为解决办法提出的输出路径。
图15PDST在实际环境中的应用实验结果[11]。(a)生成节点。(b)作为解决办法提出的输出路径。
与上一小节所述的模拟情况相比，在包括有序道路的环境中，只需很短的处理时间就能找到节点较少的解决方案路径。因此，建议的运动规划器在有组织的室外环境中也表现出色。
本文提出了一种新颖的越野UGV运动规划器，该规划器考虑了不同情况下的规定以及每种车辆的独特特性。由于作为各节点元素的和的状态生成过程是以随机抽样的方式解耦的，因此即使在相对大规模的环境中，所提出的运动规划器也能在可行的处理时间内确定解决方案。需要注意的是，在较大的配置空间内搜索6-DoF元素需要过多的计算时间。通过在粗糙地形的模拟环境和包括有组织人行道在内的真实环境中进行大量实验，对所提出的运动规划方案的有效性进行了研究。得出以下结论
1)提议的运动规划器在随机节点采样时考虑了车辆的运动模型和来自OctoMap的地形信息，因此可以生成反映车辆运动能力的路径。
2)由于采用了一种新颖的偏置采样方案来生成控制速度数据，该方案基于用户定义的自适应概率分布，因此用于生成自适应路径的运动规划器能很好地将其考虑在内。
3)由于我们根据车辆特性和配置空间中的碰撞检查设计了评估标准，因此生成了一条既能满足车辆特性又能避开物理障碍的解决路径。
最后，与本研究相关的未来工作如下。在本研究中，运动规划器是在目标环境的地图信息是预先给定的假设条件下实现的。然而，并不能保证预先给出的地图与实际环境完全匹配。此外，当车辆沿着生成的路径行驶时，还应考虑车辆定位误差。因此，我们将开发一种更稳健的运动规划方案，以管理来自地图信息和车辆定位的不确定性。此外，我们还可以考虑采用一种新策略，从众多路径方案中选择最佳方案。鉴于我们提出的规划方案能够在可行的处理时间内确定一个解决方案，因此相同的初始配置和目标配置之间的规划过程可以重复多次。此后，用户可以根据用户定义的成本函数，选择一条最合适的路径，该函数可评估各种物理量，如长度、平滑度、工作量和燃料消耗。
参考文献
| 联系人：唐老师 |
| 电话：13917148827 |
| 邮箱：tangyanqin@tongji.edu.cn |
点“阅读原文”获取论文
