跳转到内容

倒立摆控制

利用低代码控制器取代原本实验模型中包括前馈控制,PI反馈控制以及LQR控制算法在内的整个控制部分,并达到控制小车移动,摆杆不倒的控制目标。

该程序可在以下环境运行:

  1. Matlab R2019a + Simulink v9.3及以上版本;

  2. Matlab APP:Instrument Control Toolbox;

  3. 低代码控制器。

原始模型图

倒立摆控制案例的原始模型来自于Matlab官方提供的Simulink例程中的Inverted Pendulum with Animation模型

可以在matlab命令行窗口中输入以下命令打开这一案例:

openExample('simulink_general/penddemoExample')

原始仿真模型如下图所示:

倒立摆原始仿真模型

改造模型图

倒立摆改造仿真模型

图中的modbusCom就是本实验模型中的控制器模块,实验模型中可以看出,控制器取代了包括PI反馈控制,前馈控制和LQR控制环节在内的所有控制环节,模型十分简洁。

modbusCom.m是本实验模型控制器的源文件,建立与低代码控制器起通信的作用,仍然是以S-Function模板为基础编写的Modbus通信模块。

下面表示的是模块输入和输出值的个数,本实验中的输入有三个,包括设定小车的位置,测定小车的位置以及测定的摆杆与垂直方向的夹角。输出有一个,就是控制器的输出,控制的是输出在小车上的作用力。

sizes.NumContStates = 0;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 1;
sizes.NumInputs = 3;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;

下面表示的是模块的采样时间为0.02s。

ts = [0.02,0];

下面的代码表示建立与控制器的通信。

m = modbus('tcpip', '127.0.0.1', 502); % 建立modbus通信

如下面的代码所示,需要将输入写入控制器,再从控制器中读取控制信号。图中的pause(0.05)用于设置等待时间,当控制器和计算机的性能较好时,可以适当修改这个参数,可以加快仿真通信的速度,使动画跟随的动作加快。

