高精度视觉和IMU紧融合定位方法与系统与流程

本发明涉及机器人，尤其涉及一种高精度视觉和imu紧融合定位方法与系统。

背景技术：

现有的室内服务机器人一般配备的是激光雷达传感器，利用激光雷达进行定位，激光雷达的缺点，第一其价格昂贵，第二其体积较大，安装不太方便。当然激光雷达的优点就是其精度高、稳定。针对激光雷达的缺点，又开始有使用价格比较便宜的摄像头作为传感器，通过采集环境中的图片来进行定位，并且相机提供的图像信息相比于激光雷达而言，又更丰富，更有利于以后的应用开发。比如行人检测，目标识别等。

这些使用纯视觉方法来进行定位与建图也有其固有的不可避免的缺点。纯视觉的定位与建图，在环境中没有特征或者是运动过快的情况下容易失效。

技术实现要素：

为了解决现有技术中的问题，本发明提供了一种高精度视觉和imu紧融合定位方法与系统，用于解决传统视觉定位中精度和鲁棒性差的技术问题。

本发明提供了一种高精度视觉和imu紧融合定位方法，包括以下步骤：

s1、从获取的图像提取特征点进行双目匹配得到特征点深度，用于跟踪；

s2、对imu数据进行预积分操作，通过积分给定时间段的imu数据，获得初步的位姿信息；

s3、对系统进行初始化，获得imu的零偏和重力加速度，为跟踪做准备；

s4、实时的跟踪特征点，以相机帧率实时的输出位姿。

作为本发明的进一步改进，在步骤s1中，将相机得到的左右两幅图像进行畸变校正和极线矫正，得到没有畸变并且极线水平的图，然后以左相机为参考，提取左右两图中的特征点，采用fast角点和改进版的brief描述子进行特征点匹配得到左图中特征点的深度。

作为本发明的进一步改进，对于畸变矫正，通过优化的方法，最小化式子：

其中：k1，k2，p1，p2表示需要求的相机畸变系数；x表示理想的像素点的横坐标；y表示理想的像素点的纵坐标；r²＝x²+y²；

得到畸变系数k1，k2，p1，p2；

对于极线矫正，是通过计算，将左右两幅图片的极线处于同一个平面内，并且水平的操作。

作为本发明的进一步改进，在步骤s2中，通过imu数据得到绕三个轴旋转的角速度和沿着三个轴方向的加速度，角速度的积分是旋转角度，加速度的积分是速度，速度的积分是位移，通过给定时间t的imu积分，得知给定时间t内的相对位姿变化，进而累计得到每一时刻的位置。

作为本发明的进一步改进，在步骤s2中，对imu数据进行预积分操作，得到任意一段时间间隔内的相对位姿变化，

其中△rij表示第i个图像和第j个图像之间的相对旋转；表示第k个陀螺仪旋转的测量值；表示第k个陀螺仪旋转测量所对应的零偏；△t表示两个陀螺仪测量值的时间间隔；△vij表示第i个图像和第j个图像之间的相对速度变化；表示第k个加速度计的测量值；表示第k个加速度计测量值所对应的零偏；△pij表示第i个图像和第j个图像之间的相对位置变化。

作为本发明的进一步改进，在步骤s3中，对系统进行初始化，获得重力加速度gw，陀螺仪的初始零偏，加速度计的初始零偏，之后进行系统的跟踪。

作为本发明的进一步改进，在步骤s3中，通过纯视觉的跟踪，选择多个连续的关键帧得到这些关键帧的位姿，根据这些关键帧的位姿和这些关键帧间的imu预积分来获取需要的初始值。

作为本发明的进一步改进，在骤s3中，选择20个具有代表性的帧并估计其相对准确的位姿，包括：

当传来第一个图像时将此图像帧做为关键帧；

当第二帧来时，与第一个关键帧进行匹配，计算平均视差，如果视差足够大，将这一帧作为关键帧，否则，丢掉这一帧，只保留这一帧的imu数据；

得到这一关键帧和上一关键帧的匹配之后，使用pnp算法，解出两帧之间的相对旋转rcl和相对位移pcl，将此值作为初始值，用于优化两个关键帧共同看到地图点的重投影误差，得到精确的相对旋转rcl和相对位移pcl；

