CN110376550B - 一种基于位置补偿的三维空间定位方法及系统 - Google Patents

Abstract

本发明公开了一种基于位置补偿的三维空间定位方法及系统。其中，在对信号接收装置进行定位计算的过程中，通过对接收超声波信号的超声波信号接收单元和对接收激光平面信号的激光平面信号接收单元进行准确定位，然后根据超声波信号接收单元和激光平面信号接收单元在信号接收装置上的位置，结合信号接收装置的位姿数据，计算出激光平面信号接收单元的精确位置，进而计算信号接收装置中心的三维坐标，提高了空间定位精度。本发明所公开的基于位置补偿的三维空间定位方法及系统，能够基于超声波信号和激光平面信号实现空间精确定位。

Description

一种基于位置补偿的三维空间定位方法及系统

技术领域

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

背景技术

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

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

在上述定位过程中，在进行电信号处理时将多路电信号融合成了一路，然后计算信号接收装置的三维坐标。这种将多路信号融合处理后计算三维坐标的方式，仅能大致确定信号接收装置检测到超声波信号及激光平面信号的时间，而且，由于在计算三维坐标的过程中没有考虑信号接收单元在信号接收装置上的位置，因此利用这些定位信号进行定位计算的准确度有待提高。

发明内容

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

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

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

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

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

确定超声波信号接收单元检测到所述超声波信号的接收时刻以及激光平面信号接收单元检测到所述第一激光平面信号和第二激光平面信号的第一时刻和第二时刻；

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

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

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

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

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

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

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

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

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

根据所述发射时刻和所述接收时刻，确定所述最优超声波信号接收单元和所述信号发射装置之间的距离。

其中较优地，所述确定激光平面接收单元检测到所述第一激光平面信号和第二激光平面信号的第一时刻和第二时刻；包括：确定最优激光平面接收单元检测到所述第一激光平面信号和第二激光平面信号的第一时刻和第二时刻；其中，所述最优激光平面信号接收单元是所述信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的激光平面信号接收单元；所述第一时刻和所述第二时刻分别为所述最优激光平面信号接收单元接收到第一激光平面信号和第二激光平面信号的时刻。

其中较优地，当信号接收装置为球体时，以位于O₁位置的激光平面信号接收单元B为所述激光平面信号接收单元，以位于O₂位置的超声波信号接收单元A为所述超声波信号接收单元，球心位于O₃位置，则激光平面信号接收单元B距离信号发射装置O的距离OB的计算过程如下：

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

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

在△ABO中，根据下式计算∠ABO：

∠ABO＝θ+∠BOG，

其中，θ为运动中的向量AB与向量OG的夹角，∠BOG为激光平面信号接收单元B与基站法线OG之间的夹角。

其中较优地，激光平面信号接收单元B的三维坐标(x_B，y_B，z_B)的计算过程如下：

y_B＝z_B×tan(a)；

x_B＝z_B×tan(b)；

其中，OB为激光平面信号接收单元B到信号发射装置的距离，角a为第一旋转角度，角b为第二旋转角度。

其中较优地，球心O₃的三维坐标的计算过程如下：

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

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

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

所述信号接收装置，包括用于检测所述超声波信号的超声波信号接收单元和用于检测所述第一激光平面信号和所述第二激光平面信号的激光平面信号接收单元；

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

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

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

其中较优地，在所述信号接收装置上设置有多个激光平面信号接收单元；

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

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

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

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

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

本发明所提供的三维空间定位方法，在根据第一旋转角度、第二旋转角度以及超声波信号接收单元和信号发射装置之间的距离对信号接收装置进行定位计算的过程中，通过对超声波信号和激光平面信号的接收位置进行准确定位，然后根据超声波信号接收单元和激光平面信号接收单元在信号接收装置上的位置，结合信号接收装置的位姿数据，计算出激光平面信号接收单元的精确位置，进而计算信号接收装置中心的三维坐标，提高了空间定位精度。

附图说明

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

图2是信号接收装置的一种三维坐标计算的原理示意图；

图3是信号接收装置的另一种三维坐标计算的原理示意图；

图4是第一实施例中位置补偿算法的流程图；

图5是在一球体上，位置补偿算法的过程示意图；

图6是将超声波信号接收单元位置补偿到激光平面信号接收单元位置的原理示意图；

图7是激光平面信号接收单元的三维坐标计算的原理示意图；

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

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

图10是第二实施例中位置补偿算法的流程图；

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

具体实施方式

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

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

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

第一实施例

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

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

确定信号发射装置发射第一激光平面信号的第一参考时刻；确定信号接收装置检测到第一激光平面信号的第一时刻；其中，在信号接收装置的表面设置有一个激光平面信号接收单元，第一时刻为信号接收装置上的上述激光平面信号接收单元接收到第一激光平面信号的时刻；

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

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

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

在一个可选实施例中，如图2所示，信号发射装置绕着第一旋转轴(X轴)旋转发射第一激光平面信号，转速为w_l；绕着第二旋转轴(Y轴)旋转发射第二激光平面信号，转速为w₂；超声波发射器所在位置为三维测量坐标系的原点；X轴和Y轴相互垂直，X轴和Y轴所确定的平面为XOY平面。在该实施例中第一参考时刻、第二参考时刻分别为第一激光平面、第二激光平面分别旋转到第一旋转轴和第二旋转轴所确定的平面的时刻。第一参考时刻和第二参考时刻根据信号接收装置接收到同步信号的时刻推算得出。第一旋转角度α为第一时刻第一激光平面信号相对于第一旋转轴和第二旋转轴所确定平面的角度，第二旋转角度β为第二时刻第二激光平面信号相对于第一旋转轴和第二旋转轴所确定平面的角度。按照公开号为CN106526537A的中国发明申请中所公开的内容可计算得出第一旋转角度和第二旋转角度，在此不再重复。

在另一个可选实施例中，如图3所示，信号发射装置绕着一个旋转轴旋转发射呈设定角度的第一激光平面信号和第二激光平面信号，转速为w_l；超声波发射器所在位置为三维测量坐标系的原点。在该实施例中第一参考时刻和第二参考时刻为同一个时刻，根据接收到同步信号的时刻推算得出。第一旋转角度α为目标激光平面基于第一参考时刻到第一时刻旋转的角度，第二旋转角度β为目标激光平面基于第二参考时刻到第二时刻旋转的角度，目标激光平面为两个激光平面信号中最先扫描到信号接收装置的激光平面。按照公开号为CN106646365A的中国发明申请中可计算得出第一旋转角度和第二旋转角度，在此不再重复。

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

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

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

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

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

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

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

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

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

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

下面以图2所示的定位方法为例说明具体计算过程如下：

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

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

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

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

具体地，参照图5和图6，已知向量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的计算过程如下：

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

∠ABO＝θ+∠BOG，

其中，θ为向量AB与向量OG的夹角，∠BOG为激光平面信号接收单元B与信号发射装置法线OG之间的夹角。由于图6中的信号发射装置法线OG等同于图8中的Z轴，如激光平面信号接收单元B先接受到X轴的第一激光平面信号，则∠BOG＝a。如激光平面信号接收单元B先接受到Y轴的第一激光平面信号，则∠BOG＝b。

其中，角a为通过x轴电机测得的x轴旋转角度a＝90°-α，角b为通过y轴电机测得的y轴旋转角度，b＝90°-β。

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

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

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

3)如图6所示，计算获得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的三维坐标相加即得到球心的三维坐标。

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

第二实施例

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

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

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

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

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

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

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

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

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

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

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

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

在一个可选实施例中，如图2所示，信号发射装置绕着第一旋转轴(X轴)旋转发射第一激光平面信号，转速为w_l；绕着第二旋转轴(Y轴)旋转发射第二激光平面信号，转速为w₂；超声波发射器所在位置为三维测量坐标系的原点；X轴和Y轴相互垂直，X轴和Y轴所确定的平面为XOY平面。在该实施例中第一参考时刻、第二参考时刻分别为第一、第二激光平面分别旋转到第一旋转轴和第二旋转轴所确定的平面的时刻。第一参考时刻和第二参考时刻根据信号接收装置接收到同步信号的时刻推算得出。第一旋转角度α为第一时刻第一激光平面信号相对于第一旋转轴和第二旋转轴所确定平面的角度，第二旋转角度β为第二时刻第二激光平面信号相对于第一旋转轴和第二旋转轴所确定平面的角度。按照公开号为CN106526537A的中国发明申请中可计算得出第一旋转角度和第二旋转角度，在此不再重复。

在另一个可选实施例中，如图3所示，信号发射装置绕着一个旋转轴旋转发射第一激光平面信号和第二激光平面信号，转速为w_l；超声波发射器所在位置为三维测量坐标系的原点。在该实施例中第一参考时刻和第二参考时刻为同一个时刻，可根据接收到同步信号的时刻推算得出。第一旋转角度α为目标激光平面基于第一参考时刻到第一时刻旋转的角度，第二旋转角度β为目标激光平面基于第二参考时刻到第二时刻旋转的角度，目标激光平面为两个激光平面信号中最先扫描到信号接收装置的激光平面。按照公开号为CN106646365A的中国发明申请中可计算得出第一旋转角度和第二旋转角度，在此不再重复。

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

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

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

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

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

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

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

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

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

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

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

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

以图2所示的定位方法为例说明具体计算过程如下：

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

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

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

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

具体地，参照图5和图6，已知向量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的计算过程如下：

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

