分享我开发的Arduino模糊控制库FuzzyControl

这是我开发的第一个库函数。使用这个库能够非常方便地构造一个双输入单输出模糊控制器，来实现模糊控制。希望能够给有需要的人带来帮助。

库下载地址： https://github.com/re-high/Arduino-FuzzyControl-Library

为什么要开发模糊控制库

  • 模糊控制的优点：无模型控制
    在现代控制理论中，控制器的分析和综合大多依赖于精确的数学模型。由于被控对象过程的非线性、参数间的强耦合、过程机理复杂以及现场测量仪表条件不足，以致很多被控对象不能建立起数学模型，对于那些不能直接获得数学模型描述的系统，使用模糊控制往往能取得很好的效果。
    人们在手动控制中，被控过程的操作人员在长期观察、实践中积累许多经验，这些经验常用定性的、不精确的语言规则等形式加以描述，如“若炉温偏高则燃料适当减少”。系统在运行过程中，人们将观察到的过程输出与设定值比较，得到过程输出偏离设定值变化快慢的模糊语义描述，经逻辑推理得到控制量的模糊量：“适当减少燃料”，经反模糊化后，转化为一精确的控制量，实现整个模糊过程。 以模糊集和模糊推理为基础，对上述手工操作过程进行建模，即可得到模糊控制器。
  • 相对于另外一种无模型控制方法PID法，模糊控制算法更为复杂，要经过模糊化、模糊推理、解模糊，因此想在硬件中实现模糊控制算法代码量大很多。
    在以往的项目中，我一般用面向过程的编程思想来写构造模糊控制器的代码，代码量随着隶属度函数和规则数的增加而增加，当想修改规则时，要改的地方非常多，造成代码可读性差且不易移植的困难。
    所以我有了一个想法，用面向对象的编程思想开发一个Arduino的模糊控制库，把模糊化、模糊推理、解模糊这些步骤放在库文件里实现，用户只需要在主程序中调用相关函数添加变量的隶属度函数和添加控制规则，就能直接计算出输出量。这大大减少了用户用Arduino实现模糊控制时构造模糊控制器的时间，不需要自己编写模糊控制的过程。

受到仿真软件matlab中模糊工具箱的启发，我编写了FuzzyControl库，让在硬件中实现模糊控制和在matlab中使用模糊工具箱函数搭建模糊控制器一样简单。

Arduino的模糊控制库FuzzyControl

FuzzyControl库简介

FuzzyControl是我为了在Arduino中更方便地实现模糊控制而开发的Arduino库。不再需要用户自己编写模糊化、模糊推理、解模糊的过程，你只要添加变量的隶属度函数和控制规则就能直接计算出输出量。当你想实现电机转速控制、双轮自平衡车的角度控制时，都可以用FuzzyControl库轻松方便地构建一个双输入单输出模糊控制器。
下面，我将首先介绍FuzzyControl库函数，然后以Arduino Uno配合带编码器的直流电机，演示如何使用FuzzyControl库构造一个模糊控制器，实现电机的位置追踪控制。

FuzzyControl库函数

setRange
功能：设置输入输出变量的论域
格式：setRange(input1rangeMin, input1rangeMax,input2rangeMin, input2rangeMax, outputrangeMin, outputrangeMax)
说明：setRange函数有6个输入变量：

  • input1rangeMin：输入量1的论域最小值
  • input1rangeMax：输入量1的论域最大值
  • input2rangeMin：输入量2的论域最小值
  • input2rangeMax：输入量2的论域最大值
  • outputrangeMin：输出量的论域最小值
  • outputrangeMax：输出量的论域最大值

addmf
功能：添加隶属度函数
格式：addmf( varType, mfIndex, trimf_a, trimf_b, trimf_c); addmf( varType, mfIndex, trapmf_a, trapmf_b, trapmf_c, trapmf_d);
说明：addmf函数有5或6个输入变量（根据隶属度函数是三角型还是梯形决定）：

  • varType：要添加隶属度函数的变量类型（1表示input1，2表示input2，3表示input3）
  • mfIndex：模糊子集索引（模糊子集名称）
  • trimf_a、trimf_b、trimf_c是三角型隶属度函数的参数
  • trapmf_a、trapmf_b、trapmf_c、trapmf_d是梯型隶属度函数的参数

setrulenum
功能：输入控制规则总数目
格式：setrulenum(ruleNum);
说明：setrulenum函数有1个输入变量：

  • ruleNum：控制规则总数目

