CN113109819B - 一种多目标六自由度超声定位系统和方法 - Google Patents

Abstract

本发明涉及一种多目标六自由度超声定位系统和方法，属于六自由度超声定位方法。系统包括超声信号发射单元和超声信号接收单元，采用TOA定位法求得超声信号接收传感器在世界坐标系下的坐标，并以此为基础结合超声信号接收传感器已知在超声接收坐标系下的坐标构建坐标系变换方程，求得超声信号接收单元在世界坐标系下的姿态。本发明优点是采用支持多个超声信号接收单元同时工作的六自由度超声定位系统，和一种实时并行求解多目标六自由度信息的超声定位方法，最终实现在满足高刷新率、高精度条件下对多目标物体六自由度信息的获取。

Description

一种多目标六自由度超声定位系统和方法

技术领域

本发明涉及一种用于六自由度超声定位系统和方法，具体涉及一种支持多个超声信号接收单元同时工作的六自由度超声定位系统和一种实时并行求解多目标六自由度信息的超声定位方法，可应用于虚拟现实、人机交互、动画制作、医学健康、运动分析和工业测量与控制等各个领域。

背景技术

随着智能家居产业、可穿戴设备和智能装备产业、影视动画产业以及医疗健康产业等领域的迅猛发展和人民生活水平的不断提高，六自由度定位技术的应用范围越来越广泛，精度需求也越来越高。目前市场上常见的六自由度定位技术主要是：(1)基于光学的六自由度定位技术，其原理主要分两类，第一类是通过对运动物体的关键节点加装识别器来实现对目标物体的六自由度定位，场景布置复杂，前期准备工作较多。第二类则是通过相应的算法直接对摄像头拍下的运动物体进行六自由度定位，由于现有的AI识别技术精确度并不高，定位难度大，而且运算复杂，刷新频率低；(2)基于IMU的六自由度定位技术，IMU主要由加速度计和陀螺仪组成，基本原理是通过对传感器采集信号的积分来获得目标物体的六自由度信息，但积分会造成误差的累积，无法在较长时间内保持定位的精度；(3)基于电磁感应的六自由度定位技术，其基本原理是电磁信号发射器发射出电磁场，运动物体上加装有多个接收器，接收器将捕获到的电磁信号接收处理后传回处理器，处理器再综合分析得到目标物体的六自由度信息，但这种技术受金属影响较大，需要其使用空间中较少金属，甚至是无金属，应用领域较窄；(4)基于WIFI的六自由度定位技术，其基本原理是基于RSSI的指纹定位技术，通过分析无线同步信号的信号强度来确定目标物体的六自由度信息，但其精度过低(米级)；(5)基于超声的六自由度定位技术，其基本原理是信号基站同时发射超声信号和另一种不同传播速度的信号，接收器根据两种信号通过的时间差以及传播速度，计算出信号基站与接收器之间的距离，并以此为基础解算出接收器的六自由度信息，但目前这种技术计算流程复杂，刷新率较低，且无法同时满足多目标的六自由度信息获取。

目前已有一些关于基于超声的六自由度定位技术的介绍。

在论文“虚拟现实系统头盔与数据手套的三维精确定向与定位系统与设计”中，熊春山等人提出了一种基于超声的六自由度定位方法，利用三角形测量原理实现六自由度信息的获取。在多超声信号发射器的条件下，该技术为了识别超声信号的来源，超声信号发射器以固定时间间隔轮流发射超声信号，且该时间间隔需大于最大定位距离下超声信号的传播时间，这导致该方法的实际刷新频率低，无法满足多数场景的需求。同时，该方法若要实现对多个物体的六自由度信息的获取，每增加一个物体，采集过程所需时间就会翻倍，没有实际应用意义。

中国专利“一种实现六自由度测量的超声三维定位系统和定位方法”(申请号201810093554.9)提供了一种只需两个超声信号发射器和三个超声接收传感器的六自由度信息测量方法，这种方法不再需要另一种不同传输速度的无线同步信号，减轻了系统复杂度，但这种方法无法测量得到的六自由度数据中的翻滚角姿态数据，同时这种方法测量得到的航向角和俯仰角姿态数据只能是相对于上一时刻的姿态数据而不是在绝对坐标系下的姿态数据，无法满足大多数的应用场景。中国专利“一种超声定位引导的测头空间姿态测量方法”(申请号201310504782.8)提供了一种需要四个超声信号发射器(要求每个超声信号发射器均能够发射4种不同频率的超声信号)，四个超声信号接收传感器(要求每个超声信号接收传感器均能够接收4种不同频率的超声信号)，一个电磁信号发射器和一个电磁信号接收传感器的六自由度信息测量方法。这种方法虽然能够实现目标物体六自由度信息的获取，但这种方法需要较多的超声信号发射器和超声信号接收传感器，且对这两种元件的接收发射频率有一定的要求，这样就提高了系统的功耗、成本和复杂度。

发明内容

本发明提供一种多目标六自由度超声定位系统和方法，相对于部分现有的实现目标六自由度超声定位的系统，本发明不再需要对超声信号或无线同步信号进行编码或需求超声信号发射器发射不同频率、振幅的超声信号来保证超声信号接收传感器能明确当前接收的超声信号来自哪个超声信号发射器，极大的降低了系统的功耗和计算量。

本发明采取的的技术方案是：一种多目标六自由度超声定位系统，包括：一个超声信号发射单元和N个超声信号接收单元，N≥1,每个超声信号发射单元包含一个无线同步信号发射器和m₁个超声信号发射器、其中至少包含三个不共线的超声信号发射器，每个超声信号接收单元包含一个无线同步信号接收传感器和m₂个超声信号接收传感器、其中至少包含三个不共线的超声信号接收传感器；

所述超声信号发射单元包括无线同步信号发射器、超声信号发射器一、超声信号发射器二、超声信号发射器三、微处理器一、滤波放大电路一、滤波放大电路二，其中三个超声信号发射器在世界坐标系O_B下的坐标已知且不共线，其中微处理器一为超声信号发射单元提供发射信号，经滤波放大电路一、滤波放大电路二分别生成超声驱动信号、无线驱动信号，经超声信号发射器一、超声信号发射器二、超声信号发射器三、无线同步信号发射器，发出超声信号、无线同步信号。

所述N个超声信号接收单元中的一个超声信号接收单元包括无线同步信号接收传感器、超声信号接收传感器一、超声信号接收传感器二、超声信号接收传感器三、微处理器二、滤波放大电路三、滤波放大电路四、增益控制电路、模数转换电路，其中超声信号接收传感器一、超声信号接收传感器二、超声信号接收传感器三在该超声信号接收单元所在坐标系O_C1的坐标已知、且不共线；其中滤波放大电路四的增益可调，超声信号接收传感器一、超声信号接收传感器二、超声信号接收传感器三、无线同步信号接收传感器接收到超声信号、无线同步信号产生电压信号，信号经滤波放大后，由模数转换电路对其采样，并传送到微处理器二。

本发明所述无线同步信号发射器采用一种以光速传播的无线信号的信号发射器。

本发明所述无线同步信号发射器采用包括红外、2.4G、5G、射频、无线电的无线同步信号的信号发射器。

本发明所述无线同步信号接收传感器采用一种以光速传播的无线信号的信号接收传感器。

