四旋翼動力學和仿真翻譯（Quadcopter Dynamics and Simulation）

本文翻譯自Andrew Gibiansky的同名文章，該文獻介紹了四旋翼的動力學模型和Matlab仿真的具體實現，對四旋翼入門非常有好處。原文如下
http://andrew.gibiansky.com/blog/physics/quadcopter-dynamics/

由於Neo已經於2014年對該文章前半部分進行了翻譯（從Introduction到Physics），因此本文將從Simulation部分開始翻譯。已經翻譯部分如下

http://share.weiyun.com/9c1869b64a76fea67047adedc2f2eb14

本文的翻譯工作由BUAA飛機總體設計HT11課程小組成員完成，具體分工如下:

孟德超 Page6~8，simulation和Control（PD control之前）
李志宇 page 8~11 PD control
劉倚銘 page11-14 PID control，
唐既堯 關智元 周震 page15 ~18 Automatic PID Tuning & conclusion

仿真

既然我們已經建立了描述此系統動力學的完整的方程組，因此我們可以搭建一個仿真環境，從而我們可以在仿真環境中實時觀察到我們的變量和控制器的變化。盡管有很多更先進的方法可以選擇，我們仍可以通過歐拉方程建立差分方程，進而模擬不同系統狀態的發展情況。在Matlab中，這個仿真環境的部分代碼如下

% Simulation times, in seconds.
start_time = 0;
end_time = 10;
dt = 0.005;
times = start_time:dt:end_time;

% Number of points in the simulation.
N = numel(times);

% Initial simulation state.
x = [0; 0; 10];
xdot = zeros(3, 1);
theta = zeros(3, 1);

% Simulate some disturbance in the angular velocity.
% The magnitude of the deviation is in radians / second.
deviation = 100;
thetadot = deg2rad(2 * deviation * rand(3, 1) - deviation);

% Step through the simulation, updating the state.
for t = times
    % Take input from our controller.
    i = input(t);

    omega = thetadot2omega(thetadot, theta);

    % Compute linear and angular accelerations.
    a = acceleration(i, theta, xdot, m, g, k, kd);
    omegadot = angular_acceleration(i, omega, I, L, b, k);

    omega = omega + dt * omegadot;
    thetadot = omega2thetadot(omega, theta); 
    theta = theta + dt * thetadot;
    xdot = xdot + dt * a;
    x = x + dt * xdot;
end

我們還需要計算力和力矩的函數

% Compute thrust given current inputs and thrust coefficient.
function T = thrust(inputs, k)
    % Inputs are values for ${\omega_i}^2$
    T = [0; 0; k * sum(inputs)];
end

% Compute torques, given current inputs, length, drag coefficient, and thrust coefficient.
function tau = torques(inputs, L, b, k)
    % Inputs are values for ${\omega_i}^2$
    tau = [
        L * k * (inputs(1) - inputs(3))
        L * k * (inputs(2) - inputs(4))
        b * (inputs(1) - inputs(2) + inputs(3) - inputs(4))
    ];
end

function a = acceleration(inputs, angles, xdot, m, g, k, kd)
    gravity = [0; 0; -g];
    R = rotation(angles);
    T = R * thrust(inputs, k);
    Fd = -kd * xdot;
    a = gravity + 1 / m * T + Fd;
end

function omegadot = angular_acceleration(inputs, omega, I, L, b, k)
    tau = torques(inputs, L, b, k);
    omegaddot = inv(I) * (tau - cross(omega, I * omega));
end

現在我們還需要的是一些物理常數，一個計算旋轉矩陣的函數，以及一個把\(\omega\)轉化為滾轉，俯仰和偏航（roll,pitch,yaw）角的微分的函數以及它的反函數。這些函數沒有寫明。我們可以繪制一個三維坐標下的可視化的四旋翼並且讓它在仿真運行時不停更新自己的位置。
注意：雖然原文沒有寫明該部分的實現，但是考慮到matlab仿真並非易事，此處給出一個譯者實現的源代碼鏈接：http://pan.baidu.com/s/1slDZWK1 密碼：zb5o。

