一种融合UWB测距信息的单目视觉车辆定位系统及其方法技术方案

本发明专利技术涉及一种融合UWB测距信息的单目视觉车辆定位系统及其方法，该系统包括：量测预处理模块用于对单目相机原始图像进行去畸变和光度校正处理、根据UWB标签测距信息插值获取同步后距离量测；单目视觉里程计模块用于增量式估计相机位姿、构建两种视觉相对位姿量测；非视距识别与剔除模块用于对同步后距离量测进行非视距量测剔除；初始化模块用于估计初始全局航向角和视觉里程计初始尺度；因子图优化模块则结合单目视觉里程计模块、非视距识别与剔除模块、初始化模块的输出数据，以估计得到车辆全局位姿信息。与现有技术相比，本发明专利技术能同时解决单目视觉里程计存在的问题，并大大降低所需UWB基站数量，能够在各种复杂工况下实时准确估计车辆全局位姿。实时准确估计车辆全局位姿。实时准确估计车辆全局位姿。

【技术实现步骤摘要】
一种融合UWB测距信息的单目视觉车辆定位系统及其方法


[0001]本专利技术涉及无人驾驶
，尤其是涉及一种融合UWB测距信息的单目视觉车辆定位系统及其方法。

技术介绍

[0002]随着汽车智能化、电动化的推进，无人驾驶已经成为未来汽车发展的主流趋势之一。车辆定位是无人驾驶领域的关键技术，其作为下游算法例如路径规划、轨迹跟踪等的输入，将直接影响无人驾驶车辆行驶任务完成情况。在当前众多的定位方法之中，以来自单目相机时序图像作为输入的单目视觉里程计定位方式，凭借其简单灵巧、成本低、功耗低的特点而受到了广泛关注。
[0003]然而，想要将单目视觉里程计实际运用于车辆定位，仍需解决以下几个难题：
[0004]由于无法直接从单目图像中获取物体的实际深度信息，单目视觉里程计只能确定相机的运动方向和相对距离，即存在尺度不可观问题；
[0005]随着车辆行驶和场景变换，单目尺度不可观问题将导致单目视觉里程计前后尺度不一致，即尺度漂移问题；
[0006]单目视觉里程计是通过不断计算相邻帧间的相对位姿来实现持续定位，这种增量式计算原理将导致定位误差随着车辆行驶里程而不断累积，并且单目视觉里程计估计的位姿是相对于第一帧的位姿，而非车辆在某个全局坐标系下的位姿，也就是说，单目视觉里程计仅仅能估计出相对位姿，而无法获知全局位姿。
[0007]此外，现有技术考虑到UWB(Ultra
‑
Wideband)测距技术是一种基于无线电波的短距离测距技术，可以实现高精度的距离测量，目前该技术广泛应用于室内定位、无人驾驶、智能家居等领域，具有低功耗、高可靠性等优点，因此可将UWB测距技术应用于无人驾驶车辆定位，但在实际应用中，由于遮挡导致的非视距效应和多径效应，将严重降低UWB测距精度、无法在复杂工况下准确估计车辆全局位姿，而为了在有大量遮挡的复杂环境下能够有效使用UWB定位，往往需要依据实际场景精心设计UWB基站的布置方案，才能保证不同区域内有足够数量的视距基站并按区域切换选用基站，这必然会大大增加UWB定位技术的实际使用成本。

技术实现思路

[0008]本专利技术的目的就是为了克服上述现有技术存在的缺陷而提供一种融合UWB测距信息的单目视觉车辆定位系统及其方法，充分结合单目视觉定位信息和UWB测距信息，能同时解决单目视觉里程计存在的问题，并大大降低所需UWB基站数量，能够在各种复杂工况下实时准确估计车辆全局位姿。
[0009]本专利技术的目的可以通过以下技术方案来实现：一种融合UWB测距信息的单目视觉车辆定位系统，包括量测预处理模块、单目视觉里程计模块、初始化模块、非视距识别与剔除模块、因子图优化模块，所述量测预处理模块分别与单目视觉里程计模块、非视距识别与
剔除模块、初始化模块相连接，所述单目视觉里程计模块分别与初始化模块、非视距识别与剔除模块相连接，所述单目视觉里程计模块、非视距识别与剔除模块、初始化模块分别与因子图优化模块相连接；
[0010]所述量测预处理模块用于对单目相机采集的原始图像进行去畸变和光度校正处理，以及根据UWB标签测距信息插值获取同步后距离量测；
[0011]所述单目视觉里程计模块根据去畸变和光度校正后的单目相机图像，用于增量式估计相机位姿、构建两种视觉相对位姿量测；
[0012]所述非视距识别与剔除模块用于对同步后距离量测进行筛选，以剔除非视距量测；
[0013]所述初始化模块根据同步后距离量测以及相机位姿估计结果，用于估计初始全局航向角和视觉里程计初始尺度；
[0014]所述因子图优化模块用于结合单目视觉里程计模块、非视距识别与剔除模块、初始化模块的输出数据，以估计得到车辆全局位姿信息。
[0015]一种融合UWB测距信息的单目视觉车辆定位方法，包括以下步骤：
[0016]S1、获取单目相机采集的原始图像以及UWB标签的测距信息；
[0017]S2、对单目相机图像依次进行去畸变和光度校正处理；
[0018]针对UWB标签的测距信息，通过判断同步前相应距离量测质量，以确定出相应同步后距离量测的质量；
[0019]S3、基于去畸变和光度校正后的单目相机图像，采用滑动窗口优化增量式估计相机位姿，使用滑动窗口优化结果构建两种视觉相对位姿量测；
[0020]S4、对同步后距离量测进行非视距识别及剔除处理；
[0021]S5、结合同步后距离量测以及相机位姿估计结果，对初始全局航向角和视觉里程计初始尺度进行估计；
[0022]S6、利用初始全局航向角和视觉里程计初始尺度构造初始位姿因子图，利用两种视觉相对位姿量测构造视觉相对位姿约束，结合通过非视距剔除的距离量测以及设定的历史先验约束，采用距离噪声模型估计与因子图优化交替进行的方式，估计得到关键帧频率位姿，即得到车辆全局位姿。
[0023]进一步地，所述步骤S2的具体过程为：
[0024]S21、对于单目相机图像，同时使用几何相机模型和光度相机模型模拟图像生成过程，其中，几何相机模型采用传统针孔相机模型，通过相机内参标定获取相机内参矩阵及畸变系数，并去除原始图像径向畸变和切向畸变；
[0025]光度相机模型采用非线性响应函数、非参数化渐晕图和曝光时间进行建模；
[0026]基于相机光度标定结果对去除畸变后的图像进行光度校正；
[0027]S22、对于UWB标签的测距消息，依据时间戳使用线性插值获取与图像帧时间戳同步的距离量测，首先依据原始消息中信号强度信息，判断同步前相应距离量测质量quality，当图像帧时间戳两侧最近距离量测质量均为good时，则相应同步后距离量测的质量被设定为good；否则，相应同步后距离量测的质量被设定为bad。
[0028]进一步地，所述步骤S22中具体是通过以下公式对同步前相应距离量测质量quality进行判断：
[0029][0030]其中，rx_rssi和fp_rssi分别为总接收信号强度指示和第一路径信号强度指示，θ
thrs
为设定筛选阈值。
[0031]进一步地，所述步骤S3中两种视觉相对位姿量测具体为相邻相对位姿量测和共视相对位姿量测。
[0032]进一步地，所述步骤S3的具体过程为：
[0033]S31、首先将去畸变和光度校正后的单目相机图像划分为关键帧和非关键帧，其中，只有关键帧才能被加入滑动窗口并参与滑动窗口优化，对于关键帧，其位姿及其内高梯度点深度会经历多次滑动窗口优化直到其被边缘化出滑动窗口；
[0034]对于非关键帧，将其与最新关键帧直接对齐求取位姿；
[0035]S32、之后利用视觉里程计滑动窗口共构建两种视觉相对位姿量测，在每一次滑动窗口优化完成后，计算最新关键帧位姿T
k
和次新关键帧位姿T
k
‑1之间的相对变换，即为相邻相对位姿量测；
[0036]在每次边缘化关键帧之前，计算最新关键帧位姿T
k
和最旧关键帧位姿本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种融合UWB测距信息的单目视觉车辆定位系统，其特征在于，包括量测预处理模块(1)、单目视觉里程计模块(2)、初始化模块(3)、非视距识别与剔除模块(4)、因子图优化模块(5)，所述量测预处理模块(1)分别与单目视觉里程计模块(2)、非视距识别与剔除模块(4)、初始化模块(3)相连接，所述单目视觉里程计模块(2)分别与初始化模块(3)、非视距识别与剔除模块(4)相连接，所述单目视觉里程计模块(2)、非视距识别与剔除模块(4)、初始化模块(3)分别与因子图优化模块(5)相连接；所述量测预处理模块(1)用于对单目相机采集的原始图像进行去畸变和光度校正处理，以及根据UWB标签测距信息插值获取同步后距离量测；所述单目视觉里程计模块(2)根据去畸变和光度校正后的单目相机图像，用于增量式估计相机位姿、构建两种视觉相对位姿量测；所述非视距识别与剔除模块(4)用于对同步后距离量测进行筛选，以剔除非视距量测；所述初始化模块(3)根据同步后距离量测以及相机位姿估计结果，用于估计初始全局航向角和视觉里程计初始尺度；所述因子图优化模块(5)用于结合单目视觉里程计模块(2)、非视距识别与剔除模块(4)、初始化模块(3)的输出数据，以估计得到车辆全局位姿信息。2.一种应用如权利要求1所述车辆定位系统的融合UWB测距信息的单目视觉车辆定位方法，包括以下步骤：S1、获取单目相机采集的原始图像以及UWB标签的测距信息；S2、对单目相机图像依次进行去畸变和光度校正处理；针对UWB标签的测距信息，通过判断同步前相应距离量测质量，以确定出相应同步后距离量测的质量；S3、基于去畸变和光度校正后的单目相机图像，采用滑动窗口优化增量式估计相机位姿，使用滑动窗口优化结果构建两种视觉相对位姿量测；S4、对同步后距离量测进行非视距识别及剔除处理；S5、结合同步后距离量测以及相机位姿估计结果，对初始全局航向角和视觉里程计初始尺度进行估计；S6、利用初始全局航向角和视觉里程计初始尺度构造初始位姿因子图，利用两种视觉相对位姿量测构造视觉相对位姿约束，结合通过非视距剔除的距离量测以及设定的历史先验约束，采用距离噪声模型估计与因子图优化交替进行的方式，估计得到关键帧频率位姿，即得到车辆全局位姿。3.根据权利要求2所述的一种融合UWB测距信息的单目视觉车辆定位方法，其特征在于，所述步骤S2的具体过程为：S21、对于单目相机图像，同时使用几何相机模型和光度相机模型模拟图像生成过程，其中，几何相机模型采用传统针孔相机模型，通过相机内参标定获取相机内参矩阵及畸变系数，并去除原始图像径向畸变和切向畸变；光度相机模型采用非线性响应函数、非参数化渐晕图和曝光时间进行建模；基于相机光度标定结果对去除畸变后的图像进行光度校正；S22、对于UWB标签的测距消息，依据时间戳使用线性插值获取与图像帧时间戳同步的距离量测，首先依据原始消息中信号强度信息，判断同步前相应距离量测质量quality，当
图像帧时间戳两侧最近距离量测质量均为good时，则相应同步后距离量测的质量被设定为good；否则，相应同步后距离量测的质量被设定为bad。4.根据权利要求3所述的一种融合UWB测距信息的单目视觉车辆定位方法，其特征在于，所述步骤S22中具体是通过以下公式对同步前相应距离量测质量quality进行判断：其中，rx_和fp_分别为总接收信号强度指示和第一路径信号强度指示，θ
thrs
为设定筛选阈值。5.根据权利要求2所述的一种融合UWB测距信息的单目视觉车辆定位方法，其特征在于，所述步骤S3中两种视觉相对位姿量测具体为相邻相对位姿量测和共视相对位姿量测。6.根据权利要求5所述的一种融合UWB测距信息的单目视觉车辆定位方法，其特征在于，所述步骤S3的具体过程为：S31、首先将去畸变和光度校正后的单目相机图像划分为关键帧和非关键帧，其中，只有关键帧才能被加入滑动窗口并参与滑动窗口优化，对于关键帧，其位姿及其内高梯度点深度会经历多次滑动窗口优化直到其被边缘化出滑动窗口；对于非关键帧，将其与最新关键帧直接对齐求取位姿；S32、之后利用视觉里程计滑动窗口共构建两种视觉相对位姿量测，在每一次滑动窗口优化完成后，计算最新关键帧位姿T
k
和次新关键帧位姿T
k
‑1之间的相对变换，即为相邻...

【专利技术属性】
技术研发人员：卓桂荣，付武飞，
申请(专利权)人：同济大学，
类型：发明
国别省市：