路径规划

技术领域

本发明涉及路径规划，尤其涉及一种用于确定起点与目标点之间的无障碍物路径的方法和系统，特别是用于移动机器人装置。

背景技术

移动机器人装置变得越来越普遍且被使用在多种领域，如太空探索、远程呈现、家庭协助、割草和地面清洁。近来，家用机器人领域有了飞速发展，其主要目的是在用户的房屋中自主导航，并进行吸尘和清洁等操作，同时要求人类用户提供尽可能少的帮助，优选地没有帮助。

在执行此类任务时，移动机器人装置必须能够在其环境中自主导航和越过障碍物。通常情况下，移动机器人装置被配置为使用路径模式沿着已建立的路径行进。在路径上遇到障碍物时，移动机器人装置将进行重新调整和重新操纵，以达成绕过障碍物的途径，直到它可以恢复该路径为止。随着移动机器人装置的进步，越来越强调在移动机器人装置导航环境之前执行的提前路径规划。替代移动机器人简单地遵循模式然后在遇到障碍物时临时处理它们，取而代之的是提前规划出路径，该路径考虑了已知位于环境中的障碍物。通过以这种方式提前规划路径，移动机器人装置能够更快速、更有效地导航环境，从而延长电池运行时间并最大化覆盖范围。

路径规划在本领域中是众所周知的，并且有许多可用的方案。但是，路径规划方案通常需要大量的处理时间和功率。因此，需要功能强大的处理器来执行既昂贵又耗电的路径规划，或者该过程可能要花费大量时间才能完成。这会使路径规划系统变得昂贵，并且还难以在移动机器人装置上实现，特别是如果移动机器人装置是例如机器人真空吸尘器的消费机器人装置。

路径规划的一个示例在WO2016/050274A1中描述。这描述了一种路径规划方案，该方案计算了起点和终点之间的所有可能路径，这些路径穿过了两者之间的一系列可能节点。然后排除那些被障碍物阻塞的路径中的任何一条，并计算与剩余路径中的每条路径相关联的成本函数。然后从剩余的未排除路径中选择具有最低成本函数的穿过可能节点范围的路径。尽管该方案在找到避免所有障碍物的有效路径方面是有效的，但是与计算所有可能路线以及计算成本函数相关的处理要求是相当大的。

因此，需要改进的路径规划方案，该方案在确定无障碍物路径时仍然有效，但是需要少得多的处理能力。

发明内容

本发明的第一方面提供了一种确定起点与目标点之间的无障碍物路径的方法。该方法包括以下步骤：

·提供多个路径点(waypoint)，其中所述路径点在起点和目标点之间线性分布；

·确定第一路径点位于起点、最后一个路径点位于目标点的路径点序列；以及

·从第二路径点到最后一个路径点迭代地处理序列中的多个路径点中的每个。

对多个路径点中的每个路径点进行处理的步骤包括：识别最接近感兴趣路径点的障碍物并计算该感兴趣路径点和所识别的最接近障碍物之间的第一距离，并且如果第一距离小于间隙阈值，则随后更改感兴趣路径点的位置。

因此，实现了一种确定无障碍物路径的方法，该方法将路径规划任务划分为多个较小的过程，这些过程可以快速且容易地进行处理。因此，与其它路径规划算法相比，可以使用低功率处理器来执行该方法。这又增加了可以在移动机器人上执行这种方法而无需为移动机器人配备高功率或高性能处理器的可能性，配备高功率或高性能处理器可能会大大增加移动机器人的成本。而且，当由电池供电的装置执行该方法时，由于执行该方法所需的处理器的低性能要求，可以延长装置的电池寿命。

如果第一距离小于间隙阈值，则更改感兴趣路径点的位置的步骤可以包括以下步骤：

·计算一组点，这些点既定位在与感兴趣路径点的上一个路径点相距第一预定距离处，又定位在与所识别的最接近障碍物相距第二预定距离处；

·从一组点中选择优选点；和

·将感兴趣路径点的位置更改为优选点。

因此，可以实现一种用于识别感兴趣路径点的新位置的快速方法，该方法基于简单的几何分析并且几乎不需要处理能力。

