无人机速度测量方法、装置及无人机与流程

本发明涉及无人机领域，尤其涉及一种无人机速度测量方法、装置及无人机。

背景技术：

本部分旨在为权利要求书中陈述的本发明的实施方式提供背景或上下文。此处的描述不因为包括在本部分中就承认是现有技术。

传统无人机利用惯性测量单元(Inertial Measurement Unit，IMU)测得的三围加速度数据、三轴角速度数据、以及磁罗盘测得航向数据进行处理分析，进行姿态控制处理；无人机控制系统同时引用卫星信号测得经纬度、高度数据、及航速数据，进行飞行位置的计算和校正。

由于惯性测量单元中的加速度计存在系统误差和测量误差，且通常对加速度进行积分可以得到当前速度，故这个方法会产生累积误差，因此需要使用全球定位系统(Global Positioning System，简称为GPS)提供的位置/速度信息对飞机当前速度进行修正。目前的无人机在GPS信号消失或者有较大误差的时候，飞行控制系统无法获得一个有效的速度信息来修正无人机加速度计带来的误差，从而导致飞机无法对目前的飞行速度做出准确的计算，导致飞机就会慢慢失控，且飞机悬停时会发生漂移。

技术实现要素：

本发明提出一种无人机速度测量方法、装置及无人机，以解决现有的无人机在GPS信号消失或者有较大误差的时候，不能获得一个有效的速度信息来修正无人机加速度计带来的误差的问题。

为了达到上述目的，本发明实施例提出一种无人机速度测量方法，包括：在第一时刻的第一位置，获取无人机的激光雷达的多个激光束在多个平面的第一组距离；在第二时刻的第二位置，获取所述多个激光束在所述多个平面的第二组距离；依据预设的几何关系处理所述第一组距离和所述第二组距离，得到所述无人机在同一个坐标系下所述第一位置和所述第二位置的坐标；依据在同一个坐标系下所述第一位置和所述第二位置的位置坐标，以及所述第一时刻和所述第二时刻的时间差，确定所述无人机的移动速度信息。

为了达到上述目的，本发明实施例还提出一种无人机速度测量装置，包括：距离获取模块，用于在第一时刻的第一位置，获取无人机的激光雷达的多个激光束在多个平面的第一组距离；且在第二时刻的第二位置，获取所述多个激光束在所述多个平面的第二组距离；坐标处理模块，用于依据预设的几何关系处理所述第一组距离和所述第二组距离，得到所述无人机在同一个坐标系下所述第一位置和所述第二位置的坐标；速度确定模块，用于依据在同一个坐标系下所述第一位置和所述第二位置的位置坐标，以及所述第一时刻和所述第二时刻的时间差，确定所述无人机的移动速度信息。

为了达到上述目的，本发明实施例还提出一种无人机，所述无人机包括：激光雷达；存储器；一个或多个处理器；一个或多个模块，所述一个或多个模块存储在所述存储器中并且由所述一个或多个处理器执行，所述一个或多个模块包括：距离获取模块，用于在第一时刻的第一位置，获取所述激光雷达的多个激光束在多个平面的第一组距离；且在第二时刻的第二位置，获取所述多个激光束在所述多个平面的第二组距离；坐标处理模块，用于依据预设的几何关系处理所述第一组距离和所述第二组距离，得到所述无人机在同一个坐标系下所述第一位置和所述第二位置的坐标；速度确定模块，用于依据在同一个坐标系下所述第一位置和所述第二位置的位置坐标，以及所述第一时刻和所述第二时刻的时间差，确定所述无人机的移动速度信息。

本发明实施方式提出的无人机速度测量方法、装置及无人机，使得无人机在GPS信号不稳定或者消失的时候，能够利用机载的激光雷达信号进行对自身速度的检测，从而能够利用该检测的速度对从IMU获得的速度进行修正，以实现无人机的悬停。

附图说明

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

图1为本发明实施例的无人机的方框示意图；

图2为本发明实施例的无人机速度测量方法的处理流程图；

图3为本发明实施例的计算原理示意图；

图4为本发明实施例的无人机速度测量装置的结构示意图；

图5为图4所示实施例的坐标处理模块302的结构示意图；

图6为图4所示实施例的坐标处理模块302的另一实施例的结构示意图；

