CN114111785A - 一种基于rir倒谱提取一阶回波的声slam系统及方法 - Google Patents

一种基于rir倒谱提取一阶回波的声slam系统及方法 Download PDF

Info

Publication number
CN114111785A
CN114111785A CN202111259582.1A CN202111259582A CN114111785A CN 114111785 A CN114111785 A CN 114111785A CN 202111259582 A CN202111259582 A CN 202111259582A CN 114111785 A CN114111785 A CN 114111785A
Authority
CN
China
Prior art keywords
order
rir
cepstrum
echo
acoustic
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
CN202111259582.1A
Other languages
English (en)
Other versions
CN114111785B (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.)
Guilin University of Electronic Technology
Original Assignee
Guilin University of Electronic Technology
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 Guilin University of Electronic Technology filed Critical Guilin University of Electronic Technology
Priority to CN202111259582.1A priority Critical patent/CN114111785B/zh
Publication of CN114111785A publication Critical patent/CN114111785A/zh
Application granted granted Critical
Publication of CN114111785B publication Critical patent/CN114111785B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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/20—Position of source determined by a plurality of spaced direction-finders
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明涉及室内定位技术领域，具体涉及一种基于RIR倒谱提取一阶回波的声SLAM系统及方法，在机器人移动时通过声脉冲发生器主动发射高频脉冲信号；利用共址微小麦克风阵列接收、量化和存储高频脉冲信号经反射体作用后的回波信号；通过回波提取器提取回波信号的RIR倒谱特征，辨识反射体和与反射体对应的一阶回波，通过位姿优化器利用一阶回波估计的镜像声源位置，结合IMU实时位姿估计，最小化所有观测点的负对数似然和，优化IMU与声传感器估计位姿的权重，进而计算、绘制和显示机器人的移动轨迹和室内轮廓地图；解决了传统激光/视觉SLAM高成本、高复杂度及视觉采集带来的隐私安全问题，改善了声SLAM的定位精度和复杂度。

Description

