CN109556598B - 一种基于超声波传感器阵列的自主建图与导航定位方法 - Google Patents

Abstract

本发明公开了一种基于超声波传感器阵列的自主建图与导航定位方法，包括：S100，在运动平台上安装矩阵式排布的超声波传感器阵列；S200，建立运动平台上传感器阵列对飞行地形的观测模型；S300，通过传感器阵列进行数据采集，并对传感器阵列测量信号预处理；S400，根据采集得到的数据进行计算，得到自适应地形匹配的导航算法；S500，根据采集得到的数据进行计算，建立飞行地形对应的3D地图。该方法基于超声波传感器阵列在运动平台上实现了3D‑SLAM技术，设计了超声波传感器阵列获取运动平台下方的地形信息并建立3D地图，同时实现基于自适应地形匹配的相对导航定位算法，通过算法设计获得高精度的导航定位与速度信息。该发明的精度高、成本低、实时性好。

Description

一种基于超声波传感器阵列的自主建图与导航定位方法

技术领域

本发明属于制导与导航领域，涉及一种基于超声波传感器阵列的自主建图与导航定位方法。

技术背景

由于SLAM技术实现未知区域相对导航的独特优势，使得该技术受到科研与工程领域的重视，在近年来有关无人系统技术的研究中受到高度关注。目前，2D-SLAM算法已较为成熟，但3D-SLAM算法面临实时性不足等问题，这也是该技术走向应用面临的瓶颈性问题。

2D-SLAM技术中，Hector SLAM利用现有的已经获得的地图对激光传感器获得的数据进行优化，并得到物体在地图中的位置和相应的概率。该算法利用高斯牛顿方法进行扫描的数据与地图数据的匹配，从而找到激光点集映射到已有地图的刚体变换，进而对地图进行更新。Gmapping算法是目前应用最广的SLAM算法，该算法使用的是RBPF，即Rao-Blackwellized粒子滤波器。该算法通过大量的粒子来预测物体在空间中的位置，并且通过相关性将这些粒子赋予不同的权重，从而得到最终确定的位置，并更新地图。Cartographer是谷歌提出的实时室内建图项目，该算法在获得每一次的激光扫描得到的数据后，通过扫描匹配(Scan match)在地图中的最佳位置插入一个子地图，相当于进行了一次局部的定位，最后在所有扫描完成后，再基于各个子图的特征和每次扫描的定位得到最后的全局地图。

目前的3D-SLAM算法通常为vSLAM，即使用视觉深度传感器或者双目相机代替2D-SLAM中的激光距离传感器对目标区域进行探测，通过姿态转换算法得到相对位置并进行建图。该种算法通过检测连续两幅拍摄的图象之中的相似点来判断在这两次拍摄之间姿态和位置的改变。在累加过程中，误差也有可能跟着累加，因此这种算法产生了许多变种以减少误差，比较著名的就是通过加装里程计或者惯性传感器获得自身的姿态和位置变化，并通过扩展卡尔曼滤波减少误差，例如Davison等提出单目摄像头定位算法。近些年，由于扩展卡尔曼滤波复杂性以及计算量会随着运行时间的增加而增加，新的算法源源不断地提出以克服此困难，例如R Kümmerle等人提出的基于图优化的SLAM算法。这些算法显著减少了vSLAM算法由于噪声累计带来的误差，将3D-SLAM提上了实用化的程度。但是以上的算法仍需要强大的计算能力支持，由于无人机搭载的设备有限，一般仅能搭载配置要求较低的单片机等，因此在无人机上应用实时的SLAM较为困难。

发明内容

为了避免现有方法中建图精度不高和实时性不好的不足之处，本发明提供了一种基于超声波传感器阵列的自主建图与导航定位方法，通过采用超声波阵列的3D-SLAM技术，不仅可靠性高、成本低，同时由于计算量小，使得该发明实时性更好，该发明是实现无人机实时构图与定位的新体制解决方案。

为实现上述目的，本发明采用的技术方案如下：