图7为本发明另一实施例的无人机速度测量装置的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本领域技术技术人员知道，本发明的实施方式可以实现为一种系统、装置、设备、方法或计算机程序产品。因此，本公开可以具体实现为以下形式，即：完全的硬件、完全的软件(包括固件、驻留软件、微代码等)，或者硬件和软件结合的形式。

下面参考本发明的若干代表性实施方式，详细阐释本发明的原理和精神。

如图1所示，是本发明实施例提供的无人机100的方框示意图。所述无人机100的组成元件包括飞行控制装置101、存储器102、处理器103、输入输出单元104、功能设备105、定位设备106以及其他。各元件相互之间直接或间接地电性连接，以实现数据的传输或交互。例如，这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。所述飞行控制装置101包括至少一个可以软件或固件(firmware)的形式存储于所述存储器102或处理器103中的软件功能模块。所述处理器103用于执行该存储于存储器102或处理器103中的可执行的软件功能模块，例如所述飞行控制装置101包括的软件功能模块或计算机程序。所述处理器103在接收到执行指令后，执行所述可执行的软件功能模块包括的程序，本发明实施例任一实施例揭示的无人机所执行的方法可以应用于处理器103中，或者由处理器103实现。

其中，所述存储器102用于存储所述无人机100的各类数据。所述存储器102可以是所述无人机100的内部存储器，也可为可移除的存储器，存储器101可以是，但不限于，随机存取存储器102(Random Access Memory，RAM)，只读存储器102(Read Only Memory，ROM)，可编程只读存储器102(Programmable Read-Only Memory，PROM)，可擦除只读存储器102(Erasable Programmable Read-Only Memory，EPROM)，电可擦除只读存储器102(Electric Erasable Programmable Read-Only Memory，EEPROM)等。其中，存储器102可用于存储程序。

处理器103可能是一种集成电路芯片，具有信号的处理能力。上述的处理器103可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器103(Network Processor，简称NP)等；还可以是数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器103可以是微处理器或者该处理器103也可以是任何常规的处理器等。

输入输出单元104用于接收无人机100的控制终端通过有线或者无线发送的数据，或者，输入输出单元104通过有线或者无线发送无人机100的数据到所述无人机100的控制终端，实现控制终端与该无人机100的交互。

功能设备105包括摄像设备、LED灯、蜂鸣器以及红外发生装置等，用于无人机执行特定飞行任务(例如，拍照，灯的闪烁，红外线遥测等)。

定位设备106包括定位装置(例如，惯性测量单元IMU)、导航装置(例如，GPS)、激光雷达及气压计等，用于对无人机的当前位置进行定位，其中，激光雷达作为一种重要的传感器，广泛应用在无人机领域。激光雷达是一种光学遥感技术，它通过首先向目标物体发射一束激光，再根据接收－反射的时间间隔来确定目标物体的实际距离，然后根据距离及激光发射的角度，通过简单的几何变化可以推导出物体的位置信息。由于激光的传播受外界影响小，能够检测的距离一般可达100m以上。

本发明的实施例提出一种无人机速度测量方法、装置及无人机，利用上述定位设备106中激光雷达提供的距离信息，计算出无人机在两个时刻的绝对位置坐标，从而得出无人机在当前时刻的即时速度，利用这个速度来替代或者补偿GPS给出的速度对当前无人机速度进行修正。

图2为本发明实施例的无人机速度测量方法的处理流程图。如图所示，包括：

步骤S101，在第一时刻的第一位置，获取无人机的激光雷达的多个激光束在多个平面的第一组距离；

步骤S102，在第二时刻的第二位置，获取所述多个激光束在所述多个平面的第二组距离；

步骤S103，依据预设的几何关系处理所述第一组距离和所述第二组距离，得到所述无人机在同一个坐标系下所述第一位置和所述第二位置的坐标；

步骤S104，依据在同一个坐标系下所述第一位置和所述第二位置的位置坐标，以及所述第一时刻和所述第二时刻的时间差，确定所述无人机的移动速度信息。

