CN112837374A - 一种空间定位方法及*** - Google Patents

Abstract

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

Description

一种空间定位方法及***

技术领域

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

背景技术

视觉惯性里程计技术(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特征点的数量是否大于设定特征点阈值；

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

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

在匹配相邻的所述图像帧后，利用RANSAC算法去除所述ORB特征点中误匹配的异常点，将剩余的特征点作为标记点标记在所述图像帧上。

可选的，在对所述图像帧做预处理之后，利用视觉SFM算法做初始化处理之前还包括图像关键帧的判断过程，具体包括：

若所述图像帧的视差距离大于视差距离设定阈值，或者所述图像帧的标记点数量小于标记点设定阈值，则认为所述图像帧是所述图像关键帧；

若所述图像帧的视差距离小于或等于视差距离设定阈值，或者所述图像帧的标记点数量大于或等于标记点设定阈值，则认为所述图像帧不是所述图像关键帧；

利用视觉SFM算法对所述图像关键帧做初始化处理。

可选的，所述利用视觉SFM算法对所述图像关键帧做初始化处理具体包括：

构建初始相邻两图像关键帧的本质矩阵和单应性矩阵；

计算所述本质矩阵对应的重投影误差和所述单应性矩阵对应的重投影误差；

利用重投影误差小的矩阵计算载体的位姿信息，并三角化地图点坐标；

基于剩余的图像关键帧，利用PnP算法不断进行三角化，优化所述载体所处空间的地图点和位姿信息；

利用所述载体在设定时间范围内的位姿信息对所有所述图像关键帧构建光束法平差，对所述载体在初始化过程中的位姿信息进行优化，完成对所述图像关键帧的初始化处理。

可选的，利用所述载体基于所述图像帧的位姿信息对预积分后的IMU数据进行解耦处理，得到所述载体的初始化参数具体包括：

构建所述图像帧所在的相机坐标系与所述IMU数据所在的IMU坐标系的坐标变换矩阵；

利用所述坐标变换矩阵对所述视觉SFM处理结果和预积分后的IMU数据进行匹配，得到所述载体的初始化参数。

可选的，利用基于滑动窗口的非线性优化方法得到所述载体的第一位姿信息具体包括：

在滑动窗口内构建基于所述图像帧和所述IMU数据的误差方程；

对所述误差方程进行非线性优化迭代求解，得到所述载体的第一位姿信息；

基于滑动窗口的非线性优化迭代求解的具体过程包括：

采用边缘化策略，使所述滑动窗口内所述图像帧的数量固定，包括：

判断实时获取的图像帧中的次新帧是否为图像关键帧；

若是，对所述滑动窗口内的第一个图像帧边缘化，即从所述滑动窗口内删除；对第一个图像帧关联的IMU数据进行边缘化，并将边缘化的IMU数据作为先验项中的先验信息；

若否，直接扔掉实时获取的所述图像帧中的所述次新帧，但保留所述次新帧关联的IMU数据。

可选的，利用所述UWB信号计算所述载体的第二位姿信息具体包括：

获取所述载体所处空间内的所述UWB信号；

基于距离定位原理，根据所述UWB信号确定所述载体在UWB坐标系下的位姿信息，所述位姿信息作为所述第二位姿信息。

可选的，构建所述第一位姿信息和所述第二位姿信息的联合误差方程，利用所述联合误差方程计算所述载体的位姿信息具体包括：

对所述第一位姿信息和所述第二位姿信息进行坐标转换，均统一到UWB坐标系下；

经坐标转换后的第一位姿信息和第二位姿信息组成联合误差方程；

根据所述联合误差方程得到所述载体的位姿信息；

通过实时计算所述联合误差方程的值实现对所述载体的位姿信息的实时求解。

本发明还提供了一种空间定位***，包括：

单目摄像头模块，位于载体的外表面，用于实时对所述载体所在空间拍摄视频，对所述视频中的图像帧做预处理，并利用视觉SFM算法对所述图像帧做初始化处理得到所述载体的视觉SFM处理结果；

IMU处理模块，位于所述载体的外表面或内部，用于实时获取所述图像帧对应的IMU数据，并对所述IMU数据做预积分处理；利用所述载体的视觉SFM处理结果对预积分后的IMU数据进行解耦处理，得到所述载体的初始化参数，并利用基于滑动窗口的非线性优化方法得到所述载体的第一位姿信息；

UWB处理模块，位于所述载体所在空间内，用于获取UWB信号，利用所述UWB信号计算所述载体的第二位姿信息；构建所述第一位姿信息和所述第二位姿信息的联合误差方程，利用所述联合误差方程计算所述载体的位姿信息。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

1)将UWB与VIO相结合，利用VIO在短时间内的高精度定位优势以及UWB的定位精度不随时间变化的优势，利用滑动窗口优化该组合，实现了在短时间或长时间内移动机器人在所处空间内较高的定位精度，且不随时间的变化产生累计误差，不会对定位精度产生较大影响；

2)将UWB与VIO结合，在VIO中设置了光流法和特征点法的自主选择机制，提高了无UWB信号且环境纹理较弱区域内的***定位鲁棒性；

3)仅需在空间内关键位置布设少量的UWB处理模块即可实现对空间内移动机器人的高精度定位，进而实现对空间内环境的高精度定位，其布设所需面积以及建设成本较小。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例1中的空间定位方法流程图；

图2为本发明实施例1中图像帧预处理过程的流程图；

图3为本发明实施例1中次新帧为关键帧时的边缘化策略的示意图；

图4为本发明实施例1中次新帧为非关键帧时的边缘化策略的示意图；

图5为本发明实施例1中初始化处理的流程图；

图6为本发明实施例2中空间定位***的位置关系示意图。

符号说明：

1-路标点、2-图像关键帧、3-相邻两个图像关键帧的IMU约束、4-边缘化操作、5-删除操作、6-载体、7-单目摄像头模块、8-IMU处理模块、9-UWB处理模块、10-UWB信号。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

视觉里程计技术(Visual Odometry,VO)指的是物体在运动过程中依赖所携带的视觉传感器实现对自身的定位，是移动机器人定位导航技术的重要方向。

当前的视觉传感器与激光传感器在定位领域都得到了广泛应用，但相机与激光传感器相比具有更低的价格以及更小的体积，且获得图像中所包含的信息更为丰富；更具体的，单目相机与双目相机和RGB-D深度相机相比具有更低的耗电量、更小的体积以及更轻的重量。因此，将单目相机的视觉里程计(单目视觉里程计)应用于不同的移动机器人平台，尤其是小型移动机器人平台，有着重要的研究价值。

但仅依靠单目相机进行移动机器人自身的定位导航，存在尺度模糊的问题，无法获得运动轨迹的真实长度。为了解决这种缺陷，现有的解决方案中倾向于将传感器融合，利用传感器获取数据的不同特性，进行优劣互补。其中，大多将相机与IMU组合起来以解决上述缺陷，该组合鲁棒性好且成本低廉。

另外，VIO技术具有以下优势：

无论在纹理丰富的环境，还是弱纹理的环境，单目相机与IMU融合的VIO在短时间内都可实现移动机器人的高精度定位。单目相机具有尺度不确定性问题，且在弱纹理环境下容易定位失败，IMU与单目相机具有很好的互补性质，可恢复单目相机的尺度信息，并估计重力方向，提供可视的绝对仰俯和滚动信息。两者的结合对移动机器人的定位精度和鲁棒性都有大幅提升。

然而VIO只在短时间内具有较高的定位精度，长时间运行会不可避免地产生定位累计误差，导致定位精度大幅下降。

同步定位与建图技术(Simultaneous IocalizationAnd Mapping，SLAM)相比于VIO包含有回环检测模块，但该模块在运动过程中必须是闭环设置，才能够消除累计误差，这在应用中往往是不切实际的。即使能够做到闭环处理，也会造成其定位精度的提高具有一定的滞后性，不能满足长时间的高精度定位。

基于上述，如何既提高VIO长时间运行时的定位精度，又能够使得***具有较好的鲁棒性和较小的延迟，同时使得***具有较小的体积和建设成本，是目前亟待解决的问题。