计算一组点的步骤可以包括计算第一形状和第二形状之间的相交点，其中，第一形状定心于感兴趣路径点的上一个路径点处，并且其半径等于第一预定距离，第二形状定心于所识别的最接近障碍物，并且其半径等于第二预定距离，并且其中，相交点被视为一组点。第一和第二形状可以是圆形或球形。这提供了可易于用于识别相交点的位置的简单几何形状。此外，可以根据需要设置圆形或球形的半径，以匹配需要沿路径导航而不接触障碍物的移动车辆的间隙距离。

第一预定距离可以等于路径点之间的步长。因此，路径点的新位置将始终与上一个路径点保持步长的距离，从而降低了在路径上行进的车辆需要行进路径点之间的较大距离的风险，而对于路径点之间的较大距离，尚未进行分析以清除障碍物。

第二预定距离可以等于间隙阈值。因此，路径点的新位置肯定将至少远离障碍物一间隙阈值，并且路径点的新位置将确保路径避开障碍物。

选择优选点的步骤可以包括计算一组点中的每个计算点与水平点(horizonpoint)之间的距离，以及选择最接近水平点的点。水平点可以位于连接感兴趣路径点和目标点之间的路径点序列中的两个连续路径点的线上。因此，选择了最合适点以确保路径尽可能接近起点和目标点之间原始的、未经处理的路径点的线性路径。

迭代地处理序列中的多个路径点中的每个的步骤可以进一步包括：计算感兴趣路径点与序列中上一个路径点之间的第三距离；以及将感兴趣路径点和上一个路径点之间的第三距离与预定步长阈值进行比较。如果感兴趣路径点与上一个路径点之间的第三距离大于预定步长阈值，则可以将感兴趣路径点的位置更改为位于连接所述上一个路径点和序列中下一个路径点的线段上的点，所述点位于线段上，与上一个路径点之间的距离等于预定步长。因此，路径点将保持与上一个路径点相距步长，从而降低了在路径上行进的车辆需要行进路径点之间的较大距离的风险，而对于路径点之间的较大距离，尚未进行分析以清除障碍物。

上一个路径点可以是从起点到感兴趣路径点的序列中的任何路径点，而下一个路径点是从感兴趣路径点到目标点的序列中的任何路径点。

该方法可以进一步包括：在将路径点的位置更改为优选点之后，识别最接近感兴趣路径点的新位置的障碍物，并计算感兴趣路径点与所识别的最接近障碍物之间的第四距离，然后，如果第四距离小于间隙阈值，则从序列中删除该感兴趣路径点。因此，如果路径点的新位置使路径点太靠近另一障碍物，则新位置不会在规划的路径内使用，因为新路径点将被放弃。这确保了通过该方法确定的路径保持无障碍物。

该方法可以进一步包括：在将感兴趣路径点的位置更改为优选点之后，计算感兴趣路径点与序列中的上一个路径点之间的第五距离的附加步骤；计算感兴趣路径点与序列中的上一个路径点之前的路径点之间的第六距离；以及将第五距离与第六距离进行比较。如果第六距离小于第五距离，则可以从序列中移除感兴趣路径点。因此，如果感兴趣路径点的新位置将路径带回序列中的较早路径点，并因此将路径引向自身而不是目标点，则可以将其从序列中移除。

本发明的第二方面提供了一种系统，包括：

·存储器，其存储与环境有关的数据，包括位于环境中的任何障碍物的位置；

·存储器，其存储与路径规划有关的数据，包括起点和目标点的位置、多个路径点、所述多个路径点的序列和间隙阈值；和

·路径规划引擎，其包括至少一个处理器，并且被配置为迭代地处理所述多个路径点中的每一个，以确定起点与目标点之间的无障碍物路径。

为了处理多个路径点中的每一个，路径规划引擎被配置为识别最接近感兴趣路径点的障碍物，计算感兴趣路径点与所识别的最接近障碍物之间的第一距离，如果第一距离小于间隙阈值，则随后更新与感兴趣路径点的位置相对应的数据以更改其位置。

因此，该系统能够通过将起点和目标点之间的路径分成多个可单独处理的离散路径点来确定起点和目标点之间的无障碍物路径。这减少了对处理器的需求，并且可以使用低功率处理器。继而，这增加了可以在移动机器人上实施这种系统而无需为移动机器人配备高功率或高性能处理器的可能性，配备高功率或高性能处理器可能会大大增加移动机器人的成本。此外，当在电池供电的装置上实施该系统时，由于处理器的低性能要求，可以延长装置的电池寿命。

为了更改感兴趣路径点的位置，路径规划引擎可以配置为：

