基于双端双向测距的飞行器室内定位装置与悬停方法

技术领域

本申请涉及飞行器应用技术领域，具体涉及一种基于双端双向测距的飞行器室内定位装置与悬停方法。

背景技术

随着飞行器技术的不断发展，飞行器室内应用的场景越来越多。在绝大多数室内应用场景中，由于建筑环境遮挡和卫星定位信号不良等诸多因素，人们无法将户外应用场景中常用的卫星定位技术直接应用到室内场景中。现有的视觉光流技术在应用时容易受应用场景中环境因素的限制，比如光照条件较差、地面纹理或墙面纹理丰富性小等情况，往往使得所获取的视觉光流数据质量较差，因此在对这些视觉光流数据进行处理和解算时就无法满足飞行器在室内精确悬停的要求。

目前对于飞行器的控制通常采用同时定位与地图构建(SimultaneousLocalization and Mapping，SLAM)技术，其中SLAM技术主要包括激光SLAM和视觉SLAM。其中激光SLAM设备一般对安装有结构要求，且激光SLAM设备的体积和重量都较大，不便于在一些小体积或低荷载能力的飞行器上部署；而视觉SLAM无法直接获取环境中的点云数据，需要在飞行器移动过程中通过提取、匹配特征点来逐步构建空间地图，并且易受环境光影响，对设备的算力要求也较高。

此外，还可以采用超宽带技术(Ultra Wide Band，UWB)，UWB技术通过发送纳秒级的超窄脉冲来传输数据，但由于电磁波在空气中的传播速度较快，业界常用的单边双向测距方法会在测量飞行时间的过程中因定时精度等问题产生一定的误差，因此UWB技术在小范围室内应用场景中相对来说定位误差稍大，无法满足一些精细作业需求。

此外，还可以采用惯性导航，惯性导航主要利用惯性传感器采集飞行器的运动数据，根据运动数据进行积分定位或基于航位推测，经过运算得到飞行器的定位信息。但随着飞行器运动时长的增加，惯性导航的定位误差也在不断积累，并且飞行器在大多数应用场景中有6个自由度，更容易产生定位误差。

由此可知，现有技术主要存在如下缺点：

1.视觉光流技术对环境条件依赖度较高，且往往只能完成悬停任务。

2.SLAM技术部分设备不便于在小型飞行器上部署，或对室内飞行设备需求较高。

3.UWB技术由于飞行时间测量精度问题，其定位精度不能满足部分精细作业需求。

4.惯性导航会积累定位误差，无法依靠其本身消除定位误差。

发明内容

为解决上述现有技术的不足，本申请提出了一种基于双端双向测距的飞行器室内定位装置和悬停方法。

本申请第一方面提供一种基于双端双向测距的飞行器室内定位装置，该飞行器室内定位装置包括数据主站单元、数据从站单元和飞行器控制单元；

数据主站单元，分别与数据从站单元和飞行器控制单元通信连接，用于发送获取飞行器的定位信息的指令，并处理相关数据以得到飞行器的定位信息；

数据从站单元包含第一从站节点、第二从站节点和第三从站节点，用于基于测距指令分别测量飞行器至第一从站节点、第二从站节点和第三从站节点的距离，并将所述距离反馈至所述数据主站单元；

飞行器控制单元，用于从所述数据主站单元获取所述定位信息并基于所述定位信息控制所述飞行器执行预设飞行任务。

其中，第一从站节点、第二从站节点和第三个从站节点设置于室内空间中的三个预设位置，均包括微控制模块(Mirco Controller Unit，MCU)、WiFi(Wireless Fidelity)模块和超声波收发模块；

WiFi模块和超声波收发模块均与MCU模块通信连接，WiFi模块与数据主站单元无线通信连接；

超声波收发模块包括接收端口和发送端口，MCU模块控制超声波收发模块通过接收端口和发送端口分别接收和/或发出预设频段的超声波。

其中，数据主站单元安装于飞行器之下，包括另一MCU模块、另一WiFi模块、红外测距模块、另一超声波收发模块和窄带滤波模块，且所述另一超声波收发模块的结构和功能与所述超声波收发模块相同；另一MCU模块分别与另一WiFi模块和红外测距模块通信连接，且通过窄带滤波模块与另一超声波收发模块通信连接，且控制数据主站单元与第一从站节点、第二从站节点和第三从站节点分别进行无线通信；另一MCU模块控制红外测距模块测量飞行器的飞行高度，且控制另一超声收发模块发送和/或接收另一预设频段的超声波，且控制窄带滤波模块滤除螺旋桨产生的噪声。

