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

权利要求书

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。