Apollo代码学习(一)—控制模块概述

Apollo(阿波罗)是一个开放的、完整的、安全的自动驾驶平台，以灵活和高性能的技术架构，为全自动驾驶提供支持。

---

补充

2018.11.08更新

仿真平台的搭建请参考我的另一篇博文：
Apollo代码学习(四)—Windows下编译Apollo并与Carsim和Simulink联调
需要了解车辆运动学模型和动力学模型，可以参考我的另两篇博文：
【1】 Apollo代码学习(二)—车辆运动学模型
【2】 Apollo代码学习(三)—车辆动力学模型
横纵向控制详解可看：
Apollo代码学习(五)—横纵向控制

2018.11.15更新

对标定表的生成进行了补充。

2018.11.20更新

MPC解析可看：
Apollo代码学习(六)—模型预测控制(MPC)

前言

本人出于工作需要，对Apollo的Control模块及其相关的部分代码进行了解析。Apollo的控制逻辑主要源于此书：Vehicle Dynamics and Control，因此，研究Apollo控制代码前请先准备一下这本书，结合此书去理解Control模块的相关代码会事半功倍。此外，还需要对Frenet坐标系有一定的了解，可参考这篇文章:Optimal trajectory generation for dynamic street scenarios in a Frenét Frame。

提倡大家支持正版资源，本人提供文档仅限交流学习使用，侵删：
【1】Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. | CSDN资源
【2】Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame | CSDN资源

控制

研究控制前，先了解一下Apollo项目的整体结构，如下图所示，它包含了感知、定位、决策、控制、通信等几大模块。

其中，Apollo Control模块提供了三种控制方法：纵向控制、横向控制和MPC控制。本文主要介绍纵向控制和横向控制，横纵向控制详解请见我的另一篇博文：Apollo代码学习(五)—横纵向控制。

纵向控制

纵向控制主要通过控制汽车的刹车/油门来实现对汽车速度的控制。它主要由一个级联控制器和一个标定表构成。
级联控制器由位置PID闭环控制器和速度PID闭环控制器构成；
标定表是指速度-加速度-刹车/油门命令标定表。
计算纵向控制命令的接口：

Status LonController::ComputeControlCommand(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis,
    const planning::ADCTrajectory *planning_published_trajectory,
    control::ControlCommand *cmd)

输入为：定位信息（localization）、自车底盘信息（chassis）、规划信息（planning_published_trajectory）
输出为：油门/刹车命令（cmd）
Apollo纵向控制的工作原理框图如下所示：

位置PID闭环控制器

模块	变量
输入	期望位置+当前实际位置
输出	速度补偿量

速度PID闭环控制器

模块	变量
输入	速度补偿+当前位置速度偏差
输出	加速度补偿量

速度-加速度-刹车/油门命令标定表

模块	变量
输入	加速度补偿+规划加速度，车速
输出	油门/刹车控制量

纵向控制中理解起来较为复杂的是纵向误差的计算：

void LonController::ComputeLongitudinalErrors(
	const TrajectoryAnalyzer *trajectory_analyzer, const double preview_time,
	SimpleLongitudinalDebug *debug) 

涉及到Frenet坐标系¹

标定表的生成

标定表的更新请参考文档：

Apollo/docs/howto/how_to_update_vehicle_calibration.md

标定流程大致如下图所示：

其中，命令集可自定义。因为本人是在Windows下利用carsim和matlab进行仿真的，所以采集数据的过程和文档有出入，但处理和转化过程相同。
由于我搭建的是基于泊车场景的模型，对车速要求较低，因此带速下的车速完全够用，再适当加一点刹车即可，因此标定的时候没有标定油门值，标定流程大致如下：
1.首先，搭建CarSim和Smulink模型，CarSim中让汽车在带速下走直线，同时给出一定的刹车值，使车辆模型伴随一定的刹车值(0~1)进行带速行驶，判断哪些刹车值状态下，车辆可以进行加速运动；然后让车加速到带速状态下最大速度，给刹车值，测试多大刹车值下可使车辆在最高速情况下停止；
对于我来说，我测定并需要标定的值如下（m文件）：

% 倒车刹车标定。车辆靠带速行驶，通过控制刹车压力，控制车辆加/减速
% 加速阶段刹车压力范围：0:0.05:0.6    acc 
% 减速阶段刹车压力范围：0.6:0.05:1.6  dec 
% 一个加速度值对应多个减速度值，共312组数据
if(brakeflag==0)
    k=1;
    acc=0:0.05:0.6;
    dec=0.6:0.05:1.6;
    
    rows=size(acc,2);
    nols=size(dec,2);    
    des_brake=cell(1,rows*nols);
    for i=1:1:rows
        for j=1:1:nols
            des_brake{k}=[acc(i),dec(j)];
            k=k+1;
        end
    end   
