自控课程设计 基于BP神经网络的一阶倒立摆PID控制

神经网络 倒立摆

---

本文主要分为四部分

• 倒立摆动力学建模
• 一阶倒立摆PID控制
• 一阶倒立摆神经网络控制
• 基于BP神经网络的PID控制

本文所展示的代码全部由matlab实现。

一级倒立摆的动力学模型

不计空气阻力，一级倒立摆可抽象为小车与均质杆组成的系统。
小车在水平方向受到的合力为：

$$\begin{equation} M\ddot{x} = F-f\dot x -N \end{equation}$$
式中，$M$为小车的质量；

$F$为电机对小车施加的力；

$f$为小车摩擦系数；

$x$为小车的水平位移；

$N$为小车与摆杆相互作用力的水平分量。

摆杆在水平方向受到的合力为：

$$\begin{equation} N = m\frac{d^2}{dt^2}(x+l\sin{\theta}) \end{equation}$$
即：
$$\begin{equation} N = m\ddot {x}+ml\ddot \theta\cos{\theta}-ml\dot \theta^2\sin{\theta} \end{equation}$$
式中，$m$为摆杆的质量；

$l$为摆杆转动轴心到杆质心的长度；

$\theta$为摆杆转动的角度；

摆杆在竖直方向受到的合力为：

$$\begin{equation} P-mg = ml\ddot{\theta}\sin{\theta}-ml\dot\theta^2\cos{\theta} \end{equation}$$
式中，$P$为小车与摆杆相互作用力的竖直分量；

此外，摆杆的力矩平衡方程为：

$$\begin{equation} -Pl\sin{\theta}-Nl\cos{\theta} = I\ddot{\theta} \end{equation}$$
综上，可得到系统的运动学方程为：
$$\begin{align} F &= (M+m)\ddot x +f\dot x + ml\ddot {\theta}\cos{\theta}-ml\dot\theta^2\sin{\theta}\\ -ml\ddot x\cos{\theta} &= (I+ml^2)\ddot \theta +mgl\sin{\theta} \end{align}$$

可以进行近似处理，线性化后两个运动方程如下：

$$\begin{align} (I+ml^2)\ddot \phi - mgl\phi = ml\ddot x\\ (M+m)\ddot x + f\dot x - ml\ddot \psi = u \end{align}$$
进行拉普拉斯变换，得到：
$$\begin{align} (I+ml^2)\Phi(s)s^2 - mgl\Phi(s)=mlX(s)s^2\\ (M+m)X(s)s^2+fX(s)s - ml\Phi(s)s^2 = U(s) \end{align}$$
整理后得到传递函数：
$$\begin{equation} \frac{\Phi(s)}{U(s)}=\frac{\frac{ml}{q}s^2}{s^4+\frac{f(I+ml^2)}{q}s^3 - \frac{(M+m)mgl}{q}s^2-\frac{fmgl}{q}s} \end{equation}$$

其中$q = (M+m)(I+ml^2)-(ml)^2$

实际系统的模型参数：

符号	物理意义
$M$	小车质量	$1.096kg$
$m$	摆杆质量	$0.109kg$
$f$	小车摩擦系数	$0.1N/m$
$1$	摆杆转动轴心到杆质心的长度	$0.25m$
$I$	摆杆惯量	$0.0034kg*m$

把上述参数代入，可以得到系统的实际模型为：

$$\begin{equation} \frac{\Phi(s)}{U(s)}=\frac{2.35655s}{s^3+0.0883167s^2-27.9169s-2.30942} \end{equation}$$

根轨迹分析

用Matlab对系统进行根轨迹分析。

$$\begin{figure}[H] \includegraphics[width=0.8\textwidth]{phiF.eps} \centering \caption{传递函数根轨迹图} \label{1_2} \end{figure}$$
\section{频率分析}
$$\begin{figure}[H] \includegraphics[width=0.8\textwidth]{bodeandnyquist.eps} \centering \caption{频率分析图} \label{1_2} \end{figure}$$

由根轨迹图可以看出特征根的始终有一根在右半平面，该系统不稳定。因此需要引入外作用对其进行控制。这里采用PID控制。

PID控制