addrule
功能：增加控制规则
格式：addrule(ruleIndex, var1Index, var2Index, var3Index)
说明：addrule函数有4个输入变量：

  • ruleIndex：要添加的控制规则是第几条规则
  • var1Index：这条控制规则中的input1所属的模糊子集索引
  • var2Index：这条控制规则中的input2所属的模糊子集索引
  • var3Index：这条控制规则中的output所属的模糊子集索引

caculate
功能：完成模糊推理计算
格式：caculate( value1, value2);
说明：caculate函数有2个输入变量：

  • value1：输入量1的数值
  • value2：输入量2的数值

实例：使用FuzzyControl库实现电机的位置追踪控制

介绍完FuzzyControl库函数，下面就用来看一下怎么用FuzzyControl库构造一个两输入单输出模糊控制器，来控制直流电机追踪到理想的位置。本程序设置电机的理想位置信号设置为y=50sin(0.1pi * t )，即20s产生一个完整的正弦波，控制目标就是让电机的位置追踪这个理想正弦信号。

电路连接图

用到的硬件包括：Arduino Uno（控制器）、带编码器的直流电机、电机驱动模块TB6612FNG、电源。
注意：由于未找到编码器的素材，所以图中少画了编码器与Arduino的连接图。为了得到位置信号，应将编码器A相连接Arduino数字引脚2（中断0）、B相连接Arduino数字引脚3（中断1）。

实现过程

总体方案：选用两输入单输出模糊控制器，控制器输入为追踪误差（e）和误差变化率（ec），输出为PWM值。

  1. 定义覆盖输入、输出变量的模糊子集
    • 确定输入输出变量的论域：输入为追踪误差（e）和误差变化率（ec），初步设置输入范围为[-20，20]；输出为PWM值，输出变化范围为[-255，255]。
    • 确定模糊集个数及各模糊集的隶属度函数：将追踪误差（e）和误差变化率（ec）都分为3个模糊集：N（负）Z（零）P(正)；将输出的PWM值分为5个模糊集：NB（负大）NS（负小）ZZ（零）PS（正小）PB（正大）。
    • 输入隶属度函数选取为三角形+梯形隶属函数，输出隶属度函数选取为三角形隶属函数。
    • 模糊集参数的选取：对于输入追踪误差（e）和误差变化率（ec），初步选取模糊集参数如下：
      NumMFs（模糊子集个数）=3
      MF1=’N’:’trapmf’,[-20 -20 -5 0]；
      MF2=’Z’:’trimf’,[-3 0 3]；
      MF3=’P’:’trapmf’,[0 5 20 20] 对于输出PWM值，初步选取模糊集参数如下：NumMFs（模糊子集个数）=5 MF1='NB':'trimf',[-255 -180 -100]； MF2='NS':'trimf',[-200 -110 -20]； MF3='ZZ':'trimf',[-75 0 75]； MF4='PS':'trimf',[20 110 200]； MF5='PB':'trimf',[100 180 255]

