一种无人机高度控制方法及系统与流程

本发明属于无人机技术领域，尤其涉及一种无人机高度控制方法及系统。

背景技术：

当前对无人机技术的研究主要集中在如何提高无人机的飞行能力，使其能应对不同的飞行环境，并且在复杂的环境中能够进行自适应飞行，安全稳定的完成目标任务。近些年，随着各种新技术的不断应用，无人机系统的复杂性及功能的自动化程度日益增加。但是，由于飞行环境的高度动态化、不确定性以及飞行任务的复杂性，无人机在规划与决策方面面临着新的技术挑战，现有的基于程序化的自动控制策略已经不能满足未来先进多功能无人机对复杂飞行环境下的多任务的需求，自主飞行控制能力的提高将是未来无人机飞行控制系统发展的主要目标。无人机的纵向控制方法是无人机自主飞行研究领域的一个重要课题。无人机的飞行环境具有复杂性和不确定性，在自主飞行过程中会遇到来自上侧和下侧的障碍或地形的起伏变化，因此无人机在垂直方向上必须有一定的自主控制能力，以确保飞行过程的安全和稳定。

无人机在水平方向的控制方法研究起步较早，技术也相对成熟。在纵向控制方面，现阶段无论在学校还是商业应用都缺乏相应研究。现今无人机在纵向的控制研究主要集中在利用气压计实现的高度保持功能，以及融合了超声波传感器数据的简单地形跟随。这种简单的纵向控制策略，既不能满足快速地形跟随中对障碍物的预判，也无法对来自上侧的障碍物进行有效规避。

现有无人机高度控制的方法主要有超声波法、激光/红外法以及视觉法等，这些方法都存在缺陷和不足。

超声波法是利用超声波测距传感器来探测物体的位置，超声波测距使用时间差测距法。超声波测距的成本低，实现方法简单，技术成熟，是无人机测距的常用方法。但是超声波传感器的采样频率低，受吸音材质影响较大。另一方面，单超声波传感器避障存在由于超声波的方向性不好造成对障碍物的定位不精确，存在探测盲区等缺点，而多个超声波传感器同时使用会产生串扰。

激光/红外法是利用电磁波来探测物体的位置。激光传感器使用时间法或相位法来测量距离。激光传感器的光束高度集中，方向性好，因此可得到障碍物的准确位置，弥补了超声波测距在方向性上的不足。激光测距的精确度很高，但是激光传感器成本高，设备体积大，功耗高，而且一些激光传感器发射的激光，对人的眼睛有伤害。现有的红外传感器的测距原理与激光传感器类似，采用与激光传感器不同频率的红外光进行测距，不同的是现有的红外传感器采用测量反射光强度的变化来测量距离。由于被测物体表面材质的反射率不同，所以红外测距的精度较差，距离也有限。

视觉法是利用单目或者双目摄像头来探测物体的位置。无人机通过摄像头采集周围环境的图像信息，并通过图像算法得到自身与被测物体之间的距离，然后实施控制策略。视觉测距法具有探测范围广，同时可以获得物体的形状、速度等信息的优势。但是图像数据在后期处理中，由于边缘锐化、特征提取等图像处理方法计算量大，实时性差，对中央处理器要求高。而且视觉测距法不能检测到玻璃等透明障碍物的存在，另外受视场光线强弱、烟雾的影响很大，成本也较高。

技术实现要素：

本发明实施例所要解决的技术问题在于提供一种无人机高度控制方法及系统，旨在解决现有技术中的无人机的纵向控制不精确的问题。

本发明实施例第一方面提供了一种无人机高度控制方法，所述方法包括：

将目标高度与测量高度做差，得到高度误差，通过气压计测得的高度数据对所述高度误差进行数据修正，将经数据修正后的高度误差输入比例控制器，并结合预估速度，得到目标速度；

将所述目标速度与测量速度做差，得到速度误差，并将所述速度误差输入所述比例控制器，并结合预估加速度，得到目标加速度；

