百度Apollo 2.0 車輛控制算法之LQR控制算法解讀

Apollo 中橫向控制的LQR控制算法在Latcontroller..cc 中實現(xiàn)

根據(jù)車輛的二自由度動力學(xué)模型

(1)

根據(jù)魔術(shù)公式在小角度偏角的情況下有,輪胎的側(cè)向力與輪胎的偏離角成正比. ,分別為前、后輪的側(cè)偏剛度,

(2)

(3)在小角度的情況下有

所以有

(4)

因此上述車輛的動力學(xué)模型可以簡化寫成

(5)

?(6)期望橫擺角角速度

(7) 橫擺角角度偏差

?(7)橫向偏差變化率求導(dǎo)數(shù)

?(8)橫向偏差變化率

?

車輛模型的連續(xù)狀態(tài)空間方程

(9)

狀態(tài)變量X的選擇分別為橫向偏差、橫向偏差變化率，橫擺角角度偏差，橫擺角角度偏差變化率。控制量u為前輪偏角。

選擇合適的狀態(tài)變量后得到A，B，B1，B2矩陣分別如下

???

由于只對橫擺角角度偏差變化率的導(dǎo)數(shù)產(chǎn)生影響，在橫向控制中主要是控制橫向偏差、橫向偏差變化率，橫擺角角度偏差，橫擺角角度偏差變化率，因而忽略了公式中的項。車輛系統(tǒng)的狀態(tài)空間方程表示為

?(10)

Init（）函數(shù)中將A,B, 與Vx無關(guān)的系數(shù)先行計算，與Vx相關(guān)的系數(shù)參數(shù)計算根據(jù)Vx不斷更新。

上述連續(xù)的狀態(tài)空間方程用于計算機控制需要對連續(xù)的狀態(tài)空間方程進行離散化，其中At采用雙線性變換得到

, At來源見右邊公式

, ，T為控制周期，本程序中為0.01s。

上述參數(shù)計算和離散化的過程在UpdateMatrix()函數(shù)和UpdateMatrixCompound()函數(shù)中

的離散化過程在Init（）函數(shù)中實現(xiàn)

? matrix_b_ = Matrix::Zero(basic_state_size_, 1);

? matrix_bd_ = Matrix::Zero(basic_state_size_, 1);

? matrix_bdc_ = Matrix::Zero(matrix_size, 1);

? matrix_b_(1, 0) = cf_ / mass_;

? matrix_b_(3, 0) = lf_ * cf_ / iz_;

? matrix_bd_ = matrix_b_ * ts_;

的離散化過程在UpdateMatrix()函數(shù)中實現(xiàn)

void LatController::UpdateMatrix() {

??const double v =

????? std::max(VehicleStateProvider::instance()->linear_velocity(), 0.2);

? matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;

? matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;

? matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;

? matrix_a_(3, 3) = matrix_a_coeff_(3, 3) / v;

? Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());

? matrix_ad_ = (matrix_i + ts_ * 0.5 * matrix_a_) *

?????????????? (matrix_i - ts_ * 0.5 * matrix_a_).inverse(); //雙線性變換離散化A

}

通過上述介紹我們得到了車輛離散狀態(tài)空間方程(11)中的At,Bt,則系統(tǒng)的最優(yōu)前輪轉(zhuǎn)角?(12)

定義如下目標(biāo)函數(shù)

?(13)

其中Q為狀態(tài)權(quán)重系數(shù)，R為控制量權(quán)重系數(shù)

當(dāng)上述目標(biāo)函數(shù)最小時就得到最優(yōu)的狀態(tài)反饋矩陣K

上述公式的,(14)

同時矩陣P滿足黎卡提方程：

(15)

求解狀態(tài)反饋矩陣K的在

??? common::math::SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_,

?????????????? ???????????????????matrix_r_, lqr_eps_, lqr_max_iteration_,

????????????????????????????????? &matrix_k_); 函數(shù)中實現(xiàn)

函數(shù)輸入為At,Bt,Q，R,最大迭代次數(shù)lqr_max_iteration_，最小精度lqr_eps_，函數(shù)輸出為狀態(tài)反饋矩陣matrix_k_。

當(dāng)前最新的狀態(tài)量是通過UpdateStateAnalyticalMatching()函數(shù)獲得，由此我們可以通過?(12)

計算出當(dāng)前的最優(yōu)反饋控制量即最優(yōu)前輪偏角。然而這還沒玩完。

將公式(12)的控制量帶入公式（10）得到系統(tǒng)狀態(tài)反饋后的狀態(tài)空間方程如下

? (13)

