CN111240362A - 一种智能引导飞行器转弯的控制方法及装置 - Google Patents

Abstract

本发明一种基于智能纠偏控制引导飞行器转弯的方法及装置，该方法包括提供飞行器X‑Y平面取向的飞行工作区域；提供飞行器起始位置和目标位置；构建基准飞行轨迹，获取飞行器的基准飞行轨迹参数：转弯起始位置、转弯半径、转弯角度；并根据上述基准飞行轨迹参数进一步计算转弯角速度、转弯时间、转弯中心点位置；根据轨迹偏航角计算设定飞行器的实际飞行轨迹参数：偏航角转弯开始时间、偏航角转弯结束时间；并计算偏航角指令；再将偏航角指令和基准飞行轨迹参数进行PID控制计算，实现该飞行器的航向纠偏控制。本发明使用PID控制计算来调整飞行器的转弯飞行轨迹偏差，从而智能、准确地引导飞行器尽量不偏离基准飞行轨迹，准确到达目标位置。

Description

一种智能引导飞行器转弯的控制方法及装置

技术领域

本发明属于飞行控制技术领域，更具体地涉及种智能引导飞行器转弯的控制方法及装置。

背景技术

众所周知，飞行器用途广泛、成本低、效费比好、无人员伤亡风险、生存能力强、机动性能好、使用方便，在民用领域可广泛用于地图测绘、地质勘测、灾害监测、空中交通管制、边境巡逻监控、通信中继、农药喷洒等。同时，在现代军事中有着极其重要的作用，飞行器常常被运用于防空实战训练，目前传统飞行器能够实现点火、起爆、分离、爬升、平飞等纵向平面机动等。

但是，现有的飞行器在实战过程却无法实现侧向平面的大范围机动，随着防空系统作战能力的不断提升，以及日趋复杂的作战环境，以及试验环境限制常常对此类飞行器的性能提出了更高的要求，使其能适应各种不同环境限制、降低试验成本，提升试验效率成为当期亟待解决的问题。

发明内容

针对现有技术的改进需求，本发明提供了一种智能引导飞行器转弯的控制方法及装置，其中，在设计原有飞行器纵向机动的基础上增加侧向机动纠偏功能，实现侧向平面多方向、多角度、大范围的精确转弯功能，可以针对试验现场具体情况构建不同角度、半径的转弯轨迹，并通过PID控制计算航向纠偏，为防空训练以及飞行器性能验证提供更多试验可能。

为实现上述目的，本发明第一方面提供一种基于智能纠偏控制引导飞行器转弯的方法，所述方法包括：

提供飞行器X-Y平面取向的飞行工作区域；

提供飞行器起始位置(0,0)和目标位置；

构建基准飞行轨迹，获取所述飞行器的基准飞行轨迹参数：转弯起始位置(X_Turn，0)、转弯半径R_Turn、转弯角度φ_Turn；

并根据上述基准飞行轨迹参数进一步计算转弯角速度ω_Turn＝57.3×V_m/R_Turn、转弯时间t_Turn＝|φ_Turn|/ω_Turn、转弯中心点位置(X_Turn,Z_Turn)；

其中，Vm为所述飞行器在X-Y平面的平均线速度，

根据轨迹偏航角计算设定所述飞行器的实际飞行轨迹参数：偏航角转弯开始时间t_{Turn_Begin_psi}、偏航角转弯结束时间t_{Turn_End_psi}＝t_{Turn_Begin_psi}+t_Turn；

并根据上述实际飞行轨迹参数计算偏航角指令ψ_c，如下式(Ⅰ)：

其中，t为实际飞行时间；Sign(φ_Turn)为函数符号，

将所述偏航角指令ψ_c和基准飞行轨迹参数进行PID控制计算，实现所述飞行器的航向纠偏控制。

进一步地，所述方法还包括：

根据轨迹偏航位置计算设定所述飞行器的实际飞行轨迹参数：侧偏转弯开始时间t_{Turn_Begin}、侧偏转弯结束时间t_{Turn_End}＝t_{Turn_Begin}+t_Turn；

