倒立摆控制
6.2 倒立摆控制
Section titled “6.2 倒立摆控制”利用低代码控制器取代原本实验模型中包括前馈控制,PI反馈控制以及LQR控制算法在内的整个控制部分,并达到控制小车移动,摆杆不倒的控制目标。
该程序可在以下环境运行:
-
Matlab R2019a + Simulink v9.3及以上版本;
-
Matlab APP:Instrument Control Toolbox;
-
低代码控制器。
原始模型图
倒立摆控制案例的原始模型来自于Matlab官方提供的Simulink例程中的Inverted Pendulum with Animation模型
可以在matlab命令行窗口中输入以下命令打开这一案例:
openExample('simulink_general/penddemoExample')原始仿真模型如下图所示:

改造模型图

图中的modbusCom就是本实验模型中的控制器模块,实验模型中可以看出,控制器取代了包括PI反馈控制,前馈控制和LQR控制环节在内的所有控制环节,模型十分简洁。
实验模型中的通信文件
Section titled “实验模型中的通信文件”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。
| 点号 | 别名 | 是否离散 | 用途 |
|---|---|---|---|
100001 | Cartset_POINT | FALSE | 设定的小车位置 |
100002 | Cart_POINT | FALSE | 测量的小车位置 |
100003 | Pendulum_POINT | FALSE | 测量的倒立摆与竖直方向夹角 |
100004 | U_POINT | FALSE | 控制器下发的遥调->设定作用在小车上的力 |
100005 | DoCal_POINT | TRUE | 根据测点状态判断控制器是否需要进行计算 |
完整配置表(保留导入列顺序)
| 序号 | 点号 | 名称 | 别名 | 是否离散 | 是否计算点 | 默认值 | 备注 |
|---|---|---|---|---|---|---|---|
| 1 | 100001 | 测点1 | Cartset_POINT | FALSE | FALSE | 0 | 设定的小车位置 |
| 2 | 100002 | 测点2 | Cart_POINT | FALSE | FALSE | 0 | 测量的小车位置 |
| 3 | 100003 | 测点3 | Pendulum_POINT | FALSE | FALSE | 0 | 测量的倒立摆与竖直方向夹角 |
| 4 | 100004 | 测点4 | U_POINT | FALSE | FALSE | 0 | 控制器下发的遥调->设定作用在小车上的力 |
| 5 | 100005 | 测点5 | DoCal_POINT | TRUE | FALSE | 0 | 根据测点状态判断控制器是否需要进行计算 |


本程序需要五个测点:设定的小车位置、测量的小车位置、测量的摆杆角度、控制器输出作用力,以及DoCal_POINT输入就绪信号。其中的别名对应的就是AOE文件之中设置节点事件的表达式的参量。表示在matlab的Simulink模型中是如图所示的几个参数。
第五个测点DoCal_POINT不是实际物理测点,并不是用于获取控制信号的,每通信一次,这个测点将被设成1让控制器进行计算,控制器开始执行AOE控制策略的时候这个测点会被置0。
通信通道配置
Section titled “通信通道配置”通道参数
| 参数 | 设置 |
|---|---|
| 通道名称 | server测试通道 |
| 连接个数 | 1 |
| 服务端口 | 502 |
连接参数
| 参数 | 设置 |
|---|---|
| 连接名称 | 测试通道 |
| 测点个数 | 5 |
| 客户端IP | 127.0.0.1 |
| 客户端端口 | 9999 |
| slave id | 1 |
| 通信协议 | XA |
| 一次读寄存器数上限 | 125 |
| 一次读开关数上限 | 2000 |
服务端监听502,映射5个测点。客户端IP填MATLAB所在计算机的地址;同机实验可用127.0.0.1。客户端端口9999是任意源端口的匹配标记,不要求MATLAB绑定9999。
共同字段:寄存器类型均为 HOLDING;新请求标志均为 FALSE;轮询周期均为 2000。
| 点号 | 起始地址 | 数据类型 |
|---|---|---|
100001 | 1 | EightByteFloat |
100002 | 5 | EightByteFloat |
100003 | 9 | EightByteFloat |
100004 | 13 | EightByteFloat |
100005 | 17 | TwoByteIntUnsigned |
完整配置表(保留导入列顺序)
| 序号 | 寄存器类型 | 起始地址 | 数据类型 | 新请求标志 | 轮询周期 | 点号 |
|---|---|---|---|---|---|---|
| 1 | HOLDING | 1 | EightByteFloat | FALSE | 2000 | 100001 |
| 2 | HOLDING | 5 | EightByteFloat | FALSE | 2000 | 100002 |
| 3 | HOLDING | 9 | EightByteFloat | FALSE | 2000 | 100003 |
| 4 | HOLDING | 13 | EightByteFloat | FALSE | 2000 | 100004 |
| 5 | HOLDING | 17 | TwoByteIntUnsigned | FALSE | 2000 | 100005 |
本实验映射5个测点,前四个存储的是四个有测量数据的测点的监测结果,数据类型为8字节64位浮点数,最后一个寄存器存储的是不进行监测的测点的值,这个测点只有0和1两个结果,所以数据类型设置为无符号整数。
本实验中的AOE网络图如下图所示:

