机器人技术-第5讲路径规划和避障

1、第五讲第五讲 路径规划与避障路径规划与避障参考书目参考书目 Roland Siegwart and Illah R. Nourbakhsh. Introduc8on to Autonomous Mobile Robots. The MIT Press, 2004. Where am I ? (在哪里在哪里?) Where am I going ? (到哪里到哪里?) How do I get there ? (怎么去怎么去?)自主移动的三个关键问题自主移动的三个关键问题?自定位自定位目标规划目标规划导航规划导航规划环环 境境 地地 图图地图表示地图表示 未知环境地图构建未知环境地图构建自主移动

2、必须具备的四要素自主移动必须具备的四要素 环境模型环境模型/地图（预先已知或者自主构建）地图（预先已知或者自主构建） 感知和分析环境感知和分析环境 确定自己在环境中的位置确定自己在环境中的位置 规划并执行移动规划并执行移动自主移动的一般软件体系结构自主移动的一般软件体系结构目标规划目标规划导航规划导航规划信息处理信息处理感知感知运动控制运动控制执行执行测量数据测量数据局部观测局部观测/地图地图位置位置 全局地图全局地图参考点运动轨迹参考点运动轨迹驱动指令驱动指令感感 知知控控 制制任务指令任务指令知识、数据库知识、数据库环境环境自定位自定位 地图构建地图构建导航规划导航规划 导航规划：在给定环

3、境的全局或局部知识以及一个导航规划：在给定环境的全局或局部知识以及一个 或者一系列目标位置的条件下，使机器人能够根据或者一系列目标位置的条件下，使机器人能够根据 知识和传感器感知信息高效可靠地到达目标位置知识和传感器感知信息高效可靠地到达目标位置 导航方式：导航方式：路径导航路径导航自主导航自主导航路径导航路径导航 在路径上连续敷设电缆、反射条带等引导标记，在路径上连续敷设电缆、反射条带等引导标记， 或采用电波、光等连续引导信号，或设置磁铁、或采用电波、光等连续引导信号，或设置磁铁、 路标等断续的引导标记路标等断续的引导标记优点：简单优点：简单 缺点缺点 将机器人的移动自由度限制在一维将机器人

4、的移动自由度限制在一维 降低了机器人环境适应的能力降低了机器人环境适应的能力 预设路标定无法适应灵活多变的作业要预设路标定无法适应灵活多变的作业要求求 给机器人的应用增加了额外的硬件开销给机器人的应用增加了额外的硬件开销7自主导航的主要问题自主导航的主要问题 路径规划路径规划：根据所给定的地图和目标位置，规划一条使根据所给定的地图和目标位置，规划一条使 机器人到达目标位置的路径（只考虑工作空间的几何约机器人到达目标位置的路径（只考虑工作空间的几何约 束，不考虑机器人的运动学模型和约束）束，不考虑机器人的运动学模型和约束） 避障：根据所得到的实时传感器测量信息，调整路径避障：根据所得到的实时传感

5、器测量信息，调整路径/ /轨轨 迹以避免发生碰撞迹以避免发生碰撞 轨迹生成：根据机器人的运动学模型和约束，寻找适当轨迹生成：根据机器人的运动学模型和约束，寻找适当 的控制命令，将可行路径转化为可行轨迹。的控制命令，将可行路径转化为可行轨迹。路径规划和避障规划路径规划和避障规划路径规划根据所给定的地图和目标根据所给定的地图和目标 位置，规划一条使机器人位置，规划一条使机器人 到达目标位置的路径到达目标位置的路径避障规划根据所得到的实时传感器根据所得到的实时传感器 测量信息，调整轨迹以避测量信息，调整轨迹以避 免发生碰撞免发生碰撞面向长期目标面向长期目标 的战略方法的战略方法解决当前问题的解决当前

6、问题的 战术方法战术方法互互 补补对对 立立路径规划和轨迹规划路径规划和轨迹规划 路径不包含时间轴，轨迹包含时间轴路径不包含时间轴，轨迹包含时间轴 路径规划只考虑工作空间的几何约束，不考虑路径规划只考虑工作空间的几何约束，不考虑 机器人的运动学模型和约束。机器人的运动学模型和约束。 轨迹规划则需要根据机器人的运动学模型和轨迹规划则需要根据机器人的运动学模型和 约束，寻找适当的控制命令，将可行路径转化约束，寻找适当的控制命令，将可行路径转化 为可行轨迹，使机器人鲁棒地跟随期望路径。为可行轨迹，使机器人鲁棒地跟随期望路径。路径规划路径规划姿态空间姿态空间(Configura8on space) 将