本发明所述无线同步信号接收传感器包括红外、2.4G、5G、射频、无线电的无线同步信号的信号接收传感器。

一种多目标六自由度超声定位的方法，包括下列步骤：

(一)、每个超声信号接收单元可以定位一个目标，定义某任意给定三维直角坐标系O_C1为一个超声信号接收单元所在的坐标系，即该超声信号接收单元在坐标系O_C1下的位置已知且固定，分别为P₁(x′₁,y′₁,z′₁)，P₂(x′₂,y′₂,z′₂)和P₃(x′₃,y′₃,z′₃)，定义某任意给定三维直角坐标系OB为世界坐标系，在世界坐标系中三个超声信号接收传感器的坐标分别为T₁(x′₄,y′₄,z′₄)，T₂(x′₅,y′₅,z′₅)和T₃(x′₆,y′₆,z′₆)，超声信号发射单元在世界坐标系下的位置已知且固定，其中三个超声信号发射器的坐标分别为E₁(x′₇,y′₇,z′₇)，E₂(x′₈,y′₈,z′₈)和E₃(x′₉,y′₉,z′₉)；

(二)、定义该超声信号发射单元的三个不共线超声信号发射器所构成的平面P的一般方程为Ax+By+Cz+D＝0，其中A、B、C、D由超声信号发射器在世界坐标系下的坐标决定，世界坐标系被平面P分成Ax+By+Cz+D>0和Ax+By+Cz+D≤0两部分,超声信号发射器发射端朝向Ax+By+Cz+D>0部分；

(三)、由E₁(x′₇,y′₇,z′₇)在平面P上可知，平面P的数学表达式亦可写作：

A(x-x′₇)+B(y-y′₇)+C(z-z′₇)＝0.............①

结合方程①与平面P的一般方程可以得到：

D＝-(Ax′₇+By′₇+Cz′₇).............②

又因为E₂(x′₈,y′₈,z′₈)和E₃(x′₉,y′₉,z′₉)都在此平面上，因此它们的坐标都满足方程A(x-x′₇)+B(y-y′₇)+C(z-z′₇)＝0，将它们的坐标依次代入得：

A(x′₈-x′₇)+B(y′₈-y′₇)+C(z′₈-z′₇)＝0.............③

A(x′₉-x′₇)+B(y′₉-y′₇)+C(z′₉-z′₇)＝0.............④

公式①③④是关于A、B、C的线性方程组，此方程组有非零解的充要条件是关于A、B、C的系数行列式Δ＝0；即：

展开此行列式可得：

A＝(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)

B＝(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)

C＝(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)

将A、B、C代入公式②中，可得：

D＝x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈

所以平面P的一般式方程为：

{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈＝0

(四)、开始定位，超声信号发射器一、超声信号发射器二、超声信号发射器三在一个周期中以时间间隔t₀依次发射超声信号，使超声信号接收传感器能明确当前接收的超声信号来自哪个超声信号发射器，其中时间间隔t₀需大于等于三个超声信号发射器间的最大距离与超声信号在空气中传播速度的比值，在超声信号发射器一发射超声信号之前一段时间到超声信号发射器三发射超声信号之前的任意时刻无线同步信号发射器发射一次无线同步信号作为超声信号的时间同步信号；

(五)、超声信号发射单元以固定周期不间断发送定位信号，在超声信号发射单元两个相邻发射周期中，前一周期最后一个发射结束到后一个周期第一个发射开始的时间间隔为t₁，t₁需大于本系统最大可定位距离与超声信号在空气中传播速度的比值；

(六)、采用渡越时间TOA定位法获得超声信号接收传感器的实时坐标值，因为无线同步信号在空气中传播速度接近光速，远大于超声在空气中传播的速度，因此将无线同步信号接收传感器接收到无线同步信号的时间作为时间基准，可以解算出超声信号在空气中的传播时间，将其与超声在空气中的传播速度相乘，可以获得超声信号传播的距离，通过这种方法可以得到超声信号接收传感器一到超声信号发射器一、二、三的距离值，分别为L₁、L₂、L₃；

根据上述可列出几何方程：

由于上式为三个分别含有x′₄,y′₄,z′₄三个未知数的三元二次方程，因此可以解算出超声信号接收传感器一坐标T₁(x′₄,y′₄,z′₄)的两组解；

因为超声信号发射器发射端朝向{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈>0部分，去除一组{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈≤0部分的解，得到超声信号接收传感器一在世界坐标系下的坐标T₁(x′₄,y′₄,z′₄)；

同理，亦可以得到超声信号接收传感器二、三在世界坐标系下的坐标T₂(x′₅,y′₅,z′₅)、T₃(x′₆,y′₆,z′₆)，从而实现任意时刻下，任意超声信号接收传感器在世界坐标系下坐标值的获取；

还可以取其中任意一个超声信号接收传感器的坐标加任意固定修正常量作为超声信号接收单元的坐标，亦可以取其中两个或三个超声信号接收传感器的坐标的加权平均值作为超声信号接收单元的坐标；

(七)、定义超声信号接收单元一所在超声接收坐标系O_C1相对于世界坐标系O_B的方向余弦矩阵为DCM_BC1：

那么超声信号接收单元一在世界坐标系下的姿态可以由方向余弦矩阵DCM_BC1表示，解算方向余弦矩阵DCM_BC1的具体步骤包括：

1)已知在超声接收坐标系O_C1下，三个超声信号接收传感器的坐标固定，且分别为P₁(x′₁,y′₁,z′₁)，P₂(x′₂,y′₂,z′₂)和P₃(x′₃,y′₃,z′₃)，可以获得： 设x₁＝x′₂-x′₁,y₁＝y′₂-y′₁,z₁＝z′₂-z′₁,x₂＝x′₃-x′₁,y₂＝y′₃-y′₁,z₂＝z′₃-z′₁,那么：

所以：

设x₃＝y₂z₁-y₁z₂,y₃＝z₂x₁-x₂z₁,z₃＝x₂y₁-x₁y₂,则

2)如步骤(六)所述，可以实现任意时刻下，任意超声信号接收传感器在世界坐标系下坐标值的获取，定义超声信号接收传感器一、二、三在世界坐标系下的坐标分别为T₁(x′₄,y′₄,z′₄)，T₂(x′₅,y′₅,z′₅)、T₃(x′₆,y′₆,z′₆)，那么 设x₄＝x′₅-x′₄,y₄＝y′₅-y′₄,z₄＝z′₅-z′₄,x₅＝x′₆-x′₄,y₅＝y′₆-y′₄,z₅＝z′₆-z′₄,那么：

所以：

设x₆＝y₅z₄-y₄z₅,y₆＝z₅x₄-x₅z₄,z₆＝x₅y₄-x₄y₅,则

3)根据上述可以列出几何方程：

整理可得：

由上述步骤1)、2)可知，x_1～6、y_1～6和z_1～6均为已知量，因此通过上式可以得到共包含a₁₁、a₁₂、a₁₃、a₂₁、a₂₂、a₂₃、a₃₁、a₃₂、a₃₃九个未知量的九个三元一次方程，从而可以解得超声信号接收单元一在世界坐标系下的姿态DCM_BC1，从而实现对目标物体六自由度信息的获取。

本发明还包括步骤(八)、还可以同样获得其他可能存在的超声信号接收单元在在世界坐标系下的六自由度信息，最终实现在满足高刷新率、高精度条件下对多目标物体六自由度信息的获取。

