CN109764880B - 紧耦合车辆轮子编码器数据的视觉惯性测程方法及*** - Google Patents

紧耦合车辆轮子编码器数据的视觉惯性测程方法及*** Download PDF

Info

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
Application number
CN201910127778.1A
Other languages
English (en)
Other versions
CN109764880A (zh
Inventor
高伟
刘津旭
胡占义
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Automation of Chinese Academy of Science
Original Assignee
Institute of Automation of Chinese Academy of Science
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Institute of Automation of Chinese Academy of Science filed Critical Institute of Automation of Chinese Academy of Science
Priority to CN201910127778.1A priority Critical patent/CN109764880B/zh
Publication of CN109764880A publication Critical patent/CN109764880A/zh
Application granted granted Critical
Publication of CN109764880B publication Critical patent/CN109764880B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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，依次计算
Figure GDA0002727288530000041
Figure GDA0002727288530000042
Figure GDA0002727288530000043
Figure GDA0002727288530000051
Figure GDA0002727288530000052
Figure GDA0002727288530000053
Figure GDA0002727288530000054
Ji+1＝AiJi
Figure GDA00027272885300000510
取值为Ji+1的第7行到第9行、第16列到第18列构成的矩阵；
直到最终结果
Figure GDA0002727288530000055
∑s-1和
Figure GDA00027272885300000511
被计算出来；
其中，s表示参与预积分的IMU量测值和轮子编码器读数的个数，
Figure GDA00027272885300000512
以及
Figure GDA00027272885300000513
是由四元数表示的旋转，
Figure GDA0002727288530000056
表示四元数的乘法，R(·)表示将一个四元数转换成它对应的旋转矩阵的函数，
Figure GDA0002727288530000057
表示第i个IMU量测值中的三轴加速度，
Figure GDA0002727288530000058
表示第i个IMU量测值中的三轴角速度，
Figure GDA0002727288530000059
表示第i个轮子编码器的读数，ba为当前IMU中加速度计的偏移，bω为当前IMU中陀螺仪的偏移，δti表示第i+1个IMU量测值的时间戳和第i个IMU量测值的时间戳的差值，
Figure GDA00027272885300000514
表示从车体坐标系到IMU坐标系的旋转矩阵，用车体坐标系中的向量得到IMU坐标系中的向量，通过离线标定获得；
其中，车体坐标系的定义为：以左后轮为原点，x轴指向右后轮，y轴指向左前轮，z轴垂直底盘向上。
可选的，步骤四具体包括：
判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目K，若是则步骤四结束；否则执行下一步；
对于第一滑动窗口内的所有图像，如果存在一帧图像与窗口内最后一帧图像有超过m对匹配点，且这些匹配点对的平均视差超过p个像素，且使用这些匹配点对利用RANSAC框架，使用5点法计算本质矩阵，有超过n对匹配点是内点，则执行步骤下一步；否则步骤四结束；
判断第二滑动窗口中所有图像对应的本质矩阵的有效性为有效的个数，若其小于有效的本质矩阵个数的阈值r，则步骤四结束；否则利用第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵，以及第二滑动窗口中IMU-轮子编码器预积分的结果，使用最小二乘法优化计算陀螺仪的偏移：
Figure GDA0002727288530000061
其中，bω为待优化的陀螺仪偏移，初值为零向量；F表示第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的本质矩阵有效的帧序号集合，m表示第二滑动窗口中图像序号，
Figure GDA0002727288530000064
为分解本质矩阵得到的旋转矩阵对应的四元数，表示从第二滑动窗口中第m+1帧图像的相机坐标系到第m帧图像的相机坐标系的旋转，
Figure GDA0002727288530000065
是表示从相机坐标系到IMU坐标系的旋转的四元数，
Figure GDA0002727288530000062
Figure GDA0002727288530000066
可以事先通过离线标定获得，
Figure GDA0002727288530000063
表示四元数的乘法；
Figure GDA0002727288530000067
是一个旋转四元数，为陀螺仪偏移bω的函数，r表示有效的本质矩阵个数的阈值，‖ ‖2表示向量的2范数：
Figure GDA0002727288530000071
δbω是bω的更新量，
Figure GDA0002727288530000075
和
Figure GDA0002727288530000076
分别是第二滑动窗口中的第m帧和第m+1帧图像之间IMU-轮子编码器预积分的旋转四元数和雅可比矩阵的一部分，随着第二滑动窗口的滑动，窗口中任意相邻两帧图像之间预积分得到的
Figure GDA0002727288530000077
和
Figure GDA0002727288530000078
均为已知；
对于第二滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数，依次重新进行预积分操作，其中陀螺仪偏移bω为优化得到的值；
利用计算得到的预积分结果，依次计算从第二滑动窗口内第一帧到第m帧的旋转矩阵
Figure GDA0002727288530000072
Figure GDA0002727288530000073
其中m依次取1,2,…,M，M是第二滑动窗口中图像的数目；
Figure GDA0002727288530000079
为单位阵；
对于第二滑动窗口中每两帧连续的图像，构造方程
Figure GDA0002727288530000074
其中，
Figure GDA00027272885300000710
和
Figure GDA00027272885300000711
为第二滑动窗口中第m帧和第m+1帧图像之间IMU-轮子编码器预积分的结果；
Figure GDA00027272885300000712
为四元数
Figure GDA00027272885300000713
对应的旋转矩阵形式，Δtm为第二滑动窗口中第m+1帧和第m帧图像之间时间戳的差值；
Figure GDA00027272885300000811
为旋转矩阵；
Figure GDA00027272885300000812
为
Figure GDA00027272885300000813
是在窗口内第一帧图像对应的IMU坐标系下的重力加速度，它的初值可以由
Figure GDA00027272885300000814
计算，g为当地重力加速度的大小；B是
Figure GDA00027272885300000815
的切平面上的一组正交基；
Figure GDA0002727288530000081
为第m帧图像对应的IMU位置在第m帧图像对应的IMU坐标系下的运动速度，
Figure GDA00027272885300000816
为第m+1帧图像对应的IMU位置在第m+1帧图像对应的IMU坐标系下的运动速度；Δg是重力加速度的更新量；
所述方程可以记作：
Figure GDA0002727288530000082
m依次取1,2,…M-1，M是第二滑动窗口中图像的数目,可以得到M-1个方程，把它们联立得到误差方程：
Figure GDA0002727288530000083
记作L＝AX
X包含的所有元素均为待优化的变量，L和A的所有元素均为已知；
求解
Figure GDA0002727288530000084
利用X中的Δg更新
Figure GDA0002727288530000085
Figure GDA0002727288530000086
利用更新后的
Figure GDA0002727288530000087
重新计算它的切空间的正交基B，并利用新的
Figure GDA0002727288530000088
和B重新计算L和A，重复本步骤，迭代计算；当迭代次数达到预先设定的迭代次数阈值时，步骤四结束；记录此时的
Figure GDA0002727288530000089
和
Figure GDA00027272885300000810
对于第一滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数，依次重新进行预积分操作；
计算一个矩阵
Figure GDA0002727288530000091
使得
Figure GDA0002727288530000092
利用所述预积分结果
Figure GDA00027272885300000911
和
Figure GDA0002727288530000093
k为第一滑动窗口中图像序号，使用航位推算的方法计算表示从第一滑动窗口内第k帧图像对应的IMU坐标系到第一滑动窗口内第一帧图像的对应的IMU坐标系的旋转矩阵
Figure GDA00027272885300000912
以及第一滑动窗口内第k帧图像对应的IMU坐标系原点在第一滑动窗口内第一帧图像对应的IMU坐标系中的位置
Figure GDA0002727288530000094
考虑到第一滑动窗口和第二滑动窗口的第一帧图像相同，且第一滑动窗口内的每一帧图像都可以在第二滑动窗口内找到和它相同的图像，计算IMU误差项中待优化的参数的初值：
Figure GDA0002727288530000095
Figure GDA0002727288530000096
Figure GDA0002727288530000097
Figure GDA0002727288530000098
Figure GDA0002727288530000099
其中，Q(·)表示将一个旋转矩阵转换为对应的四元数的函数，
Figure GDA00027272885300000913
为步骤S4006中计算出的第二滑动窗口中第m(k)帧图像对应的IMU位置在第m(k)帧图像对应的IMU坐标系下的运动速度，m(k)表示第一滑动窗口中的第k帧图像在第二滑动窗口中对应的图像序号，bω为陀螺仪偏移；
利用所述第一滑动窗口中的图像对应的IMU坐标系的位姿
Figure GDA00027272885300000914
和
Figure GDA00027272885300000910
可以推算出第一滑动窗口中的图像位姿；利用第一滑动窗口中的图像位姿以及图像上的匹配点进行三角化计算，得到匹配点的深度，将所述深度作为构造重投影误差项所需待优化参数中的匹配点深度参数的初值；
设置初始化的状态为“已完成”。
可选的，步骤五具体包括：
按照基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法在第一滑动窗口中构造代价函数，并进行非线性优化求解，得到传感器位姿、速度和偏移以及匹配点深度的估计值；
其中，所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项；
所述重投影误差项和所述边缘化误差项使用基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法进行构造；
所述IMU误差项使用下述方法进行构造：
Figure GDA0002727288530000101
其中，CIMU(x)表示IMU误差项；x由x1,x2,…,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，∑k,k+1表示在步骤S2006中预积分得到的协方差矩阵，随着第一滑动窗口的滑动，窗口中任意相邻两帧图像之间预积分得到的协方差矩阵均为已知；
Figure GDA0002727288530000102
表示残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关：
Figure GDA0002727288530000103
Figure GDA0002727288530000119
和
Figure GDA00027272885300001110
分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度；
Figure GDA00027272885300001111
和
Figure GDA00027272885300001112
分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度；
Figure GDA0002727288530000111
和
Figure GDA0002727288530000112
分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure GDA00027272885300001113
和
Figure GDA00027272885300001114
分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure GDA00027272885300001115
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；
Figure GDA00027272885300001116
为
Figure GDA00027272885300001117
的旋转矩阵形式；
Figure GDA00027272885300001118
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；
Figure GDA00027272885300001119
为
Figure GDA00027272885300001120
的旋转矩阵形式；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；
Figure GDA00027272885300001121
是车体坐标系的原点在IMU坐标系中的坐标，通过
Figure GDA0002727288530000113
计算得到，
Figure GDA0002727288530000114
表示从车体坐标系到IMU坐标系的旋转矩阵，
Figure GDA0002727288530000115
表示IMU的中心在车体坐标系中的坐标，事先量测；
Figure GDA0002727288530000116
和
Figure GDA00027272885300001122
为第k帧和第k+1帧图像之间IMU-轮子编码器预积分的结果，随着第一滑动窗口的滑动，窗口中任意相邻两帧图像之间预积分得到的
Figure GDA0002727288530000117
和
Figure GDA00027272885300001123
均为已知；
Figure GDA0002727288530000118
和
Figure GDA00027272885300001124
均为待优化的参数。
为解决上述问题，本发明还提供另一种技术方案：
一种紧耦合车辆轮子编码器数据的视觉惯性测程***，所述包括视觉惯性测程***包括：
初始化单元，用于设置所述视觉惯性测程方法的初始化状态为“未完成”；
获取单元，用于在初始化和测程的过程中，实时获取相机图像、惯性测量单元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，依次计算
Figure GDA0002727288530000211
Figure GDA0002727288530000212
Figure GDA0002727288530000213
Figure GDA0002727288530000214
Figure GDA0002727288530000215
Figure GDA0002727288530000216
Figure GDA0002727288530000217
Ji+1＝AiJi；
Figure GDA00027272885300002112
取值为Ji+1的第7行到第9行、第16列到第18列构成的矩阵；
直到最终结果
Figure GDA0002727288530000218
∑s-1和
Figure GDA00027272885300002113
被计算出来；
其中，s表示参与预积分的IMU量测值和轮子编码器读数的个数，
Figure GDA00027272885300002114
以及
Figure GDA00027272885300002115
是由四元数表示的旋转，
Figure GDA0002727288530000219
表示四元数的乘法，R(·)表示将一个四元数转换成它对应的旋转矩阵的函数，
Figure GDA00027272885300002110
表示第i个IMU量测值中的三轴加速度，
Figure GDA00027272885300002111
表示第i个IMU量测值中的三轴角速度，
Figure GDA0002727288530000221
表示第i个轮子编码器的读数，ba为当前IMU中加速度计的偏移，bω为当前IMU中陀螺仪的偏移，δti表示第i+1个IMU量测值的时间戳和第i个IMU量测值的时间戳的差值，
Figure GDA0002727288530000223
表示从车体坐标系到IMU坐标系的旋转矩阵，用它左乘车体坐标系中的向量得到IMU坐标系中的向量，可以通过离线标定获得；
车体坐标系的定义为：以左后轮为原点，x轴指向右后轮，y轴指向左前轮，z轴垂直底盘向上。
步骤S4000具体包括：
步骤S4001，判断第一滑动窗口内的图像数目，若其小于第一滑动窗口中容纳的最大图像数目K则结束步骤S4000；否则步骤S4000结束；
步骤S4002，对于第一滑动窗口内的所有图像，如果存在一帧图像与窗口内最后一帧图像有超过m对匹配点，且这些匹配点对的平均视差超过p个像素，且使用这些匹配点对利用RANSAC框架，使用5点法计算本质矩阵，有超过n对匹配点是内点，则执行步骤下一步；否则步骤S4000；
步骤S4003，判断第二滑动窗口中所有图像对应的本质矩阵的有效性为有效的个数，若其小于有效的本质矩阵个数的阈值r，则结束；否则利用第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵，以及第二滑动窗口中IMU-轮子编码器预积分的结果，使用最小二乘法优化计算陀螺仪的偏移：
Figure GDA0002727288530000222
其中，bω为待优化的陀螺仪偏移，初值为零向量；F表示第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的本质矩阵有效的帧序号集合，m表示第二滑动窗口中图像序号，
Figure GDA0002727288530000224
为分解本质矩阵得到的旋转矩阵对应的四元数，表示从第二滑动窗口中第m+1帧图像的相机坐标系到第m帧图像的相机坐标系的旋转，
Figure GDA0002727288530000236
是表示从相机坐标系到IMU坐标系的旋转的四元数，
Figure GDA0002727288530000231
Figure GDA0002727288530000237
可以事先通过离线标定获得，
Figure GDA0002727288530000232
表示四元数的乘法；
Figure GDA0002727288530000238
是一个旋转四元数，它是陀螺仪偏移bω的函数：
Figure GDA0002727288530000233
δbω是bω的更新量，
Figure GDA0002727288530000239
和
Figure GDA00027272885300002310
分别是第二滑动窗口中的第m帧和第m+1帧图像之间IMU-轮子编码器预积分的旋转四元数和雅可比矩阵的一部分，随着第二滑动窗口的滑动，窗口中任意相邻两帧图像之间预积分得到的
Figure GDA00027272885300002311
和
Figure GDA00027272885300002312
均为已知；
步骤S4004，对于第二滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数，依次重新进行预积分操作，其中陀螺仪偏移bω为优化得到的值。
步骤S4005，利用计算得到的预积分结果，依次计算从第二滑动窗口内第一帧到第m帧的旋转矩阵
Figure GDA0002727288530000234
Figure GDA0002727288530000235
其中m依次取1,2,…,M，M是第二滑动窗口中图像的数目；
Figure GDA00027272885300002313
为单位阵；
步骤S4006，对于第二滑动窗口中每两帧连续的图像，构造方程
Figure GDA0002727288530000241
其中，
Figure GDA0002727288530000245
和
Figure GDA0002727288530000246
为第二滑动窗口中第m帧和第m+1帧图像之间IMU-轮子编码器预积分的结果；
Figure GDA0002727288530000247
为四元数
Figure GDA0002727288530000248
对应的旋转矩阵形式，Δtm为第二滑动窗口中第m+1帧和第m帧图像之间时间戳的差值；
Figure GDA0002727288530000249
为旋转矩阵；
Figure GDA00027272885300002410
为
Figure GDA00027272885300002411
是在窗口内第一帧图像对应的IMU坐标系下的重力加速度，它的初值可以由
Figure GDA00027272885300002412
计算，g为当地重力加速度的大小；B是
Figure GDA00027272885300002413
的切平面上的一组正交基；
Figure GDA0002727288530000242
为第m帧图像对应的IMU位置在第m帧图像对应的IMU坐标系下的运动速度，
Figure GDA00027272885300002414
为第m+1帧图像对应的IMU位置在第m+1帧图像对应的IMU坐标系下的运动速度；Δg是重力加速度的更新量；
所述方程可以记作
Figure GDA0002727288530000243
m依次取1,2,…M-1，M是第二滑动窗口中图像的数目,可以得到M-1个方程，把它们联立得到误差方程：
Figure GDA0002727288530000244
记作L＝AX
X包含的所有元素均为待优化的变量，L和A的所有元素均为已知；
求解
Figure GDA0002727288530000251
利用X中的Δg更新
Figure GDA0002727288530000252
Figure GDA0002727288530000253
利用更新后的
Figure GDA0002727288530000254
重新计算它的切空间的正交基B，并利用新的
Figure GDA0002727288530000255
和B重新计算L和A，重复本步骤，迭代计算；当迭代次数达到预先设定的迭代次数阈值时，步骤S4000结束；记录此时的
Figure GDA0002727288530000256
和
Figure GDA0002727288530000257
步骤S4007，对于第一滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数，依次重新进行预积分操作，其中陀螺仪偏移bω使用步骤S4003中优化得到的值；
步骤S4008，计算一个矩阵
Figure GDA0002727288530000258
使得
Figure GDA0002727288530000259
利用所述预积分结果
Figure GDA00027272885300002512
和
Figure GDA00027272885300002510
k为第一滑动窗口中图像序号，使用航位推算的方法计算表示从第一滑动窗口内第k帧图像对应的IMU坐标系到第一滑动窗口内第一帧图像的对应的IMU坐标系的旋转矩阵
Figure GDA00027272885300002513
以及第一滑动窗口内第k帧图像对应的IMU坐标系原点在第一滑动窗口内第一帧图像对应的IMU坐标系中的位置
Figure GDA00027272885300002511
考虑到第一滑动窗口和第二滑动窗口的第一帧图像相同，且第一滑动窗口内的每一帧图像都可以在第二滑动窗口内找到和它相同的图像，计算IMU误差项中待优化的参数的初值：
Figure GDA0002727288530000261
Figure GDA0002727288530000262
Figure GDA0002727288530000263
Figure GDA0002727288530000264
Figure GDA0002727288530000265
其中，Q(·)表示将一个旋转矩阵转换为对应的四元数的函数，
Figure GDA0002727288530000267
为步骤S4006中计算出的第二滑动窗口中第m(k)帧图像对应的IMU位置在第m(k)帧图像对应的IMU坐标系下的运动速度，m(k)表示第一滑动窗口中的第k帧图像在第二滑动窗口中对应的图像序号，bω为计算出的陀螺仪偏移；
利用所述第一滑动窗口中的图像对应的IMU坐标系的位姿
Figure GDA0002727288530000268
和
Figure GDA0002727288530000266
可以推算出第一滑动窗口中的图像位姿；利用第一滑动窗口中的图像位姿以及图像上的匹配点进行三角化计算，得到匹配点的深度，将所述深度作为构造重投影误差项所需待优化参数中的匹配点深度参数的初值。
步骤S4009，设置本测程法的初始化的状态为“已完成”。
在本实施例中，m＝20,p＝14,n＝10,r＝3,c＝4。
进一步地，步骤S5000，具体包括：
按照基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法在第一滑动窗口中构造代价函数，使用Dogleg算法进行非线性优化求解，得到传感器位姿、速度和偏移以及匹配点深度的估计值。
其中，所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项；
所述重投影误差项和所述边缘化误差项使用基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法进行构造；
所述IMU误差项使用下述方法进行构造：
Figure GDA0002727288530000271
其中，CIMU(x)表示IMU误差项；x由x1,x2,…,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，∑k,k+1表示在步骤S2006中预积分得到的协方差矩阵，随着第一滑动窗口的滑动，窗口中任意相邻两帧图像之间预积分得到的协方差矩阵均为已知。
Figure GDA0002727288530000272
表示残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关：
Figure GDA0002727288530000273
Figure GDA0002727288530000276
和
Figure GDA0002727288530000277
分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度；
Figure GDA0002727288530000278
和
Figure GDA0002727288530000279
分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度；
Figure GDA0002727288530000274
和
Figure GDA0002727288530000275
分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure GDA00027272885300002710
和
Figure GDA00027272885300002711
分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure GDA00027272885300002712
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；
Figure GDA00027272885300002713
为
Figure GDA00027272885300002714
的旋转矩阵形式；
Figure GDA00027272885300002715
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；
Figure GDA00027272885300002716
为
Figure GDA00027272885300002717
的旋转矩阵形式；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；
Figure GDA0002727288530000285
是车体坐标系的原点在IMU坐标系中的坐标，它可以通过
Figure GDA0002727288530000281
计算得到，
Figure GDA0002727288530000282
表示IMU的中心在车体坐标系中的坐标，可以事先量测。
Figure GDA0002727288530000283
和
Figure GDA0002727288530000286
为第k帧和第k+1帧图像之间IMU-轮子编码器预积分的结果，随着第一滑动窗口的滑动，窗口中任意相邻两帧图像之间预积分得到的
Figure GDA0002727288530000284
和
Figure GDA0002727288530000287
均为已知；
Figure GDA0002727288530000288
和
Figure GDA0002727288530000289
均为待优化的参数。
表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，依次计算
Figure FDA0002745554800000041
Figure FDA0002745554800000042
Figure FDA0002745554800000043
Figure FDA0002745554800000044
Figure FDA0002745554800000045
Figure FDA0002745554800000046
Figure FDA0002745554800000047
Ji+1＝AiJi
Figure FDA0002745554800000048
取值为Ji+1的第7行到第9行、第16列到第18列构成的矩阵；
直到最终结果
Figure FDA0002745554800000049
∑S-1和
Figure FDA00027455548000000410
被计算出来；
其中，s表示参与预积分的IMU量测值和轮子编码器读数的个数，
Figure FDA00027455548000000411
以及
Figure FDA00027455548000000412
是由四元数表示的旋转，
Figure FDA00027455548000000413
表示四元数的乘法，R(·)表示将一个四元数转换成它对应的旋转矩阵的函数，
Figure FDA00027455548000000414
表示第i个IMU量测值中的三轴加速度，
Figure FDA00027455548000000415
表示第i个IMU量测值中的三轴角速度，
Figure FDA00027455548000000416
表示第i个轮子编码器的读数，ba为当前IMU中加速度计的偏移，bω为当前IMU中陀螺仪的偏移，δti表示第i+1个IMU量测值的时间戳和第i个IMU量测值的时间戳的差值，
Figure FDA0002745554800000051
表示从车体坐标系到IMU坐标系的旋转矩阵，用车体坐标系中的向量得到IMU坐标系中的向量，通过离线标定获得；
其中，车体坐标系的定义为：以左后轮为原点，x轴指向右后轮，y轴指向左前轮，z轴垂直底盘向上。
6.根据权利要求1所述的紧耦合车辆轮子编码器数据的视觉惯性测程方法，其特征在于，步骤四具体包括：
判断第一滑动窗口内的图像数目是否小于第一滑动窗口中容纳的最大图像数目K，若是则步骤四结束；否则执行下一步；
对于第一滑动窗口内的所有图像，如果存在一帧图像与窗口内最后一帧图像有超过m对匹配点，且这些匹配点对的平均视差超过p个像素，且使用这些匹配点对利用RANSAC框架，使用5点法计算本质矩阵，有超过n对匹配点是内点，则执行步骤下一步；否则步骤四结束；
判断第二滑动窗口中所有图像对应的本质矩阵的有效性为有效的个数，若其小于有效的本质矩阵个数的阈值r，则步骤四结束；否则利用第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的有效的本质矩阵，以及第二滑动窗口中IMU-轮子编码器预积分的结果，使用最小二乘法优化计算陀螺仪的偏移：
Figure FDA0002745554800000052
其中，bω为待优化的陀螺仪偏移，初值为零向量；F表示第二滑动窗口中从第二帧图像起每一帧图像与它的前一帧图像之间的本质矩阵有效的帧序号集合，m表示第二滑动窗口中图像序号，
Figure FDA0002745554800000053
为分解本质矩阵得到的旋转矩阵对应的四元数，表示从第二滑动窗口中第m+1帧图像的相机坐标系到第m帧图像的相机坐标系的旋转，
Figure FDA0002745554800000054
是表示从相机坐标系到IMU坐标系的旋转的四元数，
Figure FDA0002745554800000061
Figure FDA0002745554800000062
通过预设的离线标定获得，
Figure FDA0002745554800000063
表示四元数的乘法；
Figure FDA0002745554800000064
是一个旋转四元数，为陀螺仪偏移bω的函数，r表示有效的本质矩阵个数的阈值，‖‖2表示向量的2范数：
Figure FDA0002745554800000065
δbω是bω的更新量，
Figure FDA0002745554800000066
和
Figure FDA0002745554800000067
分别是第二滑动窗口中的第m帧和第m+1帧图像之间IMU-轮子编码器预积分的旋转四元数和雅可比矩阵的一部分，随着第二滑动窗口的滑动，窗口中任意相邻两帧图像之间预积分得到的
Figure FDA0002745554800000068
和
Figure FDA0002745554800000069
均为已知；
对于第二滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数，依次重新进行预积分操作；
利用计算得到的预积分结果，依次计算从第二滑动窗口内第一帧到第m帧的旋转矩阵
Figure FDA00027455548000000610
Figure FDA00027455548000000611
其中m依次取1,2,…,M，M是第二滑动窗口中图像的数目；
Figure FDA00027455548000000612
为单位阵；
对于第二滑动窗口中每两帧连续的图像，构造第一方程lm
Figure FDA00027455548000000613
其中，
Figure FDA0002745554800000071
和
Figure FDA0002745554800000072
为第二滑动窗口中第m帧和第m+1帧图像之间IMU-轮子编码器预积分的结果；
Figure FDA0002745554800000073
为四元数
Figure FDA0002745554800000074
对应的旋转矩阵形式，Δtm为第二滑动窗口中第m+1帧和第m帧图像之间时间戳的差值；
Figure FDA0002745554800000075
为旋转矩阵；
Figure FDA0002745554800000076
为
Figure FDA0002745554800000077
是在窗口内第一帧图像对应的IMU坐标系下的重力加速度，它的初值由
Figure FDA0002745554800000078
计算，g为当地重力加速度的大小；B是
Figure FDA0002745554800000079
的切平面上的一组正交基；
Figure FDA00027455548000000710
为第m帧图像对应的IMU位置在第m帧图像对应的IMU坐标系下的运动速度，
Figure FDA00027455548000000711
为第m+1帧图像对应的IMU位置在第m+1帧图像对应的IMU坐标系下的运动速度；Δg是重力加速度的更新量；
所述方程记作：
Figure FDA00027455548000000712
m依次取1,2,…M-1，M是第二滑动窗口中图像的数目,得到M-1个方程，把它们联立得到误差方程：
Figure FDA00027455548000000713
记作L＝AX
X包含的所有元素均为待优化的变量，其中，
Figure FDA00027455548000000714
为第m帧图像对应的IMU位置在第m帧图像对应的IMU坐标系下的运动速度，
Figure FDA00027455548000000715
为第m+1帧图像对应的IMU位置在第m+1帧图像对应的IMU坐标系下的运动速度；Δg是重力加速度的更新量；L和A的所有元素均为已知；
求解
Figure FDA00027455548000000716
利用X中的Δg更新
Figure FDA00027455548000000717
Figure FDA0002745554800000081
利用更新后的
Figure FDA0002745554800000082
重新计算它的切空间的正交基B，并利用新的
Figure FDA0002745554800000083
和B重新计算L和A，重复本步骤，迭代计算；当迭代次数达到预先设定的迭代次数阈值时，步骤四结束；记录此时的
Figure FDA0002745554800000084
和
Figure FDA0002745554800000085
对于第一滑动窗口中的每一帧图像对应的IMU量测值和轮子编码器读数，依次重新进行预积分操作；
计算一个矩阵
Figure FDA0002745554800000086
使得
Figure FDA0002745554800000087
利用所述预积分结果
Figure FDA0002745554800000088
和
Figure FDA0002745554800000089
k为第一滑动窗口中图像序号，使用航位推算的方法计算表示从第一滑动窗口内第k帧图像对应的IMU坐标系到第一滑动窗口内第一帧图像的对应的IMU坐标系的旋转矩阵
Figure FDA00027455548000000810
以及第一滑动窗口内第k帧图像对应的IMU坐标系原点在第一滑动窗口内第一帧图像对应的IMU坐标系中的位置
Figure FDA00027455548000000811
考虑到第一滑动窗口和第二滑动窗口的第一帧图像相同，且第一滑动窗口内的每一帧图像都在第二滑动窗口内找到和它相同的图像，计算IMU误差项中待优化的传感器参数的初值：
Figure FDA00027455548000000812
Figure FDA00027455548000000813
Figure FDA00027455548000000814
Figure FDA00027455548000000815
Figure FDA00027455548000000816
其中，Q(·)表示将一个旋转矩阵转换为对应的四元数的函数，
Figure FDA00027455548000000817
为所述第一方程lm计算出的第二滑动窗口中第m(k)帧图像对应的IMU位置在第m(k)帧图像对应的IMU坐标系下的运动速度，m(k)表示第一滑动窗口中的第k帧图像在第二滑动窗口中对应的图像序号，bω为陀螺仪偏移；
利用所述第一滑动窗口中的图像对应的IMU坐标系的位姿
Figure FDA0002745554800000091
和
Figure FDA0002745554800000092
推算出第一滑动窗口中的图像位姿；利用第一滑动窗口中的图像位姿以及图像上的匹配点进行三角化计算，得到匹配点的深度，将所述深度作为构造重投影误差项所需待优化传感器参数中的匹配点深度传感器参数的初值；
设置初始化的状态为“已完成”。
7.根据权利要求2所述的紧耦合车辆轮子编码器数据的视觉惯性测程方法，其特征在于，步骤五具体包括：
按照基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法在第一滑动窗口中构造代价函数，并进行非线性优化求解，得到传感器位姿、速度和偏移以及匹配点深度的估计值；
其中，所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项；
所述重投影误差项和所述边缘化误差项使用基于滑动窗口中非线性优化的视觉惯性测程方法的通用做法进行构造；
所述IMU误差项使用下述方法进行构造：
Figure FDA0002745554800000093
其中，CIMU(x)表示IMU误差项；x由x1,x2,…,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，∑k,k+1表示权利要求5中预积分得到的协方差矩阵∑S-1，随着第一滑动窗口的滑动，窗口中任意相邻两帧图像之间预积分得到的协方差矩阵均为已知；
Figure FDA0002745554800000094
表示残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关：
Figure FDA0002745554800000101
Figure FDA0002745554800000102
和
Figure FDA0002745554800000103
分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度；
Figure FDA0002745554800000104
和
Figure FDA0002745554800000105
分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度；
Figure FDA0002745554800000106
和
Figure FDA0002745554800000107
分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure FDA0002745554800000108
和
Figure FDA0002745554800000109
分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移；
Figure FDA00027455548000001010
是用四元数表示的在第k帧曝光时刻从IMU本体坐标系到世界坐标系的旋转；
Figure FDA00027455548000001011
为
Figure FDA00027455548000001012
的旋转矩阵形式；
Figure FDA00027455548000001013
为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；
Figure FDA00027455548000001014
为
Figure FDA00027455548000001015
的旋转矩阵形式；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值；
Figure FDA00027455548000001016
是车体坐标系的原点在IMU坐标系中的坐标，通过
Figure FDA00027455548000001017
计算得到，
Figure FDA00027455548000001018
表示从车体坐标系到IMU坐标系的旋转矩阵，
Figure FDA00027455548000001019
表示IMU的中心在车体坐标系中的坐标，事先量测；
Figure FDA00027455548000001020
Figure FDA00027455548000001021
和
Figure FDA00027455548000001022
为第k帧和第k+1帧图像之间IMU-轮子编码器预积分的结果，随着第一滑动窗口的滑动，窗口中任意相邻两帧图像之间预积分得到的
Figure FDA00027455548000001023
Figure FDA00027455548000001024
和
Figure FDA00027455548000001025
均为已知；
Figure FDA00027455548000001026
和
Figure FDA00027455548000001027
均为待优化的参数。
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-轮子编码器误差项和边缘化误差项的代价函数，将传感器位姿、速度和偏移以及匹配点深度一起进行迭代优化，得到第一滑动窗口内各张图像对应的优化后传感器参数；
步骤六：根据所述优化后传感器参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。
CN201910127778.1A 2019-02-19 2019-02-19 紧耦合车辆轮子编码器数据的视觉惯性测程方法及*** Active CN109764880B (zh)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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辅助跟踪模型

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