基于UWB与IMU融合的鲁棒自主定位与建图方法、装置及介质

技术领域

本发明涉及一种基于UWB与IMU融合的鲁棒自主定位与建图方法、装置及介质，属于自主定位技术领域。

背景技术

无人系统近年来在各个领域的应用不断增多，如环境勘探、灾后搜救、自动驾驶等领域，这得益于无人系统在无人操控的场景下可以自主行动且可以完成特定的功能。无人机飞行通常依赖于全球导航卫星系统(Global Navigation Satellite System,GNSS)进行定位，这满足了大量室外无人机的定位需求。然而很多应用场景是复杂且未知的，经常出现GNSS信号不佳甚至丢失的情况，如室内、洞穴、丛林等环境，在这些环境下GNSS无法提供有效的定位信息，使无人机的应用受到极大限制。因此通过无人机自身传感器进行自主定位与导航的方法，是目前无人机领域热门的研究课题。

不依赖于外部传感器，通过无人机自身所携带的设备进行自主导航的技术被称为SLAM(Simultaneous LocalizationAndMapping)技术。其中，无人机的自主定位问题是一个状态估计问题，无人机的状态可以包含了无人机的位置、速度、姿态等状态。可靠的姿态估计可以保证四旋翼的稳定飞行，可靠的位置估计环境感知、障碍物规避、运动路径规划、无人机控制的重要保障。建图问题是指无人机对周围环境进行感知重建，是无人机的障碍物规避与运动规划的基础。可见，SLAM技术是环境勘探、抢险救灾等各种上层应用的核心技术之一。

超宽带(ultra-wideband，UWB)是近年来一种新颖的电波测距技术，超宽带技术通过测量电波的到达时间、到达时间差或到达角计算出两个模块之间的距离。由于发射电波的波段在3.1GHz-4.8GHz之间，能够有效克服其他电波信号的干扰。此外其较高的带宽能够轻易克服多径效应、减弱非视距测量的影响。

UWB定位方法只需部署低成本的传感器即可完成在GPS拒止环境下的位置估计，不受光照和天气的影响，更重要的这种方法具备全局参考系，为多无人机的直接应用提供了便利。但UWB传感器单独作用时的定位效果并不理想，首先是UWB信号容易受到干扰，在障碍物密集的非视距传播场景下定位精度有所下降，稳定性一般；其次因为采用常值速度假设导致UWB定位方法的精度不高；第三是位置估计更新速率受限于UWB测量频率，大约在20Hz左右，要保持无人机的稳定性，位置更新速率至少要100Hz以上，显然这并不满足控制器对状态估计的要求。

发明内容

本发明的目的在于克服现有技术中的不足，本发明旨在提出一种基于UWB与IMU融合的鲁棒自主定位与建图方法、装置及介质，提升无人机在GPS拒止环境下的定位性能。

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

第一方面，本发明提供了一种基于UWB与IMU融合的鲁棒自主定位与建图方法，包括以下步骤：

步骤1：获取UWB原始数据和IMU数据；

步骤2：对所述UWB原始数据进行距离校准，得到校准后的UWB数据；

步骤3：判断锚点位置是否已知，若否，根据所述校准后的UWB数据进行锚点标定后跳转步骤4，若是，跳转步骤4；

步骤4：对所述校准后的UWB数据进行离群值检测，得到筛选后的UWB数据；

步骤5：对所述IMU数据进行低通滤波，得到滤波后的IMU数据；

步骤6：将所述筛选后的UWB数据和滤波后的IMU数据输入状态估计器，得到无人机的位置、速度和姿态信息。所述状态估计器以无迹卡尔曼滤波为核心，对状态的概率分布进行近似，从而估计出均值和协方差。

所述状态估计器的输入是经信息校准和离群值剔除后的UWB测距数据和通过低通滤波处理噪声后的IMU数据；输出是无人机的位置、速度、姿态信息。

进一步的，步骤2：对所述UWB原始数据进行距离校准，包括：

将UWB距离测量

其中n

采用线性回归法，通过将两个UWB模块摆放于不同的位置测距，得到数个真值-测量样本

其中a为距离真值的均值，