为此，本发明提出了一种空间定位方法及***。将UWB与VIO技术结合，利用UWB定位精度不随时间变化的特点提升了***在对移动机器人长时间定位时的定位精度。通过在VIO中设置光流法和特征点法的自主选择机制，提升了无UWB信号且环境纹理较弱区域的鲁棒性。并且，仅需布置少量的UWB就可实现短时间或长时间内的高精度定位，建设成本较低。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例1：

如图1所示，为本发明实施例1中空间定位方法的流程图，S1-S7表示空间定位方法的各步骤。

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

对IMU数据做预积分处理，对图像帧做预处理；

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

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

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

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

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

基于上述：首先，本发明提出的空间定位方法及空间定位***用于对载体6的位姿信息进行估计。其中，载体6指运载其他物质的物质。在本发明中，载体6可以是移动机器人，可以是交通工具，也可以是人等等。只要能通过本发明的空间定位方法或空间定位***估计出载体6的位姿信息，无论载体6具体是什么，均在本发明的保护范围内。

其次，本发明中载体6的位姿信息包括载体6的位置信息、速度信息和旋转角度信息，利用本发明的空间定位方法和空间定位***可对这些信息进行实时估计。

接下来对本发明上述空间定位方法的各步骤做详细解释说明。

需要先实时获取空间内载体6拍摄的视频流及视频流中各图像帧对应的IMU数据。在该过程中，视频流是指视频数据的传输，视频是连续多帧图像构成的动态画面，获取的IMU数据也是实时动态变化的。即视频中的每个图像帧对应一组IMU数据。

在获取视频图像帧和IMU数据后，需要对图像帧和IMU分别进行预处理。

如图2所示，为本发明实施例1中图像帧预处理过程的流程图，包括：

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

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

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

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

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

在匹配相邻的图像帧后，利用RANSAC算法去除ORB特征点中误匹配的异常点，将剩余的特征点作为标记点标记在图像帧上。

上述过程中，本发明考虑到了特征点法和光流法各自的特点。

目前的视觉里程计前端主要包括光流法、特征点法和直接法这三种形式，用于进行视觉里程计的计算。在对视觉里程计计算时，特征点法严重依赖周围环境的纹理信息，但具有较高的视觉里程计计算精度和定位精度；光流法和直接法受周围环境的纹理信息影响较小，但光流法的视觉里程计计算精度和定位精度不及特征点法，直接法容易陷入局部最优。因此，在视觉里程计计算时，若单独使用特征点法、光流法或直接法的其中一种，鲁棒性较低。本发明考虑到了该问题，在对图像帧做预处理时，增加了一个特征点法与光流法和选择机制，使本发明的空间定位方法适用于不同的纹理信息环境，在不同的纹理信息环境中均具有较高的视觉里程计计算精度和定位精度。

对IMU数据做预积分处理具体包括：

通过对IMU数据做预积分处理得到IMU数据对应的位姿信息、相邻图像帧的预积分增量、相邻图像帧的预积分误差中的雅克比矩阵和协方差项。其中，计算相邻图像帧的预积分增量、相邻图像帧的预积分误差中雅克比矩阵和协方差项的目的是对估计出的载体6位姿信息进行实时优化。

另外，在实际使用时，由于相邻图像帧的匹配过程中存在很多误匹配，若直接利用包含很多误匹配点对的图像帧进行视觉里程计的计算及定位，将会造成较大的计算误差，且会大幅影响计算效率。为解决该问题，本发明在得到匹配后的相邻图像帧后，利用RANSAC算法对误匹配的异常点进行了去除，保证了后续计算时较高的准确度。

之后，本发明利用视觉SFM算法对图像帧做初始化处理，具体包括对图像关键帧2的判断过程，具体包括：

若图像帧的视差距离大于视差距离设定阈值，或者图像帧的标记点数量小于标记点设定阈值，则认为图像帧是图像关键帧2；

若图像帧的视差距离小于或等于视差距离设定阈值，或者图像帧的标记点数量大于或等于标记点设定阈值，则认为图像帧不是图像关键帧2；

在图像关键帧2的判断过程之后，利用视觉SFM算法对图像关键帧2做初始化处理，具体包括：

构建初始相邻两图像关键帧2的本质矩阵和单应性矩阵；

计算本质矩阵对应的重投影误差和单应性矩阵对应的重投影误差；

