CN112162570B - 一种四旋翼直升飞机小范围动态跟踪的方法 - Google Patents

Abstract

本发明是关于一种四旋翼直升飞机小范围动态跟踪的方法。其首先安装泰览Tele‑15型激光雷达测距仪，测量动态目标的位置信息，再安装INS300自主导航定位定向系统，测量四旋翼直升机的俯仰角与偏航角以及加速度信息，通过积分解算得到位置信息。再通过设计一类基于目标距离自适应调节的非线性数字微分器，一器四用，实现偏航角期望值、纵向位置误差、偏航角误差、俯仰角误差的微分解算。同时通过目标视线驱动的逻辑设计偏航角期望信号，通过纵向距离误差驱动的逻辑设计俯仰角期望信号，最终实现对雷达测距范围内的目标的动态跟踪。其优点在于设计简洁，跟踪的稳定性好，精度高。

Description

一种四旋翼直升飞机小范围动态跟踪的方法

技术领域

本发明涉及四旋翼飞行器飞行控制与目标跟踪领域，具体而言，涉及一种四旋翼直升飞机小范围动态跟踪的方法。

背景技术

四旋翼飞机由于其陈本低，携带方便，设计与操纵简单易学，而在民用的各个领域中广泛使用，如高空航拍、短途快递、灾情勘探等等领域均有使用。在工业领域以及警用领域中，有时需要对某一个运动目标，进行动态跟踪与监视，那么其功能与目的则完全不同于上述航拍等。传统的直升机质心控制一般采用航路规划与位置误差驱动的方式实现位置的精确控制。但由于监视问题中的直升飞机目的地的不确定性，目标的运动等特点，单单使用误差驱动的常规方法容易出现目标丢失等问题。基于上述背景原因，本发明提出了一种采用视线解算实现航向跟踪，采用距离解算实现纵向跟踪，两者复合实现激光雷达测距范围内小目标的动态跟踪方法，结果具有很好的跟踪精度与快速性，也具有很高的工程实用价值。

需要说明的是，在上述背景技术部分发明的信息仅用于加强对本发明的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

发明内容

本发明的目的在于提供一种四旋翼直升飞机小范围动态跟踪的方法，进而至少在一定程度上克服由于相关技术的限制和缺陷而导致的直升飞机目标跟踪的精度不高与目标丢失问题。

根据本发明的一个方面，提供一种四旋翼直升飞机小范围动态跟踪的方法，包括以下步骤：

步骤S10，在四旋翼直升飞机上安装泰览Tele-15型激光雷达测距仪，测量动态目标的位置信息，并通过转换解算得到目标位置信息；在四旋翼直升飞机上安装INS300自主导航定位定向系统，测量四旋翼直升机的加速度信息，并通过转换，得到直升机的位置信息；

步骤S20，根据四旋翼直升机与目标的相对位置信号，解算理想偏航角信号，然后设计非线性数字微分器，得到理想偏航角速率信号，再根据非线性组合，得到偏航角指令信号；

步骤S30，根据目标的纵向位置信息与直升机的位置信息，设计非线性数字微分器，求解纵向位置偏差信息的微分信号，然后设计位置姿态折算算法，得到俯仰角期望信号；

步骤S40，采用INS300自主导航定位定向系统，测量四旋翼直升飞机的俯仰角，同时通过设计数字微分器，得到俯仰角速率角速率信号，然后进行线性综合，得到俯仰角稳定跟踪控制信号，并对期望俯仰角进行信号跟踪；

步骤S50，采用INS300自主导航定位定向系统，测量四旋翼直升飞机的偏航角，同时通过设计数字微分器，得到偏航角速率信号，然后进行非线性综合，得到偏航角稳定跟踪控制信号，并对偏航角指令进行跟踪。

步骤S60，通过参数调试，以及由高度通道的定高稳定飞行，由俯仰通道与偏航通道对期望俯仰角与期望偏航角进行角度稳定跟踪，即可实现对小范围动态目标的无人跟踪值守。

