自动驾驶代客泊车AVP巡航规划详细设计

随着产品的升级迭代，对算法的泛化能力和指标都提出了新的需求。一方面后续需要低成本迁移不同停车场，可能面临更多的人流车流、更多的场景（如双车道、不同弯道、道路情况、不同车库层数等），因此对当前模块的工程通用性和算法的泛化能力都有更高的需求；另一方面，基于降成本以及后续性能及产品效果提升的需求，对当前算法的资源占用、效果指标等也也会有更的要求。

当前算法模块主要有以下问题。首先是算法架构的可扩展性存在较大的局限。当前算法主要基于matlab/simulink平台开发，没有通用的工程框架，添加新算法或者更新算法非常困难，而且适配其他模块及工具繁琐且工作量巨大、无法做深层次性能优化。其次是当前框架未能较好利用现有开源生态及借鉴别人的工作。Matlab是商业软件，其生态尤其自动驾驶方面的生态较apollo等开源生态有明显的差距，而且部分重要的组件（如图形计算等）无法进行代码生成，导致当前开发经常需要重复造轮子，工作处于一种闭门造车的状态，效率低下。最后是当前算法通用性不够强。当前算法对场景和逻辑等抽象不够，很多场景基于经验公式开发，对于特定的场景使用特定的策略，后续泛化困难。

当前设计的主要目标是借鉴apollo开源框架进行重构。一方面变更开发语言，使模块能够进行性能的深度调优，并有效地借鉴开源的成果提高效率；另一方面构建统一的工程框架，提升模块的工程通用性，以支撑后续的开发和算法升级；最后，通过将接口与算法与主流平台对齐，后续算法可以逐步更换以提高泛化能力和效果指标。

系统环境
1. 巡航规划与其它模块联系

巡航规划主要根据决策状态机、处理过的引导线、边界信息等及底盘的反馈信号，综合对不同场景（如直行、转弯、道闸口等）的处理、对直行及绕障等模式的处理、对路径和速度的生成和优化，最终输出一条轨迹参考线发给控制，进而决定整车运动路径及速度。

1. 巡航规划接口说明

根据规划的功能，其主要的输入信息应包含如下：

- 地图及定位信息（目前版本算法主要放在边界信息接口内）
- 感知障碍物信息（目前版本算法主要放在边界信息接口内）
- 全局导航信息（目前版本算法主要放到引导线信息接口内）
- 车身反馈信息（包含速度、加速度、档位等）
- 预测信息（后续能力提升及泛化需要）
- 交通规则信息（后续场景升级可能需要）

同时，规划的输出信息主要如下：

- 规划轨迹（包含路径、速度两大类及衍生信息如曲率、加速度等信息）
- 本模块的状态信息（主要是异常信息等，后续做产品化安全需要）

下面针对当前算法的接口进行具体说明：

A）输入接口

1、DEC //决策信息（包含状态机、引导线、边界信息）

2、SMC //车身反馈信息

2、PTP_trajectory_detail //规划的反馈信息（实际未使用）

3、PTP_out //旧的规划输出接口（实际未使用）

4、PTP_trajectory_head //规划的车头轨迹（实际未使用）

5、PTP_Inner_Monitor_out //规划内部信息监控（实际未使用）

规划模块设计
1. 巡航规划架构图

|---------------|
| |
| 巡航规划模块架构图 |

如图所示，巡航规划模块的整体架构包含三个部分，预处理、规划器（planner）、后处理，其中规划器为主体模块。规划器为各种具体的规划算法的总称。当前的算法主要基于曲率几何推导，因此把其命名为geometry planner。其余为业内主流的一些规划算法，包括lattice、EM、openspace、RRT、reinforce等。下面针对每个模块做概要说明：

预处理：对planner所需的数据进行处理，主要对输入的数据降噪、去毛刺、转换为模块所需要的数据格式及单位转换等。

Planner：根据地图定位信息、障碍物信息、决策状态机、车身反馈、导航信息等，进行综合考虑，同时结合如平稳、高效、资源占用少等指标，计算出一条优化的轨迹。考虑到模块的拓展性，会在当前算法的基础上根据场景的需求不断增加新算法，比如lattice、EM、openspace、RRT等，补充其能力。目前主要的算法为Geometry planner，主要原理如下：1、通过以当前位置，根据引导线跟随、绕行各障碍物的曲率结合人工驾驶经验，算出下一时刻的行驶曲率和速度。2、再根据下一时刻的曲率、速度等推导下一时刻的位置，并在此基础上重复计算驾驶行为。3、通过上述两个步骤的反复迭代，最终生成一条包含40个点的整体轨迹，并输出给下游。

后处理：对当前模块的状态进行监控、对算法生成的轨迹等进行评价，同时支持手动调式模式，可以手动生成指定的轨迹输出给下游进行调试。

1. 预处理（由框架处理）

滤波：针对底盘反馈的速度信号进行2阶滤波及限幅处理，使其降频到3hz左右（经验值），幅度限制在正负70以内，以免出现异常值。

信号转换：将方向盘转角转换为弧度表示，同时通过车速和方向盘转速双重验证，检测输入的方向盘信号是否有效。

1. Planner
  1. Geometry planner
    1. 环境信息更新

主要包含如下功能：

1、根据之前计算出来的控制量，更新下一时刻的车辆位置信息，供计算后续控制量使用。

2、计算目标线与主车的相对位置，相对角度，供后续计算引导线跟随使用。

3、根据算法估计的状态，更新每一个轨迹点上边界线，障碍物边界等与车的相对位置，供后续计算避障使用。

1. 1. 1. 1. 车辆位置推算

|--------------------------|
| ( x t , y t) |

|------------------------------|
| ( x t+1 , y t+1) |

|-----|
| x |

|-----|
| y |

计算在每个循环推算的时刻，车身的坐标及yaw角。是一种基于运动学的理想位姿迭代。计算关系如下式：

x t+1 = x t + v x *dt* cosyawrate*dt

θ t+1 = θ t +yawrate*dt

其中：v x

为车辆x 方向速度，v y

为车辆y 方向速度，dt 为时间间隔，yawrate为角速度。

在实际计算过程中，考虑了感知的延时和系统的延时对车辆姿态的影响，因此分别对两种延时进行估计，根据估计参数进行系统姿态延时估算，并将其作为当前初始值。

另外，在车道线跟随的时候，有的时候可能并不需要车后轴中心在中心线上，尤其是转弯的时候，因此添加对原点位置的平移，使其更符合人的驾驶习惯。

通过每个时刻计算得到的纵向速度、横向速度、角速度，由上式计算下一个时刻的位置。通过反复计算40次，得到40个时刻的车辆位置，输出到下游。

自动驾驶代客泊车AVP巡航规划详细设计

目 录

目录