CN112241125B - 一种基于微分平坦特性的无人机轨迹跟踪方法 - Google Patents

Abstract

本发明公开了一种基于微分平坦特性的无人机轨迹跟踪方法，通过对四旋翼飞行器进行动力学与运动学建模，并对其模型进行微分平坦属性判定，基于此性质通过非线性变化，将欠驱动四旋翼模型转化为全驱动系统。设置虚拟控制量，将高度耦合系统解耦，并将其转化为线性系统。利用模型预测控制处理有约束系统的航迹跟踪控制问题，针对系统时变特性，构造多面体描述系统包含原时变系统，减少在线优化计算复杂度，并以此推导出使系统稳定的实时终端罚值。本发明设计了针对时变系统的模型预测控制方法，处理无人机轨迹跟踪过程中受到的约束与系统时变特性，能够有效的完成无人机轨迹跟踪任务，很好的处理无人机非线性特性，计算复杂度低，跟踪速度快。

Description

一种基于微分平坦特性的无人机轨迹跟踪方法

技术领域

本发明涉及高度耦合、欠驱动、时变、非线性系统自动控制技术领域，具体涉及一种基于微分平坦特性的无人机轨迹跟踪方法。

背景技术

四旋翼无人机以其无人驾驶、高机动性、小体积、垂直起降、低制造成本等优势迅速受到军用与民用的青睐。其最大优点为无人驾驶，不需要携带与飞行员有关的各种设备，其节省出来的大量的空间和重量可以用于布置更多作业载荷，实现完成更多的飞行任务，如军事地面战场的侦察、监视、情报获取、空中打击支持，民用航拍、危险地域搜救、复杂地形勘探等。复杂的飞行任务依靠的是先进的飞行控制方法，可靠的飞行控制方法可以保证无人机在执行任务时的安全性、稳定性、敏捷性，只有这样，无人机才能准确与及时的完成飞行任务。然而目前对于同时处理、协调具有高度耦合、欠驱动、时变、非线性、带约束的四旋翼无人机系统方面，并没有比较突出的方法，传统的方法或是计算复杂度高，或是进行了诸多假设以提高求解速度，在对无人机跟踪速度要求较高的任务中，很难得到应用。

发明内容

有鉴于此，本发明提供了一种基于微分平坦特性的无人机轨迹跟踪方法，能够降低计算复杂度，提高跟踪速度，适用于对无人机跟踪速度要求较高的任务。

为达到上述目的，本发明的技术方案包括以下步骤：

步骤一、针对无人机系统进行建模，并通过设置虚拟输入将无人机系统的垂直运动与水平运动解耦。

步骤二、根据无人机系统的微分平坦性质，将无人机系统从六自由度、四输入的欠驱动系统转换为四自由度、四输入的全驱动系统，并求解出姿态内环的角度期望值。

步骤三、根据步骤一解耦后的无人机系统作为实际系统，另外设置参考系统与误差系统，针对误差系统的水平运动通道进行基于模型预测控制的轨迹跟踪；针对误差系统的垂直运动通道，基于多面体微分包含理论，实时求解终端惩罚矩阵，保证系统稳定。

进一步地，步骤一，针对无人机系统进行建模，并通过设置虚拟输入将无人机系统的垂直运动与水平运动解耦，具体为：

首先针对无人机系统构建地面坐标系和机体坐标系；

地面坐标系固定于地球表面，坐标系原点O_E固定于地面上飞行器的起飞点，O_EX_E轴指向飞行器指定的飞行方向，O_EZ_E轴铅垂向下，O_EY_E轴垂直O_EX_EZ_E平面，按右手定则确定。

机体坐标系固定在无人机的机体上，并随无人机运动，原点O_B在无人机质心，O_BX_B轴在无人机对称平面内，平行于无人机前后旋翼的连接线，指向前；竖轴O_BZ_B平行于左右旋翼连线，指向右为正方向；O_BY_B与O_BX_BZ_B平面垂直，方向按照右手定则确定。

利用无人机的三个姿态角度俯仰角θ、偏航角φ和滚转角ψ计算地面坐标系与机体坐标系的转换矩阵R：

无人机为四旋翼飞行器，四旋翼飞行器是一个刚体且对称，在运动中其质量不变；地面坐标系为惯性坐标系，视地球表面为一平面，且重力加速度不随高度变化；不计入地球公转与自转对四旋翼飞行器的影响；四旋翼飞行器的形状与质量关于其几何对称轴对称；四旋翼飞行器旋翼电机所提供的升力与转速成正比，且不计旋翼的螺旋效应；四旋翼飞行器在运动过程中偏航角ψ＝0。

