基于超声波传感器阵列的无人机自主降落方法与流程

本发明属于无人机自主制导、导航与控制技术领域，涉及一种基于超声波传感器阵列的无人机自主降落方法。

背景技术：

自主降落是各种无人机自主飞行过程之中最难于解决的问题之一，其难点是对未知环境的降落目的地状况的感知、对合适降落场地的智能选取、和自主制导降落过程的实现，并确保系统功能有效以及精度和实时性要求。

目前垂直起降无人机自主降落方法未见到完善的解决方案，例如采用视频处理技术实现自主降落，而且降落点需要使用二维码作为图标引导无人机降落，这并不满足针对未知环境的自主智能降落的需求，而且因为图像处理的计算量要求，导致了降落过程的实时性与精度都不够好。

技术实现要素：

本发明的目的是提供一种基于超声波传感器阵列的无人机自主降落方法，相对于现有技术存在的实时性不够好以及精度不够高的问题。这种新体制的无人机自主降落方法实时性好、精度高、成本低。

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

一种基于超声波传感器阵列的无人机自主降落方法，包括以下步骤：

s100、在无人机上安装矩阵排布的超声波传感器阵列，并进行飞行试验；

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

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

s400、根据上述的导航算法和3d地图，采用在线或者离线的方式寻找满足要求的降落地址；

s500、根据降落地址，通过导航算法和3d地图导引无人机自主降落。

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

作为本发明的进一步改进，s200的具体步骤为：

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

di，j＝h-hi，j+vi，j

其中，h为飞行设定的高度，hi，j为地面的起伏高度，vi，j为传感器误差；

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

对于使得

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

当置信度参数ε为足够小的量时，保证和相似，即：

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

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

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

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

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

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

由于传感器得到的数据di，j＝h-hi，j+vi，j，其中h为飞行设定的高度，通常取为常值，因此由hi，j和vi，j的分布得知：

di，j～n(h，σr²+σg²)

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

令ci，j＝di，j-h，则：

ci，j～n(0，σr²+σg²)

同时，

则，

等价于

则此时求得ε，使

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

为了保证地形匹配的准确性与计算量的平衡，令依据经验取α置信水平；因此给定了n，σr²和σg²后积分计算，求得从而求得ε；

地形特征判断算法步骤：

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

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

上式分布特点如下：

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

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

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

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

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

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

此时得到的k1为前文中提到的与后排传感器的相似的数据向量位置，即为前排传感器数据n组数据向量中与最接近的数据向量；

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

此时后排、前排传感器分别获取的新一组测量数据和则数据量扩大到p+1组；下面寻找后排数据的第2组数据在前排传感器数据向量中的相似向量；在得到上述k1后，以k1+1为中心，重新分配前排传感器每组数据的权重；

计算相距k1+1位置m距离内的数据向量的相似度权重，m的取值预先估计一个式中的k，然后取m＝3k，得到的k2为：

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

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

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

根据ki的值，计算出k＝ki-i，代入式和式中即求得距离传感器每次探测间的飞行距离以及飞行速度。

作为本发明的进一步改进，s300的具体步骤为：

使用占据栅格图进行3d的地图表示，第三维坐标表示障碍物的高度；设定飞行器以匀速直线运动模式飞行，前排或者后排的传感器以一定的采样周期探测得到数据，由于阵列中的传感器每次探测采样周期相同，通过获取当前采样周期内无人机前排或者后排传感器阵列的移动距离，并根据该排每个传感器测到的距离构造出地形的3d地图。

作为本发明的进一步改进，s400中，在线的自主降落选址具体步骤为：

根据无人机降落对场地长度l和宽度w的要求，以及对降落场地所允许的最大起伏程度，在飞行过程中遍历所获得3d地图中的每一个点，并实时判断是否满足降落场地要求：

1)假设传感器阵列在当前时刻k之前获得的数据满足降落要求并作为地形判断的基准数据集，

