拓扑规划

本文提出了一种分布式异步系统解决方案用于多机器人在未知障碍物丰富的场景中仅利用机载资源进行自主导航。

在基于梯度的局部规划框架下建立规划系统通过将碰撞风险制定为非线性优化问题的惩罚来实现避碰。

为了提高鲁棒性和避免局部极小值结合了一个轻量级拓扑轨迹生成方法。

然后智能体使用不可靠的轨迹共享网络能在几毫秒内生成安全、平滑且动态可行的轨迹。

通过使用深度图像中的智能体检测校正智能体之间的相对定位漂移。

通过仿真和实际实验验证了该方法的有效性。

注agent为智能体

四旋翼的灵活(敏捷性)使该机器能够在未知环境中执行单智能体自主导航并在开放或已知领域中执行多智能体精确编队控制。

(针对的问题)然而很少有工作将两者结合起来以呈现任何能够导航共享相同未知空间的四旋翼机群的真实系统尤其是仅使用机载处理的系统。

在未知环境中部署多个四旋翼机的困难包括但不限于障碍物参数化的重要性、有限的传感范围、不可靠且带宽有限的通信以及定位不一致导致的定位漂移。

一些相关的工作例如[12]将现实世界中的四旋翼群推得更远。

然而在以往的大多数工作中上述困难在运动捕捉系统或纯仿真中总是被忽视限制了其算法在实际中的应用。

本文提出了一种系统的解决方案在上述困难存在的情况下使四旋翼无人机群能够在杂乱的环境中进行高性能探索。

此外它不需要外部定位和计算也不需要预先构建地图。

该系统名为EGO

Swarm是我们之前工作的扩展基于无ESDF梯度的局部规划器EGO

planner[3]它为未知环境中单个四旋翼的机载局部规划奠定了坚实的基础。

如图3所示非凸配置空间可能导致不期望的行为例如动力学不可行性或拥挤导航。

基于EGO

Planner中的碰撞代价公式前端拓扑路径搜索是隐式进行的因此几乎不需要计算。

通过在目标函数中加入群碰撞的加权惩罚实现了分布式机间避碰。

通过比较未来某一时刻的agent分布和正在优化的轨迹来评估此惩罚。

为了尽量减少数据传输并允许不可靠的通信使用广播网络共享轨迹。

为了纠正相对定位漂移可以将其增加精度到半米我们比较了目标agent的观测结果和轨迹评估的预测结果。

真实世界的实验验证了我们提出的swarm系统。

据我们所知这是在未知的杂乱环境中完全自主分散四旋翼集群的第一个系统化解决方案这意味着感知、规划和控制被集成到机载系统中。

与几种SOTA方法的比较表明了计算效率和鲁棒性。

本文的贡献总结如下:

Planner提出一种几乎不增加额外计算量的新型鲁棒拓扑规划方法。

我们提出了一个分布式和异步四旋翼集群框架它对不可靠的通信和定位漂移不敏感。

所提出的方法被集成到一个完全自主的四旋翼系统中并发布了硬件和软件供社区参考1。

相关工作

基于梯度的运动规划是四旋翼局部规划的主流。

在将局部规划问题描述为无约束非线性优化的开创性工作[4,5]的基础上提出了一系列工作[6]–[10]。

他们使用各种参数化方法包括多项式和B样条考虑轨迹的平滑性、可行性和安全性。

最近我们提出了一个名为EGO_Planner[3]的单四旋翼导航系统它使用更紧凑的环境表示法进一步减少了计算时间。

这就是本文所基于的工作。

拓扑规划

中的homology等价关系要简单得多。

为了捕捉独特的有用路径Jaillet

[14]

构建可见性变形路图其编码比homotogy类的代表性路径更丰富、更相关的信息。

基于

[14],Zhou

[15]通过提出有效的拓扑等效检查来实现实时拓扑规划。

我们扩展了

EGO-Planner

例如[16]-[20]中提出了去中心化方法。

