[运动规划算法]基于滚动优化的路径规划器lexicographic_planning

最新推荐文章于 2024-03-25 16:38:28 发布

Travis.X

最新推荐文章于 2024-03-25 16:38:28 发布

阅读量2.4k

点赞数 5

分类专栏：运动规划

本文链接：https://blog.csdn.net/Travis_X/article/details/115485931

版权

运动规划专栏收录该内容

8 篇文章 11 订阅

订阅专栏

标题：A Receding Horizon Multi-Objective Plannerfor Autonomous Surface Vehicles in Urban Waterways
作者：Tixiao Shan, Wei Wang, Brendan Englot, Carlo Ratti, and Daniela Rus
来源：https://arxiv.org/abs/2007.08362
代码：https://github.com/TixiaoShan/lexicographic_planning

摘要

lexicographic_planning是一种新颖的滚动优化规划器，适用于在城市水道中执行路径规划的无人船（ASV），通过从传感器观测构建的图中重复生成并进行搜索直至找到可行的路径。它将导航过程中需要解决的各种挑战建模成最小化代价，这样就将规划问题转化为多目标的优化问题。通过一种词典式高效的多目标搜索算法对所有目标进行分层排名快速解决多目标的规划问题，而无需对参数进行调整。

实际中实时快速生成路径的方案有很多，譬如基于采样的运动规划算法概率路线图（PRM）、快速随机搜索树（RRT）以及它们的变体PRM*、RRT*、RRG等，这些方法通常是考虑了时间、能量、行进距离、碰撞、交通规则等约束。

而lexicographic_planning规划器考虑的资源管理的问题，当两个或多个约束同时存在时会对这些约束进行分级惩罚的管理，优先级高的约束承担惩罚的代价也就越高，当主要的惩罚代价没增加时，则会引入次要的代价或者次次要的代价。

在这里插入图片描述

主要贡献

提出新颖的滚动优化规划器，适用于城市航道中ASV的自主导航。
高效的多目标搜索算法，可通过对目标进行分层排名，从而实现实时性能而无需迭代调整约束。
框架通过仿真和现实环境中的测试来进行验证。

一、算法流程

1.路径规划

C表示的是机器人构造的空间，x∈C表示构建的机器人。C（objs）⊂ C表示空间中的障碍物，S代表传感器的数据。C（free）= cl（C \ C（obst）），其中cl（）代表开放列表的闭集，表示无碰撞的自由空间。假设我们给定X（c）∈ C（free) 和全局路径G。机器人必须在adj（G）上移动，adj（G）代表C中G的邻近区域，并达到G末尾时的目标状态为Xg。

一条路径为连续的函数σ ：[0,1]->C中限定的路径。Σ表示给定的构造空间中的所有路径σ。如果σ ∈ C（f ree）, σ(0) = Xc and σ(1) = Xg则表示一条无碰撞的可行路径。
一条可行路径包含两部分σ = σG ∪ σg. σG，在adj（G）当中，通过搜索有向图G（V，E）而获得，节点为V边界为E。σg∈ G能从G中直接获取，σg 和 σG 能关联为 σg(1) = σG(0)。边界e（i,j）∈ E 表示边界，路径σ(p,q)∈G是连接边界的集合，σ(p,q) = {e(pi1),e(i1i2),…e(einq)}。为寻找一条可行路径需要使用特定的远组(G（free），Xc, G)。

2.词典优化

定义总代价函数为ck(σ)， ck : Σ → R(+0)，k∈ {1, 2, …, K}, K表示在多目标规划中子代价函数的数量，这些子代价函数适用于词典优化，可以在无碰撞路径上表示为
在这里插入图片描述
将无冲突路径作为输入的代价函数。假设 ck : Σ → R(+0)，这意味着cK在一条路径上是单调递增，并伴随着如行进的距离或经历的时间等成本。因此，联系很少出现在层次结构的最底层，其发生率取决于用于生成路径的算法。。在等式（1）的一次迭代过程中，如果相比先前计算的代价函数j <k代价值不增大时，将返回新解σ∗。Rentmeesters [19]首先建立了方程（1）最优解的必要条件，还提出了该公式的松弛形式，其中cj（σk）> cj（σj*)是允许的，条件是cj（σk）的值不大于cj（σj∗ ）的小百分比。这种称之为分层方法[20]的方法，该方法也已应用于最优控制中的多准则问题[21]。

3.代价函数

设计的总代价函数由碰撞代价、航向代价以及行进代价组成，并进行分层排名。

碰撞代价

沿着全局路径σ的风险计算可以这样表示：
在这里插入图片描述
其中函数Risk函数评估单个机器人状态下的风险。假设R（x）定义为x与离x的最近障碍物之间的距离的倒数。如果R（x）大于风险阈值T hrisk，则会激活Risk（）函数。例如，让T hrisk = 2，当机器人在障碍物的0.5 m以内时，Risk（）会给出非零值。令让T hrisk = ∞时，在C（free）中任何一处Risk()都会返回0。

设计采用该代价函数的逻辑是，希望在机器人与障碍物之间放置一块安全区域（下图灰色部分），机器人尽量避开该区域，以最大程度地减少其对其他船只的影响。创建该区域的另一种方法是单纯地对障碍区域进行膨胀。但是，如果障碍物之间离得很近，膨胀后也可能阻塞整个通行区域，即使是有一条可行的路径可以在它们之间通过。
图2：滚动优化规划器示例。机器人位于左侧，参考路径G和Obstare分别显示为粉红色和红色。考虑到机器人尺寸，黑色表示膨胀的障碍物。灰色表示Section III-C中定义的风险区域。在（b）中，由于障碍物相交而生成了图G（c），从中找到了一条可行的路径σ = σG ∪ σg，其中σG是对图G的粗略搜索,σg是直接从G中获得。