并根据上述实际飞行轨迹参数计算侧偏距指令ΔZ，如下式(Ⅱ)：

其中，t为实际飞行时间，Z_c为通过纠偏计算设定的侧偏指令，(X,Z)为实际飞行位置，Sign(φ_Turn)为函数符号，

将所述侧偏距指令ΔZ和基准飞行轨迹参数进行PID控制计算，以使控制所述飞行器的转弯飞行轨迹偏差。

进一步地，所述方法还包括：

根据u_2h＝k_2h·(ψ-ψ_c)进行偏航角指令ψ_c与基准飞行轨迹参数的PID控制解算所述飞行器的航向角比例控制电压；其中，u_2h为所述飞行器的航向角比例的控制电压，k_2h为航向角比例PID控制解算参数，ψ为所述基准飞行轨迹的航向角。

进一步地，所述方法还包括：

根据u_3h＝k_3h·ΔZ进行侧偏距指令ΔZ的PID控制解算所述飞行器的航向侧偏比例控制电压；其中，u_3h为所述飞行器的航向侧偏比例控制电压，k_3h为航向侧偏比例PID控制解算参数。

进一步地，所述方法还包括：

根据u_4h＝k_4h·JF₅进行侧偏距指令ΔZ的PID控制解算所述飞行器的航向侧偏积分控制电压；其中，u_4h为所述飞行器的航向侧偏积分控制电压，k_4h为航向侧偏积分PID控制解算参数，

为侧偏积分限幅，t_S为点火起爆时间，t_h为侧偏控制起控时间。

本发明第二方面提供一种基于智能纠偏控制引导飞行器转弯的装置，所述装置包括：

后台终端，用于根据飞行器的起始位置和目标位置构建基准飞行轨迹，获取所述飞行器的基准飞行轨迹参数信息，并发送至控制系统；其中，所述基准飞行轨迹参数信息包括：所述飞行器的转弯起始位置信息、转弯半径信息、转弯角度信息、转弯角速度信息、转弯时间信息以及转弯中心点位置信息；

控制系统，用于接受所述飞行器的基准飞行轨迹参数信息，同时根据轨迹偏航角和偏航位置计算设定所述飞行器的实际飞行轨迹参数信息，并发送至PID控制解算模块；其中，所述实际飞行轨迹参数信息包括：偏航角转弯开始时间信息、偏航角转弯结束时间信息、偏航角指令信息、航向角速度信息、侧偏转弯开始时间信息、侧偏转弯结束时间信息以及侧偏距指令信息；

PID控制解算模块，用于接受所述飞行器的基准飞行轨迹参数信息和实际飞行轨迹参数信息，进行PID控制计算获取航向电压参数信息，以使控制所述飞行器的转弯飞行轨迹偏差；其中，所述航向电压参数信息包括：航向角比例的控制电压信息、航向角控制电压信息、航向侧偏比例控制电压信息、航向侧偏积分控制电压信息。

进一步地，所述PID控制解算模块包括：

航向角比例控制器，用于根据偏航角指令信息进行PID控制解算出所述飞行器的航向角比例控制电压；

航向角速度控制器，用于根据航向角速度信息进行PID控制解算出所述飞行器的航向角控制电压；

航向侧偏比例控制器，用于根据侧偏距指令信息进行PID控制解算出所述飞行器的航向侧偏比例控制电压；

航向侧偏积分控制器，用于根据侧偏距指令信息进行PID控制解算出所述飞行器的航向侧偏积分控制电压。

进一步地，所述装置还包括：

舵机，用于接受所述飞行器的航向电压参数信息，并输出舵控电压，从而控制舵机的舵面偏转实现航向纠偏控制。

进一步地，所述装置还包括：

传感模块，用于采集所述飞行器的航向角速度信息以及实际飞行位置信息，并发送至控制系统。

进一步地，所述传感模块包括液浮陀螺和GPS定位装置。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益效果：