进一步的，步骤3中，根据所述校准后的UWB数据进行锚点标定，包括：

首先建立全局坐标系，从俯视角度看，该坐标系以锚点A

将A

标定阶段令无人机保持静止，标签按照设定好的时隙表周期性的向锚点发出测距请求，根据TW-ToF原理解析出标签到每个锚点的距离。

开启UWB模块的回声测距功能，即标签会收到锚点与锚点之间的距离信息。锚点与锚点之间也要开启相互测距，距离测量值存储在锚点的缓存中。当标签向锚点发送测距请求后，锚点向标签回复时会将回声测距值加入响应信息中，这样标签就收到了两锚点间的距离测量。

锚点标定阶段的待估计量为标签在全局系G下的位置p

构建非线性最小二乘问题：

式(7)中C是锚点的集合，通过求解代价函数可标定出标签和每个锚点的位置。标签与机体系原点的偏移量

进一步的，步骤4：对所述校准后的UWB数据进行离群值检测，包括：

采用添加阈值方法检测离群值，一旦认为本次距离测量为离群值，将直接被剔除。

若p

则本次距离测量被判定为离群值。这里γ是可调阈值,v

进一步的，步骤5：对所述IMU数据进行低通滤波，包括：

通过截止频率约为30Hz的IIR数字低通滤波器对所述IMU数据进行低通滤波，通过低通滤波处理的IMU数据能够有效地降低噪声污染。

IIR低通滤波算法频域表示如式(4)所示：

其中，m和n，a

y(t)＝a

其中u代表IMU原生测量数据，y代表低通滤波处理结果，通过低通滤波后的数据作为IMU测量值再进行多传感器融合。

进一步的，步骤6：将所述筛选后的UWB数据和滤波后的IMU数据输入状态估计器，包括：

状态估计器以无迹卡尔曼滤波(UKF)为核心，状态估计器以无迹卡尔曼滤波为理论基础进行设计，系统方程和观测方程具有如下形式：

其中u

估计器主要对位置、速度和姿态进行估计，因此选择系统的状态矢量为：

其中p为无人机在全局系下的位置，v为速度，q为单位四元数，表示无人机的姿态，b

加入IMU数据后，系统方程可列写为如下形式：

这里IMU的读数作为系统方程的输入

当k＝0时启动状态估计器，首先对系统状态迭代初值

位置初值p

无人机起飞后的k时刻采集到IMU数据时，利用此数据对k+1时刻系统状态做预测。假设此时系统状态x

通过对系统状态应用UT变换得到Sigma点的集合矩阵：

其中，λ是标量参数，λ＝α

然后将Sigma点按照非线性的系统方程进行传播得到新的Sigma点：

其中

这里W

上述为估计器的预测步骤，当k+1时刻有UWB或定高雷达的观测量y

然后按照UT变换生成新的Sigma点集χ

由此，可计算卡尔曼增益K进行状态的更新，得到后验状态的均值

进一步的，本方法还包括：

获取定高雷达测量信号，通过融合定高雷达测量信号对系统状态中位置p的z轴分量进行修正，包括：

当读取雷达测量数据

式(24)即为投影方程，R

当获取到定高雷达测量

当

当