一种基于超声波传感器阵列的自主建图与导航定位方法，包括以下步骤：

S100，在运动平台上安装矩阵式排布的超声波传感器阵列；

S200，建立运动平台上传感器阵列对飞行地形的观测模型；

S300，通过传感器阵列进行数据采集，并对传感器阵列测量信号预处理；

S400，根据采集得到的数据进行计算，得到自适应地形匹配的导航算法；

S500，根据采集得到的数据进行计算，建立飞行地形对应的3D地图。

作为本发明的进一步改进，步骤S100中，所述的超声波传感器阵列包括M排，每排间距排布N个传感器，M≥2，N≥2，每排传感器安装在运动平台的体坐标系上，传感器的数据采集端口均垂直地面设置。

作为本发明的进一步改进，步骤S200的具体步骤如下：

根据Lindeberg-Fellercentral中心极限定理，地面的起伏高度应满足h～N(0,σ_g ²)，传感器误差应该满足ν～N(0,σ_r ²)，其中的σ_g ²和σ_r ²分别代表地面起伏情况的方差以及传感器误差的方差；D_i,j表示在第i次探测时第j个传感器得到的数据，则观测模型为：

D_i,j＝H-h_i,j+ν_i,j

其中，H为飞行设定的高度，h_i,j为地面的起伏高度，ν_i,j为传感器误差。

作为本发明的进一步改进，步骤S300的具体步骤如下：

对测量得到的数据进行去异常值处理和去噪处理，使用箱型图进行异常值去除，并通过高斯卷积滤波器进行噪声平滑处理。

作为本发明的进一步改进，步骤S300的具体步骤如下：

选取两排前后传感器阵列，对于匀速直线运动的运动平台，前排的距离传感器会率先飞越过某个地形，随后经过k次测量周期后，后排的距离传感器会随后飞过同一地形，前排和后排距离传感器数据中，有相似的传感器数据向量，设第i次测量前排得到的传感器数据向量为

第i次后排得到的传感器数据向量为

根据上述条件：

对于

使得

其中，

为第i次测量前排得到的传感器数据向量，

为第i+k次后排得到的传感器数据向量，ε为置信度参数；

当置信度参数ε为足够小的量时，保证

和

相似，即：

由于前排和后排距离传感器的距离为L，在设定的飞行模式下，每次探测的采样周期T是固定的，L和T都是已知量，因此根据k得到每个探测周期T的行进距离d如下：

其中，L为前排和后排距离传感器的距离，k为测量周期次数；

根据上式，由运动平台采样的次数获得其运动距离，飞行的速度v根据上式得到：

其中，T为每次探测的采样周期，d为每个探测周期的行进距离；

即由速度累加得到从对地面探测时间开始到任何时刻运动平台导航所需的相对位置、速度信息。

作为本发明的进一步改进，置信度参数ε的计算步骤如下：

由于传感器得到的数据D_i,j＝H-h_i,j+ν_i,j，其中H为飞行设定的高度，通常取为常值，因此由h_i,j和ν_i,j的分布得知：

D_i,j～N(H,σ_r ²+σ_g ²)

其中，H为飞行设定的高度，σ_r ²为传感器误差的方差，σ_g ²为地面起伏高度的方差；

令C_i,j＝D_i,j-H，则：

C_i,j～N(0,σ_r ²+σ_g ²)

同时，

则，

等价于

则此时求得ε，使

由于：n个标准正态分布的平方和符合自由度为n的χ²分布，即：

为了保证地形匹配的准确性与计算量的平衡，令

依据经验取α置信水平；因此给定了n,σ_r ²和σ_g ²后积分计算，求得

从而求得ε。

作为本发明的进一步改进，S400中，还包括地形特征判断算法步骤：

对于将要选取的i，若相邻的两组测量数据满足：

其中的E为表示地形特征变化的阈值，下面确定E的具体数值；

上式分布特点如下：

同样，令

的值取为0.05以保证运动平台运动过程中地形匹配的置信度_ε；同上，通过数据计算得到