在机体坐标系下，四旋翼飞行器四个旋翼产生的升力为F_i(i∈{1,2,3,4})，F₁～F₄分别为第1～第4旋翼产生的升力，四个旋翼产生的力矩为M_i(i∈{1,2,3,4})，M₁～M₄分别为第1～第4旋翼产生的力矩；四旋翼飞行器升力U₁、横滚矩U₂、俯仰矩U₃均由四个旋翼升力提供，而偏航力矩U4由旋翼产生的力矩提供；设置无人机的四个输入分别为

其中l为四旋翼飞行器的基体坐标系中旋翼中心线到O_BY_B的距离。

根据牛顿第二定律与动量矩定律得到如下无人机系统模型

其中m是无人机的质量；g是重力加速度；r为无人机重心在地面坐标系下的位置，

为r的二阶导数；i_z＝[0 0 1]^T为方向向量；J为转动惯量矩阵；ω＝[p,q,r]为机体坐标系下无人机角速度，p、q、r分别为无人机角速度在机体坐标系下三轴分量，

为ω的一阶导数；τ为控制无人机的合外力矩。

将公式(3)、(4)展开为以下解析形式无人机动力学方程为

其中(x、y、z)为四旋翼飞行器机体在地面坐标系下的位置坐标；I_x、I_y、I_z分别为四旋翼飞行器机体相对于机体坐标系三轴的转动惯量。

定义第一虚拟控制量u_x和第二虚拟控制量u_y；

u_x＝cosψsinθcosφ+sinψsinφ (6)

u_y＝sinψsinθcosφ-cosψsinφ (7)

通过设置第一虚拟控制量u_x和第二虚拟控制量u_y，将无人机系统的垂直运动与水平运动解耦，得到四旋翼飞行器的动力学空间状态方程为：

进一步地，步骤二具体为：设置微分平坦输出包括四个，分别为第一输出Z₁＝x，第二输出Z₂＝y，第三输出Z₃＝z，第四输出Z₄＝ψ。

则U₁，u_x，u_y表示为

俯仰角θ、滚转角φ由微分平坦输出Z_i,i∈{1,2,3,4}及其有限阶导数表示

解得滚转角期望值φ_r和俯仰角期望值θ_r用于姿态内环的控制，具体表达式为

φ_r＝arcsin(u_r,xsinψ_r-u_r,ycosψ_r) (12)

其中ψ_r为偏航角期望值，其数值为0；u_r,x为水平运动通道内的第一虚拟参考输入，u_r,y为水平运动通道内的第二虚拟参考输入，u_r,z为垂直运动通道的参考输入。

进一步地，步骤三中，针对误差系统的水平运动通道采用模型预测控制方法进行轨迹跟踪，具体为：

设置水平运动通道内的状态向量为

水平运动通道内的输入量为u_xy＝[u_x,u_y]^T，u_x为第一虚拟控制量，u_y为第二虚拟控制量；水平运动通道内的参考轨迹为

x_r、y_r为参考轨迹中无人机机体在地面坐标系的位置坐标，

分别为x_r、y_r的一阶导数；水平运动通道内的参考输入为u_r,xy＝[u_r,x,u_r,y]^T；水平运动通道内的误差状态向量

水平运动通道内的输入误差量

则四旋翼飞行器水平运动通道的误差动力学方程，即水平通道误差系统具体为：

其中水平误差状态向量系数矩阵为

输入误差量系数矩阵为

利用零阶保持器将水平运动的误差动力学方程进行离散化，对于采样点k，得到水平运动通道离散化误差状态方程：

其中T_s为离散化过程中的采样时间；

为离散后的误差状态向量系数矩阵，

为离散后的输入误差量系数矩阵，

为水平运动通道内的误差状态向量的离散化后在采样点k处数值；

为水平运动通道内的输入误差量的离散化后在采样点k处数值；

利用模型预测控制方法对水平通道误差系统设计控制器，其控制策略为在每个采样时刻k通过求解优化控制问题，将解得的控制量输入系统；定义如下水平运动通道的优化代价函数：

其中N为预测时域；Q_xy为水平运动通道的状态权重矩阵；R_xy为水平运动通道的输入权重矩阵；P_xy为水平运动通道的终端惩罚矩阵；X_xy为水平运动通道状态集合；U_xy为水平运动通道输入集合；

Ω_xy为平衡点附近的邻域，

其中γ_xy决定终端域的大小；

表示在k时刻预测k+i时刻的水平运动通道内的误差状态向量；

表示在k时刻预测k+i时刻的水平运动通道内的输入误差量；

表示在终端域内水平运动通道的误差状态向量；ξ_r,xy(k)为k时刻水平运动通道的参考轨迹；u_r,xy(k)为k时刻水平运动通道内的参考输入。

进一步地，针对误差系统的垂直运动通道，基于多面体微分包含理论，实时求解终端惩罚矩阵，保证系统稳定，具体为：

