原文链接

控制器设计

基于roll, pitch, yaw的传递函数表达的系统的线性数学模型

使用Matlab仿真工具设计了一个闭环控制器。

首先，在Matlab脚本里面定义系统模型的常量。

%% Constants
d    = 0.45;                % diameter quadrocopter
l    = sqrt(d^2/8);         % distance between one rotor and center of mass
m    = 1.0024;              % mass of quadrocopter
g    = 9.81;                % gravitational acceleration
kPWM = 7.8502;              % proportional constant omega/PWM

Jxx = 0.01046412;           % mass moment of inertia xb
Jyy = 0.010746965;          % mass moment of inertia yb
Jzz = 0.020576095;          % mass moment of inertia zb

bT = 7.90261e-06;           % proportional coefficient blade thrust/(omega^2)
bH = 1.66057e-7;            % proportional coefficient blade torque/(omega^2)

% system gains
k_phi   = l*bT*kPWM*sqrt(m*g*bT)/Jxx;
k_theta = l*bT*kPWM*sqrt(m*g*bT)/Jyy;
k_psi   = bH*kPWM*sqrt(m*g*bT)/Jzz;

Z变换

在设计控制器之前，考虑到要控制的系统是离散的，所以需要变换到z域。传感器MPU-9150的测量频率是50Hz，是整个系统中最慢的部分，所以由它来定义控制系统的采样频率。

%% Z-transform
% sample time
T_sample = 1/50;

% LTI-System of quadcopters phi (roll)
Gs_phi_s = tf(k_phi,[1 0 0]);

% transfer function continuous to discrete
Gs_phi_z = c2d(Gs_phi_s, T_sample);

PD极点配置

第一步，通过离散z域的极点配置，设计了控制器中四旋翼的roll变化部分。在复z平面实轴上的0.83处有3个稳定极点，用它们来设计闭环控制系统。在下面的Matlab脚本里面还做了关于控制器传递函数中的多项式的计算。

%% Pole-placement
% desired poles
d_phi = [0.83 0.83 0.83];

[Gs_phi_z_num, Gs_phi_z_den] = tfdata(Gs_phi_z,‘v‘);
Gs_phi_z_num = Gs_phi_z_num(2:end);
[Gc_phi_z_num, Gc_phi_z_den] = placepole(Gs_phi_z_num, Gs_phi_z_den, d_phi);

% controller
Gc_phi_z = tf(Gc_phi_z_num,Gc_phi_z_den,T_sample);

闭环控制系统的复极点-零点映射图（pole-zero map）表明，所有的极点的实部，都是指定的0.83。

这样的闭环系统，用matlab仿真结果如下，是一个建立时间大约为1s的阶梯响应。

设计的控制器

和一个一般传函形式的离散PD控制器是等价的

上式可以写成

为了能在飞行测试中常规调整控制器参数，控制器设计成PD参数形式。
比较上面几个式子，可以找到转换关系

这些参数可以轻易地在测试飞行器的过程中调整。

%% PID controller
% Controller coefficients
a_phi = fliplr(Gc_phi_z_den(2:end));
b_phi = fliplr(Gc_phi_z_num);

% PD-Parameters
P_phi = (b_phi(1)+b_phi(2))/(a_phi(1)+1);
N_phi = (a_phi(1)+1)/T_sample;
D_phi = (b_phi(2)-(b_phi(1)+b_phi(2))/(a_phi(1)+1))*T_sample/(a_phi(1)+1);

同样的流程用于pitch和yaw。值得注意的是，对于yaw，要控制的是角速度，而不是角度。所以，在相应的极点配置下，最终模型就是一个简单的P控制器。

控制器输出

As the controller outputs for controlling the quadcopter’s attitude are summarized by

控制飞行器姿态的飞控输出可以总结为以下形式

每个PWM信号代表一个电机，计算的时候，要考虑悬停所需要的PWM信号部分。

仿真

闭环控制系统在Matlab的仿真器里面仿真，仿真模型包括AttitudeControl描述控制器，以及MM代表四旋翼的非线性数学模型。考虑到控制系统的离散形式，零阶极点元素也输入了模型中。

子系统AttitudeControl包含3个PID控制器，利用输出来精确计算输入。

闭环控制系统中每个角度的阶跃响应如下所示。第三个图看起来不太一箱，是因为它控制的是角速度，而不是角度。

拓展成PID控制器

第一次飞行测试发现，由于飞机的质心和几何中心不重合，导致飞机侧翻。所以，控制器需要一个额外积分部分加以扩展。离散的PID控制器模型如下表示：

转换分数形式，得到

这就得到了分子和分母的多项式形式。这些多项式以数组的形式，送给控制算法。

积分增益I通过实验确定。

Implementation

数字控制器在Infineon XMC-4500上面实现了，在AttitudeController.c 和 AttitudeController.h两个文件中。

控制器代码的实现细节，可以在5. Software找到。