本发明涉及一种超声三维定位方法，具体涉及一种实现六自由度测量的超声三维定位系统和定位方法，可实现发射装置在未使用同步信号的情况下实现三维定位。
背景技术：
目前，定位技术主要是三种：1.无线电定位(包括射频技术)，其原理是通过测量无线电信号的强度来确定位置，系统复杂、定位精度不高，只能到米级；2.视觉定位，其原理主要是基于摄像机采集到地标的图像，采用几何方法来确定位置，其缺陷主要是对摄像头要求较高，对计算能力要求较高，一旦地标脱离摄像机的视野范围则无法定位，因而这类系统成本高、功耗大、结构复杂；3.超声定位，原理主要是测试通过测量超声波发射点到接收点的传送时间，计算出发射点到接收点的距离，然后利用几何的方法求出接收点的位置，这类系统的结构简单、成本低、定位精度较高，可达到厘米级。目前，超声波三维定位主要采用一个发射器和多个接收器的方式对发射器进行定位，然而，需要依靠(红外、电磁等)同步信号测量超声波发射点到接收点的传送时间，易受红外和电磁的干扰，且这能给出三维坐标，无法获取姿态角。
目前已有一些关于超声定位的介绍。
中国专利“基于一发多收的超声波精确测量垂直距离的方法”(申请号201410340275.x)提出一种一发多收的波超声波测量垂直距离的方法，通过放置四个超声波接收器构成正方形，超声波接收器放置在四个超声波接收器构成正方形的对角线交点，通过判断四个超声波接收器接收到的反射波测得的距离是否相等，实现超声波垂直距离的精确测量；然而超声波测量过程中利用反射波的原理进行超声测距，容易受到温度和风向的干扰，使得测量精度不高。中国专利“室内超声波三维定位系统与方法”(申请号201710331249.4)提供了一种应用于室内的超声波三维定位系统及方法，利用红外光作同步信号测量出发射模块到接收模块的时间，计算出发射模块到接收模块的距离，计算出发射模块的三维坐标，红外信号作为同步信号的测量，容易带来时间基准误差的问题。
中国专利“超声波三维定位系统及其方法”(申请号201510623025.1)提供了一种三维定位方法及系统，利用电磁做同步信号，利用多发多收实现多目标定位；电磁同步信号易受到电磁干扰，带来时间测量不准的问题，从而影响测距精度。
技术实现要素：
本发明提供一种实现六自由度测量的超声三维定位系统和定位方法。
本发明采取的技术方案是：系统包括发射装置和接收装置，其中：
所述发射装置包括第一发射模块、第二发射模块、第一微处理器、第一供电模块、第一时钟模块、第一通信模块、第一开关，两个发射模块设置在距离固定的位置上；所述第一通信模块、超声发射模块分别与第一微控制器连接，所述第一供电模块同时为两个发射模块供电，且两发射模块在发射装置中对称放置；
所述接收装置包括三个接收模块、第二微处理器、第二供电模块、第二时钟模块，所述超声接收模块坐标已知且且不共线，所述超声接收模块分别与第二微控制器连接，所述第二供电模块同时为三个接收模块供电。
本发明所述三个接收模块中，将首先接收到超声波的接收模块称为目标模块，剩余两个接收模块称为目标节点。
本发明所述第一发射模块或第二发射模块，两个发射模块以间隔时隙t发射超声波，使接收模块能明确当前接收的数据来自哪个发射模块，且间隔时隙t保证先发射的发射模块o1(x4,y4,z4)发射的超声波被最远距离的接收模块接收后，发射模块o2(x5,y5,z5)才能被最近距离的超声接收模块接收到。
一种实现六自由度测量的超声三维定位方法，包括以下步骤：
a.两个发射模块间的距离固定，放置在发射装置上，坐标分别为o1(x4,y4,z4)、o2(x5,y5,z5)，三个接收模块放置在坐标已知且不共线的位置上，坐标分别为t1(x1,y1,z1)、t2(x2,y2,z2)、t3(x3,y3,z3)，且两个发射模块间的距离设为l1；
b.接收模块中，将其中一个接收模块定为目标模块，剩余两个接收模块定为目标节点，将目标模块接收到的两个发射模块发射超声波的时刻分别作为两个发射模块发射超声波到目标节点的时间基准；
c.采用tdoa的方式，以目标模块接收到超声波的时刻作为目标节点的超声波接收时刻为时间基准，可以计算不同发射模块发射超声波时，目标节点和目标模块接收到超声波的时间差值；
d.两个发射模块以间隔时隙t发射超声波，以目标模块首先接收到超声波的时刻开始计时，得到两个发射模块到目标模块的时间差值，计算得出两个发射模块到目标模块的距离差值为l2；
e.计算得出不同发射模块发射超声波时，发射模块到目标节点和目标模块的距离差值，分别为l3、l4、l5、l6；
f.根据下述可列出几何方程：
解算出关于o1(x4,y4,z4)、o2(x5,y5,z5)八组解；
g.八组解中有四组虚数解、四组实数解，因三维坐标值都是实数，去除四组虚数解，又因为发射装置工作在z轴的正半轴空间，再去除两组z值负半轴的解，剩下两组解，根据发射装置是上电状态和非上电(正常工作状态)状态，分别采用上电校准算法和卡尔曼滤波算法确定唯一解，具体步骤包括：
(1)上电校准时的计算方法：发射装置处于上电状态开始工作时，控制发射装置在xz平面内、且发射装置中两发射模块的对称轴同z轴同方向，即两发射模块o1(x4,y4,z4)、o2(x5,y5,z5)的x、y的坐标近似相同，即满足
(2)非上电状态时采用卡尔曼滤波算法：利用运动坐标、运动轨迹和上一时刻的坐标值结合已解算出的两组解确定下一时刻的唯一发射模块o1(x4,y4,z4)、o2(x5,y5,z5)的三维坐标值；
解算发射装置的姿态角θ、φ、
1)根据上述求出的o1(x4,y4,z4)、o2(x5,y5,z5)的三维坐标值，可以获得两个发射模块带有方向的距离值l1，且规定
2)两发射模块对称放置在发射装置中，与发射装置的方向垂直，由此可知发射装置的方向与两个发射模块的距离方向垂直；
3)分别可以解算出
4)可以解算出发射装置的姿态角θ、φ、
本发明将首先接收到超声的接收模块称为目标模块，剩余两个接收模块称为目标节点，将目标模块接收到超声波的时刻作为目标节点的时间基准，且两个发射模块的时间基准不同，测量出不同发射模块发射超声波时，目标节点与目标模块接收到超声波时刻的时间差值，从而解算出不同发射模块发射超声波时，发射模块到目标节点和目标模块的距离差值，通过计算得到发射模块的坐标值，实现对发射模块的定位，根据获得的两发射模块的坐标值，可提供发射装置的距离方向，及其在xy，yz，xz面上的投影距离，从而获得发射装置的姿态角。
本发明提出了一种超声三维定位的实现方法，利用两个发射模块，三个接收模块的多发多少系统，采用tdoa的方式，可在一定的空间区域内，实现三维姿态、坐标的获取，不受红外信号和电磁信号的干扰，且有结构新颖，功耗低等优点，适用于虚拟现实中头盔、发射装置的定位应用。
附图说明
图1是本发明接收装置结构图；
图2是本发明发射模块结构示意图；
图3是本发明系统整体组成图；
图4是本发明发射模块校准示意图；
图5是本发明超声定位方法原理图；
图6是本发明发射模块到目标节点和目标模块的距离差值图；
图7是本发明发射模块坐标计算流程图；
图8是本发明发射装置姿态角示意图；
图9是本发明卡尔曼滤波预测下一时刻坐标示意图；
图10a是本发明两发射模块以最远距离发射超声波示意图；
图10b是本发明两发射模块以间隔时隙t发射超声波示意图。
具体实施方式
系统包括发射装置和接收装置，其中：
所述发射装置包括第一发射模块、第二发射模块、第一微处理器、第一供电模块、第一时钟模块、第一通信模块、第一开关，两个发射模块设置在距离固定的位置上；所述第一通信模块、超声发射模块分别与第一微控制器连接，所述第一供电模块同时为两个发射模块供电，且两发射模块在发射装置中对称放置；
所述接收装置包括三个接收模块、第二微处理器、第二供电模块、第二时钟模块，所述超声接收模块坐标已知且且不共线，所述超声接收模块分别与第二微控制器连接，所述第二供电模块同时为三个接收模块供电。
本发明所述三个接收模块中，将首先接收到超声波的接收模块称为目标模块，剩余两个接收模块称为目标节点。
本发明所述第一发射模块或第二发射模块，两个发射模块以间隔时隙t发射超声波，使接收模块能明确当前接收的数据来自哪个发射模块，且间隔时隙t保证先发射的发射模块o1(x4,y4,z4)发射的超声波被最远距离的接收模块接收后，发射模块o2(x5,y5,z5)才能被最近距离的超声接收模块接收到。
一种实现六自由度测量的超声三维定位方法，包括以下步骤：
a.两个发射模块间的距离固定，放置在发射装置上，坐标分别为o1(x4,y4,z4)、o2(x5,y5,z5)，三个接收模块放置在坐标已知且不共线的位置上，坐标分别为t1(x1,y1,z1)、t2(x2,y2,z2)、t3(x3,y3,z3)，且两个发射模块间的距离设为l1；
b.接收模块中，将其中一个接收模块定为目标模块，剩余两个接收模块定为目标节点，将目标模块接收到的两个发射模块发射超声波的时刻分别作为两个发射模块发射超声波到目标节点的时间基准；
c.采用tdoa的方式，以目标模块接收到超声波的时刻作为目标节点的超声波接收时刻为时间基准，可以计算不同发射模块发射超声波时，目标节点和目标模块接收到超声波的时间差值；
d.两个发射模块以间隔时隙t发射超声波，以目标模块首先接收到超声波的时刻开始计时，得到两个发射模块到目标模块的时间差值，计算得出两个发射模块到目标模块的距离差值为l2；
e.计算得出不同发射模块发射超声波时，发射模块到目标节点和目标模块的距离差值，分别为l3、l4、l5、l6；
f.根据下述可列出几何方程：
解算出关于o1(x4,y4,z4)、o2(x5,y5,z5)八组解；
g.八组解中有四组虚数解、四组实数解，因三维坐标值都是实数，去除四组虚数解，又因为发射装置工作在z轴的正半轴空间，再去除两组z值负半轴的解，剩下两组解，根据发射装置是上电状态和非上电(正常工作状态)状态，分别采用上电校准算法和卡尔曼滤波算法确定唯一解，具体步骤包括：
(1)上电校准时的计算方法：发射装置处于上电状态开始工作时，控制发射装置在xz平面内、且发射装置中两发射模块的对称轴同z轴同方向，即两发射模块o1(x4,y4,z4)、o2(x5,y5,z5)的x、y的坐标近似相同，即满足
(2)非上电状态时采用卡尔曼滤波算法：利用运动坐标、运动轨迹和上一时刻的坐标值结合已解算出的两组解确定下一时刻的唯一发射模块o1(x4,y4,z4)、o2(x5,y5,z5)的三维坐标值；
解算发射装置的姿态角θ、φ、
1)根据上述求出的o1(x4,y4,z4)、o2(x5,y5,z5)的三维坐标值，可以获得两个发射模块带有方向的距离值l1，且规定
2)两发射模块对称放置在发射装置中，与发射装置的方向垂直，由此可知发射装置的方向与两个发射模块的距离方向垂直；
3)分别可以解算出
4)可以解算出发射装置的姿态角θ、φ、
为了使本技术领域的人员更好的理解本发明，下面结合附图和实施方式对本发明作进一步的详细说明。
1、图1和图2为本发明应用的超声定位系统组成示意图，包括接收装置[100]和发射装置[200]两部分，接收装置[100]主要由三个接收模块[101，第一微处理器[104]，第一供电模块[105]，第一时钟模块[106]组成。发射装置[200]主要包括第一发射模块[201]，第二发射模块[202]，第二微处理器[203]，第二供电模块[204]，第二通信模块[205]，第二时钟模块[206]，开关[207]组成。系统整体组成图可参考[300]，接收装置[301]，发射装置[302]，射装置[302]的两个发射模块[201]、[202]同时开始发射超声波，接收装置[100]的三个接收模块[101]开始接收超声波，通过三维定位技术对发射模块实现定位。
2、首先参照图4，图4为发射装置校准示意图[400]，开始工作时，发射装置垂直对准接收装置[404]，即发射装置放置的方向与z轴的方向之间的夹角尽可能接近于0，进行上电校准[402]，为之后的发射装置定位提供初始坐标位置，偏移垂直位置[402],如[401]、[403]校准时，提供的初始坐标就会出现偏移误差，求解的下一时刻的发射模块坐标就会出现错解。
3、图5为超声定位方法原理图[500]，o1(x4,y4,z4)[201]、o2(x5,y5,z5)[202]两个发射模块，且两个发射模块间的距离设为l1[，t1(x1,y1,z1)[101]、t2(x2,y2,z2)[101]、t3(x3,y3,z3)[101]，为三个接收模块，将首先接收到超声的接收模块称为目标模块，剩余两个接收模块称为目标节点，将目标模块接收到超声波的时刻作为目标节点的时间基准，且两个发射模块发射的超声波到目标模块的时刻不同，所以时间基准不同，测量出不同发射模块发射超声波时，目标节点与目标模块接收到超声波时刻的时间差值，从而解算出不同发射模块发射超声波时，发射模块到目标节点和目标模块的距离差值，通过计算得到发射模块的坐标值，实现对发射模块的定位。
4、图6为发射模块到目标节点和目标模块的距离差值图[600]，示意性地给出本发明的一个实施例的三维定位原理方法，o1(x4,y4,z4)[201]、o2(x5,y5,z5)[202]为两个距离固定的发射模块，设t1(x1,y1,z1)[101]为目标模块，t2(x2,y2,z2)[101]、t3(x3,y3,z3)[101]为目标节点，将目标模块[602]接收到o1(x4,y4,z4)[201]、o2(x5,y5,z5)[202]两个发射模块发射的超声波的时刻作为目标节点的时间基准，且两个发射模块发射的超声波到目标模块的时刻不同，所以时间基准不同，分别为[605]、[606]，测量出不同发射模块发射超声波时，目标节点与目标模块接收到超声波时刻的时间差值△t1、△t2、△t3、△t4，可以计算得出不同发射模块发射超声波时，发射模块到目标节点和目标模块的距离差值，分别为l3[601]、l4[602]、l5[603]、l6[604]，两个发射模块发射超声波的时间间隙为t[607]。
图7为发射模块坐标计算流程图[700]，按下开关开始[701]，发射装置垂直对准接收装置，进行上电校准[702]，两个发射模块以间隔时隙t发射超声波，以目标模块首先接收到超声波的时刻开始计时，得到两个发射模块到目标模块的时间差值，计算出两个发射模块到目标模块的距离差值为l2[703]，将目标模块[602]接收到o1(x4,y4,z4)[601]、o2(x5,y5,z5)[605]两个发射模块发射的超声波的时刻作为目标节点的时间基准，且两个发射模块的时间基准不同，分别为[6010]、[6011]，测量出不同发射模块发射超声波时，目标节点与目标模块接收到超声波时刻的时间差值△t1、△t2、△t3、△t4[704]，计算得出不同发射模块发射超声波时，发射模块到目标节点和目标模块的距离差值[705]，分别为l3[606]、l4[607]、l5[608]、l6[609]，根据上述可列出几何方程：
可以结算出关于o1(x4,y4,z4)、o2(x5,y5,z5)八组解[706]，八组解中去除四组虚数解和两组z轴的负半轴的解，剩下两组解[707]，判断发射装置是否为上电状态[708]，如果是上电状态，采用上电状态算法[7010],确定唯一解[7011]，如果是正常工作状态，采用卡尔曼滤波算法[709]，得到唯一解[7011]，计算两发射模块的距离及在xy、yz、xz面上的投影值[7012]，计算出姿态角[7013]。
5、图8为发射装置姿态角示意图[800]，发射模块o1(x4,y4,z4)[201]、o2(x5,y5,z5)[202]的三维坐标值，可以获得两个发射模块带有方向的距离值l1，且规定
可以解算出发射装置的姿态角θ[801]、φ[802]、
下面示意性地给出本发明的一个具体实施例。
1)发射装置开始工作时，垂直对准接收装置，即发射装置放置的方向与y轴的方向之间的夹角尽可能接近于0，进行上电校准，为之后的发射装置定位提供初始坐标位置；；
2)采用tdoa的方式，将目标模块接收到超声波的时刻作为目标节点的时间基准，且两个发射模块的时间基准不同，测量出不同发射模块发射超声波时，目标节点与目标模块接收到超声波时刻的时间差值△t1、△t2、△t3、△t4；
3)解算出不同发射模块发射超声波时，发射模块到目标节点和目标模块的距离差值li＝△ti·c，i＝1,2,3,4，c为超声波传播速度；
4)利用几何方程及卡尔曼滤波计算得到发射模块的坐标值。
两个发射模块的距离控制在50mm；三个接收模块放置距离控制在80mm左右；本实施案列中给定坐标为t1(0,80,0)，t2(0,0,0)，t3(10,0,0)，两个发射模块距离固定l1＝200，设发射模块的三维坐标值为o1(x4,y4,z4)、o2(x5,y5,z5)设x4＝100；y4＝100；z4＝150；x5＝300；y5＝100；z5＝150；单位为mm级的，利用几何方程：
可以计算出l1＝200、l2＝154.622、l3＝24.771、l4＝20.110、l5＝-426.2087、l6＝5.462的值，根据计算出的距离差值，可以获得发射模块的三维坐标值，且有八组解，
o1(x4,y4,z4)的八组解：
①[100.000,100.000,149.999]
②[114.610,113.258,-92.167]
③[114.610,113.258,92.167]
④[100.000,100.000,-149.999]
⑤[909.752+195.076*i,834.815+177.023*i,85.431-818.386*i]
⑥[909.752-195.076*i,834.815-177.023*i,-85.431-818.386*i]
⑦[909.752+195.076*i,834.815+177.023*i,-85.431+818.386*i]
⑧[909.752-195.076*i,834.815-177.023*i,85.431+818.386*i]
o2(x5,y5,z5)的八组解：
①[300.000,100.000,149.999]
②[307.653,110.936,-39.928]
③[307.653,110.936,39.928]
④[300.000,100.000,-149.999]
⑤[724.195+102.192*i,706.141+146.024*i,-57.870-670.279*i]
⑥[724.195-102.192*i,706.141-146.024*i,57.870-670.279*i]
⑦[724.195+102.192*i,706.141+146.024*i,57.870+670.279*i]
⑧[724.195-102.192*i,706.141-146.024*i,-57.870+670.279*i]
因为三维坐标是实数解，所以可以去除四组虚数解：
o1(x4,y4,z4)的四组解：①②③④
o2(x5,y5,z5)的四组解：①②③④
考虑到三维坐标z轴是正数，可以去除两组z轴负半轴的解：
o1(x4,y4,z4)的两组解：①③
o2(x5,y5,z5)的两组解：①③
剩下两组解，根据发射装置是上电状态和非上电状态，分别采用上电校准算法和卡尔曼滤波算法确定唯一解，具体步骤包括：
(1)上电校准时的计算方法：发射装置处于上电状态开始工作时，用户尽量控制发射装置在xz平面内且发射装置中两发射模块的对称轴z轴同方向，即两发射模块o1(x4,y4,z4)、o2(x5,y5,z5)的x、y的坐标近似相同，即满足
发射模块o1(x4,y4,z4)、o2(x5,y5,z5)的三维坐标值:
o1(x4,y4,z4)：[100.000,100.000,149.999]
o2(x5,y5,z5)：[300.000,100.000,149.999]
图10a为两发射模块发射超声波示意图[1001]，两个发射模块以间隔时隙t发射超声波，使接收模块能明确当前接收的数据来自哪个发射模块，且间隔时隙t保证先发射的发射模块o1(x4,y4,z4)发射的超声波被最远距离的接收模块接收后，发射模块o2(x5,y5,z5)才能被最近距离的超声接收模块接收到。两发射模块到接收模块的距离差为：△l＝(o2t2-o1t1)，如图所示当发射模块和接收模块在共线的状态下，距离差最大[10011]、[10012]，所需的传播时间也就最长
上文的描述针对特定的实施例，在实际的实现过程中，可以对描述的实施例进行其他的变形和修改，以实现更多的按键操作或更方便的控制方式。由此，这些描述应该被理解为仅对各种原理进行说明，而非对本发明进行限制。