基于UWB与视觉融合的紧耦合里程计方法及系统

本发明涉及一种基于uwb与视觉融合的紧耦合里程计方法及系统，属于自主定位。

背景技术：

1、四旋翼无人飞行器在近十几年来因为其体积小、可垂直起降与悬停、机动能力强、易于操控等优点，对于其相关技术的研究迅速发展。当下的实际生产生活中，四旋翼无人机在航拍、植物保护、未知环境探测、电力巡检与灾害监测等领域都得到了越来越广泛的应用。传统的无人机飞行时依赖于全球卫星定位系统(global positioning system，gps)等卫星定位信息，然而在诸如地下空间、从林、复杂楼宇环境等场景下gps信号不稳定或者消失，无人机在此类场景下的使用受到极大限制。因此无人机在gps拒止环境下进行自主定位与导航的问题，是目前无人机领域热门的研究课题。

2、实现无人机在gps拒止环境下进行自主定位与导航，需要无人机具备两个功能，一是在未知环境下根据自身传感器信息实时估计自身运动，二是构建周围环境地图，使得无人机能够感知障碍物与可用空域。实现了这两个功能，无人机才能结合控制算法、轨迹规划算法等其他必要算法实现自主定位导航。同步定位与建图(simultaneouslocalizationand mapping，slam)技术，指搭载了特定传感器的主体，在没有环境先验信息的情况下，在运动过程中构建环境，同时估计自身运动的技术。所以slam算法因为其运动估计功能也可以简称为里程计(odometry)。如果将slam技术应用到无人机平台上，可以满足上述无人机进行自主定位与导航需要具备的两个功能，因此基于四旋翼无人机的slam技术研究与应用逐渐受到广泛的关注。

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

4、uwb定位方法只需部署低成本的传感器即可完成在gps拒止环境下的位置估计，不受光照和天气的影响，更重要的这种方法具备全局参考系，为多无人机的直接应用提供了便利。但uwb传感器单独作用时的定位效果并不理想，本发明在基于uwb与imu融合的基础上，针对uwb定位方法轨迹估计不够平滑、容易受到电波干扰但存在全局参考系的特点，和针对视觉自主定位方法容易产生漂移现象但短时间内估计精度较高的特点，提出了基于uwb和视觉融合无人机定位方法。

技术实现思路

1、本发明的目的在于克服现有技术中的不足，本发明旨在提出一种基于uwb与视觉融合的紧耦合里程计方法及系统，提升无人机在gps拒止环境下的定位性能。

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

3、第一方面，本发明提供了一种基于uwb与视觉融合的紧耦合里程计方法，包括以下步骤：

4、通过无迹卡尔曼滤波器融合uwb、imu和定高雷达数据，估计得到无人机在全局系下位姿；

5、进行数据融合构成视觉惯性里程计，估计得到无人机在惯性系下的位姿；数据融合算法为基于滑动窗口的紧耦合非线性优化；

6、将所述无人机在全局系下位姿和无人机在全局系下位姿融合，得到无人机的高精度、无漂移的全局最优位姿。

7、进一步的，将所述无人机在全局系下位姿和无人机在全局系下位姿融合，得到无人机的高精度、无漂移的全局最优位姿，包括：

8、通过构建位姿优化图，以无人机在全局系下位姿作为待优化变量的角点，将视觉惯性里程计测量的惯性系下的位姿结果表示成位姿增量的形式，以二元增量边的形式在图中绘制，使用全局一元位姿边表示uwb定位测量结果，定义图优化问题的代价函数，通过计算代价函数的最小值，求出滑窗中的位姿状态矢量的最优估计，得到无人机的高精度、无漂移的全局最优位姿。

9、进一步的，进行数据融合构成视觉惯性里程计，估计得到无人机在惯性系下的位姿；数据融合算法为基于滑动窗口的紧耦合非线性优化，包括：

10、对imu进行预积分，imu预积分项将在后端优化时用作构建集束调整方程的残差项；

11、预处理视觉观测的最新帧，利用klt稀疏光流法跟踪现有特征，同时检测新的特征，筛选出关键帧，关键帧将被送入后端优化器做状态估计和深度恢复，将视觉测量残差定义在单位球面上，视觉测量残差即重投影残差；