航向代价

定义航向作为第二代价来惩罚了机器人和全局参考路径G之间的航向差异：
在这里插入图片描述
其中函数H（x）给出x与G上最接近x的路径段的航向之间的航向差。由于风浪干扰，将机器人的航向与G完美对齐几乎是不可能的。为了避免全面的控制工作，我们定义了航向差异阈值T hhead。当误差H（x）大于T hhead时，Heading（x）返回一个非零值。合并此代价可确保生成的路径在保持全局路径G航向的同时能相对平滑。

行进代价

将行进距离定义为第三代价，严格来说这是正数。这样可以确保不会像主要或次级代价那样频繁地发生。距离代价定义如下：
在这里插入图片描述

碰撞代价主要是为保证机器人和其他车辆的安全。航向代价有助于使乘客平稳行驶，同时最大程度地减少控制的工作量。严格的正距离行进代价是为了确保在分层机制出现较少，并在可能的情况下最小化了行进距离。

4.滚动优化规划器

为了能将期望的路径规划器应用到有限计算资源的平台上，我们在设计的规划器上使用了滚动优化的方式。给定全局路径G作为参考路径，我们仅在必要条件下（例如，位于G上的障碍物或要执行的路径）在adj（G）中搜索新路径。除了制定参考路径G所需的基本拓扑信息外，我们还假定规划人员无法获得先前的环境图。这是因为城市航道的环境由于人类活动（例如运河的维护和游船活动）而不断变化。通过使用实时感知传感器数据S，可以获得对机器人环境的详细了解。不需要事先准备地图，就可以轻松地部署期望的计划器。

算法1中介绍了滚动优化规划器。规划器将机器人的当前状态xc，全局参考路径G和感知传感器数据S作为输入。当机器人未完全执行全局路径G时，我们使用感知到的传感器数据S检查路径σ的可行性。请注意，当在规划过程的开始时收到G时，我们使σ=G。计划的规划器的说明性示例如图2所示，其规划范围由关键参数d(span)，d(roll)和d(sensor)决定，如下所述：

在这里插入图片描述
如果障碍物位于σ上，我们就G为参考生成候选的机器人状态V。我们采用[22]中介绍的方法，并提出了一种用于基于V采样生成roll-out 和 roll-in方法。roll-out 生成的状态需要满足机器人的动力学约束，同时要偏离xc并跨越adj（G）。在roll-in生成的阶段，采样状态收敛到G，同时维持运动学上可行的状态。 adj（G）由d(span)和d(roll)定义。 d(span)是采样状态和G之间的最大距离。roll-out和oll-in部分之间的总距离表示为d(roll) 。实际上，在障碍物覆盖的区域d(roll)设置的值比传感器的范围要大，注意，为了清晰明了，使用固定的密度值对V进行采样。图2（b）显示了生成的V，其固定密度值为0.1m。然后，我们在V中连接状态并获得图G =（V，E）。为了可视化的目的，我们在这里仅将一个状态与其八个紧邻的状态连接起来，并在图2（c）中显示结果图。

在这里插入图片描述

我们使用Dijkstra的算法[23]在G上执行lexicographic图搜索，这在算法2中进行了详细说明。提供了图G（V，E）和xinit作为输入，并在队列Xqueue中填充了图的节点。图（第1行），并且该算法为每个节点初始化代价（第2-4行）。这些代价中的每一个都沿等式（1）中代价函数的排名到目前为止确定的最佳路径描述了各个节点的第k个优先级代价。在实时搜索中，将xinit指定为图中最接近机器人当前状态xc的配置，而xgoal是图中G在发生滚动时收敛到G的状态。

算法中的每次迭代，FindMinCostk() 操作是返回一组配置，这些配置共享作为输入提供的节点（第9行）中的第k个最低优先级到达代价。如果Xmin包含多个配置，则将检查此集中节点的较低优先级成本，直到Xmin集为止包含单个节点，并对其邻居进行详细的检查。选择的节点被指定为Xi（第11行），如果存在边缘e(ij)，则如果可能，然后使用节点xi来降低与相邻节点xj相关联的成本。在第16行中，如果代表通过xi从xinit到xj的第k个优先级成本的ck（xi，xj）低于当前成本xj .ck，则通过选择xi为xj来更新xj的成本。它的新父母（17-19行）。但是，如果从xinit通过xi到xj的第k个优先级成本与当前成本xj .ck（第21行）相关联，则算法2进行至较低优先级成本k + 1并评估潜在值（k + 1 通过xi到达xj的第一个优先获得成本改进。为了减少最终阶段联系的可能性，最低优先级成本K假定在配置空间中的所有路径上均严格为正。

正如方程（1）所阐述的问题只允许在不会对较高优先级成本产生不利影响的情况下提高一个解的低优先级代价，所提出的搜索方法也仅允许在低优先级代价和高优先级代价至之间发生联系时进行改进。无论是否进行了这些改进，算法2产生的单源最短路径解决方案都将承担相同的代价，但是联系的出现使我们可以按词典优化的方式机会性地解决辅助代价函数。