The present invention relates to a visual inertial range measurement method and system for tightly coupled vehicle wheel encoder data. The visual inertial range measurement method includes: step 1: setting the initialization state of the visual inertial range measurement method as \incomplete\ and in the initialization process; step 2: calculating the essential matrix between the current image and the previous frame image, and combining the essential matrix with the original one. The corresponding IMU measurements and wheel encoder readings are added to the first sliding window and the second sliding window at the same time. Step 3: IMU wheel encoder pre-integration is carried out on the IMU measurements and wheel encoder readings corresponding to the last frame image; whether the initialization state is \completed\ is judged; if it is, step 5 is executed, otherwise step 4 is executed; step 4: Pre-integration knot is used; The parameters are initialized. Step 5: Get the corresponding parameters of each image in the first sliding window. Step 6: Measure the visual inertia range of tightly coupled vehicle wheel encoder data according to the parameters.
【技术实现步骤摘要】
紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统
本专利技术涉及融合多传感器的定位导航领域，具体涉及一种紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统。
技术介绍
使用相机和IMU(InertialMeasurementUnit，惯性测量单元)的视觉惯性测程方法近年来取得了很好的效果，因为视觉和IMU的信息有互补的特点。然而，因为车辆的运动形式较为特殊，例如易于做近似匀速直线运动，视觉惯性测程方法中的尺度估计有时不准确。另一方面，轮子编码器可以提供用来确定尺度的准确信息，而且它的数据易于获取。因此，融合相机、IMU和轮子编码器是室内车辆定位的明智选择。几种现有的方法分别使用扩展卡尔曼滤波和非线性优化等不同的策略融合相机、IMU和轮子编码器的信息。这三种方法都需要轮子编码器系统提供角速度，尽管因为有IMU中陀螺仪的使用上述角速度并非必要。一方面，为了获取上述角速度，至少需要安装两个轮子编码器。另一方面，因为由轮子编码器推导出的角速度总是在地平面内，所以事实上做出了时间上相邻的两个状态之间车辆几乎是平面运动的假设。但是这个假设在车辆上下坡或颠簸时可能会造成较大误差。...
【技术保护点】
1.一种紧耦合车辆轮子编码器数据的视觉惯性测程方法，其特征在于，所述视觉惯性测程方法包括：步骤一：设置所述视觉惯性测程方法的初始化状态为“未完成”，并在初始化和测程的过程中，实时获取相机图像、惯性测量单元IMU量测值和轮子编码器读数；步骤二：计算当前图像与前一帧图像之间的本质矩阵，将所述本质矩阵以及与对应的IMU量测值和轮子编码器读数同时加入第一滑动窗口和第二滑动窗口，同时根据当前图像是否为关键帧对所述第一滑动窗口和第二滑动窗口进行相应调整，所述当前图片为第一帧图像之后的任意图像；步骤三：对所述第一滑动窗口和第二滑动窗口中各自的最后一帧图像对应的IMU量测值和轮子编码器读数...
【技术特征摘要】
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取值为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轴垂直底盘向上。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量测值和轮子编码器读数，依次重新进行预积分操作，其中陀螺仪偏移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坐标系到第一滑...
【专利技术属性】
技术研发人员：刘津旭，高伟，胡占义，
申请(专利权)人：中国科学院自动化研究所，
类型：发明
国别省市：北京,11
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。