其中，另一MCU模块为一体集成的单片机模块，包括定时器、串口接口、数模转换器、IIC接口和STM32F765主控芯片。

其中，飞行器控制单元包括供电模块和飞控模块，供电模块为飞控模块提供稳定的工作电压输入；飞控模块通过串口接口与另一MCU模块连接，用于获取从数据主站单元返回的定位信息，且基于定位信息控制飞行器执行悬停任务。

此外，本申请的第二方面提供一种基于双端双向测距的飞行器室内悬停方法，该飞行器室内悬停方法应用于如本申请第一方面所述的飞行器室内定位装置，该飞行器室内悬停方法包括：

分别获取飞行器至第一从站节点、第二从站节点和第三从站节点的距离以及飞行器的飞行高度；

根据距离和飞行高度进行空间位置解算，得到飞行器的空间位置坐标；

对多个空间位置坐标进行近似微分运算，得到飞行器的飞行速度，且将飞行高度转换为标准光流接口的光流数据；

利用飞控模块基于光流数据启用光流飞行模式，以使飞行器执行预设飞行任务。

其中，分别获取飞行器至第一从站节点、第二从站节点和第三从站节点的距离和飞行器的飞行高度的步骤，包括：

根据WiFi模块和另一WiFi模块的握手协议，分别建立数据主站单元与第一从站节点、第二从站节点和第三从站节点的通信连接；

基于双端双向超声波测距方法，分时段分别测量飞行器至第一从站节点、第二从站节点和第三从站节点的距离，且控制红外测距模块测量飞行器的飞行高度；

其中以飞行器至第一从站节点的距离为例，其表达式为：

其中，c为常温常压下超声波波速，d为超声波收发模块的发射端口至接收端口之间的距离，T

其中，根据直线距离和飞行高度进行空间位置解算，得到飞行器的空间位置坐标步骤，包括：

基于室内空间建立xyz三维空间直角坐标系；

将飞行高度作为飞行器在直角坐标系下的z轴坐标，且将数据主站单元至第一从站节点、第二从站节点和第三从站节点的距离分别投影至x和y二维投影平面，基于几何投影与数学解算，分别得到第一从站节点、第二从站节点和第三从站节点在二维投影平面中至飞行器的投影距离，从而解算出飞行器的空间位置坐标；

其中飞行器在直角坐标系下的x轴坐标和y轴坐标表达式为：

其中，r

其中，对多个空间位置坐标进行近似微分运算，得到飞行器的飞行速度，且将飞行速度转换为标准光流接口的光流数据步骤，包括：

利用定时器记录相邻两次得到空间位置坐标的时间间隔；

根据时间间隔和对应时间间隔的空间位置坐标的变化量进行计算，分别得到飞行器在x轴方向的速度分量和在y轴方向的速度分量；

将x轴方向的速度分量和y轴方向的速度分量以预定接口协议伪装成标准光流接口的光流数据，如CX-OF光流数据。

本申请的有益效果是：本申请通过提供一种基于双端双向测距的飞行器室内定位装置与悬停方法，使用超声波作为测距媒介，且超声波的波速相对适中，则可减少在测量飞行时间时产生的误差，同时采用双端双向超声波测距方法从算法方面进一步提高了测距的精度，从而提高了飞行器室内定位的准确度以及悬停的稳定性。该飞行器室内定位装置由于集成化高所占空间较小所以便于部署，对环境条件以及飞行器设备性能的要求较为普适，扩大了适用场景，提高了室内飞行器工作的鲁棒性。本申请提供飞行器的定位信息和标准光流接口的光流数据，使得装置和方法有较强的通用性和易用性，从而提高了飞行器二次开发的工作效率和兼容性。

附图说明

图1是本申请中飞行器室内定位装置的一实施例结构示意图；

图2是本申请中飞行器室内悬停方法的大致步骤流程示意图；

图3是本申请中数据主站单元和第一从站节点的超声波收发时序示意图；

图4是本申请中数据主站单元和第一从站节点、第二从站节点和第三从站节点分时段测距流程的信令时序流程示意图；

图5是本申请中数据主站单元和第一从站节点进行测距的信令时序流程示意图；

图6是本申请中飞行器与基站模块空间位置及几何关系的一实施例示意图；

图7是本申请中飞行器与基站模块在投影二维平面的几何关系的一实施例示意图。

