机器人控制理论与技术7

1路径规划研究的三个方面地图划分寻找可行路径路径优化2地图划分寻找可行路径路径优化路径规划研究的三个方面栅格地图拓扑地图线路图可视图Voronoi图3地图划分寻找可行路径路径优化路径规划研究的三个方面Bug2势场法A*方法动态规划法遗传算法4路径规划问题示例SG5A*算法简介P.E.Hart,N.J.Nilsson和B.Raphael共同发表了一篇在启发式搜索方面有深远影响力的论文：IEEETrans.Syst.Sci.andCybernetics,SSC-4(2):100-107,1968N.J.NilssonP.E.HartAformalbasisfortheheuristicdeterminationofminimumcostpathsingraphs6最常用的搜索算法—深度优先算法DepthFirstSearchJohnHopcroftRobertTarjan7老鼠走迷宫89最常用的搜索算法—广度优先算法BreadthFirstSearch10A*算法示例11第一步，环境划分SG12第二步，生成“开启”和“关闭”列表生成一个空的开启列表生成一个空的关闭列表将S添加至开启列表。寻找当前选中点（起点）周围所有可到达或者可通过的方格，也把他们加入开启列表，为所有这些方格保存点A作为“父方格”。13第三步，生成“关闭列表”从开启列表中删除点A，把它加入到一个“关闭列表”。SG14第四步，更新“关闭列表”搜索下一个放入关闭列表的节点F=G+HG=从起点A，沿着产生的路径，移动到网格上指定方格的移动耗费。H=从网格上那个方格移动到终点B的预估移动耗费。把F值最小的节点，从开启列表中删除，然后添加到关闭列表中。15G=10H=30F=G+H=40SGA16G=14H=40F=G+H=54SGBA17SGF54G14H40F60G10H50F74G14H60F40G10H30F?G?H?F54G14H40F60G10H50F74G14H6018第四步，更新“关闭列表”搜索下一个放入关闭列表的节点F=G+HG=从起点A，沿着产生的路径，移动到网格上指定方格的移动耗费。H=从网格上那个方格移动到终点B的预估移动耗费。把F值最小的节点，从开启列表中删除，然后添加到关闭列表中，并将该节点作为当前选中节点。19第五步，更新“开启列表”检查所有相邻节点。如果某个相邻节点还不在开启列表中，把他们添加进开启列表。如果某个相邻节点已经在开启列表里了，检查通过当前节点到达该相邻节点是否G值更低（路径更短）。如果G值更低，则将该节点的父节点更新为当前选中节点；否则不做任何更新。2021第六步，判断终点∈开启列表如果终点不在开启列表中，则重返第四步；如果终点在开启列表中，则从终点开始，按照父节点方向，即可得到从起点到终点的路径。2223A*算法的具体步骤结束目标节点是否在关闭列表？把开始节点放入开启列表寻找开启列表中F值最小的节点，作为当前选中节点，并将其放入关闭列表分析当前节点所有相邻节点根据父节点方向，生成路径该节点是否可通过？该节点是否在关闭列表？该节点是否在开启列表？当前路径是否更好？跳过该节点跳过该节点添加至开启列表确定父节点，并计算F，G，H下一个相邻节点更新该节点信息是否是是是否否否否是开始24A*算法的具体步骤1，把起始格添加到开启列表。2，重复如下的工作：a)寻找开启列表中F值最低的格子。我们称它为当前格。b)把它切换到关闭列表。c)对相邻的8格中的每一个？*如果它不可通过或者已经在关闭列表中，略过它。反之如下。*如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。*如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。d)停止，当你把目标格添加进了开启列表，这时候路径被找到，或者没有找到目标格，开启列表已经空了。这时候，路径不存在。3.保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格25A*算法的特点A*算法是一种典型的启发式搜索算法；F=G+HA*算法与广度优先和深度优先的联系：当g(n)=0时，该算法类似于DFS当h(n)=0时，该算法类似于BFS26A*算法有待改进的地方路径的平滑性搜索速度的快慢三维地图非方形搜索区域不同的地形损耗多个目标……27Bug算法qinitqtargetM-lineLjHjNewleavepointcondition:dd(Hj,Target)28Bug算法1.FrompointLj-1movealongM-lineuntil:a.Targetisreached.Stopb.AnobstacleishitatHj.Goto22.Turnleftandfollowtheboundaryuntil:a.Targetisreached.Stopb.M-linemetatdistancedfromtargetsuchthat:ddist(Hj,qtarget)DefineLj,setj=j+1,andgoto1.c.IfwereturntoHjwithoutmeetingtheM-line,stop.Thetargetistrapped.29Bug2失败示例30人工势场法目标产生一个吸引势场拉动机器人向目标运动障碍物产生排斥势场推开机器人，使其远离障碍物吸引势场与排斥势场之和的负梯度作为机器人的驱动力使得机器人平滑地向目标运动，同时避免碰撞障碍物31人工势场法32人工势场法Attractivepotential33人工势场法Repulsivepotential34C-obstacleAttractivepotentialRepulsivepotentialSumofpotentials人工势场法35得到整个人工势场后，计算势场的负梯度，并将其作为机器人的驱动力。等势图驱动力整个势场人工势场法36障碍物生长法根据机器人的半径扩展障碍物的外边缘37扩展后环境，radius=1.0m障碍物生长法（二）原始环境扩展后环境，radius=0.25m