目 录
??????? 略
???? 表2.1 名词解释
| 名词 | 定义 |
本方案是在动态窗口法基础上，着眼于四轮车的高速道路，低速道路，以及泊车场景，演进而来的算法，简称M-DWA（Multiply step Dynamic Window Approach）。
总体模块划分为：决策模块，路径规划模块，LLC（Low level control）三大部分。
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的计算量。
由总体设计可知，决策模块根据上游算法给出的环境信息，生成不同的决策场景，每一个决策场景包括一个目标，多个避障边界，同时赋予每一个避障边界一个方向，以保证每一个决策场景只有一个局部最优解；
行驶目标，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表示安全距离。
EU模块的功能有两个：
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.
PNC的核心算法为目标车道的巡航算法，目标车道包括方位信息（x，y, ψ），x表示与目标车道线的横向距离，y表示在纵向距离多远处，车道保持状态OK，ψ表示目标车道与车辆的夹角。
目标车道线巡航算法有两种情况，一个是弯道巡航，一个是直线巡航。弯道巡航是直线巡航的特殊形式，差别只在于在直线巡航上累加一个弯道曲率的控制量。
最优解参数获取模块的目的是获取最优螺旋线的曲率，以供螺旋线算法计算当前位置的路径曲率。
输入接口：
| 接口名称 | 接口说明 |
| 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-x2R21-cosψR2+2Rx*cosψ+x-R*sinψ-x2-y2=0
则可通过以上公式估计最优螺旋线的曲率半径。
纵向控制系统ACC提供了自动驾驶过程中车辆的纵向规划策略。
ACC是按照跟车模式设计的控制律。跟车即本车以一定的安全距离跟随前车行进。当前方没有车辆时，假设距本车足够远处有一辆速度很大的虚拟车辆，本车为跟随前车，将加速直至达到速度上限追赶虚拟车辆，即进入定速巡航模式。根据以上策略，可将跟车模式扩展为囊括所有纵向控制环境的控制策略，有车时保持安全距离跟车，无车时设定速度上限，以定速巡航的模式追赶前车。
ACC控制策略
| 有目标车辆 | 以安全距离跟车 | 跟车模式 |
| 无目标车辆 | 假设前车很远，追车 | 定速巡航 |
为了跟车时保持安全距离，ACC首先利用位置控制完成跟车目标。
本车为保持安全车距所需要调整的距离S可由车距和前车车速求得。
s=Ldis-τ*vb
Ldis-为与前车的距离，Vb为前车车速，
?为安全时距，一般在1.5s至2.2s之间。
由S求出当前时刻的速度指令Vstp。再根据Vstp求出加速度指令。
vstp=2ass
astp_s=vstp-vxk
as为根据舒适性和操控性设定的加速度，Vx-为本车车速，K为求取加速度的比例系数。
由关系式可知，K值越小，相同速度差得到的加速度越大，跟车反应速度越快，但是基于位置环的控制律响应速度慢，K值较小时，车处于安全距离附近会造成来回震荡这一缺点，严重影响安全。因此，控制系统引入速度环的控制，来弥补这一缺点。速度环的引入能加快控制律的响应速度，可有效抑制车在安全距离时的震荡。
速度环控制直接利用本车与前车的相对速度变化，得出加速度指令。
astp_v=vb-vxk
Vb为前车车速。将速度环与位置环得到的加速度进融合得到astp_0
，当本车离安全车距较远时，位置环控制的权重大；当本车距安全距离越小时，速度环的权重越大。
采用速度环与位置环控制律的目标是在保持良好的体感状态下，完成跟车控制。然而，在某些危险情况下，需要紧急刹车以保证安全，舒适性加速度as不能满足安全性要求。为此，控制律引入了可保证行车安全的加速度，当此加速度astp_1小于舒适性加速度as的最小值时，判定情况较危险，采用astp_1进行刹车，当astp_1在as范围内时，采用astp_0
。采用的加速度定义为astp_safe
astp_1=-?v22s
由于上述公式中距离变化s处于分母，当s接近于0时，较大的速度差和较小的s会产生过大的加速度，即在达到安全距离前再猛踩一脚刹车，严重影响体感，如图1所示。
图1 ?S接近0时产生的加速度突变
因此，引入了新的距离|sdanger|>|s|，求得新的加速度，在到达安全距离前替换这一过大的加速度。
sdanger=Ldis-τdanger*vb
astp_danger=vb-vxk-?v22sdanger
s接近于0时，Sdanger还相对较大，astp_danger
产生的大减速将晚于astp_safe
，当astp_safe
出现突变时，使用astp_danger
的值进行控制，屏蔽在达到安全距离时astp_safe
产生的刹车，提高体感。
图2 安全距离附近astp_safe
与astp_danger
对比
最后对加速度指令进行软化处理，防止每一步加速度的跳变过于激烈，由此得到最终的加速度指令astp