为您推荐:
精华内容
最热下载
问答
  • 5星
    7KB tkl32172 2021-04-19 20:04:50
  • 493KB weixin_38744270 2019-09-06 17:09:27
  • 本博文主要讨论ROS导航包中集成的局部路径规划算法,DWA、TEB、MPC等算法在使用过程中的各自的优缺点。以下均为自己在使用过程中总结的经验及查阅资料得来,如有理解不到位的地方,还希望在评论区多多讨论。 1. 动态...

    本博文主要讨论ROS导航包中集成的局部路径规划算法,DWA、TEB、MPC等算法在使用过程中的各自的优缺点。以下均为自己在使用过程中总结的经验及查阅资料得来,如有理解不到位的地方,还希望在评论区多多讨论。

    1. 动态窗口法(DWA)

    算法简介:
    DWA算法全称为dynamic window approach,其原理主要是在速度空间(v,w)中采样多组速度,并模拟出这些速度在一定时间内的运动轨迹,并通过评价函数对这些轨迹进行评价,选取最优轨迹对应的速度驱动机器人运动。

    动态窗口法与ROS默认局部路径规划算法TrajectoryPlanner类似,不同之处在于对机器人控制空间的采样:在给定机器人的加速度极限的情况下,TrajectoryPlanner在整个前向模拟周期内从可实现的速度集合中进行采样,而DWA在给定机器人的加速度极限的情况下仅针对一个模拟步骤从可实现的速度集合中进行采样。在实际使用过程中,TrajectoryPlanner和DWA算法效果类似,但是DWA算法更加高效,占用内存更少,所以在这两种算法中间一般直接选择DWA。

    DWA算法在ROS中为dwa_local_planner
    DWA算法分析参考博文DWA算法分析及实现

    优点:

    • 计算复杂度低:考虑到速度和加速度的限制,只有安全的轨迹会被考虑,且每次采样的时间较短,因此轨迹空间较小
    • 可以实现避障:可以实时避障,但是避障效果一般
    • 适用与差分和全向车模

    缺点:

    • 前瞻性不足:只模拟并评价了下一步,如在机器人前段遇见“C”字形障碍时,不能很好的避障
    • 动态避障效果差: 模拟运动轨迹断,动态避障效果差
    • 非全局最优路径: 每次都选择下一步的最佳路径,而非全局最优路径
    • 不适用于阿克曼模型车模

    2. 时间弹性带(TEB)

    算法简介:
    TEB全称为Time Elastic Band,算法浅析参考博文TEB浅析,文中关于eletic band(橡皮筋)的定义:连接起始、目标点,并让这个路径可以变形,变形的条件就是将所有约束当做橡皮筋的外力。关于time eletic band的简述:起始点、目标点状态由用户/全局规划器指定,中间插入N个控制橡皮筋形状的控制点(机器人姿态),当然,为了显示轨迹的运动学信息,我们在点与点之间定义运动时间Time,即为Timed-Elastic-Band算法。通过此方法可以把问题描述为一个多目标优化问题,通过构建超图(hyper-graph),使用g2o框架中的图优化来求解。

    TEB算法在ROS中为teb_local_planner
    TEB算法分析参考以上链接中所列论文如图
    参考论文
    亦可参考博文TEB轨迹优化算法-代码解析
    针对速度障碍模型(VO)参考博文VO避障

    优点:

    • 适用于各种常见车模:如差分、全向、阿克曼模型
    • 有很强的前瞻性: 对前方一段轨迹进行优化
    • 动态避障: 对动态障碍有较好的避障效果,可直接使用其封装好障碍类Obstacle
      如:静态障碍时TEB算法轨迹规划效果
      静态障碍
      当障碍有个0.15m/s向右的速度时,TEB算法轨迹规划效果如下图
      在这里插入图片描述

    缺点:

    • 计算复杂度较大:可通过牺牲预测距离来降低复杂度
    • 速度和角度波动较大、控制不稳定: 源码中是通过两状态之间的距离和角度差及时间差来计算该控制周期内的速度和角速度,使得在控制过程中速度和角度波动较大,如下图所示。在计算资源足够的情况下,提高控制频率可以改善此现象。
      在这里插入图片描述
    • 非全局最优: 但是好于DWA

    改进策略
    针对其控制不稳定问题,主要原因是每个控制周期内,速度变化较大。一种方法是提高控制频率,另一种方法是使用优化的方法,即修改TEB算法的评价函数,把每次速度和角度的变化量除以时间再乘一个代价系数。

    3. 模型预测控制(MPC)

    算法简介
    MPC(Model Predictive Control)与上文提到的DWA和TEB算法不同,MPC只是一个控制器,在自动驾驶领域,其与PID控制器一样,控制器输入包括车辆下一步的运行轨迹,车辆的当前状态,输出是速度和转角。不同之处在于,PID控制器是实时处理当前车辆与目标轨迹的差距来调整输出,使车辆接近目标轨迹,而MPC控制器将未来一个时间段 t 分成 N 个节点,预测每个节点的车辆状态,再调整控制器的输出使车辆尽可能接近参考轨迹。相比于PID控制器的单输入单输出特性,模型预测控制更加适用于多输入多输出的复杂控制系统,可以通过调参,使得车辆的控制更加平稳、更接近于期望轨迹等。

    MPC参考博文基于阿克曼模型和差速模型小车的MPC算法推到及分析
    MPC在ros中为mpc_local_planner

    优缺点:
    目前正在调试MPC,由于现在对MPC的认知还不是太深刻所以具体调试效果及调试经验日后分享,敬请期待。

    展开全文
    weixin_44504228 2021-06-07 20:07:39
  • 滚动规划算法的基本原理: 环境信息预测:在滚动的每一步,机器人根据探测到的视野内的信息、或所有已知的环境信息,建立环境模型,包括设置已知区域内的节点类型信息等; 局部滚动优化:将上述环境信息模型看成一...

    点上方蓝字计算机视觉联盟获取更多干货

    在右上方 ··· 设为星标 ★,与你不见不散

    仅作学术分享,不代表本公众号立场,侵权联系删除

    转载于:https://zhuanlan.zhihu.com/p/51372134, 知乎ID:搬砖的旺财

    AI博士笔记系列推荐

    周志华《机器学习》手推笔记正式开源!可打印版本附pdf下载链接

    1.自主机器人近距离操作运动规划体系

    在研究自主运动规划问题之前,首先需建立相对较为完整的自主运动规划体系,再由该体系作为指导,对自主运动规划的各项具体问题进行深入研究。本节将根据自主机器人的思维方式、运动形式、任务行为等特点,建立与之相适应的自主运动规划体系。并按照机器人的数量与规模,将自主运动规划分为单个机器人的运动规划与多机器人协同运动规划两类规划体系。

    1.1 单个自主机器人的规划体系

    运动规划系统是自主控制系统中主控单元的核心部分,因此有必要先研究自主控制系统和其主控单元的体系结构问题。

    自主控制技术研究至今,先后出现了多种体系结构形式,目前被广泛应用于实践的是分布式体系结构,其各个功能模块作为相对独立的单元参与整个体系。随着人工智能技术的不断发展,基于多Agent的分布式体系结构逐渐成为了主流,各功能模块作为独立的智能体参与整个自主控制过程,该体系结构应用的基本形式如图1所示。一方面,主控单元与测控介入处理、姿态控制系统、轨道控制系统、热控系统、能源系统、数传、有效载荷控制等功能子系统相互独立为智能体,由总线相连;另一方面,主控单元为整个系统提供整体规划,以及协调、管理各子系统Agent的行为。测控介入处理Agent保证地面系统对整个系统任意层面的控制介入能力,可接受上行的使命级任务、具体的飞行规划和底层的控制指令;各子系统Agent存储本分系统的各种知识和控制算法,自主完成主控单元发送的任务规划,并将执行和本身的健康等信息传回主控单元,作为主控单元Agent运行管理和调整计划的依据。

    图1 基于多Agent的分布式自主控制系统体系结构基本形式示意图

    主控单元Agent采用主流的分层递阶式结构,这种结构层次鲜明,并且十分利于实现,其基本结构如图2所示。主控单元由任务生成与调度、运动行为规划和控制指令生成三层基本结构组成,由任务生成与调度层获得基本的飞行任务,经过运动行为规划层获得具体的行为规划,再由控制指令生成层得到最终的模块控制指令,发送给其它功能Agent。各功能Agent发送状态信息给主控单元的状态检测系统,状态检测系统将任务执行情况和子系统状态反馈回任务生成与调度层,以便根据具体情况对任务进行规划调整。当遇到突发情况时,还可启用重规划模块,它可根据当时情况迅速做出反应快速生成行为规划,用以指导控制指令生成层得到紧急情况的控制指令。

    此外,地面控制系统在三个层次上都分别具有介入能力。图2中,点划线内是主控单元全部模块,虚线内为运动规划系统,包括运动行为规划模块和重规划模块,这也是运动规划系统的主要功能。

    图2  主控单元基本结构示意图

    明确了自主控制系统与其主控单元的基本结构,以及运动规划系统在主控单元中的基本功能,便可建立运动规划系统的体系结构。运动规划系统的体系结构如图3所示,该系统由规划器和重规划器两大执行单元组成,分别承担对飞行任务的一般规划和对突发事件紧急处理的运动规划。当然,这两部分也可理解为离线规划与在线规划两种,离线规划一般解决平时按部就班的飞行任务,在线规划一般解决突然下达的飞行任务。除规划器以外,系统还配有知识域模块,用以利用特定语言描述相关知识。知识域包括行为域和模型域两个部分,行为域用来存储服务系统一般的运动行为描述和紧急情况下的一些运动行为方面的处理方法(如急停、转向等),模型域用来存储规划所需模型知识,包括环境模型、组装体模型、组装任务对象模型和任务模型等等。

    图3 运动规划系统体系结构示意图

    1.2 多自主机器人协同规划体系

    多智能体系统的群体体系结构一般分为集中式、分散式两种基本结构,分散式结构又可以进一步分为分层式和分布式结构。集中式结构通常由一个主控单元掌握全部环境和受控机器人信息,运用规划算法对任务进行分解,并分配给各受控机器人,组织它们完成任务。其优点是理论条理清晰,实现较为直观;缺点是容错性、灵活性和对环境的适应性较差,与各受控机器人存在通讯瓶颈问题。相对于集中式结构,分散式结构无法得到全局最优解,但它凭借着可靠性、灵活性和较强的环境适应性越来越受到广泛的青睐。分散式结构中的分布式结构没有主控单元,各智能体地位平等,通过各智能体间的通讯和信息交流达到协商的目的,实现最终的决策,但该结构容易片面强调个体,导致占用资源过多,且难于得到磋商结果。分层式结构介乎于集中式和分布式之间,存在主控单元,但并不是由主控单元掌控一切,各智能体也具备一定的自主性,上下级之间按照一定的规则,通过信息流形成完整的整体,共同完成协同任务。

    多自主机器人系统应采用分层式结构,以保证整个系统既适于统一领导,又满足系统灵活、快速的需求。多自主机器人协同规划体系结构如图4所示,按照分层式结构建立两种工作模式:事先的离线规划由主控单元负责,首先获得协同任务,经过规划器得到具体的行为运动规划,并分发给各分系统执行单元,相关的知识域中主要是用于描述各分系统协商规则的协商域,主控单元从外界获取环境信息,从各分系统获取状态信息;当遇到突发事件或紧急任务变更以及主控单元停止工作时,各分系统采用分布式结构,单独规划各自运动行为,并从各自的知识域中获取协商方式,外界环境信息由主控单元发送和自我感知相结合获得(主控单元停止工作时,仅靠自我感知获取信息),其它机器人信息的传输由机器人间的数据链实现。

    图4 多自主机器人协同规划体系结构示意图

                                                         

                                            

    2.路径规划研究

    当给定了某一特定的任务之后,如何规划机器人的运动方式将至关重要。机器人的规划包括两部分内容:基座移动到适合操作的位置和转动手臂关节完成操作。包括三个问题:基座点到点运动规划;关节空间规划;综合规划。

    本章研究几种常用的运动规划算法:图搜索法、RRT算法、人工势场法、BUG算法。并对部分算法的自身缺陷进行了一些改进。

    2.1 图搜索法

    图搜索法依靠已知的环境地图以及地图中的障碍物信息构造从起点到终点的可行路径。主要分成深度优先和广度优先两个方向。深度优先算法优先扩展搜索深度大的节点,可以快速的得到一条可行路径,但是深度优先算法得到的第一条路径往往是较长的路径。广度优先算法优先扩展深度小的节点,呈波状的搜索方式。广度优先算法搜索到的第一条路径就是最短路径。

    2.1.1 可视图法

    可视图法由Lozano-Perez和Wesley于1979年提出,是机器人全局运动规划的经典算法。可视图法中,机器人用点来描述,障碍物用多边形描述。将起始点  、目标点  和多边形障碍物的各顶点(设  是所有障碍物的顶点构成的集合)进行组合连接,要求起始点和障碍物各顶点之间、目标点和障碍物各顶点之间以及各障碍物顶点与顶点之间的连线均不能穿越障碍物,即直线是“可视的”。给图中的边赋权值,构造可见图  。其中点集  ,  为所有弧段即可见边的集合。然后釆用某种优化算法搜索从起始点  到目标点  的最优路径,那么根据累加和比较这些直线的距离就可以获得从起始点到目标点的最短路径。

    图5 可视图

    由此可见,利用可视图法规划避障路径主要在于构建可视图,而构建可视图的关键在于障碍物各顶点之间可见性的判断。判断时主要分为两种情况,同一障碍物各顶点之间可见性的判断以及不同障碍物之间顶点可见性的判断。

    1. 同一障碍物中,相邻顶点可见(通常不考虑凹多边形障碍物中不相邻顶点也有可能可见的情况),不相邻顶点不可见,权值赋为  。

    2. 不同障碍物之间顶点可见性的判断则转化为判断顶点连线是否会与其它顶点连线相交的几何问题。如下图虚线所示, 分别是障碍物  的顶点,但  与  连线与障碍物其它顶点连线相交,故  之间不可见;而实线所示的  与  连线不与障碍物其它顶点连线相交,故  、  之间可见。

     图6 顶点可见性判断

    可视图法能求得最短路径,但搜索时间长,并且缺乏灵活性,即一旦机器人的起始点和目标点发生改变,就要重新构造可视图,比较麻烦。可视图法适用于多边形障碍物,对于圆形障碍物失效。切线图法和Voronoi图法对可视图法进行了改进。切线图法用障碍物的切线表示弧,因此是从起始点到目标点的最短路径的图,移动机器人必须几乎接近障碍物行走。其缺点是如果控制过程中产生位置误差,机器人碰撞障碍物的可能性会很高。Voronoi图法用尽可能远离障碍物和墙壁的路径表示弧。因此,从起始点到目标点的路径将会增长,但采用这种控制方式时,即使产生位置误差,移动机器人也不会碰到障碍物。

    2.1.2 Dijkstra算法

    Dijkstra算法由荷兰计算机科学家艾兹赫尔·戴克斯特拉(Edsger Wybe Dijkstra)发明,通过计算初始点到自由空间内任何一点的最短距离可以得到全局最优路径。算法从初始点开始计算周围4个或者8个点与初始点的距离,再将新计算距离的点作为计算点计算其周围点与初始点的距离,这样计算像波阵面一样在自由空间内传播,直到到达目标点。这样就可以计算得到机器人的最短路径。

    Dijkstra算法是一种经典的广度优先的状态空间搜索算法,即算法会从初始点开始一层一层地搜索整个自由空间直到到达目标点。这样会大大增加计算时间和数据量。而且搜索得到的大量对于机器人运动是无用的。

    2.1.3 A*算法

    为了解决Dijkstra算法效率低的问题,A*算法作为一种启发式算法被提出。该算法在广度优先的基础上加入了一个估价函数。

    2.2 RRT算法

    快速搜索随机树(RRT)算法是一种增量式采样的搜索方法,该方法在应用中不需要任何参数整定,具备良好的使用性能。它利用增量式方法构建搜索树,以逐渐提高分辨能力,而无须设置任何分辨率参数。在极限情况,该搜索树将稠密的布满整个空间,此时搜索树由很多较短曲线或路经构成,以实现充满整个空间的目的。增量式方法构建的搜索树其导向取决于稠密采样序列,当该序列为随机序列时,该搜索树称为快速搜索随机树(Rapidly Exploring Random Tree,RRT),而不论该序列为随机还是确定性序列,都被称为快速搜索稠密树(Rapidly Exploring Dense Trees,RDTs),这种规划方法可处理微分等多种约束。

    2.2.1 算法步骤

    考虑二维和三维工作空间,环境中包含静态障碍物。初始化快速随机搜索树T,只包括根节点,即初始状态S。在自由空间中随机选取一个状态点  ,遍历当前的快速随机搜索树T,找到T上距离  最近的节点  ,考虑机器人的动力学约束从控制输入集  中选择输入  ,从状态  开始作用,经过一个控制周期  到达新的状态  。满足  与  的控制输入  为最佳控制量。将新状态  添加到快速随机搜索树T中。按照这样得到方法不断产生新状态,直到到达目标状态G。完成搜索树构建后,从目标点开始,逐次找到父节点直到到达初始状态,即搜索树的根节点。

    图7 随机树构建过程

    由于在搜索过程中考虑了机器人的动力学约束,因此生成的路径的可行性很好。但是算法的随机性导致其只具备概率完备性。

    2.2.2 改进算法

    LaValle等人的工作奠定了RRT方法的基础。在采样策略方面,RRTGoalBiaS方法在控制机器人随机运动的同时,以一定概率向最终目标运动;RRTGoalZoom方法分别在整个空间和目标点周围的空间进行采样;RRTCon方法则通过加大随机步长改进规划速度。双向规划思想也被采用,衍生出RRTExtExt,RRTExtCon,RRTConCon等多种算法。

    基本RRT算法收敛到终点位姿的速度可能比较慢。为了提高算法的效率和性能,需不断对该算法进行改进。如为了提高搜索效率采用双向随机搜索树(Bi~RRT),从起始点和目标点并行生成两棵RRT,直至两棵树相遇,算法收敛。由于这个算法相比于原始RRT有更好的收敛性,因此在目前路径规划中是很常见的。NikAMelchior提出的粒子RRT算法,考虑了地形的不确定性,保证了在不确定性环境下搜索树的扩展。

    Kuffner和Lavane又提出RRT-connectlv,使得节点的扩展效率大大提高。运动规划中,距离的定义非常复杂,Pengcheng研究了在RRT生长过程中距离函数不断学习的算法以降低距离函数对环境的敏感性。考虑到基本RRT规划器得到的路径长度一般是最优路径的1.3~1.5倍,英国的J.desmithl研究了变分法技术使其达到最优。Amna A引入KD树作为二级数据结构加速查找距离从环境中取出的随机点最近的叶节点,降低了搜索成本。该算法在动态障碍物、高维状态空间和存在运动学、动力学等微分约束的环境中的运动规划已经得到广泛的应用。

    2.3 滚动在线RRT算法

    基本RRT算法倾向于遍历整个自由空间直到获得可行路径,这使其不可能用于未知或动态环境中的机器人在线运动规划。利用滚动规划的思想可以将RRT算法进行改进,使其具备在线规划能力。

    2.3.1 滚动规划

    机器人在未知或动态环境中运动时,只能探知其传感器范围内有限区域内的环境信息。机器人利用局部信息进行局部运动规划,并根据一定的评价准则得到局部目标。机器人到达局部目标后再次进行新的局部规划。如此反复进行直到到达全局目标。

    滚动规划算法的基本原理:

    1. 环境信息预测:在滚动的每一步,机器人根据探测到的视野内的信息、或所有已知的环境信息,建立环境模型,包括设置已知区域内的节点类型信息等;

    2. 局部滚动优化:将上述环境信息模型看成一个优化的窗口,在此基础上,根据目标点的位置和特定的优化策略计算出下一步的最优子目标,然后根据子目标和环境信息模型,选择局部规划算法,确定向子目标行进的局部路径,并实施当前策略,即依所规划的局部路径行进若干步,窗口相应向前滚动;

    3. 反馈信息校正:根据局部最优路径,驱动机器人行走一段路径后,机器人会探测到新的未知信息,此时可以根据机器人在行走过程探测到的新信息补充或校正原来的环境模型,用于滚动后下一步的局部规划。

    其中,局部子目标是在滚动窗口中寻找一个全局目标的映射,它必须避开障碍物,且满足某种优化指标。子目标的选择方法反映了全局优化的要求与局部有限信息约束的折衷,是在给定信息环境下企图实现全局优化的自然选择。

    基于滚动窗口的路径规划算法依靠实时探测到的局部环境信息,以滚动方式进行在线规划。在滚动的每一步,根据探测到的局部信息,用启发式方法生成优化子目标,在当前滚动窗口内进行局部路径规划,然后实施当前策略(依局部规划路径移动一步),随滚动窗口推进,不断取得新的环境信息,从而在滚动中实现优化与反馈的结合。由于规划问题压缩到滚动窗口内,与全局规划相比其计算量大大下降。

    基于滚动窗口的路径规划算法的具体步骤如下:

    • 步骤0:对起点、终点、工作环境、机器人的视野半径、步长进行初始化;

    • 步骤1:如果终点到达,规划中止;

    • 步骤2:对当前滚动窗口内的环境信息进行刷新;

    • 步骤3:产生局部子目标;

    • 步骤4:根据子目标及已知环境信息,在当前滚动窗口内规划一条优化的局部可行路径;

    • 步骤5:依规划的局部路径行进一步,步长小于视野半径;

    • 步骤6:返回步骤1。

    2.3.2 滚动在线RRT算法流程

    在一个滚动窗口内,随机树以当前位置为起始点,构建传感器范围内的随机树。构建方法与基本RRT算法一致。为了使全局环境中随机树具有向目标方向生长的趋势,在运动规划时引入启发信息,减少随机树的随机性,提高搜索效率。

    令  代表随机树中两个位姿节点间的路径代价,  代表随机树中两个位姿节点间的欧几里德距离。类似于A*算法,本算法为随机树中每个节点定义一个估价函数:  。其中  是随机节点  到树中节点  所需的路径代价。  为启发估价函数,这里取随机节点  到目标点  的距离为估价值,  。因此  表示从节点  经随机节点  到目标节点  的路径估计值。遍历滚动窗口内随机树T,取估价函数最小值的节点  ,有  。这使得随机树沿着到目标节点估价值  最小的方向进行扩展。

    由于在随机树生长中引入了导向目标的启发估价因子,叶节点  总是选择离目标最近的节点,这可能会使随机树遇到局部极小值问题。因此随机树生长的新节点  必须要克服这个问题,引导随机树更好的探索未知空间。

    这里利用统计学中回归分析生成新节点,将RRT算法探索未知空间的能力进一步增强以避免因启发估价因子导致的局部极小。其思想是探索以前到过的空间是无用的,而且容易陷入局部极小。引进回归分析(regression analysis)是考察新节点与其他节点之间关系,利用回归函数约束,使得随机树不探索以前到过的空间,因此避免了局部极小。

    新节点生成方法是遍历随机树,如果  与其父节点  的距离小于  与扩展树上其他任意节点的距离,即  ,则选择该节点为随机树新生节点。下图解释了新节点的判断过程。

                                                 

     图8 新节点的判断

    上图中各个空心点是中间的父节点的可能扩展。椭圆圈起的空心点表示这个新节点不符合回归函数约束,剩下的两个未被圈起的空心节点到其父节点的距离小于该节点到随机树上任意节点的距离,这两个点可以成为随机树的新节点。

    综上,滚动窗口内随机树构建的具体步骤如下:

    1. 对滚动窗口随机树T初始化,T开始只包含初始位置S;

    2. 滚动窗口自由空间中随机选择一个状态 

    3. 根据最短路径思想寻找树T中和  距离最近的节点 

    4. 选择输入  ,使机器人状态由  到 

    5. 确定  是否符合回归分析,不符合则回到第4步;

    6. 将  作为随机树T的一个新节点,  则被记录在连接节点  和  的边上。

    滚动窗口状态空间进行K次采样后,遍历随机树,根据启发估价思想寻找滚动窗口子目标 。  是当前滚动窗口中的子树中估价函数最小的点。确定子目标后,机器人前进到子目标点,进行下一轮的滚动RRT规划。如此反复,直到到达目标点G。

    2.4 人工势场法

    人工势场法是由Khatib提出的一种用于机器人运动规划的虚拟力方法。其基本思想是将目标和障碍物对机器人运动的影响具体化成人造势场。目标处势能低,障碍物处势能高。这种势差产生了目标对机器人的引力和障碍物对机器人的斥力,其合力控制机器人沿势场的负梯度方向向目标点运动。人工势场法计算方便,得到的路径安全平滑,但是复杂的势场环境可能在目标点之外产生局部极小点导致机器人无法到达目标。

    为了解决人工势场法的局部极小点问题,学者们提出了各种改进方法。主要分成两个方向:一个是构造合适的势函数以减小或避免局部极小点的出现;另一种是在机器人遇到局部极小点后结合其他的方法使机器人离开局部极小点。前者一般需要全局地图信息,并且依赖于障碍物的形状。当环境复杂时难以应用。后者多利用搜索法、多势场法和沿墙行走法等方法使机器人离开局部极小点。搜索法利用最佳优先、模拟退火、随即搜索等策略寻找比局部极小点势场值更低的点使机器人继续移动。由于未知环境中大多缺乏启发信息,搜索方法的效率很低。多势场法构造多个全局极小点相同,而局部极小点不同的势函数,在机器人陷入某个局部极小点时,规划器就切换势函数使机器人离开该点。

    但是在未知的环境中这样的多个势场很难构造,而且该方法可能导致机器人在回到曾逃离的局部极小点。由于局部极小点是某个或多个障碍物的斥力势场与引力势场共同作用产生,其位置与障碍物距离必然不远,沿墙行走法正是利用这样的远离,使机器人在遇到局部极小点后参照类似BUG算法的环绕行为绕过产生局部极小点的障碍物继续前进。这种方法可靠性高,不依赖环境的先验信息和障碍物形状。

    本节构造人工势场进行机器人平动的在线运动规划,利用一种沿墙行走法对基本的人工势场法进行改进。

    2.4.1 基本人工势场法

    作用在机器人上的假想引力和斥力为势函数的负梯度,因而人工势函数应该具有以下特征:

    1. 非负且连续可微;

    2. 斥力势强度距离障碍物越近其强度越大;

    3. 引力势强度离目标位置越近其强度越小。

    空间中的合势场是引力势场与斥力势场之和: 

    其中,  是目标产生的引力势场;  是各个障碍物产生的斥力势场之和,即: 

    这里构造如下的引力势函数和斥力势函数:

    其中,  表示引力势的相对影响;  表示第  个障碍物的斥力势的相对影响,  表示机器人当前位置,  表示目标点位置,  表示机器人距目标的距离,  的作用是在机器人距离目标较远时,削弱目标引力势的作用,  表示机器人距离第  个障碍物的距离,  表示第  个障碍物的斥力势作用范围。

     和  对势场形状的影响很大,适当的增大  能够增强引力势场的作用,有助于减少产生局部极小点的可能,并加快机器人向目标运动。  影响机器人在障碍物附近的运动特性,  比较大可以使机器人距离障碍物更远,运动路径更安全;  比较小,机器人在避开障碍物时运动比较平滑。

    利用上面势函数的梯度可以计算机器人收到的假想引力和斥力:

    2.4.2 人工势场法算法改进

    当机器人的运行环境中包含形状复杂或者距离很近的障碍物时,可能出现势场局部极小点,导致机器人在该处停止或在其周围振动。如下图所示,当环境中出现“陷阱”形障碍物或者与目标成特定位置关系的障碍物时,可能在人工势场中产生局部极小点(图中L点),当机器人运动到局部极小点附近时,势场的负梯度方向指向L点。机器人将在L点处停止或在其附近振动或作圆周运动。

    图9 人工势场法的局部极小点

    为了使机器人从局部极小点中逃离,在人工势场法的基础上引入应激行为,即增加绕行行为。当机器人遇到局部极小点时,忽略目标引力势的作用,沿着斥力势的等势面方向移动,直到机器人离开局部极小区域。改进的算法流程如下:

    1. 根据传感器信息计算当前位置的引力和斥力;

    2. 判断是否处于绕行行为,若是,执行3;若否,执行4;

    3. 判断是否离开局部极小区域,若是,机器人沿着合力方向运动,结束绕行行为;若否,机器人沿着斥力场等势线运动,继续绕行行为;

    4. 判断是否遇到局部极小点,若是,机器人沿着斥力场等势线运动,开始绕行行为;若否,机器人沿着合力方向运动;

    5. 判断是否到达目标,若是,退出算法;若否,继续1;

    使用下面的判别条件判断机器人是否遇到局部极小点。

    条件1: 

    条件2: 

    当条件1或者条件2出现时,就认为机器人遇到了局部极小点。条件1中  是一个很小的正数,其含义是机器人受到的虚拟合力接近0。这是最直接局部极小点判断方法。条件2中  为0,1之间某一正数,  为机器人运动过程中某一状态,  表示机器人从  到达当前位置  的总路程,条件2成立意味着机器人在运动很长路程后,位移很小。用来检测机器人在局部极小点附近发生的振动和圆周运动。

    2.5 BUG算法

    BUG算法是一种完全应激的机器人避障算法。其算法原理类似昆虫爬行的运动决策策略。在未遇到障碍物时,沿直线向目标运动;在遇到障碍物后,沿着障碍物边界绕行,并利用一定的判断准则离开障碍物继续直行。这种应激式的算法计算简便,不需要获知全局地图和障碍物形状,具备完备性。但是其生成的路径平滑性不够好,对机器人的各种微分约束适应性比较差。

    2.5.1 BUG1算法

    该算法的基本思想是在没有障碍物时,沿着直线向目标运动可以得到最短的路线。当传感器检测到障碍物时,机器人绕行障碍物直到能够继续沿直线项目标运动。BUG1算法实现了最基本的向目标直行和绕行障碍物的思想。

    假设机器人能够计算两点之间的距离,并且不考虑机器人的定位误差。初始位置和目标位置分别用  和  表示;机器人在  时刻的位置表示为  ;  表示连接机器人位置  和目标点的直线。初始时,  。若没有探测到障碍物,那么机器人就沿着  向目标直行,直到到达目标点或者遇到障碍物。当遇到障碍物时,记下当前位置  。然后机器人环绕障碍物直到又一次到达  ,找到环绕路线上距离目标最近的点  ,并沿着障碍物边界移动到该点。随后,直线  更新,机器人继续沿直线向目标运动。如果沿这条直线运动时还会遇到该障碍物,那么机器人不能到达目标点。否则算法不断循环直到机器人到达目标点或者规划器认为机器人无法到达目标点。

                                                   

    图10 BUG1算法运动规划

                                

    图11 BUG1算法中认为机器人无法到达目标点的情况

                                                        

    图12 BUG1算法伪代码

    2.5.2 BUG2算法

    BUG2算法也有两种运动:朝向目标的直行和沿边界绕行。与BUG1算法不同的是,BUG2算法中的直线  是连接初始点和目标点的直线,在计算过程中保持不变。当机器人在点遇到障碍物时,机器人开始绕行障碍物,如果机器人在绕行过程中在距离目标更近的点再次遇到直线  ,那么就停止绕行,继续沿着直线  向目标直行。如此循环,直到机器人到达目标点  。如果机器人在绕行过程中未遇到直线  上与目标更近的  点而回到了  点,那么得出结论,机器人不能到达目标。

    图13  BUG2算法运动规划

    图14  BUG2算法中认为机器人无法到达目标点的情况

    图15 BUG2算法伪代码

    BUG1和BUG2算法提供了搜索问题的两种基本方法:比较保守的BUG1算法进行详细的搜索来获得最佳的离开点。这需要机器人环绕整个障碍物的边界。而BUG2算法使用一种投机的方法。机器人不环绕完整的障碍物,而选择第一个可用的点作为离开点。对于一般的环境,BUG2算法的效率更高;而对于复杂形状的障碍物,保守的BUG1算法性能更优。

    2.5.3 TangentBUG算法

    TangentBUG算法是对BUG2算法的提高。它利用机器人上距离传感器的读数对障碍物做出提前规避,可以获得更短更平滑的机器人路径。在一个静态环境中,传感器读数  是机器人位置  和传感器扫描角度  的函数,具体点说,  是沿着  的射线以角度  到达最近障碍物的距离,

    其中  。

    对于某一个固定的位置  ,  被传感器视野内的障碍物分割成多个连续区间。如下图所示。  出现不连续或者到达传感器最大测量范围的角度就是这些连续区间的端点。TangentBUG算法利用者些区间的端点避开工作空间中的障碍物。

                                               

    图16 距离传感器扫描障碍物

    对  不连续的情况做出说明(如图17所示):点  ,  ,  ,  ,  ,  ,  和  与障碍物的不连续性相关;请注意,这里的射线与障碍物相切。点  是不连续的,因为障碍物边界落在传感器的范围之外。  和  ,  和  ,  和  ,  和  之间的free space边界上的点集是连续性的间隔(加粗线部分)。

    图17 不连续的示意图

    与其他的BUG算法一样,TangentBUG算法也有两种行为:直行(motion-to-go)和环绕障碍物(boundary-following)。

    首先机器人沿着直线向目标运动,直到它利用传感器观测到在其运动方向的前方有障碍物。不在机器人前方的障碍物对其向目标运动没有影响。比如下图中的障碍物 ,障碍物  在传感器视野内,但是不阻碍机器人的运动。机器人在刚刚探测到障碍物时,传感器视野圆与障碍物边界相切。随着机器人继续向前移动,这个切点分裂成两个交点  和  (  和  ),  和  之间的障碍物边界区间与机器人运动直线相交。

                                          

    图18 机器人向目标运动遇到障碍物

    此时,机器人不能继续向目标运动,而从两个交点中选择一个作为局部目标。为比较两个交点对于机器人向目标运动的优劣性,定义探索距离 。在没有关于障碍物的其他信息时,可以将探索距离  定义为从  经过一个交点到目标点的折线长度,即: 

    例如图19,机器人“看见”了障碍  ,并选择向  移动,因为  。

    当机器人位于  时,它无法知道  阻挡了从  到目标  的路径。

    在图20中,当机器人位于  但目标  不同时,它具有足够的传感器信息来得出结论:  确实阻挡了从  到目标  的路径,从而朝  行驶。

    因此,选择向  行驶刚开始的时候可能使得  最小化,而不是向  行驶,但是planner有效地为  分配无限大的成本代价,因为它有足够的信息来推断任何通过  的路径都不是最理想的。

                                                                   

    图19


    图20

    机器人将选择探索距离短的交点作为局部目标,向之运动。随着机器人不断运动,交点不断更新,探索距离也不断减小。如下图所示。当  时,机器人还没有探测到障碍物,因而它向目标作直线运动;当  时,机器人开始探测到障碍物,并朝向障碍物探索距离近的一侧运动;当  和  时,机器人继续移动,同时更新探测区域,在这个过程中探索距离不断减小。

    图21 机器人运动时不断更新局部目标和探测距离

    在机器人运动过程中,探索距离不再减小时,就停止向目标运动行为,切换到环绕边界行为。此时,机器人找到了探测距离的一个极小值,并可计算已探测的障碍物边界与目标  的最近距离 。机器人按照原来的方向环绕障碍物运动,同时机器人更新当前探测的障碍物边界与目标的最近距离 。当发现  时,机器人停止障碍物环绕行为,继续向目标运动。

    图22 机器人环绕障碍物运动

    如上图所示,当机器人探索到障碍物上的  点后,探索距离就不再减小,即  点是机器人探索距离在障碍物边界上的局部极小点。机器人开始沿着障碍物边界进行环绕,图中虚线路径就是机器人环绕障碍物时所走的路径。当机器人探测到与目标距离相比  点更近的点时,重新开始接近目标的运动。

    2.6 增量式启发算法

    2.6.1 LPA*算法

    LPA*算法,即Lifelong Planning A*算法,该算法于2001年由Sven Koenig和Maxim Likhachev提出,是一种增量启发式搜索版本的A*算法,这种路径规划算法适用于随着时间改变导致有限栅格地图上的边缘代价c(s1,s2)改变的问题,也就是随着时间改变障碍物增多或减少,网格点发生增删等,在许多场合下比再利用A*重新搜索更高效。

    2.6.2 D* Lite算法

    D* Lite算法是以LPA*为基础,是Maxim Likhachev和Sven Koenig于2002年基于LPA*,结合A*算法思想,提出一种增量启发式算法,适用于在未知环境中的导航以及路径规划,广泛用于目前各种移动机器人和自主车辆载具,例如“机遇号”和“勇气号”火星车测试的原型导航系统。

    2.7 小结

    本章研究了几种常用的运动规划算法。其中,人工势场法应用灵活,可以在保证安全的情况下获得一条平滑路径,并且对于动态环境可以实现实时运动控制。适合用于长距离机动且障碍物较少的情况。而基于随机采样的搜索树方法可以在复杂约束环境中获得可行解,适合用于机械臂近距离操作。

    参考文献:

    【1】Choset H , Kantor G A , Thrun S . Principles of Robot Motion: Theory, Algorithms, and Implementations[M]. MIT Press, 2005.

    end

    这是我的私人微信,还有少量坑位,可与相关学者研究人员交流学习 

    目前开设有人工智能、机器学习、计算机视觉、自动驾驶(含SLAM)、Python、求职面经、综合交流群扫描添加CV联盟微信拉你进群,备注:CV联盟

    王博的公众号,欢迎关注,干货多多

    王博的系列手推笔记(附高清PDF下载):

    博士笔记 | 周志华《机器学习》手推笔记第一章思维导图

    博士笔记 | 周志华《机器学习》手推笔记第二章“模型评估与选择”

    博士笔记 | 周志华《机器学习》手推笔记第三章“线性模型”

    博士笔记 | 周志华《机器学习》手推笔记第四章“决策树”

    博士笔记 | 周志华《机器学习》手推笔记第五章“神经网络”

    博士笔记 | 周志华《机器学习》手推笔记第六章支持向量机(上)

    博士笔记 | 周志华《机器学习》手推笔记第六章支持向量机(下)

    博士笔记 | 周志华《机器学习》手推笔记第七章贝叶斯分类(上)

    博士笔记 | 周志华《机器学习》手推笔记第七章贝叶斯分类(下)

    博士笔记 | 周志华《机器学习》手推笔记第八章(上)

    博士笔记 | 周志华《机器学习》手推笔记第八章(下)

    博士笔记 | 周志华《机器学习》手推笔记第九章

    点个在看支持一下吧

    展开全文
    Sophia_11 2020-05-31 18:01:00
  • VFH+避障/局部路径规划算法 这篇文章是我看论文《VFH+: Reliable Obstacle Avoidance for Fast Mobile Robots》(Iwan Ulrich and Johann Borenstein)的笔记,所以说是文章的翻译也可以。想看原paper的可以自己去找...


    做个正直的人

    这篇文章是我看论文《VFH+: Reliable Obstacle Avoidance for Fast Mobile Robots》(Iwan Ulrich and Johann Borenstein)的笔记,所以说是文章的翻译也可以。想看原paper的可以自己去找,看过VFH算法的话,这个VFH+算法还是非常好懂的。

    我前面写过一篇讲VFH算法的文章,感兴趣的可以去看看。
    VFH局部路径规划算法

    我在讲VFH算法的最后,写了一点我对VFH算法的评价,主要有两点,一是VFH算法涉及的参数比较多,很麻烦;二是机器人总是会像个傻子一样在原地摆头,我设置了一个不灵敏区域稍微缓解了一下摆头的问题,但是这导致机器人对障碍物的灵敏性降低。

    今天看了VFH+算法,才发现原来这些问题前人们早就研究过了。VFH+对VFH提出了几点修正,VFH还带了个正号,这是增强版的意思吗?hhhhhhhh…接下来进入正题。
    VFH+算法同样分成四个部分,不过和VFH的四个部分可是不一样。

    1、第一部分:映射到极坐标系

    VFH算法的第一部分就是把局部地图从笛卡尔空间映射到极坐标系,VFH+算法的做法跟VFH一样,只不过VFH+不再像VFH那样采用正方形的工作空间(可以当成是局部地图),而是采用了圆形的工作空间,本来就该是圆形的好吧,VFH那是为了简便而已。这工作空间一旦变成圆形,就有了一个性质——机器人原地旋转感知到的局部地图是不变的,这称为旋转不变性。

    VFH+对VFH的改进——考虑机器人的宽度&对障碍物进行膨胀

    VFH并没有考虑机器人的宽度,而是采用一个低通滤波器进行补偿,这个滤波器的参数的设置还比较麻烦。
    VFH+考虑到了机器人的宽度 r r r_r rr ,这里假定机器人是正圆盘形状。同时为了安全起见又设置了一个距离障碍物的安全距离 d x d_x dx ,基于此,我们就可以得到需要对障碍区进行膨胀的幅度 r r + x = r r + d x r_{r+x}=r_r+d_x rr+x=rr+dx 。这样我们就可以把机器人完全看做一个点来进行规划,是不是感觉事情变得简单了。
    在这里插入图片描述
    从上图可以看出,如果障起舞膨胀幅度为 r r + x = r r + d x r_{r+x}=r_r+d_x rr+x=rr+dx ,机器人距离障碍物的距离为
    d i j d_{ij} dij ,此时这一个障碍物对应的张角为 2 γ i , j 2\gamma_{i,j} 2γi,j 。其中 γ i , j = a r c s i n ( r r + x / d i , j ) \gamma_{i,j}=arcsin(r_{r+x}/d_{i,j}) γi,j=arcsin(rr+x/di,j)

    此时,这个障碍物很可能就不再仅仅位于一个sector内了,而是同时处于多个sector上,这时候更新工作空间就必须同时对多个sector进行更新。

    2、第二部分:二值化极坐标直方图

    VFH算法的一个大问题就是机器人会出现在原地频繁的左右摆头的现象,我在调试VFH算法的时候分析是VFH算法对新计算出的前进方向过于敏感,导致机器人频繁的更新自己的前进方向。所以我给VFH算法设置了一个不灵敏区间,只有当新的前进方向和自己当前的前进方向角度相差大于一定阈值的时候才会更新前进的方向,否则不会更新。但是这导致机器人对障碍物的灵敏性降低,限制了机器人的运行速度。

    VFH+算法对此提出了一种解决办法——二值化极坐标直方图

    VFH算法之所以会出现摆头现象(VFH+算法称之为犹豫不决的行为),是因为在存在一些比较窄(不是真的特别特别窄,只是比较窄而已)的通道的环境中,一些山谷可能会在opening和block的状态之间频繁变换。比如对于一扇开着的门,当离得比较远的时候,这一扇门占据的sector比较窄,而VFH更加倾向于选择那些宽的山谷(占据sector比较多的山谷方向),这时候这一扇门被当作是block,不会成为机器人的候选前进方向;可是随着机器人的运动,机器人离门越来越近,这一扇门占据的sector越来越多,当大于一个阈值的时候,这一扇门突然成了宽的山谷,而成为了候选的前进方向。这就导致一些方向频繁的在opening和block之间跳换,就像一条路,一会跟我说这条路可以走,一会又跟我说不可以走,这不是在拿机器人涮着玩嘛。

    VFH+算法的做法是设置两个阈值, τ H i g h \tau_{High} τHigh τ L o w \tau_{Low} τLow ,这样做的目的是降低一些方向在opening和block状态之间跳转的速度,你不是变大了吗(变小也是一样),虽然满足了我的第一个条件,但是我并不马上认可你,而是继续考核你一下,满足我的第二个要求后才是真的合格了,我才会认可你。就是这个思路,在机器人运动过程中,极坐标直方图也在慢慢的发生着变化,有些区域离开了工作空间,有些区域新加入工作空间,并且工作空间内部的物体相对于机器人的位置也在变化,所以会在每一个时刻生成一个极坐标直方图。将n时刻的极坐标直方图中的每一项 H k , n p H_{k,n}^p Hk,np 都和前一时刻对应的项 H k , n − 1 p H_{k,n-1}^p Hk,n1p 做比较,同时跟设定的两个阈值 τ H i g h \tau_{High} τHigh τ L o w \tau_{Low} τLow 进行比较,依次来决定这一项应该二值化为1还是0,亦或是继续保持前一时刻的值。下面的公式一看就明白了

    • H k , n p H_{k,n}^p Hk,np =1 if H k , n p > τ H i g h H_{k,n}^p >\tau_{High} Hk,np>τHigh :这一项都大于更大的那个阈值了,肯定得是1,也就是block
    • H k , n p = 0 H_{k,n}^p =0 Hk,np=0 if H k , n p < τ L o w H_{k,n}^p <\tau_{Low} Hk,np<τLow :这一项比更小的那个阈值都要小,得是0,也就是openning
    • H k , n p = H k , n − 1 b H_{k,n}^p = H_{k,n-1}^b Hk,np=Hk,n1b others :这一项不算大,也不算小,咋办?就维持前一时刻的状态吧,先观望一下。其中, H b H^b Hb 是二值化之后的极坐标直方图。

    3、第三部分:限制机器人可选的前进方向

    VFH算法中,机器人可选的前进方向是一圈360度都可能称为机器人的前进方向,看下图a。想一想这合适吗?我可不希望我的机器人一会向前,一会向后,一会向左,一会又向右,机器人的运动轨迹非常的不平滑。而且还有一个最大的问题,机器人很难进行高速的运动。想一想机器人的车轮正在飞速的旋转,机器人在欢快的向前奔跑,突然控制器发来命令,说,“嘿,老哥,你下一时刻的前进方向在你后面,跟现在的正相反”。机器人听了是什么感受,我正先前飞奔,你现在让我倒退,玩我呢?就算是电机再好,老这么搞,电机也是遭不住啊!
    在这里插入图片描述
    所以,VFH+算法对此作出了改进,限制了机器人可选的前进方向,看上图b,大大平滑了运动轨迹。骑过小电瓶车的小伙伴应该都有感受,车速越快,越是忌讳急转弯,太危险了,搞不赢就躺地上了啊。也就是说,车速越快,转弯半径越大,相应的曲率就越小;车速越慢,可以承受的转弯半径就越小,相应的曲率就越大。如此,机器人的可选择的前进方向就只有上图b中被两个(转弯)圆夹出来的部分。(姑且叫它转弯圆吧,这名字应该好理解吧。)
    假如说机器人当前的速度是v ,对应的最小转弯半径是 R 。前面提过,障碍物的膨胀幅度是 r r + x r_{r+x} rr+x ,看下图
    在这里插入图片描述
    在机器人的前面存在两个障碍物A和B,这两个障碍物被膨胀之后分别为上图中两个阴影圆所示。机器人的左右侧两个转弯圆的半径为R,那么机器人会和障碍物碰撞的条件是什么呢?或者说满足什么条件的话,机器人很大概率上会撞上障碍物呢?上图中的障碍物A ,A 和左侧转弯圆的圆心的距离为 d C , A d_{C,A} dC,A 由于存在 d C , A < r r + x + R d_{C,A}<r_{r+x}+R dC,A<rr+x+R ,所以此时机器人再不采取措施避开A,大概率就撞上了。所以此时机器人应当向右偏转避开A,从A和B的中间穿过。

    那么机器人是怎么实现避开A 的呢?下图中,b是第二部分二值化之后的极坐标直方图。由于障碍物A和机器人的左侧转弯圆有重合部分,再加上机器人可选择前进方向是两个转弯圆之间夹出来的部分,那么对于机器人来说,从A 障碍物开始逆时针旋转大半圈(一直转到右侧转弯圆为止)都不可以通过,都是block。这一来二值化的极坐标直方图就从b变成了c,这时候机器人该选择哪一个方向前进就一目了然了。
    在这里插入图片描述

    4、第四部分:方向选择

    这一部分,对应VFH算法的第二部分——方向控制。只不过VFH是直接从第一步生成的极坐标直方图来生成方向,而VFH+是经过了第二部分和第三部分的处理之后才开始生成前进方向。

    VFH +和VFH的方向产生一样,只不过,VFH在这一步——考虑目标方向、当前方向、前一时刻方向。什么意思呢?在VFH算法中,方向控制就是单纯的从极坐标直方图生成前进方向,没有考虑目标在哪里、当前的方向为何、前一时刻方向为何;而且由于工作空间的更新,方向控制生成的前进方向变化比较剧烈,这也是导致机器人频繁的原地摆头的原因。VFH+算法同时将目标方向、当前方向、前一时刻方向考虑进来。采用如下的代价函数来得到最佳的前进方向

    g ( c ) = μ 1 ∗ Δ ( c , k t ) + μ 2 ∗ Δ ( c , θ n / α ) + μ 3 ∗ Δ ( c , k d , n − 1 ) g(c)=\mu_1*\Delta(c,k_t)+\mu_2*\Delta(c,\theta_n/\alpha)+\mu_3*\Delta(c,k_{d,n-1}) g(c)=μ1Δ(c,kt)+μ2Δ(c,θn/α)+μ3Δ(c,kd,n1)

    其中,第一项的 Δ \Delta Δ 是生成的前进方向和目标方向的差值,系数是 μ 1 \mu_1 μ1 ;第二项的 Δ \Delta Δ 是生成的前进方向和当前前进方向的差值,系数是 μ 2 \mu_2 μ2 ;第三项的 Δ \Delta Δ 是生成的前进方向和前一时刻前进方向的差值,系数是 μ 3 \mu_3 μ3 。通过配置三个系数的大小可以得到不同特性的VFH+算法。

    如果是第一项的系数最大,那么无疑我们更加看中目标方向的作用,这时候得到目标导向的VFH+算法。相应的路径很可能不是那么光滑。此时最好满足关系 μ 1 > μ 2 + μ 3 \mu_1>\mu_2+\mu_3 μ1>μ2+μ3 。这是论文作者提出来的,我不是太理解,我的三个系数分别是2、2、1,感觉效果也挺好的。可能是我还没有发现一些东西吧。

    如果第二项系数最大,那么说明我们更加看中路径的光滑程度。

    如果觉得路径还是不够光滑,当然可以增大 μ 2 \mu_2 μ2 ,然而如果 μ 2 \mu_2 μ2 太大,比如 μ 1 \mu_1 μ1 是1, μ 2 \mu_2 μ2 是100,那说真的,还搞个什么劲啊,直接取消第一项算了。所以第二项的系数也不能大的离谱,这时候就可以加入第三项,来使得路径更加的光滑。
    除此以外,还可以增加其他的项,比如我们还希望关注一下各个opening的宽度,这时候就可以考虑加入第四项将opening的宽度也纳入。

    展开全文
    qq_41845878 2021-08-31 17:27:39
  • 转发自知乎 搬砖的旺财 1 自主机器人近距离操作运动规划体系 在研究自主运动规划问题之前,首先...并按照机器人的数量与规模,将自主运动规划分为单个机器人的运动规划与多机器人协同运动规划两类规划体系。 1.1...

    转发自知乎 搬砖的旺财

    1 自主机器人近距离操作运动规划体系

    在研究自主运动规划问题之前,首先需建立相对较为完整的自主运动规划体系,再由该体系作为指导,对自主运动规划的各项具体问题进行深入研究。本节将根据自主机器人的思维方式、运动形式、任务行为等特点,建立与之相适应的自主运动规划体系。并按照机器人的数量与规模,将自主运动规划分为单个机器人的运动规划与多机器人协同运动规划两类规划体系。

    1.1 单个自主机器人的规划体系

    运动规划系统是自主控制系统中主控单元的核心部分,因此有必要先研究自主控制系统和其主控单元的体系结构问题。

    自主控制技术研究至今,先后出现了多种体系结构形式,目前被广泛应用于实践的是分布式体系结构,其各个功能模块作为相对独立的单元参与整个体系。随着人工智能技术的不断发展,基于多Agent的分布式体系结构逐渐成为了主流,各功能模块作为独立的智能体参与整个自主控制过程,该体系结构应用的基本形式如图1所示。一方面,主控单元与测控介入处理、姿态控制系统、轨道控制系统、热控系统、能源系统、数传、有效载荷控制等功能子系统相互独立为智能体,由总线相连;另一方面,主控单元为整个系统提供整体规划,以及协调、管理各子系统Agent的行为。测控介入处理Agent保证地面系统对整个系统任意层面的控制介入能力,可接受上行的使命级任务、具体的飞行规划和底层的控制指令;各子系统Agent存储本分系统的各种知识和控制算法,自主完成主控单元发送的任务规划,并将执行和本身的健康等信息传回主控单元,作为主控单元Agent运行管理和调整计划的依据。

    图1 基于多Agent的分布式自主控制系统体系结构基本形式示意图Überschrift

    主控单元Agent采用主流的分层递阶式结构,这种结构层次鲜明,并且十分利于实现,其基本结构如图2所示。主控单元由任务生成与调度、运动行为规划和控制指令生成三层基本结构组成,由任务生成与调度层获得基本的飞行任务,经过运动行为规划层获得具体的行为规划,再由控制指令生成层得到最终的模块控制指令,发送给其它功能Agent。各功能Agent发送状态信息给主控单元的状态检测系统,状态检测系统将任务执行情况和子系统状态反馈回任务生成与调度层,以便根据具体情况对任务进行规划调整。当遇到突发情况时,还可启用重规划模块,它可根据当时情况迅速做出反应快速生成行为规划,用以指导控制指令生成层得到紧急情况的控制指令。此外,地面控制系统在三个层次上都分别具有介入能力。图2中,点划线内是主控单元全部模块,虚线内为运动规划系统,包括运动行为规划模块和重规划模块,这也是运动规划系统的主要功能。

    图2 主控单元基本结构示意图

    明确了自主控制系统与其主控单元的基本结构,以及运动规划系统在主控单元中的基本功能,便可建立运动规划系统的体系结构。运动规划系统的体系结构如图3所示,该系统由规划器和重规划器两大执行单元组成,分别承担对飞行任务的一般规划和对突发事件紧急处理的运动规划。当然,这两部分也可理解为离线规划与在线规划两种,离线规划一般解决平时按部就班的飞行任务,在线规划一般解决突然下达的飞行任务。除规划器以外,系统还配有知识域模块,用以利用特定语言描述相关知识。知识域包括行为域和模型域两个部分,行为域用来存储服务系统一般的运动行为描述和紧急情况下的一些运动行为方面的处理方法(如急停、转向等),模型域用来存储规划所需模型知识,包括环境模型、组装体模型、组装任务对象模型和任务模型等等。

    图3 运动规划系统体系结构示意图

    1.2 多自主机器人协同规划体系

    多智能体系统的群体体系结构一般分为集中式、分散式两种基本结构,分散式结构又可以进一步分为分层式和分布式结构。集中式结构通常由一个主控单元掌握全部环境和受控机器人信息,运用规划算法对任务进行分解,并分配给各受控机器人,组织它们完成任务。其优点是理论条理清晰,实现较为直观;缺点是容错性、灵活性和对环境的适应性较差,与各受控机器人存在通讯瓶颈问题。相对于集中式结构,分散式结构无法得到全局最优解,但它凭借着可靠性、灵活性和较强的环境适应性越来越受到广泛的青睐。分散式结构中的分布式结构没有主控单元,各智能体地位平等,通过各智能体间的通讯和信息交流达到协商的目的,实现最终的决策,但该结构容易片面强调个体,导致占用资源过多,且难于得到磋商结果。分层式结构介乎于集中式和分布式之间,存在主控单元,但并不是由主控单元掌控一切,各智能体也具备一定的自主性,上下级之间按照一定的规则,通过信息流形成完整的整体,共同完成协同任务。

    多自主机器人系统应采用分层式结构,以保证整个系统既适于统一领导,又满足系统灵活、快速的需求。多自主机器人协同规划体系结构如图4所示,按照分层式结构建立两种工作模式:事先的离线规划由主控单元负责,首先获得协同任务,经过规划器得到具体的行为运动规划,并分发给各分系统执行单元,相关的知识域中主要是用于描述各分系统协商规则的协商域,主控单元从外界获取环境信息,从各分系统获取状态信息;当遇到突发事件或紧急任务变更以及主控单元停止工作时,各分系统采用分布式结构,单独规划各自运动行为,并从各自的知识域中获取协商方式,外界环境信息由主控单元发送和自我感知相结合获得(主控单元停止工作时,仅靠自我感知获取信息),其它机器人信息的传输由机器人间的数据链实现。

    2 路径规划研究

    当给定了某一特定的任务之后,如何规划机器人的运动方式将至关重要。机器人的规划包括两部分内容:基座移动到适合操作的位置和转动手臂关节完成操作。包括三个问题:基座点到点运动规划;关节空间规划;综合规划。

    本章研究几种常用的运动规划算法:图搜索法、RRT算法、人工势场法、BUG算法。并对部分算法的自身缺陷进行了一些改进。

    2.1 图搜索法

    图搜索法依靠已知的环境地图以及地图中的障碍物信息构造从起点到终点的可行路径。主要分成深度优先和广度优先两个方向。深度优先算法优先扩展搜索深度大的节点,可以快速的得到一条可行路径,但是深度优先算法得到的第一条路径往往是较长的路径。广度优先算法优先扩展深度小的节点,呈波状的搜索方式。广度优先算法搜索到的第一条路径就是最短路径。

    2.1.1 可视图法

    可视图法由Lozano-Perez和Wesley于1979年提出,是机器人全局运动规划的经典算法。可视图法中,机器人用点来描述,障碍物用多边形描述。将起始点 S 、目标点 G 和多边形障碍物的各顶点(设 V_o 是所有障碍物的顶点构成的集合)进行组合连接,要求起始点和障碍物各顶点之间、目标点和障碍物各顶点之间以及各障碍物顶点与顶点之间的连线均不能穿越障碍物,即直线是“可视的”。给图中的边赋权值,构造可见图 G\left( V,E \right) 。其中点集 V=V_o\cup \left\{ S,G \right\}E 为所有弧段即可见边的集合。然后釆用某种优化算法搜索从起始点 S 到目标点 G 的最优路径,那么根据累加和比较这些直线的距离就可以获得从起始点到目标点的最短路径。

    图5 可视图

    由此可见,利用可视图法规划避障路径主要在于构建可视图,而构建可视图的关键在于障碍物各顶点之间可见性的判断。判断时主要分为两种情况,同一障碍物各顶点之间可见性的判断以及不同障碍物之间顶点可见性的判断。

    1. 同一障碍物中,相邻顶点可见(通常不考虑凹多边形障碍物中不相邻顶点也有可能可见的情况),不相邻顶点不可见,权值赋为 \infty
    2. 不同障碍物之间顶点可见性的判断则转化为判断顶点连线是否会与其它顶点连线相交的几何问题。如下图虚线所示,V_1V_2 分别是障碍物 O_1O_2 的顶点,但 V_1V_2 连线与障碍物其它顶点连线相交,故 V_1V_2 之间不可见;而实线所示的 V_3V_4 连线不与障碍物其它顶点连线相交,故 V_3V_4 之间可见。
    图6 顶点可见性判断

    可视图法能求得最短路径,但搜索时间长,并且缺乏灵活性,即一旦机器人的起始点和目标点发生改变,就要重新构造可视图,比较麻烦。可视图法适用于多边形障碍物,对于圆形障碍物失效。切线图法和Voronoi图法对可视图法进行了改进。切线图法用障碍物的切线表示弧,因此是从起始点到目标点的最短路径的图,移动机器人必须几乎接近障碍物行走。其缺点是如果控制过程中产生位置误差,机器人碰撞障碍物的可能性会很高。Voronoi图法用尽可能远离障碍物和墙壁的路径表示弧。因此,从起始点到目标点的路径将会增长,但采用这种控制方式时,即使产生位置误差,移动机器人也不会碰到障碍物。

    2.1.2 Dijkstra算法

    Dijkstra算法由荷兰计算机科学家艾兹赫尔·戴克斯特拉(Edsger Wybe Dijkstra)发明,通过计算初始点到自由空间内任何一点的最短距离可以得到全局最优路径。算法从初始点开始计算周围4个或者8个点与初始点的距离,再将新计算距离的点作为计算点计算其周围点与初始点的距离,这样计算像波阵面一样在自由空间内传播,直到到达目标点。这样就可以计算得到机器人的最短路径。

    Dijkstra算法是一种经典的广度优先的状态空间搜索算法,即算法会从初始点开始一层一层地搜索整个自由空间直到到达目标点。这样会大大增加计算时间和数据量。而且搜索得到的大量对于机器人运动是无用的。

    2.1.3 A*算法

    为了解决Dijkstra算法效率低的问题,A*算法作为一种启发式算法被提出。该算法在广度优先的基础上加入了一个估价函数。

    2.2 RRT算法

    快速搜索随机树(RRT)算法是一种增量式采样的搜索方法,该方法在应用中不需要任何参数整定,具备良好的使用性能。它利用增量式方法构建搜索树,以逐渐提高分辨能力,而无须设置任何分辨率参数。在极限情况,该搜索树将稠密的布满整个空间,此时搜索树由很多较短曲线或路经构成,以实现充满整个空间的目的。增量式方法构建的搜索树其导向取决于稠密采样序列,当该序列为随机序列时,该搜索树称为快速搜索随机树(Rapidly Exploring Random Tree,RRT),而不论该序列为随机还是确定性序列,都被称为快速搜索稠密树(Rapidly Exploring Dense Trees,RDTs),这种规划方法可处理微分等多种约束。

    2.2.1 算法步骤

    考虑二维和三维工作空间,环境中包含静态障碍物。初始化快速随机搜索树T,只包括根节点,即初始状态S。在自由空间中随机选取一个状态点 x_{rand} ,遍历当前的快速随机搜索树T,找到T上距离 x_{rand} 最近的节点 x_{near} ,考虑机器人的动力学约束从控制输入集 U 中选择输入 u\in U ,从状态 x_{near} 开始作用,经过一个控制周期 \Delta t 到达新的状态 x_{new} 。满足 x_{new}x_{rand} 的控制输入 u 为最佳控制量。将新状态 x_{new} 添加到快速随机搜索树T中。按照这样得到方法不断产生新状态,直到到达目标状态G。完成搜索树构建后,从目标点开始,逐次找到父节点直到到达初始状态,即搜索树的根节点。

    图7 随机树构建过程

    由于在搜索过程中考虑了机器人的动力学约束,因此生成的路径的可行性很好。但是算法的随机性导致其只具备概率完备性。

    2.2.2 改进算法

    LaValle等人的工作奠定了RRT方法的基础。在采样策略方面,RRTGoalBiaS方法在控制机器人随机运动的同时,以一定概率向最终目标运动;RRTGoalZoom方法分别在整个空间和目标点周围的空间进行采样;RRTCon方法则通过加大随机步长改进规划速度。双向规划思想也被采用,衍生出RRTExtExt,RRTExtCon,RRTConCon等多种算法。

    基本RRT算法收敛到终点位姿的速度可能比较慢。为了提高算法的效率和性能,需不断对该算法进行改进。如为了提高搜索效率采用双向随机搜索树(Bi~RRT),从起始点和目标点并行生成两棵RRT,直至两棵树相遇,算法收敛。由于这个算法相比于原始RRT有更好的收敛性,因此在目前路径规划中是很常见的。NikAMelchior提出的粒子RRT算法,考虑了地形的不确定性,保证了在不确定性环境下搜索树的扩展。Kuffner和Lavane又提出RRT-connectlv,使得节点的扩展效率大大提高。运动规划中,距离的定义非常复杂,Pengcheng研究了在RRT生长过程中距离函数不断学习的算法以降低距离函数对环境的敏感性。考虑到基本RRT规划器得到的路径长度一般是最优路径的1.3~1.5倍,英国的J.desmithl研究了变分法技术使其达到最优。Amna A引入KD树作为二级数据结构加速查找距离从环境中取出的随机点最近的叶节点,降低了搜索成本。该算法在动态障碍物、高维状态空间和存在运动学、动力学等微分约束的环境中的运动规划已经得到广泛的应用。

    关于改进RRT算法详情可参考:

    搬砖的旺财:路径规划——改进RRT算法​zhuanlan.zhihu.com

    2.3 滚动在线RRT算法

    基本RRT算法倾向于遍历整个自由空间直到获得可行路径,这使其不可能用于未知或动态环境中的机器人在线运动规划。利用滚动规划的思想可以将RRT算法进行改进,使其具备在线规划能力。

    2.3.1 滚动规划

    机器人在未知或动态环境中运动时,只能探知其传感器范围内有限区域内的环境信息。机器人利用局部信息进行局部运动规划,并根据一定的评价准则得到局部目标。机器人到达局部目标后再次进行新的局部规划。如此反复进行直到到达全局目标。

    滚动规划算法的基本原理:

    1. 环境信息预测:在滚动的每一步,机器人根据探测到的视野内的信息、或所有已知的环境信息,建立环境模型,包括设置已知区域内的节点类型信息等;
    2. 局部滚动优化:将上述环境信息模型看成一个优化的窗口,在此基础上,根据目标点的位置和特定的优化策略计算出下一步的最优子目标,然后根据子目标和环境信息模型,选择局部规划算法,确定向子目标行进的局部路径,并实施当前策略,即依所规划的局部路径行进若干步,窗口相应向前滚动;
    3. 反馈信息校正:根据局部最优路径,驱动机器人行走一段路径后,机器人会探测到新的未知信息,此时可以根据机器人在行走过程探测到的新信息补充或校正原来的环境模型,用于滚动后下一步的局部规划。

    其中,局部子目标是在滚动窗口中寻找一个全局目标的映射,它必须避开障碍物,且满足某种优化指标。子目标的选择方法反映了全局优化的要求与局部有限信息约束的折衷,是在给定信息环境下企图实现全局优化的自然选择。

    基于滚动窗口的路径规划算法依靠实时探测到的局部环境信息,以滚动方式进行在线规划。在滚动的每一步,根据探测到的局部信息,用启发式方法生成优化子目标,在当前滚动窗口内进行局部路径规划,然后实施当前策略(依局部规划路径移动一步),随滚动窗口推进,不断取得新的环境信息,从而在滚动中实现优化与反馈的结合。由于规划问题压缩到滚动窗口内,与全局规划相比其计算量大大下降。

    基于滚动窗口的路径规划算法的具体步骤如下:

    • 步骤0:对起点、终点、工作环境、机器人的视野半径、步长进行初始化;
    • 步骤1:如果终点到达,规划中止;
    • 步骤2:对当前滚动窗口内的环境信息进行刷新;
    • 步骤3:产生局部子目标;
    • 步骤4:根据子目标及已知环境信息,在当前滚动窗口内规划一条优化的局部可行路径;
    • 步骤5:依规划的局部路径行进一步,步长小于视野半径;
    • 步骤6:返回步骤1。

    2.3.2 滚动在线RRT算法流程

    在一个滚动窗口内,随机树以当前位置为起始点,构建传感器范围内的随机树。构建方法与基本RRT算法一致。为了使全局环境中随机树具有向目标方向生长的趋势,在运动规划时引入启发信息,减少随机树的随机性,提高搜索效率。

    Road\left( x_1,x_2 \right) 代表随机树中两个位姿节点间的路径代价, Dis\left( x_1,x_2 \right) 代表随机树中两个位姿节点间的欧几里德距离。类似于A*算法,本算法为随机树中每个节点定义一个估价函数: f\left( x \right)=g\left( x \right)+h\left( x \right) 。其中 g\left( x \right)=Road\left( x,x_{rand} \right) 是随机节点 x_{rand} 到树中节点 x 所需的路径代价。 h\left( x \right) 为启发估价函数,这里取随机节点 x_{rand} 到目标点 x_{goal} 的距离为估价值, h\left( x \right)=Dis\left( x_{rand},x_{goal} \right) 。因此 f\left( x \right) 表示从节点 x 经随机节点 x_{rand} 到目标节点 x_{goal} 的路径估计值。遍历滚动窗口内随机树T,取估价函数最小值的节点 x_{near} ,有 f\left( x_{near} \right)=min\left( f\left( x \right) \right) 。这使得随机树沿着到目标节点估价值 f\left( x \right) 最小的方向进行扩展。

    由于在随机树生长中引入了导向目标的启发估价因子,叶节点 x_{near} 总是选择离目标最近的节点,这可能会使随机树遇到局部极小值问题。因此随机树生长的新节点 x_{new} 必须要克服这个问题,引导随机树更好的探索未知空间。

    这里利用统计学中回归分析生成新节点,将RRT算法探索未知空间的能力进一步增强以避免因启发估价因子导致的局部极小。其思想是探索以前到过的空间是无用的,而且容易陷入局部极小。引进回归分析(regression analysis)是考察新节点与其他节点之间关系,利用回归函数约束,使得随机树不探索以前到过的空间,因此避免了局部极小。

    新节点生成方法是遍历随机树,如果 x_{new} 与其父节点 x_{near} 的距离小于 x_{new} 与扩展树上其他任意节点的距离,即 \exists \ n\in T|Dis\left( x_{near},x_{new} \right)<Dis\left( n,x_{new} \right) ,则选择该节点为随机树新生节点。下图解释了新节点的判断过程。

    图8 新节点的判断

     

    上图中各个空心点是中间的父节点的可能扩展。椭圆圈起的空心点表示这个新节点不符合回归函数约束,剩下的两个未被圈起的空心节点到其父节点的距离小于该节点到随机树上任意节点的距离,这两个点可以成为随机树的新节点。

    综上,滚动窗口内随机树构建的具体步骤如下:

    1. 对滚动窗口随机树T初始化,T开始只包含初始位置S;
    2. 滚动窗口自由空间中随机选择一个状态 x_{rand}
    3. 根据最短路径思想寻找树T中和 x_{rand} 距离最近的节点 x_{near}
    4. 选择输入 u ,使机器人状态由 x_{near}x_{new}
    5. 确定 x_{new} 是否符合回归分析,不符合则回到第4步;
    6. x_{new} 作为随机树T的一个新节点, u 则被记录在连接节点 x_{near}x_{new} 的边上。

    滚动窗口状态空间进行K次采样后,遍历随机树,根据启发估价思想寻找滚动窗口子目标 x_{sub}x_{sub} 是当前滚动窗口中的子树中估价函数最小的点。确定子目标后,机器人前进到子目标点,进行下一轮的滚动RRT规划。如此反复,直到到达目标点G。

    2.4 人工势场法

    人工势场法是由Khatib提出的一种用于机器人运动规划的虚拟力方法。其基本思想是将目标和障碍物对机器人运动的影响具体化成人造势场。目标处势能低,障碍物处势能高。这种势差产生了目标对机器人的引力和障碍物对机器人的斥力,其合力控制机器人沿势场的负梯度方向向目标点运动。人工势场法计算方便,得到的路径安全平滑,但是复杂的势场环境可能在目标点之外产生局部极小点导致机器人无法到达目标。为了解决人工势场法的局部极小点问题,学者们提出了各种改进方法。主要分成两个方向:一个是构造合适的势函数以减小或避免局部极小点的出现;另一种是在机器人遇到局部极小点后结合其他的方法使机器人离开局部极小点。前者一般需要全局地图信息,并且依赖于障碍物的形状。当环境复杂时难以应用。后者多利用搜索法、多势场法和沿墙行走法等方法使机器人离开局部极小点。搜索法利用最佳优先、模拟退火、随即搜索等策略寻找比局部极小点势场值更低的点使机器人继续移动。由于未知环境中大多缺乏启发信息,搜索方法的效率很低。多势场法构造多个全局极小点相同,而局部极小点不同的势函数,在机器人陷入某个局部极小点时,规划器就切换势函数使机器人离开该点。但是在未知的环境中这样的多个势场很难构造,而且该方法可能导致机器人在回到曾逃离的局部极小点。由于局部极小点是某个或多个障碍物的斥力势场与引力势场共同作用产生,其位置与障碍物距离必然不远,沿墙行走法正是利用这样的远离,使机器人在遇到局部极小点后参照类似BUG算法的环绕行为绕过产生局部极小点的障碍物继续前进。这种方法可靠性高,不依赖环境的先验信息和障碍物形状。

    本节构造人工势场进行机器人平动的在线运动规划,利用一种沿墙行走法对基本的人工势场法进行改进。

    2.4.1 基本人工势场法

    作用在机器人上的假想引力和斥力为势函数的负梯度,因而人工势函数应该具有以下特征:

    1. 非负且连续可微;
    2. 斥力势强度距离障碍物越近其强度越大;
    3. 引力势强度离目标位置越近其强度越小。

    空间中的合势场是引力势场与斥力势场之和: U\left( x \right)=U_{att}\left( x \right)+U_{rep}\left( x \right)

    其中, U_{att}\left( x \right) 是目标产生的引力势场; U_{rep}\left( x \right) 是各个障碍物产生的斥力势场之和,即: U_{rep}\left( x \right)=\sum_{j=1}^{n}{U_{rep,j}\left( x \right)}

    这里构造如下的引力势函数和斥力势函数:

    U_{att}\left( x \right)=\left\{              \begin{array}{lr}              \frac{1}{2}\zeta d^2\left( x,G \right)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ d\left( x,G \right)\leq d{^*_G}  &  \\              d{^*_G}\zeta d\left( x,G \right)-\frac{1}{2}d{^*_G}\zeta \ \ \ \ \ d\left( x,G \right)> d{^*_G}&                \end{array} \right.

    U_{rep,j}=\left\{              \begin{array}{lr}              \frac{1}{2}\eta_j \left( \frac{1}{d_j\left( x \right)}-\frac{1}{Q{^*_j}} \right)^2\ \ \ d_j\left( x \right)\leq Q{^*_j}  &  \\              0\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ d\left( x,G \right)> Q{^*_j}&                \end{array} \right.

    其中, \zeta 表示引力势的相对影响; \eta_j 表示第 j 个障碍物的斥力势的相对影响, x 表示机器人当前位置, G 表示目标点位置, d\left( x,G \right) 表示机器人距目标的距离, d{^*_G} 的作用是在机器人距离目标较远时,削弱目标引力势的作用, d_j\left( x \right) 表示机器人距离第 j 个障碍物的距离, Q{^*_j} 表示第 j 个障碍物的斥力势作用范围。

    \zeta\eta_j 对势场形状的影响很大,适当的增大 \zeta 能够增强引力势场的作用,有助于减少产生局部极小点的可能,并加快机器人向目标运动。 \eta_j 影响机器人在障碍物附近的运动特性, \eta_j 比较大可以使机器人距离障碍物更远,运动路径更安全; \eta_j 比较小,机器人在避开障碍物时运动比较平滑。

    利用上面势函数的梯度可以计算机器人收到的假想引力和斥力:

    f_{att}=-\nabla U_{att}\left( x \right)=\left\{              \begin{array}{lr}              \zeta \left( G-x \right)\ \ \ \ d\left( x,G \right)\leq d{^*_G}  &  \\              \frac{d{^*_G}\zeta d\left( G-x \right)}{d\left( x,G \right)} \ \ \ \ \ d\left( x,G \right)> d{^*_G}&                \end{array} \right.

    f_{rep,j}=-\nabla U_{rep,j}=\left\{              \begin{array}{lr}              \frac{1}{2}\eta_j \left( \frac{1}{Q{^*_j}}-\frac{1}{d_j\left( x \right)} \right)\frac{1}{d{^2_j}\left( x \right)}\nabla d_j\left( x \right)\ \ \ d_j\left( x \right)\leq Q{^*_j}  &  \\              0\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ d\left( x,G \right)> Q{^*_j}&                \end{array} \right.

    2.4.2 人工势场法算法改进

    当机器人的运行环境中包含形状复杂或者距离很近的障碍物时,可能出现势场局部极小点,导致机器人在该处停止或在其周围振动。如下图所示,当环境中出现“陷阱”形障碍物或者与目标成特定位置关系的障碍物时,可能在人工势场中产生局部极小点(图中L点),当机器人运动到局部极小点附近时,势场的负梯度方向指向L点。机器人将在L点处停止或在其附近振动或作圆周运动。

    图9 人工势场法的局部极小点

    为了使机器人从局部极小点中逃离,在人工势场法的基础上引入应激行为,即增加绕行行为。当机器人遇到局部极小点时,忽略目标引力势的作用,沿着斥力势的等势面方向移动,直到机器人离开局部极小区域。改进的算法流程如下:

    1. 根据传感器信息计算当前位置的引力和斥力;
    2. 判断是否处于绕行行为,若是,执行3;若否,执行4;
    3. 判断是否离开局部极小区域,若是,机器人沿着合力方向运动,结束绕行行为;若否,机器人沿着斥力场等势线运动,继续绕行行为;
    4. 判断是否遇到局部极小点,若是,机器人沿着斥力场等势线运动,开始绕行行为;若否,机器人沿着合力方向运动;
    5. 判断是否到达目标,若是,退出算法;若否,继续1;

    使用下面的判别条件判断机器人是否遇到局部极小点。

    条件1: \left| f_{att}+\sum_{j=1}^{n}{f_{rep,j}} \right|<\varepsilon

    条件2: \left| x-x_A \right|<\alpha s_A

    当条件1或者条件2出现时,就认为机器人遇到了局部极小点。条件1中 \varepsilon 是一个很小的正数,其含义是机器人受到的虚拟合力接近0。这是最直接局部极小点判断方法。条件2中 \alpha 为0,1之间某一正数, x_A 为机器人运动过程中某一状态, s_A 表示机器人从 x_A 到达当前位置 x 的总路程,条件2成立意味着机器人在运动很长路程后,位移很小。用来检测机器人在局部极小点附近发生的振动和圆周运动。

    2.5 BUG算法

    BUG算法是一种完全应激的机器人避障算法。其算法原理类似昆虫爬行的运动决策策略。在未遇到障碍物时,沿直线向目标运动;在遇到障碍物后,沿着障碍物边界绕行,并利用一定的判断准则离开障碍物继续直行。这种应激式的算法计算简便,不需要获知全局地图和障碍物形状,具备完备性。但是其生成的路径平滑性不够好,对机器人的各种微分约束适应性比较差。

    2.5.1 BUG1算法

    该算法的基本思想是在没有障碍物时,沿着直线向目标运动可以得到最短的路线。当传感器检测到障碍物时,机器人绕行障碍物直到能够继续沿直线项目标运动。BUG1算法实现了最基本的向目标直行和绕行障碍物的思想。

    假设机器人能够计算两点之间的距离,并且不考虑机器人的定位误差。初始位置和目标位置分别用 \textrm{q}_\textrm{start}\textrm{q}_\textrm{goal} 表示;机器人在 i 时刻的位置表示为 x_il 表示连接机器人位置 x_i 和目标点的直线。初始时, x_i=\textrm{q}_\textrm{start} 。若没有探测到障碍物,那么机器人就沿着 l 向目标直行,直到到达目标点或者遇到障碍物。当遇到障碍物时,记下当前位置 q_i^H 。然后机器人环绕障碍物直到又一次到达 q_i^H ,找到环绕路线上距离目标最近的点 q_i^L ,并沿着障碍物边界移动到该点。随后,直线 l 更新,机器人继续沿直线向目标运动。如果沿这条直线运动时还会遇到该障碍物,那么机器人不能到达目标点。否则算法不断循环直到机器人到达目标点或者规划器认为机器人无法到达目标点。

    图10 BUG1算法运动规划
    图11 BUG1算法中认为机器人无法到达目标点的情况
    图12 BUG1算法伪代码

    2.5.2 BUG2算法

    BUG2算法也有两种运动:朝向目标的直行和沿边界绕行。与BUG1算法不同的是,BUG2算法中的直线 l 是连接初始点和目标点的直线,在计算过程中保持不变。当机器人在点遇到障碍物时,机器人开始绕行障碍物,如果机器人在绕行过程中在距离目标更近的点再次遇到直线 l ,那么就停止绕行,继续沿着直线 l 向目标直行。如此循环,直到机器人到达目标点 \textrm{q}_\textrm{goal} 。如果机器人在绕行过程中未遇到直线 l 上与目标更近的 q_i^L 点而回到了 q_i^H 点,那么得出结论,机器人不能到达目标。

    图13 BUG2算法运动规划
    图14 BUG2算法中认为机器人无法到达目标点的情况
    图15 BUG2算法伪代码

    BUG1和BUG2算法提供了搜索问题的两种基本方法:比较保守的BUG1算法进行详细的搜索来获得最佳的离开点。这需要机器人环绕整个障碍物的边界。而BUG2算法使用一种投机的方法。机器人不环绕完整的障碍物,而选择第一个可用的点作为离开点。对于一般的环境,BUG2算法的效率更高;而对于复杂形状的障碍物,保守的BUG1算法性能更优。

    2.5.3 TangentBUG算法

    TangentBUG算法是对BUG2算法的提高。它利用机器人上距离传感器的读数对障碍物做出提前规避,可以获得更短更平滑的机器人路径。在一个静态环境中,传感器读数 \rho \left( x,\theta \right) 是机器人位置 x 和传感器扫描角度 \theta 的函数,具体点说, \rho \left( x,\theta \right) 是沿着 x 的射线以角度 \theta 到达最近障碍物的距离,

    \rho\left( x,\theta \right)=\mathop{\textrm{min}}_{\lambda \in \left[ 0,\infty \right]}d\left( x,x+\lambda\left[ \textrm{cos}\theta,\textrm{sin}\theta \right]^{\textrm T} \right)

    其中 x+\lambda\left[ \textrm{cos}\theta,\textrm{sin}\theta \right]^{\textrm T}\in\bigcup_{i}{\cal W}{\cal O}_i

    对于某一个固定的位置 x\rho 被传感器视野内的障碍物分割成多个连续区间。如下图所示。 \rho 出现不连续或者到达传感器最大测量范围的角度就是这些连续区间的端点。TangentBUG算法利用者些区间的端点避开工作空间中的障碍物。

    图16 距离传感器扫描障碍物

    \rho 不连续的情况做出说明(如图17所示):点 O_1O_2O_3O_4O_5O_6O_7O_8 与障碍物的不连续性相关;请注意,这里的射线与障碍物相切。 点 O_4 是不连续的,因为障碍物边界落在传感器的范围之外。 O_1O_2O_3O_4O_5O_6O_7O_8 之间的free space边界上的点集是连续性的间隔(加线部分)。

    图17 不连续的示意图

    与其他的BUG算法一样,TangentBUG算法也有两种行为:直行(motion-to-go)和环绕障碍物(boundary-following)。

    首先机器人沿着直线向目标运动,直到它利用传感器观测到在其运动方向的前方有障碍物。不在机器人前方的障碍物对其向目标运动没有影响。比如下图中的障碍物 {\cal W}{\cal O}_2,障碍物 {\cal W}{\cal O}_2 在传感器视野内,但是不阻碍机器人的运动。机器人在刚刚探测到障碍物时,传感器视野圆与障碍物边界相切。随着机器人继续向前移动,这个切点分裂成两个交点 O_1O_2O_3O_4 ), O_1O_2 之间的障碍物边界区间与机器人运动直线相交。

    图18 机器人向目标运动遇到障碍物

    此时,机器人不能继续向目标运动,而从两个交点中选择一个作为局部目标。为比较两个交点对于机器人向目标运动的优劣性,定义探索距离 d_h。在没有关于障碍物的其他信息时,可以将探索距离 d_h 定义为从 x 经过一个交点到目标点的折线长度,即: d_{hi}=d\left( x,O_i \right)+d\left( O_i,\textrm{q}_\textrm{goal} \right)

    例如图19,机器人“看见”了障碍 {\cal W}{\cal O}_1 ,并选择向 O_2 移动,因为 d_{h2}=d\left( x,O_2 \right)+d\left( O_2,\textrm{q}_\textrm{goal} \right) < d_{h1}=d\left( x,O_1 \right)+d\left( O_1,\textrm{q}_\textrm{goal} \right)

    当机器人位于 x 时,它无法知道 {\cal W}{\cal O}_2 阻挡了从 O_2 到目标 \textrm{q}_\textrm{goal} 的路径。

    在图20中,当机器人位于 x 但目标 \textrm{q}_\textrm{goal} 不同时,它具有足够的传感器信息来得出结论: {\cal W}{\cal O}_2 确实阻挡了从 O_2 到目标 \textrm{q}_\textrm{goal} 的路径,从而朝 O_4 行驶。

    因此,选择向 O_2 行驶刚开始的时候可能使得 d\left( x,O_i \right)+d\left( O_i,\textrm{q}_\textrm{goal} \right) 最小化,而不是向 O_4 行驶,但是planner有效地为 d\left( O_2,\textrm{q}_\textrm{goal} \right) 分配无限大的成本代价,因为它有足够的信息来推断任何通过 O_2 的路径都不是最理想的。

    图19
    图20

    机器人将选择探索距离短的交点作为局部目标,向之运动。随着机器人不断运动,交点不断更新,探索距离也不断减小。如下图所示。当 t=1 时,机器人还没有探测到障碍物,因而它向目标作直线运动;当 t=2 时,机器人开始探测到障碍物,并朝向障碍物探索距离近的一侧运动;当 t=3t=4 时,机器人继续移动,同时更新探测区域,在这个过程中探索距离不断减小。

    图21 机器人运动时不断更新局部目标和探测距离

    在机器人运动过程中,探索距离不再减小时,就停止向目标运动行为,切换到环绕边界行为。此时,机器人找到了探测距离的一个极小值,并可计算已探测的障碍物边界与目标 \textrm{q}_\textrm{goal} 的最近距离 d_{followed}。机器人按照原来的方向环绕障碍物运动,同时机器人更新当前探测的障碍物边界与目标的最近距离 d_{reach}。当发现 d_{reach}<d_{followed} 时,机器人停止障碍物环绕行为,继续向目标运动。

    图22 机器人环绕障碍物运动

    如上图所示,当机器人探索到障碍物上的 M 点后,探索距离就不再减小,即 M 点是机器人探索距离在障碍物边界上的局部极小点。机器人开始沿着障碍物边界进行环绕,图中虚线路径就是机器人环绕障碍物时所走的路径。当机器人探测到与目标距离相比 M 点更近的点时,重新开始接近目标的运动。

    2.6 增量式启发算法

    2.6.1 LPA*算法

    LPA*算法,即Lifelong Planning A*算法,该算法于2001年由Sven Koenig和Maxim Likhachev提出,是一种增量启发式搜索版本的A*算法,这种路径规划算法适用于随着时间改变导致有限栅格地图上的边缘代价c(s1,s2)改变的问题,也就是随着时间改变障碍物增多或减少,网格点发生增删等,在许多场合下比再利用A*重新搜索更高效。

    2.6.2 D* Lite算法

    D* Lite算法是以LPA*为基础,是Maxim Likhachev和Sven Koenig于2002年基于LPA*,结合A*算法思想,提出一种增量启发式算法,适用于在未知环境中的导航以及路径规划,广泛用于目前各种移动机器人和自主车辆载具,例如“机遇号”和“勇气号”火星车测试的原型导航系统。

    本章研究了几种常用的运动规划算法。其中,人工势场法应用灵活,可以在保证安全的情况下获得一条平滑路径,并且对于动态环境可以实现实时运动控制。适合用于长距离机动且障碍物较少的情况。而基于随机采样的搜索树方法可以在复杂约束环境中获得可行解,适合用于机械臂近距离操作。

     

    展开全文
    yuxuan20062007 2019-02-10 12:13:46
  • 1.92MB weixin_38743602 2019-09-20 19:46:40
  • RoboChengzi 2020-01-28 00:03:00
  • qq_16775293 2017-12-20 15:34:23
  • weixin_46643879 2021-03-11 21:12:45
  • ZXQHBD 2017-06-14 14:23:15
  • u011978022 2015-11-18 22:41:41
  • xiekaikaibing 2018-10-30 11:30:43
  • 1.32MB weixin_38744153 2019-09-11 03:16:54
  • weixin_43145941 2021-02-24 13:10:32
  • 385KB weixin_38739942 2021-01-15 09:44:32
  • qq_41845878 2021-09-06 11:42:21
  • 1.17MB weixin_38556737 2021-05-22 08:47:02
  • 1.21MB u012814946 2020-07-06 19:58:02
  • hanmoge 2021-02-02 12:06:35
  • 607KB weixin_38743737 2019-09-07 03:11:59
  • 938KB weixin_38663113 2021-05-14 06:29:38
  • 1.7MB weixin_38528680 2020-07-07 09:29:01
  • jmh1996 2018-10-15 00:10:08
  • 171KB weixin_38665668 2021-05-11 09:53:04
  • m0_60703264 2021-10-10 21:57:22
  • qq_42688495 2021-01-25 13:28:33
  • 3.42MB weixin_38698174 2021-02-12 00:12:17
  • 873KB weixin_38599430 2021-05-08 00:56:52
  • qq_41845878 2021-09-03 08:23:43
  • guomutian911 2015-01-22 09:25:50

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 9,842
精华内容 3,936
关键字:

形状规划算法