本发明首先定义了两个可以为任意位置和方向的三维直角坐标系，超声接收坐标系(数量不限)和世界坐标系，然后利用三个不共线超声信号发射器所构成的平面将世界坐标系分割成超声信号发射器朝向端和背向端两部分。将无线同步信号接收传感器接收到无线同步信号的时间作为时间基准，解算出超声信号接收传感器与超声信号发射器之间的距离值，构建三维空间中两点间距离公式，以超声信号接收传感器位于世界坐标系的超声信号发射器朝向端部分为筛选条件，得到超声信号接收传感器在世界坐标系下的坐标，以此类推，进而可以得到一个或多个超声信号接收单元在世界坐标系下的位置坐标。最后以三个超声信号接收传感器在世界坐标系下两两构成的相交向量及两相交向量构成的平面的法向量等于超声信号接收坐标系相对于世界坐标系的方向余弦矩阵(即世界坐标系下超声信号接收单元的姿态)与三个超声信号接收传感器在超声坐标系下两两构成的相应向量及两相交向量构成的平面的法向量的叉乘，得到一个或多个超声信号接收单元在世界坐标系下姿态信息，完成对多目标物体六自由度信息的获取。

本发明提出了一种用于多目标六自由度超声定位的方法，利用一个无线同步信号发射器，三个超声信号发射器，一个无线同步信号接收传感器和三个或三的倍数个超声信号接收传感器构成多目标六自由度超声定位系统，采用TOA定位法，将超声信号接收单元接收到无线同步信号的时间作为计算超声渡越时间的时间基准，求出超声信号接收传感器与三个超声信号发射器的距离，从而根据坐标距离公式，求得超声信号接收传感器在世界坐标系下的坐标，并以此为基础结合超声信号接收传感器已知在超声接收坐标系下的坐标构建坐标系变换方程，求得超声信号接收单元在世界坐标系下的姿态，最终实现对多目标物体六自由度信息的获取。

与其他部分相似研究不同的是，本发明不再需要对超声信号或无线同步信号进行编码或需求超声信号发射器发射不同频率、振幅的超声信号来保证超声信号接收传感器能明确当前接收的超声信号来自哪个超声信号发射器，极大的降低了系统的功耗和计算量，提高了系统的刷新率；同时与其他相似研究中另一种为了保证超声信号接收传感器能明确当前接收的超声信号来自哪个超声信号发射器而令超声信号接收传感器接收到上一个超声信号发射器发射的超声信号后，下一个超声信号发射器再发射超声信号的方法相比，本发明中超声信号发射器一、二、三在一个周期中以时间间隔t₀依次发射超声信号，使超声信号接收传感器能明确当前接收的超声信号来自哪个超声信号发射器的方式极大地缩短了系统的发射周期，提高了系统的刷新率。

附图说明

图1是本发明超声信号发射单元结构示意图，示出了可在其上实现本文所述的方法和系统的示例性设备；

图2是本发明超声信号接收单元(头盔型)结构示意图，示出了可在其上实现本文所述的方法和系统的示例性设备；

图3是本发明超声信号接收单元(手柄型)结构示意图，示出了可在其上实现本文所述的方法和系统的示例性设备；

图4是本发明系统整体组成示意图，示出了可在其上实现本文所述的方法和系统的示例性设备和系统；

图5是本发明超声定位方法原理图；

图6是本发明超声定位方法的实际系统应用示意图；

图7是本发明无线、超声信号发射时序图；

图8是本发明无线、超声信号接收时序图；

图9是本发明超声信号接收传感器在世界坐标系下坐标计算流程图；

图10是本发明超声信号接收单元在世界坐标系下姿态计算流程图；

图11是本发明超声信号发射单元电路结构框图，其中只列出了实现本发明超声信号发射单元部分的一些主要结构，没有详细地描述众所周知的部件、电路等；

图12是本发明超声信号接收单元电路结构框图，其中只列出了实现本发明超声信号接收单元部分的一些主要结构，没有详细地描述众所周知的部件、电路等。

具体实施方式

下面的详细描述中示出许多具体细节，以便提供对各种所描述的实施方案的充分理解。但是，对本领域的普通技术人员显而易见的是，各种所描述的实施方案可以在没有这些具体细节的情况下被实践。在其他情况下，没有详细地描述众所周知的方法、过程、部件、电路和网络，从而不会不必要地使实施方案的各个方面晦涩难懂。

还将理解的是，虽然在一些情况下，术语“一”“二”等在本文中作为后缀用于描述各种元件，但是这些元件不应受到这些术语限制。这些术语只是用于将一个元件与另一元件区分开。例如，超声信号接收传感器一可被命名为超声信号接收传感器二，并且类似地，超声信号接收传感器二可被命名为超声信号接收传感器一，而不脱离各种所描述的实施方案的范围。超声信号接收传感器一和超声信号接收传感器二都是超声信号接收传感器，但是它们不是同一超声信号接收传感器。

在本文中对各种所述实施方案的描述中所使用的术语只是为了描述特定实施方案的目的，而并非旨在进行限制。如在对各种所述实施方案中所使用的描述和所附权利要求书中所使用的那样，单数形式“一个”和“该”旨在也包括复数形式，除非上下文另外明确地指示。还应理解的是，本文中所使用的术语“包括”和/或“包含”在本说明书中使用时是指定存在说陈述的特征、整数、步骤、操作、元件、部件和/或其分组。

一种多目标六自由度超声定位系统，包括：超声信号发射单元、超声信号接收单元；所述系统由一个超声信号发射单元和N个超声信号接收单元组成，N≥1，每个超声信号发射单元包含一个无线同步信号发射器和m₁个超声信号发射器(其中至少包含三个不共线的超声信号发射器)，每个超声信号接收单元包含一个无线同步信号接收传感器和m₂个超声信号接收传感器(其中至少包含三个不共线的超声信号接收传感器)。原理上，存在同步信号的条件下，3个超声信号发射器和3个超声信号接收传感器即可实现目标定位，采用大于3个超声信号发射器或3个超声信号接收传感器，一般是为了提升系统的定位精度和抗干扰能力，本发明不涉及定位精度和抗干扰能力的提升方法。

所述超声信号发射单元包括无线同步信号发射器(采用一种以光速传播的无线信号的信号发射器，比如红外、2.4G、5G、射频、无线电等无线同步信号的信号发射器)、超声信号发射器一、超声信号发射器二、超声信号发射器三、微处理器一、滤波放大电路一、滤波放大电路二(三路)，其中三个超声信号发射器在世界坐标系O_B下的坐标已知且不共线。所述超声信号发射器一、超声信号发射器二、超声信号发射器三、无线同步信号发射器、滤波放大电路一、滤波放大电路二(三路)顺序连接，微处理器一为超声信号发射单元提供发射信号，经滤波放大电路一、滤波放大电路二(三路)分别生成超声驱动信号、无线驱动信号，经超声信号发射器一、超声信号发射器二、超声信号发射器三、无线同步信号发射器，发出超声信号、无线同步信号；