在模拟控制系统中，控制器最常用的控制规律是PID控制。模拟PID控制系统原理框图，系统由模拟PID控制器和被控对象组成。
PID控制器是一种线性控制器，他根据给定值$y_d(t)$与实际输出值$y(t)$构成偏差

$$\begin{equation} error(t) = y_d(t)-y(t) \end{equation}$$
PID的控制规律为
$$\begin{equation} u(t)=k_p[ error(t)+\frac{1}{T_1}\int^t_0error(t)dt+\frac{T_Dderror(t)}{dt} ] \end{equation}$$
PID控制器各校正环节作用如下：
1. 比例环节：成比例的反应控制系统的偏差信号error(t)，偏差一旦产生，控制器立即产生控制作用以减小偏差。
2. 积分环节：主要用于消除静差，提高系统的无差度。积分作用的强弱取决于积分时间常数$T_I$，$T_I$越大，积分作用越弱，反之越强。
3. 微分环节：反映偏差信号的变化趋势(变化速率)，并能在偏差信号变得太大，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度，减少调节时间。
由于执行机构为电动机因此采用增量式PID控制。
$$\begin{equation} \delta u(k) = kp(error(k)-error(k-1)+k_ierror(k)+k_d(error(k)-2error(k-1)-error(k-2))) \end{equation}$$
根据增量式算法设计仿真程序，其中$k_p = 100,k_I = 30,k_d = 50$ ,仿真结果如下：
$$\begin{figure}[H] \includegraphics[width=0.7\textwidth]{pid1.eps} \centering \caption{小车状态} \label{2_1} \end{figure}$$

$$\begin{figure}[H] \includegraphics[width=0.7\textwidth]{pid2.eps} \centering \caption{输出力的变换} \label{2_2} \end{figure}$$

error_1 = 0;
error_2 = 0;
i1_1 = -10;
p1_1 = -10;
d1_1 = 0;
%kp，ki，kd
xiteP = 100;
xiteI = 30;
xiteD = 50;
p2_1 = -10;
i2_1 = -10;
d2_1 = 0;
p3_1 = -10;
i3_1 = -10;
d3_1 = 0;
p4_1 = -10;
i4_1 = -10;
d4_1 = 0;
e = [0 0;0 0;0 0;0 0];
%u（k-1）
u_1 = 0;
%初值
xk = [-20/57.3,0.8,0.1,0.2];

ts = 0.02;

