CN112684805B - 一种考虑姿态约束的高机动微型无人机控制方法 - Google Patents

Abstract

本发明公开了一种考虑姿态约束的高机动微型无人机控制方法，首先搭建高机动微型无人机的运动学和动力学模型，其中，利用旋转矩阵对无人机的姿态进行表示；然后根据非线性模型对位置控制算法和姿态控制算法分别进行设计。通过本发明的技术方案，能够解决线性控制在高机动姿态约束场景下控制系统不稳定问题和欧拉角表示姿态时的不连续问题，具有姿态表示连续，控制系统鲁棒性强等优点。

Description

一种考虑姿态约束的高机动微型无人机控制方法

技术领域

本发明属于飞行器控制技术领域，尤其涉及一种考虑姿态约束的高机动微型无人机控制方法。

背景技术

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

与传统飞行器不同，微型无人机在复杂环境下的自主飞行技术是一项极具挑战性的研究课题。随着无人机应对的场景越来越复杂，比如穿梭丛林或者窗口等姿态约束下的场景，要求的高机动性也相应的提高，所以使用传统线性方法进行算法的设计难以满足任务需求。

传统的控制系统的设计思路首先根据小扰动线性化的方法对微型无人机的数学模型进行线性化，然后将位置控制回路与姿态控制回路分离，分别对两部分依据线性模型单独设计。其中，在姿态控制回路中采用欧拉角对姿态表示。这是一种简单可行的方案，但实际上缺点同样明显，首先传统设计方案依据的是线性化的模型，而忽略了微型无人机模型中的非线性项，在姿态约束场景下非线性项会变得很大，往往会造成机体的不稳定；其次，姿态采用欧拉角表示，实质上是将姿态间的变换转为了绕三轴旋转的角的组合，存在姿态不连续问题，不适合姿态约束下的控制任务，针对以上两个问题，提出非线性控制方案，在线性控制的基础上，加入了非线性补偿，增加了控制系统的稳定性；用旋转矩阵作为微型无人机的姿态表示，弥补了欧拉角姿态表示的缺陷。通过以上两个改进确保了高机动微型无人机在姿态约束时的控制稳定性。

发明内容

为了解决上述已有技术存在的不足，本发明提出一种考虑姿态约束的高机动微型无人机控制方法，是一种基于旋转矩阵的控制方法，以解决线性控制在高机动姿态约束场景下控制系统不稳定问题和欧拉角表示姿态时的不连续问题，具有姿态表示连续，控制系统鲁棒性强等优点。本发明的具体技术方案如下：

一种考虑姿态约束的高机动微型无人机控制方法，其特征在于，包括以下步骤：

S1：搭建高机动微型无人机的非线性运动学和动力学模型，采用旋转矩阵表示高机动微型无人机的姿态；

S2：设计位置控制算法；根据步骤S1中的非线性运动学和动力学模型，将控制算法设计问题转为位置控制设计和姿态控制设计两个子问题，设计位置控制算法得出所需的加速度控制指令以及期望拉力控制输入；

S3：解算期望旋转矩阵；由步骤S2的位置控制算法输出的期望加速度解算出所需的旋转矩阵以实现姿态控制；

S4：设计姿态控制算法；根据步骤S3解算的期望旋转矩阵设计姿态误差矩阵，依据非线性动力学对姿态控制算法进行补偿，计算出所需期望力矩，完成姿态控制算法设计；

S5：调节控制算法参数；所述算法是针对姿态约束的场景，固定的控制增益不能控制所有高机动微型无人机飞行状态，需在飞行时利用增益调度的方法调节控制算法参数，保证多种飞行状态乃至姿态约束时高机动微型无人机的可控性。

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

其中，X₁＝[P R]^T，X₂＝[v ψ]^T分别为运动学模型和动力学模型的状态向量；

