一种高机动微型无人机的制导控制一体化方法

摘要

本发明公开了一种高机动微型无人机的制导控制一体化方法，包括建立高机动微型无人机的动力学模型和运动学模型；根据部分制导控制一体化设计思想将模型转为慢速动力学和快速动力学模型；利用小扰动线性化方法对慢速动力学模型进行线性化得到线性化模型，线性化的平衡点为上一采样时刻的状态量；根据线性二次调节器设计思想，构造价值函数中的Q矩阵和R矩阵，依据线性化模型在线计算反馈增益；对控制器进行参数整定。本发明的技术方案，基于线性二次调节器的部分制导控制一体化设计方法，以解决制导回路与控制回路延迟特性和三维通道耦合的问题，具有较好的控制性能和鲁棒性能。

权利要求

1.一种高机动微型无人机的制导控制一体化方法，其特征在于，包括以下步骤：S1：建立高机动微型无人机的动力学模型和运动学模型；高机动微型无人机的运动学模型为：

其中， X2(t)＝[vx(t),vy(t),vz(t),p(t),q(t),rT

(t)]均为运动学模型的状态向量；

为运动学模型的状态向量的导数，[x(t),y(t),z(t)]为高机动微型无人机在地面坐标系下的位置坐标， 分别为滚转角，俯仰角和偏航角；[vx(t),vy(t),vz(t)]为地面坐标系下的速度矢量，[p(t),q(t),r(t)]为机体坐标系下角速度矢量；P(X1(t))是非线性函数，t为当前时间；

高机动微型无人机的动力学模型为：其中， 为动力学模型的状态向量的导数，F(X1(t),X2(t))是非线性函数，g(t)是非线性控制分配函数，u(t)为控制输入向量；

S2：将涉及旋转运动的动力学称为快速动力学，涉及平移运动的动力学称为慢速动力学，根据部分制导控制一体化的设计思想将步骤S1得到的模型转为慢速动力学模型和快速动力学模型，表示为：

T

其中， Z2(t)＝[p(t),q(t),r(t)]分别为慢速动力学模型的状态向量和快速动力学模型的状态向量 ；

分别为慢速

动力学模型的状态向量的导数和快速动力学模型的状态向量的导数；u(t)＝[u1(t),u2(t)T

]为控制输入向量；f1(Z1(t)),f2(Z2(t))是非线性函数；g1(t),g2(t)是非线性控制分配函数；

S3：根据线性二次调节器的设计思想，构造价值函数中的状态代价矩阵Q和输入代价矩阵R，依据所述步骤S2的慢速动力学模型将其线性化、离散化，然后在线计算反馈增益，完成线性二次调节器的设计；

S4：根据PID控制器的设计思想，依据所述步骤S2的快速动力学模型，设计非线性PID控制器，完成整个控制系统的设计；

S5：对控制算法进行参数整定；在约束条件下，使所述步骤S4中设计的控制系统稳定以及控制目标达到控制要求。

2.根据权利要求1所述的一种高机动微型无人机的制导控制一体化方法，其特征在于，所述步骤S3中线性二次调节器的设计过程为：S3‑1：将上一时刻的状态量设为平衡点，利用小扰动线性化方法将慢速动力学模型线性化，即：

其中， 为慢速动力学模型t‑1时刻的状态向量；A为状态矩阵，B为输入矩阵，t‑1时刻慢速动力学模型的状态向量构成A(Z1,t‑1)和B(Z1,t‑1)；

S3‑2：根据被控量权重大小设计状态代价矩阵Q和输入代价矩阵R的对应的系数，将已经连续的线性化慢速动力学模型离散化，即：其中，T为单位采样时刻，kT为第k个采样时刻，k＝1,2,....,n，Z1(kT)为kT采样时刻的慢速动力学状态变量，Z1((k+1)T)为(k+1)T采样时刻的慢速动力学状态变量，G为离散后的状态矩阵，H为离散后的输入矩阵，t‑1时刻慢速动力学模型的状态向量构成G(Z1,t‑1)和H(Z1,t‑1)，u1(kT)为kT采样时刻的慢速动力学控制输入向量；

S3‑3：设定线性二次调节器的价值函数为： 其中，X(t)为状态向量，u(t)为控制输入向量，使得V最小从而迭代算出线性二次调节器的反馈增益K，完成线性二次型调节器的设计。

说明书

