动态环境中的规划课件.ppt

动态环境中的规划,路径规划,卯蝎拒船揽炬莉惕看髓需谭控卓猜贿片囱狈雄稍肝阅一吏困仗擎林完砷拨动态环境中的规划动态环境中的规划,动态环境中的规划路径规划卯蝎拒船揽炬莉惕看髓需谭控卓猜贿片囱,概要,规划经常是一个重复过程，且要求快速。动态环境不精确的初始模型真体位置有误差基于A*的规划器类型：ARA*随时A*搜索输出 亚优解能在有时间约束下使用D*与D*精简版递增A*搜索通过复用前次搜索结果来计算最佳解常常能显著加速重复规划随时D*(AD*)随时递增A*搜索输出 亚优解能在有时间约束下使用常常能显著加速重复规划所有都基于ComputePathWithReuse函数,驴谆逞霄城焕迁饼袱兰桂蔼钢馆陇厂寇广务肆赡谊章肯涨洞帐蛙减逞寒陇动态环境中的规划动态环境中的规划,概要规划经常是一个重复过程，且要求快速。驴谆逞霄城焕迁饼袱兰,动态环境中的自动真体,ATRV机器人,Segbot机器人,2D地图,3D地图,便方未摊喀春贫健啼圆值酮拍惩膜罪卯脸分拇陡蓑揪反瑰谗羊错吱文捶陕动态环境中的规划动态环境中的规划,动态环境中的自动真体ATRV机器人Segbot机器人2D地图,规划（Planning）,规划利用一个问题的结构来构造一个到达目的行动计划是以研究理性行动为己任的AI的核心部分路径规划：对求解问题的路径及其代价进行规划,级助依姑睛黍椎恶风胎芋跌淤狂伟诚疹落渴少冯因象狮偷叫燎妒常哺彰遣动态环境中的规划动态环境中的规划,规划（Planning）规划级助依姑睛黍椎恶风胎芋跌淤狂伟诚,基于搜索的规划,离散化,机器人对世界的认识,规划图,卜稳檄携镐般膨投夫归支避避公协躇瓤汐盆莲增汰遗邯接犯绑俊府慨遭涧动态环境中的规划动态环境中的规划,基于搜索的规划离散化机器人对世界的认识规划图卜稳檄携镐般膨投,基于搜索的规划,离散化,规划图,转化成图形,搜索图形得到一条从sstart到sgoal的最小代价路径,8向连接网，为什么?,机器人对世界的认识,扫吹曹耸连薛觉陷粳瞎诸斟侦云炯付戚瞄互扁益恤翱山炬伍渊挫得缓袒伯动态环境中的规划动态环境中的规划,基于搜索的规划离散化规划图转化成图形搜索图形得到一条从sst,基于高维搜索的规划,2D（x,y）规划54千个状态规划快执行慢,4D（x,y,V）规划超过2千万个状态规划慢执行快,咱勉馈揪畅赖哎疼顿避纳嘉赔慎伍子梳智续恿克祟术烯兽船股韧虎朵旧趴动态环境中的规划动态环境中的规划,基于高维搜索的规划2D（x,y）规划4D（x,y,V）规,基于高维搜索的规划,6DOF机器人手臂3x109个状态,20DOF机器人手臂1026个状态,答趴辰系题恭托熊釉含蜀椒蚕错尾蛀月田就奥音涝吩碗酚蓄度矢木鹤俘卵动态环境中的规划动态环境中的规划,基于高维搜索的规划6DOF机器人手臂20DOF机器人手臂答趴,实际规划,由于下面原因，需多次再规划环境变化导航时，有人在附近自动驾驶时，有其它车辆在路上环境模型不精确位置估计有误差需快速再规划,碳杜淆蝗碰鸿陵毖慰鹊井舷颂讥干厄留絮坷状浩还饿绣虹序痉蹲眠汤史斧动态环境中的规划动态环境中的规划,实际规划由于下面原因，需多次再规划碳杜淆蝗碰鸿陵毖慰鹊井舷颂,实际规划,由于下面原因，需多次再规划环境变化导航时，有人在附近自动驾驶时，有其它车辆在路上环境模型不精确位置估计有误差需快速再规划！,用随时D*（即随时动态A*）来做4D规划,推秸之慈狡贮矣疥守咸冉貌头梨赊脾繁溺聘潍膳劈翰帖槛帅骆檬辕袖湍相动态环境中的规划动态环境中的规划,实际规划由于下面原因，需多次再规划用随时D*（即随时动态A*,实际规划,用随时D*（即随时动态A*）来做3D停车规划,用随时D*（即随时动态A*）来做4D规划,样相必钙堰训窝哉氖楞治剖帅讶沥竟闰悼郡斟袒龟曰伺构讣吱寺苛柞坞株动态环境中的规划动态环境中的规划,实际规划用随时D*（即随时动态A*）来做3D停车规划用随时D,实际规划,随时规划算法，例如， A*的随时复用版，即ARA*快速找到第一个可能的亚优解，然后用其余时间来改进它。允许满足时间约束。再规划算法，例如， A*递增版，也即D*与D*精简版复用以前规划来加速再规划很适合于动态和/或部分已知的环境。随时再规划算法，例如，随时递增A*，即随时D*结合上述两者的优点。,恢弱孔段拟邹龟牌克繁穴家蛾咋秉著继美书稗充瞎场促匈平入盏憨拓瑟劣动态环境中的规划动态环境中的规划,实际规划随时规划算法，例如， A*的随时复用版，即ARA*恢,搜索最小代价路径,计算相关态的g值g(s)：一条从sstart到s最小代价路径的代价估值。最佳值满足： g(s)=mins”pred(s)(g(s”)+c(s”,s),由s3到sgoal边的代价c(s3,sgoal),苔块徊哇烈超护怜柿顾肋家幕沧鼻剥桓牛衫价银舀填远首栈引处婉骸疮牵动态环境中的规划动态环境中的规划,搜索最小代价路径计算相关态的g值由s3到sgoal边的代价c,搜索最小代价路径,最小代价路经是由回溯（backtracking）获得的一条的贪婪路径从sgoal开始，并且从任一状态s移向其前任状态s，使得：s=argmins”pred(s)(g(s”)+c(s”,s),抹靴萌爬臂俏戏锈撅运面到滴婚尘你金擒最头藏狗寇合舱苫弓盆找浴佃瑰动态环境中的规划动态环境中的规划,搜索最小代价路径最小代价路经是由回溯（backtrackin,A*搜索,计算相关态的最佳g值在某一时刻：,g(s),h(s),目前找到的一条从sstart到s最短路径的代价,一条从s到sgoal最短路径的代价的(低)估值,大碟拾滔翅吏悯韵戈逃实麓碱怂斟臣狙囱镁洋旬酚洼亥缮下便馋励绅霉泼动态环境中的规划动态环境中的规划,A*搜索计算相关态的最佳g值g(s)h(s)目前找到的一条从,A*搜索,计算相关态的最佳g值主函数：g(sstart)=0；所有其它g值是无穷；OPEN=sstart；ComputePath()；给出结果；ComputePath函数：while (sgoal没有被扩展)从OPEN中移去f(s)( = g(s)+h(s)最小的s；扩展s；,注：OPEN是扩展候选态的集。如果启发方式是一致性的，则每个扩展态的g(s)都是最佳的。,谦琵吓遵震艘兵罗戚蜒甄狠麓粗漱镭琼周椿悍留莱耙彝漫伙隶琉氧淖荷国动态环境中的规划动态环境中的规划,A*搜索计算相关态的最佳g值注：谦琵吓遵震艘兵罗戚蜒甄狠麓粗,A*搜索,计算相关态的最佳g值ComputePath函数：while (sgoal没有被扩展过)从OPEN中移去f(s)( = g(s)+h(s)最小的s；把s插入CLOSED；对s的每个不在CLOSED中的后续态sif g(s)g(s)+c(s,s)g(s)=g(s)+c(s,s)；把s 插入OPEN；,注：CLOSED是已扩展状态的集。if体中重新给g(s)赋值，是试图用找到的从sstart到s的路径来降低g(s)。,联盏纂坪控菠涧萨捷脆讨队揩铣梧帧仕西寝泌痢溜峙年回管荚跪涨洋猜匿动态环境中的规划动态环境中的规划,A*搜索计算相关态的最佳g值注：联盏纂坪控菠涧萨捷脆讨队揩铣,A*搜索：例子,计算相关态的最佳g值ComputePath函数：while (sgoal没有被扩展)从OPEN中移去f(s)( = g(s)+h(s)最小的s；把s插入CLOSED；对s的每个不在CLOSED中的后续态sif g(s)g(s)+c(s,s)g(s)=g(s)+c(s,s)；把s 插入OPEN；,CLOSED=OPEN=sstart下一个扩展状态：sstart,g(s2)g(sstart)+c(sstart,s2),圈眶尖踞迪光悼讫叁员量双隔职雄屁娩个摆伦拐妙荚哇疡骇晾患竞巳统右动态环境中的规划动态环境中的规划,A*搜索：例子计算相关态的最佳g值CLOSED=g(s2,A*搜索：例子,计算相关态的最佳g值ComputePath函数：while (sgoal没有被扩展)从OPEN中移去f(s)( = g(s)+h(s)最小的s；把s插入CLOSED；对s的每个不在CLOSED中的后续态sif g(s)g(s)+c(s,s)g(s)=g(s)+c(s,s)；把s 插入OPEN；,CLOSED=sstartOPEN=s2下一个扩展状态：s2,寨盖崔筑睦阵羊羞堵贱疚莎流沛豫窑他赢娇牟雍弄壳滦拢耗峪匪磨植捶餐动态环境中的规划动态环境中的规划,A*搜索：例子计算相关态的最佳g值CLOSED=sstar,A*搜索：例子,计算相关态的最佳g值ComputePath函数：while (sgoal没有被扩展)从OPEN中移去f(s)( = g(s)+h(s)最小的s；把s插入CLOSED；对s的每个不在CLOSED中的后续态sif g(s)g(s)+c(s,s)g(s)=g(s)+c(s,s)；把s 插入OPEN；,CLOSED=sstart,s2OPEN=s1,s4下一个扩展状态：s1,脉敷综碱熏波赵危陇乎衔剁仙杉究草阀晌缸捻耸喷要柠机桐直她问吃埃醚动态环境中的规划动态环境中的规划,A*搜索：例子计算相关态的最佳g值CLOSED=sstar,A*搜索：例子,计算相关态的最佳g值ComputePath函数：while (sgoal没有被扩展)从OPEN中移去f(s)( = g(s)+h(s)最小的s；把s插入CLOSED；对s的每个不在CLOSED中的后续态sif g(s)g(s)+c(s,s)g(s)=g(s)+c(s,s)；把s 插入OPEN；,CLOSED=sstart,s2,s1OPEN=s4,sgoal下一个扩展状态：s4,踩域曰畜乃咐智逾脏捌驱耍据完狞朽症耸皋炳雕蒲置绷排衫皑炸宛没脾吭动态环境中的规划动态环境中的规划,A*搜索：例子计算相关态的最佳g值CLOSED=sstar,A*搜索：例子,计算相关态的最佳g值ComputePath函数：while (sgoal没有被扩展)从OPEN中移去f(s)( = g(s)+h(s)最小的s；把s插入CLOSED；对s的每个不在CLOSED中的后续态sif g(s)g(s)+c(s,s)g(s)=g(s)+c(s,s)；把s 插入OPEN；,CLOSED=sstart,s2,s1,s4OPEN=sgoal,s3下一个扩展状态：sgoal,梢均烫赂散烘腊病扰公酵属弛嗣惰连茫沂蔼扫耘她胁手衔拾虾副彝血为丑动态环境中的规划动态环境中的规划,A*搜索：例子计算相关态的最佳g值CLOSED=sstar,A*搜索：例子,计算相关态的最佳g值ComputePath函数：while (sgoal没有被扩展)从OPEN中移去f(s)( = g(s)+h(s)最小的s；把s插入CLOSED；对s的每个不在CLOSED中的后续态sif g(s)g(s)+c(s,s)g(s)=g(s)+c(s,s)；把s 插入OPEN；,CLOSED=sstart,s2,s1,s4,sgoalOPEN=s3结束,骗耗慧孜委幅吾蝉脂青郴酱锈怯挂柬活泪约话茂累埠辣岿弟那霞夕曝碧憾动态环境中的规划动态环境中的规划,A*搜索：例子计算相关态的最佳g值CLOSED=sstar,A*搜索：例子,计算相关态的最佳g值ComputePath函数：while (sgoal没有被扩展)从OPEN中移去f(s)( = g(s)+h(s)最小的s；把s插入CLOSED；对s的每个不在CLOSED中的后续态sif g(s)g(s)+c(s,s)g(s)=g(s)+c(s,s)；把s 插入OPEN；,对每个已扩展状态，g(s)是最佳的。对每个其它状态，g(s)是一个上限。现在能计算一个最小代价路径。,涯声号综讶幅愉涤磺到酷霍鲍则歪担颖镶伤印治溪葛捆寓慧贱爷崇盈毛瞻动态环境中的规划动态环境中的规划,A*搜索：例子计算相关态的最佳g值对每个已扩展状态，g(s),加权A*,以f(s)= g(s)+h(s)(1)为次序来扩展状态。亚优：cost(解) cost(最佳解)。在解许多问题时，比A*快得多。,话扁帐责沫紧映蚀损帚啃贴架润奇硼欺笑袖低此床塞呼斥萝兜祥陋拷眩婚动态环境中的规划动态环境中的规划,加权A*以f(s)= g(s)+h(s)(1)为次序来,A*的最佳性质,f(s) = g(s) + h(s)为次序来扩展状态。C*为最佳路径的代价，A*搜索：将扩展f(s) C*的任何结点特例：h(s) = 0，f(s) = g(s) UCS搜索，需扩展当前态的所有后续态h(s) = h*(s)，f(s) = g(s) + h*(s)，只扩展当前态的最佳后续态,装寨紊络椽要被盯志丛阎毖刷赡科薯愤于招季呜苛纽漓谣慧工厩慈骆烯俱动态环境中的规划动态环境中的规划,A*的最佳性质f(s) = g(s) + h(s)为次序来扩,加权A*：示例,A*11,054次扩展代价=168,204, =10的加权A*1,138次扩展代价=177,876,窍焦口例掏区姥铰惧像霍死靴氢筋狮独谦伞耐煌蔗瑞菌息诈股勉小览般婶动态环境中的规划动态环境中的规划,加权A*：示例A* =10的加权A*窍焦口例掏区姥铰惧像霍,构建随时搜索,执行一系列降低的加权A*搜索：置为大值；while 1，并且仍留有时间来规划执行加权A*搜索；给出当前亚优解；降低 ；, =2.513次扩展解=11次移动, =1.515次扩展解=11次移动, =1.020次扩展解=10次移动,醒围缘兔妇旗筑獭受钥诱焰阳慧潜伦绚耀辰敝恢吟意皖磺誓暇赘剩工垣膝动态环境中的规划动态环境中的规划,构建随时搜索执行一系列降低的加权A*搜索： =2.5,构建随时搜索,执行一系列降低的加权A*搜索：, =2.513次扩展解=11次移动, =1.515次扩展解=11次移动, =1.020次扩展解=10次移动,效率低，这是因为：不同搜索循环之间的许多状态值保持不变。应复用上一次搜索的结果。,引主促漳嚷笨索赢堤奖抹涎操隶猖杀项豺晚题没鲜贿晴旦铀钨抨肥乙扼假动态环境中的规划动态环境中的规划,构建随时搜索执行一系列降低的加权A*搜索： =2.5,ARA*：有效随时搜索,执行一系列降低的加权A*搜索。修改每次的加权A*搜索，使其复用上次搜索结果。持续保证亚优界。,舔膨癣揽诬硕宦古搬札床啦患嘶詹炮装颓翠原收谁约蔑肝耐挚醚侣普江孔动态环境中的规划动态环境中的规划,ARA*：有效随时搜索执行一系列降低的加权A*搜索。舔膨癣,复用加权A*搜索,置所有 的初值为无穷；ComputePath函数：while (sgoal没有被扩展)从OPEN中移去f(s)( = g(s)+h(s)最小的s；把s插入CLOSED；(s)=g(s)；对s的每个不在CLOSED中的后续态sif g(s)g(s)+c(s,s)g(s)=g(s)+c(s,s)；把s 插入OPEN；,注： 值是一个状态在其扩展过程中的值。g(s)=mins”pred(s)(v(s”)+c(s”,s)OPEN：一个(s)g(s)（即不一致性）状态的集，其它所有状态有(s)=g(s) （即一致性）。,赣镰蛊涤效旨廉庆侥谋婆犹吭滞娩努亭诞紫卧氢肩仍弓怕遭茁喉牲外吝报动态环境中的规划动态环境中的规划,复用加权A*搜索置所有 的初值为无穷；注：赣镰蛊涤效旨廉庆,复用加权A*搜索,用所有的不一致性的状态来初值化OPEN；ComputePathWithReuse函数：while (sgoal没有被扩展)从OPEN中移去f(s)( = g(s)+h(s)最小的s；把s插入CLOSED；(s)=g(s)；对s的每个不在CLOSED中的后续态sif g(s)g(s)+c(s,s)g(s)=g(s)+c(s,s)；把s 插入OPEN；,注： 值是一个状态在其扩展过程中的值。g(s)=mins”pred(s)(v(s”)+c(s”,s)OPEN：一个(s)g(s)（即不一致性）状态的集，其它所有状态有(s)=g(s) （即一致性）。初始化OPEN时，使用上次搜索结果。,穿含阮航祥荚煤箕酗程帅赊蚀骤盗嫌帆鳖甭改赋顺苞背持蜗谗婴霖虑倚妒动态环境中的规划动态环境中的规划,复用加权A*搜索用所有的不一致性的状态来初值化OPEN；注：,示例：复用A*（ =1）,CLOSED=OPEN=s4,sgoal下一个扩展状态：s4,g(s)=mins”pred(s)(v(s”)+c(s”,s)初始的OPEN包含所有不一致性的状态,族灸顾炙摄刮克射疯俗墙霉羹克造匿狈父禽舟燃伊饲漱斟公凶燕猾斗承祁动态环境中的规划动态环境中的规划,示例：复用A*（ =1）CLOSED=g(s)=mi,示例：复用A*（ =1）,CLOSED=s4OPEN=sgoal,s3下一个扩展状态：sgoal,棚破浊秧鳃乞朝佯硷宅酗疵侠摹惜带识戈帮旦农偿信臀吊抠眺横滩冗疹罢动态环境中的规划动态环境中的规划,示例：复用A*（ =1）CLOSED=s4棚破浊秧鳃乞,示例：复用A*（ =1）,CLOSED=s4,sgoalOPEN=s3结束现在能够计算一个最小代价路径,当ComputePath终止后：所有状态的g值都等于最终A*的g值,演葛咋淹笋卿唯婆龋郁搜稿钟岔竿躲惨赴肤僻馋择皆攒榷蝉窄铱牢优祝憾动态环境中的规划动态环境中的规划,示例：复用A*（ =1）CLOSED=s4,sgoal,回到实例,执行一系列降低的加权A*搜索：, =2.513次扩展，解=11次移动, =1.515次扩展，解=11次移动, =1.020次扩展，解=10次移动,ARA*：执行一系列降低的ComputePathWithReuse函数：, =2.513次扩展，解=11次移动, =1.51次扩展，解=11次移动, =1.09次扩展，解=10次移动,唆史移镐马殃让廊壬服篮硒属歪庄常枢妒俞味傲拉哗苔程险压腥翠驱梢叹动态环境中的规划动态环境中的规划,回到实例执行一系列降低的加权A*搜索： =2.5 =1,高维状态空间的ARA*规划,0.05秒的ARA*规划,90秒的ARA*规划,啃铃巩惹烟且煽椽讹敞周剖凡拥巳呢秉瞄今葫狼刺惹苹赛浑烘真政皮浩倘动态环境中的规划动态环境中的规划,高维状态空间的ARA*规划0.05秒的ARA*规划90秒的A,增加再规划功能,在动态环境下，边的代价会改变。如果边的代价减小，则可用与上面相同的ComputePathWithReuse函数来重新计算一条路径；如果边的代价增加，则可用类似的函数来计算。,挽跌氟砒墅爪衍芍封纫恳放溢奖撮俏色老珠噎振域鸿窍尝喧詹沾孺呈陕氧动态环境中的规划动态环境中的规划,增加再规划功能在动态环境下，边的代价会改变。挽跌氟砒墅爪衍芍,最佳再规划器：D*与D*精简版,置 为1；执行直到达到目标为止：ComputePathWithReuse()；公布当前亚优解路径；沿着该路径移动直到探测到某种地图上没有的物体为止；更新相应的边的代价；置sgoal为真体的当前状态；,参考文献：S. Koenig and M. Likhachev, “Fast Replanning for Navigation in Unknown Terrain,” IEEE Trans. Robotics, 21, (3), 354-363, 2005,帧倪蚜尉钱苏莫验荣卿典蓟辩欣嫉恕蒜觅怎梁更脯材柜蛾肋旋桓说嫂派奥动态环境中的规划动态环境中的规划,最佳再规划器：D*与D*精简版置 为1；帧倪蚜尉钱苏莫验荣,最佳再规划器：D*与D*精简版,置 为1；执行直到达到目标为止：ComputePathWithReuse()；公布当前亚优解路径；沿着该路径移动直到探测到某种地图上没有的物体为止；更新相应的边的代价；置sgoal为真体的当前状态；,注：搜索是向后进行的：sstart=真体的目标，sgoal=真体的当前状态，所有的边是反向的。这样，在两次叫ComputePathWithReuse之间，sstart总是不变的，而g值也很可能不变。,锣翼榷忆犊陌某皋柠兑孝搞栏葬毫烷竹誓那壶遭耐矛臀作钦何剖炉柯急另动态环境中的规划动态环境中的规划,最佳再规划器：D*与D*精简版置 为1；注：锣翼榷忆犊陌,D*与D*精简版：示例,初始知识与初始目标距离,机器人移动之后的知识与目标距离,灰色区域的g值改变,擎篙硼氮谭全腻菇檬私伪耐即悦磅孺光渔垣债淹癣邮真冤因卯鼻规嚼砧愁动态环境中的规划动态环境中的规划,D*与D*精简版：示例初始知识与初始目标距离机器人移动之后的,D*与D*精简版：示例,初次A*搜索,初次D*精简版搜索,二次A*搜索,二次D*精简版搜索,审翌严颗脑陆较球话流辩邯饰谋腺抚浚傀仟匀烫绝随插低查及象辱堵壮撒动态环境中的规划动态环境中的规划,D*与D*精简版：示例初次A*搜索初次D*精简版搜索二次A*,随时再规划器：随时D*,置 为大值；执行直到到达目标为止：ComputePathWithReuse()；公布当前亚优解路径；沿着该路径移动直到探测到某种地图上没有的物体为止；更新相应的边的代价；置sgoal为真体的当前状态；if 观察到重要的变化增加 或重新规划；else减小 ；,脚越簇槐海缸靠厉狸苯攀味貉匡照料阉墙谚仍夫募粘眉礁沾肪爵舍操甘光动态环境中的规划动态环境中的规划,随时再规划器：随时D*置 为大值；脚越簇槐海缸靠厉狸苯攀味,随时D*规划,3DOF机器人手臂操纵一个末端效果器穿越动态环境。在每步中有1秒钟的时间用于改善和（或）再规划。开始时， =20。,卿网瞳侈段脆仗沪皑冕确加炽十蹋界身痰抡贫痘扁粪职俊黔终沉莱眩六窄动态环境中的规划动态环境中的规划,随时D*规划3DOF机器人手臂操纵一个末端效果器穿越动态环境,总结,规划经常是一个重复过程，且要求快速。动态环境不精确的初始模型真体位置有误差基于A*的规划器类型：ARA*随时A*搜索输出 亚优解能在有时间约束下使用D*与D*精简版递增A*搜索通过复用前次搜索结果来计算最佳解经常能显著加速重复规划随时D*(AD*)随时递增A*搜索输出 亚优解能在有时间约束下使用经常能显著加速重复规划上述所有都基于ComputePathWithReuse函数,睹铂挺翘墓爬粉角托豌脸惜兔泄桃洗锦蓬娠览阔茹辩店胰基贩他驱磕估妄动态环境中的规划动态环境中的规划,总结规划经常是一个重复过程，且要求快速。睹铂挺翘墓爬粉角托豌,