在本发明的一种示例实施例中，在四旋翼直升飞机上安装泰览Tele-15型激光雷达测距仪与INS300自主导航定位定向系统，通过变换与解算得到目标与直升机的位置信息包括：

v_x＝∫a_xdt；v_y＝∫a_ydt；

x_j＝∫v_xdt；z_j＝∫v_zdt；

x_t＝rcos(κ)+x_j；

z_t＝rsin(κ)+z_j；

其中a_x为通过INS300自主导航定位定向系统测量四旋翼直升机的纵向加速度信号，a_z为测量得到的侧向加速度信号。v_x为加速度积分得到的纵向速度信号，v_z为侧向速度信号。x_j为纵向位置信号记作，z_j为侧向位置信号，dt表示对时间信号的积分。r为采用泰览Tele-15型激光雷达测距仪测量待跟踪目标的距离信息，κ为角度信息，x_t为目标的纵向位置坐标，z_t为目标的侧向位置坐标。

在本发明的一种示例实施例中，根据四旋翼直升机与目标的相对位置信号，解算理想偏航角信号，然后设计非线性数字微分器，得到理想偏航角速率信号，再根据非线性组合，得到偏航角指令信号包括：

Δx＝x_t-x_j；

Δz＝z_t-z_j；

φ_d2(n+1)＝φ_d2(n)+k₁₄φ_d1；

φ_d3＝k₁₅φ_d+k₁₆φ_d1+k₁₇φ_d2；

其中Δx为根据四旋翼直升机与目标的位置信息，解算得到的纵向位置偏差信息，Δz为侧向位置偏差信息。φ_d为理想偏航角信号，φ_d1为理想偏航角速率信号，φ_d2为理想偏航角滞后信号，k₁₁、k₁₂、k₁₃、k₁₄、k₁₅、k₁₆、k₁₇为常值参数信号，其详细设计见后文案例实施，φ_d3为偏航角期望信号。

在本发明的一种示例实施例中，根据目标的纵向位置信息与直升机的位置信息，设计非线性数字微分器，求解纵向位置偏差信息的微分信号，然后设计位置姿态折算算法，得到俯仰角期望信号包括：

Δx₂(n+1)＝Δx₂(n)+k₂₄Δx₁；

其中Δx为纵向位置偏差信号，Δx₁为理想向位置偏差微分信号，Δx₂为纵向位置偏差滞后信号，k₂₁、k₂₂、k₂₃、k₂₄、k₂₅、k₂₆、k₂₇为常值参数信号，其详细设计见后文案例实施。θ_d3为俯仰角期望信号。

在本发明的一种示例实施例中，采用INS300自主导航定位定向系统，测量四旋翼直升飞机的俯仰角，同时通过设计数字微分器，得到俯仰角速率角速率信号，然后进行线性综合，得到俯仰角稳定跟踪控制信号包括：

e_a＝θ-θ_d3；

e_a2(n+1)＝e_a2(n)+k₃₄e_a1；

u_f＝k₃₅e_a+k₃₆e_a1+k₃₇s_1a；

其中θ为采用INS300自主导航定位定向系统测量得到的四旋翼直升飞机俯仰角，θ_d3为俯仰角期望信号，e_a为俯仰角误差信号，e_a1为俯仰角误差微分信号，k₃₁、k₃₂、k₃₃、k₃₄、k₃₅、k₃₆、k₃₇为常值参数信号，其详细设计见后文案例实施。s_1a为俯仰角误差非线性积分信号，dt表示对时间信号的积分。u_f为俯仰角稳定跟踪控制信号。

在本发明的一种示例实施例中，测量四旋翼直升飞机的偏航角，同时通过设计数字微分器，得到偏航角速率信号，然后进行非线性综合，得到偏航角稳定跟踪控制信号包括：