一种高机动微型无人机的制导控制一体化方法

技术领域

[0001] 本发明属于飞行器控制技术领域，尤其涉及一种高机动微型无人机的制导控制一体化方法。

背景技术

[0002] 微型无人机具有体积小、重量轻、操作性强、可单人携带、隐蔽性好、操作方便等特点，具有巨大的军事价值和民用价值。

[0003] 与传统飞行器不同，微型无人机在复杂环境下的自主飞行技术是一项极具挑战性的研究课题。首先随着无人机应对的情况越来越复杂，要求的高机动性也相应的增高，可控
调整时间也相应变短。并且高机动微型无人机三维通道间具有较大的耦合特性。因此，使用
传统线性方法进行制导控制律的设计可能难以满足任务需求。

[0004] 传统制导控制系统的设计思路基于制导环节和控制环节的频谱分离原则，将制导回路与控制回路分离，分别对两部分单独设计。但实际上制导环节和控制环节并不相互独
立，单独设计的缺陷是两个回路会以不同的频率运行，两个回路之间存在延迟。所以出现了
一种提高机动性的设计思路，即：制导环节和控制环节合二为一变成全状态一体化的设计，
但是此种方案忽略了快速动力学和慢速动力学的基本特性，在高机动情况下，极易造成飞
行器的不稳定。

发明内容

[0005] 为了解决上述已有技术存在的不足，本发明提出一种基于线性二次调节器的部分制导控制一体化设计方法，以解决制导回路与控制回路延迟特性和三维通道耦合的问题，
具有较好的控制性能和鲁棒性能。本发明的具体技术方案如下：

[0006] 一种高机动微型无人机的制导控制一体化方法，包括以下步骤：

[0007] S1：建立高机动微型无人机的动力学模型和运动学模型；

[0008] S2：根据部分制导控制一体化的设计思想将步骤S1得到的模型转为慢速动力学模型和快速动力学模型；

[0009] S3：根据线性二次调节器的设计思想，构造价值函数中的状态代价矩阵Q和输入代价矩阵R，依据所述步骤S2的慢速动力学模型将其线性化、离散化，然后在线计算反馈增益，
完成线性二次调节器的设计；

[0010] S4：根据PID控制器的设计思想，依据所述步骤S2的快速动力学模型，设计非线性PID控制器，完成整个控制系统的设计；

[0011] S5：对控制算法进行参数整定；在约束条件下，使所述步骤S4中设计的控制系统稳定以及控制目标达到控制要求。

[0012] 进一步地，所述步骤S1中高机动微型无人机的运动学模型为：

[0013]

[0014] 其中， X2(t)＝[vx(t),vy(t),vz(t),p(t),qT
(t),r(t)] 均为运动学模型的状态向量； 为运动学模
型的状态向量的导数，[x(t),y(t),z(t)]为高机动微型无人机在地面坐标系下的位置坐
标， 分别为滚转角，俯仰角和偏航角；[vx(t),vy(t),vz(t)]为地面坐标系下
的速度矢量，[p(t),q(t),r(t)]为机体坐标系下角速度矢量；P(X1(t))是非线性函数，t为
当前时间；

[0015] 高机动微型无人机的动力学模型为：

[0016]

[0017] 其中， 为动力学模型的状态向量的导数，F(X1(t),X2(t))是非线性函数，g(t)是非线性控制分配函数，u(t)为控制输入向量。

[0018] 进一步地，将涉及旋转运动的动力学称为快速动力学，涉及平移运动的动力学称为慢速动力学，所述步骤S2中根据部分制导控制一体化设计思想将步骤S1得到的模型转为
慢速动力学模型和快速动力学模型，表示为：

[0019]

[0020] 其中， Z2(t)＝[p(t),qT
(t),r(t)] 分别为慢速动力学模型的状态向量和快速动力学模型的状向量态；
分别为慢速
动力学模型的状态向量的导数和快速动力学模型的状态向量的导数；u(t)＝[u1(t),u2(t)
T
]为控制输入向量；f1(Z1(t)),f2(Z2(t))是非线性函数；g1(t),g2(t)是非线性控制分配函
数。

[0021] 进一步地，所述步骤S3中线性二次调节器的设计过程为：

[0022] S3‑1：将上一时刻的状态量设为平衡点，利用小扰动线性化方法将慢速动力学模型线性化，即：

[0023]

