设备定位方法、智能设备和可读存储介质与流程

本技术实施例涉及智能设备自主导航，尤其涉及一种设备定位方法、智能设备和可读存储介质。

背景技术：

1、随着新能源汽车的发展，自动充电是新能源汽车生态链中的重要环节，行业内已经推出移动充电机器人，可以为车辆提供一对一的充电服务。

2、移动充电机器人需要在狭窄的车缝空间内自主移动到车辆的充电口附近，用机械臂将充电枪插入车辆的充电口，完成充电。在移动充电机器人移动的过程中，任何剐蹭都会带来较大的经济损失。因此，移动充电机器人具有安全、可靠的自主导航能力和移动避障能力是尤其重要的。

3、移动充电机器人的底盘上安装有摄像头，通过摄像头拍摄的图像实现设备定位和地图重建。但是，由于充电环境的复杂性和移动充电机器人运动的复杂性，单传感器的感知能力极其不足，导致设备定位不精细。

技术实现思路

1、本技术实施例提供一种设备定位方法、智能设备和可读存储介质，提高了设备定位精度。

2、第一方面，提供了一种设备定位方法，应用于智能设备，智能设备上设置有imu和至少两个相机，至少两个相机之间的位姿可变化，方法包括：获取至少两个相机在第一时刻的初始外参，以及在第一时刻之后imu采集的多帧imu数据和至少两个相机采集的多帧图像；对相邻两帧图像之间的imu数据进行预积分，得到imu预积分数据；获取图像中的特征点的特征信息；对于多帧图像中的当前帧，判断当前帧是否为关键帧；若当前帧为关键帧，则将当前帧加入滑动窗口内，并根据滑动窗口内关键帧的相对位姿、imu预积分数据、特征点的特征信息、初始外参和至少两个相机当前的外参，构建当前帧的状态向量和目标函数；其中，滑动窗口内包括至少一个关键帧；对目标函数求取最优解，得到稀疏地图、智能设备的当前位姿和至少两个相机当前的优化外参。

3、第一方面提供的设备定位方法，适用于智能设备的实时定位，智能设备上的imu和至少两个相机构成了时变基线的传感器系统。多个相机分布在智能设备上的不同位置、不同高度，多个相机之间的相对位置和相对姿态可以实时变化，增强了环境探测的视角。相比于单传感器，基于imu采集的imu数据和多个相机采集的图像，克服了单传感器视场角的不足，鲁棒性更强，有利于设备的精准定位。而且，在对环境建模时，将相机的外参加入待估计的状态向量，在相机初始外参的基础上，计算多个相机相对于智能设备的imu的相对位姿，确保设备定位方法适用于多传感器时变基线的情况，提高了设备定位的精度和可靠性。

4、一种可能的实现方式中，根据滑动窗口内关键帧的相对位姿、imu预积分数据、特征点的特征信息、初始外参和至少两个相机当前的外参，构建当前帧的状态向量和目标函数，包括：根据滑动窗口内关键帧的相对位姿和imu预积分数据，获取滑动窗口内每个关键帧的imu状态数据；imu状态数据包括关键帧相对于上一个关键帧的相对平移、相对角度、相对速度和随机游走误差；根据特征点的特征信息，获取滑动窗口内所有关键帧的所有特征点的特征信息；根据滑动窗口内每个关键帧的imu状态数据、滑动窗口内所有特征点的特征信息、初始外参和至少两个相机当前的外参，构建当前帧的状态向量和目标函数。

5、一种可能的实现方式中，判断当前帧是否为关键帧，包括：根据imu预积分数据获取当前帧和当前帧之前上一个关键帧之间的第一相对位置偏移；若第一相对位置偏移大于第一预设位置偏移，则判断当前帧为关键帧；和/或，根据imu预积分数据获取当前帧和当前帧之前上一个关键帧之间的第一相对角度偏移；若第一相对角度偏移大于第一预设角度偏移，则判断当前帧为关键帧；和/或，若当前帧和当前帧之前上一个关键帧之间的时间差大于预设时间差，则判断当前帧为关键帧。