for k = 1:1:1000
    time(k) = k*ts;
    %tspan = [0,ts];
    global para 
    para = u_1;
    [t,x] = RK4(@nn_pidf,xk,0.01,0,0.02);%离散采样
    xk = x(:,3);
    %输入
    r1(k) = 0.0;%摆角
    r2(k) = 0.0;%角速度
    r3(k) = 0.0;%汽车位置
    r4(k) = 0.0;%汽车加速度

    %输出
    x1(k) = xk(1);
    x2(k) = xk(2);
    x3(k) = xk(3);
    x4(k) = xk(4);

    e1(k) = r1(k)-x1(k);
    e2(k) = r2(k)-x2(k);
    e3(k) = r3(k)-x3(k);
    e4(k) = r4(k)-x4(k);

    %增量误差
    error(k) = 0.2*e1(k)+0.8*e3(k);
    %PID
    xx(1) = error(k)-error_1;
    xx(2) = error(k);
    xx(3) = error(k)-2*error_1+error_2;

    i1(k) = i1_1+xiteI*xx(2);
    p1(k) = p1_1+xiteP*xx(1);
    d1(k) = d1_1+xiteD*xx(3);

    i2(k) = i2_1+xiteI*xx(2);
    p2(k) = p2_1+xiteP*xx(1);
    d2(k) = d2_1+xiteD*xx(3);

    i3(k) = i3_1+xiteI*xx(2);
    p3(k) = p3_1+xiteP*xx(1);
    d3(k) = d3_1+xiteD*xx(3);

    i4(k) = i4_1+xiteI*xx(2);
    p4(k) = p4_1+xiteP*xx(1);
    d4(k) = d4_1+xiteD*xx(3);

    de1(k) = e1(k) - e(1,1);
    cel(k) = e1(k) - 2*e(1,1)+e(1,2);
    u1(k) = i1(k)*e1(k)+p1(k)*de1(k)+d1(k)*cel(k);

    de2(k) = e2(k)-e(2,1);
    ce2(k) = e2(k)-2*e(2,1)+e(2,2);
    u2(k) = i2(k)*e2(k)+p2(k)*de2(k)+d2(k)*ce2(k);

    de3(k) = e3(k)-e(3,1);
    ce3(k) = e3(k)-2*e(3,1)+e(3,2);
    u3(k) = i3(k)*e3(k)+p3(k)*de3(k)+d3(k)*ce3(k);

    de4(k) = e4(k)-e(4,1);
    ce4(k) = e4(k)-2*e(4,1)+e(4,2);
    u4(k) = i4(k)*e4(k)+p4(k)*de4(k)+d4(k)*ce4(k);

    %u(k) = u1(k)+u2(k)+u3(k)+u4(k);
    u(k) = (11*u1(k)+20*u2(k)+96.5*u3(k)+35.5*u4(k))/50;
    if u(k)>=10
        u(k) = 10;
    elseif u(k)<=-10
        u(k)=-10;
    end

    error_2 = error_1;
    error_1 = error(k);
    i1_1 = i1(k);i2_1 = i2(k);i3_1 = i3(k);i4_1 = i4(k);
    p1_1 = p1(k);p2_1 = p2(k);p3_1 = p3(k);p4_1 = p4(k);
    d1_1 = d1(k);d2_1 = d2(k);d3_1 = d3(k);d4_1 = d4(k);
    e(1,2) = e(1,1);e(1,1) = e1(k);
    e(2,2) = e(2,1);e(2,1) = e2(k);
    e(3,2) = e(3,1);e(3,1) = e3(k);
    e(4,2) = e(4,1);e(4,1) = e4(k);
    u_1 = u(k);
    end

    figure(1);
    subplot(411);
    plot(time,r1,'k',time,x1,'k');
    xlabel('时间(s)');
    ylabel('角度');
    subplot(412);
    plot(time,r2,'k',time,x2,'k');
    xlabel('时间(s)');
    ylabel('角速度');
    subplot(413);
    plot(time,r3,'k',time,x3,'k');
    xlabel('时间');
    ylabel('小车位移');
    subplot(414);
    plot(time,r4,'k',time,x4,'k');
    xlabel('时间');
    ylabel('速度');
    figure(5);
    plot(time,u,'k');
    xlabel('时间');
    ylabel('力');

从图中可以看出，普通PID控制可以控制使倒立摆处于稳定状态。但由于$k_p,k_i,k_d$均固定，一旦改变初值，PID控制器就不能使其保持稳定。例如当倒立摆初始状态变为$[-20/57.3,0.8,0.1,0.2]$

$$\begin{figure}[H] \includegraphics[width=0.7\textwidth]{pidn2.eps} \centering \caption{小车状态} \label{2_1} \end{figure}$$

$$\begin{figure}[H] \includegraphics[width=0.7\textwidth]{pidn1.eps} \centering \caption{输出力的变换} \label{2_2} \end{figure}$$
因此需要使用一种新的方法使PID控制器能够自我调节。

使用BP神经网络工具箱训练PID控制器

Matlab携带有人工神经网络工具箱，运用神经网络代替PID进行控制。训练神经网络工具箱代码如下：

%建立训练样本，训练样本为PID训练结果
PP = [x1;x2;x3;x4];
T = u;
net = newff([-0.35 0.35;-1 1;-1 1;-33],[12,1],{'tansig','purelin'},'trainlm','learngdm');
net.trainParam.show = 25;
net.trainParam.epochs = 300;
net = train(net,PP,T,[],[]);
%gensim(net,-1);%建立simulink模块

对神经网络进行训练后，便可将其代替PID进行控制，输入为小车角度，角速度，位移，输出则为电机拉力。训练代码如下：

clear all;
close all;
p;%PID控制器m文件
BP;%训练神经网络的m文件

u_1 = 0;
xk = [0,0,0.02,0];

ts = 0.02;

