CN110703196A - 基于超声波传感器三位置测量的环境轮廓重构方法 - Google Patents

Abstract

基于超声波传感器三位置测量的环境轮廓重构方法，涉及检测技术领域，基于超声波传感器三位置测量的环境轮廓重构方法，超声波传感器安装于数据采集装置上，使超声波传感器在设定范围内转动，根据环境特点，规划测量位置和测试次数，以测量位置为圆心，测量距离为半径，测量点为弧中点做弧；本发明根据三位置的测量弧拓扑关系判断被测量特征物是否为直线或圆弧，降低了超声波测量不确定性引起的高误差率，更合理与有效的使用了超声波传感器多次测量数据，使得测量结果的准确性更高。

Description

基于超声波传感器三位置测量的环境轮廓重构方法

技术领域

本发明属于检测技术与自动化系统技术领域,特别涉及基于超声波传感器三位置测量的环境轮廓重构方法。

背景技术

环境地图重构是机器人导航技术的核心，描述了机器人工作的环境，是机器人定位与路径规划的前提。

机器人导航在机器人自主控制过程中，面对未知环境或环境变化的情况，及时进行环境建图，是机器人自主学习的基础。

当前科技技术日益增加新鲜技术，在我们机器人技术领域作为其环境感知器的种类繁多，比如，激光传感器，红外线传感器，超声波传感器，视觉传感器等。下面分别对其介绍：激光传感器作为速度最快数据最准确经常在高端的设备中使用，但是其造价非常高，在一般的实验移动机器人很少使用；红外线传感器是利用其本身的物理性质对物体进行一个测距，由于是不需要接触并且敏感度高以及反应快的诸多优点，在医学和军事方面都有应，在移动机器人中经常被应用到避障系统；超声波传感器由于价格以及其检测准确经常被使用作为测距的首选传感器，但是也不排除其存在着缺点，比如测量存在一定的误差，测量角度存在着不确定的问题；视觉传感器是最近几年才应用起来的，其算法识别过程复杂，开发研制者不多，目前应用范围不广。

超声波传感器参数不确定性来自两部分:位姿和测量，为解决这一问题，在文献《基于高斯分布的机器人超声波环境建模研究》中，在RCD聚类法基础上，提出一种最大化外接圆概率的关联模型。在RCD聚类中增加探测目标分布信息，并判断RCD来自同一特征；然后，最大化外切圆分布概率以提高关联准确性，对特征参数不确定性进行分析，利用最小二乘法进行特征参数估计，最后，实现了特征匹配和融合，有提高关联准确性的优点。

但这种两位置测量方法只是基础级的，缺乏对整体环境的统一构建方法。特别，在特征关联方法中基于对两个位置测量中对点、线、弧三种情况的特征关联，其逻辑关系存在强弱之分，且在不同的环境重构中可能分布不同的特征关联情况。例如：在环境轮廓重构中，将轮廓微小范围化，对于环境轮廓规则的重构中，两位置测量特征关联数据偏多，环境轮廓拟合会出现重复偏差和杂乱的结果，对于复杂的环境，两位置测量方法有效解决特征关联的同时，也会规避环境轮廓特殊特征。这些无疑对环境轮廓的完整性和可信度造成影响。

正因为环境轮廓测量特征关联方法情况繁多，但又必须制定明确、统一的关联标准，且要便于测量、融合、重构，研究人员在进行环境重时，应分析位置测量之间依赖关系和耦合特性，单纯的基于高斯分布两位置测量环境重构基础方法，重构后的环境将无法得到期望的改善。

发明内容

为了解决上述问题，本发明提供了一种基于超声波传感器三位置测量的环境轮廓重构方法，将复杂、不规则的环境轮廓分解为在微小范围内呈直线和曲线的障碍物轮廓，重构成更清晰、更简洁的环境轮廓。

为了实现上述目的，根据本申请的一个方面，提供了一种：基于超声波传感器三位置测量的环境轮廓重构方法，具体包括以下步骤：

采集数据，将超声波传感器设置在数据采集装置上，获取超声波传感器测量位置坐标及数据采集装置前进朝向角度，以测量位置为圆心，测量距离为半径，测量点为弧中点做弧；

同一位置弧融合，筛除误差范围之外的错误信息并融合相邻的相近测量数据；

三个位置弧的相交情况及筛选，其中，所述三位置包括第一位置、第二位置和第三位置，利用测量弧信息及位置关系，筛选出三位置测量弧中两两相交的组合，记录各相交弧信息；

