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

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

Info

Publication number
CN108387871B
CN108387871B CN201810093554.9A CN201810093554A CN108387871B CN 108387871 B CN108387871 B CN 108387871B CN 201810093554 A CN201810093554 A CN 201810093554A CN 108387871 B CN108387871 B CN 108387871B
Authority
CN
China
Prior art keywords
transmitting
module
modules
receiving
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.)
Active
Application number
CN201810093554.9A
Other languages
English (en)
Other versions
CN108387871A (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

Images

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

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.根据下述可列出几何方程：
Figure GDA0003271328130000031
解算出关于O1(x4,y4,z4)、O2(x5,y5,z5)八组解；
g.八组解中有四组虚数解、四组实数解，因三维坐标值都是实数，去除四组虚数解，又因为发射装置工作在Z轴的正半轴空间，再去除两组Z值负半轴的解，剩下两组解，根据发射装置是上电状态和非上电(正常工作状态)状态，分别采用上电校准算法和卡尔曼滤波算法确定唯一解，具体步骤包括：
(1)上电校准时的计算方法：发射装置处于上电状态开始工作时，控制发射装置在XZ平面内、且发射装置中两发射模块的对称轴同Z轴同方向，即两发射模块O1(x4,y4,z4)、O2(x5,y5,z5)的x、y的坐标近似相同，即满足
Figure GDA0003271328130000032
(2)非上电状态时采用卡尔曼滤波算法：利用运动坐标、运动轨迹和上一时刻的坐标值结合已解算出的两组解确定下一时刻的唯一发射模块O1(x4,y4,z4)、O2(x5,y5,z5)的三维坐标值；
解算发射装置的姿态角θ、φ、
Figure GDA0003271328130000033
具体步骤包括：
1)根据上述求出的O1(x4,y4,z4)、O2(x5,y5,z5)的三维坐标值，可以获得两个发射模块带有方向的距离值L1，且规定
Figure GDA0003271328130000034
为正方向；
2)两发射模块对称放置在发射装置中，与发射装置的方向垂直，由此可知发射装置的方向与两个发射模块的距离方向垂直；
3)分别可以解算出
Figure GDA0003271328130000035
在XY，YZ，XZ面上的投影距离，分别为L11、L12、L13，表达式：
Figure GDA0003271328130000036
Figure GDA0003271328130000037
Figure GDA0003271328130000038
4)可以解算出发射装置的姿态角θ、φ、
Figure GDA0003271328130000041
表达式为：
Figure GDA0003271328130000042
本发明将首先接收到超声的接收模块称为目标模块，剩余两个接收模块称为目标节点，将目标模块接收到超声波的时刻作为目标节点的时间基准，且两个发射模块的时间基准不同，测量出不同发射模块发射超声波时，目标节点与目标模块接收到超声波时刻的时间差值，从而解算出不同发射模块发射超声波时，发射模块到目标节点和目标模块的距离差值，通过计算得到发射模块的坐标值，实现对发射模块的定位，根据获得的两发射模块的坐标值，可提供发射装置的距离方向，及其在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.根据下述可列出几何方程：
Figure GDA0003271328130000061
解算出关于O1(x4,y4,z4)、O2(x5,y5,z5)八组解；
g.八组解中有四组虚数解、四组实数解，因三维坐标值都是实数，去除四组虚数解，又因为发射装置工作在Z轴的正半轴空间，再去除两组Z值负半轴的解，剩下两组解，根据发射装置是上电状态和非上电(正常工作状态)状态，分别采用上电校准算法和卡尔曼滤波算法确定唯一解，具体步骤包括：
(1)上电校准时的计算方法：发射装置处于上电状态开始工作时，控制发射装置在XZ平面内、且发射装置中两发射模块的对称轴同Z轴同方向，即两发射模块O1(x4,y4,z4)、O2(x5,y5,z5)的x、y的坐标近似相同，即满足
Figure GDA0003271328130000062
(2)非上电状态时采用卡尔曼滤波算法：利用运动坐标、运动轨迹和上一时刻的坐标值结合已解算出的两组解确定下一时刻的唯一发射模块O1(x4,y4,z4)、O2(x5,y5,z5)的三维坐标值；
解算发射装置的姿态角θ、φ、
Figure GDA0003271328130000063
具体步骤包括：
1)根据上述求出的O1(x4,y4,z4)、O2(x5,y5,z5)的三维坐标值，可以获得两个发射模块带有方向的距离值L1，且规定
Figure GDA0003271328130000064
为正方向；
2)两发射模块对称放置在发射装置中，与发射装置的方向垂直，由此可知发射装置的方向与两个发射模块的距离方向垂直；
3)分别可以解算出
Figure GDA0003271328130000065
在XY，YZ，XZ面上的投影距离，分别为L11、L12、L13，表达式：
Figure GDA0003271328130000066
Figure GDA0003271328130000067
Figure GDA0003271328130000068
4)可以解算出发射装置的姿态角θ、φ、
Figure GDA0003271328130000069
表达式为：
Figure GDA0003271328130000071
为了使本技术领域的人员更好的理解本发明，下面结合附图和实施方式对本发明作进一步的详细说明。
1、图1和图2为本发明应用的超声定位系统组成示意图，包括接收装置[100]和发射装置[200]两部分，接收装置[100]主要由三个接收模块[101]，第二微处理器[104]，第二供电模块[102]，第二时钟模块[103]组成。发射装置[200]主要包括第一发射模块[201]，第二发射模块[202]，第一微处理器[203]，第一供电模块[204]，第一通信模块[205]，第一时钟模块[206]，开关[207]组成，两个发射模块间的距离为L1[208]。系统整体组成图可参考[300]，接收装置[100]，发射装置[200]，发射装置[200]的两个发射模块[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]为目标节点，将目标模块[101]接收到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]，将目标模块[101]接收到O1(x4,y4,z4)[201]、O2(x5,y5,z5)[202]两个发射模块发射的超声波的时刻作为目标节点的时间基准，且两个发射模块的时间基准不同，分别为[605]、[606]，测量出不同发射模块发射超声波时，目标节点与目标模块接收到超声波时刻的时间差值△t1、△t2、△t3、△t4[704]，计算得出不同发射模块发射超声波时，发射模块到目标节点和目标模块的距离差值[705]，分别为L3[601]、L4[602]、L5[603]、L6[604]，根据上述可列出几何方程：
Figure GDA0003271328130000081
可以结算出关于O1(x4,y4,z4)、O2(x5,y5,z5)八组解[706]，八组解中去除四组虚数解和两组Z轴的负半轴的解，剩下两组解[707]，判断发射装置是否为上电状态[708]，如果是上电状态，采用上电状态算法[7010],确定唯一解[7011]，如果是正常工作状态，采用卡尔曼滤波算法[709]，得到唯一解[7011]，计算两发射模块的距离及在XY、YZ、XZ面上的投影值[7012]，计算出姿态角[7013]。
8、图8为发射装置姿态角示意图[800]，发射模块O1(x4,y4,z4)[201]、O2(x5,y5,z5)[202]的三维坐标值，可以获得两个发射模块带有方向的距离值L1，且规定
Figure GDA0003271328130000082
为正方向，两发射模块对称放置在发射装置中，与发射装置的方向垂直，由此可知发射装置的方向与两个发射模块的距离方向垂直；分别可以解算出
Figure GDA0003271328130000091
在XY，YZ，XZ面上的投影距离，分别为L11、L12、L13，表达式：
Figure GDA0003271328130000092
Figure GDA0003271328130000093
Figure GDA0003271328130000094
可以解算出发射装置的姿态角θ[801]、φ[802]、
Figure GDA0003271328130000095
表达式为：
Figure GDA0003271328130000096
下面示意性地给出本发明的一个具体实施例。
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级的，利用几何方程：
Figure GDA0003271328130000101
可以计算出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的坐标近似相同，即满足
Figure GDA0003271328130000111
(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保证先发射的发射模块O2(x5,y5,z5)发射的超声波被最远距离的接收模块接收后，发射模块O1(x4,y4,z4)才能被最近距离的超声接收模块接收到。两发射模块到接收模块的距离差为：△L＝(O2T2-O1T1)，如图所示当发射模块和接收模块在共线的状态下，距离差最大[10011]、[10012]，所需的传播时间也就最长
Figure GDA0003271328130000112
加上安全距离所需的时长，保证O2发射的超声波被接收模块全部接收时，最近模块O1才可被距离最近的接收模块接收到[10012]；图10B两发射模块以间隔时隙t[607]发射超声波示意图[1002]，发射模块[202]、[201]分别向最远接收模块[101]发射超声波，最远距离时超声波的传播时间为t'，为了保证O2发射的超声波被接收模块全部接收时，最近模块O1才可被距离最近的接收模块接收到，所以加上一定的安全距离，在本次实施案例中，最远距离为两发射模块距离加上接收模块之间的距离，总长为15cm，所以传播所需时间为0.44ms，加上安全距离所需的时长，本案例中给定的时间间隙为1ms。
上文的描述针对特定的实施例，在实际的实现过程中，可以对描述的实施例进行其他的变形和修改，以实现更多的按键操作或更方便的控制方式。由此，这些描述应该被理解为仅对各种原理进行说明，而非对本发明进行限制。

Claims (2)

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.根据下述可列出几何方程：
Figure FDA0003276227240000011
解算出关于O1(x4,y4,z4)、O2(x5,y5,z5)八组解；
g.八组解中有四组虚数解、四组实数解，因三维坐标值都是实数，去除四组虚数解，又因为发射装置工作在Z轴的正半轴空间，再去除两组Z值负半轴的解，剩下两组解，根据发射装置是上电状态和非上电状态，分别采用上电校准算法和卡尔曼滤波算法确定唯一解，具体步骤包括：
(1)上电校准时的计算方法：发射装置处于上电状态开始工作时，控制发射装置在XZ平面内、且发射装置中两发射模块的对称轴同Z轴同方向，即两发射模块O1(x4,y4,z4)、O2(x5,y5,z5)的x、y的坐标近似相同，即满足
Figure FDA0003276227240000021
(2)非上电状态时采用卡尔曼滤波算法：利用运动坐标、运动轨迹和上一时刻的坐标值结合已解算出的两组解确定下一时刻的唯一发射模块O1(x4,y4,z4)、O2(x5,y5,z5)的三维坐标值；
解算发射装置的姿态角θ、φ、
Figure FDA0003276227240000022
具体步骤包括：
1)根据上述求出的O1(x4,y4,z4)、O2(x5,y5,z5)的三维坐标值，可以获得两个发射模块带有方向的距离值L1，且规定
Figure FDA0003276227240000023
为正方向；
2)两发射模块对称放置在发射装置中，与发射装置的方向垂直，由此可知发射装置的方向与两个发射模块的距离方向垂直；
3)分别可以解算出
Figure FDA0003276227240000024
在XY，YZ，XZ面上的投影距离，分别为L11、L12、L13，表达式：
Figure FDA0003276227240000025
Figure FDA0003276227240000026
Figure FDA0003276227240000027
4)可以解算出发射装置的姿态角θ、φ、
Figure FDA0003276227240000028
表达式为：
Figure FDA0003276227240000029
2.根据权利要求1所述的实现六自由度测量的超声三维定位系统的实现六自由度测量的超声三维定位方法，其特征在于：所述第一发射模块或第二发射模块，两个发射模块以间隔时隙t发射超声波，使接收模块能明确当前接收的数据来自哪个发射模块，且间隔时隙t保证先发射的发射模块O1(x4,y4,z4)发射的超声波被最远距离的接收模块接收后，发射模块O2(x5,y5,z5)才能被最近距离的超声接收模块接收到。
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 CN108387871A (zh) 2018-08-10
CN108387871B true 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)