四旋翼仿真。每個螺旋槳上的長條粗略的表示其相對升力大小。

控制

推導四旋翼的數學模型是為了協助我們為真實的四旋翼開發一個控制器。系統的輸入包括四個電機的角速度，因為我們可以通過控制電壓輸出值直接控制角速度。注意在我們的簡化模型里，我們僅僅采用角速度的平方\({\omega_i}^2\)而不使用角速度本身。為了計數簡單，我們引入輸入變量\(\gamma_i={\omega_i}^2\)。這是因為既然我們可以直接控置\({\omega_i}\)，那我們也可以直接控制\(\gamma_i\)。這樣，我們就可以將我們的系統描述為狀態空間中的一階微分方程。令
\(x_1\)為四旋翼在空間中的位置，\(x_2\)為四旋翼的線速度，\(x_3\)為四旋翼的滾轉、俯仰、偏航角，\(x_4\)為角速度。（注意這些變量都是三維向量）。有了這些變量，我們可以寫出狀態空間方程如下：

\[\begin{aligned} \dot{x_1} &= x_2 \\ \dot{x_2} &= {\begin{bmatrix} 0 \\ 0 \\ -g\end{bmatrix}} + \frac{1}{m} RT_B + \frac{1}{m} F_D \\ \dot{x_3} &= {\begin{bmatrix} 1 &0 & -s_\theta \\ 0 & c_\phi & c_\theta s_\phi \\ 0 & -s_\phi & c_\theta c_\phi \end{bmatrix}}^{-1} x_4 \\ \dot{x_4} &= {\begin{bmatrix} \tau_\phi {I_{xx}}^{-1} \\ \tau_\theta {I_{yy}}^{-1} \\ \tau_\psi {I_{zz}}^{-1} \end{bmatrix}} - {\begin{bmatrix} \frac{I_{yy} - I_{zz}}{I_{xx}} \omega_y\omega_z \\ \frac{I_{zz} - I_{xx}}{I_{yy}} \omega_x\omega_z \\ \frac{I_{xx} - I_{yy}}{I_{zz}} \omega_x\omega_y \end{bmatrix}}\end{aligned} \]

注意我們的輸入並沒有直接作用在這些方程上。
然而，正如我們所看到的，我們可以選擇\(\tau\)和\(T\)的值，然后解出\(\gamma_i\)的值。

PD控制

為了控制四軸飛行器，我們將使用一個PD控制器。給期望的軌跡與觀測到的軌跡之間的誤差和該誤差的導數各加一個比例環節。我們的四軸飛行器將只有一個陀螺，所以在控制器中只能使用角度導數\(\dot \phi,\dot \theta,\dot \phi\)來控制飛行器；這些測量值提供誤差的導數，其積分將為我們提供實際的誤差。我們想讓直升機穩定在一個水平位置,所以我們期望的速度和角度都是零。扭矩與角速度有關：\(\tau=I \ddot \theta\) ，所以我們將力矩設置成與控制器的輸出成正比，即 \(\tau=Iu(t)\)。因此

\[{\begin{bmatrix} \tau_\phi \\ \tau_\theta \\ \tau_\psi\end{bmatrix}} = {\begin{bmatrix} -I_{xx} \left( K_d\dot\phi + K_p \int_0^T \dot\phi {\text{d}\;}t \right) \\ -I_{yy} \left( K_d\dot\theta + K_p \int_0^T \dot\theta {\text{d}\;}t \right) \\ -I_{zz} \left( K_d\dot\psi + K_p \int_0^T \dot\psi {\text{d}\;}t \right) \\ \end{bmatrix}} \]

我們先前推導出轉矩和輸入之間的關系,所以我們知道

