CN106933104A - 一种基于dic‑pid的四旋翼飞行器姿态与位置的混合控制方法 - Google Patents

Abstract

本发明公开了一种基于DIC‑PID的四旋翼飞行器姿态与位置的混合控制方法，包括以下步骤：步骤1：将四旋翼飞行器当作单刚体，通过牛顿‑欧拉方程获得其非线性动力学模型，确立***的输入输出关系；步骤2：基于DIC动态逆控制理论针对四旋翼飞行器的内回路进行姿态控制，形成内环姿态控制器；步骤3：基于PID理论针对四旋翼飞行器的外回路进行位置控制，形成外环位置控制器，实现姿态与位置通道的解耦控制；步骤4：在仿真环境下，通过阶跃响应和跟踪上升螺旋线检验所提出的控制方法。本发明所述混合控制方法对外界扰动与未建模动态有较好的抑制能力，能够满足四旋翼飞行器姿态控制与位置控制的需求。

Description

一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制 方法

技术领域

本发明涉及四旋翼飞行器姿态与位置的解耦控制领域，特别涉及一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法。

背景技术

随着航空技术、传感器技术及通信技术的发展，无人机因其巨大的应用前景而受到人们的广泛关注。在所有类型的无人机中，四旋翼飞行器机械结构简单，易于拆装，具有垂直起降、定点悬停的功能，因此它也是国内外学者热衷研究的对象。然而，四旋翼是一个非线性、强耦合的欠驱动***，其动力学方程并不满足叠加性与齐次性，故要实现其高精度的姿态与位置控制难度颇大。

针对上述问题，国内外的学者们对四旋翼飞行器的控制策略进行了一系列相关研究，包括反步法控制、自抗扰控制、自适应滑模控制等。近年来的研究表明基于动态逆控制(Dynamic inversion control，简称DIC)来设计四旋翼的控制器是一种有效途径。DIC的基本原理是通过构造***的全局状态反馈来消除***的非线性，使原***转变为新的线性***。例如，利用DIC理论分别设计了四旋翼的内外回路控制，实现了姿态与位置的解耦控制。

但是，这种单纯的DIC控制策略会因外界扰动与建模不确定性而导致控制效果不佳。因此，考虑多种控制策略结合的方法受到学者们的青睐。例如：

将DIC理论与定量反馈理论相机和设计了四旋翼姿态回路的鲁棒控制器，并在气动参数摄动的情况下验证了控制器的有效性。

在***回路中引入模型参考自适应与DIC结合的方法设计了PX4四旋翼的姿态与位置控制律，仿真结果表明该控制器能较快适应外界扰动与负载变化。

发明内容

为了克服现有技术中的不足，本发明提供一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法，该控制方法对外界扰动与未建模动态有较好的抑制能力，能够满足四旋翼飞行器姿态控制与位置控制的需求。

为了达到上述发明目的，解决其技术问题所采用的技术方案如下：

一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法，包括以下步骤：

步骤1：将四旋翼飞行器当作单刚体，通过牛顿-欧拉方程获得其非线性动力学模型，确立***的输入输出关系；

步骤2：基于DIC动态逆控制理论针对四旋翼飞行器的内回路进行姿态控制，形成内环姿态控制器；

步骤3：基于PID理论针对四旋翼飞行器的外回路进行位置控制，形成外环位置控制器，实现姿态与位置通道的解耦控制；

步骤4：在仿真环境下，通过阶跃响应和跟踪上升螺旋线检验所提出的控制方法。

进一步的，在步骤1中具体包括：

在机体坐标系O_BX_BY_BZ_B下，四旋翼飞行器产生的升力F＝[F₁ F₂ F₃ F₄]^T正向O_BZ_B方向；

当F₁＝F₂＝F₃＝F₄，飞行器做垂直起降或悬停运动；

当F₂＝F₄且F₁≠F₃，飞行器做俯仰运动；

当F₁＝F₃且F≠F₄，飞行器做横滚运动；

当F₁＝F₃≠F₂＝F₄时，飞行器做偏航运动；

基于上述驱动原理，可得到四旋翼控制输入的表达形式：

其中，L为旋翼中心线各桨叶法向线的距离，U＝[U₁,U₂,U₃,U₄]^T为俯仰、横滚、偏航与总距输入信号，M＝[M₁,M₂,M₃,M₄]^T为各旋翼的力矩；