将所述目标加速度输入比例积分微分控制器，并结合预置的控制器初始值，得到油门输出量，以控制无人机的高度；

其中，所述测量高度为下向传感器当前时刻测得的距离数据，所述下向传感器包括多个与无人机平面呈预置角度且方向向下的传感器，所述下向传感器测得的距离数据，具体为各所述与无人机平面呈预置角度且方向向下的传感器测得的距离数据与各所述与无人机平面呈预置角度且方向向下的传感器的权重的乘积之和。

本发明实施例第二方面提供了一种无人机高度控制系统，所述系统包括：

目标速度获取模块，用于将目标高度与测量高度做差，得到高度误差，通过气压计测得的高度数据对所述高度误差进行数据修正，将经数据修正后的高度误差输入比例控制器，并结合预估速度，得到目标速度；

目标加速度获取模块，用于将所述目标速度与测量速度做差，得到速度误差，并将所述速度误差输入所述比例控制器，并结合预估加速度，得到目标加速度；

油门输出量获取模块，用于将所述目标加速度输入比例积分微分控制器，并结合预置的控制器初始值，得到油门输出量，以控制无人机的高度；

其中，所述测量高度为下向传感器当前时刻测得的距离数据，所述下向传感器包括多个与无人机平面呈预置角度且方向向下的传感器，所述下向传感器测得的距离数据，具体为各所述与无人机平面呈预置角度且方向向下的传感器测得的距离数据与各所述与无人机平面呈预置角度且方向向下的传感器的权重的乘积之和。

从上述本发明实施例可知，本发明通过将目标高度与测量高度做差，得到高度误差，通过气压计测得的高度数据对高度误差进行数据修正，将经数据修正后的高度误差输入比例控制器，并结合预估速度，得到目标速度，将目标速度与测量速度做差，得到速度误差，并将速度误差输入比例控制器，并结合预估加速度，得到目标加速度，将目标加速度输入比例积分微分控制器，并结合预置的控制器初始值，得到油门输出量，以控制无人机的高度，其中，测量高度为下向传感器当前时刻测得的距离数据，下向传感器包括多个与无人机平面呈预置角度且方向向下的传感器，下向传感器测得的距离数据，具体为各与无人机平面呈预置角度且方向向下的传感器测得的距离数据与各与无人机平面呈预置角度且方向向下的传感器的权重的乘积之和。相较于现有技术，本发明中的无人机可以实现对地形的快速跟随，高度保持，并对来自无人机上方的障碍物进行有效地规避，本发明的成本低、功耗小、适用场景多、飞行效果好，产品粘性得到提高。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

附图1是本发明第一实施例提供的无人机高度控制方法的实现流程示意图；

附图2是本发明第二实施例提供的无人机高度控制系统的结构示意图；

附图3是本发明中tof距离传感器测量原理框图；

附图4是本发明中pid控制器的数学模型。

具体实施方式

为使得本发明实施例的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而非全部实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅附图1，附图1为本发明第一实施例提供的无人机高度控制方法的实现流程示意图。

本实施例中的上向传感器和下向传感器均可使用窄波束单点测量传感器，即飞行时间(timeofflight，tof)距离传感器，其具有测量距离远(10m)、精度高(10cm)、体积小、成本低，互相之间无串扰，可以多个同时使用的优点。

如附图1所示，该方法主要包括以下步骤：

s101、将目标高度与测量高度做差，得到高度误差，通过气压计测得的高度数据对高度误差进行数据修正，将经数据修正后的高度误差输入比例控制器，并结合预估速度，得到目标速度；

修正后的高度误差的计算公式为：

erroralt＝(tof_alttarget-tof_alt)-(baro_alttarget-baro_alt)①；