筛选出一种易于环境重构三位置测量弧相交形式；

判断三位置弧所表示的环境轮廓类型。

进一步地，数据采集步骤包括以下具体步骤：

建立测量位置坐标系，并将每个测量位置用坐标表示，根据测量位置关系公式，将超声波传感器测量位置坐(X_i,Y_i)表示出来，公式如下：

x＝len*cos(ψ_i-1+θ_i-1)

y＝len*cos(ψ_i-1+θ_i-1)

X_i＝X_i-1+x

Y_i＝Y_i-1+y

其中，数据采集装置朝向角ψ，数据采集装置朝向角ψ是以出发点为原点的实际极坐标角度，数据采集装置实际位置角度θ，相对于前一点的距离len，x为数据采集装置移动横坐标变化量，y为数据采集装置移动纵坐标变化量；

将数据采集装置朝向角ψ、相对于前一点的距离len、数据采集装置实际位置角度θ，依次存入一个结构体中，超声波测量距离数据整合并重新按照测试位置分组。

进一步地，三个位置弧的相交情况及筛选包括以下具体步骤；

判断第一位置、第二位置测量弧的相关情况，从第一位置的第一个弧开始，与第二位置的所有弧依次判断，相交弧统一编为一个组，比较之后再从第一位置的第二个弧开始，与第二位置的所有弧依次比较，将所有相交的弧编组；

判断第三位置与第一位置和第二位置相交弧的相关情况，并防止有相交弧的信息重复存储的情况；

三相交弧进行筛选，选出三位置对应的三个测量弧均两两相交的组合情况。

进一步地，判断第一位置、第二位置测量弧的相关情况包括以下具体步骤：

判断测量弧是否相交；

将同向相交的圆弧筛选出来；

将交点在圆弧上的相交弧筛选出来；

将所有相交的弧编组。

进一步地，所述方法还包括：验证方法的准确定和精确性的步骤，具体地：

根据三条相交弧的环境轮廓类型，提取环境轮廓特征点，再将环境轮廓描绘出来，验证方法的准确定和精确性。

进一步地，筛选出一种易于环境重构三位置测量弧相交形式步骤，包括建立数据筛选模型，通过分析确定三位置弧所需的相交形式，通过运用距离关系比较，筛选出最终数据，距离关系满足条件如下：

Dist((3),(1,2))＞R₃

Dist((2),(1,3))＜R₂

Dist((1),(2,3))＞R₁

其中Dist表示某一位置圆心与另两位置相交弧交点的距离。

可选地，判断三位置弧所表示的环境轮廓类型，包括使用MATLAB将三位置筛选后的弧组合数据表示的环境轮廓展现出来，完成三位置测量的环境轮廓重构。

为了实现上述目的，根据本申请的一个方面，提供了一种基于超声波传感器三位置测量的环境轮廓重构系统，包括:

数据采集模块，包括设置在数据采集装置上超声波传感器，其用于获取超声波传感器测量位置坐标及数据采集装置前进朝向角度信息；

融合模块，其用于筛除误差范围之外的错误信息并融合相邻的相近测量数据；

第一筛选模块，利用测量弧信息及位置关系，筛选出三位置测量弧中两两相交的组合，记录各相交弧信息；

第二筛选模块，其用于筛选出一种易于环境重构三位置测量弧相交形式；

判断模块，其用于判断三位置弧所表示的环境轮廓类型。

进一步地，所述第一筛选模块包括：

第一判断单元，其用于判断第一位置、第二位置测量弧的相关情况，从第一位置的第一个弧开始，与第二位置的所有弧依次判断，相交弧统一编为一个组，比较之后再从第一位置的第二个弧开始，与第二位置的所有弧依次比较，将所有相交的弧编组；

第二判断单元，其用于判断第三位置与第一位置和第二位置相交弧的相关情况，并防止有相交弧的信息重复存储的情况；

筛选单元，其用于对三相交弧进行筛选，选出三位置对应的三个测量弧均两两相交的组合情况。

进一步地，所述系统还包括：

验证模块，其用于对基于超声波传感器三位置测量的环境轮廓重构方法进行验证，将环境轮廓描绘出来，验证方法的准确定和精确性。

有益效果：

根据三位置的测量弧拓扑关系判断被测量特征物的轮廓类型，降低了超声波测量不确定性引起的高误差率，更合理与有效的使用了超声波传感器多次测量数据，使得测量结果的准确性更高；

