一种基于声波的自动定位装置及定位方法与流程

1.本发明涉及水下定位技术领域，特别是涉及一种基于声波的自动定位装置及定位方法。


背景技术：

2.地球上海洋面积是地球表面积的71％左右，海洋在我们在生产和生活有着十分重要的作用。水下机器人在开发海洋资源、保护海洋和谋求海洋新的发展等方面发挥着重要作用，水下机器人可以代替人工进行水下作业和水下监控等工作。
3.水下机器人工作时需要进行定位，现有的对水下机器人进行定位的方法是通过gps或北斗等定位系统进行定位，能够进行实时定位。但是gps或北斗等定位系统是利用电磁波进行定位和导航的，由于电磁波在水中呈指数规律衰减，使得水下机器人接收的信号不稳定，gps或北斗等定位系统在局部范围的特殊环境内难以实现对水下机器人的精确定位。
4.因此，针对现有技术不足，提供一种基于声波的自动定位装置及定位方法以克服现有技术不足甚为必要。


技术实现要素：

5.本发明的目的之一在于避免现有技术的不足之处而提供一种基于声波的自动定位装置，定位稳定，且可以在局部范围的特殊环境中实现精准定位。
6.本发明的上述目的通过以下技术措施实现。
7.提供一种基于声波的自动定位装置，设置有待测端、数据端以及三个固定端，三个固定端位于同一水平面，所述待测端设置于三个固定端所在水平面的下方，所述待测端和数据端均与固定端信号连接；
8.所述待测端，用于向固定端发送声波信号；
9.所述固定端，用于接收待测端发送的声波信号并向数据端发送角度测量信号；
10.所述数据端，用于接收固定端发送的角度测量信号并计算待测端的位置坐标。
11.优选的，所述待测端设置有声波安装盘，所述声波发射盘的上半圆均匀安装6个声波发射头。
12.优选的，所述固定端设置有用于沿水平面转动接收声波信号的第一传感器和用于沿垂直面转动接收声波信号的第二传感器。
13.优选的，所述第一传感器设置有声波套、接收头和罗盘仪，所述声波套设置有安装槽，接收头和罗盘仪均固定装配于安装槽的底部。
14.优选的，所述第二传感器设置有声波套、接收头和角度仪，所述声波套设置有接收槽，接收头和角度仪均固定装配于接收槽的底部。
15.优选的，所述固定端还设置有定位立柱、浮台、支架和智能模块，所述浮台和支架均套接于定位立柱，所述支架与浮台固定连接，智能模块固定装配于所述浮台，接收头、罗
盘仪和角度仪均与智能模块信号连接。
16.优选的，所述支架设置有套柱、第一安装臂和第二安装臂，所述套住套接于所述定位立柱，所述浮台与套柱固定连接，所述第一传感器与第一安装臂固定连接，所述第二传感器与第二安装臂固定连接；
17.所述第一安装臂和第二安装臂的尾端均设置有集电环和电机，所述集电环与安装臂固定连接，电机与集电环固定连接。
18.第一传感器与第一安装臂的电机固定连接，第二传感器与第二安装臂的电机固定连接。
19.优选的，所述待测端还设置有固定架，所述固定架设置有用于连接杆和与待测物体连接的固定盘，所述固定盘和声波安装盘固定装配于连接杆的两端。
20.本发明的基于声波的自动定位装置，设置有待测端、数据端以及三个固定端，三个固定端位于同一水平面，待测端设置于三个固定端所在水平面的下方，待测端和数据端均与固定端信号连接；待测端，用于向固定端发送声波信号；固定端，用于接收待测端发送的声波信号并向数据端发送角度测量信号；数据端，用于接收固定端发送的角度测量信号并计算待测端的位置坐标。固定端通过接收待测端的声波信号，确定出待测端在基于三个固定端和待测端建立的空间坐标系中的角度关系，数据端能够根据角度关系以及三个固定端之间的距离计算出待测端在所建立的空间坐标系中的位置坐标，实现对待测端的定位。本发明的基于声波的自动定位装置可以在局部范围特殊环境精确定位，且定位过程中不需要依赖gps或北斗等定位系统，定位稳定。
21.本发明的另一目的在于避免现有技术的不足之处而提供一种基于声波的自动定位方法，可以在局部范围的特殊环境中实现精准定位，且定位过程稳定。
22.本发明的上述目的通过以下技术措施实现。
23.提供一种基于声波的自动定位方法，采用上述基于声波的自动定位装置进行。
24.优选的，包括以下步骤：
25.s1：建立空间坐标系；
26.具体是，定义第一个固定端所在点为a点，第二个固定端所在点为b点，安放在a点的正东方向，第三个固定端所在点为c点，安放在a点的正北方向，待测端所在点为o点，以a点、b点和c点所构成的水平面为坐标系的xy平面。则a点为坐标原点，b点为x轴上一点，c点为y轴上一点，o点为定位坐标点建立空间坐标系；
27.s2：测量a点和b点之间的距离记为l，测量a点和c点之间的距离记为l’；
28.s3：记o点在xy平面上的投影点为o
xy
点，o点在xz平面上的投影点为o
xz
点，o
xy
点在x轴上的投影点为o
x
点，o
xy
点在y轴上的投影点为oy点，o
xy
a与x轴的夹角∠1，o
xy
b与x轴的夹角∠2，o
xz
a与x轴的夹角∠3，o
xy
c与y轴的夹角∠4；
29.s4：定义位于a点的固定端的罗盘仪的读数为∠1’，位于b点的固定端的罗盘仪的读数为∠2’，位于a点的固定端的角度仪的读数为∠3’，位于c点的固定端的罗盘仪的读数为∠4’；
30.测量获得∠1’、∠2’、∠3’和∠4’，具体包括如下步骤：
31.s41：在a点的固定端控制电机带动第一传感器转动，当第一传感器装配的接收头接收到声波信号，智能模块将罗盘仪的读数附值给∠1’，控制电机带动第二传感器转动，当
第二传感器装配的接收头接收到声波信号，智能模块将角度仪的读数附值给∠3’；
32.s42：在b点的固定端控制电机带动第一传感器转动，当第一传感器装配的接收头接收到声波信号，智能模块将罗盘仪的读数附值给∠2’；
33.s43：在c点的固定端控制电机带动第一传感器转动，当第一传感器装配的接收头接收到声波信号，智能模块将罗盘仪的读数附值给∠4’；
34.s5：智能模块将∠1’、∠2’、∠3’和∠4’发送至数据端；
35.s6：数据端根据位于a、b和c点的固定端智能模块分别发来的∠1’、∠2’、∠3’和∠4’的角度数据计算出o点所在坐标(x，y，z)的位置，数据端的运算步骤如下：
36.s61：设o
x
a距离为l
x
，o
xoxy
距离为ly，o
xoxz
距离为lz，点(x，y，z)为o点在空间坐标系中的坐标；
37.s62：计算o
xy
落在xy水平面坐标的象限位置(x，y)，计算情形包括：
[0038]ⅰ、当∠1'《90
°
时，数据端进行如下运算：
[0039]
1、计算∠1和∠2，其中∠1＝90
°‑
∠1'，∠2＝∠2'-270
°
。
[0040]
2、根据式(1)和式(2)计算出ly和l
x
的值。
[0041][0042][0043]
3、由式(1)式(2)式可得：ly＝l
x
×
tan∠1。
[0044]
4、将l
x
的值赋给x，将ly的值赋给y。
[0045]ⅱ、当∠1'＝90
°
时，数据端进行如下运算：
[0046]
1、计算∠4，∠4＝180
°‑
∠4'。
[0047]
2、根据式(3)计算出l
x
的值。
[0048][0049]
3、由式(3)可得l
x
＝l'
×
tan∠4。
[0050]
4、将l
x
的值赋给x，将0赋给y。由于o
xy
落xy水平面坐标系的x轴上，所以ly＝0，
[0051]ⅲ、当90
°
《∠1'≦180
°
时，数据端进行如下运算：
[0052]
1、计算∠1、∠2，其中，∠1＝∠1'-90
°
，∠2＝270
°‑
∠2'。
[0053]
2、根据式(1)和式(2)计算出ly和l
x
的值。
[0054][0055][0056]
3、由式(1)式(2)式可得：ly＝l
x
×
tan∠1。
[0057]
4、将l
x
的值赋给x，将-ly的值赋给y。
[0058]ⅳ、当180
°
《∠1'《270
°
时，数据端进行如下运算：
[0059]
1、计算∠1、∠2，其中∠1＝270
°‑
∠1'，∠2＝270
°‑
∠2'。
[0060]
2、根据式(1)和式(2)计算出ly和l
x
的值。
[0061][0062][0063]
3、由式(1)式(2)式可得：ly＝l
x
×
tan∠1。
[0064]
4、将-l
x
的值赋给x，将-ly的值赋给y。
[0065]
ⅴ
、当∠1'＝270
°
时，数据端进行如下运算：
[0066]
1、计算∠4，∠4＝∠4'-180
°
。
[0067]
2、根据式(3)计算出l
x
的值。
[0068][0069]
3、由式(3)可得l
x
＝l'
×
tan∠4。
[0070]
4、将-l
x
的值赋给x，将0赋给y。
[0071]ⅵ、当270
°
《∠1'≦360
°
时，数据端进行如下运算：
[0072]
1、计算∠1、∠2，其中∠1＝∠1'-270
°
∠2＝∠2'-270
°
。
[0073]
2、根据式(1)和式(2)计算出ly和l
x
的值。
[0074][0075][0076]
3、由式(1)式(2)式可得：ly＝l
x
×
tan∠1。
[0077]
4、将-l
x
的值赋给x，将ly的值赋给y。
[0078]
s63：计算o
xy
落在水下的深度z。
[0079]
1、计算∠3，∠3＝∠3’。
[0080]
2、根据式(4)计算出lz的值。
[0081][0082]
3、由式(4)式可得：lz＝l
x
×
tan∠3。
[0083]
4、将-lz赋值给z。
[0084]
s64：将步骤s63中(x,y)的和步骤s64中的z进行耦合形成o点所在位置坐标(x,y,z)。
[0085]
本发明的基于声波的自动定位方法，采用基于声波的自动定位装置进行，该基于声波的自动定位装置设置有待测端、数据端以及三个固定端，三个固定端位于同一水平面，
待测端设置于三个固定端所在水平面的下方，待测端和数据端均与固定端信号连接；待测端，用于向固定端发送声波信号；固定端，用于接收待测端发送的声波信号并向数据端发送角度测量信号；数据端，用于接收固定端发送的角度测量信号并计算待测端的位置坐标。固定端通过接收待测端的声波信号，确定出待测端在基于三个固定端和待测端建立的空间坐标系中的角度关系，数据端能够根据角度关系以及三个固定端之间的距离计算出待测端在所建立的空间坐标系中的位置坐标，实现对待测端的定位。本发明的基于声波的自动定位方法可以在局部范围特殊环境精确定位，且定位过程中不需要依赖gps或北斗等定位系统，定位稳定。
附图说明
[0086]
利用附图对本发明作进一步的说明，但附图中的内容不构成对本发明的任何限制。
[0087]
图1是本发明一种基于声波的自动定位装置的结构示意图。
[0088]
图2是本发明一种基于声波的自动定位装置待测端的结构示意图。
[0089]
图3是本发明一种基于声波的自动定位装置固定端的结构示意图。
[0090]
图4是本发明一种基于声波的自动定位装置第一传感器的结构示意图。
[0091]
图5是本发明一种基于声波的自动定位装置第二传感器的结构示意图。
[0092]
图6是本发明一种基于声波的自动定位装置支架与浮台的装配关系示意图。
[0093]
图7是实施例3中建立的空间坐标关系示意图。
[0094]
图8是实施例3中空间坐标系的xy水平面示意图，其中图8(a)是点o
xy
在xy水平面坐标系第二象限内的示意图，图8(b)是点o
xy
在xy水平面坐标系第一象限内的示意图，图8(c)是点o
xy
在xy水平面坐标系第三象限内的示意图，图8(d)是点o
xy
在xy水平面坐标系第四象限内的示意图，图8(e)是点o
xy
在xy水平面坐标系x轴负向的示意图，图8(f)是点o
xy
在xy水平面坐标系x轴正向的示意图。
[0095]
图9是实施例3中空间坐标系的xz垂直面示意图。
[0096]
在图1至图9中，包括：
[0097]
待测端100、声波安装盘110、声波发射头120、连接杆130、固定盘140、固定端200、定位立柱210、浮台230、支架240、智能模块250、
[0098]
第一传感器260、第二传感器261、声波套262、接收槽263、接收头264、罗盘仪265、角度仪266、
[0099]
套柱241、第一安装臂242、第二安装臂243、集电环244、电机245、
[0100]
数据端300。
具体实施方式
[0101]
结合以下实施例对本发明作进一步说明。
[0102]
实施例1。
[0103]
一种基于声波的自动定位装置，如图1所示，设置有待测端100、数据端300以及三个固定端200，三个固定端200位于同一水平面，待测端100设置于三个固定端200所在水平面的下方，待测端100和数据端300均与固定端200信号连接。待测端100，用于向固定端200
发送声波信号。固定端200，用于接收待测端100发送的声波信号并向数据端300发送角度测量信号。数据端300，用于接收固定端200发送的角度测量信号并计算待测端100的位置坐标。固定端200根据接收到待测端100发送的声波信号的时刻确定角度测量信号，数据端300根据三个固定端200之间的距离和角度测量信号可以计算出待测端100的位置坐标，可以实现局部范围特殊环境内的精确定位。另外，定位过程中不需要使用gps和北斗等导航系统，是依据声波信号进行定位的，定位过程稳定。
[0104]
本发明的基于声波的自动定位装置在使用时，将三个固定端200安装于待定位物体所在水域的水面上，三个固定端200的距离根据待定位物体的活动范围确定，使待定位物体的活动范围处于三个固定端200之间。待测端100与待定位物体固定连接。数据端300可以由测量人员进行监管，数据端300由计算机、通信模块和配套电路等组成，这些都是本领域人员所熟知技术，在此不再赘述。
[0105]
如图2所示，待测端100设置有声波安装盘110，声波发射盘的上半圆均匀安装6个声波发射头120。本实施例中的声波发射头120采用水下水平无方向型水声换能器。此换能器为水平无方向的，发散角为35
°
，每个声波发射头120的夹角为30
°
，使得声波安装盘110发送的声波形成180
°
全方位无死角，保证固定端200可以接收到声波安装盘110发送的声波信号。
[0106]
如图3所示，固定端200设置有用于沿水平面转动接收声波信号的第一传感器260和用于沿垂直面转动接收声波信号的第二传感器261。第一传感器260可以测量待测端100与固定端200的水平方向上的夹角，第二传感器261可以测量待测端100与固定端200的垂直方向上的夹角。
[0107]
如图4所示，第一传感器260设置有声波套262、接收头264和罗盘仪265，声波套262设置有安装槽，接收头264和罗盘仪265均固定装配于安装槽的底部。只有当安装槽位于待测端100所在平面时，即接收头264位于待测端100所在平面，接收头264才能接收到待测端100发送的声波信号。若接收头264与待测端100不在一个平面上，待测端100发送的声波信号会被声波套262挡住。本实施例中，罗盘仪265选用gy-26电子指南针模块，但不限于gy-26电子指南针模块。
[0108]
如图5所示，第二传感器261设置有声波套262、接收头264和角度仪266，声波套262设置有接收槽263，接收头264和角度仪266均固定装配于接收槽263的底部。本实施例中，角度仪266选用rs485倾角仪，但不限于rs485倾角仪。
[0109]
本实施例中，固定端200还设置有定位立柱210、浮台230、支架240和智能模块250，浮台230和支架240均套接于定位立柱210，支架240与浮台230固定连接，智能模块250固定装配于浮台230，接收头264、罗盘仪265和角度仪266均与智能模块250信号连接。智能模块250内部装配有单片机at89c52、通信模块、控制模块和配套电路等，本实施例中通信模块采用lora无线远程通信成熟产品模块但不限于lora无线远程通信成熟产品模块。单片机at89c52、通信模块、控制模块和配套电路连接关系等这些均为本领域人员所熟知技术，在此不再赘述。
[0110]
本实施例中，基于xy坐标系的习惯方向(正北落在y轴正向，正东落在x轴正向)，在安装定位立柱210时，第一个固定端200的定位立柱210一般安装在作业水面的偏西南的一个角落(可根据实际情况位于三个固定点的西南角)，第二个固定端200的定位立柱210一般
安放在第一个定位立柱210的正东方向，第三个固定端200的定位立柱210一般安放在第一个定位立柱210的正北方向。
[0111]
本实施例中，支架240设置有套柱241、第一安装臂242和第二安装臂243，套住套接于定位立柱210，浮台230与套柱241固定连接，第一传感器260与第一安装臂242固定连接，第二传感器261与第二安装臂243固定连接。浮台230与水面接触，套柱241在浮台230所受浮力的作用下，沿着定位立柱210随水位升降，带动第一传感器260和第二传感器261随水位上下移动。
[0112]
本实施例中，待测端100还设置有固定架，固定架设置有用于连接杆130和与待测物体连接的固定盘140，固定盘140和声波安装盘110固定装配于连接杆130的两端。
[0113]
需要说明的是，固定端200的电机245可以持续转动，实时接收待测端100的声波信号并向数据端300发送角度测量信号，数据端300实时计算待测端100空间位置坐标，可以对待测目标物体进行实时定位并获得待测物体的移动轨迹，且定位过程不需要依赖于gps或者北斗等系统方位标定方法，省时省力且稳定。
[0114]
该基于声波的自动定位装置，通过固定端200接收待测端100发送的声波信号的确定待测端100相对于固定端200的角度测量信号，然后由数据端300根据角度测量信号以及三个固定端200之间的距离计算出待测端100的空间位置坐标，实现对待测端100的定位，能够实现局部范围特殊环境中的精确定位，使用声波信号进行定位，定位过程稳定。不需要依赖于外部gps和北斗等系统方位标定方法，省时省力，且能够获得待测物体的运动轨迹。
[0115]
实施例2。
[0116]
一种基于声波的自动定位装置，其它特征与实施例1相同，不同之处在于：如图6所示，第一安装臂242和第二安装臂243的尾端均设置有集电环244和电机245，集电环244与安装臂固定连接，电机245与集电环244固定连接，第一传感器260与第一安装臂242的电机245固定连接，第二传感器261与第二安装臂243的电机245固定连接。电机245用于带动第一传感器260和第二传感器261转动，集电环244能够解决电机245和传感器之间的卷线问题。
[0117]
该基于声波的自动定位装置，能够实现局部范围特殊环境中的精确定位，定位过程稳定，且定位过程中不会出现卷线问题。
[0118]
实施例3。
[0119]
一种基于声波的自动定位方法，采用如实施例1至2任意一种基于声波的自动定位装置进行，包括以下步骤：
[0120]
s1：建立空间坐标系。
[0121]
具体是，定义第一个固定端200所在点为a点，第二个固定端200所在点为b点，安放在a点的正东方向，第三个固定端200所在点为c点，安放在a点的正北方向，待测端100所在点为o点，以a点、b点和c点所构成的水平面为坐标系的xy平面。则a点为坐标原点，b点为x轴上一点，c点为y轴上一点，o点为定位坐标点建立空间坐标系。本实施例建立的空间坐标系如图7所示。将第二个固定端200安放在第一个固定端200的正东方向，第三个固定端200安放在第一个固定端200的正北方向，能够简化空间坐标系的建立，方便角度的测量。
[0122]
s2：测量a点和b点之间的距离记为l，测量a点和c点之间的距离记为l’。
[0123]
s3：记o点在xy平面上的投影点为o
xy
点，o点在xz平面上的投影点为o
xz
点，o
xy
点在x轴上的投影点为o
x
点，o
xy
点在y轴上的投影点为oy点，o
xy
a与x轴的夹角∠1，o
xy
b与x轴的夹角
∠2，o
xz
a与x轴的夹角∠3，o
xy
c与y轴的夹角∠4。
[0124]
s4：定义位于a点的固定端200的罗盘仪256的读数为∠1’，位于b点的固定端200的罗盘仪256的读数为∠1’，位于c点的固定端200的罗盘仪256的读数为∠4’，位于a点的固定端200的角度仪266的读数为∠3’。罗盘仪读数都是以正北n为00或3600开始读数的，测量正北方向到测线的夹角，所以∠1’为从a点到o
xy
读数，∠2’为b点到o
xy
读数，∠4’为c点到o
xy
读数。
[0125]
测量获得∠1’、∠2’、∠3’和∠4’，具体包括如下步骤：
[0126]
s41：在a点的固定端200控制电机245带动第一传感器260转动，当第一传感器260装配的接收头264接收到声波信号，智能模块250将罗盘仪265的读数附值给∠1’。控制电机245带动第二传感器261转动，当第二传感器261装配的接收头264接收到声波信号，智能模块250将角度仪266的读数附值给∠3’。
[0127]
s42：在b点的固定端200控制电机245带动第一传感器260转动，当第一传感器260装配的接收头264接收到声波信号，智能模块250将罗盘仪265的读数附值给∠2’。
[0128]
s43：在c点的固定端200控制电机245带动第一传感器260转动，当第一传感器260装配的接收头264接收到声波信号，智能模块250将罗盘仪265的读数附值给∠4’。
[0129]
s5：智能模块250将∠1’、∠2’、∠3’和∠4’发送至数据端300。具体是，a点固定端200的智能模块250将∠1’和∠3’的角度数据、b点固定端200的智能模块250将∠2’角度数据以及c点固定端200的智能模块将∠4’的角度数据均发送至数据端300。
[0130]
s6：数据端300根据位于a、b和c点的固定端200发来的∠1’、∠2’、∠3’和∠4’的角度数据计算出o点所在坐标(x，y，z)的位置，数据端300的运算步骤如下：
[0131]
s61：定义o
x
a距离为l
x
，o
xoxy
距离为ly,o
xoxz
距离为lz，点(l
x
，ly，lz)为o点在空间坐标系中的坐标；
[0132]
s62：计算o
xy
落在xy水平面坐标的象限位置(x，y)。计算情形包括如下几种：
[0133]ⅰ、如图8b所示，当∠1'《90
°
时，o
xy
落在xy水平面坐标系的第一象限。数据端300进行如下运算：
[0134]
1、计算∠1和∠2，其中∠1＝90
°‑
∠1'，∠2＝∠2'-270
°
。
[0135]
2、根据式(1)和式(2)计算出ly和l
x
的值。
[0136][0137][0138]
3、由式(1)式(2)式可得：ly＝l
x
×
tan∠1。
[0139]
4、将l
x
的值赋给x，将ly的值赋给y。
[0140]ⅱ、如图8f所示，当∠1'＝90
°
时，o
xy
落xy水平面坐标系的x轴的正向。数据端300进行如下运算：
[0141]
1、计算∠4，∠4＝180
°‑
∠4'。
[0142]
2、根据式(3)计算出l
x
的值。
[0143][0144]
3、由式(3)可得l
x
＝l'
×
tan∠4。
[0145]
4、将l
x
的值赋给x，将0赋给y。由于o
xy
落xy水平面坐标系的x轴上，所以ly＝0，
[0146]ⅲ、如图8d所示，当90
°
《∠1'≦180
°
时，o
xy
落在xy水平面坐标系的第四象限。数据端300进行如下运算：
[0147]
1、计算∠1、∠2，其中，∠1＝∠1'-90
°
，∠2＝270
°‑
∠2'。
[0148]
2、根据式(1)和式(2)计算出ly和l
x
的值。
[0149][0150][0151]
3、由式(1)式(2)式可得：ly＝l
x
×
tan∠1。
[0152]
4、将l
x
的值赋给x，将-ly的值赋给y。
[0153]ⅳ、如图8c所示，当180
°
《∠1'《270
°
时，o
xy
落在xy水平面坐标系的第三象限。数据端300进行如下运算：
[0154]
1、计算∠1、∠2，其中∠1＝270
°‑
∠1'，∠2＝270
°‑
∠2'。
[0155]
2、根据式(1)和式(2)计算出ly和l
x
的值。
[0156][0157][0158]
3、由式(1)式(2)式可得：ly＝l
x
×
tan∠1。
[0159]
4、将-l
x
的值赋给x，将-ly的值赋给y。
[0160]
ⅴ
、如图8e所示，当∠1'＝270
°
时，o
xy
落在xy水平面坐标系的x轴的负向。数据端300进行如下运算：
[0161]
1、计算∠4，∠4＝∠4'-180
°
。
[0162]
2、根据式(3)计算出l
x
的值。
[0163][0164]
3、由式(3)可得l
x
＝l'
×
tan∠4。
[0165]
4、将-l
x
的值赋给x，将0赋给y。
[0166]ⅵ、如图8d所示，当270
°
《∠1'≦360
°
时，o
xy
落在xy水平面坐标系的第二象限。数据端300进行如下运算：
[0167]
1、计算∠1、∠2，其中∠1＝∠1'-270
°
∠2＝∠2'-270
°
。
[0168]
2、根据式(1)和式(2)计算出ly和l
x
的值。
[0169][0170][0171]
3、由式(1)式(2)式可得：ly＝l
x
×
tan∠1。
[0172]
4、将-l
x
的值赋给x，将ly的值赋给y。
[0173]
s63：计算o
xy
落在水平面xy下的深度z。
[0174]
1、计算∠3，∠3＝∠3’。
[0175]
2、根据式(4)计算出lz的值。
[0176][0177]
3、由式(4)式可得：lz＝l
x
×
tan∠3。
[0178]
4、将-lz赋值给z。
[0179]
s64：将步骤s63中(x,y)的和步骤s64中的z进行耦合形成o点所在位置坐标(x,y,z)。坐标(x,y,z)即为待测端所在位置坐标。
[0180]
需要说明的是，在实际应用中第二个固定端200的安装方向不限于在第一个固定端200的正东方向，第三个固定端200的安装方向也不限于在第一个固定端200的正北方向。当第二个固定端200的安装方向不在第一个固定端200的正东方向，第三个固定端200的安装方向不在第一个固定端200的正北方向时，可以参照本实施例上述方法建立相应的空间坐标系和几何关系计算出待测端100的空间位置坐标，在此不再赘述。
[0181]
该基于声波的自动定位方法，能够实现局部范围特殊环境中的精确定位，定位过程稳定，且可以进行实时定位，获取待定位物体的运动轨迹。
[0182]
最后应当说明的是，以上实施例仅用以说明本发明的技术方案而非对本发明保护范围的限制，尽管参照较佳实施例对本发明作了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的实质和范围。