买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

龙图腾网恭喜河海大学申请的专利基于改进关键帧决策及姿态优化的水下定位与建图方法获国家发明授权专利权，本发明授权专利权由国家知识产权局授予，授权公告号为：CN119687904B 。

龙图腾网通过国家知识产权局官网在2025-05-09发布的发明授权授权公告中获悉：该发明授权的专利申请号/专利号为：202510205752.X，技术领域涉及：G01C21/00；该发明授权基于改进关键帧决策及姿态优化的水下定位与建图方法是由黄浩乾;张未;石瑞峰;王迪;王俊玮设计研发完成，并于2025-02-25向国家知识产权局提交的专利申请。

本基于改进关键帧决策及姿态优化的水下定位与建图方法在说明书摘要公布了：本发明公开了基于改进关键帧决策及姿态优化的水下定位与建图方法。首先获取AUV所配置传感器的数据；处理得到的传感器的数据，初始化系统参数，进行局部地图的跟踪，在基于ORB‑SLAM3关键帧选择的基础上，根据IMU的观测信息判断当前是否为动态环境，AUV是否剧烈运动，再继续进行关键帧的选择；对选择的关键帧的姿态进行优化，使用双约束进行姿态校正，恢复大视角快速弯曲运动下的局部轨迹，生成新的地图点；进行闭环检测与校正；生成全局轨迹与地图。本发明解决了自主水下航行器可能遇到动态环境或者本身运动剧烈导致位姿漂移的问题，同时使得视觉惯性同步定位与地图构建系统的准确性和鲁棒性大幅提高。

本发明授权基于改进关键帧决策及姿态优化的水下定位与建图方法在权利要求书中公布了：1.一种基于改进关键帧决策及姿态优化的水下定位与建图方法，其特征在于，包括以下步骤：S1.获取AUV所配置传感器的数据；S2.处理步骤S1得到的传感器的数据，初始化系统参数，进行局部地图的跟踪，在基于ORB-SLAM3关键帧选择的基础上，根据IMU的观测信息判断当前是否为动态环境，AUV是否剧烈运动，再继续进行关键帧的选择，具体包括：S21.对步骤S1中获得的图像帧进行特征提取和匹配：使用SIFT算法进行特征点提取，利用暴力匹配算法将帧与帧之间的特征进行匹配，建立特征对应关系；S22.对步骤S1中获得的IMU数据进行积分；S23.通过步骤S21匹配的特征点和步骤S22积分后的IMU数据，计算使用最小化重投影误差来优化相机位姿，实现局部地图跟踪；其中，重投影误差ei计算如式4所示：ei＝‖xi-πRtXi+tt‖24其中，xi是第i个特征点在图像上的观测位置；π是相机的投影函数，将三维点映射到二维图像平面；Rt是t时刻的相机旋转矩阵；tt是t时刻的相机平移向量；Xi是第i个特征点在世界坐标系中的位置；‖·‖2表示欧几里得范数，计算实际观测点和重投影点之间的距离；S24.基于ORB-SLAM3算法选择关键帧：设定一个阈值E1，判断当前帧中特征点的重投影误差ei是否超过阈值E1，如果超过，则说明当前帧提供了新的视觉信息，将其作为主关键帧，用于提供视觉信息；S25.基于IMU观测信息进行关键帧决策：步骤S1中获得的IMU观测数据中加速度的均方根角速度的均方根其中t指时间，b表示第b个采样时刻，N表示取样的数据点的总数，如果RMS1T1或者RMS2T2，其中T1为设定的加速度均方根的阈值，T2为设定的角速度均方根的阈值，则按照新的阈值E2选择新的关键帧，将其作为副关键帧，用于提供惯性信息，E2如式5所示：E2＝E1-m·RMS1+RMS25其中，m是设定的调节系数；S3.对步骤S2最后选择的关键帧的姿态进行优化，使用双约束进行姿态校正，恢复大视角快速弯曲运动下的局部轨迹，生成新的地图点，具体包括：S31.建立步骤S24中的主关键帧与步骤S25中的副关键帧之间的双测量约束：首先主关键帧通过误差ri建立视觉约束，ri计算如式6所示：ri＝‖xi-πRkXi+tk‖26其中，Rk和tk分别是主关键帧k的旋转矩阵和平移向量；然后副关键帧通过IMU预积分描述两个关键帧之间的运动来建立惯性约束，IMU预积分如式7所示： 其中，Ck,k-1是相机姿态变化的协方差矩阵，Rk-1和tk-1分别是副关键帧k-1的旋转矩阵和平移向量，是Rk-1的转置矩阵，pk和pk-1分别是主关键帧k和副关键帧k-1的位置向量；S32.设立优化目标，通过最小化目标函数来优化相机位姿，将目标函数设置为其中，第一项指的是视觉误差，第二项指的是IMU预积分误差；S33.通过滑模滤波算法来处理快速大曲率运动情况，优化并恢复曲线运动下的轨迹，首先，将相机和IMU系统建模为状态空间模型，如式8所示： 其中，xk、xk+1是系统状态向量，uk是IMU测量，yk是观测数据，ωk和vk分别是过程噪声和观测噪声，Fk是状态转移矩阵，Bk是控制输入矩阵，Hk是观测矩阵；接着设计滑模面σk用于引导估计状态xk收敛到期望值，σk如式9所示：σk＝Ckxk-dk9其中，Ck是滑模面设计矩阵，dk是期望值；然后选择滑模控制律zk，使得估计的状态xk在滑模面上保持稳定，zk如式10所示：zk＝-Kksgnσk10其中，Kk是控制增益矩阵，sgn·是符号函数；最后设计滑模滤波器，将滑模控制律应用于状态更新，如式11所示： 滑模滤波算法通过优化相机的位姿和IMU的状态来处理快速大曲率运动中的动态变化，恢复局部曲线运动；S34.根据优化后的相机位姿，生成新的地图点；S4.进行闭环检测与校正；S5.生成全局轨迹与地图。

如需购买、转让、实施、许可或投资类似专利技术，可联系本专利的申请人或专利权人河海大学，其通讯地址为：210024 江苏省南京市鼓楼区西康路1号；或者联系龙图腾网官方客服，联系龙图腾网可拨打电话0551-65771310或微信搜索“龙图腾网”。