CN108845588B - 一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法 - Google Patents

Abstract

本发明提供一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法，属于飞行器控制技术领域。本发明首先对四旋翼飞行器建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系；然后计算四旋翼飞行器跟踪轨迹所需的高度、期望的航向角、期望的俯仰角、期望的侧向加速度；四旋翼飞行器的高度控制器获取跟踪轨迹所需的高度，姿态角控制器获取期望航向角，俯仰角控制器获取期望的俯仰角，滚转角控制器获取期望的侧向加速度，最后四旋翼飞行器在高度控制器、姿态角控制器、俯仰角控制器以及滚转角控制器的控制下按照预定轨迹飞行。本发明解决了现有四旋翼飞行器跟踪控制无法保证匀速飞行、响应延时大的问题。本发明可用于四旋翼飞行器轨迹跟踪控制。

Description

一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法

技术领域

本发明涉及一种四旋翼飞行器轨迹跟踪控制方法，属于飞行器控制技术领域。

背景技术

四旋翼飞行器具有垂直起降、悬停和机动飞行的特性，特别适合室内狭小空间和城市复杂环境执行侦察和监视任务。四旋翼飞行器是一个多变量非线性系统，动力学模型较为复杂，国内外许多高校与科研机构对其进行了广泛深入的研究。

四旋翼飞行器在执行任务的过程中，需要跟踪预先规划好的飞行轨迹，现有方法是将飞行路径离散为位置跟踪点，然后针对四旋翼飞行器设计相应的位置控制器，跟踪离散后的位置点。具体步骤为：

步骤1：对四旋翼飞行器进行姿态动力学建模和位置动力学建模，对动力学模型进行解耦，并通过线性化得到四旋翼飞行器的传递函数；建立控制对象的六自由度仿真模型，包括姿态动力学建模、位置动力学建模、执行机构建模以及模型的线性化，四旋翼无人机参考系示意图如图1所示。在对四旋翼无人机的动力学建模时，需要进行以下两条假设：

(1)将四旋翼无人机看作是刚体，认为其不发生弹性形变，并且重心位置不变，质量也不变。

(2)四旋翼一般飞行高度为相对地面几十米的距离，因此可以忽略地球曲率，以及地球的自转和公转的因素，可以将所在地面视作平面。

步骤2：分别设计姿态环串级PID控制器和位置环串级PID控制器；以姿态环为例形式如图2所示，串级PID内外两环并联调节，这样的好处的是增加系统的稳定性，抗干扰。串级PID控制器的缺点也是显而易见的，和直接对内环进行控制相比较会延长响应时间。串级PID就是将控制系统分解为内环和外环两个单级PID控制器，它增强了系统的抗干扰性能(也就是增强稳定性)，对于旋翼飞行器而言相当于抵消了角速度和速度的干扰。因为有两个控制器控制飞行器，它会比单个控制器控制更多的变量，使得飞行器的适应能力更强。在整定串级PID时的经验则是：先整定内环参数，再整定外环参数。因为内环靠近输出，效果直接。

步骤3：根据特定的飞行任务设计四旋翼飞行器的飞行路线，并将航迹按照一定规则离散化为一系列的位置指令；

步骤4：将当前位置点和位置指令输入到四旋翼飞行器位置控制器中，使四旋翼飞行器跟踪离散化后的位置指令。

上述方法首先对四旋翼飞行器进行建模，并且分别设计位置环和姿态环PID控制器，然后给四旋翼飞行器的位置控制器输入一系列航迹点，飞行器逐个点跟踪来逼近整条轨迹，但是在跟踪过程中无法保证速度为期望值。如果输入的航迹点与当前位置点相距较大，则无法保证两点之间的飞行器轨迹为一条直线，而且由于控制特性，在接近目标位置点时会减速；如果给入的航迹点与当前位置点相距较小，飞行器到达每个航迹点时，会产生停顿，速度降为零，反复的加减速会大大降低轨迹跟踪品质，加长了跟踪时间；

综上可知，现有方法的缺陷在于离散的规则不容易选取，而且无法保证四旋翼飞行器匀速飞行，加入位置控制器后响应延时也会变大，最终导致飞行品质不高。

发明内容

本发明为解决现有四旋翼飞行器跟踪控制无法保证匀速飞行、响应延时大的问题，提供了一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法。

本发明所述一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法，通过以下技术方案实现：

步骤一、对四旋翼飞行器建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系OXYZ；

