基于视觉的自动引导车定位方法与流程

本发明涉及机器人视觉领域，具体涉及一种基于视觉的自动引导车定位方法。

背景技术：

自动引导车，又称自动导引车(automatedguidedvehicle，简称agv)，是指具有磁条、轨道或者激光等自动导引设备，可沿规划好的路径行驶，以电池为动力，并且装备了安全保护以及各种辅助机构(例如移载，装配机构)的无人驾驶自动化车辆。在控制中心的监控及任务调度下，agv可以准确地按照规定的路径行走，到达任务指定位置后，完成一系列的作业任务。

目前的agv定位导航方式主要有电磁导航、磁条导航、惯性定位导航、二维码定位导航、激光定位导航等几种方式。

电磁导航需要预先规划好agv行走路径，然后将金属导线铺设在路径上，导线通电进而产生磁场，agv车载感应线圈，通过跟踪磁场进行导航。磁条导航是在地上安装铁磁条，在agv左右侧贴上磁条感应装置，通过比较两侧的磁场强度来矫正偏差，进行导航。电磁导航和磁条导航只能使agv走固定的路线，而且只能用来牵引agv，难以做到定位的功能。

惯性定位导航通过对惯导积分得到速度和位置，自由度高、灵活性高，且没有固定路径的限制。但是普通的惯性传感器的积分误差极大，高精度的惯性传感器造价过高难以普及。

激光定位导航的方式精度较高，无需其它辅助设备，行驶路线灵活，缺点是造价相对较高，算法较为复杂，信号易受干扰，对光线、能见度等要求较为严苛。

二维码定位方式需要提前铺设二维码，在agv的底侧安装摄像头，通过拍摄识别二维码来获取当前位置。二维码定位优点是铺设成本相对较低，缺点是二维码能提供的定位是离散的，且需要对二维码进行频繁的维护。

以上背景技术内容的公开仅用于辅助理解本发明的发明构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日前已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。

技术实现要素：

本发明的主要目的在于克服现有定位方式的不足，提出一种基于视觉的自动引导车定位方法，定位过程中通过利用二维码定位技术对视觉里程计定位的累积误差进行不断的矫正，从而保证了高精度的实时定位。

本发明为达上述目的提出以下技术方案：

一种基于视觉的自动引导车定位方法，包括以下步骤：

s1、使用双目相机实时采集自动引导车前方的图像；

s2、提取双目图像的特征点进行匹配，并计算匹配成功的特征点的深度；将左目图像舍弃未在右目图像中找到匹配的特征点后，作为当前图片帧；

s3、将所述当前图片帧中的特征点与地图中上一关键帧中的特征点进行匹配，并根据匹配结果，利用迭代最近点算法解算左目相机的当前位姿；其中，所述地图是所有关键帧及关键帧中所有特征点的集合；

s4、根据左目相机的当前位姿以及自动引导车与左目相机之间的位姿变换关系，解算自动引导车的当前位置；

s5、检测是否有二维码定位信息输入，若有，则执行步骤s6；若无，则执行步骤s7；

s6、将所述当前图片帧设为当前关键帧并对所述地图进行全局优化；

s7、判断所述当前图片帧是否满足预设条件，若满足，则将所述当前图片帧设为当前关键帧并对所述地图进行局部滑动窗口优化；若不满足，则舍弃当前图片帧并返回步骤s1；

s8、利用经过全局优化或者局部滑动窗口优化的关键帧及其特征点更新所述地图，然后返回步骤s1开始下一时刻的定位。

本发明上述技术方案所提供的定位方法，采用视觉里程计技术作为主要的定位手段，并在实时定位的过程中使用agv的二维码定位技术对视觉里程计的累积误差进行矫正，从而能够保证在有限的计算资源情况下，视觉里程计定位具有较高的实时定位精度。

附图说明

图1是本发明基于视觉的自动引导车定位方法的流程图。

具体实施方式

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

本发明的具体实施方式提供了一种基于视觉的自动引导车定位方法，采用视觉里程计技术作为主要的定位手段，并在实时定位的过程中采用二维码定位技术来不断地矫正视觉里程计的累积误差，参考图1，本发明的该定位方法包括如下步骤s1～s8：

s1、使用双目相机实时采集自动引导车前方的图像，每一时刻可以得到左目图像和右目图像。其中，采集图像之前的准备工作包括：使用张氏标定法，用棋盘格对双目相机进行标定，使得标定后的相机无畸变，成像平面在同一平面上，且极线位于相机成像平面的同一水平线上。