具体实施时，在步骤S101和S102中，可以通过无人机上的激光雷达传感器直接获取到激光照射的距离数据。例如，参看图3，在t₁时刻(第一时刻)，激光雷达的位置为P_t1(第一位置)，一束激光照射到空间中的P₁点，在t₂时刻(第二时刻)，雷达的位置为P_t2(第二位置)，相同方向的一束激光照射到空间中的P₂点，可以直接从激光雷达传感器获取的数据中得到线段P_t1P₁的长度l₁以及线段P_t2P₂的长度l₂。

此时，存在一个参考平面，使得P₁点和P₂点均位于该参考平面上，且t₁时刻(第一时刻)的激光雷达距离该参考平面的距离为l₁，t₂时刻(第二时刻)的激光雷达距离该参考平面的距离为l₂，如图3中所示的平面为β。

在本发明实施例中，对应于图3，确定参考平面β的方法如下：

1)对P₁周围的点进行平面拟合，生成第一平面。具体实施时，取P₁点周围各点进行平面拟合，如果P₁点周围各点到某一平面的距离小于某一阈值，则认为P₁及周围各点都在某一平面上，该平面设为第一平面。

2)对P₂周围的点进行平面拟合，生成第二平面。具体实施时，取P₂点周围各点进行平面拟合，如果P₂点周围各点到某一平面的距离小于某一阈值，则认为P₂及周围各点都在某一平面上，该平面设为第二平面。

3)计算所述第一平面和第二平面的法向量n₁和n₂。

4)判断所述第一平面和第二平面的法向量n₁和n₂的夹角是否小于某一阈值，若小于某一阈值，则认为第一平面与第二平面平行，进而确定所述第一位置点和所述第二位置点位于同一平面β上。如果不平行，则选取其他激光束，重新获取P₁点和P₂点，进行上述的相同处理。

同理，在步骤S101和S102中，获取激光雷达在第一时刻t₁的第一位置P_t1发出的其他方向的激光束与激光雷达在第二时刻t₂的第二位置P_t2发出的相同方向的激光束，根据上述确定参考平面的方法，可以确定对应于该方向的激光束的一第二参考平面，并得到t₁时刻(第一时刻)的激光雷达距离该第二参考平面的距离，以及t₂时刻(第二时刻)的激光雷达距离该第二参考平面的距离。

因此，通过选择不同方向的激光束，可以得到在第一时刻的第一位置，激光雷达的多个激光束在多个平面的第一组距离；以及在第二时刻的第二位置，多个激光束在所述多个平面的第二组距离，所述第一组距离与所述第二组距离根据激光束的方向，存在对应关系，即不同时刻的相同的方向的两束激光对应生成一个参考平面。

具体实施时，在步骤S103中，依据预设的几何关系处理所述第一组距离和所述第二组距离，得到所述无人机在同一个坐标系下所述第一位置和所述第二位置的坐标。在一实施方式中，即是得到所述第二位置在所述第一位置的坐标系下的坐标，其具体实现方法(即预设的几何关系)如下：

在第一位置到参考平面的距离连线上确定一参考点，使得所述参考点到参考平面的距离等于第二位置到参考平面的距离，通过所述参考点生成一平行于所述参考平面的第一辅助平面。同理，根据选择的不同时刻的具有相同方向的一对激光束，可以对应生成第二辅助平面和第三辅助平面，根据所述第一辅助平面、所述第二辅助平面和所述第三辅助平面，得到所述第二位置在所述第一位置的坐标系下的坐标。

结合图3详细阐述步骤S103的具体实现过程，在线段P_t1P₁上取参考点A，使得AP₁＝l₂，通过所述参考点A生成一平行于所述参考平面β的第一辅助平面α，根据平面几何知识，得知该第一辅助平面α是唯一确定的。

理论上，如果可以得到三个互不平行的辅助平面(第一辅助平面、第二辅助平面和第三辅助平面)，即可通过线性方程组得到P_t2在t₁时刻的坐标系下的坐标，但是，考虑到测量误差，可以获取多个(大于3个)互不平行的辅助平面α_i。

(1)如果只利用三个互不平行的辅助平面，采用的计算方法如下：

