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

Apollo 中橫向控制的LQR控制算法在Latcontroller..cc 中實現

根據車輛的二自由度動力學模型

(1)

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

(2)

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

所以有

(4)

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

(5)

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

(7) 橫擺角角度偏差

?(7)橫向偏差變化率求導數

?(8)橫向偏差變化率

?

車輛模型的連續狀態空間方程

(9)

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

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

???

由于只對橫擺角角度偏差變化率的導數產生影響，在橫向控制中主要是控制橫向偏差、橫向偏差變化率，橫擺角角度偏差，橫擺角角度偏差變化率，因而忽略了公式中的項。車輛系統的狀態空間方程表示為

?(10)

Init（）函數中將A,B, 與Vx無關的系數先行計算，與Vx相關的系數參數計算根據Vx不斷更新。

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

, At來源見右邊公式

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

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

的離散化過程在Init（）函數中實現

? 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()函數中實現

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

}

通過上述介紹我們得到了車輛離散狀態空間方程(11)中的At,Bt,則系統的最優前輪轉角?(12)

定義如下目標函數

?(13)

其中Q為狀態權重系數，R為控制量權重系數

當上述目標函數最小時就得到最優的狀態反饋矩陣K

上述公式的,(14)

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

(15)

求解狀態反饋矩陣K的在

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

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

????????????????????????????????? &matrix_k_); 函數中實現

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

當前最新的狀態量是通過UpdateStateAnalyticalMatching()函數獲得，由此我們可以通過?(12)

計算出當前的最優反饋控制量即最優前輪偏角。然而這還沒玩完。

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

? (13)

車輛沿固定曲率的軌跡運行時不為零。因此通過LQR調節的特征值使系統趨于穩定，但是系統的穩態偏差并不為0。

???? 為此在原有最優控制量的基礎上增加一個前饋環節，使系統趨于穩定的同時系統的橫向穩態偏差為0。

?? ?(14)

公式中為前饋環節提供的前輪轉角。

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

?(15)

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

? (16)

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

?? (6)

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

?(17)

假設為固定值，則其拉普拉斯變換結果為

(18)

根據終值定理，系統的穩態誤差為

?(19)

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

?(20)

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

?(21)

要想橫向偏差的穩態值趨于零，則

?(22)

因此

?? (23)

令，不足轉向梯度系數? (24)

公式(23)可以簡化為

??(25)

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

?

前饋環節計算的前輪轉角對應如下程序

double LatController::ComputeFeedForward(double ref_curvature) const {

? const double kv =

????? lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_; //對應公式(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; //對應公式(25),并將角度由弧度轉變為角度，最后轉變為百分比

? return steer_angle_feedforwardterm;

}

?

最終的前輪轉角的控制量為最優狀態反饋控制量與前饋控制前輪轉角之和。對應程序如下

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

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

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

?//計算狀態反饋對應的控制量，將弧度轉變為角度，最后轉變為百分比。

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

// steer_single_direction_max_degree表示最大的方向盤轉動的角度，單位為度

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

//計算前饋控制對應的控制量

?

? // 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); //將狀態反饋的控制量與前饋控制控制量進行疊加，并進行限幅處理。

計算的出前輪轉角經過上下限的限幅后進行輸出

? 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; //計算前輪轉角的上下限限幅值。

?

??? // Clamp the steer angle

??? double steer_angle_limited =

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

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

??? cmd->set_steering_target(steer_angle_limited);

??? debug->set_steer_angle_limited(steer_angle_limited);

? } else {

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

??? cmd->set_steering_target(steer_angle);

? }

?

?

部分成員函數介紹

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

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

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

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

?

3、Init成員函數用于初始化 狀態空間方程的A，B，K, Q，R 以及控制系統的相關參數。

LoadLatGainScheduler函數的作用？

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

?

4、ComputeControlCommand 函數最重要，計算控制量

?

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

SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_updated_,

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

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

//在高速運行時，控制量權重矩陣matrix_q_的系數根據速度變化進行調整

? // 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()函數

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

matrix_a_ 由中系數分為兩類，一類與速度無關，另外一類與速度相關放在matrix_a_coeff_

從何得到線性時變狀態空間方程。

matrix_adc_ 由matrix_ad_ 和 過去的變量對應的矩陣組合而成

?matrix_bdc_. 由matrix_bd_ 和過去變量對應的矩組合而成。

?

6、GetLateralError()函數

獲取橫向偏差，具體計算過程為根據車輛當前的位置查找參考軌跡最近點，形成直線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()函數獲取狀態偏差，ComputeLateralErrors()函數主要通過根據當前車輛的位置計算出在參考軌跡上上離車輛當前位置最近點作為參考點，通過參考點與實際車輛位置就可以獲得各種狀態偏差（橫向偏差、橫向偏差變化率、航向角偏差、航向角偏差變化率）。

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。