其中，①式中的erroralt为修正后的高度误差，tof_alttarget为目标高度，tof_alt为测量高度，baro_alttarget为前一时刻气压计测得的高度数据，baro_alt为当前时刻气压计测得的高度数据。baro_alttarget-baro_alt为前后两个时刻的气压计测得的高度值的差值，因此本步骤中融合了气压计的值。

下向传感器可包括多个与无人机平面呈预置角度且方向向下的传感器，在本实施例中，下向传感器包括两个与无人机平面垂直且方向向下的传感器，其测量的数据分别为tof1和tof2，其权重分别为k1和k2；下向传感器还包括两个向前下方与无人机平面呈30°夹角的传感器，其测量的数据分别为tof3和tof4，其权重分别为k3和k4。下向传感器测得的距离数据，具体为：各与无人机平面呈预置角度且方向向下的传感器测得的距离数据与各与无人机平面呈预置角度且方向向下的传感器的权重的乘积之和。

测量高度tof_alt的计算公式为：

tof_alt＝k1tof1+k2tof2+k3tof3+k4tof4②；

其中，②式中的tof_alt为测量高度，k1和k2均为0.3，k3和k4均为0.2。

需要说明的是，下向传感器的个数、预置角度均可根据需要自行设定，各下向传感器的权重也可自行设置。

目标速度计算公式为：

veltarget＝velestimate+kp0·erroralt③；

其中，③式中的veltarget为目标速度，velestimate为预估速度，是当前时刻无人机在垂直方向上的速度的估算值，估算值的计算不依赖传感器，只与当前油门值和死区上下边界油门值以及最低最高油门值、垂直方向允许的最大速度有关，其中最大速度是用户设定的参数，kp0为比例系数，erroralt为修正后的高度误差。

①式中的目标高度tof_alttarget和测量高度tof_alt的获取方法包括：

在保证地形跟随效果的基础上，为了保证地形跟随中无人机对上侧障碍物有一定预判和规避能力，本实施例可在无人机上方相应安装上向tof距离传感器，①式中的目标高度tof_alttarget的计算方法有以下两种：

当上向传感器当前时刻测得的距离数据小于预置安全距离时，将下向传感器当前时刻测得的距离数据与目标距离的差作为目标高度，目标距离为预置安全距离与上向传感器当前时刻测得的距离数据的差，上向传感器为与无人机平面垂直且方向向上的传感器，此时目标高度tof_alttarget的计算公式为：

tof_alttarget＝tofbottom-(distancesafe-toftop)④；

其中，④式中的tof_alttarget为目标高度，tofbottom为下向传感器当前时刻测得的距离数据，distancesafe为预置安全距离，toftop为上向传感器当前时刻测得的距离数据。

当上向传感器当前时刻测得的距离数据大于或等于预置安全距离时，将无人机油门杆处于死区时，下向传感器测得的距离数据作为目标高度tof_alttarget。

将下向传感器当前时刻测得的距离数据作为测量高度tof_alt。

进一步地，由于环境因素的影响，无人机平面可能不处于标准的水平面，因此下向传感器当前时刻测得的距离数据可能不准确，可以对该距离数据进行修正，修正公式如下：

tof_altcorrect＝tof_alt·cosαpitchcosβroll⑤；

其中，⑤式中的tof_altcorrect为测量高度的修正值，tof_alt为测量高度，αpitch为当前时刻无人机的俯仰角，βroll为当前时刻无人机的横滚角，当前时刻的俯仰角和横滚角均可由陀螺仪测得。可使用tof_altcorrect替换①式中的tof_alt进行计算，以提高获取的高度误差的精确度。由于αpitch和βroll均为陀螺仪积分计算得到的姿态角角度值，因此本方法融合了陀螺仪的值。

s102、将目标速度与测量速度做差，得到速度误差，并将速度误差输入比例控制器，并结合预估加速度，得到目标加速度；

其中，预估速度为当前时刻的测量加速度在垂直方向的分量的积分，无人机当前时刻的测量加速度可由加速度计直接测得。

速度误差计算公式为：

errorvel＝veltarget-accel_vel⑥；

