轨迹规划属于机器人学中的上层问题，其主要目标是计划机器人从A移动到B并避开所有障碍的路线。
1、轨迹计划的对象
轨迹规划的对象是map，机器人通过SLAM获得地map后，则可在地图中选定任意两点进行轨迹规划。暂时不考虑三维地图，以平面二维图为例，map主要有以下几种：
如上所示，二维图可以分为有权重的节点图以及无权重的网格图。以飞行机器人移动为例：如果场地上每个部分移动所消耗的能量都是等价的，则可以使用网格图进行建模。如果在不同的区域移动所消耗的能量不同，则应该使用节点图来进行建模。
2、轨迹规划的方法
2.1、 grassfire method
grassfire method是一种最简单的轨迹规划算法。其数据结构包括三个部分：
1.map，记录了起点，终点，障碍物的位置
2.distancetoStart，记录了每个探索步骤后，方格与初始点的距离
3.parent，记录了每个节点的父节点
算法步骤：
1、获取map
2、初始化distancetostart，将起点设为current, 将distance_ 设为0
3、进入循环 如果current = destination 则跳出循环
将distancetostart中最小值对应的节点设为current
distance_ = distance_+1
寻找current所有的相邻节点
将相邻节点的distancetostart设为distance_，parent设为current
将current的distance_设为无穷大
4、依据parent map反求轨迹
显然，这种grassfire算法是极其消耗计算能力的，因为其需要遍历网格中几乎每一个格子，甚至明明destination在上方，grassfire算法依旧需要向下探索。图中红色的区域代表已探索的点，蓝色是candidate,绿色是起点，黄色是终点。显然这个算法是各向同性的。
2.2、A* 算法
grassfire算法的最大缺点是没有使用终点与起点相对位置关系信息。从而导致各项同性的探索策略。如果能增加一个将candidate拉向目标点的趋势，则可以减小被探索的点数目。
A* 算法的数据结构包括五个部分：
1、map
2、g:相当于distancetostart;
3、H:一种衡量算子，用于衡量当前点到目标点的绝对距离
4、f:g+H，综合了当前点到起点的距离与到终点的距离
5、parent，用于记录轨迹
算法步骤：
1、初始化，获取map，并将起点设为current. 获取H map，distance_ = 0
2、进入循环 如果current为destination则退出循环
distance_ = distance_ +1;
获取当前点的相邻点
将相邻点的g map值设为distance_
将相邻点的f map值设为g+H
选择f map值最小的点为current
将current f map值设为无穷大
3、根据parent map, 反求轨迹
结果如上所示。
显然，由于引入了H map，所以算法会优先探索相对终点较近的点。obj = distance to start + distance to end 代表了最后需要走过的距离。每次都选择最小的obj，并且最后能够达到终点，那么总路程一定是全局最小的。
3、总结
对于飞行机器人而言，每次移动都会消耗大量的能量。好的轨迹规划算法可以保证飞行器少走弯路，避开障碍。所以减少计算量必须是在少走弯路的前提下。移动所消耗的能量是计算所消耗能量的十倍左右。