AOE网络声明
| 设置项 | 值 |
|---|---|
| AOE ID | 70001 |
| 是否启用 | 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 & K4 | LQR控制环节的最优反馈增益矩阵的元素,分别为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 |
| LQRout | LQR环节的输出,初始值为0 |
| ctrlout | PI反馈环节,前馈环节和LQR环节的总输出,初始值为0 |
变量声明
本实验中不需要此模块的填写,在此不多赘述。
AOE节点声明
AOE ID 70001;节点类型均为 Condition;超时时间均为 10 ms。
| 节点ID | 名称 | 表达式 |
|---|---|---|
1 | simulink通信事件 | DoCal_POINT>0.5 |
2 | 执行顺序计算中间节点1 | 1 |
3 | 执行顺序计算中间节点2 | 1 |
4 | 执行顺序计算中间节点3 | 1 |
5 | 执行顺序计算中间节点4 | 1 |
6 | 执行顺序计算中间节点5 | 1 |
7 | 执行顺序计算中间节点6 | 1 |
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;2 | aoe触发测点复位、计算小车位置偏差 |
2;3 | 计算比例和积分环节输出 |
3;4 | 存储小车位置误差,计算积分环节限幅输出和前馈环节输出 |
4;5 | 存储前馈环节输入值,计算小车速度、倒立摆角速度 |
5;6 | 计算LQR控制输出值 |
6;7 | 存储小车位置和倒立摆角度,计算比例环节、积分限幅环节、前馈控制环节、LQR控制总输出 |
7;8 | 作用在小车上的力限幅 |
完整配置表(保留导入列顺序)
网络声明
| AOE ID | 是否启用 | 名称 | 触发条件 | 触发条件参数 | 变量初始值 |
|---|---|---|---|---|---|
| 70001 | TRUE | 倒立摆控制 | Event_Drive | Ts: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) | 表达式 |
|---|---|---|---|---|---|
| 70001 | 1 | simulink通信事件 | Condition | 10 | DoCal_POINT>0.5 |
| 70001 | 2 | 执行顺序计算中间节点1 | Condition | 10 | 1 |
| 70001 | 3 | 执行顺序计算中间节点2 | Condition | 10 | 1 |
| 70001 | 4 | 执行顺序计算中间节点3 | Condition | 10 | 1 |
| 70001 | 5 | 执行顺序计算中间节点4 | Condition | 10 | 1 |
| 70001 | 6 | 执行顺序计算中间节点5 | Condition | 10 | 1 |
| 70001 | 7 | 执行顺序计算中间节点6 | Condition | 10 | 1 |
| 70001 | 8 | 结束节点 | Condition | 10 | DoCal_POINT==0 |
边声明
| AOE ID | 首尾节点 | 动作名称 | 失败模式 | 动作类型 | 动作参数1(离散量/模型) | 动作参数2(模拟量/选项) |
|---|---|---|---|---|---|---|
| 70001 | 1;2 | aoe触发测点复位、计算小车位置偏差 | Default | Set_Points | DoCal_POINT:0; | Ecar: Cartset_POINT - Cart_POINT; |
| 70001 | 2;3 | 计算比例和积分环节输出 | Default | Set_Points | Pout: Pgain * Ecar; Iout: Iout + Igain * Ecarn1 * Ts; | |
| 70001 | 3;4 | 存储小车位置误差,计算积分环节限幅输出和前馈环节输出 | Default | Set_Points | 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; | |
| 70001 | 4;5 | 存储前馈环节输入值,计算小车速度、倒立摆角速度 | Default | Set_Points | cartsetn1: Cartset_POINT;dcart: (Cart_POINT - cartn1) / Ts;dPendulum: (Pendulum_POINT - Pendulumn1)/Ts; | |
| 70001 | 5;6 | 计算LQR控制输出值 | Default | Set_Points | LQRout: K1 * Cart_POINT + K2 * dcart + K3 * Pendulum_POINT + K4 * dPendulum; | |
| 70001 | 6;7 | 存储小车位置和倒立摆角度,计算比例环节、积分限幅环节、前馈控制环节、LQR控制总输出 | Default | Set_Points | cartn1: Cart_POINT;Pendulumn1: Pendulum_POINT;ctrlout: Pout + Ilimout + Feedforwardout - LQRout; | |
| 70001 | 7;8 | 作用在小车上的力限幅 | Default | Set_Points | U_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的小车输入端。
前馈环节公式推导
前置环节的输出计算公式可以表示为:
将分母乘出之后:
利用反拉式变换可以得到:
利用离散化计算可以得到:
将输出表示出来可以得到:
连接低代码控制器
Section titled “连接低代码控制器”使用实体控制器时,先连接电源并打开设备电源开关,再用网线连接控制器和计算机。
将matlab的文件夹打开到低代码控制器替代后的Simulink模型文件的文件夹。
当与实体的控制器连接时:就是将所实现的功能放入实体的控制器中,只需要将控制器与电脑通过网线连接后,修改主站和从站的ip地址。
当控制器启动并接入计算机,打开一个浏览器,并输入控制器地址可以得到如下界面。