根据牛顿-欧拉方程，可以推导出四旋翼飞行器的动力学方程：

其中，m为四旋翼总质量，为大地坐标系O_IX_IY_IZ_I到机体坐标系的转换矩阵，g为重力加速度，F_D为空气阻力，I＝[I_xx I_yy I_zz]^T为转动惯量，q＝[φ θ ψ]^T为姿态矩阵，D为姿态角到角速度的映射关系，S(·)为旋量矩阵，φ、θ和ψ分别为横滚角、俯仰角和偏航角。

进一步的，若不考虑空气阻力的影响，在低速飞行或悬停状态下，有φ≈0、θ≈0，另外，由四旋翼结构对称性可得I_xx≈I_yy，同时，由于不需要频繁控制偏航角，有因此，公式(2)可近似为：

进一步的，在步骤2中具体包括：

设定非线性***为：

为了跟踪期望轨迹r(t)，定义跟踪误差为：

e(t)＝r(t)-y(t) (5)

若对式(4)中***输出进行微分，则有：

当G(x)可逆时，定义动态逆控制器为：

其中，v为逆动态模型的控制量，将公式(6)代入公式(5)中，可得误差动态***为：

利用线性控制技术配置v使***极点位于原点处，令：

v＝Ke (9)

其中，K为整定矩阵，

因此，***完整的动态逆控制器为：

进一步的，在步骤2中，内回路的被控量为三轴姿态角，

内回路的输出向量为y_1c＝[φ θ ψ]^T，

逆动态控制量为：

因此，内回路的控制律为：

进一步的，在步骤3中，外回路的被控量为三轴位置与线速度，公式(3)给出了四旋翼相对于大地坐标系的线加速度，位置变量y_1w＝[x y z]^T可通过期望输入信号与姿态角获得，外回路中的线加速度为：

采用PID控制技术可得到线加速度为：

其中，K_P、K_I和K_D分别为比例系数、积分系数与微分系数；e_w为位置误差；e_v为线速度误差。

本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：

(1)本发明采用DIC-PID控制策略设计了四旋翼飞行器姿态与位置的控制器，实现了其在模型不确定和外界扰动下的鲁棒控制。

(2)相比于基于PID的四旋翼飞行器控制***，本发明提出的控制***控制效果更好，鲁棒性更强，稳定性更优，能够解决四旋翼易受未建模动态与外界扰动影响的问题。

(3)实现了四旋翼飞行器姿态与位置的解耦控制，仿真测试与飞行实验验证了本发明所设计控制策略的有效性。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。附图中：

图1是本发明一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法的流程示意图；

图2是本发明中四旋翼飞行器受力图；

图3是本发明中四旋翼飞行器的控制***图；

图4是本发明中四旋翼飞行器的内回路控制***图；

图5是本发明中四旋翼飞行器在阵风扰动下的位置控制轨迹图；

图6是本发明中四旋翼飞行器的三维跟踪轨迹图；

图7是本发明中四旋翼飞行器姿态角的变化示意图；

图8是本发明中四旋翼飞行器实验平台飞行试验效果图。

具体实施方式

以下将结合本发明的附图，对本发明实施例中的技术方案进行清楚、完整的描述和讨论，显然，这里所描述的仅仅是本发明的一部分实例，并不是全部的实例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。

如图1所示，本发明公开了一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法，包括以下步骤：

步骤1：将四旋翼飞行器当作单刚体，通过牛顿-欧拉方程获得其非线性动力学模型，确立***的输入输出关系；

步骤2：基于DIC动态逆控制理论针对四旋翼飞行器的内回路进行姿态控制，形成内环姿态控制器；

步骤3：基于PID理论针对四旋翼飞行器的外回路进行位置控制，形成外环位置控制器，实现姿态与位置通道的解耦控制；

步骤4：在仿真环境下，通过阶跃响应和跟踪上升螺旋线检验所提出的控制方法。

国际上通常将四旋翼飞行器当作六自由度刚体建模。如图2所示，其在沿着自身法向升力的作用下在三维空间内***，则在步骤1中具体包括：

在机体坐标系O_BX_BY_BZ_B下，四旋翼飞行器产生的升力F＝[F₁ F₂ F₃ F₄]^T正向O_BZ_B方向；

当F₁＝F₂＝F₃＝F₄，飞行器做垂直起降或悬停运动；

