四旋翼动力学和仿真翻译（Quadcopter Dynamics and Simulation）

由于Neo已经于2014年对该文章前半部分进行了翻译（从Introduction到Physics），因此本文将从Simulation部分开始翻译。已经翻译部分如下


本文的翻译工作由BUAA飞机总体设计HT11课程小组成员完成，具体分工如下:

仿真

既然我们已经建立了描述此系统动力学的完整的方程组，因此我们可以搭建一个仿真环境，从而我们可以在仿真环境中实时观察到我们的变量和控制器的变化。尽管有很多更先进的方法可以选择，我们仍可以通过欧拉方程建立差分方程，进而模拟不同系统状态的发展情况。在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更好。