RflySim|定点位置控制器设计实验四

本章涉及的位置控制是定点控制问题，位置控制器的输出为期望姿态，所以位置控制器的设计是以姿态控制器为基础的，整个位置控制实验流程与姿态控制基本一致。本章将详细介绍多旋翼飞行器的位置控制器的设计原理和设计方法，并通过层次分明的三个分步实验，让读者更好地掌握这部分的知识。

01 设计实验

1.建立位置控制通道的传递函数模型

使用MATLAB “ControlSystemDesigner”设计校正控制器,使得加入校正环节后系统速度控制环阶跃响应稳态误差 ，相位裕度>75°截止频率>2.0rad/s。位置控制环截止频率>1rad/s,相位裕度>60° ；

2.使用自己设计的控制器进行软件在环仿真实验和硬件在环仿真实验；

本实验对应demo文件对于RflySim v3.0以下版本地址为：*
PX4PSPRflySimAPIsExp02_FlightControle6-PositionCtrle6.3；

对于RflySim v3.0及以上版本地址为：*
PX4PSPRflySimAPIs5.RflySimFlyCtrl1.BasicExpse6-PositionCtrle6.3。

打开
e6-PositionCtrlPID-Confige6.3tuneInit_control.m文件，点击运行初始化参数，PosControl_tune.slx文件将会自动打开，如下图所示。

速度环分析：输入为期望速度，输出为实际速度。设置输入输出点如下图所示。

点击运行后，进行如下图所示操作生成Bode图

生成 Bode 图后，在左侧“Linear Analysis Workspace”中会出现“linsys1”变量。如下图所示操作，即可得到传递函数模型。

由下图所示得到的传递函数为：

由上述传递函数建立如下图的.m 文件

num=[3331 5.039e05 2.563e07 4.486e08 5.371e08];

den=[1 200 1.567e04 6.045e05 1.189e07 1.154e08 5.557e08 5.371e08 280.1];

G=tf(num,den);Z=[-1.29];

P=[-5.214e-7 -1.253 -33.92];

zpk1=zpk(G)

GG=tf(zpk1)

controlSystemDesigner('bode',G);

运行即可使用 MATLAB 基于 Bode 图的控制系统计，如下图所示。

使用工具箱校正：由上图可知，系统的响应较慢，向上拖动Bode图曲线增大开环增益。增益增大，从阶跃响应曲线上看，响应时间 变短，但是带来了超调。从Bode图上看，相 位裕度为50.1°，相比设计目标偏小。

图. 加大增益对Bode图和阶跃响应的影响

增加一个超前校正环节，提高相位裕度，还能进一步提高截止频率，增加响应速度。在 Bode 图中右键“add Pole/Zero”-“Lead”， 直接拖动零极点观测响应曲线，得到合适的矫正环节，如下图所示。

图. 加入超前环节

在 Bode 图中右键“Edit Conpensator”，如下图所示

看最终的得到的校正器为：

对x通道位置环进行校正：加入Step 3中得到的速度环校正器，如下图所示。

得到位置环的 Bode 图如下图所示， 相位裕度为 75.8o，截止频率 0.99，基本满足要求。可略微增加增益，提高截止频率。（例如取位置环增 益为1.2，重新绘制Bode图，相位裕度为65.3° ， 截止频率为1.12,满足实验指标。）

仿真实验：前述设计的校正环节是时域连续的环节，加入该模型应将其变为时域离散的环节。使用 c2d 函数将s域的传递函数（按下述提示自行输入）变为 z 域传递函数。

H = tf([num], [den])

Hd = c2d(H, Ts, ’foh’)

其中“num”为传递函数分子系数向量，“den”为传递函数分母系数向量，“Ts”为仿真步长，本例 中为 0.004s。

这里替换后的传递函数为：

将 Simulink模型中的 PID 控制器替换成离散模块, （在"
e6-PositionCtrlPID-Confige6.3HILPosControl_HIL.slx"中找到PosControl_HIL/Control
System/Subsystem1/position_control子模块）如下图所示

图 替换原来的 PID 校正器

将"
e6-PositionCtrlPID-Confige6.3HILPosControl_HIL.slx"模型编译完成后烧入飞控，进行硬件在环仿真, 多旋翼可以实现基本的直线飞行和悬停。

参考文献：

[1] 全权,杜光勋,赵峙尧,戴训华,任锦瑞,邓恒译.多旋翼飞行器设计与控制[M],电子工业出版社,2018.

[2] 全权,戴训华,王帅.多旋翼飞行器设计与控制实践[M],电子工业出版社,2020.