本发明将飞行器完成转弯任务的飞行轨迹分为三部分：前直线轨迹，转弯中圆弧轨迹，转弯结束后沿圆弧切线方向直线轨迹。通过转弯起始位置、转弯半径、转弯角度、估算转弯角速度、转弯开始时间、转弯中心点坐标等参数，规划出一条基准飞行轨迹。同时针对转弯中圆弧轨迹，转弯结束后沿圆弧切线方向直线轨迹的飞行过程进行纠偏控制，在控制过程中需要对基准飞行轨迹与实际飞行轨迹之间的角度以及位置距离两个变量进行控制，使用PID控制计算处理来调整飞行器的转弯飞行轨迹偏差，从而智能、准确地引导飞行器尽量不偏离基准飞行轨迹，准确到达目标位置。

本发明在设计原有飞行器纵向机动的基础上增加侧向机动纠偏功能，实现侧向平面多方向、多角度、大范围的精确转弯功能，可以针对试验现场具体情况构建不同角度、半径的转弯轨迹，并通过PID控制计算航向纠偏，为防空训练以及飞行器性能验证提供更多试验可能。

附图说明

图1为按照本发明实现的一种智能引导飞行器转弯的控制方法及其装置中飞行器的转弯轨迹曲线示例；

图2为按照本发明实现的一种智能引导飞行器转弯的控制方法及其装置中飞行器的偏航角控制曲线示例；

图3为按照本发明实现的一种智能引导飞行器转弯的控制方法及其装置中飞行器的侧偏距控制曲线示例；

图4为按照本发明实现的一种智能引导飞行器转弯的控制方法及其装置中PID控制解算模块的结构示图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

本发明提供了一种智能引导飞行器转弯的控制方法。如图1所示，设定飞行器的飞行工作区域为X-Y的坐标平面，X轴、Y轴为飞行过程中相对于地面俯视坐标系的位移值，X轴为发射方向，Y轴为转弯方向，其中起始位置位(0,0)，该飞行器完成转弯任务其飞行轨迹分为三部分：前直线轨迹(S1)，转弯中圆弧轨迹(S2)，转弯结束后沿圆弧切线方向直线轨迹(S3)。通过转弯起始位置、转弯半径、转弯角度、估算转弯角速度、转弯开始时间、转弯中心点坐标等参数，规划出一条基准飞行轨迹。而往往在实际实践的过程中，飞行器在前直线轨迹(S1)段飞行器一般不会发生偏航；而在进行转弯时，由于飞行器制动系统产生误差，其转弯中圆弧轨迹(S2)和转弯结束后沿圆弧切线方向直线轨迹(S3)段，飞行器会发生偏航；即飞行器在飞行中实际飞行轨迹并不能与基准飞行轨迹完全重合，需要对飞行器进行纠偏控制，在控制过程中需要对基准飞行轨迹与实际飞行轨迹之间的角度以及位置距离两个变量进行控制，使用PID控制计算处理来调整飞行器的转弯飞行轨迹偏差。本发明具体步骤如下：

步骤1：根据飞行器起始位置(0,0)和目标位置，建立如图1所示的飞行器的转弯弹道曲线，确定飞行器的基准飞行轨迹参数：转弯起始位置(X_Turn，0)，转弯半径R_Turn，转弯角度φ_Turn；

步骤2：根据上述参数估算转弯角速度ω_Turn＝57.3×V_m/R_Turn，转弯时间t_Turn＝|φ_Turn|/ω_Turn，确定转弯中心点坐标(X_Turn,Z_Turn)，其中V_m为飞行器在X-Y平面的平均线速度，

步骤3：根据轨迹偏航角和偏航位置计算设定所述飞行器的实际飞行轨迹参数：偏航角转弯开始时间t_{Turn_Begin_psi}、偏航角转弯结束时间t_{Turn_End_psi}＝t_{Turn_Begin_psi}+t_Turn；侧偏转弯开始时间t_{Turn_Begin}、侧偏转弯结束时间t_{Turn_Begin}+t_Turn；

步骤4：根据上述参数计算参控侧偏距ΔZ，侧偏指令Z_c、偏航角指令ψ_c。

如下式(Ⅰ)、(Ⅱ)所示：

