教研室网站建设,照片编辑软件app,软件编程和网站开发差别,新手学做网站书来源 | 知乎知圈 | 进“高精度地图社群”#xff0c;请加微信15221054164#xff0c;备注地图目录1 自主机器人近距离操作运动规划体系1.1 单个自主机器人的规划体系1.2 多自主机器人协同规划体系2 路径规划研究2.1 图搜索法2.1.1 可视图法2.1.2 Dijkstra算法2.1.3 A*算法2.2… 来源 | 知乎知圈 | 进“高精度地图社群”请加微信15221054164备注地图目录1 自主机器人近距离操作运动规划体系········1.1 单个自主机器人的规划体系········1.2 多自主机器人协同规划体系2 路径规划研究········2.1 图搜索法················2.1.1 可视图法················2.1.2 Dijkstra算法················2.1.3 A*算法········2.2 RRT算法················2.2.1 算法步骤················2.2.2 改进算法········2.3 滚动在线RRT算法················2.3.1 滚动规划················2.3.2 滚动在线RRT算法流程········2.4 人工势场法················2.4.1 基本人工势场法················2.4.2 人工势场法算法改进········2.5 BUG算法················2.5.1 BUG1算法················2.5.2 BUG2算法················2.5.3 TangentBUG算法········2.6 增量式启发算法················2.6.1 LPA*算法················2.6.2 D* Lite算法········2.7 小结自主机器人近距离操作运动规划体系在研究自主运动规划问题之前首先需建立相对较为完整的自主运动规划体系再由该体系作为指导对自主运动规划的各项具体问题进行深入研究。本节将根据自主机器人的思维方式、运动形式、任务行为等特点建立与之相适应的自主运动规划体系。并按照机器人的数量与规模将自主运动规划分为单个机器人的运动规划与多机器人协同运动规划两类规划体系。1.1 单个自主机器人的规划体系运动规划系统是自主控制系统中主控单元的核心部分因此有必要先研究自主控制系统和其主控单元的体系结构问题。自主控制技术研究至今先后出现了多种体系结构形式目前被广泛应用于实践的是分布式体系结构其各个功能模块作为相对独立的单元参与整个体系。随着人工智能技术的不断发展基于多Agent的分布式体系结构逐渐成为了主流各功能模块作为独立的智能体参与整个自主控制过程该体系结构应用的基本形式如图1所示。一方面主控单元与测控介入处理、姿态控制系统、轨道控制系统、热控系统、能源系统、数传、有效载荷控制等功能子系统相互独立为智能体由总线相连另一方面主控单元为整个系统提供整体规划以及协调、管理各子系统Agent的行为。测控介入处理Agent保证地面系统对整个系统任意层面的控制介入能力可接受上行的使命级任务、具体的飞行规划和底层的控制指令各子系统Agent存储本分系统的各种知识和控制算法自主完成主控单元发送的任务规划并将执行和本身的健康等信息传回主控单元作为主控单元Agent运行管理和调整计划的依据。图1 基于多Agent的分布式自主控制系统体系结构基本形式示意图主控单元Agent采用主流的分层递阶式结构这种结构层次鲜明并且十分利于实现其基本结构如图2所示。主控单元由任务生成与调度、运动行为规划和控制指令生成三层基本结构组成由任务生成与调度层获得基本的飞行任务经过运动行为规划层获得具体的行为规划再由控制指令生成层得到最终的模块控制指令发送给其它功能Agent。各功能Agent发送状态信息给主控单元的状态检测系统状态检测系统将任务执行情况和子系统状态反馈回任务生成与调度层以便根据具体情况对任务进行规划调整。当遇到突发情况时还可启用重规划模块它可根据当时情况迅速做出反应快速生成行为规划用以指导控制指令生成层得到紧急情况的控制指令。此外地面控制系统在三个层次上都分别具有介入能力。图2中点划线内是主控单元全部模块虚线内为运动规划系统包括运动行为规划模块和重规划模块这也是运动规划系统的主要功能。图2 主控单元基本结构示意图明确了自主控制系统与其主控单元的基本结构以及运动规划系统在主控单元中的基本功能便可建立运动规划系统的体系结构。运动规划系统的体系结构如图3所示该系统由规划器和重规划器两大执行单元组成分别承担对飞行任务的一般规划和对突发事件紧急处理的运动规划。当然这两部分也可理解为离线规划与在线规划两种离线规划一般解决平时按部就班的飞行任务在线规划一般解决突然下达的飞行任务。除规划器以外系统还配有知识域模块用以利用特定语言描述相关知识。知识域包括行为域和模型域两个部分行为域用来存储服务系统一般的运动行为描述和紧急情况下的一些运动行为方面的处理方法(如急停、转向等)模型域用来存储规划所需模型知识包括环境模型、组装体模型、组装任务对象模型和任务模型等等。图3 运动规划系统体系结构示意图1.2 多自主机器人协同规划体系多智能体系统的群体体系结构一般分为集中式、分散式两种基本结构分散式结构又可以进一步分为分层式和分布式结构。集中式结构通常由一个主控单元掌握全部环境和受控机器人信息运用规划算法对任务进行分解并分配给各受控机器人组织它们完成任务。其优点是理论条理清晰实现较为直观缺点是容错性、灵活性和对环境的适应性较差与各受控机器人存在通讯瓶颈问题。相对于集中式结构分散式结构无法得到全局最优解但它凭借着可靠性、灵活性和较强的环境适应性越来越受到广泛的青睐。分散式结构中的分布式结构没有主控单元各智能体地位平等通过各智能体间的通讯和信息交流达到协商的目的实现最终的决策但该结构容易片面强调个体导致占用资源过多且难于得到磋商结果。分层式结构介乎于集中式和分布式之间存在主控单元但并不是由主控单元掌控一切各智能体也具备一定的自主性上下级之间按照一定的规则通过信息流形成完整的整体共同完成协同任务。多自主机器人系统应采用分层式结构以保证整个系统既适于统一领导又满足系统灵活、快速的需求。多自主机器人协同规划体系结构如图4所示按照分层式结构建立两种工作模式事先的离线规划由主控单元负责首先获得协同任务经过规划器得到具体的行为运动规划并分发给各分系统执行单元相关的知识域中主要是用于描述各分系统协商规则的协商域主控单元从外界获取环境信息从各分系统获取状态信息当遇到突发事件或紧急任务变更以及主控单元停止工作时各分系统采用分布式结构单独规划各自运动行为并从各自的知识域中获取协商方式外界环境信息由主控单元发送和自我感知相结合获得(主控单元停止工作时仅靠自我感知获取信息)其它机器人信息的传输由机器人间的数据链实现。图4 多自主机器人协同规划体系结构示意图路径规划研究当给定了某一特定的任务之后如何规划机器人的运动方式将至关重要。机器人的规划包括两部分内容基座移动到适合操作的位置和转动手臂关节完成操作。包括三个问题基座点到点运动规划关节空间规划综合规划。本章研究几种常用的运动规划算法图搜索法、RRT算法、人工势场法、BUG算法。并对部分算法的自身缺陷进行了一些改进。2.1 图搜索法图搜索法依靠已知的环境地图以及地图中的障碍物信息构造从起点到终点的可行路径。主要分成深度优先和广度优先两个方向。深度优先算法优先扩展搜索深度大的节点可以快速的得到一条可行路径但是深度优先算法得到的第一条路径往往是较长的路径。广度优先算法优先扩展深度小的节点呈波状的搜索方式。广度优先算法搜索到的第一条路径就是最短路径。2.1.1 可视图法可视图法由Lozano-Perez和Wesley于1979年提出是机器人全局运动规划的经典算法。可视图法中机器人用点来描述障碍物用多边形描述。将起始点 、目标点 和多边形障碍物的各顶点(设 是所有障碍物的顶点构成的集合)进行组合连接要求起始点和障碍物各顶点之间、目标点和障碍物各顶点之间以及各障碍物顶点与顶点之间的连线均不能穿越障碍物即直线是“可视的”。给图中的边赋权值构造可见图 。其中点集 为所有弧段即可见边的集合。然后釆用某种优化算法搜索从起始点 到目标点 的最优路径那么根据累加和比较这些直线的距离就可以获得从起始点到目标点的最短路径。图5 可视图由此可见利用可视图法规划避障路径主要在于构建可视图而构建可视图的关键在于障碍物各顶点之间可见性的判断。判断时主要分为两种情况同一障碍物各顶点之间可见性的判断以及不同障碍物之间顶点可见性的判断。同一障碍物中相邻顶点可见(通常不考虑凹多边形障碍物中不相邻顶点也有可能可见的情况)不相邻顶点不可见权值赋为 。不同障碍物之间顶点可见性的判断则转化为判断顶点连线是否会与其它顶点连线相交的几何问题。如下图虚线所示、 分别是障碍物 、 的顶点但 与 连线与障碍物其它顶点连线相交故 、 之间不可见而实线所示的 与 连线不与障碍物其它顶点连线相交故 、 之间可见。图6 顶点可见性判断可视图法能求得最短路径但搜索时间长并且缺乏灵活性即一旦机器人的起始点和目标点发生改变就要重新构造可视图比较麻烦。可视图法适用于多边形障碍物对于圆形障碍物失效。切线图法和Voronoi图法对可视图法进行了改进。切线图法用障碍物的切线表示弧因此是从起始点到目标点的最短路径的图移动机器人必须几乎接近障碍物行走。其缺点是如果控制过程中产生位置误差机器人碰撞障碍物的可能性会很高。Voronoi图法用尽可能远离障碍物和墙壁的路径表示弧。因此从起始点到目标点的路径将会增长但采用这种控制方式时即使产生位置误差移动机器人也不会碰到障碍物。2.1.2 Dijkstra算法Dijkstra算法由荷兰计算机科学家艾兹赫尔·戴克斯特拉(Edsger-Wybe Dijkstra)发明通过计算初始点到自由空间内任何一点的最短距离可以得到全局最优路径。算法从初始点开始计算周围4个或者8个点与初始点的距离再将新计算距离的点作为计算点计算其周围点与初始点的距离这样计算像波阵面一样在自由空间内传播直到到达目标点。这样就可以计算得到机器人的最短路径。Dijkstra算法是一种经典的广度优先的状态空间搜索算法即算法会从初始点开始一层一层地搜索整个自由空间直到到达目标点。这样会大大增加计算时间和数据量。而且搜索得到的大量对于机器人运动是无用的。2.1.3 A*算法为了解决Dijkstra算法效率低的问题A*算法作为一种启发式算法被提出。该算法在广度优先的基础上加入了一个估价函数。2.2 RRT算法快速搜索随机树(RRT)算法是一种增量式采样的搜索方法该方法在应用中不需要任何参数整定具备良好的使用性能。它利用增量式方法构建搜索树以逐渐提高分辨能力而无须设置任何分辨率参数。在极限情况该搜索树将稠密的布满整个空间此时搜索树由很多较短曲线或路经构成以实现充满整个空间的目的。增量式方法构建的搜索树其导向取决于稠密采样序列当该序列为随机序列时该搜索树称为快速搜索随机树(Rapidly Exploring Random TreeRRT)而不论该序列为随机还是确定性序列都被称为快速搜索稠密树(Rapidly Exploring Dense TreesRDTs)这种规划方法可处理微分等多种约束。2.2.1 算法步骤考虑二维和三维工作空间环境中包含静态障碍物。初始化快速随机搜索树T只包括根节点即初始状态S。在自由空间中随机选取一个状态点 遍历当前的快速随机搜索树T找到T上距离 最近的节点 考虑机器人的动力学约束从控制输入集 中选择输入 从状态 开始作用经过一个控制周期 到达新的状态 。满足 与 的控制输入 为最佳控制量。将新状态 添加到快速随机搜索树T中。按照这样得到方法不断产生新状态直到到达目标状态G。完成搜索树构建后从目标点开始逐次找到父节点直到到达初始状态即搜索树的根节点。图7 随机树构建过程由于在搜索过程中考虑了机器人的动力学约束因此生成的路径的可行性很好。但是算法的随机性导致其只具备概率完备性。2.2.2 改进算法LaValle等人的工作奠定了RRT方法的基础。在采样策略方面RRTGoalBiaS方法在控制机器人随机运动的同时以一定概率向最终目标运动RRTGoalZoom方法分别在整个空间和目标点周围的空间进行采样RRTCon方法则通过加大随机步长改进规划速度。双向规划思想也被采用衍生出RRTExtExtRRTExtConRRTConCon等多种算法。基本RRT算法收敛到终点位姿的速度可能比较慢。为了提高算法的效率和性能需不断对该算法进行改进。如为了提高搜索效率采用双向随机搜索树(Bi~RRT)从起始点和目标点并行生成两棵RRT直至两棵树相遇算法收敛。由于这个算法相比于原始RRT有更好的收敛性因此在目前路径规划中是很常见的。NikAMelchior提出的粒子RRT算法考虑了地形的不确定性保证了在不确定性环境下搜索树的扩展。Kuffner和Lavane又提出RRT-connectlv使得节点的扩展效率大大提高。运动规划中距离的定义非常复杂Pengcheng研究了在RRT生长过程中距离函数不断学习的算法以降低距离函数对环境的敏感性。考虑到基本RRT规划器得到的路径长度一般是最优路径的1.3~1.5倍英国的J.desmithl研究了变分法技术使其达到最优。Amna A引入KD树作为二级数据结构加速查找距离从环境中取出的随机点最近的叶节点降低了搜索成本。该算法在动态障碍物、高维状态空间和存在运动学、动力学等微分约束的环境中的运动规划已经得到广泛的应用。2.3 滚动在线RRT算法基本RRT算法倾向于遍历整个自由空间直到获得可行路径这使其不可能用于未知或动态环境中的机器人在线运动规划。利用滚动规划的思想可以将RRT算法进行改进使其具备在线规划能力。2.3.1 滚动规划机器人在未知或动态环境中运动时只能探知其传感器范围内有限区域内的环境信息。机器人利用局部信息进行局部运动规划并根据一定的评价准则得到局部目标。机器人到达局部目标后再次进行新的局部规划。如此反复进行直到到达全局目标。滚动规划算法的基本原理环境信息预测在滚动的每一步机器人根据探测到的视野内的信息、或所有已知的环境信息建立环境模型包括设置已知区域内的节点类型信息等局部滚动优化将上述环境信息模型看成一个优化的窗口在此基础上根据目标点的位置和特定的优化策略计算出下一步的最优子目标然后根据子目标和环境信息模型选择局部规划算法确定向子目标行进的局部路径并实施当前策略即依所规划的局部路径行进若干步窗口相应向前滚动反馈信息校正根据局部最优路径驱动机器人行走一段路径后机器人会探测到新的未知信息此时可以根据机器人在行走过程探测到的新信息补充或校正原来的环境模型用于滚动后下一步的局部规划。其中局部子目标是在滚动窗口中寻找一个全局目标的映射它必须避开障碍物且满足某种优化指标。子目标的选择方法反映了全局优化的要求与局部有限信息约束的折衷是在给定信息环境下企图实现全局优化的自然选择。基于滚动窗口的路径规划算法依靠实时探测到的局部环境信息以滚动方式进行在线规划。在滚动的每一步根据探测到的局部信息用启发式方法生成优化子目标在当前滚动窗口内进行局部路径规划然后实施当前策略(依局部规划路径移动一步)随滚动窗口推进不断取得新的环境信息从而在滚动中实现优化与反馈的结合。由于规划问题压缩到滚动窗口内与全局规划相比其计算量大大下降。基于滚动窗口的路径规划算法的具体步骤如下步骤0对起点、终点、工作环境、机器人的视野半径、步长进行初始化步骤1如果终点到达规划中止步骤2对当前滚动窗口内的环境信息进行刷新步骤3产生局部子目标步骤4根据子目标及已知环境信息在当前滚动窗口内规划一条优化的局部可行路径步骤5依规划的局部路径行进一步步长小于视野半径步骤6返回步骤1。2.3.2 滚动在线RRT算法流程在一个滚动窗口内随机树以当前位置为起始点构建传感器范围内的随机树。构建方法与基本RRT算法一致。为了使全局环境中随机树具有向目标方向生长的趋势在运动规划时引入启发信息减少随机树的随机性提高搜索效率。令代表随机树中两个位姿节点间的路径代价 代表随机树中两个位姿节点间的欧几里德距离。类似于A*算法本算法为随机树中每个节点定义一个估价函数其中是随机节点 到树中节点 所需的路径代价。为启发估价函数这里取随机节点 到目标点 的距离为估价值。因此 表示从节点 经随机节点 到目标节点 的路径估计值。遍历滚动窗口内随机树T取估价函数最小值的节点 有 。这使得随机树沿着到目标节点估价值 最小的方向进行扩展。由于在随机树生长中引入了导向目标的启发估价因子叶节点 总是选择离目标最近的节点这可能会使随机树遇到局部极小值问题。因此随机树生长的新节点 必须要克服这个问题引导随机树更好的探索未知空间。这里利用统计学中回归分析生成新节点将RRT算法探索未知空间的能力进一步增强以避免因启发估价因子导致的局部极小。其思想是探索以前到过的空间是无用的而且容易陷入局部极小。引进回归分析(regression analysis)是考察新节点与其他节点之间关系利用回归函数约束使得随机树不探索以前到过的空间因此避免了局部极小。新节点生成方法是遍历随机树如果与其父节点的距离小于与扩展树上其他任意节点的距离即 则选择该节点为随机树新生节点。下图解释了新节点的判断过程。 图8 新节点的判断上图中各个空心点是中间的父节点的可能扩展。椭圆圈起的空心点表示这个新节点不符合回归函数约束剩下的两个未被圈起的空心节点到其父节点的距离小于该节点到随机树上任意节点的距离这两个点可以成为随机树的新节点。综上滚动窗口内随机树构建的具体步骤如下对滚动窗口随机树T初始化T开始只包含初始位置S滚动窗口自由空间中随机选择一个状态 根据最短路径思想寻找树T中和 距离最近的节点 选择输入 使机器人状态由 到 确定 是否符合回归分析不符合则回到第4步将 作为随机树T的一个新节点 则被记录在连接节点 和 的边上。滚动窗口状态空间进行K次采样后遍历随机树根据启发估价思想寻找滚动窗口子目标 。 是当前滚动窗口中的子树中估价函数最小的点。确定子目标后机器人前进到子目标点进行下一轮的滚动RRT规划。如此反复直到到达目标点G。2.4 人工势场法人工势场法是由Khatib提出的一种用于机器人运动规划的虚拟力方法。其基本思想是将目标和障碍物对机器人运动的影响具体化成人造势场。目标处势能低障碍物处势能高。这种势差产生了目标对机器人的引力和障碍物对机器人的斥力其合力控制机器人沿势场的负梯度方向向目标点运动。人工势场法计算方便得到的路径安全平滑但是复杂的势场环境可能在目标点之外产生局部极小点导致机器人无法到达目标。为了解决人工势场法的局部极小点问题学者们提出了各种改进方法。主要分成两个方向一个是构造合适的势函数以减小或避免局部极小点的出现另一种是在机器人遇到局部极小点后结合其他的方法使机器人离开局部极小点。前者一般需要全局地图信息并且依赖于障碍物的形状。当环境复杂时难以应用。后者多利用搜索法、多势场法和沿墙行走法等方法使机器人离开局部极小点。搜索法利用最佳优先、模拟退火、随即搜索等策略寻找比局部极小点势场值更低的点使机器人继续移动。由于未知环境中大多缺乏启发信息搜索方法的效率很低。多势场法构造多个全局极小点相同而局部极小点不同的势函数在机器人陷入某个局部极小点时规划器就切换势函数使机器人离开该点。但是在未知的环境中这样的多个势场很难构造而且该方法可能导致机器人在回到曾逃离的局部极小点。由于局部极小点是某个或多个障碍物的斥力势场与引力势场共同作用产生其位置与障碍物距离必然不远沿墙行走法正是利用这样的远离使机器人在遇到局部极小点后参照类似BUG算法的环绕行为绕过产生局部极小点的障碍物继续前进。这种方法可靠性高不依赖环境的先验信息和障碍物形状。本节构造人工势场进行机器人平动的在线运动规划利用一种沿墙行走法对基本的人工势场法进行改进。2.4.1 基本人工势场法作用在机器人上的假想引力和斥力为势函数的负梯度因而人工势函数应该具有以下特征非负且连续可微斥力势强度距离障碍物越近其强度越大引力势强度离目标位置越近其强度越小。空间中的合势场是引力势场与斥力势场之和 其中 是目标产生的引力势场 是各个障碍物产生的斥力势场之和即 。这里构造如下的引力势函数和斥力势函数其中表示引力势的相对影响表示第 个障碍物的斥力势的相对影响 表示机器人当前位置表示目标点位置表示机器人距目标的距离的作用是在机器人距离目标较远时削弱目标引力势的作用表示机器人距离第个障碍物的距离表示第 个障碍物的斥力势作用范围。和 对势场形状的影响很大适当的增大 能够增强引力势场的作用有助于减少产生局部极小点的可能并加快机器人向目标运动。影响机器人在障碍物附近的运动特性比较大可以使机器人距离障碍物更远运动路径更安全 比较小机器人在避开障碍物时运动比较平滑。利用上面势函数的梯度可以计算机器人收到的假想引力和斥力2.4.2 人工势场法算法改进当机器人的运行环境中包含形状复杂或者距离很近的障碍物时可能出现势场局部极小点导致机器人在该处停止或在其周围振动。如下图所示当环境中出现“陷阱”形障碍物或者与目标成特定位置关系的障碍物时可能在人工势场中产生局部极小点(图中L点)当机器人运动到局部极小点附近时势场的负梯度方向指向L点。机器人将在L点处停止或在其附近振动或作圆周运动。图9 人工势场法的局部极小点为了使机器人从局部极小点中逃离在人工势场法的基础上引入应激行为即增加绕行行为。当机器人遇到局部极小点时忽略目标引力势的作用沿着斥力势的等势面方向移动直到机器人离开局部极小区域。改进的算法流程如下根据传感器信息计算当前位置的引力和斥力判断是否处于绕行行为若是执行3若否执行4判断是否离开局部极小区域若是机器人沿着合力方向运动结束绕行行为若否机器人沿着斥力场等势线运动继续绕行行为判断是否遇到局部极小点若是机器人沿着斥力场等势线运动开始绕行行为若否机器人沿着合力方向运动判断是否到达目标若是退出算法若否继续1使用下面的判别条件判断机器人是否遇到局部极小点。条件1 条件2 当条件1或者条件2出现时就认为机器人遇到了局部极小点。条件1中 是一个很小的正数其含义是机器人受到的虚拟合力接近0。这是最直接局部极小点判断方法。条件2中 为0,1之间某一正数为机器人运动过程中某一状态 表示机器人从 到达当前位置 的总路程条件2成立意味着机器人在运动很长路程后位移很小。用来检测机器人在局部极小点附近发生的振动和圆周运动。2.5 BUG算法BUG算法是一种完全应激的机器人避障算法。其算法原理类似昆虫爬行的运动决策策略。在未遇到障碍物时沿直线向目标运动在遇到障碍物后沿着障碍物边界绕行并利用一定的判断准则离开障碍物继续直行。这种应激式的算法计算简便不需要获知全局地图和障碍物形状具备完备性。但是其生成的路径平滑性不够好对机器人的各种微分约束适应性比较差。2.5.1 BUG1算法该算法的基本思想是在没有障碍物时沿着直线向目标运动可以得到最短的路线。当传感器检测到障碍物时机器人绕行障碍物直到能够继续沿直线项目标运动。BUG1算法实现了最基本的向目标直行和绕行障碍物的思想。假设机器人能够计算两点之间的距离并且不考虑机器人的定位误差。初始位置和目标位置分别用 和 表示机器人在 时刻的位置表示为 表示连接机器人位置 和目标点的直线。初始时。若没有探测到障碍物那么机器人就沿着 向目标直行直到到达目标点或者遇到障碍物。当遇到障碍物时记下当前位置 。然后机器人环绕障碍物直到又一次到达 找到环绕路线上距离目标最近的点并沿着障碍物边界移动到该点。随后直线 更新机器人继续沿直线向目标运动。如果沿这条直线运动时还会遇到该障碍物那么机器人不能到达目标点。否则算法不断循环直到机器人到达目标点或者规划器认为机器人无法到达目标点。图10 BUG1算法运动规划图11 BUG1算法中认为机器人无法到达目标点的情况图12 BUG1算法伪代码2.5.2 BUG2算法BUG2算法也有两种运动朝向目标的直行和沿边界绕行。与BUG1算法不同的是BUG2算法中的直线 是连接初始点和目标点的直线在计算过程中保持不变。当机器人在点遇到障碍物时机器人开始绕行障碍物如果机器人在绕行过程中在距离目标更近的点再次遇到直线 那么就停止绕行继续沿着直线 向目标直行。如此循环直到机器人到达目标点 。如果机器人在绕行过程中未遇到直线 上与目标更近的 点而回到了 点那么得出结论机器人不能到达目标。图13 BUG2算法运动规划图14 BUG2算法中认为机器人无法到达目标点的情况图15 BUG2算法伪代码BUG1和BUG2算法提供了搜索问题的两种基本方法比较保守的BUG1算法进行详细的搜索来获得最佳的离开点。这需要机器人环绕整个障碍物的边界。而BUG2算法使用一种投机的方法。机器人不环绕完整的障碍物而选择第一个可用的点作为离开点。对于一般的环境BUG2算法的效率更高而对于复杂形状的障碍物保守的BUG1算法性能更优。2.5.3 TangentBUG算法TangentBUG算法是对BUG2算法的提高。它利用机器人上距离传感器的读数对障碍物做出提前规避可以获得更短更平滑的机器人路径。在一个静态环境中传感器读数 是机器人位置 和传感器扫描角度 的函数具体点说 是沿着 的射线以角度 到达最近障碍物的距离其中 。对于某一个固定的位置 被传感器视野内的障碍物分割成多个连续区间。如下图所示。 出现不连续或者到达传感器最大测量范围的角度就是这些连续区间的端点。TangentBUG算法利用者些区间的端点避开工作空间中的障碍物。图16 距离传感器扫描障碍物对 不连续的情况做出说明(如图17所示)点 和 与障碍物的不连续性相关请注意这里的射线与障碍物相切。点 是不连续的因为障碍物边界落在传感器的范围之外。 和 和 和 和 之间的free space边界上的点集是连续性的间隔(加粗线部分)。图17 不连续的示意图与其他的BUG算法一样TangentBUG算法也有两种行为直行(motion-to-go)和环绕障碍物(boundary-following)。首先机器人沿着直线向目标运动直到它利用传感器观测到在其运动方向的前方有障碍物。不在机器人前方的障碍物对其向目标运动没有影响。比如下图中的障碍物 障碍物 在传感器视野内但是不阻碍机器人的运动。机器人在刚刚探测到障碍物时传感器视野圆与障碍物边界相切。随着机器人继续向前移动这个切点分裂成两个交点 和 ( 和 ) 和 之间的障碍物边界区间与机器人运动直线相交。图18 机器人向目标运动遇到障碍物此时机器人不能继续向目标运动而从两个交点中选择一个作为局部目标。为比较两个交点对于机器人向目标运动的优劣性定义探索距离 。在没有关于障碍物的其他信息时可以将探索距离 定义为从 经过一个交点到目标点的折线长度即 。例如图19机器人“看见”了障碍并选择向移动因为 当机器人位于 时它无法知道 阻挡了从 到目标 的路径。在图20中当机器人位于 但目标 不同时它具有足够的传感器信息来得出结论 确实阻挡了从 到目标 的路径从而朝 行驶。因此选择向 行驶刚开始的时候可能使得 最小化而不是向 行驶但是planner有效地为 分配无限大的成本代价因为它有足够的信息来推断任何通过 的路径都不是最理想的。图19图20机器人将选择探索距离短的交点作为局部目标向之运动。随着机器人不断运动交点不断更新探索距离也不断减小。如下图所示。当 时机器人还没有探测到障碍物因而它向目标作直线运动当 时机器人开始探测到障碍物并朝向障碍物探索距离近的一侧运动当 和 时机器人继续移动同时更新探测区域在这个过程中探索距离不断减小。图21 机器人运动时不断更新局部目标和探测距离在机器人运动过程中探索距离不再减小时就停止向目标运动行为切换到环绕边界行为。此时机器人找到了探测距离的一个极小值并可计算已探测的障碍物边界与目标 的最近距离 。机器人按照原来的方向环绕障碍物运动同时机器人更新当前探测的障碍物边界与目标的最近距离 。当发现 时机器人停止障碍物环绕行为继续向目标运动。图22 机器人环绕障碍物运动如上图所示当机器人探索到障碍物上的 点后探索距离就不再减小即 点是机器人探索距离在障碍物边界上的局部极小点。机器人开始沿着障碍物边界进行环绕图中虚线路径就是机器人环绕障碍物时所走的路径。当机器人探测到与目标距离相比 点更近的点时重新开始接近目标的运动。2.6 增量式启发算法2.6.1 LPA*算法LPA*算法即Lifelong Planning A*算法该算法于2001年由Sven Koenig和Maxim Likhachev提出是一种增量启发式搜索版本的A*算法这种路径规划算法适用于随着时间改变导致有限栅格地图上的边缘代价c(s1,s2)改变的问题也就是随着时间改变障碍物增多或减少网格点发生增删等在许多场合下比再利用A*重新搜索更高效。2.6.2 D* Lite算法D* Lite算法是以LPA*为基础是Maxim Likhachev和Sven Koenig于2002年基于LPA*结合A*算法思想提出一种增量启发式算法适用于在未知环境中的导航以及路径规划广泛用于目前各种移动机器人和自主车辆载具例如“机遇号”和“勇气号”火星车测试的原型导航系统。2.7 小结本章研究了几种常用的运动规划算法。其中人工势场法应用灵活可以在保证安全的情况下获得一条平滑路径并且对于动态环境可以实现实时运动控制。适合用于长距离机动且障碍物较少的情况。而基于随机采样的搜索树方法可以在复杂约束环境中获得可行解适合用于机械臂近距离操作。参考文献【1】Choset H , Kantor G A , Thrun S . Principles of Robot Motion: Theory, Algorithms, and Implementations[M]. MIT Press, 2005.这本书作者有pdf原文如果需要请知乎私信作者(ID搬砖的旺财)。(PS这本书真的很经典如果做运动规划挺适合细读并且推导里面的公式。)说明文章观点仅供分享交流不代表焉知自动驾驶的立场转载请注明出处如涉及版权等问题请您告知(小老虎13636581676微信同)我们将及时沟通处理。原文链接https://zhuanlan.zhihu.com/p/51372134