所述N(N≥1)个超声信号接收单元中的一个超声信号接收单元包括无线同步信号接收传感器(采用一种以光速传播的无线信号的信号接收传感器，比如红外、2.4G、5G、射频、无线电等无线同步信号的信号接收传感器)、超声信号接收传感器一、超声信号接收传感器二、超声信号接收传感器三、微处理器二、滤波放大电路三、滤波放大电路四(三路)、增益控制电路、模数转换电路，其中超声信号接收传感器一、超声信号接收传感器二、超声信号接收传感器三在超声信号接收单元一所在坐标系O_C1的坐标已知且不共线；所述超声信号接收传感器一、超声信号接收传感器二、超声信号接收传感器三、无线同步信号接收传感器、滤波放大电路三、滤波放大电路四(三路)、模数转换电路、微处理器二顺序连接，其中滤波放大电路四(三路)的增益可调，超声信号接收传感器一、超声信号接收传感器二、超声信号接收传感器三、无线同步信号接收传感器接收到超声信号、无线同步信号产生电压信号，信号经滤波放大后，由模数转换电路对其采样，并传送到微处理器二；

所述超声信号发射单元的超声信号发射器一、二、三在一个周期中以时间间隔t₀依次发射超声信号，使超声信号接收传感器能明确当前接收的超声信号来自哪个超声信号发射器，其中时间间隔t₀需大于等于三个超声信号发射器间的最大距离与超声信号在空气中传播速度的比值。在超声信号发射器一发射超声信号之前一段时间到超声信号发射器三发射超声信号之前的任意时刻无线同步信号发射器发射一次无线同步信号作为超声信号的时间同步信号。

一种多目标六自由度超声定位的方法，包括下列步骤：

(一)、每个超声信号接收单元可以定位一个目标，定义某任意给定三维直角坐标系O_C1为一个超声信号接收单元所在的坐标系，即该超声信号接收单元在坐标系O_C1下的位置已知且固定，分别为P₁(x′₁,y′₁,z′₁)，P₂(x′₂,y′₂,z′₂)和P₃(x′₃,y′₃,z′₃)。定义某任意给定三维直角坐标系O_B为世界坐标系，在世界坐标系中三个超声信号接收传感器的坐标分别为T₁(x′₄,y′₄,z′₄)，T₂(x′₅,y′₅,z′₅)和T₃(x′₆,y′₆,z′₆)，该超声信号发射单元在世界坐标系下的位置已知且固定，三个超声信号发射器的坐标分别为E₁(x′₇,y′₇,z′₇)，E₂(x′₈,y′₈,z′₈)和E₃(x′₉,y′₉,z′₉)；

(二)、定义该超声信号发射单元的三个不共线超声信号发射器所构成的平面P的一般方程为Ax+By+Cz+D＝0，其中A、B、C、D由超声信号发射器在世界坐标系下的坐标决定，世界坐标系被平面P分成Ax+By+Cz+D>0和Ax+By+Cz+D≤0两部分,超声信号发射器发射端朝向Ax+By+Cz+D>0部分；

(三)由E₁(x′₇,y′₇,z′₇)在平面P上可知，平面P的数学表达式亦可写作：

A(x-x′₇)+B(y-y′₇)+C(z-z′₇)＝0.............①

结合方程①与平面P的一般方程可以得到：

D＝-(Ax′₇+By′₇+Cz′₇).............②

又因为E₂(x′₈,y′₈,z′₈)和E₃(x′₉,y′₉,z′₉)都在此平面上，因此它们的坐标都满足方程A(x-x′₇)+B(y-y′₇)+C(z-z′₇)＝0，将它们的坐标依次代入得：

A(x′₈-x′₇)+B(y′₈-y′₇)+C(z′₈-z′₇)＝0.............③

A(x′₉-x′₇)+B(y′₉-y′₇)+C(z′₉-z′₇)＝0.............④

公式①③④是关于A、B、C的线性方程组，此方程组有非零解的充要条件是关于A、B、C的系数行列式Δ＝0；即：

展开此行列式可得：

A＝(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)

B＝(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)

C＝(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)

将A、B、C代入公式②中，可得：

D＝x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈

所以平面P的一般式方程为：

{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈＝0

(四)、开始定位，超声信号发射器一、二、三在一个周期中以时间间隔t₀依次发射超声信号，使超声信号接收传感器能明确当前接收的超声信号来自哪个超声信号发射器，其中时间间隔t₀需大于等于三个超声信号发射器间的最大距离与超声信号在空气中传播速度的比值。在超声信号发射器一发射超声信号之前一段时间到超声信号发射器三发射超声信号之前的任意时刻无线同步信号发射器发射一次无线同步信号作为超声信号的时间同步信号；

(五)、所述超声信号发射单元以固定周期不间断发送定位信号，在超声信号发射单元两个相邻发射周期中，前一周期最后一个发射结束到后一个周期第一个发射开始的时间间隔为t₁，t₁需大于本系统最大可定位距离与超声信号在空气中传播速度的比值；

(六)、采用渡越时间(TOA)定位法获得超声信号接收传感器的实时坐标值，因为无线同步信号在空气中传播速度接近光速，远大于超声在空气中传播的速度，因此将无线同步信号接收传感器接收到无线同步信号的时间作为时间基准，可以解算出超声信号在空气中的传播时间，将其与超声在空气中的传播速度相乘，可以获得超声信号传播的距离，通过这种方法可以得到超声信号接收传感器一到超声信号发射器一、二、三的距离值，分别为L₁、L₂、L₃。

根据上述可列出几何方程：

由于上式为三个分别含有x′₄,y′₄,z′₄三个未知数的三元二次方程，因此可以解算出超声信号接收传感器一坐标T₁(x′₄,y′₄,z′₄)的两组解；

因为超声信号发射器发射端朝向{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈>0部分，去除一组{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈≤0部分的解，得到超声信号接收传感器一在世界坐标系下的坐标T₁(x′₄,y′₄,z′₄)；

同理，亦可以得到超声信号接收传感器二、三在世界坐标系下的坐标T₂(x′₅,y′₅,z′₅)、T₃(x′₆,y′₆,z′₆)，从而实现任意时刻下，任意超声信号接收传感器在世界坐标系下坐标值的获取。

在实际应用过程中，可以取其中任意一个超声信号接收传感器的坐标加任意固定修正常量作为超声信号接收单元的坐标，亦可以取其中两个或三个超声信号接收传感器的坐标的加权平均值作为超声信号接收单元的坐标；

(六)、定义超声信号接收单元一所在超声接收坐标系O_C1相对于世界坐标系O_B的方向余弦矩阵为DCM_BC1：

那么超声信号接收单元一在世界坐标系下的姿态可以由方向余弦矩阵DCM_BC1表示，解算方向余弦矩阵DCM_BC1的具体步骤包括：

1)已知在超声接收坐标系O_C1下，三个超声信号接收传感器的坐标固定，且分别为P₁(x′₁,y′₁,z′₁)，P₂(x′₂,y′₂,z′₂)和P₃(x′₃,y′₃,z′₃)，可以获得： 设x₁＝x′₂-x′₁,y₁＝y′₂-y′₁,z₁＝z′₂-z′₁,x₂＝x′₃-x′₁,y₂＝y′₃-y′₁,z₂＝z′₃-z′₁,那么：

/>

所以：

设x₃＝y₂z₁-y₁z₂,y₃＝z₂x₁-x₂z₁,z₃＝x₂y₁-x₁y₂,则

2)如步骤(六)所述，可以实现任意时刻下，任意超声信号接收传感器在世界坐标系下坐标值的获取，定义超声信号接收传感器一、二、三在世界坐标系下的坐标分别为T₁(x′₄,y′₄,z′₄)，T₂(x′₅,y′₅,z′₅)、T₃(x′₆,y′₆,z′₆)，那么 设x₄＝x′₅-x′₄,y₄＝y′₅-y′₄,z₄＝z′₅-z′₄,x₅＝x′₆-x′₄,y₅＝y′₆-y′₄,z₅＝z′₆-z′₄,那么

所以

设x₆＝y₅z₄-y₄z₅,y₆＝z₅x₄-x₅z₄,z₆＝x₅y₄-x₄y₅,则

3)根据上述可以列出几何方程：

整理可得：

由上述步骤1)、2)可知，x_1～6、y_1～6和z_1～6均为已知量，因此通过上式可以得到共包含a₁₁、a₁₂、a₁₃、a₂₁、a₂₂、a₂₃、a₃₁、a₃₂、a₃₃九个未知量的九个三元一次方程，从而可以解得超声信号接收单元一在世界坐标系下的姿态DCM_BC1，从而实现对目标物体六自由度信息的获取。

同理，(八)，还可以获得其他可能存在的超声信号接收单元在在世界坐标系下的六自由度信息，最终实现在满足高刷新率、高精度条件下对多目标物体六自由度信息的获取。

为了使本技术领域的人员更好的理解本发明，下面结合附图和实施例对本发明作进一步的详细说明。

1、图1、图2和图3是本发明应用的超声定位系统组成示意图，包括超声信号发射单元[100]、超声信号接收单元(头戴型)[200]和超声信号接收单元(手柄型)[300]。超声信号发射单元[100]主要由超声信号发射器一[101]、超声信号发射器二[102]、超声信号发射器三[103]、无线同步信号发射器[104]、微处理器一[105]、滤波放大电路一[106]、滤波放大电路二(三路)[107]组成。超声信号接收单元(头戴型)[200]主要包括超声信号接收传感器一[201]、超声信号接收传感器二[202]、超声信号接收传感器三[203]、模数转换电路[204]、微处理器二[205]、滤波放大电路三[206]、滤波放大电路四(三路)[207]、无线同步信号接收传感器[208]、增益控制电路[209]组成。超声信号接收单元(手柄型)[300]的内部组成部分与超声信号接收单元(头戴型)一致，此处不再赘述。

2、图4为系统整体组成示意图[400]，超声信号发射单元[401]，超声信号接收单元一[402]、超声信号接收单元二[403]、超声信号接收单元三[404]，其中超声信号发射单元[401]发射无线同步信号和超声信号，超声信号接收单元一[402]、超声信号接收单元二[403]、超声信号接收单元三[404]在不同时刻分别接收到无线同步信号和超声信号，分别解算出各自在世界坐标系下的六自由度信息，实现同时对多目标六自由度信息的获取。

3、图5为本发明超声定位方法原理图[500]，在已知三点[501]坐标的情况下，测得三点[501]到点[502]的距离L₁、L₂、L₃，即可构建几何方程，实现对点[502]坐标的求解；

图6为本发明超声定位方法的实际系统应用示意图[600]，E₁(x′₇,y′₇,z′₇)[601]，E₂(x′₈,y′₈,z′₈)[602]、E₃(x′₉,y′₉,z′₉)[603]和T₁(x′₄,y′₄,z′₄)[604]分别为超声信号发射器一、超声信号发射器二、超声信号发射器三和超声信号接收传感器一。

E₁(x′₇,y′₇,z′₇)[601]，E₂(x′₈,y′₈,z′₈)[602]、E₃(x′₉,y′₉,z′₉)[603]三个点构成了一个平面P，假设平面P的一般方程为Ax+By+Cz+D＝0，其中世界坐标系被平面P分成Ax+By+Cz+D>0和Ax+By+Cz+D≤0两部分,超声信号发射器发射端朝向Ax+By+Cz+D>0部分。

由E₁(x′₇,y′₇,z′₇)[601]在平面P上可知，平面P的数学表达式亦可写作

A(x-x′₇)+B(y-y′₇)+C(z-z′₇)＝0.............①

结合方程①与平面P的一般方程可以得到：

D＝-(Ax′₇+By′₇+Cz′₇).............②

又因为E₂(x′₈,y′₈,z′₈)[602]、E₃(x′₉,y′₉,z′₉)[603]都在此平面上，因此它们的坐标都满足方程A(x-x′₇)+B(y-y′₇)+C(z-z′₇)＝0，将它们的坐标依次代入得：

A(x′₈-x′₇)+B(y′₈-y′₇)+C(z′₈-z′₇)＝0.............③

A(x′₉-x′₇)+B(y′₉-y′₇)+C(z′₉-z′₇)＝0.............④

方程①③④是关于A、B、C的线性方程组，此方程组有非零解的充要条件是关于A、B、C的系数行列式Δ＝0；即：

/>

展开此行列式可得：

A＝(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)

B＝(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)

C＝(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)

将A、B、C代入公式②中，可得：

D＝x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈

所以平面P的一般式方程为：

{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈＝0

4、图7为无线、超声信号发射时序图[700]，其中无线同步信号发射器发射无线同步信号的持续时间为[701]，超声信号发射器一、二、三发射超声信号的持续时间均为[702]，无线同步信号发射器发射无线同步信号起始时间与超声信号发射器一发射超声信号起始时间的时间间隔为[703]，超声信号发射器一发射超声信号起始时间与超声信号发射器二发射超声信号起始时间的时间间隔和超声信号发射器二发射超声信号起始时间与超声信号发射器三发射超声信号起始时间的时间间隔均为[704]，无线、超声信号发射的两个周期间的时间间隔即超声信号发射器三发射超声信号的结束时间与下一个周期中无线同步信号发射器发射无线同步信号的起始时间的时间间隔为[705]；

图8为无线、超声信号接收时序图[800]，其中无线同步信号接收传感器接收无线同步信号的持续时间为[801]与无线同步信号发射器发射无线同步信号的持续时间[701]相等；超声信号接收传感器接收超声信号的持续时间为[802]与超声信号发射器发射超声信号的持续时间[702]相等。超声信号接收传感器接收到超声信号发射器一、二、三发出的超声信号的先后顺序与超声信号发射器一、二、三发射超声信号的先后顺序相同。从无线同步信号接收传感器接收到无线同步信号到超声信号接收传感器接收到超声信号发射器一发出的超声信号的时间间隔[803]，略大于无线同步信号发射器发射无线同步信号起始时间与超声信号发射器一发射超声信号起始时间的时间间隔为[703]。超声信号接收传感器接收到超声信号发射器一与超声信号发射器二发出的超声信号的时间间隔[804]和超声信号接收传感器接收到超声信号发射器二与超声信号发射器三发出的超声信号的时间间隔[805]大小与超声信号发射器一发射超声信号起始时间与超声信号发射器二发射超声信号起始时间的时间间隔[704]大小相似但不相同。超声信号接收传感器接收到超声信号发射器三发出的超声信号的结束时间与无线同步信号接收传感器接收到下一个周期中无线同步信号发射器发出的无线同步信号的起始时间的时间间隔为[806]略小于超声信号发射器三发射超声信号的结束时间与下一个周期中无线同步信号发射器发射无线同步信号的起始时间的时间间隔[705]。

5、图9为超声信号接收传感器在世界坐标系下的坐标计算流程图[900]，在超声信号接收单元接收到无线同步信号与超声信号[901]后，因为无线同步信号在空气中传播速度接近光速，远大于超声在空气中传播的速度，因此可以将无线同步信号发射器开始发射无线同步信号的时间作为无线同步信号接收传感器接收到无线同步信号的时间，即可以将无线同步信号接收传感器接收到无线同步信号的时间作为时间基准，得到超声信号接收传感器一第一次接收到超声信号的时间；

将超声信号接收传感器一第一次接收到超声信号的时间与无线同步信号发射器发射无线同步信号起始时间与超声信号发射器一发射超声信号起始时间的时间间隔[603]做差可以得到超声信号发射器一发射的超声信号在空气中的传播时间，用该时间与超声在空气中的传播速度相乘，可以获得超声信号发射器一发射的超声信号传播的距离即超声信号接收传感器一到超声信号发射器一的距离值L₁，同理可以得到超声信号接收传感器一到超声信号发射器二、三的距离值分别为L₂、L₃，流程图[900]中超声信号接收传感器一到超声信号发射器一、二、三的距离L₁、L₂、L₃的计算[902]完成。

根据上述条件，我们可以列出几何方程计算出超声信号接收传感器一在世界坐标系下的两个可能坐标[903]：

由于上式为三个分别含有x′₄,y′₄,z′₄三个未知数的三元二次方程，因此可以解算出超声信号接收传感器一坐标T₁(x′₄,y′₄,z′₄)的两组解。

接下来是对上述两组解的合理性判断[904]，因为超声信号发射器发射端朝向{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈>0部分，去除一组坐标位于{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈≤0部分的解[905]，得到超声信号接收传感器一在世界坐标系下的坐标T₁(x′₄,y′₄,z′₄)[906]；

同理，亦可以得到超声信号接收传感器二、三在世界坐标系下的坐标T₂(x′₅,y′₅,z′₅)、T₃(x′₆,y′₆,z′₆)，从而实现任意时刻下，任意超声信号接收传感器在世界坐标系下坐标值的获取。

在实际应用过程中，可以取其中任意一个超声信号接收传感器的坐标加任意固定修正常量作为超声信号接收单元的坐标，亦可以取其中两个或三个超声信号接收传感器的坐标的加权平均值作为超声信号接收单元的坐标。

6、图10是超声信号接收单元在世界坐标系下姿态计算流程图[1000]，定义超声信号接收单元一所在超声接收坐标系O_C1相对于世界坐标系O_B的方向余弦矩阵为DCM_BC1：

那么超声信号接收单元一在世界坐标系下的姿态可以由方向余弦矩阵DCM_BC1表示，解算方向余弦矩阵DCM_BC1的具体步骤包括：

1)已知在超声接收坐标系O_C1下，三个超声信号接收传感器的坐标固定，且分别为P₁(x′₁,y′₁,z′₁)，P₂(x′₂,y′₂,z′₂)和P₃(x′₃,y′₃,z′₃)[1001]。可以获得： 设x₁＝x′₂-x′₁,y₁＝y′₂-y′₁,z₁＝z′₂-z′₁,x₂＝x′₃-x′₁,y₂＝y′₃-y′₁,z₂＝z′₃-z′₁,那么

所以：

设x₃＝y₂z₁-y₁z₂,y₃＝z₂x₁-x₂z₁,z₃＝x₂y₁-x₁y₂,则

2)根据上述第5部分所述，可以实现任意时刻下，任意超声信号接收传感器在世界坐标系下坐标值的获取，定义超声信号接收传感器一、二、三在世界坐标系下的坐标分别为T₁(x′₄,y′₄,z′₄)，T₂(x′₅,y′₅,z′₅)、T₃(x′₆,y′₆,z′₆)[1003]。那么 设x₄＝x′₅-x′₄,y₄＝y′₅-y′₄,z₄＝z′₅-z′₄,x₅＝x′₆-x′₄,y₅＝y′₆-y′₄,z₅＝z′₆-z′₄,那么：

所以：

设x₆＝y₅z₄-y₄z₅,y₆＝z₅x₄-x₅z₄,z₆＝x₅y₄-x₄y₅,则

3)根据上述可以列出几何方程：

整理可得：

由权利要求4(七)中的步骤1)2)可知，x_1～6、y_1～6和z_1～6均为已知量，因此通过上式可以得到共包含a₁₁、a₁₂、a₁₃、a₂₁、a₂₂、a₂₃、a₃₁、a₃₂、a₃₃九个未知量的九个三元一次方程，从而可以解得超声信号接收单元一在世界坐标系下的姿态DCM_BC1，从而实现对目标物体六自由度信息的获取。

同理，八、还可以获得其他可能存在的超声信号接收单元在在世界坐标系下的六自由度信息，最终实现在满足高刷新率、高精度条件下对多目标物体六自由度信息的获取。

下面示意性地给出本发明的一个具体实施样例。

1、超声信号发射器一与超声信号发射器二之间之间的距离为80mm，超声信号发射器一与超声信号发射器三之间的距离均为超声信号发射器二与超声信号发射器三之间的距离为160mm，超声信号接收传感器一与超声信号接收传感器二之间，超声信号接收传感器一与超声信号接收传感器三之间的距离均为47.50mm，超声信号接收传感器二与超声信号接收传感器三之间的距离为79.80mm。本系统的最大可定位范围为350000mm，超声在空气中的传播速度为340000mm/s；

超声信号发射器一、二、三在世界坐标系下的坐标分别为E₁(-80,40,0)，E₂(-80,-40,0)，E₃(80,-40,0)，超声信号接收传感器一、二、三在超声接收坐标系下的坐标分别为P₁(0,0,0)，P₂(39.90,-25.77,0)，P₃(-39.90,-25.77,0)，那么

所以：

2、由超声信号发射器一、二、三在世界坐标系下的坐标分别为E₁(-80,40,0)，E₂(-80,-40,0)，E₃(80,-40,0)可知超声信号接收传感器一、二、三所构成的平面P的一般方程为z＝0；

3、超声信号发射单元开始工作，无线同步信号发射器、超声信号发射器一、二、三依次发射无线同步信号或超声信号。无线同步信号发射器与超声信号发射器一发射信号的时间间隔[603]为100μs，超声信号发射器一与超声信号发射器二及超声信号发射器二与超声信号发射器三发射超声信号的时间间隔[604]为t₀，因为时间间隔[604]需大于等于三个超声信号发射器间的最大距离与超声信号在空气中传播速度的比值4.1595μs，t₀实际取值为100μs。超声信号发射器一、二、三发射超声信号的持续时间[602]均为100μs；

4、超声信号发射单元两个发射周期之间的时间间隔[605]为t₁，时间间隔[605]需大于本系统最大可定位距离与超声信号在空气中传播速度的比值10.2941μs，t₁实际取值为100μs；

