首页 / 专利库 / 工业自动化和数控机床 / 传感器系统 / 单目视觉和惯性传感器融合的远距离测距系统及方法

单目视觉和惯性传感器融合的远距离测距系统及方法

阅读:393发布:2023-01-22

专利汇可以提供单目视觉和惯性传感器融合的远距离测距系统及方法专利检索,专利查询,专利分析的服务。并且本公开提供一种单目视觉和惯性 传感器 融合的远距离测距系统及方法,所述系统包括:单目视觉SLAM模 块 ,包括单目相机,用于采集视觉图像数据;IMU模块,用于获得IMU模块的IMU 位姿 数据;以及尺度估计模块,将所述视觉图像数据和IMU位姿数据进行融合,实现尺度估计和 定位 建图,获得具备真实尺度的地图信息,完成待测目标的距离测距。所述测距方法包括:步骤S1:通过单目相机采集视觉图像数据,并进行视觉定位和重投影构建地图;步骤S2:在步骤S1所构建的地图中标注出待测距的目标物体;步骤S3:对所构建的地图构建的地图进行尺度估计;以及步骤S4:根据尺度估计结果,对构建的地图进行尺度缩放,从而获得具备真实尺度的地图信息,获得待测目标距离数据。,下面是单目视觉和惯性传感器融合的远距离测距系统及方法专利的具体信息内容。