在另外一个线程里面，选择一个固定大小的窗口，大小为10个关键桢，使用优化的方法，优化这10个关键桢和这10个关键帧能看到的所有地图点，当这个窗口里的关键帧数目超过10的时候，将最老的一个关键帧抛出窗口。

作为本发明的进一步改进，在步骤s4中，最小化重投影误差和imu的约束，实时的跟踪特征点得到当前帧的位姿。

本发明还提供了一种高精度视觉和imu紧融合定位系统，包括快速传输imu数据的imu数据传输模块、用于快速并且保证不丢帧的传递双目图像信息的视觉传输模块、保证imu和图像数据在时间戳上是一致的的imu和相机同步模块、用于将传感器数据传输到后端电脑进行快速处理的无线传输模块，所述imu数据传输模块、视觉传输模块的输出端分别与所述imu和相机同步模块的输入端连接，所述imu和相机同步模块的输出端与所述无线传输模块的输入端连接。

本发明的有益效果是：提高了视觉定位中的精度和鲁棒性。

附图说明

图1是本发明一种高精度视觉和imu紧融合定位方法的流程图。

图2是本发明一种高精度视觉和imu紧融合定位方法的实现前端跟踪策略的流程图。

图3是本发明一种高精度视觉和imu紧融合定位方法的后端优化流程图。

图4是本发明一种高精度视觉和imu紧融合定位方法的回环策略流程图。

图5是本发明一种高精度视觉和imu紧融合定位系统的示意图。

具体实施方式

下面结合附图说明及具体实施方式对本发明作进一步说明。

如图1所示，一种高精度视觉和imu紧融合定位方法，包括：从获取的图像提取特征点进行双目匹配得到特征点深度；对imu(inertialmeasurementunit，惯性测量单元)数据进行积分处理的具体方法；对系统进行初始化获得imu的零偏以及重力加速度的获取；实时的以相机的帧率进行跟踪。通过本发明可以实时的计算当前帧的位姿，解决现有技术中，定位精度差和鲁棒性弱的问题。

一种高精度视觉和imu紧融合定位方法，具体包括以下过程：

1、从获取的图像提取特征点进行双目匹配得到特征点深度，用于跟踪；将相机得到的左右两幅图像进行畸变校正和基线矫正，得到没有畸变并且极线水平的图，然后以左相机为参考，提取左右两图中的特征点，决定采用提取速度比较快的fast角点和改进版的brief描述子进行特征点匹配得到左图中特征点的深度。

2、对imu数据进行预积分操作，通过积分给定时间段的imu数据，获得初步的位姿信息；对惯性测量器件(imu)，可以得到绕三个轴旋转的角速度和沿着三个轴方向的加速度。角速度的积分是旋转角度，加速度的积分是速度，速度的积分是位移。通过给定时间t的imu积分，可以知道这段时间内的相对位姿变化。进而可以累计的得到每一时刻的位置。

3、对系统进行初始化，获得imu的零偏和重力加速度，为跟踪做准备；在视觉和imu结合的系统需要进行初始化，来获得重力加速度gw，陀螺仪的初始bias(零偏)，加速度计的初始bias(零偏)，之后就能进行系统的跟踪；通过纯视觉的跟踪，选择20个连续的关键帧得到这20个关键帧的位姿(简单的structureformmotion)。根据这20帧的位姿和这20帧间的imu预积分来获取需要的初始值；选择20个具有代表性的帧并估计其相对准确的位姿，包括：

当传来第一个图像时将此图像帧做为关键帧；

3.当第二帧来时，与第一个关键帧进行匹配，计算平均视差如果视差足够大(大于20个像素)，将这帧作为关键帧，否则丢掉这一帧，只保留这一帧的imu数据；

得到这一关键帧和上一关键帧的匹配之后，使用pnp(perspective-n-point)算法，解出两帧之间的相对旋转rcl和相对位移pcl，将此值作为初始值，用于优化两个关键帧共同看到地图点的重投影误差，得到精确的相对旋转rcl和相对位移pcl；

在另外一个线程里面，选择一个固定大小的窗口，大小为10个关键桢，使用优化的方法，优化这10个关键桢和这10个关键帧能看到的所有地图点。当这个窗口里的关键帧数目超过10的时候，将最老的一个关键帧抛出窗口。

4、实时的跟踪特征点，以相机帧率实时的输出位姿；最小化重投影误差和imu的约束，实时的跟踪特征点得到当前帧的位姿。

