机器人学 —— 轨迹规划（Introduction）

　　轨迹规划属于机器人学中的上层问题，其主要目标是计划机器人从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、总结

　　对于飞行机器人而言，每次移动都会消耗大量的能量。好的轨迹规划算法可以保证飞行器少走弯路，避开障碍。所以减少计算量必须是在少走弯路的前提下。移动所消耗的能量是计算所消耗能量的十倍左右。