1、根据所述第一辅助平面的法向量及对应的参考点在所述第一时刻的坐标系下的坐标，生成所述第一辅助平面的方程式。继续参考图3的实施例，平面α在t₁时刻的坐标系下表示为：A₁x+B₁y+C₁z+D₁＝0，其中，A₁,B₁,C₁,D₁为该平面方程的参数，由平面β的法向量n₁及点A坐标确定，若n₁＝(a,b,c)，A点坐标为(x_A,y_A,z_A)，那么：A₁＝a，B₁＝b，C₁＝c，D₁＝-(ax_A+by_A+cz_A)。根据平行四边形法则，易知P_t2位于平面α上，即P_t2在t₁时刻的坐标系下的坐标(x_t2,y_t2,z_t2)满足A₁x_t2+B₁y_t2+C₁z_t2+D₁＝0。

2、根据所述第二辅助平面的法向量及对应的参考点在所述第一时刻的坐标系下的坐标，生成所述第二辅助平面的方程式。根据相同的计算方法同样可以得到第二辅助平面的方程式A₂x+B₂y+C₂z+D₂＝0，P_t2在t₁时刻的坐标系下的坐标(x_t2,y_t2,z_t2)同样满足A₂x_t2+B₂y_t2+C₂z_t2+D₂＝0。

3、根据所述第三辅助平面的法向量及对应的参考点在所述第一时刻的坐标系下的坐标，生成所述第三辅助平面的方程式。根据相同的计算方法同样可以得到第三辅助平面的方程式A₃x+B₃y+C₃z+D₃＝0，P_t2在t₁时刻的坐标系下的坐标(x_t2,y_t2,z_t2)同样满足A₃x_t2+B₃y_t2+C₃z_t2+D₃＝0。

4、利用所述第一辅助平面的方程式、第二辅助平面的方程式、第三辅助平面的方程式，计算生成所述第二位置在所述第一位置的坐标系下的坐标。

具体实施时，通过求解下述线性方程组，获取P_t2在第一位置的坐标系下的坐标(x_t2,y_t2,z_t2)。

(2)如果利用多个(大于3个)互不平行的辅助平面，采用的计算方法如下所示：

1、建立所述第二位置在所述第一位置的坐标系下的坐标与所述第二位置到每一辅助平面的距离的目标函数。

在一实施方式中，该目标函数为：其中，Distance(P_t2,α_i)表示点P_t2到平面α_i的距离。

2、通过最小化所述目标函数生成所述第二位置点在所述第一时刻的坐标系下的坐标。

具体实施时，在所述步骤S104中，在根据以上两种方法的任一种得到第二位置在所述第一位置的坐标系下的坐标P_t2后，根据所述第一时刻的第一位置的坐标(P_t1点的坐标)、第二位置点在所述第一位置的坐标系下的坐标P_t2(x_t2,y_t2,z_t2)以及第一时刻和第二时刻的时间差(t₂-t₁)，计算所述激光雷达测得的无人机速度，从而确定无人机的移动速度信息。

其中，在一实施方式中，确定无人机的移动速度信息，包括判断所述无人机的飞行速度参数是否与目标飞行参数一致，在不一致的情况下，调整所述飞行速度参数到所述目标飞行参数。通过修正飞行速度参数，从而实现无人机的悬停。

另外，在另一实施方式中，确定所述无人机的移动速度信息之后，还包括：

通过扩展卡尔曼滤波EKF处理所述飞行速度信息，以及所述无人机上IMU测量的加速度信息，将所述处理后的所述无人机上IMU测量的加速度信息和所述无人机的飞行速度信息发送给所述无人机的飞行控制装置，其中，所述飞行控制装置依据所述无人机上惯性测量单元IMU测量的加速度信息和所述无人机的飞行速度信息确定所述无人机的飞行速度参数。

如果存在GPS位置速度信息，则利用所述激光雷达的速度，并结合IMU的速度信息和GPS位置速度信息，通过基于扩展卡尔曼滤波进行数据融合后，对无人机的速度进行修正。例如，将激光雷达的速度，IMU的速度信息和GPS位置速度信息进行基于扩展卡尔曼滤波的数据融合处理，生成融合后的无人机速度v，如果v与无人机的目标速度相等，则证明无人机的速度无需修正，如果不相等，无人机的飞控通过控制加速度向速度偏移的相反方向以对速度进行修正。

应当注意，尽管在附图中以特定顺序描述了本发明方法的操作，但是，这并非要求或者暗示必须按照该特定顺序来执行这些操作，或是必须执行全部所示的操作才能实现期望的结果。附加地或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。