[0024] 其中， 为慢速动力学模型t‑1时刻的状态向量；A为状态矩阵，B为输入矩阵，t‑1时刻慢速动力学模型的状态向量构成A(Z1,t‑1)
和B(Z1,t‑1)。

[0025] S3‑2：根据被控量权重大小设计状态代价矩阵Q和输入代价矩阵R的对应的系数，将已经连续的线性化慢速动力学模型离散化，即：

[0026]

[0027] 其中，T为单位采样时刻，kT为第k个采样时刻，k＝1,2,....,n，Z1(kT)为kT采样时刻的慢速动力学状态变量，Z1((k+1)T)为(k+1)T采样时刻的慢速动力学状态变量，G为离散
后的状态矩阵，H为离散后的输入矩阵，t‑1时刻慢速动力学模型的状态向量构成G(Z1,t‑1)和
H(Z1,t‑1)，u1(kT)为kT采样时刻的慢速动力学控制输入向量；

[0028] S3‑3：设定线性二次调节器的价值函数为： 其中，X(t)为状态向量，u(t)为控制输入向量，使得V最小从而迭代算出线性二次调节器的反
馈增益K，完成线性二次型调节器的设计。

[0029] 本发明的有益效果在于：

[0030] 1.本发明的方法能够实现部分制导控制一体化设计，弥补了传统控制方案的缺陷：首先，两个回路存在延迟会造成高机动性的丧失，发挥不出高机动微型无人机的机动优
势，本发明的方法将原本姿态回路中的姿态角状态变量放到了第一回路，极大的提升了控
制系统的快速性；其次，机动飞行时，位置环会产生一个特别大的姿态控制指令，容易造成
电机的饱和并且由于忽略姿态动力学，因此需要规划平滑轨迹，本发明的方法考虑了系统
姿态动力学，给控制系统加入了姿态约束，减少了电机饱和的可能性。

[0031] 2.依据线性二次调节器的在线反馈增益计算；本发明依据三通道耦合的线性化模型，并且每一采样时刻线性模型是不同的，并在每一采样时刻在线计算反馈增益，从而提升
了控制系统的鲁棒性。

附图说明

[0032] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，通过参考附图会更加清楚的理解本发明的特征和优点，附
图是示意性的而不应理解为对本发明进行任何限制，对于本领域普通技术人员来讲，在不
付出创造性劳动的前提下，可以根据这些附图获得其他的附图。其中：

[0033] 图1为本发明的部分制导控制一体化控制框图；

[0034] 图2为本发明实施例的微型四旋翼坐标系及坐标定义图；

[0035] 图3为线性二次调节器反馈增益计算流程图；

[0036] 图4为常规轨迹跟踪仿真图，(a)为菱形轨迹，(b)为圆形轨迹；

[0037] 图5为复杂轨迹跟踪仿真图。

具体实施方式

[0038] 为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明的实施
例及实施例中的特征可以相互组合。

[0039] 在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开
的具体实施例的限制。

[0040] 灵活且功能多样的微型无人机能够执行非常复杂的任务，比如穿越丛林或建筑物，进出狭窄的间隙，快速定位目标，从而对其进行打击。本发明方法的主要步骤是：对三通
道耦合的微型无人机的模型进行线性化，随后根据线性二次调节器的设计方法算出每一时
刻的反馈增益，最后根据部分制导一体化的设计框架对整个回路进行设计。

[0041] 传统控制算法通常基于假设和简化，利用的是平移和旋转动力学之间的尺度分离特性。其基本设计思路为基于系统的欠驱动特性，将控制问题分解为多个子问题，并且利用
期望的加速度来定义高机动无人机的姿态。但是这种设计方法也引入一定的局限性：两个
回路会以不同的频率运行，存在延迟会造成高机动性的丧失，并且由于忽略姿态动力学，因
此需要规划平滑轨迹。本发明的方法能够实现部分制导控制一体化设计。

[0042] 另外，由于三通道之间的耦合特性，目前对微型无人机的飞行控制主要采用不依赖于模型或者对模型精确性要求不高的控制方法。线性二次调节器需要根据线性模型设计
价值函数从而离线算出控制算法的反馈增益。本发明依据的是三通道耦合的线性化模型，
并且每一采样时刻线性模型是不同的，并在每一采样时刻在线计算反馈增益。从而提升了
控制系统的鲁棒性。