为运动学模型的状态向量的导数，其中，P＝[x,y,z]为高机动微型无人机在地面坐标系下的位置坐标，R＝[r₁,r₂,r₃]为滚转角、俯仰角和偏航角组成的旋转矩阵；v＝[v_x,v_y,v_z]为地面坐标系下的速度矢量，ω＝[p,q,r]为机体坐标系下角速度矢量，p为高机动微型无人机滚转角速度，q为高机动微型无人机俯仰角速度，r为高机动微型无人机偏航角速度；P(X₁)是非线性函数；

高机动微型无人机的动力学方模型为：

其中，

为动力学模型的状态向量的导数，U＝[T,τ_x,τ_y,τ_z]^T为控制输入向量，T为高机动微型无人机拉力输入，τ_x,τ_y,τ_z分别为高机动微型无人机在各机体轴上的滚转力矩输入、俯仰力矩输入和偏航力矩输入，F(X₁,X₂)是非线性函数，g是非线性控制分配函数。

进一步地，所述步骤S2中设计位置控制算法的过程为：

依据平移运动设计位置控制算法，平移运动模型为：

其中，g为加速度向量，R为旋转矩阵，z_w为惯性坐标系中z轴坐标轴向量，m为高机动微型无人机质量；

由平移运动模型知，平移运动包含位置P信息、速度v信息、产生期望加速度指令a_d和期望拉力T_d，则状态变量和期望输入指令为：

x₁＝[P v]^T

u₁＝[a_d T_d]^T

其中，x₁为涉及平移运动的状态变量，u₁为平移运动的控制输入；

控制目的是设计期望加速度a_d，使

以及

成立；将位置误差e_p(t)简化为e_p，将速度误差e_v(t)简化为e_v，则位置误差e_P＝P_d-P，速度误差e_v＝v_d-v，P_d,v_d分别为期望位置和期望速度；

整个位置控制算法采用串级PID进行设计，位置内回路为速度环，采用P控制，保证速度控制快速响应，由平移动力学特性加入非线性补偿项，记K_v,P为内回路比例控制算法参数，具体算法为：

a_d＝K_v,Pe_v-g

外回路同样采用P控制，记v′为虚拟控制输入，记K_P,P为外回路比例控制算法参数，具体算法为：

v′＝K_P,Pe_p

最后，记机体坐标系中的z坐标轴向量在惯性坐标系的投影向量为

由此计算出期望拉力：

进一步地，所述步骤S3中解算期望旋转矩阵的步骤为：

S3-1：设期望的旋转矩阵为R_d＝[r_1,d r_2,d r_3,d]，r_1,d，r_2,d，r_3,d分别为期望旋转矩阵中的三个元素向量，其实质为期望机体坐标三个期望的坐标轴在惯性坐标系中的投影向量，由于采用北东地坐标系，期望旋转矩阵三个元素向量为机体坐标轴在地面坐标系的投影的反向量；

计算期望旋转矩阵第三个元素向量，利用平移动力学公式，即：

根据位置控制算法，将上述公式转为：

S3-2：设计中间向量为r₁′＝[-sinψ_d,cosψ_d,0]^T，ψ_d为期望偏航角，根据线面垂直定理证得：r₁′⊥x_b⊥z_b，x_b为机体坐标系中x坐标轴向量，z_b为机体坐标系中z坐标轴向量，得到期望旋转矩阵的第一个元素向量：

S3-3：由于旋转矩阵为正交矩阵，根据正交矩阵的性质得R_d＝[r_1,d,r_3,d×r_1,d,r_3,d]。

进一步地，所述步骤S3中存在两个奇点，即：||a_d||＝0或||r₁×r_3,d||＝0，对于第一种奇点，对||a_d||加入限制，使得||a_d||＞0，则能够保证不会碰到第一种奇点；对于第二种奇点，包括奇异点处和奇异点两侧：

若姿态变换小于90°时，期望旋转矩阵按照步骤S3的方法解算；

若姿态变换大于90°时，引入负轴检测算法，即：

R_d＝[r_1,d,r_3,d×r_1,d,r_3,d]

从而完成姿态的转换，旋转之后和原有姿态有一定的连续性；

在奇点处||r₁′×r_3,d||＝0，不能解算期望旋转矩阵，r₁′加入额外的角度γ能够避免奇点的影响，即设计r₁ ^′＝[-sinγsinψ_d,cosγcosψ_d,sinγ]^T。

进一步地，所述步骤S4中设计姿态控制算法的步骤为：

依据旋转动力学设计姿态控制算法，其中，旋转运动模型为：

其中，τ为高机动微型无人机受到的力矩，J为高机动微型无人机转动惯量矩阵；

由上述模型知旋转运动包含姿态R信息、角速度ω信息和产生期望力矩τd指令；

首先选择状态变量和期望指令：

x₂＝[R ω]^T

u₂＝[τ_x τ_y τ_z]^T

其中，x₂为涉及旋转运动的状态变量，u₂为旋转运动的控制输入；姿态控制算法的目的是设计期望力矩τ_d，使得

和

可旋转矩阵误差e_R(t)简化为e_R，角速度误差e_ω(t)简化为e_ω，则旋转矩阵误差

角速度追踪误差定义为e_ω＝R^TR_dω_d-ω，其中，R_d和ω_d分别为期望旋转矩阵和期望的角速度，vex函数具有将斜对称矩阵转为列向量的作用，起到解算三个轴角作用；

整个姿态控制算法采用串级PID进行设计，姿态内回路为角速度环，采用PID控制，记K_ω,P,K_ω,I,K_ω,D分别为角速度环PID控制算法的比例参数，积分参数以及微分参数，具体算法为：

外回路采用P控制，记ω′为虚拟控制输入，K_R,P为控制算法的比例控制参数，具体算法为：ω′＝K_R,Pe_R。

本发明的有益效果在于：

1.本发明的提出的控制方案，在线性控制的基础上，加入了非线性补偿，增加了控制系统的稳定性，能够实现姿态约束下的控制系统设计，弥补了传统控制方案的缺陷。

2.本发明利用旋转矩阵作为微型无人机的姿态表示，旋转矩阵表示姿态是唯一的，能够实现旋转的连续性，即：可以直接对旋转进行叠加，没有万向节死锁等缺陷；解决了欧拉角表示姿态会出现万向节死锁的问题，没有办法对旋转进行叠加。

3.传统控制算法的设计是基于假设和简化的线性模型，忽略了无人机模型中的非线性项，但是对于姿态约束场景下高机动的飞行场景，这些非线性项会变得很大，如果忽略这些非线性项，会造成机体的不稳定。本发明根据非线型模型设计控制算法，加入非线性补偿项，提高了控制系统的稳定裕度，适合姿态约束场景下的控制要求。

附图说明

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

图1为姿态约束场景下飞行演示图；

图2为微型四旋翼坐标系及坐标定义图；

图3为计算期望旋转矩阵线面垂直证明图；

图4为未采用算法绕x轴旋转图示；

图5为加入算法后绕x轴旋转图示；

图6为三通道姿态信号跟踪曲线。

具体实施方式

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

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

传统控制算法通常基于假设和简化。其基本设计思路为基于系统的欠驱动特性，将控制问题分解为位置控制和姿态控制子问题，并且利用期望的加速度解算出期望的欧拉角。但是这种方法有一定的局限性。

本发明针对高机动微型无人机在姿态约束场景下的飞行特点(如图1所示)，提出合适可靠的控制方法，以实现复杂近战环境下的快速高机动性能，提升微型无人机的自主作战能力。本发明的方法具体为一种基于旋转矩阵的控制方法，首先搭建高机动微型无人机的运动学和动力学模型，其中，利用旋转矩阵对无人机的姿态进行表示；然后根据非线性模型对位置控制算法和姿态控制算法分别进行设计。本发明的方法能够实现姿态约束下的控制系统设计，弥补了传统控制方案的缺陷；本发明的方法能够解决普通控制方案在高机动姿态约束场景下控制系统不稳定问题和欧拉角表示姿态时的不连续问题，具有姿态表示连续，控制系统鲁棒性强等优点。

