视觉里程计的实现方法和实现装置制造方法及图纸

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

【技术实现步骤摘要】
视觉里程计的实现方法和实现装置
本专利技术涉及视觉里程计
，尤其涉及一种视觉里程计的实现方法和实现装置。
技术介绍
视觉里程计的目标是根据摄像机拍摄的图像估计摄像机的运动轨迹。当前视觉里程计的实现方法中，没有考虑摄像机采集的每帧图像之间的时域约束关系，独立求解每帧图像的摄像机姿态，忽略了连续帧图像之间摄像机运动的物理约束，这样导致最终的摄像机姿态计算结果会在实际位置周围抖动，不具有连续性且不符合轨迹应该满足的物理约束，会对机器人、自动驾驶的控制带来致命问题。
技术实现思路
有鉴于此，本专利技术提供一种视觉里程计的实现方法和实现装置，用于解决现有的视觉里程计得到的摄像机姿态的计算结果不准确的问题。为解决上述技术问题，本专利技术提供一种视觉里程计的实现方法，包括：建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力Fw，摄像机坐标系下大小保持不变的力Fb，和，世界坐标系下在时刻tu新增加的力Fu；所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度包括：对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由Fu确定。可选的，所述第一模型函数为：其中，s(t)是所述摄像机在时刻t在三维空间中的位置，ti为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与ti的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。可选的，所述第二模型函数为：其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，ti为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与ti的时间差，°为四元数的乘法运算。可选的，所述目标函数为：其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，nk为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，是重投影误差，p为三维特征点对应的二维特征点，Tcw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，aimu(t)为惯性传感器在时刻t观测得到的加速度值，ωimu(t)为惯性传感器在时刻t观测得到的角速度值。可选的，所述对所述目标函数进行优化之后，还包括：若优化后的Fu大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为tu。本专利技术还提供一种视觉里程计的实现装置，包括：第一建立模块，用于建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；第二建立模块，用于建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；目标函数建立模块，用于根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；优化模块，用于对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力Fw，摄像机坐标系下大小保持不变的力Fb，和，世界坐标系下在时刻tu新增加的力Fu；所述优化模块，还用于对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由Fu确定。可选的，所述第一模型函数为：其中，s(t)是所述摄像机在时刻t在三维空间中的位置，ti为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与ti的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。可选的，所述第二模型函数为：其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，ti为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与ti的时间差，°为四元数的乘法运算。可选的，所述目标函数为：其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，nk为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，是重投影误差，p为三维特征点对应的二维特征点，Tcw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，aimu(t)为惯性传感器在时刻t观测得到的加速度值，ωimu(t)为惯性传感器在时刻t观测得到的角速度值。可选的，所述视觉里程计的实现装置还包括：时间窗确定模块，用于若优化后的Fu大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为tu。本专利技术还提供一种视觉里程计的实现装置，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述视觉里程计的实现方法的步骤。本专利技术还提供一种计算机可读存储介质，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现上述视觉里程计的实现方法的步骤。本专利技术的上述技术方案的有益效果如下：本专利技术实施例中，采用动力学模型对摄像机的运动轨迹进行连续建模，故在每一个时刻都可以得到摄像机的姿态(包括速度、加速度和/或角速度等)，考虑了连续帧图像之间摄像机运动的物理约束，可以保证求解得到的摄像机轨迹是本文档来自技高网...

【技术保护点】
1.一种视觉里程计的实现方法，其特征在于，包括：/n建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；/n建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；/n根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；/n对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。/n

【技术特征摘要】
1.一种视觉里程计的实现方法，其特征在于，包括：
建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；
建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；
根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；
对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。


2.如权利要求1所述的视觉里程计的实现方法，其特征在于，作用在所述摄像机上的力分解成：世界坐标系下方向和大小保持不变的力Fw，摄像机坐标系下大小保持不变的力Fb，和，世界坐标系下在时刻tu新增加的力Fu；
所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度包括：
对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由Fu确定。


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



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


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



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


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



其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，nk为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，是重投影误差，p为三维特征点对应的二维特征点，Tcw(t)＝[R(q(t)),s(t)]，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，h为指定的权重值，ω(t)为所述摄像机在时刻t的角速度，α，β为指定数值，m为所述摄像机的质量，aimu(t)为惯性传感器在时刻t观测得到的加速度值，ωimu(t)为惯性传感器在时刻t观测得到的角速度值。


6.如权利要求2所述的视觉里程计的实现方法，其特征在于，所述对所述目标函数进行优化之后，还包括：
若优化后的Fu大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为tu。


7.一种视觉里程计的实现装置，其特征在于，包括：
第一建立模块，用于建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；
第二建立...

【专利技术属性】
技术研发人员：查红彬，王欣，姜立，方奕庚，
申请(专利权)人：京东方科技集团股份有限公司，北京大学，
类型：发明
国别省市：北京;11