基于采样的自动驾驶规划算法 - PRM,RRT,RRT*,CL-RRT

2023-12-26 20:38:37

本文将讲解PRM,RRT,RRT*自动驾驶规划算法原理,不正之处望读者指正

0 前言

机器人运动规划的基本任务:从开始位置到目标位置的运动
(1)如何躲避构型空间出现的障碍物
(2)如何满足机器人本身在机械、传感方面的速度、加速度等限制

基于采样的运动规划算法就是解决如何躲避构型空间出现的障碍物。

配置空间

机器人规划的配置空间概念:一个空间包含所有机器人自由度的机器人配置,描述为 C ? s p a c e C-space C?space

机器人配置:表示对机器人上点的位置的描述
机器人自由度:规划的时候用最少的坐标数量去表示机器人配置
机器人配置空间:一个空间包含所有机器人自由度的机器人配置,描述为C-space

机器人的位姿在C-space中描述为一个点
机器人配置空间的意义:

在工作空间中进行规划,机器人有不同的形状和大小,需要根据不同的形状大小去做碰撞检测,是费时费力的。

在配置空间中做规划
Alt
机器人在C-space中表示一个点,障碍物做特殊的处理,把工作空间中的障碍物变成配置空间中的障碍物C-obstacle,这个工作是在运动规划前完成的,一次完成的工作

障碍物按照机器人尺寸进行膨胀,上面机器人被设置成了一个点,只要点在障碍物外面,就不会发生碰撞

C-space = C-obstacle + C-free
经过配置空间的处理,路径规划变成了在C-free中找到起点到终点的路径寻找

1 概率路线图(PRM)

在这里插入图片描述

1.1 核心思想

(1)学习预处理阶段

  1. 在配置空间中随机采样足够密集的点
  2. 如果可以相互到达,连接附近的点

(2)查询搜索阶段

采用图搜索算法对G搜索,如果能找到起始点S到终点G的路线,存在可行路径

1.2 PRM主要步骤

(1)采样足够密集的点学习地图结构
在这里插入图片描述
(2)对采样的点碰撞检查,只保留在C-free中的采样点
在这里插入图片描述
(3)每个点通过直线连接到最近的邻居
在这里插入图片描述
(4)删除碰撞连接
在这里插入图片描述
(5)无碰撞连接被保留为边构造图
在这里插入图片描述
(6)添加起点s和终点g到Graph中
在这里插入图片描述
(7)利用图搜索算法A*/Dijstra在路线图里面搜索出一条最优路径
在这里插入图片描述

1.3 算法流程

在这里插入图片描述
PRM算法流程

1 learning-phase阶段:
V V V:构建的图的所有顶点的集合
E E E:图中所有边的集合
2 采样点个数为n
3 通过某种采样策略,不同分布得到采样点
4 以采样点为中心, r r r为半径,在这个圆范围内的邻居节点,把它记录到 U U U
5 把采样点加入到顶点集 V V V
6 遍历邻居节点集 U U U的每个节点
7-8 定义一些规则滤除一些节点和边
7 采样点 x r a n d x_{rand} xrand?和已有的节点处在相同的邻接元素下,跳过
8 碰撞检测,检测 x r a n d x_{rand} xrand? u u u是不是发生碰撞,如果Free,就把 x r a n d x_{rand} xrand? u u u连成的边加入到 E E E
9 重复n次之后,就得到了一个完整的图 G = ( V , E ) G = (V,E) G=(V,E)
最后应用图搜索算法在G上找到一条最优路径

sPRM算法与PRM算法的区别:

只要采样到某个节点,就把以r为半径圆里面所有的节点都进行一个连接,边比PRM多,搜索消耗的资源更大

选择节点之间的连接方式:
(1)k近邻PRM
选择采样点周围最近的k个邻居
U ← k N e a r e s t ( G = ( V , E ) , v , k ) U\gets kNearest(G=(V,E),v,k) UkNearest(G=(V,E),v,k)
(2)有界维度PRM
就是以常规的PRM算法为基础,如果圆里面采样点过多,就找采样点的k个邻居取交集
U ← N e a r ( G , x r a n d , r ) ∩ k N e a r e s t ( G = ( V , E ) , v , k ) U\gets Near(G,x_{rand},r)\cap kNearest(G=(V,E),v,k) UNear(G,xrand?,r)kNearest(G=(V,E),v,k)
(3)可变半径PRM
把r为半径的圆作为采样节点个数n的函数,采样点较少情况下,r可以取大一点,采样点足够多的时候,r取小一点

PRM*算法流程
在这里插入图片描述
d d d:维度
n n n:采样节点个数

1.4 PRM算法的优点和缺点

优点:

概率完备性,如果运行时间足够长(或者采集足够多的点),如果有解一定是最优解

缺点:

(1)在整个状态空间上构造图,需要连接特定的开始和目标,可能浪费一些不必要的资源
(2)使用直线连接不符合车辆运动学约束
(3)抽样方法的完备性很弱,即使空间存在合理的路径,由于抽样参数的设置,也可能无法找到路径。因为随机抽样,所以该方法稳定性也不好,对于同样的问题,前后两次解不一样,在严格要求稳定性的场合不适用