当F₂＝F₄且F₁≠F₃，飞行器做俯仰运动；

当F₁＝F₃且F≠F₄，飞行器做横滚运动；

当F₁＝F₃≠F₂＝F₄时，飞行器做偏航运动；

基于上述驱动原理，可得到四旋翼控制输入的表达形式：

其中，L为旋翼中心线各桨叶法向线的距离，U＝[U₁,U₂,U₃,U₄]^T为俯仰、横滚、偏航与总距输入信号，M＝[M₁,M₂,M₃,M₄]^T为各旋翼的力矩；

根据牛顿-欧拉方程，可以推导出四旋翼飞行器的动力学方程：

其中，m为四旋翼总质量，为大地坐标系O_IX_IY_IZ_I到机体坐标系的转换矩阵，g为重力加速度，F_D为空气阻力，I＝[I_xx I_yy I_zz]^T为转动惯量，q＝[φ θ ψ]^T为姿态矩阵，D为姿态角到角速度的映射关系，S(·)为旋量矩阵，φ、θ和ψ分别为横滚角、俯仰角和偏航角。

进一步的，若不考虑空气阻力的影响，在低速飞行或悬停状态下，有φ≈0、θ≈0，另外，由四旋翼结构对称性可得I_xx≈I_yy，同时，由于不需要频繁控制偏航角，有因此，公式(2)可近似为：

进一步的，在步骤2中具体包括：

设定非线性***为：

为了跟踪期望轨迹r(t)，定义跟踪误差为：

e(t)＝r(t)-y(t) (5)

若对式(4)中***输出进行微分，则有：

当G(x)可逆时，定义动态逆控制器为：

其中，v为逆动态模型的控制量，将公式(6)代入公式(5)中，可得误差动态***为：

利用线性控制技术配置v使***极点位于原点处，令：

v＝Ke (9)

其中，K为整定矩阵，

因此，***完整的动态逆控制器为：

根据时间尺度分离原理，由内向外依次设计四旋翼飞行器的控制器，如图3所示。其中，内回路进行姿态控制，外回路进行位置控制。内回路的设计是为了镇定四旋翼的姿态，被控量为三轴姿态角。如图4所示，在步骤2中，内回路的被控量为三轴姿态角，

内回路的输出向量为y_1c＝[φ θ ψ]^T，

逆动态控制量为：

因此，内回路的控制律为：

进一步的，外回路的设计是为了镇定四旋翼的位置，在步骤3中，外回路的被控量为三轴位置与线速度，公式(3)给出了四旋翼相对于大地坐标系的线加速度，位置变量y_1w＝[x y z]^T可通过期望输入信号与姿态角获得，外回路中的线加速度为：

采用PID控制技术可得到线加速度为：

其中，K_P、K_I和K_D分别为比例系数、积分系数与微分系数；e_w为位置误差；e_v为线速度误差。

为了验证所提混合控制方法的有效性，在Matlab2012b的仿真环境下对四旋翼飞行器的控制性能进行验证，仿真中所选四旋翼飞行器的参数如表1所示：

参数	L(m)	m(kg)	Ixx(kg.m2)	Iyy(kg.m2)	Izz(kg.m2)
						数值	0.250	0.612	0.0045	0.0045	0.0053

表1四旋翼飞行器的物理参数

Case 1：给定参考阶跃信号为(1.5m,1m,0.5m)，分别采用本发明提出的混合控制方法与现有技术中的PID控制策略去跟踪此信号。为了更好地模拟实际飞行，在输入端引入幅值为0.05米的阵风扰动信号，仿真结果如图5所示。从图中可以看出，两种控制器均呈现出大的超调量。相对而言，DIC-PID控制器的响应速度明显要快于PID控制器。同时，DIC-PID的稳定时间小于5s，且稳态误差不大于2％。这说明在阵风扰动的情况下，本文所提控制算法依然可以提供相对稳定的响应。

Case 2：给定三维参考轨迹为一个上升螺旋线，轨迹方程如下所示：

为验证DIC-PID的抗干扰能力，在仿真中引入一个0.1m/s2的外界干扰，整个仿真持续30s，结果如图6所示。从图中可以看出，在初始阶段，参考轨迹与仿真轨迹存在较大的误差，这里需指明该误差是由于四旋翼跟踪偏差所导致。但整体上讲，DIC-PID控制器响应速度很快，能够快速精确地跟踪上参考轨迹。另外，图7给出了四旋翼飞行器姿态角的变化情况，4.3s时内环便趋于稳定。同时，图7也给出了四个通道的控制信号。