Families Citing this family (3)

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

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
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
CN104062633A (zh) * 2014-07-11 2014-09-24 西安电子科技大学 一种基于超声波的室内定位系统及方法
CN105093181A (zh) * 2015-09-25 2015-11-25 吴云松 超声波三维定位系统及其定位方法
CN107271962A (zh) * 2017-07-31 2017-10-20 成都英萨传感技术研究有限公司 基于超声波的室内定位系统及其定位方法

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3107651B2 (ja) * 1992-07-29 2000-11-13 積水化学工業株式会社 移動体の姿勢及び３次元位置測定装置
JP2004340883A (ja) * 2003-05-19 2004-12-02 Hideo Kishimoto 超音波による水中の三次元座標位置計測方法と装置
WO2006119764A1 (en) * 2005-05-13 2006-11-16 Brüel & Kjær Sound & Vibration Measurement A/S Determination of the position of an object
CN100570397C (zh) * 2005-07-26 2009-12-16 清华大学 空间位置检测系统及其检测方法
CN101471734B (zh) * 2007-12-28 2012-12-12 中国科学院声学研究所 一种多发多收声定位网络系统及其定位方法
US8370085B2 (en) * 2009-08-06 2013-02-05 Honda Motor Co., Ltd. Device, method, and program for detecting position
JP2013186087A (ja) * 2012-03-09 2013-09-19 Toshiba Corp 超音波探傷システム及び超音波探傷装置
US9201522B2 (en) * 2013-09-03 2015-12-01 Qualcomm Incorporated Acoustic position tracking system
CN103455171B (zh) * 2013-09-09 2017-04-05 吉林大学 一种三维交互电子白板系统及方法
CN103487787A (zh) * 2013-09-26 2014-01-01 上海大学 一种四旋翼无人机的超声波定位装置及方法
JP2016045119A (ja) * 2014-08-25 2016-04-04 古河機械金属株式会社 ３次元位置測定システム及び３次元位置測定方法
CN205157772U (zh) * 2015-09-25 2016-04-13 吴云松 超声波三维定位系统
CN105929362B (zh) * 2016-04-17 2018-09-21 吉林大学 一种多目标六自由度电磁定位装置和方法
CN106441304B (zh) * 2016-10-08 2019-05-21 东南大学 一种利用可见光通信的三维室内定位方法
CN106933390B (zh) * 2017-03-08 2019-12-31 吉林大学 惯性技术和超声波相结合的触控笔位姿检测方法及系统
CN107121667B (zh) * 2017-04-18 2018-07-13 广州铁路职业技术学院 运动目标室内定位方法及系统
CN107102749B (zh) * 2017-04-23 2019-11-08 吉林大学 一种基于超声波和惯性传感器的三维笔式定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
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
CN104062633A (zh) * 2014-07-11 2014-09-24 西安电子科技大学 一种基于超声波的室内定位系统及方法
CN105093181A (zh) * 2015-09-25 2015-11-25 吴云松 超声波三维定位系统及其定位方法
CN107271962A (zh) * 2017-07-31 2017-10-20 成都英萨传感技术研究有限公司 基于超声波的室内定位系统及其定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于超声波与红外的AGV定位方法;燕学智 等;《吉林大学学报（信息科技辑）》;20071130;第25卷(第6期);第577-581页 *
空间目标姿态角测量计算方法研究;吕日好 等;《仪器仪表学报》;20061230;第27卷(第6期);第1211-1212页 *

