CN112129272B - 视觉里程计的实现方法和实现装置 - Google Patents

Abstract

本发明提供一种视觉里程计的实现方法和实现装置，该实现方法包括：建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。本发明可以保证求解得到的摄像机轨迹是满足实际的物理规律的且具有速度连续的轨迹，同时能够在任意时刻与惯性传感器的数据进行融合。

Description

视觉里程计的实现方法和实现装置

技术领域

本发明涉及视觉里程计技术领域，尤其涉及一种视觉里程计的实现方法和实现装置。

背景技术

视觉里程计的目标是根据摄像机拍摄的图像估计摄像机的运动轨迹。当前视觉里程计的实现方法中，没有考虑摄像机采集的每帧图像之间的时域约束关系，独立求解每帧图像的摄像机姿态，忽略了连续帧图像之间摄像机运动的物理约束，这样导致最终的摄像机姿态计算结果会在实际位置周围抖动，不具有连续性且不符合轨迹应该满足的物理约束，会对机器人、自动驾驶的控制带来致命问题。

发明内容

有鉴于此，本发明提供一种视觉里程计的实现方法和实现装置，用于解决现有的视觉里程计得到的摄像机姿态的计算结果不准确的问题。

为解决上述技术问题，本发明提供一种视觉里程计的实现方法，包括：

建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；

建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；

根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；

对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。

可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力F_w，摄像机坐标系下大小保持不变的力F_b，和，世界坐标系下在时刻t_u新增加的力F_u；

所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度包括：

对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F_u确定。

可选的，所述第一模型函数为：

其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t_i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t_i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)] 为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。

可选的，所述第二模型函数为：

其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t_i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t_i的时间差，°为四元数的乘法运算。

可选的，所述目标函数为：

其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n_k为第 k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，

是重投影误差，p为三维特征点对应的二维特征点， T_cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t) 为所述摄像机在时刻t的朝向的四元数表示，R[q(t)] 为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻 t的角速度，α，β为指定数值，m为所述摄像机的质量，a_imu(t)为惯性传感器在时刻t观测得到的加速度值，ω_imu(t)为惯性传感器在时刻t观测得到的角速度值。

可选的，所述对所述目标函数进行优化之后，还包括：

若优化后的F_u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t_u。

本发明还提供一种视觉里程计的实现装置，包括：

第一建立模块，用于建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；

第二建立模块，用于建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；

目标函数建立模块，用于根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；

优化模块，用于对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。

可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力F_w，摄像机坐标系下大小保持不变的力F_b，和，世界坐标系下在时刻t_u新增加的力F_u；

所述优化模块，还用于对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F_u确定。

可选的，所述第一模型函数为：

其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t_i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t_i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)] 为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。

可选的，所述第二模型函数为：

其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t_i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t_i的时间差，°为四元数的乘法运算。

可选的，所述目标函数为：

其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n_k为第 k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，

是重投影误差，p为三维特征点对应的二维特征点， T_cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t) 为所述摄像机在时刻t的朝向的四元数表示，R[q(t)] 为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a_imu(t)为惯性传感器在时刻t观测得到的加速度值，ω_imu(t)为惯性传感器在时刻t观测得到的角速度值。

可选的，所述视觉里程计的实现装置还包括：

时间窗确定模块，用于若优化后的F_u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t_u。

本发明还提供一种视觉里程计的实现装置，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述视觉里程计的实现方法的步骤。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现上述视觉里程计的实现方法的步骤。

本发明的上述技术方案的有益效果如下：

本发明实施例中，采用动力学模型对摄像机的运动轨迹进行连续建模，故在每一个时刻都可以得到摄像机的姿态(包括速度、加速度和/或角速度等)，考虑了连续帧图像之间摄像机运动的物理约束，可以保证求解得到的摄像机轨迹是满足实际的物理规律的且具有速度连续的二阶连续性质的轨迹，从而解决了相关的逐帧求解摄像机姿态的方法中，没有考虑帧与帧之间的时域约束导致最终的摄像机姿态计算结果会在实际位置周围抖动，对机器人或无人驾驶车的控制带来危害的问题。

同时，由于在每一个时刻都可以得到摄像机的姿态，故可以在任意时刻与惯性传感器的数据进行融合，这样能够充分利用惯性传感器的高帧率的特点，增强了视觉里程计的稳定性，减小了误差累积的问题。

附图说明

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

图1为本发明实施例的视觉里程计的实现方法的流程示意图；

图2为本发明实施例的作用在摄像机上的力的分解示意图；

图3为本发明实施例的摄像机的角速度与摄像机的朝向的关系示意图；

图4为本发明实施例的动力学模型的优化方法的示意图；

图5为本发明实施例的对摄像机运动轨迹的分段方法示意图；

图6为本发明实施例的视觉里程计的实现装置的结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例的附图，对本发明实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的本发明的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1，图1为本发明实施例的视觉里程计的实现方法的流程示意图，该视觉里程计的实现方法包括：

步骤11：建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；

即采用作用在摄像机上的力建模摄像机的位置变化(即平移轨迹)。

步骤12：建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；

即采用摄像机的角速度建模摄像机的朝向变化。

步骤13：根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；

步骤14：对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。

作用在摄像机上的力与摄像机的加速度相关，加速度等于作用在摄像机上的合力除以摄像机的质量。因此得到作用在摄像机的力，即可得到摄像机的加速度。

本发明实施例中，采用动力学模型对摄像机的运动轨迹进行连续建模，故在每一个时刻都可以得到摄像机的姿态(包括速度、加速度和/或角速度等)，考虑了连续帧图像之间摄像机运动的物理约束，可以保证求解得到的摄像机轨迹是满足实际的物理规律的且具有速度连续的二阶连续性质的轨迹，从而解决了相关的逐帧求解摄像机姿态的方法中，没有考虑帧与帧之间的时域约束导致最终的摄像机姿态计算结果会在实际位置周围抖动，对机器人或无人驾驶车的控制带来危害的问题。

同时，由于在每一个时刻都可以得到摄像机的姿态，故可以在任意时刻与惯性传感器的数据进行融合，这样能够充分利用惯性传感器的高帧率的特点，增强了视觉里程计的稳定性，减小了误差累积的问题。

在本发明的一些实施例中，请参考图2，可以将作用在所述摄像机上的力分解成：世界坐标系下方向和大小保持不变的力F_w，摄像机坐标系下大小保持不变的力F_b，和，世界坐标系下在时刻t_u新增加的力F_u。图2所示的实施例中，使用三个力F_w、F_b和F_u的合力近似摄像机的平移轨迹，图2所示的实施例中，假设摄像机架设在汽车上，故图2中摄像机采用汽车表示。图2中C_w为世界坐标系，C_c为摄像机坐标系，V(t_i)为摄像机在t_i时刻的速度，V(t_i+Δt)为摄像机在t_i+Δt时刻的速度，ω为摄像机的角速度。

本发明实施例中，可以根据作用在所述摄像机上的F_u，将摄像机的运动轨迹划分为多个时间窗下的多个分段，在同一个时间窗内，摄像机的运动具有一致性，可以采用相同的优化后的作用在所述摄像机上的力和所述摄像机的角速度描述摄像头的姿态。

即，所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度包括：对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F_u确定。

当然，在本发明的其他一些实施例中，也可以只考虑作用在所述摄像机上 F_w，不考虑F_b和F_u，从而使得动力学模型更加简化。

下面对如何建立用于表述摄像机的位置变化的第一模型函数进行说明。

本发明实施例中，可选的，所述第一模型函数为：

其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t_i是第i个时间窗内的开始时刻，Δt是第i个时间窗内的时刻t与t_i的时间差，m是所述摄像机的质量，q(t)是所述摄像机在时刻t的朝向的四元数表示，R[q(t)] 是在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，第i个时间窗内的t＝t_i+Δt，t 是第i个时间窗内的任意时刻。

上述公式中，摄像机的位置变化(即摄像机的平移轨迹)采用各个力(F_w、F_b和F_u)的积分进行表示。

下面对如何建立用于表述所述摄像机的朝向变化的第二模型函数进行说明。

本发明实施例中，请参考图3，可以对摄像机的角速度进行线性近似，使用角速度和朝向之间的关系，得到摄像机的朝向随时间的变化函数(即第二模型函数)。

摄像机的角速度满足以下公式：

ω(t_i+Δt)＝ω(t_i)+ω^′(t_i)Δt……(1)

其中，ω(t)表示摄像机在时刻t的角速度，ω′(t)为摄像机在时刻t的角加速度，t_i是第i个时间窗内的开始时刻，Δt是第i个时间窗内的时刻t与t_i的时间差。

摄像机的朝向q(t)与角速度满足以下公式：

其中，q’(t)数学上指的是q(t)的一阶导数或一阶微分，表示朝向变化趋势，°为指四元数的乘法运算，t＝t_i+Δt。

