一种面向三维地图的机器人动态轨迹快速推演方法

本发明公开了一种面向三维地图的机器人动态轨迹快速推演方法，包括以下步骤：使用多张二维地图拼合成为三维地图；在轨迹推演过程中，对地图进行遍历，从而寻找中途点，确定某一点是否为轨迹推演的中途点；在轨迹推演完成后，使用三角边优化法对推演轨迹进行距离优化；通过角度可导化，将带有线段的角度以寻找切点后绘制圆弧的方法进行优化。本申请可以增加机器人在三维空间位置迭代过程中的鲁棒性，安全性，高效性和普适性。相比与传统轨迹推演的效果，增加了更对约束条件，并增加了地距因子，这可以让不同机器人在运行过程中针对不同的地图状况做出合理的轨迹推演效果。