Also Published As

Publication number Publication date
CN108387871A (zh) 2018-08-10

Similar Documents

Publication Publication Date Title
CN108387871B (zh) 一种实现六自由度测量的超声三维定位系统和定位方法
CN107817469B (zh) 基于非视距环境下超宽频测距实现室内定位方法
Shi et al. Anchor self-localization algorithm based on UWB ranging and inertial measurements
CN106646355B (zh) 一种信号发送装置以及三维空间定位系统
CN106405500B (zh) 智能机器人跟随方法、跟随系统及用于跟随的发射装置
WO2017020641A1 (zh) 基于光电扫描的室内移动机器人位姿测量系统及测量方法
CN104062633B (zh) 一种基于超声波的室内定位系统及方法
CN110855343B (zh) 一种水声定位与授时浮标及其工作方法
CN102662159B (zh) 一种反射式室内定位的方法及系统
CN110026993B (zh) 一种基于uwb及热释电红外传感器的人体跟随机器人
CN105182319A (zh) 一种基于射频和双目视觉的目标定位系统及方法
CN108520640B (zh) 基于超宽带的无人机导航方法、导航设备、无人机
CN101504275A (zh) 一种基于空间无线定位的手持式线激光三维测量系统
CN102752845A (zh) 基于无线传感器网络的超声波立体定位系统时间同步机制
CN205157772U (zh) 超声波三维定位系统
CN109751992B (zh) 面向室内三维空间的定位校正方法、定位方法及其设备
CN111398903A (zh) 船载短基线定位系统及方法
KR20140126790A (ko) 무선 센서 네트워크 기반의 위치 추정방법
WO2017049940A1 (zh) 室内空间测量定位网络动态坐标测量多站数据同步方法
CN106338290B (zh) 一种机器人自主充电的导航系统及方法
CN207215126U (zh) 一种基于机器视觉的激光光斑定位仪
CN110376550B (zh) 一种基于位置补偿的三维空间定位方法及系统
CN107490782A (zh) 一种利用超声波和电磁波进行定位的定位方法及定位系统
CN106054164B (zh) 空间相对位移变化监测系统及方法
CN111142144A (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