本发明专利技术涉及导航定位技术领域，特别涉及一种基于位姿图更新的导航定位方法、装置及介质。通过传感器获取当前环境数据，以及获取当前位姿图；在历史关键帧数据中选取若干历史环境数据构建候选集；当前环境数据与候选集中的若干历史环境数据进行ICP匹配；根据匹配结果计算并动态更新历史关键帧的状态值；根据历史关键帧的状态值删除若干历史关键帧，和/或增加新关键帧。本发明专利技术能够避免目标在环境中不断移动时关键帧数量过多导致的计算复杂度和内存占用率问题，同时保持了定位系统的准确性和可靠性，实现长期鲁棒定位功能。实现长期鲁棒定位功能。实现长期鲁棒定位功能。
【技术实现步骤摘要】
一种基于位姿图更新的导航定位方法、装置及介质
[0001]本专利技术涉及导航定位
，特别涉及一种基于位姿图更新的导航定位方法、装置及介质。
技术介绍
[0002]近年来，国内外学术界和产业界对长期动态鲁棒定位的研究给与了广泛关注。在动态场景中，长期动态鲁棒定位是一个具有广泛商业应用前景的方案。该方案通常基于位姿图优化来实现长期鲁棒定位功能，通过对关键帧位姿和激光数据进行图优化，确保构建的地图与原始地图保持一致，从而实现在原始地图上的长期定位。
[0003]然而，这种方案存在一些问题，随着机器人在环境中移动，关键帧数量会不断增加，导致算法的计算复杂度和内存占用率严重耗尽，由于复杂的优化问题和大量的约束条件，算法的收敛速度较慢，无法满足实时应用的要求。
技术实现思路
[0004]本专利技术公开了一种基于位姿图更新的导航定位方法、装置及介质，它在可长期鲁棒性定位的基础上，可合理的删除无用关键帧，能够避免计算复杂度和内存占用率问题，提高算法的效率和实际工程可用性。
[0005]为达到上述目的，一方面，提供一种基于位姿图更新的导航定位方法，具体方法如下：
[0006]通过传感器获取当前环境数据，以及获取当前位姿图；
[0007]在历史关键帧数据中选取若干历史环境数据构建候选集；
[0008]当前环境数据与候选集中的若干历史环境数据进行ICP匹配；
[0009]根据匹配结果计算并动态更新历史关键帧的状态值；
[0010]根据历史关键帧的状态值删除若干历史关键帧，和/或增加新关键帧。
[0011]该实施例的优点在于，通过候选集筛选出了可删除关键帧的范围，通过状态值反应了历史关键帧中历史环境数据的匹配结果，合理的筛选出了具体可删除的关键帧，在删除和/或更新关键帧时，仅需根据状态值既可完成判断，在保证了有效的长期鲁棒性定位的基础上，显著减小了内存的占用率，提高了算法的效率。
[0012]所述传感器为激光雷达或摄像头或激光雷达与摄像头的组合。
[0013]进一步地，通过SLAM算法关联环境数据和位姿图，生成关键帧。
[0014]进一步地，构建候选集的具体方法如下：
[0015]在每个时间步长t，使用相对于当前机器人位姿的阈值距离ρ
t
＞0，并排除预设时间段添加的关键帧，选择一组闭环候选集关键帧C
t
；
[0016]使用占据栅格地图生成方法，根据选集关键帧C
t
中各关键帧对应的历史环境数据生成局部占据栅格地图局部占据栅格地图用于丢弃与当前周围局部环境拓扑结构不一致的历史环境数据，并生成最终用于进行与当前环境数据进行ICP匹配的候选集C
t
。
[0017]该实施例的优点在于，从系统存储数据中挑选出了可能需要删除的关键帧作为候选集，不需要处理所有存储的关键帧，减小了计算量；可根据实际应用场景，优化候选集的筛选条件，以实现历史关键帧的删除与更新操作。
[0018]进一步地，更新历史关键帧状态值的具体计算方法如下：
[0019]引入与当前关键帧位姿和当前环境数据相关的二进制时变随机变量它们编码在时刻t，判断前环境数据是否与历史环境数据Cur
t
匹配，定义当前环境数据为与历史环境地图环境一致时状态为A，与当前环境差异很大时状态为0；
[0020]定义二元随机变量编码ICP算法是否能够正确匹配计算出相对测量值
[0021]若和Cur
t
是不可匹配(
⊥
)；将的置信度定义为
[0022]若与历史环境地图Cur
t
有效匹配，即则通过在ICP算法最后一次迭代计算后对关联点之间的平均平方偏差ε进行评分来获得的置信度，其公式如下：
[0023][0024]其中是形状参数为θ的单调递减函数，使得φ(0)＝1。
[0025]该实施例的优点在于，引入了变量判断是否可匹配，自定义了置信度算法判断匹配是否可靠。特别是置信度的构建，需要考虑环境数据发生变化的情况，既有匹配的变化且变化的范围不至于超过一定的范围，不能仅通过相似度来判断匹配是否可信。
[0026]进一步地，删除若干历史关键帧，和/或增加新关键帧具体方法如下：
[0027]基于当前环境数据状态值，若当前环境数据与历史环境数据Cur
t
匹配无效，即即则不能直接观察到变量应用贝叶斯规则，可以得到以下过滤方程：
[0028][0029]式中的和是对由假设的二进制值的求和，设而X，Y∈(A，O)，假设关键帧状态变为O后不能返回A，即则们可以将上述公式用概率表示为如下的非齐次递归更新：
[0030][0031]其中，其中，其中，匹配置信度的演化仅取决于关键帧过时的先验概率和给定关键帧现状的扫描匹配失败概率的比值
[0032]当前关键帧状态值只能降级为状态O，当其置信度降至设定阈值以下时，即当前关键帧的状态值小于设定阈值N
loc
，则将其状态标记为O，删除位姿图中标记为O状态的关键帧；
[0033]基于当前环境数据状态值，若当前环境数据与待定位目标的历史环境地图Cur
t
匹配有效；如果局部环境没有明显变化，则不更新该区域历史关键帧；如果局部环境发生明显变化，可以使当前环境数据增加为新的关键帧并添加到位姿图中。
[0034]该实施例的优点在于，首先判断是否可匹配，在可匹配时再判断是否可信，在可信时再判断环境差异，仅挑选出可匹配、匹配结果可信、匹配后发现信息有变化的历史关键帧数据作为删除或更新的对象，从逻辑上排除了大量误操作的可能性，提高了操作的精准度。
[0035]进一步地，删除若干历史关键帧，和/或增加新关键帧后，对位姿图进行优化，具体方法如下：
[0036]在历史关键帧数据中选取一组闭环候选关键帧C
t
，其中t＝1，2，...，n，误差函数为E(C1，C2，...，C
n
)，表示关键帧之间的约束误差；
[0037]优化目标函数为最小化误差函数E，可以表示为：
[0038]J(C1，C2，...，C
n
)＝ΣE(C1，C2，...，C
n
)2[0039]迭代优化：
[0040]计算误差函数E关于关键帧C
t
的梯度计算Hessian矩阵计算Hessian矩阵计算步长更新当前关键帧C＝C+ΔC。
[0041]为达到上述目的，另一方面，提供一种基于位姿图更新的导航定位装置，包括：信息获取模块、候选集筛选模块、ICP匹配模块、状态值计算模块和关键帧变更模块；
[0042]所述信息获取模块，获取当前环境数据，以及获取当前位姿图；
[0043]所述候选集筛选模块，在历史关键帧数据中选取若干历史环境数据构建候选集；
[0044]所述ICP匹配模块，当前环境数据与候选集中的若干历史环本文档来自技高网...
【技术保护点】
【技术特征摘要】
1.一种基于位姿图更新的导航定位方法，其特征在于，具体方法如下：通过传感器获取当前环境数据，以及获取当前位姿图；在历史关键帧数据中选取若干历史环境数据构建候选集；当前环境数据与候选集中的若干历史环境数据进行ICP匹配；根据匹配结果计算并动态更新历史关键帧的状态值；根据历史关键帧的状态值删除若干历史关键帧，和/或增加新关键帧。2.如权利要求1所述的基于位姿图更新的导航定位方法，其特征在于，所述传感器为激光雷达或摄像头或激光雷达与摄像头的组合。3.如权利要求2所述的基于位姿图更新的导航定位方法，其特征在于，通过SLAM算法关联环境数据和位姿图，生成关键帧。4.如权利要求1所述的基于位姿图更新的导航定位方法，其特征在于，构建候选集的具体方法如下：在每个时间步长t，使用相对于当前机器人位姿的阈值距离ρ
t
>0，并排除预设时间段添加的关键帧，选择一组闭环候选集关键帧C
t
；使用占据栅格地图生成方法，根据选集关键帧C
t
中各关键帧对应的历史环境数据生成局部占据栅格地图局部占据栅格地图用于丢弃与当前周围局部环境拓扑结构不一致的历史环境数据，并生成最终用于进行与当前环境数据进行ICP匹配的候选集C
t
。5.如权利要求1所述的基于位姿图更新的导航定位方法，其特征在于，更新历史关键帧状态值的具体计算方法如下：引入与当前关键帧位姿和当前环境数据相关的二进制时变随机变量它们编码在时刻t，判断前环境数据是否与历史环境数据Cur
t
匹配，定义当前环境数据为与历史环境地图环境一致时状态为A，与当前环境差异很大时状态为O；定义二元随机变量编码ICP算法是否能够正确匹配计算出相对测量值若和Cur
t
是不可匹配(
⊥
)；将的置信度定义为若与历史环境地图Cur
t
有效匹配，即则通过在ICP算法最后一次迭代计算后对关联点之间的平均平方偏差ε进行评分来获得的置信度，其公式如下：其中φ:是形状参数为θ的单调递减函数，使得φ(0)＝1。6.如权利要求5所述的基于位姿图更新的导航定位方法，其特征在于，删除若干历史关键帧，和/或增加新关键帧具体方法如下：基于当前环境数据状态值，若当前环境数据与历史环境数据Cur
t
匹配无效，即匹配无效，...
【专利技术属性】
技术研发人员：李润童，蔡亚，陈宇，
申请(专利权)人：合肥中科深谷科技发展有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。