CN110376543A - 一种三维空间定位方法和系统 - Google Patents

Abstract

本发明公开了一种三维空间定位方法和系统。其中，当信号接收装置检测到第一激光平面信号、第二激光平面信号以及超声波信号时，不对信号接收装置所接收到的多路激光平面信号做融合处理，通过选取信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的激光平面信号接收单元作为最优激光平面信号接收单元，并根据最优激光平面信号接收单元接收到第一激光平面信号的时刻和接收到第二激光平面信号的时刻计算第一旋转角度和第二旋转角度，根据信号接收装置接收到超声波信号的时刻计算信号接收装置与信号发射装置之间的距离，进而确定信号接收装置在三维测量坐标系中的三维坐标，提高了空间定位精度。

Description

一种三维空间定位方法和系统

技术领域

本发明涉及一种三维空间定位方法，同时涉及用于实现三维空间定位方法的系统，属于空间定位技术领域。

背景技术

现有的VR(虚拟现实)设备为了进行头部或者手部的定位，需要采用各种定位方法。通过激光平面信号或超声波信号进行定位的方法，为了避免来自信号发射器的信号被遮挡导致定位失败，通常在信号接收装置上布置若干激光平面信号接收单元或超声波信号接收单元。例如在公告号为CN304056557S的中国外观专利中公开了一种声光信号接收装置，在该声光信号接收装置中，在球体的表面均匀设置有多个激光平面信号接收单元和多个超声波信号接收单元。

在现有技术中，在信号接收装置中，每个信号接收单元(包括激光平面信号接收单元和超声波信号接收单元)均对应于一路处理电路，然后把所有的处理电路的电信号给微处理器处理。对于这些电信号的处理，通常是进行融合后增益放大，然后再发送给信号处理单元进行处理。上述针对超声波信号的融合、增益放大可以通过公开号为CN206369805U的中国实用新型专利中所公开的超声波全向接收电路及设备实现。针对激光平面信号的处理也可以通过类似的接收电路实现。

在上述定位过程中，由于在进行电信号处理时将多路电信号合成了一路，这样仅能大致确定信号接收装置检测到超声波信号及激光平面信号的时间，并且无法分辨出具体哪个信号接收单元接收到的超声波信号或者激光平面信号，利用这些定位信号进行定位计算的准确度不高。

发明内容

本发明所要解决的首要技术问题在于提供一种三维空间定位方法。

本发明所要解决的另一技术问题在于提供一种三维空间定位系统。

为了实现上述目的，本发明采用下述的技术方案：

根据本发明实施例的第一方面，提供一种三维空间定位方法，包括如下步骤：

确定信号发射装置发射超声波信号的发射时刻、发射第一激光平面信号的第一参考时刻和发射第二激光平面信号的第二参考时刻；

确定信号接收装置检测到所述超声波信号的接收时刻、检测到所述第一激光平面信号和第二激光平面信号的第一时刻和第二时刻；其中，所述第一时刻为所述信号接收装置上的最优激光平面信号接收单元接收到所述第一激光平面信号的时刻；所述最优激光平面信号接收单元是所述信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的激光平面信号接收单元；所述第二时刻为所述最优激光平面信号接收单元接收到第二激光平面的时刻；

根据所述第一参考时刻和所述第一时刻确定第一旋转角度；

根据所述第二参考时刻和所述第二时刻确定第二旋转角度；

根据所述超声波信号的发射时刻和所述超声波信号的接收时刻确定信号发射装置和信号接收装置的距离；

根据所述第一旋转角度、第二旋转角度以及所述信号发射装置和信号接收装置之间的距离，确定信号接收装置的三维坐标。

其中较优地，所述确定信号接收装置检测到所述超声波信号的接收时刻，包括：所述接收时刻为所述信号接收装置上的最优超声波信号接收单元接收到所述超声波信号的时刻；最优超声波信号接收单元是所述信号接收装置上多个超声波信号接收单元中最先接收到所述超声波信号的超声波信号接收单元。

其中较优地，所述确定信号发射装置发射第一激光平面信号的第一参考时刻和发射第二激光平面信号的第二参考时刻，包括：确定信号发射装置绕第一旋转轴发射第一激光平面信号的第一参考时刻和绕第二旋转轴发射第二激光平面信号的第二参考时刻；

根据所述第一参考时刻和所述第一时刻确定第一旋转角度；根据所述第二参考时刻和所述第二时刻确定第二旋转角度。

其中较优地，所述确定信号发射装置发射第一激光平面信号的第一参考时刻和第二激光平面信号的第二参考时刻，包括：确定信号发射装置绕一个旋转轴旋转发射呈设定角度的第一激光平面信号和第二激光平面信号的第一参考时刻和第二参考时刻；

根据所述第一参考时刻、所述第一时刻以及所述旋转轴的转速，确定第一旋转角度；根据所述第二参考时刻、所述第二时刻以及所述旋转轴的转速，确定第二旋转角度。

其中较优地，所述根据所述第一旋转角度、所述第二旋转角度和所述最优超声波信号接收单元和所述信号发射装置之间的距离，确定信号接收装置的三维坐标，包括：

根据所述第一旋转角度、所述第二旋转角度以及所述最优超声波信号接收单元与所述信号发射装置之间的距离，结合所述最优激光平面信号接收单元和所述最优超声波信号接收单元在所述信号接收装置中的位置数据，以及所述信号接收装置的姿态数据，确定所述信号接收装置的中心在三维测量坐标系中的三维坐标。

其中较优地，确定所述信号接收装置的中心在三维测量坐标系中的三维坐标的过程，包括如下步骤：

(1)根据最优超声波信号接收单元和所述信号发射装置之间的距离、最优超声波信号接收单元和最优激光平面信号接收单元在信号接收装置上的位置数据，以及信号接收装置的姿态数据，计算最优激光平面信号接收单元和信号发射装置之间的距离；

(2)根据与最优激光平面信号接收单元对应的第一旋转角度、第二旋转角度以及最优激光平面信号接收单元与信号发射装置之间的距离，计算最优激光平面信号接收单元的三维坐标；

(3)结合信号接收装置的姿态数据、最优激光平面信号接收单元在信号接收装置上的位置数据和最优激光平面信号接收单元的三维坐标，计算信号接收装置中心的三维坐标。