具体实施方式

在本发明的描述中，需要理解的是，所描述的实施例仅为本发明的一部分实施例，并不是全部实施例。

下面通过具体实施方式结合附图对本申请作进一步详细说明。

请参考图1，本申请公开了一种基于双端双向测距的飞行器室内定位装置，该飞行器室内定位装置包括数据从站单元M1、数据主站单元M2和飞行器控制单元M3。

其中，数据主站单元M2分别与数据从站单元M1和飞行器控制单元M3通信连接。因此，飞行器控制单元M3可以获取从数据主站单元M2返回的定位信息数据；数据主站单元M2可以向数据从站单元M1发送测距指令，从而得到数据从站单元M1向其返回的距离数据以解算出飞行器的定位信息。

具体地，下面对从站单元M1、数据主站单元M2和飞行器控制单元M3分别进行说明。

数据从站单元M1包含第一从站节点、第二从站节点和第三从站节点，用于测量飞行器至第一从站节点、第二从站节点和第三从站节点的距离。如图1所示，三个从站节点的结构和功能都相同，仅用第一、第二、第三作出区分。第一从站节点、第二从站节点和第三个从站节点设置于室内空间中的三个预设位置，也即三个从站节点分别摆放在室内空间的三个不同位置，其摆放位置可自行规划，满足实际需要即可，此处不作具体限定。在一具体实施例中，三个从站节点应尽量摆放在靠近室内空间边缘的位置，以便达到较好的定位效果。

具体地，第一从站节点包括MCU模块101、WiFi模块102和超声波收发模块103；第二从站节点包括MCU模块111、WiFi模块112和超声波收发模块113；第三从站节点包括MCU模块121、WiFi模块122和超声波收发模块123。

其中，MCU模块101与WiFi模块102通信连接，从而控制第一从站节点与数据主站单元M2进行无线通信，MCU模块111与WiFi模块112通信连接，从而控制第二从站节点与数据主站单元M2进行无线通信，MCU模块121与WiFi模块122无线连接，从而控制第三从站节点与数据主站单元M2进行无线通信。

超声波收发模块103、超声波收发模块113和超声波收发模块123均包含接收端口和发送端口，MCU模块101、MCU模块111和MCU模块121分别与超声波收发模块103、超声波收发模块113和超声波收发模块123一一对应连接并控制其接收和/或发出预设频段的超声波，比如特定频段的超声波。也即，超声波收发模块103的接收端口与发送端口可以同时工作运行而互不打扰如此以提升工作效率，也可以分别工作以满足顺序运行。在一具体实施例中，超声波收发模块103的收发端口应朝向室内中心，以便更好地与数据主站单元M2协作工作。在一较优的实施例中，所述超声波收发模块103应配有感应角度更大的收发端口，从而达到更佳的测距效果。

数据主站单元M2安装在飞行器下方，如此便于采集飞行器的相关数据，并解算出飞行器的位置数据，也即定位信息，以便后续将飞行器位置数据伪装成标准光流接口的光流数据。数据主站单元M2包括另一MCU模块、另一WiFi模块、红外测距模块、另一超声波收发模块和窄带滤波模块，且另一超声波收发模块的结构和功能与超声波收发模块相同。具体地，如图1所示，数据主站单元M2包括MCU模块201、WiFi模块202、超声波收发模块203、窄带滤波模块204以及红外测距模块205。在一实施例中，MCU模块201为一STM32F765芯片主控的小型单片机模块，集成了高精度定时器、串口接口、数模转换器和集成电路总线接口(Inter-Integrated Circuit，IIC)等外设资源；红外测距模块205的感应部位应正对室内地面，以提升其测得高度数据的准确性。

MCU模块201与WiFi模块202连接，并控制数据主站单元M2与数据从站单元M1的三个从站节点进行无线通信；且MCU模块201与红外测距模块205连接并控制其测量飞行器的飞行高度数据。MCU模块201还通过窄带滤波模块204与超声波收发模块203相连，控制其发送和/或接收另一预设频段的超声波，此处的另一预设频段的超声波可以与上文中提到的预设频段的超声波相同，也可以不同，可根据实际情况进行选择，此处不做限定。

