第三讲 路径规划
ps:排版有一些问题,懒得改了,见Github
一、导航规划简介
-
导航规划:在给定环境的全局或局部知识以及一个或者一系列目标位置的条件下,使机器人能够根据知识和传感器感知信息高效可靠地到达目标位置。
-
导航规划类型
- 固定路径导引
- 有人工标识导引的固定路径导航
- 无轨导航
- 有人工标识导引的无固定路径(无轨)导航
- 无人工标识导引的无轨导航
- 固定路径导引
-
无轨导航规划的主要研究内容
- 路径规划(全局):根据所给定的地图和目标位置,规划一条使机器人到达目标位置的路径(只考虑工作空间的几何约束,不考虑机器人的运动学模型和约束)
- 避障规划(局部):根据所得到的实时传感器测量信息,调整路径/轨迹以避免发生碰撞
- 轨迹生成:根据机器人的运动学模型和约束,寻找适当的控制命令,将可行路径转化为可行轨迹
二、路径规划基本概念
-
文献中路径规划都是在位形空间(Configuration Space)中进行规划。
-
从工作空间转换到位形空间,只需要:障碍物按机器人半径进行膨胀(机器人成为一个点),忽略非完整约束对姿态的限制(机器人是完整的)。
-
位形空间
- 障碍物空间:不可行的位形空间
- 自由空间:可行的位形空间
- 路径规划就是在自由位形空间中为机器人寻找一条路径,使其从初始位置运行到目标位置。
-
路径规划的完备性要求
- 完备性:当解存在时,能够在有限的时间内找到解
-
路径规划的两个基本问题
-
路径规划要先构建出拓扑连通图,再执行路径搜索算法。
-
拓扑连通图
- 构建方法
- 基本思路:对空间作离散化
- 分辨率完备 resolution completeness 解析性离散化,确保获得可行解
- 行车图法:基于障碍物几何形状分解姿态空间
- 单元分解法:区分空闲单元和被占单元
- 势场法:根据障碍物和目标对空间各点施加虚拟力
- 概率完备 probabilistic completeness 基于概率进行随机采样离散化,使获得解的概率趋近于1
- PRM(Probabilistic RoadMaps)
- RRT(Rapid-Exploring Radom Trees)
- 构建方法
-
最优路径搜索
- 精确最优搜索法:深度优先法、宽度优先法
- 近似最优搜索法
- 启发式搜索法:A*, D*
- 准启发式搜索算法:退火、进化、蚁群优化等
-
三、分辨率完备的拓扑连通图构建
-
行车图法:基于障碍物几何形状分解位形空间,将自由空间的连通性用一维曲线的网格表示,在加入起始点和目标点后,在该一维无向连通图中寻找一条无碰路径。
- 可视图法
- 可视图由所有连接可见顶点对的边组成
- 可见指(障碍物)顶点间无障碍物
- 初始位置和目标位置也作为顶点
- 可视图由所有连接可见顶点对的边组成
- Voronoi diagram(维诺图)
- 基本思想:取障碍物之间的中间点,以最大化机器人和障碍物之间的距离
- 可视图法
-
单元分解法
-
基本思想
-
精确单元分解
- 单元边界严格基于环境几何形状分解,所得单元完全空闲
- 缺点:不存在通用的算法
-
近似单元分解
- 栅格表示法,将环境分解成若干个大小相同的栅格。将每个栅格看作拓扑连通图中的结点
- 可变大小的近似单元分解:减小存储空间
- 四叉树表示法:递归地把环境分为4个大小相等的子区域,直到每个区域中所包含的基本元素全为0或全为1。
- 使用四叉树数据结构进行存储
- 四叉树表示法:递归地把环境分为4个大小相等的子区域,直到每个区域中所包含的基本元素全为0或全为1。
-
-
人工势场法
-
基本思想:目标点对机器人产生吸引力,障碍物对机器人产生排斥力,所有力的合成构成机器人的控制律。
-
针对路径规划,计算环境中每个点的力
-
步骤1:构建人工势场
-
计算目标点对空间中每一点的吸引势场
- 希望到达目标点时能停下,所以距离越近吸引力越小。
- 防止距离过远导致吸引力太大,采用分段函数。
-
障碍物:推斥势场
- 大于阈值p0后障碍物在评估点不存在斥力
-
-
步骤2:根据人工势场计算力:对势场求偏导数
-
步骤3:计算合力,进而由力计算得到控制律。力的方向就是机器人运动方向,大小可以对应加速度控制
-
机器人是受人工势场影响的一个点,沿着势场方向就可以避开障碍物达到目标点。 同时得到了路径,不需要再进行路径搜索。既可以规划全局路径,也可以局部避障。
-
缺点:存在局部最小,容易产生振荡和死锁
-
四、路径搜索算法
-
最优路径搜索算法:在构建形成的拓扑连通图(3.1 3.2)中搜索最优路径
- 精确算法:生成精确的最优解,遍历所有路径后选择最优解
- 深度优先法、广度优先法
- 广度优先法优化形成了Dijkstra算法
- 缺点:耗时,难以满足机器人在线快速规划要求
- 近似算法
- 启发式搜索算法:A*,D*, Focused D*等
- 准启发式搜索算法:退火、进化和蚁群优化等
- 精确算法:生成精确的最优解,遍历所有路径后选择最优解
-
深度优先法 就是深度优先搜索 DFS
-
广度优先法 就是广度优先搜索 BFS
-
Dijkstra算法
-
启发式搜索算法 A*
- 算法流程
-
- openlist:可以用来选择的结点
- closelist :已选择结点(仍有机会加入openlist)
- 算法流程
-
准启发式搜索算法:蚁群算法
- 模拟蚂蚁觅食过程中寻找路径的行为
- 基本思想:
- 用一个蚂蚁的行走路径表示待优化问题的一个可行解
- 用整个蚂蚁群体的所有路径作为待优化问题的解空间
- 用蚂蚁群体收敛选择的路径作为问题的优化解
- 实现方式:
五、概率完备的连通图构建
-
PRM(Probabilistic Roadmap) 概率路图
-
基本思想:通过随机采样和碰撞检测找到自由位形空间中的路径点和无碰路径,构建连通图。
-
步骤
- 在位形空间坐标系中随机取点
- 对采样的姿态进行碰撞检测
- 无碰撞姿态成为图节点
- 每个图节点和其最近相邻的k个节点直线连接
- 保留无碰路径为图的边,构成自由位形空间中的Roadmap
- 加入起始点和终止点,在PRM中搜索一条从起始点到终止点的路径
-
算法
-
PRM需要考虑的几个问题:
- 随机位形选择:通常采用均匀随机采样方式
- 寻找最近邻点:可以采用KD树方式加速
- 生成局部路径:在一定范围内直接连接图节点
- 检查路径无碰:可以增量式取点或者二分法取点,判断点是否在障碍物区域内
- 进行路径优化:从目标点开始逐渐向前,若无碰撞则直接连接
- 非全连通情况:例如形成两个连通子图
-
优缺点
-
-
RRT(Rapid-Exploring Random Tree) 快速探索随机树
-
基本思想:
- 连通图采用树的形式,以起始点作为树的根节点
- 采样在空间中随机采样、连接树中最近节点的方式拓展树
- 考虑机器人的运动执行能力(运动学约束)
- 通过树结构可以直接回溯得到路径,不需要使用路径搜索算法
-
基本流程
-
算法
-
影响规划收敛速度的三个步骤
- 随机状态的采样
- 针对问题:RRT扩张偏向状态空间未探测部分,但不是偏向目标点,当环境复杂时计算效率低
- 双向搜索Bidirectional-RRT
- 在搜索树中查找与随机状态距离最近的节点
- 采用KD树
- 新生成节点的防碰检测
- 随机状态的采样
-
RRT* 使路径更平滑
- 针对问题:随机性小步扩展导致路径曲折,成本高
- RRT*:为实现渐进最优,考虑路径成本
- 寻找树中新节点邻域内到新节点路径最短的节点,建立连接,加入树集合
- 对树中新节点邻域内节点进行判断,如果从新节点到该节点形成的路径优于现有树中路径,则将该节点父节点修改为新节点
-
-
分辨率完备与概率完备路径规划方法比较
六、路径规划研究趋势
- 采用学习的路径规划:将路径规划问题转化为马尔可夫决策问题(MDP, Markov Decision Process),采用强化学习/深度强化学习进行求解
- 用深度学习学RRT的采样分布
- social行为路径规划
- 综合地考虑与环境中人的交互
- 学习地图成本:基于观察/指导反馈方式学习空间中人的行为,获得行为空间分布,构建成本地图
- 基于人演示行为轨迹的cost function学习
- 综合地考虑与环境中人的交互