2)当前时刻k获得的地形数据作为备选，并统计在k时刻加入基准数据集之后的均值μdk和方差σdk，根据接收到的新数据是否满足3σd≤hmax；

3)若不满足3σd≤hmax的要求，则舍弃1)的基准数据集，并基于k时刻收到的数据开始重新构造基准数据集，并从k+1时刻开设重复2)的过程判断k+1时刻的数据是否加入基准数据集；

4)若一直满足3σd≤hmax则k时刻数据加入1)基准数据集，并持续接收k+1时刻的数据并重复2)；

5)重复1)-4)步骤直到场地达到降落要求，则地址在线选取完成。

作为本发明的进一步改进，s400中，离线的自主降落选址具体步骤为：

无人机先按照预定的飞行模式飞行一定的距离，然后悬停；根据无人机降落的参数要求，包括降落场地长度l和宽度w要求以及降落场地的起伏的最大值，然后对遍历获取的3d地图中的每一个点，然后选取以该点为中心的能降落区域作为待判定区域，并统计该区域的均值μd和方差σd，判断数据是否满足3σd≤hmax，若不满足则舍弃当前的待判定区域，基于下一点的数据重新选取新的待判定区域；若一直不满足3σd≤hmax则继续遍历该点周围的数据，直到满足场地尺寸的要求，则地址离线选取完成。

作为本发明的进一步改进，s500中，导引无人机自主降落具体包括：

在得到降落的地点后，根据当前确定的无人机位置xn和降落地点选址位置xl之间的距离d＝|xn-xl|，飞行器以速度vc反向运动，此时设定飞行运动时间经过该tl时间后，控制飞行器停止运动，此时无人机悬停在选定的降落地点上方；无人机悬停在降落地点上方后开始降落，降落过程中无人机距离地面的高度通过超声波传感器阵列实时获取，采用分阶段降落的方法，先以较快的速度降落到距离地面接近的高度，然后控制无人机以缓慢的速度实现对地平稳降落，以保证无人机与搭载载荷的安全性，从而实现无人机的安全自主降落。

作为本发明的进一步改进，无人机自主降落过程中，还包括姿态稳定与控制步骤：

在无人机寻址飞行制导与降落制导的过程中，无人机机身位置改变，通过无人机的姿态稳定以确保平稳飞行，并通过姿态控制实现飞行器的运动控制。

与现有技术相比，本发明的有益效果为：

本发明针对垂直起降无人机设计了一种自主降落的新方法。首次使用超声波传感器阵列3d-slam技术实现无人机在未知环境中的快速构图、自主选址与精确降落制导，比现有激光雷达2d-slam技术获取的信息量大、计算量小、实时性好、成本低，与现有以及基于视觉的3d-slam技术相比，这种新途径的计算量小、实时性好，而且该技术满足垂直起降无人机的自主降落需求。该系统由基于超声传感器阵列对地形的探测、基于3d-slam的导航与构图、降落地点寻址与基于方案制导的自主降落所构成，得到用于制导的高精度位置速度信息以及降落地点，该发明不仅具有较好的自主导航精度与制导效果，通过上述模块的配合，实现了垂直起降无人机在未知环境中的自主降落功能。

附图说明

图1是本发明的无人机自主降落方法流程图。

图2是本发明的无人机自主降落方法控制原理图。

图3是本发明的传感器安装位置导航图，(a)2排传感器，(b)m排传感器；

图4是本发明使用的原型机图。

图5是本发明的实际地形图。

图6是本发明的模拟降落选址结果三维图。

图7是本发明的模拟降落选址平面图。

具体实施方式

现结合实施例、附图对本发明做进一步描述：

如图1和图2所示，本发明一种基于超声波传感器阵列的无人机自主降落方法，包括以下步骤：

步骤1、设计了基于超声波传感器阵列无人机自主降落的gnc系统总体方案，各功能模块的关系如图2所示，本发明涉及其中的制导与导航技术。