for k = 1:1:1000
    time(k) = k*ts;
    %tspan = [0,ts];
    global para 
    %global F
    para = u_1;
    %F = u_1;
    [t,x] = RK4(@nn_pidf,xk,0.01,0,0.02);
    %[t,x] = RK4(@func,xk,0.01,0,0.02);
    xk = x(:,3);

    r1(k) = 0.0;%摆角
    r2(k) = 0.0;%角速度
    r3(k) = 0.0;%汽车位置
    r4(k) = 0.0;%汽车加速度
    x1(k) = xk(1);
    x2(k) = xk(2);
    x3(k) = xk(3);
    x4(k) = xk(4);

    %u(k) = u1(k)+u2(k)+u3(k)+u4(k);
    Pr = [x1(k);x2(k);x3(k);x4(k)];

    u(k) = sim(net,Pr);%采用训练好的神经网络代替PID控制器
    if u(k)>=10
        u(k) = 10;
    elseif u(k)<=-10
        u(k)=-10;
    end
    u_1 = u(k);
end
figure(6);
subplot(411);
plot(time,r1,'k',time,x1,'k');
xlabel('时间(s)');
ylabel('角度');
subplot(412);
plot(time,r2,'k',time,x2,'k');
xlabel('时间(s)');
ylabel('角速度');
subplot(413);
plot(time,r3,'k',time,x3,'k');
xlabel('时间');
ylabel('小车位移');
subplot(414);
plot(time,r4,'k',time,x4,'k');
xlabel('时间');
ylabel('速度');
figure(10);
plot(time,u,'k');
xlabel('时间');
ylabel('力');

采用神经网络代替PID控制器控制，对初始状态要求受限于其训练样本，且控制效果并不好。为此，考虑采用BP与PID结合的控制方式。

基于BP控制的PID控制器

PID控制要取得较好的控制效果，就必须通过调整好比例、积分和微分三种控制作用，形成控制量中既相互配合又相互制约的关系，这种关系不一定是简单的“线性组合”，从变化无穷的非线性组合中可以找到最佳的关系。神经网络所具有的任意非线性表达能力，通过对系统性能的学习来实现具有最佳组合的PID控制。采用BP网络，可以建立参数$k_p$,$k_i$,$k_d$自学习的PID控制器。

基于BP（Back Propagation）网络的PID控制系统结构由两部分构成：

1. 经典的PID控制器，直接对被控对象进行闭环控制，并且三个参数$k_p$,$k_I$,$k_d$为在线调整方式；
2. 神经网络，根据系统的运行状态，调节PID控制器的参数，以期达到某种性能指标的最优化，是输出层神经元的输出状态对应与PID控制器的三个可调参数$k_P,k_I,k_d$通过神经网络的自学习、加权系数调整，是神经网络输出对英语某种最优控制律下的PID控制器参数。
   采用三层BP网络，网络输入层的输入为
   $$\begin{equation} O^{(1)}_j= x(j),j = 1,2,\cdots M \end{equation}$$
   式中，输入变量的个数M取决于被控系统的复杂程度。

网络隐含层的输入输出为

$$\begin{align} net^{(2)}_i(k) = \sum^{M}_{j=0}w_{ij}^{(2)}O_j^{(1)}\\ O^{(2)}_i(k)=f(net_i^{(2)}(k)),i=1\cdots Q \end{align}$$
式中，$w_{ij}^{(2)}$为隐含层加权系数；

隐含层神经元的活化函数为Sigmoid函数

$$\begin{equation} f(x) = \tanh{x}=\frac{e^x-e^{-x}}{e^x+e^{-x}} \end{equation}$$
网络输出层的输入输出为
$$\begin{align} net_l^{(3)}(k)&=\sum^{O}_{i=0}w_{ij}^{(3)}O_i^{(2)}(k)\\ O_i^{(3)}(k)&=g(net_l^{(3)}(k)),l=1,2,3\\ O_1^{(3)}&=k_p\\ O_2^{(3)}&=k_i\\ O_3^{(3)}&=k_d \end{align}$$
输出层的输出节点分别对应三个可调参数$k_p,k_i,k_d$ 。由于$k_p,k_i,k_d$不能为负值，所以输出层神经元的活化函数取非负的Sigmoid函数
$$\begin{equation} g(x)=\frac{1}{2}(1+\tanh{x})=\frac{e^x}{e^x+e^{-x}} \end{equation}$$

性能指标函数为

