系列文章目錄

前言

本示例展示了如何利用非線性模型預測控制（MPC）為四旋翼飛行器設計一個跟蹤軌跡的控制器。

一、四旋翼模型

四旋翼飛行器有四個向上的旋翼。從四旋翼飛行器的質(zhì)量中心出發(fā)，旋翼呈等距離的正方形排列。四旋翼飛行器動力學數(shù)學模型采用歐拉-拉格朗日方程 [1]。

四旋翼飛行器的十二種狀態(tài)為

其中

• [x,y,z]表示慣性參考系中的位置。
• 角度位置[j,θ,ψ]分別表示滾動、俯仰和偏航。
• 其余狀態(tài)為位置和角度的速度。

四旋翼飛行器的控制輸入（也稱為操縱變量，用 MV 表示）是四個旋翼的角速度平方：

這些控制輸入可在機身 Z 軸方向產(chǎn)生力、扭矩和推力。在此示例中，每個狀態(tài)都是可測量的，控制輸入范圍限制為：

?

非線性模型預測控制器使用的預測模型包括一個狀態(tài)函數(shù)（表示作為當前狀態(tài)和輸入函數(shù)的狀態(tài)導數(shù)）和一個狀態(tài)雅各布函數(shù)（分別表示狀態(tài)函數(shù)相對于狀態(tài)和輸入的導數(shù)）。這兩個函數(shù)都是通過 Symbolic Math Toolbox? 軟件建立和導出的。更多詳情，請參閱為非線性模型預測控制推導四旋翼飛行器動力學（符號數(shù)學工具箱）。

調(diào)用腳本 getQuadrotorDynamicsAndJacobian 生成狀態(tài)及其雅各布函數(shù)并寫入文件。

getQuadrotorDynamicsAndJacobian;

getQuadrotorDynamicsAndJacobian 腳本生成以下文件：

• QuadrotorStateFcn.m - 狀態(tài)函數(shù)
• QuadrotorStateJacobianFcn.m - 狀態(tài)雅各布函數(shù)

有關任一函數(shù)的詳細信息，請打開相應文件。

二、設計非線性模型預測控制器

創(chuàng)建一個具有 12 個狀態(tài)、12 個輸出和 4 個輸入的非線性 MPC 對象。默認情況下，所有輸入均為操縱變量 (MV)。

nx = 12;
ny = 12;
nu = 4;
nlmpcobj = nlmpc(nx, ny, nu);

Zero weights are applied to one or more OVs because there are fewer MVs than OVs.

?使用函數(shù)名稱指定預測模型狀態(tài)函數(shù)。也可以使用函數(shù)句柄指定函數(shù)。

nlmpcobj.Model.StateFcn = "QuadrotorStateFcn";

最佳做法是為預測模型提供分析雅各布。這樣做可以大大提高仿真效率。使用函數(shù)句柄指定返回狀態(tài)函數(shù)雅各布的函數(shù)。

nlmpcobj.Jacobian.StateFcn = @QuadrotorStateJacobianFcn;

?修復隨機發(fā)生器種子，實現(xiàn)可重復性。

rng(0)

要檢查 nlobj 的預測模型函數(shù)是否有效，請使用 validateFcns 對狀態(tài)輸入空間中的一個隨機點進行檢查。

validateFcns(nlmpcobj,rand(nx,1),rand(nu,1));

Model.StateFcn is OK.
Jacobian.StateFcn is OK.
No output function specified. Assuming "y = x" in the prediction model.
Analysis of user-provided model, cost, and constraint functions complete.

指定采樣時間為 0.1 秒，預測范圍為 18 步，控制范圍為 2 步。

Ts = 0.1;
p = 18;
m = 2;
nlmpcobj.Ts = Ts;
nlmpcobj.PredictionHorizon = p;
nlmpcobj.ControlHorizon = m;

?將所有四個控制輸入限制在 [0,10] 范圍內(nèi)。同時將控制輸入變化率限制在 [-2,2] 的范圍內(nèi)，以防止突然和粗暴的移動。

nlmpcobj.MV = struct( ...
    Min={0;0;0;0}, ...
    Max={10;10;10;10}, ...
    RateMin={-2;-2;-2;-2}, ...
    RateMax={2;2;2;2} ...
    );

?非線性 MPC 的默認代價函數(shù)是標準二次代價函數(shù)，適用于參考跟蹤和干擾抑制。在本例中，要求前 6 個狀態(tài) [x,y,z,?,θ,ψ] 遵循給定的參考軌跡。由于 MV 的數(shù)量（4 個）少于參考輸出軌跡的數(shù)量（6 個），因此沒有足夠的自由度來獨立跟蹤所有輸出的軌跡。

nlmpcobj.Weights.OutputVariables = [1 1 1 1 1 1 0 0 0 0 0 0];