一种基于RIR倒谱提取一阶回波的声SLAM系统及方法
技术领域
本发明涉及室内定位技术领域，尤其涉及一种基于RIR倒谱提取一阶回波的声SLAM系统及方法。
背景技术
随着物联网技术发展及其产业化推进,常用的服务型机器人例如扫地机器人、搬运机器人等已经走进千家万户，它们的出现极大的方便了人们的生活。作为机器人能够自主移动的核心技术-同步定位与地图构建(Simultaneous Localization and Mapping，SLAM)，一出现就受到了社会各界的广泛关注。
SLAM是指在未知环境下的机器人仅依靠搭载的传感器，构建周围环境地图，同时确定自身在地图中的位置。依据使用传感器的差异对目前主流的SLAM技术进行分类，可分为：激光雷达SLAM技术、视觉SLAM技术和声SLAM技术。
IDAR传感器具有准确和高分辨率的优点，但是它们也有缺点：LIDAR是一种非常昂贵的传感器，并且在操作中会带来健康和安全问题；相机虽然成本越来越低，但在低光照环境下需要高处理能力以及低信噪比，而且相机采集图像可能会带来隐私安全隐患。相较于激光/视觉SLAM技术，采用声传感器的声SLAM技术，不仅价格低廉而且可以避免视觉采集带来的隐私安全问题。目前的SLAM方案采用的是静止的分布式麦克风阵列，需要提前在房间中布置声源和麦克风阵列，操作比较复杂，且定位与建图精度较低。
发明内容
本发明的目的在于提供一种基于RIR倒谱提取一阶回波的声SLAM系统及方法，旨在解决传统激光/视觉SLAM高成本、高复杂度及视觉采集带来的隐私安全问题，改善了声SLAM的定位精度和复杂度。
为实现上述目的，第一方面，本发明提供了一种基于RIR倒谱提取一阶回波的声SLAM方法，包括：
在机器人移动时通过声脉冲发生器主动发射高频脉冲信号；
利用共址微小麦克风阵列接收、量化和存储高频脉冲信号经反射体作用后的回波信号；
通过回波提取器提取回波信号的RIR倒谱特征，辨识反射体和与反射体对应的一阶回波；
通过位姿优化器利用一阶回波估计的镜像声源位置，结合IMU实时位姿估计，最小化所有观测点的负对数似然和，优化IMU与声传感器估计位姿的权重，进而计算、绘制和显示机器人的移动轨迹和室内轮廓地图。
其中，通过回波提取器提取回波信号的RIR倒谱特征，辨识反射体和与反射体对应的一阶回波的具体方式为：
通过RIR倒谱估计算法提取声回波信号序列的RIR倒谱；
基于声回波信号序列的RIR倒谱信息通过回波标记算法将备选一阶声回波脉冲组合与反射体关联，得到一阶RIR倒谱；
基于一阶RIR倒谱特征，辨识反射体和与反射体对应的一阶回波；
其中，通过位姿优化器利用一阶回波估计的镜像声源位置，结合IMU实时位姿估计，最小化所有观测点的负对数似然和，优化IMU与声传感器估计位姿的权重，进而计算、绘制和显示机器人的移动轨迹和室内轮廓地图的具体方式为：
基于一阶RIR倒谱向量，通过回环检测算法将当前观测点与本次建图过程中已经经过并采样测量的观测点数据关联；
根据等腰梯形位姿估计算法估计出机器人有数据关联观测点间的位姿变换；
将机器人的观测点设定为图的节点，根据有数据关联观测点间的位姿变换关系构建图模型，通过图优化位姿矫正算法优化机器人的路径轨迹；
根据优化后的路径以及镜像声源位置信息来绘制和显示机器人的移动轨迹和房间地图。
其中，通过RIR倒谱估计算法提取声回波信号序列的RIR倒谱的具体方式为：
求取每个麦克风的室内脉冲响应，得到室内响应脉冲组；
提取室内响应脉冲组中的每一个脉冲持续时间内的平均能量特征；
提取室内响应脉冲组中的每一个脉冲的时延特征；
基于麦克风阵列的每个麦克风接收脉冲的平均能量特征和时延特征得到对数能量向量和对数时延向量；
基于所有观测点麦克风阵列的对数能量向量和对数时延向量，得到关于室内脉冲响应平均幅度对数矩阵和声反射距离对数矩阵；
对室内脉冲响应平均幅度对数矩阵和声反射距离对数矩阵分别进行PCA降维运算，得到室内脉冲响应平均幅度主成分和声反射距离主成分；
将室内脉冲响应平均幅度主成分和声反射距离主成分拼接组成RIR倒谱。
其中，基于声回波信号序列的RIR倒谱信息通过回波标记算法将备选一阶声回波脉冲组合与反射体关联，得到一阶RIR倒谱中的具体方式为：
根据机器人移动后发射的声脉冲及每个麦克风接收到声脉冲获取对应的室内脉冲响应，构成该观测点的室内脉冲响应组合；
对每个观测点的室内脉冲响应组合按照到达时间进行升序排序，得到室内脉冲响应时间序列，取其第一到达时间对应脉冲作为直达声脉冲，选取其第二到达时间和第三到达时间对应的脉冲作为一阶反射声的备选脉冲组合；；
按照上述RIR倒谱估计算法求取一阶反射声备选脉冲组合的一阶RIR倒谱；
对获取的一阶反射声备选脉冲对应的一阶RIR倒谱分别取幅度分量和反射距离分量进行线性拟合，得到的与不同反射体材质相对应的拟合线段斜率，完成距离观测点最近的两个反射体的一阶回波标记；
根据上一观测点的一阶声反射距离和IMU估计得到的当前观测点机器人位移距离，结合等腰梯形几何约束，得到当前观测点的一阶声反射距离上限和下限；
根据当前观测点一阶声反射的距离上限和下限对其室内脉冲响应时间序列进行截取，得到当前观测点对所有反射体的一阶回波到达时间备选序列；
选取当前观测点对所有反射体的一阶回波到达时间备选序列对应的回波脉冲，得到当前观测点来自所有反射体的一阶回波脉冲备选序列；
对当前观测点的一阶回波脉冲备选序列进行排列组合，采用RIR倒谱估计算法计算其RIR倒谱，得到当前观测点的一阶RIR倒谱备选序列；
计算当前观测点的一阶RIR倒谱备选序列到所有观测点对所有反射体观测的拟合线段的欧式距离，选取欧式距离小于一定阈值的一阶RIR倒谱，该一阶RIR倒谱对应的回波脉冲序列即为当前观测点接收的来自拟合线段对应反射体的一阶反射回波；
其中，基于一阶RIR倒谱向量，通过回环检测算法将当前观测点与本次建图过程中已经经过并采样测量的观测点数据关联的具体方式为：
通过机器人的已经走过路径的任一观测点位置得到所有反射体对应的一阶RIR倒谱并将一阶RIR倒谱组合为第一一阶RIR倒谱向量；
通过机器人的当前观测点位置得到所有反射体对应的一阶RIR倒谱并将一阶RIR倒谱组合为第二一阶RIR倒谱向量；
通过第一一阶RIR倒谱向量和第二一阶RIR倒谱向量的欧式距离来判断当前观测点是否需要与本次建图过程中已经经过并采样测量的观测点建立数据关联。
其中，根据等腰梯形位姿估计算法估计出机器人有数据关联观测点间的位姿变换的具体方式为：
根据所述一阶RIR倒谱可以得到所有观测点的一阶回波组合，利用TOA算法计算出在机器人坐标系下所有观测点一阶镜像声源的位置；
根据极坐标下相同反射体镜像声源角度的改变，估计出有数据关联观测点间机器人的旋转角度；
计算有数据关联观测点间机器人移动时的虚拟声源极坐标长度的改变关系；
计算有数据关联观测点间机器人移动时的矢量函数；
加权矢量函数中的误差函数，得到加权函数；
基于加权函数得到目标优化函数；
通过对目标函数优化得到有数据关联观测点间机器人的位姿变换。
其中，将机器人的观测点设定为图的节点，根据有数据关联观测点间的位姿变换关系构建图模型，通过图优化位姿矫正算法优化机器人的路径轨迹的具体方式为：
机器人每次移动或者旋转，测量观测一次，并将当前观测点作为图的节点添加到图中；
根据位姿估计模块估计出所有有数据关联观测点间机器人的位姿变换，建立图节点间的位姿约束关系，构建图模型；
计算机器人有数据关联观测点间的IMU估计的位姿变换与声传感器通过等腰梯形位姿估计算法观测得到的位姿变换之间的差；
通过最小化所有观测点间的负对数似然和，优化IMU与声传感器估计位姿的权重；
通过不断迭代优化得到机器人观测点的移动轨迹。
其中，根据优化后的路径以及镜像声源位置信息来绘制和显示机器人的移动轨迹和房间地图的具体方式为：
根据所述图优化模块优化后机器人观测点的位姿得到机器人真实声源的位置；
计算机器人所有观测点真实声源位置与虚拟声源位置连线的中点，得到反射体的位置；
根据所有观测点观测到的反射体位置，建立并绘制房间的轮廓图；
根据RI R倒谱回波标记模块对不同材料反射体的标记，在房间轮廓图中标记出墙壁、门和窗等不同反射体的位置。
第二方面，一种基于RIR倒谱提取一阶回波的声SLAM系统，应用于第一方面所述的基于RIR倒谱提取一阶回波的声SLAM方法。
本发明的一种基于RIR倒谱提取一阶回波的声SLAM系统及方法，在机器人移动时通过声脉冲发生器主动发射高频脉冲信号；利用共址微小麦克风阵列接收、量化和存储高频脉冲信号经反射体作用后的回波信号；通过回波提取器提取回波信号的RIR倒谱特征，辨识反射体和与反射体对应的一阶回波，通过位姿优化器利用一阶回波估计的镜像声源位置，结合IMU实时位姿估计，最小化所有观测点的负对数似然和，优化IMU与声传感器估计位姿的权重，进而计算、绘制和显示机器人的移动轨迹和室内轮廓地图。采用高频不可闻声信号实现SLAM，可以保证微型共址微小麦克风阵列的应用，并避免室内人声等敏感信息泄露，确保隐私安全；采用RIR倒谱提取一阶回波，可以快速实现厘米级的定位与建图服务，解决了传统激光/视觉SLAM高成本、高复杂度及视觉采集带来的隐私安全问题，改善了声SLAM的定位精度和复杂度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明提供的一种基于RIR倒谱提取一阶回波的声SLAM方法的流程图；
图2是利用共址微小麦克风阵列接收、量化和存储高频脉冲信号经反射体作用后的回波信号的的具体流程图；
图3是通过回波提取器提取回波信号的RIR倒谱特征，辨识反射体和与反射体对应的一阶回波的具体流程图；
图4是通过位姿优化器利用一阶回波估计的镜像声源位置，结合IMU实时位姿估计，最小化所有观测点的负对数似然和，优化IMU与声传感器估计位姿的权重，进而计算、绘制和显示机器人的移动轨迹和室内轮廓地图的具体流程图；
图5是声脉冲发生器、共址微小麦克风阵列和回波提取器的结构示意图；
图6是声脉冲发生器的结构示意图；
图7是共址微小麦克风阵列的结构示意图；
图8是回波提取器和位姿优化器的结构示意图；
图9是本发明提供的一种基于RIR倒谱提取一阶回波的声SLAM系统的结构示意图；
图10是位姿估计模块的移动示意图；
图11是图优化模块图节点的连接示意图；
图12是RIR倒谱回波标记模块的结构示意图。
1-声脉冲发生器、2-共址微小麦克风阵列、3-回波提取器、4-位姿优化器、11-脉冲调制模块、12-音频输出模块、21-音频拾取模块、22-带通滤波器模块、23-音频量化模块、31-RIR倒谱估计模块、32-RIR倒谱回波标记模块、42-闭环检测模块、43-位姿估计模块、44-图优化模块、45-定位与建图模块、321-关联单元、322-辨识单元。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
在本发明的描述中，需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
请参阅图1至图4，第一方面，本发明提供一种基于RIR倒谱提取一阶回波的声SLAM方法，包括：
S1、在机器人移动时通过声脉冲发生器1主动发射高频脉冲信号；
利用所述脉冲调制模块11将机器人移动时产生的信号送入所述音频输出模块12产生高频声音频脉冲信号；
所述音频输出模块12产生的高频声音频脉冲信号为16k-20k的正弦扫频信号s(t)：
Figure BDA0003325167340000071
其中f0是初始频率，f1是截止频率，T是信号时长，w(t)是汉宁窗。
S2、利用共址微小麦克风阵列2接收、量化和存储高频脉冲信号经反射体作用后的回波信号；
具体方式为：
S201、当麦克风收到高频脉冲信号经反射体作用后的回波信号后，将回波信号依次送入音频拾取模块21、带通滤波器模块22进行放大和滤波，得到滤波信号；
S202、将滤波信号送入音频量化模块23，得到量化信号，并将所述量化信号保存。
具体过程为：
1)所述共址微小麦克风阵列2的麦克风mj，j＝1，2，...，M收到经反射体作用后的高频回波信号rj(t)；
2)将rj(t)依次送入音频拾取模块21、带通滤波器模块22进行放大和滤波；
3)将滤波后的信号送入音频量化模块23，得到rj(n)；
4)分别保存所述共址微小麦克风阵列2中麦克风i量化后的信号rj(n)。
S3、通过回波提取器3提取回波信号的RIR倒谱特征，辨识反射体和与反射体对应的一阶回波。
具体方式为：
S301、通过RIR倒谱估计算法提取声回波信号序列的RIR倒谱；
具体方式为：
1)根据以下公式求取麦克风mj的室内脉冲响应hi，j(n)：
Figure BDA0003325167340000081
2)hij(n)由一系列的脉冲组成，提取第k个脉冲的平均能量特征ri，j，k：
Figure BDA0003325167340000082
其中，τij，k为hi，j(n)第k个脉冲的TOA值，2L+1为矩形窗的宽度；
3)hi，j(n)中第k个脉冲的时延特征为ti，j，k＝τi，j，k·c，其中c为声音在空气中的传播速度；
4)分别对上述两个特征进行对数运算，得到对数能量向量pi，k和对数时延向量qi，k：
pi，k＝(log(ri，1，k)log(ri，2，k)…log(ri，M，k))T
qi，k＝(log(ti，1，k)log(ti，2，k)…log(ti，M，k))T；
5)机器人移动N次(N＞M)，从观测点x1移动到观测点xN，可以得到关于室内脉冲响应平均幅度对数的矩阵Ak和到达距离对数的矩阵Bk：
Ak＝(p1，k p2，k … pn，k)T
Bk＝(q1，k q2，k … qn，k)T；
6)分别对Ak和Bk进行PCA降维运算，降维后的数据为：
Figure BDA0003325167340000083
Figure BDA0003325167340000084
7)分别选取
Figure BDA0003325167340000091
和
Figure BDA0003325167340000092
的主成分分量，即特征值最大的分量
Figure BDA0003325167340000093
组成室内脉冲响应倒谱
Figure BDA0003325167340000094
Figure BDA0003325167340000095
Figure BDA0003325167340000096
为N×2的矩阵，定义
Figure BDA0003325167340000097
为室内脉冲响应倒谱，
Figure BDA0003325167340000098
为幅度倒谱，
Figure BDA0003325167340000099
距离倒谱；当所述共址微小麦克风阵列2观测的脉冲一致时，室内脉冲响应倒谱
Figure BDA00033251673400000910
的
Figure BDA00033251673400000911
和
Figure BDA00033251673400000912
近似为线性关系。
S302、基于声回波信号序列的RIR倒谱信息通过回波标记算法将备选一阶声回波脉冲组合与反射体关联，得到一阶RIR倒谱；
具体方式为：
1)机器人机器人移动N次(N＞M)，从观测点x1移动到观测点xN，取麦克风接收到的第2个脉冲和第3个脉冲求室内脉冲响应倒谱
Figure BDA00033251673400000913
2)对
Figure BDA00033251673400000914
进行直线拟合，得到对应不同反射体i对应直线li：aix+biy+ci＝0；
3)根据上一观测点的一阶声反射距离和IMU估计得到的当前观测点机器人位移距离，结合等腰梯形几何约束可以得到来自反射体i的一阶回波候选值。这些候选值通过排列组合可以得到u个新的室内脉冲组合，对应的一阶RIR倒谱备选序列dτ为dτ＝[da，τ db，τ]，τ＝1，2，…，u；
4)如果新的组合是来自反射体i的一阶回波，则对应的一阶RIR倒谱备选序列dτ应在直线li附近，dτ到直线li的距离满足以下公式：
Figure BDA00033251673400000915
其中Δε为欧式距离阈值。
5)将所有的组合倒谱dτ根据步骤4)的方法找到与反射体i对应的一阶RIR倒谱。
S303、基于一阶RIR倒谱特征，辨识反射体和与反射体对应的一阶回波。
S4、通过位姿优化器利用一阶回波估计的镜像声源位置，结合IMU实时位姿估计，最小化所有观测点的负对数似然和，优化IMU与声传感器估计位姿的权重，进而计算、绘制和显示机器人的移动轨迹和室内轮廓地图。
S401、基于一阶RIR倒谱向量，通过回环检测算法将当前观测点与本次建图过程中已经经过并采样测量的观测点数据关联；
具体方式为：
1)机器人在xi处，根据RIR倒谱回波标记模块32可以得到k面反射体观测一致的一阶RIR倒谱，这些倒谱可以组合为一个2k维的一阶RIR倒谱向量Xi：
Xi＝[xi1 yi1 xi2 yi2 … xik yik]；
2)同理，机器人在xj处(j＜i)，可以得到一阶RIR倒谱向量Xj：
Xj＝[xj1 yj1 xj2 yj2 … xjk yjk]；
3)如果向量Xi和Xj的欧式距离满足以下公式：
Distanceij＝|Xj-Xi|＜δ；
其中δ为欧式距离阈值，说明xi和xj位置接近。
S402、根据等腰梯形位姿估计算法估计出机器人有数据关联观测点间的位姿变换；
具体方式为：
1)机器人从xi-1移动到xi处的位姿变换为：
dxi＝Xi-Xi-1＝(r，α，θ)
其中r为位移长度，α为位移方向与世界坐标系X轴的夹角，θ为机器人的旋转角度。
2)根据RIR倒谱回波标记模块32可以得到一阶回波组合，利用TOA算法可以计算出在机器人坐标系下一阶镜像声源的位置，在极坐标系下表示如下：
Figure BDA0003325167340000101
3)机器人旋转角度的估计值为：
Figure BDA0003325167340000102
4)机器人从xi-1移动到xi，虚拟声源极坐标长度的改变关系如下：
Figure BDA0003325167340000103
其中αi-1，n＝θi-1，1-θi-1，n，n＝1，2，3，4；
5)另s＝(r，α)，可以得到以下矢量函数：
d(s)＝[||ri，1-ri-1，1||，|ri，2-ri-1，2||，||ri，3-ri-1，3||，||ri，4-ri-1，4||]
＝[2rsin(α+αi-1，1)，2rsin(α+αi-1，2)，2rsin(α+αi-1，3)，2rsin(α+αi-1，4)]；
6)加权误差函数ε(s)：
Figure BDA0003325167340000111
假设从声源到共址微小麦克风阵列的所有路径拥有恒定噪声功率
Figure BDA0003325167340000112
那么权值即噪声功率矩阵为
Figure BDA0003325167340000113
I为单位矩阵。
7)通过最小化加权误差函数估计求解位姿变换，目标优化函数为：
Figure BDA0003325167340000114
8)根据列文伯格优化算法迭代求解上述目标优化函数，迭代过程如下
sk＝sk-1+Δsk-1
Δsk-1为第k次迭代的步长，sk为第k次迭代的位姿变换结果，根据列文伯格算法可以求出
Δsk-1＝(H+λI)-1g
其中H＝J(s)TJ(s)，
Figure BDA0003325167340000115
Figure BDA0003325167340000116
9)位姿变换
Figure BDA0003325167340000117
为
Figure BDA0003325167340000118
其中sO＝(r0，α0)为IMU估计的初值，n为迭代的次数。
S403、将机器人的观测点设定为图的节点，根据有数据关联观测点间的位姿变换关系构建图模型，通过图优化位姿矫正算法优化机器人的路径轨迹；
具体方式为：
1)机器人每移动lm或者旋转α°，向图中添加一个节点，并根据位姿估计算法建立顶点间的约束关系。机器人的轨迹点x为图中的节点的集合
x＝(x1，x2，…，xn)
2)机器人从xi运动到xj，
Figure BDA0003325167340000119
为IMU估计的位姿变换，Zij为声传感器观测得到的位姿变换。令e(xi，xj)为xi到xj的误差函数，该函数为机器人的预测观测值
Figure BDA00033251673400001110
与实际观测值Zij之间的差：
Figure BDA00033251673400001111
3)令
Figure BDA00033251673400001112
为约束对的集合。最大似然方法的目标是找到使所有观测值的负对数似然F(x)最小的节点x*的配置：
Figure BDA0003325167340000121
误差函数eij不确定性的测量信息矩阵Ωij＝[ox 2，σy 2，σθ 2]I，I为单位矩阵。
4)目标优化函数为：
Figure BDA0003325167340000122
5)利用非线性优化算法迭代求解F(x)的最小值，迭代过程如下
xk＝xk-1+Δxk-1
Δkk-1为第k次迭代的步长，xk为第k次迭代的机器人轨迹点。
6)机器人轨迹点x*为
Figure BDA0003325167340000123
x0为IMU估计的机器人初始轨迹点，n为迭代的次数。
S404、根据优化后的路径以及镜像声源位置信息来绘制和显示机器人的移动轨迹和房间地图。
具体方式为：
1)根据图优化模块44计算的结果得到机器人优化后的位置轨迹。
2)机器人在xi处，通过虚拟声源
Figure BDA0003325167340000124
与机器人位置连线的中点，得到反射体的位置。
3)根据机器人的轨迹点以及轨迹点对应的虚拟声源的位置就可以得到房间的轮廓图。
4)根据RIR倒谱回波标记模块32对反射体的标记，在房间轮廓图中标记出墙壁，门和窗等不同反射体位置。
本发明提出的一种基于RIR倒谱提取一阶回波的声SLAM方法的创新点：
1、回波标记方法是声SLAM系统的基础环节，常用的回波标记方法是使用欧式距离矩阵(EDM)的方法，该方法需要遍历所用可能的回波组合，计算复杂度较高。本发明从共址微小麦克风阵列接收的声信号中提取了一种室内脉冲响应倒谱，通过判断不同回波组合的倒谱特征是否在反射体拟合直线附近来区分一阶回波。基于标记好的回波组成的室内脉冲响应向量，我们又提出了一种闭环检测方法来检测机器人是否在曾经走过的位置上或位置附近。
2、声SLAM需要估计机器人前后两个位置的位姿变换来估计机器人的移动轨迹，常用的方法是利用IMU的数据估计机器人移动轨迹，但是IMU存在累计误差会导致机器人自定位误差较大。本发明根据机器人前后两个声源位置与虚拟声源位置满足等腰梯形约束关系，提出了一种声信号位姿变换估计方法。该方法通过列文伯格算法迭代求解等腰梯形构成的误差函数，求解出位姿变换结果。
本发明提出的一种基于RIR倒谱提取一阶回波的声SLAM方法，室内脉冲响应(RIR,Room Impulse Responses)在机器人移动时通过声脉冲发生器1主动发射高频脉冲信号；利用共址微小麦克风阵列2接收、量化和存储高频脉冲信号经反射体作用后的回波信号；通过回波提取器3提取回波信号的RIR倒谱特征，辨识反射体和与反射体对应的一阶回波，通过位姿优化器4利用一阶回波估计的镜像声源位置，结合IMU实时位姿估计，通过最小化所有观测点的负对数似然和，优化IMU与声传感器估计位姿的权重，进而计算、绘制和显示机器人的移动轨迹和室内轮廓地图。采用高频不可闻声信号实现SLAM，可以保证微型共址微小麦克风阵列的应用，并避免室内人声等敏感信息泄露，确保隐私安全；采用RIR倒谱提取一阶回波，可以快速实现厘米级的定位与建图服务，解决了传统激光/视觉SLAM高成本、高复杂度及视觉采集带来的隐私安全问题，改善了声SLAM的定位精度和复杂度。
请参阅图5至图12，第二方面，本发明提供一种基于RIR倒谱提取一阶回波的声SLAM系统，包括声脉冲发生器1、共址微小麦克风阵列2、回波提取器3和位姿优化器4，所述声脉冲发生器1、所述共址微小麦克风阵列2与所述回波提取器3连接，所述回波提取器4与所述位姿优化器4连接。
所述声脉冲发生器1包括脉冲调制模块11和音频输出模块12；
所述共址微小麦克风阵列2包括若干音频拾取模块21、若干带通滤波器模块22和音频量化模块23，若干所述音频拾取模块21分别与若干所述带通滤波器模块22连接，若干所述带通滤波器模块22分别与所述音频量化模块23连接；
所述回波提取器3包括RIR倒谱估计模块31、RIR倒谱回波标记模块32；
所述位姿优化器4包括闭环检测模块42、位姿估计模块43、图优化模块44和定位与建图模块45；
所述RIR倒谱回波标记模块32包括关联单元321和辨识单元322；
所述脉冲调制模块11，用于产生16k-20k的线性调频信号。
所述音频输出模块12，用于将线性调频信号转换为高频声信号。
所述音频拾取模块21，用于采集高频声定位信号经反射体作用后的高频声回声信号。
所述带通滤波器模块22，用于滤除高频声回声信号频段外的噪声信号，得到滤波信号。
所述音频量化模块23，用于对滤波信号进行模数转换，得到量化信号。
所述RIR倒谱估计模块31，用于将量化信号的室内脉冲响应倒谱。
所述关联单元321，基于脉冲响应倒谱信息将声音回波脉冲组合与反射体关联，得到RIR倒谱。
所述辨识单元322，基于RIR倒谱特征，辨识反射体和与反射体对应的一阶回波。
所述闭环检测模块42，基于一阶RIR倒谱向量，通过回环检测算法将当前观测点与本次建图过程中已经经过并采样测量的观测点数据关联。
所述位姿估计模块43，根据等腰梯形位姿估计算法估计出机器人有数据关联观测点间的位姿变换。
所述图优化模块44，将机器人的观测点设定为图的节点，根据有数据关联观测点间的位姿变换关系构建图模型，通过图优化位姿矫正算法优化机器人的路径轨迹。
所述定位与建图模块45，根据优化后的路径以及镜像声源位置信息来绘制和显示机器人的移动轨迹和房间地图。
在本实施方式中，所述声脉冲发生器1与所述共址微小麦克风阵列2通过高频声反射信号进行连接，所述声脉冲发生器1和所述共址微小麦克风阵列2处理端通过数据线与所述位姿优化器4进行连接。
以上所揭露的仅为本发明一种基于RIR倒谱提取一阶回波的声SLAM系统及方法较佳实施例而已，当然不能以此来限定本发明之权利范围，本领域普通技术人员可以理解实现上述实施例的全部或部分流程，并依本发明权利要求所作的等同变化，仍属于发明所涵盖的范围。