利用库函数设置输入输出模糊子集程序代码如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
```
 //setRange函数设置输入输出论域，输入范围为[-20，20]，输出范围为[-255，255]
   fuzzyController.setRange(-20,20,-20,20,-255,255);   
   /*addmf函数添加隶属度函数
    * 第一个参数表示变量类型（1表示input1，2表示input2，3表示input3）
    * 第二个参数表示模糊子集索引（模糊子集名称）
    * 后面的参数是三角型或梯形隶属度函数的参数    */
   fuzzyController.addmf(1,N,-20,-20,-5,0);
   fuzzyController.addmf(1,Z,-3,0,3);
   fuzzyController.addmf(1,P,0,5,20,20);
   fuzzyController.addmf(2,N,-20,-20,-5,0);
   fuzzyController.addmf(2,Z,-3,0,3);
   fuzzyController.addmf(2,P,0,5,20,20);
   fuzzyController.addmf(3,NB,-255,-180,-100);
   fuzzyController.addmf(3,NS,-200,-110,-20);
   fuzzyController.addmf(3,ZZ,-75,0,75);
   fuzzyController.addmf(3,PS,20,110,200);
   fuzzyController.addmf(3,PB,100,180,255);
  1. 设计并建立模糊控制规则：
    模糊规则就是输入量和输出量间的模糊蕴含关系。确定PWM值大小的核心思想为“当追踪误差增大时增大PWM值”，建立9条模糊控制规则：
    • if追踪误差是 N and 误差变化率是 N ，then PWM值是 NB；
    • if追踪误差是 N and 误差变化率是 Z ，then PWM值是 NS；
    • if追踪误差是 N and 误差变化率是 P ，then PWM值是 ZZ；
    • if追踪误差是 Z and 误差变化率是 N ，then PWM值是 NS；
    • if追踪误差是 Z and 误差变化率是 Z ，then PWM值是 ZZ；
    • if追踪误差是 Z and 误差变化率是 P ，then PWM值是 PS；
    • if追踪误差是 P and 误差变化率是 N ，then PWM值是 ZZ；
    • if追踪误差是 P and 误差变化率是 Z ，then PWM值是 PS；
    • if追踪误差是 P and 误差变化率是 P ，then PWM值是 PB；

在这里解释一下我这里的规则是如何建立的，比如第一条，追踪误差是N，表明追踪误差小于0，误差变化率是N，表明误差变化率也小于0，误差还在继续减小，这说明追踪误差的绝对值是在增大的，那输出的PWM值绝对值也是应该增大的，至于方向，是和连接电路时电机的转动方向有关，我在这里用实物测试出此时应该反转，所以PWM值为负，所以设置输出PWM值是 NB。再看第七条，追踪误差是P，表明追踪误差大于0，误差变化率是N，表明误差变化率小于0，两者结合在一起说明追踪误差虽然大于0但是误差量是在减少的，这个趋势是我们想要看到的所以设置输出 PWM值是 ZZ。其他规则同理。
在这里想告诉大家，规则的设定在构造模糊控制器中占有很重要的地位，一定要根据实际情况来设置模糊规则，判断逻辑推理的正确性。
利用库函数添加模糊规则程序代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
//setrulenum函数设置模糊控制规则总数
fuzzyController.setrulenum(9); 
//依次添加控制规则，第一次输入参数表示当前输入的是第几条规则
//后面三个参数分别为input1、input2、output所属的模糊子集索引（模糊子集名称）
fuzzyController.addrule(1,N,N,NB);  
fuzzyController.addrule(2,N,Z,NS); 
fuzzyController.addrule(3,N,P,ZZ); 
fuzzyController.addrule(4,Z,N,NS); 
fuzzyController.addrule(5,Z,Z,ZZ); 
fuzzyController.addrule(6,Z,P,PS); 
fuzzyController.addrule(7,P,N,ZZ); 
fuzzyController.addrule(8,P,Z,PS);
fuzzyController.addrule(9,P,P,PB);
  1. 完整程序代码
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    36
    37
    38
    39
    40
    41
    42
    43
    44
    45
    46
    47
    48
    49
    50
    51
    52
    53
    54
    55
    56
    57
    58
    59
    60
    61
    62
    63
    64
    65
    66
    67
    68
    69
    70
    71
    72
    73
    74
    75
    76
    77
    78
    79
    80
    81
    82
    83
    84
    85
    86
    87
    88
    89
    90
    91
    92
    93
    94
    95
    96
    97
    98
    99
    100
    101
    102
    103
    104
    105
    106
    107
    108
    109
    110
    111
    112
    113
    114
    115
    116
    117
    118
    119
    120
    121
    122
    123
    124
    125
    126
    127
    128
    129
    130
    131
    132
    133
    134
    135
    136
    137
    138
    139
    140
    141
    142
    /*
      Arduino通过FuzzyControl库构造一个双输入单输出的模糊控制器，控制直流电机追踪到理想的位置（本程序中理想位置设置的为正弦信号）
      by 梁悦（https://re-high.github.io/）
     
      如需获得本示例程序详细电路信息以及如何使用FuzzyControl库的更多知识，请参考我的博客：
       https://re-high.github.io/
     
    */
    #include <FuzzyControl.h>
    //初始化一个双输入单输出的模糊控制器
    //三个输入参数分别是输入量1、输入量2、输出量的模糊子集个数
    FuzzyControl fuzzyController(3,3,5);
    int fuzhi = 50;              //fuzhi：正弦波的幅值
    #define ENCODER_A_PIN 2      //直流电机编码器的A相连接引脚
    #define ENCODER_B_PIN 3      //直流电机编码器的B相连接引脚
    #define motor1 8             //电机
    #define motor2 9             //电机
    #define ENA 5                //使能引脚
    #define STBY 12
    //定义模糊子集名称及其对应的索引
    #define N 1
    #define Z 2
    #define P 3
    #define NB 1
    #define NS 2
    #define ZZ 3
    #define PS 4
    #define PB 5
    long position;               //编码器每获取一个脉冲，position+1
    double position_actual;      //当前位置
    double error_last = 0;              
    long   preTime=0;


        
    void setup() {
      // put your setup code here, to run once:
       Serial.begin(2000000);
       //setRange函数设置输入输出论域，输入范围为[-20，20]，输出范围为[-255，255]
       fuzzyController.setRange(-20,20,-20,20,-255,255);   
       /*addmf函数添加隶属度函数
        * 第一个参数表示变量类型（1表示input1，2表示input2，3表示input3）
        * 第二个参数表示模糊子集索引（模糊子集名称）
        * 后面的参数是三角型或梯形隶属度函数的参数    */
       fuzzyController.addmf(1,N,-20,-20,-5,0);
       fuzzyController.addmf(1,Z,-3,0,3);
       fuzzyController.addmf(1,P,0,5,20,20);
       fuzzyController.addmf(2,N,-20,-20,-5,0);
       fuzzyController.addmf(2,Z,-3,0,3);
       fuzzyController.addmf(2,P,0,5,20,20);
       fuzzyController.addmf(3,NB,-255,-180,-100);
       fuzzyController.addmf(3,NS,-200,-110,-20);
       fuzzyController.addmf(3,ZZ,-75,0,75);
       fuzzyController.addmf(3,PS,20,110,200);
       fuzzyController.addmf(3,PB,100,180,255);
       //setrulenum函数设置模糊控制规则总数
       fuzzyController.setrulenum(9); 
       //依次添加控制规则，第一次输入参数表示当前输入的是第几条规则
       //后面三个参数分别为input1、input2、output所属的模糊子集索引（模糊子集名称）
       fuzzyController.addrule(1,N,N,NB);  
       fuzzyController.addrule(2,N,Z,NS); 
       fuzzyController.addrule(3,N,P,ZZ); 
       fuzzyController.addrule(4,Z,N,NS); 
       fuzzyController.addrule(5,Z,Z,ZZ); 
       fuzzyController.addrule(6,Z,P,PS); 
       fuzzyController.addrule(7,P,N,ZZ); 
       fuzzyController.addrule(8,P,Z,PS);
       fuzzyController.addrule(9,P,P,PB); 
      pinMode(motor1, OUTPUT);                          //电机引脚设置
      pinMode(motor2, OUTPUT); 
      pinMode(ENA,OUTPUT);
      pinMode(STBY,OUTPUT);
      digitalWrite(STBY, HIGH);   
      pinMode(ENCODER_A_PIN, INPUT);
      pinMode(ENCODER_B_PIN, INPUT);
      attachInterrupt(0, read_quadrature, CHANGE);           //设置外部中断


         
    }

    void loop() {
    unsigned long now = millis();   //获取当前时间
    float position_ideal=fuzhi*sin(0.1*3.14*now/1000);  //产生正弦波
    float TimeCh = (now-preTime)/1000.0;  
    float SampleTime = 0.05;       //设置采样时间
    if (TimeCh>=SampleTime)
    {
     position_actual =(double) position/130;    //标准化当前位置，和编码器的线数有关，这里的position_actual单位是圈数
     double error = position_actual - position_ideal;        //计算当前时刻的追踪偏差
     double  DTerm = (error-error_last)/TimeCh;               //计算偏差变化率
     error = constrain(error,-20,20);  //避免输入量1超过设置的论域，对输入量1进行限幅
     DTerm = constrain(DTerm,-20,20);  //避免输入量2超过设置的论域，对输入量2进行限幅
    int  u = (int)fuzzyController.caculate(error,DTerm);  // 计算模糊控制器的输出量
    u = constrain(u,-255,255);  //对输出量限幅
    dianji(u);    //把计算得到的控制量传给电机转动函数
    error_last = error;   //把当前控制周期的追踪偏差赋值给error_last，给下个控制周期计算用
    preTime = now;        //把当前控制周期的时刻值赋值给preTime，给下个控制周期计算用
    serialprint(position_ideal,position_actual);  //串口监控函数
    }
    }
    /**************************************************************************
    函数功能：外部中断读取编码器数据
    **************************************************************************/
    void read_quadrature(){  
      // found a low-to-high on channel A ENA脚下降沿中断触发
      if (digitalRead(ENCODER_A_PIN) == LOW){   
        // check channel B to see which way 查询ENB的电平以确认是顺时针还是逆时针旋转
        if (digitalRead(ENCODER_B_PIN) == LOW)
          position++;
      }
      // found a high-to-low on channel A ENA脚上升沿中断触发
      else{
        // check channel B to see which way 查询ENB的电平以确认是顺时针还是逆时针旋转
        if (digitalRead(ENCODER_B_PIN) == LOW)
          position--;
      }
    } 
    /**************************************************************************
    函数功能：电机转动
    入口参数：控制量u
    **************************************************************************/
    void dianji(int u){
      if(u<0){
        digitalWrite(motor1, HIGH);  //设置电机转动方向
        digitalWrite(motor2, LOW);  
        analogWrite(ENA,abs(u));     //把PWM绝对值传入使能引脚，让电机转动
      }
      else{
         digitalWrite(motor1, LOW);  
         digitalWrite(motor2, HIGH);  
         analogWrite(ENA,abs(u));  
      }
    }
    /**************************************************************************
    函数功能：串口输出当前时刻理想的位置信号和当前位置
    **************************************************************************/
    void serialprint(float position_ideal,double position_actual){
    Serial.print(position_ideal); 
    Serial.print(" "); 
    Serial.println(position_actual);
    }

实验结果

打开Arduino IDE自带的串口绘图器，可以看到跟踪结果如下图所示，蓝色线为理想的位置信号，红色线为电机的实际位置。模糊控制的效果还是很不错的！
控制器性能的好坏取决于变量的模糊子集设定和建立的控制规则，想进一步提高控制效果可以通过定义更多的模糊子集、增加控制规则、修改隶属度函数参数等方式实现。

从串口输出的数据可以导出到matlab中进行进一步分析，透过数据可以分析这个控制器目前的问题，来设计更优的控制器。本程序中我设置的隶属度函数参数就是反复实验后确定的，下图是优化控制器参数前后的跟踪结果的对比图。可以看到，优化参数前的模糊控制器也是在追踪目标信号的，但是追踪误差很大，在根据实际情况优化隶属度函数参数后，追踪误差明显减小。

总结

从实验可以看出，使用本FuzzyControl库构造一个双输入单输出的模糊控制器是非常方便的，和在matlab中使用模糊工具箱搭建模糊控制器的过程很像，不论是给变量添加隶属度函数还是增减规则，库函数都可以直接实现。
目前的FuzzyControl库还有一些问题存在，比如说只支持三角形和梯形隶属度函数以及由于采用COG解模糊法所以输出变量的隶属度函数最好设置为等腰三角型。如果之后有时间，我会再更新FuzzyControl库，让它变得更完善。

控制算法，从仿真到实现

控制算法，从仿真到硬件实现

想对一个控制算法进行实物验证，往往需要我们搭建一个物理平台作为被控对象，这涉及到多领域学科，包括机械、软件、电子、运动学等。让控制算法从仿真到硬件平台实现，按照我的理解大致需要经过以下几个阶段：

  • 被控对象设计和建模
  • 在matlab上进行控制算法仿真验证
  • 电子元件的选择
  • 模块测试、编写控制算法进行联合调试

本文基于以上这个框架，以我用Arduino作为控制器实现倒立摆的状态反馈控制为例，展示让控制算法从仿真到硬件实现的每个阶段，并分享我在这个项目中遇到的问题、解决方案的选择和最终的效果。

被控对象设计和建模

控制算法多种多样，分为无模型控制和基于模型控制。如PID控制、模糊控制属于前者，状态反馈控制属于后者。

  • 无模型控制
    PID控制具有算法简单、无需建模的优点，所以对于建模困难的复杂系统而言，是一个优秀的选择方案。比如制作一个两轮自平衡车时，本质是对角度进行镇定控制，如果用PID算法来实现，我只需要得到当前的俯仰角，与期望的角度进行比较，通过PID算法计算输出量来控制电机转动。在这个过程中我可以把被控对象视为一个黑盒，关注输入和输出就可以了。在工控界PID是常青树，但是PID存在参数难以整定、不能保证稳定性等缺点。
    eVwlGt.md.png
  • 基于模型控制
    更多的控制算法还是依赖于被控对象模型的，不同的控制算法适用的控制系统类型也是不相同的。本项目中要实现的极点配置状态反馈控制就是最基本的一种线性系统控制算法。本文所搭建的框架同样适用于其他基于模型的控制算法。
被控对象设计
  • 选择合适的硬件平台
    不同的控制算法适用的控制系统类型也是不相同的，比如你想实现镇定控制那你肯定不能选择电机作为被控对象，因为电机本身就是稳定的，电机可以用来实现追踪控制。

我在这里要选择一个不稳定的线性系统作为被控对象，我选择了一阶倒立摆作为被控对象。倒立摆系统的控制问题一直是控制研究中的一个典型问题，控制目标是通过给小车底座施加一个力，使小车上的摆杆不倒下。虽然一阶倒立摆本身是非线性系统，但是可以在平衡点处进行线性化，在平衡点附近视为对线性系统进行控制。
eVwgZ4.jpg

  • 机械结构
    对于并不擅长机械设计的同学，可以像我一样从淘宝买倒立摆的机械部分，只需要几百元。
    eVwoQK.md.png
    光滑的水平导轨上有一个小车，小车上铰链一根摆杆，摆杆可以在垂面内做自由转动，铰链后方有一个角位移传感器可以测量摆杆偏离法线的角度。小车通过同步带和导轨一端的电机相连，电机转动可以带动小车沿水平方向左右运动，电机后端有一个编码器可以测量小车左右移动的位移。
    模型
    系统建模可以分为两种：机理建模和实验建模。机理建模是在了解研究对象的运动规律基础上，通过物理、化学的知识和数学手段建立起系统内部的输入—输出状态关系。实验建模是通过在研究对象上加上一系列的研究者事先确定的输入信号，激励研究对象并通过传感器检测其可观测的输出，应用数学手段建立起系统的输入—输出关系。

对于倒立摆系统，可以视为一个典型的运动的刚体系统，可以在惯性坐标系内应用经典力学理论建立系统的动力学方程。由于我还不会使用markdown插入latex公式，所以大家可以点击下面这个链接来学习用机理建模法建立直线一级倒立摆系统的数学模型。 这里值得注意的是：传统的倒立摆模型有两种，一是把施加在小车上的水平力作为控制输入，二是把小车的加速度作为控制输入。在这里我使用第二种方法，因为力是一个不可测的物理量，而加速度控制可以通过控制电机转速实现。
https://wenku.baidu.com/view/f09f7b287f21af45b307e87101f69e314232fa49.html
最终
在这里我忽视了所有的摩擦系数，带入了
eVc2Pe.png

极点配置控制是一种典型的全状态反馈控制方法，它可以把线性定常系统的极点移置到指定的期望闭环极点。极点配置定理：线性系统能任意配置极点的充分必要条件是，该系统状态完全可控。

1
2
3
4
A=[0 1 0 0 ; 0 0 0 0 ; 0 0 0 1 ; 0 0 39.73 0]; 
B=[0 ; 1 ; 0 ; 4.05];
P=[-1+1i -1-1i -2 -2];  %期望闭环极点
K=acker(A,B,P)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
t = 0;
dt = 0.1;       %采样周期设置为0.1s
finaltime=20;
X1=[];
X2=[];
X3=[];
X4=[];
U=[];
T=[];
x=zeros(4,1);
% 初始值设定
x(1)=0.2;
x(2)=0;
x(3)=0.04;
x(4)=0;
tSpan = [0 dt];
while t<finaltime
    u= -[ -0.2014   -0.4027   13.3164    1.5809]*[x(1);x(2);x(3);x(4)];
    [tt ,xx ]=ode45('daolibai',tSpan,x,[],u); %解ode45函数
    x = xx(length(xx),:);
    X1=[X1,x(1)];
    X2=[X2,x(2)];
    X3=[X3,x(3)];
    X4=[X4,x(4)];
    U=[U,u];
    T=[T,t];
    t=t+dt;  
end
%作图
ax1 = subplot(2,2,1);
plot(T,X1,'LineWidth',1.5);
ylabel('小车位移(m)');xlabel('时间(s)');
ax2 = subplot(2,2,2);
plot(T,X2,'LineWidth',1.5);
ylabel('小车速度(m/s)');xlabel('时间(s)');
ax3 = subplot(2,2,3);
plot(T,X3,'LineWidth',1.5);
ylabel('摆杆倾角(rad)');xlabel('时间(s)');
ax4 = subplot(2,2,4);
plot(T,X4,'LineWidth',1.5);
ylabel('摆杆角速度(rad/s)');xlabel('时间(s)');
axis([ax1 ax2],[0 15 -0.3 0.3]);
axis(ax3,[0 15 -0.05 0.05]);
axis(ax4,[0 15 -0.1 0.1]);
1
2
3
4
5
6
7
8
function dy= PlantModel(t,y,flag,para)
u = para;
dy = zeros(4,1);
dy(1)=y(2);
dy(2)=u;
dy(3)=y(4);
dy(4)=39.73*y(3)+4.05*u;
end