步骤二、根据几何关系计算四旋翼飞行器跟踪轨迹所需的高度；

步骤三、将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内，在期望路径的投影上选择一个虚拟跟踪点，并且利用虚拟跟踪点位置坐标计算：四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角、期望的航向角；

步骤四、根据四旋翼飞行器的期望的恒定飞行速度生成四旋翼飞行器的期望的俯仰角；并结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角，计算期望的侧向加速度；

步骤五、四旋翼飞行器的高度控制器获取跟踪轨迹所需的高度，姿态角控制器获取期望的航向角，俯仰角控制器获取期望的俯仰角，滚转角控制器获取期望的侧向加速度，四旋翼飞行器在高度控制器、姿态角控制器、俯仰角控制器以及滚转角控制器的控制下按照预定轨迹飞行。

作为对上述技术方案的进一步阐述：

进一步的，步骤一中所述建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系的具体过程包括：

对四旋翼飞行器建立直线路径坐标系o_px_py_pz_p、圆弧路径极坐标系C_ρN_ρP_ρ以及惯性坐标系OXYZ，定义直线路径坐标系o_px_py_pz_p的坐标原点为直线路径起点，o_px_p轴指向直线路径方向，o_pz_p轴指向和惯性坐标系OZ轴指向相同，o_py_p轴、o_px_p轴、o_pz_p轴构成右手坐标系；从惯性坐标系OXYZ到直线路径坐标系o_px_py_pz_p的转换矩阵为R_i ^p：

其中，χ_q为当前期望的直线路径方向向量的偏航角；

圆弧路径极坐标系的N_ρ轴指向地理坐标系的正北方向，圆弧路径极坐标系的P_ρ轴方向为当前圆弧路径的圆心指向四旋翼飞行器的方向；惯性坐标系的X轴、Y轴、Z轴分别指向地理坐标系下的北方、东方、地心方向。

进一步的，步骤二中所述计算四旋翼飞行器跟踪轨迹所需的高度的具体过程包括：

A1、当跟踪轨迹为直线路径时：

四旋翼飞行器的位置相对直线路径的位置相对偏差e_p在o_px_py_pz_p坐标系下表示为：

其中，e_px、e_py、e_pz分别表示e_p在o_px_py_pz_p坐标系下的x_p轴方向的分量、y_p轴方向的分量、z_p轴方向的分量，r为四旋翼飞行器期望位置向量，p为四旋翼飞行器当前位置向量；

将相对偏差e_p投影到包含直线路径方向向量的惯性坐标系下YOZ平面内，得到相对偏差的投影s：

其中，s_n、s_e、s_d分别为惯性坐标系下s在X轴方向的分量、Y轴方向的分量、Z轴方向的分量；

再结合直线路径方向向量q＝(q_n,q_e,q_d)，得到：

其中，q_n、q_e、q_d分别为惯性坐标系下q在X轴方向的分量、Y轴方向的分量、Z轴方向的分量；

得到跟踪轨迹为直线路径时，四旋翼飞行器跟踪轨迹所需的高度h为：

其中，r_d为惯性坐标系下r在Z轴方向的分量；

A2、当跟踪轨迹为圆弧路径时：

圆弧路径的圆心坐标在惯性坐标系下为c＝(c_n,c_e,c_d)^T，则四旋翼飞行器跟踪轨迹所需的高度h为：

h＝-c_d

其中，c_n、c_e、c_d分别表示惯性坐标系下c的X轴、Y轴、Z轴坐标。

进一步的，步骤二中所述四旋翼飞行器期望位置向量r和四旋翼飞行器当前位置向量p具体为：

其中，p_n、p_e、p_d分别为惯性坐标系下p在X轴方向的分量、Y轴方向的分量、Z轴方向的分量，r_n、r_e分别为惯性坐标系下r在X轴方向的分量、Y轴方向的分量。

进一步的，步骤三的具体过程包括：

将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内，在期望路径的投影上选择一个距离四旋翼飞行器L₁的虚拟跟踪点T，计算虚拟跟踪点位置坐标T(x_t,y_t)，并且利用虚拟跟踪点位置坐标T(x_t,y_t)计算：四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η、期望的航向角χ_cmd；x_t、y_t分别表示惯性坐标系下T的X轴坐标、Y轴坐标；

B1、当跟踪轨迹为直线路径时：

当前期望的直线路径方向向量的偏航角χ_q和四旋翼飞行器当前速度矢量

