@Aurora-xi 2019-01-14T17:14:53.000000Z 字数 8203 阅读 966

基于BP神经网络的四旋翼PID控制研究

一、四旋翼控制系统模型的建立

（一）四旋翼的动力学模型

坐标系是描述飞行器位置与运动规律的参考基准。四旋翼包含两个坐标系，分别是自身机体坐标系与全局导航坐标系，因此在建立四旋翼模型前需要对两个坐标系进行转换。

机体坐标系：坐标系原点位于机体质心，XY轴与四旋翼XY轴重合，Z轴与四旋翼水平位置垂直。
导航坐标系：一般采用地理坐标系作为导航坐标系

从机体坐标系到导航坐标系的转换矩阵R为

$R=R_{X}R_{Y}R_{Z}=\begin{bmatrix} cos\psi cos\phi & cos\psi sin\theta sin\phi & cos\psi sin\theta cos\phi +sin\psi sin\phi \\ sin\psi cos\theta & sin\psi sin\theta sin\phi & sin\psi sin\theta cos\phi -cos\psi sin\phi \\ -sin\theta & cos\theta sin\phi & cos\theta cos\phi \end{bmatrix}$ 其中

$\psi$ 为偏航角，

$\theta$ 为俯仰角，

$\phi$ 为翻滚角，坐标旋转顺序为ZYX。

为了建立飞行器的动力学模型，并且不失一般性，对四旋翼飞行器做出如下假设：
1. 四旋翼飞行器为均匀对称的刚体；
2. 惯性坐标系E的原点与飞行器几何中心及质心位于同一位置；
3. 四旋翼飞行器所受阻力和重力不受飞行高度等因素影响，总保持不变；
4. 四旋翼飞行器各个方向的拉力与推进器转速的平方成正比例。

结合线运动方程和角运动方程可得到四旋翼飞行器的非线性运动方程
整理后的公式如下