[0043] 为了方便理解本发明的上述技术方案，以下通过具体实施例对本发明的上述技术方案进行详细说明。

[0044] 实施例1

[0045] 以微型四旋翼为实例，即：将姿态、位置和速度视为慢速动力学的状态变量，角速度视为快速动力学的状态变量，随后对慢速动力学和快速动力学分别设计控制算法，实现
部分制导控制一体化算法的设计，克服了原有串级控制的缺陷。控制框图如图1所示，一种
高机动微型无人机的制导控制一体化方法，具体地：

[0046] S1：建立微型四旋翼的动力学和运动学模型；

[0047] 本发明中，采用北‑东‑地坐标系对微型四旋翼进行设计分析。利用一组标准正交基{xw,yw,zw}表示世界坐标系W，以及另一组正交基{xb,yb,zb}表示相对世界坐标表示的机
体坐标系B。其中，机体坐标系固定在微型四旋翼飞行器上，其原点与其质心重合。微型四旋
翼的重力和惯性矩阵分别记为 和 微型四旋翼姿态角、机体角速度、位
置、速度、加速度分别记为 质心的位置记为Ob；质心至每个电机重心的
距离记为 第i个电机产生的拉力和反扭力矩分别记为fi和τi(1≤i≤4)，方向沿‑zb；
将总拉力和电机在机体产生的力矩的大小分别记为 将方向旋转矩阵记为
b
欧拉角变化率与机体角速度之间的转移矩阵为W ，如图2所示。

[0048] 运动学模型：世界坐标系下，无人机的线速度为 利用欧拉角表示微型四旋翼的姿态：

[0049] 动力学模型：根据牛顿‑欧拉动力学方程，微型四旋翼在世界坐标系下的线速度与合外力的关系以及在机体坐标系下角速度与力矩的关系分别为：

[0050]

[0051]

[0052] 其中，J为微型四旋翼的转动惯量，Fw为微型四旋翼所受合外力在地面坐标系中的表示，M为作用在微型四旋翼上的外力矩在机体坐标系中的表示。

[0053] 根据受力分析，微型四旋翼所受合外力主要由螺旋桨产生的拉力、重力组成；拉力大小为四个螺旋桨提供拉力和；方向总是沿‑zb轴，重力大小mg，总是指向zw轴。综上所述，微
型四旋翼的平移动力学方程为：

[0054]

[0055] 力矩是由四个螺旋桨提供的升力和反扭力矩产生，一般表示为：

[0056]

[0057] τ为电机在机体产生的力矩，ki是与单个力矩和单个电机拉力相关的系数。

[0058] 根据以上分析，得出微型四旋翼的整体数学模型：

[0059]

[0060]

[0061]

[0062]

[0063] S2：根据部分制导控制一体化设计思想将步骤S1得到的模型转为慢速动力学和快速动力学模型；

[0064] 部分制导控制一体化设计思想：不再利用制导环节产生控制指令，而是将制导环节和部分控制环节合二为一，根据机体与目标的相对运动信息以及机体自身运动信息(包
括位置、速度以及姿态角)产生控制指令驱使飞行器接近目标。对于高机动微型四旋翼而
言，涉及旋转的的动力学的频带是涉及平移的动力学的四到五倍，所以旋转远远比平移快
的多。根据高机动微型四旋翼模型带宽频率的不同，将涉及旋转运动的动力学称为快速动
力学，涉及平移运动的动力学称为慢速动力学，将S1步得出的数学模型划分为慢速动力学
模型和快速动力学模型。

[0065] 慢速动力学包含位置和速度以及姿态角信息，输入为期望角速度控制量和期望拉力指令。选择状态变量和控制输入： 其中，x1高机动微型四旋翼的慢速动
力学状态向量，u1为慢速动力学的控制输入向量，ωb,des为期望角速度控制量，fdes为期望拉
力。慢速动力学模型变为：

[0066]

[0067]

[0068]

[0069] 快速动力学包含机体角速度信息，输入期望力矩。选择状态变量和控制输入：其中，x2高机动微型四旋翼的快速动力学状态向量，u2为快速动力学的控制输入
向量，τdes为期望力矩。快速动力学模型变为：

[0070] S3：根据线性二次调节器设计思想，构造价值函数中的状态代价矩阵Q和输入代价矩阵R，依据步骤S2的慢速动力学模型将其线性化、离散化，然后在线计算反馈增益，完成线
性二次调节器的设计；