由于安装在飞行器上的数据主站单元M2与飞行器螺旋桨的距离较近，故使用窄带滤波模块204滤除飞行器螺旋桨产生的噪声，从而提高测距精度和定位精度。在一具体实施例中，超声波收发模块203的收发端口应朝向下方，以便与数据从站单元M1协同分别测量飞行器至第一从站节点、第二从站节点和第三从站节点的距离。在一较优的实施例中，所述超声波收发模块203应配有感应角度更大的收发端口，从而有更佳的测距效果。

飞行器控制单元M3，具体可以为无人机控制单元，用于控制飞行器完成悬停任务，包括供电模块302和飞控模块301。供电模块302与飞控模块301相连，为其提供合适的工作电压，以稳定飞控模块的工作电压。飞控模块301通过串口接口与数据主站单元M2中的MCU模块201相连，从而获取飞行器的定位信息和标准光流接口的光流数据，并通过其内部集成的光流悬停程序控制飞行器执行悬停任务。此外还可通过飞行器定位信息和标准光流接口的光流数据进行适用于特定场景的二次应用开发。

相应地，本申请还提出一种基于双端双向测距的飞行器室内悬停方法，该飞行器室内悬停方法应用于如本申请第一方面所述的飞行器室内定位装置，如图2所示，该飞行器室内悬停方法包括以下步骤。

S10、数据采集步骤：分别获取飞行器至第一从站节点、第二从站节点和第三从站节点的距离以及飞行器的飞行高度。具体地，通过数据主站单元M2的红外测距模块205可以测得飞行器的飞行高度。如图3所示，数据主站单元M2和数据从站单元M1通过双端双向超声波测距方法可分别测得飞行器至三个从站节点的距离，即飞行器至三个从站节点的主从站距离数据。数据主站单元M2将采集到的飞行高度和主从站距离汇总于MCU模块201以便后续对飞行高度和主从站距离的处理。

如图4所示，数据主站单元M2采用分时复用的方法在三个独立时间段内分别与第一从站节点、第二从站节点和第三从站节点进行测距，以保证数据主站单元M2在与每个从站节点的测距过程中，不会受到其他来源超声波的干扰。数据主站单元M2和每个从站节点测距都通过WiFi模块进行无线通信来组织测距过程，使得系统分时段有序运行。数据主站单元M2与每个从站节点测距时所采用的双端双向超声波测距方法会在下文的解算过程中作出较为详细的说明。

S20、空间位置解算步骤：通过步骤S10中所述的距离数据和高度数据进行空间位置解算，得到飞行器的空间位置坐标。基于室内空间建立参考坐标系，具体地在室内空间中建立xyz三维空间直角坐标系。将三个从站节点和飞行器投影到平行于地面的二维平面(即x、y二维平面)，并通过步骤S10中的飞行高度数据(即z轴数据)将飞行器至三个从站节点的距离数据进行转换，从而得到二维投影平面上飞行器至三个从站节点的投影距离。在x、y二维投影平面中，数据主站单元M2中的MCU模块201使用三边定位算法根据投影距离解算出飞行器在空间中的x轴和y轴坐标，并且将高度数据记录为z轴坐标，如此利用得到的x轴、y轴和z轴坐标构成飞行器的空间位置坐标。为使该步骤便于理解，空间位置坐标的具体解算方法会在下文进一步作出详细说明。

S30、数据接口转换步骤：将飞行器的空间位置坐标转换成标准光流接口的光流数据。一般来说，可以对多个空间位置坐标进行近似微分运算，得到飞行器的飞行速度，进而将飞行速度转换为标准光流接口的光流数据。具体地，数据主站单元M2在解算步骤S20中的空间位置坐标的同时，利用定时器记录相邻两次得到空间位置坐标的时间间隔，再将相邻两次x轴与y轴坐标的位移差值结合所述时间间隔进行微分计算，也即根据时间间隔和对应时间间隔的空间位置坐标的变化量进行计算，从而得到x和y两个方向的速度分量，此处的速度分量是满足预设精度的近似值，可简称为近似速度分量。再将近似速度分量根据通信协议约定来打包成标准光流接口的光流数据。在一具体实施例中，将速度分量按照通信协议约定伪装成标准光流接口的CX-OF数据流，并通过串口将其传给飞控模块301。

S40、悬停控制步骤：利用飞控模块301通过串口接收步骤S30中标准光流接口的光流数据，并基于光流数据使得飞行器运行在光流飞行模式。具体地，飞控模块通过其内部集成的光流悬停程序，对CX-OF数据流中的光流速度信息进行积分运算和数据融合等操作，进而通过其内置的PID控制算法控制飞行器执行室内悬停任务，以实现室内飞行器悬停。