end

2.确定标定序列后（采样间隔自定，因为后期是后标定表时候需要插值计算，间隔越小相对来说插值越准确），按照一定间隔（我设置的采样周期为20s，加速12s，减速8s）在Simulink中采集CarSim传输过来的数据（主要为时间、速度、加速度）并按照Apollo采集数据的格式存入.csv文件中；
我的采样时间定义如下：

% 0s，将trajectory、count_num、saveFileflag、speedflag重置
% 0~2s，静止状态，刹车压力为2
% 3~12s，加速阶段，刹车压力：0~0.6
% 12~20s，减速阶段，刹车压力：0.6~1.6，当18s时存文件
% >20s，开始下一个文件采样

是按照Apollo的采样样式定义的，以标定工具中的文件t20b13.txt为例

a: gear_location = chassis_pb2.Chassis.GEAR_DRIVE //档位状态
a: brake = 40.0 //刹车百分比
a: steering_target = 0.0 //方向盘角度
a: throttle = 0.0 //油门百分比
c: vehicle_speed == 0.0 //车速
t: 2.0 //启动时间，静止2s后启动
a: brake = 0.0 //刹车百分比
a: throttle = 20.0 //油门百分比，开始加速
c: vehicle_speed >= 10.0 //判断车速，若车速达到10km/h，停止加速
a: throttle = 0.0 //油门百分比
t: 1.0 //匀速行驶1s
a: brake = 13.0 //刹车百分比，然后开始减速
c: vehicle_speed == 0.0 //减速至车速为0，采样结束

3.然后调用python文件进行数据处理与转换，需要针对自己的数据对部分python文件进行更改。

横向控制

横向控制主要通过调节方向盘转角来实现对航向的控制。它主要由一个前馈控制器和反馈控制器组成。横向控制器的核心是车辆动力学模型与LQR模型。
计算纵向控制命令的接口：

Status LatController::ComputeControlCommand(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis,
    const planning::ADCTrajectory *planning_published_trajectory,
    control::ControlCommand *cmd)

输入为：定位信息（localization）、自车底盘信息（chassis）、规划信息（planning_published_trajectory）
输出为：方向盘控制量（cmd）
Apollo横向控制的工作原理框图如下所示：

前馈控制器

模块	变量
输入	道路曲率
输出	方向盘前馈控制量

前馈控制量为了补偿道路曲率对稳态误差的影响，实现零稳态误差。主要依据参考书²中第3.2节的公式3.12。
$${\delta }_{ff}=\frac{L}{R}+K_V{a}_y-k_3[\frac{{\ell }_r}{R}-\frac{{\ell }_f}{2C_{\alpha r}}\frac{m{V_x}^2}{R\ell }]$$

反馈控制器

模块	变量
输入	期望航向角
输出	方向盘反馈控制量