在判断三位置测量弧的拓扑关系时，相关参数的阈值是根据超声波传感器误差参数大小设定，参数易于确定和调试，同时系统更容易维护；

超声波传感器性价比高，检测控制方法易于使用，这使得本专利可应用于各种机器人控制系统，具有通用性。

附图说明

为了实现上述以及相关目的，本文结合下面的描述和附图来描述某些说明性方面，这些方面指示了可以实践本文所公开的原理的各种方式，并且所有方面及其等效方面旨在落入所要求保护的主题的范围内。通过结合附图阅读下面的详细描述，本公开的上述以及其它目的、特征和优势将变得更加明显。遍及本公开，相同的附图标记通常指代相同的部件或元素。本申请示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1基于超声波传感器三位置测量的环境轮廓重构过程示意图；

图2同一位置测量弧融合过程的示意图；

图3同向相交测量弧判断方法的示意图；

图4位置1、2相交弧分组标志位示意图；

图5位置3与位置1、2测量弧相交关系判断过程示意图；

图6位置3与位置1、2相交弧编组规则示意图；

图7三位置测量弧两两相交选择过程示意图；

图8一种用于环境重构的三位置测量弧空间拓扑关系示意图；

图9三位置测量弧轮廓类型判断示意图；

图10三位置相交弧表示弧线的方法流程图；

图11三位置相交弧表示弧线的方法示意图；

图12三位置测量弧组合表示直线型环境轮廓的实验结果；

图13三位置测量弧组合表示弧线型环境轮廓的实验结果。

图14基于超声波传感器三位置测量的环境轮廓重构系统的结构示意图；

图15为图14中第一筛选模块的结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第

二”、“第三”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、系统固有的其它步骤或单元。

现有技术中，缺乏对整体环境的统一构建方法，在特征关联方法中基于对两个位置测量中对点、线、弧三种情况的特征关联，其逻辑关系存在强弱之分，且在不同的环境重构中可能分布不同的特征关联情况，例如：在环境轮廓重构中，将轮廓微小范围化，对于环境轮廓规则的重构中，两位置测量特征关联数据偏多，环境轮廓拟合会出现重复偏差和杂乱的结果，对于复杂的环境，两位置测量方法有效解决特征关联的同时，也会规避环境轮廓特殊特征，这些无疑对环境轮廓的完整性和可信度造成影响。

为了解决这一问题，根据本申请的实施例，提供了一种基于超声波传感器三位置测量的环境轮廓重构方法，如图1所示，具体包括以下步骤：

步骤S101,采集数据，将超声波传感器设置在数据采集装置上，获取超声波传感器测量位置坐标及数据采集装置前进朝向角度，以测量位置为圆心，测量距离为半径，测量点为弧中点做弧；

步骤S102同一位置弧融合，筛除误差范围之外的错误信息并融合相邻的相近测量数据；

步骤S103三个位置弧的相交情况及筛选，利用测量弧信息及位置关系，筛选出三位置测量弧中两两相交的组合，记录各相交弧信息；

步骤S104筛选出一种易于环境重构三位置测量弧相交形式；

步骤S105判断三位置弧所表示的环境轮廓类型。

上述步骤中首先将超声波传感器设置在数据采集装置上，获取超声波传感器测量位置坐标及数据采集装置前进朝向角度，以测量位置为圆心，测量距离为半径，测量点为弧中点做弧；然后根据筛除误差范围之外的错误信息，并融合相邻的相近测量数据，根据三位置的测量弧拓扑关系判断被测量特征物是的轮廓类型，降低了超声波测量不确定性引起的高误差率，更合理与有效的使用了超声波传感器多次测量数据，使得测量结果的准确性更高，进而实现环境轮廓的重构，将复杂、不规则的环境轮廓按照直线和曲线进行呈现，使环境轮廓重更清晰、简洁。

需要说明的是所述数据采集装置为能够完成行走功能的功能型装置，例如数据采集车，步进电机等。

所述方法还包括：验证方法的准确定和精确性的步骤，具体地：

根据三条相交弧的环境轮廓类型，提取环境轮廓特征点，再将环境轮廓描绘出来，验证方法的准确定和精确性。

其中，步骤S101包括以下具体步骤：

(a)建立测量位置坐标系，并将每个测量位置用坐标表示；

已知测试位置的关系，数据采集装置朝向角ψ，数据采集装置朝向角ψ是以出发点为原点的实际极坐标角度，数据采集装置实际位置角度θ，相对于前一点的距离len，x为数据采集装置移动横坐标变化量，y为数据采集装置移动纵坐标变化量，根据测量位置关系公式，将超声波传感器测量位置坐(X_i,Y_i)表示出来，公式如下：

x＝len*cos(ψ_i-1+θ_i-1)

y＝len*cos(ψ_i-1+θ_i-1)

X_i＝X_i-1+x

Y_i＝Y_i-1+y

(b)将数据采集装置朝向角ψ、相对于前一点的距离len、数据采集装置实际位置角度θ，依次存入一个结构体中，超声波测量距离数据整合并重新按照测试位置分组。

参见附图2，步骤S102中每组测量的数据中，存在表示同一位置的多个测量弧，即相邻测量点的测量数据存在距离相近的情况，设定条件进行相关属性判断，进行数据的相近融合；将每个位置相邻测量值相近的距离合并为一个数据，所述测量数据为小弧，合并数据为大弧，将可以组合的小弧的标志位、融合后大弧个数、大弧所属测量位置标号及融合起始和终止的标志位分别存储起来。

首先判断测量数据是否有效，范围在5mm～500mm之间，然后从每组第一个测量数据开始，与相邻的下一个数据的差进行比较，差值范围为1mm，差值范围内的相邻数据统一标记为一组，差值范围外的数据，重新标记为一组，并从这个数据再往下比较，重复上述过程。若有小弧与前后均不相同，同样单独标记为一组。判断过程要记录大弧的数量，构成大弧的起始小弧标志位，终止小弧标志位。一组数据的处理后存储，数据结构如表1。

表1一个位置弧的组合：合并每组相邻相近测量弧

步骤S103，数据中三个位置弧中存在多种相交情况，对规划的测试点进行分组，每相邻的三个超声波传感器的位置为一组，实验数据需要三个位置上有三个测量弧均两两相交的情况，对这样的数据进行选择。通过角度比较、交点计算等，确定相交弧之间的关系，记录各相交弧信息并筛选出三位置中三个测量弧两两均相交的所有组合；

包括以下具体步骤；为了在图中及说明书文字部分表述清楚申请人把第一位置、第二位置和第三位置在下文中表示为位置1、位置2、位置3.

(c)判断位置1、位置2测量弧的相关情况，从位置1的第一个弧开始，与位置2的所有弧依次判断，相交弧统一编为一个组，比较之后再从位置1的第二个弧开始，与位置2的所有弧依次比较，将所有相交的弧编组；

(d)判断位置3与位置1、位置2相交弧的相关情况，并防止有相交弧的信息重复存储的情况，首先将位置3与位置1的测量弧进行比较，之后位置3与位置2的测量弧进行比较，最后将位置1、位置2相交、位置1、位置3相交、位置2、位置3相交弧的组合的相关信息进行存放；

(e)三相交弧进行筛选，选出三位置对应的三个测量弧均两两相交的组合情况。

其中，步骤(c)包括以下具体步骤：

①测量弧是否相交判断：将超声波传感器位置和测量距离做一个圆，首先判断位置1、2上测量圆弧对应圆的空间关系，设位置1的圆心坐标为(x₁,y₁),半径为R₁；位置2的圆心坐标为(x₂,y₂)圆半径为R₂，圆心距用D表示，则：

若R₁+R₂＜D,则两位置圆相交；

②同向相交圆弧筛选：参考附图3，已知两个位置的圆弧1、圆弧2起始角和终止角即端点A₁、B₁、A₂、B₂的角度，根据几何关系，先求出中间角，所述中间角为一个弧的一个端点与另一个弧的圆心相连的角度，判断中间角是否在另一个弧起始角与终止角之间，端点角度均判断完毕后，将同向相交的圆弧筛选出来；

③交点在测量弧上的相交弧筛选：利用交点坐标，计算交点在两个圆弧的角度，判断交点角度是否在两弧圆弧的起始角与终止角之间，将交点在圆弧上的相交弧筛选出来；

④将所有相交的弧编组：参考附图4，用flags1和flags2分别表示位置1、2测量弧的编组，r1,r2代表融合后大弧的数量，从1位置的第一个弧开始，与第二个位置的所有弧依次比较，相交的弧统一编为一个编号，即为一组；比较之后再从第一个位置的第二个弧开始，与第二个位置的所有弧依次比较，直至将所有相交的弧编为几组相交的弧。