[0071] S3‑1：设上一状态量为平衡点，根据小扰动线性化方法对慢速动力学模型进行线性化，得到：

[0072]

[0073]

[0074] 最后得线性化后的模型为 其中， 为慢速动力学状态向量的导数。A为慢速动力学线性化后的状态矩阵，B为慢速动力学线性化的输入矩阵。

[0075] S3‑2：根据被控量权重设计状态代价矩阵Q和输入代价矩阵R的对应系数，将已经连续的线性化慢速动力学模型离散化。对于高机动微型四旋翼，慢速动力学模型的被控量
为位置，速度以及姿态角，根据任务需求，确定被控量的权重，为实现权重大的被控量的准
确跟踪，需修改对应Q矩阵的系数以及R矩阵的系数，从而实现任务的控制需求。

[0076] 对线性化后的慢速动力学模型进行离散化，因为对于每一状态下的线性连续状态空间模型均可视为线性连续定常系统的状态空间模型，所以可应用经典离散方法对动力学
模型进行线性化。离散过程用数学语言表示为：

[0077]

[0078] 其中，T为单位采样时间，x1((k+1)T)为(k+1)T采样时刻的慢速动力学向量，x(kT)为kT采样时刻的慢速动力学向量，u1(kT)为kT采样时刻的慢速动力学控制输入向量，G为慢
速动力学模型离散后的状态矩阵，H为慢速动力学模型离散后的输入矩阵。

[0079] 根据精确离散化的方法将原有线性定常模型离散化，可得：

[0080] G＝eAT

[0081]

[0082] 其中，eAT是关于A T的指数函数；

[0083] S3‑3：根据线性二次调节器的设计的思想，通过迭代的方式的在线计算反馈增益。

[0084] 线性二次调节器的原有价值函数为： 由于非线性系统已被离散化，所以线性二次调节器转为离散时间线性二次调节器，价值函数转为Vt:
n
R→R

[0085]

[0086] 其中，需要满足约束xt＝z,xτ+1＝Gxτ+Huτ,τ＝t,...,N‑1。即：Vt(z)给出的是从t时刻的状态z开始线性二次调节器代价函数。uτ为τ时刻的控制输入，xN为最终的状态变量，Qf
为最终状态代价矩阵；当t＝0时，V0(x0)就是原始的线性二次调节代价函数。Vt是二次型，
T T
即：Vt(z)＝zPtz，其中，t时刻的中间矩阵由Pt指代，并且当t＝N时有：VN(z)＝z Qfz，得到PN
＝Qf，令ω为每一时刻的控制输入u，则此优化问题转化为动态规划问题：

[0087]

[0088] 其中，zTQz+ωTRω是当前时刻的代价值，Vt+1(G+Hωz)是从下一时刻到N时刻的代价值；这是一个典型的动态规划问题，也就是说，当前时刻t的控制律ut取值ultqr为：

[0089]

[0090] 将Vt+1(z)＝zTPt+1z带入上面Vt(z)的表达式，Pt+1为t+1时刻的中间矩阵，即有：

[0091]

[0092] 令其导数等于零即可得到最优解：

[0093] ω＝‑(R+HTPt+1H)‑1HTPt+1Gz

[0094] 将此结果带入Vt(x)的表达式，即：

[0095]

[0096] 其中：

[0097] K＝(R+HTPt+1H)‑1HTPt+1G

[0098] Pt＝Q+GTPt+1G‑GTPt+1H(R+HTPt+1)‑1HTPt+1G

[0099] 由上述动态规划推导的线性二次调节器的架构可直接拓展到时变系统。具体流程图如图3所示。

[0100] S4：根据PID控制器的设计思想，依据步骤S2的快速动力学模型，设计非线性PID控制器：

[0101]

[0102] 其中，τdes控制输入期望力矩；Patt为控制器系数； 分别为期望角速度，估计角速度以及估计交加速度，从而完成整个控制系统的设计。

[0103] S5：参数整定；

[0104] 由前四步骤，部分制导控制一体化算法设计完毕，根据基本场景微调参数以增强算法的稳定性和鲁棒性。轨迹跟踪验证如图4和图5所示，验证了本发明的部分制导控制一
体化算法对于机动轨迹跟踪具有稳定和鲁棒性的特点。

[0105] 以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修
改、等同替换、改进等，均应包含在本发明的保护范围之内。