在介绍了本发明示例性实施方式的方法之后，接下来，参考图3对本发明示例性实施方式的无人机速度测量装置进行介绍。该装置的实施可以参见上述方法的实施，重复之处不再赘述。以下所使用的术语“模块”和“单元”，可以是实现预定功能的软件和/或硬件。尽管以下实施例所描述的模块较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

图4为本发明实施例的无人机速度测量装置的结构示意图。如图4所示，包括：

距离获取模块301，用于在第一时刻的第一位置，获取无人机的激光雷达的多个激光束在多个平面的第一组距离；且在第二时刻的第二位置，获取所述多个激光束在所述多个平面的第二组距离；

坐标处理模块302，用于依据预设的几何关系处理所述第一组距离和所述第二组距离，得到所述无人机在同一个坐标系下所述第一位置和所述第二位置的坐标；

速度确定模块303，用于依据在同一个坐标系下所述第一位置和所述第二位置的位置坐标，以及所述第一时刻和所述第二时刻的时间差，确定所述无人机的移动速度信息。

具体实施时，所述坐标处理模块302用于依据预设的几何关系处理所述第一组距离和所述第二组距离，得到所述无人机在同一个坐标系下所述第一位置和所述第二位置的坐标，如图5所示，其具体包括：

距离提取单元3021，用于获取所述多个激光束的第一激光束在所述第一时刻照射到的第一位置点的第一距离，以及所述多个激光束中与所述第一时刻相同方向的第二激光束在所述第二时刻照射到的第二位置点的第二距离，其中，所述第一距离为所述第一位置到所述第一位置点的距离，所述第二距离为所述第二位置到所述第二位置点的距离；

辅助平面生成单元3022，用于将所述第一位置点和所述第二位置点共同所在的平面生成参考平面，在所述第一距离的连线上确定一参考点，使得所述参考点与所述第一位置点的距离等于所述第二距离，通过所述参考点生成一平行于所述参考平面的第一辅助平面；且，当所述距离提取单元从所述多个激光束中提取与所述第一激光束和所述第二激光束不同方向的激光束的距离信息时，所述辅助平面生成单元还用于生成第二辅助平面和第三辅助平面；

坐标计算单元3023，用于根据所述第一辅助平面、所述第二辅助平面和所述第三辅助平面，得到所述第一位置与所述第二位置在所述第一位置的坐标系下的坐标。

具体实施时，所述辅助平面生成单元3022还生成除所述第一辅助平面、所述第二辅助平面、所述第三辅助平面之外的其他多个辅助平面；

对应的，如图6所示，所述坐标处理模块302还包括：

目标函数建立单元3024，用于建立所述第二位置在所述第一位置的坐标系下的坐标与所述第二位置到每一辅助平面的距离的目标函数；

最小化单元3025，用于最小化所述目标函数，生成所述第二位置在所述第一位置的坐标系下的坐标。

具体实施时，所述速度确定模块303用于确定所述无人机的飞行速度参数，具体包括：判断所述无人机的飞行速度参数是否于目标飞行参数一致，在不一致的情况下，调整所述飞行速度参数到所述目标飞行参数。

具体实施时，如图7所示，本发明实施例的无人机测量装置还包括：

EKF处理单元304，用于通过扩展卡尔曼滤波EKF处理所述飞行速度信息，以及所述无人机上IMU测量的加速度信息，将所述处理后的所述无人机上IMU测量的加速度信息和所述无人机的飞行速度信息发送给所述无人机的飞行控制装置，其中，所述飞行控制装置依据所述无人机上惯性测量单元IMU测量的加速度信息和所述无人机的飞行速度信息确定所述无人机的飞行速度参数。

本发明实施方式提出的无人机速度测量方法、装置及无人机，使得无人机在GPS信号不稳定或者消失的时候，能够利用机载的激光雷达信号进行对自身速度的检测，从而能够利用该检测的速度对从IMU获得的速度进行修正，从而实现无人机的悬停。

此外，尽管在上文详细描述中提及了无人机速度测量装置的若干单元，但是这种划分仅仅并非强制性的。实际上，根据本发明的实施方式，上文描述的两个或更多单元的特征和功能可以在一个单元中具体化。同样，上文描述的一个单元的特征和功能也可以进一步划分为由多个单元来具体化。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。