相交弧的分组编号方法具体解释如下，在附图4中，flags列表中，没有与之相交的测量弧，其标志位为0，如(1,4)弧；若两相交弧中有任意一个测量弧已与其他测量弧相交(即有一个测量弧已有标志位)，与之相交的弧的标志位与已有标志位的测量弧相同；若两测量弧比较前均未与其他测量弧相交(即两测量弧标志位均为0)，那么两者的标志位在已有最大的标志位数值上递增(加1)，划分为新的一组相交弧。例如：(1,1)与(2,1)(2,2)相交，与其他均不相交，它们标志位为1,(1,2)与(2,2)相交，其中的弧(2,2)标志位已为1，所以(1,2)标志位也为1，同理(1,3)与(2,3)相交，标志位为1，(1,3)与(2,4)相交，但其标志位均为0，即没有与别的弧相交，所以重新编为一组，标志位为2，按上述标志位赋值规则依次处理其余测量弧。最后将标志位一致的组的相交弧的信息放到一个结构中，信息结构如表2，

表2位置1、位置2相交弧信息结构

其中，步骤(d)中判断位置3的第i个弧与位置1第j个弧时，如果弧组编号在之前已经存在于第i个胞元里，说明位置1的第j弧与位置2的相交弧数据已经存储，直接将比较的位置3和位置1的的弧放入数据中即可；如果没有，需要把位置1、位置2的相交弧信息和位置1、位置3的相交弧信息都存进数据中，得到的此数据为位置1、位置2、位置3中两两相交弧的信息。

其中，步骤S104中筛选出一种易于环境重构三位置测量弧相交形式，包括建立数据筛选模型，通过分析确定三位置弧所需的相交形式，通过运用距离关系比较，筛选出最终数据，距离关系满足条件如下：

Dist((3),(1,2))＞R₃

Dist((2),(1,3))＜R₂

Dist((1),(2,3))＞R₁

其中Dist表示某一位置圆心与另两位置相交弧交点的距离。

其中，步骤(e)具体包括如下步骤：

(I)先筛除有两个相交点的相交弧：

(II)筛选三位置三个测量弧组合，要求三个测量弧均两两相交

其中，三位置相交弧表示直线方法为：位置2圆弧到位置1或位置3的弧的切线最近的点，分别与位置1和位置3的弧的切点求直线，并利用最小二乘法拟合出一条新的直线；

三位置相交弧表示弧线的方法为：做出三个位置相交弧的公切圆，即表示三位置所表示的环境轮廓为弧线。

作为优选，所述基于超声波传感器三位置测量的环境轮廓重构方法还包括，使用MATLAB将三位置筛选后的弧组合数据表示的环境轮廓展现出来，实现结果可视化。

步骤(d)判断位置3与位置1、2相交弧的相关情况，并防止有相交弧的信息重复存储的情况。参考附图5，首先位置3与位置1的测量弧进行比较，方法同上文判断1、2位置测量弧的相关情况，之后位置3与位置2的测量弧进行比较，最后将位置1、2相交、位置1、3相交、位置2、3相交弧的组合的相关信息进行存放。

数据存放规则：

判断位置3的第i个弧与位置1第j个弧时，如果弧组编号在之前已经存在于第i个胞元里，说明位置1的第j弧与位置2的相交弧数据已经存储，直接将比较的位置3和位置1的的弧放入数据中即可；如果没有，需要把位置1、2的相交弧信息和位置1、3的相交弧信息都存进数据中。得到的此数据为位置1、2、3中两两相交弧的信息，如表3所示：

表3三相邻位置相交弧结构

参见附图6，flags3和flags12分别表示位置3相交弧编组、位置1、2已相交测量弧的编组，其中flags1(j)来表示flags12的行数(flags12(flags1(j)))，即r12代表位置1、2相交弧的组数；r3代表融合后测量弧(大弧)的数量，从位置3的第一个弧开始，与位置1中标志位非0的弧依次比较(即只与位置1相交弧比较，方法见上文)，相交的弧统一编为一个编号，即为一组；比较之后再从位置3的第二个弧开始依次比较，直至将所有相交的弧编为几组相交的弧，标志位赋值规则同上，flags123为三位置相交弧的编组，CorArcs_CNT代表三位置相交弧分组数量，flags123{CorArcs_CNT}中存放与位置3相交的位置1的弧标志位。CorArcs_CNT代表三位置相交弧分组数量，flags123{CorArcs_CNT}中存放与位置3相交的位置1的弧标志位。