·计算一组点，这些点既定位在与感兴趣路径点的上一个路径点相距第一预定距离处，又定位在与所识别的最接近障碍物相距第二预定距离处，该第一预定距离和第二预定距离存储在存储器中；

·从一组点中选择优选点；和

·更新与感兴趣路径点的位置相对应的数据，以将其位置更改为优选点。

因此，仅需要路径规划引擎执行简单的几何分析即可更改感兴趣路径点的位置。路径规划引擎不需要大量的处理器能力来实现这一点，因此路径规划引擎可以更快地规划路径，和/或可以使用更便宜、功能更弱的处理器。

为了计算一组点，路径规划引擎可以配置为计算第一形状和第二形状之间的相交点，其中，第一形状定心于感兴趣路径点的上一个路径点处，并且其半径等于第一预定距离，第二形状定心于所识别的最接近障碍物，并且其半径等于第二预定距离，并且其中，所述相交点被视为一组点。第一和第二形状可以是圆形或球形中的一种。圆形或球形提供了简单的几何形状，其允许进行快速几何分析以识别相交点的位置。此外，可以根据需要设置圆形或球形的半径，以匹配需要沿路径导航而不接触障碍物的移动车辆的间隙距离。

第一预定距离可以等于路径点之间的步长，该步长被存储在存储器的与路径规划相关的数据中。通过这种方式，路径点的新位置将始终与上一个路径点相距固定的步长，从而降低了在规划的路径上行进的车辆需要行进路径点之间的较大距离的风险，而对于路径点之间的较大距离，尚未进行分析以清除障碍物。

第二预定距离可以等于间隙阈值。以此方式，可以确定路径点的新位置将至少是远离障碍物的间隙阈值，并且路径点的新位置将确保规划的路径避开障碍物。

为了选择优选点，路径规划引擎可以配置为计算一组点中的每个计算点与水平点之间的距离，然后选择最接近水平点的点。水平点可以位于连接感兴趣路径点和目标点之间的路径点序列中的两个连续路径点的线上。因此，路径规划引擎配置为选择最合适点以确保路径尽可能接近起点和目标点之间原始的、未经处理的路径点的线性路径。

为了迭代地处理多个路径点中的每一个，路径规划引擎可以配置为：计算感兴趣路径点与序列中上一个路径点之间的第三距离；以及将感兴趣路径点和上一个路径点之间的第三距离与预定步长阈值进行比较。如果感兴趣路径点与上一个路径点之间的第三距离大于预定步长阈值，则路径规划引擎可以配置为更新与感兴趣路径点的位置相对应的数据，以将感兴趣路径点的位置更改为位于连接所述上一个路径点和序列中下一个路径点的线段上的点，所述点位于线段上，与上一个路径点之间的距离等于预定步长。因此，路径点将与上一个路径点保持相距步长，从而降低了在路径上行进的车辆需要行进路径点之间的较大距离的风险，而对于路径点之间的较大距离，尚未进行分析以清除障碍物。

上一个路径点可以是从起点到感兴趣路径点的序列中的任何路径点，而下一个路径点可以是从感兴趣路径点到目标点的序列中的任何路径点。

在更新与感兴趣路径点的位置相对应的数据以将其位置更改为优选点之后，路径规划引擎可以进一步配置为：识别最接近感兴趣路径点的新位置的障碍物，并计算感兴趣路径点与所识别的最接近障碍物之间的第四距离；如果第四距离小于间隙阈值，则更新与路径规划相关的数据以从序列中移除该感兴趣路径点。因此，如果路径点的新位置使路径点太靠近另一障碍物，则路径规划引擎不会使用规划路径内的新位置的路径点，因为该路径点将被放弃。这确保了通过路径规划引擎确定的路径保持无障碍物。

在更新与感兴趣路径点的位置相对应的数据以将其位置更改为优选点之后，路径规划引擎还可以配置为：计算感兴趣路径点与序列中的上一个路径点之间的第五距离；计算感兴趣路径点与序列中的上一个路径点之前的路径点之间的第六距离；以及将第五距离与第六距离进行比较。如果第六距离小于第五距离，则路径规划引擎可以配置为更新与路径规划有关的数据，以从序列中移除感兴趣路径点。因此，如果感兴趣路径点的新位置将路径带回序列中的较早路径点，并因此将路径引向自身而不是目标点，则路径规划引擎将其从序列中移除。