e_b＝φ-φ_d3；

e_b2(n+1)＝e_b2(n)+k₄₄e_b1；

u_p＝k₄₅e_b+k₄₆e_b2+k₄₇s_1b；

其中φ为采用INS300自主导航定位定向系统测量得到的四旋翼直升飞机偏航角信号，φ_d3为偏航角期望信号，e_b为偏航角误差信号，e_b1为偏航角误差微分信号，其中k₄₁、k₄₂、k₄₃、k₄₄、k₄₅、k₄₆、k₄₇为常值参数信号，其详细设计见后文案例实施。s_1b为偏航角误差非线性积分信号，dt表示对时间信号的积分。u_p为最终的偏航角稳定跟踪控制信号。

最后，将所述的俯仰角稳定跟踪控制信号输送给俯仰舵系统，实现俯仰通道与对期望俯仰角的跟踪；将所述的偏航角稳定跟踪控制信号输送给偏航舵系统，实现偏航通道与对期望偏航角的跟踪，从而实现偏航通道的质心控制任务。对四旋翼直升机的高度通道进行定高稳定飞行，由于高度通道的稳定控制非本发明的保护内容，故在此不再累述，在案例实施中选取高度PID控制即可。对俯仰通道、偏航通道与高度通道进行联合调试，选取合理的参数，即可实现四旋翼直升飞机对小范围动态目标的无人跟踪与值守。

有益效果

本发明提供的一种四旋翼直升飞机小范围动态跟踪的方法，其优点在于能够通过激光雷达测距提供目标位置信息，再通过自主定位导航系统提供自身的位置信息与姿态信息，然后通过设计一类统一的具有根据距离偏差自适应调节能力的非线性微分器，即实现了偏航角期望值信号的微分解算，也实现了纵向位置误差、俯仰角误差以及偏航角误差的微分信号解算，使得上述微分信号不仅能够提供阻尼，而且能够实现随距离远近的自动调节，从而实现了对小范围目标的稳定高性能追踪。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本发明。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明提供的一种四旋翼直升飞机小范围动态跟踪的方法的流程图；

图2是本发明实施例所提供方法的泰览Tele-15型激光雷达测距仪实物图；

图3是本发明实施例所提供方法的INS300自主导航定位定向系统实物图；

图4是本发明实施例所提供方法的直升机与目标的相对运动曲线(单位：米)；

图5是本发明实施例所提供方法的偏航角指令信号曲线(单位：度)；

图6是本发明实施例所提供方法的期望俯仰角信号曲线(单位：度)；

图7是本发明实施例所提供方法的俯仰角稳定跟踪控制信号曲线(无单位)；

图8是本发明实施例所提供方法的偏航角稳定跟踪控制信号曲线(无单位)；

图9是本发明实施例所提供方法的偏航角信号曲线(单位：度)；

图10是本发明实施例所提供方法的俯仰角信号曲线(单位：度)；

图11是本发明实施例所提供方法的偏航角误差信号曲线(单位：度)；

图12是本发明实施例所提供方法的俯仰角误差信号曲线(单位：度)。

具体实施方式

现在将参考附图基础上更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的范例；相反，提供这些实施方式使得本发明将更加全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施方式中。在下面的描述中，提供许多具体细节从而给出对本发明的实施方式的充分理解。然而，本领域技术人员将意识到，可以实践本发明的技术方案而省略所述特定细节中的一个或更多，或者可以采用其它的方法、组元、装置、步骤等。在其它情况下，不详细示出或描述公知技术方案以避免喧宾夺主而使得本发明的各方面变得模糊。

本发明提供了一种四旋翼直升飞机小范围动态跟踪的方法，其通过安装激光雷达测距仪与自主定位导航定向系统，测量目标运动的实时信息，并解算自身的位置与姿态信息。然后通过设计一类能够基于直升机与目标距离之间自动调节的数字微分器，实现角度、位置误差以及角度误差信号的微分解算，提供控制所需的阻尼信号，然后通过视线角驱动与距离误差驱动相结合的方法，实现对动态目标的实时监视跟踪。