下面举例详细说明标志位赋值的几种情况和规则，参见附图6：

1)假设此时是第一次比较，i值表示位置3的第i个测量弧，j表示位置1第j个测量弧，当i＝1，j＝1时，CorArcs_CNT＝1，flags12(flags1(1))＝flags12(1)，flags1(1)≠0，可以比较，假设两弧相交，此时flags3(1)＝0且flags12(1)＝0，即均未相交，所以赋值flags3(1)＝1，flags12(1)＝1。flags123{CorArcs_CNT}＝flags123{1}，将flags1(1)＝1存入flags123{1}中。

2)当i＝1，j＝3时，假设两弧相交，flags12(flags1(3))＝flags12(2)此时flags3(1)＝1且flags12(2)＝0，即有一个弧存在相交弧，CorArcs_CNT仍为1，所以赋值flags12(2)＝flags3(1)＝1。将flags1(3)＝2存入flags123{1}中，此时flags123{1}中有两个位置1相交弧的标志位。

3)当i＝3，j＝1时，CorArcs_CNT＝1假设两弧相交，flags12(flags1(1))＝flags12(1)此时flags3(3)＝0且flags12(2)＝1，即有一个弧存在相交弧，所以赋值flags3(3)＝flags12(1)＝1。由于flags1(1)＝1已存入flags123{1}中，所以不重复放入。

4)当i＝3，j＝4时，假设两弧相交，且之前的弧均判断完毕，flags12(flags1(4))＝flags12(3)，此时flags3(3)＝0且flags12(3)＝0，即均未相交，编为新的一组CorArcs_CNT＝CorArcs_CNT+1，即CorArcs_CNT＝2，所以赋值flags3(3)＝2，flags12(3)＝2。flags123{CorArcs_CNT}＝flags123{2}，将flags1(4)＝3存入flags123{2}中。

经过上述一系列的与位置1测量弧比较后，接下来与位置2进行比较，CorArcs_CNT值接着计数，位置3与位置2相交弧信息连续存入结构体中，其余操作均同位置3与位置1比较方法，不再赘述。

其中，步骤(e)具体包括如下步骤：

(I)先筛除有两个相交点的相交弧：

(II)筛选三位置三个测量弧组合，要求三个测量弧均两两相交

参考附图7，选出三位置对应的三个测量弧均两两相交的组合情况。

1)先筛除有两个相交点的相交弧。

存放的测量弧信息的结构体中，有Is_Intersection1(是否有第一个交点)和Is_Intersection2(是否有第二个交点)两个参考信息，其值为1代表有交点，其值为0代表无交点。所以，两个参考值均为1时就代表有两个交点，可以将它们筛选掉。

2)筛选三位置三个测量弧组合，要求三个测量弧均两两相交。

上文，我们将相交弧进行了编组，每组两两相交弧中，找出满足关系的三个测量弧。

先从第i组两两相交弧的组合开始，判断第一行位置1，其编号为1，与编号为3的弧相交，寻找下一个在位置1编号为1的弧，发现他与编号为2的弧相交，判断编号2、3弧在本组相交弧中是否存相交关系，如果存在则这三个弧为一组弧，如果不存在判断第二行第一个位置是编号为a的弧与哪个弧b相交，寻找下一个弧a与弧c相交，判断b、c是否存在相交关系，依次往下判断，本组两两相交弧判断完毕后，判断第i+1组。

参考附图8，步骤S104中筛选出一种易于环境重构三位置测量弧相交形式，包括建立数据筛选模型，通过分析确定三位置弧所需的相交形式，通过运用距离关系比较，筛选出最终数据，距离关系满足条件如下：

Dist((3),(1,2))＞R₃

Dist((2),(1,3))＜R₂

Dist((1),(2,3))＞R₁

其中Dist表示某一位置圆心与另两位置相交弧交点的距离。在三位置两两相交弧的数据集合中，寻找满足预定义空间拓扑关系的三条相交弧(分别来源于三个位置)，降低测量数据中噪声的干扰。

参考附图9，步骤S105判断三位置弧所表示的环境轮廓类型。

位置1、位置3的弧的切线到位置2圆心的距离d与位置2的弧的半径R的关系，判断条件为|R-d|＜ε，满足，三位置表示环境轮廓为直线，否则为圆弧。

其中，三位置相交弧表示直线方法为：位置2圆弧到位置1或位置3的弧的切线最近的点，分别与位置1和位置3的弧的切点求直线，并利用最小二乘法拟合出一条新的直线；

