CN108387871A - 一种实现六自由度测量的超声三维定位系统和定位方法 - Google Patents

一种实现六自由度测量的超声三维定位系统和定位方法 Download PDF

Info

Publication number
CN108387871A
CN108387871A CN201810093554.9A CN201810093554A CN108387871A CN 108387871 A CN108387871 A CN 108387871A CN 201810093554 A CN201810093554 A CN 201810093554A CN 108387871 A CN108387871 A CN 108387871A
Authority
CN
China
Prior art keywords
module
transmitting
modules
emitter
ultrasonic
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201810093554.9A
Other languages
English (en)
Other versions
CN108387871B (zh
Inventor
燕学智
王海云
于嘉鑫
陈建
孙晓颖
刘国红
赵昱
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jilin University
Original Assignee
Jilin University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jilin University filed Critical Jilin University
Priority to CN201810093554.9A priority Critical patent/CN108387871B/zh
Publication of CN108387871A publication Critical patent/CN108387871A/zh
Application granted granted Critical
Publication of CN108387871B publication Critical patent/CN108387871B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G01S5/186—Determination of attitude

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Length Measuring Devices Characterised By Use Of Acoustic Means (AREA)

Abstract

本发明涉及一种实现六自由度测量的超声三维定位系统和定位方法，属于超声三维定位方法。系统包括发射装置和接收装置，将首先接收到超声的接收模块称为目标模块，剩余两个接收模块称为目标节点，测量出不同发射模块发射超声波时，目标节点与目标模块接收到超声波时刻的时间差值，从而解算出不同发射模块发射超声波时，发射模块到目标节点和目标模块的距离差值，通过计算得到发射模块的坐标值，实现对发射模块的定位，根据获得的两发射模块的坐标值，可提供发射装置的距离方向，及其在XY，YZ，XZ面上的投影距离，从而获得发射装置的姿态角。本发明在一定的空间区域内，实现三维姿态、坐标的获取，不受红外信号和电磁信号的干扰。

Description

一种实现六自由度测量的超声三维定位系统和定位方法
技术领域
本发明涉及一种超声三维定位方法，具体涉及一种实现六自由度测量的超声三维定位系统和定位方法，可实现发射装置在未使用同步信号的情况下实现三维定位。
背景技术
目前，定位技术主要是三种：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)分别可以解算出在XY，YZ，XZ面上的投影距离，分别为L11、L12、L13，表达式：
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)分别可以解算出在XY，YZ，XZ面上的投影距离，分别为L11、L12、L13，表达式：
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，且规定为正方向，两发射模块对称放置在发射装置中，与发射装置的方向垂直，由此可知发射装置的方向与两个发射模块的距离方向垂直；分别可以解算出在XY，YZ，XZ面上的投影距离，分别为L11、L12、L13，表达式：
可以解算出发射装置的姿态角θ[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的坐标近似相同，即满足 (2)非上电状态时采用卡尔曼滤波算法[900]：利用运动坐标、运动轨迹和上一时刻的坐标值结合已解算出的两组解确定下一时刻的唯一发射模块O1(x4,y4,z4)、O2(x5,y5,z5) 的三维坐标值是[901]还是[902]；
发射模块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]，所需的传播时间也就最长加上安全距离所需的时长，保证O2发射的超声波被接收模块全部接收时，最近模块O1才可被距离最近的接收模块接收到 [10012]；图10B两发射模块以间隔时隙t发射超声波示意图[1002]，发射模块[10023]、 [10024]分别向最远接收模块[10025]发射超声波，最远距离时超声波的传播时间为t'，为了保证O2发射的超声波被接收模块全部接收时，最近模块O1才可被距离最近的接收模块接收到，所以加上一定的安全距离，在本次实施案例中，最远距离为两发射模块距离加上接收模块之间的距离，总长为15cm，所以传播所需时间为0.44ms，加上安全距离所需的时长，本案例中给定的时间间隙为1ms。
上文的描述针对特定的实施例，在实际的实现过程中，可以对描述的实施例进行其他的变形和修改，以实现更多的按键操作或更方便的控制方式。由此，这些描述应该被理解为仅对各种原理进行说明，而非对本发明进行限制。

Claims (4)