采样点的数量采样点存在通路的最大距离是路径规划成功的关键

2 RRT

2.1 RRT核心思想和特点

在这里插入图片描述
RRT是一种通过随机构建空间填充树来有效搜索非凸,高维空间的算法。

核心思想:RRT 算法首先将起点初始化为随机树的根节点,然后在机器人的可达空间中随机生成采样点,从树的根节点逐步向采样点扩展节点,节点和节点之间的连线构成了整个随机树,当某个节点与目标点的距离小于设定的阈值时,即可认为找到可行路径。

在这里插入图片描述

RRT的特点就是能够快速有效地搜索高维空间,通过状态空间的随机采样点,把搜索导向空白区域,从而寻找到一条从起始点到目标点的规划路径,适合解决多自由度机器人在复杂环境下和动态环境中的路径规划

2.2 算法流程

在这里插入图片描述
1 将 x i n i t x_{init} xinit?加入到顶点集 V V V
2 采样n次
3 随机采样得到 x r a n d x_{rand} xrand?
4 图中距离 x r a n d x_{rand} xrand?最近的节点 x n e a r e s t x_{nearest} xnearest?
5 连接 x r a n d x_{rand} xrand? x n e a r e s t x_{nearest} xnearest?,之间的节点 x n e w x_{new} xnew?
6-7 只有通过碰撞检测,才会把 x n e w x_{new} xnew?加入顶点集 V V V,连接 x n e a r e s t x_{nearest} xnearest? x n e w x_{new} xnew?

2.3 RRT优缺点

优点:

(1)简单找到起点到终点的路径,比PRM更高效,该算法通过尽可能少地探索环境,来实现有效的单一路径规划,对未知环境适应能力强
(2)RRT 算法通过随机树向未观察的空间区域生长,并且不会回归到已经探索过的区域,这实现了对空间的快速探索
(3)搜索方法不是维持固定的栅格结构,而是在运行中构建随机树,通过随机树内部的节点的连接找到路径。

缺点:

(1)不满足概率完备性,只能连接最近的节点
(2)需要对输入空间进行离散化采样次数太少,则生成的路径将表现出较差的性能采样次数太多则会增加整个规划过程的计算量降低路径规划的实时性
(3)RRT算法生成的路径存在冗余的节点,增加机器人实际运行中的路程

2.4 RRG

RRT的变体,具有概率完备性
在这里插入图片描述
核心思想:

不要只连接 x n e w x_{new} xnew? x n e a r e s t x_{nearest} xnearest?
尝试连接到半径内的所有顶点

最后需要接入图搜索算法寻找一条最优路径,违背了RRT的初衷,没有把构造图和搜索步骤合二为一

2.5 基于运动学的RRT

在这里插入图片描述
区别在于5 使用基于运动学的方法来引导两个节点
在这里插入图片描述

3 RRT*

在这里插入图片描述

3.1 核心思想

(1)相比于RRG算法,维护树结构而不是图,会从图中删除多余的边
(2)相比于RRT算法,添加了“rewire"操作(每次采样到新的节点,会把以他为圆心,半径为r的圆内其他节点作为一个考量,对这些节点做一些修剪的操作)确保通过最小成本路径到达顶点

3.2 RRT*算法流程

在这里插入图片描述

前半部分与RRT相同
在这里插入图片描述

后半部分
(1)连接以r为半径的圆的所有顶点,在集合中选择cost最小的去连接
在这里插入图片描述

(2)得到了 x n e a r x_{near} xnear?,依次遍历每一个节点,判断累计成本最小的,将 x n e a r x_{near} xnear?标记为 x m i n x_{min} xmin?:保证 x n e w x_{new} xnew?本身的最优性
(3)对树做修剪:每次采样到 x n e w x_{new} xnew?之后,周围其他节点都会做一次检查,判断是否能找到cost最小的路径
在这里插入图片描述
在这里插入图片描述

4 CL-RRT

核心思想:
(1)相比于对车辆输入进行采样的标准的RRT,CL-RRT对控制器的输入进行采样
(2)通过前向模拟得到动态可行轨迹
(3)对于城市场景,优化算法策略:采样策略、节点选择策略
在这里插入图片描述
转向控制器:Pure-Pursuit Controller
在这里插入图片描述

速度控制器:PI Controller
在这里插入图片描述
在这里插入图片描述
采样策略:
在这里插入图片描述
n r 、 n θ n_r、n_\theta nr?nθ?:具有高斯分布的随机变量
σ r \sigma_r σr?:径向标准差
σ θ \sigma_\theta σθ?:圆周方向标准差

根据车辆位置和道路规则改变这些参数

Node选择策略:
(1)RRT试图将样本连接到树中最近的节点,当RRT应用于转弯能力有限的车辆时,需要进行拓展
(2)CL-RRT算法使用节点和采样点之间的Dubins路径长度作为距离度量
在这里插入图片描述
Reeds-Shepp曲线和Dubins曲线

文章来源:https://blog.csdn.net/BigDavid123/article/details/135187420
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。