利用重投影误差小的矩阵计算载体6的位姿信息，并三角化地图点坐标；

基于剩余的图像关键帧2，利用PnP算法不断进行三角化，优化载体6所处空间的地图点和位姿信息；

利用载体6在设定时间范围内的位姿信息对所有图像关键帧2构建光束法平差，对载体6在初始化过程中的位姿信息进行优化，完成对图像关键帧2的初始化处理。

其中，对重投影误差的计算具体包括：

对本质矩阵进行位姿变换恢复得到本质矩阵对应的位姿信息；

对单应性矩阵进行位姿变换恢复得到单应性矩阵对应的位姿信息；

根据本质矩阵对应的位姿信息计算本质矩阵对应的重投影误差；

根据单应性矩阵对应的位姿信息计算单应性矩阵对应的重投影误差。

选取本质矩阵对应的重投影误差与单应性矩阵对应的重投影误差中重投影误差最小的矩阵的目的是使得初始化处理过程能够不依赖于周围环境的纹理信息，即无论初始化处理的起始环境是平面还是非平面都能通过重投影误差的判断构建具有较高计算精度的初始化处理时的初始值。

本发明的上述初始化处理过程针对的是所有预处理后的图像帧。但本发明考虑到，由于所有预处理后的图像帧中包含关键帧和非关键帧，且关键帧包含了相对更精确的载体6位姿信息，而非关键帧会对载体6位姿信息的计算精度产生影响。因此，在上述利用视觉SFM算法对图像帧做初始化处理的基础上，本发明考虑到了对图像帧中关键帧和非关键帧的划分。

通过对图像关键帧2的判断，本发明在原有的所有图像帧中选出了所有的图像关键帧2，并利用前述的视觉SFM算法对图像关键帧2做初始化处理。与对图像帧做初始化处理相比，对图像关键帧2的初始化处理的处理效率大幅提升。

在利用视觉SFM算法对图像帧进行处理得到视觉SFM处理结果后，本发明利用视觉SFM处理结果对预积分后的IMU数据进行了解耦处理，以获得载体6的初始化参数。该过程具体包括：

构建图像帧所在的相机坐标系与IMU数据所在的IMU坐标系的坐标变换矩阵；

利用坐标变换矩阵对视觉SFM处理结果和预积分后的IMU数据进行匹配，得到载体6的初始化参数。

更具体的，由于利用视觉SFM算法对图像帧的处理结果在计算精度上低于利用视觉SFM算法对图像关键帧2的处理结果，使得基于图像帧的SFM处理结果构建的坐标变换矩阵的坐标变换精度低于基于图像关键帧2的SFM处理结果构建的坐标变换矩阵。因此，为保证较高的载体6位姿信息的计算精度和计算效率，本发明优选构建图像关键帧2所在的相机坐标系与IMU数据所在的IMU坐标系的坐标变换矩阵。

之后，本发明对载体6进行实时滑动窗口优化，以获得载体6的第一位姿信息。该过程具体包括：

在滑动窗口内构建基于图像帧和IMU数据的误差方程；

对误差方程进行非线性优化迭代求解，得到载体6的第一位姿信息。

其中，非线性优化迭代求解的具体过程包括：

采用边缘化策略，使滑动窗口内图像帧的数量固定，包括：

判断实时获取的图像帧中的次新帧是否为图像关键帧2；

若是，对滑动窗口内的第一个图像帧边缘化，即从滑动窗口内删除；对第一个图像帧关联的IMU数据进行边缘化，并将边缘化的IMU数据作为先验项中的先验信息；

若否，直接扔掉实时获取的图像帧中的所述次新帧，但保留次新帧关联的IMU数据。

其中，滑动窗口是指在所有图像帧的长度范围内选取设定长度的窗口，该滑动窗口内包含有设定帧数的图像帧，每次对滑动窗口内设定帧数的图像帧进行处理以计算载体6的第一位姿信息。

本发明设置的边缘化策略的具体过程可结合图3和图4理解。

结合图3和图4可看出：

