定位方法及系统与流程

本发明涉及机器人定位技术领域，尤其是涉及一种定位方法及系统。

背景技术：

定位导航是机器人实现智能化的前提之一，是赋予机器人感知和行动能力的关键因素。

目前，传统的机器人定位方法常采依靠轮式里程计进行计算，轮式里程计的优点是短时间短距离的定位精度很高，但是这种航迹推算的定位方法会存在累积误差，并且不能够根据自身的信息进行误差消除，同时也不能克服轮子打滑等因素的影响。

技术实现要素：

有鉴于此，本发明的目的在于提出了一种定位方法及系统，以改善上述问题。

为了实现上述目的，本发明采用如下技术方案：

第一方面，本发明实施例提供了一种定位方法，应用于轮式移动机器人的定位系统，所述定位系统包括编码器、陀螺仪、单目相机、底盘控制芯片和机器人主控子系统，所述编码器安装于轮式移动机器人的车轮上，所述方法包括：

所述底盘控制芯片根据所述编码器采集到的车轮的转速数据和所述陀螺仪采集到的所述轮式移动机器人的角度数据得到轮式里程计模型的第一位姿数据，所述第一位姿数据包括第一位置和第一速度；

所述机器人主控子系统根据所述单目相机获取到的图像数据计算出视觉里程计模型的第二位姿数据，所述第二位姿数据包括第二位置和第二速度；

所述机器人主控子系统将所述第一位姿数据和所述第二位姿数据进行时间戳对齐和运动轨迹对齐，恢复所述单目相机的最优相机尺度；

所述机器人主控子系统依据所述最优相机尺度对所述第二位姿数据进行尺度恢复；

所述机器人主控子系统将所述第一位姿数据与尺度恢复后的所述第二位姿数据进行融合，得到所述轮式移动机器人的最终位姿数据。

可选的，所述方法还包括：

所述机器人主控子系统对所述图像数据中的每一个关键帧图像进行回环检测；

当发生回环时，所述机器人主控子系统对所述轮式移动机器人进行重定位计算。

可选的，所述机器人主控子系统对所述图像数据中的每一个关键帧图像进行回环检测，包括：

所述机器人主控子系统对所述图像数据中的每个关键帧图像提取多个fast角点，并计算每个fast角点的brief描述子；

所述机器人主控子系统根据每个fast角点及对应的brief描述子，通过dbow2算法计算当前帧与之前关键帧的相似度大小；

当相似度大于设定的阈值时，所述机器人主控子系统判定则发生回环。

可选的，所述编码器和所述轮式移动机器人的车轮数量均为多个且一一对应，所述底盘控制芯片根据所述编码器采集到的车轮的转速数据和所述陀螺仪采集到的所述轮式移动机器人的角度数据得到轮式里程计模型的第一位姿数据，包括：

所述底盘控制芯片根据每个所述编码器采集到的车轮的转速数据得到所述轮式移动机器人在当前坐标系下的速度数据和第一姿态角；

所述底盘控制芯片根据预先建立的陀螺仪误差模型计算出所述轮式移动机器人当前在全局坐标系下的第二姿态角；

所述底盘控制芯片将所述第一姿态角与所述第二姿态角进行卡尔曼滤波融合，得到所述轮式移动机器人的最终姿态角；

所述底盘控制芯片根据所述速度数据和所述最终姿态角解算所述轮式移动机器人在世界坐标系下的速度及位置信息，得到所述第一位姿数据。

可选的，所述车轮的数量为3个，且两两之间的夹角为120°，所述轮式移动机器人在当前坐标系下的速度数据为:

其中，vx、vy分别表示在当前坐标系下的x轴和y轴的速度,ω表示在当前坐标系下绕自身几何中心的旋转速度，ω1、ω2、ω3分别表示三个车轮的旋转速度，l为轮式移动机器人的底盘半径，r为车轮半径。

可选的，所述机器人主控子系统根据所述单目相机获取到的图像数据计算出视觉里程计模型的第二位姿数据，包括：

所述机器人主控子系统对所述图像数据提取fast角点，并进行lk光流跟踪，得到图像特征点匹配信息；