$\left\{\begin{matrix} \ddot{x}=\left ( cos\psi sin\theta cos\phi +sin\psi sin\phi \right )U_{1}/m\\ \ddot{y}=\left ( sin\psi sin\phi cos\phi -cos\psi sin\phi \right )U_{1}/m \\ \ddot{z}=\left ( cos\phi cos\theta \right )U_{1}/m-g \\ \ddot{\phi }=\left [ lU_{2}+\dot{\theta }\dot{\psi }\left ( I_{y}-I_{z} \right ) \right ]/I_{x} \\ \ddot{\theta }=\left [ lU_{3}+\dot{\phi }\dot{\psi }\left ( I_{z}-I_{x} \right ) \right ]/I_{y} \\\ddot{\psi }=\left [ U_{4}+\dot{\phi }\dot{\theta }\left ( I_{x}-I_{y} \right ) \right ]/I_{z} \end{matrix}\right.$ 其中

$U_{1}$ ，

$U_{2}$ ，

$U_{3}$ ，

$U_{4}$ 为四旋翼飞行器的四个独立控制通道的控制输入量

$U_{1}$ 为垂直速度控制量

$U_{2}$ 为翻滚输入控制量

$U_{3}$ 为俯仰输入控制量

$U_{4}$ 为偏航输入控制量

$m$ 为四旋翼飞行器的质量

$I$ 为四旋翼的惯性矩阵

$l$ 为旋翼中心到坐标系原点的距离

（二）四旋翼控制系统的传递函数

整个模型可以看成由线运动和角运动两个独立的子系统构成，角运动不受线运动的影响，而线运动受角运动的影响。
以此为前提同时忽略微型扰动，以姿态角和角速度直接的准积分关系为基础，可得到四旋翼飞行器的运动方程：

$\left\{\begin{matrix} m\dot{x}=Ax+Bu\\ x=\left [ \dot{x},\dot{y},\dot{z},p,q,r,\theta ,\phi ,\psi \right ]^{T}\\ u=\left [ u_{1},u_{2},u_{3},u_{4} \right ]^{T} \end{matrix}\right.$

通过测量待测试的四轴飞行器的基本参数，并参阅相关文献，可得出四轴飞行器的参数表1。

表1 四轴飞行器参数

参数	数值
$m/kg$	0.8
$l/m$	0.25
$k_{1}\times 10^{-5}/\left ( N\cdot s^{2} \right )$	3.19
$k_{d}\times 10^{-7}/\left ( N\cdot ms^{2} \right )$	7.53
$I_{x}\times 10^{-3}/\left ( kg\cdot m^{2} \right )$	2.348
$I_{y}\times 10^{-3}/\left ( kg\cdot m^{2} \right )$	2.348
$I_{z}\times 10^{-2}/\left ( kg\cdot m^{2} \right )$	5.301

根据控制系统的传递函数，并结合四轴飞行器的参数表（表1），可得到各通道的传递函数表2。

表2 各通道传递函数

通道	传递函数
俯仰通道传递函数	$G_{1}=\frac{\theta }{u_{1}}=\frac{57s+4296}{s^{3}+102s^{2}+890s+4260}$
横滚通道传递函数	$G_{2}=\frac{\phi}{u_{2}}=\frac{65s+4563}{s^{3}+109s^{2}+1015s+2933}$
偏航通道传递函数	$G_{3}=\frac{\psi }{u_{3}}=\frac{92}{s^{2}+367s}$
X轴向俯仰角传递函数	$G_{4}=\frac{x}{\theta }=\frac{\dot{x}}{s\theta }=\frac{-191s+567}{57s^{2}+4296s}$
Y轴向滚转角传递函数	$G_{5}=\frac{\gamma }{\phi }=\frac{\dot{\gamma }}{s\phi }=\frac{-276s+743}{65s^{2}+4563s}$
Z轴向传递函数	$G_{6}=\frac{z}{u_{4}}=\frac{\dot{z}}{su_{4}}=\frac{1.78}{s^{2}+5.83s}$

二、传统PID控制研究

PID控制，即比例-积分-微分控制，是自动控制原理中的一种控制方法，也可以称为是校正方法。
PID控制主要是对系统偏差进行比例、积分、微分三种操作并线性组合成控制量，以减小系统误差，提高系统响应速度和响应效果。

对模拟信号进行的模拟PID控制，也可以称作连续PID控制，公式表示为：

$u\left ( t \right )=K_{p}\left [ e\left ( t \right )+\frac{1}{T_{i}}\int_{0}^{t}e\left ( t \right ) dt+T_{d}\frac{de\left ( t \right )}{dt}\right ]$

式中， $K_{p}$ 为比例系数， $K_{i}$ 为积分时间常数， $K_{d}$ 为微分时间常数。对上式进行拉氏变换：

$U\left ( s \right )=K_{p}\left [ E\left ( s \right )+\frac{E\left ( s \right )}{T_{i}s}+T_{d}sE\left ( s \right ) \right ]$

PID控制器的传递函数为：

$D\left ( s \right )=\frac{U\left ( s \right )}{E\left ( s \right )}=K_{p}\left ( 1+\frac{1}{T_{i}s}+T_{d}s \right )$

对连续PID控制以一定离散化方法离散后就可以得到数字PID控制。离散的本质是采样，假设采样为周期采样，采样周期为T，离散自变量为n，则离散PID控制可以表示为

$u\left [ n \right ]=K_{p}\left \{ e\left [ n \right ]+\frac{T}{T_{i}}\sum_{i=0}^{n}e\left [ i \right ]+\frac{T_{d}}{T}\left \{ e\left [ n \right ]-e\left [ n-1 \right ] \right \} \right \}$

根据上式，写出n-1时刻的控制量

$u\left [ n-1 \right ]=K_{p}\left \{ e\left [ n-1 \right ]+\frac{T}{T_{i}}\sum_{i=0}^{n-1}e\left [ i \right ]+\frac{T_{d}}{T}\left \{ e\left [ n-1 \right ]-e\left [ n-2 \right ] \right \} \right \}$

设

$\Delta u\left [ n \right ]=u\left [ n \right ]-u\left [ n-1 \right ]$

得到

$\Delta u\left [ n \right ]=K_{p}\left \{ e\left [ n \right ]-e\left [ n-1 \right ] \right \}+\frac{K_{p}T}{T_{i}}e\left [ n \right ]+\frac{K_{p}T_{d}}{T}\left \{ e\left [ n \right ] -2e\left [ n-1 \right ]+e\left [ n-2 \right ]\right \}$

令 $K_{i}=K_{p}\frac{T}{T_{i}}$ 为积分系数， $K_{d}=K_{p}\frac{T_{d}}{T}$ 为微分系数，可以将上式简化为

$\Delta u\left [ n \right ]=K_{p}\left \{ e\left [ n \right ]-e\left [ n-1 \right ] \right \}+K_{i}e\left [ n \right ]+K_{d}\left \{ e\left [ n \right ] -2e\left [ n-1 \right ]+e\left [ n-2 \right ]\right \}$

根据上述公式编写代码并调试，参数取值如下表

传递函数	$K_{p}$	$K_{p}$	$K_{p}$
$G_{1}$	2	0.02	1
$G_{2}$	1.9	6	0.1
$G_{3}$	9	0.01	10
$G_{4}$	2.2	8	0。1
$G_{5}$	3.1	1	0.25
$G_{6}$	30	1	1

在MATLAB上编写代码如下

clc;
clear ;     
ts=0.001;                    %采样时间  
sys=tf(92,[1,367,0]);
dsys=c2d(sys,ts,'z');        %把控制函数离散化，转化为拆分方程
[num,den]=tfdata(dsys,'v');  %离散化后提取分子、分母 ，提取拆分方程系数   
u_1=0.0;  
u_2=0.0;  
y_1=0.0;  
y_2=0.0;  
x=[0,0,0];
error_1=0;  
error_2=0;  
for k=1:1:10000  
time(k)=k*ts;    %采样次数  
kp=9;ki=0.01;kd=10;             %初始化PID
rin(k)=1;            %Step Signal ，阶跃信号  
du(k)=kp*x(1)+kd*x(2)+ki*x(3);      %PID Controller   控制系数    
u(k)=u_1+du(k); 
if u(k)>=10        %Restricting the output of controller，输出限幅
   u(k)=10;  
end  
if u(k)<=-10  
   u(k)=-10;  
end  
%Linear model  
yout(k)=-den(2)*y_1-den(3)*y_2+num(2)*u_1+num(3)*u_2;          %实际输出  
error(k)=rin(k)-yout(k);   %Return of parameters 误差  
u_2=u_1;    %保存上上次输入   为下次计算  
u_1=u(k);   %保存上一次控制系数   为下次计算  
y_2=y_1;    %保存上上次次输出   为下次计算  
y_1=yout(k);       %保存上一次输出   为下次计算  
x(1)=error(k)-error_1;            %Calculating P  
x(2)=error(k)-2*error_1+error_2;  %Calculating D  
x(3)=error(k);                    %Calculating I  
error_2=error_1;                      
error_1=error(k);                      
end  
plot(time,rin,'b',time,yout,'r');      %输入 和实际控制输出  
xlabel('time(s)'),ylabel('rin,yout');

以俯仰通道、横滚通道、偏航通道为例，仿真出单位阶跃输入时PID图像如下：

俯仰通道PID图像

横滚通道PID图像

偏航通道PID图像

通过仿真图可知，无论如何调节 $K_{p}$ 、 $K_{i}$ 、 $K_{d}$ 的值都无法完全消除超调和抖动。在各个通道中，偏航通道的超调量最大，最不稳定，自动控制效果最差。这主要是因为在PID控制中各参数对控制器的作用各不相同但又相互影响。随着 $K_{p}$ 值的增加，闭环系统的超调量增加，响应速度加快，控制时间加长，稳态误差减小，但 $K_{p}$ 值不能一味的增加，否则超调量将会大大增加导致系统变得极不稳定。随着 $K_{i}$ 值的增加，系统的超调量减小，但系统的响应速度减慢。随着 $K_{d}$ 值的增加，系统的超调量增大，经过起始上升阶段后响应速度减慢。

为了达到更为理想的自动控制效果，我们在PID参数调节中加入了神经网络自学习能力，根据系统的实时性自动调节PID参数。

三、BP神经网络PID控制器设计

（一）BP神经网络概述

BP神经网络是一种按误差逆传播算法训练的多层前馈网络，是目前应用最广泛的神经网络模型之一。

BP网络能学习和存储大量的输入-输出模式映射关系，而无需事前揭示描述这种映射关系的数学方程。它的学习规则是使用梯度下降法，通过反向传播来不断调整网络的权值和阈值，使网络的误差平方和最小。

3层BP神经网络结构图

权值是这条路径的实现概率。在每层神经元和相邻层的连接路径。

阈值是临界值，当外界刺激达到一定的阈值时，神经元才会受刺激，影响下一个神经元。

BP神经网络权值和阈值的初值一般是随机产生的。

(二)BP神经网络与PID控制的结合

BP神经网络具有逼近任意非线性函数的能力，可以通过BP神经网络训练和学习来实时调整PID控制器的参数，找到某一最优控制下的P、I和D参数。

设控制器中的BP神经网络是一个3层BP网络，有M个输入节点，Q个隐层节点。输入节点对应系统运行的状态，输出节点分别对应PID控制器的3个可调参数 $K_{p}$ 、 $K_{i}$ 、 $K_{d}$ 。

上文已推出，经典增量式数字PID的控制算式为：

BP神经网络PID的控制算式为：

$\Delta u\left [ n \right ]=o{_{1}}^{(3)}\left \{ e\left [ n \right ]-e\left [ n-1 \right ] \right \}+o{_{2}}^{(3)}e\left [ n \right ]+o{_{3}}^{(3)}\left \{ e\left [ n \right ] -2e\left [ n-1 \right ]+e\left [ n-2 \right ]\right \}$

将BP神经网络模型加入之前的代码，选取学习速率0.25，惯性因子0.05,编写代码如下：

clear all;
xite=0.25;  %学习速率
alfa=0.05;  %惯性因子
In=4;h=5;Out=3; %神经网络结构
wi=[-0.6394  -0.2696  -0.3756  -0.7023;
   -0.8603  -0.2013  -0.05024  -0.2596;
   -1.0749  0.5543  -1.6820  -0.5437;
   -0.3625  -0.0724  -0.6463  -0.2859;
    0.1425  0.0279  -0.5406  -0.7660];
 %wi=0.50*rands(h,In);  %中间层加权系数wi初始化
wi1=wi;wi2=wi;wi3=wi;
wo=[0.7576 0.2616 0.5820 -0.1416 -0.1325;
    -0.1146 0.2949 0.8352 0.2205 0.4508;
    0.7201 0.4566 0.7672 0.4962 0.3632];
%wo=0.50*rands(Out,h); %输出层加权系数wo初始化
wo1=wo;wo2=wo;wo3=wo;
x=[0,0,0]; %比例，积分，微分赋初值
u1=0;u2=0;u3=0;u4=0;u5=0;
y1=0;y2=0;y3=0;
oh=zeros(h,1); %神经网络中间层输出
I=oh;          %神经网络中间层输入
error1=0;
error2=0;
ts=0.001; %采样周期均值
for k=1:1:1000 %仿真开始，共6000步
    time(k)=k*ts;
    rin(k)=1.0;
    a(k)=1.2*(1-0.8*exp(-0.1*k));
    y(k)=a(k)*y1/(1+y1^2)+u1;
    error(k)=rin(k)-y(k);
    xi=[rin(k),y(k),error(k),1];
    x(1)=error(k)-error1;          %比例输出
    x(2)=error(k);                 %积分输出
    x(3)=error(k)-2*error1+error2; %微分输出
    ed=[x(1);x(2);x(3)];
    I=xi*wi; %中间层的输入，即输入层输入*权值
    for j=1:1:h
        oh(j)=(exp(I(j))-exp(-I(j)))/(exp(I(j))+exp(-I(j))); %激活函数作用下中间层的输出
    end
    K=wo*oh; %输出层的输入，即隐含层输入*权值
    for j=1:1:Out
        K(j)=exp(K(j))/(exp(K(j))+exp(-K(j))); %输出层的输出，即kp,ti,td
    end
    kp(k)=K(1);ti(k)=K(2);td(k)=K(3);
    Kd=[kp(k),ti(k),td(k)];
    du(k)=Kd*ed;
    u(k)=u1+du(k);
    if u(k)>=10 %限制控制器输出
        u(k)=10;
    end
    if u(k)<=-10
        u(k)=-10;
    end
    %权值wi、wo的在线调整
    dyu(k)=sign((y(k)-y1)/(u(k)-u1+0.0000001));
    %输出层
    for j=1:1:Out
        dK(j)=2/(exp(K(j))+exp(-K(j)))^2;
    end
    for i=1:1:Out
        de3(i)=error(k)*dyu(k)*ed(i)*dK(i);
    end
    for j=1:1:Out
        for i=1:1:h
            dwo=xite*de3(j)*oh(i)+alfa*(wo1-wo2);
        end
    end
    wo=wo1+dwo+alfa*(wo1-wo2);
    %隐含层
    for i=1:1:h
        dO(i)=4/(exp(I(i))+exp(-I(i)))^2;
    end
    seg=de3*wo;
    for i=1:1:h
        de2(i)=dO(i)*seg(i);
    end
    dwi=xite*de2*xi;
    wi=wi1+dwi+alfa*(wi1-wi2);
    %参数更新
    u5=u4;
    u4=u3;
    u3=u2;
    u2=u1;
    u1=u(k);
    y2=y1;
    y1=y(k);
    wo3=wo2;
    wo2=wo1;
    wo1=wo;
    wi3=wi2;
    wi2=wi1;
    wi1=wi;
    error2=error1;
    error1=error(k);
end
plot(time,rin,'b',time,y,'r');
xlabel('时间(秒)');ylabel('系统输出');

在单位阶跃响应时对偏航通道仿真得PID图像如下：
加入BP神经网络后

由图可见，加入BP神经网络模型后，不管是控制时间还是超调量等各方面，控制效果都有显著提升。

四、比较和总结

通过典型PID控制器和BP神经网络控制器的仿真图，我们可以看出典型PID调节控制器超调、抖动明显，而且无论怎么调节都无法消除。由于参数由比例、积分、微分环节调整，而其内部又相互影响，无法达到完美。
而BP神经网络控制器可以通过BP神经网络训练和学习来实时调整PID控制器的参数，找到某一最优控制下的P、I和D参数，使超调量和响应时间同时降低，具有良好的控制效果。使控制系统具有更强的稳定性。

将神经网络和PID相结合，利用神经网络的自学习能力和逼近任意函数的能力，可在线进行PID参数调整，有效地控制较复杂的被控对象，大大改善了常规PID控制器的性能，同时也显示了神经网络在解决高度非线性和严重不确定系统方面的潜能。