7、工作空间降为二维物理空间将工作空间降为二维物理空间 两个重要假设：两个重要假设： 机器人是一个点，障碍物按机器人半径进行膨胀机器人是一个点，障碍物按机器人半径进行膨胀 机器人是完整的，忽略非完整约束对姿态的限制机器人是完整的，忽略非完整约束对姿态的限制姿态空间姿态空间 一个机器人所有可能的姿态集合一个机器人所有可能的姿态集合 障碍物空间：不可行的姿态集合障碍物空间：不可行的姿态集合 在该空间中，机器人会与障碍物发生碰撞在该空间中，机器人会与障碍物发生碰撞 自由空间：可行的姿态集合自由空间：可行的姿态集合 在该空间中，机器人将无碰地安全移动在该空间中，机器人将无碰地安全移动路径规划：在自由姿态空

8、间中为机器人寻找路径规划：在自由姿态空间中为机器人寻找 一条路径，使其从初始姿态发展到目标姿态一条路径，使其从初始姿态发展到目标姿态路径规划方法路径规划方法 路径规划方法应确保完备性，即当存在一条从初路径规划方法应确保完备性，即当存在一条从初 始姿态到目标姿态的路径时，系统总能够到达目始姿态到目标姿态的路径时，系统总能够到达目 标姿态标姿态 达到近似完备性的方法：将姿态空间离散化，构达到近似完备性的方法：将姿态空间离散化，构 成分辨率完备的路径规划算法成分辨率完备的路径规划算法不同的空间不同的空间 离散策略离散策略1.1. 行车图法：在自由空间中构建连通网络行车图法：在自由空间中构建连通网络2

9、.2. 单元分解法：区分空闲单元和被占单元单元分解法：区分空闲单元和被占单元3.3. 势场法：对空间施加虚拟力势场法：对空间施加虚拟力行车图路径规划行车图路径规划 基本思想：基于障碍物几何形状分解姿态空间，将自由空基本思想：基于障碍物几何形状分解姿态空间，将自由空 间的连通性用一维曲线的网格表示，在加入起始点和目标间的连通性用一维曲线的网格表示，在加入起始点和目标 点后，在该一维无向连通图中寻找一条无碰路径点后，在该一维无向连通图中寻找一条无碰路径 构建行车图的典型方法：构建行车图的典型方法：可视图（Visibility graph）Voronoi diagram可视图法可视图法 可视图由所有

10、连接可见顶点对的边组成可视图由所有连接可见顶点对的边组成 可见指顶点之间无障碍物可见指顶点之间无障碍物 初始位置和目标位置也作为顶点初始位置和目标位置也作为顶点可视图法可视图法 优点：优点： 非常简单，特别是当环境表示用多边形描述物体时非常简单，特别是当环境表示用多边形描述物体时 基于可视图的路径规划可得到在路径长度上最优的解基于可视图的路径规划可得到在路径长度上最优的解 缺点：缺点： 所得路径过于靠近障碍物，不够安全。所得路径过于靠近障碍物，不够安全。 常用的解决方法：以远远大于机器人半径的尺寸扩大常用的解决方法：以远远大于机器人半径的尺寸扩大 障碍物，或者在路径规划后修改所得路径，使其与障

11、障碍物，或者在路径规划后修改所得路径，使其与障 碍物保持一定的距离碍物保持一定的距离Voronoi diagram 基本思想：取障碍物之间的中间点，以最大化机基本思想：取障碍物之间的中间点，以最大化机 器人和障碍物之间的距离器人和障碍物之间的距离Voronoi diagram 构建方法：构建方法： 对于自由空间中的每一点，计算它到最近障碍物的距离；对于自由空间中的每一点，计算它到最近障碍物的距离； 类似于画直方图，在垂直于二维空间平面的轴上用高度表类似于画直方图，在垂直于二维空间平面的轴上用高度表 示该点到障碍物的距离；示该点到障碍物的距离； 当某个点到两个或多个障碍当某个点到两个或多个障碍