6、一种可能的实现方式中，图像包括rgb图像，获取图像中的特征点的特征信息，包括：提取每帧rgb图像的fast角点；对多帧rgb图像的fast角点进行特征跟踪，获取每帧rgb图像中的特征点和特征点的特征信息。

7、一种可能的实现方式中，获取imu采集的多帧imu数据和至少两个相机采集的多帧图像，包括：获取基于同一时钟触发的imu采集的多帧imu数据和至少两个相机采集的多帧图像。

8、一种可能的实现方式中，获取imu采集的多帧imu数据和至少两个相机采集的多帧图像，包括：获取imu根据第一时钟采集的多帧imu数据，获取至少两个相机根据第二时钟采集的多帧图像；imu数据和图像均包括时间戳；根据imu数据的时间戳和图像的时间戳，对多帧imu数据和多帧图像进行时间对齐。

9、一种可能的实现方式中，图像还包括深度图，方法还包括：对深度图滤波，得到滤波后的深度图；根据至少两个相机的内参，将滤波后的深度图转换为点云；根据智能设备的当前位姿和至少两个相机当前的优化外参，将点云转换到智能设备所在空间的三维网格中，得到点云模型；其中，空间被预先划分为多个三维网格，每个三维网格包括点云中的1个点；利用射线追踪算法和点云模型，得到稠密点云图。

10、一种可能的实现方式中，方法还包括：在第一时刻之前的历史关键帧中确定是否存在目标关键帧；目标关键帧和当前帧的相对位姿偏移在预设偏移范围内；若存在目标关键帧，则开启新的线程，利用新的线程构建当前帧和目标关键帧之间的相对位姿约束；根据相对位姿约束对历史关键帧的相对位姿进行优化。

11、一种可能的实现方式中，方法还包括：对于当前帧之后的第一关键帧，根据滑动窗口内关键帧的优化后的相对位姿、imu预积分数据、特征点的特征信息、初始外参和至少两个相机当前的外参，构建第一关键帧的状态向量和目标函数。

12、一种可能的实现方式中，在第一时刻之前的历史关键帧中确定是否存在目标关键帧，包括：在历史关键帧中确定当前帧的k个第一候选关键帧；k为大于等于0的整数；若k>0，分别提取当前帧和k个第一候选关键帧的全局描述子；分别计算当前帧的全局描述子和k个第一候选关键帧的全局描述子之间的余弦相似度，在k个第一候选关键帧中确定余弦相似度大于预设余弦相似度阈值的p个第二候选关键帧；p为大于等于0且小于等于k的整数；若p>0，获取每个第二候选关键帧和当前帧之间的相对位姿偏移；在p个第二候选关键帧中确定相对位姿偏移最小的目标第二候选关键帧；若目标第二候选关键帧对应的相对位姿偏移在预设偏移范围内，则确定目标第二候选关键帧为目标关键帧。

13、一种可能的实现方式中，相对位姿偏移包括当前帧和目标关键帧之间的第二相对位置偏移和第二相对角度偏移；相对位姿偏移在预设偏移范围内，包括：第二相对位置偏移小于第二预设位置偏移；并且，第二相对角度偏移小于第二预设角度偏移。

14、一种可能的实现方式中，多帧图像中的第一帧为关键帧。

15、一种可能的实现方式中，初始外参包括相机的初始相对平移和初始相对角度。

16、一种可能的实现方式中，imu预积分数据包括相邻两帧图像之间的相对平移、相对角度和相对速度。

17、一种可能的实现方式中，方法还包括：若当前帧不是关键帧，则获取智能设备在当前帧之前上一个关键帧的位姿；根据智能设备在当前帧之前上一个关键帧的位姿和imu预积分数据，确定智能设备的当前位姿。