在此对基于双端双向测距的飞行器室内悬停方法中步骤S10所提到的双端双向超声波测距方法作出具体说明。如图5所示，以数据主站单元M2和第一从站节点为例，说明双端双向超声波测距方法的具体实施，数据主站单元M2与第二从站节点、第三从站节点使用双端双向超声波测距方法的过程与此具体实施相同，此处不再赘述。

结合图3，双端双向超声波测距方法基于双端双向测距原理，如图3所示，该测距方法分为两个测距过程，为便于理解以第一、第二作为区分，此处先对测距过程中的超声波收发过程进行说明。首先，是第一测距过程中包含的超声波U1收发过程，数据主站单元M2在T

如图5所示，首先是由数据主站单元向第一从站节点发起的第一测距过程。根据WiFi模块和另一WiFi模块的握手协议，分别建立数据主站单元与第一从站节点、第二从站节点和第三从站节点的通信连接。数据主站单元向第一从站节点发送WiFi通信包a1，请求开始第一测距过程。第一从站节点收到通信包a1，并向数据主站单元返回确认通信包b1；待数据主站单元收到确认通信包b1后，开始第一测距过程。随后进行图3所示的超声波U1收发过程，在此过程中数据主站单元M2记录其接收超声波U1时刻的时间戳T

如图5所示，其次是由第一从站节点向数据主站单元发起的第二测距过程。第一从站节点向数据主站单元发送WiFi通信包b3，请求开始第二测距过程。数据主站单元收到通信包b3，并向第一从站节点返回确认通信包a3；第一从站节点收到确认通信包a3后，开始第二测距过程。随后进行图3所示的超声波U2收发过程，在此过程中数据主站单元M2记录其收到超声波U2时刻的时间戳T

超声波收发模块103、113、123、203物理结构相同，其发射端口至接收端口的距离为已知常量d，常温常压下超声波波速为已知常量c，通过第一测距过程和第二测距过程得到时间戳T

从数据主站单元M2向第一从站节点发起测距：

D＝c*(T

从第一从站节点向数据主站单元M2发起测距：

D＝c*(T

超声波收发模块203的发送端至接收端距离d可表示为式(3)和(4)：

由数据主站单元M2发射端至接收端：

d＝c*(T

由第一从站节点发射端至接收端：

d＝c*(T

为减小测距误差和消除时间同步起点变量T

联立式(1)(2)(3)(4)(5)，可计算出数据主站单元至第一从站节点的距离D(即飞行器至第一从站节点的距离)的式(6)：

其中，c为常温常压下超声波波速，d为超声波收发模块203的发射端口至接收端口之间的距离，T

根据式(6)，c和d为已知常量，则计算距离D需要数据主站单元接收超声波U2和超声波U1的时间差T

从计算推导过程可知，该双端双向测距方法在降低测距误差的同时，利用数据主站单元M2和数据从站单元M1都是双端收发超声波的特性，消除了对发出超声波时间点T

在此对计算空间位置的具体方法做出说明。如图6所示，通过步骤S20中高度数据z可将数据主站单元M2至三个从站节点的距离d

如图7所示，投影到x和y二维平面后，得到飞行器与三个从站节点的二维平面几何关系，三圆交点处(x，y)即为飞行器。利用如下三边定位算法可计算步骤S20中的飞行器在空间中的x轴和y轴坐标。

将方程组(8)化简并转化为矩阵方程(9)：

其中，r

因此，通过计算矩阵方程(9)可解出包含x轴坐标和y轴坐标的矩阵，从而得到飞行器的x轴和y轴坐标，结合步骤S20中所述的高度数据z，即可得到飞行器完整的空间位置坐标(x，y，z)。

综上所述，本申请能够在优化测距方法的基础上提高测距精度，进而保障飞行器在室内的定位精度和悬停稳定性。本申请的飞行器室内定位装置和飞行器室内悬停方法对工作环境条件和飞行器的性能要求较为普适，提高了飞行器在极端应用场景中飞行的鲁棒性。此外，本申请提供飞行器定位信息和标准光流接口的光流数据，使得装置和方法有较强的通用性和易用性，从而提高了飞行器二次开发的工作效率和兼容性。

以上内容是结合具体实施方式对本申请所作的进一步详细说明，不能认定本申请的具体实施只局限于这些说明。对于本领域的普通技术人员而言，在不脱离本发明构思的情况下可以对这些实施例进行多种变化、修改和替换。