其中，t为实际飞行时间，Z_c为通过纠偏计算设定的侧偏指令，(X,Z)为实际飞行位置，Sign(φ_Turn)为函数符号，

具体地，式(Ⅰ)中，ψ_c为偏航角指令，并规定1)沿发射方向左侧转弯，即沿X轴发射，沿Y轴正方向转弯，则转弯角度为正；2)沿发射方向右侧转弯，即沿X轴发射，沿Y轴负方向转弯，转弯角度为负。

更具体地，当未到达偏航角转弯开始时间t_{Turn_Begin_psi}时，飞行器偏航角指令为0°，飞行器状态为平飞不转弯；当到达偏航角转弯开始时间t_{Turn_Begin_psi}时，偏航角指令ψ_c以所估算转弯角速度ω_Turn在规定时间内达到所需转弯角度，转弯角度大于0°为正，小于0°为负；当到达偏航角转弯结束时间t_{Turn_End_psi}时，航角指令ψ_c保持所需角度，飞行器状态为沿圆弧切线方向平飞。

具体地，式(Ⅱ)中，ΔZ为侧偏距指令，并规定1)基准飞行轨迹左侧其侧偏距则为正，即沿飞行器机头方向若实际飞行轨迹位于基准飞行轨迹左侧其侧偏距则为正；2)基准飞行轨迹右侧其侧偏距则为负，沿飞行器机头方向若实际飞行轨迹位于基准飞行轨迹右侧其侧偏距则为负。

更具体地，式(Ⅱ)中，侧偏距指令ΔZ为实际飞行轨迹与基准飞行轨迹之间的侧偏差值，当未达到侧偏转弯开始时间t_{Turn_Begin}时，设定侧偏指令Z_c为0，侧偏距指令ΔZ为飞行器飞行侧偏值，即实际飞行轨迹距离X轴发射直线侧偏数值等于侧偏距指令ΔZ；当达到侧偏转弯开始时间t_{Turn_Begin}时，设定侧偏指令Z_c为转弯半径R_Turn，侧偏距指令ΔZ为飞行器转弯飞行侧偏值，即实际飞行转弯圆弧与基准飞行转弯圆弧之间的距离值；当达到侧偏转弯结束时间t_{Turn_End}时，偏距指令ΔZ为实际飞行转弯圆弧切线方向轨迹与基准转弯圆弧切线方向轨迹之间的距离数值。

更具体地，式(Ⅱ)中，当达到侧偏转弯结束时间t_{Turn_En}d以后，侧偏距指令ΔZ完成从圆弧轨迹侧偏距到直线轨迹侧偏距的缓慢切变，遂采取按

随时间衰减的方式完成侧偏距的平缓过渡。

步骤5：将所述偏航角指令ψ_c、侧偏距指令ΔZ、侧偏指令Z_c和基准飞行轨迹参数进行PID控制航向电压解算，通过飞行中航向的电压控制航向的动力大小，从而控制所述飞行器的转弯飞行轨迹偏差。

具体地，如图4所示，为飞行器的PID控制解算模块，其可以用于根据上述偏航角指令ψ_c、侧偏距指令ΔZ、侧偏指令Z_c和基准飞行轨迹参数进行PID控制航向电压解算，再通过飞行中航向的电压控制航向的动力大小，从而控制所述飞行器的转弯飞行轨迹偏差。

更具体地，e_psi——kp_psi——u_2h为飞行器航向角比例控制电压。其中，u_2h＝k_2h·(ψ-ψ_c)，k_2h为航向角比例PID控制解算参数，ψ为所述基准飞行轨迹的航向角。上式为偏航角指令ψ_c与基准飞行轨迹参数进行PID控制航向电压解算。

更具体地，wy——kd_psi——u_5h为飞行器航向角控制电压。其中，u_5h＝k_5h·ω_y，k_5h为航向角PID控制解算参数，ω_y为航向角速度，上式为基准飞行轨迹参数φ_Turn进行PID控制航向电压解算。

