[发明专利]视觉里程计的实现方法和实现装置

说明书

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

技术领域

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

背景技术

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

发明内容

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

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

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

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

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

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

可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力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的时间差，°为四元数的乘法运算。

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