matlab練習程式（LQR路徑跟蹤）

Dsp Tian發表於2024-06-01
Matlab

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;

結果如下：

相關文章