机器人足球 第二章.移动机器人路径规划

1、第二章 移动机器人的路径规划,内容提要,几何学基础：平面上图形的解析表示 Dijkstra算法 双向Dijkstra算法 Dijkstra算法的局限 A*算法 Lifelong Planning A*,1 几何学基础：平面上图形的解析表示,平面可以表示为一个二维世界 W=R R 凸多边形：其中任意两点的连线上的点仍在多边形内。点集X R2是凸的，当且仅当对任意的两点x1,x2X和0,1有 x1+(1-)x2 X 非凸多边形：如果点集X R2不是凸的，则称X是非凸的。,凸多边形 非凸多边形,1.1 凸多边形区域的表示,一个m面的多边形通过两类特征表示：顶点集合和边集；两个相邻顶点确定了一条直线

2、凸多边形可以用m条边的半平面（half-plane）的交集表示,1.1 凸多边形区域的表示,直线的半平面 给定两点A=(x1,y1)和B=(x2,y2)，所有在直线AB左侧的点具有哪些性质，在AB右侧的点具有哪些性质？ 根据A=(x1,y1)和B=(x2,y2)写出AB对应的直线方程ax+by+c=0；令f(x,y) = ax+by+c，一侧的点满足 f(x,y)0；在直线上的点满足f(x,y)=0 假定A=(x1,y1)和B=(x2,y2)对应的直线从A指向B，使AB左侧的点满足f(x,y)0 直线AB左侧的半平面表示为 HAB=(x,y)W | fAB(x,y) 0,1.1 凸多边形区域的

3、表示,凸多边形上的点表示为其每条边对应的左侧半平面的交集 对于m面的多边形，逆时针方向选取顶点，相邻顶点构成一条直线，分别标号为1.m. 令fi(x,y)是根据i号顶点(xi,yi)和i+1号顶点(xi+1,yi+1)得到的公式，1id(v)+l(v,w)则更新d(w)=d(v)+l(v,w) 标记v为已考察 在扫描到t时算法停止,2.1 Dijkstra算法的特点,算法将扩展一个类似以s为中心的圆形区域，直到t被扫描,s,t,2.1 Dijkstra算法的特点,改进：从s出发搜索，并发地从t出发搜索,s,s,t,2.2 双向Dijkstra算法思想,待解的任务：从源点s到目标点t的最短路径

4、（前向搜索）从s开始执行Dijkstra算法，每个顶点标记其到s的距离df 在原图上进行 （逆向搜索）从t开始执行Dijkstra算法，每个顶点标记其到t的距离dr 在原图的反向图上执行搜索； 反向图与原图的节点相同，有向边(v,w)变为(w,v) 交替地执行“前向搜索”和“逆向搜索”,2.2.1 双向Dijkstra算法的停止条件,a) 当一个顶点v将要被第二次扫描时停止 每个方向各被扫描一次 v一定在最短路径上吗？ b) 记录算法执行过程中已发现的最短路径的长度u c) 你认为正确的停止条件？,作业：,准备Dijkstra算法的实验课 语言：C，C+ （Java） 定义表示加权有向图的数据

5、结构； 在该图上使用Dijkstra算法求解最短路径 验证你的设计的正确性 参考“数据结构”教材 思考：如何实现双向Dijkstra算法,第二节 A*算法和LPA*算法,回顾,障碍物区域在计算机中的表示方法 假定障碍物区域是一个凸多边形 一个点P是否在凸多边形内可以通过判断P与凸多边形各个边对应的“半平面”的关系进行判断 直线的“半平面”：如果f(x,y)=0是一条直线L对应的函数，则L一侧的点（x1,y1）满足f(x1,y1)0 对于凸多边形的每条边，规定其所在直线的方向，使得凸多边形内的点均在每条直线的左侧 如果点P在每条直线的左侧，则P在多边形内部，否则P在多边形外部,回顾,障碍物区域在

6、计算机中的表示方法 障碍物区域表示为在该区域内的所有点构成的集合 判断机器人与障碍物碰撞的方法 将机器人的目的位置看做平面内的一个点 如果该点属于障碍物区域对应的点集，则机器人的目的位置与障碍物将发生碰撞 路径规划算法的目的：在机器人运动之前，计算出一条与障碍物无碰撞的机器人运动路径,回顾,在能够判断机器人与障碍物发生碰撞之后，我们能够在“离散化”的“配置空间”图上标记出机器人不能到达的单元格,Obstacle,回顾,最短路径的算法: Dijkstra算法 分析Dijkstra算法 疑问 Is it efficient enough? Are there ways to improve it?

