紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统与流程

本发明涉及融合多传感器的定位导航领域，具体涉及一种紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统。
背景技术：
：使用相机和imu(inertialmeasurementunit，惯性测量单元)的视觉惯性测程方法近年来取得了很好的效果，因为视觉和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取值为ji+1的第7行到第9行、第16列到第18列构成的矩阵；直到最终结果∑s-1和被计算出来；其中，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,2,…,m，m是第二滑动窗口中图像的数目；为单位阵；对于第二滑动窗口中每两帧连续的图像，构造方程其中，和为第二滑动窗口中第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＝axx包含的所有元素均为待优化的变量，l和a的所有元素均为已知；求解minx(ax-l)t(ax-l)，利用x中的δg更新利用更新后的重新计算它的切空间的正交基b，并利用新的和b重新计算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是本发明紧耦合车辆轮子编码器数据的视觉惯性测程系统的模块结构示意图。符号说明：获取单元—1，相机姿态计算模块—2，判断单元—3，优化单元—4，测程单元—5。具体实施方式下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是，这些实施方式仅仅用于解释本发明的技术原理，并非旨在限制本发明的保护范围。本发明提供一种紧耦合车辆轮子编码器数据的视觉惯性测程方法，利用单目相机的图像、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；取值为ji+1的第7行到第9行、第16列到第18列构成的矩阵；直到最终结果∑s-1和被计算出来；其中，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ω为优化得到的值。步骤s4005，利用计算得到的预积分结果，依次计算从第二滑动窗口内第一帧到第m帧的旋转矩阵其中m依次取1,2,…,m，m是第二滑动窗口中图像的数目；为单位阵；步骤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＝axx包含的所有元素均为待优化的变量，l和a的所有元素均为已知；求解minx(ax-l)t(ax-l)，利用x中的δg更新利用更新后的重新计算它的切空间的正交基b，并利用新的和b重新计算l和a，重复本步骤，迭代计算；当迭代次数达到预先设定的迭代次数阈值时，步骤s4000结束；记录此时的和步骤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的中心在车体坐标系中的坐标，可以事先量测。和为第k帧和第k+1帧图像之间imu-轮子编码器预积分的结果，随着第一滑动窗口的滑动，窗口中任意相邻两帧图像之间预积分得到的和均为已知；和均为待优化的参数。表1表示本发明的初始化模块和现有的初始化模块在初始化成功之前车辆行驶的路程在两套数据上的对比。可以看出，使用本发明的初始化模块在初始化成功之前车辆行驶的路程明显更短，说明本方法的初始化速度更快。表1初始化成功之前车辆行驶的路程在两套数据上的对比本发明[5]中的初始化模块数据12.01m56.6m数据21.13m29.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：根据所述参数进行紧耦合车辆轮子编码器数据的视觉惯性测程。相对于现有技术，本发明计算机可读存储介质、紧耦合车辆轮子编码器数据的视觉惯性测程系统与上述紧耦合车辆轮子编码器数据的视觉惯性测程方法的有益效果相同，在此不再赘述。至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。当前第1页12