![](https://img-blog.csdnimg.cn/img_convert/e7a77642714e4159a44058df09b44614.png)
![](https://img-blog.csdnimg.cn/img_convert/c89192d63afb403db78c8bb0fadb6871.png)
1 运动规划基本概念
![](https://img-blog.csdnimg.cn/img_convert/97434c7a04794de2b6af1d9c5c2ee3cc.png)
1.1运动规划的问题
![](https://img-blog.csdnimg.cn/img_convert/9d3ae679d62e4715913f7b980d6819b2.png)
目的地,全局规划——动作规划,宏观决策——微观决策——动作控制
微观决策——动作规划
![](https://img-blog.csdnimg.cn/img_convert/3d6c183d313b411bba29280aaeb1d462.png)
规划的本质问题:求解函数的最优解:优化搜索
将当前的状态转换为动作的映射:强化学习
1.2规划器的评价指标
![](https://img-blog.csdnimg.cn/img_convert/8af72d36d813468b85c86646ea254de6.png)
完备性,总是能找到解
![](https://img-blog.csdnimg.cn/img_convert/122a0ed516854fd898ce2df05314ad1f.png)
概率完备性
![](https://img-blog.csdnimg.cn/img_convert/3d881f9155c947ebbb323d3dc90a7212.png)
决策完备性
![](https://img-blog.csdnimg.cn/img_convert/fe13b3e1cfd8405ebe9fbfdcbefb4d11.png)
时间复杂度
![](https://img-blog.csdnimg.cn/img_convert/e128891055f04839bd1169df21674970.png)
问题复杂度
![](https://img-blog.csdnimg.cn/img_convert/9348d006e48b4922841c95d464901b78.png)
约束
![](https://img-blog.csdnimg.cn/img_convert/34f33f3f540e47158822ff64f0f607eb.png)
完整约束
![](https://img-blog.csdnimg.cn/img_convert/85d3a411bbaf473da739dae2b251283b.png)
1.3问题再次表述
研究者转换为点的运动规划问题是不行的,车辆具有运动学特征
研究方法:configuration space
在哪里搜索?
![](https://img-blog.csdnimg.cn/img_convert/866dd427d1d34faebc998cf482ed5804.png)
怎么去搜索?
(1)建立可见图,障碍物节点,路径节点,搜索方法
(2)抽样的方法,网格或者道路结构图
最后需要通过优化、平滑后处理以提高路径的质量。
![](https://img-blog.csdnimg.cn/img_convert/f2523fd212c948609b2dbf56368bbfa0.png)
2 基于抽样的方法——RRT
![](https://img-blog.csdnimg.cn/img_convert/60b6e712cac144cea8c0c82adcf28b4c.png)
2.1 快速搜索随机树
特点:快速、高效
适用于多自由度、复杂环境、动态环境的路径规划问题
![](https://img-blog.csdnimg.cn/img_convert/093282155d8243919e9bb9dbaa659580.png)
RRT的步骤伪代码
(1)随机采样点X_rand
(2)找到距离随机采样最近的当前节点X_near
![](https://img-blog.csdnimg.cn/img_convert/fa2d055a0a8d4faf8793cc43f1d81b85.png)
(3)将X_near朝着X_rand方向连一条线,并沿着该线走一个步长,即找到了X_new
![](https://img-blog.csdnimg.cn/img_convert/50eb7e6b9dfe41a1a314366b36ae489d.png)
(4)进行碰撞约束检测
不满足约束则返回前面,找寻新的点
![](https://img-blog.csdnimg.cn/img_convert/09024844c0324feca2bbf957c0799be7.png)
如果满足约束,则将新的点加入集合,进行树枝的生长。
![](https://img-blog.csdnimg.cn/img_convert/46a084a3e57c4f88bba3185d3ff5ccde.png)
RRT算法的特点:
![](https://img-blog.csdnimg.cn/img_convert/a4f41bb68c31424aac6dd0a75a86e2bb.png)
2.2 RRT算法的改进
RRT算法虽然可以很快找到问题的解,但是没有优化的效果,所以存在了很多针对RRT算法的改进。
RRT*就是一种改进方法,先用RRT找到初始路径,再不断优化路径,直到达到最大循环次数。
核心:重新选择父节点、重布线
RRT算法的搜索时间是主要研究问题,RRT*算法可以提升。
![](https://img-blog.csdnimg.cn/img_convert/de6eaff979954680ad20bfe7d834ee78.png)
RRT*的伪代码
![](https://img-blog.csdnimg.cn/img_convert/d1a9556af0474c4190f8fad2549504ba.png)
与RRT的最大区别:X_New节点的替换:
搜索多个节点
在新产生的节点X_new附近以一个半径定义一个圆,找到里面的所有节点,(X_new,X_1,X_2,一共三个,加上起点X_start)
![](https://img-blog.csdnimg.cn/img_convert/8d5f90ceb45540c4b33c0fa6699ef515.png)
以路径最短的为父节点,
X_new——X_near——X_start最短,所以选择X_near为X_new的父节点
![](https://img-blog.csdnimg.cn/img_convert/f98db19536fd4252b196a91d803d9f8a.png)
将这一条路径添加到集合中(X_new——X_near)
![](https://img-blog.csdnimg.cn/img_convert/2e89746e7e2b435c9ee741edf52e1bed.png)
对其他点X_1、X_2进行运算,判断哪个点为父节点路径(与起点)最短
X_1仍然是X_near,X_2替换为X_new,如下图所示
![](https://img-blog.csdnimg.cn/img_convert/3975dd28c30242ddb08248b1f0fd0694.png)
重写集合,绘制路径
![](https://img-blog.csdnimg.cn/img_convert/c320a835d2484115b907e7271fa3f72e.png)
![](https://img-blog.csdnimg.cn/img_convert/40609bb7cbaa48978abc95145425b0e7.png)
![](https://img-blog.csdnimg.cn/img_convert/17c6c4f82da1402b8a94242c43b3fb96.png)
![](https://img-blog.csdnimg.cn/img_convert/7334700abb71400f8c9835cbbda6f0b9.png)
2.3 如何判断是否与障碍物碰撞
将障碍物设置为规整的几何形状
圆形:判断是否与外接正方形碰撞。如果用圆心与直线的距离,运算量会增大。
![](https://img-blog.csdnimg.cn/img_convert/78e5852587d1476b8642bc774a188626.png)
长方形:判断规划的两个连续节点是否与长方形的任意两边相交
![](https://img-blog.csdnimg.cn/img_convert/ec3f1c5b3c684b91bd71b8e5bfbae740.png)
通过斜率来运算:
![](https://img-blog.csdnimg.cn/img_convert/73a143bc8fe24f63923aed7f28d5f590.png)
2.4 RRT的其他问题与解决
(1)RRT规划出的路径不够连续的折线,影响了实际车辆的运动
可以通过插值拟合解决。提高连续性与可导性。
![](https://img-blog.csdnimg.cn/img_convert/6bc4fb001b524bbd87b05f6a9e12cd08.png)
(2)RRT效率问题
核心思想,将搜索树引入空旷区域,远离障碍物,避免节点在障碍物处的碰撞检查。
有如下方法:
1偏置采样,在随机采样的过程中以一定比例插入目标状态,引领树向目标状态扩展。
2拒绝采样,在感兴趣的区域生成更多样本
3剪枝
4稀疏化,将图提前搜索,了解障碍物位置
5对碰撞节点加采样惩罚,减少该区域的采样
5提高实时性
![](https://img-blog.csdnimg.cn/img_convert/6a64717c94614d82b50575fdc6e6f1f7.png)
3 基于抽样的方法——Lattice
3.1 基本概念
输入:定位、感知、高精地图的参考线
输出:光滑的、安全的、没有碰撞风险的轨迹,直接用于车辆控制器
![](https://img-blog.csdnimg.cn/img_convert/4b6cc1b0b6c74e12af6981f481c4c566.png)
过程:
![](https://img-blog.csdnimg.cn/img_convert/51ab4e05185346c9b93c1cb5bf00b062.png)
![](https://img-blog.csdnimg.cn/img_convert/a7b85e378ec3437f89caf36262f19e8d.png)
3.2 参考线坐标系
![](https://img-blog.csdnimg.cn/img_convert/7cc40feb889e42bc930f334c75d1f7a8.png)
与车辆坐标系的比较:
![](https://img-blog.csdnimg.cn/img_convert/be59ca83d0f748dbb335b86393ca9f9e.png)
两个坐标系的转换:
参考线坐标系到Frenet坐标系
![](https://img-blog.csdnimg.cn/img_convert/0dcfce721a964bcb81b7a73f3b4ba27f.png)
Frenet坐标系到参考线坐标系
![](https://img-blog.csdnimg.cn/img_convert/34848211eb8642a5800f6ac2ac8e3c5f.png)
3.3 lattice采样
采样的基本类型
![](https://img-blog.csdnimg.cn/img_convert/80acdb77421b4413bc1c6cedd1db6042.png)
生成规划轨迹方程:s(t)与l(t)
![](https://img-blog.csdnimg.cn/img_convert/f9b87144d636403cbf38c381bda570a7.png)
具体运算过程:
(1)采样生成轨迹方程
初始状态与采样状态
纵向:
![](https://img-blog.csdnimg.cn/img_convert/9608915925204fe59cc9a7bfc77b7559.png)
![](https://img-blog.csdnimg.cn/img_convert/5ec9922cb28b476d88315db90b31d59b.png)
横向:
![](https://img-blog.csdnimg.cn/img_convert/1bec63c4ae12474087d3a65d0640047e.png)
(2)轨迹可行性评估
筛选一些不可行的轨迹
![](https://img-blog.csdnimg.cn/img_convert/e33b6082b74441c08d78b4cd80309189.png)
(3)计算损失(cost)并排序
![](https://img-blog.csdnimg.cn/img_convert/96a00fdc499843f29e8455714510b216.png)
纵向速度损失函数
![](https://img-blog.csdnimg.cn/img_convert/4abd5fa16f424a609e2a84e573e4a0a0.png)
舒适度损失函数:考察加速度变化率
![](https://img-blog.csdnimg.cn/img_convert/ad8406b6291e422d98e5614bfdfa4ec2.png)
向心加速度损失函数:目的是转弯和掉头时应该减速慢行
![](https://img-blog.csdnimg.cn/img_convert/abea20b91fcb478ab7f90fb6ccfd14ba.png)
碰撞损失函数
![](https://img-blog.csdnimg.cn/img_convert/9fd6760f670a4df593a98541b29ccefc.png)
横向偏移损失函数
![](https://img-blog.csdnimg.cn/img_convert/6ae36b0b636d48bf9bac0a62a6591dcb.png)
横向加速度损失函数
![](https://img-blog.csdnimg.cn/img_convert/d05bc574bd6b472b904fdde559f69c2d.png)
总结
![](https://img-blog.csdnimg.cn/img_convert/800b8bb3666947b0a42b747d84bdcbfc.png)