对q(t)进行二阶泰勒展开并代入上述公式(1)，在去掉高阶项后可得摄像机朝向的函数表达(即第二模型函数)：

其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t_i是第i个时间窗内的开始时刻，Δt是第i个时间窗内的时刻t与t_i的时间差，°为四元数的乘法运算。

下面对如何获得目标函数进行说明。

本发明实施例中，可以采用上述第一模型函数(s(t))、第二模型函数(q (t))对每帧图像的摄像机姿态T_cw(t)进行计算：

T_cw(t)＝[R(q(t)),s(t)]

T_cw(t)为从世界坐标系变换到摄像机坐标系的变换，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示， R[q(t)] 为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。

本发明实施例中，可以采用多种目标函数，例如可以采用最小化重投影误差得到的目标函数。

如果是采用最小化重投影误差得到的目标函数，上述计算出T_cw(t)之后，还需要计算重投影误差π(T(t)X)-p。

然后，得到目标函数：

其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，即θ＝{F_w,F_b,F_u}，φ为所述摄像机的角速度的集合，即φ＝{ω,ω′}，N为第i 个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k 帧图像中的特征点的计数值，n_k为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，

是重投影误差，p为三维特征点对应的二维特征点，T_cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示， R[q(t)] 为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a_imu(t)为惯性传感器在时刻t观测得到的加速度值，ω_imu(t)为惯性传感器在时刻t观测得到的角速度值。

得到上述目标函数之后，可以采用优化算法对目标函数进行优化，得到优化后的作用在摄像机上的力和摄像机的角速度。本发明实施例中，可以采用多种类型的优化算法，例如Levenberg-Marquardt算法等。

上述公式中的后两项

和β‖ω(t)- ω_imu(t)‖²)为惯性传感器的约束误差项，即，直接使用动力学模型中的动力学参数与惯性传感器数据进行比较，定义误差。

本发明实施例中，由于在每一个时刻都可以得到摄像机的姿态(包括速度、加速度和/或角速度等)，故可以直接与惯性传感器的观测数据(加速度和角速度)做比较，惯性传感器的数据直接与视觉定位过程深度融合，请参考图4，不需要再将惯性传感器的数据进行积分得到摄像机姿态再做融合，这样能够充分利用惯性传感器的高帧率的特点，增强了视觉里程计的稳定性，减小了误差累积的问题。

图4中，θ为作用在所述摄像机上的力的集合，即θ＝{F_w,F_b,F_u}，φ为所述摄像机的角速度的集合，即φ＝{ω,ω′}，

为即上述目标函数中的

图4中上面的汽车轨迹曲线是优化之后的轨迹，下面的曲线是优化之前的初始值，优化过程中，会让r越来越小，得到的动力学参数也更加准确，得到的轨迹也会越来越准确。

本发明实施例中，由于真实世界中的力和角速度应当在合理的范围内，可选的，还可以加入了这些动力学参数(力和角速度)的范围约束：

其中，F_min为人为指定的力的最小值，F_max为人为指定的力的最大值，ω_min为人为指定的角速度的最小值，ω_max为人为指定的角速度的最大值。

本发明实施例中，可选的，所述对所述目标函数进行优化之后，还包括：若优化后的F_u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t_u。即如果优化后的F_u大于或等于指定阈值，说明新增力F_u具有显著的作用，则认为当前的运行一致性被破坏，需要重新开始一个时间窗，并从F_u出现的时刻t_u开始，利用新的动力学参数描述摄像机的运动轨迹，如图5所示。图5中的状态变化函数即上述T_cw(t)＝[R(q(t)),s(t)]，将时间窗开始时刻的角速度、位置等信息，变化到t时刻的角速度、位置等信息。从图 5中可以看出，将摄像机的运动轨迹划分为多个分段，每一分段对应一个时间窗(时间窗i、j和k)。

请参考图6，本发明实施例还提供一种视觉里程计的实现装置，包括：

第一建立模块61，用于建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；

第二建立模块62，用于建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；

目标函数建立模块63，用于根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；

优化模块64，用于对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。

可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力F_w，摄像机坐标系下大小保持不变的力F_b，和，世界坐标系下在时刻t_u新增加的力F_u；

所述优化模块，还用于对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F_u确定。

可选的，所述第一模型函数为：

其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t_i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t_i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)] 为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。

可选的，所述第二模型函数为：

其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t_i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t_i的时间差，°为四元数的乘法运算。

可选的，所述目标函数为：

其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n_k为第 k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，