相邻两个图像帧存在IMU约束3，若次新帧是图像关键帧2(图3和图4中的X_n-1是次新帧)，则对滑动窗口内的第一个图像帧边缘化4，即从滑动窗口内删除滑动窗口内原有的第一个图像帧。也对第一个图像帧关联的IMU数据进行边缘化4，即撤销第一个图像帧与相邻的图像帧存在的IMU约束3，并将滑动窗口内第一个图像帧边缘化4的IMU数据作为先验项中的先验信息。收集先验信息并构建先验项的目的是在后续对载体6的位姿信息进行优化。

若次新帧不是图像关键帧2，则直接扔掉次新帧，但保留次新帧关联的IMU数据。在该过程中，由于一段视频流中的次新帧与当前帧很相似，即次新帧和路标点1的约束与当前帧和路标点1的约束很接近，使得直接扔掉次新帧并不会使得滑动窗口内图像帧与路标点1的约束关系丢失过多的信息，进而不会对载体6的位姿信息计算产生较大影响，因此若次新帧不是图像关键帧2，可直接扔掉次新帧。

另外，前述介绍到了，在对IMU数据做预积分处理时，会根据相邻图像帧的IMU数据计算相邻图像帧的预积分增量、相邻图像帧的预积分误差中雅克比矩阵和协方差项。若在判断次新帧不是图像关键帧2后将次新帧关联的IMU数据删除，将导致对IMU数据做预积分处理时，上述预积分结果的非连贯性，从而影响利用滑动窗口内图像帧计算载体6位姿信息时的计算精度。因此，在判断次新帧不是图像关键帧2后，并不删除次新帧关联的IMU数据。

而且，本发明为了进一步提高滑动窗口内载体6基于图像帧的载体6位姿信息的计算精度，还将前述构建的先验项中的先验信息与对IMU数据做预积分处理得到的相邻图像帧的预积分增量、相邻图像帧的预积分误差中的雅克比矩阵和协方差项联合起来，用以在进行位姿信息估计时，对滑动窗口内的图像帧进行优化，使得实时构建的滑动窗口内的图像帧不丢失历史信息数据，进一步保证载体6基于图像帧计算位姿信息的计算精度。

如图5所示，为本发明实施例1中初始化处理的流程图；为了进一步提高对载体6位姿信息的计算精度，可将“采用基于滑动窗口的非线性优化方法对滑动窗口范围内的图像帧进行处理”替换为“采用基于滑动窗口的非线性优化方法对滑动窗口范围内的图像关键帧2进行处理”。也就是说，先对图像关键帧2进行判断，再直接采用基于滑动窗口的非线性优化方法对图像关键帧2进行处理。

此时基于滑动窗口对图像关键帧2的处理效率要比前述对所有图像帧的处理效率高得多。而且由于此时不仅在图像帧中筛选出了图像关键帧2，且通过设置滑动窗口将实时处理的图像关键帧2数量限制得较小，另外设置了对滑动窗口内图像关键帧2的实时优化策略，使得此时对图像关键帧2的处理精度相比前述对图像帧的处理精度也要高得多。

基于上述，若未对图像帧中的图像关键帧2进行判断，且未利用基于滑动窗口的非线性优化方法，则可直接采用视觉SFM算法对所有图像帧进行初始化处理，得到载体6基于图像帧的位姿信息。但由于图像帧中包含了图像关键帧2和非图像关键帧，且未利用基于滑动窗口的非线性优化方法对参与载体6位姿信息计算的图像帧数量进行限定，随着***的运行容易导致基于图像帧计算载体6位姿信息的计算量繁重，达不到实时性的目的。因此，为保证较高的载体6位姿信息计算精度和计算效率，本发明优选对图像帧做图像关键帧2判断，且采用基于滑动窗口的非线性优化方法对实时参与载体6位姿信息计算的图像关键帧2数量进行限定后，基于图像关键帧2对载体6的位姿信息进行估计。

在通过上述过程计算载体6的第一位姿信息的同时，需要实时获取载体6所处空间内的UWB信号，并基于距离定位原理，根据UWB信号确定载体6在UWB坐标系下的位姿信息，该位姿信息作为载体6的第二位姿信息。

具体的，本发明对第一位姿信息和第二位姿信息进行了坐标转换，均统一到UWB坐标系下；

之后将坐标转换后的第一位姿信息和第二位姿信息组成联合误差方程；