设置垂直运动通道内的状态向量为

垂直运动通道内的输入量为u_z；垂直运动通道的参考轨迹为

z_r为参考轨迹中无人机机体在地面坐标系的高度坐标；垂直运动通道内的参考输入为u_r,z；垂直运动通道内的误差状态向量

水平运动通道内的输入误差量

针对四旋翼飞行器垂直运动通道的误差动力学方程，利用零阶保持器将垂直方向误差系统进行离散化，得到垂直方向离散化误差状态方程：

其中垂直方向误差状态向量系数矩阵为

垂直方向输入误差量系数矩阵为

θ(k)为k时刻的俯仰角，φ(k)为k时刻的滚转角；

分别为垂直方向的误差状态向量的离散化后在采样点k，k+1处数值；

为垂直方向的输入误差量的离散化后在采样点k处数值。

垂直方向误差系统为时变系统，利用模型预测控制方法对误差系统设计控制器，其控制策略为在每个采样时刻k通过求解优化控制问题，将解得的控制量输入系统；通过滚动优化的方式，实现对飞行器的控制；定义如下垂直运动通道的优化代价函数：

其中N为预测时域；Q_z为垂直运动通道的状态权重矩阵；R_z为垂直运动通道的输入权重矩阵；P_z为垂直运动通道的终端惩罚矩阵；X_z为垂直运动状态集合；U_z为垂直运动输入集合，U_z:＝{u_z||u_z|≤μ_z}。

Ω_z为平衡点邻域，

其中γ_z决定终端域的大小；ξ_z(k+i|k)表示在k时刻预测k+i时刻的状态量。

在z方向时变系统中存在凸包包含垂直方向输入误差量系数矩阵

即

定义

Co表示凸包，凸包C具有L个顶点，

为凸包的第i个顶点，i为整数，i∈[1,L]；

在每个采样时刻，通过对预测域的最后一步，求解垂直运动通道的终端惩罚矩阵P_z，确保在k>N时系统稳定；

P_z按照如下方式求解：

其中tr(P_z)为矩阵P_z的迹。

LMI_i(Y,W)为约束条件，

Y为第一指代值，Y＝P_z ^-1；W为第二指代值，W＝KP_z ^-1，K为线性状态反馈控制系数；Q_z为垂直运动通道的状态权重矩阵；R_z为垂直运动通道的输入权重矩阵；I为单位矩阵。

在求得终端惩罚矩阵P_z后，对垂直运动通道采用与水平运动通道相同的模型预测控制方法进行轨迹跟踪。

有益效果：

本发明提出了一种基于微分平坦特性的无人机轨迹跟踪方法，通过对四旋翼飞行器进行动力学与运动学建模，并对其模型进行微分平坦属性判定，基于此性质通过非线性变化，将欠驱动四旋翼模型转化为全驱动系统。设置虚拟控制量，将高度耦合系统解耦，并将其转化为线性系统。利用模型预测控制处理有约束系统的航迹跟踪控制问题，针对系统时变特性，构造多面体描述系统包含原时变系统，减少在线优化计算复杂度，并以此推导出使系统稳定的实时终端罚值。本发明设计了针对时变系统的模型预测控制方法，处理无人机轨迹跟踪过程中受到的约束与系统时变特性，为进一步降低求解复杂度，将模型预测控制转化为二次规划问题求解。本发明能够有效的完成无人机轨迹跟踪任务，很好的处理无人机非线性特性，计算复杂度低，工程实践中易于实现，跟踪速度快，仿真效果较好。

附图说明

图1为三维空间中的仿真轨迹与目标轨迹示意图；

图2为仿真轨迹与目标轨迹示意图；

图3为仿真实验中无人机姿态角曲线示意图；

图4为本发明实施例提供的一种基于微分平坦特性的无人机轨迹跟踪方法的流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明通过对四旋翼飞行器进行动力学与运动学建模，并对其模型进行微分平坦属性判定，基于此性质通过非线性变化，将欠驱动四旋翼模型转化为全驱动系统。设置虚拟控制量，将高度耦合系统解耦，并将其转化为线性系统。利用模型预测控制处理有约束系统的航迹跟踪控制问题，针对系统时变特性，构造多面体描述系统包含原时变系统，减少在线优化计算复杂度，并以此推导出使系统稳定的实时终端罚值。

本发明提供的一种基于微分平坦与模型预测控制的无人机轨迹跟踪控制方法，其流程如图4所示，包括如下步骤：

步骤一、无人机建模、解耦处理。无人机系统具有高耦合特征，通过设置虚拟输入将无人机系统各个通道解耦，便于之后分通道进行轨迹跟踪控制。