其中较优地，上述三维空间定位方法，还包括如下步骤：同步所述信号发射装置和所述信号接收装置的基准时刻。

根据本发明实施例的第二方面，提供一种三维空间定位系统，包括信号发射装置、信号接收装置以及数据处理单元，其中：

所述信号发射装置，用于向所述信号接收装置发射超声波信号、第一激光平面信号以及第二激光平面信号；

所述信号接收装置，用于检测所述超声波信号、所述第一激光平面信号以及所述第二激光平面信号；其中，在所述信号接收装置上设置有多个激光平面信号接收单元；

所述数据处理单元用于选取最优激光平面信号接收单元；其中，所述最优激光平面信号接收单元是所述信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的激光平面信号接收单元；

所述数据处理单元根据所述最优激光平面信号接收单元接收到第一激光平面信号和第二激光平面信号的时刻计算第一旋转角度、第二旋转角度；

所述数据处理单元还用于根据所述信号发射装置发送超声波信号的时刻和所述信号接收装置检测到超声波信号的时刻计算所述信号接收装置与所述信号发射装置之间的距离；

所述数据处理单元还用于根据所述第一旋转角度、所述第二旋转角度、所述信号接收装置与所述信号发射装置之间的距离，计算所述信号接收装置的三维坐标。

其中较优地，在所述信号接收装置上设置有多个超声波信号接收单元；

所述数据处理单元还用于选取最优超声波信号接收单元，其中，所述最优超声波信号接收单元是多个超声波信号接收单元中最先接收到超声波信号的超声波信号接收单元；

所述数据处理单元还用于根据所述信号发射装置发送超声波信号的时刻和所述最优超声波信号接收单元检测到超声波信号的时刻计算所述信号接收装置与所述信号发射装置之间的距离。

其中较优地，所述数据处理单元根据所述第一旋转角度、所述第二旋转角度以及所述最优超声波信号接收单元与所述信号发射装置之间的距离，结合最优激光平面信号接收单元和最优超声波信号接收单元在所述信号接收装置中的位置数据，以及所述信号接收装置的姿态数据，确定所述信号接收装置的中心在三维测量坐标系中的三维坐标。

本发明所提供的三维空间定位方法，不对信号接收装置所接收到的多路并联激光平面信号做融合处理，通过选取接收到最强的激光平面信号的激光平面信号接收单元作为最优激光平面信号接收单元，根据最优激光平面信号接收单元接收到第一激光平面信号的时刻和接收到第二激光平面信号的时刻计算第一旋转角度和第二旋转角度，进而结合信号接收装置和信号发射装置之间的距离，计算信号接收装置的三维坐标，提高了空间定位精度。

此外，本发明所提供的三维空间定位方法，在选取最优激光平面信号接收单元进行角度计算的同时，还可以不对信号接收装置所接收到的多路并联超声波信号做融合处理，通过选取最先接收到超声波信号的超声波信号接收单元作为最优超声波信号接收单元，并根据最优超声波信号接收单元接收到超声波信号的时刻计算信号接收装置和信号发射装置之间的距离，进而通过第一旋转角度、第二旋转角度以及信号接收装置和信号发射装置之间的距离，计算信号接收装置的三维坐标，进一步提高了空间定位精度。

附图说明

图1是本发明第一实施例中，三维空间定位方法的流程图；

图2是一个信号周期T内接收到的三路电信号的波形示意图；

图3是本发明第一实施例中，信号接收装置的三维坐标计算的原理示意图；

图4是本发明第二实施例中，三维空间定位方法的流程图；

图5是本发明第二实施例中，信号接收装置的三维坐标计算的原理示意图；

图6是本发明第三实施例中，三维空间定位方法的流程图；

图7是第三实施例中，位置补偿算法的流程图；

图8是第三实施例中，位置补偿算法的过程示意图；

图9是第三实施例中，位置补偿算法的原理示意图；

图10是第三实施例中，激光平面信号接收单元的三维坐标计算的原理示意图；

图11是应用于上述实施例的三维空间定位系统的结构示意图。

具体实施方式

下面结合附图和具体实施例对本发明的发明内容做进一步地详细说明。应当理解，以下所说明的实施例仅用于说明和解释本申请，并不用于限定本申请。

本发明所提供的用于实现室内空间定位的三维空间定位系统，包括信号发射装置和信号接收装置。信号发射装置作为三维空间定位的参考坐标基点，信号接收装置作为待定位点。

其中，信号发射装置用于向信号接收装置发射超声波信号、第一激光平面信号以及第二激光平面信号；信号接收装置用于检测超声波信号、第一激光平面信号以及第二激光平面信号。其中，在信号接收装置中设置有多个用于检测超声波信号的超声波信号接收单元和多个用于检测激光平面信号的激光平面信号接收单元。较佳地，信号发射装置还用于向信号接收装置发射时间同步信号，信号接收装置还用于从信号发射装置接收时间同步信号，从而实现在信号发射装置和信号接收装置之间同步基准时刻。

第一实施例

如图1所示，本实施例提供一种三维空间定位方法，包括如下步骤：

步骤101：当信号接收装置检测到信号发射装置绕着第一旋转轴旋转发射的第一激光平面信号以及绕着第二旋转轴旋转发射的第二激光平面信号时；

确定信号发射装置绕第一旋转轴发射第一激光平面信号的第一参考时刻；确定信号接收装置检测到第一激光平面信号的第一时刻；其中，第一时刻为信号接收装置上的最优激光平面信号接收单元接收到第一激光平面信号的时刻；最优激光平面信号接收单元是信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的激光平面信号接收单元；

确定信号发射装置绕第二旋转轴发射第二激光平面信号的第二参考时刻；确定信号接收装置检测到第二激光平面信号的第二时刻；其中，第二时刻为上述最优激光平面信号接收单元接收到第二激光平面信号的时刻；

根据信号发射装置发射第一激光平面信号的第一参考时刻、信号接收装置检测到第一激光平面信号的第一时刻，确定第一旋转角度；根据信号发射装置发射第二激光平面信号的第二参考时刻、信号接收装置检测到第二激光平面信号的第二时刻，确定第二旋转角度。