1.一种实现六自由度测量的超声三维定位系统，其特征在于：包括发射装置和接收装置，其中：
所述发射装置包括第一发射模块、第二发射模块、第一微处理器、第一供电模块、第一时钟模块、第一通信模块、第一开关，两个发射模块设置在距离固定的位置上；所述第一通信模块、超声发射模块分别与第一微控制器连接，所述第一供电模块同时为两个发射模块供电，且两发射模块在发射装置中对称放置；
所述接收装置包括三个接收模块、第二微处理器、第二供电模块、第二时钟模块，所述超声接收模块坐标已知且且不共线，所述超声接收模块分别与第二微控制器连接，所述第二供电模块同时为三个接收模块供电。
2.根据权利要求1所述的实现六自由度测量的超声三维定位系统，其特征在于：所述三个接收模块中，将首先接收到超声波的接收模块称为目标模块，剩余两个接收模块称为目标节点。
3.根据权利要求1所述的实现六自由度测量的超声三维定位系统，其特征在于：所述第一发射模块或第二发射模块，两个发射模块以间隔时隙t发射超声波，使接收模块能明确当前接收的数据来自哪个发射模块，且间隔时隙t保证先发射的发射模块O1(x4,y4,z4)发射的超声波被最远距离的接收模块接收后，发射模块O2(x5,y5,z5)才能被最近距离的超声接收模块接收到。
4.采用如权利要求1所述的实现六自由度测量的超声三维定位系统的实现六自由度测量的超声三维定位方法，其特征在于，包括以下步骤：
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)分别可以解算出在XY，YZ，XZ面上的投影距离，分别为L11、L12、L13，表达式：
4)可以解算出发射装置的姿态角θ、φ、表达式为：
CN201810093554.9A 2018-01-30 2018-01-30 一种实现六自由度测量的超声三维定位系统和定位方法 Active CN108387871B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810093554.9A CN108387871B (zh) 2018-01-30 2018-01-30 一种实现六自由度测量的超声三维定位系统和定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810093554.9A CN108387871B (zh) 2018-01-30 2018-01-30 一种实现六自由度测量的超声三维定位系统和定位方法

Publications (2)

Publication Number Publication Date
CN108387871A true CN108387871A (zh) 2018-08-10
CN108387871B CN108387871B (zh) 2021-10-29

Family

ID=63074183

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810093554.9A Active CN108387871B (zh) 2018-01-30 2018-01-30 一种实现六自由度测量的超声三维定位系统和定位方法

Country Status (1)

Country Link
CN (1) CN108387871B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109782914A (zh) * 2019-01-13 2019-05-21 吉林大学 基于笔式装置轴向旋转的虚拟三维场景中目标的选择方法
CN110335001A (zh) * 2019-08-08 2019-10-15 重庆邮电大学 一种快递件定位指引系统及方法
WO2023230964A1 (zh) * 2022-06-01 2023-12-07 深圳市韶音科技有限公司 一种人体姿态识别系统