5、将无线同步信号接收传感器接收到无线同步信号的时间作为时间基准，得到超声信号接收传感器一第一次接收到超声信号的时间102.8370μs，将其与无线同步信号发射器发射无线同步信号起始时间与超声信号发射器一发射超声信号起始时间的时间间隔100μs做差可以得到超声信号发射器一发射的超声信号在空气中的传播时间2.8370μs，用该时间与超声在空气中的传播速度相乘，可以获得超声信号接收传感器一到超声信号发射器一的距离值964.5738mm。由超声信号接收传感器一第二次接收到超声信号的时间302.9033μs，将其与无线同步信号发射器发射无线同步信号起始时间与超声信号发射器一发射超声信号起始时间的时间间隔100μs、超声信号发射器一发射超声信号的持续时间100μs及超声信号发射器一与超声信号发射器二发射超声信号的时间间隔100μs做差可以得到超声信号发射器二发射的超声信号在空气中的传播时间2.9033μs，用该时间与超声在空气中的传播速度相乘，可以获得超声信号接收传感器一到超声信号发射器二的距离值987.1075mm。与上面计算过程类似，由超声信号接收传感器一第三次接收到超声信号的时间502.8823μs，可以得到超声信号接收传感器一到超声信号发射器三的距离值979.9718mm。同理，以超声信号接收传感器二接收到无线同步信号的时间作为时间基准，得到超声信号接收传感器二第一次、第二次和第三次接收到超声信号的时间分别为102.9106μs、302.9687μs和502.9362μs；以超声信号接收传感器三接收到无线同步信号的时间作为时间基准，得到超声信号接收传感器三第一次、第二次和第三次接收到超声信号的时间分别为102.7278μs、302.7908μs和502.7835μs，从而可以得到所述超声信号传感器二与超声信号发射器一、二、三之间的距离分别为989.5977mm、1009.3684mm、998.3119mm，超声信号接收传感器三与超声信号发射器一、二、三之间的距离分别为927.4509mm、948.8811mm、946.3877mm；

结合公式：

可以得到超声信号接收传感器一、二、三在世界坐标系下的坐标分别为：

T₁(43.8640,274.8660,927.3070)或(43.8640,274.8660,-927.3070)、T₂(69.3684,247.0061,956.1073)或(69.3684,247.0061,-956.1073)、T₃(14.7675,251.3130,898.0708)或(14.7675,251.3130,-898.0708)；

又因为超声信号发射器发射端朝向z>0部分，去除三组z≤0的解，得到超声信号接收传感器一、二、三在世界坐标系下的坐标分别为T₁(43.8640,274.8660,927.3070)、T₂(69.3684,247.0061,956.1073)、T₃(14.7675,251.3130,898.0708)，从而实现超声信号接收单元一在世界坐标系下坐标值的获取；

又因为：

所以：

所以超声信号接收单元一在世界坐标系下的姿态DCM_BC1为：

则超声信号接收单元一的横滚角、航向角、俯仰角为：

(0.0123,-0.8143,-0.0787)

实现了对超声信号接收单元一所对应目标物体的六自由度信息的获取；

同理，也可以获得其他可能存在的超声信号接收单元在在世界坐标系下的六自由度信息，最终实现在满足高刷新率、高精度条件下对多目标物体六自由度信息的获取。

上文是针对特殊实施样例的描述，在实际的实现过程中，为了实现对更多功能或更方便的控制方式，可以对描述中的实施样例做适当的变形或修改。由此，上文中的描述应该被理解为仅对各种原理进行说明，而非对本发明进行限制。

Claims (9)

1.一种采用多目标六自由度超声定位系统的定位方法，所述多目标六自由度超声定位系统包括：一个超声信号发射单元和N个超声信号接收单元，N≥1，每个超声信号发射单元包含一个无线同步信号发射器和m₁个超声信号发射器，其中至少包含三个不共线的超声信号发射器，每个超声信号接收单元包含一个无线同步信号接收传感器和m₂个超声信号接收传感器、其中至少包含三个不共线的超声信号接收传感器；

其特征在于，包括下列步骤：

(一)、每个超声信号接收单元可以定位一个目标，定义某任意给定三维直角坐标系O_C1为一个超声信号接收单元所在的坐标系，即该超声信号接收单元在坐标系O_C1下的位置已知且固定，分别为P₁(x′₁,y′₁,z′₁)，P₂(x′₂,y′₂,z′₂)和P₃(x′₃,y′₃,z′₃)；定义某任意给定三维直角坐标系O_B为世界坐标系，在世界坐标系中三个超声信号接收传感器的坐标分别为T₁(x′₄,y′₄,z′₄)，T₂(x′₅,y′₅,z′₅)和T₃(x′₆,y′₆,z′₆)；超声信号发射单元在世界坐标系下的位置已知且固定，三个超声信号发射器的坐标分别为E₁(x′₇,y′₇,z′₇)，E₂(x′₈,y′₈,z′₈)和E₃(x′₉,y′₉,z′₉)；

(二)、定义该超声信号发射单元的三个不共线超声信号发射器所构成的平面P的一般方程为Ax+By+Cz+D＝0，其中A、B、C、D由超声信号发射器在世界坐标系下的坐标决定，世界坐标系被平面P分成Ax+By+Cz+D>0和Ax+By+Cz+D≤0两部分,超声信号发射器发射端朝向Ax+By+Cz+D>0部分；

(三)、由E₁(x′₇,y′₇,z′₇)在平面P上可知，平面P的数学表达式亦可写作：

A(x-x′₇)+B(y-y′₇)+C(z-z′₇)＝0.............①

结合方程①与平面P的一般方程可以得到：

D＝-(Ax′₇+By′₇+Cz′₇).............②

又因为E₂(x′₈,y′₈,z′₈)和E₃(x′₉,y′₉,z′₉)都在此平面上，因此它们的坐标都满足方程A(x-x′₇)+B(y-y′₇)+C(z-z′₇)＝0，将它们的坐标依次代入得：

A(x′₈-x′₇)+B(y′₈-y′₇)+C(z′₈-z′₇)＝0.............③

A(x′₉-x′₇)+B(y′₉-y′₇)+C(z′₉-z′₇)＝0.............④

公式①③④是关于A、B、C的线性方程组，此方程组有非零解的充要条件是关于A、B、C的系数行列式Δ＝0；即：

展开此行列式可得：

A＝(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)

B＝(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)

C＝(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)

将A、B、C代入公式②中，可得：

D＝x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈

所以平面P的一般式方程为：

