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

第二章移动机器人的路径规划内容提要几何学基础：平面上图形的解析表示Dijkstra算法双向Dijkstra算法Dijkstra算法的局限A*算法LifelongPlanningA*1几何学基础：平面上图形的解析表示平面可以表示为一个二维世界W=RR凸多边形：其中任意两点的连线上的点仍在多边形内。点集XR2是凸的，当且仅当对任意的两点x1,x2X和[0,1]有x1+(1-)x2X非凸多边形：如果点集XR2不是凸的，则称X是非凸的。凸多边形非凸多边形1.1凸多边形区域的表示一个m面的多边形通过两类特征表示：顶点集合和边集；两个相邻顶点确定了一条直线凸多边形可以用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；在直线上的点满足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凸多边形区域的表示凸多边形上的点表示为其每条边对应的左侧半平面的交集对于m面的多边形，逆时针方向选取顶点，相邻顶点构成一条直线，分别标号为1..m.令fi(x,y)是根据i号顶点(xi,yi)和i+1号顶点(xi+1,yi+1)得到的公式，1im；令fm(x,y)是由(xm,ym)和(x1,y1)得到的公式第i号直线对应的左半平面Hi={(x,y)W|fi(x,y)0}一个凸的，m面的多边形障碍区域O表示为O=H1H2…Hm1.2非凸多边形的表示首先，将一个非凸多边形分解为多个凸多边形：当有多种分解方法时，任取一种令一个非凸多边形O分解为n个凸多边形O1,O2,…,On，则O=O1O2…On说明：在上式中，Oi和Oj（ij）不一定是不相交的。1.3一般多边形的表示更复杂的多变形可以表示为有限个“原语”的有限次交、并、差运算原语的“差”运算可以转化为“原语”的“并”运算方法？1.4碰撞的表示碰撞：如果机器人对应的图形与障碍物对应的图形存在交点，则产生碰撞。这就要构造一个函数判断某个点是否在一个多边形内部碰撞谓词什么是谓词（Predicate）？},{:FalseTrueW1.4碰撞的表示碰撞谓词对于一条直线f(x,y)=0，定义谓词e(x,y)：对于一个凸多边形，定义谓词(x,y)：对于由n个凸多边形组成的多边形，谓词(x,y)：otherwiseFalseyxfifTrueyxe,0),(,),(),(),(),(),(21yxeyxeyxeyxm),(),(),(1yxyxyxn1.5非线性函数的对平面的分割半径为2的圆周线：f=x2+y2-41.5.1非线性函数对应的“原语”H={(x,y)W|f(x,y)0}检验f=x2+y2–4对应的“原语”是否正确点(1,1)相对于f的位置？点(2,3)相对于f的位置？1.5.2GingerbreadFace的表示xy1.6路径规划问题的求解在二维平面上，我们具有了计算“碰撞”的方法使用“单元格分解法”对二维平面进行离散化我们得到一个网格（Grid）：一个单元格如果与障碍区域相交，则特殊标记。1.6路径规划问题的求解GFFFFFFFFFIObstacle从初始点I到G的路径规划问题转化为网格上的路径搜索问题2Dijkstra算法两点之间的最短路径问题给定一个加权有向图权值l(v,w)0源点s；目标点t求：从s到t的最短路径2.1Dijkstra算法思想以距离的升序考察顶点为每个顶点v记录一个从s到v的已知最短距离d(v)初始化，d(s)=0,对于其它顶点,d(v)=在每个迭代过程中在未考察的顶点中选择一个具有最小d(.)值的顶点v扫描v对于每条边(v,w)，如果d(w)d(v)+l(v,w)则更新d(w)=d(v)+l(v,w)标记v为已考察在扫描到t时算法停止2.1Dijkstra算法的特点算法将扩展一个类似以s为中心的圆形区域，直到t被扫描st2.1Dijkstra算法的特点改进：从s出发搜索，并发地从t出发搜索sst2.2双向Dijkstra算法思想待解的任务：从源点s到目标点t的最短路径（前向搜索）从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）定义表示加权有向图的数据结构；在该图上使用Dijkstra算法求解最短路径验证你的设计的正确性参考“数据结构”教材思考：如何实现双向Dijkstra算法第二节A*算法和LPA*算法回顾障碍物区域在计算机中的表示方法假定障碍物区域是一个凸多边形一个点P是否在凸多边形内可以通过判断P与凸多边形各个边对应的“半平面”的关系进行判断直线的“半平面”：如果f(x,y)=0是一条直线L对应的函数，则L一侧的点（x1,y1）满足f(x1,y1)0，另一侧的点（x2,y2）满足f(x2,y2)0对于凸多边形的每条边，规定其所在直线的方向，使得凸多边形内的点均在每条直线的左侧如果点P在每条直线的左侧，则P在多边形内部，否则P在多边形外部回顾障碍物区域在计算机中的表示方法障碍物区域表示为在该区域内的所有点构成的集合判断机器人与障碍物碰撞的方法将机器人的目的位置看做平面内的一个点如果该点属于障碍物区域对应的点集，则机器人的目的位置与障碍物将发生碰撞路径规划算法的目的：在机器人运动之前，计算出一条与障碍物无碰撞的机器人运动路径回顾在能够判断机器人与障碍物发生碰撞之后，我们能够在“离散化”的“配置空间”图上标记出机器人不能到达的单元格ObstacleGIGI回顾最短路径的算法:Dijkstra算法分析Dijkstra算法疑问Isitefficientenough?Aretherewaystoimproveit?Ifyes,how?双向Dijkstra算法(BidirectionalDijkstraAlgorithm)这就是“研究”并“创新”的过程！你学习了！你研究了么？你创新了么？A*算法和LPA*算法我们继续学习最短路径问题的求解方法A*算法是有计算机科学家提出的算法；而Dijkstra算法是由数学家提出的A*算法的优势何在?其重要因素是什么本节还将介绍另一类最短路径问题环境动态变化条件下的最短路径问题该问题的求解算法——LifelongPlanningA*（SvenKoenigetal.,ArtificialIntelligenceJournal,2004）1.1A*算法？1.1A*算法的细节估价函数(EvaluationFunction)f(s)=g(s)+h(s)g(s):从起始节点sstart到s的最短距离启发函数(HeuristicFunction)h(s)：当前节点到目的节点的距离估计Open表：节点的优先队列，以f值为优先级；记录未被访问的节点Closed表：记录已被访问的节点1.2A*算法的特点针对“边权非负”的最短路径问题w(s1,s2)0经济领域的“收益”问题满足“边权非负”吗？启发函数：h(s)h是由人类定义的：不同的人可以定义不同的启发函数h表达了人类对于求解某一具体问题的知识我们定义的函数h需要满足：如果s是目标节点则h(s)=0；如果s不是目标节点则h(s)h*(s)1.3A*算法的特例与推广当h(s)=0时，A*算法退化为“宽度优先搜索算法”如果f(s)=h(s)，则A*算法退化为“贪婪最好优先搜索算法(GreedyBest-first)”；不保证找到最优解如果f(s)=g(s)+wh(s)(w[0,1])，则称为加权A*算法（WeightedA*Algorithm）2动态环境中的最短路径问题在以上的问题中，在平面内的每个障碍物都假定被机器人看到而，实际环境中，机器人仅能看到它周围一定范围内的障碍物这意味着我们对整个运动区域的了解不完全，即，我们对世界具有不完全的知识如何在动态环境中规划机器人的路径？2.1动态环境下的最短路径问题模型运动区域表示为一个由单元格组成的网格（Grid）两个网格之间的边带有相应的权：如果从网格s1可以到达s2，则c(s1,s2)=1，否则c(s1,s2)=未被机器人观测到的障碍物被认为不存在计算出一条从起始节点sstart到目的节点sgoal的最短路径GIGI在右图中当红色的障碍物未被观测到时，对应的网格右图所示2.2动态路径规划的基本思想基于机器人的已有观测，计算从sstart到sgoal的最短路径，之后按照该路径运动；如果在运动到s点时，观测到新的障碍物，则重新计算从sstart到sgoal的最短路径，之后机器人沿新计算的路径运动；……一种朴素的方法在每次获得新的观测结果后，机器人调用A*算法计算最短路径在大多数情况下，不高效。？2.3LifelongPlanningA*（LPA*）算法思想通常，一次观测，仅增加少量的世界知识（一次观测所发现的障碍物是有限的）。而且，新增加的障碍物仅影响平面内一部分区域的可达性我们可以在重新规划路径时，利用上一次规划过程中保留的信息，避免重复的计算，降低计算时间，提高机器人的实时性。GIGs新观测2.3LifelongPlanningA*（LPA*）算法思想哪些网格的信息可以重用，哪些网格的信息必须重新计算？2.4LPA*算法记录路径的方法假定每个单元格s的g(s)值均被计算出来，则可以通过如下方法记录从目标点sgoal到起始点sstart的路径1）s=sgoal2）从s开始，从它的相邻节点中选在一个s’使得g(s’)g(sgoal)；3）s=s’，重复步骤2）基于这种记录路径的方法，LPA*只需要在机器人发现新的障碍物时，更新那些被新障碍物改变g值的节点2.5LPA*算法使用的符号sstart：起始节点sgoal：目标节点c(s,s’)：从节点s到节点s’的有向边的代价succ(s)：节点s的子节点集合pred(s)：结点s的父节点集合g(s)：从sstart到s的最优距离h(s)：从s到sgoal的估计距离2.5LPA*算法使用的符号优先队列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.5LPA*算法使用的符号2.6LPA*算法2.6LPA*算法（续）2.7LPA*算法的执行过程以8个方向均可通行的网格平面问题为例h(s)=max(|xs-xgoal|,|ys-ygoal|)h(B1)=max(1,4)=4第一次搜索过程第二次搜索过程疑问？对于一个节点，它的rhs值与g值的大