車輛沿固定曲率的軌跡運行時不為零。因此通過LQR調(diào)節(jié)的特征值使系統(tǒng)趨于穩(wěn)定，但是系統(tǒng)的穩(wěn)態(tài)偏差并不為0。

???? 為此在原有最優(yōu)控制量的基礎(chǔ)上增加一個前饋環(huán)節(jié)，使系統(tǒng)趨于穩(wěn)定的同時系統(tǒng)的橫向穩(wěn)態(tài)偏差為0。

?? ?(14)

公式中為前饋環(huán)節(jié)提供的前輪轉(zhuǎn)角。

將公式(14)帶入公式（10）得到

?(15)

設(shè)初始條件為0，對公式(15)進行拉普拉斯變換得到

? (16)

假設(shè)汽車以固定縱向速度Vx沿某一固定曲率的彎道行駛，則通過縱向車速Vx和道路的半徑R可以計算出期望汽車橫擺角速度：

?? (6)

公式(6)可知橫擺角速度的拉普拉斯變換結(jié)果為

?(17)

假設(shè)為固定值，則其拉普拉斯變換結(jié)果為

(18)

根據(jù)終值定理，系統(tǒng)的穩(wěn)態(tài)誤差為

?(19)

將A，B,B1,K帶入公式(19)得到

?(20)

觀察公式(20)中的第一項和第三項。可知道對橫擺角角度偏差無影響，選擇合適的可將橫向偏差穩(wěn)態(tài)值趨向與0。

?(21)

要想橫向偏差的穩(wěn)態(tài)值趨于零，則

?(22)

因此

?? (23)

令，不足轉(zhuǎn)向梯度系數(shù)? (24)

公式(23)可以簡化為

??(25)

Apollo程序中計算控制量的函數(shù)為ComputeControlCommand()函數(shù)

?

前饋環(huán)節(jié)計算的前輪轉(zhuǎn)角對應(yīng)如下程序

double LatController::ComputeFeedForward(double ref_curvature) const {

? const double kv =

????? lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_; //對應(yīng)公式(24)

?

? // then change it from rad to %

? const double v = VehicleStateProvider::instance()->linear_velocity();

? const double steer_angle_feedforwardterm =

????? (wheelbase_ * ref_curvature + kv * v * v * ref_curvature -

?????? matrix_k_(0, 2) *

?????????? (lr_ * ref_curvature -

??????????? lf_ * mass_ * v * v * ref_curvature / 2 / cr_ / wheelbase_)) *

????? 180 / M_PI * steer_transmission_ratio_ /

????? steer_single_direction_max_degree_ * 100; //對應(yīng)公式(25),并將角度由弧度轉(zhuǎn)變?yōu)榻嵌?#xff0c;最后轉(zhuǎn)變?yōu)榘俜直?/span>

? return steer_angle_feedforwardterm;

}

?

最終的前輪轉(zhuǎn)角的控制量為最優(yōu)狀態(tài)反饋控制量與前饋控制前輪轉(zhuǎn)角之和。對應(yīng)程序如下

? const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 /

????????????????????????????????????? M_PI * steer_transmission_ratio_ /

????????????????????????????????????? steer_single_direction_max_degree_ * 100;

?//計算狀態(tài)反饋對應(yīng)的控制量，將弧度轉(zhuǎn)變?yōu)榻嵌?#xff0c;最后轉(zhuǎn)變?yōu)榘俜直取?/p>

// steer_transmission_ratio_表示方向盤轉(zhuǎn)動角度與車輪轉(zhuǎn)動角度的比值，在車輛信息中定義該參數(shù)，

// steer_single_direction_max_degree表示最大的方向盤轉(zhuǎn)動的角度，單位為度

? const double steer_angle_feedforward = ComputeFeedForward(debug->curvature());

//計算前饋控制對應(yīng)的控制量

?

? // Clamp the steer angle to -100.0 to 100.0

? double steer_angle = common::math::Clamp(

????? steer_angle_feedback + steer_angle_feedforward, -100.0, 100.0); //將狀態(tài)反饋的控制量與前饋控制控制量進行疊加，并進行限幅處理。

計算的出前輪轉(zhuǎn)角經(jīng)過上下限的限幅后進行輸出