$$\begin{equation} E(k)=\frac{1}{2}(rin(k)-yout(k))^2 \end{equation}$$
按照梯度下降法修正网络的权系数，即按$E(k)$对加权系数的负梯度方向搜索调整，并附加一使搜索快速收敛全局极小的惯性项

$$\begin{equation} \Delta w_{ij}^{(3)}(k)=-\eta\frac{\partial{E(k)}}{\partial{w_{ij}^{(3)}}}+\alpha\Delta w_{il}^{(3)}(k-1) \end{equation}$$
式中，$\eta$为学习速率，$\alpha$为惯性系数。

又有

$$\begin{align} \frac{\partial E(k)}{\partial w_{ij}^{(3)}}= \frac {\partial E(k)}{\partial y(k)}\cdot \frac{\partial{y(k)}}{\partial{u(k)}} \cdot \frac{\partial{u(k)}}{\partial{O_i^{(3)}}} \cdot \frac{\partial{O^{(3)}_i(k)}}{\partial{net^{(3)}_i(k)}} \cdot \frac{\partial{net_i^{(3)}(k)}}{\partial w_{ij}^{(3)}(k)} \end{align}$$
最终得到网络输出层权的学习算法为
$$\begin{align} \Delta w_{ij}^{(2)}(k)=\alpha\Delta w_{ij}^{(2)}(k-1)+\eta \delta_i^{(2)} O_j^{(1)}(k)\\ \delta_i^{(3)} = f(net_i^{(2)}(k))\sum^{(3)}_{i=1}\delta_i^{(3)}w_{ij}^{(3)}(k) \end{align}$$
同理可得隐含层加权系数的学习算法为
$$\begin{align} \Delta w_{ij}^{(2)}(k)=\alpha\Delta w_{ij}^{(2)}(k-1)+\eta\delta_i^{(2)}O_j^{(1)}(k)\\ \delta_i^{(2)}=f(net_i^{(2)}(k))\sum^3_{i=1}w_{ij}^{(3)}(k) \end{align}$$

该控制器控制算法归纳如下：
1. 确定BP网络的结构，即确定输入层节点数M和隐含层节点数Q，并给出各层加权系数的初值，选定学习速率$\eta$和惯性系数$\alpha$，此时$k=1$；
2. 采样得到$rin(k)$和$yout(k)$，计算该时刻误差$error(k)=rin(k)-yout(k)$；
3. 计算神经网络NN各层神经元的输入、输出，NN输出层的输出即为PID控制器的三个可调参数$k_p,k_i,k_d$；
4. 根据计算PID控制器的输出$u(k)$；
5. 进行神经网络学习，在线调整加权系数，实现PID控制参数的自适应调整；
6. 置$k=k+1$

$$\begin{figure}[H] \includegraphics[width=0.7\textwidth]{bp2.eps} \centering \caption{小车状态} \label{2_1} \end{figure}$$

$$\begin{figure}[H] \includegraphics[width=0.7\textwidth]{xubp1.eps} \centering \caption{输出力的变换} \label{2_2} \end{figure}$$
通过不断调整，系统最终达到比较稳定的状态。
代码如下：

clear all;
close all;
%BP based on PID control

xite = 0.30;%学习速率
alfa = 0.05;%惯性系数

%输入层权
wi = [];
%输出层权
wo = [];

IN  = 13;H = 5;Out = 3;
%系数
wi = 0.5*rand(H,IN);
wi_1 = wi;wi_2 = wi;wi_3=wi;
wo = 0.5*rand(Out,H);
wo_1 = wo;wo_2 = wo;wo_3 = wo;

%output from NN middle
Oh = zeros(H,1);

%input from NN middle
net_2 = Oh;

error_1 = 0;
error_2 = 0;

%xk = [-10/57.3,0.1,0.02,0.5];
xk = [-20/57.3,0.8,0.1,0.2];
u_1 = 0;
y_1 = 0;y_2 = 0;y_3 = 0;y_4 = 0;
ts = 0.02;