步骤二、微分平坦特性判定与平坦输出选择。设置微分平坦输出为Z₁＝x，Z₂＝y，Z₃＝z，Z₄＝ψ。通过推导可知姿态角φ和θ、系统其他状态量x,y,z,ψ与输入U₁,u_x,u_y,U₄可以由平坦输出Z_i(i＝1,2,3,4)及其有限阶导数表示。通过微分平坦性质证明，可将系统从六自由度，四输入的欠驱动系统转换为四自由度，四输入的全驱动系统。同时，依据此性质求解出姿态内环的角度期望值。

步骤三、构造四旋翼轨迹跟踪问题。根据步骤一解耦后的系统，另外设置参考系统与误差系统，针对误差系统进行基于模型预测控制的轨迹跟踪。针对垂直运动通道的时变特性，基于多面体微分包含理论，实时求解终端惩罚矩阵，保证系统稳定。

本发明实施例中，步骤一具体为，首先对飞行器系统运行的坐标系进行定义，其中O_E为地面坐标系，O_B为机体坐标系。地面坐标系固定于地球表面，坐标系原点固定于地面上飞行器的起飞点，O_EX_E轴指向飞行器指定的飞行方向，O_EZ_E轴铅垂向下，O_EY_E轴垂直O_EX_EZ_E平面，按右手定则确定。机体坐标系固定在机体上，并随飞行器运动，原点在飞行器质心，O_BX_B轴在飞行器对称平面内，平行于前后旋翼的连接线，指向前；竖轴O_BZ_B平行于左右旋翼连线，指向右为正方向；O_BY_B与O_BX_BZ_B平面垂直，方向按照右手定则确定。θ,φ,ψ分别表示俯仰角、滚转角、偏航角，利用三个姿态角度可以将地面坐标系与机体坐标系联系起来，转换矩阵表达式为：

在这里做出以下假设：

1.四旋翼飞行器是一个刚体且对称，在运动中其质量不变；

2.地面坐标系为惯性坐标系，视地球表面为一平面，且重力加速度不随高度的变化而变化；

3.不计入地球公转与自转对飞行器的影响；

4.飞行器的形状与质量关于其几何对称轴对称；

5.飞行器旋翼电机所提供的升力与转速成正比F＝kω²，且不计旋翼的螺旋效应；

6.飞行器在运动过程中偏航角ψ＝0。

在机体坐标系下，四旋翼飞行器四个旋翼产生的升力为F_i(i∈{1,2,3,4})，F₁～F₄分别为第1～第4旋翼产生的升力，四个旋翼产生的力矩为M_i(i∈{1,2,3,4})，M₁～M₄分别为第1～第4旋翼产生的力矩；四旋翼飞行器升力U₁、横滚矩U₂、俯仰矩U₃均由四个旋翼升力提供，而偏航力矩U4由旋翼产生的力矩提供；设置无人机的四个输入分别为：

其中l为四旋翼飞行器的基体坐标系中，旋翼中心线到O_BY_B的距离；

根据牛顿第二定律与动量矩定律得到如下无人机系统模型

其中m是无人机的质量；g是重力加速度；r为无人机重心在地面坐标系下的位置，

为r的二阶导数；i_z＝[0 0 1]^T为方向向量；J为转动惯量矩阵；ω＝[p,q,r]为机体坐标系下无人机角速度，p、q、r分别为无人机角速度在机体坐标系下三轴分量，

为ω的一阶导数；τ为控制无人机的合外力矩。

可将式(3)、(4)展开为以下解析形式飞行器动力学方程为

其中(x、y、z)为四旋翼飞行器机体在地面坐标系下的位置坐标；I_x、I_y、I_z分别为四旋翼飞行器机体相对于机体坐标系三轴的转动惯量。

定义第一虚拟控制量u_x和第二虚拟控制量u_y；

u_x＝cosψsinθcosφ+sinψsinφ (6)

u_y＝sinψsinθcosφ-cosψsinφ (7)

通过设置第一虚拟控制量u_x和第二虚拟控制量u_y，将无人机系统的垂直运动与水平运动解耦，得到四旋翼飞行器的动力学空间状态方程为：

通过设置虚拟控制量，将飞行器垂直运动与水平运动解耦，便于第三步中基于模型预测控制的轨迹跟踪计算，并减少对时变系统的在线计算复杂度。

本发明实施例中，步骤二微分平坦性质证明具体为：设置微分平坦输出包括四个，分别为第一输出Z₁＝x，第二输出Z₂＝y，第三输出Z₃＝z，第四输出Z₄＝ψ；

则U₁，u_x，u_y表示为

俯仰角θ、滚转角φ由微分平坦输出Z_i,i∈{1,2,3,4}及其有限阶导数表示