? if (FLAGS_set_steer_limit) {

??? const double steer_limit =

??????? std::atan(max_lat_acc_ * wheelbase_ /

????????????????? (VehicleStateProvider::instance()->linear_velocity() *

?????????????????? VehicleStateProvider::instance()->linear_velocity())) *

??????? steer_transmission_ratio_ * 180 / M_PI /

??????? steer_single_direction_max_degree_ * 100; //計算前輪轉(zhuǎn)角的上下限限幅值。

?

??? // Clamp the steer angle

??? double steer_angle_limited =

??????? common::math::Clamp(steer_angle, -steer_limit, steer_limit); //對前輪轉(zhuǎn)角進行上下限的限幅處理

??? steer_angle_limited = digital_filter_.Filter(steer_angle_limited); //對前輪轉(zhuǎn)角進行低通濾波處理

??? cmd->set_steering_target(steer_angle_limited);

??? debug->set_steer_angle_limited(steer_angle_limited);

? } else {

??? steer_angle = digital_filter_.Filter(steer_angle);//對前輪轉(zhuǎn)角進行低通濾波處理

??? cmd->set_steering_target(steer_angle);

? }

?

?

部分成員函數(shù)介紹

1、LoadControlConf 成員函數(shù)用于獲取控制參數(shù)包括車身參數(shù)、LQR控制的精度、最大迭代次數(shù)等。

其中車輛的參數(shù)來自modules\common\data\ mkz_config.pb.txt文件

LQR的控制參數(shù)來自modules\control\conf\ lincoln.pb.txt文件

2、InitializeFilters成員函數(shù)用于設(shè)置巴斯沃特低通低通濾波參數(shù)， 以及對 lateral_error、heading_error進行均值濾波。濾波器的參數(shù)來自modules\control\conf\ lincoln.pb.txt文件

?

3、Init成員函數(shù)用于初始化 狀態(tài)空間方程的A，B，K, Q，R 以及控制系統(tǒng)的相關(guān)參數(shù)。

LoadLatGainScheduler函數(shù)的作用？

LoadLatGainScheduler函數(shù)用于獲取 lateral_error、heading_error 增益的策略。

?

4、ComputeControlCommand 函數(shù)最重要，計算控制量

?

通過LQR求解黎卡提方程得到控制量

SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_updated_,

????????????????????????????????? matrix_r_, lqr_eps_, lqr_max_iteration_,

?????????????????? ???????????????&matrix_k_);

//在高速運行時，控制量權(quán)重矩陣matrix_q_的系數(shù)根據(jù)速度變化進行調(diào)整

? // Add gain sheduler for higher speed steering

? if (FLAGS_enable_gain_scheduler) {

??? matrix_q_updated_(0, 0) =

??????? matrix_q_(0, 0) *

??????? lat_err_interpolation_->Interpolate(

? ??????????VehicleStateProvider::instance()->linear_velocity());

??? matrix_q_updated_(2, 2) =

??????? matrix_q_(2, 2) *

??????? heading_err_interpolation_->Interpolate(

??????????? VehicleStateProvider::instance()->linear_velocity());

??? common::math::SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_updated_,

????????????????????????????????? matrix_r_, lqr_eps_, lqr_max_iteration_,

????????????????????????????????? &matrix_k_);

? } else {

??? common::math::SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_,

????????????????????????????????? matrix_r_, lqr_eps_, lqr_max_iteration_,

????????????????????????????????? &matrix_k_);

? }

?

5、UpdateMatrix()函數(shù)

用于更新matrix_a_(離散之前的A矩陣) 和matrix_ad_（離散之后的A矩陣）

matrix_a_ 由中系數(shù)分為兩類，一類與速度無關(guān)，另外一類與速度相關(guān)放在matrix_a_coeff_

從何得到線性時變狀態(tài)空間方程。

matrix_adc_ 由matrix_ad_ 和 過去的變量對應(yīng)的矩陣組合而成

?matrix_bdc_. 由matrix_bd_ 和過去變量對應(yīng)的矩組合而成。

?

6、GetLateralError()函數(shù)

獲取橫向偏差，具體計算過程為根據(jù)車輛當(dāng)前的位置查找參考軌跡最近點，形成直線L。

計算出車輛位置與最近點的距離std::sqrt(dx * dx + dy * dy)，同時計算出該條線與x軸正方向之間的角度point_angle，將該角度減去參考軌跡的方向角得到直線L與參考軌跡速度方向之間的夾角point2path_angle。

橫向偏差為std::sin(point2path_angle) * std::sqrt(dx * dx + dy * dy);

7、UpdateStateAnalyticalMatching()函數(shù)獲取狀態(tài)偏差，ComputeLateralErrors()函數(shù)主要通過根據(jù)當(dāng)前車輛的位置計算出在參考軌跡上上離車輛當(dāng)前位置最近點作為參考點，通過參考點與實際車輛位置就可以獲得各種狀態(tài)偏差（橫向偏差、橫向偏差變化率、航向角偏差、航向角偏差變化率）。

總結(jié)

以上是生活随笔為你收集整理的百度Apollo 2.0 车辆控制算法之LQR控制算法解读的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。