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

2023-11-02

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整体框架
其中，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坐标系¹

Trajectory generation in a Frene ́t-frame

标定表的生成

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

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。
δ f f = L R + K V a y − k 3 [ ℓ r R − ℓ f 2 C α r m V x 2 R ℓ ] \delta_{ff}=\frac{L}{R}+K_Va_y-k_3[\frac{\ell_r}{R}-\frac{\ell_f}{2C_{\alpha r}}\frac{m{V_x}^2}{R\ell}] δff=RL+KVay−k3[Rℓr−2CαrℓfRℓmVx2]

反馈控制器

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

闭环控制器的主要公式为，详见参考书²中第2章关于横向控制的描述，重点关注公式2.31、2.45、2.46:
d d t [ e 1 e 1 ˙ e 2 e 2 ˙ ] = [ 0 1 0 0 0 − 2 C a f + 2 C a r m V x 2 C a f + 2 C a r m − 2 C a f ℓ f + 2 C a r ℓ r m V x 0 0 0 1 0 − − 2 C a f ℓ f − 2 C a r ℓ r I z V x 2 C a f ℓ f − 2 C a r ℓ r I z − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V x ] [ e 1 e 1 ˙ e 2 e 2 ˙ ] + [ 0 2 C a f m 0 2 C a f ℓ f I z ] δ + [ 0 − 2 C a f ℓ f − 2 C a r ℓ r m V x − V x 0 − 2 C a f ℓ f 2 + 2 C a r ℓ r 2 I z V x ] ψ ˙ d e s \frac{d}{dt} \begin{bmatrix} e_1 \\ \dot{e1} \\ e_2 \\ \dot{e2} \\ \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2C_{af}+2C_{ar}}{mV_x} & \frac{2C_{af}+2C_{ar}}{m} & \frac{-2C_{af}\ell_f+2C_{ar}\ell_r}{mV_x} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{-2C_{af}\ell_f-2C_{ar}\ell_r}{I_zV_x} & \frac{2C_{af}\ell_f-2C_{ar}\ell_r}{I_z} & -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \\ \end{bmatrix} \begin{bmatrix} e_1 \\ \dot{e1} \\ e_2 \\ \dot{e2} \\ \end{bmatrix} \\ +\begin{bmatrix} 0 \\ \frac{2C_{af}}{m} \\ 0 \\ \frac{2C_{af}\ell_f}{I_z} \\ \end{bmatrix}\delta + \begin{bmatrix} 0 \\ -\frac{2C_{af}\ell_f-2C_{ar}\ell_r}{mV_x}-V_x \\ 0 \\ -\frac{2C_{af}\ell_f^2+2C_{ar}\ell_r^2}{I_zV_x} \\ \end{bmatrix}\dot{\psi}_{des} dtd⎣⎢⎢⎡e1e1˙e2e2˙⎦⎥⎥⎤=⎣⎢⎢⎢⎡00001−mVx2Caf+2Car0−IzVx−2Cafℓf−2Carℓr0m2Caf+2Car0Iz2Cafℓf−2Carℓr0mVx−2Cafℓf+2Carℓr1−IzVx2Cafℓf2+2Carℓr2⎦⎥⎥⎥⎤⎣⎢⎢⎡e1e1˙e2e2˙⎦⎥⎥⎤+⎣⎢⎢⎡0m2Caf0Iz2Cafℓf⎦⎥⎥⎤δ+⎣⎢⎢⎢⎡0−mVx2Cafℓf−2Carℓr−Vx0−IzVx2Cafℓf2+2Carℓr2⎦⎥⎥⎥⎤ψ˙des
闭环控制主要结合车辆动力学模型得出状态矩阵，然后利用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格式，
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提供规划路径和控制逻辑实现了自动泊车的仿真。
CarSim仿真界面
Simulink仿真界面

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. ↩︎
Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. ↩︎ ↩︎

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)