\[\tau_B = {\begin{bmatrix} Lk({\gamma_1} - {\gamma_3}) \\ Lk({\gamma_2} - {\gamma_4}) \\ b\left( {\gamma_1} - {\gamma_2} + {\gamma_3} - {\gamma_4}\right) \end{bmatrix}} = {\begin{bmatrix} -I_{xx} \left( K_d\dot\phi + K_p \int_0^T \dot\phi {\text{d}\;}t \right) \\ -I_{yy} \left( K_d\dot\theta + K_p \int_0^T \dot\theta {\text{d}\;}t \right) \\ -I_{zz} \left( K_d\dot\psi + K_p \int_0^T \dot\psi {\text{d}\;}t \right) \\ \end{bmatrix}} \]

這給了我們一組包含四個未知數的三個方程。我們可以加一個限制條件，即輸入必須保持四軸飛行器懸浮在空中:

\[T=mg \]

注意，這個方程忽視了一個事實：推力沒有直接給出。這會限制我們的控制器的適用性，但不會對小的穩定偏離造成重大問題。如果我們有辦法確定當前角度的准確值，我們就可以補償這個不足。如果我們的陀螺足夠精確,我們可以用從陀螺采集到的數據進行積分，得到角度θ和φ。在這種情況下，我們可以把mg投影到z軸上來計算保持四旋翼懸浮所需的推力。我們發現

\[T_{proj}=mgcos\theta cos\phi \]

因此，有精密的角度測量，我們就可以要求推力等於

\[T = \frac{mg}{cos\theta cos\phi} \]

在這種情況下,指向正z軸的推力分量就等於mg。我們知道推力正比於輸入的加權總和:

\[T = \frac{mg}{\cos\theta\cos\phi} = k\sum \gamma_i \implies \sum\gamma_i = \frac{mg}{k\cos\theta\cos\phi} \]

這個額外的約束,我們有一組包含四個未知數 的四個線性方程。我們
可以求解每個未知量 ,並獲得以下輸入值:

\[\begin{aligned} \gamma_1 &= \frac{mg}{4k\cos\theta\cos\phi}-\frac{2 b {e_\phi} {I_{xx}}+{e_\psi} {I_{zz}} k L}{4 b k L} \\ \gamma_2 &= \frac{mg}{4k\cos\theta\cos\phi}+\frac{ {e_\psi} {I_{zz}}}{4 b}-\frac{ {e_\theta} {I_{yy}}}{2 k L} \\ \gamma_3 &= \frac{mg}{4k\cos\theta\cos\phi}-\frac{-2 b {e_\phi} {I_{xx}}+{e_\psi} {I_{zz}} k L}{4 b k L} \\ \gamma_4 &= \frac{mg}{4k\cos\theta\cos\phi}+\frac{ {e_\psi} {I_{zz}}}{4 b}+\frac{ {e_\theta} {I_{yy}}}{2 k L}\end{aligned}\]

這是一個PD控制器的完整描述。我們可以使用我們的仿真環境來模擬這個控制器。控制器使角速度和角度為零。

左:角速度。右:角位移。θ,φψ是依次為紅、綠、藍色的。
然而,注意角度最終不是完全為零。平均穩態誤差(仿真10秒后的誤差)大約是\(0.3 ^\circ\)。
對機械系統使用PD控制器這是一個常見的問題，可以用PID控制器改善,就像我們將在下一節中討論的那樣。
此外,請注意,由於我們只有控制角速度，位置和線速度不收斂於零。然而,z軸上的位置保持不變，因為我們用 “保持四軸飛行器懸浮在空中” 限制了總的垂直推力，沒有上升或下降。然而,這只不過是滿足好奇心罷了。除了有限的傳感器（陀螺儀）,對四軸飛行器的線性位置和速度控制我們什么也做不了。雖然在理論上我們可以用角速度計算位置和線速度，但實際中值的噪聲太大而完全不可行。因此，我們將限制自己去穩定四軸飛行器角度和角速度（傳統上，導航是由人完成，穩定只是為了使對操縱手來說更加簡單。）
我們在仿真中實現了這個PD控制。控制器的實現作為一種手段，它被給定一些狀態（對應控制器狀態,而非系統狀態）和傳感器輸入，必須計算出輸入γ我和更新后的狀態。下面給出了PD控制的代碼。