18、一种可能的实现方式中，方法还包括：将当前帧之前上一个关键帧对应的稀疏地图和至少两个相机的优化外参，确定为当前帧对应的稀疏地图和至少两个相机的优化外参。

19、一种可能的实现方式中，智能设备为充电机器人，充电机器人包括移动底座和用于带动充电接口移动的机械臂，移动底座和机械臂上分别设置有至少一个相机。

20、第二方面，提供了一种智能设备，包括：数据采集模块，用于获取所述至少两个相机在第一时刻的初始外参，以及在所述第一时刻之后所述imu采集的多帧imu数据和所述至少两个相机采集的多帧图像；数据处理模块，用于对相邻两帧所述图像之间的所述imu数据进行预积分，得到imu预积分数据；所述数据处理模块，还用于获取所述图像中的特征点的特征信息；定位模块，用于判断所述多帧图像中的当前帧是否为关键帧；若所述当前帧为关键帧，则将所述当前帧加入滑动窗口内，并根据所述滑动窗口内关键帧的相对位姿、所述imu预积分数据、所述特征点的特征信息、所述初始外参和所述至少两个相机当前的外参，构建所述当前帧的状态向量和目标函数；其中，所述滑动窗口内包括至少一个关键帧；对所述目标函数求取最优解，得到稀疏地图、所述智能设备的当前位姿和所述至少两个相机当前的优化外参。

21、一种可能的实现方式中，所述定位模块用于：根据所述滑动窗口内关键帧的相对位姿和所述imu预积分数据，获取所述滑动窗口内每个所述关键帧的imu状态数据；所述imu状态数据包括所述关键帧相对于上一个关键帧的相对平移、相对角度、相对速度和随机游走误差；根据所述特征点的特征信息，获取所述滑动窗口内所有关键帧的所有特征点的特征信息；根据所述滑动窗口内每个所述关键帧的imu状态数据、所述滑动窗口内所有特征点的特征信息、所述初始外参和所述至少两个相机当前的外参，构建所述当前帧的状态向量和目标函数。

22、一种可能的实现方式中，所述定位模块用于：根据所述imu预积分数据获取所述当前帧和所述当前帧之前上一个关键帧之间的第一相对位置偏移；若所述第一相对位置偏移大于第一预设位置偏移，则判断所述当前帧为关键帧；和/或，根据所述imu预积分数据获取所述当前帧和所述当前帧之前上一个关键帧之间的第一相对角度偏移；若所述第一相对角度偏移大于第一预设角度偏移，则判断所述当前帧为关键帧；和/或，若所述当前帧和所述当前帧之前上一个关键帧之间的时间差大于预设时间差，则判断所述当前帧为关键帧。

23、一种可能的实现方式中，所述图像包括rgb图像，所述数据处理模块用于：提取每帧所述rgb图像的fast角点；对多帧所述rgb图像的fast角点进行特征跟踪，获取每帧所述rgb图像中的特征点和所述特征点的特征信息。

24、一种可能的实现方式中，所述数据采集模块用于：获取基于同一时钟触发的所述imu采集的多帧imu数据和所述至少两个相机采集的多帧图像。

25、一种可能的实现方式中，所述数据采集模块用于：获取所述imu根据第一时钟采集的多帧imu数据，获取所述至少两个相机根据第二时钟采集的多帧图像；所述imu数据和所述图像均包括时间戳；根据所述imu数据的时间戳和所述图像的时间戳，对所述多帧imu数据和所述多帧图像进行时间对齐。

26、一种可能的实现方式中，所述图像还包括深度图，智能设备还包括地图应用模块；所述地图应用模块用于：对所述深度图滤波，得到滤波后的深度图；根据所述至少两个相机的内参，将所述滤波后的深度图转换为点云；根据所述智能设备的当前位姿和所述至少两个相机当前的优化外参，将所述点云转换到所述智能设备所在空间的三维网格中，得到点云模型；其中，所述空间被预先划分为多个三维网格，每个所述三维网格包括所述点云中的1个点；利用射线追踪算法和所述点云模型，得到稠密点云图。

