CN110134135A - 基于改进mpc-pid的四旋翼飞行器控制方法 - Google Patents
基于改进mpc-pid的四旋翼飞行器控制方法 Download PDFInfo
- Publication number
- CN110134135A CN110134135A CN201910447080.8A CN201910447080A CN110134135A CN 110134135 A CN110134135 A CN 110134135A CN 201910447080 A CN201910447080 A CN 201910447080A CN 110134135 A CN110134135 A CN 110134135A
- Authority
- CN
- China
- Prior art keywords
- channel
- quadrotor
- desired value
- error
- model
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 28
- 230000004044 response Effects 0.000 claims abstract description 20
- 238000013461 design Methods 0.000 claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims description 32
- 238000005530 etching Methods 0.000 claims description 12
- 238000011017 operating method Methods 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 8
- 238000012546 transfer Methods 0.000 claims description 7
- 230000005540 biological transmission Effects 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000004260 weight control Methods 0.000 claims description 3
- 238000005096 rolling process Methods 0.000 claims description 2
- 210000001367 artery Anatomy 0.000 claims 1
- 238000006243 chemical reaction Methods 0.000 claims 1
- 230000005611 electricity Effects 0.000 claims 1
- 210000003462 vein Anatomy 0.000 claims 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 9
- 230000000694 effects Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 230000006872 improvement Effects 0.000 description 2
- 230000033228 biological regulation Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000005489 elastic deformation Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
- G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability
- G05D1/0825—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability using mathematical models
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Algebra (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Feedback Control In General (AREA)
Abstract
本发明提出了一种基于改进MPC‑PID的四旋翼飞行器控制方法，其步骤为：首先，根据四旋翼飞行器结构及力学原理建立四旋翼飞行器的动力学模型，并结合小扰动原理在悬停点对动力学模型进行线性化转化为状态空间形式；其次，设计四旋翼飞行器的控制系统，包括改进MPC控制器和不完全微分PID控制器，并根据不同通道分别设计相应的控制器；最后，利用本发明的控制系统对四旋翼飞行器进行仿真实验，使机体稳定飞行至预先设定的空间位置。本发明根据不同通道采用改进MPC控制的外环控制器和不完全微分PID控制的内环控制器，使得控制器具有更快的响应速度与动态性能，更够更好的应用于实际工程。
Description
技术领域
本发明涉及自动控制技术领域，特别是指一种基于改进MPC-PID的四旋翼飞行器控制方法。
背景技术
四旋翼飞行器具有灵活性高，安全性强，控制方式多样等优点，随着近年来传感器技术和微处理器的不断发展，四旋翼飞行器也越来越广泛的应用到各个行业之中，如航拍摄影、违章拍照、仪器检测、疑犯追踪等。而上述应用场景均对四旋翼飞行器的稳定控制有着基本要求，由于四旋翼飞行器各个通道之间相互耦合，且工作环境易受到阵风干扰，因此四旋翼飞行器的位姿控制是其能否广泛应用的关键一环。
串级PID控制是目前四旋翼飞行器最为常用的控制方法之一，其设计简单、应用场景多且易于实现，但PID控制也有着动态性能较差，易引起超调等等，在对四旋翼飞行器性能要求较高的场景下无法使用。
发明内容
针对现有的四旋翼飞行器的控制方法存在的动态性能较差的技术问题，本发明提出一种基于改进MPC-PID的四旋翼飞行器控制方法，通过外环采用改进模型预测控制方法，内环采用PID控制方法，能够提供一种动态性能好、响应速度快且精度高的控制方法，实现对四旋翼飞行器的稳定控制。
本发明的技术方案是这样实现的：
一种基于改进MPC-PID的四旋翼飞行器控制方法，其步骤如下：
S1、根据四旋翼飞行器结构及力学原理，建立四旋翼飞行器的动力学模型；
S2、结合小扰动原理，在悬停点将四旋翼飞行器的动力学模型转化为线性化模型，并将线性化模型转化为状态空间方程；
S3、将机体的状态空间方程通过矩阵转化得到各个通道的传递函数，通道包括高度z通道、水平x通道、水平y通道、俯仰θ通道、横滚通道和偏航ψ通道；
S4、设计MPC控制器，并通过引入比例函数和加权平均系数对MPC控制器进行改进，得到改进MPC控制器模型，设计PID控制器，将低通滤波器与PID控制器串联得到不完全微分PID控制器；
S5、设置四旋翼飞行器的高度z通道、水平x通道和水平y通道的期望值，初始化偏航ψ通道的期望值和输出值，初始化高度z通道、水平x通道、水平y通道、俯仰θ通道和横滚通道的输出值；
S6、计算高度z通道的期望值与其当前值的误差z1、水平x通道的期望值与其当前值的误差x1、水平y通道的期望值与其当前值的误差y1、偏航ψ通道的期望值与其当前值的误差ψ1；
S7、将步骤S6中得到的误差z1输入改进MPC控制器模型中得到油门输入量t1，将步骤 S6中得到的误差x1输入改进MPC控制器模型中得到俯仰θ通道的期望值，将步骤S6中得到的误差y1输入改进MPC控制器模型中得到横滚通道的期望值，将步骤S6中得到的误差ψ1输入改进MPC控制器中得到油门输入量t4；
S8、计算步骤S7中得到的俯仰θ通道的期望值与其当前值的误差θ1、步骤S7中得到的横滚通道的期望值与其当前值的误差
S9、将步骤S8中得到误差θ1输入不完全微分PID控制器得到油门输入量t2，将步骤S8 中得到的误差输入不完全微分PID控制器得到油门输入量t3；
S10、将油门输入量t1、油门输入量t2、油门输入量t3和油门输入量t4通过矩阵转换得到力矩U1,U2,U3,U4，并将力矩U1,U2,U3,U4输入四旋翼动力学模型中更新步骤S5中的高度z 通道、水平x通道、水平y通道、俯仰θ通道、横滚通道和偏航ψ通道的输出值；
S11、循环执行步骤S6至S10直到各个通道的输出值稳定在各个通道的期望值时结束循环。
优选地，步骤S1中建立的四旋翼飞行器的动力学模型为：
其中，(x,y,z)为四旋翼飞行器空间坐标，为x对时间的导数，为y对时间的导数，为z对时间的导数，u、v、w分别为x方向、y方向和z方向上的线速度，为u对时间的导数，为v对时间的导数，为w对时间的导数，θ、ψ均为四旋翼的姿态角，p为姿态角θ的角速度，q为姿态角的角速度，r为姿态角ψ的角速度，b为电机拉力系数，d为电机阻力系数，Ix、Iy、Iz分别为x方向、y方向和z方向上的扭矩，U1,U2,U3,U4分别为高度z通道、水平x通道、水平y通道和偏航ψ通道上的力矩，ωi为电机转速，为电机加速度，i＝1,2,3,4，Ts为电机时间常数，k为电机增益系数，Vi为电机占空比，g为重力加速度。
优选地，所述步骤S2的线性化模型为：其中，X为状态向量，X包含空间坐标x、y、z，线速度u、v、w，姿态角θ、ψ和角速度p、q、r组成的向量，为X对时间的导数，t为四旋翼飞行器的动力学模型的油门输入量；
对线性化模型进行泰勒展开并保留一次项即可得到四旋翼飞行器的状态空间方程：其中，A、B均为函数矩阵，A＝[ΔXf1,…,ΔXfn]T，B＝[Δtf1,…,Δtfn]T，X＝Xe，t＝te，Xe为平衡点的状态量，te为平衡点的油门输入量，ΔXfn是函数fn关于X一次项微分算子的行向量，Δtfn是函数fn关于t一次项微分算子的行向量，n为函数矩阵的大小。
优选地，所述对状态空间方程进行Laplace变换得到步骤S3中的传递函数模型： G(s)＝(sI-A)-1B，其中，A、B均为函数矩阵，I为单位矩阵。
优选地，所述水平x通道对应的改进MPC控制器模型的设计方法为：
S51、对水平x通道的传递函数模型进行Laplace反变换，得到离散脉冲响应序列形式的脉冲响应模型：
其中，y(k+1)为k+1时刻系统的输出，u(k)为k时刻系统的输入，l1,l2,...,lN为系统脉冲响应序列值，ζ(k+1)为k+1时刻系统的随机干扰，N为脉冲响应序列长度；系统的输入为k时刻的俯仰角θ的期望值，系统的输出为k+1时刻的水平x通道的输出值，系统和MPC 控制器构成反馈回路；
S52、根据步骤S51的脉冲响应模型可得水平x通道对应的MPC控制器模型为：
y1(k+j)＝lj(z-1)u(k+j-1) (8)，
其中，j＝1,2,...,P，P为预测时域；
将MPC控制器模型展开，得到：
其中，M为控制时域，M≤P≤N；
由于控制时域为M，则在经历过k+M-1个时刻后，控制量不再发生变化，因此公式(9)可变为：
Y1(k+1)＝DU(k)+GU(k-1) (10)，
其中，为预测未来P个时刻的系统输出，U(k)为待求P-M+1 个控制量，U(k-1)为N-1个已知控制量，
S53、增加MPC控制器模型输出误差项对MPC控制器模型进行改进：
其中，Y2(k+1)为系统输出预测矢量，e(k)为k时刻预测模型输出误差，y(k)为k时刻系统输出，a＝[a1,a2...ap]T为输出误差矩阵；
S54、选择对控制量加权的二次型性能指标评价MPC控制器模型的性能：
其中，r(k+j)为水平x方向的期望值，qj为输出误差加权系数，rm为控制误差加权系数；
将性能指标转化为矩阵形式：
其中，Q＝diag[q1,q2...qP]为输出加权矩阵，R＝diag[r1,r2...rM]为控制加权矩阵， U(k)待求控制量矩阵；
S55、对性能指标的矩阵形式中的U(k)进行求导，令可得：
U(k)＝(DTQD+R)-1DTQ[R(k+1)-GU(k-1)-ae(k)] (14)，
利用第一个控制量替代M个控制量作为有效值去控制系统，则公式(14)可转化为：
其中，dT＝(1,0,...)(DTQD+R)-1DTQ＝(d1,d2,...,dP)，u(k)为待求控制量；
S56、比例函数为：F(x)＝L[r(k)-y(k)]/r(k)(16)，其中，L为比例系数；
利用比例函数对公式(15)进行改进，得到当前时刻的控制量为：u1(k)＝F(x)u(k)(17)；
S57、引入加权平均系数对MPC控制器进行改进：
其中，ε(m)为加权平均系数，ut(k)为俯仰角θ通道的期望值；
所述油门输入量t1为：将高度z通道的期望值与其输出值的误差z1输入改进MPC控制器，按照步骤S51至S57操作方法得到油门输入量t1；
所述横滚通道的期望值为：将水平y通道的期望值与其输出值的误差y1输入改进MPC 控制器，按照步骤S51至S57操作方法得到横滚通道的期望值；
所述油门输入量t4为：将偏航ψ通道的期望值与其输出值的误差ψ1输入改进MPC控制器，按照步骤S51至S57操作方法得到油门输入量t4。
优选地，所述不完全微分PID控制器的实现方法为：
S61、普通PID控制器微分方程为：
其中，Kp为比例系数、Ti为积分时间常数、Td为微分时间常数，T为采样周期，b(t)为俯仰角θ通道的期望值与当前俯仰角θ的差值，t为采样时间，为b(t)对采样时间t的微分；
S62、将低通滤波器与PID控制器串联得到不完全微分PID控制器：
其中，Tf为滤波系数，θd(t)为油门输入量t2；
所述油门输入量t3为：将横滚通道的期望值与其输出值的误差输入不完全微分PID控制器，按照步骤S61至S62操作方法得到油门输入量t3。
优选地，所述油门输入量和力矩之间的关系为：
其中，U1,U2,U3,U4分别为高度z通道、水平x通道、水平y通道和偏航ψ通道上的力矩， l为电机中心到机体中心的距离，b为电机拉力系数，d为电机阻力系数，ω1,ω2,ω3,ω4均为电机转速，均为电机加速度，k为电机增益系数，Ts为电机时间常数，V1,V2,V3,V4均为电机占空比，t1,t2,t3,t4均为油门输入量。
本技术方案能产生的有益效果：本发明采用了基于传递函数模型的模型预测控制的外环控制器和不完全微分PID控制的内环控制器，并考虑了建模过程中的电机动态响应，可使控制器具有更快的响应速度与动态性能，更快的稳定在期望值，而且能够大大降低了PID控制方法的计算量与控制器设计的复杂程度，便于实际工程中的应用。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明的整体控制框图。
图2为本发明的四旋翼飞行器基准坐标系。
图3为x方向PID控制与本发明控制效果对比图。
图4为y方向PID控制与本发明控制效果对比图。
图5为z方向PID控制与本发明控制效果对比图。
图6为本发明与串级PID控制的俯仰角响应对比图。
图7为本发明与串级PID控制的横滚角响应对比图。
图8为本发明与串级PID控制的偏航角响应对比图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1和图2所示，一种基于改进MPC-PID的四旋翼飞行器控制方法，具体步骤如下：
S1、根据四旋翼飞行器结构及力学原理，建立四旋翼飞行器的动力学模型。
假设四旋翼飞行器为刚体，忽略机体振动以及弹性形变；四旋翼飞行器结构完全对称，飞行器中心与机体中心重合；将北东地坐标系定为惯性坐标系，忽略地球曲率和自转；电机动态响应为一阶惯性环节。则四旋翼飞行器的动力学模型如下：
其中，(x,y,z)为四旋翼飞行器空间坐标，为x对时间的导数，为y对时间的导数，为z对时间的导数，u、v、w分别为x方向、y方向和z方向上的线速度，为u对时间的导数，为v对时间的导数，为w对时间的导数，θ、ψ均为四旋翼的姿态角，p为姿态角θ的角速度，q为姿态角的角速度，r为姿态角ψ的角速度，b为电机拉力系数，d为电机阻力系数，Ix、Iy、Iz分别为x方向、y方向和z方向上的扭矩，U1,U2,U3,U4分别为高度z通道、水平x通道、水平y通道和偏航ψ通道上的力矩，ωi为电机转速，为电机加速度，i＝1,2,3,4，Ts为电机时间常数，k为电机增益系数，Vi为电机占空比，g为重力加速度。
所述力矩为：
所述电机占空比为：
其中，t1,t2,t3,t4均为油门输入量，由公式(1)、公式(2)和公式(3)可将油门输入量和力矩进行相互转化。
S2、结合小扰动原理，在悬停点将四旋翼飞行器的动力学模型转化为线性化模型，并将线性化模型转化为状态空间方程。
线性化模型为：
其中，X为状态向量，X包含空间坐标x、y、z，线速度u、v、w，姿态角θ、ψ和角速度p、q、r组成的向量，为X对时间的导数，t为四旋翼飞行器的动力学模型的油门输入量。
对线性化模型进行泰勒展开并保留一次项建立四旋翼飞行器的状态空间方程：
其中，A、B均为函数矩阵，A＝[ΔXf1,…,ΔXfn]T，B＝[Δtf1,…,Δtfn]T，X＝Xe，t＝te， Xe为平衡点的状态量，te为平衡点的油门输入量，ΔXfn是函数fn关于X一次项微分算子的行向量，Δtfn是函数fn关于t一次项微分算子的行向量，n为函数矩阵的大小。
S3、根据机体的状态空间方程通过矩阵转化得到各个通道的传递函数，通道包括高度z通道、水平x通道、水平y通道、俯仰θ通道、横滚通道和偏航ψ通道。
对状态空间方程进行Laplace变换得到传递函数模型：
G(s)＝(sI-A)-1B (6)，
其中，A、B均为函数矩阵，A＝[ΔXf1,…,ΔXfn]T，B＝[Δtf1,…,Δtfn]T，I为单位矩阵， s为状态量X和油门输入量t的复数形式。
S4、设计MPC控制器，并通过引入比例函数和加权平均系数对MPC控制器进行改进，得到改进MPC控制器模型，设计PID控制器，将低通滤波器与PID控制器串联得到不完全微分PID控制器。
S5、设置四旋翼飞行器的高度z通道、水平x通道和水平y通道的期望值，初始化偏航ψ通道的期望值和输出值，初始化高度z通道、水平x通道、水平y通道、俯仰θ通道和横滚通道的输出值。
S6、计算高度z通道的期望值与其当前值的误差z1、水平x通道的期望值与其当前值的误差x1、水平y通道的期望值与其当前值的误差y1、偏航ψ通道的期望值与其当前值的误差ψ1。
S7、将步骤S6中得到的误差z1输入改进MPC控制器模型中得到油门输入量t1，将步骤 S6中得到的误差x1输入改进MPC控制器模型中得到俯仰θ通道的期望值，将步骤S6中得到的误差y1输入改进MPC控制器模型中得到横滚通道的期望值，将步骤S6中得到的误差ψ1输入改进MPC控制器中得到油门输入量t4。
所述水平x通道通过改进MPC控制器模型得到俯仰θ通道的期望值的实现方法为：
S51、对水平x通道的传递函数模型进行Laplace反变换，得到离散脉冲响应序列形式的脉冲响应模型：
其中，y(k+1)为k+1时刻系统的输出，u(k)为k时刻系统的输入，l1,l2,...,lN为系统脉冲响应序列值，ζ(k+1)为k+1时刻系统的随机干扰，N为脉冲响应序列长度；系统的输入为k时刻的俯仰角θ的期望值，系统的输出为k+1时刻的水平x通道的输出值，系统和MPC 控制器构成反馈回路；
S52、根据步骤S51的脉冲响应模型可得水平x通道对应的MPC控制器模型为：
y1(k+j)＝lj(z-1)u(k+j-1) (8)，
其中，j＝1,2,...,P，P为预测时域；
将MPC控制器模型展开，得到：
其中，M为控制时域，M≤P≤N；
由于控制时域为M，则在经历过k+M-1个时刻后，控制量不再发生变化，因此公式(9)可变为：
Y1(k+1)＝DU(k)+GU(k-1) (10)，
其中，为预测未来P个时刻的系统输出，U(k)为待求P-M+1 个控制量，U(k-1)为N-1个已知控制量，
S53、增加MPC控制器模型输出误差项对MPC控制器模型进行改进：
其中，Y2(k+1)为系统输出预测矢量，e(k)为k时刻预测模型输出误差，y(k)为k时刻系统输出，a＝[a1,a2...ap]T为输出误差矩阵；
S54、选择对控制量加权的二次型性能指标评价MPC控制器模型的性能：
其中，r(k+j)为水平x方向的期望值，qj为输出误差加权系数，rm为控制误差加权系数；
将性能指标转化为矩阵形式：
其中，Q＝diag[q1,q2...qP]为输出加权矩阵，R＝diag[r1,r2...rM]为控制加权矩阵， U(k)待求控制量矩阵；
S55、对性能指标的矩阵形式中的U(k)进行求导，令可得：
U(k)＝(DTQD+R)-1DTQ[R(k+1)-GU(k-1)-ae(k)] (14)，
利用第一个控制量替代M个控制量作为有效值去控制系统，则公式(14)可转化为：
其中，dT＝(1,0,...)(DTQD+R)-1DTQ＝(d1,d2,...,dP)，u(k)为待求控制量；
S56、比例函数为：F(x)＝L[r(k)-y(k)]/r(k)(16)，其中，L为比例系数；
利用比例函数对公式(15)进行改进，得到当前时刻的控制量为：u1(k)＝F(x)u(k)(17)；
S57、引入加权平均系数对MPC控制器进行改进：
其中，ε(m)为加权平均系数，ut(k)为俯仰角θ通道的期望值；
所述油门输入量t1为：将高度z通道的期望值与其输出值的误差z1输入改进MPC控制器模型，按照步骤S51至S57操作方法得到油门输入量t1。
所述横滚通道的期望值为：将水平y通道的期望值与其输出值的误差y1输入改进MPC 控制器模型，按照步骤S51至S57操作方法得到横滚通道的期望值。
所述油门输入量t4为：将偏航ψ通道的期望值与其输出值的误差ψ1输入改进MPC控制器模型，按照步骤S51至S57操作方法得到油门输入量t4。
S8、计算步骤S7中得到的俯仰θ通道的期望值与其输出值的误差θ1、步骤S7中得到的横滚通道的期望值与其输出值的误差
S9、将步骤S8中得到误差θ1输入不完全微分PID控制器得到油门输入量t2，将步骤S8 中得到的误差输入不完全微分PID控制器得到油门输入量t3。
所述俯仰θ通道的期望值与其输出值的误差θ1通过不完全微分PID控制器得到的油门输入量t2的实现方法为：
S61、普通PID控制器微分方程为：
其中，Kp为比例系数、Ti为积分时间常数、Td为微分时间常数，T为采样周期，b(t)为俯仰角θ通道的期望值与当前俯仰角θ的差值，t为采样时间，为b(t)对采样时间t的微分。
S62、将低通滤波器与PID控制器串联得到不完全微分PID控制器：
其中，Tf为滤波系数，θd(t)为油门输入量t2；
所述油门输入量t3为：将横滚通道的期望值与其输出值的误差输入不完全微分PID控制器，按照步骤S61值S62操作方法得到油门输入量t3。
S10、将油门输入量t1、油门输入量t2、油门输入量t3和油门输入量t4通过公式(1)、公式(2)和公式(3)之间的转换关系得到力矩U1,U2,U3,U4，并将力矩U1,U2,U3,U4输入四旋翼动力学模型方程中更新步骤S5中的高度z通道、水平x通道、水平y通道、俯仰θ通道、横滚通道和偏航ψ通道的输出值。
S11、循环执行步骤S6至S10直到各个通道的输出值稳定在各个通道的期望值时结束循环。
将所有通道的MPC-PID控制器设计完成后，在simulink搭建仿真模型，进行控制器参数调试，具体参数如表1所示。设定初始值(x,y,z)＝(0,0,0)，期望值为(x,y,z)＝(10,10,-10)，四旋翼飞行器控制结果如图3至图8所示。图3至图5给出了本发明与串级PID控制的高度z 通道、水平x通道、水平y通道上的控制效果对比图，图6至图8给出了本发明和串级PID 控制的姿态角(俯仰角、横滚角和偏航角)响应图，对比可知：相比串级PID控制方法，本发明控制的俯仰角、横滚角和偏航角的波动较小，幅值较小，本发明能够使四旋翼飞行器更快的稳定在期望值，响应速度快，动态性能较好。
表1动力学模型的参数
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (7)
1.一种基于改进MPC-PID的四旋翼飞行器控制方法，其特征在于，其步骤如下：
S1、根据四旋翼飞行器结构及力学原理，建立四旋翼飞行器的动力学模型；
S2、结合小扰动原理，在悬停点将四旋翼飞行器的动力学模型转化为线性化模型，并将线性化模型转化为状态空间方程；
S3、将机体的状态空间方程通过矩阵转化得到各个通道的传递函数，通道包括高度z通道、水平x通道、水平y通道、俯仰θ通道、横滚通道和偏航ψ通道；
S4、设计MPC控制器，并通过引入比例函数和加权平均系数对MPC控制器进行改进，得到改进MPC控制器模型，设计PID控制器，将低通滤波器与PID控制器串联得到不完全微分PID控制器；
S5、设置四旋翼飞行器的高度z通道、水平x通道和水平y通道的期望值，初始化偏航ψ通道的期望值和输出值，初始化高度z通道、水平x通道、水平y通道、俯仰θ通道和横滚通道的输出值；
S6、计算高度z通道的期望值与其当前值的误差z1、水平x通道的期望值与其当前值的误差x1、水平y通道的期望值与其当前值的误差y1、偏航ψ通道的期望值与其当前值的误差ψ1；
S7、将步骤S6中得到的误差z1输入改进MPC控制器模型中得到油门输入量t1，将步骤S6中得到的误差x1输入改进MPC控制器模型中得到俯仰θ通道的期望值，将步骤S6中得到的误差y1输入改进MPC控制器模型中得到横滚通道的期望值，将步骤S6中得到的误差ψ1输入改进MPC控制器中得到油门输入量t4；
S8、计算步骤S7中得到的俯仰θ通道的期望值与其当前值的误差θ1、步骤S7中得到的横滚通道的期望值与其当前值的误差
S9、将步骤S8中得到误差θ1输入不完全微分PID控制器得到油门输入量t2，将步骤S8中得到的误差输入不完全微分PID控制器得到油门输入量t3；
S10、将油门输入量t1、油门输入量t2、油门输入量t3和油门输入量t4通过矩阵转换得到力矩U1,U2,U3,U4，并将力矩U1,U2,U3,U4输入四旋翼动力学模型中更新步骤S5中的高度z通道、水平x通道、水平y通道、俯仰θ通道、横滚通道和偏航ψ通道的输出值；
S11、循环执行步骤S6至S10直到各个通道的输出值稳定在各个通道的期望值时结束循环。
2.根据权利要求1所述的基于改进MPC-PID的四旋翼飞行器控制方法，其特征在于，步骤S1中建立的四旋翼飞行器的动力学模型为：
其中，(x,y,z)为四旋翼飞行器空间坐标，为x对时间的导数，为y对时间的导数，为z对时间的导数，u、v、w分别为x方向、y方向和z方向上的线速度，为u对时间的导数，为v对时间的导数，为w对时间的导数，θ、ψ均为四旋翼的姿态角，p为姿态角θ的角速度，q为姿态角的角速度，r为姿态角ψ的角速度，b为电机拉力系数，d为电机阻力系数，Ix、Iy、Iz分别为x方向、y方向和z方向上的扭矩，U1,U2,U3,U4分别为高度z通道、水平x通道、水平y通道和偏航ψ通道上的力矩，ωi为电机转速，为电机加速度，i＝1,2,3,4，Ts为电机时间常数，k为电机增益系数，Vi为电机占空比，g为重力加速度。
3.根据权利要求1所述的基于改进MPC-PID的四旋翼飞行器控制方法，其特征在于，所述步骤S2的线性化模型为：其中，X为状态向量，X包含空间坐标x、y、z，线速度u、v、w，姿态角θ、ψ和角速度p、q、r组成的向量，为X对时间的导数，t为四旋翼飞行器的动力学模型的油门输入量；
对线性化模型进行泰勒展开并保留一次项即可得到四旋翼飞行器的状态空间方程：其中，A、B均为函数矩阵，A＝[ΔXf1,…,ΔXfn]T，B＝[Δtf1,…,Δtfn]T，X＝Xe，t＝te，Xe为平衡点的状态量，te为平衡点的油门输入量，ΔXfn是函数fn关于X一次项微分算子的行向量，Δtfn是函数fn关于t一次项微分算子的行向量，n为函数矩阵的大小。
4.根据权利要求1或3所述的基于改进MPC-PID的四旋翼飞行器控制方法，其特征在于，所述对状态空间方程进行Laplace变换得到步骤S3中的传递函数模型：G(s)＝(sI-A)-1B，其中，A、B均为函数矩阵，I为单位矩阵。
5.根据权利要求4所述的基于改进MPC-PID的四旋翼飞行器控制方法，其特征在于，所述水平x通道对应的改进MPC控制器模型的设计方法为：
S51、对水平x通道的传递函数模型进行Laplace反变换，得到离散脉冲响应序列形式的脉冲响应模型：
其中，y(k+1)为k+1时刻系统的输出，u(k)为k时刻系统的输入，l1,l2,...,lN为系统脉冲响应序列值，ζ(k+1)为k+1时刻系统的随机干扰，N为脉冲响应序列长度；系统的输入为k时刻的俯仰角θ的期望值，系统的输出为k+1时刻的水平x通道的输出值，系统和MPC控制器构成反馈回路；
S52、根据步骤S51的脉冲响应模型可得水平x通道对应的MPC控制器模型为：
y1(k+j)＝lj(z-1)u(k+j-1) (8)，
其中，j＝1,2,...,P，P为预测时域；
将MPC控制器模型展开，得到：
其中，M为控制时域，M≤P≤N；
由于控制时域为M，则在经历过k+M-1个时刻后，控制量不再发生变化，因此公式(9)可变为：
Y1(k+1)＝DU(k)+GU(k-1) (10)，
其中，为预测未来P个时刻的系统输出，U(k)为待求P-M+1个控制量，U(k-1)为N-1个已知控制量，
S53、增加MPC控制器模型输出误差项对MPC控制器模型进行改进：
其中，Y2(k+1)为系统输出预测矢量，e(k)为k时刻预测模型输出误差，y(k)为k时刻系统输出，a＝[a1,a2...ap]T为输出误差矩阵；
S54、选择对控制量加权的二次型性能指标评价MPC控制器模型的性能：
其中，r(k+j)为水平x方向的期望值，qj为输出误差加权系数，rm为控制误差加权系数；
将性能指标转化为矩阵形式：
其中，Q＝diag[q1,q2...qP]为输出加权矩阵，R＝diag[r1,r2...rM]为控制加权矩阵，U(k)待求控制量矩阵；
S55、对性能指标的矩阵形式中的U(k)进行求导，令可得：
U(k)＝(DTQD+R)-1DTQ[R(k+1)-GU(k-1)-ae(k)] (14)，
利用第一个控制量替代M个控制量作为有效值去控制系统，则公式(14)可转化为：
其中，dT＝(1,0,...)(DTQD+R)-1DTQ＝(d1,d2,...,dP)，u(k)为待求控制量；
S56、比例函数为：F(x)＝L[r(k)-y(k)]/r(k)(16)，其中，L为比例系数；
利用比例函数对公式(15)进行改进，得到当前时刻的控制量为：u1(k)＝F(x)u(k)(17)；
S57、引入加权平均系数对MPC控制器进行改进：
其中，ε(m)为加权平均系数，ut(k)为俯仰角θ通道的期望值；
所述油门输入量t1为：将高度z通道的期望值与其输出值的误差z1输入改进MPC控制器，按照步骤S51至S57操作方法得到油门输入量t1；
所述横滚通道的期望值为：将水平y通道的期望值与其输出值的误差y1输入改进MPC控制器，按照步骤S51至S57操作方法得到横滚通道的期望值；
所述油门输入量t4为：将偏航ψ通道的期望值与其输出值的误差ψ1输入改进MPC控制器，按照步骤S51至S57操作方法得到油门输入量t4。
6.根据权利要求5所述的基于改进MPC-PID的四旋翼飞行器控制方法，其特征在于，所述不完全微分PID控制器的实现方法为：
S61、普通PID控制器微分方程为：
其中，Kp为比例系数、Ti为积分时间常数、Td为微分时间常数，T为采样周期，b(t)为俯仰角θ通道的期望值与当前俯仰角θ的差值，t为采样时间，为b(t)对采样时间t的微分；
S62、将低通滤波器与PID控制器串联得到不完全微分PID控制器：
其中，Tf为滤波系数，θd(t)为油门输入量t2；
所述油门输入量t3为：将横滚通道的期望值与其输出值的误差输入不完全微分PID控制器，按照步骤S61至S62操作方法得到油门输入量t3。
7.根据权利要求6所述的基于改进MPC-PID的四旋翼飞行器控制方法，其特征在于，所述油门输入量和力矩之间的关系为：
其中，U1,U2,U3,U4分别为高度z通道、水平x通道、水平y通道和偏航ψ通道上的力矩，l为电机中心到机体中心的距离，b为电机拉力系数，d为电机阻力系数，ω1,ω2,ω3,ω4均为电机转速， 均为电机加速度，k为电机增益系数，Ts为电机时间常数，V1,V2,V3,V4均为电机占空比，t1,t2,t3,t4均为油门输入量。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910447080.8A CN110134135B (zh) | 2019-05-27 | 2019-05-27 | 基于改进mpc-pid的四旋翼飞行器控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910447080.8A CN110134135B (zh) | 2019-05-27 | 2019-05-27 | 基于改进mpc-pid的四旋翼飞行器控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110134135A true CN110134135A (zh) | 2019-08-16 |
| CN110134135B CN110134135B (zh) | 2022-03-18 |
Family
ID=67581994
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910447080.8A Active CN110134135B (zh) | 2019-05-27 | 2019-05-27 | 基于改进mpc-pid的四旋翼飞行器控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110134135B (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111506099A (zh) * | 2020-05-28 | 2020-08-07 | 西北工业大学 | 一种无人机高度智能控制系统及方法 |
| CN112904711A (zh) * | 2019-12-02 | 2021-06-04 | 沃科波特有限公司 | 操作致动器系统的方法、致动器系统和飞行器 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080188957A1 (en) * | 2006-10-13 | 2008-08-07 | Cutler Charles R | Adaptive multivariable MPC controller with LP constraints |
| JP2009181392A (ja) * | 2008-01-31 | 2009-08-13 | Omron Corp | モデル予測制御方法およびモデル予測制御装置 |
| CN103995539A (zh) * | 2014-05-15 | 2014-08-20 | 北京航空航天大学 | 一种无人机自主编队评价指标与mpc编队控制方法 |
| CN104317194A (zh) * | 2014-09-23 | 2015-01-28 | 杭州电子科技大学 | 非最小化状态空间模型预测控制优化的温度控制方法 |
| US20150275786A1 (en) * | 2014-03-26 | 2015-10-01 | GM Global Technology Operations LLC | Non-model predictive control to model predictive control transitions |
| CN106896722A (zh) * | 2017-03-29 | 2017-06-27 | 郑州轻工业学院 | 采用状态反馈与神经网络的高超飞行器复合控制方法 |
| CN107065902A (zh) * | 2017-01-18 | 2017-08-18 | 中南大学 | 基于非线性模型的无人机姿态模糊自适应预测控制方法及系统 |
-
2019
- 2019-05-27 CN CN201910447080.8A patent/CN110134135B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20080188957A1 (en) * | 2006-10-13 | 2008-08-07 | Cutler Charles R | Adaptive multivariable MPC controller with LP constraints |
| JP2009181392A (ja) * | 2008-01-31 | 2009-08-13 | Omron Corp | モデル予測制御方法およびモデル予測制御装置 |
| US20150275786A1 (en) * | 2014-03-26 | 2015-10-01 | GM Global Technology Operations LLC | Non-model predictive control to model predictive control transitions |
| CN103995539A (zh) * | 2014-05-15 | 2014-08-20 | 北京航空航天大学 | 一种无人机自主编队评价指标与mpc编队控制方法 |
| CN104317194A (zh) * | 2014-09-23 | 2015-01-28 | 杭州电子科技大学 | 非最小化状态空间模型预测控制优化的温度控制方法 |
| CN107065902A (zh) * | 2017-01-18 | 2017-08-18 | 中南大学 | 基于非线性模型的无人机姿态模糊自适应预测控制方法及系统 |
| CN106896722A (zh) * | 2017-03-29 | 2017-06-27 | 郑州轻工业学院 | 采用状态反馈与神经网络的高超飞行器复合控制方法 |
Non-Patent Citations (3)
| Title |
|---|
| HAMID SAEED KHAN等: "Attitude and altitude control of quadrotor by discrete PID control and non-linear model predictive control", 《2015 INTERNATIONAL CONFERENCE ON INFORMATION AND COMMUNICATION TECHNOLOGIES (ICICT)》 * |
| IMAN SADEGHZADEH等: "Payload Drop Application Using an Unmanned Quadrotor Helicopter Based on Gain-Scheduled PID and Model Predictive Control", 《UNMANNED SYSTEMS》 * |
| 邢小军等: "关于四旋翼无人机姿态优化控制仿真研究", 《计算机仿真》 * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112904711A (zh) * | 2019-12-02 | 2021-06-04 | 沃科波特有限公司 | 操作致动器系统的方法、致动器系统和飞行器 |
| US11794918B2 (en) | 2019-12-02 | 2023-10-24 | Volocopter Gmbh | Method of operating an actuator system, actuator system, and aircraft comprising said method and system |
| CN112904711B (zh) * | 2019-12-02 | 2024-02-02 | 沃科波特有限公司 | 操作致动器系统的方法、致动器系统和飞行器 |
| CN111506099A (zh) * | 2020-05-28 | 2020-08-07 | 西北工业大学 | 一种无人机高度智能控制系统及方法 |
| CN111506099B (zh) * | 2020-05-28 | 2023-03-14 | 西北工业大学 | 一种无人机高度智能控制系统及方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110134135B (zh) | 2022-03-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Sydney et al. | Dynamic control of autonomous quadrotor flight in an estimated wind field | |
| CN110806756B (zh) | 基于ddpg的无人机自主引导控制方法 | |
| Szafranski et al. | Different approaches of PID control UAV type quadrotor | |
| Raza et al. | Intelligent flight control of an autonomous quadrotor | |
| CN109445456A (zh) | 一种多无人机集群导航方法 | |
| CN106527122B (zh) | 平流层飞艇定高飞行非线性pid控制方法 | |
| CN108536014A (zh) | 一种考虑飞轮动态特性的航天器姿态规避的模型预测控制方法 | |
| CN110134135A (zh) | 基于改进mpc-pid的四旋翼飞行器控制方法 | |
| CN113985740B (zh) | 一种基于粒子自抗扰的稳定控制方法及装置 | |
| CN107807521A (zh) | 面向固定时间的编队飞行器协同控制方法及系统 | |
| Jiao et al. | Analysis and design the controller for quadrotors based on PID control method | |
| Gao et al. | Dubins path‐based dynamic soaring trajectory planning and tracking control in a gradient wind field | |
| CN115639830A (zh) | 一种空地智能体协同编队控制系统及其编队控制方法 | |
| CN114442640A (zh) | 一种水面无人艇轨迹跟踪控制方法 | |
| Azid et al. | Wind gust estimation for precise quasi-hovering control of quadrotor aircraft | |
| Yamasaki et al. | Integrated guidance and autopilot for a path-following UAV via high-order sliding modes | |
| Kong et al. | Experimental validation of a robust prescribed performance nonlinear controller for an unmanned aerial vehicle with unknown mass | |
| CN103235515A (zh) | 一种利用零运动避免单框架控制力矩陀螺群框架轴转速死区的方法 | |
| CN109308074A (zh) | 一种无人机重心偏移的补偿方法及系统 | |
| US10909875B2 (en) | Method and system for determining a recirculation effect from an obstacle on a main rotor induced velocity of a simulated rotorcraft | |
| Ghouri et al. | Attitude control of quad-copter using deterministic policy gradient algorithms (DPGA) | |
| Rontsis et al. | Improved path following for kites with input delay compensation | |
| CN108227723A (zh) | 一种水下机器人及其稳定性分析及结构优化的应用方法 | |
| Rossouw et al. | An open-source autopilot and source localisation for bio-inspired miniature blimps | |
| CN113821054B (zh) | 基于鸽群智能优化动态逆控制的无人机轨迹跟踪制导方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |