CN109764880B - 紧耦合车辆轮子编码器数据的视觉惯性测程方法及*** - Google Patents
紧耦合车辆轮子编码器数据的视觉惯性测程方法及*** Download PDFInfo
- Publication number
- CN109764880B CN109764880B CN201910127778.1A CN201910127778A CN109764880B CN 109764880 B CN109764880 B CN 109764880B CN 201910127778 A CN201910127778 A CN 201910127778A CN 109764880 B CN109764880 B CN 109764880B
- Authority
- CN
- China
- Prior art keywords
- sliding window
- image
- imu
- frame
- wheel encoder
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 106
- 230000000007 visual effect Effects 0.000 title claims abstract description 67
- 238000005259 measurement Methods 0.000 claims abstract description 136
- 239000011159 matrix material Substances 0.000 claims abstract description 112
- 230000010354 integration Effects 0.000 claims abstract description 104
- 238000005457 optimization Methods 0.000 claims description 39
- 230000006870 function Effects 0.000 claims description 28
- 239000013598 vector Substances 0.000 claims description 23
- 230000001133 acceleration Effects 0.000 claims description 18
- 238000004364 calculation method Methods 0.000 claims description 18
- 230000005484 gravity Effects 0.000 claims description 11
- 238000005070 sampling Methods 0.000 claims description 5
- 238000007429 general method Methods 0.000 claims description 3
- 230000003287 optical effect Effects 0.000 claims description 3
- 230000004927 fusion Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
Images
Landscapes
- Gyroscopes (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种紧耦合车辆轮子编码器数据的视觉惯性测程方法及***,视觉惯性测程方法包括:步骤一:设置所述视觉惯性测程方法的初始化状态为“未完成”,并在初始化的过程中;步骤二:计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口;步骤三:对最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU‑轮子编码器预积分;判断初始化的状态是否为“已完成”,若是则执行步骤五,否则执行步骤四;步骤四:利用预积分结果对参数进行初始化;步骤五:得到第一滑动窗口内各张图像对应的参数;步骤六:根据所述参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
Description
技术领域
本发明涉及融合多传感器的定位导航领域,具体涉及一种紧耦合车辆轮子编码器数据的视觉惯性测程方法及***。
背景技术
使用相机和IMU(Inertial Measurement Unit,惯性测量单元)的视觉惯性测程方法近年来取得了很好的效果,因为视觉和IMU的信息有互补的特点。然而,因为车辆的运动形式较为特殊,例如易于做近似匀速直线运动,视觉惯性测程方法中的尺度估计有时不准确。另一方面,轮子编码器可以提供用来确定尺度的准确信息,而且它的数据易于获取。因此,融合相机、IMU和轮子编码器是室内车辆定位的明智选择。
几种现有的方法分别使用扩展卡尔曼滤波和非线性优化等不同的策略融合相机、IMU和轮子编码器的信息。这三种方法都需要轮子编码器***提供角速度,尽管因为有IMU中陀螺仪的使用上述角速度并非必要。一方面,为了获取上述角速度,至少需要安装两个轮子编码器。另一方面,因为由轮子编码器推导出的角速度总是在地平面内,所以事实上做出了时间上相邻的两个状态之间车辆几乎是平面运动的假设。但是这个假设在车辆上下坡或颠簸时可能会造成较大误差。
发明内容
为了解决现有技术中的上述问题,即为了降低车辆行驶过程中产生的误差,本发明提供一种紧耦合车辆轮子编码器数据的视觉惯性测程方法及***。
为解决上述技术问题,本发明提供了如下方案:
一种紧耦合车辆轮子编码器数据的视觉惯性测程方法,所述视觉惯性测程方法包括:
步骤一:设置所述视觉惯性测程方法的初始化状态为“未完成”,并在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;
步骤二:计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整,所述当前图片为第一帧图像之后的任意图像;
步骤三:对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”,若是则依次执行步骤五和步骤六,否则执行步骤四;
步骤四:判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则步骤四结束,否则继续判断第二滑动窗口内的图像间的视差是否小于设定的视差阈值,若是则步骤四结束,否则继续判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,如果是则步骤四结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对参数进行初始化;
步骤五:在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的参数;
步骤六:根据所述参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
可选的,所述参数包括传感器位姿、速度、偏移以及三维点深度。
可选的,所述实时获取相机图像、IMU量测值和轮子编码器读数,具体包括:
获取当前图像的同时,获取该图像与前一帧图像获取时刻之间的IMU量测值和轮子编码器读数,作为所述图像对应的IMU量测值与轮子编码器读数;
其中,所述图像、IMU量测值和轮子编码器读数均带有各自的时间戳;使得在对轮子编码器读数进行采样的过程中,采样的时间戳与IMU量测值的时间戳一致。
可选的,步骤二具体包括:
获取当前图像的角点:针对第一帧图像,使用施-托马西(Shi-Tomasi)方法提取所述第一帧图像的角点;针对除第一帧以外的其他图像,使用光流法跟踪出现在前一帧图像上的角点;
其中,如果前一帧图像的某个角点能够成功跟踪得到所述当前图像上的一个角点,则这两个角点形成所述当前图像与前一帧图像之间的一对匹配点;
利用所述当前图像与前一帧图像之间的所有匹配点对,使用随机抽样一致算法RANSAC框架,利用5点法计算所述当前图像与前一帧图像之间的本质矩阵;如果所述本质矩阵的内点数目超过阈值,则记录所述本质矩阵,并分解所述本质矩阵,得到所述图像与前一帧图像之间的旋转矩阵,同时记录所述本质矩阵有效性为有效;否则记录其有效性为无效;
将所述当前图像同时加入第一滑动窗口和第二滑动窗口,帧序号分别为k+1、m+1,如果k<K则结束,否则判断当前图像加入窗口前第一滑动窗口内最后一帧图像是否为关键帧;若其不是关键帧则将第一滑动窗口中的该当前图像丢弃,同时将第一滑动窗口中该当前图像对应的所有IMU量测值和轮子编码器读数加入到第一滑动窗口中新加入的当前图像对应的IMU量测值和轮子编码器读数中,第一滑动窗口内最后一帧图像序号减1;若是关键帧,则对第一滑动窗口内最前面一帧图像对应的参数进行边缘化操作得到边缘化误差项;并将所述图像和对应的所有IMU量测值和轮子编码器读数丢弃,第一滑动窗口内各帧序号减1,随后将第二滑动窗口内所有时间戳小于当前第一滑动窗口内第1帧图像时间戳的n张图像及其对应的IMU量测值和轮子编码器读数丢弃,同时第二滑动窗口内各帧序号减n;
其中,k和m分别为当前第一滑动窗口和第二滑动窗口中的图像数目,K为第一滑动窗口中能容纳的最多图像数目;
对第一滑动窗口中的最后一帧图像对应的IMU量测值和轮子编码器读数,以及对于第二滑动窗口中的最后一帧图像对应的IMU量测值和轮子编码器读数,进行预积分操作。
可选的,所述进行预积分操作,具体包括:
i依次取0,2…,s-2,依次计算
Ji+1=AiJi
其中,s表示参与预积分的IMU量测值和轮子编码器读数的个数,以及是由四元数表示的旋转,表示四元数的乘法,R(·)表示将一个四元数转换成它对应的旋转矩阵的函数,表示第i个IMU量测值中的三轴加速度,表示第i个IMU量测值中的三轴角速度,表示第i个轮子编码器的读数,ba为当前IMU中加速度计的偏移,bω为当前IMU中陀螺仪的偏移,δti表示第i+1个IMU量测值的时间戳和第i个IMU量测值的时间戳的差值,表示从车体坐标系到IMU坐标系的旋转矩阵,用车体坐标系中的向量得到IMU坐标系中的向量,通过离线标定获得;
其中,车体坐标系的定义为:以左后轮为原点,x轴指向右后轮,y轴指向左前轮,z轴垂直底盘向上。
可选的,步骤四具体包括:
判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目K,若是则步骤四结束;否则执行下一步;
对于第一滑动窗口内的所有图像,如果存在一帧图像与窗口内最后一帧图像有超过m对匹配点,且这些匹配点对的平均视差超过p个像素,且使用这些匹配点对利用RANSAC框架,使用5点法计算本质矩阵,有超过n对匹配点是内点,则执行步骤下一步;否则步骤四结束;
判断第二滑动窗口中所有图像对应的本质矩阵的有效性为有效的个数,若其小于有效的本质矩阵个数的阈值r,则步骤四结束;否则利用第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,使用最小二乘法优化计算陀螺仪的偏移:
其中,bω为待优化的陀螺仪偏移,初值为零向量;F表示第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的本质矩阵有效的帧序号集合,m表示第二滑动窗口中图像序号,为分解本质矩阵得到的旋转矩阵对应的四元数,表示从第二滑动窗口中第m+1帧图像的相机坐标系到第m帧图像的相机坐标系的旋转,是表示从相机坐标系到IMU坐标系的旋转的四元数, 可以事先通过离线标定获得,表示四元数的乘法;是一个旋转四元数,为陀螺仪偏移bω的函数,r表示有效的本质矩阵个数的阈值,‖ ‖2表示向量的2范数:
δbω是bω的更新量,和分别是第二滑动窗口中的第m帧和第m+1帧图像之间IMU-轮子编码器预积分的旋转四元数和雅可比矩阵的一部分,随着第二滑动窗口的滑动,窗口中任意相邻两帧图像之间预积分得到的和均为已知;
对于第二滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数,依次重新进行预积分操作,其中陀螺仪偏移bω为优化得到的值;
对于第二滑动窗口中每两帧连续的图像,构造方程
其中,和为第二滑动窗口中第m帧和第m+1帧图像之间IMU-轮子编码器预积分的结果;为四元数对应的旋转矩阵形式,Δtm为第二滑动窗口中第m+1帧和第m帧图像之间时间戳的差值;为旋转矩阵;为是在窗口内第一帧图像对应的IMU坐标系下的重力加速度,它的初值可以由计算,g为当地重力加速度的大小;B是的切平面上的一组正交基;为第m帧图像对应的IMU位置在第m帧图像对应的IMU坐标系下的运动速度,为第m+1帧图像对应的IMU位置在第m+1帧图像对应的IMU坐标系下的运动速度;Δg是重力加速度的更新量;
所述方程可以记作:
m依次取1,2,…M-1,M是第二滑动窗口中图像的数目,可以得到M-1个方程,把它们联立得到误差方程:
记作L=AX
X包含的所有元素均为待优化的变量,L和A的所有元素均为已知;
对于第一滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数,依次重新进行预积分操作;
计算一个矩阵使得利用所述预积分结果和k为第一滑动窗口中图像序号,使用航位推算的方法计算表示从第一滑动窗口内第k帧图像对应的IMU坐标系到第一滑动窗口内第一帧图像的对应的IMU坐标系的旋转矩阵以及第一滑动窗口内第k帧图像对应的IMU坐标系原点在第一滑动窗口内第一帧图像对应的IMU坐标系中的位置考虑到第一滑动窗口和第二滑动窗口的第一帧图像相同,且第一滑动窗口内的每一帧图像都可以在第二滑动窗口内找到和它相同的图像,计算IMU误差项中待优化的参数的初值:
其中,Q(·)表示将一个旋转矩阵转换为对应的四元数的函数,为步骤S4006中计算出的第二滑动窗口中第m(k)帧图像对应的IMU位置在第m(k)帧图像对应的IMU坐标系下的运动速度,m(k)表示第一滑动窗口中的第k帧图像在第二滑动窗口中对应的图像序号,bω为陀螺仪偏移;
利用所述第一滑动窗口中的图像对应的IMU坐标系的位姿和可以推算出第一滑动窗口中的图像位姿;利用第一滑动窗口中的图像位姿以及图像上的匹配点进行三角化计算,得到匹配点的深度,将所述深度作为构造重投影误差项所需待优化参数中的匹配点深度参数的初值;
设置初始化的状态为“已完成”。
可选的,步骤五具体包括:
按照基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法在第一滑动窗口中构造代价函数,并进行非线性优化求解,得到传感器位姿、速度和偏移以及匹配点深度的估计值;
其中,所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项;
所述重投影误差项和所述边缘化误差项使用基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法进行构造;
所述IMU误差项使用下述方法进行构造:
其中,CIMU(x)表示IMU误差项;x由x1,x2,…,xk组成;k表示滑动窗口内图像的帧序号;xk是第k帧的状态向量,∑k,k+1表示在步骤S2006中预积分得到的协方差矩阵,随着第一滑动窗口的滑动,窗口中任意相邻两帧图像之间预积分得到的协方差矩阵均为已知;表示残差向量,与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关:
和分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度;和分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度;和分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移;和分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移;是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转;为的旋转矩阵形式;为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转;为的旋转矩阵形式;gw为世界坐标系中的重力加速度,是一个常量;Δtk为第k+1帧和第k帧曝光时刻时间戳的差值;是车体坐标系的原点在IMU坐标系中的坐标,通过计算得到,表示从车体坐标系到IMU坐标系的旋转矩阵,表示IMU的中心在车体坐标系中的坐标,事先量测;和为第k帧和第k+1帧图像之间IMU-轮子编码器预积分的结果,随着第一滑动窗口的滑动,窗口中任意相邻两帧图像之间预积分得到的和均为已知;
为解决上述问题,本发明还提供另一种技术方案:
一种紧耦合车辆轮子编码器数据的视觉惯性测程***,所述包括视觉惯性测程***包括:
初始化单元,用于设置所述视觉惯性测程方法的初始化状态为“未完成”;
获取单元,用于在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;配置为:一台相机,实时获取图像;一台IMU,实时获取IMU量测值;一套获取车辆上轮子编码器的数据的设备,实时获取轮子编码器读数;一套计时设备,为所述三种数据提供在同一时间基准下的时间戳;
相机姿态计算单元,用于计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整;
判断单元,用于对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”;
判断执行单元,用于在所述第一判断单元的判断结果为初始化的状态为“未完成”时,判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则结束,否则继续判断第二滑动窗口内的图像间的视差是否小于阈值,若是则结束,否则继续;如果判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,若是则结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对参数进行初始化;
优化单元,用于在所述第一判断单元的判断结果为初始化的状态为“已完成”时或者所述判断执行单元初始化完成后,在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的参数;
测程单元,用于根据所述参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
为解决上述问题,本发明还提供另一种技术方案:
一种紧耦合车辆轮子编码器数据的视觉惯性测程***,包括:
处理器;以及
被安排成存储计算机可执行指令的存储器,所述可执行指令在被执行时使所述处理器执行以下操作:
步骤一:设置所述视觉惯性测程方法的初始化状态为“未完成”,并在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;
步骤二:计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整,所述当前图片为第一帧图像之后的任意图像;
步骤三:对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”,若是则执行步骤五,否则执行步骤四;
步骤四:判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则步骤四结束,否则继续判断第二滑动窗口内的图像间的视差是否小于设定的视差阈值,若是则步骤四结束,否则继续判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,如果是则步骤四结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对参数进行初始化;
步骤五:在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的参数;
步骤六:根据所述参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
为解决上述问题,本发明还提供另一种技术方案:
一种计算机可读存储介质,所述计算机可读存储介质存储一个或多个程序,所述一个或多个程序当被包括多个应用程序的电子设备执行时,使得所述电子设备执行以下操作:
步骤一:设置所述视觉惯性测程方法的初始化状态为“未完成”,并在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;
步骤二:计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整,所述当前图片为第一帧图像之后的任意图像;
步骤三:对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”,若是则执行步骤五,否则执行步骤四;
步骤四:判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则步骤四结束,否则继续判断第二滑动窗口内的图像间的视差是否小于设定的视差阈值,若是则步骤四结束,否则继续判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,如果是则步骤四结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对参数进行初始化;
步骤五:在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的参数;
步骤六:根据所述参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
根据本发明的实施例,本发明公开了以下技术效果:
本发明利用单目相机的图像、IMU中的加速度计数据和陀螺仪数据以及轮子编码器数据,在预积分阶段融合了IMU和轮子编码器数据,从而在非线性优化阶段进行有准确尺度的自由度相机跟踪,从而可降低车辆行驶过程中产生的误差。
附图说明
图1是本发明紧耦合车辆轮子编码器数据的视觉惯性测程方法的流程图;
图2是数据优化对比图;
图3是本发明紧耦合车辆轮子编码器数据的视觉惯性测程***的模块结构示意图。
符号说明:
在附图3中,1表示初始化单元,2表示获取单元,3表示相机姿态计算单元,4表示判断单元,5表示判断执行单元,6表示优化单元,7表示测程单元。
具体实施方式
下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是,这些实施方式仅仅用于解释本发明的技术原理,并非旨在限制本发明的保护范围。
本发明提供一种紧耦合车辆轮子编码器数据的视觉惯性测程方法,利用单目相机的图像、IMU中的加速度计数据和陀螺仪数据以及轮子编码器数据,在预积分阶段融合了IMU和轮子编码器数据,从而在非线性优化阶段进行有准确尺度的自由度相机跟踪,从而可降低车辆行驶过程中产生的误差。
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本发明作进一步详细的说明。
本发明紧耦合车辆轮子编码器数据的视觉惯性测程方法包括:
步骤1:设置所述视觉惯性测程方法的初始化状态为“未完成”,并在初始化的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;
步骤2:计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整,所述当前图片为第一帧图像之后的任意图像;
步骤3:对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”,若是则执行步骤五,否则执行步骤四;
步骤4:判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则步骤4结束,否则继续判断第二滑动窗口内的图像间的视差是否小于设定的视差阈值,若是则步骤4结束,否则继续判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,如果是则步骤4结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对参数进行初始化;
步骤5:在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的参数;
步骤6:根据所述参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
优选地,在优化参数的之前,一般要对各参数进行初始化。下面以一具体实施例说明:
如图1所示,本发明紧耦合车辆轮子编码器数据的视觉惯性测程方法包括:
步骤S1000,设置本测程法的初始化的状态为“未完成”;
步骤S2000,实时获取相机图像、IMU量测值和轮子编码器读数;每获取一帧图像,计算它与前一帧图像之间的本质矩阵,将其以及与其对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据其是否是关键帧对上述两个滑动窗口进行相应调整;对两个滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;
步骤S3000,每当两个窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数的IMU-轮子编码器预积分都完成时,判断本测程法的初始化的状态是否为“已完成”,若是则执行步骤S5000,否则执行步骤S4000;
步骤S4000,判断第一滑动窗口内的图像数目,若其小于第一滑动窗口中容纳的最大图像数目则本步骤结束,否则继续;判断第二滑动窗口内的图像间的视差,若其小于阈值则本步骤结束,否则继续;如果第二滑动窗口中有效的本质矩阵个数小于阈值则本步骤结束,否则继续;根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用上述陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用上述预积分结果对本测程法所需传感器位姿、速度、偏移以及三维点深度等参数初始化,并设置本测程法的初始化的状态为“已完成”;
步骤S5000,在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,从而得到第一滑动窗口内各张图像对应的传感器位姿、速度和偏移以及匹配点深度。
优选地,步骤S2000具体包括:
步骤S2001,实时获取相机图像;获取一帧图像的同时,同时获取该图像与前一帧图像获取时刻之间的IMU量测值和轮子编码器读数,作为所述图像对应的IMU量测值与轮子编码器读数;
其中,所述图像、IMU量测值和轮子编码器读数均带有各自的时间戳;
IMU量测值和轮子编码器读数的获取频率均显著高于图像的获取频率。
步骤S2002,对轮子编码器读数进行采样,采样后的时间戳与IMU量测值的时间戳一致。
步骤S2003,对于一帧获取到的图像,使用施-托马西(Shi-Tomasi)方法提取角点;如果该图像并非本测程法启动以来获取的第一帧图像,那么使用光流法跟踪出现在前一帧图像上的角点,并将所述两种来源的角点作为出现在本图像上的角点;
其中,如果前一帧图像的某个角点能够成功跟踪得到本图像上的一个角点,那么这两个角点形成本图像与前一帧图像之间的一对匹配点。
步骤S2004,如果步骤S2003中所述本图像并非本测程法启动以来获取的第一帧图像,那么利用本图像与前一帧图像之间的所有匹配点对,使用随机抽样一致算法(RANSAC)框架,利用5点法计算本图像与前一帧图像之间的本质矩阵;如果满足所述本质矩阵的内点数目超过阈值,那么记录所述本质矩阵,并分解它得到本图像与前一帧图像之间的旋转矩阵,同时记录所述本质矩阵有效性为有效;否则记录其有效性为无效;如果步骤S2003中所述本图像为本测程法启动以来获取的第一帧图像,记录所述本质矩阵的有效性为无效。
步骤S2005,将所述图像同时加入第一滑动窗口和第二滑动窗口,帧序号分别为k+1、m+1,如果k<K则本步骤结束,否则判断图像加入窗口前第一滑动窗口内最后一帧图像是否为关键帧;若其不是关键帧则将第一滑动窗口中的该图像丢弃,同时将第一滑动窗口中该图像对应的所有IMU量测值和轮子编码器读数加入到第一滑动窗口中新加入的图像对应的IMU量测值和轮子编码器读数中,第一滑动窗口内最后一帧图像序号减1;若其是关键帧,则对第一滑动窗口内最前面一帧图像对应的参数进行边缘化操作得到边缘化误差项,并将它和它对应的所有IMU量测值和轮子编码器读数丢弃,第一滑动窗口内各帧序号减1,随后将第二滑动窗口内所有时间戳小于当前第一滑动窗口内第1帧图像时间戳的n张图像及其对应的IMU量测值和轮子编码器读数丢弃,同时第二滑动窗口内各帧序号减n。
其中,k和m分别为当前第一滑动窗口和第二滑动窗口中的图像数目,K为第一滑动窗口中能容纳的最多图像数目。在本实施例中,K=10。
本发明所述最后一帧与最前面一帧分别指滑动窗口中时间戳最新的和最旧的一帧图像。
步骤S2006,对于第一滑动窗口中的最后一帧图像对应的IMU量测值和轮子编码器读数,及第二滑动窗口中的最后一帧图像对应的IMU量测值和轮子编码器读数,进行预积分操作。
优选地,所述进行预积分操作,具体包括:
i依次取0,2…,s-2,依次计算
Ji+1=AiJi;
其中,s表示参与预积分的IMU量测值和轮子编码器读数的个数,以及是由四元数表示的旋转,表示四元数的乘法,R(·)表示将一个四元数转换成它对应的旋转矩阵的函数,表示第i个IMU量测值中的三轴加速度,表示第i个IMU量测值中的三轴角速度,表示第i个轮子编码器的读数,ba为当前IMU中加速度计的偏移,bω为当前IMU中陀螺仪的偏移,δti表示第i+1个IMU量测值的时间戳和第i个IMU量测值的时间戳的差值,表示从车体坐标系到IMU坐标系的旋转矩阵,用它左乘车体坐标系中的向量得到IMU坐标系中的向量,可以通过离线标定获得;
车体坐标系的定义为:以左后轮为原点,x轴指向右后轮,y轴指向左前轮,z轴垂直底盘向上。
步骤S4000具体包括:
步骤S4001,判断第一滑动窗口内的图像数目,若其小于第一滑动窗口中容纳的最大图像数目K则结束步骤S4000;否则步骤S4000结束;
步骤S4002,对于第一滑动窗口内的所有图像,如果存在一帧图像与窗口内最后一帧图像有超过m对匹配点,且这些匹配点对的平均视差超过p个像素,且使用这些匹配点对利用RANSAC框架,使用5点法计算本质矩阵,有超过n对匹配点是内点,则执行步骤下一步;否则步骤S4000;
步骤S4003,判断第二滑动窗口中所有图像对应的本质矩阵的有效性为有效的个数,若其小于有效的本质矩阵个数的阈值r,则结束;否则利用第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,使用最小二乘法优化计算陀螺仪的偏移:
其中,bω为待优化的陀螺仪偏移,初值为零向量;F表示第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的本质矩阵有效的帧序号集合,m表示第二滑动窗口中图像序号,为分解本质矩阵得到的旋转矩阵对应的四元数,表示从第二滑动窗口中第m+1帧图像的相机坐标系到第m帧图像的相机坐标系的旋转,是表示从相机坐标系到IMU坐标系的旋转的四元数, 可以事先通过离线标定获得,表示四元数的乘法;是一个旋转四元数,它是陀螺仪偏移bω的函数:
δbω是bω的更新量,和分别是第二滑动窗口中的第m帧和第m+1帧图像之间IMU-轮子编码器预积分的旋转四元数和雅可比矩阵的一部分,随着第二滑动窗口的滑动,窗口中任意相邻两帧图像之间预积分得到的和均为已知;
步骤S4004,对于第二滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数,依次重新进行预积分操作,其中陀螺仪偏移bω为优化得到的值。
步骤S4006,对于第二滑动窗口中每两帧连续的图像,构造方程
其中,和为第二滑动窗口中第m帧和第m+1帧图像之间IMU-轮子编码器预积分的结果;为四元数对应的旋转矩阵形式,Δtm为第二滑动窗口中第m+1帧和第m帧图像之间时间戳的差值;为旋转矩阵;为是在窗口内第一帧图像对应的IMU坐标系下的重力加速度,它的初值可以由计算,g为当地重力加速度的大小;B是的切平面上的一组正交基;为第m帧图像对应的IMU位置在第m帧图像对应的IMU坐标系下的运动速度,为第m+1帧图像对应的IMU位置在第m+1帧图像对应的IMU坐标系下的运动速度;Δg是重力加速度的更新量;
所述方程可以记作
m依次取1,2,…M-1,M是第二滑动窗口中图像的数目,可以得到M-1个方程,把它们联立得到误差方程:
记作L=AX
X包含的所有元素均为待优化的变量,L和A的所有元素均为已知;
步骤S4007,对于第一滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数,依次重新进行预积分操作,其中陀螺仪偏移bω使用步骤S4003中优化得到的值;
步骤S4008,计算一个矩阵使得利用所述预积分结果和k为第一滑动窗口中图像序号,使用航位推算的方法计算表示从第一滑动窗口内第k帧图像对应的IMU坐标系到第一滑动窗口内第一帧图像的对应的IMU坐标系的旋转矩阵以及第一滑动窗口内第k帧图像对应的IMU坐标系原点在第一滑动窗口内第一帧图像对应的IMU坐标系中的位置考虑到第一滑动窗口和第二滑动窗口的第一帧图像相同,且第一滑动窗口内的每一帧图像都可以在第二滑动窗口内找到和它相同的图像,计算IMU误差项中待优化的参数的初值:
其中,Q(·)表示将一个旋转矩阵转换为对应的四元数的函数,为步骤S4006中计算出的第二滑动窗口中第m(k)帧图像对应的IMU位置在第m(k)帧图像对应的IMU坐标系下的运动速度,m(k)表示第一滑动窗口中的第k帧图像在第二滑动窗口中对应的图像序号,bω为计算出的陀螺仪偏移;
利用所述第一滑动窗口中的图像对应的IMU坐标系的位姿和可以推算出第一滑动窗口中的图像位姿;利用第一滑动窗口中的图像位姿以及图像上的匹配点进行三角化计算,得到匹配点的深度,将所述深度作为构造重投影误差项所需待优化参数中的匹配点深度参数的初值。
步骤S4009,设置本测程法的初始化的状态为“已完成”。
在本实施例中,m=20,p=14,n=10,r=3,c=4。
进一步地,步骤S5000,具体包括:
按照基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法在第一滑动窗口中构造代价函数,使用Dogleg算法进行非线性优化求解,得到传感器位姿、速度和偏移以及匹配点深度的估计值。
其中,所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项;
所述重投影误差项和所述边缘化误差项使用基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法进行构造;
所述IMU误差项使用下述方法进行构造:
其中,CIMU(x)表示IMU误差项;x由x1,x2,…,xk组成;k表示滑动窗口内图像的帧序号;xk是第k帧的状态向量,∑k,k+1表示在步骤S2006中预积分得到的协方差矩阵,随着第一滑动窗口的滑动,窗口中任意相邻两帧图像之间预积分得到的协方差矩阵均为已知。表示残差向量,与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关:
和分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度;和分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度;和分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移;和分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移;是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转;为的旋转矩阵形式;为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转;为的旋转矩阵形式;gw为世界坐标系中的重力加速度,是一个常量;Δtk为第k+1帧和第k帧曝光时刻时间戳的差值;是车体坐标系的原点在IMU坐标系中的坐标,它可以通过计算得到,表示IMU的中心在车体坐标系中的坐标,可以事先量测。
表1表示本发明的初始化模块和现有的初始化模块在初始化成功之前车辆行驶的路程在两套数据上的对比。可以看出,使用本发明的初始化模块在初始化成功之前车辆行驶的路程明显更短,说明本方法的初始化速度更快。
表1初始化成功之前车辆行驶的路程在两套数据上的对比
本发明 | [5]中的初始化模块 | |
数据1 | 2.01m | 56.6m |
数据2 | 1.13m | 29.9m |
图2中(a)部分和(b)部分是本发明视觉惯性测程方法分别使用步骤S4000中所述方法与现有的初始化模块进行初始化后,前100帧图像依次进行步骤S5000中所述传感器位姿优化时,优化结束后的目标函数值对比图。
可以看出,在两套数据上使用本方法的初始化模块,前100帧图像进行所述位姿优化时,优化结束后的目标函数值更小,因为目标函数值代表误差,所以这表明本方法的初始化结果更准确。
本发明涉及融合多传感器的定位导航领域,具体涉及一种紧耦合车辆轮子编码器数据的视觉惯性测程法与设备,可提高初始化速度和精度以及提高轨迹的准确度。
此外,本发明还提供一种紧耦合车辆轮子编码器数据的视觉惯性测程***。如图3所示,本发明紧耦合车辆轮子编码器数据的视觉惯性测程***包括初始化单元1,获取单元2、相机姿态计算单元3、判断单元4、判断执行单元5、优化单元6及测程单元7。
具体地,所述初始化单元1用于设置所述视觉惯性测程方法的初始化状态为“未完成”;
所述获取单元2用于在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;配置为:一台相机,实时获取图像;一台IMU,实时获取IMU量测值;一套获取车辆上轮子编码器的数据的设备,实时获取轮子编码器读数;一套计时设备,为所述三种数据提供在同一时间基准下的时间戳;
所述相机姿态计算单元3用于计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整;
所述判断单元4用于对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”;
所述判断执行单元5用于在所述第一判断单元的判断结果为初始化的状态为“未完成”时,判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则结束,否则继续判断第二滑动窗口内的图像间的视差是否小于阈值,若是则结束,否则继续;如果判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,若是则结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对参数进行初始化;
所述优化单元6用于在所述第一判断单元的判断结果为初始化的状态为“已完成”时或者所述判断执行单元初始化完成后,在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的参数;
所述测程单元7用于根据所述参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
进一步地,本发明还提供一种紧耦合车辆轮子编码器数据的视觉惯性测程***,包括:
处理器;以及
被安排成存储计算机可执行指令的存储器,所述可执行指令在被执行时使所述处理器执行以下操作:
步骤1:设置所述视觉惯性测程方法的初始化状态为“未完成”,并在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;
步骤2:计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整,所述当前图片为第一帧图像之后的任意图像;
步骤3:对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”,若是则执行步骤5,否则执行步骤4;
步骤4:判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则步骤4结束,否则继续判断第二滑动窗口内的图像间的视差是否小于设定的视差阈值,若是则步骤4结束,否则继续判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,如果是则步骤4结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对参数进行初始化;
步骤5:在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的参数;
步骤6:根据所述参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
进一步地,本发明还提供一种计算机可读存储介质,所述计算机可读存储介质存储一个或多个程序,所述一个或多个程序当被包括多个应用程序的电子设备执行时,使得所述电子设备执行以下操作:
步骤1:设置所述视觉惯性测程方法的初始化状态为“未完成”,并在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;
步骤2:计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整,所述当前图片为第一帧图像之后的任意图像;
步骤3:对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”,若是则执行步骤5,否则执行步骤4;
步骤4:判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则步骤4结束,否则继续判断第二滑动窗口内的图像间的视差是否小于设定的视差阈值,若是则步骤4结束,否则继续判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,如果是则步骤4结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对参数进行初始化;
步骤5:在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的参数;
步骤6:根据所述参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
相对于现有技术,本发明计算机可读存储介质、紧耦合车辆轮子编码器数据的视觉惯性测程***与上述紧耦合车辆轮子编码器数据的视觉惯性测程方法的有益效果相同,在此不再赘述。
至此,已经结合附图所示的优选实施方式描述了本发明的技术方案,但是,本领域技术人员容易理解的是,本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下,本领域技术人员可以对相关技术特征作出等同的更改或替换,这些更改或替换之后的技术方案都将落入本发明的保护范围之内。
Claims (10)
1.一种紧耦合车辆轮子编码器数据的视觉惯性测程方法,其特征在于,所述视觉惯性测程方法包括:
步骤一:设置所述视觉惯性测程方法的初始化状态为“未完成”,并在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;
步骤二:计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整,所述当前图像为第一帧图像之后的任意图像;
步骤三:对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”,若是则执行步骤五,否则执行步骤四;
步骤四:判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则步骤四结束,否则继续判断第二滑动窗口内的图像间的视差是否小于设定的视差阈值,若是则步骤四结束,否则继续判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,如果是则步骤四结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对传感器参数进行初始化;
步骤五:在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的优化后传感器参数;
步骤六:根据所述优化后传感器参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
2.根据权利要求1所述的紧耦合车辆轮子编码器数据的视觉惯性测程方法,其特征在于,所述传感器参数包括传感器位姿、速度、偏移以及匹配点深度。
3.根据权利要求1所述的紧耦合车辆轮子编码器数据的视觉惯性测程方法,其特征在于,所述实时获取相机图像、IMU量测值和轮子编码器读数,具体包括:
获取当前图像的同时,获取当前图像与前一帧图像获取时刻之间的IMU量测值和轮子编码器读数,作为所述当前图像对应的IMU量测值与轮子编码器读数;
其中,所述当前图像、IMU量测值和轮子编码器读数均带有各自的时间戳;使得在对轮子编码器读数进行采样的过程中,采样的时间戳与IMU量测值的时间戳一致。
4.根据权利要求3所述的紧耦合车辆轮子编码器数据的视觉惯性测程方法,其特征在于,步骤二具体包括:
获取当前图像的角点:针对第一帧图像,使用施-托马西方法提取所述第一帧图像的角点;针对除第一帧以外的其他图像,使用光流法跟踪出现在前一帧图像上的角点;
其中,如果前一帧图像的某个角点能够成功跟踪得到所述当前图像上的一个角点,则这两个角点形成所述当前图像与前一帧图像之间的一对匹配点;
利用所述当前图像与前一帧图像之间的所有匹配点对,使用随机抽样一致算法RANSAC框架,利用5点法计算所述当前图像与前一帧图像之间的本质矩阵;如果所述本质矩阵的内点数目超过阈值,则记录所述本质矩阵,并分解所述本质矩阵,得到所述图像与前一帧图像之间的旋转矩阵,同时记录所述本质矩阵有效性为有效;否则记录其有效性为无效;
将所述当前图像同时加入第一滑动窗口和第二滑动窗口,帧序号分别为k+1、m+1,如果k<K则结束,否则判断当前图像加入第一滑动窗口和第二滑动窗口之前的第一滑动窗口内最后一帧图像是否为关键帧;若其不是关键帧则将第一滑动窗口中的该当前图像丢弃,同时将第一滑动窗口中该当前图像对应的所有IMU量测值和轮子编码器读数加入到第一滑动窗口中新加入的当前图像对应的IMU量测值和轮子编码器读数中,第一滑动窗口内最后一帧图像序号减1;若是关键帧,则对第一滑动窗口内最前面一帧图像对应的优化后传感器参数进行边缘化操作得到边缘化误差项;并将所述当前图像和对应的所有IMU量测值和轮子编码器读数丢弃,第一滑动窗口内各帧序号减1,随后将第二滑动窗口内所有时间戳小于当前第一滑动窗口内第1帧图像时间戳的n张图像及其对应的IMU量测值和轮子编码器读数丢弃,同时第二滑动窗口内各帧序号减n;
其中,k和m分别为当前第一滑动窗口和第二滑动窗口中的图像数目,K为第一滑动窗口中能容纳的最多图像数目;
对第一滑动窗口中的最后一帧图像对应的IMU量测值和轮子编码器读数,以及对于第二滑动窗口中的最后一帧图像对应的IMU量测值和轮子编码器读数,进行预积分操作。
5.根据权利要求4所述的紧耦合车辆轮子编码器数据的视觉惯性测程方法,其特征在于,所述进行预积分操作,具体包括:
i依次取0,2…,s-2,依次计算
Ji+1=AiJi
其中,s表示参与预积分的IMU量测值和轮子编码器读数的个数,以及是由四元数表示的旋转,表示四元数的乘法,R(·)表示将一个四元数转换成它对应的旋转矩阵的函数,表示第i个IMU量测值中的三轴加速度,表示第i个IMU量测值中的三轴角速度,表示第i个轮子编码器的读数,ba为当前IMU中加速度计的偏移,bω为当前IMU中陀螺仪的偏移,δti表示第i+1个IMU量测值的时间戳和第i个IMU量测值的时间戳的差值,表示从车体坐标系到IMU坐标系的旋转矩阵,用车体坐标系中的向量得到IMU坐标系中的向量,通过离线标定获得;
其中,车体坐标系的定义为:以左后轮为原点,x轴指向右后轮,y轴指向左前轮,z轴垂直底盘向上。
6.根据权利要求1所述的紧耦合车辆轮子编码器数据的视觉惯性测程方法,其特征在于,步骤四具体包括:
判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目K,若是则步骤四结束;否则执行下一步;
对于第一滑动窗口内的所有图像,如果存在一帧图像与窗口内最后一帧图像有超过m对匹配点,且这些匹配点对的平均视差超过p个像素,且使用这些匹配点对利用RANSAC框架,使用5点法计算本质矩阵,有超过n对匹配点是内点,则执行步骤下一步;否则步骤四结束;
判断第二滑动窗口中所有图像对应的本质矩阵的有效性为有效的个数,若其小于有效的本质矩阵个数的阈值r,则步骤四结束;否则利用第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,使用最小二乘法优化计算陀螺仪的偏移:
其中,bω为待优化的陀螺仪偏移,初值为零向量;F表示第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的本质矩阵有效的帧序号集合,m表示第二滑动窗口中图像序号,为分解本质矩阵得到的旋转矩阵对应的四元数,表示从第二滑动窗口中第m+1帧图像的相机坐标系到第m帧图像的相机坐标系的旋转,是表示从相机坐标系到IMU坐标系的旋转的四元数, 通过预设的离线标定获得,表示四元数的乘法;是一个旋转四元数,为陀螺仪偏移bω的函数,r表示有效的本质矩阵个数的阈值,‖‖2表示向量的2范数:
δbω是bω的更新量,和分别是第二滑动窗口中的第m帧和第m+1帧图像之间IMU-轮子编码器预积分的旋转四元数和雅可比矩阵的一部分,随着第二滑动窗口的滑动,窗口中任意相邻两帧图像之间预积分得到的和均为已知;
对于第二滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数,依次重新进行预积分操作;
对于第二滑动窗口中每两帧连续的图像,构造第一方程lm
其中,和为第二滑动窗口中第m帧和第m+1帧图像之间IMU-轮子编码器预积分的结果;为四元数对应的旋转矩阵形式,Δtm为第二滑动窗口中第m+1帧和第m帧图像之间时间戳的差值;为旋转矩阵;为是在窗口内第一帧图像对应的IMU坐标系下的重力加速度,它的初值由计算,g为当地重力加速度的大小;B是的切平面上的一组正交基;为第m帧图像对应的IMU位置在第m帧图像对应的IMU坐标系下的运动速度,为第m+1帧图像对应的IMU位置在第m+1帧图像对应的IMU坐标系下的运动速度;Δg是重力加速度的更新量;
所述方程记作:
m依次取1,2,…M-1,M是第二滑动窗口中图像的数目,得到M-1个方程,把它们联立得到误差方程:
记作L=AX
X包含的所有元素均为待优化的变量,其中,为第m帧图像对应的IMU位置在第m帧图像对应的IMU坐标系下的运动速度,为第m+1帧图像对应的IMU位置在第m+1帧图像对应的IMU坐标系下的运动速度;Δg是重力加速度的更新量;L和A的所有元素均为已知;
对于第一滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数,依次重新进行预积分操作;
计算一个矩阵使得利用所述预积分结果和k为第一滑动窗口中图像序号,使用航位推算的方法计算表示从第一滑动窗口内第k帧图像对应的IMU坐标系到第一滑动窗口内第一帧图像的对应的IMU坐标系的旋转矩阵以及第一滑动窗口内第k帧图像对应的IMU坐标系原点在第一滑动窗口内第一帧图像对应的IMU坐标系中的位置考虑到第一滑动窗口和第二滑动窗口的第一帧图像相同,且第一滑动窗口内的每一帧图像都在第二滑动窗口内找到和它相同的图像,计算IMU误差项中待优化的传感器参数的初值:
其中,Q(·)表示将一个旋转矩阵转换为对应的四元数的函数,为所述第一方程lm计算出的第二滑动窗口中第m(k)帧图像对应的IMU位置在第m(k)帧图像对应的IMU坐标系下的运动速度,m(k)表示第一滑动窗口中的第k帧图像在第二滑动窗口中对应的图像序号,bω为陀螺仪偏移;
利用所述第一滑动窗口中的图像对应的IMU坐标系的位姿和推算出第一滑动窗口中的图像位姿;利用第一滑动窗口中的图像位姿以及图像上的匹配点进行三角化计算,得到匹配点的深度,将所述深度作为构造重投影误差项所需待优化传感器参数中的匹配点深度传感器参数的初值;
设置初始化的状态为“已完成”。
7.根据权利要求2所述的紧耦合车辆轮子编码器数据的视觉惯性测程方法,其特征在于,步骤五具体包括:
按照基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法在第一滑动窗口中构造代价函数,并进行非线性优化求解,得到传感器位姿、速度和偏移以及匹配点深度的估计值;
其中,所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项;
所述重投影误差项和所述边缘化误差项使用基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法进行构造;
所述IMU误差项使用下述方法进行构造:
其中,CIMU(x)表示IMU误差项;x由x1,x2,…,xk组成;k表示滑动窗口内图像的帧序号;xk是第k帧的状态向量,∑k,k+1表示权利要求5中预积分得到的协方差矩阵∑S-1,随着第一滑动窗口的滑动,窗口中任意相邻两帧图像之间预积分得到的协方差矩阵均为已知;表示残差向量,与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关:
和分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度;和分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度;和分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移;和分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移;是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转;为的旋转矩阵形式;为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转;为的旋转矩阵形式;gw为世界坐标系中的重力加速度,是一个常量;Δtk为第k+1帧和第k帧曝光时刻时间戳的差值;是车体坐标系的原点在IMU坐标系中的坐标,通过计算得到,表示从车体坐标系到IMU坐标系的旋转矩阵,表示IMU的中心在车体坐标系中的坐标,事先量测; 和为第k帧和第k+1帧图像之间IMU-轮子编码器预积分的结果,随着第一滑动窗口的滑动,窗口中任意相邻两帧图像之间预积分得到的 和均为已知;
8.一种紧耦合车辆轮子编码器数据的视觉惯性测程***,其特征在于,所述视觉惯性测程***包括:
初始化单元,用于设置视觉惯性测程***的初始化状态为“未完成”;
获取单元,用于在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;配置为:一台相机,实时获取图像;一台IMU,实时获取IMU量测值;一套获取车辆上轮子编码器的数据的设备,实时获取轮子编码器读数;一套计时设备,为所述相机图像、IMU量测值和轮子编码器读数提供在同一时间基准下的时间戳;
相机姿态计算单元,用于计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整;
判断单元,用于对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”;
判断执行单元,用于在所述判断单元的判断结果为初始化的状态为“未完成”时,判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则结束,否则继续判断第二滑动窗口内的图像间的视差是否小于阈值,若是则结束,否则继续;如果判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,若是则结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对传感器参数进行初始化;
优化单元,用于在所述判断单元的判断结果为初始化的状态为“已完成”时或者所述判断执行单元初始化完成后,在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的优化后传感器参数;
测程单元,用于根据所述优化后传感器参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
9.一种紧耦合车辆轮子编码器数据的视觉惯性测程***,其特征在于,包括:
处理器;以及
被安排成存储计算机可执行指令的存储器,所述可执行指令在被执行时使所述处理器执行以下操作:
步骤一:设置视觉惯性测程***的初始化状态为“未完成”,并在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;
步骤二:计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整,所述当前图像为第一帧图像之后的任意图像;
步骤三:对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”,若是则执行步骤五,否则执行步骤四;
步骤四:判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则步骤四结束,否则继续判断第二滑动窗口内的图像间的视差是否小于设定的视差阈值,若是则步骤四结束,否则继续判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,如果是则步骤四结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对传感器参数进行初始化;
步骤五:在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的优化后传感器参数;
步骤六:根据所述优化后传感器参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质存储一个或多个程序,所述一个或多个程序当被包括多个应用程序的电子设备执行时,使得所述电子设备执行以下操作:
步骤一:设置视觉惯性测程方法的初始化状态为“未完成”,并在初始化和测程的过程中,实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数;
步骤二:计算当前图像与前一帧图像之间的本质矩阵,将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口,同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整,所述当前图像为第一帧图像之后的任意图像;
步骤三:对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数进行IMU-轮子编码器预积分;判断初始化的状态是否为“已完成”,若是则执行步骤五,否则执行步骤四;
步骤四:判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目,若是则步骤四结束,否则继续判断第二滑动窗口内的图像间的视差是否小于设定的视差阈值,若是则步骤四结束,否则继续判断第二滑动窗口中有效的本质矩阵个数是否小于设定的个数阈值,如果是则步骤四结束,否则根据第二滑动窗口中的从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵,以及第二滑动窗口中IMU-轮子编码器预积分的结果,标定IMU中的陀螺仪偏移的初始值;利用陀螺仪偏移对第二滑动窗口内每张图像对应的IMU量测值和轮子编码器读数重新进行IMU-轮子编码器预积分;利用预积分结果对传感器参数进行初始化;
步骤五:在第一滑动窗口内计算包含重投影误差项、IMU-轮子编码器误差项和边缘化误差项的代价函数,将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化,得到第一滑动窗口内各张图像对应的优化后传感器参数;
步骤六:根据所述优化后传感器参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910127778.1A CN109764880B (zh) | 2019-02-19 | 2019-02-19 | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910127778.1A CN109764880B (zh) | 2019-02-19 | 2019-02-19 | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109764880A CN109764880A (zh) | 2019-05-17 |
CN109764880B true CN109764880B (zh) | 2020-12-25 |
Family
ID=66456923
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910127778.1A Active CN109764880B (zh) | 2019-02-19 | 2019-02-19 | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109764880B (zh) |
Families Citing this family (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110260861B (zh) * | 2019-06-13 | 2021-07-27 | 北京华捷艾米科技有限公司 | 位姿确定方法及装置、里程计 |
CN110428452B (zh) * | 2019-07-11 | 2022-03-25 | 北京达佳互联信息技术有限公司 | 非静态场景点的检测方法、装置、电子设备及存储介质 |
CN110956665B (zh) * | 2019-12-18 | 2023-06-23 | 中国科学院自动化研究所 | 车辆拐弯轨迹双向计算方法、***、装置 |
CN111145267B (zh) * | 2019-12-25 | 2023-09-05 | 重庆自行者科技有限公司 | 基于imu辅助的360度全景视图多相机标定方法 |
CN111795686B (zh) * | 2020-06-08 | 2024-02-02 | 南京大学 | 一种移动机器人定位与建图的方法 |
CN111753938B (zh) * | 2020-06-23 | 2021-12-24 | 联想(北京)有限公司 | 一种位置获取方法、装置及电子设备 |
CN112815939B (zh) * | 2021-01-04 | 2024-02-23 | 清华大学深圳国际研究生院 | 移动机器人的位姿估计方法及计算机可读存储介质 |
CN113074754A (zh) * | 2021-03-27 | 2021-07-06 | 上海智能新能源汽车科创功能平台有限公司 | 一种基于车辆运动学约束的视觉惯性slam***初始化方法 |
CN113295089B (zh) * | 2021-04-07 | 2024-04-26 | 深圳市异方科技有限公司 | 一种基于视觉惯性slam的车厢车厢容积率测量方法 |
CN113793381A (zh) * | 2021-07-27 | 2021-12-14 | 武汉中海庭数据技术有限公司 | 单目视觉信息和轮速信息融合的定位方法及*** |
CN113962115B (zh) * | 2021-12-23 | 2022-04-05 | 深圳佑驾创新科技有限公司 | 车辆轮胎系数图优化标定方法、装置、设备及存储介质 |
CN115855117B (zh) * | 2023-02-16 | 2023-06-02 | 深圳佑驾创新科技有限公司 | 相机和惯性测量单元相对车体安装姿态的联合标定方法 |
CN117168530B (zh) * | 2023-11-03 | 2024-01-26 | 泉州昆泰芯微电子科技有限公司 | 磁编码器的自校准方法、磁编码器及电机 |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102155955A (zh) * | 2011-03-11 | 2011-08-17 | 天津理工大学 | 立体视觉里程计及测量方法 |
US9709404B2 (en) * | 2015-04-17 | 2017-07-18 | Regents Of The University Of Minnesota | Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation |
KR101985344B1 (ko) * | 2017-06-26 | 2019-06-04 | 서울대학교산학협력단 | 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치 |
CN107869989B (zh) * | 2017-11-06 | 2020-02-07 | 东北大学 | 一种基于视觉惯导信息融合的定位方法及*** |
CN108629793B (zh) * | 2018-03-22 | 2020-11-10 | 中国科学院自动化研究所 | 使用在线时间标定的视觉惯性测程法与设备 |
CN108717712B (zh) * | 2018-05-29 | 2021-09-03 | 东北大学 | 一种基于地平面假设的视觉惯导slam方法 |
CN109029448B (zh) * | 2018-06-28 | 2021-11-12 | 东南大学 | 单目视觉惯性定位的imu辅助跟踪模型 |
-
2019
- 2019-02-19 CN CN201910127778.1A patent/CN109764880B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN109764880A (zh) | 2019-05-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109764880B (zh) | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及*** | |
CN111811506B (zh) | 视觉/惯性里程计组合导航方法、电子设备及存储介质 | |
CN109540126B (zh) | 一种基于光流法的惯性视觉组合导航方法 | |
CN111024066B (zh) | 一种无人机视觉-惯性融合室内定位方法 | |
CN110956665B (zh) | 车辆拐弯轨迹双向计算方法、***、装置 | |
Ahmed et al. | Accurate attitude estimation of a moving land vehicle using low-cost MEMS IMU sensors | |
CN111795686B (zh) | 一种移动机器人定位与建图的方法 | |
CN107941217B (zh) | 一种机器人定位方法、电子设备、存储介质、装置 | |
CN107909614B (zh) | 一种gps失效环境下巡检机器人定位方法 | |
US8447116B2 (en) | Identifying true feature matches for vision based navigation | |
CN106052584B (zh) | 一种基于视觉及惯性信息融合的轨道空间线形测量方法 | |
CN112815939B (zh) | 移动机器人的位姿估计方法及计算机可读存储介质 | |
CN111380514A (zh) | 机器人位姿估计方法、装置、终端及计算机存储介质 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN109141411B (zh) | 定位方法、定位装置、移动机器人及存储介质 | |
CN115371665B (zh) | 一种基于深度相机和惯性融合的移动机器人定位方法 | |
CN111220155A (zh) | 基于双目视觉惯性里程计估计位姿的方法、装置与处理器 | |
JP2014240266A (ja) | センサドリフト量推定装置及びプログラム | |
CN112179373A (zh) | 一种视觉里程计的测量方法及视觉里程计 | |
CN112284381B (zh) | 视觉惯性实时初始化对准方法及*** | |
CN114690229A (zh) | 一种融合gps的移动机器人视觉惯性导航方法 | |
CN108827287B (zh) | 一种复杂环境下的鲁棒视觉slam*** | |
CN111539982B (zh) | 一种移动平台中基于非线性优化的视觉惯导初始化方法 | |
CN113450411B (zh) | 一种基于方差分量估计理论的实时自身位姿计算方法 | |
CN115452003A (zh) | 一种微机电陀螺非线性偏置在线估计方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
CB03 | Change of inventor or designer information |
Inventor after: Gao Wei Inventor after: Liu Jinxu Inventor after: Hu Zhanyi Inventor before: Liu Jinxu Inventor before: Gao Wei Inventor before: Hu Zhanyi |
|
CB03 | Change of inventor or designer information | ||
GR01 | Patent grant | ||
GR01 | Patent grant |