所述机器人主控子系统根据预设的图像特征点发布频率发布所述图像特征点匹配信息；

所述机器人主控子系统将所述图像数据的第一帧设为关键帧，其他图像帧根据当前图像跟踪上一关键帧图像的特征点数及特征点的平均视差确定是否设定为关键帧；

所述机器人主控子系统建立图像跟踪的滑动窗口；

所述机器人主控子系统通过对极几何、三维重建、pnp算法计算得到所述滑动窗口中各帧图像的位置关系得到旋转矩阵，并对求取得到的旋转矩阵选取偏航角作为初始旋转矩阵，平移向量取在x轴与y轴水平面上的平移，建立重投影误差代价函数，进行3自由度最小化重投影误差计算，得到图像关键帧之间缺乏尺度的旋转和平移矩阵。

可选的，所述机器人主控子系统将所述第一位姿数据和所述第二位姿数据进行时间戳对齐和运动轨迹对齐，恢复所述单目相机的最优相机尺度，包括：

所述机器人主控子系统将所述第一位姿数据与所述第二位姿数据时间戳对齐；

所述机器人主控子系统将时间戳对齐后的所述第一位姿数据与所述第二位姿数据进行轨迹对齐；

所述机器人主控子系统通过求取损失函数的最小二乘解，得到所述单目相机的最优相机尺度。

第二方面，本发明实施例提供了一种定位系统，应用于轮式移动机器人，包括：编码器、陀螺仪、单目相机、底盘控制芯片和机器人主控子系统，所述编码器安装于轮式移动机器人的车轮上；

所述底盘控制芯片用于根据所述编码器采集到的车轮的转速数据和所述陀螺仪采集到的所述轮式移动机器人的角度数据得到轮式里程计模型的第一位姿数据，所述第一位姿数据包括第一位置和第一速度；

所述机器人主控子系统用于根据所述单目相机获取到的图像数据计算出视觉里程计模型的第二位姿数据，所述第二位姿数据包括第二位置和第二速度；

所述机器人主控子系统还用于将所述第一位姿数据和所述第二位姿数据进行时间戳对齐和运动轨迹对齐，恢复所述单目相机的最优相机尺度；

所述机器人主控子系统还用于依据所述最优相机尺度对所述第二位姿数据进行尺度恢复；

所述机器人主控子系统还用于将所述第一位姿数据与尺度恢复后的所述第二位姿数据进行融合，得到所述轮式移动机器人的最终位姿数据。

可选的，所述机器人主控子系统还用于对所述图像数据中的每一个图像关键帧进行回环检测；以及当发生回环时对所述轮式移动机器人进行重定位计算。

可选的，所述编码器和所述轮式移动机器人的车轮数量均为多个且一一对应；

所述底盘控制芯片用于根据每个所述编码器采集到的车轮的转速数据得到所述轮式移动机器人在当前坐标系下的速度数据和第一姿态角；

所述底盘控制芯片还用于根据预先建立的陀螺仪误差模型计算出所述轮式移动机器人当前在全局坐标系下的第二姿态角；

所述底盘控制芯片还用于将所述第一姿态角与所述第二姿态角进行卡尔曼滤波融合，得到所述轮式移动机器人的最终姿态角；

所述底盘控制芯片还用于根据所述速度数据和所述最终姿态角解算所述轮式移动机器人在世界坐标系下的速度及位置信息，得到所述第一位姿数据。

与现有技术相比，本发明的有益效果在于：

本发明提供的定位方法及系统针对轮式里程计会存在累积误差且不能自动消除，轮子打滑等外界因素对定位精度的影响，通过传感器融合的思想来进一步优化定位信息，将轮式里程计与单目视觉里程计结合起来，克服相互的缺点，保留各自的优点，能够克服定位过程中单目相机没有尺度和鲁棒性差的问题，同时也可以解决轮式里程计累积误差的问题。

附图说明

图1为本发明较佳实施例提供的定位系统的结构示意图。

图2为本发明较佳实施例提供的定位方法的流程图。

图3为图2中步骤s101的子步骤的流程图。