1.一种单目视觉和惯性传感器融合的远距离测距系统,包括:
单目视觉SLAM模,包括单目相机,用于采集视觉图像数据;
IMU模块,用于获得IMU模块的IMU位姿数据;以及
尺度估计模块,将所述视觉图像数据和IMU位姿数据进行融合,实现尺度估计和定位建图,获得具备真实尺度的地图信息,完成待测目标的距离测距。
2.根据权利要求1所述的单目视觉和惯性传感器融合的远距离测距系统,通过对单目相机采集的无绝对尺度信息的视觉图像数据进行处理,获得视觉位姿数据。
3.根据权利要求2所述的单目视觉和惯性传感器融合的远距离测距系统,所述视觉位姿数据包括:单目相机位置,单目相机旋转矩阵,单目相机旋转四元数,以及单目相机旋转速度。
4.根据权利要求1所述的单目视觉和惯性传感器融合的远距离测距系统,所述IMU模块包括陀螺仪加速度计;所述IMU位姿数据包括:IMU加速度、IMU角速度。
5.一种单目视觉和惯性传感器融合的远距离测距方法,通过如权利要求1至4任一项所述的单目视觉和惯性传感器融合的远距离测距系统进行远距离测距,所述测距方法,包括:
步骤S1:通过单目相机采集视觉图像数据,并进行视觉定位和重投影构建地图;
步骤S2:在步骤S1所构建的地图中标注出待测距的目标物体;
步骤S3:对所构建的地图构建的地图进行尺度估计;以及
步骤S4:根据尺度估计结果,对构建的地图进行尺度缩放,从而获得具备真实尺度的地图信息,获得待测目标距离数据。
6.根据权利要求5所述的单目视觉和惯性传感器融合的远距离测距方法,步骤S1中,单目视觉SLAM模块通过单目相机观测路标点,将三维路标点投影到相机的成像平面,得到像素坐标;成像时的模型采用针孔成像模型;单目视觉SLAM模块的单个路标点按照如下方程进行观测:
ZPuv=K(RPW+t)=KTPW;
其中,Pu,v是图像上的像素坐标,PW=(X Y Z)T是路标点在相机坐标系下的三维坐标,矩阵K是相机内参数矩阵,T是相机从相机坐标系到世界坐标系的变换矩阵,R为相机的旋转矩阵,t是相机从相机坐标系到世界坐标系的平移向量,Z是路标点在相机坐标系下的Z轴坐标,也即路标点在相机坐标系下的深度值;之后将观测结果进行最小化重投影误差,π(·)是投影函数,从而可以得到视觉观测的单目相机位姿结果,即相机的旋转矩阵R和相机的位置p:
其中,Xi是第i个路标点在相机坐标系下的位置, 是第i个路标点在像素平面的投影坐标,ρ()是鲁棒核函数,χ是从单目相机捕获的图像中提取的所有特征点(也即路标点)的集合,∑是求和符, 是指使其后面式子达到最小值时的变量R,t的取值,p是相机的位置数据,也即相机的平移向量t。
7.根据权利要求5所述的单目视觉和惯性传感器融合的远距离测距方法,单目视觉SLAM模块使用ORB特征点和三个主要的并行线程:实时跟踪特征点的前端跟踪线程;用于构建局部集束调整的后端局部建图优化线程;以及用于消除累积漂移和全局位姿图优化的后端回环检测线程。
8.根据权利要求5所述的单目视觉和惯性传感器融合的远距离测距方法,步骤S2中,在定位和建图的过程中,当稳定的相机图像画面之中出现了待测距的目标物体,则在图像中手动选取目标物体的特征点,之后保持相机稳定继续移动一定距离,继续建图,并再次在图像中手动选取之前已选定的特征点,使包含目标物体的场景在所构建的地图中形成闭环;
当构建稳定的闭环地图之后,操作结束,否则继续移动和选取目标物体特征点。
9.根据权利要求5所述的单目视觉和惯性传感器融合的远距离测距方法,在获得无尺度信息的定位地图之后,首先根据球面上采样插值的相机位姿四元数和经过EKF滤波之后的IMU的角速度实现单目相机和IMU模块之间的空间位置关系对齐以及时间偏置关系对齐;
空间对齐和时间对齐可以根据如下方程实现:
其中, 是单目相机的旋转角速度在相机坐标系下的表达, 是IMU的角速度在IMU坐标系下的表达,上标V表示视觉量,上标I表示IMU量,下标C表示相机坐标系,下标S表示IMU坐标系; 是陀螺仪角速度偏置在相机坐标系下的表达;RS表示从相机坐标系到IMU体坐标系的旋转矩阵,td是IMU相对于相机的时间偏置值,∑是求和符,argmin是指使其后面式子达到最小值时下标变量的取值。
10.根据权利要求5所述的单目视觉和惯性传感器融合的远距离测距方法,通过离散线性时不变系统求解视觉加速度,并采用RTS平滑器对视觉加速度进行平滑处理,之后进行线性上采样插值以匹配IMU加速度,同时对IMU加速度信息进行EKF滤波;最终,采用和频域下最小化视觉加速度和IMU加速度代价函数的方式实现鲁棒的尺度估计,从而在粗糙数据的情况下能达到良好的尺度估计结果,方程如下:
||gW||2=9.8;
其中,s为尺度因子,gW为加速度,ba为IMU的加速度偏置, 和 分别是包含视觉加速度和惯性加速度的傅里叶变换矩阵,同时分开计算三个轴向的傅里叶变换,f表示频率,fmax表示最大频率上限;最终的尺度因子s求解是最小化幅值谱|AV(f)|和|AI(f)|。
11.根据权利要求5所述的单目视觉和惯性传感器融合的远距离测距方法,在系统中目标待测距点P在完成闭环前后的两图像中的像素分别表示为p1和p2,点P在世界坐标系下的位置为PW=(X Y Z)T,那么根据投影关系则满足:
z1p1=KP,z2P2=K(RP+t);
其中,z1,z2分别是三维点P在两帧的相机坐标系下的深度信息,R为相机的旋转矩阵,t是相机从相机坐标系到世界坐标系的平移向量;由于单目相机的深度不可观性,上式在齐次坐标下丢失了深度信息:
p1=KP,p2=K(RP+t)=KTP;
从而构建的地图不具有尺度信息;其中,T是相机在第二帧时的位姿变换矩阵;对上式引入p1和p2的归一化平面坐标x1和x2,进而基于上式构建对极约束方程:
对上式分解成基础矩阵和本质矩阵进行求解,可以求出具有尺度等价性的R和t,同时对t进行归一化处理,从而得到逆投影之后3D点的尺度不确定性结果:
那么经过尺度估计模块之后的校准结果即为:
PW=s·P′W=(s·X′s·Y′s·Z′)T=(X Y Z)T;
其中,P′W为待测点经过单目SLAM模块得到的在世界坐标系下不具有尺度信息的相对位置坐标,PW为待测点在世界坐标系下的真实位置,PC为待测点在相机坐标系下的真实位置;
最终,被还原出的选取的特征点在相机坐标系下的空间深度信息就是待测距距离,从而测距结果Ldist为:
进而完成了待测目标距离测距。

说明书全文

单目视觉和惯性传感器融合的远距离测距系统及方法

技术领域