本发明的第三方面提供了一种机器人装置，包括：

·与前述关于系统的任何陈述相关的系统；

·一个或多个致动器，以使机器人装置能够在周围的三维环境中导航；和

·导航引擎，其包括至少一个处理器以控制所述一个或多个致动器，其中，导航引擎使用存储在存储器中的与环境有关的数据和与路径规划有关的数据来沿周围三维环境内的无障碍物路径导航机器人装置。

本发明的第四方面提供了一种包括计算机可执行指令的非暂时性计算机可读存储介质，当由处理器执行时，计算机可执行指令使计算装置执行与确定无障碍物路径的方法有关的前述陈述中的任一项的方法。

附图说明

为了本发明可被更容易地理解,本发明的实施例现在将要参考下面的附图通过实例而被描述，其中：

图1示出了机器人装置的示例；

图2是图1的机器人装置上的系统的示意图；

图3是图2的系统的存储器的示意图；

图4是图1的机器人装置的地面清洁系统的示意图；

图5是图1的机器人装置的驱动系统的示意图；

图6示出了需要机器人装置行进的环境的图示；

图7至图24示出了在对图6的环境图示进行路径规划操作期间的各个步骤；

图25示出了在图7至24中执行的路径规划操作的最终结果，其中原始路径与更新后的处理路径一起显示；以及

图26至29是流程图，详细说明了在算法路径规划操作期间执行的迭代步骤。

具体实施方式

图1示出了机器人装置的示例。机器人装置是机器人真空吸尘器1，并具有地面清洁系统，该地面清洁系统包括旋风分离系统2和清洁器头4。设置在机器人真空吸尘器1的主体内部的真空电机(未示出)从清洁器头4通过旋风分离器2抽吸脏空气，在旋风分离器2处，污垢颗粒从气流中去除，然后通过机器人真空吸尘器1背面的排气孔(未示出)排出干净的空气。机器人真空吸尘器1具有坦克履带6形式的驱动致动器，该驱动致动器可被驱动以使机器人真空吸尘器1在其所处的环境中移动。机器人真空吸尘器1具有视觉传感器8，该视觉传感器能够捕获机器人真空吸尘器1周围的环境的图像。机器人装置的控制系统在视觉传感器8捕获的图像上使用同步定位和地图绘制(SLAM)技术，以建立环境地图并在该地图中识别机器人装置的位置。由控制系统执行的SLAM技术还使用从驱动致动器提供的里程测量，以及位于旋风分离器52两侧的传感器壳体9中的其他传感器(例如接近传感器)提供的信息传感器是例如。

图2是图1的机器人装置1上的系统20的示意图。系统20具有控制系统22、任务执行系统23、驱动系统24和存储器25。尽管被分开示出，但存储器25可以可替代地是控制系统22的一部分。控制系统22包括路径规划引擎26、导航引擎27和处理器20。

如图3示意性所示，存储器25存储机器人装置所需的所有数据，并包括环境数据32和路径规划数据34。环境数据32可以包括例如机器人装置周围的环境的地图，以及与该环境内的任何障碍物的位置有关的数据。该数据可以是机器人装置在环境内的上一次导航操作期间收集的数据，或者可以使用例如由其他装置先前生成或绘制的环境的3D表示，将该数据预编程到存储器中。路径规划数据34可以包括与机器人装置的期望路径、期望路径的起点和目标点的位置、起点和目标点之间的中间路径点以及所述多个路径点的序列有关的信息。还可以包括与路径规划有关的其他数据，例如间隙阈值，该间隙阈值说明了路径点和障碍物之间可能的最小距离，并且可能特定于机器人装置。

任务执行系统23是机器人系统20的一部分，负责执行分配给机器人装置的任务或操作。在图1的机器人装置中，任务执行系统23是地面清洁系统。地面清洁系统40的示意图在图4中示出。地面清洁系统40包括清洁器头42、分离系统44和真空电机46。地面清洁系统的这些特征是地面清洁系统的普通特征，这里将不提供对这些系统的进一步说明。在替代实施例中，例如，机器人装置可以是机器人割草机，在这种情况下，任务执行系统23可以是割草和/或收集系统。任务执行系统的其他示例将显而易见。