7、 If yes, how? 双向Dijkstra算法(Bidirectional Dijkstra Algorithm) 这就是“研究”并“创新”的过程！ 你学习了！你研究了么？你创新了么？,A*算法和LPA*算法,我们继续学习最短路径问题的求解方法 A*算法是有计算机科学家提出的算法；而Dijkstra算法是由数学家提出的 A*算法的优势何在?其重要因素是什么 本节还将介绍另一类最短路径问题 环境动态变化条件下的最短路径问题 该问题的求解算法Lifelong Planning A* （Sven Koenig et al., Artificial Intelligence Journal, 2

8、004）,1.1 A*算法,？,1.1 A*算法的细节,估价函数(Evaluation Function) f(s) = g(s) + h(s) g(s):从起始节点sstart到s的最短距离 启发函数(Heuristic Function) h(s)：当前节点到目的节点的距离估计 Open表：节点的优先队列，以f值为优先级；记录未被访问的节点 Closed表：记录已被访问的节点,1.2 A*算法的特点,针对“边权非负”的最短路径问题 w() 0 经济领域的“收益”问题满足“边权非负”吗？ 启发函数：h(s) h是由人类定义的：不同的人可以定义不同的启发函数 h表达了人类对于求解某一具体问题的

9、知识 我们定义的函数h需要满足：如果s是目标节点则h(s)=0；如果s不是目标节点则h(s)h*(s),1.3 A*算法的特例与推广,当h(s)=0时， A*算法退化为“宽度优先搜索算法” 如果f(s)=h(s)，则A*算法退化为“贪婪最好优先搜索算法(Greedy Best-first)”；不保证找到最优解 如果f(s)=g(s)+wh(s) (w0,1)，则称为加权A*算法（Weighted A* Algorithm）,2 动态环境中的最短路径问题,在以上的问题中，在平面内的每个障碍物都假定被机器人看到 而，实际环境中，机器人仅能看到它周围一定范围内的障碍物 这意味着我们对整个运动区域的了

10、解不完全，即，我们对世界具有不完全的知识 如何在动态环境中规划机器人的路径？,2.1 动态环境下的最短路径问题模型,运动区域表示为一个由单元格组成的网格（Grid） 两个网格之间的边带有相应的权：如果从网格s1可以到达s2，则c(s1,s2)=1，否则c(s1,s2)= 未被机器人观测到的障碍物被认为不存在 计算出一条从起始节点sstart到目的节点sgoal的最短路径,在右图中当红色的障碍物未被观测到时，对应的网格右图所示,2.2 动态路径规划的基本思想,基于机器人的已有观测，计算从sstart到sgoal的最短路径，之后按照该路径运动；如果在运动到s点时，观测到新的障碍物，则重新计算从ss

11、tart到sgoal的最短路径，之后机器人沿新计算的路径运动； 一种朴素的方法 在每次获得新的观测结果后，机器人调用A*算法计算最短路径 在大多数情况下，不高效。 ？,2.3 Lifelong Planning A*（LPA*）算法思想,通常，一次观测，仅增加少量的世界知识（一次观测所发现的障碍物是有限的）。而且，新增加的障碍物仅影响平面内一部分区域的可达性 我们可以在重新规划路径时，利用上一次规划过程中保留的信息，避免重复的计算，降低计算时间，提高机器人的实时性。,新观测,2.3 Lifelong Planning A*（LPA*）算法思想,哪些网格的信息可以重用，哪些网格的信息必须重新计算

12、？,2.4 LPA*算法记录路径的方法,假定每个单元格s的g(s)值均被计算出来，则可以通过如下方法记录从目标点sgoal到起始点sstart的路径 1） s=sgoal 2）从s开始，从它的相邻节点中选在一个s使得g(s)g(sgoal)； 3）s=s，重复步骤2） 基于这种记录路径的方法，LPA*只需要在机器人发现新的障碍物时，更新那些被新障碍物改变g值的节点,2.5 LPA*算法使用的符号,sstart：起始节点 sgoal：目标节点 c(s,s)：从节点s到节点s的有向边的代价 succ(s)：节点s的子节点集合 pred(s)：结点s的父节点集合 g(s)：从sstart到s的最优距离 h(s)：从s到sgoal的估计距离,2.5 LPA*算法使用的符号,优先队列U中的每个元素的形式为 (s, k(s) k(s)=k1(s),k2(s) k(s)与k(s)的大小根据字典顺序进行比较： 如果k1(s)k1(s)则k(s)k(s)； 否则，如果k1(s)=k1(s)且k2(s)k2(s)则k(s)k(s)； 否则k(s)k(s),2.5 LPA*算法使用的符号,2.6 LPA*算法,2.6 LPA*算法（续）,2.7 LPA*算法的执行过程