较优地，本步骤可以包括：根据信号发射装置和信号接收装置同步基准时刻之后的第一参考时刻、第二参考时刻、第一时刻以及第二时刻，确定第一旋转角度以及第二旋转角度。通过同步信号发射装置和信号接收装置的基准时刻，确保信号发射装置和信号接收装置的时钟同步，以确保后续进行的计算基于相同的基准，从而提高三维坐标计算的准确性。信号发射装置可以向信号接收装置发射时间同步信号，信号接收装置可以通过接收到时间同步信号的时刻推算出基准时刻。

具体来说，第一旋转角度为第一时刻第一激光平面信号相对于第一旋转轴和第二旋转轴所确定平面的角度，第二旋转角度为第二时刻第二激光平面信号相对于第一旋转轴和第二旋转轴所确定平面的角度。第一参考时刻和第二参考时刻分别为第一、第二激光平面旋转到第一旋转轴和第二旋转轴所确定的平面的时刻。第一参考时刻和第二参考时刻根据信号接收装置接收到同步信号的时刻推算得出。

其中，在第一旋转轴与第二旋转轴所确定的唯一平面或一对平行平面中，第一旋转轴与第二旋转轴之间的夹角为预设角度。较佳地，第一旋转轴与第二旋转轴相互垂直。

在该实施例中，在信号接收装置中设置有多个激光平面信号接收单元用于检测激光平面信号。在一个信号周期T内，当信号发射装置发射第一激光平面信号或第二激光平面信号后，多个激光平面信号接收单元会先后检测到激光平面信号，从而产生多路并联的电信号并向微处理器发送。

为了提高定位的精确度，当信号发射装置发射第一激光平面信号(或第二激光平面信号)后，信号接收装置对于接收到的多路并联电信号不进行信号合并，直接从接收到的多路并联电信号中分辨出最强的(即脉宽最宽的)电信号来自于哪个激光平面信号接收单元，并选取该激光平面信号接收单元作为最优激光平面信号接收单元，然后以该激光平面信号接收单元接收到第一激光平面信号的时刻和该激光平面信号接收单元接收到第二激光平面信号的时刻作为信号接收装置检测到第一激光平面信号的第一时刻和检测到第二激光平面信号的第二时刻。其中，仅需选取接收到信号最强的第一激光平面信号的一个激光平面信号接收单元或者接收到信号最强的第二激光平面信号的一个激光平面信号接收单元作为最优激光平面信号接收单元，即可以上述最优激光平面信号接收单元接收到两个激光平面信号的时刻，进行第一旋转角度和第二旋转角度的计算；针对第一激光平面信号和第二激光平面信号，不必分别选取其中信号最强的激光平面信号接收单元作为最优激光平面信号接收单元来实现后续第一旋转角度和第二旋转角度的计算。

由于激光平面信号在传输的过程中易被反射或散射而发生衰减，因此，其中信号最强的电信号的产生必然是第一激光平面信号或第二激光平面信号直接照射到激光平面信号接收单元后产生的电信号，以该激光平面信号接收单元接收到第一激光平面信号的时刻和接收到第二激光平面信号的时刻作为计算第一旋转角度和第二旋转角度的基础，将会极大地提高定位计算的精度。

下面以信号接收装置在一个信号周期T内收到三路电信号为例对最优激光平面信号接收单元的选择进行说明。如图2所示，在一个信号周期T内，信号接收装置接收到三路激光平面信号接收单元传来的电信号，其中第三路电信号最强(即脉宽最宽的)，则信号接收装置选择第三路电信号对应的激光平面信号接收单元作为最优激光平面信号接收单元，并以该激光平面信号接收单元接收到第一激光平面信号的时刻和接收到第二激光平面信号的时刻作为信号接收装置接收到第一激光平面信号的第一时刻和接收到第二激光平面信号的第二时刻来进行第一旋转角度和第二旋转角度的计算。

具体来说，根据信号发射装置发射第一激光平面信号的第一参考时刻以及最优激光平面信号接收单元检测到第一激光平面信号的第一时刻，确定第一旋转角度；根据信号发射装置发射第二激光平面信号的第二参考时刻以及最优激光平面信号接收单元检测到第二激光平面信号的第二时刻，确定第二旋转角度。

以三维测量坐标系为笛卡尔坐标系为例对第一旋转角度和第二旋转角度的计算过程进行说明。如图3所示，以第一旋转轴为X轴，以第二旋转轴为Y轴，信号发射装置的超声波发射器所在位置为三维测量坐标系的原点。信号发射装置的旋转激光平面发射器A绕着X轴旋转发射第一激光平面信号，转速例如为w_l；信号发射装置的旋转激光平面发射器B绕着Y轴旋转发射第二激光平面信号，转速例如为w₂。X轴和Y轴所确定的平面为XOY平面。在公开号为CN106526537A的中国发明申请中分别给出了第一旋转角度和第二旋转角度的计算过程，在此不再重复。

步骤102：确定信号发射装置发射超声波信号的发射时刻；确定信号接收装置检测到超声波信号的接收时刻，其中，接收时刻为信号接收装置上的最优超声波信号接收单元接收到超声波信号的时刻；最优超声波信号接收单元是信号接收装置上多个超声波信号接收单元中最先接收到超声波信号的超声波信号接收单元；根据信号发射装置发射超声波信号的发射时刻以及最优超声波信号接收单元检测到超声波信号的接收时刻，确定最优超声波信号接收单元与信号发射装置之间的距离。

信号发射装置发射超声波信号的发射时刻可以根据接收到同步信号的时刻推算得出。

其中，步骤102可以包括：根据信号发射装置发射超声波信号的发射时刻以及最优超声波信号接收单元检测到超声波信号的接收时刻，确定超声波信号从信号发射装置到最优超声波信号接收单元的传输时长，根据传输时长以及超声波信号在空气中的传输速度，确定最优超声波信号接收单元与信号发射装置之间的距离。