s2、提取双目图像的特征点进行匹配，并计算匹配成功的特征点的深度；将左目图像舍弃未在右目图像中找到匹配的特征点后，作为当前图片帧。本发明中优选地提取双目图像的orb特征点进行匹配，匹配过程包括：对于每一个在左目图像中出现的特征点，根据其特征描述子在右目图像中附近的行的左侧寻找与其匹配的特征点，并进行抛物线拟合来获得亚像素级别的匹配。匹配成功的每一对特征点，均对应于空间中的同一个点。对于每一对匹配成功的特征点，可采用如下公式计算其深度d：

其中，f是相机的焦距，b是双目相机的光心距离，ul、ur分别为匹配成功的特征点在左目图像和右目图像中竖直方向上的位置。

s3、将所述当前图片帧中的特征点与地图中上一关键帧中的特征点进行匹配，并根据匹配结果，利用迭代最近点(icp)算法解算左目相机的当前位姿；其中，所述地图是所有关键帧及关键帧中所有特征点的集合。具体而言，可以使用光束平差法评估所述地图中的参考点和左目相机的观测之间的误差，并使用列文伯格马夸尔特方法最小化所述误差，从而计算出左目相机的当前位姿；其中，所述参考点是所述上一关键帧中与所述当前图片帧的特征点匹配成功的特征点。使用列文伯格马夸尔特方法最小化所述误差的目标函数如下：

其中，{r,t}为左目相机的当前位姿，也即当前图片帧的位姿；p为当前图片帧与上一关键帧匹配成功的特征点集合，ρ为huber罚函数，xi表示相机坐标系下第i个特征点的位置，r表示左目相机的旋转，t表示左目相机的平移向量，xⁱ表示世界坐标系下第i个特征点的位置。其中，xi的横纵坐标可从图像中获取，深度可利用前述公式(1)计算得到，xⁱ可利用xi进行坐标系转换得到。

s4、根据左目相机的当前位姿以及自动引导车与左目相机之间的位姿变换关系，解算自动引导车的当前位置。一般情况下，相机坐标系与引导车的坐标系之间保持固定，有一个固定的位姿变换关系，固定的位姿变换关系是已知的。因此，在得到相机的位姿信息时，可利用位姿变换关系，解算引导车的当前位置，从而完成粗定位。

s5、检测是否有二维码定位信息输入，若有，则执行步骤s6；若无，则执行步骤s7。

s6、将所述当前图片帧设为当前关键帧并对所述地图进行全局优化。所述全局优化包括：

将所述当前关键帧及其特征点的位姿作为固定变量，将所述当前关键帧周围预定范围内的关键帧及其特征点的位姿作为待优化变量，进行联合优化，优化的公式为：

关键帧的位姿等同于相应时刻左目相机的位姿，关键帧中特征点的位姿根据特征点自身的深度、左目相机的位姿以及左目相机内部参数解算得到；在式(3)中，表示所述预定范围内所有特征点的集合，表示所述预定范围内所有关键帧的集合，r^j表示第j个关键帧的旋转矩阵，t^j表示第j个关键帧的平移向量。

s7、判断所述当前图片帧是否满足预设条件，若满足，则将所述当前图片帧设为当前关键帧并对所述地图进行局部滑动窗口优化；若不满足，则舍弃当前图片帧并返回步骤s1。所述预设条件包括：与上一关键帧之间间隔了至少10个图片帧，至少包括20对匹配点，以及，与上一关键帧的匹配点对数少于80％。所述局部滑动窗口优化包括：

将所述当前关键帧及其特征点的位姿与之前数个(优选地，是选取之前的20个关键帧，如果之前的总关键帧数小于20则取最大值，有多少关键帧就取多少关键帧)关键帧及其特征点的位姿做联合优化，优化过程将前一半数量的关键帧及其特征点的位姿作为固定变量，将后一半数量的关键帧及其特征点的位姿作为待优化变量；其中，关键帧的位姿等同于相应时刻左目相机的位姿，关键帧中特征点的位姿根据特征点自身的深度、左目相机的位姿以及左目相机内部参数解算得到；优化的公式为：

式(4)中，表示所述后一半数量的关键帧中所有特征点的集合，表示所述后一半数量的关键帧的集合。

s8、利用经过全局优化或者局部滑动窗口优化的关键帧及其特征点更新所述地图，然后返回步骤s1开始下一时刻的定位。

在一些实施例中，更新地图的操作还包括，依据以下规则删除所述地图中的部分关键帧及其特征点：删除实际可见和预测可见的比值小于25％的特征点，删除90％以上的特征点都能被其它关键帧观测到的关键帧。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。