下面，将结合附图对本发明的一种四旋翼直升飞机小范围动态跟踪的方法进行进一步的解释以及说明。参考图1所示，该一种四旋翼直升飞机小范围动态跟踪的方法包括以下步骤：

步骤S10，在四旋翼直升飞机上安装泰览Tele-15型激光雷达测距仪，测量动态目标的位置信息，并通过转换解算得到目标位置信息，在四旋翼直升飞机上安装INS300自主导航定位定向系统，测量四旋翼直升机的加速度信息，并通过转换，得到直升机的位置信息；

具体的，首先在四旋翼直升飞机上安装泰览Tele-15型激光雷达测距仪，其测量距离为500米，角度精度小于0.03度，其实物如图2所示。并在四旋翼直升飞机上，安装INS300自主导航定位定向系统，该系统采用光纤陀螺，具有精度高，稳定性好的优点，其实物图片如图3所示。

其次，通过INS300自主导航定位定向系统，测量四旋翼直升机的加速度信号，其中纵向加速度信号记作a_x，侧向加速度信号记作a_z。然后进行积分，得到速度与位置信号。其中纵向速度信号记作v_x，侧向速度信号记作v_z，纵向位置信号记作x_j，侧向位置信号记作z_j。其中速度与位置的积分运算如下所示：

v_x＝∫a_xdt；v_y＝∫a_ydt；

x_j＝∫v_xdt；z_j＝∫v_zdt；

其中dt表示对时间信号的积分。

最后，采用激光雷达测距仪，测量待跟踪目标的位置信息，包含距离信息与角度信息。其中将距离信息记作r；角度信息记作κ，通过如下的转换公式，得到目标的纵向位置坐标与侧向位置坐标。其中目标的纵向位置坐标记作x_t，目标的侧向位置坐标记作z_t。其计算方式如下：

x_t＝rcos(κ)+x_j；

z_t＝rsin(κ)+z_j；

步骤S20，根据四旋翼直升机与目标的相对位置信号，解算理想偏航角信号，然后设计非线性数字微分器，得到理想偏航角速率信号，再根据非线性组合，得到偏航角指令信号。

具体的，首先根据四旋翼直升机与目标的位置信息，求解纵向与侧向位置偏差，其中纵向位置偏差记作Δx，侧向位置偏差记作Δz，其计算方式如下：

Δx＝x_t-x_j；

Δz＝z_t-z_j；

其次，根据纵向与侧向位置偏差求解理想偏航角信号，记作φ_d，其计算方式如下：

然后，根据理想的偏航角信号，设计非线性数字微分器，得到理想偏航角速率信号，记作φ_d1，其计算方式如下：

φ_d2(n+1)＝φ_d2(n)+k₁₄φ_d1；

其中φ_d2为理想偏航角滞后信号，k₁₁、k₁₂、k₁₃、k₁₄为常值参数信号，其详细设计见后文案例实施。

最后，针对所求的理想偏航角信号与理想偏航角速率信号进行线性组合，得到偏航角期望信号，记作φ_d3，其计算方式如下：

φ_d3＝k₁₅φ_d+k₁₆φ_d1+k₁₇φ_d2；

其中k₁₅、k₁₆、k₁₇为常值参数信号，其详细设计见后文案例实施。

步骤S30，根据目标的纵向位置信息与直升机的位置信息，设计非线性数字微分器，求解纵向位置偏差信息的微分信号，然后设计位置姿态折算算法，得到俯仰角期望信号。

具体的，根据所述的纵向位置偏差信号Δx，设计如下的非线性数字微分器，得到理想向位置偏差微分信号，记作Δx₁，其计算方式如下：

