VGRAPH路径规划（Lozano-Perez and Wesley, 1979）

本文中的方法来自文章：

• Lozano-Pérez T, Wesley M A. An algorithm for planning collision-free paths among polyhedral obstacles[J]. Communications of the ACM, 1979, 22(10): 560-570.

本文参考了以下项目代码（特别是地图数据、增长障碍物部分代码、线段是否相交检查部分代码），特表示感谢：

• https://github.com/jingxixu/vgraph

点击原文链接获取本文代码下载链接。

算法的主要思想是，将运动体看作一个点，通过将障碍物“增长”适当的程度，以满足避碰需求。在图中搜寻一条从起点到目标点的路径即可。

该路径的重要特性是它由通过障碍物顶点序列将原点连接到目的地的直线组成。在具有任意多边形运动体的平面中运动的情况下，连接任何两个可访问点的最短无碰撞路径始终具有此属性。

如下图所示，正方形运动体（绿色框）要从当前位置（起点）移动到终点（红色*），不考虑运动体的旋转，以运动体的中心为参考点，为该参考点确定一条路径。

由于运动体被看作一个点，因此需要对障碍物进行增长，以满足避碰需要：

从上图可见，即便运动体的参考点（正方形中心）在增长后障碍物的边缘，运动体与障碍物之间正好不会发生碰撞。

之后，寻找可直接相连的路径：

最后，搜索地图以得到最短通行路径：