一种使用均匀B样条和3D循环缓存的MAV实时轨迹重规划方法

泡泡图灵智库（航行天下），带你精读机器人顶级会议文章

标题：Real-Time Trajectory Replanning for MAVs using Uniform B-splines and a 3D Circular Buffer

作者：Vladyslav Usenko, Lukas von Stumberg, Andrej Pangercic, Daniel Cremers

来源：IROS.2017

编译：郑立强

审核：李裕超

欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权

摘要

大家好，今天为大家带来的文章是——一种使用均匀B样条和3D循环缓存的MAV实时轨迹重规划方法.

本文提出了一种用于微小型无人机实时局部路径重规划的方法。利用目前已有的方法，微小型无人机的轨迹生成可以达到比较高的成功率，但是，这是建立在静态环境以及事先已经对地图有了先验知识的基础上的。在本文的研究中，在已有的规划方法的基础上，我们提出了一种能够同时处理障碍物变化以及尽量贴合全局轨迹的局部重规划方法。为了保证该方法的实时性，我们将无人机周围的环境信息以占据栅格的方式保存在了一个三维的循环缓存区中，这个缓存区能够随无人机移动而移动（循环更新），并且使用均匀B样条来表示轨迹。这种表示方式能够保证轨迹足够光滑，同时能够被有效地优化。

主要贡献

1、将局部轨迹规划问题表征成B样条优化问题，同时与其他表征方式进行比较（多项式，离散）；

2、实现了高性能地3D循环缓存区进行局部障碍建图以及碰撞检测，同时与其他方法进行比较；

3、除了与现有的方法进行比较之外，还分别针对仿真器和真实硬件进行系统设计和评估；

算法流程

1、均匀B样条

B样条的值可用下式进行计算：

其中pi为时刻ti对应的控制点，Bik(t)为基函数，基函数可以使用 De Boor – Cox 递归公式计算得到。均匀B样条控制点间的时间间隔是固定的。

在五次均匀B样条的情况下，在[ti,ti+1)中t时刻p(t)的值仅取决于六个节点[ti-2, ti-1, ti, ti+1, ti+2, ti+3]。为了简化计算，我们将时间均匀地表示为s(t)=(t-t0)/∆t，那么节点就可以用si来表示（i在[0,....,n]中取值）。我们定义函数u(t)=s(t)-si，来评估从当前时间节段开始所经过的时间。B样条就可表示为如下形式：

速度跟加速度也可相应地表示为如下形式：

2、使用3D循环缓存方式来表征局部环境地图

为了在飞行中避开障碍物，需要维护局部环境地图。本文使用一个3D循环缓存区来表征环境地图。为了方便查询，我们将空间离散为一个个尺寸为r的体素，这样就建立了空间中任一点p到特定的体素索引x的映射，以及逆映射。

循环缓存区由大小为N的连续数组和定义了坐标系位置的偏移索引o组成。我们可以检查到空间中任一点所对应的体素是否在循环缓存区所表示的范围内，以及它具体的存储位置：

将数组的大小限制为N=2^p的话，上述两种操作可以用更高效的方式来进行：

通过修改偏移索引o以及清除更新的部分，我们可以使得循环缓存区所表征的地图范围随着传感器的移动而移动，这种方式，减少了大量的复制操作。

从传感器中心点出发，我们使用光线投射法来更新地图，并用Bresenham的直线算法来加速光线投射操作。

为了加速碰撞检测的效率，我们对地图使用欧几里得距离变换（EDT），这种方式可以很方便地查询到地图范围内某一点与障碍物的距离以及距离变化的梯度。

3. 轨迹优化

我们将B样条的优化表示成非线性优化的方式，优化目标函数如下：

式中Eeq表示全局路径跟踪误差的耗散函数，Ec是障碍物距离的耗散函数，Eq是平滑性的耗散函数，El是对速度和加速度最大值限制的软约束。

每一项的具体计算方式如下：

4. 实现细节

先使用多项式优化的方式得到全局路径，然后增加六个控制点在当前位置，作为起始点。

开始迭代之后，每个时刻，使用时间tep作为全局路径的输入计算出跟踪的目标点以及目标速度，作为跟踪误差的耗散函数参数；障碍物耗散函数的参数则来自于循环缓存区以及EDT。

每次优化之后，当前被优化的控制点中，第一个控制点就固定下来，传送给控制器去计算新的控制输入。而新的控制点会被添加进来，添加了新控制点之后，teq自增。如此循环往复。

主要结果

表1 在仿真实验中轨迹重规划操作的平均计算时间，其中每次迭代有七个控制点被优化。

表2 不同的路径规划算法之间的比较。除本文提出的方法外，其他结果均来自于论文 Continuous time trajectory optimization for online UAV replanning. 上述结果说明B样条可以用来表征多项式所能表征的曲线。我们对控制点直接进行无约束优化，使得我们的方法有更高的计算效率，这与其他需要首先将问题转化为无约束优化的方法不同。

图1 实时重规划的例子。红色的线为全局路径，蓝色表示障碍物，青色是优化后的局部路径，绿色是待优化的路径。从上图可以看出，本文提出的方法，可以在没有障碍物的时候尽可能地贴近全局路径，而在有障碍物的地方产生一条平滑的路径避开它，然后又重新回到全局路径上。

图2 在柱子中穿行的仿真实验。粉色为全局路径，绿色和黄色为实时重规划的路径。

图3 穿过一片森林的仿真实验。粉色为全局路径，青色为实时重规划的路径。

图4 真实飞行实验。机载RGB-D相机（Intel Realsense R200）,机载计算机为2.1GH的Intel i7 CPU。

Abstract

In this paper, we present a real-time approach to local trajectory replanning for microaerial vehicles (MAVs). Current trajectory generation methods for multicopters achieve high success rates in cluttered environments, but assume that the environment is static and require prior knowledge of the map. In the presented study, we use the results of such planners and extend them with a local replanning algorithm that can handle unmodeled (possibly dynamic) obstacles while keeping the MAV close to the global trajectory. To ensure that the proposed approach is real-time capable, we maintain information about the environment around the MAV in an occupancy grid stored in a three-dimensional circular buffer, which moves together with a drone, and represent the trajectories by using uniform B-splines. This representation ensures that the trajectory is sufficiently smooth and simultaneously allows for efficient optimization.

如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。