的偏航角χ由下式得出：

其中，v_e表示惯性坐标系的Y轴方向的分量，v_n惯性坐标系的X轴方向的分量；

计算e_p在o_px_py_pz_p坐标系下y_p轴方向的分量e_py：

e_py＝-sin(χ_q)·(p_n-r_n)+cos(χ_q)·(p_e-r_e)

然后根据几何关系能够得到虚拟跟踪点T的位置坐标：

其中，

表示四旋翼飞行器当前位置向量p与四旋翼飞行器期望位置向量r的向量差；

再结合当前速度矢量

的偏航角χ能够得到η：

利用虚拟跟踪点位置坐标计算期望的航向角χ_cmd：

B2、当跟踪轨迹为圆弧路径时：

虚拟跟踪点T的位置坐标计算公式为：

其中，

表示四旋翼飞行器相对圆弧路径的角位置，ρ为圆弧路径的半径，λ为圆弧方向，λ∈{-1,1}，当λ＝-1时表示圆弧路径是逆时针，当λ＝1时表示圆弧路径为顺时针；

为圆心c到虚拟跟踪点T的方向向量与圆心c到四旋翼飞行器当前位置p的方向向量的夹角；

四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η为：

利用虚拟跟踪点位置坐标计算期望的航向角χ_cmd：

进一步的，步骤四的具体过程包括：

将四旋翼飞行器的期望的恒定飞行速度V_a*转化为期望的俯仰角θ_cmd；

然后结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η，通过以下公式计算期望的侧向加速度a_scmd：

L₁＝2Rsinη

其中，V_g是四旋翼飞行器当前飞行速度，R是对应当前侧向加速度等效的转弯半径。

进一步的，步骤三中所述

的计算具体为：

其中，d表示四旋翼飞行器当前位置到圆弧路径的圆心c的距离。

本发明最为突出的特点和显著的有益效果是：

本发明所涉及的一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法，可以快速精确的响应姿态指令，然后利用非线性制导方法将期望轨迹转化为四旋翼飞行器跟踪轨迹所需的高度和期望的直线路径方向向量的偏航角，结合期望的恒定飞行速度，最终分别转化为期望的侧向加速度、期望的航向角以及期望的俯仰角。通过非线性制导方法直接给出内环控制器指令，相比于外环指令，响应速度快，延迟降低约10％；四旋翼飞行器在飞行过程中控制速度大小不变，与传统方法相比较可以大大缩短跟踪轨迹的飞行时间，而且由于速度值不变，飞行品质较好，跟踪轨迹较为平滑。可以有效的跟踪直线和圆弧轨迹，大大提高了飞行品质。

附图说明

图1为四旋翼飞行器参考系示意图；

图2为姿态环串级PID控制器原理示意图，ω_qw为期望姿态角，ω_dq为当前姿态角，θ_qw为期望角速度，θ_dq为当前角速度；

图3为本发明恒定飞行速度控制方式示意图；

图4为本发明进行垂直平面的投影示意图；

图5为本发明垂直平面直线轨迹跟踪示意图；

图6为本发明直线虚拟点选取原理示意图；

图7为本发明圆弧路径跟踪示意图；

图8为本发明轨迹跟踪制导逻辑示意图。

具体实施方式

具体实施方式一：结合图3对本实施方式进行说明，本实施方式给出的一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法，具体包括以下步骤：

步骤一、对四旋翼飞行器建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系OXYZ；

步骤二、根据几何关系计算四旋翼飞行器跟踪轨迹所需的高度；

步骤三、将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内，在期望路径的投影上选择一个虚拟跟踪点，并且利用虚拟跟踪点位置坐标计算：四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角、期望的航向角；

步骤四、根据四旋翼飞行器的期望的恒定飞行速度生成四旋翼飞行器的期望的俯仰角；并结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角，计算期望的侧向加速度；

步骤五、四旋翼飞行器的高度控制器获取跟踪轨迹所需的高度，姿态角控制器获取期望的航向角，俯仰角控制器获取期望的俯仰角，滚转角控制器获取期望的侧向加速度，四旋翼飞行器在高度控制器、姿态角控制器、俯仰角控制器以及滚转角控制器的控制下按照预定轨迹飞行。

本实施方式分别根据直线路径和圆弧路径生成相应的四旋翼飞行器跟踪轨迹所需的高度、期望的航向角、期望的俯仰角，然后基于虚拟跟踪点获取期望的侧向加速度。为保证轨迹跟踪的准确性和快速性，结合四旋翼飞行器的控制特点，本发明选择以期望的恒定飞行速度跟踪轨迹，控制方式示意图如图3所示。