12、用紧耦合融合的方法保证求解的精度，采用边缘化的思想将这些约束信息转换成状态先验条件加入到优化当中，通过构造集束调整方程做非线性最小二乘优化；

13、以无人机在全局系下位姿作为角点构建图，即图中的待优化变量是无人机全局位姿，包括一元边和二元边，基于图优化求解。

14、进一步的，对imu进行预积分的方式达到减少计算量，imu预积分项将在后端优化时用作构建集束调整方程的残差项，包括：

15、对于两帧图像之间的时间区间，存在着较多imu的测量值，为了避免重复传播，采用对imu进行预积分的方式：

16、对于滑动窗口中第k帧图像和其相邻的第k+1帧图像之间的imu测量值，其imu预积分的残差项可定义为：

17、

18、是两个连续帧间利用带有噪声的陀螺仪和加速度计测量值得到的imu预积分观测。表示四元数的三维误差状态，[·]xyz表示提取四元数q的虚部。加速度计和陀螺仪零偏也包括在在线修正的残差项中。

19、进一步的，预处理视觉观测的最新帧，利用klt稀疏光流法跟踪现有特征，同时需要检测新的特征，筛选出关键帧，关键帧将被送入后端优化器做状态估计和深度恢复，将视觉观测残差定义在单位球面上，视觉测量残差即重投影残差，包括：

20、当相机拍摄的最新帧到达之后，首先进行视觉观测的预处理，步骤包括使用klt稀疏光流法跟踪现有特征，为了确保特征点的数目维持在特定数量，在跟踪原有特征的基础上同时需要检测新的特征，另外该步骤还负责筛选出关键帧，关键帧将被送入后端优化器做状态估计和深度恢复，选择关键帧有两个准则：一是与前一关键帧的平均视差，如果最新帧与前一帧的平均视差大于某一阈值κ1，就将最新帧设置为新关键帧；二是特征跟踪的质量，如果特征跟踪数量低于某一阈值κ2，就将最新帧设置为新关键帧。

21、视觉测量残差即重投影残差，假设在第i帧图像时首次观测到了第l个特征点，它的像素坐标记为随着无人机的运动，特征点l在第j帧图像中被再次观测到，由此可定义视觉测量残差，这里将视觉测量残差定义在单位球面上，则视觉测量残差定义为：

22、

23、其中是在第i帧中首次观测到的第l个特征点的像素坐标，是该特征点在第j帧图像中的像素坐标，是包含相机内参的反投影函数。

24、由于视觉测量残差只有两个自由度，而残差矢量是个三维矢量，这里将残差矢量投影到正切平面上，从而保证正确的自由度。b1，b2是两个任意选择的正交基，它们能够张成的正切平面。

25、进一步的，以无人机在全局系下位姿作为角点构建图，即图中的待优化变量是无人机全局位姿，包括一元边和二元边，基于图优化求解，包括：

26、以无人机在全局系下位姿作为角点，即图中的待优化变量是无人机全局位姿。图中的边包括一元边和二元边。

27、采用se(3)的变换矩阵形式：

28、

29、其中，rk表示无人机在k时刻的姿态，pk表示无人机在k时刻的位置。

30、构建的图，以无人机在全局系下位姿作为角点，即图中的待优化变量是无人机全局位姿。图中的边包括一元边和二元边。

31、(3)增量边

32、由于视觉惯性里程计测量的是无人机在惯性系下的位姿而待优化变量是无人机在全局系下的位姿tk。为了摆脱参考系的限制，这里使用位姿增量的形式，表示无人机k时刻位姿与k-l时刻位姿之间的增量，表示在k-l时刻的机体系下。位姿增量可根据视觉惯性里程计的估计结果推导：

33、

34、因此图中的增量边连接两个时刻的位姿角点，表示两时刻的位姿的增量，增量边是二元边。

35、(4)全局位姿边

36、全局位姿边来自uwb定位的结果，表示无人机在某时刻的全局位姿测量，全局位姿边是一元边。