灵活且功能多样的微型无人机能够执行非常复杂的任务，比如穿越丛林或建筑物，进出狭窄的间隙，快速定位目标，从而对其进行打击。为了方便理解本发明的上述技术方案，以下通过高机动微型四旋翼对本发明的上述技术方案进行详细说明，说明本发明的方法克服了传统控制算法的缺陷。一种考虑姿态约束的高机动微型无人机控制方法，包括以下步骤：

S1：建立高机动微型四旋翼的动力学模型和运动学模型；由于面临的是姿态约束的场景，利用欧拉角表示姿态将不再适用，为解决欧拉角万向节死锁问题、插值均匀问题以及姿态表示的连续问题，采用旋转矩阵表示高机动微型无人机的姿态。

本发明中，采用北-东-地坐标系对微型四旋翼进行设计分析。利用一组标准正交基{x_w,y_w,z_w}表示世界坐标系W，以及另一组正交基{x_b,y_b,z_b}表示相对世界坐标表示的机体坐标系B。其中，机体坐标系固定在微型四旋翼飞行器上，其原点与其质心重合。微型四旋翼的质量和转动惯量矩阵分别记为

和

微型四旋翼机体角速度、位置、速度、加速度分别记为ω,P,v,

质心的位置记为O_b；质心至每个电机重心的距离记为

第i个电机产生的拉力和反扭力矩记为f_i和τ_i(1≤i≤4)，方向沿-z_b；将总拉力和电机在机体产生的力矩的大小分别记为f,

将方向旋转矩阵记为R＝[r₁ r₂ r₃]∈SO³，微型四旋翼相关坐标系如图2所示。

运动学模型：世界坐标系下，无人机的线速度为

利用旋转矩阵表示微型四旋翼的姿态：

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

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

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

作用微型四旋翼无人机上外部力矩M主要由力矩τ组成，力矩是由四个螺旋桨提供的升力和反扭力矩生，一般由下表示：

其中，k_f是电机拉力力矩系数，一般可以视为固定常数。综上所述，微型四旋翼的旋转动力学方程为：

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

S2：设计位置控制算法；

依据平移运动设计位置控制算法，平移运动模型为：

其中，g为加速度向量，R为旋转矩阵，z_w为惯性坐标系中z轴坐标轴向量，m为无人机质量；

由平移运动模型知，平移运动包含位置P信息、速度v信息、产生期望加速度指令a_d和期望拉力T_d，则状态变量和期望输入指令为：

x₁＝[P v]^T

u₁＝[a_d T_d]^T

其中，x₁为涉及平移运动的状态变量，u₁为平移运动的控制输入；

控制目的是设计期望加速度a_d，使

以及

成立；将位置误差e_p(t)简化为e_p，将速度误差e_v(t)简化为e_v，则位置误差e_P＝P_d-P，速度误差e_v＝v_d-v，P_d,v_d分别为期望位置和期望速度；

整个位置控制算法采用串级PID进行设计，位置内回路为速度环，采用P控制，保证速度控制快速响应，由平移动力学特性加入非线性补偿项，记K_v,P为内回路比例控制算法参数，具体算法为：

a_d＝K_v,Pe_v-g

外回路同样采用P控制，使得微型四旋翼在姿态改变过后能够快速恢复自身的稳定性，记v′为虚拟控制输入，记K_P,P为外回路比例控制算法参数，具体算法为：

v′＝K_P,Pe_p

最后，记机体坐标系中的z坐标轴向量在惯性坐标系的投影向量为

由此计算出期望拉力：

S3：解算期望旋转矩阵；S3-1：设期望的旋转矩阵为R_d＝[r_1,d r_2,d r_3,d]，r_1,d，r_2,d，r_3,d分别为期望旋转矩阵中的三个元素向量，其实质为期望机体坐标三个期望的坐标轴在惯性坐标系中的投影向量，由于采用北东地坐标系，期望旋转矩阵三个元素向量为机体坐标轴在地面坐标系的投影的反向量；

计算期望旋转矩阵第三个元素向量，利用平移动力学公式，即：

根据位置控制算法，将上述公式转为：