[0001] 本公开涉及同步定位与建图、测绘测控技术领域,尤其涉及一种单目视觉和惯性传感器融合的远距离测距系统及方法。

背景技术

[0002] 测距技术传统上是利用传感器主动的发射物理信息到目标物体之后反射回来,利用发射和接受的时间差或者相位差来进行距离测量。随着科学技术的不断发展,目前已经涌现了激光测距、微波雷达测距、声波测距和红外测距等基本原理类似的技术,这些技术具有应用广泛、测量精度高等优点,此外,各种测距技术还具有自身的物理属性优势,比如红外测距具备红外线的传播不扩散、折射率小等优点。这些测距技术都是利用自身发射模发出物理信息,进而发射到待测物体反射回来被接受模块接受,之后通过获得的时间差或者相位差等根据距离公式实现精准测距。这类测距技术虽然精准稳定,但是都需要有特定的传感器设备,通常而言这些设备价格昂贵而且不便携带,难以在现有的大众化设备中进行普及。
[0003] 同步定位与建图技术(Simultaneous Localization and Mapping,简称SLAM),是设备在未知环境和未知位置下,根据搭载的特定传感器的输入信息,增量式地构建全局一致性的环境地图的同时进行位置定位的间题。视觉SLAM(Visual SLAM)是使用单目相机、双目相机、GB-D相机等视觉传感器构建的SLAM系统。视觉SLAM与人眼类似,通过主动或者被动接受环境光的方式实现对周围环境的信息感知,同时基于多视图几何原理对图像中的特征点进行三维空间还原,从而构建一个与真实环境一致的三维地图。单目视觉SLAM具有成本低、体积小、实时性较高、可获取的信息丰富可靠等优点,在最近十年间取得了飞速的发展。但是因为单目相机在多视图几何投影理论上从三维空间投影到二维像素成像平面,损失了一个维度的信息,从而缺失了3D点的深度信息,从而最终构建出一致性地图不具有尺度信息。因此,单目解决方案中都没有绝对尺度,都无法获得所建地图的绝对尺度信息。这一缺陷限制了单目在现实场景下机器人的应用,也限制了定位和建图的应用。
[0004] 单目视觉惯性系统(monocular visual inertial system,monocular VINS)是指结合了视觉和惯性测量单元IMU(Inertial Measurement Unit,简称IMU)实现定位、建图和导航的系统技术。依据IMU自身的惯性物理量可以还原出真实的轨迹,因此,可以通过低成本惯性测量单元辅助单目视觉系统解决尺度问题。目前低成本的移动设备上基本都配备了单目相机和IMU。基于单目视觉的SLAM系统可以解决测量和定位的问题,IMU可以解决单目视觉的尺度估计问题,这两种设备的结合满足了测距系统的基本物理要求,但目前缺少完善的技术方案将视觉信息和惯性信息进行快速有效的融合,从而难以实现基于低成本硬件的远距离测距。
[0005] 公开内容
[0006] (一)要解决的技术问题
[0007] 基于上述问题,本公开提供了一种单目视觉和惯性传感器融合的远距离测距系统及方法,以缓解现有技术中缺少完善的技术方案将视觉信息和惯性信息进行快速有效的融合,难以实现基于低成本硬件的远距离测距等技术问题。
[0008] (二)技术方案
[0009] 本公开的一个方面,提供一种单目视觉和惯性传感器融合的远距离测距系统,包括:单目视觉SLAM模块,包括单目相机,用于采集视觉图像数据;IMU模块,用于获得IMU模块的IMU位姿数据;以及尺度估计模块,将所述视觉图像数据和IMU位姿数据进行融合,实现尺度估计和定位建图,获得具备真实尺度的地图信息,完成待测目标的距离测距。
[0010] 在本公开实施例中,通过对单目相机采集的无绝对尺度信息的视觉图像数据进行处理,获得视觉位姿数据。
[0011] 在本公开实施例中,所述视觉位姿数据包括:单目相机位置,单目相机旋转矩阵,单目相机旋转四元数,以及单目相机旋转速度。
[0012] 在本公开实施例中,所述IMU模块包括陀螺仪加速度计;所述IMU位姿数据包括:IMU加速度、IMU角速度。
[0013] 本公开的另一方面,提供一种单目视觉和惯性传感器融合的远距离测距方法,通过以上任一项所述的单目视觉和惯性传感器融合的远距离测距系统进行远距离测距,所述测距方法,包括:
[0014] 步骤S1:通过单目相机采集视觉图像数据,并进行视觉定位和重投影构建地图;
[0015] 步骤S2:在步骤S1所构建的地图中标注出待测距的目标物体;
[0016] 步骤S3:对所构建的地图构建的地图进行尺度估计;以及
[0017] 步骤S4:根据尺度估计结果,对构建的地图进行尺度缩放,从而获得具备真实尺度的地图信息,获得待测目标距离数据。
[0018] 在本公开实施例中,步骤S1中,单目视觉SLAM模块通过单目相机观测路标点,将三维路标点投影到相机的成像平面,得到像素坐标;成像时的模型采用针孔成像模型;单目视觉SLAM模块的单个路标点按照如下方程进行观测:
[0019] ZPuv=K(RPW+t)=KTPW;
[0020] 其中,Pu,v是图像上的像素坐标,PW=(X Y Z)T是路标点在相机坐标系下的三维坐标,矩阵K是相机内参数矩阵,T是相机从相机坐标系到世界坐标系的变换矩阵,R为相机的旋转矩阵,t是相机从相机坐标系到世界坐标系的平移向量,Z是路标点在相机坐标系下的Z轴坐标,也即路标点在相机坐标系下的深度值;之后将观测结果进行最小化重投影误差,π(·)是投影函数,从而可以得到视觉观测的单目相机位姿结果,即相机的旋转矩阵R和相机的位置p:
[0021]
[0022] 其中,Xi是第i个路标点在相机坐标系下的位置, 是第i个路标点在像素平面的投影坐标,ρ()是鲁棒核函数,χ是从单目相机捕获的图像中提取的所有特征点(也即路标点)的集合,∑是求和符, 是指使其后面式子达到最小值时的变量R,t的取值,p是相机的位置数据,也即相机的平移向量t。
[0023] 在本公开实施例中,单目视觉SLAM模块使用ORB特征点和三个主要的并行线程:实时跟踪特征点的前端跟踪线程;用于构建局部集束调整的后端局部建图优化线程;以及用于消除累积漂移和全局位姿图优化的后端回环检测线程。
[0024] 在本公开实施例中,步骤S2中,在定位和建图的过程中,当稳定的相机图像画面之中出现了待测距的目标物体,则在图像中手动选取目标物体的特征点,之后保持相机稳定继续移动一定距离,继续建图,并再次在图像中手动选取之前已选定的特征点,使包含目标物体的场景在所构建的地图中形成闭环;当构建稳定的闭环地图之后,操作结束,否则继续移动和选取目标物体特征点。
[0025] 在本公开实施例中,在获得无尺度信息的定位地图之后,首先根据球面上采样插值的相机位姿四元数和经过EKF滤波之后的IMU的角速度实现单目相机和IMU模块之间的空间位置关系对齐以及时间偏置关系对齐;
[0026] 空间对齐和时间对齐可以根据如下方程实现:
[0027]
[0028] 其中, 是单目相机的旋转角速度在相机坐标系下的表达, 是IMU的角速度在IMU坐标系下的表达,上标V表示视觉量,上标I表示IMU量,下标C表示相机坐标系,下标S表示IMU坐标系; 是陀螺仪角速度偏置在相机坐标系下的表达;RS表示从相机坐标系到IMU体坐标系的旋转矩阵,td是IMU相对于相机的时间偏置值,∑是求和符,argmin是指使其后面式子达到最小值时下标变量的取值。
[0029] 在本公开实施例中,通过离散线性时不变系统求解视觉加速度,并采用RTS平滑器对视觉加速度进行平滑处理,之后进行线性上采样插值以匹配IMU加速度,同时对IMU加速度信息进行EKF滤波;最终,采用和频域下最小化视觉加速度和IMU加速度代价函数的方式实现鲁棒的尺度估计,从而在粗糙数据的情况下能达到良好的尺度估计结果,方程如下:
[0030]
[0031] 其中,s为尺度因子,gW为加速度,ba为IMU的加速度偏置, 和 分别是包含视觉加速度和惯性加速度的傅里叶变换矩阵,同时分开计算三个轴向的傅里叶变换,f表示频率,fmax表示最大频率上限;最终的尺度因子s求解是最小化幅值谱|AV(f)|和|AI(f)|。
[0032] 在本公开实施例中,在系统中目标待测距点户在完成闭环前后的两图像中的像素分别表示为p1和p2,点户在世界坐标系下的位置为PW=(X Y Z)T,那么根据投影关系则满足:
[0033] z1p1=KP,z2p2=K(RP+t);
[0034] 其中,z1,z2分别是三维点户在两帧的相机坐标系下的深度信息,R为相机的旋转矩阵,t是相机从相机坐标系到世界坐标系的平移向量;由于单目相机的深度不可观性,上式在齐次坐标下丢失了深度信息:
[0035] p1=KP,p2=K(RP+t)=KTP;
[0036] 从而构建的地图不具有尺度信息;其中,T是相机在第二帧时的位姿变换矩阵;对上式引入p1和p2的归一化平面坐标x1和x2,进而基于上式构建对极约束方程:
[0037]
[0038] 对上式分解成基础矩阵和本质矩阵进行求解,可以求出具有尺度等价性的R和t,同时对t进行归一化处理,从而得到逆投影之后3D点的尺度不确定性结果:
[0039]
[0040] 那么经过尺度估计模块之后的校准结果即为:
[0041] PW=s·P′W=(s·X′s·Y′s·Z′)T=(X Y Z)T;
[0042]
[0043] 其中,P′W为待测点经过单目SLAM模块得到的在世界坐标系下不具有尺度信息的相对位置坐标,PW为待测点在世界坐标系下的真实位置,PC为待测点在相机坐标系下的真实位置;最终,被还原出的选取的特征点在相机坐标系下的空间深度信息就是待测距距离,从而测距结果Ldist为:
[0044] 进而完成了待测目标距离测距。
[0045] (三)有益效果
[0046] 从上述技术方案可以看出,本公开单目视觉和惯性传感器融合的远距离测距系统及方法至少具有以下有益效果其中之一或其中一部分:
[0047] (1)仅使用低成本的单目摄像头和IMU传感器就能实现测距能力,不需要使用激光、红外等相对较大型且昂贵的设备;
[0048] (2)仅采用SLAM与IMU结合的方式就可以获得鲁棒的测距能力;
[0049] (3)基于角速度的进行时间空间对齐的方式不需要在有严格的硬件要求下就可以实现较好的视觉和IMU之间的时间偏置对齐和空间对齐;
[0050] (4)能避免移动端低成本硬件时钟不同步带来的误差,从而不需要硬件中视频序列和IMU传感器的时间戳严格同步。附图说明
[0051] 图1为本公开实施例单目视觉和惯性传感器融合的远距离测距系统的组成示意图。
[0052] 图2为本公开实施例单目视觉和惯性传感器融合的远距离测距方法的流程示意图。
[0053] 图3为本公开实施例单目视觉和惯性传感器融合的远距离测距方法的具体流程架构示意图。
[0054] 图4为DBow2词袋模型架构示意图。

