本发明涉及slam定位，尤其涉及一种基于slam-imu耦合的gps信号盲区下实时定位方法。
背景技术：
1、现有的定位技术的实现方法主要依赖于工人佩戴的定位标签所发出的信号。这些信号通过智能部署的定位基站进行接收和处理，以获取工人的位置信息，从而对工人进行精确定位管理。该技术的实现基础是外部固定基站与现场移动目标之间的信号传输。然而，值得注意的是，这种定位技术对于遮挡严重、信号干扰强烈的环境具有较高的敏感性，这在一定程度上影响了其定位精度和稳定性。
2、slam是指在未知环境中实现自身定位的同时并构建环境地图的方法。slam这个术语最早出现在机器人领域，其主要功能在于在未知环境下，对设备的姿态以及环境的三维结构进行同步估计。slam是一种利用视觉信息或激光和传感器数据来实现同时定位与地图构建的技术。视觉slam用摄像头来完成环境的感知工作。在未知环境中，自主移动的人或机器人通过摄像头捕捉的信息数据，对自身位置及其周边环境进行初步预估。随着机器人的移动，它依据实时位置估计与摄像头所感知的数据进行精准定位，并在此过程中持续构建与更新地图，同时合理规划行进路径。它被广泛应用于三维建模、虚拟现实(vr)、机器人、自动驾驶等领域。这些领域的成功运用，给卫星空三定位无效场景下的工人精确定位带来了可能，只需要给人员佩戴视觉相机，通过场地信息的变化来确定当前的位置，可用于卫星定位差等场景。公开号为cn114136311a的中国发明申请公开了一种基于imu预积分的激光slam定位方法，结合imu和激光slam实现位姿求解，然而激光slam较为缺乏回环检测的能力，累计误差的消除较为困难。
技术实现思路
1、本发明的目的在于解决现有技术中的问题。
2、本发明解决其技术问题所采用的技术方案是：提供一种基于slam-imu耦合的gps信号盲区下实时定位方法，基于包括设有slam传感器的双目相机和imu元件的定位装置，定位方法包括以下步骤：
3、双目相机将捕捉到的不同离散时间点的图像序列传输给计算机；
4、imu元件将采集到的实时角速度和加速度传输给计算机；
5、计算机结合图像序列、实时角速度和加速度，建立包含多个优化变量的非线性目标函数，通过多次迭代使优化变量达到全局最优解，从而得到实时位置和行动轨迹。
6、优选的，所述计算机获得工作人员的实时位置和行动轨迹的过程包括以下步骤：
7、对采集到的图像序列进行角点提取、角点优化和角点匹配以建立视觉信息；所述角点指显著特征点；
8、根据角速度和加速度构建imu运动模型，并对imu运动模型进行预积分，获得简化imu运动模型；
9、结合视觉信息和简化imu运动模型建立包含多个优化变量的非线性目标函数；
10、通过基于传感器测量的近似值确定优化变量的初始迭代值，通过多次迭代获得全局最优解，得到工作人员的实时位置和行动轨迹。
11、优选的，所述计算机建立视觉信息的过程包括以下步骤：
12、角点提取，采用shi-tomasi算法提取角点，通过选择最佳的角点来提高特征点的质量；
13、角点优化，采用插值法进行亚像素角点检测，通过沿着物体边缘进行线性插值，避免因为像素值突变带来的值的突变；
14、角点匹配，采用光流法跟踪匹配角点，利用相邻帧之间的像素亮度变化来计算相机的位移和旋转，实时估计相机的运动，从而在地图构建过程中提供定位信息。
15、优选的，所述插值法利用线性插值求解像素灰度值，将所需点周围的已知像素灰度值与两个已知量的直线结合起来，从而找到所需点的像素灰度值。
16、优选的，对于所需点p(x,y)，已知p(x,y)周围的四个点的像素灰度值，包括a(x1,y2)、b(x2,y2)、c(x1,y1)和d(x2,y1)的像素灰度值，y2＞y＞y1，x2＞x＞x1；计算p(x,y)像素灰度值的过程包括：
17、在ab之间和cd之间分别插入像素点f＝(x,y1)和e＝(x,y2)；
18、根据e、f的像素值计算p点的像素值，表示为：
19、
20、其中，f(·)表示像素值。
21、优选的，所述角点匹配采用光流法跟踪匹配角点，当在m×m的窗口中，图像里m个空间点在连续的时间内运动一致，表示为：
22、
23、其中，μ表示像素点沿x轴上的速度矢量，v表示像素点沿y轴上的速度矢量；fx表示fy表示ft表示f表示为f(x,y,t)，指坐标(x,y)处的点在t时刻的像素灰度值。
24、优选的，所述imu运动模型，表示为：
25、
26、其中，表示j时刻的位置，表示j时刻的速度，表示j时刻的姿态；表示i时刻的位置，表示i时刻的速度，δt表示i到j的时间间隔，表示t时刻的姿态；表示t时刻加速度的实际值，gw表示全球坐标系统下的重力矢量，δt表示很短的时间间隔；表示t时刻角速度的实际值；表示表示四元素乘法。
27、优选的，所述对imu运动模型进行预积分，具体为，将第一帧的imu数据与第a帧的图像对齐，再将最后一帧的imu数据与第b帧的图像对齐，最后对数据进行积分处理，得到简化imu运动模型表示为：
28、
29、其中，表示第k+1时刻imu坐标系相对于世界坐标系的位置，表示第k+1时刻imu坐标系相对于世界坐标系的速度，表示表示第k+1时刻imu坐标系到世界坐标系的姿态；分别表示第k时刻imu坐标系相对于世界坐标系的位置、速度、姿态，δtk表示表示k到k+1的时间间隔，表示t时刻加速度计偏置，表示t时刻受陀螺仪偏置；表示body系下加速度计测量值，表示body系下陀螺仪测量值，bg和ba是指陀螺仪的零偏和加速度计的零偏，是imu的body坐标系相对于世界坐标系的旋转矩阵。
30、优选的，所述结合视觉信息和简化imu运动模型建立非线性目标函数，包括以下步骤：
31、构建状态向量，包括传感器状态、外参和空间路标的逆深度，状态向量x表示为：
32、
33、其中，x0,x1,…,xn表示传感器状态，包括传感器的监测参数(坐标、速率、转动角度)、加速度计偏差和陀螺仪读数，其中第k帧的状态量xk包括平移量速度量旋转量ba表示imu中的加速度计的偏差，bg表示imu中陀螺仪的偏差；λ0,λ1,…,λm表示空间路标的逆深度，空间路标的逆深度为路标点深度的倒数；表示相机和imu的外参，包括相机与imu之间的平移和旋转关系；
34、对imu模型施加约束，主要为两帧之间的位移、速度、旋转、加速度偏差和陀螺仪偏差的变化，得到imu测量残差表示为：
35、
36、其中，δa为位置预积分误差，δθ为角度预积分误差，δβ为速度预积分误差，δba为加速度零偏误差，δbg为陀螺仪零偏误差；其中，表示四元素乘法，[·]xyz为只取四元数的虚部(x,y,z)的三维向量；表示第k到k+1帧imu本体坐标系下的旋转矩阵，第k+1时刻imu坐标系相对于世界坐标系的位置，第k时刻imu坐标系相对于世界坐标系的位置，第k时刻imu坐标系相对于世界坐标系的位置的速度，第k到k+1帧的imu本体坐标系下的加速度，表示从帧bk到帧bk+1imu本体坐标系下的四元数形式的旋转，从世界坐标系到第k个时刻imu本体坐标系的旋转四元数的逆，第k+1时刻从本体坐标系转换到世界坐标系的旋转四元数(姿态)，表示第k个时刻的imu本体坐标系到世界坐标系的旋转矩阵，表示第k+1时刻imu坐标系相对于世界坐标系的速度，从第k个时刻的imu本体坐标系转换到第k+1个时刻的imu本体坐标系的偏差，表示第k+1时刻imu本体坐标系的加速度偏差，表示第k时刻imu本体坐标系的加速度误差，表示第k+1时刻imu本体坐标系的角速度偏差，第k时刻imu本体坐标系的角速度偏差；
37、对视觉信息施加约束，通过使像素重投影导致的视觉残差最小化获取理想的传感器姿态；对第1个路标点在第i个相机坐标系中首次投影所得的像素坐标值进行坐标转换，并将其映射到第j个坐标系，由此转换而来的像素坐标值与直接观测到的对应像素坐标值之间的差异构成了视觉残差，表示为：
38、
39、其中，投影到第cj帧相机坐标系下x值，投影到第cj帧相机坐标系下y值，投影到第cj帧相机坐标系下z值，表示从imu坐标系转换回相机坐标系的逆齐次变换矩阵，
40、表示从世界坐标系到第j个imu坐标系的齐次变换矩阵的逆矩阵，表示从世界坐标系到第j个imu坐标系的齐次变换矩阵的逆矩阵，tbc表示从相机坐标系到imu坐标系下的齐次变换矩阵；特征点在第ci帧上的像素坐标为λ表示逆深度；
41、综合考虑先验信息、视觉数据和imu约束信息，经过非线性优化方法得到综合多传感器信息的最佳状态向量，表示为：
42、
43、其中，||rp-jpx||2是边缘化的先验信息，是imu测量残差，是视觉残差，表示imu预积分的噪声协方差，表示视觉产生的噪声协方差。
44、优选的，所述通过多次迭代使优化变量达到全局最优解，所述优化变量包括imu测量残差视觉残差imu预积分的噪声协方差和视觉产生的噪声协方差所述优化变量的初始迭代值基于传感器测量的近似值确定。
45、本发明具有如下有益效果：
46、(1)通过shi-tomasi角点检测算法、插值法和光流跟踪算法优化双目视觉slam构建的场地三维图形，以及slam和imu耦合定位算法进一步提升slam视觉方法的完整性、精确性和稳定性并提供尺度信息，从而实现现场作业人员的实时定位；其中，角点优化采用插值法进行亚像素角点检测，通过沿着物体边缘进行线性插值，可以避免因为像素值突变带来的值的突变，使得最终获取的点的像素灰度值更加准确；
47、(2)对比激光slam定位技术，本发明所采用的视觉slam使用了大量冗余的纹理信息，回环检测较为容易，即使在前端累计一定误差的情况下仍能通过回环修正将误差消除。并且激光雷达的价格远高于视觉传感器，部署成本高，因此在实际应用中的可操作性不大，相比起来，本发明采用的视觉slam具有成本低廉、部署方便、可视化程度高等优点，在实际应用中更具可操作性；
48、(3)对比将imu残差项、点特征和线特征残差项加入到ba联合优化位姿计算，本发明采用了滑动窗口方法来优化计算过程的同时通过舒尔补方法，将滑出窗口的帧与窗口内帧的约束转化为先验误差信息，并将其耦合到耦合定位的目标函数中，以减轻了耦合算法的计算量和计算时间；
49、(4)本发明结合imu和激光slam实现人员实时定位，有效解决了现有定位管理系统中信号盲区导致定位不准确的问题，有助于保护施工现场工人的生命安全，提高了施工现场监管人员的管理效率，为整个施工过程的安全稳定提供了有利保障。
50、以下结合附图及实施例对本发明作进一步详细说明，但本发明不局限于实施例。
1.一种基于slam-imu耦合的gps信号盲区下实时定位方法，其特征在于，基于包括设有slam传感器的双目相机和imu元件的定位装置，定位方法包括以下步骤：
2.根据权利要求1所述的基于slam-imu耦合的gps信号盲区下实时定位方法，其特征在于，所述计算机获得工作人员的实时位置和行动轨迹的过程包括以下步骤：
3.根据权利要求1所述的基于slam-imu耦合的gps信号盲区下实时定位方法，其特征在于，所述计算机建立视觉信息的过程包括以下步骤：
4.根据权利要求3所述的基于slam-imu耦合的gps信号盲区下实时定位方法，其特征在于，所述插值法利用线性插值求解像素灰度值，将所需点周围的已知像素灰度值与两个已知量的直线结合起来，从而找到所需点的像素灰度值。
5.根据权利要求4所述的基于slam-imu耦合的gps信号盲区下实时定位方法，其特征在于，对于所需点p(x,y)，已知p(x,y)周围的四个点的像素灰度值，包括a(x1,y2)、b(x2,y2)、c(x1,y1)和d(x2,y1)的像素灰度值，y2＞y＞y1，x2＞x＞x1；计算p(x,y)像素灰度值的过程包括：
6.根据权利要求4所述的基于slam-imu耦合的gps信号盲区下实时定位方法，其特征在于，所述角点匹配采用光流法跟踪匹配角点，当在m×m的窗口中，图像里m个空间点在连续的时间内运动一致，表示为：
7.根据权利要求6所述的基于slam-imu耦合的gps信号盲区下实时定位方法，其特征在于，所述imu运动模型，表示为：
8.根据权利要求7所述的基于slam-imu耦合的gps信号盲区下实时定位方法，其特征在于，所述对imu运动模型进行预积分，具体为，将第一帧的imu数据与第a帧的图像对齐，再将最后一帧的imu数据与第b帧的图像对齐，最后对数据进行积分处理，得到简化imu运动模型表示为：
9.根据权利要求8所述的基于slam-imu耦合的gps信号盲区下实时定位方法，其特征在于，所述结合视觉信息和简化imu运动模型建立非线性目标函数，包括以下步骤：
10.根据权利要求9所述的基于slam-imu耦合的gps信号盲区下实时定位方法，其特征在于，所述通过多次迭代使优化变量达到全局最优解，所述优化变量包括imu测量残差视觉残差imu预积分的噪声协方差和视觉产生的噪声协方差所述优化变量的初始迭代值基于传感器测量的近似值确定。
