一种基于邻域节点代价预估的路径规划方法

本发明公开了一种基于邻域节点代价预估的路径规划方法，综合考虑较小定位精度与较短路径长度，以惯性传感器为核心传感器，通过因子图融合算法估计载体当前位置并计算载体当前定位误差。针对载体的导航定位精度问题，传统方法基本只考虑在避障中的最短路径，本发明将障碍物（路标）信息与惯性传感器数据进行融合，估计载体在已规划路径中的位置，并根据已规划路径，计算得到载体在已规划路径中的位置均方根误差，从而构建位置均方根误差代价，将其与路径长度共同作为代价函数对路径进行规划。由于其利用了障碍物与载体之间的相对信息以及误差模型的自适应调整能力，该方法更加贴合实际应用场景，适合在无人驾驶、机器人导航、载体定位等领域中推广使用。