利用速度障碍来保证质点机器人

[16]、完整agent

[18]的无碰撞轨迹。

刘等人[21]提出了一种分散和异步的无人机规划策略以避免静态/动态障碍物和机间碰撞。

虽然这些算法是通过仿真验证的但没有集成传感、测绘、规划能力。

实验结果已在[19,

20]中显示。

[19]可以在没有外部障碍的情况下实现多机点对点过渡并且[20]依赖于临时规划优先级。

然而它们都没有在外场环境中实现完全自主。

[3]上的工作这是我们提出的swarm系统的基础。

然后对提出的拓扑规划策略进行了说明。

作为基于梯度的局部规划器EGO-Planner将轨迹生成公式化为非线性优化问题该问题在平滑度Js、碰撞Jc、动态可行性Jd和终端进度Jt之间进行权衡。

使用均匀B样条φ的控制点Q作为决策变量来参数化轨迹的优化问题由下式给出

其中r

{scdt}下标λ表示相应的权重。

J项可以分为两类:最小误差和软单边约束。

最小误差项Js和Jt使决策变量L(Q)的线性变换和期望值D之间的总误差最小化公式如下

软单边约束项Jc和Jd,

其中参数S、n和ε,影响单边约束近似精度如[13]所述。

根据惩罚类型选择变换L(.)和参数。

由于论文篇幅有限这里省略了L(.)的确切形式可以在[3]中找到。

在EGO-Planner中我们提出了一种新的障碍物距离估计方法该方法利用了每个Q独立拥有的环境信息。

由几个{p,v}对参数化的信息是从周围障碍物中高度抽象的其中p表示障碍物表面的锚点v表示从该障碍物内部指向外部的安全方向如图4a所示:

{pv}对生成和轨迹优化程序如图4a和4b所示。

首先给出一个简单的初始轨迹φ不考虑碰撞。

然后搜索连接φ的碰撞段两端的安全路径Γ。

之后从φ到Γ生成向量vp为锚点定义在障碍物表面。

利用生成的{p,v}对规划器最大化dij并返回优化的轨迹。

由于文章篇幅有限这里我们只对EGO-Planner的基本思想做一个简单的描述。

详细的解释可以在[3]中找到。

B.隐式拓扑轨迹生成

中的分析表明广泛使用的homotopy概念不足以捕获3D空间中的候选轨迹如图

因此Jaillet等人[14]在

deformation)以及Zhou等人[15]进一步提取了称为统一可见性变形(UVD)的VD子集它可以实现实时操作。

然而我们在本文中仍然使用术语拓扑规划因为之前的工作没有歧义。

UVD

的轨迹被认为是同胚的homeomorphic。

[15]中定义的UVD是

定义1:由s

τ2(1)的两个轨迹τ1(s)τ2(s)属于同一个UVD类若对所有s直线τ1(s)τ2(s)无碰撞。

传统的拓扑规划方法[12]-[15]由拓扑上不同的路径搜索和后端优化组成主要关注于在不同的homotopy中找到多个初始路径。

与这些方法不同所提出的方法通过将v反转为vnew:

v来构建不同方向的距离场。

然后搜索过程在障碍物表面单独确定新的锚点pnew如图4c所示。

它们构成新的一对{pnewvnew}这导致不同的局部最小值。

注意没有采用显式路径搜索但是分别通过p和pnew的任何一对路径自然在这两点上违反Def.1。

随后在不同的线程中并行优化不同的轨迹如图4d所示。

执行具有最低成本的轨迹。

无人机集群导航

不同于文献[22]在这里我们忽略了障碍物和动力学限制这部分已经在第三节A部分讨论过。

类似于避障和动力学可行性的惩罚函数我们将第k个agent的集群机间避碰的惩罚函数J_{w,k}定义为软单边约束如下式

其中(t那些)轨迹时间跨度内的全局开始和结束时间。

c是用户定义的agent许可。

E

diag(111/c)c

