CN112161639B - 一种基于角度光流法的垂直双目惯导里程计及其计算方法 - Google Patents

Abstract

本发明公开了一种基于角度光流法的垂直双目惯导里程计及其计算方法，属于计算机视觉领域，包含通过惯性测量模块获得姿态信息，得到相对于相机坐标系的row和pitch自由度；获取相对于主摄像头方向的x，y和yaw自由度；获取相对于补充摄像头方向的x，y和yaw自由度,得到相对于主摄像头的z自由度信息；通过两个垂直方向得到的自由度信息，获得相对于主摄像头的x，y，z和yaw自由度；融合所有的自由度数据，通过卡尔曼滤波优化后即可得到相机的六自由度位姿信息。结合惯性测量模块，使用角度光流法和IMU姿态解算算法设计出垂直的双目惯导里程计，实时且精准得到六个自由度的位姿信息，该方法通过垂直双目设计，能大幅度降低算法复杂度。

Description

一种基于角度光流法的垂直双目惯导里程计及其计算方法

技术领域

本发明属于计算机视觉领域，尤其涉及一种基于角度光流法的垂直双目惯导里程计及其计算方法。

背景技术

视觉惯导里程计的主要功能是实现物体运动轨迹的采集和运动距离的估算。视觉惯导里程计综合了惯性里程计的灵敏性和视觉里程计的准确性，具有高效率高精度的特点。目前视觉惯导里程计主要由惯性测量模块(比如IMU)和双目摄像头在数据融合后实现测准。

经典的视觉惯导里程计不能实时得到大尺度范围内所需要的x，y，z，roll，yaw和pitch六个自由度的位姿信息。其通过横置的双目相机装置，在使用特征点法估计运动位姿时，描述子的计算非常耗时，起不到实时的作用。同时，满足实时性的VO算法受限于时间和运算单元，进行低精度数据融合时效果往往不够理想。

发明内容

本发明所要解决的技术问题是针对背景技术的不足提供一种基于角度光流法的垂直双目惯导里程计及其计算方法，结合惯性测量模块，使用角度光流法和IMU姿态解算算法设计出垂直的双目惯导里程计，实时且精准得到六个自由度的位姿信息，该方法通过垂直双目设计，能大幅度降低算法复杂度。

本发明为解决上述技术问题采用以下技术方案：

一种基于角度光流法的垂直双目惯导里程计，包含惯性测量模块，垂直的双目摄像机模块和基于角度光流法的特征分析及姿态估计模块；

其中，惯性测量模块包含加速度计和陀螺仪，用于测量加速度和角速度参数，进而得到相对于相机坐标系的row和pitch自由度；

垂直的双目摄像机模块，用于获取x，y和yaw自由度；

基于角度光流法的特征分析及姿态估计模块，用于获取x,y,z,yaw自由度数据。

作为本发明一种基于角度光流法的垂直双目惯导里程计的进一步优选方案，所述垂直双目模块包含主摄像头和补充摄像头，其中，主摄像头居中位于侧面，补充摄像头居中位于顶面，两摄像头的中心延长线相交于模块内部，两摄像头的光心汇聚于模块内同一点，构成垂直双目面，用于坐标信息数据融合。

一种基于角度光流法的垂直双目惯导里程计的计算方法，具体包含如下步骤：

步骤1，通过惯性测量模块获得姿态信息，计算得到相对于相机坐标系的row和pitch自由度；

步骤2，在惯性测量模块侧面安装一个主摄像头，得到相对于主摄像头方向的x，y和yaw自由度；

步骤3，在惯性测量模块上在垂直主摄像头侧面再安装一个补充摄像头，得到相对于补充摄像头方向的x，y和yaw自由度,得到相对于主摄像头的z自由度信息；

步骤4，通过两个垂直方向得到的自由度信息，获得相对于主摄像头的x，y，z和yaw自由度；

步骤5，融合所有的自由度数据，通过卡尔曼滤波优化后即可得到相机的六自由度位姿信息。

作为本发明一种基于角度光流法的垂直双目惯导里程计的计算方法的进一步优选方案，所述步骤1具体如下：

通过惯性测量模块获取角速度的信息，使用Runge-Kutta法求解四元数：

得到欧拉角中横滚角(Roll)和俯仰角(Pitch)的计算公式：

Roll＝sin^-1(-2(q₁q₃-q₀q₂))

获得姿态信息，计算得到roll和pitch自由度；

其中，ω_x,ω_y,ω_z为x，y，z方向的角速度，q₀，q₁，q₂，q₃为便于描述的单位四元数，t为某时刻。

作为本发明一种基于角度光流法的垂直双目惯导里程计的计算方法的进一步优选方案，所述步骤2具体如下：

