[发明专利]一种空间定位方法及系统

说明书

本发明涉及一种空间定位方法及系统，涉及定位技术领域。空间定位系统包括单目摄像头模块、IMU处理模块和UWB处理模块。先利用单目摄像头模块获取空间内载体拍摄的视频流，利用IMU处理模块获取载体的IMU数据；之后利用视觉SFM算法对预处理后的图像帧做初始化处理得到载体的视觉SFM处理结果；利用视觉SFM处理结果对预积分后的IMU数据进行解耦处理，得到载体的初始化参数；对载体进行基于滑动窗口的实时优化得到载体的第一位姿信息；UWB处理模块获取UWB信号并计算得到载体的第二位姿信息；最后基于第一位姿信息和第二位姿信息的联合误差方程计算载体的位姿信息。本发明解决了仅利用VIO技术长时间定位存在的定位累计误差，大幅提高了空间内载体的长时间定位精度。

技术领域

本发明涉及定位技术领域，特别是涉及一种空间定位方法及系统。

背景技术

视觉惯性里程计技术(Visual Inertial Odometry,VIO)用于移动机器人的导航和自我定位，利用获取的移动机器人定位信息实现移动机器人所处空间的定位。VIO包括单目相机和惯性测量单元(Inertial Measurement Unit,IMU)。其中，单目相机根据拍摄的视频流实现移动机器人自身的定位导航，IMU与单目相机组合，解决单目相机定位导航时存在的尺度模糊问题并提高系统的鲁棒性。

然而，利用现有的VIO技术仅能在短时间内保证移动机器人的定位精度，若长时间运行，将不可避免地产生定位累计误差，导致长时间定位时定位精度的大幅下降。

发明内容

为了解决上述问题，本发明提供了一种空间定位方法及系统。将超宽带(UltraWide Band,UWB)结合到VIO中，提升VIO长时间运行时的定位精度，具有较好的系统鲁棒性和较小的定位延迟，且结合后的系统具有较小的体积和较低的建设成本。

为实现上述目的，本发明提供了如下方案：

一种空间定位方法，包括以下步骤：

实时获取空间内载体拍摄的视频流及所述视频流中各图像帧对应的IMU数据；

对所述IMU数据做预积分处理；

对所述图像帧做预处理；

利用视觉SFM算法对预处理后的图像帧做初始化处理得到所述载体的视觉SFM处理结果；

利用所述载体的视觉SFM处理结果对预积分后的IMU数据进行解耦处理，得到所述载体的初始化参数；

利用基于滑动窗口的非线性优化方法得到所述载体的第一位姿信息；

获取UWB信号，利用所述UWB信号计算所述载体的第二位姿信息；

构建所述第一位姿信息和所述第二位姿信息的联合误差方程，利用所述联合误差方程计算所述载体的位姿信息。

可选的，所述预积分后的IMU数据包括：

所述IMU数据对应的位姿信息、相邻所述图像帧的预积分增量、相邻所述图像帧的预积分误差中的雅克比矩阵和协方差项；

利用所述载体基于所述图像帧的位姿信息对所述IMU数据对应的位姿信息进行解耦处理，得到所述载体基于所述IMU数据的位姿信息。

可选的，对所述图像帧做预处理具体包括：

对所述图像帧做图像金字塔处理，得到以金字塔形状排列、分辨率由下至上逐步降低的多层图像结构；

对所述多层图像结构中的每层图像提取ORB特征点；

判断所述ORB特征点的数量是否大于设定特征点阈值；

若是，利用特征点法匹配相邻的所述图像帧；

若否，利用光流法匹配相邻的所述图像帧；