更具体地，e_z——kp_z——u_3h为飞行器航向侧偏比例控制电压。其中，u_3h＝k_3h·ΔZ，其中，u_3h为所述飞行器的航向侧偏比例控制电压，k_3h为航向侧偏比例PID控制解算参数。上式为侧偏距指令ΔZ进行PID控制航向电压解算。

更具体地，[JFCF_zi]——1/s——ki_z——u_4h为航向侧偏积分控制电压。其中，u_4h＝k_4h·JF₅，u_4h为所述飞行器的航向侧偏积分控制电压，k_4h为航向侧偏积分PID控制解算参数，

为侧偏积分限幅，上式为侧偏距指令ΔZ和基准飞行轨迹参数进行PID控制航向电压解算。

更具体地，图4中[t]为实际飞行时间，t_b为点火起爆时间，t_h为侧偏控制起控时间。

步骤6：将上述航向角比例的控制电压、航向角控制电压、航向侧偏比例控制电压、航向侧偏积分控制电压输入，舵机输出舵控电压u_rh——K_DJ den(s)——deltay控制舵机舵面偏转实现航向控制，从而完成控制飞行器的转弯飞行轨迹偏差。

本发明中还提供一种基于智能纠偏控制引导飞行器转弯的装置，该装置包括：

后台终端，用于根据飞行器的起始位置和目标位置构建基准飞行轨迹，获取所述飞行器的基准飞行轨迹参数信息，并发送至控制系统；其中，所述基准飞行轨迹参数信息包括：转弯起始位置信息、转弯半径信息、转弯角度信息、转弯角速度信息、转弯时间信息以及转弯中心点位置信息；

控制系统，用于接受所述飞行器的基准飞行轨迹参数信息，同时根据轨迹偏航角和偏航位置计算设定所述飞行器的实际飞行轨迹参数信息，并发送至PID控制解算模块；其中，所述实际飞行轨迹参数信息包括：偏航角转弯开始时间信息、偏航角转弯结束时间信息、偏航角指令信息、航向角速度信息、侧偏转弯开始时间信息、侧偏转弯结束时间信息以及侧偏距指令信息；

PID控制解算模块，用于接受所述飞行器的基准飞行轨迹参数信息和实际飞行轨迹参数信息，进行PID控制计算获取航向电压参数信息，以使控制所述飞行器的转弯飞行轨迹偏差；其中，所述航向电压参数信息包括：航向角比例的控制电压信息、航向角控制电压信息、航向侧偏比例控制电压信息、航向侧偏积分控制电压信息。

具体地，后台终端根据飞行器起始位置和目标位置，建立如图1所示的飞行器的转弯弹道曲线，确定飞行器的基准飞行轨迹参数：转弯起始位置(X_Turn，0)，转弯半径R_Turn，转弯角度φ_Turn。然后在根据上述参数估算转弯角速度ω_Turn＝57.3×V_m/R_Turn，转弯时间t_Turn＝|φ_Turn|/ω_Turn，确定转弯中心点坐标(X_Turn,Z_Turn)，其中V_m为飞行器在X-Y平面的平均线速度，

具体地，后台终端向控制系统设定上述实际飞行轨迹参数信息，包括：偏航角转弯开始时间信息、偏航角转弯结束时间信息、偏航角指令信息、侧偏转弯开始时间信息、侧偏转弯结束时间信息以及侧偏距指令信息后，再根据轨迹偏航角和偏航位置计算设定所述飞行器的实际飞行轨迹参数：偏航角转弯开始时间t_{Turn_Begin_psi}、偏航角转弯结束时间t_{Turn_End_psi}＝t_{Turn_Begin_psi}+t_Turn；侧偏转弯开始时间t_{Turn_Begin}、侧偏转弯结束时间t_{Turn_Begin}+t_Turn；同时根据上述参数计算参控侧偏距ΔZ，侧偏指令Z_c、偏航角指令ψ_c。如下式(Ⅰ)、(Ⅱ)所示：

其中，t为实际飞行时间，Z_c为通过纠偏计算设定的侧偏指令，(X,Z)为实际飞行位置，Sign(φ_Turn)为函数符号，

具体地，PID控制解算模块接受所述飞行器的基准飞行轨迹参数信息和实际飞行轨迹参数信息，如图4所示，为飞行器的PID控制解算模块，其可以用于根据上述偏航角指令ψ_c、侧偏距指令ΔZ、侧偏指令Z_c和基准飞行轨迹参数进行PID控制航向电压解算，再通过飞行中航向的电压控制航向的动力大小，从而控制所述飞行器的转弯飞行轨迹偏差。

更具体地，PID控制解算模块包括航向角比例控制器，用于根据偏航角指令信息进行PID控制解算出所述飞行器的航向角比例控制电压。如图4所示，e_psi——kp_psi——u_2h为飞行器航向角比例控制电压。其中，u_2h＝k_2h·(ψ-ψ_c)，k_2h为航向角比例PID控制解算参数，ψ为所述基准飞行轨迹的航向角。上式为偏航角指令ψ_c与基准飞行轨迹参数进行PID控制航向电压解算。

更具体地，PID控制解算模块包括航向角速度控制器，用于根据航向角速度信息进行PID控制解算出所述飞行器的航向角控制电压。wy——kd_psi——u_5h为飞行器航向角控制电压。其中，u_5h＝k_5h·ω_y，k_5h为航向角PID控制解算参数，ω_y为航向角速度，ω_y主要为基准飞行轨迹参数角度φ_Turn和转弯ω_Turn的轨迹变量，上式为基准飞行轨迹参数φ_Turn进行PID控制航向电压解算。

更具体地，PID控制解算模块包括航向侧偏比例控制器，用于根据侧偏距指令信息进行PID控制解算出所述飞行器的航向侧偏比例控制电压。e_z——kp_z——u_3h为飞行器航向侧偏比例控制电压。其中，u_3h＝k_3h·ΔZ，其中，u_3h为所述飞行器的航向侧偏比例控制电压，k_3h为航向侧偏比例PID控制解算参数。上式为侧偏距指令ΔZ进行PID控制航向电压解算。

更具体地，PID控制解算模块包括航向侧偏积分控制器，用于根据侧偏距指令信息进行PID控制解算出所述飞行器的航向侧偏积分控制电压。[JFCF_zi]——1/s——ki_z——u_4h为航向侧偏积分控制电压。其中，u_4h＝k_4h·JF₅，u_4h为所述飞行器的航向侧偏积分控制电压，k_4h为航向侧偏积分PID控制解算参数，

为侧偏积分限幅，上式为侧偏距指令ΔZ和基准飞行轨迹参数进行PID控制航向电压解算。

本发明中，基于智能纠偏控制引导飞行器转弯的装置还包括：

舵机，用于接受航向电压参数信息输出舵控电压，控制舵机舵面偏转实现航向纠偏控制。

传感模块，用于采集所述飞行器的航向角速度信息以及实际飞行位置信息，并发送至控制系统。

更具体地，传感模块包括液浮陀螺和GPS定位装置；其中，液浮陀螺用于螺感知飞行器的航向角速度信息，GPS定位装置用于感知飞行器的实际飞行位置信息。

根据一种具体地实施方式，具体操作方式和步骤如下：

步骤1：如图1所示，构建基准飞行轨迹，由后台终端向飞行器的控制系统设定转弯起始位置为X＝10000m，转弯半径为R＝3000m，转弯角度为

步骤2：控制系统根据飞行器巡航速度约为270m/s，估算转弯角速度为ω_Turn＝5.157rad/s，转弯时间为t_Turn＝29s，确定转弯中心点位置为(10000,3000)。

步骤3：控制系统获得如图2所示偏航:转弯开始时间t_{Turn_Begin_psi}、偏航角转弯结束时间t_{Turn_End_psi}＝t_{Turn_Begin_psi}+t_Turn。获得如图3所示侧偏距转弯开始时间t_{Turn_Begin}、侧偏距转弯结束时间t_{Turn_Begin}+t_Turn。