Citations (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0651050A (ja) * 1992-07-29 1994-02-25 Sekisui Chem Co Ltd 移動体の姿勢及び３次元位置測定装置
EP1022583A2 (en) * 1999-01-22 2000-07-26 AT&T Laboratories - Cambridge Limited A method of increasing the capacity and addressing rate of an ultrasonic location system
JP2004340883A (ja) * 2003-05-19 2004-12-02 Hideo Kishimoto 超音波による水中の三次元座標位置計測方法と装置
CN1719277A (zh) * 2005-07-26 2006-01-11 清华大学 空间位置检测系统及其检测方法
WO2006119764A1 (en) * 2005-05-13 2006-11-16 Brüel & Kjær Sound & Vibration Measurement A/S Determination of the position of an object
CN1914481A (zh) * 2004-02-03 2007-02-14 伊斯拉视像系统股份公司 用于确定物体在空间中的位置的方法
EP2040035A1 (en) * 2007-09-18 2009-03-25 Honeywell International Inc. Ultrasonic multilateration system for stride vectoring
CN101471734A (zh) * 2007-12-28 2009-07-01 中国科学院声学研究所 一种多发多收声定位网络系统及其定位方法
US20110035183A1 (en) * 2009-08-06 2011-02-10 Honda Motor Co., Ltd. Device, method, and program for detecting position
JP2013186087A (ja) * 2012-03-09 2013-09-19 Toshiba Corp 超音波探傷システム及び超音波探傷装置
CN103455171A (zh) * 2013-09-09 2013-12-18 吉林大学 一种三维交互电子白板系统及方法
CN103487787A (zh) * 2013-09-26 2014-01-01 上海大学 一种四旋翼无人机的超声波定位装置及方法
CN104062633A (zh) * 2014-07-11 2014-09-24 西安电子科技大学 一种基于超声波的室内定位系统及方法
US20150062091A1 (en) * 2013-09-03 2015-03-05 Qualcomm Incorporated Acoustic position tracking system
CN105093181A (zh) * 2015-09-25 2015-11-25 吴云松 超声波三维定位系统及其定位方法
JP2016045119A (ja) * 2014-08-25 2016-04-04 古河機械金属株式会社 ３次元位置測定システム及び３次元位置測定方法
CN205157772U (zh) * 2015-09-25 2016-04-13 吴云松 超声波三维定位系统
CN105929362A (zh) * 2016-04-17 2016-09-07 吉林大学 一种多目标六自由度电磁定位装置和方法
CN106441304A (zh) * 2016-10-08 2017-02-22 东南大学 一种利用可见光通信的三维室内定位方法
CN106933390A (zh) * 2017-03-08 2017-07-07 吉林大学 惯性技术和超声波相结合的触控笔位姿检测方法及系统
CN107102749A (zh) * 2017-04-23 2017-08-29 吉林大学 一种基于超声波和惯性传感器的三维笔式定位方法
CN107121667A (zh) * 2017-04-18 2017-09-01 广州铁路职业技术学院 运动目标室内定位方法及系统
CN107271962A (zh) * 2017-07-31 2017-10-20 成都英萨传感技术研究有限公司 基于超声波的室内定位系统及其定位方法

Patent Citations (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0651050A (ja) * 1992-07-29 1994-02-25 Sekisui Chem Co Ltd 移動体の姿勢及び３次元位置測定装置
EP1022583A2 (en) * 1999-01-22 2000-07-26 AT&T Laboratories - Cambridge Limited A method of increasing the capacity and addressing rate of an ultrasonic location system
JP2004340883A (ja) * 2003-05-19 2004-12-02 Hideo Kishimoto 超音波による水中の三次元座標位置計測方法と装置
CN1914481A (zh) * 2004-02-03 2007-02-14 伊斯拉视像系统股份公司 用于确定物体在空间中的位置的方法
WO2006119764A1 (en) * 2005-05-13 2006-11-16 Brüel & Kjær Sound & Vibration Measurement A/S Determination of the position of an object
CN1719277A (zh) * 2005-07-26 2006-01-11 清华大学 空间位置检测系统及其检测方法
EP2040035A1 (en) * 2007-09-18 2009-03-25 Honeywell International Inc. Ultrasonic multilateration system for stride vectoring
CN101471734A (zh) * 2007-12-28 2009-07-01 中国科学院声学研究所 一种多发多收声定位网络系统及其定位方法
US20110035183A1 (en) * 2009-08-06 2011-02-10 Honda Motor Co., Ltd. Device, method, and program for detecting position
JP2013186087A (ja) * 2012-03-09 2013-09-19 Toshiba Corp 超音波探傷システム及び超音波探傷装置
US20150062091A1 (en) * 2013-09-03 2015-03-05 Qualcomm Incorporated Acoustic position tracking system
CN103455171A (zh) * 2013-09-09 2013-12-18 吉林大学 一种三维交互电子白板系统及方法
CN103487787A (zh) * 2013-09-26 2014-01-01 上海大学 一种四旋翼无人机的超声波定位装置及方法
CN104062633A (zh) * 2014-07-11 2014-09-24 西安电子科技大学 一种基于超声波的室内定位系统及方法
JP2016045119A (ja) * 2014-08-25 2016-04-04 古河機械金属株式会社 ３次元位置測定システム及び３次元位置測定方法
CN105093181A (zh) * 2015-09-25 2015-11-25 吴云松 超声波三维定位系统及其定位方法
CN205157772U (zh) * 2015-09-25 2016-04-13 吴云松 超声波三维定位系统
CN105929362A (zh) * 2016-04-17 2016-09-07 吉林大学 一种多目标六自由度电磁定位装置和方法
CN106441304A (zh) * 2016-10-08 2017-02-22 东南大学 一种利用可见光通信的三维室内定位方法
CN106933390A (zh) * 2017-03-08 2017-07-07 吉林大学 惯性技术和超声波相结合的触控笔位姿检测方法及系统
CN107121667A (zh) * 2017-04-18 2017-09-01 广州铁路职业技术学院 运动目标室内定位方法及系统
CN107102749A (zh) * 2017-04-23 2017-08-29 吉林大学 一种基于超声波和惯性传感器的三维笔式定位方法
CN107271962A (zh) * 2017-07-31 2017-10-20 成都英萨传感技术研究有限公司 基于超声波的室内定位系统及其定位方法

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
HONG JIANG 等: "Lunar Rover Positioning Based on Time of Arrival Measurements of UWB Signals", 《2008 4TH INTERNATIONAL CONFERENCE ON WIRELESS COMMUNICATIONS, NETWORKING AND MOBILE COMPUTING》 *
SEONG JIN KIM 等: "Accurate Hybrid Global Self-Localization Algorithm for Indoor Mobile Robots With Two-Dimensional Isotropic Ultrasonic Receivers", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》 *
U. SCHMINKE 等: "Three-dimensional power-mode ultrasound for quantification of the progression of carotid artery atherosclerosis", 《JOURNAL OF NEUROLOGY》 *
吕日好 等: "空间目标姿态角测量计算方法研究", 《仪器仪表学报》 *
周建民 等: "基于阵列探头的超声缺陷检测仿真及其定位", 《华东交通大学学报》 *
周舜 等: "医科达Axesse直线加速器机房设计与安装经验", 《中国医疗设备》 *
孙志洁 等: "双基地声纳两坐标目标三维定位算法精度分析", 《计算机仿真》 *
燕学智 等: "基于超声波与红外的AGV定位方法", 《吉林大学学报（信息科技辑）》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109782914A (zh) * 2019-01-13 2019-05-21 吉林大学 基于笔式装置轴向旋转的虚拟三维场景中目标的选择方法
CN110335001A (zh) * 2019-08-08 2019-10-15 重庆邮电大学 一种快递件定位指引系统及方法
CN110335001B (zh) * 2019-08-08 2024-04-02 重庆邮电大学 一种快递件定位指引系统及方法
WO2023230964A1 (zh) * 2022-06-01 2023-12-07 深圳市韶音科技有限公司 一种人体姿态识别系统

Also Published As

Publication number Publication date
CN108387871B (zh) 2021-10-29

Similar Documents

Publication Publication Date Title
CN206400088U (zh) 一种信号发送装置
CN108387871A (zh) 一种实现六自由度测量的超声三维定位系统和定位方法
CN104062633B (zh) 一种基于超声波的室内定位系统及方法
JP2017537309A (ja) 向き付けおよび位置付けのための装置および方法
CN111398903B (zh) 船载短基线定位系统及方法
CN106851575B (zh) 一种统一定位基站坐标系的方法和定位校准装置
US20230333670A1 (en) Method and system utilizing phased array beamforming for six degree of freedom tracking for an emitter in augmented reality systems
CN108520640B (zh) 基于超宽带的无人机导航方法、导航设备、无人机
CN104101879B (zh) 基于超声波的车辆空间定位方法及系统
CN109324634A (zh) 一种飞行器及其定位方法、控制方法及光流模块
CN208636719U (zh) 一种基于超声波测距的自动跟随系统
CN106774901B (zh) 基于超声定位的远距离pc体感输入方法
CN107942284A (zh) 基于二维正交非均匀线阵的水下波达方向估计方法与装置
CN109270956A (zh) 一种基于uwb的无人飞行器自主定位导航系统
CN109861762B (zh) 一种基于声-光的跨介质隐蔽通信系统和方法
CN106338290B (zh) 一种机器人自主充电的导航系统及方法
CN105824011A (zh) 无人机自动导引降落相对位置测量装置及方法
CN110376550B (zh) 一种基于位置补偿的三维空间定位方法及系统
CN106878944B (zh) 一种校准定位基站坐标系的方法和定位校准装置
CN108919297A (zh) 一种无人机室内定位系统及方法
CN108459617A (zh) 一种基于投影手机的无人机飞行控制系统
CN107346013A (zh) 一种校准定位基站坐标系的方法及装置
CN207037070U (zh) 一种数字雷达综合模拟器
CN109856592A (zh) 一种无人机的超声定位装置及其定位方法
CN110376543A (zh) 一种三维空间定位方法和系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant