自动驾驶代客泊车AVP路径规划详细设计

目 录

[1 背景... 1](#1 背景... 1)

[2 名词解释... 2](#2 名词解释... 2)

[3 路径规划及控制总体设计（SMPC）... 3](#3 路径规划及控制总体设计（SMPC）... 3)

[4 路径规划及控制详细设计... 4](#4 路径规划及控制详细设计... 4)

[4.1 决策场景... 4](#4.1 决策场景... 4)

[4.2 接口定义... 4](#4.2 接口定义... 4)

[4.2.1 目标车道线的巡航场景... 4](#4.2.1 目标车道线的巡航场景... 4)

[4.2.2 目标车道线与动态障碍物的混合场景... 4](#4.2.2 目标车道线与动态障碍物的混合场景... 4)

[4.2.3 垂直泊车场景... 4](#4.2.3 垂直泊车场景... 4)

[4.2.4 水平泊车场景... 6](#4.2.4 水平泊车场景... 6)

[4.2.5 倾斜车位泊车场景... 6](#4.2.5 倾斜车位泊车场景... 6)

[4.2.6 有可行使区域的目标车道线巡航场景... 6](#4.2.6 有可行使区域的目标车道线巡航场景... 6)

[4.3 决策与PNC接口... 6](#4.3 决策与PNC接口... 6)

[4.4 规划算法模块设计... 6](#4.4 规划算法模块设计... 6)

[4.4.1 目标车道巡航算法概述... 6](#4.4.1 目标车道巡航算法概述... 6)

[4.4.2 螺旋线规划算法... 8](#4.4.2 螺旋线规划算法... 8)

[4.4.3 最优解参数估计... 8](#4.4.3 最优解参数估计... 8)

[4.5 纵向控制ACC.. 9](#4.5 纵向控制ACC.. 9)

[4.5.1 ACC控制系统基本流程... 10](#4.5.1 ACC控制系统基本流程... 10)

[4.5.2 舒适性控制（该部分为废弃方案）... 12](#4.5.2 舒适性控制（该部分为废弃方案）... 12)

[4.5.3 障碍物置信度的要求... 13](#4.5.3 障碍物置信度的要求... 13)

[4.6 横向控制... 13](#4.6 横向控制... 13)

[4.6.1 Yawrate闭环... 13](#4.6.1 Yawrate闭环... 13)

[4.6.2 车辆模型... 14](#4.6.2 车辆模型... 14)

[5 故障诊断... 17](#5 故障诊断... 17)

[6 输入输出接口... 18](#6 输入输出接口... 18)

[6.1 输入接口... 18](#6.1 输入接口... 18)

[6.1.1 车身信息接口... 18](#6.1.1 车身信息接口... 18)

[6.1.2 决策模块接口... 20](#6.1.2 决策模块接口... 20)

[6.2 输出接口... 23](#6.2 输出接口... 23)

[6.2.1 输出接口方式1. 23](#6.2.1 输出接口方式1. 23)

[6.2.2 输出接口方式2. 23](#6.2.2 输出接口方式2. 23)

[6.2.3 输出接口方式3. 25](#6.2.3 输出接口方式3. 25)

1. 背景

略

1. 名词解释

表2.1 名词解释

|----|----|
| 名词 | 定义 |
| | |
| | |
| | |
| | |
| | |
| | |
| | |

1. 路径规划及控制总体设计（M-DWA）

本方案是在动态窗口法基础上，着眼于四轮车的高速道路，低速道路，以及泊车场景，演进而来的算法，简称M-DWA（Multiply step Dynamic Window Approach）。

总体模块划分为：决策模块，路径规划模块，LLC（Low level control）三大部分。

1. 决策模块：决策模块有三大功能。1，根据上游算法给出的环境信息，将整个规划模块切分成不同的决策场景，每一个决策场景包括一个目标，多个避障边界，同时赋予每一个避障边界一个方向，以保证每一个决策场景只有一个局部最优解；2，决策模块同时负责法律，道德，油耗，驾驶员偏好，交通规则......，等非避障的策略权衡；3，决策模块需要根据路径规划出来的最优路径的状态，来维护状态机，最基本的功能是泊车入库中的入库准备，泊车，揉库的状态。
2. 路径规划模块：对每一个决策场景，利用最优状态逼近控制算法（OSAC），预测未来每一个时间点的轨迹。
3. LLC模块(Low level control)模块：LLC有两个功能，1，加速度闭环，以响应路径规划的目标速度，以及目标加速度。2，Yawrate闭环，实时调整车辆姿态，以响应路径规划的目标yawrate值。

M-DWA的特性：

（1）在求取轨迹的方式上，是M-DWA本质不同于DWA算法的地方。DWA在计算轨迹的过程中，控制量是不变的，因此轨迹表现上是一个标准的圆弧或者直线；而多步动态窗口法M-DWA，则在每一个轨迹点上预测环境信息的变化，在每个点上都通过控制率（OSAC）来更新控制量。这使得M-DWA算得的路径是更接近未来真实行驶的轨迹。

（2）M-DWA的控制量为Yawrate，以及Ax，然后由yawrate闭环和加速度闭环翻译成方向盘，油门，制动，档位信息。这种方法使决策和路径规划与控制目标车辆的性能解耦，因此决策和路径规划算法的参数调试与车型无关，实车或者仿真调试好的决策、路径规划参数可以适应于不同车辆类型。

（3）M-DWA算法无需像DWA算法那样生成很多条轨迹，M-DWA算法的每一条轨迹是特定决策场景下由OSAC算法计算出来的最优轨迹。因此M-DWA算法得轨迹数量是由决策场景来决定的，一般而言，决策模块难以决定的决策数量只有2到3个。这有利于减少M-DWA的计算量。

1. 路径规划及控制详细设计
   1. 决策场景

由总体设计可知，决策模块根据上游算法给出的环境信息，生成不同的决策场景，每一个决策场景包括一个目标，多个避障边界，同时赋予每一个避障边界一个方向，以保证每一个决策场景只有一个局部最优解；

1. 1. 1. 目标车道线的巡航场景
      2. 目标车道线与动态障碍物的混合场景
      3. 垂直泊车场景
      4. 水平泊车场景
      5. 倾斜车位泊车场景
      6. 有可行使区域的目标车道线巡航场景
   2. 决策算法模块设计
      1. 决策模块输出接口定义

行驶目标，STG_Objective。

决策的目标，既可能是车道中心线，亦可能是待入库的车位中心线。STG_TargetLine中包括速度限制信息Speed_Limit，用来定义弯道降速，定点停车，以及泊车终止点；stoppoint_error用来定义车辆停止点的停止误差（高精度的停止误差需要加速度闭环进行"蹭车"控制）,如无误差要求，则为999；target_line指的是车位的目标中线，也可以指巡航的车道中心线，当为车位的目标中线时，只有两个点是有效的，此时index_start=0，index_end=1；drive_direction用来指示行车方向是前进还是倒车。

避障边界，STG_Boundary.

决策的避障边界，既可能是车道的边线（线段可延长），也可能是几个点组成的墙体（不可延长线），亦可能是单个点指示的障碍物。

boundary,表示的是避障的边界；boundary_type表示避障的类型，有三种避障类型，分别为左边超越，右边超越，后面跟随；distance_danger表示危险距离，distance_safe表示安全距离。

1. 1. 规划算法模块设计
      1. Environment update(EU)

EU模块的功能有两个：

1. 计算目标线、避障边界与车的相对位置，相对角度。以供OSAC算法使用。
2. 根据OSAC算法估计的轨迹，更新每一个轨迹点上目标线、避障边界与车的相对位置，相对角度。

EU模块的输入数据来自决策模块（STG）,输出接口定义与决策模块的输出接口类似，同样分为目标，和避障边界，它是在决策模块接口的基础上添加了与车的相对位置，相对角度信息。接口如下：

行驶目标，EU_Objective。

接口解析如下：

|-------------------|---------------------------|
| 接口名称 | 接口说明 |
| yaw_line2car | 目标线相对于车身方向的夹角。即下图中的ψ |
| distance_line2car | 目标线相对于车身方向的位置。即下图中的x |
| distance_y | 在纵向距离多远处，车道保持状态OK。即下图解说的y |
| r_reverse_line | 目标线的曲率半径 |
| drive_direction | 前进挡模式，还是倒车模式 |

方向定义，遵守向左为正，向右为负。

如下图，目标车道线用箭头表示，箭头朝向表示巡航的目标方向。

则在这种场景下：

（1）车No1：车道线相对车的位置在左，x>0；车道线相对车的夹角朝右，ψ<0；如果不限制什么时候达到lanekeep目的，则Y=999。

(2) 车No2: 车道线相对车的位置在左，x>0；车道线相对车的夹角朝左，ψ>0；如果不限制什么时候达到lanekeep目的，则Y=999。

（3）车No3: 车道线相对车的位置在左(注意目标车道线是有方向的，若目标方向箭头朝下则位置在右)，x>0；车道线相对车的夹角朝左，ψ>0，并且ψ>pi/2；如果不限制什么时候达到lanekeep目的，则Y=999。

关于泊车场景，是将目标车位的中线看成为目标车道线，方向定义与上述相同。泊车场景如下图：

（4）车No4: 车道线相对车的位置在左(注意目标车道线是有方向的，若目标方向箭头朝下则位置在右)，x>0；车道线相对车的夹角朝左，ψ>0；限制车在到达车位下边界前达到lanekeep目的，因此有Y=-5。

避障边界，EU_Boundary.

1. 1. 1. 最优状态逼近控制
         1. 垂直泊车场景
2. 1. 1. 目标车道巡航算法概述

PNC的核心算法为目标车道的巡航算法，目标车道包括方位信息（x，y, ψ），x表示与目标车道线的横向距离，y表示在纵向距离多远处，车道保持状态OK，ψ表示目标车道与车辆的夹角。

目标车道线巡航算法有两种情况，一个是弯道巡航，一个是直线巡航。弯道巡航是直线巡航的特殊形式，差别只在于在直线巡航上累加一个弯道曲率的控制量。

1. 1. 1. 螺旋线规划算法
      2. 最优解参数估计

最优解参数获取模块的目的是获取最优螺旋线的曲率，以供螺旋线算法计算当前位置的路径曲率。

输入接口：

|----------------------|--------------------------------|
| 接口名称 | 接口说明 |
| Final_point_distance | 在纵向距离多远处，车道保持状态OK。即目标车道线的方位信息y |
| Yaw_Lane2Car | 目标车道相对于车身方向的夹角。即目标车道先的方位信息ψ。 |
| Distance_Lane2Car | 目标车道相对于车身方向的位置。即目标车道线的方位信息x |
| V_ms | 当前车速 |
| Vehicle_direction | 目标车道巡航是前进挡模式，还是倒车模式 |

输出接口：

|---------------|--------------|
| 接口名称 | 接口说明 |
| R_Reverse_Req | 最优解螺旋线的曲率。 |
| CurveSpd_Req | 最优解螺旋线的曲率变化率 |

算法设计如下：

根据几何图形的运算，R与x,y, ψ，和θ满足一下关系式。其中θ表示的是车辆在左转和右转中间临界状态时，车辆方向和预描点切线方向的夹角。

R *sin ψ +y =2R *sin θ R +Rcos ψ -x =2R *cos θ

化简可得如下方程式：

cosθ = Rcosψ +R -x 2R 21-cos ψ R 2+2R x *cos ψ +x -R *sin ψ -x 2-y2=0

则可通过以上公式估计最优螺旋线的曲率半径。

1. 1. 纵向控制ACC

纵向控制系统ACC提供了自动驾驶过程中车辆的纵向规划策略。

ACC是按照跟车模式设计的控制律。跟车即本车以一定的安全距离跟随前车行进。当前方没有车辆时，假设距本车足够远处有一辆速度很大的虚拟车辆，本车为跟随前车，将加速直至达到速度上限追赶虚拟车辆，即进入定速巡航模式。根据以上策略，可将跟车模式扩展为囊括所有纵向控制环境的控制策略，有车时保持安全距离跟车，无车时设定速度上限，以定速巡航的模式追赶前车。

ACC控制策略

|-------|-----------|------|
| 有目标车辆 | 以安全距离跟车 | 跟车模式 |
| 无目标车辆 | 假设前车很远，追车 | 定速巡航 |

1. 1. 1. ACC控制系统基本流程

为了跟车时保持安全距离，ACC首先利用位置控制完成跟车目标。

本车为保持安全车距所需要调整的距离S可由车距和前车车速求得。

s =L dis - τ*v b

Ldis­为与前车的距离，Vb为前车车速，

为安全时距，一般在1.5s至2.2s之间。

由S求出当前时刻的速度指令Vstp。再根据Vstp求出加速度指令。

v stp = 2 a s s

a stp_s = v stp - v x k

as为根据舒适性和操控性设定的加速度，Vx­为本车车速，K为求取加速度的比例系数。

由关系式可知，K值越小，相同速度差得到的加速度越大，跟车反应速度越快，但是基于位置环的控制律响应速度慢，K值较小时，车处于安全距离附近会造成来回震荡这一缺点，严重影响安全。因此，控制系统引入速度环的控制，来弥补这一缺点。速度环的引入能加快控制律的响应速度，可有效抑制车在安全距离时的震荡。

速度环控制直接利用本车与前车的相对速度变化，得出加速度指令。

a stp_v = v b - v x k

Vb为前车车速。将速度环与位置环得到的加速度进融合得到a stp_0

，当本车离安全车距较远时，位置环控制的权重大；当本车距安全距离越小时，速度环的权重越大。

采用速度环与位置环控制律的目标是在保持良好的体感状态下，完成跟车控制。然而，在某些危险情况下，需要紧急刹车以保证安全，舒适性加速度as不能满足安全性要求。为此，控制律引入了可保证行车安全的加速度，当此加速度astp_1小于舒适性加速度as的最小值时，判定情况较危险，采用astp_1进行刹车，当astp_1在as范围内时，采用a stp_0

。采用的加速度定义为a stp_safe

a stp_1 =-∆v 2 2s

由于上述公式中距离变化s处于分母，当s接近于0时，较大的速度差和较小的s会产生过大的加速度，即在达到安全距离前再猛踩一脚刹车，严重影响体感，如图1所示。

图1 S接近0时产生的加速度突变

因此，引入了新的距离|sdanger|>|s|，求得新的加速度，在到达安全距离前替换这一过大的加速度。

s danger = L dis - τ danger *v b

a stp_danger = v b - v x k -∆v 2 2 s danger

s接近于0时，Sdanger还相对较大，a stp_danger

产生的大减速将晚于a stp_safe

，当a stp_safe

出现突变时，使用a stp_danger

的值进行控制，屏蔽在达到安全距离时a stp_safe

产生的刹车，提高体感。

图2 安全距离附近a stp_safe

与a stp_danger

对比

最后对加速度指令进行软化处理，防止每一步加速度的跳变过于激烈，由此得到最终的加速度指令a stp