从而得到E的取值。

作为本发明的进一步改进，S400中，还包括通过滤波优化实现高精度导航步骤：

首先，让运动平台试飞行以实现算法的初始化：

在算法初始化阶段，选取后排传感器测得的第1组有地形特征的数据，在前排传感器已经获得的n组数据中通过均布粒子播撒累加相似度的方式找到近似于贝叶斯后验概率确定的相似数据向量；后排第i组数据向量和前排第j组数据向量相似度函数S_i,j为：

均布粒子后，给予前排传感器数据的p组数据的每组以相同的权重，累加并且归一化后得到的对应于后排传感器第1组数据的前排传感器数据中的相似向量位置k₁为：

其中，S_1,j为后排第1组数据向量和前排第j组数据向量相似度函数；

此时得到的k₁为前文中提到的与后排传感器的

相似的数据向量位置，即

为前排传感器数据n组数据向量中与

最接近的数据向量；

然后，算法初始化结束，则开始算法迭代：

此时后排、前排传感器分别获取的新一组测量数据

和

则数据量扩大到p+1组；下面寻找后排数据的第2组数据在前排传感器数据向量中的相似向量；在得到上述k₁后，以k₁+1为中心，重新分配前排传感器每组数据的权重；

计算相距k₁+1位置m距离内的数据向量的相似度权重，m的取值预先估计一个式

中的k，然后取m＝3k，得到的k₂为：

其中，k₁为对应于后排传感器第1组数据的前排传感器数据中的相似向量位置，G(x)为正态分布概率密度函数：

上式中的σ²的取值根据正态分布的特点，即正态分布的大部分数据分布于(μ-3σ，μ+3σ)这个区间内，取

据此，得到的k₂视为算法根据前一轮计算的结果加入高斯分布的粒子滤波迭代的结果，以此类推获得第p+i组数据

和

后，相应的k_i为：

根据k_i的值，计算出k＝k_i-i，代入式

和式

中即求得距离传感器每次探测间的飞行距离以及飞行速度。

作为本发明的进一步改进，步骤S300的具体步骤如下：

使用占据栅格图进行3D的地图表示，第三维坐标表示障碍物的高度，在运动平台建图时，认为该运动平台以匀速直线模式运动，前排的传感器以采样周期T探测得到数据，由于阵列中的传感器每次探测采样周期T相同，先得到当前采样周期内运动平台前排或后排传感器阵列的移动距离，再根据该排每个传感器测到的距离构造出地形的3D地图。

作为本发明的进一步改进，所述的运动平台为垂直起降无人机。

与现有技术相比，本发明具有以下有益效果：

针对以垂直起降无人机为代表的运动平台对于实时构图与相对导航的需求，提出基于超声波阵列的3D-SLAM技术，这种途径不仅可靠性高、成本低，同时由于计算量小，使得该发明实时性更好。基于超声波传感器阵列在运动平台上实现了3D-SLAM技术，设计了超声波传感器阵列获取运动平台下方的地形信息并建立3D地图，同时实现基于自适应地形匹配的相对导航定位算法，通过算法设计获得高精度的导航定位与速度信息。该发明的精度高、成本低、实时性好。

附图说明

图1：本发明一种基于超声波传感器阵列的自主建图与导航定位方法流程图；

图2：为本发明采用的传感器阵列安装位置示意图；

图3：为本发明得到的仿真地图；

图4：为传感器原始数据示意图；

图5：为处理后的数据示意图；

图6：为计算得到的速度示意图；

图7：为相对误差示意图；

图8：为实际地形示意图；

图9：为实际地形的三维地图。

具体实施方式

以下结合附图和实施例对本发明的特征及其它相关特征作进一步详细说明，以便于同行业技术人员的理解。

如图1所示，本发明一种基于超声波传感器阵列的自主建图与导航定位方法，包括以下步骤：

S100，在无人机上安装矩阵式排布超声波传感器阵列。