在该实施例中，在信号接收装置中设置有多个超声波信号接收单元用于检测超声波信号。在一个信号周期T内，当信号发射装置发射一个超声波信号后，多个超声波信号接收单元会先后接收到超声波信号，从而产生多路并联电信号。为了提高定位的精确度，信号接收装置对于接收到的多路超声波信号不进行信号合并，直接从接收到的多路并联电信号中分辨出最先接收到的电信号来自于哪个超声波信号接收单元，并选取该超声波信号接收单元作为最优超声波信号接收单元，然后将该超声波信号接收单元接收到超声波信号的时刻作为信号接收装置接收到超声波信号的接收时刻，将其作为测距计算的基础。由于超声波在传输过程中容易发生反射，当信号发射装置发射一个超声波信号后，超声波信号接收单元可能接收到多个超声波信号，其中最先接收到的超声波信号必然是超声波第一次传输至超声波信号接收单元时检测到的超声波信号，因此，以最优超声波信号接收单元接收到超声波信号的时刻作为信号接收装置检测到超声波信号的接收时刻进行距离计算，可以极大地提高定位计算的精度。

下面以信号接收装置在一个信号周期T内收到三路超声波信号接收单元传来的电信号为例进行说明。如图2所示，在一个信号周期T内，信号接收装置收到三路超声波信号接收单元传来的电信号，从中可见第一路电信号最先，则信号接收装置选择第一路电信号对应的超声波信号接收单元作为最优超声波信号接收单元，并以该超声波信号接收单元接收到超声波信号的时刻来进行距离计算，并将计算所得结果作为信号发射装置与信号接收装置之间的距离。

具体来说，可以根据下式确定最优超声波信号接收单元与信号发射装置之间的距离:

L＝(T₃-T₀')×v，

其中，L为最优超声波信号接收单元与信号发射装置之间的距离，T₃为最优超声波信号接收单元检测到超声波信号的接收时刻，T₀’为信号发射装置发射超声波信号的发射时刻，v为声音在空气中的传播速度。其中，v在1个标准大气压和15℃的条件下约为340米/秒。

步骤103：根据第一旋转角度、第二旋转角度以及信号发射装置与最优超声波信号接收单元之间的距离，确定信号接收装置在三维测量坐标系中的三维坐标(X₀，Y₀，Z₀)。

其中，当三维测量坐标系为笛卡尔坐标系，如图3所示，以第一旋转轴为X轴，以第二旋转轴为Y轴时，可以根据下式求解得到信号接收装置在该三维测量坐标系中的三维坐标：

X₀ ²+Y₀ ²+Z₀ ²＝L²

Y₀×tanα＝X₀×tanβ＝Z₀

其中，(X₀，Y₀，Z₀)表示信号接收装置在三维测量坐标系中的三维坐标，L为最优超声波信号接收单元与信号发射装置之间的距离，α为第一旋转角度，β为第二旋转角度。

第二实施例

下面结合图4至图5对使用另一种信号发射装置实现的三维空间定位方法进行介绍。

如图4所示，本实施例提供一种三维空间定位方法，包括如下步骤：

步骤201，当信号接收装置接收到信号发射装置旋转发射的呈设定角度的两个激光平面信号以及信号发射装置发射的超声波信号时；

确定信号发射装置绕一个旋转轴发射第一激光平面信号的第一参考时刻；确定信号接收装置检测到第一激光平面信号的第一时刻；其中，第一时刻为信号接收装置上的最优激光平面信号接收单元接收到第一激光平面信号的时刻；最优激光平面信号接收单元是信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的激光平面信号接收单元；

确定信号发射装置绕上述旋转轴发射第二激光平面信号的第二参考时刻；确定信号接收装置检测到第二激光平面信号的第二时刻；其中，第二时刻为上述最优激光平面信号接收单元接收到第二激光平面信号的时刻；第一时刻和第二时刻分别为两个激光平面信号扫描到最优激光平面信号接收单元的时间，且第一时刻早于第二时刻。

根据第一参考时刻、第二参考时刻，第一时刻、第二时刻以及信号发射装置中旋转激光平面发射单元的转速，确定目标激光平面基于第一基准时刻到第一时刻旋转的第一旋转角度，和目标激光平面基于第一基准时刻到第二时刻旋转的第二旋转角度。其中,目标激光平面为两个激光平面信号中最先扫描到信号接收装置的激光平面。

较优地，本步骤可以包括：根据信号发射装置和信号接收装置同步基准时刻之后的第一参考时刻、第二参考时刻、第一时刻以及第二时刻，确定第一旋转角度以及第二旋转角度。信号发射装置可以向信号接收装置发射时间同步信号，信号接收装置可以通过接收到时间同步信号的时刻推算出信号发射装置向信号接收装置发送第一激光平面信号的第一参考时刻和第二激光平面信号的第二参考时刻。第一参考时刻和第二参考时刻可以为同一个时刻。

本实施例中信号接收装置的第一旋转角度和第二旋转角度的具体计算过程可以参见公开号为CN106646365A的中国专利申请中详细公开的内容，在此不再详述。

步骤202，确定信号发射装置发射超声波信号的发射时刻；确定信号接收装置检测到超声波信号的接收时刻，其中，接收时刻为信号接收装置上多个超声波信号接收单元中最先接收到超声波信号的超声波信号接收单元(即，最优超声波信号接收单元)接收到该超声波信号的时间；根据信号发射装置发射超声波信号的发射时刻以及最优超声波信号接收单元检测到超声波信号的接收时刻，确定最优超声波信号接收单元与信号发射装置之间的距离。

在本实施例中，可以根据接收到同步信号的时刻推算得到超声波信号的发射时刻，根据超声波信号的发射时刻以及信号接收装置接收到超声波信号的接收时刻确定信号接收装置与信号发射装置之间的距离。信号发射装置发射超声波信号的发射时刻可以根据接收到同步信号的时刻推算得出。第一参考时刻、第二参考时刻和超声波信号的发射时刻可以为同一个时刻。最优超声波信号接收单元与信号发射装置之间的距离的计算方法和实施例一相同，在此不再赘述。

步骤203，基于第一旋转角度、第二旋转角度、以及最优超声波信号接收单元与信号发射装置之间的距离确定信号接收装置的三维坐标。

在本实施例中，当第一激光平面信号形成的平面与旋转轴平行，且当第二激光平面信号形成的平面与旋转轴呈45度夹角时，基于第一旋转角度、第二旋转角度以及信号接收装置与信号发射装置之间的距离确定信号接收装置的位置，包括：

基于如下的公式确定信号接收装置的位置：

其中，x为信号接收装置在X轴方向的坐标，y为信号接收装置在Y轴方向的坐标，z为信号接收装置在Z轴方向的坐标；l为最优超声波信号接收单元与信号发射装置之间距离；θ＝θ₂-θ₁，θ₁为第一旋转角度，θ₂为第二旋转角度。

图5所示为计算信号接收装置三维坐标的示意图，在本实施例中，第一激光平面信号形成的平面与第二激光平面信号形成的平面之间的夹角可以是其他角度。信号接收装置的三维坐标的具体计算过程可以参见公开号为CN106646365A的中国专利申请中详细公开的内容，在此不再详述。

第三实施例

本实施例和第一、第二实施例的区别在于：在信号接收装置中的微处理器确定出作为角度计算的最优激光平面信号接收单元和作为距离计算的最优超声波信号接收单元后，根据最优超声波信号接收单元和最优激光平面信号接收单元在信号接收装置上的位置数据，以及信号接收装置的姿态数据，计算出最优激光平面信号接收单元和信号发射装置之间的距离；然后利用最优激光平面信号接收单元的第一旋转角度、第二旋转角度以及最优激光平面信号接收单元和信号发射装置之间的距离，计算出最优激光平面信号接收单元的三维坐标；最后利用最优激光平面信号接收单元的三维坐标、最优激光平面信号接收单元在信号接收装置上的位置数据和信号接收装置的姿态数据计算出信号接收装置的中心的三维坐标，从而提高定位的精度。

具体来说，如图6所示，本实施例提供一种三维空间定位方法，包括如下步骤：

步骤301：当信号接收装置检测到信号发射装置发射的第一激光平面信号和第二激光平面信号时，

确定信号发射装置发射第一激光平面信号的第一参考时刻；确定信号接收装置检测到第一激光平面信号的第一时刻；其中，第一时刻为信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的最优激光平面信号接收单元接收到该激光平面信号的时间；

确定信号发射装置发射第二激光平面信号的第二参考时刻；确定信号接收装置检测到第二激光平面信号的第二时刻；第二时刻为上述最优激光平面信号接收单元接收到第二激光平面信号的时间；

根据信号发射装置发射第一激光平面信号的第一参考时刻、最优激光平面信号接收单元检测到第一激光平面信号的第一时刻，确定第一旋转角度；

根据信号发射装置发射第二激光平面信号的第二参考时刻、最优激光平面信号接收单元检测到第二激光平面信号的第二时刻，确定第二旋转角度。

为了提高定位的精确度，信号接收装置对于接收到的多路并联电信号不进行信号合并，直接从多路并联电信号中分辨出最强的(即脉宽最宽的)电信号来自于哪个激光平面信号接收单元，然后选取该激光平面信号接收单元作为最优激光平面信号接收单元，并以该激光平面信号接收单元接收到第一激光平面信号的时刻和接收到第二激光平面信号的时刻作为计算第一旋转角度或第二旋转角度的基础。其中，以最优激光平面信号接收单元接收到第一激光平面信号的时刻作为最优激光平面信号接收单元检测到第一激光平面信号的第一时刻，以最优激光平面信号接收单元接收到第二激光平面信号的时刻作为最优激光平面信号接收单元检测到第二激光平面信号的第二时刻。关于最优激光平面信号接收单元的选取与第一实施例相同，利用最优激光平面信号进行第一旋转角度和第二旋转角度的计算可参考第一实施例和第二实施例，于此不再赘述。

并且，为了进一步提高信号接收装置的定位精度，在本实施例的步骤303中，当计算信号接收装置的中心的三维坐标时，以该接收到最强信号的激光平面信号接收单元(即最优激光平面信号接收单元)在信号接收装置上的位置作为位置补偿计算的基础。

步骤302：确定信号发射装置发射超声波信号的发射时刻；确定信号接收装置检测到超声波信号的接收时刻，其中，接收时刻为信号接收装置上多个超声波信号接收单元中最先接收到超声波信号的超声波信号接收单元(即，最优超声波信号接收单元)接收到该超声波信号的时间；根据信号发射装置发射超声波信号的发射时刻以及最优超声波信号接收单元检测到超声波信号的接收时刻，确定最优超声波信号接收单元与信号发射装置之间的距离。

其中，步骤302可以包括：根据超声波信号的发射时刻以及最优超声波信号接收单元检测到超声波信号的接收时刻，确定超声波信号从信号发射装置到最优超声波信号接收单元的传输时长，根据传输时长以及超声波信号在空气中的传输速度，确定最优超声波信号接收单元与信号发射装置之间的距离。

在该实施例中，在信号接收装置中设置有多个超声波信号接收单元用于检测超声波信号。在一个信号周期T内，当信号发射装置发射一个超声波信号后，多个超声波信号接收单元会先后接收到超声波信号，从而产生多路并联电信号。为了提高定位的精确度，信号接收装置对于接收到的多路并联电信号不进行信号合并，直接从接收到的电信号中分辨出最先接收到的电信号来自于哪个超声波信号接收单元，然后将该超声波信号接收单元作为最优超声波信号接收单元，并将该超声波信号接收单元接收到超声波信号的时刻作为最优超声波信号接收单元检测到超声波信号的接收时刻，并以此作为测距计算的基础。计算最优超声波信号接收单元和信号发射装置之间的距离的过程与第一实施例中计算最优超声波信号接收单元和信号发射装置之间的距离的过程相同，故于此不再赘述。

并且，为了进一步提高信号接收装置的定位精度，在本实施例的步骤303中，当计算信号接收装置的中心的三维坐标时，以该最先接收到超声波信号的超声波信号接收单元(即最优超声波信号接收单元)在信号接收装置上的位置作为位置补偿计算的基础。

步骤303：根据第一旋转角度、第二旋转角度以及最优超声波信号接收单元与信号发射装置之间的距离，结合最优激光平面信号接收单元和最优超声波信号接收单元在信号接收装置中的位置数据，以及信号接收装置的姿态数据，确定信号接收装置的中心在三维测量坐标系中的三维坐标。

如上所述，经过步骤301和步骤302，得到信号最强的光信号的激光平面信号接收单元(即最优激光平面信号接收单元)和最先接收到超声波信号的超声波信号接收单元(即最优超声信号接收单元)。由于激光平面信号接收单元和超声波信号接收单元在信号接收装置上的分布方式通常是间隔排列的，如果进行定位计算时，角度计算和距离计算分别通过位于不同位置的激光平面信号接收单元和超声波信号接收单元实现，对于定位精度在毫米级的空间定位方法，这种计算方式会引起误差。为了减少定位误差，在步骤303的计算过程中，则需要进行位置补偿，将数据补偿到信号接收器的中心位置进行三维坐标的计算，对于球体则补偿到球心位置。

具体来说，如图7所示，利用位置补偿算法确定信号接收装置的中心在三维测量坐标系中的三维坐标的过程，包括如下三个过程：1)根据最优超声波信号接收单元和信号发射装置之间的距离、最优超声波信号接收单元和最优激光平面信号接收单元在信号接收装置上的位置数据，以及信号接收装置的姿态数据，计算最优激光平面信号接收单元和信号发射装置之间的距离；2)根据与最优激光平面信号接收单元对应的第一旋转角度、第二旋转角度以及最优激光平面信号接收单元与信号发射装置之间的距离，计算最优激光平面信号接收单元的三维坐标；3)结合信号接收装置的姿态数据、最优激光平面信号接收单元在信号接收装置上的位置数据和最优激光平面信号接收单元的三维坐标，计算信号接收装置中心的三维坐标。