图4为图2中步骤s102的子步骤的流程图。

图5为图2中步骤s103的子步骤的流程图。

图6为图2中步骤s106的子步骤的流程图。

附图标记说明：110-编码器；120-陀螺仪；130-单目相机；140-底盘控制芯片；150-机器人主控子系统。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。

因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

请参阅图1，是本发明较佳实施例提供的定位系统的结构示意图，所述定位系统应用于轮式移动机器人，定位系统包括有编码器110、陀螺仪120、单目相机130、底盘控制芯片140和机器人主控子系统150，底盘控制芯片140分别与编码器110、陀螺仪120和机器人主控子系统150连接以进行数据通信或交互，机器人主控子系统150与单目相机130连接以进行数据通信或交互。

所述底盘控制芯片140用于根据所述编码器110采集到的车轮的转速数据和所述陀螺仪120采集到的所述轮式移动机器人的角度数据得到轮式里程计模型的第一位姿数据。

本发明实施例中，所述编码器110安装于轮式移动机器人的车轮上，用于采集对应车轮的转速数据，并将采集到的转速数据发送给底盘控制芯片140。轮式移动机器人的车轮数量可以为多个，此时编码器110的数量也可以为多个，多个编码器110与多个车轮数量相等且一一对应设置。当编码器110的数量为多个时，每个编码器110均将采集到的对应车轮的转速数据发送给底盘控制芯片140。陀螺仪120安装于轮式移动机器人的底盘上，用于采集到的所述轮式移动机器人的角度数据，并将采集到的角度数据发送给底盘控制芯片140。底盘控制芯片140根据每个编码器110采集到的车轮的转速数据和陀螺仪120采集到的轮式移动机器人的角度数据进行运算，得到轮式里程计模型的第一位姿数据，第一位姿数据包括第一位置和第一速度。

具体的，首先底盘控制芯片140根据每个编码器110采集到的车轮的转速数据得到轮式移动机器人在当前坐标系下的速度数据和第一姿态角。例如，当车轮和编码器110的数量均为3个，且车轮两两之间的夹角为120°，轮式移动机器人在当前坐标系下的速度数据为:其中，vx、vy分别表示在当前坐标系下的x轴和y轴的速度,ω表示在当前坐标系下绕自身几何中心的旋转速度，ω1、ω2、ω3分别表示三个车轮的旋转速度，l为轮式移动机器人的底盘半径，r为车轮半径。

底盘控制芯片140根据陀螺仪120预先多次检测到的角度数据与实际的角度数据建立有一陀螺仪误差模型，底盘控制芯片140在获得陀螺仪120采集到的角度数据后，根据先建立的陀螺仪误差模型计算出所述轮式移动机器人当前在全局坐标系下的第二姿态角，同时对该第二姿态角进行单自由度约束，即选择轮式移动机器人在绕着竖直轴的旋转角度(偏航角)。

得到第一姿态角与第二姿态角后，底盘控制芯片140将第一姿态角与所述第二姿态角进行卡尔曼滤波融合，得到轮式移动机器人的最终姿态角。最后，底盘控制芯片140根据得到的速度信息和轮式移动机器人当前在全局坐标系下的第二姿态角解算轮式移动机器人在世界坐标系下的速度及位置信息，得到所述第一位姿数据，第一位姿数据包括第一位置和第一速度。

机器人主控子系统150用于根据单目相机130获取到的图像数据计算出视觉里程计模型的第二位姿数据，第二位姿数据包括第二位置和第二速度。

