无人驾驶的规划-基础概念

一．路径规划、轨迹规划、路径跟踪、轨迹跟踪

（1）简单的说运动规划可以分为路径规划（path planning）和轨迹生成(trajectory generation)两部分。

路径规划（path planning）的目的是找到无干涉，并能完成任务的路径点。而轨迹生成一系列运动连续的参考点，以发送到控制器驱动机器人运动。可以简单一点理解为，路径规划是找到一系列要经过的路径点，这些点只是空间中的一些位置，或者关节角度，而轨迹规划则是确定怎么走，走多快，它需要赋予这条路径以时间信息。

对于路径规划，一般的方法如RRT， PRM以及他们的各种改进算法。而轨迹规划一般采用S型规划，或者采用一些优化算法如时间最优，能量最小等方法规划。

（2）对于无人车辆来说，全局路径点只要包含空间位置信息即可，也可以包含姿态信息，而不需要与时间相关，但局部规划时，则可以考虑时间信息。这里规定轨迹点也是一种路径点，即当路径点信息中加入时间约束，就可以被称为轨迹点。从这个角度理解，轨迹规划就是一种路径规划，当路径规划过程要满足无人车辆的纵向和横向动力学约束时，就成为轨迹规划。路径规划和轨迹规划既可以在状态空间中表示，也可以在笛卡尔坐标系中表示。

    路径跟踪过程中，参考路径曲线可与时间参数无关，跟踪控制时，可以假设无人车辆以当前速度匀速前进，以一定的代价规则使行驶路径趋近于参考路径；而轨迹跟踪时，参考路径曲线与时间和空间均相关，并要求无人车辆在规定的时间内到达某一预设好的参考路径点。

路径跟踪不同于轨迹跟踪，不受制于时间约束，只需要在一定误差范围内跟踪参考路径。路径跟踪中的运动控制就是寻找一个有界的控制输入序列，以使无人车辆从一个初始位形到设定的期望位形。

（3）第一本无人驾驶技术。

（4）路径规划的要求(1)在环境地图中寻找一条路径，机器人沿该路径移动时不与外界发生碰撞(2)能够处理用传感器感知的环境模型中的不确定因素和路径执行中出现的误差(3)通过使机器人避开外界物体而使其对机器人传感器感知范围的影响降到最小(4)能够按照目标点的需要寻找最优路径。

轨迹规划算法的输入：路径描述、路径约束、机械手动力学约束

输出：按时间顺序给出的位置、速度、加速度序列

机器人轨迹规划_飘零过客的博客-CSDN博客

（5）环境信息完全已知的全局路径规划和环境信息完全未知或部分未知，通过传感器在线对智能车辆的行驶环境进行感知，以获取障碍物的位置形状和尺寸等信息的局部路径规划。

智能车辆的轨迹规划方法是在局部环境信息已知的局部路径规划方法的基础上考虑时间因素，进一步给出移动主体运动过程中的速度，以车辆运动学模型为研究对象，将速度平方的相关函数作为优化指标，在非完整约束和执行机构约束下求解轨迹的过程。

参加《智能车辆运动轨迹规划方法的研究_李爱娟》、

（6）路径规划是运动规划的主要研究内容之一。运动规划由路径规划和轨迹规划（时间）组成，连接起点位置和终点位置的序列点或曲线称之为路径，构成路径的策略称之为路径规划。

路径是机器人位姿的一定序列，而不考虑机器人位姿参数随时间变化的因素。路径规划（一般指位置规划）是找到一系列要经过的路径点，路径点是空间中的位置或关节角度，而轨迹规划是赋予路径时间信息。

运动规划（又称运动插补）是在给定的路径端点之间插入用于控制的中间点序列从而实现沿给定的平稳运动。运动控制则是主要解决如何控制目标系统准确跟踪指令轨迹的问题。即对于给定的指令轨迹，选择适合的控制算法和参数，产生输出，控制目标实时，准确地跟踪给定的指令轨迹。

路径规划的目标是使路径与障碍物的距离尽量远同时路径的长度尽量短；轨迹规划的目的主要是机器人关节空间移动中使得机器人的运行时间尽可能短，或者能量尽可能小。轨迹规划在路径规划的基础上加入时间序列信息，对机器人执行任务时的速度与加速度进行规划，以满足光滑性和速度可控性等要求。

运动规划/路径规划/轨迹规划_小白鼠2号的博客-CSDN博客

（7）首先来说明三个概念， 路径规划、避障规划、轨迹规划 。 路径规划通常指全局的路径规划，也可以叫全局导航规划，从出发点到目标点之间的纯几何路径规划，无关时间序列，无关车辆动力学。