Δx₂(n+1)＝Δx₂(n)+k₂₄Δx₁；

其中Δx₂为纵向位置偏差滞后信号，k₂₁、k₂₂、k₂₃、k₂₄为常值参数信号，其详细设计见后文案例实施。

最后，针对所求的纵向位置偏差与理想向位置偏差微分信号进行线性组合，得到俯仰角期望信号，记作θ_d3，其计算方式如下：

其中k₂₅、k₂₆、k₂₇为常值参数信号，其详细设计见后文案例实施。

步骤S40，采用INS300自主导航定位定向系统，测量四旋翼直升飞机的俯仰角，同时通过设计数字微分器，得到俯仰角速率角速率信号，然后进行线性综合，得到俯仰角稳定跟踪控制信号，并对期望俯仰角进行信号跟踪。

具体的，首先采用INS300自主导航定位定向系统测量四旋翼直升飞机的俯仰角，记作θ。然后与俯仰角期望信号θ_d3相比较，得到俯仰角误差信号，记作e_a，其计算方式为：

e_a＝θ-θ_d3；

其次，设计非线性数字微分器，求解俯仰角误差微分信号，记作e_a1，其计算方式如下：

e_a2(n+1)＝e_a2(n)+k₃₄e_a1；

其中k₃₁、k₃₂、k₃₃、k₃₄为常值参数信号，其详细设计见后文案例实施。

再次，对俯仰角误差信号进行非线性积分，得到俯仰角误差非线性积分信号，记作s_1a，其计算方式如下：

其中dt表示对时间信号的积分。

最后，对所述的俯仰角误差信号、俯仰角误差微分信号与俯仰角误差非线性积分信号进行线性叠加，得到最终的俯仰角稳定跟踪控制信号，记作u_f，其计算方式如下：

u_f＝k₃₅e_a+k₃₆e_a1+k₃₇s_1a；

其中k₃₅、k₃₆、k₃₇为常值参数信号，其详细设计见后文案例实施。

步骤S50，采用INS300自主导航定位定向系统，测量四旋翼直升飞机的偏航角，同时通过设计数字微分器，得到偏航角速率信号，然后进行非线性综合，得到偏航角稳定跟踪控制信号，并对偏航角指令进行跟踪。

具体的，采用INS300自主导航定位定向系统测量四旋翼直升飞机的偏航角，记作φ。然后与偏航角期望信号φ_d3相比较，得到偏航角误差信号，记作e_b，其计算方式为：

e_b＝φ-φ_d3；

其次，设计非线性数字微分器，求解偏航角误差微分信号，记作e_b1，其计算方式如下：

e_b2(n+1)＝e_b2(n)+k₄₄e_b1；

其中k₄₁、k₄₂、k₄₃、k₄₄为常值参数信号，其详细设计见后文案例实施。

再次，对偏航角误差信号进行非线性积分，得到偏航角误差非线性积分信号，记作s_1b，其计算方式如下：

其中dt表示对时间信号的积分。

最后，对所述的偏航角误差信号、偏航角误差微分信号与偏航角误差非线性积分信号进行线性叠加，得到最终的偏航角稳定跟踪控制信号，记作u_p，其计算方式如下：

u_p＝k₄₅e_b+k₄₆e_b2+k₄₇s_1b；

其中k₄₅、k₄₆、k₄₇为常值参数信号，其详细设计见后文案例实施。

步骤S60，通过参数调试，以及由高度通道的定高稳定飞行，由俯仰通道与偏航通道对期望俯仰角与期望偏航角进行角度稳定跟踪，即可实现对小范围动态目标的无人跟踪值守。

首先，将所述的俯仰角稳定跟踪控制信号输送给俯仰舵系统，实现俯仰通道与对期望俯仰角的跟踪，从而实现俯仰通道的质心控制任务。

其次，将所述的偏航角稳定跟踪控制信号输送给偏航舵系统，实现偏航通道与对期望偏航角的跟踪，从而实现偏航通道的质心控制任务。