是重投影误差，p为三维特征点对应的二维特征点， T_cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t) 为所述摄像机在时刻t的朝向的四元数表示，R[q(t)] 为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻 t的角速度，α，β为指定数值，m为所述摄像机的质量，a_imu(t)为惯性传感器在时刻t观测得到的加速度值，ω_imu(t)为惯性传感器在时刻t观测得到的角速度值。

可选的，所述视觉里程计的实现装置还包括：

时间窗确定模块，用于若优化后的F_u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t_u。

在本发明的实施例中，为了便于理解和描述，使用了与所要执行的功能对应的模块的描述方式，容易理解，这些模块是功能实体，不一定必须与物理或逻辑上独立的实体相对应。可以通过通用处理器运行对应功能的软件以执行计算机指令的形式来实现这些功能实体，或在一个或多个硬件模块或集成电路中可编程地实现这些功能实体，或设计为专门执行对应功能的集成电路来实现这些功能实体。

本发明实施例还提供一种视觉里程计的实现装置，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述视觉里程计的实现方法的步骤。

本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现上述视觉里程计的实现方法的步骤。

在本发明的实施例中，处理器可以是中央处理器(CPU)、现场可编程逻辑阵列(FPGA)、单片机(MCU)、数字信号处理器(DSP)、专用集成电路 (ASIC)等具有数据处理能力和/或程序执行能力的逻辑运算器件。

在本发明的实施例中，所述计算机可读存储介质包括但不限于例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(Cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。

除非另作定义，本发明中使用的技术术语或者科学术语应当为本发明所属领域内具有一般技能的人士所理解的通常意义。本发明中使用的“第一”、“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区分不同的组成部分。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。“上”、“下”、“左”、“右”等仅用于表示相对位置关系，当被描述对象的绝对位置改变后，则该相对位置关系也相应地改变。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (12)

1.一种视觉里程计的实现方法，其特征在于，包括：

建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；

建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；

根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；

对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度；

作用在所述摄像机上的力分解成：世界坐标系下方向和大小保持不变的力F_w，摄像机坐标系下大小保持不变的力F_b，和，世界坐标系下在时刻t_u新增加的力F_u；

所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度包括：

对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F_u确定。

2.如权利要求1所述的视觉里程计的实现方法，其特征在于，所述第一模型函数为：

其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t_i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t_i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)]为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。

3.如权利要求1所述的视觉里程计的实现方法，其特征在于，所述第二模型函数为：

其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t_i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t_i的时间差，°为四元数的乘法运算。

4.如权利要求1所述的视觉里程计的实现方法，其特征在于，所述目标函数为：

其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n_k为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，

是重投影误差，p为三维特征点对应的二维特征点，T_cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)]为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a_imu(t)为惯性传感器在时刻t观测得到的加速度值，ω_imu(t)为惯性传感器在时刻t观测得到的角速度值。

5.如权利要求1所述的视觉里程计的实现方法，其特征在于，所述对所述目标函数进行优化之后，还包括：

若优化后的F_u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t_u。

6.一种视觉里程计的实现装置，其特征在于，包括：

第一建立模块，用于建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；

第二建立模块，用于建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；

目标函数建立模块，用于根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；

优化模块，用于对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度；

作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力F_w，摄像机坐标系下大小保持不变的力F_b，和，世界坐标系下在时刻t_u新增加的力F_u；

所述优化模块，还用于对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由F_u确定。

7.如权利要求6所述的视觉里程计的实现装置，其特征在于，所述第一模型函数为：

其中，s(t)是所述摄像机在时刻t在三维空间中的位置，t_i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t_i的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)]为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。

8.如权利要求6所述的视觉里程计的实现装置，其特征在于，所述第二模型函数为：

其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，t_i为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与t_i的时间差，°为四元数的乘法运算。

9.如权利要求6所述的视觉里程计的实现装置，其特征在于，所述目标函数为：

其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，n_k为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，

是重投影误差，p为三维特征点对应的二维特征点，T_cw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)]为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，a_imu(t)为惯性传感器在时刻t观测得到的加速度值，ω_imu(t)为惯性传感器在时刻t观测得到的角速度值。

10.如权利要求6所述的视觉里程计的实现装置，其特征在于，还包括：

时间窗确定模块，用于若优化后的F_u大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为t_u。

11.一种视觉里程计的实现装置，其特征在于，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如权利要求1至5中任一项所述的视觉里程计的实现方法的步骤。

12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现如权利要求1至5中任一项所述的视觉里程计的实现方法的步骤。

Images