具体实施方式

[0055] 本公开提供了一种单目视觉和惯性传感器融合的远距离测距系统及方法,结合了单目视觉信息与IMU信息,通过借助现有的相机和IMU实现一种面向低成本的移动端设备的被动式接受环境光方式进行远距离测距,摆脱对高成本、大体积的专业性测距设备的依赖。
[0056] 为使本公开的目的、技术方案和优点更加清楚明白,以下结合具体实施例,并参照附图,对本公开进一步详细说明。
[0057] 在本公开实施例中,提供一种单目视觉和惯性传感器融合的远距离测距系统,结合图1和图3所示,所述单目视觉和惯性传感器融合的远距离测距系统,包括:
[0058] 单目视觉SLAM模块,包括单目相机,用于采集视觉图像数据;
[0059] IMU模块,用于获得IMU模块的IMU位姿数据;以及
[0060] 尺度估计模块,将所述视觉图像数据和IMU位姿数据进行融合,实现尺度估计和定位建图。
[0061] 通过对单目相机采集的无绝对尺度信息的视觉图像数据进行处理,获得单目相机位姿数据,即视觉位姿数据;
[0062] 所述视觉位姿数据包括位置数据和姿态数据:位置数据指单目相机位置,通常用平移向量t表示;姿态数据指单目相机的姿态,通常用单目相机(视觉)旋转矩阵R或者单目相机(视觉)旋转四元数q表示。位姿数据也可以用同时包含了位置信息和姿态信息的变换矩阵T表示。从视觉位姿数据中可以根据现有知识推导出其他位姿信息,比如单目相机旋转角速度(视觉角速度);
[0063] 所述IMU模块包括陀螺仪、加速度计;
[0064] 所述IMU位姿数据包括:IMU加速度、IMU角速度;
[0065] IMU加速度也可称为惯性加速度,IMU角速度也可称为惯性角速度;
[0066] 所述尺度估计模块可以为离线尺度估计模块,也可以为在线尺度估计模块;
[0067] 所述视觉图像数据为无尺度信息数据,首先由单目视觉SLAM模块进行视觉定位,通过非线性优化的方法对相机位姿和3D点进行局部集束调整,同时采用闭环检测方法实现鲁棒的全局定位,从而获得一个鲁棒的定位地图。
[0068] 在本公开的实施例中,还提供一种单目视觉和惯性传感器融合的远距离测距方法,所述测距方法融合了单目视觉信息(视觉图像数据)与IMU信息,采用和频域下最小化视觉加速度和IMU加速度代价函数的方式实现鲁棒的尺度估计,从而在粗糙数据的情况下能达到良好的尺度估计结果,根据尺度估计结果,进而对单目视觉图像数据构建的地图进行尺度缩放,从而获得具备真实尺度的地图信息,从而可以在基于单目相机及低成本消费级的IMU的情况下实现低成本和高精度的测距;结合图2至图4所示,所述单目视觉和惯性传感器融合的远距离测距方法,包括:
[0069] 步骤S1:通过单目相机采集视觉图像数据,并进行视觉定位和重投影构建地图;
[0070] 所述单目视觉SLAM模块通过单目相机观测路标点,将三维路标点投影到相机的成像平面,得到像素坐标;成像时的模型采用针孔成像模型;单目视觉SLAM模块的单个路标点按照如下方程进行观测:
[0071] ZPuv=K(RPW+t)=KTPW
[0072] 其中,Pu,v是图像上的像素坐标,PW=(X Y Z)T是路标点在相机坐标系下的三维坐标,矩阵K是相机内参数矩阵,T是相机从相机坐标系到世界坐标系的变换矩阵,R为相机的旋转矩阵,t是相机从相机坐标系到世界坐标系的平移向量,Z是路标点在相机坐标系下的Z轴坐标,也即路标点在相机坐标系下的深度值。
[0073] 之后将观测结果进行最小化重投影误差,π(·)是投影函数,从而可以得到视觉观测的单目相机位姿结果,即相机的旋转矩阵R和相机的位置p:
[0074]
[0075] 其中,Xi是第i个路标点的在相机坐标系下的位置, 是第i个路标点在像素平面的投影坐标,ρ()是鲁棒核函数,χ是从单目相机捕获的图像中提取的所有特征点(也即路标点)的集合,∑是求和符, 是指使其后面式子达到最小值时的变量R,t的取值,p是相机的位置数据,也即相机的平移向量t。
[0076] SLAM模块通过特征点的方式对视觉图像信息进行特征提取和特征匹配。基于匹配好的特征点采用非线性优化的方法对相机位姿和3D点进行局部集束调整,同时采用闭环检测方法实现鲁棒的全局定位,从而获得一个鲁棒的定位地图。步骤S1包含定位和构建地图两过程,更具体包括特征前端和求解的后端两个部分。
[0077] SLAM模块使用ORB特征点和三个主要的并行线程:实时跟踪特征点的前端跟踪线程;用于构建局部集束调整的后端局部建图优化线程;以及用于消除累积漂移和全局位姿图优化的后端回环检测线程。系统在ORB特征的基础上采用DBow2词袋模型构建词袋位置识别器作为回环检测的依据,同时使用共视图实现在大尺度场景下的鲁棒性。采用ORB-SLAM2的单目模式作为重投影建图核心进行实现。
[0078] 步骤S2:在步骤S1所构建的地图中标注出待测距的目标物体;
[0079] 在上述定位和建图的过程中,当稳定的相机图像画面之中出现了待测距的目标物体,则在图像中手动选取目标物体的特征点,之后保持相机稳定继续移动一定距离,继续建图,并再次在图像中手动选取之前已选定的特征点,使包含目标物体的场景在所构建的地图中形成闭环。当构建稳定的闭环地图之后,测距操作结束,否则继续移动和选取目标物体特征点。
[0080] 步骤S3:对所构建的地图构建的地图进行尺度估计;
[0081] 在获得无尺度信息的定位地图之后,首先根据球面上采样插值的相机位姿四元数和经过EKF滤波之后的IMU的角速度实现单目相机和IMU模块之间的空间位置关系对齐以及时间偏置关系对齐。
[0082] 时间对齐是指通过算法将并未严格按照时钟记录的视觉相机帧和IMU位姿数据的时间戳恢复到实际情况的一种方法;空间对齐是通过算法将并未严格参照硬件设计上的相机和IMU坐标系位置关系恢复到实际情况的一种技术。空间对齐和时间对齐可以根据如下方程实现:
[0083]
[0084] 其中, 是单目相机的旋转角速度在相机坐标系下的表达, 是IMU的角速度在IMU坐标系下的表达,上标V表示视觉量,上标I表示IMU量,下标C表示相机坐标系,下标S表示IMU坐标系; 是陀螺仪角速度偏置在相机坐标系下的表达;RS表示从相机坐标系到IMU体坐标系的旋转矩阵,td是IMU相对于相机的时间偏置值,∑是求和符,argmin是指使其后面式子达到最小值时下标变量的取值。之后,再通过离散线性时不变系统求解视觉加速度,并采用Rauch-Tung-Striebel(RTS)平滑器对视觉加速度进行平滑处理,之后进行线性上采样插值以匹配IMU加速度,同时对IMU加速度信息进行EKF滤波;最终,采用和频域下最小化视觉加速度和IMU加速度代价函数的方式实现鲁棒的尺度估计,从而在粗糙数据的情况下能达到良好的尺度估计结果,方程如下:
[0085]
[0086] 其中,s为尺度因子,gW为重力加速度,ba为IMU的加速度偏置, 和 分别是包含视觉加速度和惯性加速度的傅里叶变换矩阵,同时分开计算三个轴向的傅里叶变换,f表示频率,fmax表示最大频率上限。最终的尺度因子s求解是最小化幅值谱|AV(f)|和|AI(f)|;
[0087] 步骤S4:根据尺度估计结果,对构建的地图进行尺度缩放,从而获得具备真实尺度的地图信息,获得待测目标距离数据。
[0088] 设在系统中目标待测距点户在完成闭环前后的两帧图像中的像素分别表示为p1和p2,点户在世界坐标系下的位置为PW=(X Y Z)T,那么根据投影关系则满足:
[0089] z1p1=KP,z2p2=K(RP+t);
[0090] 其中,z1,z2分别是三维点户在两帧的相机坐标系下的深度信息,R为相机的旋转矩阵,t是相机从相机坐标系到世界坐标系的平移向量。由于单目相机的深度不可观性,上式在齐次坐标下丢失了深度信息:
[0091] p1=KP,p2=K(R户+t)=KTP;
[0092] 从而构建的地图不具有尺度信息。其中,T是相机在第二帧时的位姿变换矩阵。对上式引入p1和p2的归一化平面坐标x1和x2,进而上式可以构建对极约束方程:
[0093]
[0094] 对上式分解成基础矩阵和本质矩阵进行求解,可以求出具有尺度等价性的R和t,同时对t进行归一化处理,从而得到逆投影之后3D点的尺度不确定性结果:
[0095]
[0096] 那么经过尺度估计模块之后的校准结果即为:
[0097] PW=s·P′W=(s·X′s·Y′s·Z′)T=(X Y Z)T
[0098]
[0099] 其中,P′W为待测点经过单目SLAM模块得到的在世界坐标系下不具有尺度信息的相对位置坐标,PW为待测点在世界坐标系下的真实位置,PC为待测点在相机坐标系下的真实位置。最终,被还原出的选取的特征点在相机坐标系下的空间深度信息就是待测距距离,从而测距结果Ldist为:
[0100]
[0101] 进而完成了远距离测距。
[0102] 在本公开中,定位精度误差低于尺度规模的1%,尺度估计的精度误差通常低于尺度规模的1%,整套测距系统的精度低于尺度规模的2%。
[0103] 至此,已经结合附图对本公开实施例进行了详细描述。需要说明的是,在附图或说明书正文中,未绘示或描述的实现方式,均为所属技术领域中普通技术人员所知的形式,并未进行详细说明。此外,上述对各元件和方法的定义并不仅限于实施例中提到的各种具体结构、形状或方式,本领域普通技术人员可对其进行简单地更改或替换。
[0104] 依据以上描述,本领域技术人员应当对本公开单目视觉和惯性传感器融合的远距离测距系统及方法有了清楚的认识。
[0105] 综上所述,本公开单目视觉和惯性传感器融合的远距离测距系统及方法,结合了单目视觉信息与IMU信息,通过借助现有的相机和IMU实现一种面向低成本的移动端设备的被动式接受环境光方式进行远距离测距,摆脱对高成本、大体积的专业性测距设备的依赖。仅使用低成本的单目摄像头和IMU传感器就能实现测距能力,不需要使用激光、红外等相对较大型且昂贵的设备。
[0106] 还需要说明的是,实施例中提到的方向用语,例如“上”、“下”、“前”、“后”、“左”、“右”等,仅是参考附图的方向,并非用来限制本公开的保护范围。贯穿附图,相同的元素由相同或相近的附图标记来表示。在可能导致对本公开的理解造成混淆时,将省略常规结构或构造。
[0107] 并且图中各部件的形状和尺寸不反映真实大小和比例,而仅示意本公开实施例的内容。另外,在权利要求中,不应将位于括号之间的任何参考符号构造成对权利要求的限制。
[0108] 除非有所知名为相反之意,本说明书及所附权利要求中的数值参数是近似值,能够根据通过本公开的内容所得的所需特性改变。具体而言,所有使用于说明书及权利要求中表示组成的含量、反应条件等等的数字,应理解为在所有情况中是受到「约」的用语所修饰。一般情况下,其表达的含义是指包含由特定数量在一些实施例中±10%的变化、在一些实施例中±5%的变化、在一些实施例中±1%的变化、在一些实施例中±0.5%的变化。
[0109] 再者,单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。
[0110] 说明书与权利要求中所使用的序数例如“第一”、“第二”、“第三”等的用词,以修饰相应的元件,其本身并不意味着该元件有任何的序数,也不代表某一元件与另一元件的顺序、或是制造方法上的顺序,该些序数的使用仅用来使具有某命名的一元件得以和另一具有相同命名的元件能做出清楚区分。
[0111] 此外,除非特别描述或必须依序发生的步骤,上述步骤的顺序并无限制于以上所列,且可根据所需设计而变化或重新安排。并且上述实施例可基于设计及可靠度的考虑,彼此混合搭配使用或与其他实施例混合搭配使用,即不同实施例中的技术特征可以自由组合形成更多的实施例。
[0112] 本领域那些技术人员可以理解,可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件,以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外,可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述,本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。并且,在列举了若干装置的单元权利要求中,这些装置中的若干个可以是通过同一个硬件项来具体体现。
[0113] 类似地,应当理解,为了精简本公开并帮助理解各个公开方面中的一个或多个,在上面对本公开的示例性实施例的描述中,本公开的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而,并不应将该公开的方法解释成反映如下意图:即所要求保护的本公开要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说,如下面的权利要求书所反映的那样,公开方面在于少于前面公开的单个实施例的所有特征。因此,遵循具体实施方式的权利要求书由此明确地并入该具体实施方式,其中每个权利要求本身都作为本公开的单独实施例。
[0114] 以上所述的具体实施例,对本公开的目的、技术方案和有益效果进行了进一步详细说明,所应理解的是,以上所述仅为本公开的具体实施例而已,并不用于限制本公开,凡在本公开的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本公开的保护范围之内。
相关专利内容
标题 发布/更新时间 阅读量
传感器系统 2020-05-11 232
传感器系统 2020-05-11 802
传感器系统 2020-05-12 984
传感器系统 2020-05-12 183
传感器系统 2020-05-13 308
传感器系统 2020-05-12 834
传感器系统 2020-05-12 118
传感器系统 2020-05-11 673
传感器系统 2020-05-11 930
传感器系统 2020-05-13 561
高效检索全球专利

专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台,是提供专利分析,专利查询,专利检索等数据服务功能的知识产权数据服务商。

我们的产品包含105个国家的1.26亿组数据,免费查、免费专利分析。

申请试用

分析报告

专利汇分析报告产品可以对行业情报数据进行梳理分析,涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度,系统通过AI智能系统对图表进行解读,只需1分钟,一键生成行业专利分析报告。

申请试用

QQ群二维码
意见反馈