1将欧氏距离转换为z轴上主轴较短的椭球距离以缓解向下运动的风险。

将加权的Jwk加到等式上。

(1)产生每个agent的总优化问题

何轨迹参数化方法包含一个从决策变量映射到轨迹上点的过程(5).。

本文使用pb阶均匀B样条将轨迹参数化这为位置评估提供了矩阵表示[9]

这里Mpb1是一个常数矩阵当t属于节点跨度(tmtm1]时由pbs(t)(t-tm)/Δt确定。

定位漂移补偿

由于在未知环境中的个体定位(没有可靠和高频的闭环)漂移在飞行期间累积。

Xu等人[23]提出了一种具有额外UWB距离测量的无人机集群的状态估计方法并实现了精确的协作定位。

然而我们更关注穿越障碍丰富的环境必须为其他应用程序预留计算和通信资源。

因此受[23]的启发通过比较从接收到的其他agent轨迹评估的预测位置和从观测者的深度图像测量的位置提出了一种简单且轻量的相对漂移估计方法。

当轨迹跟踪误差可以忽略不计并且任何两个可能发生碰撞的agent中至少有一个能看到另一个时这种策略是有效的。

因此我们使用[24]中的控制器进行精确跟踪并使用广角摄像机来降低丢失agent的可能性。

漂移消除程序如下。

在评估agent-i的当前位置φi(tnow)之后确定以φi(tnow)为中心、半径为r的球形信任区域S

然后在S’⊂

SK和Tcw是相机的内参和外参矩阵z是S沿主光轴与光学中心的偏差。

S’是需要复杂计算才能获得的椭圆圆锥曲线。

因此我们采用近似的轴对齐椭圆S’而不是精确的S’。

没有必要精确地定义信任区域因为它只是一个经验域。

然后我们将S’内的每个点投影到世界框架中收集属于S的点得到一个点簇P

如果P仅包含相应agent的观察而没有任何不相关的对象则等式(9)成立这是不保证的。

然而由于每个agent规划的轨迹都有到附近物体的安全距离因此等式(9)大多数时间是成立的。

添加了其他标准以提高agent检测的鲁棒性例如像素数、P的第二中心矩、当前测量值与先前测量值的偏差等。

更严格的标准增加了假阴性率但由于定位漂移变化缓慢假阴性率是无害的。

最后φi(tnow)与P之间的误差被馈入一个滤波器然后从该滤波器获得估计的漂移。

从深度图像中去除agent

我们使用占用栅格地图存储静态障碍物使用深度图像进行地图融合。

移动agent在第四节-A

得到处理因此在地图构建中记录移动agent并将其视为障碍是不必要的甚至是有害的。

为了消除运动物体的影响我们从深度图像中屏蔽并去除在第四节-B中检测到的agent的像素如图

除了覆盖大部分视图的移动物体对VIO是干扰之外。

因此对于相应的深度图像也使用相同的遮罩来去除灰度图像上的agent。

此处使用的病原体检测标准不太严格因为假阳性比假阴性更有害。

为了消除移动物体的影响我们从深度图像中屏蔽并去除在第四节-B中检测到的agent的像素如图

VIO

造成干扰。

因此灰度图像上的agent也被移除对相应的深度图像使用相同的掩码。

这里使用的agent检测标准不那么严格因为错误的正值比错误的负值更有害。

系统结构

系统架构如图8所示其中包含单个agent和多个agen通信系统的详细架构。

单个agent系统包括硬件和软件设置是基于我们以前的工作EGO-Planner

[3]带有一个额外的模块来补偿VIO漂移和去除图像上的被看到的agent。

对于未知环境中的轨迹生成使用局部规划器。

当当前轨迹与新发现的障碍物发生碰撞或者agent接近当前轨迹的末端时规划被激活。

两个网络连接系统即共享轨迹的广播网络和同步时间戳并管理顺序启动的链式网络。

1)广播网络:一旦agent生成新的无碰撞轨迹就立即广播给所有agent。