步骤2、超声波传感器阵列设计：本发明采用了阵列式传感器安装方案。在无人机的机体坐标系上安装m排每排n个的竖直朝下的超声波传感器阵列来获得未知地形的特征。m和n的取值根据垂直起降无人机的尺寸来决定，为便于表达，在本发明中取m＝2的情形说明发明所采用的方案和技术，如图3所示。

步骤3、自适应地形匹配的高精度导航算法：使得无人机进入降落程序以后，首先使得无人机以匀速直线方式飞行，根据超声波传感器阵列获取的数据，实时自适应地开展基于形匹配的高精度导航计算，得到无人机飞行的速度v与飞行距离l：

1)、建立运动平台上传感器阵列对飞行地形的观测模型：

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

di，j＝h-hi，j+vi，j

其中，h为飞行设定的高度，hi，j为地面的起伏高度，vi，j为传感器误差。

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

3)对于匀速直线运动的飞行器，前排的传感器会率先飞越过某个地形，随后经过k次测量周期后，后排的传感器会飞过相同的地形，因此前排和后排传感器数据中，会有相似的传感器数据向量，设第i次测量时前排得到的传感器数据向量为第i次后排得到的传感器数据向量为根据上述条件：

对于使得

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

此时的ε为某一足够小的量，以保证和相似，

由于前排和后排距离传感器的距离为l，在设定的飞行模式下，每次探测的采样周期t是固定的，l和t都是已知量，又因为垂直起降无人机在探测寻址与自主降落过程中设置为匀速运动模式，因此根据k得到每个探测周期t无人机质心的行进距离d如下：

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

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

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

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

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

由于传感器得到的数据di，j＝h-hi，j+vi，j，其中h为飞行设定的高度，通常取为常值，因此由hi，j和vi，j的分布得知：

di，j～n(h，σr²+σg²)

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

令ci，j＝di，j-h，则：

ci，j～n(0，σr²+σg²)

同时，

则，

等价于

则此时求得ε，使

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

为了保证地形匹配的准确性与计算量的平衡，令依据经验取α置信水平；因此给定了n，σr²和σg²后积分计算，求得从而求得ε。

地形特征判断算法步骤：

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

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

上式分布特点如下：

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

滤波优化实现高精度导航步骤：

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

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

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

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

此时得到的k1为前文中提到的与后排传感器的相似的数据向量位置，即为前排传感器数据n组数据向量中与最接近的数据向量；

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

此时后排、前排传感器分别获取的新一组测量数据和则数据量扩大到p+1组；下面寻找后排数据的第2组数据在前排传感器数据向量中的相似向量；在得到上述k1后，以k1+1为中心，重新分配前排传感器每组数据的权重；

计算相距k1+1位置m距离内的数据向量的相似度权重，m的取值预先估计一个式中的k，然后取m＝3k，得到的k2为：

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

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

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

根据ki的值，计算出k＝ki-i，代入式和式中即求得距离传感器每次探测间的飞行距离以及飞行速度。

通过滤波算法优化提升上述距离和速度导航估计的结果，实现高精度导航。

步骤4、建立3d地图：本发明使用占据栅格图进行3d的地图表示，第三维坐标表示障碍物的高度。设定飞行器以匀速直线运动模式飞行，前排的传感器以一定的采样周期探测得到数据，由于阵列中的传感器每次探测采样周期相同，因此只要知道当前采样周期内无人机前排传感器阵列的移动距离，就根据该排每个传感器测到的距离构造出地形的3d地图。

作为一种备选方案，后排传感器也用同样的方法建立3d地形图。无人机在匀速直线运动模式下飞行时前排和后排传感器阵列移动的距离信息与该无人机质心的运动相同，而质心的运动信息通过本发明中的导航算法得到。

步骤5、为了实现垂直起降无人机自主降落，采用在线或者离线的方式寻找满足要求的降落地址。