其中：(p

当|l

当

第二方面，本发明提供一种基于UWB与IMU融合的鲁棒自主定位与建图装置，所述装置包括：

输入模块：用于获取UWB原始数据和IMU数据；

校准模块：用于对所述UWB原始数据进行距离校准，得到校准后的UWB数据；

判断模块：用于判断锚点位置是否已知，若否，根据所述校准后的UWB数据进行锚点标定后跳转筛选模块，若是，跳转筛选模块；

筛选模块：用于对所述校准后的UWB数据进行离群值检测，得到筛选后的UWB数据；

滤波模块：用于对所述IMU数据进行低通滤波，得到滤波后的IMU数据；

估计模块：用于将所述筛选后的UWB数据和滤波后的IMU数据输入状态估计器，得到无人机的位置、速度和姿态信息。所述状态估计器以无迹卡尔曼滤波为核心，对状态的概率分布进行近似，从而估计出均值和协方差。

第三方面，本发明提供一种基于UWB与IMU融合的鲁棒自主定位与建图装置，包括处理器及存储介质；

所述存储介质用于存储指令；

所述处理器用于根据所述指令进行操作以执行根据第一方面所述方法的步骤。

第四方面，本发明提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现第一方面所述方法的步骤。

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

本发明对GNSS拒止环境下四旋翼无人机的定位与导航算法研究具有十分重要的意义。本发明稳定可靠，不受天气和光线等的影响，同时算法节省计算资源，对硬件要求较低，具有很高的理论与实用价值。本发明主要具有以下的特点和优点：

(1)本发明选取美国Time domain公司生产的P440模块实现GNSS拒止环境下无人机的自主定位与建图技术，该方案定位精度高且成本低廉，使用范围广泛。传统GNSS拒止环境下自主导航方案所使用的传感器价格昂贵且稳定性无法得到保证。借助本方法的锚点标定单元，可以实现室内室外等多种场景的自主定位与建图。

(2)本发明提出了基于UWB和IMU融合的定位算法，辅以定高雷达的冗余约束，使用无迹卡尔曼滤波实现无人机的鲁棒自主定位与建图技术。借助IMU更新频率快的特性，与UWB融合减轻基于UWB定位出现定位轨迹出现锯齿的现象，达到轨迹平滑化的效果。本方法提高了定位精度且针对无人机的高度进行进一步约束，有效改善高度方向估计精度不足的问题。

(3)本发明采用的位姿求解框架为无迹卡尔曼滤波，该框架易于扩展，除了IMU、UWB和雷达高度计等传感器设备之外，还可根据开发者自行添加传感器设备，如激光雷达、双目相机等，可进行二次开发。

附图说明

附图1为基于UWB与IMU融合的鲁棒自主定位与建图方法、装置及介质流程图；

附图2为基于UWB与IMU融合的鲁棒自主定位与建图方法、装置及介质系统硬件架构图；

附图3为基于UWB与IMU融合的鲁棒自主定位与建图方法、装置及介质系统算法流程图；

附图4为锚点标定示意图；

附图5为无迹卡尔曼滤波流程图；

附图6为俯仰、横滚不为零时的定高雷达工作情况；

附图7为地形估计几种情况；

附图8为UWB与IMU融合室内轨迹跟踪效果；

附图9为UWB与IMU融合定位绝对位置误差(APE)曲线；

附图10为UWB融合IMU定位与UWB定位误差箱型图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

实施例一：

本发明的目的在于克服现有技术中的不足，本发明旨在提出解决在GNSS拒止环境下基于UWB与IMU融合的鲁棒自主定位与建图方法、装置及介质。GNSS拒止环境下四旋翼无人机位姿估计平台如图1所示。Karol Hausman等人曾在2016年提出了一个以扩展卡尔曼滤波(Extended Kalman Filter,EKF)为核心的UWB与IMU融合定位框架。EKF通过对系统进行线性化近似舍弃了二阶以上的高阶项，导致针对强非线性系统的状态估计精度不足。四旋翼无人机具备强非线性的特点，为了解决强非线性条件下的估计问题，同时更便捷的进行传感器拓展，本方法设计了一套以无迹卡尔曼滤波(Unscented Kalman Filter，UKF)为核心的定位框架，使得UWB和惯性传感器在功能上可以实现互相补充。在定位过程中，IMU可以配合UWB提高定位精度，在UWB测量短暂出现离群值时仍可以表现出较好的定位性能。本定位算法框架具有较强的通用性，可拓展其他传感器，如框架中还融合了定高雷达测量，一是为了提高z轴方向的估计精度，二是针对无人机平稳飞行设计多传感器冗余策略。通过设计四旋翼无人机平台并运行该定位算法，实现无人机快速精确定位，具有极大的应用价值。本发明采取的技术方案包括硬件和软件两部分，硬件包括：包括UWB测距模块，IMU惯性测量传感器、雷达高度计、嵌入式机载处理器和无人机平台。软件包括：IMU低通滤波单元、距离校准单元、锚点标定单元、卡尔曼滤波单元、离群值检测单元。整个算法流程见附图3。下面针对各部分做具体说明。

硬件部分：

无人机为轴距380mm的四旋翼飞行器，配备DJI E300电机电调和4S高压电池。传感器搭载了3DM-GX5-25惯性测量单元(IMU)，P440 UWB测距模块，TF mini激光测距模块(定高雷达)，姿态控制由Pixhawk飞行控制器完成，机载计算机采用Intel第八代NUC(处理器为Core i7-8559U 2.7GHz，四核八线程，内存大小32G)。定位程序是在Linux操作系统下基于ROS(Robot Operating System)机器人开源框架进行开发，所有算法均采用C++语言实现，定位程序采用ROS Message实时接收传感器采集的数据，程序解析出的位置和姿态使用Mavlink协议经由串口发送到Pixhawk飞行控制器做进一步姿态控制。

考虑到系统的可靠性，UWB模块需要有较高的测距精度和较鲁棒的抗多径干扰能力，本平台选择搭载美国Time domain公司生产的P440模块。P440模块是一种波段在3.1GHz到4.8GHz之间的超宽带无线收发器，它主要采用TW-ToF原理，可同时执行四种功能(测距、数据传输、单基地雷达和多基地雷达)，这里仅使用测距功能，测距包括单点测距和组网测距模式，实验中标签和锚点组成测距网络，组网测距可以采用ALOHA(随机)协议或者TDMA(时分多址)协议。此UWB模块的电波发射功率极小(～50uW)，标签使用机载电池经电压转换后供电，锚点使用移动电源直接供电。

软件部分：

(1)数据预处理单元

本部分包括对UWB原始数据的测距信息校准、离群值剔除以及IMU噪声处理三部分。理后的IMU数据能够有效地降低噪声污染。其处理后的结果作为输入送入卡尔曼滤波单元。

本方法在测量值送入卡尔曼滤波器之前加入了距离校准环节，经由距离校准之后可认为是更贴近真值的测量值。每当采集到UWB传感器测量数据时，默认通过距离校准环节进行线性回归处理，推算出更合理的距离值。这里采用添加阈值方法检测离群值，一旦认为本次距离测量为离群值，将直接被剔除。对于IMU而言，根据IMU测量噪声主要来源是机体的高频震动，本方法设计了截止频率约为30Hz的IIR数字低通滤波器，通过低通滤波处理的IMU数据能够有效地降低噪声污染。

(2)UWB锚点实时标定单元

针对室内室外等多场景通用性的需求，本方法设计了一个实时锚点标定单元。由于地面锚点在布置之前相对于无人机的位置是未知的，通过人工的方法去测量可以得到相对位置但却耗时耗力。本方法采用非线性最小二乘理论设计了锚点标定算法，在创建全局系的同时标定出环境四周锚点的位置，锚点标定的结果被送入状态估计器保存起来，以便后续的算法顺利进行。

(3)无迹卡尔曼滤波单元

本方法设计的状态估计器以无迹卡尔曼滤波(UKF)为核心，UKF不同于EKF那样对系统进行线性近似，而是对状态的概率分布进行近似，其核心思想是在状态传播前对状态进行采样，将采样后的点集(称为Sigma点)按照非线性的系统方程和观测方程进行状态传播，然后对传播后的Sigma点按照高斯分布去估计其分布函数，从而估计出均值和协方差。尽管还是在高斯分布的假设下，但避免了EKF线性化过程带来的误差，同时通过增加采样点使传播后的状态的分布更接近真实分布。UKF与EKF几乎具有相同的时间复杂度，但是UKF的估计精度更高，在实现上也比EKF更为简单。

(4)多传感器冗余单元

定高雷达的测量原理类似于UWB，雷达的激光发射器向地面发射一束激光，传感器根据发射时间与接收时间计算到地面的直线距离。当地面始终水平时，定高雷达的测量通过投影方程后可直接送入估计器进行状态更新，而当地形发生变化时(如地面某处凹凸不平)，定高雷达测量结果还蕴含了地形信息。为此，本方法提出了针对地形估计的多传感器冗余策略，当地形发生变化时，无人机可估计地形并适当改变飞行高度，以免出现意外情况。地形估计器的原理如下：

当获取到定高雷达测量时，根据当前姿态将测量值投影到全局系的z轴上。

本发明对GNSS拒止环境下四旋翼无人机的定位与导航算法研究具有十分重要的意义。本发明稳定可靠，不受天气和光线等的影响，同时算法节省计算资源，对硬件要求较低，具有很高的理论与实用价值。本发明主要具有以下的特点和优点：

(1)本发明选取美国Time domain公司生产的P440模块实现GNSS拒止环境下无人机的自主定位与建图技术，该方案定位精度高且成本低廉，使用范围广泛。传统GNSS拒止环境下自主导航方案所使用的传感器价格昂贵且稳定性无法得到保证。借助本方法的锚点标定单元，可以实现室内室外等多种场景的自主定位与建图。

(2)本发明提出了基于UWB和IMU融合的定位算法，辅以定高雷达的冗余约束，使用无迹卡尔曼滤波实现无人机的鲁棒自主定位与建图技术。借助IMU更新频率快的特性，与UWB融合减轻基于UWB定位出现定位轨迹出现锯齿的现象，达到轨迹平滑化的效果。本方法提高了定位精度且针对无人机的高度进行进一步约束，有效改善高度方向估计精度不足的问题。

(3)本发明采用的位姿求解框架为无迹卡尔曼滤波，该框架易于扩展，除了IMU、UWB和雷达高度计等传感器设备之外，还可根据开发者自行添加传感器设备，如激光雷达、双目相机等，可进行二次开发。

下面结合附图对本发明的GNSS拒止环境下四旋翼无人机定位算法做进一步描述。

硬件部分：

无人机为轴距380mm的四旋翼飞行器，配备DJI E300电机电调和4S高压电池。传感器搭载了3DM-GX5-25惯性测量单元(IMU)，P440 UWB测距模块，TF mini激光测距模块(定高雷达)，姿态控制由Pixhawk飞行控制器完成，机载计算机采用Intel第八代NUC(处理器为Core i7-8559U 2.7GHz，四核八线程，内存大小32G)。定位程序是在Linux操作系统下基于ROS(Robot Operating System)机器人开源框架进行开发，所有算法均采用C++语言实现，定位程序采用ROS Message实时接收传感器采集的数据，程序解析出的位置和姿态使用Mavlink协议经由串口发送到Pixhawk飞行控制器做进一步姿态控制。

考虑到系统的可靠性，UWB模块需要有较高的测距精度和较鲁棒的抗多径干扰能力，本平台选择搭载美国Time domain公司生产的P440模块。P440模块是一种波段在3.1GHz到4.8GHz之间的超宽带无线收发器，它主要采用TW-ToF原理，可同时执行四种功能(测距、数据传输、单基地雷达和多基地雷达)，这里仅使用测距功能，测距包括单点测距和组网测距模式，实验中标签和锚点组成测距网络，组网测距可以采用ALOHA(随机)协议或者TDMA(时分多址)协议。此UWB模块的电波发射功率极小(～50uW)，标签使用机载电池经电压转换后供电，锚点使用移动电源直接供电。

软件部分：

(1)数据预处理单元

本部分包括对UWB原始数据的测距信息校准、离群值剔除以及IMU噪声处理三部分。处理后的IMU数据能够有效地降低噪声污染。其处理后的结果作为输入送入卡尔曼滤波单元。

理想情况下UWB距离测量的精度能够达到厘米级，但实际上测量值常常因为某种原因带有静差和误差。为了尽可能减少影响定位精度的因素，本方法在测量值送入卡尔曼滤波器之前加入了距离校准环节，经由距离校准之后可认为是更贴近真值的测量值。

将UWB距离测量

其中n

其中a为距离真值的均值，

每当采集到UWB传感器测量数据时，默认通过距离校准环节进行线性回归处理，推算出更合理的距离值。

虽然UWB具备的优良特性能最大限度的避免多径效应、非视距的影响，但在障碍物密集的场景下仍有可能引入离群值，离群值指明显有悖于事实的测量，如读数出现负数，突然发生很大跳变等情况，离群值的引入会导致错误的状态估计。这里采用添加阈值方法检测离群值，一旦认为本次距离测量为离群值，将直接被剔除。

若p

则本次距离测量被判定为离群值。这里γ是可调阈值,v

对于IMU而言，由于其采用捷联方式直接固定在机体上，无人机在飞行过程中受到螺旋桨震动的影响，导致测量数据受到较大的噪声污染。根据IMU测量噪声主要来源是机体的高频震动，本发明设计了截止频率约为30Hz的IIR数字低通滤波器，通过低通滤波处理的IMU数据能够有效地降低噪声污染。IIR低通滤波算法频域表示如式(4)所示：

其中，m和n，a

y(t)＝a

其中u代表IMU原生测量数据，y代表低通滤波处理结果，通过低通滤波后的数据作为IMU测量值再进行多传感器融合。

(2)UWB锚点在线标定单元

本节采用非线性最小二乘理论设计了锚点标定算法，在创建全局系的同时标定出环境四周锚点的位置，锚点标定的结果被送入状态估计器保存起来，以便后续的算法顺利进行。锚点标定环节在整个流程中仅被执行一次。

锚点标定示意图如附图3所示。首先建立全局坐标系，从俯视角度看，该坐标系以锚点A

锚点标定阶段的待估计量为标签在全局系G下的位置p

根据图4，可构建非线性最小二乘问题：

式(7)中C是锚点的集合，通过求解代价函数可标定出标签和每个锚点的位置。标签与机体系原点的偏移量

(3)无迹卡尔曼滤波单元

卡尔曼(R.E.Kalman)在1960年首次提出卡尔曼滤波(Kalman Filter,KF)，KF是一种基于模型线性最小方差估计，其标准形式具有递推计算等优点。但是KF只适用于系统方程和观测方程均为线性的情况，即系统是线性高斯的。在线性高斯系统中使用KF可将状态与噪声分别进行传播然后构成状态传播后的线性高斯系统，从而完成递推估计，这个结论在非线性系统时却不能成立。扩展卡尔曼滤波(EKF)通过对方程做线性近似可解决系统和观测为非线性时的估计问题，但是线性化过程舍弃了二阶以上的高阶项，导致针对强非线性系统的状态估计精度不足。

本节设计的状态估计器以无迹卡尔曼滤波(UKF)为核心，状态估计器以无迹卡尔曼滤波为理论基础进行设计，其基本流程如附图5所示。UKF不同于EKF那样对系统进行线性近似，而是对状态的概率分布进行近似，其核心思想是在状态传播前对状态进行采样，将采样后的点集(称为Sigma点)按照非线性的系统方程和观测方程进行状态传播，然后对传播后的Sigma点按照高斯分布去估计其分布函数，从而估计出均值和协方差。尽管还是在高斯分布的假设下，但避免了EKF线性化过程带来的误差，同时通过增加采样点使传播后的状态的分布更接近真实分布。UKF与EKF几乎具有相同的时间复杂度，但是UKF的估计精度更高，在实现上也比EKF更为简单。

无迹卡尔曼滤波是基于模型状态估计方法，因此在进行传感器融合和状态估计之前需要确定系统方程和观测方程，选择估计器的系统状态矢量。一般来说系统方程和观测方程具有如下形式：

其中u

估计器主要对无人机位置、速度和姿态等进行估计，因此选择系统的状态矢量为：

其中p为无人机在全局系下的位置，v为速度，q为单位四元数，表示无人机的姿态，b

加入了IMU传感器后，系统方程可列写为如下形式：

这里IMU的读数作为系统方程的输入

当k＝0时启动状态估计器，首先对系统状态迭代初值

位置初值p

无人机起飞后的k时刻采集到IMU数据时，利用此数据对k+1时刻系统状态做预测。假设此时系统状态x

通过对系统状态应用UT变换得到Sigma点的集合矩阵：

其中，λ是标量参数，λ＝α

然后将Sigma点按照非线性的系统方程进行传播得到新的Sigma点，而不是对系统进行线性化近似：

其中

这里W

上述为估计器的预测步骤，当k+1时刻有UWB或定高雷达的观测量y

然后按照UT变换生成新的Sigma点集χ

由此，可计算卡尔曼增益K进行状态的更新，得到后验状态的均值

/>

多传感器融合主要是指UWB测距模块和IMU惯性测量传感器融合，还加入雷达高度计辅助测量z轴方向的高度，使无人机状态估计更加准确，步骤六介绍了融合的过程。

UWB定位方法只需部署低成本的传感器即可完成在GPS拒止环境下的位置估计，不受光照和天气的影响，更重要的这种方法具备全局参考系，为多无人机的直接应用提供了便利。但UWB传感器单独作用时的定位效果并不理想，首先是UWB信号容易受到干扰，在障碍物密集的非视距传播场景下定位精度有所下降，稳定性一般；其次因为采用常值速度假设导致UWB定位方法的精度不高；第三是位置估计更新速率受限于UWB测量频率，大约在20Hz左右，要保持无人机的稳定性，位置更新速率至少要100Hz以上，显然这并不满足控制器对状态估计的要求。IMU可以高频测量机体的三轴的加速度和角速度，恰好可以弥补UWB单独作用时的缺陷。因此可以将UWB与IMU测量进行融合，从而提升无人机在GPS拒止环境下的定位性能。

(4)多传感器冗余单元

本方法通过融合定高雷达用来提升z轴估计精度，传感器安装在机架背面，当机体俯仰角、横滚角不为零时，定高雷达工作情况如附图6所示。

为了使用定高雷达对系统状态中位置p的z轴分量进行修正，在将定高雷达测量值送入估计器更新前需要对雷达测量数据进行一步处理，当读取雷达测量数据

式(24)即为投影方程，R

定高雷达的测量原理类似于UWB，雷达的激光发射器向地面发射一束激光，传感器根据发射时间与接收时间计算到地面的直线距离。当地面始终水平时，定高雷达的测量通过投影方程后可直接送入估计器进行状态更新，而当地形发生变化时(如地面某处凹凸不平)，定高雷达测量结果还蕴含了地形信息。为此，本节提出了针对地形估计的多传感器冗余策略，当地形发生变化时，无人机可估计地形并适当改变飞行高度，以免出现意外情况。地形估计器的原理如下：

当获取到定高雷达测量

当

当

当

下面给出实验结果

本案例对上述定位系统进行了多组基于多传感器融合的GNSS拒止环境下基于UWB与IMU融合的鲁棒自主定位实验。

附图8为GNSS拒止环境下室内轨迹跟踪效果，该实验的场景为室内，起点与终点为同一地点。根据结果可以看出基于本方法的轨迹相较于仅使用UWB进行定位得到的轨迹更加平滑，且与真值更加接近。

附图9和10分别为GNSS拒止环境下绝对位置误差(APE)曲线和UWB融合IMU定位与UWB定位误差箱型图，该曲线和箱型图定量的分析了无人机自主定位的误差，由分析可知，本方法不论是绝对误差的最大值和平均值相较于基于UWB的定位都有改进。

实施例二：

本实施例提供一种基于UWB与IMU融合的鲁棒自主定位与建图装置，所述装置包括：

输入模块：用于获取UWB原始数据和IMU数据；

校准模块：用于对所述UWB原始数据进行距离校准，得到校准后的UWB数据；

判断模块：用于判断锚点位置是否已知，若否，根据所述校准后的UWB数据进行锚点标定后跳转筛选模块，若是，跳转筛选模块；

筛选模块：用于对所述校准后的UWB数据进行离群值检测，得到筛选后的UWB数据；

滤波模块：用于对所述IMU数据进行低通滤波，得到滤波后的IMU数据；

估计模块：用于将所述筛选后的UWB数据和滤波后的IMU数据输入状态估计器，得到无人机的位置、速度和姿态信息。所述状态估计器以无迹卡尔曼滤波为核心，对状态的概率分布进行近似，从而估计出均值和协方差。

本实施例的装置可用于实现实施例一所述的方法。

实施例三：

本实施例提供一种基于UWB与IMU融合的鲁棒自主定位与建图装置，包括处理器及存储介质；

所述存储介质用于存储指令；

所述处理器用于根据所述指令进行操作以执行根据实施例一所述方法的步骤。

实施例四：

本实施例提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现实施例一所述方法的步骤。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。