导航引擎27与驱动系统24一起使机器人装置能够在环境中进行操纵和导航。图5提供了驱动系统24的示意图。驱动系统24设置有驱动致动器50和多个导航传感器52A-E。驱动致动器50可以是例如从动轮或坦克履带，并且能够将里程测量提供给机器人系统20的控制系统22。这些里程测量可以由控制系统22，特别是导航引擎27使用，以估计移动机器人装置行进的距离和路径。里程数据还可以由其他传感器(例如光流量传感器)提供。导航传感器52A-E是能够向控制系统22提供关于机器人装置周围的环境的附加信息的传感器。例如，导航传感器52A可以是视觉相机，并且导航传感器52B和52C可以是例如飞行时间(TOF)传感器的接近传感器，并且导航传感器52D和52E可以是跌落传感器或悬崖传感器。其他导航传感器可以被附加使用或者替代这些传感器。例如，机器人装置可以包括激光测距仪。在替代实施例中，导航传感器52A-E可以形成机器人装置20的导航引擎27的一部分。机器人装置的机器人系统20通常将使用多种不同类型的导航传感器，以便能够更成功地自主导航环境。导航传感器52A-E向控制系统22提供关于机器人装置周围的环境的信息，该信息使控制系统22能够建立可以被机器人装置用来导航的环境地图。该环境地图可以存储在机器人系统20的存储器25中。图3示出了机器人系统20的存储器25的示意图。示出的存储器25包含环境数据32。环境数据32包括上述环境地图，并且还包括位于环境中的任何障碍物的位置。路径规划数据34也存储在存储器25中。路径规划数据34包括机器人系统20为了在导航环境之前执行路径规划所需的任何数据。可以包括在路径规划数据34中的数据的示例是起点和目标点、多个中间路径点、所述多个路径点的序列以及间隙阈值。这些数据如何用于路径规划将在下面更详细地描述。

如本文通篇所使用的术语“障碍物”旨在包括可能引起环境中的移动机器人装置的可操纵性障碍的任何物体或危险。例如，术语“障碍物”包括旨在放置在地板表面上的物理物体和物品，例如家具、家庭用品、鞋类等。此外，“障碍物”还包括虚拟对象(例如用户定义的禁区或虚拟对象)，并且还可以包括环境本身的某些方面，这些方面可能会对移动机器人装置造成困难，例如跌落、楼梯、区域边界等。

现在将描述确定无障碍物路径或路径规划的方法。该方法是算法的，并且将由机器人系统20的控制系统22中的路径规划引擎26来执行，如前所述。在替代实施例中，尽管不是优选实施例，但该方法可以由路径规划引擎在单独的计算设备上执行，然后可以将所得的路径数据传输至机器人装置的机器人系统。

图26至29是流程图，其阐明了在总体路径规划算法期间执行的多个迭代阶段的步骤。在图6至24的环境100中执行的路径规划操作中例示了在图26至29的流程图中列出的各个步骤。路径规划算法迭代地处理序列中的每个路径点wp1至wp7。图26至29的流程图的每一种方法都在处理每一个路径点期间执行，但是为清楚起见，在图6至25中未示出某些步骤。图25示出了路径规划算法的最终结果，调整后的路径由标记为wp1-7的圆形路径点表示，并与由十字形路径点表示且标记为owp1-7的线性路径上的原始路径点一起显示。

现在将描述图26至29的方法，然后将在后面说明如何在图6至25所示的路径规划操作中应用这些方法。