避障规划又叫局部路径规划，又可叫动态路径规划，也可以叫即时导航规划。 主要是探测障碍物，并对障碍物的移动轨迹跟踪（ Moving Object Detection and Tracking ，一般缩写为MODAT）做出下一步可能位置的推算，最终绘制出一幅包含现存碰撞风险和潜在碰撞风险的障碍物地图，这个潜在的风险提示是100毫秒级，未来需要进一步提高，这对传感器、算法的效率和处理器的运算能力都是极大的挑战，避障规划不仅考虑空间还考虑时间序列，在复杂的市区运算量惊人，可能超过30TFLOPS，这是无人车难度最高的环节。未来还要加入V2X地图，避障规划会更复杂，加入V2X地图，基本可确保无人车不会发生任何形式的主动碰撞。

轨迹规划则源自机器人研究，通常是说机械臂的路径规划。 在无人车领域迹规划的定义感觉不统一。有人将避障规划与轨迹规划混淆了。轨迹规划应该是在路径规划和避障规划的基础上，考虑时间序列和车辆动力学对车辆运行轨迹的规划，主要是车纵向加速度和车横向角速度的设定。将设定交给执行系统，转向、油门、刹车。如果有主动悬挂，那么轨迹规划可能还要考虑地形因素。https://blog.csdn.net/roslei/article/details/53142074

二．环境建模技术

环境建模即对移动机器人工作空间(环境信息)进行有效表达，是移动机器人导航定位的基础。环境建模方法有很多种，目前，在移动机器人路径规划中，大部分将环境信息转换成图的问题，对环境空间的骨架图进行描述。对于低维姿态空间的环境地图表示方法，可概括为栅格地图法(grid map)、几何特征地图法（可视图法）(geometric feature map)和拓扑地图法（Voronoi图法）(topological lap) 三种基本地图表示法。

由它们可引申得到顶点图像法、链接图法、最短Voronoi图法、广义Voronoi图法、切线图、Q—M图法、自由空间法、广义锥法等多种表示方法。

对于高维姿态空间，目前研究最多、应用最广、适用性较强的是于随机采样建立的无向路标图和快速扩展随机树状图。

三．常用传感器

http://www.cnblogs.com/flyinggod/p/8512454.html

四．全局路径规划

1.完备的（complete)

所谓完备就是要达到一个systematic的标准，即：如果在起始点和目标点间有路径解存在那么一定可以得到解，如果得不到解那么一定说明没有解存在。
    这一大类算法在移动机器人领域通常直接在occupancy grid网格地图上进行规划（可以简单理解成二值地图的像素矩阵）以深度优先寻路算法、广度优先寻路算法、Dijkstra(迪杰斯特拉)算法为始祖，以A*算法(Dijstra算法上以减少计算量为目的加上了一个启发式代价)最为常用。

A*:  A星算法详解(个人认为最详细,最通俗易懂的一个版本)_Colin丶的博客-CSDN博客

A*算法详解-知识-名师课堂-爱奇艺

Theta*是A*的一种变体，它会沿着图的边传播信息，但不会使路径限制在图的边上（寻找“任意角度”的路径）。Theta*是易于理解和实现的，快速的，且能找到看起来真实的短路径。Theta*的伪代码只比A*的伪代码多几行且有相近的执行速度，但是找到的路径几乎等同于最短路径（Theta*不需要后处理）。

Theta*: 连续环境下平滑的任意角度的路径规划_theta算法_CloudJin的博客-CSDN博客

https://www.cnblogs.com/Leonhard-/p/6866070.html

完备的算法的优势在与它对于解的捕获能力是完全的，但是由此产生的缺点就是算法复杂度较大。这种缺点在二维小尺度栅格地图上并不明显，但是在大尺度，尤其是多维度规划问题上，比如机械臂、蛇形机器人的规划问题将带来巨大的计算代价。这样也直接促使了第二大类算法的产生。
2. 基于采样的（sampling-based）又称为概率完备的

这种算法一般是不直接在grid地图进行最小栅格分辨率的规划，它们采用在地图上随机撒一定密度的粒子来抽象实际地图辅助规划。

随机路径图（PRM）及其变种就是在原始地图上进行撒点，抽取roadmap在这样一个拓扑地图上进行规划。用随机路径图（PRM）法寻找给定地图中两点之间的路径，PRM进行路径规划的步骤：学习阶段：在给定图的自由空间里随机撒点（自定义个数），构建一个路径网络图。查询阶段：查询从一个起点到一个终点的路径，可以用A*。路径规划方法之-随机路径图法（PRM）_随机路图法_Leo_Xu06的博客-CSDN博客

RRT以及其优秀的变种RRT-connect则是在地图上每步随机撒一个点，迭代生长树的方式，连接起止点为目的，最后在连接的图上进行规划。这些基于采样的算法速度较快，但是生成的路径代价（可理解为长度）较完备的算法高，而且会产生“有解求不出”的情况（PRM的逢Narrow space卒的情况）。这样的算法一般在高维度的规划问题中广泛运用。