飞行实验建立在上一节测试环境的基础上，实验中的四旋翼飞行器经过改装，装载有GPS、飞控、减振板、数传等。

设定四旋翼沿这样的轨迹飞行：起始点-A-B-C-D-E-着陆点。先在仿真环境下确定DIC-PID控制器参数，再通过Matlabde的代码转化功能将控制算法转换成C代码写入到飞控中进行试验验证。这里需指出，由于实验室缺少测量噪声干扰的设备，故在仿真中添加不同的噪声信号来模拟实际扰动，最后选择一组最优或次优的控制器参数。

图8给出了飞行实验的结果，可以看出根据本发明设计的控制器四旋翼飞行器基本能够跟踪上期望轨迹，可实现对四旋翼姿态与位置进行有效控制。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (6)

1.一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法，其特征在于，包括以下步骤：

步骤1：将四旋翼飞行器当作单刚体，通过牛顿-欧拉方程获得其非线性动力学模型，确立***的输入输出关系；

步骤2：基于DIC动态逆控制理论针对四旋翼飞行器的内回路进行姿态控制，形成内环姿态控制器；

步骤3：基于PID理论针对四旋翼飞行器的外回路进行位置控制，形成外环位置控制器，实现姿态与位置通道的解耦控制；

步骤4：在仿真环境下，通过阶跃响应和跟踪上升螺旋线检验所提出的控制方法。

2.根据权利要求1所述的一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法，其特征在于，在步骤1中具体包括：

在机体坐标系O_BX_BY_BZ_B下，四旋翼飞行器产生的升力F＝[F₁ F₂ F₃ F₄]^T正向O_BZ_B方向；

当F₁＝F₂＝F₃＝F₄，飞行器做垂直起降或悬停运动；

当F₂＝F₄且F₁≠F₃，飞行器做俯仰运动；

当F₁＝F₃且F≠F₄，飞行器做横滚运动；

当F₁＝F₃≠F₂＝F₄时，飞行器做偏航运动；

基于上述驱动原理，可得到四旋翼控制输入的表达形式：