% Compute system inputs and updated state.
% Note that input = [$\gamma_1$, $\ldots$, $\gamma_4$]
function [input, state] = pd_controller(state, thetadot)
    % Controller gains, tuned by hand and intuition.
    Kd = 4;
    Kp = 3;

    % Initialize the integral if necessary.
    if ~isfield(state, 'integral')
        params.integral = zeros(3, 1);
    end

    % Compute total thrust
    total = state.m * state.g / state.k / (cos(state.integral(1)) * cos(state.integral(2)));

    % Compute errors
    e = Kd * thetadot + Kp * params.integral;

    % Solve for the inputs, $\gamma_i$
    input = error2inputs(params, accels, total);

    % Update the state
    params.integral = params.integral + params.dt .* thetadot;
end

PID控制

PD控制器的優點是簡易，且易於實施，但是他們還不足以控制機械系統。特別是在有噪聲和干擾的情況下，PD控制器會產生穩態誤差。PID控制相對於PD控制，補充了對過程變量的積分比例項。增加的積分項會消除穩態誤差，所以一個PID控制器應該能夠跟蹤我們的軌跡並穩定四旋翼，穩態誤差也會顯著減小。方程與在PD的情況下給出的相同，但是增加了附加項的誤差：

\[\begin{aligned} e_\phi &= K_d\dot\phi + K_p \int_0^T\dot\phi{\text{d}\;}t + K_i \int_0^T\int_0^T\dot\phi{\text{d}\;}t{\text{d}\;}t \\ e_\theta &= K_d\dot\theta + K_p \int_0^T\dot\theta{\text{d}\;}t + K_i \int_0^T\int_0^T\dot\theta{\text{d}\;}t{\text{d}\;}t \\ e_\psi &= K_d\dot\psi + K_p \int_0^T\dot\psi{\text{d}\;}t + K_i \int_0^T\int_0^T\dot\psi{\text{d}\;}t{\text{d}\;}t \\\end{aligned} \]

然而，PID控制也有弊端，通常出現積分飽和的問題。

在某些情況下，積分飽和會導致長時間的振盪。在其他情況下，積分飽和最終可能導致系統不穩定，並不會花費更長的時間去達到穩定狀態。
如果對過程變量的擾動較大，由於積分項的存在，這個大擾動對時間積分，將成為一個更大的控制信號。然而，即使系統穩定，積分結果仍然很大，從而造成控制器超調。然后可能開始一系列逐漸變弱的振盪，變得不穩定，或者經過相當長的時間達到穩定狀態。為了避免這種情況，在接近接近穩定狀態之前，我們要禁用積分環節。一旦我們離穩定狀態的區域是可控制的，我們將啟用積分環節，這樣促使系統朝向一個更低的穩態誤差。

通過恰當的PID控制，在10秒后，誤差大約是0.06 °。
我們已經實現了用PID控制仿真，與前面的PD控制器一樣，有一個額外的參數需要調整。為了對比控制器的效果，用於所有測試的干擾都是相同的。

% Compute system inputs and updated state.
% Note that input = [$\gamma_1$, $\ldots$, $\gamma_4$] 
function [input, state] = pid_controller(state, thetadot)     
% Controller gains, tuned by hand and intuition.     
Kd = 4;     
Kp = 3;     
Ki = 5.5;      

% Initialize the integral if necessary.     
if ~isfield(state, 'integral')       
  params.integral = zeros(3, 1);       
  params.integral2 = zeros(3, 1);     
end      
% Prevent wind-up     
if max(abs(params.integral2)) > 0.01       
  params.integral2(:) = 0;     
end      
% Compute total thrust     
total=state.m*state.g / state.k / (cos(state.integral(1))* cos(state.integral(2)));      
% Compute errors     
e = Kd * thetadot + Kp * params.integral - Ki * params.integral2;      
% Solve for the inputs, $\gamma_i$     
input = error2inputs(params, accels, total);      
% Update the state     
params.integral = params.integral + params.dt .* thetadot;     
params.integral2 = params.integral2 + params.dt .* params.integral; end

自動PID調節