下面以图8所示的球体为例进行说明。其中，最优激光平面信号接收单元B位于O₁位置，最优超声波信号接收单元A位于O₂位置，球心位于O₃位置。首先，将超声波信号接收单元A的位置O₂补偿到最优激光平面信号接收单元B的位置O₁,计算出最优激光平面信号接收单元B距离信号发射装置的距离；然后，结合最优激光平面信号接收单元B和信号发射装置之间的距离、最优激光平面信号接收单元B的第一旋转角度和第二旋转角度计算最优激光平面信号接收单元B的三维坐标；最后，根据最优激光平面信号接收单元B在球体上的位置和最优激光平面信号接收单元B的三维坐标计算出球心位置O₃的三维坐标。

以实施例一的定位方法为例说明具体计算过程如下：

如图9所示，∠Lmax O Rmax为信号发射装置定位范围，定位球上O₁位置设置有最优超声波信号接收单元A点，O₂位置设置有最优激光平面信号接收单元B，AC垂直BO，OG为信号发射装置法线，OG即图10所示的Z轴。

在步骤102中，已根据最优超声波信号接收单元A检测到超声波信号的接收时刻和信号发射装置发射超声波信号的发射时刻，计算出OA长度。

在步骤101中，已根据最优激光平面信号接收单元B接收到激光平面信号的时间计算出第一旋转角度和第二旋转角度。例如，如图10所示，以信号接收装置接收到信号发射装置发送的同步信号开始计时，信号发射装置发送同步信号的同时确保x轴激光电机面向y轴负方向沿x轴顺时针旋转，确保y轴激光电机面向x轴负方向沿y轴顺时针旋转，因此当信号接收装置在信号发射装置的z轴正方向时，前半个周期(两次同步信号的时间段为一次周期)收到的激光平面信号一定为x轴激光，后半个周期为y轴激光。当最优激光平面信号接收单元B产生信号，记录当前的周期时间判断是前半周期还是后半周期。如果当前时间是前半周期，用此时间减去第一参考时刻乘以电机转速获得当前激光平面信号接收单元相对于信号发射装置的第一旋转角度α，如果当前时间是后半周期，则用此时间减去第二参考时刻乘以电机转速获得当前激光平面信号接收单元相对于信号发射装置的第二旋转角度β。

1)由于最优超声波信号接收单元A与最优激光平面信号接收单元B固定在球上，已知最优超声波信号接收单元A与最优激光平面信号接收单元B在球上的原始位置，并且可以通过安装于球中的惯性传感器IMU获得球的姿态，即可计算获得运动中的向量AB。

具体地，参照图8和图9，已知向量O₂O₁的长度，最优超声波信号接收单元A与最优激光平面信号接收单元B固定在球上，根据球的姿态四元数，可得到新的姿态中向量O₂O₁的四元数数据，从而得到运动中的向量AB。

求运动中的向量AB的过程如下：

初始向量O₂O₁＝(x_AB,y_AB,z_AB)，转换到四元数为n＝(0,x_AB,y_AB,z_AB)，球体的姿态四元数为q＝(w_t,x_t,y_t,z_t)；将初始向量O₂O₁做姿态四元数的旋转，算法如下：(w_t’,x_AB’,y_AB’,z_AB’)＝(qi)×(n)×(qi*)，其结果将以四元数方式呈现；

其中，将四元数q求逆得到qi；将qi取反得到qi*；(x_AB’,y_AB’,z_AB’)就是运动中的向量AB。

2)最优激光平面信号接收单元B距离信号发射装置O的距离OB的计算过程如下：

如图9所示，在△ABO中，根据下式计算∠ABO：

∠ABO＝θ+∠BOG，

其中，θ为向量AB与向量OG的夹角，∠BOG为最优激光平面信号接收单元B与信号发射装置法线OG之间的夹角。由于图9中的信号发射装置法线OG等同于图10中的Z轴，则∠BOG＝a或∠BOG＝b，如果选取接收到信号最强的X轴激光平面信号的激光平面信号接收单元作为最优激光平面信号接收单元，则∠BOG＝a；如果选取接收到信号最强的Y轴激光平面信号的激光平面信号接收单元作为最优激光平面信号接收单元，则∠BOG＝b。其中，角a为通过x轴电机测得的第一旋转角度，a＝90°-α，角b为通过y轴电机测得的y轴旋转角度,b＝90°-β。

因此，在△ABO中，已知AO长度、AB长度和∠ABO，利用正弦定理和余弦定理计算OB的长度。具体计算过程如下：

其中，∠OAB＝180-∠ABO-∠AOB，

∠AOB＝arcsin((AB/AO)×sin∠ABO)。

3)如图10所示，计算获得OB长度，然后通过下述方法计算得到B点坐标(x_B，y_B，z_B)。

y_B＝IH＝OI×tan(a)；

x_B＝KI＝OI×tan(b)；

4)最后，根据B点坐标(x_B，y_B，z_B)和运动中的向量BO₃，计算球心O₃的三维坐标。