然后，对四旋翼直升机的高度通道进行定高稳定飞行，由于高度通道的稳定控制非本发明的保护内容，故在此不再累述，在案例实施中选取高度PID控制即可。

最后，对俯仰通道、偏航通道与高度通道进行联合调试，选取合理的参数，即可实现四旋翼直升飞机对小范围动态目标的无人跟踪与值守。

案例实施与计算机仿真模拟结果分析

在步骤S10中，在四旋翼直升飞机上安装Tele泰览-15型激光雷达测距仪与INS300自主导航定位定向系统，得到直升机与目标的相对运动曲线如图4所示。

在步骤S20中，设置k₁₁＝10、k₁₂＝300、k₁₃＝400、k₁₄＝0.001、k₁₅＝0.1、k₁₆＝0.1、k₁₇＝0.05，得到偏航角指令信号如图5所示。

在步骤S30中，设置k₂₁＝10、k₂₂＝300、k₂₃＝600、k₂₄＝0.001、k₂₅＝0.17、k₂₆＝0.05、k₂₇＝0.01，得到期望俯仰角信号如图6所示。

在步骤S40中，设置k₃₁＝20、k₃₂＝500、k₃₃＝600、k₃₄＝0.001、k₃₅＝0.3、k₃₆＝0.05、k₃₇＝0.01，得到俯仰角稳定跟踪控制信号如图7所示。

在步骤S50中，设置k₄₁＝30、k₄₂＝700、k₄₃＝800、k₄₄＝0.001、k₄₅＝0.3、k₄₆＝0.05、k₄₇＝0.01，得到偏航角稳定跟踪控制信号如图8所示。

步骤S60，通过参数调试，以及由高度通道的定高稳定飞行，由俯仰通道与偏航通道对期望俯仰角与期望偏航角进行角度稳定跟踪，最终四旋翼直升机的俯仰角如图9所示，偏航角如图10所示。俯仰角误差信号如图11所示，偏航角误差如图12所示。

由图11与12可以看出，四旋翼直升机的俯仰角与偏航角能够稳定跟着期望指令，误差能够快速收敛到零，从而其姿态跟踪系统的设计是快速有效的。由图9与图10以及图5与图6可以看出，俯仰角与偏航角均能大致趋近与指令信号，在最大值与突变的时候有超调，但由图4可以看出，不影响四旋翼直升机对目标的跟踪效果，最终四旋翼与目标的距离越来越近，从而能够实现对目标的跟踪、监视与无人值守，不会出现丢失目标的情况。本案例也标明本发明所提供的小范围跟踪方法是完全正确可行的，从而具有很高的工程价值。

本领域技术人员在考虑说明书及实践这类的发明后，将容易想到本发明的其他实施例。本申请旨在涵盖本发明的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本发明的一般性原理并包括本发明未指明的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本发明的真正范围和精神由权利要求指出。

Claims (1)

1.一种四旋翼直升飞机小范围动态跟踪的方法，其特征在于，包括以下步骤：

步骤S10，在四旋翼直升飞机上安装泰览Tele-15型激光雷达测距仪，测量动态目标的位置信息，并通过转换解算得到目标位置信息；在四旋翼直升飞机上安装INS300自主导航定位定向系统，测量四旋翼直升机的加速度信息，并通过转换，得到直升机的位置信息如下：

v_x＝∫a_xdt；v_y＝∫a_ydt；

x_j＝∫v_xdt；z_j＝∫v_zdt；

x_t＝r cos(κ)+x_j；

z_t＝r sin(κ)+z_j；