图26是概述一些初始方法步骤的流程图，其中包括计算水平点。水平点将在该方法的后续步骤中使用，尽管不是必需的，但在开始时计算水平点是有用的。该方法开始于步骤260，该步骤选择当前路径点(wp

但是，如果要处理路径点wp

但是，如果当前路径点wp

图27说明了step_length检查的步骤。该方法确保了路径点保持与上一个路径点相距预定的step_length阈值，这又降低了随后在该路径上行进的车辆需要行进路径点之间的较大距离的风险，因为这些较大距离尚未进行分析以清除障碍物。在步骤271中，计算当前路径点wp

但是，如果在步骤272中，距离D1大于step_length阈值，则在步骤273中移动当前路径点wp

图28说明了确保路径保持与环境内的任何障碍物相距至少设定距离的方法的步骤。在步骤281中，识别与当前路径点wp

然而，如果在步骤283中距离D2小于minimum_clearance阈值，则该方法继续进行到步骤284，在该步骤中执行两个圆的相交。尽管图28的方法描述了两个圆的相交，但是应该理解，这仅仅是一个特定的实施例。实际需要的是计算一组点，其中这组点既定位在与上一个路径点wp

步骤285确定是否从步骤284返还了两个相交点ic。如果仅返还了一个点，则没有必要对优选点进行选择，并且该方法直接进行到步骤288，在步骤288中，更改当前路径点wp

但是，如果从步骤284返还了一个以上的相交点ic，则在步骤286中，为每个相交点ic计算距离H，其中距离H是相交点ic与先前在图26的方法中计算的水平点之间的距离。然后，在步骤287中，通过选择与水平点具有最小距离H的相交点，来选择两个相交点中的优选点。之所以这样选择是因为，通过选择与水平点具有最小距离H的相交点为优选点可以使规划的路径尽可能接近原始未经处理的路径。一旦选择了优选点，该方法继续进行到步骤288，在步骤288中，更改当前路径点wp

然后该方法可以继续进行到步骤289，该步骤依次选择下一个路径点wp

一旦当前路径点wp

然而，如果在步骤292中距离D3大于minimum_clearance阈值，则该方法继续进行到步骤294，在步骤294中，检查当前路径点wp

如果当前路径点不是目标点，则明智的做法是确保当前路径点wp

但是，如果在步骤297的比较中距离D5大于距离D4，则可以将当前路径点wp

图6示出了机器人装置行进的环境100的图示。起点和目标点用大X符号表示，并且障碍物110位于环境100中，机器人装置将需要在该环境中操纵。标记为wp1-wp7的路径点序列在起点和目标点之间均匀且线性分布，其中第一个路径点wp1位于起点，最后一个路径点wp7位于目标点，中间路径点wp2-wp6位于wp1和wp7之间的线性路径中，并用小x符号表示。

在处理每个路径点期间，执行图26至29的流程图的每个方法。但是，为清楚起见，该方法的某些步骤未在图6至25中示出。

起点或路径点wp1不需要进行处理，因为假设由于机器人装置从那里开始，所以它可以在那里出现而不会与障碍物重叠。因此，要处理的第一个路径点是wp2。图7示出了在处理第二个路径点wp2期间的环境。在每个图中，正在处理的当前路径点由用方框包围的x符号表示。在图7中，路径点wp5被标记为水平点。该水平点是根据上述图26的方法计算的。在此示例中，horizont_step已设置为三个路径点长度，因此，当前水平点已计算为wp

图8示出了处理wp3的步骤。在此，wp3仍在wp2的step_length阈值内。然而，通过障碍物110落在虚线圆内的方式可以清楚地看出，wp3与障碍物110之间的距离(等于图28中的距离D2)小于minimum_clearance阈值。因此，必须更改路径点wp3的位置。图9示出了与图28的步骤284中描述的两个圆相对应的两个圆C1和C2。圆C1以上一个路径点wp2为圆心，其半径R1等于step_length阈值。圆C2以障碍物110为圆心，其半径R2等于minimum_clearance阈值。两个圆C1和C2产生两个相交点ic1和ic2。为了确定相交点ic1和ic2中的哪一个是优选的相交点，计算相交点ic1、ic2中的每一个与现在已经被计算为wp6的水平点之间的距离A和B。由于距离B是距离中的较短者，因此将相交点ic2选择为优选点，并将wp3的位置更改为ic2所在的位置，如图10所示。尽管未示出，但是现在在wp3的新位置上执行图29的方法，一旦执行了该方法，wp3就保持在原位置。

图11示出了处理wp4的步骤。再次，通过障碍物110落在虚线圆内的方式可以清楚地看出，wp4与障碍物110之间的距离(等于图28中的距离D2)小于minimum_clearance阈值。因此，必须更改路径点wp4的位置。图12再次示出了与图28的步骤284中描述的两个圆相对应的两个圆C1和C2。和之前处理wp3时一样，圆C1以上一个路径点(现在是wp3)为圆心，其半径R1等于step_length阈值。圆C2以障碍物110为圆心，其半径R2等于minimum_clearance阈值。两个圆C1和C2产生两个新的相交点ic3和ic4。为了确定相交点ic3和ic4中的哪一个是优选的相交点，计算相交点ic3、ic4中的每一个与现在已经被计算为wp7的水平点(也是目标点)之间的距离C和D。由于距离D是距离中的较短者，因此将相交点ic4选择为优选点，并将wp4的位置更改为ic4所在的位置，如图13所示。图13还示出了wp4的新位置至少距离障碍物minimum_clearance，并且由于障碍物110仍然是最接近wp4的新位置的障碍物，所以不需要放弃路径点wp4。

图14、15和16再次示出了用于处理wp5重复的相同过程。这次，两个圆C1和C2产生了相交点ic5和ic6，并且ic6的位置被选为优选点，当距离F小于距离E时，路径点wp5应该移动到该优选点。水平点仍为wp7，因为这是序列中的最后一个路径点，并且不存在可能是水平点的其他路径点。

图17显示了对路径点wp6的处理开始。从图17可以清楚地看到，最接近障碍物(仍然是障碍物110)和wp6之间的距离大于由虚线圆表示的minimum_clearance阈值。但是，wp6与wp5的距离相当大。现在将参考图18和对路径点wp6的处理来演示图27的方法。wp6和wp5之间的距离(由标记为G的虚线表示)大于step_length阈值，该阈值以虚线G旁边的双头箭头显示。因此，当前路径点wp6被移动到沿着连接wp5和wp6的虚线G的点。wp6移动到的点与wp5的距离等于step_length阈值。

wp6的新位置如图19所示，但不幸的是，wp6的新位置导致从wp6到其最靠近障碍物的距离小于minimum_clearance阈值。因此，必须在wp6的新位置再次执行图28的相交圆方法。这在图20中示出。这次，两个圆C1和C2产生了相交点ic7和ic8，并且ic8的位置被选为优选点，当距离J小于距离H时，路径点wp6应该移动到该优选点。水平点仍为wp7，因为这是序列中的最后一个路径点，并且不存在可能是水平点的其他路径点。

最后要处理的路径点是wp7，它也是目标点。再次，图22示出了当前路径点wp7与上一个路径点wp6之间的距离(如标记为K的虚线所示)大于step_length阈值。结果，当前路径点wp7因此被移动到沿着连接wp6和wp7的虚线K的点。wp7移动到的位置与wp5的距离等于step_length阈值。wp7的新位置在图23中示出。图23还示出wp7的新位置距最靠近障碍物110的距离大于虚线圆所表示的minimum_clearance阈值，因此，路径点无需再次移动。由于wp7也是目标点，因此也是序列中要处理的最后一个路径点，因此路径规划操作结束。在图24中可以看到显示已处理和更新的路径点位置的最终路径。图24清楚地显示了路径如何避开障碍物。因此，当车辆采用该路径规划操作已经规划的路径时，车辆将能够在不接触障碍物的情况下导航该路径，或者在导航期间无需重新调整其轨迹。

图25示出了路径规划算法的最终结果，调整后的路径由标记为wp1-7的圆形路径点表示，并与由十字形路径点表示且标记为owp1-7的线性路径上的原始路径点一起显示。

上述算法旨在在机器人装置运动期间定期运行。例如，机器人装置可能具有以大约30–50Hz的频率运行的运动更新，并且路径规划算法可以在运动更新期间运行，以规划用于机器人装置的路径以覆盖机器人装置的即将到来的预期路径的一部分。替代地，该算法可以在操作开始时运行一次，并且可以绘制该机器人装置在该操作期间打算采用的整个路径。在该替代实施例中，可以将整个路径划分为多个路径段，并且可以针对每个路径段运行路径规划算法，直到已经处理了整个路径为止。

尽管到目前为止已经描述了特定的示例和实施例，但是应当理解，在不脱离由权利要求限定的本发明的范围的情况下，可以进行各种修改，上面已经描述了其中的一些。

例如，在以上给出的示例中，上一个路径点始终被视为紧接当前路径点之前的路径点，例如，如果当前路径点为wp

此外，本文描述的各种预定阈值和参数(例如horizon_step、minimum_clearance和step_length)可能与以上示例中建议的阈值和参数不同。例如，替代于minimum_clearance等于机器人的外径，可以使用用户选择的置信度来设置minimum_clearance，如果用户担心机器人过于靠近精致的家具或物体导航，则用户可以调整该置信度。

另外，上述方法使用圆的相交提供简单的几何分析，以返还相交点。但是，可以使用其他形状代替圆形，例如多边形。此外，如果要规划的路径是穿过3D空间的路径，与本文中例示的2D路径相反，则所使用的形状可以是3D形状，例如球形。