for k = 1:1:1000
    time(k) = k*ts;
    %tspan = [0,ts];
    global para 
    para = u_1;
    [t,x] = RK4(@nn_pidf,xk,0.01,0,0.02);
    xk = x(:,3);

   %input 无输入
    rin1(k) = 0.0;%摆角
    rin2(k) = 0.0;%角速度
    rin3(k) = 0.0;%汽车位置
    rin4(k) = 0.0;%汽车加速度

    %output
    yout1(k) = xk(1);
    yout2(k) = xk(2);
    yout3(k) = xk(3);
    yout4(k) = xk(4);

    %error
    e1(k) = rin1(k)-yout1(k);
    e2(k) = rin2(k)-yout2(k);
    e3(k) = rin3(k)-yout3(k);
    e4(k) = rin4(k)-yout4(k);

    error(k) = 0.25*e1(k)+0.25*e2(k)+0.25*e3(k)+0.25*e4(k);

    %PID 只训练得到pid系数因此需要求总error
    x(1) = error(k)-error_1;
    x(2) = error(k);
    x(3) = error(k)-2*error_1+error_2;
    %kp,ki,kd
    epid = [x(1);x(2);x(3)];

    xi = [rin1(k),yout1(k),e1(k),rin2(k),yout2(k),e2(k),rin3(k),yout3(k),e3(k),rin4(k),yout4(k),e4(k),1];
    net_2 = xi*wi';

    for j = 1:1:H
        Oh(j) = (exp(net_2(j))-exp(-net_2(j)))/(exp(net_2(j))+exp(-net_2(j)));%Middle layer
    end
    net_3 = wo*Oh;

    for j = 1:1:Out
        K(j) = exp(net_3(j))/(exp(net_3(j))+exp(-net_3(j)));%Get Kp,Ki,Kd;
    end
    kp(k) = K(1);ki(k) = K(2);kd(k) = K(3);

    kpid = [kp(k),ki(k),kd(k)];

    %pid控制器输出uk
    du(k) = kpid*epid;
    u(k) = u_1+du(k);

    if u(k)>= 10 % Restricting of the output of controller
        u(k) = 10;
    end
    if u(k)<= -10
        u(k) = -10;
    end

    dyu(k) = sign((yout1(k)-y_1+yout2(k)+yout3(k)+yout4(k)-y_2-y_3-y_4)/(4*(u(k)-u_1+0.0000001)));

    %Out layer
    for j = 1:1:Out
        dK(j) = 2/(exp(K(j))+exp(-K(j)))^2;
    end
    for j = 1:1:Out
        delta3(j) = error(k) * dyu(k) * epid(j) * dK(j);
    end

    for ii = 1:1:Out
        for j = 1:1:H
            d_wo = xite*delta3(ii) * Oh(j) + alfa*(wo_1-wo_2);
        end
    end
    wo = wo_1 + d_wo +alfa*(wo_1-wo_2);

    %Hidden layer

    for j = 1:1:H
        dO(j) = 4/(exp(net_2(j))+exp(-net_2(j)))^2;
    end
    segma = delta3*wo;
    for j = 1:1:H
        delta2(j) = dO(j)*segma(j);
    end

    d_wi = xite*delta2'*xi;

    wi =wi_1 + d_wi + alfa * (wi_1-wi_2);

    %Parameters Update
    u_1=u(k);
    y_4 = yout4(k);y_3 = yout3(k);y_2 = yout2(k);y_1 = yout1(k);

    wo_3 = wo_2;
    wo_2 = wo_1;
    wo_1 = wo;

    wi_3 = wi_2;
    wi_2 = wi_1;
    wi_1 = wi;

    error_2 = error_1;
    error_1 = error(k);

end
figure(1);
subplot(411);
plot(time,rin1,'k',time,yout1,'k');
xlabel('时间(s)');
ylabel('角度');
subplot(412);
plot(time,rin2,'k',time,yout2,'k');
xlabel('时间(s)');
ylabel('角速度');
subplot(413);
plot(time,rin3,'k',time,yout3,'k');
xlabel('时间');
ylabel('小车位移');
subplot(414);
plot(time,rin4,'k',time,yout4,'k');
xlabel('时间');
ylabel('速度');
figure(5);
plot(time,u,'k');
xlabel('时间');
ylabel('力');

经过测试发现在典型PID控制器无法收敛的初值下，基于神经网络的PID控制器仍能较好控制。但角度在-4度收敛，未回到零点。