本发明涉及计算机定位领域，具体涉及一种基于aruco标签的视觉惯性定位方法、系统、设备及存储介质。
背景技术：
1、视觉惯导里程计(visual inertial odometry，简称vio)是一种融合了视觉传感器与惯性测量单元(imu)信息的关键技术，用于实现精确的定位与导航功能，该技术广泛应用于机器人、自动驾驶车辆以及无人机等众多领域；即便在gps信号缺失或受限的环境中，vio技术也能帮助这些设备准确感知自身位置和运动状态，提供稳健可靠的导航解决方案。
2、而现有的vio在长时间运行中，会产生较大的累积误差，通常是通过回环检测模块进行位姿的修正，而在没有检测模块的情况下，vio的定位精度会逐渐降低，综上所述，提出一种不依赖回环检测的高精度vio定位技术现有技术人员需要解决的问题。
技术实现思路
1、针对现有技术中提到的问题，本发明提出一种基于aruco标签的视觉惯性定位方法、系统、设备及存储介质，可以解决现有的vio系统在长时间运行并且在没有检测模块的情况下累积误差较大的问题。
2、本发明一种基于aruco标签的视觉惯性定位方法，包括:
3、s1:在使用环境中布置多个id唯一的aruco标签，获取相邻aruco标签的相对位姿关系；
4、s2：建立vio系统，获取视觉惯性里程计的图像以及imu数据，根据所述图像以及imu数据得到vio坐标系；
5、s3：将vio系统检测的初始aruco标签定义为aruco坐标系，获取相机在aruco坐标系下的位姿以及相机在vio坐标系下的位姿，得到vio坐标系和aruco坐标系的相对位姿关系；
6、s4：判断vio系统运行时是否检测到图像帧中的aruco标签，若否则优化相机在vio坐标系下的位姿，直至vio系统检测到aruco标签；若是则建立自适应全局优化函数，消除累计误差，完成定位。
7、优选地，所述步骤s4中优化相机在vio坐标系下的位姿具体过程是：利用imu数据获得图像帧的起始位姿，根据起始位姿、图像帧的视觉重投影误差、imu数据帧间预积分误差以及边缘化的先验误差来优化相机在vio坐标系下的位姿，直至vio系统检测到aruco标签。
8、优选地，根据所述边缘化的先验误差、imu数据帧间预积分误差以及视觉重投影误差构建优化目标函数：
9、
10、式中:hp为海森矩阵；为滑动窗口内的所有相机状态；是所有imu测量值的集合；是预积分量；为视觉观测量；是在当前滑动窗口中至少观察到两次的特征集合；ρ是损失函数；rp是边缘化的先验误差；是imu数据帧间预积分误差；是视觉重投影误差的误差函数。
11、优选地，所述滑动窗口内的所有相机状态表达式如下：
12、
13、式中：为imu在vio坐标系中的位置；为imu在vio坐标系中的速度；为imu在vio坐标系中的方向；ba为imu坐标系中的加速度偏置；bg为imu坐标系中的陀螺仪偏置；n为关键帧的总数；m是滑动窗口中的特征点总数；λl为第l个特征点在被第一次观测到的图像帧中的逆深度；是相机和imu之间的外参。
14、优选地，所述步骤s4建立自适应全局优化函数具体过程是：当相机在识别到aruco标签后，对该aruco标签id检测，确定相机与当前aruco标签的相对位姿关系；
15、根据步骤s1中相邻aruco标签的相对位姿关系、步骤s3中相机在aruco坐标系下的位姿以及步骤s3中vio坐标系和aruco坐标系的相对位姿关系得到相机在vio坐标系的真实位姿；
16、根据关键帧间的约束以及当前aruco标签定位后产生的标签约束建立自适应全局优化的目标函数，如下式所示：
17、
18、式中，s是所有关键帧的集合；m是根据标签检测得到的相机位姿集合；ri,j是关键帧的帧间约束；ri,k是识别当前aruco标签得到的标签约束；
19、自适应全局优化的目标函数通过评估位姿的不确定性来确定帧间约束的关键帧数量，位姿的不确定性如下式所示：
20、
21、式中，pk表示第k帧位姿的协方差矩阵，δt表示第k帧和k-1帧之间的相对位姿，表示第k帧imu的协方差矩阵；
22、将帧间最大特征值之差与所设阈值进行对比，将小于阈值的关键帧带入自适应全局优化的目标函数中进行约束，消除累计误差，完成定位。
23、优选地，所述ri,j定义如下式所示：
24、
25、式中：为第i帧关键帧和第j帧关键帧在vio坐标系下的位置；为在第i帧坐标系下，i，j两帧的位置之差；ψi为第i帧关键帧在vio坐标系下的偏航角；ψj为第j帧关键帧在vio坐标系下的偏航角；为i，j两帧的偏航角之差；r为第i帧关键帧在vio坐标系下的旋转矩阵；为第i帧的俯仰角；为第i帧的滚转角；
26、所述ri,k定义如下式所示：
27、
28、式中：为第i帧关键帧在vio坐标系下的位置；为相机在vio坐标系的真实位姿；为在第i帧坐标系下，i，k两帧的位置之差；ψi为相机在vio坐标系下的偏航角；ψk为相机在vio坐标系的真实偏航角；为i，k两帧的偏航角之差；r为第i帧关键帧在vio坐标系下的旋转矩阵；为第i帧的俯仰角；为第i帧的滚转角。
29、优选地，所述阈值为前i帧关键帧的最大特征值的均值。
30、一种基于aruco标签的视觉惯性定位系统，包括：
31、第一获取模块，在使用环境中布置多个id唯一的aruco标签，获取相邻aruco标签的相对位姿关系；
32、第二获取模块，建立vio系统，获取视觉惯性里程计的图像以及imu数据，根据所述图像以及imu数据得到vio坐标系；
33、第三获取模块，将vio系统检测的初始aruco标签定义为aruco坐标系，获取相机在aruco坐标系下的位姿以及相机在vio坐标系下的位姿，得到vio坐标系和aruco坐标系的相对位姿关系；
34、定位模块，判断vio系统运行时是否检测到图像帧中的aruco标签，若否则优化相机在vio坐标系下的位姿，直至vio系统检测到aruco标签；若是则建立自适应全局优化函数，消除累计误差，完成定位。
35、一种基于aruco标签的视觉惯性定位设备，包括处理器和存储器，其中，所述处理器执行所述存储器中保存的计算机程序时实现上述的基于aruco标签的视觉惯性定位方法。
36、一种计算机可读存储介质，其特征在于，用于存储计算机程序，其中，所述计算机程序被处理器执行时实现上述的基于aruco标签的视觉惯性定位方法。
37、本发明相对于现有技术，取得了以下的技术效果：
38、本发明通过布置aruco标签，根据标签解决了vio系统在没有回环检测模块的情况下，系统定位过程中累积误差过大的问题。
1.一种基于aruco标签的视觉惯性定位方法，其特征在于，包括:
2.根据权利要求1所述一种基于aruco标签的视觉惯性定位方法，其特征在于，所述步骤s4中优化相机在vio坐标系下的位姿具体过程是：利用imu数据获得图像帧的起始位姿，根据起始位姿、图像帧的视觉重投影误差、imu数据帧间预积分误差以及边缘化的先验误差来优化相机在vio坐标系下的位姿，直至vio系统检测到aruco标签。
3.根据权利要求2所述一种基于aruco标签的视觉惯性定位方法，其特征在于，根据所述边缘化的先验误差、imu数据帧间预积分误差以及视觉重投影误差构建优化目标函数：
4.根据权利要求3所述一种基于aruco标签的视觉惯性定位方法，其特征在于，所述滑动窗口内的所有相机状态表达式如下：
5.根据权利要求1所述一种基于aruco标签的视觉惯性定位方法，其特征在于，所述步骤s4建立自适应全局优化函数具体过程是：当相机在识别到aruco标签后，对该aruco标签id检测，确定相机与当前aruco标签的相对位姿关系；
6.根据权利要求5所述一种基于aruco标签的视觉惯性定位方法，其特征在于，所述ri,j定义如下式所示：
7.根据权利要求5所述一种基于aruco标签的视觉惯性定位方法，其特征在于，所述阈值为前i帧关键帧的最大特征值的均值。
8.一种基于aruco标签的视觉惯性定位系统，其特征在于，包括：
9.一种基于aruco标签的视觉惯性定位设备，其特征在于，包括处理器和存储器，其中，所述处理器执行所述存储器中保存的计算机程序时实现如权利要求1至7任一项所述的基于aruco标签的视觉惯性定位方法。
10.一种计算机可读存储介质，其特征在于，用于存储计算机程序，其中，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述的基于aruco标签的视觉惯性定位方法。