其中a_x为通过INS300自主导航定位定向系统测量四旋翼直升机的纵向加速度信号，a_z为测量得到的侧向加速度信号；v_x为加速度积分得到的纵向速度信号，v_z为侧向速度信号；x_j为纵向位置信号记作，z_j为侧向位置信号，dt表示对时间信号的积分；r为采用泰览Tele-15型激光雷达测距仪测量待跟踪目标的距离信息，κ为角度信息，x_t为目标的纵向位置坐标，z_t为目标的侧向位置坐标；

步骤S20，根据四旋翼直升机与目标的相对位置信号，解算理想偏航角信号，然后设计非线性数字微分器，得到理想偏航角速率信号，再根据非线性组合，得到偏航角期望信号如下：

Δx＝x_t-x_j；

Δz＝z_t-z_j；

φ_d2(n+1)＝φ_d2(n)+k₁₄φ_d1；

φ_d3＝k₁₅φ_d+k₁₆φ_d1+k₁₇φ_d2；

其中Δx为根据四旋翼直升机与目标的位置信息，解算得到的纵向位置偏差信息，Δz为侧向位置偏差信息；φ_d为理想偏航角信号，φ_d1为理想偏航角速率信号，φ_d2为理想偏航角滞后信号，k₁₁、k₁₂、k₁₃、k₁₄、k₁₅、k₁₆、k₁₇为常值参数信号，φ_d3为偏航角期望信号；

步骤S30，根据目标的纵向位置信息与直升机的位置信息，设计非线性数字微分器，求解纵向位置偏差信息的微分信号，然后设计位置姿态折算算法，得到俯仰角期望信号如下：

Δx₂(n+1)＝Δx₂(n)+k₂₄Δx₁；

其中Δx为纵向位置偏差信号，Δx₁为理想向位置偏差微分信号，Δx₂为纵向位置偏差滞后信号，k₂₁、k₂₂、k₂₃、k₂₄、k₂₅、k₂₆、k₂₇为常值参数信号；θ_d3为俯仰角期望信号；

步骤S40，采用INS300自主导航定位定向系统，测量四旋翼直升飞机的俯仰角，同时通过设计数字微分器，得到俯仰角速率角速率信号，然后进行线性综合，得到俯仰角稳定跟踪控制信号，并对期望俯仰角进行信号跟踪如下：

e_a＝θ-θ_d3；

e_a2(n+1)＝e_a2(n)+k₃₄e_a1；

u_f＝k₃₅e_a+k₃₆e_a1+k₃₇s_1a；

其中θ为采用INS300自主导航定位定向系统测量得到的四旋翼直升飞机俯仰角，θ_d3为俯仰角期望信号，e_a为俯仰角误差信号，e_a1为俯仰角误差微分信号，k₃₁、k₃₂、k₃₃、k₃₄、k₃₅、k₃₆、k₃₇为常值参数信号；s_1a为俯仰角误差非线性积分信号，dt表示对时间信号的积分；u_f为俯仰角稳定跟踪控制信号；

步骤S50，采用INS300自主导航定位定向系统，测量四旋翼直升飞机的偏航角，同时通过设计数字微分器，得到偏航角速率信号，然后进行非线性综合，得到偏航角稳定跟踪控制信号，并对偏航角指令进行跟踪如下：

e_b＝φ-φ_d3；

e_b2(n+1)＝e_b2(n)+k₄₄e_b1；

u_p＝k₄₅e_b+k₄₆e_b2+k₄₇s_1b；

其中φ为采用INS300自主导航定位定向系统测量得到的四旋翼直升飞机偏航角信号，φ_d3为偏航角期望信号，e_b为偏航角误差信号，e_b1为偏航角误差微分信号，其中k₄₁、k₄₂、k₄₃、k₄₄、k₄₅、k₄₆、k₄₇为常值参数信号；s_1b为偏航角误差非线性积分信号，dt表示对时间信号的积分；u_p为最终的偏航角稳定跟踪控制信号；最后，对俯仰通道、偏航通道进行联合调试，即可实现四旋翼直升飞机对小范围动态目标的无人跟踪与值守。

Images