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

申请/专利权人：广东工业大学

摘要：本发明涉及水域定位技术领域，尤其涉及一种近岸复杂水域的强一致性里程计及高精度建图方法和系统。本发明采用自适应可观测性分析算法，保障了里程计输出的一致性和相对精度；其次，基于滑窗优化器综合里程计约束、预积分约束、全球卫星导航约束和先验地图约束，确保水面移动载体在复杂近岸水域仍能实现厘米级的高精度全局定位；利用两步两次全局位姿图优化的手段，剔除全球卫星导航约束和闭环约束中的离群值，有效提升了三维点云地图的全局一致性和精度，相对于现有技术，能够提升水面移动载体在复杂近岸水域定位效率和一致性，以及建图的精度和平滑度。

主权项：1.一种近岸复杂水域的强一致性里程计及高精度建图的方法，其特征在于，所述方法包括以下步骤：S1、基于迭代误差卡尔曼滤波器构建激光-惯性里程计，并通过所述激光-惯性里程计采集近岸复杂水域的局部里程计数据，同时，获取近岸复杂水域的惯性测量单元数据、全球卫星导航观测数据以及先验地图数据；S2、通过对预设时间段内的所述局部里程计数据、所述惯性测量单元数据、所述全球卫星导航观测数据以及所述先验地图数据进行综合处理，构建批量非线性最小二乘问题，并根据所述非线性最小二乘问题计算输出得到全局定位信息；S3、通过对所有所述局部里程计数据、所述惯性测量单元数据、所述全球卫星导航观测数据进行综合处理，构建关于所述全局定位信息的全局位姿图优化问题，并根据所述全局位姿图优化问题计算输出得到三维点云地图，并进一步生成概率体素地图，之后，融合所述全局定位信息在所述概率体素地图实现近岸复杂水域的定位；其中，步骤S1还包括：对所述激光-惯性里程计进行可观测度分析，对于所述迭代误差卡尔曼滤波器中每一个状态向量的分量在0时刻协方差与k时刻协方差的比值： ；其中，表示为0时刻协方差矩阵中第j行第j列的元素；表示为k时刻协方差矩阵中第j行第j列的元素；当小于预设阈值时，将所述激光-惯性里程计的输出变换为： ；其中，为ENU系相对于LIO系的外参变换矩阵，为所述先验地图数据；步骤S2中，对于所述批量非线性最小二乘问题，若小于所述预设阈值时，构建里程计约束残差，并表示为： ；其中，为李群到李代数的对数映射运算符；分别为第i时刻，第j时刻机体系相对于LIO系的变换矩阵；分别为第i时刻，第j时刻机体系相对于ENU系的变换矩阵；若当前位姿在概率体素地图的范围内，构建点到概率分布约束，并表示为： ；其中，为激光点，为所述激光点的区块索引，分别为原始点云地图x，y轴方向的最小值；为向下取整函数；对应的，根据体素索引值查询指定体素中的均值和协方差，构建点到概率分布残差，并表示为： ；其中，为ENU系相对于MAP系的变换矩阵；为第i帧相对于ENU系的变换矩阵；为某个激光点；为第j个体素中的均值；为第j个体素中的协方差；构建预积分约束残差，并表示为： ；其中，分别为第i时刻，第j时刻机体系相对于ENU系的旋转矩阵，是待优化值；分别为第i时刻，第j时刻机体系相对于ENU系的平移矩阵，是待优化值；分别为第i时刻，第j时刻机体系相对于ENU系的速度，是待优化值；分别为第i时刻，第j时刻陀螺仪的零偏，是待优化值；分别为第i时刻，第j时刻加速度计的零偏，是待优化值；为加速度t时刻的测量值；为陀螺仪t时刻的测量值；若当前帧有全球卫星导航观测，构建全球卫星导航观测对当前帧的全球卫星导航约束，并表示为： ；其中，，为全球卫星导航观测的时间戳，为全球卫星导航与惯性测量单元之间的时间不同步误差；为全球卫星导航观测在ENU系下表示；为惯性测量单元与全球卫星导航之间的外参在ENU系下表示；分别为k时刻，l时刻机体系相对于ENU系的平移。

全文数据：

权利要求：

百度查询： 广东工业大学 近岸复杂水域的强一致性里程计及高精度建图方法和系统