最后根据联合误差方程得到载体6的位姿信息；

通过实时求解联合误差方程的值可实现对载体6位姿信息的实时优化。

本发明借助此过程，根据UWB信号客观确定了载体6的位姿信息，将该位姿信息与前述得到的第一位姿信息联合，如同将载体6主观获取的位姿信息与客观获取的位姿信息联合，相比单一的第一位姿信息或单一的第二位姿信息，具有相对更高的载体6位姿信息计算精度，能够进一步提升对载体6的定位精度以及载体6对所处空间的定位精度。

除了上述，本发明还在实施例2中介绍了一种空间定位***，***的结构组成和位置关系如图6所示。

其中，***包括：

单目摄像头模块7，位于载体6的外表面，用于实时对载体6所在空间拍摄视频，对视频中的图像帧做预处理，并利用视觉SFM算法对图像帧做初始化处理得到载体6的视觉SFM处理结果；

IMU处理模块8，位于载体6的外表面或内部，用于实时获取图像帧对应的IMU数据，并对IMU数据做预积分处理；利用载体6的视觉SFM处理结果对预积分后的IMU数据进行解耦处理，得到载体6的初始化参数，并利用基于滑动窗口的非线性优化方法得到载体6的第一位姿信息；

UWB处理模块9，位于载体6所在空间内，用于获取UWB信号10，利用UWB信号10计算载体6的第二位姿信息；构建第一位姿信息和第二位姿信息的联合误差方程，利用联合误差方程计算载体6的位姿信息。

需要说明的是，本发明的空间定位***不仅限于图6所示的形状、大小和位置关系，只要能够实现本发明中单目摄像头模块7、IMU处理模块8和UWB处理模块9对应的功能，空间定位***的任何形状、大小和位置关系均在本发明的保护范围内。

本发明介绍的上述空间定位***基于前述的空间定位方法实现。为了提升空间定位***对载体6位姿信息的计算精度和计算效率，进而提升定位精度和定位效率，可在单目摄像头模块7利用视觉SFM算法做初始化处理之前，对图像帧做前述的图像关键帧2判断操作以在所有图像帧中选取图像关键帧2，并利用视觉SFM算法对图像关键帧2进行初始化处理，进而对载体6的位姿信息进行计算。

基于上述，本发明利用空间定位方法和空间定位***，将载体6基于图像帧的位姿信息、载体6基于IMU数据的位姿信息与载体6基于UWB信号10的位姿信息联合，提升了单独使用VIO技术对载体6和载体6所处空间长时间定位时的定位精度和鲁棒性。另外，根据本发明设置的图像关键帧2判断操作、基于滑动窗口的非线性优化操作、边缘化操作4等过程，在图像帧中选取对载体6位姿信息准确度影响较大的图像关键帧2，并根据图像关键帧2实时估计载体6的位姿信息，可进一步提升将UWB信号10结合到***中对载体6位姿信息的计算准确度以及载体6所处空间的长时间定位精度。能够解决现有技术中单独利用VIO技术定位精度不高、鲁棒性较差的问题，且仅需较少的建设成本，在载体6所处空间内的关键位置布置较少的UWB处理模块9，就可结合上述空间定位方法实现对载体6位姿信息的准确估计，并实现对载体6所处空间的准确定位。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，对以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种空间定位方法，其特征在于，包括以下步骤：

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

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

对所述图像帧做预处理；

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

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

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

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

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

2.根据权利要求1所述的空间定位方法，其特征在于，所述预积分后的IMU数据包括：

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

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

3.根据权利要求1所述的空间定位方法，其特征在于，对所述图像帧做预处理具体包括：

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

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

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

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

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

在匹配相邻的所述图像帧后，利用RANSAC算法去除所述ORB特征点中误匹配的异常点，将剩余的特征点作为标记点标记在所述图像帧上。

4.根据权利要求1所述的空间定位方法，其特征在于，在对所述图像帧做预处理之后，利用视觉SFM算法做初始化处理之前还包括图像关键帧的判断过程，具体包括：

若所述图像帧的视差距离大于视差距离设定阈值，或者所述图像帧的标记点数量小于标记点设定阈值，则认为所述图像帧是所述图像关键帧；