参见附图10、11，三位置相交弧表示弧线的方法为：做出三个位置相交弧的公切圆，即表示三位置所表示的环境轮廓为弧线。

参考附图12、13，使用MATLAB将三位置筛选后的弧组合数据表示的环境轮廓展现出来，完成三位置测量的环境轮廓重构。

完成以上步骤，可实现环境轮廓的重构，将复杂、不规则的环境轮廓按照直线和曲线进行呈现，使环境轮廓重更清晰、简洁。

本申请实施例还提供了一种基于超声波传感器三位置测量的环境轮廓重构系统，需要说明的是，本申请实施例的基于超声波传感器三位置测量的环境轮廓重构系统可以用于执行本申请实施例所提供的基于超声波传感器三位置测量的环境轮廓重构方法。以下对本申请实施例提供的深度图像的构建装置进行介绍。

基于超声波传感器三位置测量的环境轮廓重构系统，包括:

数据采集模块10，包括设置在数据采集装置上超声波传感器，其用于获取超声波传感器测量位置坐标及数据采集装置前进朝向角度信息；

融合模块20，其用于筛除误差范围之外的错误信息并融合相邻的相近测量数据；

第一筛选模块30，利用测量弧信息及位置关系，筛选出三位置测量弧中两两相交的组合，记录各相交弧信息；

第二筛选模块40，其用于筛选出一种易于环境重构三位置测量弧相交形式；

判断模块50，其用于判断三位置弧所表示的环境轮廓类型。

上述系统将超声波传感器设置在数据采集装置上，获取超声波传感器测量位置坐标及数据采集装置前进朝向角度，以测量位置为圆心，测量距离为半径，测量点为弧中点做弧；融合模块20根据筛除误差范围之外的错误信息，并融合相邻的相近测量数据，第一筛选模块30，利用测量弧信息及位置关系，筛选出三位置测量弧中两两相交的组合，记录各相交弧信息；第二筛选模块40，其用于筛选出一种易于环境重构三位置测量弧相交形式；最终由判断模块50，判断三位置弧所表示的环境轮廓类型，根据三位置的测量弧拓扑关系判断被测量特征物是的轮廓类型，降低了超声波测量不确定性引起的高误差率，更合理与有效的使用了超声波传感器多次测量数据，使得测量结果的准确性更高，进而实现环境轮廓的重构，将复杂、不规则的环境轮廓按照直线和曲线进行呈现，使环境轮廓重更清晰、简洁。

其中，所述第一筛选模块30包括：

第一判断单元301，其用于判断位置1、位置2测量弧的相关情况，从位置1的第一个弧开始，与位置2的所有弧依次判断，相交弧统一编为一个组，比较之后再从位置1的第二个弧开始，与位置2的所有弧依次比较，将所有相交的弧编组；

第二判断单元302，其用于判断位置3与位置1和位置2相交弧的相关情况，并防止有相交弧的信息重复存储的情况；

筛选单元303，其用于对三相交弧进行筛选，选出三位置对应的三个测量弧均两两相交的组合情况。

所述基于超声波传感器三位置测量的环境轮廓重构系统还包括：

验证模块60，其用于对基于超声波传感器三位置测量的环境轮廓重构方法进行验证，将环境轮廓描绘出来，验证方法的准确定和精确性，根据三条相交弧的环境轮廓类型，提取环境轮廓特征点，再将环境轮廓描绘出来，验证方法的准确定和精确性。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.基于超声波传感器三位置测量的环境轮廓重构方法，其特征在于：具体包括以下步骤：

采集数据，将超声波传感器设置在数据采集装置上，获取超声波传感器测量位置坐标及数据采集装置前进朝向角度，以测量位置为圆心，测量距离为半径，测量点为弧中点做弧；

同一位置弧融合，筛除误差范围之外的错误信息并融合相邻的相近测量数据；

三个位置弧的相交情况及筛选，其中，所述三位置包括第一位置、第二位置和第三位置，利用测量弧信息及位置关系，筛选出三位置测量弧中两两相交的组合，记录各相交弧信息；

筛选出一种易于环境重构三位置测量弧相交形式；

判断三位置弧所表示的环境轮廓类型。

2.根据权利要求1所述的基于超声波传感器三位置测量的环境轮廓重构方法，其特征在于，数据采集步骤包括以下具体步骤：