Claims (10)

1.一种基于RIR倒谱提取一阶回波的声SLAM方法，其特征在于，包括：
在机器人移动时通过声脉冲发生器主动发射高频脉冲信号；
利用共址微小麦克风阵列接收、量化和存储高频脉冲信号经反射体作用后的回波信号；
通过回波提取器提取回波信号的RIR倒谱特征，辨识反射体和与反射体对应的一阶回波；
通过位姿优化器利用一阶回波估计的镜像声源位置，结合IMU实时位姿估计，最小化所有观测点的负对数似然和，优化IMU与声传感器估计位姿的权重，进而计算、绘制和显示机器人的移动轨迹和室内轮廓地图。
2.如权利要求1所述的基于RIR倒谱提取一阶回波的声SLAM方法，其特征在于，通过回波提取器提取回波信号的RIR倒谱特征，辨识反射体和与反射体对应的一阶回波的具体方式为：
通过RIR倒谱估计算法提取声回波信号序列的RIR倒谱；
基于声回波信号序列的RIR倒谱信息通过回波标记算法将备选一阶声回波脉冲组合与反射体关联，得到一阶RIR倒谱；
基于一阶RIR倒谱特征，辨识反射体和与反射体对应的一阶回波。
3.如权利要求1所述的基于RIR倒谱提取一阶回波的声SLAM方法，其特征在于，通过位姿优化器利用一阶回波估计的镜像声源位置，结合IMU实时位姿估计，最小化所有观测点的负对数似然和，优化IMU与声传感器估计位姿的权重，进而计算、绘制和显示机器人的移动轨迹和室内轮廓地图的具体方式为：
基于一阶RIR倒谱向量，通过回环检测算法将当前观测点与本次建图过程中已经经过并采样测量的观测点数据关联；
根据等腰梯形位姿估计算法估计出机器人有数据关联观测点间的位姿变换；
将机器人的观测点设定为图的节点，根据有数据关联观测点间的位姿变换关系构建图模型，通过图优化位姿矫正算法优化机器人的路径轨迹；
根据优化后的路径以及镜像声源位置信息来绘制和显示机器人的移动轨迹和房间地图。
4.如权利要求2所述的基于RIR倒谱提取一阶回波的声SLAM方法，其特征在于，通过RIR倒谱估计算法提取声回波信号序列的RIR倒谱的具体方式为：
求取每个麦克风的室内脉冲响应，得到室内响应脉冲组；
提取室内响应脉冲组中的每一个脉冲持续时间内的平均能量特征；
提取室内响应脉冲组中的每一个脉冲的时延特征；
基于麦克风阵列的每个麦克风接收脉冲的平均能量特征和时延特征得到对数能量向量和对数时延向量；
基于所有观测点麦克风阵列的对数能量向量和对数时延向量，得到关于室内脉冲响应平均幅度对数矩阵和声反射距离对数矩阵；
对室内脉冲响应平均幅度对数矩阵和声反射距离对数矩阵分别进行PCA降维运算，得到室内脉冲响应平均幅度主成分和声反射距离主成分；
将室内脉冲响应平均幅度主成分和声反射距离主成分拼接组成RIR倒谱。
5.如权利要求2所述的基于RIR倒谱提取一阶回波的声SLAM方法，其特征在于，基于声回波信号序列的RIR倒谱信息通过回波标记算法将备选一阶声回波脉冲组合与反射体关联，得到一阶RIR倒谱中的具体方式为：
根据机器人移动后发射的声脉冲及每个麦克风接收到声脉冲获取对应的室内脉冲响应，构成该观测点的室内脉冲响应组合；
对每个观测点的室内脉冲响应组合按照到达时间进行升序排序，得到室内脉冲响应时间序列，取其第一到达时间对应脉冲作为直达声脉冲，选取其第二到达时间和第三到达时间对应的脉冲作为一阶反射声的备选脉冲组合；；
按照上述RIR倒谱估计算法求取一阶反射声备选脉冲组合的一阶RIR倒谱；
对获取的一阶反射声备选脉冲对应的一阶RIR倒谱分别取幅度分量和反射距离分量进行线性拟合，得到的与不同反射体材质相对应的拟合线段斜率，完成距离观测点最近的两个反射体的一阶回波标记；
根据上一观测点的一阶声反射距离和IMU估计得到的当前观测点机器人位移距离，结合等腰梯形几何约束，得到当前观测点的一阶声反射距离上限和下限；
根据当前观测点一阶声反射的距离上限和下限对其室内脉冲响应时间序列进行截取，得到当前观测点对所有反射体的一阶回波到达时间备选序列；
选取当前观测点对所有反射体的一阶回波到达时间备选序列对应的回波脉冲，得到当前观测点来自所有反射体的一阶回波脉冲备选序列；
对当前观测点的一阶回波脉冲备选序列进行排列组合，采用RIR倒谱估计算法计算其RIR倒谱，得到当前观测点的一阶RIR倒谱备选序列；
计算当前观测点的一阶RIR倒谱备选序列到所有观测点对所有反射体观测的拟合线段的欧式距离，选取欧式距离小于一定阈值的一阶RIR倒谱，该一阶RIR倒谱对应的回波脉冲序列即为当前观测点接收的来自拟合线段对应反射体的一阶反射回波。
6.如权利要求4所述的基于RIR倒谱提取一阶回波的声SLAM方法，其特征在于，基于一阶RIR倒谱向量，通过回环检测算法将当前观测点与本次建图过程中已经经过并采样测量的观测点数据关联的具体方式为：
通过机器人的已经走过路径的任一观测点位置得到所有反射体对应的一阶RIR倒谱并将一阶RIR倒谱组合为第一一阶RIR倒谱向量；
通过机器人的当前观测点位置得到所有反射体对应的一阶RIR倒谱并将一阶RIR倒谱组合为第二一阶RIR倒谱向量；
通过第一一阶RIR倒谱向量和第二一阶RIR倒谱向量的欧式距离来判断当前观测点是否需要与本次建图过程中已经经过并采样测量的观测点建立数据关联。
7.如权利要求4所述的基于RIR倒谱提取一阶回波的声SLAM方法，其特征在于，根据等腰梯形位姿估计算法估计出机器人有数据关联观测点间的位姿变换的具体方式为：
根据所述一阶RIR倒谱可以得到所有观测点的一阶回波组合，利用TOA算法计算出在机器人坐标系下所有观测点一阶镜像声源的位置；
根据极坐标下相同反射体镜像声源角度的改变，估计出有数据关联观测点间机器人的旋转角度；
计算有数据关联观测点间机器人移动时的虚拟声源极坐标长度的改变关系；
计算有数据关联观测点间机器人移动时的矢量函数；
加权矢量函数中的误差函数，得到加权函数；
基于加权函数得到目标优化函数；
通过对目标函数优化得到有数据关联观测点间机器人的位姿变换。
8.如权利要求6所述的基于RIR倒谱提取一阶回波的声SLAM方法，其特征在于，将机器人的观测点设定为图的节点，根据有数据关联观测点间的位姿变换关系构建图模型，通过图优化位姿矫正算法优化机器人的路径轨迹的具体方式为：
机器人每次移动或者旋转，测量观测一次，并将当前观测点作为图的节点添加到图中；
根据位姿估计模块估计出所有有数据关联观测点间机器人的位姿变换，建立图节点间的位姿约束关系，构建图模型；
计算机器人有数据关联观测点间的IMU估计的位姿变换与声传感器通过等腰梯形位姿估计算法观测得到的位姿变换之间的差；
通过最小化所有观测点间的负对数似然和，优化IMU与声传感器估计位姿的权重；
通过不断迭代优化得到机器人观测点的移动轨迹。
9.如权利要求7所述的基于RIR倒谱提取一阶回波的声SLAM方法，其特征在于，根据优化后的路径以及镜像声源位置信息来绘制和显示机器人的移动轨迹和房间地图的具体方式为：
根据所述图优化模块优化后机器人观测点的位姿得到机器人真实声源的位置；
计算机器人所有观测点真实声源位置与虚拟声源位置连线的中点，得到反射体的位置；
根据所有观测点观测到的反射体位置，建立并绘制房间的轮廓图；
根据RIR倒谱回波标记模块对不同材料反射体的标记，在房间轮廓图中标记出墙壁、门和窗等不同反射体的位置。
10.一种基于RIR倒谱提取一阶回波的声SLAM系统，应用于如权利要求1-权利要求8任意一项所述的基于RIR倒谱提取一阶回波的声SLAM方法。
CN202111259582.1A 2021-10-28 2021-10-28 一种基于rir倒谱提取一阶回波的声slam系统及方法 Active CN114111785B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111259582.1A CN114111785B (zh) 2021-10-28 2021-10-28 一种基于rir倒谱提取一阶回波的声slam系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111259582.1A CN114111785B (zh) 2021-10-28 2021-10-28 一种基于rir倒谱提取一阶回波的声slam系统及方法