单目相机130安装于轮式移动机器人上并与机器人主控子系统150连接，轮式移动机器人在移动的过程中，单目相机130获取其视场区域内的图像数据并将获取到的图像数据发送给机器人主控子系统150。机器人主控子系统150获得单目相机130发送的图像数据后对图像数据提取fast角点(featuresfromacceleratedsegmenttest)，并进行lk流光跟踪，得到图像特征点匹配信息。机器人主控子系统150根据预设的图像特征点发布频率发布所述图像特征点匹配信息。然后，机器人主控子系统150将图像数据的第一帧设为关键帧，其他图像帧根据当前图像跟踪上一关键帧图像的特征点数及特征点的平均视差确定是否设定为关键帧，并根据关键帧(根据当前图像帧跟踪上一个图像帧的特征点数以及当前图像帧与上一个关键帧的平均视差)建立图像跟踪的滑动窗口。最后，机器人主控子系统150通过对极几何、三维重建、pnp算法计算得到滑动窗口中各帧图像的位置关系得到旋转矩阵，对求取得到的旋转矩阵选取偏航角作为初始旋转矩阵，平移向量取在x轴与y轴水平面上的平移，建立重投影误差代价函数，进行3自由度最小化重投影误差计算，得到图像关键帧之间缺乏尺度的旋转和平移矩阵，即视觉里程计模型的第二位姿数据。

机器人主控子系统150还用于将第一位姿数据和第二位姿数据进行时间戳对齐和运动轨迹对齐，恢复单目相机130的最优相机尺度。

具体的，在得到第一位姿数据和第二位姿数据后，机器人主控子系统150将第一位姿数据与第二位姿数据时间戳对齐。然后，机器人主控子系统150将时间戳对齐后的第一位姿数据与第二位姿数据进行轨迹对齐。最后，机器人主控子系统150通过求取损失函数的最小二乘解，得到单目相机130的最优相机尺度。其中，最优相机尺度是指，图像中的距离与实际距离的对应关系，例如每100个像素距离对应实际距离1米。

机器人主控子系统150还用于依据最优相机尺度对第二位姿数据进行尺度恢复。

得到单目相机130的最优相机尺度后，机器人主控子系统150即可根据该最优相机尺度重新计算视觉里程计模型中个关键帧在世界坐标系中的平移矩阵以及特征点的3d坐标。

机器人主控子系统150还用于将第一位姿数据与尺度恢复后的第二位姿数据进行融合，得到轮式移动机器人的最终位姿数据。

对第二位姿数据进行尺度恢复后，机器人主控子系统150将第一位姿数据与尺度恢复后的第二位姿数据进行融，得到轮式移动机器人的最终位姿数据。

机器人主控子系统150还用于对图像数据中的每一个图像关键帧进行回环检测，以及当发生回环时对轮式移动机器人进行重定位计算。

本发明实施例中，回环检测及重定位的具体步骤为：

步骤1，机器人主控子系统150对图像数据中的每个关键帧图像提取多个fast角点，并计算每个fast角点的brief描述子，由于brief描述子具有旋转尺度不变性，而且计算速度快，所以适合用来做实时的特征点匹配。

步骤2，机器人主控子系统150根据每个fast角点及对应的brief描述子，通过dbow2算法计算当前帧与之前关键帧的相似度大小。

步骤3，当当前帧与之前关键帧的相似度相似度大于设定的阈值时，则机器人主控子系统150判定则发生回环，此时机器人主控子系统150根据回环候选帧(即回环检测到的相似度大于设定阈值的关键帧)进行位置计算，并纠正回环中其他关键帧的位置。

在步骤3中，具体的计算步骤为：

步骤s31，将当前帧的特征点与回环检测到的帧及其附近几帧进行brief描述子匹配，匹配准则为对应描述子的汉明距离。

步骤s32，对得到的匹配点进行ransac误匹配剔除。

步骤s33，针对匹配点已知的3d世界坐标，通过pnp算法求解得到当前帧在世界坐标系中的相对位置，消除累积误差。

步骤s34，根据回环中，关键帧的匹配特征点，建立最小化重投影误差优化函数，优化得到每个关键帧的重定位之后的旋转矩阵和平移矩阵，并更新特征点3d坐标。

在本发明实例中，每成功通过一次回环检测及重定位过程，就可以对之前的轮式里程计累积误差进行消除，保证了定位的精度，根据特征点对的数量以及两种里程计模型的位置距离调整融合参数，增强了系统的鲁棒性，通过轮式里程计和视觉里程计的融合，克服了轮式里程计的局限性，同时使得系统定位精度进一步提升。