步骤2.1，在惯性测量模块侧面安装一个主摄像头，使用FAST算法获得特征点，使用重心法(Intensity Centroid Method)确定特征点组成物体的角度方向；具体通过定义如下公式：

其中，x和y为特征点位置坐标，I为像素强度Pixel Intensity函数，m_pq为图像块的矩；

步骤2.2，定义质心C为：

通过构造一个从焦点重心O到质心C的矢量OC，以特征点为坐标原点，得到的方向角为

θ＝atan2(m₀₁,m₁₀)

根据所获得的θ，加上可能存在的预设角速度ω，通过公式yaw＝θ+ωt即能得到某时刻相对于主摄像头方向的偏航角；

步骤2.3，基于灰度不变性假设，则

在角光流中，设窗口中的所有像素具有相同的运动，则在t时刻，考虑一个大小为w×w的窗口，它包含w²像素，窗口中的像素具有相同的运动；则得到总共w²个方程：

对得到的多组像素运动速度u和ν取平均后，分别得到

和

其中

是x轴运动速度，

是y轴运动速度；通过公式

计算得到位置信息p_x和p_y，得到相机坐标系的x和y自由度信息。

作为本发明一种基于角度光流法的垂直双目惯导里程计的计算方法的进一步优选方案，在步骤5中：利用惯性测量模块处理得到的pitch和roll自由度数据，加上通过角度光流法的特征分析及姿态估计算法模块处理得到的x,y,z,yaw自由度数据，通过卡尔曼滤波算法优化融合后得到六自由度信息x,y,z,yaw,roll,pitch。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明简单易实现，同时惯性测量模块只使用了陀螺仪便可实现测准，惯性测量模块的精度不需要太高就能得到较为准确的结果，具有较低的成本费用；

2.本发明通过角度光流法可以获得实时精确全面的位姿信息，光学流的精度高且适用范围广，也可应用于实时需求较高的场所，能让机器人自主导航系统更普及；角度光流法(AOF)，它将基于特征方法的大量特征信息与光学流的准确性和速度相结合，能起到实时的作用，同时仅使用一台摄像机就能获得三个自由度的信息，对硬件要求低，处理速度快，能精确得到y，z和任何水平面上的偏航角，角度光流法相较于经典视觉里程计的特征点法，有更快的速度，实时性更高；相较于经典视觉里程计的直接法，能以同样快的速度获取额外的偏航角的自由度信息，能使硬件更为精简；

3.本发明使用角度光流法替代经典特征点法，该里程计省去了传统里程计算法复杂度高、计算量大、系统架构繁杂衍生的一些优化问题，是兼具创新性和实用性的视觉惯导里程计。

附图说明

图1是本发明一种基于角度光流法的垂直双目惯导里程计的计算方法的流程图；

图2是本发明一种基于角度光流法的垂直双目惯导里程计的结构示意图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

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

本发明提出的基于角度光流法的垂直双目惯导里程计，包括惯性测量模块，垂直设置的双目摄像机模块(如图2所示)和基于角度光流法的特征分析及姿态估计算法模块。其中，所述惯性测量模块和姿态估计模块电性连接，所述标准摄像头模块和特征分析模块电性连接，所述主摄像头居中位于侧面，补充摄像头居中位于顶面，两摄像头的中心延长线相交于模块内部，两摄像头的光心汇聚于模块内同一点，构成垂直双目面，方便于坐标信息数据融合。

惯性测量模块(IMU)由加速度计和陀螺仪组成的组合单元，陀螺仪能检测载体相对于导航坐标系的角速度信号，对这些信号进行处理之后，便可解算出物体的姿态，即能精确得到翻滚角和俯仰角。

垂直设置的双目摄像机模块有别于传统的横向双目设计。经典双目设计双目相机一般位于侧面，在同一水平线上，之间距离称为基线距离。垂直双目模块使用的摄像头称为主摄像头和补充摄像头，主摄像头居中位于侧面，补充摄像头居中位于顶面，两摄像头的中心延长线相交于模块内部，两摄像头的光心汇聚于模块内同一点，构成垂直双目面，方便于坐标信息数据融合。

角度光流法(AOF)，它将基于特征方法的大量特征信息与光学流的准确性和速度相结合，能起到实时的作用，同时仅使用一台摄像机就能获得三个自由度的信息，对硬件要求低，处理速度快，能精确得到y，z和任何水平面上的偏航角。角度光流法相较于经典视觉里程计的特征点法，有更快的速度，实时性更高；相较于经典视觉里程计的直接法，能以同样快的速度获取额外的偏航角的自由度信息，能使硬件更为精简。

本实施例提供了创新的垂直双目视觉里程计，如图1所示，主要包括以下步骤：

1.通过惯性测量模块获得姿态信息，计算得到相对于相机坐标系的row和pitch自由度；