Publications (2)

Publication Number Publication Date
CN114111785A true CN114111785A (zh) 2022-03-01
CN114111785B CN114111785B (zh) 2024-03-01

Family

ID=80377242

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111259582.1A Active CN114111785B (zh) 2021-10-28 2021-10-28 一种基于rir倒谱提取一阶回波的声slam系统及方法

Country Status (1)

Country Link
CN (1) CN114111785B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170370710A1 (en) * 2016-06-24 2017-12-28 Syracuse University Motion sensor assisted room shape reconstruction and self-localization using first-order acoustic echoes
CN108828501A (zh) * 2018-04-29 2018-11-16 桂林电子科技大学 在室内声场环境中对移动声源进行实时跟踪定位的方法
CN109541535A (zh) * 2019-01-11 2019-03-29 浙江智澜科技有限公司 一种基于uwb和视觉slam的agv室内定位及导航的方法
CN109709520A (zh) * 2018-12-29 2019-05-03 桂林电子科技大学 基于麦克风阵列的室内定位与建图方法
CN109901112A (zh) * 2019-03-29 2019-06-18 桂林电子科技大学 基于多通道声获取的声学同时定位与建图方法
US20210116555A1 (en) * 2019-10-17 2021-04-22 Bang & Olufsen A/S Echo Based Room Estimation

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170370710A1 (en) * 2016-06-24 2017-12-28 Syracuse University Motion sensor assisted room shape reconstruction and self-localization using first-order acoustic echoes
CN108828501A (zh) * 2018-04-29 2018-11-16 桂林电子科技大学 在室内声场环境中对移动声源进行实时跟踪定位的方法
CN109709520A (zh) * 2018-12-29 2019-05-03 桂林电子科技大学 基于麦克风阵列的室内定位与建图方法
CN109541535A (zh) * 2019-01-11 2019-03-29 浙江智澜科技有限公司 一种基于uwb和视觉slam的agv室内定位及导航的方法
CN109901112A (zh) * 2019-03-29 2019-06-18 桂林电子科技大学 基于多通道声获取的声学同时定位与建图方法
US20210116555A1 (en) * 2019-10-17 2021-04-22 Bang & Olufsen A/S Echo Based Room Estimation

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
DOKMANIC IVAN ET AL.: "FROM ACOUSTIC ROOM RECONSTRUCTION TO SLAM", 《 IEEE》, pages 6345 - 6349 *
胡文超等: "音频感知哈希闭环检测的无人机仿生声呐SLAM算法研究", 《智能系统学报》, vol. 14, no. 2, pages 338 - 345 *
覃虹菱;王玫;宋浠瑜;周陬;罗丽燕;: "基于声学脉冲响应的室内建图算法研究", 声学技术, no. 02, pages 119 - 126 *