对于imu数据的处理，使用了预积分的方法，该方法可以得到任意一段时间间隔内的相对位姿变化。

其中δrij表示第i个图像和第j个图像之间的相对旋转；表示第k个陀螺仪旋转的测量值；表示第k个陀螺仪旋转测量所对应的零偏；δt表示两个陀螺仪测量值的时间间隔；δvij表示第i个图像和第j个图像之间的相对速度变化；表示第k个加速度计的测量值；表示第k个加速度计测量值所对应的零偏；δpij表示第i个图像和第j个图像之间的相对位置变化。

对image的处理，指的是对双目的图像进行去畸变和极线矫正的过程。对于畸变矫正，通过优化的方法，最小化式子：

其中：k1，k2，p1，p2表示需要求的相机畸变系数；x表示理想的像素点的横坐标；y表示理想的像素点的纵坐标；r²＝x²+y²；

得到畸变系数k1，k2，p1，p2。

对于极线矫正，是通过计算，将左右两幅图片的极线处于同一个平面内，并且水平的操作。具体的算法实现，直接可以调用现成的opencv函数。

系统的初始化过程，就是为了获得重力加速度gw，陀螺仪的初始bias，加速度计的初始bias。

具体的，首先通过纯视觉的方法，得到20个关键帧的准确位置，然后通过imu的预积分，得到相邻两个关键帧之间的相对位姿。通过这两步计算的关系来解出所要求的量。

图2是实现前端跟踪策略的详细步骤，如图2所示，该方法具体包括一下步骤：

首先根据imu数据，通过预积分得到，当前帧与上一帧之间的相对位姿关系，然后将上一图像帧里面的特征点投影到当前帧中，进行小范围的特征匹配搜索。得到特征匹配之后，通过最小化重投影误差和imu预积分的误差来实现位姿的精调。

其次，如果跟踪上一帧失败的话，就不用imu的数据，简单的利用纯视觉的方法，对当前位姿进行恢复和估计。

最后，如果依旧没法计算当前帧的准确位姿，就会使用词袋模型进行重定位。通过重定位的方法，获取特征匹配，得到当前帧的位姿。如果真的找不回当前帧的位姿，就只有重启系统，才能进行继续跟踪。

这个模块在跟踪上一帧成功之后，还会将前五个图像帧看到的地图点投影到当前帧，找出更多的约束来对当前帧位姿的进一步优化。

图3是实现后端跟踪策略的详细策略。

首先，对于关键帧的选择，关键帧的选择需要两个关键帧之间有足够的运动，并且有足够的关联性。这里选择关键帧的时候，要求关键帧之间的视差足够大。

其次在生成关键帧之后，需要通过关键帧的位姿，将该帧看到的特征点，转化到世界坐标系下，融合到局部地图里。最后对这个局部地图里面的所有地图点和图像帧的位姿进行优化求解，优化所有的地图点位置和关键帧的位姿。

通过以上的步骤，能够在较短时间内得到比较准确的位姿图，但是随着误差累积，误差逐渐增大。得到的位姿愈来愈不准确，这里通过dbow来检测回环，检查回环所使用的策略，如图4所示。在得到回环帧时候，首先进行位姿图优化，优化所有的关键帧。然后，对所有的地图点和关键帧进行同时优化求解，得到更加准确的三维点位置和相机的位姿。

如图5所示，本发明还提供了一种高精度视觉和imu紧融合定位系统，包括快速传输imu数据的imu数据传输模块1、用于快速并且保证不丢帧的传递双目图像信息的视觉传输模块2、保证imu和图像数据在时间戳上是一致的imu和相机同步模块3、用于将传感器数据传输到后端电脑进行快速处理的无线传输模块，所述imu数据传输模块1、视觉传输模块2的输出端分别与所述imu和相机同步模块3的输入端连接，所述imu和相机同步模块3的输出端与所述无线传输模块的输入端连接,无线传输模块优选为wifi传输模块4。

视觉传输模块1，用于快速并且保证不丢帧的传递双目图像信息；imu数据传输模块2，由于imu数据的高频性，本发明设计了快速传输imu数据的imu数据传输模块2；imu和相机同步模块3，保证imu和图像数据在时间戳上是一致的；wifi传输模块4用于将传感器数据传输到后端电脑进行快速处理。

本发明提供的一种高精度视觉和imu紧融合定位系统，用于执行上述高精度视觉和imu紧融合定位方法。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。