∠ABO＝θ+∠BOG，

其中，θ为向量AB与向量OG的夹角，∠BOG为最优激光平面信号接收单元B与信号发射装置法线OG之间的夹角。由于图6中的信号发射装置法线OG等同于图7中的Z轴，则∠BOG＝a或∠BOG＝b，如果选取接收到信号最强的X轴激光平面信号的激光平面信号接收单元作为最优激光平面信号接收单元，则∠BOG＝a；如果选取接收到信号最强的Y轴激光平面信号的激光平面信号接收单元作为最优激光平面信号接收单元，则∠BOG＝b。

其中，角a为通过x轴电机测得的x轴旋转角度a＝90°-α，角b为通过y轴电机测得的y轴旋转角度，b＝90°-β。

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

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

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

3)如图6所示，计算获得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 (2)

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

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

确定超声波信号接收单元检测到所述超声波信号的接收时刻以及激光平面信号接收单元检测到所述第一激光平面信号和第二激光平面信号的第一时刻和第二时刻；

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

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

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

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

根据超声波信号接收单元和所述信号发射装置之间的距离、超声波信号接收单元和激光平面信号接收单元在信号接收装置上的位置数据，以及信号接收装置的姿态数据，计算激光平面信号接收单元和信号发射装置之间的距离；根据与激光平面信号接收单元对应的第一旋转角度、第二旋转角度以及激光平面信号接收单元与信号发射装置之间的距离，计算激光平面信号接收单元的三维坐标；结合信号接收装置的姿态数据、激光平面信号接收单元在信号接收装置上的位置数据和激光平面信号接收单元的三维坐标，计算信号接收装置中心的三维坐标；确定超声波信号接收单元检测到所述超声波信号的接收时刻，包括：确定最优超声波信号接收单元检测到所述超声波信号的接收时刻，其中，最优超声波信号接收单元是所述信号接收装置上多个超声波信号接收单元中最先接收到所述超声波信号的超声波信号接收单元；

根据所述发射时刻和所述接收时刻，确定所述最优超声波信号接收单元和所述信号发射装置之间的距离；所述确定激光平面接收单元检测到所述第一激光平面信号和第二激光平面信号的第一时刻和第二时刻；包括：确定最优激光平面接收单元检测到所述第一激光平面信号和第二激光平面信号的第一时刻和第二时刻；其中，所述最优激光平面信号接收单元是所述信号接收装置上多个激光平面信号接收单元中接收到最强的第一激光平面信号的激光平面信号接收单元；所述第一时刻和所述第二时刻分别为所述最优激光平面信号接收单元接收到第一激光平面信号和第二激光平面信号的时刻；

信号接收装置为球体，以位于O₁位置的激光平面信号接收单元B为所述激光平面信号接收单元，以位于O₂位置的超声波信号接收单元A为所述超声波信号接收单元，球心位于O₃位置，则激光平面信号接收单元B距离信号发射装置O的距离OB的计算过程如下：

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

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

在ΔABO中，根据下式计算∠ABO：

∠ABO＝θ+∠BOG，

其中，θ为运动中的向量AB与向量OG的夹角，∠BOG为激光平面信号接收单元B与基站法线OG之间的夹角；

激光平面信号接收单元B的三维坐标(x_B，y_B，z_B)的计算过程如下：

y_B＝z_B×tan(a)；

x_B＝z_B×tan(b)；

其中，OB为激光平面信号接收单元B到信号发射装置的距离，角a为x轴旋转角度，角b为y轴旋转角度；球心O₃的三维坐标的计算过程如下：

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

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

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

所述信号接收装置，包括用于检测所述超声波信号的超声波信号接收单元和用于检测所述第一激光平面信号和所述第二激光平面信号的激光平面信号接收单元；

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

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

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

所述数据处理单元还用于进行如下数据处理：

信号接收装置为球体，以位于O₁位置的激光平面信号接收单元B为所述激光平面信号接收单元，以位于O₂位置的超声波信号接收单元A为所述超声波信号接收单元，球心位于O₃位置，则激光平面信号接收单元B距离信号发射装置O的距离OB的计算过程如下：

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

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

在ΔABO中，根据下式计算∠ABO：

∠ABO＝θ+∠BOG，

其中，θ为运动中的向量AB与向量OG的夹角，∠BOG为激光平面信号接收单元B与基站法线OG之间的夹角；

激光平面信号接收单元B的三维坐标(x_B，y_B，z_B)的计算过程如下：

y_B＝z_B×tan(a)；

x_B＝z_B×tan(b)；

其中，OB为激光平面信号接收单元B到信号发射装置的距离，角a为x轴旋转角度，角b为y轴旋转角度；球心O₃的三维坐标的计算过程如下：

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

在所述信号接收装置上设置有多个激光平面信号接收单元；

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

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

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

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

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