闭环控制器的主要公式为，详见参考书²中第2章关于横向控制的描述，重点关注公式2.31、2.45、2.46:
$$\begin{gathered} \frac{d}{dt}\left[\begin{array}{c}{e}_1\\ \dot{e1}\\ e_2\\ \dot{e2}\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{2C_{af}+2C_{ar}}{m{V}_x}& \frac{2C_{af}+2C_{ar}}{m}& \frac{-2C_{af}{\ell }_f+2C_{ar}{\ell }_r}{m{V}_x}\\ 0& 0& 0& 1\\ 0& -\frac{-2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{I_z{V}_x}& \frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{I_z}& -\frac{2C_{af}{\ell }_f^2+2C_{ar}{\ell }_r^2}{I_z{V}_x}\end{array}\right]\left[\begin{array}{c}{e}_1\\ \dot{e1}\\ e_2\\ \dot{e2}\end{array}\right] \\ +\left[\begin{array}{c}0\\ \frac{2C_{af}}{m}\\ 0\\ \frac{2C_{af}{\ell }_f}{I_z}\end{array}\right]\delta +\left[\begin{array}{c}0\\ -\frac{2C_{af}{\ell }_f-2C_{ar}{\ell }_r}{m{V}_x}-V_x\\ 0\\ -\frac{2C_{af}{\ell }_f^2+2C_{ar}{\ell }_r^2}{I_z{V}_x}\end{array}\right]{\dot{\psi }}_{des} \end{gathered}$$
闭环控制主要结合车辆动力学模型得出状态矩阵，然后利用LQR模型获取K矩阵，进而计算最优控制解steer_angle_feedback，它的实现过程如下：
a、依据阿克曼转角几何和车辆动力学原理，搭建车辆动力学模型，列写状态空间方程；
b、根据已知车辆信息，计算横向偏差，更新状态矩阵、状态矩阵系数、控制矩阵系数等；
更新状态矩阵，代码对应接口：

void LatController::UpdateStateAnalyticalMatching(SimpleLateralDebug *debug)

输入：debug，包含横向误差、航向误差等信息
输出：更新状态矩阵

计算横向误差，代码对应接口：

double LatController::ComputeLateralErrors(
	const double x, 
	const double y, 
	const double theta, 
	const double linear_v, 
	const double angular_v, 
	const TrajectoryAnalyzer &trajectory_analyzer, 
	SimpleLateralDebug *debug)

输入：当前车辆位置坐标（x, y）、当前航向角theta、当前线速度linear_v、当前角速度angular_v、规划轨迹trajectory_analyzer
输出：debug信息，主要为生成状态矩阵，状态矩阵包含：lateral_error、lateral_error_rate、heading_error、heading_error_rate四个元素

更新状态矩阵系数matrix_a_和离散状态矩阵系数matrix_ad_，代码对应接口：

void LatController::UpdateMatrix()

更新混合状态矩阵系数matrix_adc_和混合控制矩阵系数matrix_bdc_，代码对应接口：

void LatController::UpdateMatrixCompound()

c、计算K矩阵；
代码对应接口：

void common::math::SolveLQRProblem(
	const Matrix &A, 
	const Matrix &B, 
	const Matrix &Q, 
	const Matrix &R, 
	const double tolerance, 
	const uint max_num_iteration, 
	Matrix *ptr_K)

输入：状态矩阵系数A、控制矩阵系数B、控制权重矩阵R、状态权重矩阵Q、迭代次数max_num_iteration、计算阈值tolerance
输出：增益矩阵ptr_K
d、计算最优控制解。

const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 / 
                                      M_PI * steer_transmission_ratio_ /
                                      steer_single_direction_max_degree_ * 100;

反馈量：steer_angle = steer_angle_feedback + steer_angle_feedforward，即为方向盘控制量。

控制信号

Apollo中Control模块计算完cmd后按周期发送给canbus模块，再由canbus模块发送给CANBUS，并定期从canbus模块接收相关信息。
Apollo can信号采用DBC格式，

了解消息收发请查看如下相关代码：

Apollo\modules\canbus\canbus.cc
Apollo\modules\canbus\vehicle\lincoln\protocol
Apollo\modules\canbus\vehicle\lincoln\lincoln_controller.cc
Apollo\modules\drivers\canbus\can_comm\message_manager.h
Apollo\modules\drivers\canbus\can_comm\can_receiver.h
Apollo\modules\drivers\canbus\can_comm\protocol_data.h

消息分为接收和发送，不同消息对应不同ID，对于同一类型消息，接收和发送使用不同的消息ID，具体如下：
接收模块

信号	发送周期	发送	接收
brake_61	10ms	can_client_	Apollo_module
throttle_63	10ms	can_client_	Apollo_module
steering_65	10ms	can_client_	Apollo_module
gear_67	10ms	can_client_	Apollo_module
misc_69	10ms	can_client_	Apollo_module
wheelspeed_6a	10ms	can_client_	Apollo_module
accel_6b	10ms	can_client_	Apollo_module
gyro_6c	10ms	can_client_	Apollo_module
gps_6d	10ms	can_client_	Apollo_module
gps_6e	10ms	can_client_	Apollo_module
gps_6f	10ms	can_client_	Apollo_module
tirepressure_71	10ms	can_client_	Apollo_module
fullevel_72	10ms	can_client_	Apollo_module
surround_73	10ms	can_client_	Apollo_module
brakeinfo_74	10ms	can_client_	Apollo_module
throttleinfo_75	10ms	can_client_	Apollo_module
version_7f	10ms	can_client_	Apollo_module
license_7e	10ms	can_client_	Apollo_module

发送模块

信号	发送周期	发送	接收
brake_60	20ms	can_client_	CANBUS
throttle_62	20ms	can_client_	CANBUS
steering_64	20ms	can_client_	CANBUS
gear_66	20ms	can_client_	CANBUS
turnsignal_68	50ms	can_client_	CANBUS

仿真

仿真平台及工具

仿真平台Windows
仿真工具CarSim + Simulink
本人并未在Apollo团队预定义的Docker环境中搭建Apollo，而是在同事的协助下抽调了Apollo里的纵向控制代码及部分编译依赖库，在Windows下利用CarSim搭建车模型，Matlab Simulink提供规划路径和控制逻辑实现了自动泊车的仿真。

---

1. Werling M, Ziegler J, Kammel S, et al. Optimal trajectory generation for dynamic street scenarios in a Frenét Frame[C]// IEEE International Conference on Robotics and Automation. IEEE, 2010:987-993. ↩︎

2. Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. ↩︎ ↩︎