无人机运动规划的概述

无人机运动规划的概述

前言

对于初学者最容易搞混的概念莫过于 运动规划 路径规划 轨迹规划 三个概念之间的关系。

下面通过下图让我们了解下：

1. 运动规划（motion planning）：在给定的位置A与位置B之间为机器人找到一条符合约束条件的路径。包括行进的路径以及行进的方式。
2. 路径规划（path planning）：连接起点位置和终点位置的序列点或曲线称之为路径，构成路径的策略称之为路径规划。其目标是使路径与障碍物的距离尽量远同时路径的长度尽量短。
3. 轨迹规划（trajectory planning）：规划机器人执行运动时的速度及加速度，即在路径规划的基础上加入时间序列信息，对机器人执行任务时的速度(dt)与加速度进行规划，目的是满运动曲线光滑，或是运动速度可控等要求。

一：运动规划的概述

路径规划是指无人机在一定的环境下，给定无人机源点和目标点，通过一些算法、控制、优化方法等来寻找一条安全、动态可行、最优的路径，使无人机从源点飞向目标点。运动规划可以由4部分构成：①路径规划：运动规划与机器人学有关。此规划满足约束，如飞行路径，在路径规划运动中转动曲柄。它在短路径长度和最小转弯角方面优化了路径。②轨迹优化：轨迹优化是关于飞行时运动可行性的优化。它包含了无人机运动的速度、时间和运动学的路径规划。③导航：导航是运动规划、轨迹规划、避碰和定位的一部分。它是对无人机从一个地方到另一个地方的运动进行控制和监视的总称。④定位，了解无人机所处位置，以便于执行任务遇到未预测的障碍进行认为的规划。

二：无人机规划的阶段

无人机运动规划大致可以分为三个阶段。

第一阶段是预处理阶段：在这个阶段，在有障碍物′O′的工作空间′W′上画出节点（点）和边（线）。然后，应用配置空间（c-space）的概念来描述W上的U和O。其中配置空间是指将工作空间进行转换，将机器人转化为一个质点，同时将障碍物按照机器人的体积进行膨胀，这样的进行路径规划时，就可以将机器人当作一个质点来处理。
第二阶段是查询阶段：从路径的起点到目标点进行节点的搜索，节点应该满足避开障碍物、路径长度小、时间上要短等，对于特定的约束，还需要满足约束的限制。这一阶段有很多路径规划的方法，如基于图搜索的Dijkstra、A、A star等，基于随机采样的RRT、RRT及其变种方法都可以用于无人机寻找最优路径。
第三个阶段就是轨迹优化：第二阶段得到路径存在不够平滑、靠近障碍物、能源消耗大等问题，进行二次的轨迹优化，可以使得到的路径适合无人机自主移动、远离障碍物、避免转弯时停下来、避免速度、高阶动力学状态的突变。轨迹优化的方法有Minimum Snap、B-spline、贝塞尔曲线、等式不等式约束、安全走廊约束等。
三个阶段完成后，可以得到一条平滑、安全、动态可行的最优轨迹。

三：无人机路径规划中的挑战

①安全性：无人机在飞行过程中对障碍物的避开，以及对未知障碍物的闪避都具有非常大挑战性。

②最优性：生成的路径应该具有时间效率高、路径长度短、能源消耗少。

③在线生成轨迹的能力：面对复杂环境或者窄缝，要求无人机能够及时的更改路径来应对当前环境或者说应当动态障碍物能够及时避开。

④平滑性：这对于无人机动态可行是不可或缺的，这要求对于算法的近一步改进，生成更适合无人机飞行的平滑路径。

四：认识地图

常见的建图：点云地图 栅格地图 欧氏距离场(ESDF) 八叉树地图 TSDF

4.1点云地图(Point cloud map)

定义：是由无数的点云构成。

4.1.1什么是点云

1. 点云是某个坐标系下的点的数据集。
2. 点包含了丰富的信息，包括三维坐标X，Y，Z、颜色、分类值、强度值、时间等等，不一一列举。
3. 在我看来点云可以将现实世界原子化，通过高精度的点云数据可以还原现实世界。
4. 万物皆点云。

4.1.2从何而来

主要是通过三维激光扫描仪进行数据采集获取点云数据，其次通过二维影像进行三维重建，在重建过程中获取点云数据，另外还有一些，通过三维模型来计算获取点云。

4.1.3 优点

点云地图能够在模型中尽可能地保留原始的环境量测信息，同时能够更好的描述环境。由于点云地图能够在模型中保留大量的环境信息，研究人员基于点云进行了环境感知建模相关的研究。例如三维重建

4.1.3 优点

