精华内容
下载资源
问答
  • 线性系统大作业——0.一阶和二阶倒立摆建模与控制系统设计
    2022-01-12 23:54:38

    0.摘要

    本文将针对一阶倒立摆和二阶倒立摆系统,对其利用《线性系统理论》课程中的相
    关知识进行分析,主要包括如下内容:

    1. 建立数学模型,1)牛顿定律;2)分析力学,即欧拉拉格朗日方程(两个方程应
      该是一样的);
    2. 基于 1 的结果,运用 Simulink 工具箱搭建仿真;
    3. 学习 Multibody 工具箱(即 SimMechancis 工具箱)建立基于图形用户界面的建
      模方式;
    4. 推导研究对象的线性化模型(平衡点、泰勒线性化展开);
    5. 指定系统的测量与控制变量,讨论系统线性化系统的的能控性和能观性、以及稳
      定性(Routh-Hurwitz 判据、Lyapunov 函数);
    6. 基于极点配置方法的控制器设计(要求闭环的仿真),使用 place、acker 等 MATLAB
      命令;
    7. 涉及状态观测器,包括全阶观测器、降阶观测器;
    8. 基于观测器的极点配置问题仿真;
    9. 基于 LQR 控制的设计方法,如 Riccatti 方程等。

    最后,本文中使用的 MATLAB 代码和 Simulink 仿真模型已经上传到 GitHub 上。
    Code: https://github.com/Cc19245/Inverted-pendulum.git

    1.一阶倒立摆分析与控制

    内容太多,在单独一篇文章中:线性系统大作业——1.一阶倒立摆建模与控制系统设计

    2.二阶倒立摆分析与控制

    内容太多,在单独一篇文章中:线性系统大作业——2.二阶倒立摆建模与控制系统设计

    3.问题与总结

    3.1.数学建模问题

    一阶倒立摆的建模相对来说比较简单,二阶倒立摆的建模就比较复杂了,并且经过验证本文中二阶倒立摆的建模也是存在一点小问题的。

    二阶倒立摆建模的参考了一篇西北工业大学的硕士论文,名字是《二级倒立摆系统的稳定控制研究》,作者叫刘琛。主要问题是文中对两个摆杆使用动量矩方程的时候是在非惯性系中使用的,也就是考虑了惯性力,然后对绕着转轴的点列力矩方程,也就是推导的公式中的转动惯量 I i I_i Ii应该是杆绕着转轴的转动惯量,即 1 3 m L 2 \frac{1}{3}mL^2 31mL2 L L L是gan杆的总长。但是最终建立的模型和SimMechancis 进行对比发现对不上,而把这个转动惯量改成杆绕着质心的转动惯量结果就对上了,也就是 1 12 m L 2 \frac{1}{12}mL^2 121mL2

    我感觉公式推导错误的原因可能是在非惯性系中的动量矩方程没有那么简单,可能有附加项。但是因为这个方法推导比较方便,而且结果只是转动惯量优点差别,加之推导比较麻烦,我就没有自己再去推导。如果想自己推导的话,方法还是牛顿定律和拉格朗日方程两种,有两点可以注意:

    1. 牛顿力学分析的话,针对摆杆分析可以使用刚体平面运动微分方程来建立数学模型,具体可以参见哈工大版的理论力学课本。这种分析方法应该是绝对正确的。
    2. 拉格朗日方程的推导,需要求导和求偏导,比较复杂,可以使用MATLAB进行符号函数的求导,非常方便。同理,上面使用牛顿力学推导的方法也可以使用MATLAB符号求导来简化计算量。

    3.2.二阶倒立摆的观测器设计

    二阶倒立摆的可观性分析中,如果只输出小车位移 x x x,那么系统也是能观的。但是这个时候如果设计观测器,会发现观测器的反馈增益 K e K_e Ke非常大,数量级在 1 0 5 10^5 105左右。然后仿真的时候就会出现数值稳定性问题,经常报错NaN错误或者出现复数的错误,但是如果把观测器的初始误差设置为0,就是初始观测就是完全准确的,那么就不会出现这些错误。初步分析了一下应该就是因为观测器的反馈增益太大了,导致计算溢出。

    因此后面选择输出小车位移 x x x和两个摆杆的角度 θ 1 \theta_1 θ1 θ 2 \theta_2 θ2,这样系统也是可观的,但是设计的观测器增益数量级只在 1 0 2 10^2 102左右,就不存在数值稳定性的问题。

    问题:为什么会这样呢?自己没有想的很明白。

    4.参考资料

    1.建模参考上面说的那篇硕士论文

    2.观测器控制器设计参考

    现代控制工程(第五版),相关的中英文电子书也在代码所在的github上。

    5.文件格式转化

    本文是大作业整理出来的,原文使用LaTeX撰稿,然后使用工具将LaTeX转化为MarkDown文件,转换后还是有点小错误,需要手动修改。

    转换方法:如何将tex文件改为markdown文件

    更多相关内容
  • 二阶倒立摆控制算法,三种方法,simulink实现
  • 二阶倒立摆控制算法,三种方法,simulink实现
  • 二阶倒立摆matlab代码古田摆 克莱尔·克里斯滕森(Claire Christensen),安德里亚·科尔特斯(Andrea Cortez),基思·格拉姆科(Keith Gramcko),米格尔·冈萨雷斯(Miguel Gonzalez),雅德利·奥多涅斯...
  • 二阶倒立摆matlab代码作业00 这些介绍性任务旨在为Matlab编码练习做准备。 其中包括代码审查和管理系统的设置,以及解决微分方程的计算方法的基本介绍。 如果您还没有一个GitHub帐户,请创建一个。 GitHub将是分配...
  • 使用模糊PID对simscape所建立的直线二阶倒立摆进行实时控制,使用环境为matlab2019b,压缩包内有两个文件,一个是simulink的模型,另外一个是使用模糊工具箱建立的模糊规则
  • 二阶倒立摆matlab代码倒立摆模型 该报告介绍了一个倒立摆的例子以及用于设计和实现模糊控制器的典型程序。 为了模拟模糊控制系统,必须指定倒立摆的数学模型。 使用MATLAB集成了表示钟摆数学模型的代码,实现了隶属...
  • 二阶倒立摆matlab代码项目清单。 每个分支机构都托管一个我在学习(到目前为止)计算机工程(专注于智能系统)过程中所做的项目。 说明和清单仍然是TODO。 硕士1 集成软件项目: isp 敏捷开发汽车共享移动应用程序的...
  • 二阶倒立摆建模与仿真 matlab simulink仿真
  • 二阶倒立摆,二阶倒立摆的simulink仿真,matlab源码.rar
  • 二阶倒立摆,二阶倒立摆的simulink仿真,matlab源码.zip
  • 二阶倒立摆建模与控制系统设计(上) 7.状态观测器设计 7.1.全阶观测器 前文已经验证,取系统的输出变量为x,θ1,θ2x,\theta_1,\theta_2x,θ1​,θ2​,系统是能观的。因此这里可以直接进行观测器的配置。假设所配置...

    本文上半部分见:线性系统大作业——2.二阶倒立摆建模与控制系统设计(上)

    最后,本文中使用的 MATLAB 代码和 Simulink 仿真模型已经上传到 GitHub 上。
    Code: https://github.com/Cc19245/Inverted-pendulum.git

    7.状态观测器设计

    7.1.全阶观测器

    前文已经验证,取系统的输出变量为 x , θ 1 , θ 2 x,\theta_1,\theta_2 x,θ1,θ2,系统是能观的。因此这里可以直接进行观测器的配置。假设所配置的观测器极点是状态反馈控制器的极点的三倍,即为 3 ∗ [ − 2 + j ∗ 2 , − 2 − j ∗ 2 , − 6 , − 7 , − 8 , − 9 ] 3*[-2+j*2,-2-j*2,-6,-7,-8,-9] 3[2+j2,2j2,6,7,8,9],编写MATLAB代码进行进行极点配置如下,需要注意的是此时系统属于多输入系统,因此只能使用place命令进行极点配置,而不能使用acker命令进行配置。

    A_ = A';
    B_ = C';
    C_ = B';
    J_ = 3*J;
    Ke = (place(A_, B_, J_))';
    disp('Ke = ');
    disp(Ke);
    

    程序输出结果为 K e = [ 132.9964 143.1468 0.4900 − 122.4083 214.2149 − 33.0750 0 − 99.2250 570.5250 27.2900 6.8972 0 − 5.1003 29.7100 0 0 0 45.0000 ] \begin{aligned} K_e = \left[ \begin{array}{ccc} 132.9964 & 143.1468 & 0.4900 \\ -122.4083 & 214.2149 & -33.0750 \\ 0 & -99.2250 & 570.5250 \\ 27.2900 & 6.8972 & 0 \\ -5.1003 & 29.7100 & 0 \\ 0 & 0 & 45.0000 \end{array}\right]\end{aligned} Ke=132.9964122.4083027.29005.10030143.1468214.214999.22506.897229.710000.490033.0750570.52500045.0000

    7.2.最小阶观测器

    对于二阶倒立摆系统来说,状态变量 x , θ 1 , θ 2 x,\theta_1,\theta_2 x,θ1,θ2等于输出 y y y,因此可以直接测量,剩余的状态变量这是不可测量的部分。对状态空间模型进行划分,划分后的降阶观测器是6-3=3阶的系统,假设期望配置的极点位置为 3 ∗ [ − 2 + j ∗ 2 , − 2 − j ∗ 2 , − 6 ] 3*[-2+j*2,-2-j*2,-6] 3[2+j2,2j2,6],则可编写MATLAB代码如下:

    Abb = A(1:3, 1:3);
    Aba = A(1:3, 4:end);
    Aab = A(4:end, 1:3);
    Aaa = A(4:end, 4:end);
    J_j = 3*J(1:3);   % 选择前三个极点
    Kb = K(4:end);
    Ke_j = (place(Abb',Aab',J_j))';
    disp('Ke_j = ');  % 注意这里把状态变量顺序换了,变成[x1;x2;x3;dx1;dx2;dx3]
    disp(Ke_j);
    

    程序输出结果为 K e j = [ 6 − 6 0 6 6 0 0 0 18 ] \begin{aligned} K_{ej} = \left[ \begin{array}{ccc} 6 & -6 & 0 \\ 6 & 6 & 0 \\ 0 & 0 & 18 \end{array}\right]\end{aligned} Kej=6606600018

    8.基于观测器的状态反馈控制

    8.1.基于全阶观测器的状态反馈控制

    搭建具有全阶观测器的状态反馈控制器的二阶倒立摆非线性系统Simulink仿真模型如图所示。

    使用全阶观测器的状态反馈控制Simulink仿真模型

    Simulink仿真模型中,全阶状态观测器由于设计变量较多,使用Simulink模块搭建较为复杂,因此编写s-function来实现,代码如下:

    % 二阶倒立摆系统的全阶观测器s-function建模
    function [sys,x0,str,ts,simStateCompliance] = 
    order2_all_observer_sfun(t,x,u,flag, x_0, th1_0, th2_0)
    switch flag,
        case 0,
            [sys,x0,str,ts,simStateCompliance]=
            mdlInitializeSizes(t,x,u, x_0, th1_0, th2_0);
        case 1,
            sys=mdlDerivatives(t,x,u);
        case 2,
            sys=mdlUpdate(t,x,u);
        case 3,
            sys=mdlOutputs(t,x,u);
        case 4,
            sys=mdlGetTimeOfNextVarHit(t,x,u);
        case 9,
            sys=mdlTerminate(t,x,u);
        otherwise
            DAStudio.error('Simulink:blocks:unhandledFlag', 
            num2str(flag));
    end
    % 主函数结束
    
    % ---------------------------------------------
    function [sys,x0,str,ts,simStateCompliance]=
    mdlInitializeSizes(t,x,u,x_0, th1_0, th2_0)
    % 初始化
    sizes = simsizes;% 生成sizes数据结构
    sizes.NumContStates  = 6;% 连续状态数
    sizes.NumDiscStates  = 0;% 离散状态数,缺省为 0
    sizes.NumOutputs     = 6;% 输出量个数,缺省为 0
    sizes.NumInputs      = 3;% 输入量个数,缺省为 0
    sizes.DirFeedthrough = 1;% 是否存在直接馈通。1:存在;0:不存在,缺省为 1 
    sizes.NumSampleTimes = 1;   % at least one sample time is needed
    sys = simsizes(sizes);
    x0  = [0; 0; 0; x_0; th1_0; th2_0];% 设置初始状态
    str = [];% 保留变量置空
    ts  = [0 0]; % 连续系统
    simStateCompliance = 'UnknownSimState';
    % end mdlInitializeSizes
    
    % ---------------------------------------------
    function sys=mdlDerivatives(t, x, u)
    assert(all(imag(u)==0), 'u is imaginary or nan');
    %  计算导数例程子函数
    A =[0, 0, 0, 0,   -4.4100,    0.4900;
        0, 0, 0, 0,   77.1750,  -33.0750;
        0, 0, 0, 0,  -99.2250,   84.5250;
        1, 0, 0, 0,         0,         0;
        0, 1, 0, 0,         0,         0;
        0, 0, 1, 0,         0,         0];
    B = [0.4667; -1.5000; 0.5000; 0; 0; 0];
    K = [23.4125, -0.5709, 44.4357, 22.3907, -283.0923, 379.2252];  
    Ke = [132.9964  143.1468    0.4900;
         -122.4083  214.2149  -33.0750;
                 0  -99.2250  570.5250;
           27.2900    6.8972         0;
           -5.1003   29.7100         0;
                 0         0   45.0000];
    sys = (A-B*K)*x + Ke*u ;  % 注意这里的u就是x(1)的观测误差e(1)
    
    % ---------------------------------------------
    function sys=mdlUpdate(t,x,u)
    %3. 状态更新例程子函数
    sys = [];
    
    %% ---------------------------------------------
    function sys=mdlOutputs(t,x,u)
    %4. 计算输出例程子函数
    sys=x;
    
    % ---------------------------------------------
    function sys=mdlGetTimeOfNextVarHit(t,x,u)
    % 5. 计算下一个采样时间,仅在系统是变采样时间系统时调用
    sampleTime = 1;    %  Example, set the next hit to be one second later.
    sys = t + sampleTime;
    
    % ---------------------------------------------
    function sys=mdlTerminate(t,x,u)
    % 6. 仿真结束时要调用的例程函数
    sys = [];
    

    假设系统初始状态为 [ x 1 , x 2 , x 3 , x 4 , x 5 , x 6 ] = [ x ˙ , θ 1 ˙ , θ 2 ˙ ] = [ 0 , 0 , 0 , 0 , 0 , 5 ∘ ] \left[x_1,x_2,x_3,x_4,x_5,x_6\right]=\left[\dot{x},\dot{\theta_1},\dot{\theta_2}\right]=\left[0,0,0,0,0,5^{\circ}\right] [x1,x2,x3,x4,x5,x6]=[x˙,θ1˙,θ2˙]=[0,0,0,0,0,5],并且状态观测器的初始观测结果为 [ x 1 ~ , x 2 ~ , x 3 ~ , x 4 ~ , x 5 ~ , x 6 ~ ] = [ 0 , 0 , 0 , 0 , 0 , 0 ] \left[\widetilde{x_1},\widetilde{x_2},\widetilde{x_3},\widetilde{x_4},\widetilde{x_5},\widetilde{x_6}\right]=\left[0,0,0,0,0,0\right] [x1 ,x2 ,x3 ,x4 ,x5 ,x6 ]=[0,0,0,0,0,0],运行Simulink仿真模型,系统的动态响应如图所示,全阶观测器的观测误差如图,可见系统可以很好地稳定。

    基于全阶观测器的状态反馈控制的系统动态响应

    全阶观测器的观测误差

    8.2.基于最小观测器的状态反馈控制

    为了检验基于最小阶观测器的状态反馈控制效果,搭建具有最小阶观测器的状态反馈控制器的二阶倒立摆非线性系统Simulink仿真模型如图所示。

    使用最小阶观测器的状态反馈控制Simulink仿真模型

    Simulink仿真模型中,最小阶状态观测器由于设计变量较多,使用Simulink模块搭建较为复杂,因此编写s-function来实现,代码如下:

    % 二阶倒立摆系统的最小阶观测器s-function建模
    function [sys,x0,str,ts,simStateCompliance] =
    order2_min_observer_sfun(t,x,u,flag, x_0, th1_0, th2_0, 
    dx_0, dth1_0, dth2_0)
    switch flag,
        case 0,
            [sys,x0,str,ts,simStateCompliance]=
            mdlInitializeSizes(t,x,u, x_0, th1_0, th2_0, 
            dx_0, dth1_0, dth2_0);
        case 1,
            sys=mdlDerivatives(t,x,u);
        case 2,
            sys=mdlUpdate(t,x,u);
        case 3,
            sys=mdlOutputs(t,x,u);
        case 4,
            sys=mdlGetTimeOfNextVarHit(t,x,u);
        case 9,
            sys=mdlTerminate(t,x,u);
        otherwise
            DAStudio.error('Simulink:blocks:unhandledFlag',
            num2str(flag));
    end
    % 主函数结束
    
    % ---------------------------------------------
    function [sys,x0,str,ts,simStateCompliance]=
    mdlInitializeSizes(t,x,u,x_0,th1_0,th2_0,dx_0,dth1_0,dth2_0)
    % 初始化
    sizes = simsizes;% 生成sizes数据结构
    sizes.NumContStates  = 3;% 连续状态数, 分别是dx, dth1, dth2
    sizes.NumDiscStates  = 0;% 离散状态数,缺省为 0
    sizes.NumOutputs     = 6;% 输出量个数,缺省为 0
    sizes.NumInputs      = 4;% 输入量个数,缺省为 0
    sizes.DirFeedthrough = 1;% 是否存在直接馈通。1:存在;0:不存在,缺省为 1 
    sizes.NumSampleTimes = 1;   % at least one sample time is needed
    sys = simsizes(sizes);       
    x0  = [dx_0; dth1_0; dth2_0] - 
    [6, -6, 0; 6, 6, 0; 0,0,18]*[x_0; th1_0; th2_0];  % 设置初始状态
    str = [];% 保留变量置空
    ts  = [0 0]; % 连续系统
    simStateCompliance = 'UnknownSimState';
    % end mdlInitializeSizes
    
    % ---------------------------------------------
    function sys=mdlDerivatives(t, x, u)
    %  计算导数例程子函数
    A = [0  0  0  0   -4.4100    0.4900;
         0  0  0  0   77.1750  -33.0750;
         0  0  0  0  -99.2250   84.5250;
         1  0  0  0      0         0;
         0  1  0  0      0         0;
         0  0  1  0      0         0];
    B = [0.4667; -1.5000; 0.5000; 0; 0; 0];
    Ke_j = [6    -6     0;
            6     6     0;
            0     0    18];
    A_hat = A(1:3, 1:3) - Ke_j*A(4:end, 1:3);
    B_hat = A_hat*Ke_j + A(1:3, 4:end) - Ke_j*A(4:end, 4:end);
    F_hat = B(1:3) - Ke_j*B(4:end);
    sys = A_hat*x + B_hat*[u(2);u(3);u(4)] + F_hat*u(1) ;  % u1是输入,u2/3/4是可观的变量x,th1,th2
    
    % ---------------------------------------------
    function sys=mdlUpdate(t,x,u)
    %3. 状态更新例程子函数
    sys = [];
    
    % ---------------------------------------------
    function sys=mdlOutputs(t,x,u)
    %4. 计算输出例程子函数
    C_hat = [1 0 0; 0 1 0; 0 0 1; 0 0 0; 0 0 0; 0 0 0];
    D_hat = [6 -6 0; 6 6 0; 0 0 18; 1 0 0; 0 1 0; 0 0 1];
    sys = C_hat*x + D_hat*[u(2);u(3);u(4)]; 
    
    % ---------------------------------------------
    function sys=mdlGetTimeOfNextVarHit(t,x,u)
    % 5. 计算下一个采样时间,仅在系统是变采样时间系统时调用
    sampleTime = 1;    %  Example, set the next hit to be one second later.
    sys = t + sampleTime;
    
    % ---------------------------------------------
    function sys=mdlTerminate(t,x,u)
    % 6. 仿真结束时要调用的例程函数
    sys = [];
    

    假设系统初始状态为 [ x 1 , x 2 , x 3 , x 4 , x 5 , x 6 ] = [ x ˙ , θ 1 ˙ , θ 2 ˙ ] = [ 0.05 , 1 0 ∘ / s , 1 0 ∘ / s , 0 , 0 , 5 ∘ ] \left[x_1,x_2,x_3,x_4,x_5,x_6\right]=\left[\dot{x},\dot{\theta_1},\dot{\theta_2}\right]=\left[0.05,10^{\circ}/s, 10^{\circ}/s, 0,0,5^{\circ}\right] [x1,x2,x3,x4,x5,x6]=[x˙,θ1˙,θ2˙]=[0.05,10/s,10/s,0,0,5],并且状态观测器的初始观测结果为 [ x 1 ~ , x 2 ~ , x 3 ~ , x 4 ~ , x 5 ~ , x 6 ~ ] = [ 0 , 0 , 5 ∘ / s , 0 , 0 , 0 ] \left[\widetilde{x_1},\widetilde{x_2},\widetilde{x_3},\widetilde{x_4},\widetilde{x_5},\widetilde{x_6}\right]=\left[0,0,5^{\circ}/s,0,0,0\right] [x1 ,x2 ,x3 ,x4 ,x5 ,x6 ]=[0,0,5/s,0,0,0],运行Simulink仿真模型,系统的动态响应如图所示,最小阶观测器的观测误差如图,可见系统可以很好地稳定。其中由于状态变量 x , θ 1 , θ 2 x,\theta_1,\theta_2 x,θ1,θ2是系统输出可以直接测量,因此 e 4 , e 5 , e 6 e_4,e_5,e_6 e4,e5,e6的误差一直是0。可见基于最小阶观测器的系统动态响应速度比使用全阶观测器效果更好,这是因为有三个状态变量可以进行准确的观测,因此系统收敛速度更快。

    基于最小阶观测器的状态反馈控制动态响应

    最小阶观测器的观测误差

    9.LQR控制

    选择 Q = I , R = I \boldsymbol Q= \boldsymbol I,\boldsymbol R= \boldsymbol I Q=I,R=I,使用MATLAB求解的代码如下:

    Q = eye(6);
    R = eye(1);
    K_lqr = lqr(A,B,Q,R);
    disp('K_lqr = ');
    disp(K_lqr);
    

    得到最优状态反馈矩阵为 K l q r = ( 3.2386 , − 10.7073 , 33.2032 , 1.0000 , − 286.7783 , 303.8728 ) K_lqr = (3.2386,-10.7073,33.2032,1.0000,-286.7783,303.8728) Klqr=(3.2386,10.7073,33.2032,1.0000,286.7783,303.8728)

    直接使用这个反馈控制矩阵在Simulink中进行二阶倒立摆的非线性模型LQR控制仿真,假设系统初始状态为 [ x 1 , x 2 , x 3 , x 4 , x 5 , x 6 ] = [ x ˙ , θ 1 ˙ , θ 2 ˙ ] = [ 0 , 0 , 0 , 0 , 0 , 5 ∘ ] \left[x_1,x_2,x_3,x_4,x_5,x_6\right]=\left[\dot{x},\dot{\theta_1},\dot{\theta_2}\right]=\left[0,0,0,0,0,5^{\circ}\right] [x1,x2,x3,x4,x5,x6]=[x˙,θ1˙,θ2˙]=[0,0,0,0,0,5],系统的动态响应如图所示,可见系统也可以稳定,不过相对于前面极点配置的控制效果来说系统的动态响应较慢。

    二阶倒立摆LOR控制的动态响应

    展开全文
  • 简介 本文是《线性系统理论》大作业的一部分,内容是一阶和二阶倒立摆的分析与控制,本文是 线性系统大作业——0.一阶和二阶倒立摆建模与控制系统设计 的一部分。 另外,由于本文字数过多,超过了CSDN单篇文章的字数...

    0.简介

    本文是《线性系统理论》大作业的一部分,内容是一阶和二阶倒立摆的分析与控制,本文是 线性系统大作业——0.一阶和二阶倒立摆建模与控制系统设计 的一部分。

    另外,由于本文字数过多,超过了CSDN单篇文章的字数限制,因此将本文分成了上下两部分。下半部分见:线性系统大作业——2.二阶倒立摆建模与控制系统设计(下)

    最后,本文中使用的 MATLAB 代码和 Simulink 仿真模型已经上传到 GitHub 上。
    Code: https://github.com/Cc19245/Inverted-pendulum.git

    1.建立数学模型

    二阶倒立摆物理模型
    给定二阶倒立摆的物理模型如图所示,给出系统的参数如表所示,假设两杆的质量都是均匀分布的,并且不考虑系统的摩擦。

    物理量数值
    小车质量( M M M) 2 k g 2kg 2kg
    下杆质量( m 1 m_1 m1) 0.5 k g 0.5kg 0.5kg
    上杆质量( m 2 m_2 m2) 0.5 k g 0.5kg 0.5kg
    下杆长度( L L L) 0.4 m 0.4m 0.4m
    上杆长度( L L L) 0.4 m 0.4m 0.4m

    1.1.牛顿运动定律分析

    对整个系统、下摆杆和上摆杆进行受力分析分别如图所示。

    系统整体受力

    下摆杆的受力

    上摆杆的受力

    上摆杆和下摆杆的质心坐标为 x 1 g = x + l 1 sin ⁡ θ 1 y 1 g = l 1 cos ⁡ θ 1 x 2 g = x + L sin ⁡ θ 1 + l 2 sin ⁡ θ 2 y 2 g = L cos ⁡ θ 1 + l 2 cos ⁡ θ 2 \begin{aligned} \begin{array}{cccc} &x_{1 g}=x+l_{1} \sin \theta_{1} \\ &y_{1 g}=l_{1} \cos \theta_{1} \\ &x_{2 g}=x+L \sin \theta_{1}+l_{2} \sin \theta_{2} \\ &y_{2 g}=L \cos \theta_{1}+l_{2} \cos \theta_{2} \end{array} \end{aligned} x1g=x+l1sinθ1y1g=l1cosθ1x2g=x+Lsinθ1+l2sinθ2y2g=Lcosθ1+l2cosθ2

    首先对系统进行整体的受力分析,在水平方向有 F = M x ¨ + m 1 ∂ 2 ( x 1 g ) ∂ t 2 + m 2 ∂ 2 ( x 2 g ) ∂ t 2 \begin{aligned} F =M\ddot{x}+m_{1} \frac{\partial^{2}\left(x_{1 g}\right)}{\partial t^{2}}+m_{2} \frac{\partial ^{2}\left(x_{2 g}\right)}{\partial t^{2}} \end{aligned} F=Mx¨+m1t22(x1g)+m2t22(x2g)

    对下摆杆在惯性系下受力分析,则下摆杆还受到自身加速度引起的惯性力,由动量矩定理,有
    f 2 y ′ L sin ⁡ θ 1 + m 1 g l 1 sin ⁡ θ 1 − f 2 x ′ L cos ⁡ θ 1 − m 1 x ¨ 1 g l 1 cos ⁡ θ 1 + m 1 y ¨ 1 g l 1 sin ⁡ θ 1 = I 1 θ ¨ 1 \begin{aligned} &f_{2 y}^{\prime} L \sin \theta_{1}+m_{1} g l_{1} \sin \theta_{1}-f_{2 x}^{\prime} L \cos \theta_{1}-m_{1} \ddot{x}_{1 g} l_1 \cos \theta_{1}+m_{1} \ddot{y}_{1 g} l_1 \sin \theta_{1}=I_{1} \ddot{\theta}_{1} \end{aligned} f2yLsinθ1+m1gl1sinθ1f2xLcosθ1m1x¨1gl1cosθ1+m1y¨1gl1sinθ1=I1θ¨1

    同理对上摆杆,有 m 2 g l 2 sin ⁡ θ 2 − m 2 l 2 cos ⁡ θ 2 x ¨ 2 g + m 2 l 2 sin ⁡ θ 2 y ¨ 2 g = I 2 θ ¨ 2 \begin{aligned} m_{2} g l_{2} \sin \theta_{2}-m_{2} l_{2} \cos \theta_{2} \ddot{x}_{2 g}+m_{2} l_{2} \sin \theta_{2} \ddot{y}_{2 g}=I_{2} \ddot{\theta}_{2} \end{aligned} m2gl2sinθ2m2l2cosθ2x¨2g+m2l2sinθ2y¨2g=I2θ¨2

    对上摆杆,在水平方向和竖直方向进行受力分析,由牛顿运动定律可得
    f 2 x = m 2 x ¨ 2 g = m 2 ( x + L sin ⁡ θ 1 + l 2 sin ⁡ θ 2 ) ′ ′ f 2 y − m 2 g = m 2 y ¨ 2 g = m 2 ( L cos ⁡ θ 1 + l 2 cos ⁡ θ 2 ) ′ ′ \begin{aligned} \begin{array}{c} f_{2 x}=m_{2} \ddot{x}_{2 g}=m_{2}\left(x+L \sin \theta_{1}+l_{2} \sin \theta_{2}\right)^{''} \\ f_{2 y} - m_2g=m_{2} \ddot{y}_{2 g}=m_{2}\left(L \cos \theta_{1}+l_{2} \cos \theta_{2}\right)^{''} \end{array} \end{aligned} f2x=m2x¨2g=m2(x+Lsinθ1+l2sinθ2)f2ym2g=m2y¨2g=m2(Lcosθ1+l2cosθ2)

    将式带入式可得 ( M + m 1 + m 2 ) x ¨ + ( m 1 l 1 + m 2 L ) cos ⁡ θ 1 ⋅ θ ¨ 1 + m 2 l 2 cos ⁡ θ 2 ⋅ θ ¨ 2 − ( M 1 l 1 + M 2 L ) sin ⁡ θ 1 ⋅ θ ˙ 1 2 − M 2 l 2 sin ⁡ θ 2 ⋅ θ ˙ 2 2 = F \begin{aligned} \begin{array}{cc} &\left(M+m_{1}+m_{2}\right) \ddot{x}+\left(m_{1} l_{1}+m_{2} L\right) \cos \theta_{1} \cdot \ddot{\theta}_{1}+m_{2} l_{2} \cos \theta_{2} \cdot \ddot{\theta}_{2} \\ &-\left(M_{1} l_{1}+M_{2} L\right) \sin \theta_{1} \cdot \dot{\theta}_{1}^{2}-M_{2} l_{2} \sin \theta_{2} \cdot \dot{\theta}_{2}^{2}=F \end{array} \end{aligned} (M+m1+m2)x¨+(m1l1+m2L)cosθ1θ¨1+m2l2cosθ2θ¨2(M1l1+M2L)sinθ1θ˙12M2l2sinθ2θ˙22=F

    将式和式带入式,可得 ( m 1 l 1 + m 2 L ) cos ⁡ θ 1 ⋅ x ¨ + ( I 1 + m 1 l 1 2 + m 2 L 2 ) θ ¨ 1 + m 2 L l 2 cos ⁡ ( θ 2 − θ 1 ) ⋅ θ ¨ 2 − m 2 L l 2 sin ⁡ ( θ 2 − θ 1 ) ⋅ θ ˙ 2 2 = ( m 1 l 1 + m 2 L ) g sin ⁡ θ 1 \begin{aligned} \begin{array}{cc} &\left(m_{1} l_{1}+m_{2} L\right) \cos \theta_{1} \cdot \ddot{x}+\left(I_{1}+m_{1} l_{1}^{2}+m_{2} L^{2}\right) \ddot{\theta}_{1}+m_{2} L l_{2} \cos \left(\theta_{2}-\theta_{1}\right) \cdot \ddot{\theta}_{2} \\ &-m_{2} L l_{2} \sin \left(\theta_{2}-\theta_{1}\right) \cdot \dot{\theta}_{2}^{2}=\left(m_{1} l_{1}+m_{2} L\right) g \sin \theta_{1} \end{array} \end{aligned} (m1l1+m2L)cosθ1x¨+(I1+m1l12+m2L2)θ¨1+m2Ll2cos(θ2θ1)θ¨2m2Ll2sin(θ2θ1)θ˙22=(m1l1+m2L)gsinθ1

    将式和式带入式,可得 m 2 l 2 cos ⁡ θ 2 ⋅ x ¨ + m 2 L l 2 cos ⁡ ( θ 2 − θ 1 ) ⋅ θ ¨ 1 + ( I 2 + m 2 l 2 2 ) θ ¨ 2 + m 2 L l 2 sin ⁡ ( θ 2 − θ 1 ) ⋅ θ ˙ 1 2 = m 2 g l 2 sin ⁡ θ 2 \begin{aligned} \begin{array}{cc} &m_{2} l_{2} \cos \theta_{2} \cdot \ddot{x}+m_{2} L l_{2} \cos \left(\theta_{2}-\theta_{1}\right) \cdot \ddot{\theta}_{1}+\left(I_{2}+m_{2} l_{2}^{2}\right) \ddot{\theta}_{2} \\ &+m_{2} L l_{2} \sin \left(\theta_{2}-\theta_{1}\right) \cdot \dot{\theta}_{1}^{2}=m_{2} g l_{2} \sin \theta_{2} \end{array} \end{aligned} m2l2cosθ2x¨+m2Ll2cos(θ2θ1)θ¨1+(I2+m2l22)θ¨2+m2Ll2sin(θ2θ1)θ˙12=m2gl2sinθ2

    则由上述三式组成的方程组即为二阶倒立摆的动力学方程。

    欧拉-拉格朗日方程分析

    选择小车位移 x x x和两个摆杆的角度 θ 1 \theta_1 θ1 θ 2 \theta_2 θ2作为广义坐标,小车推理 F F F作为广义力,则有
    T M = 1 2 M r ˙ 2 T m 1 = 1 2 I 1 θ ˙ 1 2 + 1 2 m 1 × { [ d d t ( r + l 1 sin ⁡ θ 1 ) ] 2 + [ d d t ( l 1 cos ⁡ θ 1 ) ] 2 } = 1 2 I 1 θ ˙ 1 2 + 1 2 m 1 × [ ( r ˙ + l 1 cos ⁡ θ 1 ⋅ θ ˙ 1 ) 2 + ( l 1 sin ⁡ θ 1 ⋅ θ ˙ 1 ) 2 ] T m 2 = 1 2 I 2 θ ˙ 2 2 + 1 2 m 2 × { [ d d t ( r + L 1 sin ⁡ θ 1 + l 2 sin ⁡ θ 2 ) ] 2 + [ d d t ( L 1 cos ⁡ θ 1 + l 2 cos ⁡ θ 2 ) ] 2 } = 1 2 I 2 θ ˙ 2 2 + 1 2 m 2 × [ ( r ˙ + L 1 cos ⁡ θ 1 ⋅ θ ˙ 1 + l 2 cos ⁡ θ 2 ⋅ θ ˙ 2 ) 2 + ( L 1 sin ⁡ θ 1 ⋅ θ ˙ 1 + l 2 sin ⁡ θ 2 ⋅ θ ˙ 2 ) 2 ] V M = 0 V m 1 = m 1 g l 1 cos ⁡ θ 1 V m 2 = m 2 g × ( L 1 cos ⁡ θ 1 + l 2 cos ⁡ θ 2 ) \begin{aligned} \begin{aligned} T_{M} &=\frac{1}{2} M \dot{r}^{2} \\ T_{m_1} &=\frac{1}{2} I_{1} \dot{\theta}_{1}^{2}+\frac{1}{2} m_{1} \times\left\{\left[\frac{d}{d t}\left(r+l_{1} \sin \theta_{1}\right)\right]^{2}+\left[\frac{d}{d t}\left(l_{1} \cos \theta_{1}\right)\right]^{2}\right\} \\ &=\frac{1}{2} I_{1} \dot{\theta}_{1}^{2}+\frac{1}{2} m_{1} \times\left[\left(\dot{r}+l_{1} \cos \theta_{1} \cdot \dot{\theta}_{1}\right)^{2}+\left(l_{1} \sin \theta_{1} \cdot \dot{\theta}_{1}\right)^{2}\right] \\ T_{m_2} &=\frac{1}{2} I_{2} \dot{\theta}_{2}^{2}+\frac{1}{2} m_{2} \times\left\{\left[\frac{d}{d t}\left(r+L_{1} \sin \theta_{1}+l_{2} \sin \theta_{2}\right)\right]^{2}+\left[\frac{d}{d t}\left(L_{1} \cos \theta_{1}+l_{2} \cos \theta_{2}\right)\right]^{2}\right\} \\ &=\frac{1}{2} I_{2} \dot{\theta}_{2}^{2}+\frac{1}{2} m_{2} \times\left[\left(\dot{r}+L_{1} \cos \theta_{1} \cdot \dot{\theta}_{1}+l_{2} \cos \theta_{2} \cdot \dot{\theta}_{2}\right)^{2}+\left(L_{1} \sin \theta_{1} \cdot \dot{\theta}_{1}+l_{2} \sin \theta_{2} \cdot \dot{\theta}_{2}\right)^{2}\right] \\ V_{M} &=0 \\ V_{m_1} &=m_{1} g l_{1} \cos \theta_{1} \\ V_{m_2} &=m_{2} g \times\left(L_{1} \cos \theta_{1}+l_{2} \cos \theta_{2}\right) \end{aligned}\end{aligned} TMTm1Tm2VMVm1Vm2=21Mr˙2=21I1θ˙12+21m1×{[dtd(r+l1sinθ1)]2+[dtd(l1cosθ1)]2}=21I1θ˙12+21m1×[(r˙+l1cosθ1θ˙1)2+(l1sinθ1θ˙1)2]=21I2θ˙22+21m2×{[dtd(r+L1sinθ1+l2sinθ2)]2+[dtd(L1cosθ1+l2cosθ2)]2}=21I2θ˙22+21m2×[(r˙+L1cosθ1θ˙1+l2cosθ2θ˙2)2+(L1sinθ1θ˙1+l2sinθ2θ˙2)2]=0=m1gl1cosθ1=m2g×(L1cosθ1+l2cosθ2)

    将上述变量带入欧拉-拉格朗日方程,化简可得系统数学模型为
    [ M 11 M 12 M 13 M 21 M 22 M 23 M 31 M 32 M 33 ] [ x ¨ θ ¨ 1 θ ¨ 2 ] + [ C 11 C 12 C 13 C 21 C 22 C 23 C 31 C 32 C 33 ] [ x ˙ θ ˙ 1 θ ˙ 2 ] + [ G 1 G 2 G 3 ] = [ u 0 0 ] \begin{aligned} \begin{array}{l} {\left[\begin{array}{lll} M_{11} & M_{12} & M_{13} \\ M_{21} & M_{22} & M_{23} \\ M_{31} & M_{32} & M_{33} \end{array}\right]\left[\begin{array}{c} \ddot{x} \\ \ddot{\theta}_{1} \\ \ddot{\theta}_{2} \end{array}\right]+} {\left[\begin{array}{lll} C_{11} & C_{12} & C_{13} \\ C_{21} & C_{22} & C_{23} \\ C_{31} & C_{32} & C_{33} \end{array}\right]\left[\begin{array}{c} \dot{x} \\ \dot{\theta}_{1} \\ \dot{\theta}_{2} \end{array}\right]+\left[\begin{array}{c} G_{1} \\ G_{2} \\ G_{3} \end{array}\right]=\left[\begin{array}{c} u \\ 0 \\ 0 \end{array}\right] } \end{array}\end{aligned} M11M21M31M12M22M32M13M23M33x¨θ¨1θ¨2+C11C21C31C12C22C32C13C23C33x˙θ˙1θ˙2+G1G2G3=u00

    其中, M 11 = M + m 1 + m 2 M 12 = ( m 1 l 1 + m 2 L ) cos ⁡ θ 1 M 13 = m 2 l 2 cos ⁡ θ 2 M 21 = M 12 M 22 = I 1 + m 1 l 1 2 + m 2 L 2 M 23 = m 2 L l 2 cos ⁡ ( θ 2 − θ 1 ) M 31 = M 13 M 32 = M 23 M 33 = I 2 + m 2 l 2 2 C 11 = 0 , C 12 = − ( m 1 l 1 + m 2 L ) θ ˙ 1 sin ⁡ θ 1 , C 13 = − m 2 l 2 θ ˙ 2 sin ⁡ θ 2 C 21 = 0 , C 22 = 0 , C 23 = − m 2 L l 2 θ ˙ 2 sin ⁡ ( θ 2 − θ 1 ) C 31 = 0 , C 32 = m 2 L l 2 θ ˙ 1 sin ⁡ ( θ 2 − θ 1 ) , C 33 = 0 G 1 = 0 , G 2 = − ( m 1 l 1 + m 2 L ) g sin ⁡ θ 1 , G 3 = − m 2 g l 2 sin ⁡ θ 2 u = F \begin{aligned} \begin{array}{l} M_{11}=M+m_{1}+m_{2} \\ M_{12}=\left(m_{1} l_{1}+m_{2} L\right) \cos \theta_{1} \\ M_{13}=m_{2} l_{2} \cos \theta_{2} \\ M_{21}=M_{12} \\ M_{22}= I_1 + m_{1} l_{1}^{2} + m_{2} L^{2} \\ M_{23}=m_{2} L l_{2} \cos \left(\theta_{2}-\theta_{1}\right) \\ M_{31}=M_{13} \\ M_{32}=M_{23} \\ M_{33} = I_{2} + m_{2} l_{2}^{2} \\ C_{11}=0, C_{12}=-\left(m_{1} l_{1}+m_{2} L\right) \dot{\theta}_{1} \sin \theta_{1}, C_{13}=-m_{2} l_{2} \dot{\theta}_{2} \sin \theta_{2}\\ C_{21}=0, C_{22}=0, C_{23}=-m_{2} L l_{2} \dot{\theta}_{2} \sin \left(\theta_{2}-\theta_{1}\right) \\ C_{31}=0, C_{32}=m_{2} L l_{2} \dot{\theta}_{1} \sin \left(\theta_{2}-\theta_{1}\right), C_{33}=0 \\ G_{1}=0, G_{2}=-\left(m_{1} l_{1}+m_{2} L\right) g \sin \theta_{1}, G_{3}=-m_{2} g l_{2} \sin \theta_{2} \\ u=F \end{array} \end{aligned} M11=M+m1+m2M12=(m1l1+m2L)cosθ1M13=m2l2cosθ2M21=M12M22=I1+m1l12+m2L2M23=m2Ll2cos(θ2θ1)M31=M13M32=M23M33=I2+m2l22C11=0,C12=(m1l1+m2L)θ˙1sinθ1,C13=m2l2θ˙2sinθ2C21=0,C22=0,C23=m2Ll2θ˙2sin(θ2θ1)C31=0,C32=m2Ll2θ˙1sin(θ2θ1),C33=0G1=0,G2=(m1l1+m2L)gsinθ1,G3=m2gl2sinθ2u=F

    由此可见,使用欧拉-拉格朗日方程建立的系统动力学方程和使用牛顿运动定律建立的动力学方程是完全相同的,因此初步验证了所建立的动力学方程的正确性。

    2.Simulink仿真

    由二阶倒立摆系统的动力学方程可以发现,系统存在非常严重的耦合现象,因此直接使用Simulink搭建仿真模型较为麻烦,因此这里使用S-Fuction来建立系统的仿真模型。编写MATLAB代码实现如下:

    % 二阶倒立摆系统的动力学方程s-function建模
    function [sys,x0,str,ts,simStateCompliance] =
    order2_sfun(t,x,u,flag, x_0, theta1_0, theta2_0)
    switch flag,
        case 0,
            [sys,x0,str,ts,simStateCompliance]=
            mdlInitializeSizes(t,x,u, x_0, theta1_0, theta2_0);
        case 1,
            sys=mdlDerivatives(t,x,u);
        case 2,
            sys=mdlUpdate(t,x,u);
        case 3,
            sys=mdlOutputs(t,x,u);
        case 4,
            sys=mdlGetTimeOfNextVarHit(t,x,u);
        case 9,
            sys=mdlTerminate(t,x,u);
        otherwise
            DAStudio.error('Simulink:blocks:unhandledFlag', 
            num2str(flag));
    end
    % 主函数结束
    
    % ---------------------------------------------
    function [sys,x0,str,ts,simStateCompliance]=
    mdlInitializeSizes(t,x,u, x_0, theta1_0, theta2_0)
    % 初始化
    sizes = simsizes;% 生成sizes数据结构
    sizes.NumContStates  = 6;% 连续状态数, 分别是x', theta1', theta2', x, theta1, theta2
    sizes.NumDiscStates  = 0;% 离散状态数,缺省为 0
    sizes.NumOutputs     = 6;% 输出量个数,缺省为 0, 
    sizes.NumInputs      = 1;% 输入量个数,缺省为 0
    sizes.DirFeedthrough = 1;%是否存在直接馈通。1:存在;0:不存在,缺省为 1 
    sizes.NumSampleTimes = 1;   % at least one sample time is needed
    
    sys = simsizes(sizes);
    x0  = [0; 0; 0; x_0; theta1_0; theta2_0];% 设置初始状态
    str = [];% 保留变量置空
    ts  = [0 0]; % 连续系统
    simStateCompliance = 'UnknownSimState';
    % end mdlInitializeSizes
    
    % ---------------------------------------------
    function sys=mdlDerivatives(t,x,u)
    %  计算导数例程子函数
    M=2; m1=0.5; m2=0.5; l1=0.2; l2=0.2; L=0.4; g=9.8
    I1 = 1/12*m1*(2*l1)^2; I2 = 1/12*m2*(2*l2)^2;
    dx_ = x(1); dth1_ = x(2); dth2_ = x(3);
    x_ = x(4); th1_ = x(5); th2_ = x(6); 
    M11 = M + m1 + m2;
    M12 = (m1*l1+m2*L)*cos(th1_);
    M13 = m2*l2*cos(th2_);
    M21 = M12;
    M22 = I1 + m1*l1^2 + m2*L^2;
    M23 = m2*L*l2*cos(th2_ - th1_);
    M31 = M13;
    M32 = M23;
    M33 = I2 + m2*l2^2;
    C11 = 0; C12 = -(m1*l1+m2*L)*sin(th1_)*dth1_;
    C13 = -m2*l2*sin(th2_)*dth2_;
    C21 = 0; C22 = 0; C23 = -m2*L*l2*dth2_*sin(th2_ - th1_);
    C31 = 0; C32 = m2*L*l2*dth1_*sin(th2_ - th1_); C33 = 0;
    G1 = 0; G2 = -(m1*l1+m2*L)*g*sin(th1_); 
    G3 = -m2*g*l2*sin(th2_);
    A = [M11 M12 M13 C11 C12 C13;
         M21 M22 M23 C21 C22 C23;
         M31 M32 M33 C31 C32 C33;
          0   0   0   1   0   0 ;
          0   0   0   0   1   0 ;
          0   0   0   0   0   1 ];
    B = [u-G1;
         -G2;
         -G3;
         dx_;
         dth1_;
         dth2_];
    sys = A\B;
    
    % ---------------------------------------------
    function sys=mdlUpdate(t,x,u)
    %3. 状态更新例程子函数
    sys = [];
    
    % ---------------------------------------------
    function sys=mdlOutputs(t,x,u)
    %4. 计算输出例程子函数
    sys=[x(1);x(2);x(3);x(4);x(5);x(6)];
    
    % ---------------------------------------------
    function sys=mdlGetTimeOfNextVarHit(t,x,u)
     % 5. 计算下一个采样时间,仅在系统是变采样时间系统时调用
    sampleTime = 1;    %  Example, set the next hit to be one second later.
    sys = t + sampleTime;
    
    % ---------------------------------------------
    function sys=mdlTerminate(t,x,u)
     % 6. 仿真结束时要调用的例程函数
    sys = [];
    

    利用s-function在Simulink中搭建系统的仿真模型如图所示。

    二阶倒立摆的Simulink仿真模型

    由于二阶倒立摆很不稳定,所以为了看系统初始状态下存在小扰动时系统的动态响应,假设系统的初始状态偏离渐进稳定点,即 x = 0 , θ 1 = π , θ 2 = 5 6 π x=0,\theta_1=\pi,\theta_2=\frac{5}{6}\pi x=0,θ1=π,θ2=65π,且系统无输入,则此后小车位置 x x x和倒立摆的摆角 θ 1 \theta_1 θ1 θ 2 \theta_2 θ2的变化如图所示。

    使用Simulink建模的二阶倒立摆初始响应

    3.使用SimMechancis仿真

    如图所示,是使用SimMechanicalcs建立二阶倒立摆的物理模型。其中增益模块的增益值-1是由于SimMechanicalcs中的 θ \theta θ方向和上面推导的方向相反,另外值得注意的是,在SimMechanicalcs中的 θ 2 \theta_2 θ2角度定义的参考系和上面的推导也不相同,它的角度是以下杆的方向为参考进行角度定义,也就是下杆和上杆之间的角度。因此为了和上面的公式推导结果相对比,在接入示波器之前将 θ 1 \theta_1 θ1 θ 2 \theta_2 θ2加起来。如图所示,是使用SimMechanicalcs的仿真结果。

    二阶倒立摆SimMechancis模型

    二阶倒立摆SimMechancis模型的动态响应

    由图可见这个结果和上面动力学建模并使用Simulink仿真的结果完全相同,从而证明了之前动力学建模的正确性。

    4.在平衡点附近模型线性化

    系统在平衡点附近时, θ 1 \theta_1 θ1 θ 2 \theta_2 θ2都很小,并且假设其角速度 θ 1 ˙ \dot{\theta_1} θ1˙ θ 2 ˙ \dot{\theta_2} θ2˙也很小,则可进行近似处理: cos ⁡ θ ≈ 1 , sin ⁡ θ ≈ θ , sin ⁡ θ θ ˙ ≈ 0 \cos{\theta} \approx 1, \sin\theta \approx \theta, \sin \theta \dot{\theta} \approx0 cosθ1,sinθθ,sinθθ˙0。从而得到二阶倒立摆系统在平衡点附近的动力学方程为
    [ M + m 1 + m 2 m 1 l 1 + m 2 L m 2 l 2 m 1 l 1 + m 2 L I 1 + m 1 l 1 2 + m 2 L 2 m 2 L l 2 m 2 l 2 m 2 L l 2 I 2 + m 2 l 2 2 ] [ x ¨ θ ¨ 1 θ ¨ 2 ] = [ 0 0 0 0 ( m 1 l 1 + m 2 L ) g 0 0 0 m 2 g l 2 ] [ x θ 1 θ 2 ] + [ 1 0 0 ] u \begin{aligned} \begin{array}{c} &\left[ \begin{array}{ccc} M+m_1+m_2 & m_1l_1+m_2L & m_2l_2 \\ m_1l_1+m_2L & I_1+m_1l_1^2+m_2L^2 & m_2Ll_2 \\ m_2l_2 & m_2Ll_2 & I_2+m_2l_2^2 \end{array}\right]\left[ \begin{array}{c} \ddot{x} \\ \ddot{\theta}_{1} \\ \ddot{\theta}_{2} \end{array}\right] &=\left[\begin{array}{ccc} 0 & 0 & 0 \\ 0 & (m_1l_1+m_2L)g & 0 \\ 0 & 0 & m_2gl_2 \end{array}\right]\left[ \begin{array}{c} x \\ \theta_{1} \\ \theta_{2} \end{array}\right]+\left[ \begin{array}{c} 1 \\ 0 \\ 0 \end{array}\right]u \end{array}\end{aligned} M+m1+m2m1l1+m2Lm2l2m1l1+m2LI1+m1l12+m2L2m2Ll2m2l2m2Ll2I2+m2l22x¨θ¨1θ¨2=0000(m1l1+m2L)g000m2gl2xθ1θ2+100u

    使用MATLAB符号函数求逆的功能求解可得 [ x ¨ θ ¨ 1 θ ¨ 2 ] = [ a 11 a 12 a 13 a 21 a 22 a 23 a 31 a 32 a 33 ] [ x θ 1 θ 2 ] + [ b 1 b 2 b 3 ] u \begin{aligned} \begin{array}{c} {\left[ \begin{array}{c} \ddot{x} \\ \ddot{\theta}_{1} \\ \ddot{\theta}_{2} \end{array} \right]= \left[ \begin{array}{ccc} a_{11} & a_{12} & a_{13}\\ a_{21} & a_{22} & a_{23}\\ a_{31} & a_{32} & a_{33}\\ \end{array} \right] \left[ \begin{array}{c} x \\ \theta_{1} \\ \theta_{2} \end{array} \right]+ \left[ \begin{array}{c} b_{1} \\ b_{2} \\ b_{3} \end{array} \right]u} \end{array}\end{aligned} x¨θ¨1θ¨2=a11a21a31a12a22a32a13a23a33xθ1θ2+b1b2b3u

    其中, a 11 = 0 a 12 = − I 2 g L 2 m 2 2 + g L l 1 l 2 2 m 1 m 2 2 + 2 I 2 g L l 1 m 1 m 2 + g l 1 2 l 2 2 m 1 2 m 2 + I 2 g l 1 2 m 1 2 I 1 I 2 M + I 1 I 2 m 1 + I 1 I 2 m 2 + I 2 L 2 M m 2 + I 2 M l 1 2 m 1 + I 1 M l 2 2 m 2 + I 2 L 2 m 1 m 2 + I 1 l 2 2 m 1 m 2 + I 2 l 1 2 m 1 m 2 + M l 1 2 l 2 2 m 1 m 2 − 2 I 2 L l 1 m 1 m 2 a 13 = − g m 1 l 1 2 l 2 2 m 2 2 − L g m 1 l 1 l 2 2 m 2 2 + I 1 g l 2 2 m 2 2 I 1 I 2 M + I 1 I 2 m 1 + I 1 I 2 m 2 + I 2 L 2 M m 2 + I 2 M l 1 2 m 1 + I 1 M l 2 2 m 2 + I 2 L 2 m 1 m 2 + I 1 l 2 2 m 1 m 2 + I 2 l 1 2 m 1 m 2 + M l 1 2 l 2 2 m 1 m 2 − 2 I 2 L l 1 m 1 m 2 a 21 = 0 a 22 = g ( I 2 L m 2 2 + I 2 l 1 m 1 2 + L M l 2 2 m 2 2 + I 2 L M m 2 + L l 2 2 m 1 m 2 2 + I 2 M l 1 m 1 + I 2 L m 1 m 2 + l 1 l 2 2 m 1 2 m 2 + I 2 l 1 m 1 m 2 + M l 1 l 2 2 m 1 m 2 ) I 1 I 2 M + I 1 I 2 m 1 + I 1 I 2 m 2 + I 2 L 2 M m 2 + I 2 M l 1 2 m 1 + I 1 M l 2 2 m 2 + I 2 L 2 m 1 m 2 + I 1 l 2 2 m 1 m 2 + I 2 l 1 2 m 1 m 2 + M l 1 2 l 2 2 m 1 m 2 − 2 I 2 L l 1 m 1 m 2 a 23 = − g l 2 2 m 2 2 ( L m 1 − l 1 m 1 + L M ) I 1 I 2 M + I 1 I 2 m 1 + I 1 I 2 m 2 + I 2 L 2 M m 2 + I 2 M l 1 2 m 1 + I 1 M l 2 2 m 2 + I 2 L 2 m 1 m 2 + I 1 l 2 2 m 1 m 2 + I 2 l 1 2 m 1 m 2 + M l 1 2 l 2 2 m 1 m 2 − 2 I 2 L l 1 m 1 m 2 a 31 = 0 a 32 = − g l 2 m 2 ( L 2 M m 2 − l 1 2 m 1 2 + L l 1 m 1 2 + L 2 m 1 m 2 + L M l 1 m 1 − L l 1 m 1 m 2 ) I 1 I 2 M + I 1 I 2 m 1 + I 1 I 2 m 2 + I 2 L 2 M m 2 + I 2 M l 1 2 m 1 + I 1 M l 2 2 m 2 + I 2 L 2 m 1 m 2 + I 1 l 2 2 m 1 m 2 + I 2 l 1 2 m 1 m 2 + M l 1 2 l 2 2 m 1 m 2 − 2 I 2 L l 1 m 1 m 2 a 33 = g l 2 m 2 ( I 1 m 1 + I 1 m 2 + I 1 M + l 1 2 m 1 m 2 + L 2 M m 2 + M l 1 2 m 1 + L 2 m 1 m 2 − 2 L l 1 m 1 m 2 ) I 1 I 2 M + I 1 I 2 m 1 + I 1 I 2 m 2 + I 2 L 2 M m 2 + I 2 M l 1 2 m 1 + I 1 M l 2 2 m 2 + I 2 L 2 m 1 m 2 + I 1 l 2 2 m 1 m 2 + I 2 l 1 2 m 1 m 2 + M l 1 2 l 2 2 m 1 m 2 − 2 I 2 L l 1 m 1 m 2 b 1 = I 2 m 2 L 2 + m 1 m 2 l 1 2 l 2 2 + I 2 m 1 l 1 2 + I 1 m 2 l 2 2 + I 1 I 2 I 1 I 2 M + I 1 I 2 m 1 + I 1 I 2 m 2 + I 2 L 2 M m 2 + I 2 M l 1 2 m 1 + I 1 M l 2 2 m 2 + I 2 L 2 m 1 m 2 + I 1 l 2 2 m 1 m 2 + I 2 l 1 2 m 1 m 2 + M l 1 2 l 2 2 m 1 m 2 − 2 I 2 L l 1 m 1 m 2 b 2 = − l 1 m 1 m 2 l 2 2 + I 2 L m 2 + I 2 l 1 m 1 I 1 I 2 M + I 1 I 2 m 1 + I 1 I 2 m 2 + I 2 L 2 M m 2 + I 2 M l 1 2 m 1 + I 1 M l 2 2 m 2 + I 2 L 2 m 1 m 2 + I 1 l 2 2 m 1 m 2 + I 2 l 1 2 m 1 m 2 + M l 1 2 l 2 2 m 1 m 2 − 2 I 2 L l 1 m 1 m 2 b 3 = − l 2 m 2 ( m 1 l 1 2 − L m 1 l 1 + I 1 ) I 1 I 2 M + I 1 I 2 m 1 + I 1 I 2 m 2 + I 2 L 2 M m 2 + I 2 M l 1 2 m 1 + I 1 M l 2 2 m 2 + I 2 L 2 m 1 m 2 + I 1 l 2 2 m 1 m 2 + I 2 l 1 2 m 1 m 2 + M l 1 2 l 2 2 m 1 m 2 − 2 I 2 L l 1 m 1 m 2 \begin{aligned} \begin{array}{l} a_{11}=0 \\ a_{12}= -\frac{I_2gL^2m_2^2 + gLl_1l_2^2m_1m_2^2 + 2I_2gLl_1m_1m_2 + gl_1^2l_2^2m_1^2m_2 + I_2gl_1^2m_1^2}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2} \\ a_{13}=-\frac{gm_1l_1^2l_2^2m_2^2 - Lgm_1l_1l_2^2m_2^2 + I_1gl_2^2m_2^2}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ a_{21}=0 \\ a_{22}=\frac{g(I_2Lm_2^2 + I_2l_1m_1^2 + LMl_2^2m_2^2 + I_2LMm_2 + Ll_2^2m_1m_2^2 + I_2Ml_1m_1 + I_2Lm_1m_2 + l_1l_2^2m_1^2m_2 + I_2l_1m_1m_2 + Ml_1l_2^2m_1m_2)}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ a_{23}=-\frac{gl_2^2m_2^2(Lm_1 - l_1m_1 + LM)}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ a_{31}=0 \\ a_{32}=-\frac{gl_2m_2(L^2Mm_2 - l_1^2m_1^2 + Ll_1m_1^2 + L^2m_1m_2 + LMl_1m_1 - Ll_1m_1m_2)}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ a_{33}=\frac{gl_2m_2(I_1m_1 + I_1m_2 + I_1M + l_1^2m_1m_2 + L^2Mm_2 + Ml_1^2m_1 + L^2m_1m_2 - 2Ll_1m_1m_2)}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2} \\ b_{1}=\frac{I_2m_2L^2 + m_1m_2l_1^2l_2^2 + I_2m_1l_1^2 + I_1m_2l_2^2 + I_1I_2}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ b_{2}=-\frac{ l_1m_1m_2l_2^2 + I_2Lm_2 + I_2l_1m_1}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2}\\ b_{3}=-\frac{l_2m_2(m_1l_1^2 - Lm_1l_1 + I_1)}{I_1I_2M + I_1I_2m_1 + I_1I_2m_2 + I_2L^2Mm_2 + I_2Ml_1^2m_1 + I_1Ml_2^2m_2 + I_2L^2m_1m_2 + I_1l_2^2m_1m_2 + I_2l_1^2m_1m_2 + Ml_1^2l_2^2m_1m_2 - 2I_2Ll_1m_1m_2} \end{array} \end{aligned} a11=0a12=I1I2M+I1I2m1+I1I2m2+I2L2Mm2+I2Ml12m1+I1Ml22m2+I2L2m1m2+I1l22m1m2+I2l12m1m2+Ml12l22m1m22I2Ll1m1m2I2gL2m22+gLl1l22m1m22+2I2gLl1m1m2+gl12l22m12m2+I2gl12m12a13=I1I2M+I1I2m1+I1I2m2+I2L2Mm2+I2Ml12m1+I1Ml22m2+I2L2m1m2+I1l22m1m2+I2l12m1m2+Ml12l22m1m22I2Ll1m1m2gm1l12l22m22Lgm1l1l22m22+I1gl22m22a21=0a22=I1I2M+I1I2m1+I1I2m2+I2L2Mm2+I2Ml12m1+I1Ml22m2+I2L2m1m2+I1l22m1m2+I2l12m1m2+Ml12l22m1m22I2Ll1m1m2g(I2Lm22+I2l1m12+LMl22m22+I2LMm2+Ll22m1m22+I2Ml1m1+I2Lm1m2+l1l22m12m2+I2l1m1m2+Ml1l22m1m2)a23=I1I2M+I1I2m1+I1I2m2+I2L2Mm2+I2Ml12m1+I1Ml22m2+I2L2m1m2+I1l22m1m2+I2l12m1m2+Ml12l22m1m22I2Ll1m1m2gl22m22(Lm1l1m1+LM)a31=0a32=I1I2M+I1I2m1+I1I2m2+I2L2Mm2+I2Ml12m1+I1Ml22m2+I2L2m1m2+I1l22m1m2+I2l12m1m2+Ml12l22m1m22I2Ll1m1m2gl2m2(L2Mm2l12m12+Ll1m12+L2m1m2+LMl1m1Ll1m1m2)a33=I1I2M+I1I2m1+I1I2m2+I2L2Mm2+I2Ml12m1+I1Ml22m2+I2L2m1m2+I1l22m1m2+I2l12m1m2+Ml12l22m1m22I2Ll1m1m2gl2m2(I1m1+I1m2+I1M+l12m1m2+L2Mm2+Ml12m1+L2m1m22Ll1m