本发明传感器安装方式采用了矩阵式排布。具体为：安装M排传感器，每排N个传感器。本发明中选定M＝2，N的数量根据垂直起降无人机大小决定。两排传感器分为前后排安装在垂直起降无人机的体坐标系上，传感器的面对方向均垂直向下指向地心，如图2所示。将超声波距离传感器以一排N个的数量并排安装，同一排间每个的间距为相同值，并在垂直起降无人机前端和后端各安装一排。所有的传感器均面(数据采集端)朝下布置，以便于采集所有传感器位置到其竖直下方的第一个障碍物的数据(距离等)。

S200，建立无人机上传感器阵列对飞行地形的观测模型：

由于自然环境下地面的起伏高度以及传感器误差受到大量相互独立且均匀小的随机因素影响，根据Lindeberg-Fellercentral中心极限定理，地面的起伏高度应满足h～N(0,σ_g ²)，传感器误差应该满足ν～N(0,σ_r ²)，其中的σ_g ²和σ_r ²分别代表地面起伏情况的方差以及传感器误差的方差。根据以上假设，以前排传感器得到的距离数据为例，D_i,j表示在第i次探测时第j个传感器得到的数据，则观测模型为：

D_i,j＝H-h_i,j+ν_i,j

其中，H为飞行设定的高度，h_i,j为地面的起伏高度，ν_i,j为传感器误差。

S300，无人机飞行过程中进行数据采集，并对传感器阵列测量信号预处理。

通过图2传感器对地面垂直扫描，得到距离反射面的信息。由于传感器存在偶尔的测量数据异常和一直存在的测量噪声，需要对测量得到的数据进行去异常值处理和去噪处理。本发明中使用箱型图进行异常值去除，并通过高斯卷积滤波器进行噪声平滑处理。传感器原始数据如图4所示，经过处理后的传感器数据如图5所示，看出整个地图高的地方很明显的与低洼的地方区别开，野值也得以去除。

S400，根据采集得到的数据进行计算，得到自适应地形匹配的导航算法。

由于本发明中的系统选取了两排前后传感器阵列。对于匀速直线运动的运动平台，前排的距离传感器会率先飞越过某个地形，随后经过k次测量周期后，后排的距离传感器会随后飞过同一地形，因此前排和后排距离传感器数据中，会有相似的传感器数据向量，设第i次测量前排得到的传感器数据向量为

第i次后排得到的传感器数据向量为

根据上述条件：

对于

使得

其中，

为第i次测量前排得到的传感器数据向量，

为第i+k次后排得到的传感器数据向量，ε为置信度参数。

此时的置信度参数ε为某一足够小的量，以保证

和

相似，

由于前排和后排距离传感器的距离为L，在设定的飞行模式下，每次探测的采样周期T是固定的，L和T都是已知量，又因为垂直起降无人机在进行SLAM时设置为匀速运动的，因此根据k得到每个探测周期T的行进距离d如下：

其中，L为前排和后排距离传感器的距离，k为测量周期次数。

根据上式，由运动平台采样的次数可获得其运动距离，飞行的速度v也根据上式得到：

其中，T为每次探测的采样周期，d为每个探测周期的行进距离。

有了以上信息，即可由速度累加得到从对地面探测时间开始到任何时刻运动平台导航所需的相对位置、速度信息。

还包括，传感器阵列地形相似判断步骤：基于上述导航算法，需要确定置信度参数ε。

由于传感器得到的数据D_i,j＝H-h_i,j+ν_i,j，其中H为飞行设定的高度，通常取为常值，因此由h_i,j和ν_i,j的分布得知：

D_i,j～N(H,σ_r ²+σ_g ²)

其中，H为飞行设定的高度，σ_r ²为传感器误差的方差，σ_g ²为地面起伏高度的方差。

令C_i,j＝D_i,j-H，则：

C_i,j～N(0,σ_r ²+σ_g ²)

同时，

则，

等价于

则此时求得ε，使

对于每个i对尽可能小的k成立。

