1 运动规划基本概念
1.1运动规划的问题
目的地,全局规划——动作规划,宏观决策——微观决策——动作控制
微观决策——动作规划
规划的本质问题:求解函数的最优解:优化搜索
将当前的状态转换为动作的映射:强化学习
1.2规划器的评价指标
完备性,总是能找到解
概率完备性
决策完备性
时间复杂度
问题复杂度
约束
完整约束
1.3问题再次表述
研究者转换为点的运动规划问题是不行的,车辆具有运动学特征
研究方法:configuration space
在哪里搜索?
怎么去搜索?
(1)建立可见图,障碍物节点,路径节点,搜索方法
(2)抽样的方法,网格或者道路结构图
最后需要通过优化、平滑后处理以提高路径的质量。
2 基于抽样的方法——RRT
2.1 快速搜索随机树
特点:快速、高效
适用于多自由度、复杂环境、动态环境的路径规划问题
RRT的步骤伪代码
(1)随机采样点X_rand
(2)找到距离随机采样最近的当前节点X_near
(3)将X_near朝着X_rand方向连一条线,并沿着该线走一个步长,即找到了X_new
(4)进行碰撞约束检测
不满足约束则返回前面,找寻新的点
如果满足约束,则将新的点加入集合,进行树枝的生长。
RRT算法的特点:
2.2 RRT算法的改进
RRT算法虽然可以很快找到问题的解,但是没有优化的效果,所以存在了很多针对RRT算法的改进。
RRT*就是一种改进方法,先用RRT找到初始路径,再不断优化路径,直到达到最大循环次数。
核心:重新选择父节点、重布线
RRT算法的搜索时间是主要研究问题,RRT*算法可以提升。
RRT*的伪代码
与RRT的最大区别:X_New节点的替换:
搜索多个节点
在新产生的节点X_new附近以一个半径定义一个圆,找到里面的所有节点,(X_new,X_1,X_2,一共三个,加上起点X_start)
以路径最短的为父节点,
X_new——X_near——X_start最短,所以选择X_near为X_new的父节点
将这一条路径添加到集合中(X_new——X_near)
对其他点X_1、X_2进行运算,判断哪个点为父节点路径(与起点)最短
X_1仍然是X_near,X_2替换为X_new,如下图所示
重写集合,绘制路径
2.3 如何判断是否与障碍物碰撞
将障碍物设置为规整的几何形状
圆形:判断是否与外接正方形碰撞。如果用圆心与直线的距离,运算量会增大。
长方形:判断规划的两个连续节点是否与长方形的任意两边相交
通过斜率来运算:
2.4 RRT的其他问题与解决
(1)RRT规划出的路径不够连续的折线,影响了实际车辆的运动
可以通过插值拟合解决。提高连续性与可导性。
(2)RRT效率问题
核心思想,将搜索树引入空旷区域,远离障碍物,避免节点在障碍物处的碰撞检查。
有如下方法:
1偏置采样,在随机采样的过程中以一定比例插入目标状态,引领树向目标状态扩展。
2拒绝采样,在感兴趣的区域生成更多样本
3剪枝
4稀疏化,将图提前搜索,了解障碍物位置
5对碰撞节点加采样惩罚,减少该区域的采样
5提高实时性
3 基于抽样的方法——Lattice
3.1 基本概念
输入:定位、感知、高精地图的参考线
输出:光滑的、安全的、没有碰撞风险的轨迹,直接用于车辆控制器
过程:
3.2 参考线坐标系
与车辆坐标系的比较:
两个坐标系的转换:
参考线坐标系到Frenet坐标系
Frenet坐标系到参考线坐标系
3.3 lattice采样
采样的基本类型
生成规划轨迹方程:s(t)与l(t)
具体运算过程:
(1)采样生成轨迹方程
初始状态与采样状态
纵向:
横向:
(2)轨迹可行性评估
筛选一些不可行的轨迹
(3)计算损失(cost)并排序
纵向速度损失函数
舒适度损失函数:考察加速度变化率
向心加速度损失函数:目的是转弯和掉头时应该减速慢行
碰撞损失函数
横向偏移损失函数
横向加速度损失函数