若所述图像帧的视差距离小于或等于视差距离设定阈值，或者所述图像帧的标记点数量大于或等于标记点设定阈值，则认为所述图像帧不是所述图像关键帧；

利用视觉SFM算法对所述图像关键帧做初始化处理。

5.根据权利要求4所述的空间定位方法，其特征在于，所述利用视觉SFM算法对所述图像关键帧做初始化处理具体包括：

构建初始相邻两图像关键帧的本质矩阵和单应性矩阵；

计算所述本质矩阵对应的重投影误差和所述单应性矩阵对应的重投影误差；

利用重投影误差小的矩阵计算载体的位姿信息，并三角化地图点坐标；

基于剩余的图像关键帧，利用PnP算法不断进行三角化，优化所述载体所处空间的地图点和位姿信息；

利用所述载体在设定时间范围内的位姿信息对所有所述图像关键帧构建光束法平差，对所述载体在初始化过程中的位姿信息进行优化，完成对所述图像关键帧的初始化处理。

6.根据权利要求1所述的空间定位方法，其特征在于，利用所述载体基于所述图像帧的位姿信息对预积分后的IMU数据进行解耦处理，得到所述载体的初始化参数具体包括：

构建所述图像帧所在的相机坐标系与所述IMU数据所在的IMU坐标系的坐标变换矩阵；

利用所述坐标变换矩阵对所述视觉SFM处理结果和预积分后的IMU数据进行匹配，得到所述载体的初始化参数。

7.根据权利要求1所述的空间定位方法，其特征在于，利用基于滑动窗口的非线性优化方法得到所述载体的第一位姿信息具体包括：

在滑动窗口内构建基于所述图像帧和所述IMU数据的误差方程；

对所述误差方程进行非线性优化迭代求解，得到所述载体的第一位姿信息；

基于滑动窗口的非线性优化迭代求解的具体过程包括：

采用边缘化策略，使所述滑动窗口内所述图像帧的数量固定，包括：

判断实时获取的图像帧中的次新帧是否为图像关键帧；

若是，对所述滑动窗口内的第一个图像帧边缘化，即从所述滑动窗口内删除；对第一个图像帧关联的IMU数据进行边缘化，并将边缘化的IMU数据作为先验项中的先验信息；

若否，直接扔掉实时获取的所述图像帧中的所述次新帧，但保留所述次新帧关联的IMU数据。

8.根据权利要求1所述的空间定位方法，其特征在于，利用所述UWB信号计算所述载体的第二位姿信息具体包括：

获取所述载体所处空间内的所述UWB信号；

基于距离定位原理，根据所述UWB信号确定所述载体在UWB坐标系下的位姿信息，所述位姿信息作为所述第二位姿信息。

9.根据权利要求1所述的空间定位方法，其特征在于，构建所述第一位姿信息和所述第二位姿信息的联合误差方程，利用所述联合误差方程计算所述载体的位姿信息具体包括：

对所述第一位姿信息和所述第二位姿信息进行坐标转换，均统一到UWB坐标系下；

经坐标转换后的第一位姿信息和第二位姿信息组成联合误差方程；

根据所述联合误差方程得到所述载体的位姿信息；

通过实时计算所述联合误差方程的值实现对所述载体的位姿信息的实时求解。

10.一种空间定位***，其特征在于，包括：

单目摄像头模块，位于载体的外表面，用于实时对所述载体所在空间拍摄视频，对所述视频中的图像帧做预处理，并利用视觉SFM算法对所述图像帧做初始化处理得到所述载体的视觉SFM处理结果；

IMU处理模块，位于所述载体的外表面或内部，用于实时获取所述图像帧对应的IMU数据，并对所述IMU数据做预积分处理；利用所述载体的视觉SFM处理结果对预积分后的IMU数据进行解耦处理，得到所述载体的初始化参数，并利用基于滑动窗口的非线性优化方法得到所述载体的第一位姿信息；

UWB处理模块，位于所述载体所在空间内，用于获取UWB信号，利用所述UWB信号计算所述载体的第二位姿信息；构建所述第一位姿信息和所述第二位姿信息的联合误差方程，利用所述联合误差方程计算所述载体的位姿信息。

Images