本发明公开一种用于飞行器的高度控制方法及控制系统，该方法步骤包括：1)分别在飞行器中布置雷达、加速度计、气压计以及GPS模块；2)飞行器飞行过程中，若处于雷达有效检测范围内，先获取雷达、加速度计、气压计以及GPS模块检测的信息进行融合，融合后结果再与雷达检测的信息进行融合；否则执行基于加速度计、气压计以及GPS模块的数据融合处理；得到当前高度值、当前垂直速度值以及当前垂直加速度值；3)根据得到的当前高度值、垂直速度值以及垂直加速度值对飞行器进行高度控制。该系统包括检测模块、信息融合模块以及高度控制模块。本发明具有实现控制简单、控制精度及稳定性高且适应性强等优点。
1.一种用于飞行器的高度控制方法，其特征在于，步骤包括：
1)在飞行器中分别布置雷达、加速度计、气压计以及GPS模块；
2)飞行器飞行过程中，若处于雷达有效检测范围内，执行基于雷达的数据融合处理，先获取雷达、加速度计、气压计以及GPS模块检测的信息进行融合，融合后结果再与雷达检测的信息进行融合，得到融合结果；否则执行基于加速度计、气压计以及GPS模块的数据融合处理，得到融合结果；由所述融合结果得到当前高度值、当前垂直速度值以及当前垂直加速度值；
3)根据得到的当前高度值、当前垂直速度值以及当前垂直加速度值对飞行器进行高度控制；
1.1)判断当前是否处于雷达有效检测范围，如果否，转入执行步骤1.2)，否则转入执行步骤1.3)；
1.2)由气压计、GPS模块检测到的信息融合得到第一输出高度hF、第一垂直速度vF；将所述第一输出高度hF作为当前高度值、所述第一垂直速度vF作为当前垂直速度，以及加速度计检测到的信息作为当前垂直加速度；
1.3)由气压计、GPS模块检测到的信息融合得到第一垂直速度vF，以及由雷达、加速度计、气压计、GPS模块检测到的信息融合得到当前垂直加速度aF；将雷达、加速度计检测到的信息，以及得到的当前垂直加速度aF进行融合，得到第二输出高度hR与第二垂直速度vR，并将所述第二输出高度hR作为当前高度值；将所述第一垂直速度vF与第二垂直速度vR进行非线性组合，得到当前垂直速度。
2.根据权利要求1所述的用于飞行器的高度控制方法，其特征在于，所述步骤1.3)中具体基于扩展卡尔曼滤波器进行融合，得到第二输出高度hR与第二垂直速度vR；
1.31)分别建立垂直方向运动状态方程以及状态方程、观测方程，其中所述垂直方向运动状态方程为：其中，
1.32)将建立的所述状态方程、观测方程利用一阶泰勒级数展开，其中所述状态方程展开为：Xk,k-1＝Φk,k-1Xk-1,k-1+Γk-1Uk-1+Wk-1所述观测方程展开为：
其中， T为时间步长，Wk-1为过程噪声， Vk为雷达
1.33)根据所述步骤1.32)展开的状态方程、观测方程，得到基于扩展卡尔曼滤波进行融合的融合方程，其中时间更新方程为：状态更新方程为：
Kk＝Pk,k-1HT[HPk,k-1HT+Rk]-1
其中，Rk为过程噪声协方差矩阵，Qk-1为观测噪声协方差矩阵。
3.根据权利要求1所述的用于飞行器的高度控制方法，其特征在于，所述步骤1.3)中具体按下式进行非线性组合；
其中，V为当前垂直速度，ω为非线性组合权值，hR-max为雷达能够检测到的最大高度值，hRarar为雷达检测到的高度值。
4.根据权利要求1～3中任意一项所述的用于飞行器的高度控制方法，其特征在于，所述步骤3)中还包括数据跳变修正步骤，具体步骤为：获取当前高度值，如果当前时刻与上一时刻得到的高度值之间的差值大于预设阈值，且当前时刻与上一时刻得到的高度值分别由不同所述数据融合处理得到，将当前时刻得到的高度值加上期望距离得到最终高度值，并作为修正后的目标高度值输出，所述期望距离为当前垂直速度以最大可实现的减速度所得到的距离值；否则取历史时刻高度值作为目标高度值输出。
5.根据权利要求1～3中任意一项所述的用于飞行器的高度控制方法，其特征在于：所述步骤2)中若处于雷达有效检测范围内，执行所述基于雷达的数据融合处理前，还包括雷达数据健康检测，以及得到当前高度值时，还包括姿态补偿高度步骤；
所述雷达数据健康检测的具体步骤为：若雷达在连续预设个时刻内未检测到目标，设定检测数据为有效值，并获取历史检测到的数据进行补充；若超过预设个时刻仍未检测到目标，则设定超过预设个时刻后检测数据为无效值；若当前时刻检测数据与上一时刻检测数据均有效时，且两个时刻的检测数据之间的跳变差值大于预设跳变阈值，则将当前时刻检测数据设定为无效值；
所述姿态补偿高度的具体步骤为：当飞行过程中姿态发生变化时，由姿态角对得到的高度值进行补偿。
6.根据权利要求1～3中任意一项所述的用于飞行器的高度控制方法，其特征在于，所述步骤3)具体根据得到的当前高度值、当前垂直速度值以及当前垂直加速度值，依次通过P控制、PD控制以及PID控制实现高度控制；
所述P控制、PD控制以及PID控制实现高度控制的具体步骤为：获取当前高度值以及目标高度值，经过P控制输出目标速度；由输入油门值计算飞行器垂直方向爬升速率，根据所述爬升速率以及目标速度计算期望速度；
获取当前垂直速度以及所述期望速度，经过PD控制输出目标加速度；
获取当前垂直加速度以及所述目标加速度，经过PID控制输出控制参数控制油门输出。
7.根据权利要求1～3中任意一项所述的用于飞行器的高度控制方法，其特征在于：所述雷达为毫米波雷达。
8.一种用于飞行器的高度控制系统，其特征在于包括：
检测模块，包括在飞行器中分别布置的雷达传感器、加速度计、气压计以及GPS模块；
信息融合模块，飞行器飞行过程中，若处于雷达有效检测范围内，执行基于雷达的数据融合处理，先获取雷达、加速度计、气压计以及GPS模块检测的信息进行融合，融合后结果再与雷达检测的信息进行融合，得到融合结果；否则执行基于加速度计、气压计以及GPS模块的数据融合处理，得到融合结果；由所述融合结果得到当前高度值、当前垂直速度值以及当前垂直加速度值；
高度控制模块，用于根据得到的当前高度值、当前垂直速度值以及当前垂直加速度值对飞行器进行高度控制；
判断单元，用于判断当前是否处于雷达有效检测范围，如果是，转入执行第二数据融合处理单元，否则转入执行第一数据融合处理单元；
第一数据融合处理单元，用于由气压计、GPS模块检测到的信息融合得到第一输出高度hF、第一垂直速度vF；将第一输出高度hF作为当前高度值、第一垂直速度vF作为当前垂直速度，以及加速度计检测到的信息作为当前垂直加速度；
第二数据融合处理单元，用于由气压计、GPS模块检测到的信息融合得到第一垂直速度vF，以及由雷达、加速度计、气压计、GPS模块检测到的信息融合得到当前垂直加速度aF；将雷达、加速度计检测到的信息，以及得到的当前垂直加速度aF进行融合，得到第二输出高度hR与第二垂直速度vR，并将第二输出高度hR作为当前高度值；将第一垂直速度vF与第二垂直速度vR进行非线性组合，得到当前垂直速度。
9.根据权利要求8所述的高度控制系统，其特征在于，所述高度控制模块包括依次连接的P控制单元、PD控制单元以及PID控制单元，所述P控制单元包括P控制器以及期望速度计算单元；
所述P控制器输入当前高度值以及目标高度值，输出经过P控制的目标速度至所述期望速度计算单元，所述期望速度计算单元输入由输入油门值计算飞行器垂直方向爬升速率，以及所述目标速度进行计算，输出期望速度；
所述PD控制单元输入当前垂直速度以及所述期望速度，经过PD控制输出目标加速度；
所述PID控制单元输入当前垂直加速度以及所述目标加速度，经过PID控制输出控制参数以控制油门输出。
一种用于飞行器的高度控制方法及控制系统
技术领域
[0001] 本发明涉及飞行控制技术领域，尤其涉及一种用于飞行器的高度控制方法及控制系统。
背景技术
[0002] 飞行器(如无人机)控制系统中，高度控制是基于实时测量到的高度实现，因而高度控制的平稳性主要即取决于高度计的测量精度和实时性。目前飞行器中通常是直接由高度计进行高度测量，高度计的类型较多，如GPS、气压计、超声波传感器等，其中GPS只适用于空旷环境、没有树木或无建筑物遮挡且信号稳定时进行测高，相对高度精度低，且GPS数据更新频率为1Hz，实时性差；气压计容易受气流影响，测高漂移大；超声波测距范围短，容易受外界环境干扰，且数据更新频率低，无法满足高空精准定高要求，不适合应用于10米以上高度和水面、草地、丛林等环境下定高。各类型高度计普遍存在测量高度精度低、数据不稳定、实时性差，且容易受外界环境干扰等问题，对于定高控制在低空定高时，还可能出现严重的测量高度漂移现象，不能反映真实高度，因而直接基于高度计数据进行高度控制，难以控制无人机平稳飞行。
[0003] 有从业者提出结合多个传感器信息进行高度控制，但通常都仅是基于传感器信息的简单线性融合等，各类型传感器本身精度低，且受环境等因素制约，因而经过线性融合后得到的相对高度值精度低、数据漂移大，不能反映无人机飞行真实高度及其变化，尤其在低空中，容易导致高度控制不稳定，使得无人机产生上下漂移，降低无人机飞行的稳定性。
发明内容
[0004] 本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种实现控制简单、控制精度及稳定性高且适应性强的用于飞行器的高度控制方法及控制系统。
[0005] 为解决上述技术问题，本发明提出的技术方案为：
[0006] 一种用于飞行器的高度控制方法，步骤包括：
[0007] 1)分别在飞行器中布置雷达、加速度计、气压计以及GPS模块；
[0008] 2)飞行器飞行过程中，若处于雷达有效检测范围内，执行基于雷达的数据融合处理，先获取雷达、加速度计、气压计以及GPS模块检测的信息进行融合，融合后结果再与雷达检测的信息进行融合，得到融合结果；否则执行基于加速度计、气压计以及GPS模块的数据融合处理，得到融合结果；由所述融合结果得到当前高度值、当前垂直速度值以及当前垂直加速度值；
[0009] 3)根据得到的当前高度值、当前垂直速度值以及当前垂直加速度值对飞行器进行高度控制。
[0010] 作为本发明方法的进一步改进，所述步骤2)的具体步骤为：
[0011] 1.1)判断当前是否处于雷达有效检测范围，如果否，转入执行步骤1.2)，否则转入执行步骤1.3)；
[0012] 1.2)由气压计、GPS模块检测到的信息融合得到第一输出高度hF、第一垂直速度vF；将所述第一输出高度hF作为当前高度值、所述第一垂直速度vF作为当前垂直速度，以及加速度计检测到的信息作为当前垂直加速度；
[0013] 1.3)由气压计、GPS模块检测到的信息融合得到第一垂直速度vF，以及由雷达、加速度计、气压计、GPS模块检测到的信息融合得到当前垂直加速度aF；将雷达、加速度计检测到的信息，以及得到的当前垂直加速度aF进行融合，得到第二输出高度hR与第二垂直速度vR，并将所述第二输出高度hR作为当前高度值；将所述第一垂直速度vF与第二垂直速度vR进行非线性组合，得到当前垂直速度。
[0014] 作为本发明方法的进一步改进，所述步骤1.3)中具体基于扩展卡尔曼滤波器进行融合，得到第二输出高度hR与第二垂直速度vR；
[0015] 所述基于扩展卡尔曼滤波器进行融合的具体步骤为：
[0016] 1.31)分别建立垂直方向运动状态方程以及状态方程、观测方程，其中所述垂直方向运动状态方程为：
[0017]
[0018] 其中，
[0019] 所述状态方程为：
[0020] Xk＝f[Xk-1,k-1]+Wk-1
[0021] 所述观测方程为：
[0022] Zk＝h[Xk-1,k-1]+Vk
[0023] 1.32)将建立的所述状态方程、观测方程利用一阶泰勒级数展开，其中所述状态方程展开为：
[0024] Xk,k-1＝Φk,k-1Xk-1,k-1+Γk-1Uk-1+Wk-1
[0025] 所述观测方程展开为：
[0026] Zk＝HkXk+Vk
[0027] 其中， T为时间步长，Wk-1为过程噪声， Vk为雷达测量噪声；
[0028] 1.33)根据所述步骤1.32)展开的状态方程、观测方程，得到基于扩展卡尔曼滤波进行融合的融合方程，其中时间更新方程为：
[0029]
[0030]
[0031] 状态更新方程为：
[0032]
[0033] Kk＝Pk,k-1HT[HPk,k-1HT+Rk]-1
[0034] Pk＝[I-KkH]Pk,k-1
[0035] 其中，Rk为过程噪声协方差矩阵，Qk-1为观测噪声协方差矩阵。
[0036] 作为本发明方法的进一步改进，所述步骤1.3)中具体按下式进行非线性组合；
[0037] V＝(1-ω)·vF+ω·vR
[0038]
[0039] 其中，V为当前垂直速度，ω为非线性组合权值，hR-max为雷达能够检测到的最大高度值，hRarar为雷达检测到的高度值。
[0040] 作为本发明方法的进一步改进，所述步骤3)中还包括数据跳变修正步骤，具体步骤为：获取当前高度值，如果当前时刻与上一时刻得到的高度值之间的差值大于预设阈值，且当前时刻与上一时刻得到的高度值分别由不同所述数据融合处理得到，将当前时刻得到的高度值加上期望距离得到最终高度值，并作为修正后的目标高度值输出，所述期望距离为当前垂直速度以最大可实现的减速度所得到的距离值；否则取历史时刻高度值作为目标高度值输出。
[0041] 作为本发明方法的进一步改进：所述步骤2)中若处于雷达有效检测范围内，执行所述基于雷达的数据融合处理前，还包括雷达数据健康检测，以及得到当前高度值时，还包括姿态补偿高度步骤；
[0042] 所述雷达数据健康检测的具体步骤为：若雷达在连续预设个时刻内未检测到目标，设定检测数据为有效值，并获取历史检测到的数据进行补充；若超过预设个时刻仍未检测到目标，则设定超过预设个时刻后检测数据为无效值；若当前时刻检测数据与上一时刻检测数据均有效时，且两个时刻的检测数据之间的跳变差值大于预设跳变阈值，则将当前时刻检测数据设定为无效值；
[0043] 所述姿态补偿高度的具体步骤为：当飞行过程中姿态发生变化时，由姿态角对得到的高度值进行补偿。
[0044] 作为本发明方法的进一步改进：所述步骤3)具体根据得到的当前高度值、当前垂直速度值以及当前垂直加速度值，依次通过P控制、PD控制以及PID控制实现高度控制；
[0045] 所述P控制、PD控制以及PID控制实现高度控制的具体步骤为：
[0046] 获取当前高度值以及目标高度值，经过P控制输出目标速度；由输入油门值计算飞行器垂直方向爬升速率，根据所述爬升速率以及目标速度计算期望速度；
[0047] 获取当前垂直速度以及所述期望速度，经过PD控制输出目标加速度；
[0048] 获取当前垂直加速度以及所述目标加速度，经过PID控制输出控制参数控制油门输出。
[0049] 作为本发明方法的进一步改进，所述雷达为毫米波雷达。
[0050] 本发明进一步公开一种用于飞行器的高度控制系统，包括：
[0051] 检测模块，包括分别布置在飞行器中的雷达传感器、加速度计、气压计以及GPS模块；
[0052] 信息融合模块，飞行器飞行过程中，若处于雷达有效检测范围内，执行基于雷达的数据融合处理，先获取雷达、加速度计、气压计以及GPS模块检测的信息进行融合，融合后结果再与雷达检测的信息进行融合，得到融合结果；否则执行基于加速度计、气压计以及GPS模块的数据融合处理，得到融合结果；由所述融合结果得到当前高度值、当前垂直速度值以及当前垂直加速度值；
[0053] 高度控制模块，用于根据得到的当前高度值、当前垂直速度值以及当前垂直加速度值对飞行器进行高度控制。
[0054] 作为本发明系统的进一步改进，所述高度控制模块包括依次连接的P控制单元、PD控制单元以及PID控制单元，所述P控制单元包括P控制器以及期望速度计算单元；
[0055] 所述P控制器输入当前高度值以及目标高度值，输出经过P控制的目标速度至所述期望速度计算单元，所述期望速度计算单元输入由输入油门值计算飞行器垂直方向爬升速率，以及所述目标速度进行计算，输出期望速度；
[0056] 所述PD控制单元输入当前垂直速度以及所述期望速度，经过PD控制输出目标加速度；
[0057] 所述PID控制单元输入当前垂直加速度以及所述目标加速度，经过PID控制输出控制参数以控制油门输出。
[0058] 与现有技术相比，本发明的优点在于：
[0059] 1)本发明在雷达有效检测范围内时，基于雷达高度通过先获取雷达、加速度计、气压计以及GPS模块检测的信息执行第一数据融合，融合后结果再与雷达检测的信息执行第二数据融合，有效融合雷达、加速度计、气压计以及GPS模块信息，得到精准的高度及垂直速度、垂直加速度信息，能够在雷达有效范围内实现精准测高以及飞行的稳定控制，同时能够适应于在复杂的环境下飞行；在雷达有效检测范围外时，切换由加速度计、气压计以及GPS模块数据融合进行高度控制，保证飞行稳定；
[0060] 2)本发明通过在雷达有效检测范围内，由雷达高度与垂直加速度经过扩展卡尔曼滤波器融合得到的第二输出高度确定高度值，在雷达无效检测范围时，则由气压计与GPS融合得到的第一输出高度确定高度值，在有效范围内充分利用雷达数据实现精准测高，减少环境等因素对高度值得干扰，同时保持高度检测实时有效性；
[0061] 3)本发明通过将雷达滤波输出垂直方向速度与气压计和GPS融合速度值进行非线性融合，能够有效提高垂直方向速度精度，实现飞行高精度、稳定控制；
[0062] 4)本发明进一步通过当雷达、气压计测量的绝对高度产生数据跳变时，进行高度修正以调整高度控制，防止高度误差瞬时变化过大造成控制系统产生严重振动，从而保证飞行的高度控制稳定性，提升飞行的平稳性及控制的鲁棒性；
[0063] 5)本发明进一步基于前馈补偿的串级PID控制实现高度控制，先由高度检测值与目标高度值经过P控制输出目标速度，再由垂直速度检测值与期望速度值经过PD控制输出目标加速度，最后由垂直加速度检测值以及目标加速度，经过PID控制输出控制参数控制油门输出，能够实现高度的精准控制。
附图说明
[0064] 图1是本实施例用于飞行器的高度控制方法的实现流程示意图。
[0065] 图2是本实施例数据融合处理的实现原理示意图。
[0066] 图3是本实施例数据跳变修正的具体实现流程示意图。
[0067] 图4是本实施例实现高度控制的实现原理示意图。
具体实施方式
[0068] 以下结合说明书附图和具体优选的实施例对本发明作进一步描述，但并不因此而限制本发明的保护范围。
[0069] 如图1所示，本实施例用于飞行器的高度控制方法，步骤包括：
[0070] 1)分别在飞行器中布置雷达、加速度计、气压计以及GPS模块；
[0071] 2)飞行器飞行过程中，若处于雷达有效检测范围内，执行基于雷达的数据融合处理，先获取雷达、加速度计、气压计以及GPS模块检测的信息进行融合，融合后结果再与雷达检测的信息进行融合，得到融合结果；否则执行基于加速度计、气压计以及GPS模块的数据融合处理，得到融合结果；由融合结果得到当前高度值、当前垂直速度值以及当前垂直加速度值；
[0072] 3)根据得到的当前高度值、当前垂直速度值以及当前垂直加速度值对飞行器进行高度控制。
[0073] 本实施例在雷达有效检测范围内时，基于雷达高度通过先获取雷达、加速度计、气压计以及GPS模块检测的信息执行第一数据融合，融合后结果再与雷达检测的信息执行第二数据融合，有效融合雷达、加速度计、气压计以及GPS模块信息，得到精准的高度及垂直速度、垂直加速度信息，在雷达有效范围内实现精准测高以及飞行的稳定控制，能够适应于在复杂的环境下飞行；在雷达有效检测范围外时，切换由加速度计、气压计以及GPS模块数据融合进行高度控制，保证飞行稳定。本实施例上述控制方法，可适用于小型多旋翼等不同类型无人机系统中在农业植保、测绘、电力线路巡检、管网巡检、摄影、跟拍、地形跟随等场景应用中保持高度平稳，也可以适用于其他飞行器中实现高度控制。
[0074] 本实施例中，步骤2)的具体步骤为：
[0075] 1.1)判断当前是否处于雷达有效检测范围，如果否，转入执行步骤1.2)，否则转入执行步骤1.3)；
[0076] 1.2)由气压计、GPS模块检测到的信息融合得到第一输出高度hF、第一垂直速度vF；将所述第一输出高度hF作为当前高度值、所述第一垂直速度vF作为当前垂直速度，以及加速度计检测到的信息作为当前垂直加速度；
[0077] 1.3)由气压计、GPS模块检测到的信息融合得到第一垂直速度vF，以及由雷达、加速度计、气压计、GPS模块检测到的信息融合得到当前垂直加速度aF；将雷达、加速度计检测到的信息，以及得到的当前垂直加速度aF进行融合，得到第二输出高度hR与第二垂直速度vR，并将所述第二输出高度hR作为当前高度值；将所述第一垂直速度vF与第二垂直速度vR进行非线性组合，得到当前垂直速度。
[0078] 本实施例绝对高度H具有按式(11)确定，即在雷达有效检测范围内，绝对高度H为上述雷达高度与垂直加速度aF融合得到的第二输出高度hR，雷达数据可靠性强，充分利用雷达数据实现精准测高，减少环境等因素对高度值得干扰；在雷达无效检测范围时，绝对高度H则为上述气压计与GPS融合得到的第一输出高度hF，保持高度检测实时有效性。
[0079]
[0080] 本实施例通过将雷达滤波输出垂直方向速度(即第二垂直速度vR)与气压计和GPS融合速度值(即第一垂直速度vF)进行非线性融合，能够有效提高垂直方向速度精度。
[0081] 本实施例中，步骤1.3)中具体基于扩展卡尔曼滤波器进行融合，得到第二输出高度hR与第二垂直速度vR；
[0082] 基于扩展卡尔曼滤波器进行融合的具体步骤为：
[0083] 1.31)分别建立垂直方向运动状态方程以及状态方程、观测方程，其中垂直方向运动状态方程为：
[0084]
[0085] 其中，
[0086] 状态方程为：
[0087] Xk＝f[Xk-1,k-1]+Wk-1 (2)
[0088] 观测方程为：
[0089] Zk＝h[Xk-1,k-1]+Vk (3)
[0090] 1.32)将建立的式(2)状态方程、式(3)观测方程利用一阶泰勒级数展开，其中状态方程展开为：
[0091] Xk,k-1＝Φk,k-1Xk-1,k-1+Γk-1Uk-1+Wk-1 (4)
[0092] 观测方程展开为：
[0093] Zk＝HkXk+Vk(5)
[0094] 其中， T为时间步长；Wk-1为过程噪声，即高斯白噪声；Vk为雷达测量噪声；
[0095] 1.33)根据步骤1.32)展开的状态方程、观测方程，得到基于扩展卡尔曼滤波进行融合的融合方程，其中时间更新方程为：
[0096]
[0097]
[0098] 状态更新方程为：
[0099]
[0100] Kk＝Pk,k-1HT[HPk,k-1HT+Rk]-1 (9)
[0101] Pk＝[I-KkH]Pk,k-1 (10)
[0102] 其中，Rk为过程噪声协方差矩阵，Qk-1为观测噪声协方差矩阵。
[0103] 本实施例基于扩展卡尔曼滤波器将雷达高度与垂直加速度进行数据融合，融合精度高且适应性强，能够融合得到高精度的第二输出高度hR与第二垂直速度vR。在雷达无效检测范围时，由气压计与GPS融合得到第一输出高度hF具体可采用现有融合方式。
[0104] 本实施例中，步骤1.3)中具体按下式进行非线性组合；
[0105] V＝(1-ω)·vF+ω·vR (12)
[0106]
[0107] 其中，V为当前垂直速度，ω为非线性组合权值，hR-max为雷达能够检测到的最大高度值，hRarar为雷达检测到的高度值。
[0108] 如图2所示，本实施例具体在雷达测距范围有效时，利用雷达高度与加速度计、气压计和GPS融合得到垂直加速度aF，通过扩展卡尔曼滤波器融合得到第二输出高度hR与第二垂直速度vR，再通过非线性融合得到绝对高度H与垂直速度V，可实现雷达有效范围内低空精准高度，同时提高垂直方向速度精度，从而实现精准的高度控制。
[0109] 如图3所述，本实施例中步骤3)中还包括数据跳变修正步骤，具体步骤为：
[0110] 获取当前高度值，如果当前时刻k与上一时刻k-1得到的高度值之间的差值大于预设阈值，即|hk-hk-1|＞Δh，且当前时刻k与上一时刻k-1得到的高度值分别由不同数据融合处理得到，如当前时刻k高度值为由上述基于雷达的数据融合处理得到、k-1时刻为上述基于气压计与GPS的数据融合处理得到，即hk:Radar,hk-1:Baro和GPS，或当前时刻k高度值为上述基于气压计与GPS模块的数据融合处理得到，k-1时刻为上述基于雷达的数据融合处理得到，即hk:Baro和GPS,hk-1:Radar；
[0111] 按式(14)将当前时刻得到的高度值加上期望距离得到最终高度值，并作为修正后的目标高度值输出，期望距离为当前垂直速度以最大可实现的减速度所得到的距离值；否则取历史时刻高度值作为目标高度值输出。
[0112] 最终的目标高度表达式具体为：
[0113]
[0114] 其中，hk为当前高度值，为当前垂直速度以最大可实现的减速度所得到的距离值，即期望距离。
[0115] 本实施例通过由基于雷达的数据融合处理、与由基于气压计以及GPS模块的数据融合处理得到的高度值产生跳变时，即当分别由雷达、气压计测量的绝对高度产生数据跳变时，进行高度修正以调整高度控制，防止高度误差瞬时变化过大造成控制系统产生严重振动，从而保证飞行的高度控制稳定性，提升飞行的平稳性及控制的鲁棒性。
[0116] 本实施例中，步骤2)中若处于雷达有效检测范围内，执行基于雷达的数据融合处理前，还包括雷达数据健康检测，以及得到当前高度值时，还包括姿态补偿高度步骤；
[0117] 雷达数据健康检测的具体步骤为：若雷达在连续预设个时刻内未检测到目标，设定检测数据为有效值，并获取历史检测到的数据进行补充；若超过预设个时刻仍未检测到目标，则设定超过预设个时刻后检测数据为无效值；若当前时刻检测数据与上一时刻检测数据均有效时，且两个时刻的检测数据之间的跳变差值大于预设跳变阈值，则将当前时刻检测数据设定为无效值。
[0118] 本实施例，具体当雷达在有效测距范围内没有探测到目标时，反馈零值；若判断到连续5个数据跳零，则设定数据有效，并利用历史数据进行补全；若大于5个数据后仍然存在跳零，则设定5个数据后的数据为无效值；若前后k、k-1时刻两个数据均有效，且两个数据跳变大于预设跳变阈值，则将k时刻数据设定为无效值。通过雷达数据健康检测，能够判断雷达探测过程中是否出现严重数据跳变，在出现严重数据跳变时及时进行数据修正，确保数据有效性。
[0119] 姿态发生变化时，雷达测量高度不能真实反映飞行器高度。本实施例中，姿态补偿高度的具体步骤为：当飞行过程中姿态发生变化时，由姿态角对得到的高度值进行补偿，以使得在姿态发生变化时能够真实反映飞行器高度，提高高度测量精度。
[0120] 本实施例中，步骤1.3)具体根据得到的当前高度值、当前垂直速度值以及当前垂直加速度值，依次通过P控制、PD控制以及PID控制实现高度控制，能够实现高度的精准控制。
[0121] P控制、PD控制以及PID控制实现高度控制的具体步骤为：
[0122] 获取当前高度值以及目标高度值，经过P控制输出目标速度；由输入油门值计算飞行器垂直方向爬升速率，根据爬升速率以及目标速度计算期望速度；
[0123] 获取当前垂直速度以及期望速度，经过PD控制输出目标加速度；
[0124] 获取当前垂直加速度以及目标加速度，经过PID控制输出控制参数控制油门输出。
[0125] 如图4所示，本实施例基于前馈补偿的串级PID控制实现高度控制，具体流程为：
[0126] ①获取目标速度：首先将油门输入值经过前馈补偿控制环路进行计算，通过将输入油门量进行计算，获取垂直方向爬升速率，其中当油门至中时，爬升速率为零，否则爬升速率不等于零；再将目标高度与当前高度，经过P控制得到目标速度，由目标速度与爬升率以及垂直方向的速度和加速度进行计算，最后得到期望速度；
[0127] ②获取目标加速度：将期望速度与当前速度误差利用PD控制器，得到平稳的目标加速度获取目标加速度，由PD组合控制可以改善控制环路输出动态特性，输出偏于稳态，；
[0128] ③控制油门输出：由经过上述油门输入前馈补偿、P控制以及PD控制得到目标加速度，在通过PID控制器转换得到虚拟控制油门输出。
[0129] 本实施例中P控制的增益系数具体可根据当前的高度误差值和当前速度大小确定，误差值和速度越小，则增益系数越大，反之增益系数越小，使得可以减小P控制超调并快速调节位置控制外环，以及减小对高度控制系统造成的不稳定。
[0130] 由于无人机在操作过程中或外界干扰影响，其姿态发生变化，垂直方向受力减少。本实施例PID控制后，还包括油门补偿步骤，具体步骤为：输入当前垂直加速度得到的当前姿态变化量，以及方向舵量进行计算，得到油门输出补偿值对油门输出进行补偿，以控制无人机在期望的高度上。
[0131] 本实施例中，雷达具体采用毫米波雷达。毫米波雷达传感器不受天气影响，对复杂环境适应性强、实时性好(数据更新频率可达50Hz)、测距范围远、盲区小，且测量精度高，在绝对高度40米内由毫米波雷达精准定高，同时基于毫米波雷达测高与气压计、GPS以及加速度计数据融合进行高度控制，40米以上切换为GPS、气压计高度值进行高度控制，能够实现精准测高，保证飞行器飞行稳定。
[0132] 本实施例还公开用于飞行器的高度控制系统，包括：
[0133] 检测模块，包括分别布置在飞行器中的雷达传感器、加速度计、气压计以及GPS模块；
[0134] 信息融合模块，飞行器飞行过程中，若处于雷达有效检测范围内，执行基于雷达的数据融合处理，先获取雷达、加速度计、气压计以及GPS模块检测的信息进行融合，融合后结果再与雷达检测的信息进行融合，得到融合结果；否则执行基于加速度计、气压计以及GPS模块的数据融合处理，得到融合结果；由融合结果得到当前高度值、当前垂直速度值以及当前垂直加速度值；
[0135] 高度控制模块，用于根据得到的当前高度值、当前垂直速度值以及当前垂直加速度值对飞行器进行高度控制。
[0136] 本实施例中，信息融合模块包括：
[0137] 判断单元，用于判断当前是否处于雷达有效检测范围，如果是，转入执行第二数据融合处理单元，否则转入执行第一数据融合处理单元；
[0138] 第一数据融合处理单元，用于由气压计、GPS模块检测到的信息融合得到第一输出高度hF、第一垂直速度vF；将第一输出高度hF作为当前高度值、第一垂直速度vF作为当前垂直速度，以及加速度计检测到的信息作为当前垂直加速度；
[0139] 第二数据融合处理单元，用于由气压计、GPS模块检测到的信息融合得到第一垂直速度vF，以及由雷达、加速度计、气压计、GPS模块检测到的信息融合得到当前垂直加速度aF；将雷达、加速度计检测到的信息，以及得到的当前垂直加速度aF进行融合，得到第二输出高度hR与第二垂直速度vR，并将第二输出高度hR作为当前高度值；将第一垂直速度vF与第二垂直速度vR进行非线性组合，得到当前垂直速度。
[0140] 本实施例中，高度控制模块包括依次连接的P控制单元、PD控制单元以及PID控制单元，P控制单元包括P控制器以及期望速度计算单元；
[0141] P控制器输入当前高度值以及目标高度值，输出经过P控制的目标速度至期望速度计算单元，期望速度计算单元输入由输入油门值计算飞行器垂直方向爬升速率，以及目标速度进行计算，输出期望速度；
[0142] PD控制单元输入当前垂直速度以及期望速度，经过PD控制输出目标加速度；
[0143] PID控制单元输入当前垂直加速度以及目标加速度，经过PID控制输出控制参数以控制油门输出。
[0144] 本实施例中，高度控制模块还包括与PID控制单元连接的油门补偿单元，油门补偿单元输入当前垂直加速度得到的当前姿态变化量，以及方向舵量进行计算，得到油门输出补偿值对油门输出进行补偿。
[0145] 上述只是本发明的较佳实施例，并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明。因此，凡是未脱离本发明技术方案的内容，依据本发明技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均应落在本发明技术方案保护的范围内。