S3-2：设计中间向量为r₁′＝[-cosψ_d,cosψ_d,0]^T，ψ_d为期望偏航角，如图3所示，依据线面垂直定理证得：r₁′⊥x_b⊥z_b，x_b为机体坐标系中x坐标轴向量，z_b为机体坐标系中z坐标轴向量，得到期望旋转矩阵的第一个元素向量：

S3-3：由于旋转矩阵为正交矩阵，根据正交矩阵的性质得R_d＝[r_1,d,r_3,d×r_1,d,r_3,d]。

步骤S3中存在两个奇点，即：||a_d||＝0或||r₁×r_3,d||＝0，对于第一种奇点，对||a_d||加入限制，使得||a_d||＞0，则能够保证不会碰到第一种奇点；对于第二种奇点，包括奇异点处和奇异点两侧：

若姿态变换小于90°时，期望旋转矩阵按照步骤S3的方法解算；

若姿态变换大于90°时，引入负轴检测算法，即：

R_d＝[r_1,d,r_3,d×r_1,d,r_3,d]

从而完成姿态的转换，旋转之后和原有姿态有一定的连续性；

在奇点处||r₁′×r_3,d||＝0，不能解算期望旋转矩阵，r₁′加入额外的角度γ能够避免奇点的影响，即设计₁ ^′＝[-sinγ sinψ_d，cosγ cosψ_d,sinγ]^T，本实施例设额外角度γ＝10°。

姿态解算算法验证如图4和图5所示，图4表明，当未采用本专利的方法进行解算时，姿态变换大于90°时，姿态会产生突变，造成无人机机体的不稳定，图5表明，采用本专利的方法解算旋转矩阵时，避免了姿态变换时的突变，保证了大姿态变换的连续性。

S4：设计姿态控制算法；依据旋转动力学设计姿态控制算法，其中，旋转运动模型为：

其中，ω为机体角速度，τ为无人机受到的力矩，J为无人机转动惯量矩阵；

p为无人机滚转角速度，q为无人机俯仰角速度，r为无人机偏航角速度；

由上述模型知旋转运动包含姿态R信息、角速度ω信息和产生期望力矩τd指令；

首先选择状态变量和期望指令：

x₂＝[R ω]^T

u₂＝[τ_x τ_y τ_z]^T

其中，x₂为涉及旋转运动的状态变量，u₂为旋转运动的控制输入，τ_x,τ_y,τ_z分别为无人机受到的滚转力矩，俯仰力矩以及偏航力矩；姿态控制算法的目的是设计期望力矩τ_d，使得

和

可旋转矩阵误差e_R(t)简化为e_R，角速度误差e_ω(t)简化为e_ω，则旋转矩阵误差

角速度追踪误差定义为e_ω＝R^TR_dω_d-ω，其中，R_d和ω_d分别为期望旋转矩阵和期望的角速度，vex函数具有将斜对称矩阵转为列向量的作用，起到解算三个轴角作用；

整个姿态控制算法采用串级PID进行设计，姿态内回路为角速度环，采用PID控制，保证角速度控制的快速性和准确性，并根据无人机旋转动力学模型加入非线性补偿环节，在平稳飞行情况下，非线性补偿项几乎为零，对控制性能没有任何影响，但是在姿态频繁改变时，即在姿态约束场景下非线性项会变得很大，如果没有非线性的补偿，不能保证姿态控制在姿态约束场景中的稳定性，加入非线性补偿环节以后可以保证几乎任意旋转的情况下，姿态控制系统能够指数稳定，记K_ω,P,K_ω,I,K_ω,D分别为角速度环PID控制算法的比例参数，积分参数以及微分参数，具体算法为：

外回路采用P控制，增加姿态控制时的快速响应能力，为减少姿态控制的不稳定性，比例环节增益不应过大，记ω′为虚拟控制输入，K_R,P为控制算法的比例控制参数，具体算法为：ω′＝K_R,Pe_R。

S5：调节控制算法参数。