将点B到定位球球心的原始向量O₁O₃做姿态四元数旋转得到运动中的向量O₁O₃：offset＝(x’，y’，z’)，将此向量O₁O₃与最优激光平面信号接收单元B的三维坐标相加即得到球心的三维坐标。

通过上述计算过程，可以准确计算出球心的三维坐标，从而提高定位精度。

此外，本发明还提供了应用于上述实施例所提供的三维空间定位方法的系统，如图11所示，三维空间定位系统可以包括：信号接收装置、信号发射装置以及数据处理单元。信号发射装置作为三维空间定位的参考坐标基点，信号接收装置作为待定位点。

其中，信号发射装置用于向信号接收装置同步基准时刻、发射超声波信号、发射第一激光平面信号和第二激光平面信号；信号接收装置用于从信号发射装置同步基准时刻、检测超声波信号、第一激光平面信号以及第二激光平面信号。信号接收装置上设置有多个超声波信号接收单元和多个激光平面信号接收单元。

数据处理单元用于选取最优超声波信号接收单元，其中，所述最优超声波信号接收单元是多个超声波信号接收单元中最先接收到超声波信号的超声波信号接收单元；根据所述信号发射装置发送超声波信号的时刻和所述最优超声波信号接收单元检测到超声波信号的时刻计算所述信号接收装置与所述信号发射装置之间的距离；

数据处理单元还用于选取最优激光平面信号接收单元；其中，所述最优激光平面信号接收单元是所述信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的激光平面信号接收单元；数据处理单元根据所述最优激光平面信号接收单元接收到第一激光平面信号和第二激光平面信号的时刻计算第一旋转角度、第二旋转角度。

数据处理单元还用于根据所述第一旋转角度、所述第二旋转角度、所述最优超声波信号接收单元与信号发射装置之间的距离，计算所述信号接收装置的三维坐标。

优选地，数据处理单元还用于根据所述第一旋转角度、所述第二旋转角度以及所述最优超声波信号接收单元与所述信号发射装置之间的距离，结合最优激光平面信号接收单元和最优超声波信号接收单元在所述信号接收装置中的位置数据，以及所述信号接收装置的姿态数据，确定所述信号接收装置的中心在三维测量坐标系中的三维坐标。

如图11所示，信号发射装置包括：第二通信单元、控制器、激光平面信号发射器以及超声波信号发射器。

其中，第二通信单元用于实现和信号接收装置之间的信息通信，例如可以向信号接收装置发送时间同步信号，以使得信号接收装置可以同步基准时刻。可选地，第二通信单元例如为无线电通信电路、也可以为发光二极管，通过发射光信号，用于时间同步。或者，第二通信单元也可以是IC芯片和天线组成的通信模块。

超声波信号发射器用于发射超声波信号。例如以10Hz的脉冲频率、40KHz的调制频率发射超声波。

激光平面信号发射器可以为两个旋转激光平面发射器(包括旋转激光平面发射器A和旋转激光平面发射器B)，旋转激光平面发射器A用于绕着第一旋转轴旋转发射第一激光平面信号，旋转激光平面发射器B用于绕着垂直于第一旋转轴的第二旋转轴旋转发射第二激光平面信号，第一旋转轴和第二旋转轴相互垂直。激光平面发射器也可以为一个旋转激光发射器，发射绕一个旋转轴旋转的呈设定角度的第一激光平面信号和第二激光平面信号。

控制器用于控制时间同步信号、超声波信号、第一激光平面信号以及第二激光平面信号的发射时刻。

如图11所示，信号接收装置可以包括:第一通信单元(如通信电路)、微处理器(如控制电路，可以为单片机控制器或者集成数字电路)、多路激光平面信号接收阵列(包括多个激光平面信号接收单元)以及多路超声波信号接收阵列(包括多个超声波信号接收单元)。

其中，第一通信单元用于实现和信号发射装置之间的信息通信，例如可以从信号发射装置的第二通信单元接收时间同步信号，以同步基准时刻。第一通信单元为和第二通信单元对应设置的无线电通信电路、发光二极管或者是IC芯片和天线组成的通信模块。

多路激光平面信号接收阵列用于检测信号发射装置发射的第一激光平面信号以及第二激光平面信号；多路超声波信号接收阵列用于检测信号发射装置发射的超声波信号。

微处理器用于根据同步信号确定信号发射装置发射第一激光平面信号的第一参考时刻、发射第二激光平面信号的第二参考时刻和发射超声波信号的发射时刻。

数据处理单元，用于根据上述实施例所提供的三维空间定位方法计算信号接收装置在三维测量坐标系内的三维坐标。数据处理单元可以设置在信号接收装置内部，数据处理单元也可以独立设置，并和信号接收装置通过有线或无线方式进行数据传输。或者，数据处理单元还可以设置在信号发射装置上(例如，其中的控制器)。

上面对本发明提供的三维空间定位方法及用于实现上述三维空间定位方法的系统进行了说明。可以理解，对于上述实施例中没有提供的设置有多个激光平面信号接收单元和多个超声波信号接收单元的信号接收装置的定位计算，也可以通过选取最优超声波信号接收单元和最优激光平面信号接收单元，然后以上述接收单元的接收数据进行定位计算，提高定位精度。

综上所述，本发明所提供的三维空间定位方法，不对信号接收装置所接收到的多路超声波信号及多路光电信号做融合处理，通过选取最先接收到超声波信号的超声波信号接收单元作为最优超声波信号接收单元，选取接收到最强的光电信号的激光平面信号接收单元作为最优激光平面信号接收单元，并根据最优超声波信号接收单元接收到超声波信号的时刻计算最优超声波信号接收单元和信号发射装置之间的距离，根据最优激光平面信号接收单元接收到第一激光平面信号的时刻和接收到第二激光平面信号的时刻计算第一旋转角度和第二旋转角度，进而计算信号接收装置的三维坐标，提高了空间定位精度。

以上对本发明所提供的三维空间定位方法和系统进行了详细的说明。对本领域的一般技术人员而言，在不背离本发明实质精神的前提下对它所做的任何显而易见的改动，都将构成对本发明专利权的侵犯，将承担相应的法律责任。

Claims (10)