12、物距离相等时，其距离点处物距离相等时，其距离点处 出现尖峰出现尖峰，Voronoi diagram 就由连接这些尖峰点的边组就由连接这些尖峰点的边组 成。成。Voronoi diagram 优点：安全性高优点：安全性高 缺点：计算复杂、路径长度较可视图法长、不适缺点：计算复杂、路径长度较可视图法长、不适 用于短距离定位传感器用于短距离定位传感器单元分解路径规划单元分解路径规划 基本思想基本思想 首先，将姿态空间中的自由空间地分为若干的小区域，首先，将姿态空间中的自由空间地分为若干的小区域， 每一个区域作为一个单元，以单元为顶点、以单元之每一个区域作为一个单元，以单元为顶点、以单元之 间的相邻关

13、系为边构成一张连通图；间的相邻关系为边构成一张连通图； 其次，在连通图中寻找包含初始姿态和目标姿态的单其次，在连通图中寻找包含初始姿态和目标姿态的单 元，搜索连接初始单元和目标单元的路径；元，搜索连接初始单元和目标单元的路径； 最后，根据所得路径的单元序列生成单元内部的路径最后，根据所得路径的单元序列生成单元内部的路径 主要方法主要方法 精确单元分解精确单元分解 近似单元分解近似单元分解精确单元分解精确单元分解 单元边界严格基于环境几何形状分解，所得单元单元边界严格基于环境几何形状分解，所得单元 完全空闲完全空闲精确单元分解精确单元分解 优点：优点： 机器人不需要考虑它在每个空闲单元中的具体位

14、置，机器人不需要考虑它在每个空闲单元中的具体位置， 只需要考虑如何从一个单元移动到相邻的空闲单元只需要考虑如何从一个单元移动到相邻的空闲单元 单元数与环境大小无关单元数与环境大小无关 缺点：缺点： 计算效率极大地依赖于环境中物体的复杂度计算效率极大地依赖于环境中物体的复杂度近似单元分解近似单元分解 栅格表示法，将环境分解成若干个大小相同的栅格栅格表示法，将环境分解成若干个大小相同的栅格 并不是每个单元都是完全被占或者完全空闲的，因此分并不是每个单元都是完全被占或者完全空闲的，因此分 解后的单元集合是对实际地图的一种近似解后的单元集合是对实际地图的一种近似近似单元分解近似单元分解 优点优点 非常