具体实施方式二：本实施方式与具体实施方式一不同的是，步骤一中所述建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系的具体过程包括：

对四旋翼飞行器建立直线路径坐标系o_px_py_pz_p、圆弧路径极坐标系C_ρN_ρP_ρ以及惯性坐标系OXYZ，定义直线路径坐标系o_px_py_pz_p的坐标原点为直线路径起点，其o_px_p轴指向直线路径方向，o_pz_p轴指向和惯性坐标系OZ轴指向相同，o_py_p轴、o_px_p轴、o_pz_p轴构成右手坐标系；从惯性坐标系OXYZ到直线路径坐标系o_px_py_pz_p的转换矩阵为R_i ^p：

其中，χ_q为当前期望的直线路径方向向量的偏航角，即，期望的直线路径方向向量与惯性坐标系下X轴方向的夹角；

圆弧路径极坐标系的N_ρ轴指向地理坐标系的正北方向，圆弧路径极坐标系的P_ρ轴方向为当前圆弧路径的圆心指向四旋翼飞行器的方向；惯性坐标系的X轴、Y轴、Z轴分别指向地理坐标系下的北方、东方、地心方向。

其他步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式二不同的是，步骤二中所述计算四旋翼飞行器跟踪轨迹所需的高度的具体过程包括：

A1、当跟踪轨迹为直线路径时：

四旋翼飞行器的位置相对直线路径的位置相对偏差e_p在o_px_py_pz_p坐标系下可表示为：

其中，e_px、e_py、e_pz分别表示e_p在o_px_py_pz_p坐标系下的x_p轴方向的分量、y_p轴方向的分量、z_p轴方向的分量，r为四旋翼飞行器期望位置向量，p为四旋翼飞行器当前位置向量；

为了得到所需高度h，如图4所示，将相对偏差e_p投影到包含直线路径方向向量的惯性坐标系下垂直平面(YOZ平面)内，得到在惯性坐标系下的相对偏差的投影s：

其中，s_n、s_e、s_d分别为惯性坐标系下s在X轴方向的分量、Y轴方向的分量、Z轴方向的分量；

如图5所示，再结合直线路径方向向量q＝(q_n,q_e,q_d)，由相似三角形定理可得：

其中，q_n、q_e、q_d分别为惯性坐标系下q在X轴方向的分量、Y轴方向的分量、Z轴方向的分量；

得到跟踪轨迹为直线路径时，四旋翼飞行器跟踪轨迹所需的高度h为：

其中，r_d为惯性坐标系下r在Z轴方向的分量；

A2、当跟踪轨迹为圆弧路径时：

圆弧路径的圆心坐标在惯性坐标系下为c＝(c_n,c_e,c_d)^T，则四旋翼飞行器跟踪轨迹所需的高度h为：

h＝-c_d

其中，c_n、c_e、c_d分别表示惯性坐标系下c的X轴、Y轴、Z轴坐标。

其他步骤及参数与具体实施方式二相同。

具体实施方式四：本实施方式与具体实施方式三不同的是，步骤二中所述四旋翼飞行器期望位置向量r和四旋翼飞行器当前位置向量p具体为：

其中，p_n、p_e、p_d分别为惯性坐标系下p在X轴方向的分量、Y轴方向的分量、Z轴方向的分量，r_n、r_e、r_d分别为惯性坐标系下r在X轴方向的分量、Y轴方向的分量、Z轴方向的分量。

其他步骤及参数与具体实施方式三相同。

具体实施方式五：本实施方式与具体实施方式四不同的是，所述步骤三的具体过程包括：

将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内，虚拟跟踪点的选择规则是：在期望路径的投影上选择一个与四旋翼飞行器的距离为L₁的点作为虚拟跟踪点T，根据图6所示几何关系计算虚拟跟踪点位置坐标T(x_t,y_t)，并且利用虚拟跟踪点位置坐标T(x_t,y_t)计算：四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η、期望的航向角χ_cmd；x_t、y_t分别表示惯性坐标系下T的X轴坐标、Y轴坐标；

B1、当跟踪轨迹为直线路径时：

当前期望的直线路径方向向量的偏航角χ_q和四旋翼飞行器当前速度矢量

的偏航角χ可由下式得出：