$\left\{\begin{array}{c}{U}_1=F_1+F_2+F_3+F_4\\ U_2=(F_2-F_4)L\\ U_3=(F_1-F_3)L\\ U_4=M_2+M_4-M_1-M_3\end{array}\right.---\left(1\right)$

其中，L为旋翼中心线各桨叶法向线的距离，U＝[U₁,U₂,U₃,U₄]^T为俯仰、横滚、偏航与总距输入信号，M＝[M₁,M₂,M₃,M₄]^T为各旋翼的力矩；

根据牛顿-欧拉方程，可以推导出四旋翼飞行器的动力学方程：

$\left\{\begin{array}{c}m\stackrel{\·\·}{r}=R_I^B\left(U_1{Z}_B\right)-{\mathrm{mgZ}}_I+F_D\\ I\stackrel{\·\·}{q}={\left[\begin{array}{ccc}{U}_2& U_3& U_4\end{array}\right]}^T-S\left(\stackrel{\·}{D}q\right)I\stackrel{\·}{D}q\end{array}\right.---\left(2\right)$

其中，m为四旋翼总质量，为大地坐标系O_IX_IY_IZ_I到机体坐标系的转换矩阵，g为重力加速度，F_D为空气阻力，I＝[I_xx I_yy I_zz]^T为转动惯量，q＝[φ θ ψ]^T为姿态矩阵，D为姿态角到角速度的映射关系，S(·)为旋量矩阵，φ、θ和ψ分别为横滚角、俯仰角和偏航角。

3.根据权利要求2所述的一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法，其特征在于，若不考虑空气阻力的影响，在低速飞行或悬停状态下，有φ≈0、θ≈0，另外，由四旋翼结构对称性可得I_xx≈I_yy，同时，由于不需要频繁控制偏航角，有因此，公式(2)可近似为：

$\begin{array}{c}\stackrel{\·\·}{x}=\frac{U_1}{m}(\mathrm{\θ}\cos \mathrm{\ψ}+\mathrm{\φ}\sin \mathrm{\ψ}),\stackrel{\·\·}{y}=\frac{U_1}{m}(\mathrm{\θ}\sin \mathrm{\ψ}-\mathrm{\φ}\cos \mathrm{\ψ})\\ \stackrel{\·\·}{z}=\frac{U_1}{m}-g,\stackrel{\·\·}{\mathrm{\φ}}=\frac{U_2}{I_{xx}},\stackrel{\·\·}{\mathrm{\θ}}=\frac{U_3}{I_{yy}},\stackrel{\·\·}{\mathrm{\ψ}}=\frac{U_4}{I_{zz}}\end{array}---\left(3\right).$

4.根据权利要求3所述的一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法，其特征在于，在步骤2中具体包括：

设定非线性***为：

$\left\{\begin{array}{c}\stackrel{\·}{x}=f\left(x\right)+g\left(x\right)u\\ y=h\left(x\right)\end{array}\right.---\left(4\right)$

为了跟踪期望轨迹r(t)，定义跟踪误差为：

e(t)＝r(t)-y(t) (5)

若对式(4)中***输出进行微分，则有：

$\stackrel{\·}{y}=\frac{\∂h}{\∂x}\stackrel{\·}{x}=\frac{\∂h}{\∂x}f\left(x\right)+\frac{\∂h}{\∂x}g\left(x\right)u=F\left(x\right)+G\left(x\right)u---\left(6\right)$

当G(x)可逆时，定义动态逆控制器为：

$u=G{\left(x\right)}^{-1}(-F(x)+\stackrel{\·}{r}+v)---\left(7\right)$

其中，v为逆动态模型的控制量，将公式(6)代入公式(5)中，可得误差动态***为：

$\stackrel{\·}{e}=-v---\left(8\right)$

利用线性控制技术配置v使***极点位于原点处，令：

v＝Ke (9)

其中，K为整定矩阵，

因此，***完整的动态逆控制器为：

$u=G{\left(x\right)}^{-1}(-F(x)+\stackrel{\·}{r}+Ke)---\left(10\right).$

5.根据权利要求3所述的一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法，其特征在于，在步骤2中，内回路的被控量为三轴姿态角，

内回路的输出向量为y_1c＝[φ θ ψ]^T，

逆动态控制量为：

$v_1=K_1{e}_1=\left[\begin{array}{c}{K}_{\mathrm{\φ}}({\mathrm{\φ}}_c-\mathrm{\φ})\\ K_{\mathrm{\θ}}({\mathrm{\θ}}_c-\mathrm{\θ})\\ K_{\mathrm{\ψ}}({\mathrm{\ψ}}_c-\mathrm{\ψ})\end{array}\right]---\left(11\right)$

因此，内回路的控制律为：

$\begin{array}{c}{u}_1=G{\left(x\right)}^{-1}(-F(x)+\stackrel{\·}{r}+Ke)\\ =\left[\begin{array}{ccc}1& \sin \mathrm{\φ}\tan \mathrm{\θ}& cos\mathrm{\φ}tan\mathrm{\θ}\\ 0& \cos \mathrm{\φ}& -sin\mathrm{\φ}\\ 0& \sin \mathrm{\φ}/\cos \mathrm{\θ}& cos\mathrm{\φ}/\cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{c}{K}_{\mathrm{\φ}}({\mathrm{\φ}}_c-\mathrm{\φ})\\ K_{\mathrm{\θ}}({\mathrm{\θ}}_c-\mathrm{\θ})\\ K_{\mathrm{\ψ}}({\mathrm{\ψ}}_c-\mathrm{\ψ})\end{array}\right]\end{array}---\left(12\right).$

6.根据权利要求3所述的一种基于DIC-PID的四旋翼飞行器姿态与位置的混合控制方法，其特征在于，在步骤3中，外回路的被控量为三轴位置与线速度，公式(3)给出了四旋翼相对于大地坐标系的线加速度，位置变量y_1w＝[x y z]^T可通过期望输入信号与姿态角获得，外回路中的线加速度为：

$a={\left[\begin{array}{ccc}\stackrel{\·\·}{x}& \stackrel{\·\·}{y}& \stackrel{\·\·}{z}\end{array}\right]}^T---\left(13\right)$

采用PID控制技术可得到线加速度为：

a＝K_Pe_w+K_I∫e_w+K_De_v (14)

其中，K_P、K_I和K_D分别为比例系数、积分系数与微分系数；e_w为位置误差；e_v为线速度误差。