其中，⑥式中的errorvel为速度误差，veltarget为目标速度，accel_vel为测量速度。由于accel_vel是加速度计测得的加速度值通过积分计算得到的速度值，因此本步骤融合了加速度计的值。

目标加速度获取公式为：

acceltarget＝accelestimate+kp1·errorvel⑦；

其中，⑦式中的acceltarget为目标加速度，accelestimate为预估加速度，是前馈控制器输出的加速度值，此加速度值是前后两帧目标速度的差分值，kp1为比例系数，errorvel为速度误差。

s103、将目标加速度输入比例积分微分控制器，并结合预置的控制器初始值，得到油门输出量，以控制无人机的高度。

油门输出量的获取公式为：

其中，⑧式中的throut为油门输出量，acceltarget为目标加速度，kp2、ki2、kd2均为比例系数，c为预置的控制器初始值。

tof技术是一种利用特定频率的近红外光，通过调制光线并计算调制光往返传感器与被测物体间的时间差或相位差来计算传感器到被测物体间距离的测距技术。

tof距离传感器是利用tof技术生产的距离测量传感器。tof距离传感器发出经调制的近红外光，遇物体后反射，传感器通过计算光线发射和反射时间差或相位差，再根据调制光的波长计算出被测物体的距离，以产生深度信息。根据tof测距的原理，待测距离d的计算公式为：

若调制光的角频率为ω，在待测距离d上往返一次产生的相位延迟为φ，则对应时间t可表示为:

t＝φ/ω；

将此关系代入待测距离d的计算公式中，待测距离d可表示为：

式中：

φ——调制光往返一次产生的相位延迟；

ω——调制光的角频率，ω＝2πf；

n——测量光线所包含半波长的个数；

δφ——调制光往返一次产生的相位延迟不足π的部分；

δn——测量光线所包含的半波长数的小数部分。

在给定调制和标准大气条件下，c/4πf是一个常数，此时距离的测量变成了对测量光线所包含半波长个数的测量，即测量n或φ。随着机械加工技术和无线电测相技术的不断发展，现在对φ的测量已经能达到很高的精度。图3为tof距离传感器测量原理框图。

本实施例中使用级联式比例积分微分(pid)控制器，包括比例控制器(p控制器)、积分控制器(i控制器)和微分控制器(d控制器)。pid是比例、积分、微分的缩写，将偏差的比例、偏差的积分和偏差的微分通过线性组合构成控制量，用这一控制量对被控对象进行控制，其数学模型如图4所示。

pid控制器的控制规律为：

式中：

k——采样时刻，k＝0、1、2…；

u0——pid控制器的初始值；

uk——第k次采样时刻的输出值；

ek——第k次采样时刻输入的偏差值；

ek-1——第k-1次采样时刻输入的偏差值；

kp——比例系数,改变kp可提高系统的响应速度，减小静态误差，但太大会增大超调量和稳定时间；

ki——积分系数,与kp的作用基本相似，但要使静态误差为0，必须使用积分；

kd——微分系数,与kp、ki的作用相反，主要是为了减小超调量和稳定时间。

本发明实施例提供的无人机高度控制方法，通过将目标高度与测量高度做差，得到高度误差，通过气压计测得的高度数据对高度误差进行数据修正，将经数据修正后的高度误差输入比例控制器，并结合预估速度，得到目标速度，将目标速度与测量速度做差，得到速度误差，并将速度误差输入比例控制器，并结合预估加速度，得到目标加速度，将目标加速度输入比例积分微分控制器，并结合预置的控制器初始值，得到油门输出量，以控制无人机的高度，其中，测量高度为下向传感器当前时刻测得的距离数据，下向传感器包括多个与无人机平面呈预置角度且方向向下的传感器，下向传感器测得的距离数据，具体为各与无人机平面呈预置角度且方向向下的传感器测得的距离数据与各与无人机平面呈预置角度且方向向下的传感器的权重的乘积之和。相较于现有技术，本发明中的无人机可以实现对地形的快速跟随，高度保持，并对来自无人机上方的障碍物进行有效地规避，本发明的成本低、功耗小、适用场景多、飞行效果好，产品粘性得到提高。

