1、7.3 路径规划路径规划7.3 路径规划路径规划AGV智智能能化化新新发发展展在在于于自自主主回回避避障障碍碍物物并并到到达达目目标地路径规划。标地路径规划。首先，影响路径规划是AGV自由度数和地图有没有。为了便于了解，以2自由度AGV为例，分别考虑有地图时(环境已知时)和没有地图时(环境未知时)路径动作规划。这里，假定AGV只考虑两个位置自由度(X、Y轴上位置)，不考虑姿态方面一个自由度(绕中心回转)物流工程的路径规划第1页7.3 路径规划路径规划 因为地图是由AGV和障碍物模型，所以有地图时路径规划称为基于模型路径规划(Model-based Path-Planning)。基于模型路径规划
2、称为离线路径规划。没有地图时路径规划，AGV用外部传感器(视觉、超声波传感器、光传感器等)得到一面回避障碍一面抵达目标地路径，由此称为基于传感器路径规划(Sensor-based Path-Planning)。基于传感器路径规划又称为在线路径规划。物流工程的路径规划第2页7.3 路径规划路径规划基于模型路径规划 首先说明为了快速选择最正确最正确(最短最短)路径路径，应采取怎样数据结构来表现地图。最正确最正确(最短最短)路径因为靠近障碍物，假如有位置误差，AGV与障碍物碰撞可能性很高。下面要说明是，为预防碰撞，除了最正确性以外更重视安全性方法，即为了选择离障碍物足够远安全路径，应采取怎样数据结构
3、来表现地图。这里因为采取一个OR表。物流工程的路径规划第3页7.3 路径规划路径规划基于传感器路径规划 AGV用传感器一面检测障碍物一面进行回避并最终抵达目标地路径规划算法，即使存在位置误差，AGV也不会迷失确定路径，而最终抵达目标地附近。物流工程的路径规划第4页7.3 路径规划路径规划7.3.1 路径规划模型路径规划模型1几何模型AGV有2个位置自由度(X、Y轴上位置)和1个姿态自由度(绕中心旋转)。这里，取可全方位移动AGV为例加以说明。因为AGV能全方位移动，所以可忽略AGV方向(姿态自由度)。这么，就能用以最大回转长度为半径圆表示AGV。在此基础上，能够把障碍物几何尺寸径向扩张一个AG
4、V圆半径，同时把AGV缩成一个点(图7-16)。由此，在存在扩张了障碍物地图(XY平面)上，能够规划成为几何点AGV路径。物流工程的路径规划第5页7.3 路径规划路径规划物流工程的路径规划第6页7.3 路径规划路径规划2数学模型首先，说明基于模型路径规划。为了快速选取路径，用所谓图数据结构表示规划数学模型(俗称“地图”)。所谓图就是用弧连接节点数据结构，节点意味着AGV位置，弧意味着两个位置间移动(图7-17)。将移动几何距离、几何距离、工作量或时间加权折算工作量或时间加权折算得出两个位置间移动模型费用，把模型费用希望值作为费用赋于该两个位置间弧上。物流工程的路径规划第7页7.3 路径规划路径
5、规划ABEDCF9图图7-17 7-17 图图(节点和弧节点和弧)物流工程的路径规划第8页7.3 路径规划路径规划当全部节点间移动工作量不变时，弧上赋于费用能够直接用几何距离标识。弧记忆进入节点和输出节点，总是回到原来地方(程序上称为“指针返回”)。而且，假如能在两个方向移动则用无向弧，只能单方向移动用有向弧。物流工程的路径规划第9页7.3 路径规划路径规划7.3.2 基于模型路径规划基于模型路径规划这里介绍两个经典图。一个是管理从起始节点ns到目标节点ng最短路径切线图(Tangential Graph)，另一个是连接这些节点安全路径，即管理尽可能离开障碍物路径Voronoi图(Vorono
6、i Graph)。不论哪一个图都是由节点和弧组成，用节点表示节点表示起始点、经过点、目标点；用无向弧表示其间路径，其上附加有作为费用欧几里得距离。最终，不论哪个图，都是用算法都是用算法A选出任意路径选出任意路径，用算法A*选出最正确(满足)路径。物流工程的路径规划第10页7.3 路径规划路径规划1切线图切线图用障碍物切线表示弧。由此可选择从起始节点ns到目标节点ng最正确(最短)路径。即切线图是把障碍物边界切线化得到。从起始节点ns开始，过两相邻节点向障碍物边界作切线，每两条切线交点形成辅助节点。由主节点(起始点、经过点、目标点)、辅助节点和节点间连线组成了切线图。首先把对应起始点S和目标点G
7、两个节点ns和ng标注在新切线图上，然后用算法算法A*选出最正确(最短)路径P，物流工程的路径规划第11页7.3 路径规划路径规划 最后，使点AGV沿着路径P进行PTP(Point-To-Point)控制和CP(Continuous Path)控制，把AGV引导到目地。假如在这种控制过程中产生位置误差，机器人碰撞障碍物可能性会较高，因为AGV几乎靠近障碍物行走 物流工程的路径规划第12页7.3 路径规划路径规划2Voronoi图 Voronoi图可用弧表示距两个以上障碍物和墙可用弧表示距两个以上障碍物和墙壁表面等距离点阵壁表面等距离点阵，用节点表示它们交叉位置用节点表示它们交叉位置。首先首先把
8、对应起始点S和目标点G起始节点ns和目标节点ng标注在图上，然后然后用搜索算法算法A*A*选出安全路径安全路径P P，最终，使点AGV沿着路径P进行PTP控制和CP控制，把AGV引导到目标地。因为选择是安全路径，所以，即使产生位置误差，AGV也能够在离障碍物足够远路径上走行。物流工程的路径规划第13页7.3 路径规划路径规划物流工程的路径规划第14页7.3 路径规划路径规划3.搜索算法A*(A)这里要介绍是，把前面所说切线图和Voronoi图作为搜索图G，选出从起始节点ns到目标节点ng最正确(或满足)路径算法A*(或选出任任意意路路径径算法A)。算法A*(或A)一面计算节点n费用f(n)，一
9、面搜索图G。费用f(n)是从起始节点ns经由当前节点n到目标节点ng最小费用(最短距离)估价函数。物流工程的路径规划第15页7.3 路径规划路径规划可用下式计算：f（n）=g(n)+h(n)式中，g(n)是起始节点ns和当前节点n之间现现时时点点上上最最小小费费用用(最最短短距距离离)；而h(n)是当前节点n和目标节点ng之间最最小小费费用用h*(n)预预计计值值，称为启发式函值。OPEN是管理以后扩展节点明细表，全部节点按费用f（n）递增次序排列，CLOSED是管理已扩展节点明细表。物流工程的路径规划第16页7.3 路径规划路径规划通常A*(或A)等搜索算法，从节点n扩展全部节点n中，把必要
10、节点同费用f(n)都标注OPEN上(参看算法第步)，这个操作称为“扩展节点n”。算法A*(A)程序流程说明：把起始节点ns代入OPEN。假如OPEN是空表，因为路径不存在，所以算法终止。从OPEN取出先头(费用f最小)节点n，并把它移到CLOSED。物流工程的路径规划第17页7.3 路径规划路径规划假如节点n是目标节点ng，则顺次返回到来自节点上(程序上是追寻(返回)指针)。然后，若是抵达起始节点ns，则终止算法，得到一个路径。假如不是这么，扩展节点n，把指针从其子孙节点n返回到节点n(记住从哪来)。然后，对全部子孙节点n做以下工作:物流工程的路径规划第18页7.3 路径规划路径规划节点n假如
11、不在OPEN或CLOSED表中，则它就是新搜索节点。所以，首先计算预计值h(n)(从节点n到节点ng最短距离预计值).其次,计算评价值 f(n)=g(n)+h(n)(这里g(n)=g(n)+c(n、n),g(ns)=0,c(n、n)是连接节点n和n弧费用)。然后，把节点n同预计值f（n）都代入OPEN。若节点n存在于OPEN或CLOSED表中，则它就是已被搜索节点。于是，把指针换到带来最小值g(n)路径上(变更来自地方)。然后，在这个指针发生替换时，若节点n存在于CLOSED中，则把它返回到OPEN后，再计算值f(n)=g(n)+h(n)。物流工程的路径规划第19页7.3 路径规划路径规划返回
12、到 算法A*(A)程序流程物流工程的路径规划第20页7.3 路径规划路径规划预预计计值值h比比真真值值h*小小或或相相等等时时，上述算法变变为为A*，可选出从起始节点ns到目标节点ng最最正正确确路径路径(总计费用最小路径总计费用最小路径)。若预计值h比真值大，h*算法则变为A，可选出从起始节点ns到目标节点ng满足要求路径(总计费用不是最小路径)。所以，机器人路径规划多用从当前地点(,)到目标地(,)平方范数物流工程的路径规划第21页7.3 路径规划路径规划 定义预计值。这个预计值h经常比h*小，成为算法A*，用它可选择最正确路径。图7-20搜索图G存在预计值h(在节点上用括号给出)比真值h
13、*大节点。比如，节点A和H预计值h是8和4，但抵达目标地最小真值是7和2。因为这个费用评价过大，存在于最短路径上节点H等能够忽略，算法错过了费用8最正确路径，最终得到费用9满足要求路径。物流工程的路径规划第22页7.3 路径规划路径规划EDFGHIBCAS(7)32(5)(5)(5)(3)(8)(1)(4)(3)221332322441图图7-20 7-20 搜索图搜索图G(G(有地方预计值比真值大有地方预计值比真值大)物流工程的路径规划第23页7.3 路径规划路径规划物流工程的路径规划第24页A、起始节点S被代入OPEN图7-21(a)，子节点A和B被代人OPEN图7-21(b)。起始节点S
14、扩展后移到CLOSED，B、因为节点A、B评价值分别为10、8，所以选中扩展节点B。B子节点D、E、F，评价值分别为9、8、10，全都代人OPEN，扩展后节点B被移到CLOSED图7-21(c)。C、节点E评价值最小，选中扩展节点E。E子节点H同评价值10都代入OPEN，扩展后节点E被移到CLOSED。7.3 路径规划路径规划物流工程的路径规划第25页D、OPEN中当前评价值最小子节点是D，所以选中节点D。D子节点H被再次搜索被再次搜索，节点D被移到CLOSED。注意到节点注意到节点H H值值g g，因为过，因为过去费用去费用6(6(经由节点经由节点E E、B B返回到节点返回到节点S)S)比
15、新费比新费用用7(7(经由节点经由节点D D、B B返回到返回到S)S)小，所以不更换小，所以不更换指针指针 图图7-21(d)7-21(d)。指针仍在。指针仍在E E。因为当前OPEN上存在评价值f都为10三个节点A、H、F，此时须对这三个节点A、H、F都分别向下扩展。7.3 路径规划路径规划物流工程的路径规划第26页E、用中止连接节点A，扩展节点C同评价值8都代人OPEN，节点A被移到CLOSED。然后，C两个扩展节点I、H中，选中值f最小节点I（实际上，ACH路径在先已经被否定），节点I同评价值10都代人OPEN，节点C同评价值8都被移到CLOSED。节点节点I扩展节点即扩展节点即为目标
16、，为目标，OPEN保持F、节点、节点H扩展节点即为目标，扩展节点即为目标，OPEN保持7.3 路径规划路径规划物流工程的路径规划第27页G、节点、节点F扩展节点即为目标，扩展节点即为目标，OPEN保持H、当前、当前OPEN上三个节点上三个节点I、H、F评价值都为评价值都为10,其后扩展节点都是目标其后扩展节点都是目标G。计算分别经三个节点F、H、I到目标G评价值,经节点F到目标G评价值最小。所以用中止连接扩展节所以用中止连接扩展节点点F，节点，节点G连同评价值连同评价值9都代人都代人OPEN，节，节点点F移到移到CLOSED图图7-21(e)。7.3 路径规划路径规划物流工程的路径规划第28页
17、 在图7-22搜索图G上，全部节点预计值h(在节点上用括号给出)经常比真值h*小或相等。7.3 路径规划路径规划物流工程的路径规划第29页7.3 路径规划路径规划物流工程的路径规划第30页A、起始节点S被代入OPEN，子节点A、B连同评价值f6、8被代人OPEN，起始节点S扩展后移到CLOSED。B、因为节点A、B评价值f分别为6、8，所以节点A扩展子节点C、D后移到CLOSED，节点B、C、D连同各自评价值8、8、9都代入OPEN。7.3 路径规划路径规划物流工程的路径规划第31页C、用中止连接扩展节点B，节点B扩展子节点E、F。子节点E、F连同评价值8、9都代入OPEN，节点B扩展后代入C
18、LOSED。再次搜索节点D。这时，假如注意到节点D值g，因为新费用值4(经由节点B返回到节点S)比过去费用5(经由节点A返回到节点S)小，所所以以要要更更换换指指针针，重新计算评价值f变为9图7-23(a)。D、依然用中止连接扩展节点C，节点C扩展子节点I、H。子节点I、H连同评价值10、9都代 入 OPEN，节 点 C移 到 CLOSED 图 7-23(b)，7.3 路径规划路径规划物流工程的路径规划第32页E、当当前前OPEN上上五五个个节节点点I、H、D、E、F。所以选中评价值f最小扩展节点E。然后，把节点E代入CLOSED，再次搜索节点H。F、这时，注意到节点H值g，比起过去费用7(经
19、由节点C，A返回节点S)来新费用6(经由节点E、B返回到节点S)要小，所以更换指针，重新计算H评价值f为8图7-23(c)7.3 路径规划路径规划物流工程的路径规划第33页G、当当前前OPEN上上扩展值f最小节点H，其其后后扩扩展展节节点点是是目目标标G，节点G以评价值8代人OPEN，节点H代入CLOSEDH、最终，节点G假如选择作为值f最小节点，则把指针返回到节点H、E、B、S，最终得到费用8最短路径。这里，因为节点H预计值h与真值h*相比过小，所以这个最正确路径上节点必须调整。7.3 路径规划路径规划物流工程的路径规划第34页4AGV路径规划实例Dijkstra最短路径确定法。图7-24中
20、节点15是AGV运行路线上岔口，618是货物装装卸卸点点，6是系统入入口口，9、15、16是系统出出口口，19是AGV停车处。在图7-24(b)中，将AGV可能运行方向用箭头进行了表示。7.3 路径规划路径规划物流工程的路径规划第35页7.3 路径规划路径规划物流工程的路径规划第36页(1)因为节点15是找出618货物装卸点间最短距离关键点，故先只考虑系统中各岔口即节点先只考虑系统中各岔口即节点l5间最短路径。间最短路径。为此，将对应箭头所表示距离填人下面距离矩阵。因节点1、3之间无直接连接箭头，但二者之间可经过16、18连在一起，所以C1,3=C1,16+C16,18+Cl8,3=84。C3
21、,4=表示所连表示所连两点间无直接联络，之间联络要经过岔口节点。两点间无直接联络，之间联络要经过岔口节点。由此得到系统距离矩阵(表7-2)。7.3 路径规划路径规划物流工程的路径规划第37页表表7-2 7-2 距离矩阵距离矩阵7.3 路径规划路径规划物流工程的路径规划第38页(2)搜索过程A、最短路径法首先要求找到搜索起始点i，起始点i必须是经过一个单箭头就能和其余节点相连最多节点。很显然，图7-24(b)中节点1符合这个要求，将此节点作为第一层。B、在找出由该i点出发最短距离Ci，k后，令令di，k=Ci，k再以再以k节点作为第节点作为第2次搜索起点。次搜索起点。7.3 路径规划路径规划物流
22、工程的路径规划第39页C、依据距离矩阵中与k节点对应行，计算由起始点出发，经此k点到其余节点最短距离，若到节点j距离最短，则计算di，k+Ck，j D、若、若di，k+Ck，jCi，j 再取再取i j路径路径j点作为第点作为第3次次搜索起点。搜索起点。重复上面搜索过程（将j替替换换k）E、若、若di，k+Ck，jCi，j 令di，j=di，k+Ck，j ，再取再取i k j路径路径j点作为第点作为第3次搜索起点。次搜索起点。如此往返进行，直到全部节点均被选上为止。最短路径法整个搜索过程见表7-3，最短路径计算结果见图7-25，细节见表7-4。7.3 路径规划路径规划物流工程的路径规划第40页表
23、表7-3 7-3 最短路径法搜索步骤最短路径法搜索步骤7.3 路径规划路径规划物流工程的路径规划第41页表表7-4 7-4 最短路径一览表最短路径一览表 7.3 路径规划路径规划物流工程的路径规划第42页7.3.3 基于传感器路径规划基于传感器路径规划(1)基于传感器路径规划指导思想:离障碍物一定距离时，实施变向避障,同时单调单调降低降低抵达目标地距离，从而确保AGV抵达目标地A、首先AGV向目标地直进，若是AGV受到障碍物妨碍，统计碰撞点Hi,同时AGV顺时针或反时针转过障碍物。7.3 路径规划路径规划物流工程的路径规划第43页B、假如目标地方向有空位(脱离点)，统计该脱离点Li,若是脱离点
24、Li比碰撞点Hi更靠近目标地，AGV就离开障碍物向目标地直进(图7-26)。C、若是脱离点Li比碰撞点Hi更远离目标地，AGV就返回碰撞点Hi,反向搜搜索索比碰撞点Hi更靠近目标地脱离点D、按照这个程序，脱离点Li和碰撞点Hi单调靠近目标地G GE E、AGVAGV可看成点机器人。点机器人可能与障碍物碰撞领域Ri单调降低(领域R1R2R3R4)。由此，AGV最终抵达目标地G。7.3 路径规划路径规划物流工程的路径规划第44页算法Class2程序：初始设定L0=S=R，i=0。点机器人R向目标地直进。假如点机器人抵达目标地，因为得到了解，算法终止；假如向量RG被障碍物妨碍，取i=i+1，把当前地点作为碰撞点Hi，执行。7.3 路径规划路径规划物流工程的路径规划第45页点机器人R顺时针或反时针方向绕过障碍物进行追寻。如果点机器人到达目地G，由于得到了解，算法终止；如果欧几里得距离|RG|比|HiG|小，并且向量RG不受障碍物妨碍，把当前地点作为脱离点Li,回到；若点机器人R返回到碰撞点Hi，由于不存在解，所以算法终止。7.3 路径规划路径规划物流工程的路径规划第46页7.3 路径规划路径规划物流工程的路径规划第47页