雖然PID控制有非常大的潛力，但是其控制品質非常依賴於我們給定的增益參數。鑒於增益參數之間的比率和增益參數本身一樣重要，手動調節這些參數可能非常困難。在大多數情況下，調節這些參數需要對整個系統運作有充分的理解和把握，並且了解各個PID參數具體的使用條件。之前這些增益參數就是通過手動調節的，通過做模擬仿真的方式，最終選擇那些能使系統正常工作的相對合理的值，整個過程中合理的終值可能不止一個，而模擬仿真實際總是伴隨着很多潛在的干擾。這種方式顯然不是最優的，不僅因為它非常困難而且工作量非常大，也因為最終的終值在最優性上沒有任何的保障。

理想層面上，我們總是樂意能夠使用一種算法去分析一個系統然后得出這些最優的PID增益參數。有人深入研究了這個問題，然后提出了許多算法。其中一些算法需要對被仿真系統有全面的認識，其中一些只依賴於這個系統的某些特性，比如穩定性和線性。我們來確定PID參數的算法是知名的極值搜索法。

極值搜索法，顧名思義，我們將參數的理想化標准定義為一些向量\(\vec \theta = (K_p, K_i, K_d)\)，這可以使代價函數 \(J(\vec\theta)\)達到最小化。在我們的實例中，我們定義一個價值函數用來補償那些不能忽視的誤差以及超時誤差。一個候選的代價函數給出如下

\[J(\vec\theta) = {\frac{1}{t_f - t_o}} \int_{t_0}^{t_f} e(t, \vec\theta)^2 {\text{d}\;}t \]

其中，\(e(t, \vec\theta)\)是指在帶初始干擾的軌跡跟蹤誤差，初始干擾用向量\(\vec \theta\)表示。設想如果我們能夠通過某種方式計算這個代價函數的梯度\(\nabla J(\vec\theta)\)，那樣的話，我們先定義一個參數迭代公式
\(\vec\theta(k + 1) = \vec\theta(k) - \alpha \nabla J(\vec \theta)\)
然后就可以通過反復迭代的方式來提高我們參數的品質了。
上式中\(\vec \theta(k)\)是經過\(k\)次迭代后的參數向量， \(α\)是步長，它規定了每次迭代后我們要對我們的參數向量做出多大的調整。當\(k\to\infty\)時，代價函數 \(J(\vec\theta)\)將會趨近於PID取值空間中的一個局部最小值。

留給我們的問題依然是我們如何才能估計\(\nabla J(\vec\theta)\)呢？答案是通過定義的方式。

\[\nabla J(\vec\theta) = \left({\frac{\partial }{\partial K_p}} J(\vec\theta), {\frac{\partial }{\partial K_i}} J(\vec\theta), {\frac{\partial }{\partial K_d}} J(\vec\theta)\right).\]

我們定義了一個\(J(\vec\theta)\)，
我們已經知道怎么計算\(J(\vec\theta)\)了，通過這個，我們就可以得到任何增益參數的導數的數值近似值，只需要通過計算以下公式就行了

\[{\frac{\partial }{\partial K}} J(\vec\theta) \approx \frac{J(\vec\theta + \delta\cdot \hat{u}_K) - J(\vec\theta- \delta\cdot \hat{u}_K)}{2\delta}\]

其中\(\hat{u}_K\)是\(K\)方向的單位向量，當\(δ\)趨近於0時，上述近似值變得更好。通過這個近似值，我們就可以最小化我們的代價函數然后得出局部的最佳PID參數。我們可以從一些隨機的初始化正權因子開始計算，然后將我們規定好的擾動施加於系統上，通過使用不同PID參數對系統進行模擬來估計\(J(\vec\theta)\)的值，之后我們就可以計算梯度了。然后我們使用梯度下降法通過迭代來不斷改善我們的增益參數直到得到某種形式的收斂。

然而，梯度下降法實際存在一些問題。首先，雖然通過它我們找到一個局部最小值，但這個值僅僅只能保證是局部最小的——在全局上面可能還存在其他更好的最小值，我們找到的並非是全局最小值。為了避免我們選擇的值非最優的這種情況，我們重復了最優化方案很多次，然后選擇其中最好的結果。我們的PID參數初始化是隨機的，因此每次我們通過最優化方案都會得到一個不同的結果。另外，我們並沒有采用固定的擾動然后去求在這種擾動下的最優響應，我們選用的擾動在單步迭代過程中每一步都是隨機的，然后使用其響應的平均值去計算代價和梯度。這樣就保證了我們的參數是具有通用性的而不是針對某一特定的擾動。除此之外，我們還將單步迭代的步長和擾動數設置為不一致，這是為了增加我們迭代結果的靈敏度。當我們檢測到了一個穩定態時迭代就會停止，我們可以對歷史代價函數值做一個線性回歸統計計算，然后不停迭代直到變化斜率在統計學上趨於0，我們使用了99%的置信區間。

通過我們的四旋翼仿真，我們可以定義一個函數來計算一組給定PID參數的代價。

function J = cost(theta)
    % Create a controller using the given gains.
    control = controller('pid', theta(1), theta(2), theta(3));

    % Perform a simulation.
    data = simulate(control);

    % Compute the integral, $\frac{1}{t_f - t_0} \int_{t_0}^{t_f} e(t)^2 dt$
    t0 = 0;
    tf = 1;
    J = 1/(tf - t0) * sum(data.theta(data.t >= t0 & data.t <= tf) .^ 2) * data.dt;
end

我們可以使用這個函數來得到增益值導數的近似值。

% Compute derivative with respect to first parameter.
delta = 0.01;
var = [1, 0, 0];
derivative = (cost(theta + delta * var) - cost(theta - delta * var)) / (2 * delta);

然后我們就可以使用梯度下降法來求代價函數的最小值然后得到一組優秀的PID參數。通過肉眼觀察和對比代價函數和迭代數值，我們可以驗證我們的方法是有效的，可以看到代價函數值確實變小然后在一個局部區域內達到穩定。

圖示為代價函數隨着迭代步數變化曲線，紅色線為擬合后的趨勢線，當趨勢線斜率在統計意義上為0時，迭代就停止了，使用了99%的置信區間。
 
我們將手動選擇的PID參數和這些算法計算出來的PID參數做個比較。

頂部：角速度和角位移，使用的是手動PID調節方式。
底部：角速度和角位移，使用的是算法PID調節方式。

總的來說，這種自動選擇PID的方式是更優的。它在數值上的振動更小，超調量更小，收斂速度也更快。然而，雖然通過梯度下降法選擇的參數初始收斂效果更佳，自動調節法的初值自動調節法的角位移的誤差收斂到0的耗時卻比手動調解法耗時更長。這是因為我們的代價函數重視的是方差，這樣的話最小化全局誤差就要優先於長期收斂。我們可以很容易地調整我們的代價函數來賦予長期誤差更高的優先級，這樣的話自動調解發可能表現更佳。

結論

我們導出了四旋翼的運動方程，通過無刷電機的電壓扭矩關系我們解決了四旋翼的運動學和動力學問題。我們忽略了一些空氣動力的影響比如四旋翼的揮舞運動和非零自由流速度，然后假設空氣摩擦在各個方向上都是線性的阻力。我們使用運動學方程建立了一個仿真器，通過這個仿真器我們就可以做測試，並且將四旋翼機構控制可視化。
我們首先做了PD調節控制器。雖然PD調節控制器有用，但是它卻帶有一個不可忽視的穩態誤差。為了減小這個穩態誤差，我們加入了積分項來建立PID調節控制器。我們測試了PID調節控制器然后發現當使用相同的擾動，比例增益和微分增益時，PID調節控制器比PD控制器更好地防止了穩態誤差。我們還發現手動調節PID參數非常困難，常常會導致不明原因的系統失穩。為了避免PID調節的繁瑣和困難，找到最佳的PID參數，我們使用了基於梯度下降法的極值搜索算法，並用它來求得代價函數的梯度在PID參數取值空間內的數值解，然后通過迭代的方式最終確定一組使代價函數最小的參數值。我們發現自動PID調參法明顯比手動PID更好。