Open Space - 自动驾驶开放空间规划
附赠自动驾驶最全的学习资料和量产经验以及100T的资源分享:链接
1. 总体框架
1.1. 适用场景
对于没有参考线的非结构化场景, 普通的规划算法难以进行处理, 需要在开放空间(Open Space)进行路径规划.
典型的场景如下:
-
自主泊车场景(Valet): 将车停在指定的停车位上.
-
路边停车场景(Pull Over): 到达终点时需要靠边停车从而结束规划.
1.2. 算法流程图
Open Space的算法流程图如下[1]:
Open Space的算法流程图
从流程图中可以看到, Open Space的算法的输入主要有两类:
-
感知数据, 主要是障碍物的数据(位置 / 速度等信息).
-
感兴趣区域ROI(Region of Interest), ROI蕴含了地图的信息.
简单的说, Open Space算法分为以下几个步骤:
- 根据道路边界, 车位边界生成可行驶区域ROI, 如下图中的红实线所示.
可行驶区域ROI
2. Hybird A* + RS曲线规划出一条粗糙的轨迹, 求出一个没有碰撞的参考解. 这条粗糙的轨迹可能会存在曲率突变, 这是不满足车辆的运动学要求的.
Hybird A * + RS
3. IAPS或者OBCA算法对轨迹进行进一步的平滑以满足车辆控制的需求. 图示中, 红色虚线是粗糙轨迹, 绿色虚线是平滑后的轨迹
轨迹平滑
4. 对开放空间轨迹提供任务得到的拼接路径按照前进档或者倒退档进行划分和优化.
2. Hybird A*算法
开放空间的路径规划是一个复杂度较高的非凸问题, 搜索是相对而言比较高效的方案. 混合A*算法的作用是在开放空间中生成一个粗糙的轨迹[2].
2.1. A*算法
A*算法是一种基于搜索的路径规划算法, 在机器人领域有着广泛应用.
A*算法将可行驶空间进行栅格化, 按照固定距离间隔采样节点. 通过迭代遍历周围8个方向的方法找到一条从起点到终点避开障碍物的路径.
A*算法
A*算法的伪代码如下:
* 初始化open_set和close_set
* 将起点加入open_set中, 并设置优先级为0(最高优先级)
* 如果open_set不为空, 则从open_set中选取优先级最高的节点n
* 如果节点n为终点, 则:
* 从终点开始逐步追踪parent节点, 一直达到起点
* 返回找到的结果路径, 算法结束
* 如果节点n不是终点, 则
* 将节点n从open_set中删除, 并加入close_set中
* 遍历节点n所有的邻近节点:
* 如果邻近节点m在close_set中, 则:
* 跳过, 选取下一个邻近节点
* 如果邻近节点m也不在open_set中, 则:
* 设置节点m的parent为节点n
* 计算节点m的优先级
* 将节点m加入open_set中
2.2. 混合A*算法
A*算法产生的路径都是折线, 不满足车辆运动学, 实际车辆无法走出这样的路径. 所以需要使用混合A*算法进行路径规划.
把车辆的运动路径等效为后轴中心点的不同转弯半径的弧线. 通过车辆的前轮转角可以算出后轴中心的转弯半径. 对前轮转角进行采样, 不同的前轮转角对应不同的转弯半径. 每次向前行驶一个固定的弧长. 如下图左侧所示, 其路径是由不同半径的弧组成的.
混合A*
2.3. ReedShepp曲线
搜索拓展路径的节点动作是离散的(以固定的长度, 不同的转弯半径进行节点的拓展), 而状态空间是连续的. 有可能永远也搜索不同到终点的状态, 所以我们还需要节点的解析拓展.
1990年, Reed和Shepp提出了从起点到终点, 可以通过固定半径的圆弧和直线连接起来的方法[4].
图2.1 一个运动序列例子
图2.2中, 展示了Hybrid A* + ReedShepp的规划效果[6]. 其中黑色框代表障碍物, 粉色点表示通过混合A*算法搜索得到的点. 后半段没有进行混合A*搜索, 是通过RS曲线解析算出的路径.
图2.2 Hybrid A* + ReedSheep的效果
3. OBCA算法
ReedShepp是没有办法考虑障碍物的, 所以运用到混合A*算法还需要加上碰撞检测. 把和障碍物发生碰撞的路径过滤掉. 图2.2中得到的RS曲线虽然满足车辆的动力学约束, 但是都是由弧线和直线组成的. 在弧线和直线的衔接位置, 都有曲线的突变. 所以这样的路径还需要进一步进行平滑处理. 来满足控制和乘坐舒适性的要求.
OBCA算法基于模型预测控制(MPC)建立模型, 并用优化算法进行求解. 它有如下几点优势:
-
可以加入障碍物约束. 这一点和RS曲线的解析求解不同, RS虽然可以求出最优的解析解, 但是没有办法考虑障碍物的碰撞的约束.
-
可以产生满足车辆运动学约束(曲率连续变化)的轨迹.
-
可以实现横纵向的联合规划(同时考虑速度对路径的影响).
Apollo在原始的H-OBCA算法[7]的基础上进行改进, 形成TDO-OBCA算法[8].
3.1. MPC预测模型
图3.1展示了模型预测控制MPC[9]:
图3.1 MPC
模型预测控制会通过一个采样时间将未来时域离散成多段, 在给定控制模型和参考值曲线, 计算使预测输出与参考输出最接近的输入序列, 并将输入的第一个分量作用于系统.
MPC问题最终也是转化成一个优化问题. 和其他的优化问题一样, 首先设计目标函数(约束). 对未来状态进行优化求解. 所以需要设计MPC的预测模型. 车辆运动学模型的示意图如图3.2所示[10].
图3.2 车辆运动学模型