(1)在线的自主降落选址：根据无人机降落对场地长度l和宽度w的要求，以及对降落场地所允许的最大起伏程度hmax等，在飞行过程中遍历所获得3d地图中的每一个点，并实时判断是否满足降落场地要求：

1)假设传感器阵列在当前时刻k之前获得的数据满足降落要求并作为地形判断的基准数据集，

2)当前时刻k获得的地形数据作为备选，并统计在k时刻加入基准数据集之后的均值μdk和方差σdk，根据接收到的新数据是否满足3σd≤hmax；

3)若不满足3σd≤hmax的要求，则舍弃1)的基准数据集，并基于k时刻收到的数据开始重新构造基准数据集，并从k+1时刻开设重复2)的过程判断k+1时刻的数据是否加入基准数据集；

4)若一直满足3σd≤hmax则k时刻数据加入1)基准数据集，并持续接收k+1时刻的数据并重复2)；

5)重复1)-4)步骤直到场地达到降落要求，则地址在线选取完成。

(2)离线的自主降落选址：无人机先按照预定的飞行模式飞行一定的距离，然后悬停。根据无人机降落的参数要求，包括降落场地长度l和宽度w要求以及降落场地的起伏的最大值hmax等，然后对遍历获取的3d地图中的每一个点，然后选取以该点为中心的能降落区域作为待判定区域，并统计该区域的均值μd和方差σd，判断数据是否满足3σd≤hmax，若不满足则舍弃当前的待判定区域，基于下一点的数据重新选取新的待判定区域。若一直不满足3σd≤hmax则继续遍历该点周围的数据，直到满足场地尺寸的要求，则地址离线选取完成。

步骤6、基于方案导引的无人机自主降落制导方法：在得到降落的地点后，根据当前确定的无人机位置xn和降落地点选址位置xl之间的距离d＝|xn-xl|，飞行器以速度vc反向运动，此时设定飞行运动时间经过该tl时间后，控制飞行器停止运动，此时无人机悬停在选定的降落地点上方。无人机悬停在降落地点上方后开始降落，降落过程中无人机距离地面的高度通过超声波传感器阵列实时获取，采用分阶段降落的方法，先以较快的速度降落到距离地面接近的高度，然后控制无人机以缓慢的速度实现对地平稳降落，以保证无人机与搭载载荷的安全性，从而实现无人机的安全自主降落。

在无人机寻址飞行制导与降落制导的过程中，如果存在机身位置改变，则需要通过无人机的姿态稳定以确保平稳飞行，并通过姿态控制实现飞行器的运动控制。此处通过对飞行速度、飞行高度等控制以实现无人机的匀速直线飞行，也增加单独的高度控制传感器以实现其高度的平稳控制，因而高度控制属于选环节，取决于成本控制与无人机飞行控制技术路线的选择，如图2所示。

本发明的实施例如下所述：

基于超声波传感器阵列的垂直起降无人机自主降落使用的实验机如图4所示(圆圈内为传感器)，试验中旋翼机以4cm/s的速度飞越了图5中的实际地形，在matlab中对得到了传感器数据进行处理后得到了图6中的的三维地图。本发明中使用的六旋翼飞行器尺寸为40cm*40cm，降落场地的尺寸应该留有一定的冗余值，该冗余值依经验取旋翼机尺寸的30％，因此具体要求如下：

l≥50cm，w≥50cm

hmax≤10cm

其中，l，w分别为场地的长和宽，hmax为该场地内起伏的最大值。

对于地图数据，遍历地图中的每一个点，找到以其为中心的上述大小的区域，首先得到该区域数据的平均值μd和方差σd²，方差σd²满足3σd≤hmax≤10cm。经过计算整个地形的长宽，发现误差为9.7％，符合起降的要求。图7中的蓝色方框为系统自动选取的降落地址，看到，整体地面相较于其他的场地比较平坦，适于降落。无人机悬停在降落地点选址上方后通过内部程序逐渐降低螺旋桨转速实现自主降落。

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