申请/专利权人：河北工业大学

申请日：2024-04-01

公开（公告）日：2024-06-25

公开（公告）号：CN118243087A

主分类号：G01C21/00

分类号：G01C21/00;G01C21/16;G01C21/20

优先权：

专利状态码：在审-公开

法律状态：2024.06.25#公开

摘要：本发明公开了一种基于VINS‑Mono的水下核电环境移动机器人SLAM方法，首先采集水下环境图像，并进行预处理；将图像中的高亮区域设置掩码，对图像中掩码以外的区域进行特征点检测，通过光流追踪得到匹配特征点；然后，利用匹配特征点和IMU测量数据，进行旋转平移解耦的视觉惯导初始化，得到滑动窗口内各关键帧的旋转矩阵、平移向量、速度、陀螺仪零偏、重力向量和尺度；初始化完成系统正常运行时，对视觉惯导初始化得到的各参数进行非线性优化，采用信息矩阵动态调整策略，得到移动机器人的位置和姿态。该方法通过旋转平移解耦的视觉惯导初始化，提升了视觉惯导初始化的速度与准确性；利用特征点跟踪次数判断视觉信息的可信度，动态调整视觉约束的信息矩阵，能够更好的适用于核电站一回路的复杂作业环境并拥有较好的定位精度。

主权项：1.一种基于VINS-Mono的水下核电环境移动机器人SLAM方法，其特征在于，该方法包括以下步骤：S1：采集水下环境图像，并进行预处理；将图像中的高亮区域设置掩码，对图像中掩码以外的区域进行特征点检测，通过光流追踪得到匹配特征点；S2：利用匹配特征点和IMU测量数据，进行旋转平移解耦的视觉惯导初始化；IMU预积分遵循式1～3： 式中，分别为i、j时刻IMU坐标系下的位置，分别为初始、i、j时刻IMU坐标系下的速度，为IMU坐标系下的重力向量，Δtij为从i时刻到j时刻的时间间隔，分别为i、j时刻IMU坐标系下的旋转矩阵，分别为i到j时刻平移、速度预积分量；通过式4～6对IMU测量数据进行预积分，得到平移、速度以及旋转预积分量；使用陀螺仪零偏更新旋转预积分量表示为式7； 式中，为i到j时刻旋转预积分量，为使用陀螺仪零偏bg更新后的i到j时刻旋转预积分量，分别为i到f、i到k时刻IMU坐标系下的旋转矩阵，分别为f、k时刻IMU坐标系下的加速度，Δt为时间间隔，为k时刻IMU坐标系下的角速度，为雅可比矩阵，Exp·为从李代数到李群的映射；将式6的旋转预积分量由IMU坐标系转换到相机坐标系，计算相机坐标系下相邻关键帧之间的旋转矩阵为： 式中，为相机坐标系下从i时刻到j时刻的旋转矩阵，Rbc为相机与IMU之间的旋转外参，为相机与IMU之间旋转外参的转置；根据匹配特征点以及由式4、8解算得到的初始位姿，通过三角测量计算相邻关键帧之间共视的多个3D点，每个3D点与相邻关键帧的相机中心组成一个平面，则平面法向量表示为： 式中，nu为第u个3D点对应的平面法向量，为从i时刻的相机中心指向第u个3D点的单位向量，为从j时刻的相机中心指向第u个3D点的单位向量；令矩阵N＝[n1,…,nu,…,nn]，矩阵M＝NNT，为矩阵M的最小特征值，则相机坐标系下相邻关键帧之间的旋转矩阵的迭代值为： i、j时刻之间的矩阵M表示为： 结合式7、8、11，i、j时刻之间的矩阵M进一步表示为： 迭代求解式12，得到陀螺仪零偏的迭代值为： 其中，μ为有共同特征的关键帧对集合；根据陀螺仪零偏的迭代值通过式6、7计算滑动窗口内所有相邻关键帧之间的旋转矩阵；将第一张关键帧图像视为世界坐标系，使用线性全局平移约束求解关键帧图像在世界坐标系中的位置；再求解视觉惯导系统的初始状态变量为i、k时刻IMU坐标系下的速度，s为尺度，为第0帧相机坐标系下的重力向量；S3：对视觉惯导初始化得到的各参数进行非线性优化，得到移动机器人的位置和姿态；在系统运行过程中，记录滑动窗口内的图像中特征点跟踪次数大于等于两次的次数Nt，以及图像中所有特征点的总跟踪次数Ns，计算比例；pt＝NtNts21ps＝NsNss22式中，Nts为滑动窗口内的图像中提取的特征点总个数，Nss为滑窗内的图像中所有特征点的最大跟踪次数，pt为特征点跟踪次数大于等于两次的比例，ps为所有特征点总跟踪次数的比例；若pt小于当前关键帧处于恶劣环境的判断阈值，且ps小于滑动窗口时间段内移动机器人处于恶劣环境的判断阈值，则根据式23更新视觉约束的信息矩阵； 式中，P、分别为更新前、后的视觉约束的信息矩阵。

全文数据：

权利要求：

百度查询： 河北工业大学 基于VINS-Mono的水下核电环境移动机器人SLAM方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD