系列文章目錄
?文章來源地址http://www.zghlxwxcb.cn/news/detail-813965.html
前言
本例演示了如何使用 Symbolic Math Toolbox?（符號(hào)數(shù)學(xué)工具箱）推導(dǎo)四旋翼飛行器的連續(xù)時(shí)間非線性模型。具體來說，本例討論了 getQuadrotorDynamicsAndJacobian 腳本，該腳本可生成四旋翼狀態(tài)函數(shù)及其雅各布函數(shù)。這些函數(shù)將在使用非線性模型預(yù)測(cè)控制（模型預(yù)測(cè)控制工具箱）控制四旋翼飛行器的示例中使用。
四旋翼飛行器又稱四旋翼直升機(jī)，是一種擁有四個(gè)旋翼的直升機(jī)。從四旋翼飛行器的質(zhì)量中心出發(fā)，旋翼呈等距離的正方形排列。四旋翼的運(yùn)動(dòng)是通過調(diào)整四個(gè)旋翼的角速度來控制的，而四個(gè)旋翼是由電動(dòng)馬達(dá)帶動(dòng)旋轉(zhuǎn)的。四旋翼飛行器動(dòng)力學(xué)數(shù)學(xué)模型可從牛頓-歐拉方程和歐拉-拉格朗日方程中導(dǎo)出 [1]。
?
?
一、定義狀態(tài)變量和參數(shù)
如圖所示，四旋翼飛行器有六個(gè)自由度（三個(gè)線性坐標(biāo)和三個(gè)角度坐標(biāo)），四個(gè)控制輸入。
四旋翼飛行器的 12 個(gè)狀態(tài)是
其中
表示線性位置。
分別表示滾轉(zhuǎn)角、俯仰角和偏航角。
表示線速度和角速度，或線性位置和角度位置的時(shí)間導(dǎo)數(shù)。
四旋翼飛行器的運(yùn)動(dòng)參數(shù)為
?代表四個(gè)旋翼的角速度平方。
?代表機(jī)身坐標(biāo)系中轉(zhuǎn)動(dòng)慣量矩陣的對(duì)角元素。
(k,l,m,b,g）代表升力常數(shù)、轉(zhuǎn)子與質(zhì)量中心的距離、四旋翼質(zhì)量、阻力常數(shù)和重力。
前四個(gè)參數(shù)??是控制輸入或控制四旋翼飛行器軌跡的操縱變量。其余參數(shù)固定為給定值。
二、定義變換矩陣和科里奧利矩陣
定義慣性坐標(biāo)系和主體坐標(biāo)系之間的變換矩陣。需要這些變換矩陣來描述四旋翼飛行器在兩個(gè)坐標(biāo)系中的運(yùn)動(dòng)。四旋翼飛行器的 12 個(gè)狀態(tài)在慣性系中定義，旋轉(zhuǎn)慣性矩陣在機(jī)身系中定義。
創(chuàng)建符號(hào)函數(shù)來表示隨時(shí)間變化的角度。在按照 [1] 進(jìn)行的數(shù)學(xué)推導(dǎo)中，需要這種明確的時(shí)間依賴性來評(píng)估時(shí)間導(dǎo)數(shù)。定義矩陣 Wη 以將角速度從慣性系轉(zhuǎn)換到體坐標(biāo)系。定義旋轉(zhuǎn)矩陣 R，使用局部函數(shù)部分定義的 rotationMatrixEulerZYX 函數(shù)將線性力從機(jī)身坐標(biāo)系轉(zhuǎn)換到慣性坐標(biāo)系。創(chuàng)建符號(hào)矩陣來表示這些變換矩陣。
% 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);?求用于表示慣性系中旋轉(zhuǎn)能量的雅各布矩陣 。
% 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;接下來，找出科里奧利矩陣 ，它包含角歐拉-拉格朗日方程中的陀螺項(xiàng)和向心項(xiàng)。使用符號(hào) diff 和 jacobian 函數(shù)進(jìn)行微分運(yùn)算。為了簡(jiǎn)化微分后科里奧利矩陣的符號(hào)，可以用 中的顯式時(shí)間相關(guān)項(xiàng)替換為符號(hào)變量（代表特定時(shí)間的某些值）。這種簡(jiǎn)化的符號(hào)使這里的結(jié)果更容易與 [1] 中的推導(dǎo)進(jìn)行比較。?
% 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);三、證明科里奧利矩陣定義是一致的
科里奧利矩陣 很容易用上一節(jié)的符號(hào)工作流程推導(dǎo)出來。然而，這里的 定義與 [1] 中的不同，后者使用克里斯托弗符號(hào)推導(dǎo)科里奧利矩陣。由于 ?并非唯一，因此可以有多種定義方法。但是，歐拉-拉格朗日方程中使用的 項(xiàng)必須是唯一的。本節(jié)將證明 的符號(hào)定義與 [1] 中的定義是一致的，即 項(xiàng)在兩個(gè)定義中在數(shù)學(xué)上是相等的。
利用上一節(jié)中得出的 的符號(hào)定義來評(píng)估 ?項(xiàng)。
% 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] 中得出的 的定義，對(duì) 項(xiàng)進(jìn)行評(píng)估。
% 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);證明這兩個(gè)定義對(duì) 項(xiàng)給出了相同的結(jié)果。
tf_Cdefinition = isAlways(Cpaper_etadot == Csym_etadot)tf_Cdefinition = 3x1 logical array
1
1
1
四、查找運(yùn)動(dòng)方程
求出 12 個(gè)狀態(tài)的運(yùn)動(dòng)方程。
根據(jù) [1]，求出扭矩 τ B 在滾轉(zhuǎn)、俯仰和偏航角方向上的扭矩：
- 通過降低第 2 個(gè)轉(zhuǎn)子的速度和提高第 4 個(gè)轉(zhuǎn)子的速度來設(shè)定滾轉(zhuǎn)運(yùn)動(dòng)。
- 通過降低第 1 個(gè)轉(zhuǎn)子的速度和提高第 3 個(gè)轉(zhuǎn)子的速度來設(shè)置俯仰運(yùn)動(dòng)。
- 偏航運(yùn)動(dòng)是通過增大兩個(gè)相對(duì)旋翼的速度和減小另外兩個(gè)旋翼的速度來實(shí)現(xiàn)的。
求轉(zhuǎn)子 T 在機(jī)身 Z 軸方向上的總推力。
% 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);接下來，創(chuàng)建符號(hào)函數(shù)來表示隨時(shí)間變化的位置。為線性位置、角度及其時(shí)間導(dǎo)數(shù)定義 12 個(gè)狀態(tài) 。定義好狀態(tài)后，用顯式時(shí)間項(xiàng)代替，以簡(jiǎn)化符號(hào)。
% 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 個(gè)狀態(tài)? 的時(shí)間導(dǎo)數(shù)的 12 個(gè)運(yùn)動(dòng)方程。線性加速度的微分方程為 ，角加速度的微分方程為 。代入明確的時(shí)間相關(guān)項(xiàng)，以簡(jiǎn)化符號(hào)。
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);在前面的步驟中，固定參數(shù)被定義為符號(hào)變量，以緊跟文獻(xiàn) [1] 的推導(dǎo)。接下來，用給定值替換固定參數(shù)。這些值用于設(shè)計(jì)四旋翼飛行器特定配置的軌跡跟蹤控制器。替換固定參數(shù)后，使用簡(jiǎn)化對(duì)運(yùn)動(dòng)方程進(jìn)行代數(shù)簡(jiǎn)化。
% 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);五、為非線性模型預(yù)測(cè)控制查找雅各布因子并生成文件
最后，使用符號(hào)數(shù)學(xué)工具箱查找非線性模型函數(shù)的解析雅各布因子并生成 MATLAB? 文件。這一步驟對(duì)于提高使用模型預(yù)測(cè)控制工具箱? 解決軌跡跟蹤非線性模型時(shí)的計(jì)算效率非常重要。更多信息，請(qǐng)參閱 nlmpc（模型預(yù)測(cè)控制工具箱）和 Specify Prediction Model for Nonlinear MPC（模型預(yù)測(cè)控制工具箱）。
查找狀態(tài)函數(shù)相對(duì)于狀態(tài)變量和控制輸入的雅各布。
% Calculate Jacobians for nonlinear prediction model
A = jacobian(f,state);
control = [u1; u2; u3; u4];
B = jacobian(f,control);生成狀態(tài)函數(shù)和狀態(tài)函數(shù) Jacobians 的 MATLAB 文件。這些文件可用于設(shè)計(jì)軌跡跟蹤控制器，詳見《使用非線性模型預(yù)測(cè)控制（模型預(yù)測(cè)控制工具箱）控制四旋翼飛行器》。
% 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 文件中訪問該腳本中的代碼。
六、函數(shù)
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??∞?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-813965.html
?
到了這里，關(guān)于MATLAB - 四旋翼飛行器動(dòng)力學(xué)方程的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