1.一种三维空间定位方法，其特征在于包括如下步骤：

确定信号发射装置发射超声波信号的发射时刻、发射第一激光平面信号的第一参考时刻和发射第二激光平面信号的第二参考时刻；

确定信号接收装置检测到所述超声波信号的接收时刻、检测到所述第一激光平面信号和第二激光平面信号的第一时刻和第二时刻；其中，所述第一时刻为所述信号接收装置上的最优激光平面信号接收单元接收到所述第一激光平面信号的时刻；所述最优激光平面信号接收单元是所述信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的激光平面信号接收单元；所述第二时刻为所述最优激光平面信号接收单元接收到第二激光平面的时刻；

根据所述第一参考时刻和所述第一时刻确定第一旋转角度；

根据所述第二参考时刻和所述第二时刻确定第二旋转角度；

根据所述超声波信号的发射时刻和所述超声波信号的接收时刻确定信号发射装置和信号接收装置的距离；

根据所述第一旋转角度、第二旋转角度以及所述信号发射装置和信号接收装置之间的距离，确定信号接收装置的三维坐标。

2.如权利要求1所述的三维空间定位方法，其特征在于，所述确定信号接收装置检测到所述超声波信号的接收时刻，包括：所述接收时刻为所述信号接收装置上的最优超声波信号接收单元接收到所述超声波信号的时刻；最优超声波信号接收单元是所述信号接收装置上多个超声波信号接收单元中最先接收到所述超声波信号的超声波信号接收单元。

3.如权利要求2所述的三维空间定位方法，其特征在于：所述确定信号发射装置发射第一激光平面信号的第一参考时刻和发射第二激光平面信号的第二参考时刻，包括：确定信号发射装置绕第一旋转轴发射第一激光平面信号的第一参考时刻和绕第二旋转轴发射第二激光平面信号的第二参考时刻；

根据所述第一参考时刻和所述第一时刻确定第一旋转角度；根据所述第二参考时刻和所述第二时刻确定第二旋转角度。

4.如权利要求2所述的定位方法，其特征在于，所述确定信号发射装置发射第一激光平面信号的第一参考时刻和发射第二激光平面信号的第二参考时刻，包括：确定信号发射装置绕一个旋转轴旋转发射呈设定角度的第一激光平面信号和第二激光平面信号的第一参考时刻和第二参考时刻；

根据所述第一参考时刻、所述第一时刻以及所述旋转轴的转速，确定第一旋转角度；根据所述第二参考时刻、所述第二时刻以及所述旋转轴的转速，确定第二旋转角度。

5.如权利要求2所述的三维空间定位方法，其特征在于：

所述根据所述第一旋转角度、所述第二旋转角度和所述最优超声波信号接收单元和所述信号发射装置之间的距离，确定信号接收装置的三维坐标，包括：

根据所述第一旋转角度、所述第二旋转角度以及所述最优超声波信号接收单元与所述信号发射装置之间的距离，结合所述最优激光平面信号接收单元和所述最优超声波信号接收单元在所述信号接收装置中的位置数据，以及所述信号接收装置的姿态数据，确定所述信号接收装置的中心在三维测量坐标系中的三维坐标。

6.如权利要求5所述的三维空间定位方法，其特征在于：确定所述信号接收装置的中心在三维测量坐标系中的三维坐标的过程，包括如下步骤：

(1)根据最优超声波信号接收单元和所述信号发射装置之间的距离、最优超声波信号接收单元和最优激光平面信号接收单元在信号接收装置上的位置数据，以及信号接收装置的姿态数据，计算最优激光平面信号接收单元和信号发射装置之间的距离；

(2)根据与最优激光平面信号接收单元对应的第一旋转角度、第二旋转角度以及最优激光平面信号接收单元与信号发射装置之间的距离，计算最优激光平面信号接收单元的三维坐标；

(3)结合信号接收装置的姿态数据、最优激光平面信号接收单元在信号接收装置上的位置数据和最优激光平面信号接收单元的三维坐标，计算信号接收装置中心的三维坐标。

7.如权利要求1～6中任意一项所述的三维空间定位方法，其特征在于：同步所述信号发射装置和所述信号接收装置的基准时刻。

8.一种三维空间定位系统，包括信号发射装置、信号接收装置以及数据处理单元，其特征在于：

所述信号发射装置，用于向所述信号接收装置发射超声波信号、第一激光平面信号以及第二激光平面信号；

所述信号接收装置，用于检测所述超声波信号、所述第一激光平面信号以及所述第二激光平面信号；其中，在所述信号接收装置上设置有多个激光平面信号接收单元；

所述数据处理单元用于选取最优激光平面信号接收单元；其中，所述最优激光平面信号接收单元是所述信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的激光平面信号接收单元；

所述数据处理单元根据所述最优激光平面信号接收单元接收到第一激光平面信号和第二激光平面信号的时刻计算第一旋转角度、第二旋转角度；

所述数据处理单元还用于根据所述信号发射装置发送超声波信号的时刻和所述信号接收装置检测到超声波信号的时刻计算所述信号接收装置与所述信号发射装置之间的距离；

所述数据处理单元还用于根据所述第一旋转角度、所述第二旋转角度、所述信号接收装置与所述信号发射装置之间的距离，计算所述信号接收装置的三维坐标。

9.如权利要求8所述的三维空间定位系统，其特征在于：

在所述信号接收装置上设置有多个超声波信号接收单元；

所述数据处理单元还用于选取最优超声波信号接收单元；其中，所述最优超声波信号接收单元是所述信号接收装置上多个超声波信号接收单元中最先接收到超声波信号的超声波信号接收单元；

所述数据处理单元还用于根据所述信号发射装置发送超声波信号的时刻和所述最优超声波信号接收单元检测到超声波信号的时刻计算所述信号接收装置与所述信号发射装置之间的距离。

10.如权利要求9所述的一种三维空间定位系统，其特征在于：

所述数据处理单元根据所述第一旋转角度、所述第二旋转角度以及所述最优超声波信号接收单元与所述信号发射装置之间的距离，结合最优激光平面信号接收单元和最优超声波信号接收单元在所述信号接收装置中的位置数据，以及所述信号接收装置的姿态数据，确定所述信号接收装置的中心在三维测量坐标系中的三维坐标。