?????? 随着产品的升级迭代，对算法的泛化能力和指标都提出了新的需求。一方面后续需要低成本迁移不同停车场，可能面临更多的人流车流、更多的场景（如双车道、不同弯道、道路情况、不同车库层数等），因此对当前模块的工程通用性和算法的泛化能力都有更高的需求；另一方面，基于降成本以及后续性能及产品效果提升的需求，对当前算法的资源占用、效果指标等也也会有更的要求。
?????? 当前算法模块主要有以下问题。首先是算法架构的可扩展性存在较大的局限。当前算法主要基于matlab/simulink平台开发，没有通用的工程框架，添加新算法或者更新算法非常困难，而且适配其他模块及工具繁琐且工作量巨大、无法做深层次性能优化。其次是当前框架未能较好利用现有开源生态及借鉴别人的工作。Matlab是商业软件，其生态尤其自动驾驶方面的生态较apollo等开源生态有明显的差距，而且部分重要的组件（如图形计算等）无法进行代码生成，导致当前开发经常需要重复造轮子，工作处于一种闭门造车的状态，效率低下。最后是当前算法通用性不够强。当前算法对场景和逻辑等抽象不够，很多场景基于经验公式开发，对于特定的场景使用特定的策略，后续泛化困难。
?????? 当前设计的主要目标是借鉴apollo开源框架进行重构。一方面变更开发语言，使模块能够进行性能的深度调优，并有效地借鉴开源的成果提高效率；另一方面构建统一的工程框架，提升模块的工程通用性，以支撑后续的开发和算法升级；最后，通过将接口与算法与主流平台对齐，后续算法可以逐步更换以提高泛化能力和效果指标。
巡航规划主要根据决策状态机、处理过的引导线、边界信息等及底盘的反馈信号，综合对不同场景（如直行、转弯、道闸口等）的处理、对直行及绕障等模式的处理、对路径和速度的生成和优化，最终输出一条轨迹参考线发给控制，进而决定整车运动路径及速度。
根据规划的功能，其主要的输入信息应包含如下：
同时，规划的输出信息主要如下：
下面针对当前算法的接口进行具体说明：
?????? A）输入接口
????? ????? 1、DEC? //决策信息（包含状态机、引导线、边界信息）
2、SMC?? //车身反馈信息
2、PTP_trajectory_detail //规划的反馈信息（实际未使用）
3、PTP_out? ? //旧的规划输出接口（实际未使用）
4、PTP_trajectory_head?? //规划的车头轨迹（实际未使用）
5、PTP_Inner_Monitor_out???? ?? //规划内部信息监控（实际未使用）
| 巡航规划模块架构图 |
如图所示，巡航规划模块的整体架构包含三个部分，预处理、规划器（planner）、后处理，其中规划器为主体模块。规划器为各种具体的规划算法的总称。当前的算法主要基于曲率几何推导，因此把其命名为geometry planner。其余为业内主流的一些规划算法，包括lattice、EM、openspace、RRT、reinforce等。下面针对每个模块做概要说明：
预处理：对planner所需的数据进行处理，主要对输入的数据降噪、去毛刺、转换为模块所需要的数据格式及单位转换等。
?????? Planner：根据地图定位信息、障碍物信息、决策状态机、车身反馈、导航信息等，进行综合考虑，同时结合如平稳、高效、资源占用少等指标，计算出一条优化的轨迹。考虑到模块的拓展性，会在当前算法的基础上根据场景的需求不断增加新算法，比如lattice、EM、openspace、RRT等，补充其能力。目前主要的算法为Geometry planner，主要原理如下：1、通过以当前位置，根据引导线跟随、绕行各障碍物的曲率结合人工驾驶经验，算出下一时刻的行驶曲率和速度。2、再根据下一时刻的曲率、速度等推导下一时刻的位置，并在此基础上重复计算驾驶行为。3、通过上述两个步骤的反复迭代，最终生成一条包含40个点的整体轨迹，并输出给下游。
?????? 后处理：对当前模块的状态进行监控、对算法生成的轨迹等进行评价，同时支持手动调式模式，可以手动生成指定的轨迹输出给下游进行调试。
滤波：针对底盘反馈的速度信号进行2阶滤波及限幅处理，使其降频到3hz左右（经验值），幅度限制在正负70以内，以免出现异常值。
信号转换：将方向盘转角转换为弧度表示，同时通过车速和方向盘转速双重验证，检测输入的方向盘信号是否有效。
主要包含如下功能：
1、根据之前计算出来的控制量，更新下一时刻的车辆位置信息，供计算后续控制量使用。
2、计算目标线与主车的相对位置，相对角度，供后续计算引导线跟随使用。
3、根据算法估计的状态，更新每一个轨迹点上边界线，障碍物边界等与车的相对位置，供后续计算避障使用。
|
(xt,yt) |
|
(xt+1,yt+1) |
|
x |
|
y |
计算在每个循环推算的时刻，车身的坐标及yaw角。是一种基于运动学的理想位姿迭代。计算关系如下式：
???????????????????????? xt+1=xt+vx*dt*cosyawrate*dt
???????????????????????? θt+1=θt+yawrate*dt
其中：vx
为车辆x方向速度，vy
为车辆y方向速度，dt为时间间隔，yawrate为角速度。
??????? 在实际计算过程中，考虑了感知的延时和系统的延时对车辆姿态的影响，因此分别对两种延时进行估计，根据估计参数进行系统姿态延时估算，并将其作为当前初始值。
??????? 另外，在车道线跟随的时候，有的时候可能并不需要车后轴中心在中心线上，尤其是转弯的时候，因此添加对原点位置的平移，使其更符合人的驾驶习惯。
??????? 通过每个时刻计算得到的纵向速度、横向速度、角速度，由上式计算下一个时刻的位置。通过反复计算40次，得到40个时刻的车辆位置，输出到下游。