在此示例中，MV 也有額定目標（稍后為模擬設置）。這些目標是在不需要跟蹤時為保持四旋翼飛行器漂浮而設置的平均值，可能會導致 MV 和 OV 參考跟蹤目標之間的沖突。要優(yōu)先考慮 OV 目標，可將平均 MV 跟蹤優(yōu)先級設置為低于平均 OV 跟蹤優(yōu)先級。

nlmpcobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1];

此外，通過指定 MV 變化率的調(diào)整權(quán)重，懲罰過于激進的控制行動。

nlmpcobj.Weights.ManipulatedVariablesRate = [0.1 0.1 0.1 0.1];

三、閉環(huán)模擬

按照目標軌跡對系統(tǒng)進行 20 秒鐘的模擬。

% Specify the initial conditions
x = [7;-10;0;0;0;0;0;0;0;0;0;0];

% Nominal control target (average to keep quadrotor floating)
nloptions = nlmpcmoveopt;
nloptions.MVTarget = [4.9 4.9 4.9 4.9]; 
mv = nloptions.MVTarget;

使用 nlmpcmove 函數(shù)模擬閉環(huán)系統(tǒng)，使用 nlmpcmove 對象指定模擬選項。

% Simulation duration in seconds
Duration = 20;

% Display waitbar to show simulation progress
hbar = waitbar(0,"Simulation Progress");

% MV last value is part of the controller state
lastMV = mv;

% Store states for plotting purposes
xHistory = x';
uHistory = lastMV;

% Simulation loop
for k = 1:(Duration/Ts)

    % Set references for previewing
    t = linspace(k*Ts, (k+p-1)*Ts,p);
    yref = QuadrotorReferenceTrajectory(t);

    % Compute control move with reference previewing
    xk = xHistory(k,:);
    [uk,nloptions,info] = nlmpcmove(nlmpcobj,xk,lastMV,yref',[],nloptions);

    % Store control move
    uHistory(k+1,:) = uk';
    lastMV = uk;

    % Simulate quadrotor for the next control interval (MVs = uk) 
    ODEFUN = @(t,xk) QuadrotorStateFcn(xk,uk);
    [TOUT,XOUT] = ode45(ODEFUN,[0 Ts], xHistory(k,:)');

    % Update quadrotor state
    xHistory(k+1,:) = XOUT(end,:);

    % Update waitbar
    waitbar(k*Ts/Duration,hbar);
end

% Close waitbar 
close(hbar)

四、可視化和結(jié)果

繪制結(jié)果圖，并比較計劃和實際的閉環(huán)軌跡。

plotQuadrotorTrajectory;

?

?

由于 MV 的數(shù)量少于參考輸出軌跡的數(shù)量，因此沒有足夠的自由度來獨立跟蹤所有 OV 的理想軌跡。

如圖所示，狀態(tài) [x,y,z,j,θ,ψ] 和控制輸入、

• 狀態(tài)[x,y,z]在 7 秒內(nèi)與參考軌跡非常吻合。
• 狀態(tài) [j,θ,ψ]在 9 秒內(nèi)被驅(qū)動到零點附近。
• 控制輸入在 10 秒鐘左右被驅(qū)動到目標值 4.9。

您可以將四旋翼飛行器的軌跡制成動畫。在 7 秒內(nèi)，四旋翼飛行器靠近沿參考軌跡飛行的 "目標 "四旋翼飛行器。之后，四旋翼飛行器將緊跟參考軌跡。動畫在 20 秒時結(jié)束。

animateQuadrotorTrajectory;

?

五、結(jié)論

本例展示了如何設計用于四旋翼飛行器軌跡跟蹤的非線性模型預測控制器。四旋翼飛行器的動力學和 Jacobian 是通過 Symbolic Math Toolbox 軟件得出的。四旋翼飛行器能緊密跟蹤參考軌跡。?

參考資料

[1] Raffo, Guilherme V., Manuel G. Ortega, and Francisco R. Rubio. "An integral predictive/nonlinear??∞?control structure for a quadrotor helicopter".?Automatica 46, no. 1 (January 2010): 29–39. https://doi.org/10.1016/j.automatica.2009.10.018.

[2] Tzorakoleftherakis, Emmanouil, and Todd D. Murphey. "Iterative sequential action control for stable, model-based control of nonlinear systems."?IEEE Transactions on Automatic Control?64, no. 8 (August 2019): 3170–83. https://doi.org/10.1109/TAC.2018.2885477.

[3] Luukkonen, Teppo. "Modelling and control of quadcopter". Independent research project in applied mathematics, Aalto University, 2011.文章來源地址http://www.zghlxwxcb.cn/news/detail-815923.html

到了這里，關于MATLAB - 利用非線性模型預測控制（Nonlinear MPC）來控制四旋翼飛行器的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網(wǎng)！