其中，v_e表示惯性坐标系的Y轴方向的分量，v_n惯性坐标系的X轴方向的分量；

计算四旋翼飞行器的位置相对直线路径的位置相对偏差e_p在o_px_py_pz_p坐标系下y_p轴方向的分量e_py：

e_py＝-sin(χ_q)·(p_n-r_n)+cos(χ_q)·(p_e-r_e)

然后根据几何关系能够得到虚拟跟踪点T的位置坐标：

其中，

表示四旋翼飞行器当前位置向量p与四旋翼飞行器期望位置向量r的向量差；

再结合当前速度矢量

的偏航角χ能够得到四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η：

利用虚拟跟踪点位置坐标计算期望的航向角χ_cmd：

B2、当跟踪轨迹为圆弧路径时：

如图7所示，虚拟跟踪点T的位置坐标计算公式为：

其中，

表示四旋翼飞行器相对圆弧路径的角位置，ρ为圆弧路径的半径，λ为圆弧方向，λ∈{-1,1}，当λ＝-1时表示圆弧路径是逆时针，当λ＝1时表示圆弧路径为顺时针；

为圆心c到虚拟跟踪点T的方向向量与圆心c到四旋翼飞行器当前位置p的方向向量的夹角；

四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η为：

利用虚拟跟踪点位置坐标计算期望的航向角χ_cmd：

其他步骤及参数与具体实施方式四相同。

具体实施方式六：本实施方式与具体实施方式五不同的是，所述步骤四的具体过程包括：

设计一个PID控制器，将四旋翼飞行器的期望的恒定飞行速度V_a ^*转化为期望的俯仰角θ_cmd；

轨迹跟踪制导逻辑示意图如图8所示。然后结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η，通过以下公式计算期望的侧向加速度a_scmd：

L₁＝2R sinη

其中，V_g是四旋翼飞行器当前飞行速度，R是对应当前侧向加速度等效的转弯半径。

其他步骤及参数与具体实施方式五相同。

具体实施方式七：本实施方式与具体实施方式五或四不同的是，步骤三中所述

的计算具体为：

其中，d表示四旋翼飞行器当前位置到圆弧路径的圆心c的距离。

其他步骤及参数与具体实施方式五或四相同。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (1)

1.一种基于非线性制导的四旋翼飞行器轨迹跟踪控制方法，其特征在于，方法具体包括以下步骤：

步骤一、对四旋翼飞行器建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系OXYZ；

步骤二、根据几何关系计算四旋翼飞行器跟踪轨迹所需的高度；

步骤三、将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内，在期望路径的投影上选择一个虚拟跟踪点，并且利用虚拟跟踪点位置坐标计算：四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角、期望的航向角；

步骤四、根据四旋翼飞行器的期望的恒定飞行速度生成四旋翼飞行器的期望的俯仰角；并结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角，计算期望的侧向加速度；

步骤五、四旋翼飞行器的高度控制器获取跟踪轨迹所需的高度，姿态角控制器获取期望的航向角，俯仰角控制器获取期望的俯仰角，滚转角控制器获取期望的侧向加速度，四旋翼飞行器在高度控制器、姿态角控制器、俯仰角控制器以及滚转角控制器的控制下按照预定轨迹飞行；

其特征在于，步骤一中所述建立直线路径坐标系、圆弧路径极坐标系以及惯性坐标系的具体过程包括：

对四旋翼飞行器建立直线路径坐标系o_px_py_pz_p、圆弧路径极坐标系C_ρN_ρP_ρ以及惯性坐标系OXYZ，定义直线路径坐标系o_px_py_pz_p的坐标原点为直线路径起点，o_px_p轴指向直线路径方向，o_pz_p轴指向和惯性坐标系OZ轴指向相同，o_py_p轴、o_px_p轴、o_pz_p轴构成右手坐标系；从惯性坐标系OXYZ到直线路径坐标系o_px_py_pz_p的转换矩阵为R_i ^p：

其中，χ_q为当前期望的直线路径方向向量的偏航角；

圆弧路径极坐标系的N_ρ轴指向地理坐标系的正北方向，圆弧路径极坐标系的P_ρ轴方向为当前圆弧路径的圆心指向四旋翼飞行器的方向；惯性坐标系的X轴、Y轴、Z轴分别指向地理坐标系下的北方、东方、地心方向；

步骤二中所述计算四旋翼飞行器跟踪轨迹所需的高度的具体过程包括：

A1、当跟踪轨迹为直线路径时：