建立测量位置坐标系，并将每个测量位置用坐标表示，根据测量位置关系公式，将超声波传感器测量位置坐(X_i,Y_i)表示出来，公式如下：

x＝len*cos(ψ_i-1+θ_i-1)

y＝len*cos(ψ_i-1+θ_i-1)

X_i＝X_i-1+x

Y_i＝Y_i-1+y

其中，数据采集装置朝向角ψ，数据采集装置朝向角ψ是以出发点为原点的实际极坐标角度，数据采集装置实际位置角度θ，相对于前一点的距离len，x为数据采集装置移动横坐标变化量，y为数据采集装置移动纵坐标变化量；

将数据采集装置朝向角ψ、相对于前一点的距离len、数据采集装置实际位置角度θ，依次存入一个结构体中，超声波测量距离数据整合并重新按照测试位置分组。

3.根据权利要求1所述的基于超声波传感器三位置测量的环境轮廓重构方法，其特征在于，三个位置弧的相交情况及筛选包括以下具体步骤；

判断第一位置、第二位置测量弧的相关情况，从第一位置的第一个弧开始，与第二位置的所有弧依次判断，相交弧统一编为一个组，比较之后再从第一位置的第二个弧开始，与第二位置的所有弧依次比较，将所有相交的弧编组；

判断第三位置与第一位置和第二位置相交弧的相关情况，并防止有相交弧的信息重复存储的情况；

三相交弧进行筛选，选出三位置对应的三个测量弧均两两相交的组合情况。

4.根据权利要求3所述的基于超声波传感器三位置测量的环境轮廓重构方法，其特征在于，判断第一位置、第二位置测量弧的相关情况，并防止有相交弧的信息重复存储的情况，包括以下具体步骤：

判断测量弧是否相交；

将同向相交的圆弧筛选出来；

将交点在圆弧上的相交弧筛选出来；

将所有相交的弧编组。

5.根据权利要求1所述的基于超声波传感器三位置测量的环境轮廓重构方法，其特征在于，所述方法还包括：验证方法的准确定和精确性的步骤，具体地：

根据三条相交弧的环境轮廓类型，提取环境轮廓特征点，再将环境轮廓描绘出来，验证方法的准确定和精确性。

6.根据权利要求1所述的基于超声波传感器三位置测量的环境轮廓重构方法，其特征在于，筛选出一种易于环境重构三位置测量弧相交形式步骤，包括建立数据筛选模型，通过分析确定三位置弧所需的相交形式，通过运用距离关系比较，筛选出最终数据，距离关系满足条件如下：

Dist((3),(1,2))＞R₃

Dist((2),(1,3))＜R₂

Dist((1),(2,3))＞R₁

其中Dist表示某一位置圆心与另两位置相交弧交点的距离。

7.根据权利要求1所述的基于超声波传感器三位置测量的环境轮廓重构方法，其特征在于，判断三位置弧所表示的环境轮廓类型，包括使用MATLAB将三位置筛选后的弧组合数据表示的环境轮廓展现出来。

8.一种基于超声波传感器三位置测量的环境轮廓重构系统，包括:

数据采集模块，包括设置在数据采集装置上超声波传感器，其用于获取超声波传感器测量位置坐标及数据采集装置前进朝向角度信息；

融合模块，其用于筛除误差范围之外的错误信息并融合相邻的相近测量数据；

第一筛选模块，利用测量弧信息及位置关系，筛选出三位置测量弧中两两相交的组合，记录各相交弧信息；

第二筛选模块，其用于筛选出一种易于环境重构三位置测量弧相交形式；

判断模块，其用于判断三位置弧所表示的环境轮廓类型。

9.根据权利要求8所述的系统，其特征在于，所述第一筛选模块包括：

第一判断单元，其用于判断第一位置、第二位置测量弧的相关情况，从第一位置的第一个弧开始，与第二位置的所有弧依次判断，相交弧统一编为一个组，比较之后再从第一位置的第二个弧开始，与第二位置的所有弧依次比较，将所有相交的弧编组；

第二判断单元，其用于判断第三位置与第一位置和第二位置相交弧的相关情况，并防止有相交弧的信息重复存储的情况；

筛选单元，其用于对三相交弧进行筛选，选出三位置对应的三个测量弧均两两相交的组合情况。

10.根据权利要求8所述的系统，其特征在于，所述系统还包括：

验证模块，其用于对基于超声波传感器三位置测量的环境轮廓重构方法进行验证，将环境轮廓描绘出来，验证方法的准确定和精确性。

Images