请参阅附图2，附图2是本发明第二实施例提供的无人机高度控制系统的结构示意图，为了便于说明，仅示出了与本发明实施例相关的部分。附图2示例的无人机高度控制系统，主要包括：目标高度获取模块201、测量高度获取模块202、目标速度获取模块203、目标加速度获取模块204以及油门输出量获取模块205。

目标高度获取模块201，用于当上向传感器当前时刻测得的距离数据大于或等于预置安全距离时，将无人机油门杆处于死区时，下向传感器测得的距离数据作为目标高度，上向传感器为与无人机平面垂直且方向向上的传感器，当上向传感器当前时刻测得的距离数据小于预置安全距离时，将下向传感器当前时刻测得的距离数据与目标距离的差作为目标高度，目标距离为预置安全距离与上向传感器当前时刻测得的距离数据的差。

测量高度获取模块202，用于将下向传感器当前时刻测得的距离数据作为测量高度。

其中，测量高度为下向传感器当前时刻测得的距离数据，下向传感器包括多个与无人机平面呈预置角度且方向向下的传感器，下向传感器测得的距离数据，具体为各与无人机平面呈预置角度且方向向下的传感器测得的距离数据与各与无人机平面呈预置角度且方向向下的传感器的权重的乘积之和。

目标速度获取模块203，用于将目标高度与测量高度做差，得到高度误差，通过气压计测得的高度数据对高度误差进行数据修正，将经数据修正后的高度误差输入比例控制器，并结合预估速度，得到目标速度。

目标加速度获取模块204，用于将目标速度与测量速度做差，得到速度误差，并将速度误差输入所述比例控制器，并结合预估加速度，得到目标加速度。

油门输出量获取模块205，用于将目标加速度输入比例积分微分控制器，并结合预置的控制器初始值，得到油门输出量，以控制无人机的高度。

进一步地，测量高度获取模块202，具体用于获取当前时刻下向传感器测得的距离数据，以及，通过陀螺仪测得的角度数据对当前时刻下向传感器测得的距离数据进行数据修正，将数据修正后的距离数据作为测量高度。

进一步地，上向传感器与下向传感器均为窄波束单点测量传感器。

上述各功能模块实现各自功能的具体过程，可参考前述第一实施例提供的无人机高度控制方法的相关内容，此处不再赘述。

本发明实施例提供的无人机高度控制系统，通过将目标高度与测量高度做差，得到高度误差，通过气压计测得的高度数据对高度误差进行数据修正，将经数据修正后的高度误差输入比例控制器，并结合预估速度，得到目标速度，将目标速度与测量速度做差，得到速度误差，并将速度误差输入比例控制器，并结合预估加速度，得到目标加速度，将目标加速度输入比例积分微分控制器，并结合预置的控制器初始值，得到油门输出量，以控制无人机的高度，其中，测量高度为下向传感器当前时刻测得的距离数据，下向传感器包括多个与无人机平面呈预置角度且方向向下的传感器，下向传感器测得的距离数据，具体为各与无人机平面呈预置角度且方向向下的传感器测得的距离数据与各与无人机平面呈预置角度且方向向下的传感器的权重的乘积之和。相较于现有技术，本发明中的无人机可以实现对地形的快速跟随，高度保持，并对来自无人机上方的障碍物进行有效地规避，本发明的成本低、功耗小、适用场景多、飞行效果好，产品粘性得到提高。

需要说明的是，对于前述的各方法实施例，为了简便描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其它顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定都是本发明所必须的。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其它实施例的相关描述。

以上为对本发明所提供的无人机高度控制方法及系统的描述，对于本领域的技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上，本说明书内容不应理解为对本发明的限制。