根据求得的角度表达式并带入系统方程可求解出所有用平坦输出表示的系统输入，至此飞行器的所有状态量与输入量均可由微分平坦输出表示出来，飞行器具有微分平坦特性。原有的飞行器运动学方程有6个自由度，4个控制输入，为欠驱动系统，且原有飞行器动力学方程水平运动与垂直运动高度耦合。利用飞行器微分平坦特征并设置虚拟控制量，将原系统变为4个自由度，4个控制输入的全驱动系统，并解除飞行器水平运动与垂直运动之间的耦合关系，从而简化其控制器设计。

依据此特性处理虚拟输入，解得姿态角φ_r和θ_r用于姿态内环的控制，具体表达式为

φ_r＝arcsin(u_r,xsinψ_r-u_r,ycosψ_r) (12)

其中ψ_r为偏航角期望值，其数值为0；u_r,x为水平运动通道内的第一虚拟参考输入，u_r,y为水平运动通道内的第二虚拟参考输入，u_r,z为垂直运动通道的参考输入。

进一步，步骤三描述的无人机在进行轨迹跟踪的过程中，会受到诸多状态约束与输入约束限制，有约束的轨迹跟踪问题，本质上就是一个最优控制问题，但其解析解非常难以求得，而模型预测控制能很好的处理这些带约束的轨迹跟踪问题。

设置水平运动通道内的状态向量为

水平运动通道内的输入量为u_xy＝[u_x,u_y]^T，u_x为第一虚拟控制量，u_y为第二虚拟控制量；水平运动通道内的参考轨迹为

x_r、y_r为参考轨迹中无人机机体在地面坐标系的位置坐标，

分别为x_r、y_r的一阶导数；水平运动通道内的参考输入为u_r,xy＝[u_r,x,u_r,y]^T；水平运动通道内的误差状态向量

水平运动通道内的输入误差量

则四旋翼飞行器水平运动通道的误差动力学方程，即水平通道误差系统具体为：

其中水平误差状态向量系数矩阵为

输入误差量系数矩阵为

利用零阶保持器将水平运动的误差动力学方程进行离散化，对于采样点k，得到水平运动通道离散化误差状态方程：

其中T_s为离散化过程中的采样时间；

为离散后的误差状态向量系数矩阵，

为离散后的输入误差量系数矩阵，

为水平运动通道内的误差状态向量的离散化后在采样点k处数值；

为水平运动通道内的输入误差量的离散化后在采样点k处数值。

可见，飞行器水平方向为时不变系统，可以利用模型预测控制方法对误差系统设计控制器，其控制策略为在每个采样时刻k通过求解优化控制问题，将解得的控制量输入系统。通过滚动优化的方式，实现对飞行器的控制。定义优化代价函数：

其中N为预测时域；Q_xy为水平运动通道的状态权重矩阵；R_xy为水平运动通道的输入权重矩阵；P_xy为水平运动通道的终端惩罚矩阵；X_xy为水平运动状态集合；U_xy为水平运动输入集合；Ω_xy为平衡点附近的邻域，

其中γ_xy决定终端域的大小；

表示在k时刻预测k+i时刻的水平运动通道内的误差状态向量；

表示在k时刻预测k+i时刻的水平运动通道内的输入误差量；

表示在终端域内水平运动通道的误差状态向量；ξ_r,xy(k)为k时刻水平运动通道的参考轨迹；u_r,xy(k)为k时刻水平运动通道内的参考输入。

同理，设置垂直运动通道内的状态向量为

垂直运动通道内的输入量为u_z；垂直运动通道的参考轨迹为

z_r为参考轨迹中无人机机体在地面坐标系的高度坐标；垂直运动通道内的参考输入为u_r,z；垂直运动通道内的误差状态向量

水平运动通道内的输入误差量

针对四旋翼飞行器垂直运动通道的误差动力学方程，利用零阶保持器将垂直方向误差系统进行离散化，得到垂直方向离散化误差状态方程：

其中垂直方向误差状态向量系数矩阵为

垂直方向输入误差量系数矩阵为

θ(k)为k时刻的俯仰角，φ(k)为k时刻的滚转角；

分别为垂直方向的误差状态向量的离散化后在采样点k，k+1处数值；

为垂直方向的输入误差量的离散化后在采样点k处数值；

即垂直方向误差系统为时变系统，利用模型预测控制方法对误差系统设计控制器，其控制策略为在每个采样时刻k通过求解优化控制问题，将解得的控制量输入系统；通过滚动优化的方式，实现对飞行器的控制；定义如下垂直运动通道的优化代价函数：

其中N为预测时域；Q_z为垂直运动通道的状态权重矩阵；R_z为垂直运动通道的输入权重矩阵；P_z为垂直运动通道的终端惩罚矩阵；X_z为垂直运动状态集合；U_z为垂直运动输入集合，U_z:＝{u_z||u_z|≤μ_z}。