由步骤S1-步骤S4，考虑姿态约束的控制算法设计完成，算法是针对姿态约束的场景，固定的控制增益不能控制所有无人机飞行状态，需要根据实际的试飞经验列出所需的控制增益表，在飞行时利用增益调度的方法调节控制算法参数，保证多种飞行状态乃至姿态约束时无人机的可控性。根据基本场景以及微型旋翼本身飞行状态通过增益调度表来改变不同的控制参数以增强算法的稳定性和鲁棒性。

如图6姿态变化曲线所示，三通道姿态角随时间大幅变化，所设计控制器能在跟踪信号的同时维持了控制系统的稳定性，体现了本发明的控制算法的有效性。

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

Claims (4)

1.一种考虑姿态约束的高机动微型无人机控制方法，其特征在于，包括以下步骤：

S1：搭建高机动微型无人机的非线性运动学和动力学模型，采用旋转矩阵表示高机动微型无人机的姿态；其中，高机动微型无人机的运动学模型为：

其中，X₁＝[P R]^T，X₂＝[v ω]^T分别为运动学模型和动力学模型的状态向量；

为运动学模型的状态向量的导数，其中，P＝[x，y，z]为高机动微型无人机在地面坐标系下的位置坐标，R＝[r₁，r₂，r₃]为滚转角、俯仰角和偏航角组成的旋转矩阵；v＝[v_x，v_y，v_z]为地面坐标系下的速度矢量，ω＝[p，q，r]为机体坐标系下角速度矢量，p为高机动微型无人机滚转角速度，q为高机动微型无人机俯仰角速度，r为高机动微型无人机偏航角速度；P(X₁)是非线性函数；

高机动微型无人机的动力学方模型为：

其中，

为动力学模型的状态向量的导数，U＝[T，τ_x，τ_y，τ_z]^T为控制输入向量，T为高机动微型无人机拉力输入，τ_x，τ_y，τ_z分别为高机动微型无人机在各机体轴上的滚转力矩输入、俯仰力矩输入和偏航力矩输入，F(X₁，X₂)是非线性函数，g是非线性控制分配函数；

S2：设计位置控制算法；根据步骤S1中的非线性运动学和动力学模型，将控制算法设计问题转为位置控制设计和姿态控制设计两个子问题，设计位置控制算法得出所需的加速度控制指令以及期望拉力控制输入；具体地，依据平移运动设计位置控制算法，平移运动模型为：

其中，g为加速度向量，R为旋转矩阵，z_w为惯性坐标系中z轴坐标轴向量，m为高机动微型无人机质量；

由平移运动模型知，平移运动包含位置P信息、速度v信息、产生期望加速度指令a_d和期望拉力T_d，则状态变量和期望输入指令为：

x₁＝[P v]^T

u₁＝[a_d T_d]^T

其中，x₁为涉及平移运动的状态变量，u₁为平移运动的控制输入；

控制目的是设计期望加速度a_d，使

以及

成立；将位置误差e_p(t)简化为e_p，将速度误差e_v(t)简化为e_v，则位置误差e_P＝P_d-P，速度误差e_v＝v_d-v，P_d，v_d分别为期望位置和期望速度；

整个位置控制算法采用串级PID进行设计，位置内回路为速度环，采用P控制，保证速度控制快速响应，由平移动力学特性加入非线性补偿项，记K_v，P为内回路比例控制算法参数，具体算法为：

a_d＝K_v，Pe_v-g

外回路同样采用P控制，记v′为虚拟控制输入，记K_P，P为外回路比例控制算法参数，具体算法为：

v′＝K_P，Pe_p

最后，记机体坐标系中的z坐标轴向量在惯性坐标系的投影向量为

由此计算出期望拉力：

S3：解算期望旋转矩阵；由步骤S2的位置控制算法输出的期望加速度解算出所需的旋转矩阵以实现姿态控制；

S4：设计姿态控制算法；根据步骤S3解算的期望旋转矩阵设计姿态误差矩阵，依据非线性动力学对姿态控制算法进行补偿，计算出所需期望力矩，完成姿态控制算法设计；

