Apollo代碼學習—控制子產品概述

補充
- 2018.11.08更新
- 2018.11.15更新
- 2018.11.20更新
前言
控制
- 縱向控制
- - 标定表的生成
- 橫向控制
- 控制信号
仿真
- 仿真平台及工具

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縱向控制的工作原理框圖如下所示：

Apollo代碼學習(一)—控制子產品概述補充前言控制仿真

位置PID閉環控制器

子產品	變量
輸入	期望位置+目前實際位置
輸出	速度補償量

速度PID閉環控制器

子產品	變量
輸入	速度補償+目前位置速度偏差
輸出	加速度補償量

速度-加速度-刹車/油門指令标定表

子產品	變量
輸入	加速度補償+規劃加速度，車速
輸出	油門/刹車控制量

縱向控制中了解起來較為複雜的是縱向誤差的計算：

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

涉及到Frenet坐标系1

Apollo代碼學習(一)—控制子產品概述補充前言控制仿真

标定表的生成

标定表的更新請參考文檔：

Apollo/docs/howto/how_to_update_vehicle_calibration.md

标定流程大緻如下圖所示：

Apollo代碼學習(一)—控制子產品概述補充前言控制仿真

其中，指令集可自定義。因為本人是在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橫向控制的工作原理框圖如下所示：

Apollo代碼學習(一)—控制子產品概述補充前言控制仿真

前饋控制器

子產品	變量
輸入	道路曲率
輸出	方向盤前饋控制量

前饋控制量為了補償道路曲率對穩态誤差的影響，實作零穩态誤差。主要依據參考書2中第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章關于橫向控制的描述，重點關注公式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格式，

Apollo代碼學習(一)—控制子產品概述補充前言控制仿真

了解消息收發請檢視如下相關代碼：

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提供規劃路徑和控制邏輯實作了自動泊車的仿真。

Apollo代碼學習(一)—控制子產品概述補充前言控制仿真

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. ↩︎ ↩︎

Apollo代碼學習(一)—控制子產品概述補充前言控制仿真

Apollo代碼學習—控制子產品概述

補充

2018.11.08更新

2018.11.15更新

2018.11.20更新

前言

控制

縱向控制

标定表的生成

橫向控制

控制信号

仿真

仿真平台及工具

繼續閱讀

Apollo 3.5 Cyber base代碼學習

graphql(四)——前端如何使用graphql(graphql+vue+vue-apollo)

piecewise jerk path planner

docker-compose部署apollo

使用穩定擴散快速添加産品模特。以行李箱為例，制作過程分為三步。·第一步，将産品摳圖處理，可以使用線上工具一鍵摳圖。接着在

【動态定時任務】從apollo配置中心擷取定時任務的執行時間，能夠實時修改定時任務執行時間

apollo Controller類代碼分析

最簡單的apollo分布式配置中心使用體驗

Apollo學習筆記（一）概述+大綱

一款能夠對git push實時分析及管理的工具：Gitcontrol

打開控制台的指令control

百度Apollo(一)：代碼子產品

【Baidu Apollo】硬體開發平台介紹1 自動駕駛汽車的事故分析2 自動駕駛汽車的研發流程3 自動駕駛汽車的硬體系統4 自動駕駛汽車的傳感器 5 自動駕駛汽車的計算單元 6 自動駕駛汽車的線控系統7 Apollo硬體開發平台

maven多子產品項目(dao,service,controller)的建構

好女人、好男人都是被另一半抛棄的對象。Goodwoman。好女人都是被另一半抛棄的對象。Goodmenarealways

進一步評估了EP、EP/7.5%TFXBJ和CNC1的粘接性能，首先分别研究了三個體系的樣品在UV光固化，光-熱雙固化以