Ω_z为平衡点邻域，

其中γ_z决定终端域的大小；ξ_z(k+i|k)表示在k时刻预测k+i时刻的状态量。

垂直方向的时变特性导致在模型预测控制中终端域与终端惩罚矩阵无法通过离线求解方式得到。本方法构造多面体描述系统包含原时变系统，保证系统状态始终保持在多面体系统构造的不变集内，以此降低在线求解的复杂度。具体为，在每个采样时刻，通过对预测域的最后一步，选取合适的加权矩阵P_z，确保在k>N时系统稳定。

为此，假设存在局部控制器

使得下面条件成立

1.

2.

有

3.对于所有

终端代价函数

满足Hamilton-Jacobi-Bellman不等式

引入线性微分包含理论：考虑函数y＝H(x)满足H(0)＝0，且y＝H(x)在闭凸集Ψ内连续且偏导存在，则对于闭凸集的任意一点a∈Ψ，存在向量

使得

其中

凸包Θ∈Co{ζ₁,ζ₂,…,ζ_L}，L为凸包顶点的个数。

基于此，在z方向时变系统中存在凸包包含垂直方向输入误差量系数矩阵

即

定义

Co表示凸包，凸包C具有L个顶点，

为凸包的第i个顶点，i为整数，i∈[1,L]。

则式(19)可以写成

设有线性状态反馈控制器

不等式可以写成线性微分包含的形式

令Y＝P_z ^-1 W＝KP_z ^-1

上面最优问题变为线性矩阵不等式问题，约束条件为

由上述中假设有线性状态反馈控制器

输入约束相应的可以写为

将上式写成线性矩阵不等式的形式为

综上，对终端域的求解变为对P_z的迹的求解，即P_z按照如下方式求解：

其中tr(P_z)为矩阵P_z的迹；

LMI_i(Y,W)为约束条件，

Y为第一指代值，Y＝P_z ^-1；W为第二指代值，W＝KP_z ^-1，K为线性状态反馈控制系数；Q_z为垂直运动通道的状态权重矩阵；R_z为垂直运动通道的输入权重矩阵；I为单位矩阵。

在求得终端惩罚矩阵P_z后，对垂直运动通道采用与水平运动通道相同的模型预测控制方法进行轨迹跟踪。

为进一步降低求解复杂度，本方法将继续对前述模型预测控制进行进一步优化，以垂直方向误差系统为例，设置增广状态向量

上述有约束的优化问题转化为二次规划问题

由于垂直方向为时变系统，所以二次规划问题的矩阵H(k),f(k)也为时间的函数，需要在线实时求解。

下面对本发明的具体实施方式进行详细说明

步骤一，无人机建模，根据牛顿第二定律与动量矩定律可以得到如下系统模型

这里选择的参数如下：

m＝1kg,J＝diag[0.05,0.05,0.1],g＝9.8m/s²

步骤二，微分平坦性质证明，将微分平坦输出设置为Z₁＝x，Z₂＝y，Z₃＝z，Z₄＝ψ，将系统状态量与输入量用微分平坦输出及其导数表示。

步骤三，构造四旋翼轨迹跟踪问题。无人机初始位置设置为(0,0,0)^T，初始姿态设置为(0,0,0)^T，设置目标为z_r＝5，半径R＝2的圆形轨迹。设置采样时间T_s＝0.05s，预测时域N＝20。专利提供的方法具体为

1.轨迹生成，包括生成目标位置与目标速度。

2.计算飞行器实时位置与目标轨迹的差值，将此差值作为模型预测控制的初值，计算预测时域内的预测状态值，并将计算出的第一个输入与实时的参考系统输入相加，得到实际系统的输入U₁,u_x,u_y,U₄，参考系统输入可以应用无人机微分平坦特性计算得到。

3.通过专利中提供的公式求解出内环姿态控制器的输入θ_r,φ_r,ψ_r，通过内环控制器求解出U₁,U₂,U₃,U₄控制无人机运动。

4.将无人机的状态反馈至位置外环与姿态内环进行下一步计算。

通过本发明的具体实例可以看出，本发明可以很好的处理无人机系统高度耦合、欠驱动、时变、非线性的特点，根据图1、图2的轨迹跟踪结果可以看出，该方法能够在满足约束情况下，无人机的跟踪轨迹平滑、连续，跟踪速度较快。根据图3的轨迹跟踪结果可以看出无人机姿态稳定。

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

Claims (3)

1.一种基于微分平坦特性的无人机轨迹跟踪方法，其特征在于，包括以下步骤：

步骤一、针对无人机系统进行建模，并通过设置虚拟输入将无人机系统的垂直运动与水平运动解耦；

