【技术实现步骤摘要】
本专利技术涉及一种基于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是两个任意选择的正交基，它们能够张成的正切平面。
本文档来自技高网...【技术保护点】
1.一种基于UWB与视觉融合的紧耦合里程计方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的基于UWB与视觉融合的紧耦合里程计方法，其特征在于，将所述无人机在全局系下位姿和无人机在全局系下位姿融合，得到无人机的高精度、无漂移的全局最优位姿，包括：
3.根据权利要求1所述的基于UWB与视觉融合的紧耦合里程计方法，其特征在于，进行数据融合构成视觉惯性里程计，估计得到无人机在惯性系下的位姿；数据融合算法为基于滑动窗口的紧耦合非线性优化，包括：
4.根据权利要求3所述的基于UWB与视觉融合的紧耦合里程计方法，其特征在于，对IMU进行预积分的方式达到减少计算量，IMU预积分项将在后端优化时用作构建集束调整方程的残差项，包括：
5.根据权利要求4所述的基于UWB与视觉融合的紧耦合里程计方法，其特征在于，预处理视觉观测的最新帧，利用KLT稀疏光流法跟踪现有特征，同时需要检测新的特征，筛选出关键帧，关键帧将被送入后端优化器做状态估计和深度恢复，将视觉观测残差定义在单位球面上，视觉测量残差即重投影残差，包括：
6.根据权利要求5所
7.根据权利要求1所述的基于UWB与视觉融合的紧耦合里程计方法，其特征在于，通过无迹卡尔曼滤波器融合UWB、IMU和定高雷达数据，估计得到无人机在全局系下位姿，包括以下步骤：
8.根据权利要求7所述的基于UWB与视觉融合的紧耦合里程计方法，其特征在于，步骤2：对所述UWB原始数据进行距离校准，包括：
9.根据权利要求7所述的基于UWB与视觉融合的紧耦合里程计方法，其特征在于，步骤3中，根据所述校准后的UWB数据进行锚点标定，包括：
10.一种基于UWB与视觉融合的紧耦合里程计系统，其特征在于，包括：
...【技术特征摘要】
1.一种基于uwb与视觉融合的紧耦合里程计方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的基于uwb与视觉融合的紧耦合里程计方法，其特征在于，将所述无人机在全局系下位姿和无人机在全局系下位姿融合，得到无人机的高精度、无漂移的全局最优位姿，包括：
3.根据权利要求1所述的基于uwb与视觉融合的紧耦合里程计方法，其特征在于，进行数据融合构成视觉惯性里程计，估计得到无人机在惯性系下的位姿；数据融合算法为基于滑动窗口的紧耦合非线性优化，包括：
4.根据权利要求3所述的基于uwb与视觉融合的紧耦合里程计方法，其特征在于，对imu进行预积分的方式达到减少计算量，imu预积分项将在后端优化时用作构建集束调整方程的残差项，包括：
5.根据权利要求4所述的基于uwb与视觉融合的紧耦合里程计方法，其特征在于，预处理视觉观测的最新帧，利用klt稀疏光流法跟踪现有特征，同时需要检测新的特征，筛选出关键帧，关键帧将被...
【专利技术属性】
技术研发人员：何雷，王聪，马文峰，田辉，
申请(专利权)人：中国人民解放军陆军工程大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。