S5：调节控制算法参数；所述算法是针对姿态约束的场景，固定的控制增益不能控制所有高机动微型无人机飞行状态，需在飞行时利用增益调度的方法调节控制算法参数，保证多种飞行状态乃至姿态约束时高机动微型无人机的可控性。

2.根据权利要求1所述的一种考虑姿态约束的高机动微型无人机控制方法，其特征在于，所述步骤S3中解算期望旋转矩阵的步骤为：

S3-1：设期望的旋转矩阵为R_d＝[r_1，d r_2，d r_3，d]，r_1，d，r_2，d，r_3，d分别为期望旋转矩阵中的三个元素向量，其实质为期望机体坐标三个期望的坐标轴在惯性坐标系中的投影向量，由于采用北东地坐标系，期望旋转矩阵三个元素向量为机体坐标轴在地面坐标系的投影的反向量；

计算期望旋转矩阵第三个元素向量，利用平移动力学公式，即：

根据位置控制算法，将上述公式转为：

S3-2：设计中间向量为r₁′＝[-sinψ_d，cosψ_d，0]^T，ψ_d为期望偏航角，根据线面垂直定理证得：r₁′⊥x_b⊥z_b，x_b为机体坐标系中x坐标轴向量，z_b为机体坐标系中z坐标轴向量，得到期望旋转矩阵的第一个元素向量：

S3-3：由于旋转矩阵为正交矩阵，根据正交矩阵的性质得R_d＝[r_1，d，r_3，d×r_1，d，r_3，d]。

3.根据权利要求2所述的一种考虑姿态约束的高机动微型无人机控制方法，其特征在于，所述步骤S3中存在两个奇点，即：||a_d||＝0或||r₁×r_3，d||＝0，对于第一种奇点，对||a_d||加入限制，使得||a_d||＞0，则能够保证不会碰到第一种奇点；对于第二种奇点，包括奇异点处和奇异点两侧：

若姿态变换小于90°时，期望旋转矩阵按照步骤S3的方法解算；

若姿态变换大于90°时，引入负轴检测算法，即：

R_d＝[r_1，d，r_3，d×r_1，d，r_3，d]

从而完成姿态的转换，旋转之后和原有姿态有一定的连续性；

在奇点处||r₁′×r_3，d||＝0，不能解算期望旋转矩阵，r₁′加入额外的角度γ能够避免奇点的影响，即设计r₁′＝[-sinγsinψ_d，cosγcosψ_d，sinγ]^r。

4.根据权利要求2或3所述的一种考虑姿态约束的高机动微型无人机控制方法，其特征在于，所述步骤S4中设计姿态控制算法的步骤为：

依据旋转动力学设计姿态控制算法，其中，旋转运动模型为：

其中，τ为高机动微型无人机受到的力矩，J为高机动微型无人机转动惯量矩阵；

由上述模型知旋转运动包含姿态R信息、角速度ω信息和产生期望力矩τ_d指令；

首先选择状态变量和期望指令：

x₂＝[R ω]^T

u₂＝[τ_x τ_y τ_z]^T

其中，x₂为涉及旋转运动的状态变量，u₂为旋转运动的控制输入；姿态控制算法的目的是设计期望力矩τ_d，使得

和

可旋转矩阵误差e_R(t)简化为e_R，角速度误差e_ω(t)简化为e_ω，则旋转矩阵误差

角速度追踪误差定义为e_ω＝R^TR_dω_d-ω，其中，R_d和ω_d分别为期望旋转矩阵和期望的角速度，vex函数具有将斜对称矩阵转为列向量的作用，起到解算三个轴角作用；

整个姿态控制算法采用串级PID进行设计，姿态内回路为角速度环，采用PID控制，记K_ω，P，K_ω，I，K_ω，D分别为角速度环PID控制算法的比例参数，积分参数以及微分参数，具体算法为：

外回路采用P控制，记ω′为虚拟控制输入，K_R，P为控制算法的比例控制参数，具体算法为：ω′＝K_R，Pe_R。

Images