四旋翼飞行器的位置相对直线路径的位置相对偏差e_p在o_px_py_pz_p坐标系下表示为：

其中，e_px、e_py、e_pz分别表示e_p在o_px_py_pz_p坐标系下的x_p轴方向的分量、y_p轴方向的分量、z_p轴方向的分量，r为四旋翼飞行器期望位置向量，p为四旋翼飞行器当前位置向量；

将相对偏差e_p投影到包含直线路径方向向量的惯性坐标系下YOZ平面内，得到相对偏差的投影s：

其中，s_n、s_e、s_d分别为惯性坐标系下s在X轴方向的分量、Y轴方向的分量、Z轴方向的分量；

再结合直线路径方向向量q＝(q_n,q_e,q_d)，得到：

其中，q_n、q_e、q_d分别为惯性坐标系下q在X轴方向的分量、Y轴方向的分量、Z轴方向的分量；

得到跟踪轨迹为直线路径时，四旋翼飞行器跟踪轨迹所需的高度h为：

其中，r_d为惯性坐标系下r在Z轴方向的分量；

A2、当跟踪轨迹为圆弧路径时：

圆弧路径的圆心坐标在惯性坐标系下为c＝(c_n,c_e,c_d)^T，则四旋翼飞行器跟踪轨迹所需的高度h为：

h＝-c_d

其中，c_n、c_e、c_d分别表示惯性坐标系下c的X轴、Y轴、Z轴坐标；

步骤二中所述四旋翼飞行器期望位置向量r和四旋翼飞行器当前位置向量p具体为：

其中，p_n、p_e、p_d分别为惯性坐标系下p在X轴方向的分量、Y轴方向的分量、Z轴方向的分量，r_n、r_e分别为惯性坐标系下r在X轴方向的分量、Y轴方向的分量；

所述步骤三的具体过程包括：

将四旋翼飞行器位置、期望路径、当前路径投影到惯性坐标系的XOY平面内，在期望路径的投影上选择一个距离四旋翼飞行器L₁的虚拟跟踪点T，计算虚拟跟踪点位置坐标T(x_t,y_t)，并且利用虚拟跟踪点位置坐标T(x_t,y_t)计算：四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η、期望的航向角χ_cmd；x_t、y_t分别表示惯性坐标系下T的X轴坐标、Y轴坐标；

B1、当跟踪轨迹为直线路径时：

当前期望的直线路径方向向量的偏航角χ_q和四旋翼飞行器当前速度矢量

的偏航角χ由下式得出：

其中，v_e表示惯性坐标系的Y轴方向的分量，v_n惯性坐标系的X轴方向的分量；

计算e_p在o_px_py_pz_p坐标系下y_p轴方向的分量e_py：

e_py＝-sin(χ_q)·(p_n-r_n)+cos(χ_q)·(p_e-r_e)

然后根据几何关系能够得到虚拟跟踪点T的位置坐标：

其中，

表示四旋翼飞行器当前位置向量p与四旋翼飞行器期望位置向量r的向量差；

再结合当前速度矢量

的偏航角χ能够得到η：

利用虚拟跟踪点位置坐标计算期望的航向角χ_cmd：

B2、当跟踪轨迹为圆弧路径时：

虚拟跟踪点T的位置坐标计算公式为：

其中，

表示四旋翼飞行器相对圆弧路径的角位置，ρ为圆弧路径的半径，λ为圆弧方向，λ∈{-1,1}，当λ＝-1时表示圆弧路径是逆时针，当λ＝1时表示圆弧路径为顺时针；

为圆心c到虚拟跟踪点T的方向向量与圆心c到四旋翼飞行器当前位置p的方向向量的夹角；

四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η为：

利用虚拟跟踪点位置坐标计算期望的航向角χ_cmd：

所述步骤四的具体过程包括：

将四旋翼飞行器的期望的恒定飞行速度V_a ^*转化为期望的俯仰角θ_cmd；

然后结合步骤三中求得的四旋翼飞行器当前速度方向与四旋翼飞行器位置和虚拟跟踪点位置连线之间的夹角η，通过以下公式计算期望的侧向加速度a_scmd：

L₁＝2Rsinη

其中，V_g是四旋翼飞行器当前飞行速度，R是对应当前侧向加速度等效的转弯半径；

步骤三中所述

的计算具体为：

其中，d表示四旋翼飞行器当前位置到圆弧路径的圆心c的距离。

Images