一种超声波主动式目标定位方法与装置

1.本发明涉及超声波定位技术，尤其涉及一种超声波主动式目标定位方法与装置。


背景技术：

2.随着计算机科学、传感器技术以及人工智能的发展，从制造业、交通运输业到对外太空以及深海的探索研究，机器人技术都起到了至关重要的作用。目标定位技术作为机器人技术中的关键技术之一，越来越受到重视。
3.目前，视觉传感器、激光传感器以及超声波传感器是机器人目标定位技术中采用的几种主流传感器。视觉传感器获得信息丰富，然而在弱光或黑暗条件下难以正常工作。激光传感器精度较高，但其存在安全风险、成本较高且无法实现对透明目标的测量。与其他传感器相比，超声波传感器具有成本低、功耗低、体积小和重量轻等优点，且适用范围很广，可适用于透明物体，并能适用于低能见度或黑暗环境，因此超声波传感器在机器人目标定位领域的研究和应用相当普及。
4.然而在实际应用中超声波定位技术仍然存在一些问题需要解决。目前，多数超声波定位方法是采用较多数量的超声波传感器获得多个收到超声回波的到达时间或时间差，并通过相对复杂的数值算法利用获得的到达时间求解目标位置。这些方法存在计算量较大、传感器数量较多的问题，并且多数情况下需要先验知识和人工干预。
5.寻求一种使用相对较少的超声波传感器且计算成本较低的目标定位方法是研究者们一直以来的目标。本发明旨在通过优化超声波传感器阵列构型，采用最少的超声波传感器和最简单的阵列结构，以理论上计算量最小且最稳定的解析方法来实现目标定位。本发明提出了一种超声波主动式目标定位方法与装置，该方法涉及一种超声波传感器阵列构型和目标位置解算与距离测量，其使用最少的超声波传感器，能保证三维目标位置的求解存在唯一的解析解，并具有传感器数量少、理论上计算量最小且解的稳定性最好、传感器构型结构简单、定位准确性高等优点。


技术实现要素：

6.本发明针对基于多超声波传感器的目标定位技术的现状，提供了一种超声波主动式目标定位方法与装置，该定位装置中的超声波传感器阵列构型能够实现三维空间中的目标定位。本发明采用四个超声波传感器组成超声波传感器阵列，该阵列采用一发多收式信号收发模式，其中一个超声波传感器作为基准超声波传感器，工作在t/r双模式，即既作为超声波发射器，又作为超声波接收器，其余三个超声波传感器工作在r模式，即只作为接收器。通过基准超声波传感器发射超声波的时间以及传感器收到超声回波的达到时间获得时间差，求解得到目标位置。该超声波传感器阵列构型和方法在目标定位方面具有构型结构简单、理论上计算量最小且解的稳定性最好、准确性高等优势。
7.本发明的技术方案如下：
8.本发明首先提供了一种超声波主动式目标定位的装置，其包括超声波传感器阵
列、控制模块以及计算机；
9.所述超声波传感器阵列包括四个不在同一个平面内的超声波传感器；且其中一个超声波传感器作为基准超声波传感器，其工作在发射/接收双模式下，即既作为超声波发射器，又作为超声波接收器；
10.控制模块操控各超声波传感器发射和接收超声波信号，每个传感器将包含到达时间信息的信号传送至控制模块，控制模块获得到达时间后根据基准超声波传感器发射超声波的时间以及各超声波传感器接收超声回波的时间计算相应的时间差，并通过usb数据线与计算机进行通信，将该时间差发送给计算机，计算机利用该时间差求解目标物体的三维坐标以及目标到基准超声波传感器的距离，并显示和保存定位及测距的结果。
11.作为本发明的优选方案，以基准超声波传感器所在位置为坐标原点，另外三个超声波传感器分别在三个坐标轴上，并且另外三个超声波传感器到基准超声波传感器的距离相等。
12.作为本发明的优选方案，其余三个超声波传感器工作在接收模式下，即只作为超声波接收器。
13.本发明还提供了一种基于上述装置的超声波主动式目标定位方法，其包括如下步骤：
14.步骤一：采用所述超声波传感器阵列中的基准超声波传感器主动发射超声波，超声波传播至目标物体处发生反射，四个超声波传感器分别接收反射回来的超声波，即超声回波，获得到达时间；
15.步骤二：根据基准超声波传感器发射超声波的时间t0以及四个超声波传感器收到超声回波的到达时间t1、t2、t3和t4，计算相应的时间差δt
10
、δt
21
、δt
31
和δt
41
；δt
10
＝t
1-t0，δt
i1
＝t
i-t1，i＝2，3，4；
16.步骤三：计算获得目标坐标(x,y,z)的解析解；
17.步骤四：计算获得目标到基准超声波传感器的距离ρ的解析解。
18.本发明与现有技术相比具有的有益效果：
19.1)本发明利用解析方法求解目标坐标，采用了一种新的超声波传感器阵列构型，该构型满足目标位置三维空间坐标的解存在且唯一的条件，克服了现有超声波定位技术中计算代价较高、传感器数量较多、传感器阵列相对复杂、通常需要先验知识和人工干预的问题，从而具有需要的超声波传感器数量少、构型结构简单、并且不需要先验知识和人工干预的优点。
20.2)本发明通过将超声波传感器布置在坐标轴上并从条件数的角度对传感器坐标进行优化，有效简化了解析解、减小了计算量、提高了解的稳定性，使得目标坐标求解的计算复杂度低、理论上计算量最小且具有最好的数值稳定性、定位精度高。
21.3)本发明装置安装方便、体积小、功耗低、价格低，可应用范围广。
附图说明
22.图1是一种超声波主动式目标定位装置的结构示意图；
23.图2是四个超声波传感器在同一平面内时的示意图；
24.图3是四个超声波传感器不在同一平面内时的示意图；
25.图4是图3所示超声波传感器阵列空间构型的一个进一步优化构型；
26.图5是图4所示超声波传感器阵列空间构型的一个进一步优化构型。
具体实施方式
27.本发明在基于超声波信号到达时间的目标定位问题的基础上，提出了一种新的超声波传感器阵列构型，以及针对该构型的目标定位方法。该方法较现有方法具有超声波传感器数量少，构型简单，理论上计算量最小且具有最好的数值稳定性，定位精度高的特点。
28.如图1所示，本发明提供了一种基于超声波传感器的目标定位的装置，包括超声波传感器阵列、控制模块以及计算机。四个超声波传感器组成阵列，其构型满足目标位置解算线性方程组的系数矩阵非奇异。本发明提供了的目标定位超声波传感器阵列的构型，其包含四个超声波传感器，这四个超声波传感器不在同一个平面内。取四个超声波传感器中的任意一个作为基准超声波传感器，其坐标定为(0,0,0)；基准超声波传感器工作在t/r双模式，即既发射超声波也接收超声波；其余的三个超声波传感器至少能够工作在r模式，即其需要能够作为接收器。在优选实施例中，其余三个超声波传感器仅工作在r模式，即只接收超声波。四个超声波传感器通过导线与控制模块相连，实现超声波传感器的供电、控制以及到达时间信息的传输。控制模块获得到达时间后根据基准超声波传感器发射超声波的时间以及各超声波传感器接收超声回波的时间计算相应的时间差，并通过usb数据线与计算机进行通信，将该时间差发送给计算机。计算机利用该时间差求解目标物体的三维坐标以及目标到基准超声波传感器的距离，并显示和保存定位及测距的结果。
29.本发明的超声波传感器阵列构型的确定过程如下：
30.根据目标和n个超声波传感器建立三维目标定位的基本模型。定义目标p的坐标为(x,y,z)，第i个超声波传感器si的坐标为(xi,yi,zi)(i＝1，2，
…
，n)。出于对定位实时性的考虑，本发明采用一发多收式超声波信号收发模式。令超声波传感器s1为基准超声波传感器(坐标为(0,0,0))，其可以为n个超声波传感器中的任意一个；基准超声波传感器工作在t/r双模式，即既发射超声波也接收超声波；其余的超声波传感器si(i＝2，3，
…
，n)工作在r模式，即只接收超声波。
31.令t0为基准超声波传感器s1发射超声波的时刻，ti为第i个超声波传感器si接收到超声回波的时刻(i＝1，2，
…
，n)，即到达时间。因为本发明是一个主动式的测量过程，t0和ti是已知量。
32.对于基准超声波传感器s1，它接收到的是自己发射的超声波的回波，因此它与目标坐标(x,y,z)之间的关系可以表示为以下方程。
[0033][0034]
其中，c为声速，δt
10
＝t
1-t0。
[0035]
对于其余的只接收超声波的超声波传感器si(i＝2，3，
…
，n)，超声波从s1传播到目标物体，经过目标物体反射，超声回波被超声波传感器si接收。因此，si与目标坐标之间的关系可以表示为以下方程。
[0036]
[0037]
其中，δt
i1
＝t
i-t1，i＝2，3，
…
，n。
[0038]
公式(1)和(2)中的声速由下式计算得到。
[0039][0040]
其中，t为环境的摄氏温度，由温度传感器获得，获得的声速单位为m/s。
[0041]
由此，目标定位问题可以归结为以下方程。
[0042][0043]
直接求解方程(4)所描述的非线性方程组的计算代价是相对较高的。因此，通常将方程(4)转化成关于目标p的位置坐标(x,y,z)的线性方程组再进行求解。
[0044]
首先，将公式(4)两边取平方并展开，得到：
[0045][0046]
然后，将公式(5)中的第2至n个方程依次减去第一个方程(即基准超声波传感器确定的方程)，通过整理可得：
[0047][0048]
将公式(6)转换为关于未知数p＝[x,y,z]
t
的线性方程组的形式：
[0049]
gp＝h
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0050]
其中，
[0051]
[0052][0053]
超声波传感器的坐标(xi,yi,zi)和声速c是已知的。δt
10
和δt
i1
可以由t0和ti获得。因此矩阵g和向量h是确定的。通过求解方程(7)就可以实现目标定位。
[0054]
根据线性代数可知，如果方程(7)有唯一解，则矩阵g应当是一个3
×
3的方阵并且非奇异。这就意味着对于本发明中的一发多收式超声波传感器阵列，需要四个超声波传感器，即n＝4。那么公式(7)就可以简化为：
[0055][0056]
当满足方程中g非奇异，此时可以得到目标p的坐标(x,y,z)的解析表达式，即目标坐标的解析解为：
[0057][0058]
相应的，目标到基准超声波传感器的距离ρ可以通过下式计算。
[0059][0060]
根据上文所述，公式(10)中三维空间目标坐标(x,y,z)有唯一解析解的条件为矩阵g非奇异，所述目标定位超声波传感器阵列的构型中四个超声波传感器坐标被配置为满足该条件，其中矩阵g的表达式为：
[0061][0062]
矩阵g非奇异的充要条件是超声波传感器阵列中的四个超声波传感器不在同一个平面内。具体的证明如下：
[0063]
定义超声波传感器s2、s3和s4的位置向量分别为s2＝[x2,y2,z2]，s3＝[x3,y3,z3]和s4＝[x4,y4,z4]。根据公式(13)，可以看出向量s2，s3和s4就是矩阵g的三个行向量。
[0064]
四个超声波传感器s1、s2、s3和s4在空间中的位置关系可以分为两种，一种是四个超声波传感器在同一个平面内，另一种是四个超声波传感器不在同一个平面内。
[0065]
先考虑这四个超声波传感器在同一个平面内的情况。不失一般性地，如图2所示，假设它们都在xoy平面内，即z2＝z3＝z4＝0。此时矩阵g可以表示为
[0066][0067]
g的行列式det(g)＝0，即g是奇异的。
[0068]
再考虑这四个超声波传感器不在同一个平面内的情况。不失一般性地，如图3所示，假设s1、s2和s3在xoy平面内而s4在该平面外，即z2＝z3＝0且z4≠0。此时g可以表示为
[0069][0070]
g的行列式为
[0071]
det(g)＝z4(x2y
3-x3y2)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0072]
由于z4≠0，det(g)＝0的充要条件是x2y
3-x3y2＝0。但是x2y
3-x3y2＝0意味着s1、s2和s3共线，s1、s2、s3和s4在同一平面内。然而这和四个超声波传感器不在同一个平面内的前提矛盾。因此det(g)≠0，即g是非奇异的。
[0073]
因此，矩阵g非奇异的充要条件是超声波传感器阵列中的四个超声波传感器不在同一个平面内。证明完毕。
[0074]
事实上，根据线性代数，从向量的角度我们也可以获得相同的结论。如果这四个超声波传感器在同一个平面内，向量s2，s3和s4线性相关，即矩阵g的三个行向量线性相关。因此det(g)＝0，即g是奇异的。如果这四个超声波传感器不在同一个平面内，向量s2，s3和s4线性无关，即矩阵g的三个行向量线性无关。因此det(g)≠0，即g是非奇异的。所以g是非奇异的当且仅当这四个超声波传感器不在同一个平面内。
[0075]
基于上述讨论，当且仅当这四个超声波传感器不在同一个平面内时，目标坐标(x,y,z)可以由公式(11)获得唯一的解析解。
[0076]
本发明通过求解公式(10)来实现定位。由于解析解的计算量和稳定性是重要的，本发明通过合理选择四个超声波传感器的坐标，确保目标坐标的解析解形式简单、计算量小并且稳定性好。为了简化解析解的表达式、减小计算量，通过将s2、s3和s4分别设置在三个坐标轴上，将矩阵g设计为对角阵。如图4所示，令超声波传感器s2的坐标为(x2,0,0)，超声波传感器s3的坐标为(0,y3,0)，超声波传感器s4的坐标为(0,0,z4)。该构型下的矩阵g非奇异，且形式为：
[0077][0078]
进一步的，为了提高求解目标坐标(x,y,z)的数值稳定性，可以从矩阵g的条件数cond(g)的角度优化这四个超声波传感器的坐标。
[0079]
本方法通过求解以g为系数矩阵的线性方程组实现定位。根据线性代数可知cond(g)是解的稳定性的重要指标。cond(g)大意味着测量误差可能对解产生显著的影响，解的波动就可能很大。cond(g)小意味着测量误差对解的影响就小，解就稳定。根据线性代数，不论在何种范数意义下，矩阵g的条件数cond(g)的最小值是1，当cond(g)＝1时，解的稳定性达到最好。
[0080]
基于以上两点考虑，本发明提出了一个实际的四元超声波传感器阵列构型，其要求s2、s3和s4分别在三个坐标轴上，并且这三个超声波传感器到基准超声波传感器(原点)的距离相等。令x2＝y3＝z4＝a，即超声波传感器s2的坐标为(a,0,0)，超声波传感器s3的坐标为(0,a,0)，超声波传感器s4的坐标为(0,0,a)，a为s2、s3和s4到基准超声波传感器的距离，所
提出的阵列构型的坐标如图5所示。
[0081]
相应的矩阵g可以表示为：
[0082][0083]
此时，g的对角线元素相等，不论在何种范数意义下的矩阵g的条件数为都为1，cond(g)＝1，矩阵g的条件数达到最小值，解的稳定性最好。
[0084]
对应的逆矩阵g-1
为：
[0085][0086]
相应的向量h为：
[0087][0088]
该实际四元超声波传感器阵列构型相应的目标坐标(x,y,z)的解析表达式，即目标坐标的解析解为：
[0089][0090]
相应的，目标到基准超声波传感器的距离ρ的解析解表达式为：
[0091][0092]
本发明方法的实施过程中，首先通过控制模块操控基准超声波传感器发射超声脉冲；超声脉冲向外传输并在遇到目标后发生反射，反射后的超声回波由四个超声波传感器接收；之后这四个传感器将包含到达时间信息的信号传送至控制模块；控制模块获得到达时间后，利用基准超声波传感器发射超声波的时间t0以及四个超声波传感器收到超声回波的到达时间t1、t2、t3和t4，计算相应的时间差δt
10
、δt
21
、δt
31
和δt
41
；控制模块通过usb数据线与计算机进行通信，将δt
10
、δt
21
、δt
31
和δt
41
发送给计算机，计算机根据公式(21)求解目标物体的坐标以及根据公式(22)求解目标到基准超声波传感器的距离，并显示和保存定位及测距的结果。
[0093]
具体的，可按如下步骤进行：
[0094]
步骤一：采用所述超声波传感器阵列中的基准超声波传感器主动发射超声波，超声波传播至目标物体处发生反射，四个超声波传感器分别接收反射回来的超声波，即超声回波，获得到达时间；
[0095]
步骤二：根据基准超声波传感器发射超声波的时间t0以及四个超声波传感器收到超声回波的到达时间t1、t2、t3和t4，计算相应的时间差δt
10
、δt
21
、δt
31
和δt
41
；δt
10
＝t
1-t0，δt
i1
＝t
i-t1，i＝2，3，4；
[0096]
步骤三：根据公式(21)计算获得目标坐标(x,y,z)的解析解；
[0097]
步骤四：根据公式(22)计算获得目标到基准超声波传感器的距离ρ的解析解。
[0098]
为了验证该方法与装置的有效性和定位效果，开展了目标定位实验。
[0099]
根据图5所示的坐标设计了一个实际的超声波传感器阵列，其中a＝8cm。待测目标为20
×
20cm的塑料板。塑料板的中心坐标，作为被测对象的定位坐标。实验的环境温度为25℃。
[0100]
实验表明，在3.0m的测试范围内，该方法与装置能够实现对目标物体的定位和测距，且定位与测距的精度较好。定位坐标x、y、z以及被测对象到基准超声波传感器的距离ρ的绝对误差均在3.0cm以内。
[0101]
本发明中四个超声波传感器构成传感器阵列，该阵列的构型满足目标位置坐标的求解存在唯一解析解的条件。通过本发明的超声波传感器阵列构型和基于超声波信号到达时间的目标位置求解方法，可以实现三维空间中目标物体的定位及测距。该方法较现有方法具有需要的超声波传感器数量少，构型简单，理论上计算量最小且具有最好的数值稳定性，定位精度高的特点。