非線形モデルの予測制御に関するクワッドローターのダイナミクスの導出

ライブスクリプトを開く

この例では、Symbolic Math Toolbox™ を使用してクワッドローターの連続時間非線形モデルを導出する方法を説明します。具体的にいうと、この例では、クワッドローターの状態関数とそのヤコビ関数を生成する getQuadrotorDynamicsAndJacobian スクリプトについて説明します。これらの関数は、Control of Quadrotor Using Nonlinear Model Predictive Control (Model Predictive Control Toolbox)の例で使用されます。

クワッドコプターとも呼ばれるクワッドローターは、4 つのローターをもつヘリコプターです。クワッドローターの重心から等しい距離に、正方形を成すようにローターが配置されます。クワッドローターの動きは、電気モーターで回転する 4 つのローターの角速度を調整することで制御します。クワッドローターのダイナミクスの数学的モデルは、ニュートン・オイラー方程式およびオイラー・ラグランジュ方程式から導出できます [1]。

状態変数と状態パラメーターの定義

クワッドローターは、この図に示すように、6 つの自由度 (3 つの線形座標と 3 つの角度座標) をもち、4 つの制御入力が与えられます。

クワッドローターは次の 12 個の状態をもちます。

${(x, y, z, ϕ, θ, ψ, \dot{x}, \dot{y}, \dot{z}, \dot{ϕ}, \dot{θ}, \dot{ψ})}^{T},$

ここで

$ξ = {(x, y, z)}^{T}$ は線形位置を表す。
$η = {(ϕ, θ, ψ)}^{T}$ は、それぞれロール角、ピッチ角、ヨー角を表す。
${(\dot{x}, \dot{y}, \dot{z}, \dot{ϕ}, \dot{θ}, \dot{ψ})}^{T}$ は、直線速度と角速度、または直線速度と角速度の時間微分を表す。

クワッドローターの動きに関するパラメーターは以下のとおりです。

$(u_{1}, u_{2}, u_{3}, u_{4}) = (ω_{1}^{2}, ω_{2}^{2}, ω_{3}^{2}, ω_{4}^{2})$ は、4 つのローターの角速度の 2 乗を表す。
$(I_{xx}, I_{yy}, I_{zz})$ は、ボディ座標系における回転慣性行列の対角要素を表す。
$(k, l, m, b, g)$ は、揚力係数、重心からのローターの距離、クワッドローターの質量、抗力係数、および重力を表す。

最初の 4 つのパラメーター $(u_{1}, u_{2}, u_{3}, u_{4})$ は、クワッドローターの軌跡を制御する制御入力または操作変数です。残りのパラメーターは、指定された値で固定されます。

変換行列とコリオリ行列の定義

慣性座標系とボディ座標系の間での変換を行う行列を定義します。これらの変換行列は、クワッドローターの動きをボディ座標系で表すために必要です。クワッドローターの 12 個の状態は慣性座標系で定義され、回転慣性行列はボディ座標系で定義されます。

時間に依存する角度を表すシンボリック関数を作成します。この明示的な時間依存は、[1] に従って数学的導出を行う際に時間微分を評価するために必要です。慣性座標系からボディ座標系に角速度を変換する行列 $W_{η}$ を定義します。ローカル関数のセクションで定義された関数 rotationMatrixEulerZYX を使用して、ボディ座標系から慣性座標系に線形力を変換する回転行列 $R$ を定義します。これらの変換行列を表すシンボリック行列を作成します。

% Create symbolic functions for time-dependent angles
% phi: roll angle
% theta: pitch angle
% psi: yaw angle
syms phi(t) theta(t) psi(t)

% Transformation matrix for angular velocities from inertial frame
% to body frame
W = [ 1,  0,        -sin(theta);
      0,  cos(phi),  cos(theta)*sin(phi);
      0, -sin(phi),  cos(theta)*cos(phi) ];

% Rotation matrix R_ZYX from body frame to inertial frame
R = rotationMatrixEulerZYX(phi,theta,psi);

慣性座標系における回転エネルギーを表すのに使用するヤコビ行列 $J = W_{η}^{T} I W_{η}$ を求めます。

% Create symbolic variables for diagonal elements of inertia matrix
syms Ixx Iyy Izz

% Jacobian that relates body frame to inertial frame velocities
I = [Ixx, 0, 0; 0, Iyy, 0; 0, 0, Izz];
J = W.'*I*W;

次に、角度に関するオイラー・ラグランジュ方程式のジャイロ項と求心項を含むコリオリ行列 $C (η, \dot{η}) = \frac{d}{dt} J - \frac{1}{2} \frac{\partial}{\partial η} ({\dot{η}}^{T} J)$ を求めます。シンボリック関数 diff および jacobian を使用して微分演算を実行します。微分後のコリオリ行列の表記を単純化するため、 $C (η, \dot{η})$ の明示的な時間依存項を (特定の時刻における値を表す) シンボリック変数に置き換えます。この単純化された表記によって、ここで得られた結果と [1] の導出結果の比較が簡単になります。

% Coriolis matrix
dJ_dt = diff(J);
h_dot_J = [diff(phi,t), diff(theta,t), diff(psi,t)]*J;
grad_temp_h = transpose(jacobian(h_dot_J,[phi theta psi]));
C = dJ_dt - 1/2*grad_temp_h;
C = subsStateVars(C,t);

コリオリ行列の定義が不変であることの証明

前のセクションのシンボリックワークフローを使用すると、コリオリ行列 $C (η, \dot{η})$ を簡単に導出できます。ただし、ここでの $C (η, \dot{η})$ の定義は、クリストッフェル記号を使用してコリオリ行列を導出する [1] の定義と異なります。 $C (η, \dot{η})$ は一意ではないため、複数の方法でこれを定義できます。しかし、オイラー・ラグランジュ方程式で使用される項 $C (η, \dot{η}) \dot{η}$ は一意でなければなりません。このセクションでは、項 $C (η, \dot{η}) \dot{η}$ が 2 つの定義で数学的に等しいことを示し、 $C (η, \dot{η})$ のシンボリック定義が [1] の定義と一致することを証明します。

前のセクションで導出された $C (η, \dot{η})$ のシンボリック定義を使用して、項 $C (η, \dot{η}) \dot{η}$ を評価します。

% Evaluate C times etadot using symbolic definition
phidot   = subsStateVars(diff(phi,t),t);
thetadot = subsStateVars(diff(theta,t),t);
psidot   = subsStateVars(diff(psi,t),t);
Csym_etadot = C*[phidot; thetadot; psidot];

[1] で導出された $C (η, \dot{η})$ の定義を使用して、項 $C (η, \dot{η}) \dot{η}$ を評価します。

% Evaluate C times etadot using reference paper definition
C11 = 0;
C12 = (Iyy - Izz)*(thetadot*cos(phi)*sin(phi) + psidot*sin(phi)^2*cos(theta)) ...
      + (Izz - Iyy)*(psidot*cos(phi)^2*cos(theta)) - Ixx*psidot*cos(theta);
C13 = (Izz - Iyy)*psidot*cos(phi)*sin(phi)*cos(theta)^2;
C21 = (Izz - Iyy)*(thetadot*cos(phi)*sin(phi) + psidot*sin(phi)^2*cos(theta)) ...
      + (Iyy - Izz)*psidot*cos(phi)^2*cos(theta) + Ixx*psidot*cos(theta);
C22 = (Izz - Iyy)*phidot*cos(phi)*sin(phi);
C23 = - Ixx*psidot*sin(theta)*cos(theta) + Iyy*psidot*sin(phi)^2*sin(theta)*cos(theta) ...
      + Izz*psidot*cos(phi)^2*sin(theta)*cos(theta);
C31 = (Iyy - Izz)*psidot*cos(theta)^2*sin(phi)*cos(phi) - Ixx*thetadot*cos(theta);
C32 = (Izz - Iyy)*(thetadot*cos(phi)*sin(phi)*sin(theta) + phidot*sin(phi)^2*cos(theta)) ...
      + (Iyy - Izz)*phidot*cos(phi)^2*cos(theta) + Ixx*psidot*sin(theta)*cos(theta) ...
      - Iyy*psidot*sin(phi)^2*sin(theta)*cos(theta) - Izz*psidot*cos(phi)^2*sin(theta)*cos(theta);
C33 = (Iyy - Izz)*phidot*cos(phi)*sin(phi)*cos(theta)^2 ...
      - Iyy*thetadot*sin(phi)^2*cos(theta)*sin(theta) ...
      - Izz*thetadot*cos(phi)^2*cos(theta)*sin(theta) + Ixx*thetadot*cos(theta)*sin(theta);
Cpaper = [C11, C12, C13; C21, C22, C23; C31 C32 C33];
Cpaper_etadot = Cpaper*[phidot; thetadot; psidot];
Cpaper_etadot = subsStateVars(Cpaper_etadot,t);

どちらの定義も項 $C (η, \dot{η}) \dot{η}$ に関して同じ結果になることを示します。

tf_Cdefinition = isAlways(Cpaper_etadot == Csym_etadot)

tf_Cdefinition = 3x1 logical array

   1
   1
   1

運動方程式の導出

12 個の状態に関する運動方程式を求めます。

[1] に従って、ロール角方向、ピッチ角方向、ヨー角方向のトルク $τ_{B}$ を求めます。

ロールの動きは、第 2 ローターの速度を下げ、第 4 ローターの速度を上げることで設定します。
ピッチの動きは、第 1 ローターの速度を下げ、第 3 ローターの速度を上げることで設定します。
ヨーの動きは、対角線上にある 2 つのローターの速度を上げ、他の 2 つのローターの速度を下げることで設定します。

ボディの $z$ 軸方向の総ローター推力 $T$ を求めます。

% Define fixed parameters and control input
% k: lift constant
% l: distance between rotor and center of mass
% m: quadrotor mass
% b: drag constant
% g: gravity
% ui: squared angular velocity of rotor i as control input
syms k l m b g u1 u2 u3 u4

% Torques in the direction of phi, theta, psi
tau_beta = [l*k*(-u2+u4); l*k*(-u1+u3); b*(-u1+u2-u3+u4)];

% Total thrust
T = k*(u1+u2+u3+u4);

次に、時間に依存する位置を表すシンボリック関数を作成します。直線位置、角度、およびそれらの時間微分に関する 12 個の状態 $[ξ; η; \dot{ξ}; \dot{η}]$ を定義します。状態を定義したら、明示的な時間依存項を置き換えて表記を単純化します。

% Create symbolic functions for time-dependent positions
syms x(t) y(t) z(t)

% Create state variables consisting of positions, angles,
% and their derivatives
state = [x; y; z; phi; theta; psi; diff(x,t); diff(y,t); ...
    diff(z,t); diff(phi,t); diff(theta,t); diff(psi,t)];
state = subsStateVars(state,t);

12 個の状態 $f = [\dot{ξ}; \dot{η}; \ddot{ξ}; \ddot{η}]$ の時間微分を表す 12 個の運動方程式を作成します。直線加速度の微分方程式は $\ddot{ξ} = - {(0, 0, g)}^{T} + {R (0, 0, T)}^{T} / m$ となります。角加速度の微分方程式は $\ddot{η} = J^{- 1} (τ_{B} - C (η, \dot{η}) \dot{η})$ となります。明示的な時間依存項を置き換えて表記を単純化します。

f = [ % Set time-derivative of the positions and angles
      state(7:12);

      % Equations for linear accelerations of the center of mass
      -g*[0;0;1] + R*[0;0;T]/m;

      % Euler–Lagrange equations for angular dynamics
      inv(J)*(tau_beta - C*state(10:12))
];

f = subsStateVars(f,t);

前の手順において、[1] の導出に近づけるため、固定パラメーターをシンボリック変数として定義しました。次に、この固定パラメーターを指定された値に置き換えます。これらの値は、特定の構成をもつクワッドローターの軌跡追跡コントローラーを設計するために使用します。固定パラメーターを置き換えたら、simplify を使用して運動方程式において代数的な単純化を実行します。

% Replace fixed parameters with given values here
IxxVal = 1.2;
IyyVal = 1.2;
IzzVal = 2.3;
kVal = 1;
lVal = 0.25;
mVal = 2;
bVal = 0.2;
gVal = 9.81;

f = subs(f, [Ixx Iyy Izz k l m b g], ...
    [IxxVal IyyVal IzzVal kVal lVal mVal bVal gVal]);
f = simplify(f);

非線形モデルの予測制御に関するヤコビアンの導出とファイルの生成

最後に、Symbolic Math Toolbox を使用し、非線形モデル関数の解析ヤコビアンを求めて MATLAB® ファイルを生成します。この手順は、Model Predictive Control Toolbox™ を使用して軌跡を追跡するための非線形モデルを解く際の計算効率を高めるために重要です。詳細については、nlmpc (Model Predictive Control Toolbox)およびSpecify Prediction Model for Nonlinear MPC (Model Predictive Control Toolbox)を参照してください。

状態変数と制御入力について、状態関数のヤコビアンを求めます。

% Calculate Jacobians for nonlinear prediction model
A = jacobian(f,state);
control = [u1; u2; u3; u4];
B = jacobian(f,control);

状態関数と状態関数のヤコビアンの MATLAB ファイルを生成します。これらのファイルは、Control of Quadrotor Using Nonlinear Model Predictive Control (Model Predictive Control Toolbox)で説明されているように、軌跡追跡コントローラーの設計で使用できます。

% Create QuadrotorStateFcn.m with current state and control
% vectors as inputs and the state time-derivative as outputs
matlabFunction(f,'File','QuadrotorStateFcn', ...
    'Vars',{state,control});

% Create QuadrotorStateJacobianFcn.m with current state and control
% vectors as inputs and the Jacobians of the state time-derivative
% as outputs
matlabFunction(A,B,'File','QuadrotorStateJacobianFcn', ...
    'Vars',{state,control});

このスクリプトのコードは、getQuadrotorDynamicsAndJacobian.m ファイルにあります。

ローカル関数

function [Rz,Ry,Rx] = rotationMatrixEulerZYX(phi,theta,psi)
% Euler ZYX angles convention
    Rx = [ 1,           0,          0;
           0,           cos(phi),  -sin(phi);
           0,           sin(phi),   cos(phi) ];
    Ry = [ cos(theta),  0,          sin(theta);
           0,           1,          0;
          -sin(theta),  0,          cos(theta) ];
    Rz = [cos(psi),    -sin(psi),   0;
          sin(psi),     cos(psi),   0;
          0,            0,          1 ];
    if nargout == 3
        % Return rotation matrix per axes
        return;
    end
    % Return rotation matrix from body frame to inertial frame
    Rz = Rz*Ry*Rx;
end

function stateExpr = subsStateVars(timeExpr,var)
    if nargin == 1 
        var = sym("t");
    end
    repDiff = @(ex) subsStateVarsDiff(ex,var);
    stateExpr = mapSymType(timeExpr,"diff",repDiff);
    repFun = @(ex) subsStateVarsFun(ex,var);
    stateExpr = mapSymType(stateExpr,"symfunOf",var,repFun);
    stateExpr = formula(stateExpr);
end

function newVar = subsStateVarsFun(funExpr,var)
    name = symFunType(funExpr);
    name = replace(name,"_Var","");
    stateVar = "_" + char(var);
    newVar = sym(name + stateVar);
end

function newVar = subsStateVarsDiff(diffExpr,var)
    if nargin == 1 
      var = sym("t");
    end
    c = children(diffExpr);
    if ~isSymType(c{1},"symfunOf",var)
      % not f(t)
      newVar = diffExpr;
      return;
    end
    if ~any([c{2:end}] == var)
      % not derivative wrt t only
      newVar = diffExpr;
      return;
    end
    name = symFunType(c{1});
    name = replace(name,"_Var","");
    extension = "_" + join(repelem("d",numel(c)-1),"") + "ot";
    stateVar = "_" + char(var);
    newVar = sym(name + extension + stateVar);
end

参考文献

[1] Raffo, Guilherme V., Manuel G. Ortega, and Francisco R. Rubio. "An integral predictive/nonlinear $H_{\infty}$ 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.

参考

diff | jacobian | matlabFunction