Also Published As

Publication number Publication date
CN114111785B (zh) 2024-03-01

Similar Documents

Publication Publication Date Title
Cobos et al. A survey of sound source localization methods in wireless acoustic sensor networks
May et al. Robust 3D-mapping with time-of-flight cameras
Liu et al. Accelerometer assisted robust wireless signal positioning based on a hidden Markov model
CN108828501B (zh) 在室内声场环境中对移动声源进行实时跟踪定位的方法
CN112230243A (zh) 一种移动机器人室内地图构建方法
US20230045798A1 (en) Method for predicting structure of indoor space using radio propagation channel analysis through deep learning
JP2015081831A (ja) 音源位置推定装置、移動体および移動体の制御方法
Vakulya et al. Fast adaptive acoustic localization for sensor networks
CN109901112A (zh) 基于多通道声获取的声学同时定位与建图方法
KR20180087519A (ko) 레이저 거리 센서의 측정 거리에 대해 추정된 거리 유형의 신뢰성을 평가하는 방법 및 이를 이용한 이동 로봇의 위치 추정 방법
Basiri et al. Audio-based relative positioning system for multiple micro air vehicle systems
He et al. WiFi iLocate: WiFi based indoor localization for smartphone
Batstone et al. Towards real-time time-of-arrival self-calibration using ultra-wideband anchors
CN109709520A (zh) 基于麦克风阵列的室内定位与建图方法
CN115767427A (zh) 一种基于众包机制的多径辅助蓝牙aoa室内定位方法
CN114111785B (zh) 一种基于rir倒谱提取一阶回波的声slam系统及方法
CN108459313A (zh) 基于光学微机电系统的激光雷达回波处理方法
Iscar Ruland et al. Low cost underwater acoustic localization
CN116772860A (zh) 基于无线定位技术与视觉人工智能融合的新型室内定位系统
CN116027266A (zh) Uwb与视觉紧耦合slam算法的定位方法及系统
Lin et al. Cooperative relative robot localization with audible acoustic sensing
Su et al. An acoustic sensor based novel method for 2D localization of a robot in a structured environment
JP7197003B2 (ja) 深度推定装置、深度推定方法、及び深度推定プログラム
CN113311386A (zh) 基于改进卡尔曼滤波器的tdoa无线定位方法
Walter et al. An accurate compact ultrasonic 3D sensor using broadband impulses requiring no initial calibration

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