https://www.cnblogs.com/21207-iHome/p/7210543.html

3.其他算法

除了这两类之外还有间接的规划算法：Experience-based（Experience Graph经验图算法）算法：基于经验的规划算法，这是一种存储之前规划路径，建立知识库，依赖之进行规划的方法，这种方法牺牲了一定的空间代价达到了速度与完备兼得的优势。

还有一些优化算法，如模拟退火、禁忌搜索、遗传、粒子群、蚁群、神经网络等。

基于栅格地图——遗传算法的机器人最优路径规划MATLAB源代码_MATLAB圆创工作室_新浪博客

此外还有基于广义Voronoi图的方法进行的Fast-marching规划，类似dijkstra规划和势场的融合，该方法能够完备地规划出位于道路中央，远离障碍物的路径。

五．局部路径规划

（1）至于D* 、势场法、DWA(动态窗口法)、SR-PRM属于在动态环境下为躲避动态障碍物、考虑机器人动力学模型设计的规划算法。

势场法； 人工势场法是由Khatib提出的一种虚拟力法.其基本思想是将机器人在环境中的运动视为一种虚拟的人工受力场中的运动.障碍物对机器人产生斥力,目标点产生引力,引力和斥力的合力作为机器人的加速力,来控制机器人的运动方向和计算机器人的位置.该法结构简单,便于低层的实时控制,在实时避障和平滑的轨迹控制方面,得到了广泛的应用,但对存在局部最优解的问题,容易产生死锁现象(Dead Lock),因而可能使机器人在到达目标点之前就停留在局部最优点 人人工势场法_coutamg的博客-CSDN博客

D*；  https://www.cnblogs.com/flyinggod/p/8671053.html

DWA  机器人局部路劲规划方法有很多，ROS中主要采用的是动态窗口法。动态窗口法主要是在速度（v,w）空间中采样多组速度，并模拟机器人在这些速度下一定时间（sim_period）内的轨迹。在得到多组轨迹后，对于这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。该算法突出点在与动态窗口这个名词，它的含义是依据移动机器人的加减速性能限定速度采样空间在一个可行的动态范围内。 ROS Navigation-----局部避障的动态窗口算法（DWA）_倔强不倒翁的博客-CSDN博客

SR-PRM

（2）局部路径规划有人工势场法、模糊逻辑算法和基于行为的路径规划算法等方法。

    参加《智能车辆运动轨迹规划方法的研究_李爱娟》

六．全局路径规划与局部路径规划的关系

参见ros的move_base功能包中的global_planner和local_planner。采用的dwa算法中，在评价阶段的评价函数中，评估前向模拟的每条轨迹，评估标准包括（接近障碍，接近目标，接近全局路径和速度）。丢弃不合法的轨迹（如可能碰到障碍物的轨迹）  ROS与navigation教程-base_local_planner - 创客智造/爱折腾智能机器人

七. 轨迹规划

（1） 轨迹规划器可看成是黑箱，其输入包括路径的“设定”和“约束”，输出是操作臂末端手部的“位姿序列”，表示手部在各个离散时刻的中间形位。操作臂最常用的轨迹规划方法有两种：第一种要求用户对于选定的轨迹结点（插值点）上的位姿、速度和加速度给出一组显式约束（例如连续性和光滑程度等），轨迹规划器从一类函数（例如n次多项式）中选取参数化轨迹，对结点进行插值，并满足约束条件。第二种方法要求用户给出运动路径的解析式；如直角坐标空间中的直线路径，轨迹规划器在关节空间或直角坐标空间中确定一条轨迹来逼近预定的路径。第一种方法中，约束的设定和轨迹规划均在关节空间进行。由于对操作臂手部（直角坐标形位）没有施加任何约束，用户很难弄清手部的实际路径，因此可能会发生与障碍物相碰。第二种方法的路径约束是在直角坐标空间中给定的，而关节驱动器是在关节空间中受控的。因此，为了得到与给定路径十分接近的轨迹，首先不许采用某种函数逼近的方法将直角坐标路径约束转化为关节坐标路径约束，然后确定满足关节路径约束的参数化路径。

总之，关节空间法是以关节角度的函数来描述机器人的轨迹的，关节空间法不必在直角坐标系中描述两个路径点之间的路径形状，计算简单，容易。再者，由于关节空间与直角坐标空间之间不是连续的对应关系，因而不回发生机构的奇异性问题。

在关节空间中进行轨迹规划，需要给定机器人在起始点、终止点手臂的形位。对关节进行插值时，应满足一系列约束条件。在满足所有约束条件下，可以选取不同类型的关节插值函数，生成不同的轨迹。插值方法有：

1、三次多项式插值；2、过路径点的三次多项式插值；3、高阶多项式插值；4、用抛物线过渡的线性插值；5、过路径点的用抛物线过渡的线性插值。

机器人轨迹规划（熊友伦）_过路径点的三次多项式插值_jyc1228的博客-CSDN博客

随着6R机械人的使用性越来越广，对机械人的轨迹规划的要求也越来越高，机器人轨迹规划是机器人执行作业任务的基础。使用的轨迹规划算法，直接决定了机器人的运动轨迹、平滑程度，且使点位运动有足够高的精度且满足规划路径的约束条件。

本文将在对机器人完成建模以及运动学解算后的基础上，对机器人进行笛卡尔空间轨迹规划的研究，主要分为直线轨迹规划与圆弧轨迹规划。

机器人笛卡尔空间坐标系轨迹规划的研究_机器人笛卡尔坐标_ManMan_D的博客-CSDN博客

本文将在对机器人完成建模以及运动学解算后的基础上，对机器人进行笛卡尔空间轨迹规划的研究，主要分为直线轨迹规划与圆弧轨迹规划。

参加书籍《机器人学导论》《机器人学 建模、规划与控制》

（2）车辆的运动轨迹规划与运动控制具有单自由度和双自由度两种结构。

图 1.2 的单自由度结构，运动轨迹规划与运动控制是相互耦合的，设计方法上大多采用预控制技术、驾驶员建模技术等方法。理论上已经证明，如果选择适宜的评价函数、预测控制周期或者驾驶员预瞄的时间足够准确，那么系统是稳定的。然而现实中预测控制比较复杂，且真实驾驶员行为模型的建立缺少成熟理论的指导，所以单自由度结构很难实现。

图 1.3 双自由度结构可以看出，该结构将运动轨迹规划和运动控制进行了解耦，运动轨迹规划是运动控制的前提。因此，要对某种驾驶行为实施控制，首先要根据车辆的行驶状态和道路信息规划出期望的运动轨迹，并从中提取需要的轨迹参数提供给后续跟踪控制器，以便于控制器能够控制车辆按照规划的轨迹行驶。

轨迹规划方法主要有：基于状态空间的轨迹规划方法、基于参数化曲线的轨迹规划方法、针对系统特征的规划方法和基于滚动窗口的轨迹规划等。

   基于状态空间的轨迹规划方法：主要有最优控制方法。最优控制方法是指通过最优控制理论找到可行的控制量 u*(t)，使得系统能够沿着可行轨迹 x*(t)行驶，该轨迹能够使得评价函数 J 最小。

   基于参数化曲线的轨迹规划方法：B样条曲线由一组称作控制点的向量来确定，这些控制点按顺序连接形成一个控制多边形，B样条曲线就是逼近这个控制多边形

   针对系统特征的轨迹规划方法：微分平坦法是针对系统特征的一种轨迹规划方法。微分平坦系统是指可以找到一组系统输出，使得所有状态变量和输入变量都可以由这组输出及其导数决定(不需积分)。

   基于滚动窗口的轨迹规划方法：基于滚动窗口的轨迹规划方法是根据智能车辆传感器提供的实时环境信息，以滚动方式进行在线规划。在每一步的滚动规划中，根据环境信息，生成优化子目标，在当前的滚动窗口中进行轨迹规划，然后对当前的规划策略进行实施，随着滚动窗口的逐步推进，不断获得新的环境信息，从而实现优化与反馈的结合。

   基于轨迹片段的运动规划方法：

   基于ACT-R认知体系的建模方法：人类理性思维的自适应控制(Adaptive Control of Thought-Rational, ACT-R)是认知科学的一种认知行为体系结构，研究目的在于寻找人类组织知识并产生行为的思维规律，人类的认知行为通过在 ACT-R 建立的模型得到反映，ACT-R 认知模型的构建通过编程实现。

   参加《智能车辆运动轨迹规划方法的研究_李爱娟》
（3）轨迹规划则研究的很少，因为大部分科技公司都没能力在车辆动力学领域一展拳脚。科技公司都将精力花在如何生成轨迹上，而对于生成的轨迹是否满足运动学约束、侧滑约束以及执行机构约束，即轨迹的可行性，研究相对较少。

对于无人车这一受非完整性约束的系统，研究人员通常基于车体模型进行轨迹规划。按照车体模型的精确程度，轨迹规划方法可以进一步分为基于模型预测控制 (Model predictive control, MPC) 以及基于几何轨线的规划方法。基于模型预测的无人车轨迹规划方法首先由 Kelly A和Nagy B（有点奇怪的名字） 提出。

https://blog.csdn.net/roslei/article/details/53142074