LQR 是一種最佳化控制方法，設計目標是找到一組控制輸入，使得線性系統的狀態軌跡儘可能地接近目標，同時使控制輸入儘可能小。其目標函式是一個二次型成本函式。
分為以下幾個步驟：
1. 設系統動態方程為：
其中x為狀態量，u為控制輸入，A和B為狀態轉移和控制矩陣。
2. 定義一個效能指標，即控制器的最佳化目標。在LQR中，通常採用二次型效能指標，即系統狀態和控制輸入的加權和的二次型。這個效能指標可以包括狀態的偏差、控制輸入的幅值等，可以根據具體需求進行調整：
其中，Q和R是權重矩陣，用於調整狀態偏差和控制輸入的相對重要性。
3. 設計LQR控制器，透過解離散時間的Riccati方程，可以獲得最優的狀態反饋控制器。這個控制器以狀態為輸入，根據系統的狀態資訊來生成控制輸入，以最小化效能指標。
方程如下：
解該方程可以參考之前的文章。
4. 最後得到最優控制器公式如下：
其中K為狀態反饋矩陣，根據當前系統狀態可以得到最終控制量u=-Kx。
matlab程式碼如下：
clear all;close all;clc; v =1; lambda = 2; dt = 0.1; L=2.5; x = 0:0.1:50; y = sin(x/5); path = [x' y']; for i=2:length(path) dx = path(i,1)-path(i-1,1); dy = path(i,2)-path(i-1,2); path(i-1,3) = atan2(dy,dx); end path(length(path),3) = path(length(path)-1,3); plot(path(:,1),path(:,2),'r.'); hold on; curp=[0 0 0]; for i=1:length(path) d = path(:,1:2) - curp(1:2); dis = d(:,1).^2 + d(:,2).^2; [~,ind] = min(dis); %找路徑最近點索引 dx = curp(1) - path(ind,1); dy = curp(2) - path(ind,2); dyaw = curp(3) - path(ind,3); xstate = [dx;dy;sin(dyaw)]; %狀態變數 Ad=[1 0 -v*sin(path(ind,3))*dt; 0 1 v*cos(path(ind,3))*dt; 0 0 1]; %線性離散後的系統矩陣 Bd=[cos(path(ind,3))*dt 0; sin(path(ind,3))*dt 0; 0 dt]; %線性離散後的控制矩陣 Q = [10 0 0; 0 10 0; 0 0 1]; %初始化LQR的權重矩陣 R =[1 0; 0 1]; %初始化LQR的權重矩陣 %*********黎卡提方程求解**********% PN =Q; err =1e-5; error = 1; k=1; while(error>err) PN_1 = Q + Ad'*PN*Ad - Ad'*PN*Bd*inv(R+Bd'*PN*Bd)*(Bd'*PN*Ad); error = norm(PN-PN_1); PN = PN_1; k = k+1; if k>400 break; end end P =PN_1; K = (R+Bd'*P*Bd)\Bd'*P*Ad; %計算狀態反饋增益K %***************************************% U = - K * xstate; %線性反饋控制量 deltav = U(1); deltastr = U(2); curp(1) = curp(1) + dt*v*cos(curp(3)); curp(2) = curp(2) + dt*v*sin(curp(3)); curp(3) = curp(3) + dt*v*tan(deltastr)/L; plot(curp(1),curp(2),'g.'); end %axis equal;
結果如下：
