基于改进MPC-PID的四旋翼飞行器控制方法与流程

本发明涉及自动控制技术领域，特别是指一种基于改进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)-¹b，其中，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)＝(d^tqd+r)^-1d^tq[r(k+1)-gu(k-1)-ae(k)](14)，

利用第一个控制量替代m个控制量作为有效值去控制系统，则公式(14)可转化为：

其中，d^t＝(1,0,...)(d^tqd+r)^-1d^tq＝(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)＝(d^tqd+r)^-1d^tq[r(k+1)-gu(k-1)-ae(k)](14)，

利用第一个控制量替代m个控制量作为有效值去控制系统，则公式(14)可转化为：

其中，d^t＝(1,0,...)(d^tqd+r)^-1d^tq＝(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动力学模型的参数

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。