由于：n个标准正态分布的平方和符合自由度为n的χ²分布，即：

对于一次SLAM，地形匹配判断常涉及到较多的数据，为了保证地形匹配的准确性与计算量的平衡，令

可依据经验取C置信水平。因此给定了n,σ_r ²和σ_g ²后积分计算上式，求得

从而求得ε。

S500，根据采集得到的数据进行计算，建立飞行地形对应的3D地图。

本发明使用占据栅格图进行3D的地图表示，第三维坐标表示障碍物的高度。在运动平台SLAM建图时，认为该运动平台以匀速直线模式运动。前排的传感器以一定的采样周期T探测得到数据，由于阵列中的传感器每次探测采样周期T相同，因此只要知道当前采样周期内运动平台前排传感器阵列的移动距离，就根据该排每个传感器测到的距离构造出地形的3D地图，在匀速直线模式下运动平台前排、后排传感器阵列移动的距离信息与该运动平台质心的运动相同，而质心的运动信息(速度v、距离L)通过本发明中的导航计算得到。通过计算得到的仿真地图如图3所示。

和前排传感器阵列的使用相类似，也采取基于后排传感器阵列建立无人机飞行地形的3D地图。

优选的，S400中，为了减少导航算法的计算量，进行地形特征判断算法步骤。

为了降低计算量，超声波阵列SLAM的过程中一部分“很有特征的”数据向量被用来获得飞行的导航定位信息。为了选择“很有特征的”的数据向量，本发明采用基于比较同一组距离传感器相邻数据向量差值来选取用于地形特征比较的数据向量。

对于将要选取的i，若相邻的两组测量数据满足：

其中的E为表示地形特征变化的阈值，要求在整个SLAM过程中有一定的特征突变发生。下面确定E的具体数值。

上式分布特点如下：

同样，例如令

的值取为0.05以保证SLAM过程中地形匹配的置信度_ε。同上，通过数据计算得到

从而得到E的取值。

优选的，S400中，还包括通过滤波优化实现高精度导航步骤：

首先，运动平台将飞行一小段时间以实现算法的初始化。为了不失一般性，下面的数学推导将使用p作为数据量的表示。该步骤能够提升导航精度，减少误差。

在算法初始化阶段，选取后排传感器测得的第1组有地形特征的数据，在前排传感器已经获得的n组数据中通过均布粒子播撒累加相似度的方式找到近似于贝叶斯后验概率确定的相似数据向量。后排第i组数据向量和前排第j组数据向量相似度函数S_i,j为：

均布粒子后，给予前排传感器数据的p组数据的每组以相同的权重，累加并且归一化后得到的对应于后排传感器第1组数据的前排传感器数据中的相似向量位置k₁为：

其中，S_1,j为后排第1组数据向量和前排第j组数据向量相似度函数。

此时得到的k₁为前文中提到的与后排传感器的

相似的数据向量位置，即

为前排传感器数据n组数据向量中与

最接近的数据向量。

算法初始化结束，则开始SLAM算法迭代，下面每接收一组测量数据，都要重新分配粒子密度以迭代增加定位精度。

此时后排、前排传感器分别获取的新一组测量数据

和

则数据量扩大到p+1组。下面寻找后排数据的第2组数据在前排传感器数据向量中的相似向量。在得到上述k₁后，以k₁+1为中心，重新分配前排传感器每组数据的权重，按正态分布靠近k₁+1位置的数据分配更多的权重，远离k₁+1位置的数据分配较少的权重。为了保证数据的精度，此时仅计算相距k₁+1位置m距离内的数据向量的相似度权重，m的取值预先估计一个式

中的k，然后取m＝3k。这样得到的k₂为：

其中，k₁为对应于后排传感器第1组数据的前排传感器数据中的相似向量位置，G(x)为正态分布概率密度函数

上式中的σ²的取值根据正态分布的特点，即正态分布的大部分数据分布于(μ-3σ，μ+3σ)这个区间内，取