步骤二、根据无人机系统的微分平坦性质，将无人机系统从六自由度、四输入的欠驱动系统转换为四自由度、四输入的全驱动系统，并求解出姿态内环的角度期望值；

步骤三、根据步骤一解耦后的无人机系统作为实际系统，另外设置参考系统与误差系统，针对误差系统的水平运动通道进行基于模型预测控制的轨迹跟踪；针对误差系统的垂直运动通道，基于多面体微分包含理论，实时求解终端惩罚矩阵，保证系统稳定；

所述步骤一，针对无人机系统进行建模，并通过设置虚拟输入将无人机系统的垂直运动与水平运动解耦，具体为：

首先针对无人机系统构建地面坐标系和机体坐标系；

所述地面坐标系固定于地球表面，坐标系原点O_E固定于地面上飞行器的起飞点，O_EX_E轴指向飞行器指定的飞行方向，O_EZ_E轴铅垂向下，O_EY_E轴垂直O_EX_EZ_E平面，按右手定则确定；

机体坐标系固定在无人机的机体上，并随无人机运动，原点O_B在无人机质心，O_BX_B轴在无人机对称平面内，平行于无人机前后旋翼的连接线，指向前；竖轴O_BZ_B平行于左右旋翼连线，指向右为正方向；O_BY_B与O_BX_BZ_B平面垂直，方向按照右手定则确定；

利用无人机的三个姿态角度俯仰角θ、滚转角φ和偏航角ψ计算地面坐标系与机体坐标系的转换矩阵R：

所述无人机为四旋翼飞行器，所述四旋翼飞行器是一个刚体且对称，在运动中其质量不变；所述地面坐标系为惯性坐标系，视地球表面为一平面，且重力加速度不随高度变化；不计入地球公转与自转对所述四旋翼飞行器的影响；四旋翼飞行器的形状与质量关于其几何对称轴对称；四旋翼飞行器旋翼电机所提供的升力与转速成正比，且不计旋翼的螺旋效应；四旋翼飞行器在运动过程中偏航角ψ＝0；

在机体坐标系下，所述四旋翼飞行器四个旋翼产生的升力为F_i,i∈{1,2,3,4}，F₁～F₄分别为第1～第4旋翼产生的升力，四个旋翼产生的力矩为M_i,i∈{1,2,3,4}，M₁～M₄分别为第1～第4旋翼产生的力矩；所述四旋翼飞行器升力U₁、横滚矩U₂、俯仰矩U₃均由四个旋翼升力提供，而偏航力矩U₄由旋翼产生的力矩提供；设置无人机的四个输入分别为

其中l为四旋翼飞行器的基体坐标系中，旋翼中心线到O_BY_B的距离；

根据牛顿第二定律与动量矩定律得到如下无人机系统模型

其中m是无人机的质量；g是重力加速度；r为无人机重心在地面坐标系下的位置，

为r的二阶导数；i_z＝[0 0 1]^T为方向向量；J为转动惯量矩阵；ω＝[p,q,r]为机体坐标系下无人机角速度，p、q、r分别为无人机角速度在机体坐标系下三轴分量，

为ω的一阶导数；τ为控制无人机的合外力矩；

将公式(3)、(4)展开为以下解析形式无人机动力学方程为

其中(x、y、z)为四旋翼飞行器机体在地面坐标系下的位置坐标；I_x、I_y、I_z分别为四旋翼飞行器机体相对于机体坐标系三轴的转动惯量；

定义第一虚拟控制量u_x和第二虚拟控制量u_y；

u_x＝cosψsinθcosφ+sinψsinφ (6)

u_y＝sinψsinθcosφ-cosψsinφ (7)

通过设置第一虚拟控制量u_x和第二虚拟控制量u_y，将无人机系统的垂直运动与水平运动解耦，得到四旋翼飞行器的动力学空间状态方程为：

所述步骤二具体为：设置微分平坦输出包括四个，分别为第一输出Z₁＝x，第二输出Z₂＝y，第三输出Z₃＝z，第四输出Z₄＝ψ；

则U₁，u_x，u_y表示为

俯仰角θ、滚转角φ由微分平坦输出Z_i,i∈{1,2,3,4}及其有限阶导数表示

解得滚转角期望值φ_r和俯仰角期望值θ_r用于姿态内环的控制，具体表达式为

φ_r＝arcsin(u_r,xsinψ_r-u_r,ycosψ_r) (12)

其中ψ_r为偏航角期望值，其数值为0；u_r,x为水平运动通道内的第一虚拟参考输入，u_r,y为水平运动通道内的第二虚拟参考输入，u_r,z为垂直运动通道的参考输入。

2.如权利要求1所述的方法，其特征在于，所述步骤三中，所述针对误差系统的水平运动通道采用模型预测控制方法进行轨迹跟踪，具体为：