请参阅图2，是本发明较佳实施例提供的应用于图1所示的定位系统的的定位方法的流程图，下面将对图2所示的流程进行阐述。

步骤s101，底盘控制芯片根据编码器采集到的车轮的转速数据和陀螺仪采集到的轮式移动机器人的角度数据得到轮式里程计模型的第一位姿数据。

请参阅图3，步骤s101包括如下子步骤：

子步骤s1011，底盘控制芯片根据每个编码器采集到的车轮的转速数据得到轮式移动机器人在当前坐标系下的速度数据和第一姿态角。

子步骤s1012，底盘控制芯片根据预先建立的陀螺仪误差模型计算出轮式移动机器人当前在全局坐标系下的第二姿态角。

子步骤s1013，底盘控制芯片将第一姿态角与第二姿态角进行卡尔曼滤波融合，得到轮式移动机器人的最终姿态角。

子步骤s1014，底盘控制芯片根据速度数据和最终姿态角解算轮式移动机器人在世界坐标系下的速度及位置信息，得到第一位姿数据。

步骤s102，机器人主控子系统根据单目相机获取到的图像数据计算出视觉里程计模型的第二位姿数据。

请参阅图4，步骤s102包括如下子步骤：

子步骤s1021，机器人主控子系统对图像数据提取fast角点，并进行lk光流跟踪，得到图像特征点匹配信息。

子步骤s1022，机器人主控子系统根据预设的图像特征点发布频率发布图像特征点匹配信息。

子步骤s1023，机器人主控子系统将图像数据的第一帧设为关键帧，其他图像帧根据当前图像跟踪上一关键帧图像的特征点数及特征点的平均视差确定是否设定为关键帧。

子步骤s1024，机器人主控子系统建立图像跟踪的滑动窗口。

子步骤s1025，机器人主控子系统计算滑动窗口中各帧图像的位置关系得到旋转矩阵，并选取偏航角作为初始旋转矩阵，平移向量取在x轴与y轴水平面上的平移，建立重投影误差代价函数，进行3自由度最小化重投影误差计算，得到图像关键帧之间缺乏尺度的旋转和平移矩阵。

步骤s103，机器人主控子系统将第一位姿数据和第二位姿数据进行时间戳对齐和运动轨迹对齐，恢复单目相机的最优相机尺度。

请参阅图5，步骤s103包括如下子步骤：

子步骤s1031，机器人主控子系统将第一位姿数据与第二位姿数据时间戳对齐。

子步骤s1032，机器人主控子系统将时间戳对齐后的第一位姿数据与第二位姿数据进行轨迹对齐。

子步骤s1033，机器人主控子系统通过求取损失函数的最小二乘解，得到单目相机的最优相机尺度。

步骤s104，机器人主控子系统依据最优相机尺度对第二位姿数据进行尺度恢复。

步骤s105，机器人主控子系统将第一位姿数据与尺度恢复后的第二位姿数据进行融合，得到轮式移动机器人的最终位姿数据。

步骤s106，机器人主控子系统对图像数据中的每一个关键帧图像进行回环检测。

请参阅图6，步骤s106包括如下子步骤：

子步骤s1061，机器人主控子系统对图像数据中的每个关键帧图像提取多个fast角点，并计算每个fast角点的brief描述子。

子步骤s1062，机器人主控子系统根据每个fast角点及对应的brief描述子，通过dbow2算法计算当前帧与之前关键帧的相似度大小。

子步骤s1063，当相似度大于设定的阈值时，机器人主控子系统判定则发生回环。

步骤s107，机器人主控子系统判断是否发生回环，若是，执行步骤s108。

步骤s108，机器人主控子系统对轮式移动机器人进行重定位计算。

综上所述，本发明提供的定位方法及系统针对轮式里程计会存在累积误差且不能自动消除，轮子打滑等外界因素对定位精度的影响，通过传感器融合的思想来进一步优化定位信息，将轮式里程计与单目视觉里程计结合起来，克服相互的缺点，保留各自的优点，能够克服定位过程中单目相机130没有尺度和鲁棒性差的问题，同时也可以解决轮式里程计累积误差的问题。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。