2.在惯性测量模块侧面安装一个主摄像头，可以得到相对于主摄像头方向的x，y和yaw自由度；

3.在惯性测量模块上在垂直主摄像头侧面再安装一个补充摄像头，可以得到相对于补充摄像头方向的x，y和yaw自由度,即能得到相对于主摄像头的z自由度信息；

4.通过两个垂直方向得到的自由度信息，可以综合获得相对于主摄像头的x，y，z和yaw自由度；

5.融合所有的自由度数据，经过简单计算即可得到相机的位姿运动信息。

所述步骤1中，惯性测量模块使用IMU，IMU内部包含陀螺仪和加速度计，可以测量加速度和角速度。

以MPU6050传感器为例，MPU6050传感器在商业上应用的较为广泛，它内部集成了MEMS陀螺仪。首先在主控端进行MPU6050传感器的自校准，静置10秒。

自校准完成后，利用MEMS陀螺仪实时获取角速度的信息，使用扩展卡尔曼滤波算法融合处理得到相机坐标系xyz方向的角速度估计ω_xω_yω_z，其估计值精度在正负0.01以下即可。最后，使用Runge-Kutta法求解四元数：在初始条件时，一般会设初始角度为0，则初始四元数矩阵

经过Δt的时间间隔，由公式

可知，通过不断迭代就能求出t时刻的四元数矩阵Q_t。Δt时间间隔这里取1s，随着MPU6050传感器每1秒对角速度进行测算，也同时对上1秒进行迭代求解四元数。接着通过公式Roll＝sin^-1(-2(q₁q₃-q₀q₂))，

就能快速且精确地得到roll(翻滚角)和pitch(俯仰角)二自由度信息。

所述步骤2和3中，通过基于角度光流法的特征分析及姿态估计算法模块，分别利用安装在侧面和顶面的垂直双目相机获取四自由度信息。

使用两个bumblebee2相机进行自由度的获取。对于侧面的主摄像头，在t₀时刻进行特征跟踪体初始化。在t时刻，使用FAST算法获得特征点，特征点固定5个，每隔20帧使用重心法(Intensity Centroid Method)确定特征点组成物体的角度方向。通过定义如下公式(其中，x和y为特征点位置坐标，I为像素强度Pixel Intensity函数，m_pq为图像块的矩)：

然后质心C定义为：

通过构造一个从焦点重心O到质心C的矢量OC，以特征点为坐标原点，得到的方向角为

θ＝atan2(m₀₁,m₁₀)

所获得的θ，加上主摄像头的预设角速度ω₁，通过公式yaw＝θ+ω₁*(t-t₀)，即能得到主摄像头方向的偏航角。

基于灰度不变性假设，可以知道

在角光流中，假设窗口中的所有像素具有相同的运动。在t时刻，考虑一个大小为w×w的窗口，w取图片长宽中较短的一边的20％，它包含w²像素。窗口中的像素具有相同的运动；因此得到了总共w²个方程：

对得到的多组像素运动速度u和ν取平均后，分别得到

和

其中

是x轴运动速度，

是y轴运动速度。通过公式

计算即能得到t+Δt时刻摄像头光心的位置信息p_x和p_y，得到相机坐标系的x和y自由度信息。对于20帧的视频流，Δt取1s，在t＝0时刻，默认为相机参考系的原点，即x₍₀₎＝0，y₍₀₎＝0。

同样，另一个位于顶面的补充摄像头只进行基于灰度不变性假设的位置信息估计，通过公式

在同样时刻，将补充摄像头的p'_y(t+Δt)位置信息与主摄像头的p_x(t+Δt)位置信息进行匹配，在误差允许范围内，当它们相等的时候，补充摄像头的p'_x(t+Δt)即为t+Δt时刻相机坐标系的z自由度信息。

所述步骤5中，利用IMU处理得到的pitch和roll自由度数据，加上垂直双目通过角度光流法的特征分析及姿态估计算法模块处理得到的x,y,z,yaw自由度数据，通过卡尔曼滤波算法优化融合后得到六自由度信息x,y,z,yaw,roll,pitch。在实例中，选取第1秒的20帧数据为例来说明融合过程。在MPU6050传感器通过MEMS陀螺仪得到俯仰角pitch和翻滚角roll信息；对于相邻的两帧图像，应用角度光流计算偏航角yaw和位置信息x,y,z，并不断进行迭代累加，到第20帧光流累加的结果为[Δx₁,Δy₁,Δz₁,Δyaw₁]＝[0.105,0.635,0.004,1.202]，单位为米和角度。对每两帧图像应用离散卡尔曼滤波器状态更新方程

其中，

是角度光流法计算出来的前20帧累积的位置自由度数据和航向角yaw，格式为

u_k-1是