37、构建的图g＝{v,e}，k时刻的某一角点用增量边连接较早时刻的角点，黑色连线代表全局位姿边，全局位姿边是一元边。

38、现定义滑窗中的系统状态矢量为：

39、x＝[t0,t1,…,tn]  (5)

40、在滑窗内的某一时刻k，无人机获取到新的测量值后，定义图优化问题的代价函数如下：

41、

42、式(6)中t表示滑窗中全局位姿边的集合，表示全局位姿残差：

43、

44、s表示滑窗中所有增量边的集合，表示位姿增量残差：

45、

46、其中符号！代表se(3)上的广义减法，通过整理代价函数(6)中的每一项得到：

47、

48、使代价函数(9)最小化，可求出滑窗中的状态矢量的最优估计

49、

50、定义代价函数时在残差项外加入了鲁棒核函数(robust kernel)。使用huber核函数来抑制离群值的影响，表达式为：

51、

52、进一步的，通过无迹卡尔曼滤波器融合uwb、imu和定高雷达数据，估计得到无人机在全局系下位姿，包括以下步骤：

53、步骤1：获取uwb原始数据和imu数据；

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

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

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

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

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

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

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

61、将uwb距离测量与距离d真值建模为线性关系，即：

62、

63、其中nd为零均值白噪声。

64、采用线性回归法，通过将两个uwb模块摆放于不同的位置测距，得到数个真值-测量样本每个样本中的测量值为uwb距离的读数，距离真值由高精度的室内定位系统获得。经过大量样本拟合模型，确定当前模型下的尺度因子a与静差b。

65、

66、其中a为距离真值的均值，为距离测量的均值。

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

68、首先建立全局坐标系，从俯视角度看，该坐标系以锚点a1所在位置为坐标系原点，锚点a2与a1的连线为x轴所在方向，a1的高度方向为z轴，而y轴方向则可以根据右手定则确定。

69、将a3放置在近似y轴的方向上，a4可放置在任意位置。

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

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

72、锚点标定阶段的待估计量为标签在全局系g下的位置pt和锚点位置pai：

73、

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

75、

76、式(7)中c是锚点的集合，通过求解代价函数可标定出标签和每个锚点的位置。标签与机体系原点的偏移量被认为是已知的先验信息，则可推导出此时无人机在全局系下的位置p，这个值将作为状态估计器的迭代初值p0。

77、第二方面，本发明提供一种基于uwb与视觉融合的紧耦合里程计系统，包括：

78、uwb模块：用于通过无迹卡尔曼滤波器融合uwb、imu和定高雷达数据，估计得到无人机在全局系下位姿；

79、惯性系模块：用于进行数据融合构成视觉惯性里程计，估计得到无人机在惯性系下的位姿；数据融合算法为基于滑动窗口的紧耦合非线性优化；

80、位姿融合模块：用于将所述无人机在全局系下位姿和无人机在全局系下位姿融合，得到无人机的高精度、无漂移的全局最优位姿。

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

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

83、(1)本发明选取美国time domain公司生产的p440模块实现基于uwb与视觉融合的紧耦合里程计方法及系统，该方案定位精度高且成本较低，使用范围广泛。传统gnss拒止环境下自主导航方案所使用的雷达等传感器价格昂贵且稳定性无法得到保证。借助本方法的视觉惯性里程计单元，可以实现室内室外等多种场景的高精度自主定位与建图。

84、(2)本发明提出了基于uwb和视觉融合的定位算法，辅以imu预积分约束，使用基于因子图优化的方式实现无人机的高精度、稳定自主定位与建图技术。借助imu更新频率快的特性，以及双目相机提供的视觉约束，借此解决uwb定位出现小范围震荡较多的问题。本方法提高了定位精度且对纯视觉的定位中出现对环境特征敏感、容易发生估计漂移、缺少全局参考系的问题也有改善。

85、(3)本发明采用的位姿求解框架为因子图优化，该框架易于扩展，除了imu、uwb、双目相机和雷达高度计等传感器设备之外，还可根据开发者自行添加传感器设备，如激光雷达等，可进行二次开发。基于优化的方式相较于滤波更具有优势，通过紧耦合的方式达到提升里程计精度的效果。