27、一种可能的实现方式中，智能设备还包括闭环检测模块；所述闭环检测模块用于：在所述第一时刻之前的历史关键帧中确定是否存在目标关键帧；所述目标关键帧和所述当前帧的相对位姿偏移在预设偏移范围内；若存在目标关键帧，则开启新的线程，利用所述新的线程构建所述当前帧和所述目标关键帧之间的相对位姿约束；根据所述相对位姿约束对所述历史关键帧的相对位姿进行优化。

28、一种可能的实现方式中，所述定位模块还用于：对于所述当前帧之后的第一关键帧，根据所述滑动窗口内关键帧的优化后的相对位姿、imu预积分数据、特征点的特征信息、所述初始外参和所述至少两个相机当前的外参，构建所述第一关键帧的状态向量和目标函数。

29、一种可能的实现方式中，所述闭环检测模块用于：在所述历史关键帧中确定所述当前帧的k个第一候选关键帧；k为大于等于0的整数；若k>0，分别提取所述当前帧和k个所述第一候选关键帧的全局描述子；分别计算所述当前帧的全局描述子和k个所述第一候选关键帧的全局描述子之间的余弦相似度，在k个所述第一候选关键帧中确定余弦相似度大于预设余弦相似度阈值的p个第二候选关键帧；p为大于等于0且小于等于k的整数；若p>0，获取每个所述第二候选关键帧和所述当前帧之间的相对位姿偏移；在p个所述第二候选关键帧中确定相对位姿偏移最小的目标第二候选关键帧；若所述目标第二候选关键帧对应的相对位姿偏移在所述预设偏移范围内，则确定所述目标第二候选关键帧为所述目标关键帧。

30、一种可能的实现方式中，所述相对位姿偏移包括所述当前帧和所述目标关键帧之间的第二相对位置偏移和第二相对角度偏移；所述相对位姿偏移在所述预设偏移范围内，包括：所述第二相对位置偏移小于第二预设位置偏移；并且，所述第二相对角度偏移小于第二预设角度偏移。

31、一种可能的实现方式中，所述多帧图像中的第一帧为所述关键帧。

32、一种可能的实现方式中，所述初始外参包括所述相机的初始相对平移和初始相对角度。

33、一种可能的实现方式中，所述imu预积分数据包括相邻两帧所述图像之间的相对平移、相对角度和相对速度。

34、一种可能的实现方式中，所述定位模块还用于：若所述当前帧不是关键帧，则获取所述智能设备在所述当前帧之前上一个关键帧的位姿；根据所述智能设备在所述当前帧之前上一个关键帧的位姿和所述imu预积分数据，确定所述智能设备的当前位姿。

35、一种可能的实现方式中，所述定位模块还用于：将所述当前帧之前上一个关键帧对应的稀疏地图和所述至少两个相机的优化外参，确定为所述当前帧对应的稀疏地图和所述至少两个相机的优化外参。

36、第三方面，提供一种智能设备，包括处理器，处理器用于与存储器耦合，并读取存储器中的指令并根据指令使得智能设备执行第一方面提供的方法。

37、一种可能的实现方式中，智能设备为充电机器人，充电机器人包括移动底座和用于带动充电接口移动的机械臂，移动底座和机械臂上分别设置有至少一个相机。

38、第四方面，提供一种程序，该程序在被处理器执行时用于执行第一方面提供的方法。

39、第五方面，提供一种计算机可读存储介质，计算机可读存储介质中存储有指令，当指令在计算机或处理器上运行时，实现第一方面提供的方法。

40、第六方面，提供一种程序产品，所述程序产品包括计算机程序，所述计算机程序存储在可读存储介质中，设备的至少一个处理器可以从所述可读存储介质读取所述计算机程序，所述至少一个处理器执行所述计算机程序使得该设备实施第一方面提供的方法。