位置控制函数，这里设置为u_k-1＝[Δx_k-1,Δy_k-1,Δz_k-1,Δyaw_k-1]，B是控制增益常数，A是状态转移矩阵，实例中根据测试，A取为单位矩阵，B取1。计算出当前状态

后，计算当前协方差估计

P_k-1是前一帧的协方差估计，Q是过程激励噪声协方差矩阵，这里将过程激励噪声协方差矩阵Q中的参数设为0.1，根据离散卡尔曼滤波器的时间更新方程，更新位置信息为：[x₁,y₁,z₁,yaw₁,row₁,pitch₁]＝[0.108,0.626,0.003,1.233,0.006,0.022]。

至此，得到融合优化后的x,y,z位置自由度数据和yaw,roll,pitch姿态自由读数据，能完全描述位姿，实现位置定位和姿态识别。

以上所述实施例，仅为本申请的具体实施方式，用以说明本申请的技术方案，而非对其限制，本申请的保护范围并不局限于此，尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本申请实施例技术方案的精神和范围。都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。

本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。上面对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (3)

1.一种基于角度光流法的垂直双目惯导里程计的计算方法，其特征在于：所述计算方法包括垂直双目惯导里程计，所述垂直双目惯导里程计包含惯性测量模块，垂直的双目摄像机模块和基于角度光流法的特征分析及姿态估计模块；

其中，惯性测量模块包含加速度计和陀螺仪，用于测量加速度和角速度参数，进而得到相对于相机坐标系的row和pitch自由度；

垂直的双目摄像机模块，用于获取x，y和yaw自由度数据；

基于角度光流法的特征分析及姿态估计模块，用于获取x,y,z,yaw自由度数据；

所述垂直的双目摄像机模块包含主摄像头和补充摄像头，其中，主摄像头居中位于侧面，补充摄像头居中位于顶面，两摄像头的中心延长线相交于模块内部，两摄像头的光心汇聚于模块内同一点，构成垂直双目面，用于坐标信息数据融合；

所述计算方法具体包含如下步骤：

步骤1，通过惯性测量模块获得姿态信息，计算得到相对于相机坐标系的row和pitch自由度；

步骤2，在惯性测量模块侧面安装一个主摄像头，得到相对于主摄像头方向的x，y和yaw自由度；

步骤3，在惯性测量模块上在垂直主摄像头侧面再安装一个补充摄像头，得到相对于补充摄像头方向的x，y和yaw自由度,得到相对于主摄像头的z自由度信息；

步骤4，通过两个垂直方向得到的自由度信息，获得相对于主摄像头的x，y，z和yaw自由度；

步骤5，利用惯性测量模块处理得到的pitch和roll自由度数据，加上通过角度光流法的特征分析及姿态估计算法模块处理得到的x,y,z,yaw自由度数据，通过卡尔曼滤波算法优化融合后得到六自由度信息x,y,z,yaw,roll,pitch。

2.根据权利要求1所述的一种基于角度光流法的垂直双目惯导里程计的计算方法，其特征在于：所述步骤1具体如下：

通过惯性测量模块获取角速度的信息，使用Runge-Kutta法求解四元数：

得到欧拉角中横滚角roll和俯仰角pitch的计算公式：

roll＝sin^-1(-2(q₁q₃-q₀q₂))

获得姿态信息，计算得到roll和pitch自由度；

其中，ω_x,ω_y,ω_z为x，y，z方向的角速度，q₀，q₁，q₂，q₃为便于描述的单位四元数，t为某时刻。

3.根据权利要求1所述的一种基于角度光流法的垂直双目惯导里程计的计算方法，其特征在于：所述步骤2具体如下：

步骤2.1，在惯性测量模块侧面安装一个主摄像头，使用FAST算法获得特征点，使用重心法(Intensity Centroid Method)确定特征点组成物体的角度方向；具体通过定义如下公式：

其中，x和y为特征点位置坐标，I为像素强度Pixel Intensity函数，m_pq为图像块的矩；

步骤2.2，定义质心C为：

通过构造一个从焦点重心O到质心C的矢量OC，以特征点为坐标原点，得到的方向角为

θ＝atan2(m₀₁,m₁₀)

根据所获得的θ，加上可能存在的预设角速度ω，通过公式yaw＝θ+ωt即能得到某时刻相对于主摄像头方向的偏航角；

步骤2.3，基于灰度不变性假设，则

在角光流中，设窗口中的所有像素具有相同的运动，则在t时刻，考虑一个大小为w×w的窗口，它包含w²像素，窗口中的像素具有相同的运动；则得到总共w²个方程：

对得到的多组像素运动速度u和ν取平均后，分别得到

和

其中

是x轴运动速度，

是y轴运动速度；通过公式

计算得到位置信息p_x和p_y，得到相机坐标系的x和y自由度信息。

Images