据此，得到的k₂视为算法根据前一轮计算的结果加入高斯分布的粒子滤波迭代的结果，以此类推可获得第p+i组数据

和

后，相应的k_i为：

根据k_i的值，可计算出k＝k_i-i，代入式

和式

中即可求得距离传感器每次探测间的飞行距离以及飞行速度，每经过一次迭代，精度便提高一次。

实施例

基于本发明一种基于超声波传感器阵列的自主建图与导航定位方法的实施例如下所述：

首先使用上文中提到的模拟地图的建立方法在MATLAB中建立张模拟地图，在这些地图上模拟垂直起降无人机以4cm/s的速度飞越这些地图上空获得传感器数据，再依靠这些传感器数据依照上文提到的算法计算垂直起降无人机的速度，得到的结果如图6和图7所示。其中，红色线条为未经粒子滤波优化得到的仿真结果，蓝色为经过粒子滤波优化的仿真结果，由仿真的结果看出，未应用粒子滤波优化后该算法相对误差均值为6％左右，应用了粒子滤波器后能有效的降低误差，使相对误差降低至2％，满足实验精度。

本发明还开展了算法的实验验证，手持着运动平台飞越了图8中的实际地形，得到了传感器数据后经过去野值和平滑处理后，并应用上述算法后得到了图9中的三维地图。从图8和图9两幅图的对比看出，图9较好地还原图8的实际地形特征，说明该方法是应用到实际中的。

尽管以上结合附图对本发明的具体实施方案进行了描述，但本发明并不局限于上述的具体实施方案，上述的具体实施方案仅仅是示意性的、指导性的、而不是限制性的。本领域的普通技术人员在本说明书的启示下，在不脱离本发明的权利要求所保护的范围的情况下，还做出很多种的形式，这些均属于本发明保护之列。

Claims (7)

1.一种基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，包括以下步骤：

S100，在运动平台上安装矩阵式排布的超声波传感器阵列；

S200，建立运动平台上传感器阵列对飞行地形的观测模型；

S300，通过传感器阵列进行数据采集，并对传感器阵列测量信号预处理；

S400，根据采集得到的数据进行计算，得到自适应地形匹配的导航算法；

S500，根据采集得到的数据进行计算，建立飞行地形对应的3D地图；

S400中，还包括地形特征判断算法步骤：

对于将要选取的i，若相邻的两组测量数据满足：

其中，

为第i次测量前排得到的传感器数据向量，E为表示地形特征变化的阈值，下面确定E的具体数值；

上式分布特点如下：

其中，σ_r ²为传感器误差的方差，σ_g ²为地面起伏高度的方差；

同样，令

的值取为0.05；同上，通过数据计算得到

从而得到E的取值；

S400中，还包括通过滤波优化实现高精度导航步骤：

首先，让运动平台试飞行以实现算法的初始化：

在算法初始化阶段，选取后排传感器测得的第1组有地形特征的数据，在前排传感器已经获得的n组数据中通过均布粒子播撒累加相似度的方式找到近似于贝叶斯后验概率确定的相似数据向量；后排第i组数据向量和前排第j组数据向量相似度函数S_i，j为：

均布粒子后，给予前排传感器数据的p组数据的每组以相同的权重，累加并且归一化后得到的对应于后排传感器第1组数据的前排传感器数据中的相似向量位置k₁为：

其中，S_1，j为后排第1组数据向量和前排第j组数据向量相似度函数；

此时得到的k₁为与后排传感器的

相似的数据向量位置，即

为前排传感器数据n组数据向量中与

最接近的数据向量；

然后，算法初始化结束，则开始算法迭代：

此时后排、前排传感器分别获取的新一组测量数据

和

则数据量扩大到p+1组；下面寻找后排数据的第2组数据在前排传感器数据向量中的相似向量；在得到上述k₁后，以k₁+1为中心，重新分配前排传感器每组数据的权重；

计算相距k₁+1位置m距离内的数据向量的相似度权重，m的取值预先估计一个式

中的k，k为采样周期次数；然后取m＝3k，得到的k₂为：