function sys=mdlOutputs(t,x,u)
global m;
write(m,'holdingregs',2,u','double'); % 将小车位置设定值、小车位置测量值和倒立摆角度写入控制器
write(m,'holdingregs',18,1); % 将发起通信信号写入控制器
pause(0.05); % 等待50ms后读取当前控制值;需另行核对本次计算是否完成
sys = read(m,'holdingregs',14,1,'double'); % 读取控制信号

测点配置表用于定义控制器的输入、输出和需要监视的中间结果,包含点号、名称、别名、是否离散、是否计算点和默认值等属性。点号是程序中的唯一标识,与通信通道里的点号一一对应,必须大于等于100001;别名由英文字母、数字和下划线组成,可在AOE的条件、变量定义及动作表达式中引用,使表达式更易读。离散/模拟类型决定值的存储和置点参数列;默认值用于初始化,计算点另需填写计算公式。下面列出本例实际使用的测点。

所列测点的共同字段:是否计算点均为 FALSE;默认值均为 0。

点号别名是否离散用途
100001Cartset_POINTFALSE设定的小车位置
100002Cart_POINTFALSE测量的小车位置
100003Pendulum_POINTFALSE测量的倒立摆与竖直方向夹角
100004U_POINTFALSE控制器下发的遥调->设定作用在小车上的力
100005DoCal_POINTTRUE根据测点状态判断控制器是否需要进行计算
完整配置表(保留导入列顺序)
序号点号名称别名是否离散是否计算点默认值备注
1100001测点1Cartset_POINTFALSEFALSE0设定的小车位置
2100002测点2Cart_POINTFALSEFALSE0测量的小车位置
3100003测点3Pendulum_POINTFALSEFALSE0测量的倒立摆与竖直方向夹角
4100004测点4U_POINTFALSEFALSE0控制器下发的遥调->设定作用在小车上的力
5100005测点5DoCal_POINTTRUEFALSE0根据测点状态判断控制器是否需要进行计算

模块输入端连接

模块输出端连接

本程序需要五个测点:设定的小车位置、测量的小车位置、测量的摆杆角度、控制器输出作用力,以及DoCal_POINT输入就绪信号。其中的别名对应的就是AOE文件之中设置节点事件的表达式的参量。表示在matlab的Simulink模型中是如图所示的几个参数。

第五个测点DoCal_POINT不是实际物理测点,并不是用于获取控制信号的,每通信一次,这个测点将被设成1让控制器进行计算,控制器开始执行AOE控制策略的时候这个测点会被置0。

通道参数

参数设置
通道名称server测试通道
连接个数1
服务端口502

连接参数

参数设置
连接名称测试通道
测点个数5
客户端IP127.0.0.1
客户端端口9999
slave id1
通信协议XA
一次读寄存器数上限125
一次读开关数上限2000

服务端监听502,映射5个测点。客户端IP填MATLAB所在计算机的地址;同机实验可用127.0.0.1。客户端端口9999是任意源端口的匹配标记,不要求MATLAB绑定9999。

共同字段:寄存器类型均为 HOLDING;新请求标志均为 FALSE;轮询周期均为 2000。

点号起始地址数据类型
1000011EightByteFloat
1000025EightByteFloat
1000039EightByteFloat
10000413EightByteFloat
10000517TwoByteIntUnsigned
完整配置表(保留导入列顺序)
序号寄存器类型起始地址数据类型新请求标志轮询周期点号
1HOLDING1EightByteFloatFALSE2000100001
2HOLDING5EightByteFloatFALSE2000100002
3HOLDING9EightByteFloatFALSE2000100003
4HOLDING13EightByteFloatFALSE2000100004
5HOLDING17TwoByteIntUnsignedFALSE2000100005

本实验映射5个测点,前四个存储的是四个有测量数据的测点的监测结果,数据类型为8字节64位浮点数,最后一个寄存器存储的是不进行监测的测点的值,这个测点只有0和1两个结果,所以数据类型设置为无符号整数。

本实验中的AOE网络图如下图所示:

倒立摆控制AOE网络图

AOE网络声明

设置项值
AOE ID70001
是否启用TRUE
名称倒立摆控制
触发条件Event_Drive
触发条件参数(空)

变量及初值见下方说明表,完整初始化字符串见完整配置表。

将名称定为倒立摆控制,触发条件设置为事件触发(Event_Drive)。

本实验中需要定义的变量初始值比较多。

变量名含义
Ts指Simulink仿真步长,设置为0.02s
Ecar表示小车位置与设定位置的偏差,初始值为0
Pgain比例环节的增益系数,设定值为-9.4
Igain积分环节的增益系数,设定值为0.06
Iuplim & Idownlim积分环节的输出上限和下限,设定值为5和-5
num1 & num0前馈环节传递函数分子(一次项和常数项)的系数,设定值为7.12和0
den1 & den0前馈环节传递函数分母(一次项和常数项)的系数,设定值为1和2
K1 & K2 & K3 & K4LQR控制环节的最优反馈增益矩阵的元素,分别为0、-18、-166.5和-15.2
uplim & downlim作用在小车上的作用力的上下限,分别为10和-10
Pout & Iout比例和积分环节的输出,初始值都为0
Ecarn1前一个仿真步长中小车位置与设定位置的偏差,初始值是0
Ilimout积分环节限幅之后的输出,初始值为0
Feedforwardout前馈环节的输出,初始值为0
cartsetn1前一个仿真步长中,小车位置的设定值,初始值为0
cartn1前一个仿真步长中小车的位置,初始值为0
Pendulumn1前一个仿真步长中,摆杆与垂直方向的夹角,初始值为0
dcart小车的速度,初始值为0
dPendulum摆杆的角速度,初始值为0
LQRoutLQR环节的输出,初始值为0
ctrloutPI反馈环节,前馈环节和LQR环节的总输出,初始值为0

变量声明

本实验中不需要此模块的填写,在此不多赘述。

AOE节点声明

AOE ID 70001;节点类型均为 Condition;超时时间均为 10 ms。

节点ID名称表达式
1simulink通信事件DoCal_POINT>0.5
2执行顺序计算中间节点11
3执行顺序计算中间节点21
4执行顺序计算中间节点31
5执行顺序计算中间节点41
6执行顺序计算中间节点51
7执行顺序计算中间节点61
8结束节点DoCal_POINT==0

本实验中一共有8个节点,第1个节点是simulink通信事件节点,为事件触发类型,当DoCal_POINT>0.5时,也就是测点5输出1,表示允许控制器计算时,该节点被触发,进入运算流程。

中间6个节点都是运算节点,也都是事件触发类型,当每个分步的计算步骤完成时,该节点被触发,并进入下一个计算步骤。

最后一个节点是结束节点,是事件触发类型,当所有的计算步骤都完成,信号传输后,进入结束节点,计算结束,并将DoCal_POINT的数值改为0,等待下一次通信之后,开始计算。尾节点表中超时为10ms,用于确认DoCal_POINT复位;它不是仿真采样时间。ZIP尾节点使用100ms,通信较慢时可采用较长超时。

AOE边声明

AOE ID 70001;失败模式均为 Default;动作类型均为 Set_Points。

以下各行按原表排列,分支由首尾节点确定。

首尾节点动作
1;2aoe触发测点复位、计算小车位置偏差
2;3计算比例和积分环节输出
3;4存储小车位置误差,计算积分环节限幅输出和前馈环节输出
4;5存储前馈环节输入值,计算小车速度、倒立摆角速度
5;6计算LQR控制输出值
6;7存储小车位置和倒立摆角度,计算比例环节、积分限幅环节、前馈控制环节、LQR控制总输出
7;8作用在小车上的力限幅
完整配置表(保留导入列顺序)

网络声明

AOE ID是否启用名称触发条件触发条件参数变量初始值
70001TRUE倒立摆控制Event_DriveTs:0.02; Ecar:0; Ecarn1:0; Pgain:-9.4; Igain:0.06; Iuplim:5; Idownlim:-5; num1:7.12; num0:0; den1:1; den0:2; K1:0; K2:-18; K3:-166.5; K4:-15.2; uplim:10; downlim:-10; Pout:0; Iout:0; Ilimout:0; Feedforwardout:0; cartsetn1:0; cartn1:0; Pendulumn1:0; dcart:0; dPendulum:0; LQRout:0; ctrlout:0

节点声明

AOE ID节点ID名称节点类型超时时间(ms)表达式
700011simulink通信事件Condition10DoCal_POINT>0.5
700012执行顺序计算中间节点1Condition101
700013执行顺序计算中间节点2Condition101
700014执行顺序计算中间节点3Condition101
700015执行顺序计算中间节点4Condition101
700016执行顺序计算中间节点5Condition101
700017执行顺序计算中间节点6Condition101
700018结束节点Condition10DoCal_POINT==0

边声明

AOE ID首尾节点动作名称失败模式动作类型动作参数1(离散量/模型)动作参数2(模拟量/选项)
700011;2aoe触发测点复位、计算小车位置偏差DefaultSet_PointsDoCal_POINT:0;Ecar: Cartset_POINT - Cart_POINT;
700012;3计算比例和积分环节输出DefaultSet_PointsPout: Pgain * Ecar; Iout: Iout + Igain * Ecarn1 * Ts;
700013;4存储小车位置误差,计算积分环节限幅输出和前馈环节输出DefaultSet_PointsEcarn1: Ecar;Ilimout: (Iout <= Iuplim && Iout >= Idownlim) * Iout + (Iout > Iuplim) * Iuplim + (Iout < Idownlim) * Idownlim;Feedforwardout: (num1 * (Cartset_POINT - cartsetn1) + num0 * Ts * cartsetn1 + (den1 - den0 * Ts) * Feedforwardout) / den1;
700014;5存储前馈环节输入值,计算小车速度、倒立摆角速度DefaultSet_Pointscartsetn1: Cartset_POINT;dcart: (Cart_POINT - cartn1) / Ts;dPendulum: (Pendulum_POINT - Pendulumn1)/Ts;
700015;6计算LQR控制输出值DefaultSet_PointsLQRout: K1 * Cart_POINT + K2 * dcart + K3 * Pendulum_POINT + K4 * dPendulum;
700016;7存储小车位置和倒立摆角度,计算比例环节、积分限幅环节、前馈控制环节、LQR控制总输出DefaultSet_Pointscartn1: Cart_POINT;Pendulumn1: Pendulum_POINT;ctrlout: Pout + Ilimout + Feedforwardout - LQRout;
700017;8作用在小车上的力限幅DefaultSet_PointsU_POINT: (ctrlout <= uplim && ctrlout >= downlim) * ctrlout + (ctrlout > uplim) * uplim + (ctrlout < downlim) * downlim;

本实验中的边声明全部都是SetPoints,连接节点的同时还需要进行动作。

下面按每条边说明计算关系。各步引用的中间结果来自已经完成的前驱动作;同一条边中的赋值都读取该动作前的值。

1→2:触发复位与位置偏差。 在参数1写 DoCal_POINT:0;,避免本次结束后因触发位仍为1而再次运行;参数2写 Ecar:Cartset_POINT-Cart_POINT;,得到设定位置与测量位置的偏差。

2→3:比例与积分输出。 Pout:Pgain*Ecar;计算比例分量;Iout:Iout+Igain*Ecarn1*Ts;在前次积分值上累加前一拍误差对应的积分增量。Ecarn1是已保存的前次偏差。

3→4:误差存储、积分限幅及前馈输出。 Ecarn1:Ecar;为下一拍保存误差;Ilimout:(Iout<=Iuplim&&Iout>=Idownlim)*Iout+(Iout>Iuplim)*Iuplim+(Iout<Idownlim)*Idownlim;把积分限制在上下限内;前馈式 Feedforwardout:(num1*(Cartset_POINT-cartsetn1)+num0*Ts*cartsetn1+(den1-den0*Ts)*Feedforwardout)/den1;使用上一步设定值与前馈输出计算本步结果。其离散化推导见下文。

4→5:设定值存储与速度计算。 cartsetn1:Cartset_POINT;保存本步设定值;dcart:(Cart_POINT-cartn1)/Ts;和 dPendulum:(Pendulum_POINT-Pendulumn1)/Ts;分别以位置、摆角的差分估计小车速度和摆杆角速度,Ts必须大于0。

5→6:LQR反馈。 LQRout:K1*Cart_POINT+K2*dcart+K3*Pendulum_POINT+K4*dPendulum;使用最优反馈增益矩阵K和四个状态量生成LQR分量。

6→7:历史状态与总输出。 保存 cartn1:Cart_POINT;Pendulumn1:Pendulum_POINT;,并计算 ctrlout:Pout+Ilimout+Feedforwardout-LQRout;,使比例、积分限幅、前馈和LQR反馈共同参与控制。

7→8:作用力限幅。 U_POINT:(ctrlout<=uplim&&ctrlout>=downlim)*ctrlout+(ctrlout>uplim)*uplim+(ctrlout<downlim)*downlim;在范围内输出计算值,超过上限或低于下限时输出对应限值,再由Modbus返回Simulink的小车输入端。

前馈环节公式推导

前置环节的输出计算公式可以表示为:

y=num1∗s+num0den1∗s+den0x y=\frac{num1*s+num0}{den1*s+den0}x

将分母乘出之后:

num1∗s∗x+num0∗x=den1∗s∗y+den0∗y num1*s*x+num0*x=den1*s*y+den0*y

利用反拉式变换可以得到:

num1∗x˙+num0∗x=den1∗y˙+den0∗y num1*\dot x+num0*x=den1*\dot y+den0*y

利用离散化计算可以得到:

num1∗(xn−xn−1)+num0∗Ts∗xn−1=den1∗(yn−yn−1)+den0∗Ts∗yn−1 num1*\left(x_n-x_{n-1}\right)+num0*T_s*x_{n-1}=den1*\left( y_n-y_{n-1} \right)+den0*T_s*y_{n-1}

将输出表示出来可以得到:

Yn=num1∗(xn−xn−1)+num0∗Ts∗xn−1−den0∗Ts∗yn−1+den1∗yn−1den1 Y_n = \frac{num1*\left(x_n-x_{n-1}\right)+num0*T_s*x_{n-1}-den0*T_s*y_{n-1}+den1*y_{n-1}}{den1}

使用实体控制器时,先连接电源并打开设备电源开关,再用网线连接控制器和计算机。

将matlab的文件夹打开到低代码控制器替代后的Simulink模型文件的文件夹。

当与实体的控制器连接时:就是将所实现的功能放入实体的控制器中,只需要将控制器与电脑通过网线连接后,修改主站和从站的ip地址。

当控制器启动并接入计算机,打开一个浏览器,并输入控制器地址可以得到如下界面。

网页配置界面

主站ip为本机ip地址,通过网络属性配置中可以查询。

登录控制器,在“测点”“通道”“AOE”页分别选择并上传本例对应文件;查看导入结果,回概览点击“重置”。等模型加载与AOE启动完成后,再从MATLAB运行仿真。

配置完成后,进入MATLAB的界面。然后打开控制器的源文件modbusCom.m文件。

m = modbus('tcpip', '127.0.0.1', 502);

以上这个程序段用于联系控制器与MATLAB,在建立modbus通信的代码中,输入从站ip(“127.0.0.1”,当连接实体的控制器时,需要填写查找得到的控制器的ip地址)和端口(502,这个端口是由通信通道配置里选择的)。

在命令行窗口中输入上述命令与控制器进行通信,判断是否成功连接。

运行过程中,需要将S-function name设成.m文件的文件名。

打开simulink里的configuration。

Simulink仿真设置

设置为定步长仿真(Fixed-step),仿真步长为0.02s,数值积分方法确定为一阶欧拉法,配置完成后保存配置。

先运行控制器并确认AOE已启用,再启动Simulink模型,改变小车位置设定,观察小车是否跟随目标移动、摆杆角度是否回到平衡附近。同步查看位置、角度、作用力曲线与AOE动作结果;触发点复位只说明本轮已进入处理,需等最后的限幅动作和输出更新后再取控制结果。

倒立摆动画演示

完成后,点击运行,可以得到动画模型。了解动画的效果可以观看演示视频。

正文采用点号100001–100005、AOE ID70001;ZIP采用100201–100205、AOE ID70003,尾节点超时100ms。四个double输入/输出占16个寄存器,触发点占1个,共17个16位寄存器。一次复现应整套使用同一编号和超时配置。

附件:倒立摆平衡示例.zip