{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈＝0

(四)、开始定位，超声信号发射器一、超声信号发射二、超声信号发射三在一个周期中以时间间隔t₀依次发射超声信号，使超声信号接收传感器能明确当前接收的超声信号来自哪个超声信号发射器，其中时间间隔t₀需大于等于三个超声信号发射器间的最大距离与超声信号在空气中传播速度的比值，在超声信号发射器一发射超声信号之前一段时间到超声信号发射器三发射超声信号之前的任意时刻无线同步信号发射器发射一次无线同步信号作为超声信号的时间同步信号；

(五)、超声信号发射单元以固定周期不间断发送定位信号，在超声信号发射单元两个相邻发射周期中，前一周期最后一个发射结束到后一个周期第一个发射开始的时间间隔为t₁，t₁需大于本系统最大可定位距离与超声信号在空气中传播速度的比值；

(六)、采用渡越时间TOA定位法获得超声信号接收传感器的实时坐标值，因为无线同步信号在空气中传播速度接近光速，远大于超声在空气中传播的速度，因此将无线同步信号接收传感器接收到无线同步信号的时间作为时间基准，可以解算出超声信号在空气中的传播时间，将其与超声在空气中的传播速度相乘，可以获得超声信号传播的距离，通过这种方法可以得到超声信号接收传感器一到超声信号发射器一、二、三的距离值，分别为L₁、L₂、L₃；

根据上述可列出几何方程：

由于上式为三个分别含有x′₄,y′₄,z′₄三个未知数的三元二次方程，因此可以解算出超声信号接收传感器一坐标T₁(x′₄,y′₄,z′₄)的两组解；

因为超声信号发射器发射端朝向{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈>0部分，去除一组{(y′₈-y′₇)(z′₉-z′₇)-(z′₈-z′₇)(y′₉-y′₇)}x+{(x′₈-x′₇)(z′₉-z′₇)-(z′₈-z′₇)(x′₉-x′₇)}y+{(x′₈-x′₇)(y′₉-y′₇)-(y′₈-y′₇)(x′₉-x′₇)}z+x′₇y′₉z′₈+x′₈y′₇z′₉+x′₉y′₈z′₇-x′₇y′₈z′₉-x′₈y′₉z′₇-x′₉y′₇z′₈≤0部分的解，得到超声信号接收传感器一在世界坐标系下的坐标T₁(x′₄,y′₄,z′₄)；

同理，亦可以得到超声信号接收传感器二、三在世界坐标系下的坐标T₂(x′₅,y′₅,z′₅)、T₃(x′₆,y′₆,z′₆)，从而实现任意时刻下，任意超声信号接收传感器在世界坐标系下坐标值的获取；

还可以取其中任意一个超声信号接收传感器的坐标加任意固定修正常量作为超声信号接收单元的坐标，亦可以取其中两个或三个超声信号接收传感器的坐标的加权平均值作为超声信号接收单元的坐标；

(七)、定义超声信号接收单元一所在超声接收坐标系O_C1相对于世界坐标系O_B的方向余弦矩阵为DCM_BC1：

那么超声信号接收单元一在世界坐标系下的姿态可以由方向余弦矩阵DCM_BC1表示，解算方向余弦矩阵DCM_BC1的具体步骤包括：

1)已知在超声接收坐标系O_C1下，三个超声信号接收传感器的坐标固定，且分别为P₁(x′₁,y′₁,z′₁)，P₂(x′₂,y′₂,z′₂)和P₃(x′₃,y′₃,z′₃)，可以获得： 设x₁＝x′₂-x′₁,y₁＝y′₂-y′₁,z₁＝z′₂-z′₁,x₂＝x′₃-x′₁,y₂＝y′₃-y′₁,z₂＝z′₃-z′₁,那么：

所以：

设x₃＝y₂z₁-y₁z₂,y₃＝z₂x₁-x₂z₁,z₃＝x₂y₁-x₁y₂,则：

2)如步骤(六)所述，可以实现任意时刻下，任意超声信号接收传感器在世界坐标系下坐标值的获取，定义超声信号接收传感器一、二、三在世界坐标系下的坐标分别为T₁(x′₄,y′₄,z′₄)，T₂(x′₅,y′₅,z′₅)、T₃(x′₆,y′₆,z′₆)，那么 设x₄＝x′₅-x′₄,y₄＝y′₅-y′₄,z₄＝z′₅-z′₄,x₅＝x′₆-x′₄,y₅＝y′₆-y′₄,z₅＝z′₆-z′₄,那么：

所以：

设x₆＝y₅z₄-y₄z₅,y₆＝z₅x₄-x₅z₄,z₆＝x₅y₄-x₄y₅,则

3)根据上述可以列出几何方程：

整理可得：

由上述步骤1)、2)可知，x_1～6、y_1～6和z_1～6均为已知量，因此通过上式可以得到共包含a₁₁、a₁₂、a₁₃、a₂₁、a₂₂、a₂₃、a₃₁、a₃₂、a₃₃九个未知量的九个三元一次方程，从而可以解得超声信号接收单元一在世界坐标系下的姿态DCM_BC1，从而实现对目标物体六自由度信息的获取。

2.根据权利要求1所述的一种采用多目标六自由度超声定位系统的定位方法，其特征在于：所述超声信号发射单元包括无线同步信号发射器、超声信号发射器一、超声信号发射器二、超声信号发射器三、微处理器一、滤波放大电路一、滤波放大电路二，其中三个超声信号发射器在世界坐标系O_B下的坐标已知且不共线；其中微处理器一为超声信号发射单元提供发射信号，经滤波放大电路一、滤波放大电路二分别生成超声驱动信号、无线驱动信号，经超声信号发射器一、超声信号发射器二、超声信号发射器三、无线同步信号发射器，发出超声信号、无线同步信号。

3.根据权利要求1所述的一种采用多目标六自由度超声定位系统的定位方法，其特征在于：所述N个超声信号接收单元中的一个超声信号接收单元包括无线同步信号接收传感器、超声信号接收传感器一、超声信号接收传感器二、超声信号接收传感器三、微处理器二、滤波放大电路三、滤波放大电路四、增益控制电路、模数转换电路，其中超声信号接收传感器一、超声信号接收传感器二、超声信号接收传感器三在该超声信号接收单元所在坐标系O_C1的坐标已知、且不共线；其中滤波放大电路四的增益可调，超声信号接收传感器一、超声信号接收传感器二、超声信号接收传感器三、无线同步信号接收传感器接收到超声信号、无线同步信号产生电压信号，信号经滤波放大后，由模数转换电路对其采样，并传送到微处理器二。

4.根据权利要求2所述的一种采用多目标六自由度超声定位系统的定位方法，其特征在于：所述超声信号发射单元的超声信号发射器一、超声信号发射器二、超声信号发射器三在一个周期中以时间间隔t₀依次发射超声信号，使超声信号接收传感器能明确当前接收的超声信号来自哪个超声信号发射器，其中时间间隔t₀需大于等于三个超声信号发射器间的最大距离与超声信号在空气中传播速度的比值，在超声信号发射器一发射超声信号之前一段时间到超声信号发射器三发射超声信号之前的任意时刻无线同步信号发射器发射一次无线同步信号作为超声信号的时间同步信号。

5.根据权利要求2所述的一种采用多目标六自由度超声定位系统的定位方法，其特征在于：所述无线同步信号发射器采用一种以光速传播的无线信号的信号发射器。

6.根据权利要求5所述的一种采用多目标六自由度超声定位系统的定位方法，其特征在于：所述无线同步信号发射器采用包括红外、2.4G、5G、射频、无线电的无线同步信号的信号发射器。

7.根据权利要求3所述的一种采用多目标六自由度超声定位系统的定位方法，其特征在于：所述无线同步信号接收传感器采用一种以光速传播的无线信号的信号接收传感器。

8.根据权利要求7所述的一种采用多目标六自由度超声定位系统的定位方法，其特征在于：所述无线同步信号接收传感器包括红外、2.4G、5G、射频、无线电的无线同步信号的信号接收传感器。

9.根据权利要求1所述的一种采用多目标六自由度超声定位系统的定位方法，其特征在于，还包括步骤(八)、还可以同样方式获得其他可能存在的超声信号接收单元在在世界坐标系下的六自由度信息，最终实现在满足高刷新率、高精度条件下对多目标物体六自由度信息的获取。