其中，G(x)为正态分布概率密度函数：

上式中的σ²的取值根据正态分布的特点，即正态分布的大部分数据分布于(μ-3σ，μ+3σ)这个区间内，取

据此，得到的k₂视为算法根据前一轮计算的结果加入高斯分布的粒子滤波迭代的结果，以此类推获得第p+i组数据

和

后，相应的k_i为：

根据k_i的值，计算出k＝k_i-i，代入式

和式

中，即求得距离传感器每次探测间的飞行距离以及飞行速度。

2.根据权利要求1所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，步骤S300的具体步骤如下：

选取两排前后传感器阵列，对于匀速直线运动的运动平台，前排的距离传感器会率先飞越过某个地形，随后经过k次测量周期后，后排的距离传感器会随后飞过同一地形，前排和后排距离传感器数据中，有相似的传感器数据向量，设第i次测量前排得到的传感器数据向量为

第i次后排得到的传感器数据向量为

根据条件：

对于

使得

其中，

为第i次测量前排得到的传感器数据向量，

为第i+k次后排得到的传感器数据向量，ε为置信度参数；

当置信度参数ε为足够小的量时，保证

和

相似，即：

由于前排和后排距离传感器的距离为L，在设定的飞行模式下，每次探测的采样周期T是固定的，L和T都是已知量，因此根据k得到每个采样周期T的行进距离d如下：

根据上式，由运动平台采样的次数获得其运动距离，飞行的速度v根据上式得到：

即由速度累加得到从对地面探测时间开始到任何时刻运动平台导航所需的相对位置、速度信息；

置信度参数ε的计算步骤如下：

由于传感器得到的数据D_i，j＝H-h_i，j+v_i，j，其中h_i，j为地面的起伏高度，v_i，j为传感器误差，H为飞行设定的高度，取为常值；因此由h_i，j和v_i，j的分布得知：

D_i，j～N(H，σ_r ²+σ_g ²)

σ_r ²为传感器误差的方差，σ_g ²为地面起伏高度的方差；

令C_i，j＝D_i，j-H，则：

C_i，j～N(0，σ_r ²+σ_g ²)

同时，

则，

等价于

则此时求得ε，使

由于：n个标准正态分布的平方和符合自由度为n’的χ²分布，即：

为了保证地形匹配的准确性与计算量的平衡，令

依据经验取α置信水平；给定n，σ_r ²和σ_g ²后积分计算，求得

从而求得ε。

3.根据权利要求1所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，步骤S100中，所述的超声波传感器阵列包括M排，每排间距排布N个传感器，M≥2，N≥2，每排传感器安装在运动平台的体坐标系上，传感器的数据采集端口均垂直地面设置。

4.根据权利要求1所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，步骤S200的具体步骤如下：

根据Lindeberg-Fellercentral中心极限定理，地面的起伏高度满足h_i，j～N(0，σ_g ²)，传感器误差该满足v_i，j～N(0，σ_r ²)；D_i，j表示在第i次探测时第j个传感器得到的数据，则观测模型为：

D_i，j＝H-h_i，j+v_i，j

其中，H为飞行设定的高度，h_i，j为地面的起伏高度，v_i，j为传感器误差。

5.根据权利要求1所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，步骤S300的具体步骤如下：

对测量得到的数据进行去异常值处理和去噪处理，使用箱型图进行异常值去除，并通过高斯卷积滤波器进行噪声平滑处理。

6.根据权利要求1所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，步骤S300的具体步骤如下：

使用占据栅格图进行3D的地图表示，第三维坐标表示障碍物的高度，在运动平台建图时，认为该运动平台以匀速直线模式运动，前排的传感器以采样周期T探测得到数据，由于阵列中的传感器每次探测采样周期T相同，先得到当前采样周期内运动平台前排或后排传感器阵列的移动距离，再根据该排每个传感器测到的距离构造出地形的3D地图。

7.根据权利要求1所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，所述的运动平台为垂直起降无人机。

Images