主站ip为本机ip地址,通过网络属性配置中可以查询。
配置低代码控制器的文件
Section titled “配置低代码控制器的文件”登录控制器,在“测点”“通道”“AOE”页分别选择并上传本例对应文件;查看导入结果,回概览点击“重置”。等模型加载与AOE启动完成后,再从MATLAB运行仿真。
MATLAB界面操作
Section titled “MATLAB界面操作”配置完成后,进入MATLAB的界面。然后打开控制器的源文件modbusCom.m文件。
m = modbus('tcpip', '127.0.0.1', 502);以上这个程序段用于联系控制器与MATLAB,在建立modbus通信的代码中,输入从站ip(“127.0.0.1”,当连接实体的控制器时,需要填写查找得到的控制器的ip地址)和端口(502,这个端口是由通信通道配置里选择的)。
在命令行窗口中输入上述命令与控制器进行通信,判断是否成功连接。
Simulink模型参数设置
Section titled “Simulink模型参数设置”运行过程中,需要将S-function name设成.m文件的文件名。
打开simulink里的configuration。

设置为定步长仿真(Fixed-step),仿真步长为0.02s,数值积分方法确定为一阶欧拉法,配置完成后保存配置。
先运行控制器并确认AOE已启用,再启动Simulink模型,改变小车位置设定,观察小车是否跟随目标移动、摆杆角度是否回到平衡附近。同步查看位置、角度、作用力曲线与AOE动作结果;触发点复位只说明本轮已进入处理,需等最后的限幅动作和输出更新后再取控制结果。

完成后,点击运行,可以得到动画模型。了解动画的效果可以观看演示视频。
正文采用点号100001–100005、AOE ID70001;ZIP采用100201–100205、AOE ID70003,尾节点超时100ms。四个double输入/输出占16个寄存器,触发点占1个,共17个16位寄存器。一次复现应整套使用同一编号和超时配置。
附件:倒立摆平衡示例.zip