由于点云地图需要在环境模型中存储大量无序的三维数据点，即便是采取专用的处理方法，如点云库PCL(Point Cloud Library)，其实时性仍然不理想并且随之会带来大量的存储和运算开销，无法做到对数据进行实时处理。因此，其在实际的机器人感知避障系统中使用较少。

4.2 占据栅格地图(Occupancy Grid Map)

📌详情看https://blog.csdn.net/zhao_ke_xue/article/details/109040676

4.2.1 定义

栅格地图就是用一个个栅格组成的网格来代表地图. 栅格里可以存储不同的数值, 代表这个栅格的不同含义.

ROS的栅格地图使用白色代表空闲，也就是可通过区域，其存储的值为 0；

黑色代表占用，也就是不可通过区域，其存储的值为 100；

灰色代表未知，就是说目前还不清楚这个栅格是否可以通过，其存储的值为 -1．

栅格地图由于其 占用与空闲的表示方法，在ROS中又被称为占用地图．

栅格地图的示意图如下图所示：

4.2.2 ROS中的栅格地图的消息类型

$ rosmsg show nav_msgs/OccupancyGrid 
std_msgs/Header header          # 数据的消息头
  uint32 seq                    # 数据的序号
  time stamp                    # 数据的时间戳
  string frame_id               # 地图的坐标系
nav_msgs/MapMetaData info       # 地图的一些信息
  time map_load_time            # 加载地图的时间
  float32 resolution            # 地图的分辨率，一个格子代表着多少米，一般为0.05，[m/cell]
  uint32 width                  # 地图的宽度，像素的个数, [cells]
  uint32 height                 # 地图的高度，像素的个数, [cells]
  geometry_msgs/Pose origin     # 地图左下角的格子对应的物理世界的坐标，[m, m, rad]
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
# 地图数据，优先累加行，从（0,0）开始。占用值的范围为[0,100]，未知为-1。
int8[] data

可以看到，消息可以分为3个部分，消息头header，地图信息info，地图数据data．

1. 地图信息info储存了地图相关的信息，包括 加载地图的时间，地图的分辨率，地图的宽度与高度，以及地图左下角栅格对应的物理坐标．

地图本身是只有像素坐标的，其像素坐标系为坐下角为(0, 0) 的坐标系．通过左下角栅格对应的物理坐标 origin 以及 分辨率，再通过 像素 * 分辨率 + origin , 将像素坐标转成物理世界的坐标，从而确定了整个地图的物理坐标．

4.3 八叉树地图(Octo-map)

对于三维地图，通过观察可以发现障碍物只是其中一小部分，我们可以使用一些特殊的数据结构来存储，比如八叉树(octree)。octree就是有八个子节点的树，是一种递归、轴对齐且空间间隔的数据结构，常用于3D数据的表达。

假设某个空间是一个立方体，这个立方体中有一个小障碍物，我们可以直接将这个立方体标记为占用状态，很显然这种方式太粗糙了，我们需要一种更精细的方法。当然可以使用三维栅格地图，将立方体分成很多个符合精度要求的小立方体，然后对障碍物所在的小立方体进行标记，但是占用内存太大，障碍物只是占据空间中很小一部分，很多空闲状态的立方体并不需要专门分配内存进行标记。

那么如何兼顾精度和内存呢？这时候就该祭出八叉树结构了。

可以将这个立方体分成八个小立方体，对包含障碍物的立方体继续进行分割，直到达到我们想要的精度为止。当某个节点的所有子节点都是占据、空闲或者未知状态时，我们可以将它和它的子节点全部剪掉，这样可以极大的减少占用的内存空间
但是Octomap不能直接使用索引进行查询，要按照树的结构进行递归查询，这也是一种牺牲时间换空间的方法吧。

4.4 ESDF地图

ESDF欧式符号距离场

ESDF地图的每个元素，储存了到最近的障碍物的欧氏距离

TSDF地图

📌参考：http://t.csdn.cn/xXzCy

TSDF是截断符号函数（Truncated Signed Distance Function）的缩写

4.5 voxel Hashing

为解决 volumetric fusion 重建时，重建的空间划分成等大小的 voxel，显存消耗太多，难以重建大场景，并且大量 voxel 更新耗费 GPU 资源问题，斯坦福图形学组提出了 voxel hashing 算法（参考文献：”Real-time 3D Reconstruction at Scale using Voxel Hashing”），voxel hashing 只在相机测量到的场景表面划分 voxel，而不是将整个空间都划分成 voxel，从而节省显存。算法用 hash 表的形式存储在场景表面划分的 voxel block（8x8x8 voxels），方便 voxel block 的查询