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

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

【技术实现步骤摘要】
一种基于超声波传感器阵列的自主建图与导航定位方法
本专利技术属于制导与导航领域，涉及一种基于超声波传感器阵列的自主建图与导航定位方法。技术背景由于SLAM技术实现未知区域相对导航的独特优势，使得该技术受到科研与工程领域的重视，在近年来有关无人系统技术的研究中受到高度关注。目前，2D-SLAM算法已较为成熟，但3D-SLAM算法面临实时性不足等问题，这也是该技术走向应用面临的瓶颈性问题。2D-SLAM技术中，HectorSLAM利用现有的已经获得的地图对激光传感器获得的数据进行优化，并得到物体在地图中的位置和相应的概率。该算法利用高斯牛顿方法进行扫描的数据与地图数据的匹配，从而找到激光点集映射到已有地图的刚体变换，进而对地图进行更新。Gmapping算法是目前应用最广的SLAM算法，该算法使用的是RBPF，即Rao-Blackwellized粒子滤波器。该算法通过大量的粒子来预测物体在空间中的位置，并且通过相关性将这些粒子赋予不同的权重，从而得到最终确定的位置，并更新地图。Cartographer是谷歌提出的实时室内建图项目，该算法在获得每一次的激光扫描得到的数据后，通过扫描匹配(本文档来自技高网...

【技术保护点】
1.一种基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，包括以下步骤：S100，在运动平台上安装矩阵式排布的超声波传感器阵列；S200，建立运动平台上传感器阵列对飞行地形的观测模型；S300，通过传感器阵列进行数据采集，并对传感器阵列测量信号预处理；S400，根据采集得到的数据进行计算，得到自适应地形匹配的导航算法；S500，根据采集得到的数据进行计算，建立飞行地形对应的3D地图。

【技术特征摘要】
1.一种基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，包括以下步骤：S100，在运动平台上安装矩阵式排布的超声波传感器阵列；S200，建立运动平台上传感器阵列对飞行地形的观测模型；S300，通过传感器阵列进行数据采集，并对传感器阵列测量信号预处理；S400，根据采集得到的数据进行计算，得到自适应地形匹配的导航算法；S500，根据采集得到的数据进行计算，建立飞行地形对应的3D地图。2.根据权利要求1所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，步骤S100中，所述的超声波传感器阵列包括M排，每排间距排布N个传感器，M≥2，N≥2，每排传感器安装在运动平台的体坐标系上，传感器的数据采集端口均垂直地面设置。3.根据权利要求1所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，步骤S200的具体步骤如下：根据Lindeberg-Fellercentral中心极限定理，地面的起伏高度应满足h～N(0,σg2)，传感器误差应该满足ν～N(0,σr2)，其中的σg2和σr2分别代表地面起伏情况的方差以及传感器误差的方差；Di,j表示在第i次探测时第j个传感器得到的数据，则观测模型为：Di,j＝H-hi,j+νi,j其中，H为飞行设定的高度，hi,j为地面的起伏高度，νi,j为传感器误差。4.根据权利要求1所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，步骤S300的具体步骤如下：对测量得到的数据进行去异常值处理和去噪处理，使用箱型图进行异常值去除，并通过高斯卷积滤波器进行噪声平滑处理。5.根据权利要求1所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，步骤S300的具体步骤如下：选取两排前后传感器阵列，对于匀速直线运动的运动平台，前排的距离传感器会率先飞越过某个地形，随后经过k次测量周期后，后排的距离传感器会随后飞过同一地形，前排和后排距离传感器数据中，有相似的传感器数据向量，设第i次测量前排得到的传感器数据向量为第i次后排得到的传感器数据向量为根据上述条件：对于使得其中，为第i次测量前排得到的传感器数据向量，为第i+k次后排得到的传感器数据向量，ε为置信度参数；当置信度参数ε为足够小的量时，保证和相似，即：由于前排和后排距离传感器的距离为L，在设定的飞行模式下，每次探测的采样周期T是固定的，L和T都是已知量，因此根据k得到每个探测周期T的行进距离d如下：其中，L为前排和后排距离传感器的距离，k为测量周期次数；根据上式，由运动平台采样的次数获得其运动距离，飞行的速度v根据上式得到：其中，T为每次探测的采样周期，d为每个探测周期的行进距离；即由速度累加得到从对地面探测时间开始到任何时刻运动平台导航所需的相对位置、速度信息。6.根据权利要求5所述的基于超声波传感器阵列的自主建图与导航定位方法，其特征在于，置信度参数ε的计算步骤如下：由于传感器得到的数据Di,j＝H-hi,j+νi,j，其中H为飞行设定的高度，通常取为常值，因此由hi,j和νi,j的分布得知：Di,j～N(H,σr2+σg2)其中，H为飞行设定的高度，σr2为传感器误差的方差，σg2为地面起伏高度的方差...

【专利技术属性】
技术研发人员：梁勇奇，徐闽鋆，韩凌峰，
申请(专利权)人：西安交通大学，
类型：发明
国别省市：陕西,61