设置水平运动通道内的状态向量为

水平运动通道内的输入量为u_xy＝[u_x,u_y]^T，u_x为第一虚拟控制量，u_y为第二虚拟控制量；水平运动通道内的参考轨迹为

x_r、y_r为参考轨迹中无人机机体在地面坐标系的位置坐标，

分别为x_r、y_r的一阶导数；水平运动通道内的参考输入为u_r,xy＝[u_r,x,u_r,y]^T；水平运动通道内的误差状态向量

水平运动通道内的输入误差量

则四旋翼飞行器水平运动通道的误差动力学方程，即水平通道误差系统具体为：

其中水平误差状态向量系数矩阵为

输入误差量系数矩阵为

利用零阶保持器将所述水平运动的误差动力学方程进行离散化，对于采样点k，得到水平运动通道离散化误差状态方程：

其中T_s为离散化过程中的采样时间；

为离散后的误差状态向量系数矩阵，

为离散后的输入误差量系数矩阵，

为水平运动通道内的误差状态向量的离散化后在采样点k处数值；

为水平运动通道内的输入误差量的离散化后在采样点k处数值；

利用模型预测控制方法对水平通道误差系统设计控制器，其控制策略为在每个采样时刻k通过求解优化控制问题，将解得的控制量输入系统；定义如下水平运动通道的优化代价函数：

其中N为预测时域；Q_xy为水平运动通道的状态权重矩阵；R_xy为水平运动通道的输入权重矩阵；P_xy为水平运动通道的终端惩罚矩阵；X_xy为水平运动通道状态集合；U_xy为水平运动通道输入集合；

Ω_xy为平衡点附近的邻域，

其中γ_xy决定终端域的大小；

表示在k时刻预测k+i时刻的水平运动通道内的误差状态向量；

表示在k时刻预测k+i时刻的水平运动通道内的输入误差量；

表示在终端域内水平运动通道的误差状态向量；ξ_r,xy(k)为k时刻水平运动通道的参考轨迹；u_r,xy(k)为k时刻水平运动通道内的参考输入。

3.如权利要求2所述的方法，其特征在于，所述针对误差系统的垂直运动通道，基于多面体微分包含理论，实时求解终端惩罚矩阵，保证系统稳定，具体为：

设置垂直运动通道内的状态向量为

垂直运动通道内的输入量为u_z；垂直运动通道的参考轨迹为

z_r为参考轨迹中无人机机体在地面坐标系的高度坐标；垂直运动通道内的参考输入为u_r,z；垂直运动通道内的误差状态向量

垂直运动通道内的输入误差量

针对四旋翼飞行器垂直运动通道的误差动力学方程，利用零阶保持器将垂直方向误差系统进行离散化，得到垂直方向离散化误差状态方程：

其中垂直方向误差状态向量系数矩阵为

垂直方向输入误差量系数矩阵为

θ(k)为k时刻的俯仰角，φ(k)为k时刻的滚转角；

分别为垂直方向的误差状态向量的离散化后在采样点k，k+1处数值；

为垂直方向的输入误差量的离散化后在采样点k处数值；

垂直方向误差系统为时变系统，利用模型预测控制方法对误差系统设计控制器，其控制策略为在每个采样时刻k通过求解优化控制问题，将解得的控制量输入系统；通过滚动优化的方式，实现对飞行器的控制；定义如下垂直运动通道的优化代价函数：

其中N为预测时域；Q_z为垂直运动通道的状态权重矩阵；R_z为垂直运动通道的输入权重矩阵；P_z为垂直运动通道的终端惩罚矩阵；X_z为垂直运动状态集合；U_z为垂直运动输入集合，U_z:＝{u_z||u_z|≤μ_z}；

Ω_z为平衡点邻域，

其中γ_z决定终端域的大小；ξ_z(k+i|k)表示在k时刻预测k+i时刻的状态量；

在z方向时变系统中存在凸包包含垂直方向输入误差量系数矩阵

即

定义

Co表示凸包，凸包C具有L个顶点，

为凸包的第i个顶点，i为整数，i∈[1,L]；

在每个采样时刻，通过对预测域的最后一步，求解垂直运动通道的终端惩罚加权矩阵P_z，确保在k>N时系统稳定；

P_z按照如下方式求解：

其中tr(P_z)为矩阵P_z的迹；

LMI_i(Y,W)为约束条件，

Y为第一指代值，

W为第二指代值，

K为线性状态反馈控制系数；Q_z为垂直运动通道的状态权重矩阵；R_z为垂直运动通道的输入权重矩阵；I为单位矩阵；

在求得垂直运动通道的终端惩罚矩阵P_z后，对垂直运动通道采用与水平运动通道相同的模型预测控制方法进行轨迹跟踪。

Images