15、简单，与环境的疏密和物体形状的复杂度无关非常简单，与环境的疏密和物体形状的复杂度无关 缺点：缺点： 对存储空间有要求对存储空间有要求可变大小的近似单元分解可变大小的近似单元分解四叉树表示法：递归地把环境分为4个大小相等的子区域。 直到每个区域中所包含的基本元素全为0或全为1。人工势场法人工势场法 基本思想：基本思想： 目标点对机器人产生吸引力目标点对机器人产生吸引力 障碍物对机器人产生排斥力障碍物对机器人产生排斥力 所有力的合成构成机器人的控制律所有力的合成构成机器人的控制律障碍物目标点机器人FtF人工势场法人工势场法 步骤步骤1 1：构建人工势场：构建人工势场(Ar8ficial Poten

16、8al Field) 目标点：吸引势场目标点：吸引势场Ka为系数x为被评估点 xd 为目标点 da为距离阈值2822 )adda| x x | dattdaaaK | x x |U(x) K (2d | x x | dda| x x | d人工势场法人工势场法 步骤步骤1 1：构建人工势场：构建人工势场(Ar8ficial Poten8al Field) 目标点：吸引势场目标点：吸引势场 障碍物：推斥势场障碍物：推斥势场0被评估点和障碍物点之间的距离被评估点和障碍物点之间的距离预定义距离阈值预定义距离阈值1 20r 1repU(x) K 0 1 2 00 人工势场法人工势场法 步骤步骤2 2：

17、根据人工势场计算力：根据人工势场计算力 对势场求偏导数对势场求偏导数2Ka (x xd )| x xd | daattattdaa2K dda| x x | dd x x| x x |F (x) U(x) 0200rreprep 1 1 1 K 0 F(x) U(x) x0 x0为最近障碍物的坐标向量x x xy Tx人工势场法人工势场法 步骤步骤3 3：计算合力，并进而由力计算得到控制律：计算合力，并进而由力计算得到控制律F (x) U (x) Uatt (x) Urep (x) Fatt (x) Frep (x)31人工势场法人工势场法 机器人是受人工势场影响的一个点，沿着势场方机器人是受

18、人工势场影响的一个点，沿着势场方 向就可以避开障碍物达到目标点向就可以避开障碍物达到目标点人工势场法人工势场法 不仅是一种路径规划方法，所构建的势场也构不仅是一种路径规划方法，所构建的势场也构 成了机器人的控制律，能够较好地适应目标的成了机器人的控制律，能够较好地适应目标的 变化和环境中的动态障碍物变化和环境中的动态障碍物人工势场法人工势场法 缺点：存在局部最小，容易产生振荡和死锁缺点：存在局部最小，容易产生振荡和死锁障碍物 目标点Fr2FtF机器人Fr1障碍物2最优搜索算法最优搜索算法 在连通图中寻找最优路径是一个组合最优问题，在连通图中寻找最优路径是一个组合最优问题， 属于属于NP-com

19、plete问题问题 精确算法：生成精确的最优解精确算法：生成精确的最优解 深度优先法、广度优先法（也称为深度优先法、广度优先法（也称为Dijkstra算法）算法） 近似算法近似算法 启发式搜索算法：启发式搜索算法： A*, D*, Focused D*等等 准启发式搜索算法：准启发式搜索算法： 退火、进化和蚁群优化等退火、进化和蚁群优化等A* 根据评估函数在静态连通图中寻找最优路径的经根据评估函数在静态连通图中寻找最优路径的经 典启发式搜索算法典启发式搜索算法 当前搜索结点往下选择下一步结点时，通过启发式函当前搜索结点往下选择下一步结点时，通过启发式函 数进行选择，选择代价最少的结点作为下一步

20、搜索结数进行选择，选择代价最少的结点作为下一步搜索结 点而跳转其上点而跳转其上f (n) g(n) h(n)评估函数评估函数n 表示节点表示节点g(n) 表示从起始点到节点表示从起始点到节点 的实际代价的实际代价h(n) 为从节点为从节点 到目标点的最佳路径的估计代到目标点的最佳路径的估计代价价37ab 2d1.522334h(d)=4.5ch(c)=4h(e)=2eh(a)=4h(b)=2af(a)=5.5df(d)=6.5将起始点放入Openlist 中，并计算g(n),h(n),f(n)从Openlist中取出f(n)最 小的节点回 溯建 路 径指定路径起始节点和 终止节点该节点是否为终

21、止节点？将该节点移出openlist， 放入closelist中tg=当前节点g(n) + 当前 节点到邻节点的距离忽 略 该 邻 节 点如果邻节点在closelist 中且tg = 邻节点g(n)如果邻节点不在openlist中或tg邻节点g(n)置该邻节点路径的上一 节点为当前节点，邻节 点g(n)=tg,并计算h(n)f(n)如果该邻节点不在openlist中，则加入对对 所所有有 邻邻节节 点点Y 构NYYNN路径成本的计算路径成本的计算 欧式距离欧式距离（2-norm距离）距离） 曼哈顿距离（曼哈顿距离（ManhaQan distance, Block distance, 1-nor

22、m距离距离，L1 distance） 切比雪夫距离切比雪夫距离（Chebychev distance, infinity norm距离）距离） 明可夫斯基距离明可夫斯基距离（Minkowski距离）距离）欧式距离欧式距离 2-norm距离距离22i(xi yi )x yx (x1, x2 , xn )y ( y1, y2 , yn )曼哈顿距离曼哈顿距离 ManhaQan distancex y 1 xi yii用以标明两个点上在标用以标明两个点上在标 准坐标系上的绝对轴距准坐标系上的绝对轴距 总和总和绿线为欧氏距离绿线为欧氏距离 红线为红线为曼哈顿距离曼哈顿距离黄蓝线为等价曼哈顿距离黄蓝线为

23、等价曼哈顿距离切比雪夫距离切比雪夫距离i Chebychev distancex y max xi yi起源于国际国王行走距离计算起源于国际国王行走距离计算 可用于仓储物流距离计算可用于仓储物流距离计算明可夫斯基明可夫斯基(Minkowski)距离距离 两个体各个变量值绝对差的两个体各个变量值绝对差的k次幂总和的次幂总和的k 次方根次方根Minkowski(x, y) k可以任意指定可以任意指定kkiii| x y |k 1,曼哈顿距离k 2,欧氏距离k ,切比雪夫距离其他路径规划方法其他路径规划方法进化算法进化算法44交叉操作：考虑首末点，分成三段交叉操作：考虑首末点，分成三段其他常用路径规

24、划方法其他常用路径规划方法 粒子群优化算法粒子群优化算法（PSO，Particle Swarm Optimization）vi w * vic1 rand() *( pibestc2 rand() *(gbest pi ) pi )其他常用路径规划方法其他常用路径规划方法 随机路径规划算法随机路径规划算法避障规划避障规划避障规划避障规划 也称为反应式避障法也称为反应式避障法 主要方法：主要方法： Bug算法算法 向量势直方图法向量势直方图法 曲率速度法曲率速度法 动态窗口法动态窗口法 等等Bug算法算法 基本思想：绕着机器人到目标点路上的每基本思想：绕着机器人到目标点路上的每 个障碍物的轮廓移

25、动个障碍物的轮廓移动Bug1算法：当遇到障碍物堵算法：当遇到障碍物堵 塞路径时，机器人绕着障碍塞路径时，机器人绕着障碍 物移动，直到找到障碍物上物移动，直到找到障碍物上 最靠近目标的点，然后再从最靠近目标的点，然后再从 该点向目标点移动。该点向目标点移动。优点：非常简单，可以确保机器人能够到达任何可达目标优点：非常简单，可以确保机器人能够到达任何可达目标缺点：为了找到障碍物上最靠近目标的点，需要先绕着障缺点：为了找到障碍物上最靠近目标的点，需要先绕着障 碍物移动一周，效率很低。碍物移动一周，效率很低。Bug算法算法Bug2算法：当出现障碍物算法：当出现障碍物时，机器人绕着障碍物移动，时，机器人

26、绕着障碍物移动， 一旦机器人可以直接移向目一旦机器人可以直接移向目 标时，就立即脱离障碍物标时，就立即脱离障碍物优点：在一定程度上缩短了整个路径优点：在一定程度上缩短了整个路径存在问题：优于无法在两个绕行方向之间进行择优选择，存在问题：优于无法在两个绕行方向之间进行择优选择， 所所选绕行方向可能会严重增加路径长度。选绕行方向可能会严重增加路径长度。主要缺点：机器人每一主要缺点：机器人每一 时刻的行为仅仅依据该时刻的行为仅仅依据该 时刻的传感器信息，当时刻的传感器信息，当 信息量不够时难以实现信息量不够时难以实现 高效鲁棒的避障高效鲁棒的避障向量势直方图法向量势直方图法 基本步骤：基本步骤： 算

27、法内构建并维护机器人周围环境的局部栅格地图算法内构建并维护机器人周围环境的局部栅格地图 根据占用栅格地图的单元值计算障碍物概率直方图根据占用栅格地图的单元值计算障碍物概率直方图 根据直方图，计算导航方向根据直方图，计算导航方向 识别所有可以让机器人通过的通道识别所有可以让机器人通过的通道 对每个通道计算成本对每个通道计算成本 选择具有最低成本的通道选择具有最低成本的通道成本函数成本函数 G a target_direction b wheel_orientation c previous_directiontarget_direction : 路径与目标之间的对齐量路径与目标之间的对齐量 wh

28、eel_orientation:新方向和当前机器人方向的差异量新方向和当前机器人方向的差异量 Previous_direction: 原来选择方向和原来选择方向和新方向之间的差异量新方向之间的差异量动态窗口法动态窗口法 基本思想：在速度空间中搜索适当的平基本思想：在速度空间中搜索适当的平 移速度和旋转速度指令移速度和旋转速度指令 (v,w)动态窗口法动态窗口法 (1) 基于速度控制运动模型，可以构建得到可行基于速度控制运动模型，可以构建得到可行 的速度空间的速度空间动态窗口法动态窗口法 (1) 基于速度控制运动模型，可以构建得到可行基于速度控制运动模型，可以构建得到可行 的速度空间的速度空间机

29、器人的速度控制运动模型机器人的速度控制运动模型ccy yc x xc xcy yx x r sin( )r cos( )r sin( )r sin( t)r cos( ) yc r cos( t) vr XIIY(xc , yc )r90o(x, y)(x , y )假设没有噪声假设没有噪声设控制时间间隔为设控制时间间隔为 t ，时间间隔内机器人速度和，时间间隔内机器人速度和角角 速度保持不变，则机器人绕着半径为速度保持不变，则机器人绕着半径为r r的圆周运动的圆周运动机器人的速度控制运动模型机器人的速度控制运动模型x x v sin( ) v sin( t) ty y v cos( ) v cos( t)无噪声下的理想运动模型无噪声下的理想运动模型Va (v, ) | v 2 dist(v, ) vb 2 dist(v, ) b 可以让机器人停止不与障碍物相碰的可行速度集合可以让机器人停止不与障碍物相碰