步骤4：控制系统根据公式(Ⅰ)、(Ⅱ)计算参控侧偏距指令ΔZ，侧偏指令Z_c、偏航角指令ψ_c，飞行器转弯分为三个解算如图1所示分别为S1转弯前直线飞行阶段，S2转弯中圆弧阶段，S3转完后圆弧切线方向直线阶段。S1阶段：飞行器偏航角指令为0°，飞行器平飞不转弯；S2阶段：偏航角指令以所估算角速度在规定时间内达到所需转弯角度，并且用实际飞行轨迹距离基准飞行轨迹转弯中心的距离与转弯半径之差计算侧偏距，因该实例中实际飞行轨迹位于基准飞行轨迹右侧，此时其侧偏距为负，如图3所示；S3阶段：转弯结束后记录转弯结束前一时刻侧偏距作为转弯结束后侧偏距起始值，将其缓变为直线轨迹之间的侧边距计算模式，即为在计算圆弧轨迹之间的距离转变为计算直线轨迹之间的距离过程中减少因侧偏距解算不连续所导致的控制问题。

步骤5：将偏航角指令ψ_c、侧偏距指令ΔZ、侧偏指令Z_c和基准飞行轨迹参数从控制系统输入端进入PID控制解算模块解算出航向控制电压。

步骤6：PID控制解算模块再将航向控制电输入舵机，通过舵机输出舵控电压，控制舵机舵面偏转实现纠偏控制，并由传感模块将飞行器的航向角速度信息以及实际飞行位置信息反馈给控制系统形成控制回路。

本发明提供了一种可读存储介质，其上存储有运算程序，该运算程序可被处理执行，以使实现上述基于智能纠偏控制引导飞行器转弯的方法。

本发明提供的基于智能纠偏控制引导飞行器转弯的装置，包括上述的可读存储介质。

应当理解，本发明的方法、流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。

上述提到的存储介质可以是只读存储器，磁盘或光盘等。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求及其等同限定。

Claims (10)

1.一种基于智能纠偏控制引导飞行器转弯的方法，其特征在于，所述方法包括：

提供飞行器X-Y平面取向的飞行工作区域；

提供飞行器起始位置(0,0)和目标位置；

构建基准飞行轨迹，获取所述飞行器的基准飞行轨迹参数：转弯起始位置(X_Turn，0)、转弯半径R_Turn、转弯角度φ_Turn；

并根据上述基准飞行轨迹参数进一步计算转弯角速度ω_Turn＝57.3×V_m/R_Turn、转弯时间t_Turn＝|φ_Turn|/ω_Turn、转弯中心点位置(X_Turn,Z_Turn)；

其中，V_m为所述飞行器在X-Y平面的平均线速度，

根据轨迹偏航角计算设定所述飞行器的实际飞行轨迹参数：偏航角转弯开始时间t_{Turn_Begin_psi}、偏航角转弯结束时间t_{Turn_End_psi}＝t_{Turn_Begin_psi}+t_Turn；

并根据上述实际飞行轨迹参数计算偏航角指令ψ_c，如下式(Ⅰ)：

其中，t为实际飞行时间；Sign(φ_Turn)为函数符号，

将所述偏航角指令ψ_c和基准飞行轨迹参数进行PID控制计算，实现所述飞行器的航向纠偏控制。

2.根据权利要求1所述的基于智能纠偏控制引导飞行器转弯的方法，其特征在于，所述方法还包括：

根据轨迹偏航位置计算设定所述飞行器的实际飞行轨迹参数：侧偏转弯开始时间t_{Turn_Begin}、侧偏转弯结束时间t_{Turn_End}＝t_{Turn_Begin}+t_Turn；

并根据上述实际飞行轨迹参数计算侧偏距指令ΔZ，如下式(Ⅱ)：

其中，t为实际飞行时间，Z_c为通过纠偏计算设定的侧偏指令，(X,Z)为实际飞行位置，

将所述侧偏距指令ΔZ和基准飞行轨迹参数进行PID控制计算，以使控制所述飞行器的转弯飞行轨迹偏差。

3.根据权利要求1所述的基于智能纠偏控制引导飞行器转弯的方法，其特征在于，所述方法还包括：

根据u_2h＝k_2h·(ψ-ψ_c)进行偏航角指令ψ_c与基准飞行轨迹参数的PID控制解算所述飞行器的航向角比例控制电压；其中，u_2h为所述飞行器的航向角比例的控制电压，k_2h为航向角比例PID控制解算参数，ψ为所述基准飞行轨迹的航向角。

4.根据权利要求2所述的基于智能纠偏控制引导飞行器转弯的方法，其特征在于，所述方法还包括：

根据u_3h＝k_3h·ΔZ进行侧偏距指令ΔZ的PID控制解算所述飞行器的航向侧偏比例控制电压；其中，u_3h为所述飞行器的航向侧偏比例控制电压，k_3h为航向侧偏比例PID控制解算参数。

5.根据权利要求2所述的基于智能纠偏控制引导飞行器转弯的方法，其特征在于，所述方法还包括：

根据u_4h＝k_4h·JF₅进行侧偏距指令ΔZ的PID控制解算所述飞行器的航向侧偏积分控制电压；其中，u_4h为所述飞行器的航向侧偏积分控制电压，k_4h为航向侧偏积分PID控制解算参数，

为侧偏积分限幅，t_S为点火起爆时间，t_h为侧偏控制起控时间。

6.一种基于智能纠偏控制引导飞行器转弯的装置，其特征在于，所述装置包括：

后台终端，用于根据飞行器的起始位置和目标位置构建基准飞行轨迹，获取所述飞行器的基准飞行轨迹参数信息，并发送至控制系统；其中，所述基准飞行轨迹参数信息包括：所述飞行器的转弯起始位置信息、转弯半径信息、转弯角度信息、转弯角速度信息、转弯时间信息以及转弯中心点位置信息；

控制系统，用于接受所述飞行器的基准飞行轨迹参数信息，同时根据轨迹偏航角和偏航位置计算设定所述飞行器的实际飞行轨迹参数信息，并发送至PID控制解算模块；其中，所述实际飞行轨迹参数信息包括：偏航角转弯开始时间信息、偏航角转弯结束时间信息、偏航角指令信息、航向角速度信息、侧偏转弯开始时间信息、侧偏转弯结束时间信息以及侧偏距指令信息；

PID控制解算模块，用于接受所述飞行器的基准飞行轨迹参数信息和实际飞行轨迹参数信息，进行PID控制计算获取航向电压参数信息，以使控制所述飞行器的转弯飞行轨迹偏差；其中，所述航向电压参数信息包括：航向角比例的控制电压信息、航向角控制电压信息、航向侧偏比例控制电压信息、航向侧偏积分控制电压信息。

7.根据权利要求6所述的基于智能纠偏控制引导飞行器转弯的装置，其特征在于，所述PID控制解算模块包括：

航向角比例控制器，用于根据偏航角指令信息进行PID控制解算出所述飞行器的航向角比例控制电压；

航向角速度控制器，用于根据航向角速度信息进行PID控制解算出所述飞行器的航向角控制电压；

航向侧偏比例控制器，用于根据侧偏距指令信息进行PID控制解算出所述飞行器的航向侧偏比例控制电压；

航向侧偏积分控制器，用于根据侧偏距指令信息进行PID控制解算出所述飞行器的航向侧偏积分控制电压。

8.根据权利要求6所述的基于智能纠偏控制引导飞行器转弯的装置，其特征在于，所述装置还包括：

舵机，用于接受所述飞行器的航向电压参数信息，并输出舵控电压，从而控制舵机的舵面偏转实现航向纠偏控制。

9.根据权利要求6所述的基于智能纠偏控制引导飞行器转弯的装置，其特征在于，所述装置还包括：

传感模块，用于采集所述飞行器的航向角速度信息以及实际飞行位置信息，并发送至控制系统。

10.根据权利要求9所述的基于智能纠偏控制引导飞行器转弯的装置，其特征在于，所述传感模块包括液浮陀螺和GPS定位装置。

Images