然后其他agent接收并存储该轨迹用于在必要时为自己生成安全轨迹。

这种闭环策略在连接稳定且延迟可以忽略的理想情况下工作正常。

然而这在实践中并不能得到保证。

因此我们提出两种方法来减少碰撞的可能性。

首先在网络负载下以给定频率广播一个轨迹。

这不会导致计算负担因为包含三维航迹点和其他参数的典型轨迹的大小小于0.5KB。

Bluetooth等现代无线网络可以达到1

Mbps以上的速度。

第二一旦从广播网络接收到轨迹每个agent就检查碰撞如果检测到潜在的碰撞则生成新的无碰撞轨迹。

这种策略可以处理当多个agent在接近的时间生成轨迹而由于延迟或分组丢失而没有接收到其他agent的轨迹的情况。

此外还考虑了计算复杂度随着agent数量的增加而增长的情况。

在规划之前每个agent将其当前位置与接收到的周围agent的轨迹进行比较其中任何超出规划范围的轨迹都将被忽略。

2)链式网络:基于连接的稳定链式网络用于时间戳同步和系统启动管理。

在系统启动时agent以预定义的顺序生成轨迹。

每个agent在通过链网络接收到来自具有更高优先级的agent的轨迹后生成其初始轨迹。

这种策略避免了在系统启动时由同时轨迹生成引起的混乱因为此时agent没有其它agent的轨迹信息。

1.0λc

0.1。

地图分辨率为0.1米。

每秒触发一次重新规划否则会发生碰撞。

该设置在模拟实验和真实实验中都可以实现。

拓扑规划

[15]在候选轨迹数和前端拓扑路径搜索的计算时间方面的拓扑规划性能。

如图9所示。

EGOSwarm找到的候选轨迹更少这意味着找到全局最优的概率更低但比[15]快两个数量级。

因为Fast-Planner通过PRM

[25]图搜索、路径缩短和路径修剪来找到拓扑上不同的路径与所提出的隐式拓扑路径搜索方法相比这是耗时的但是具有更高的自由度。

集群规划

我们在飞行距离(dfly)、飞行时间(tfly)、每个agent的碰撞次数和计算时间(tcal)方面将所提出的方法与DMPC

[19]、奥卡[16]和RBP

[20]进行比较。

除了最大速度和加速度之外每个比较方法都使用默认参数。

如图10所示

八个agent在一个圆上执行交换转换。

表I中的结果是所有agent的平均值。

tcal用“*”标记因为我们记录的计算时间是规划所有agent的整个轨迹的总时间(对DMPC和RBP而言)而对于ORCA和EGO

标签I和图10表明RBP趋向于生成安全但保守的轨迹因为构建凸相对安全飞行走廊[20]显著压缩了解空间。

DMPC是为分布式部署设计的。

然而它需要精确和高频的姿态通信这在现实应用中是无法保证的。

高效的规则使得ORCA更新快速。

但是使用速度作为控制命令会使其与三阶系统不兼容例如四旋翼飞行器。

碰撞的风险也限制了它的应用。

相比之下提出的方法生成最短的无碰撞、非保守的轨迹计算速度快。

因此它能够实现四旋翼飞行器的实时应用。

我们模拟十架无人机从地图的一边飞到另一边限速2m/s四旋翼半径0.2米。

图2是0.42障碍物/平方米的模拟快照。

每个agent独立感知环境构建的局部地图以不同的颜色显示。

结果总结在表II中

其中dfly是平均飞行距离dsafe是飞行试验中离障碍物最近的距离。

反向点对点转换被设计成使得在地图中心周围的相互碰撞避免不可避免。

在这种情况下每个属于群体的agent规划光滑和安全的轨迹。

我们在一个场景中评估计算性能在该场景中排列成直线的agent飞往50米外的随机目标点。

如图11所示由于SecV

-B.1中的按需冲突检查策略时间复杂度随着agent数量的增加逐渐平缓。