基于PS的路径规划算法 之 MATLAB仿真程序修改

在原程序的基础之上，做3方面修改：

1. 修改原来的节点选择方式：原来是PSet_AFA_to_AAF；现在改成与专利相同的节点选择模式，即简化原来的程序。
2. 增加一条节点选择限制：原来是 当前起点可以与任意一个邻近节点相连；而现在为了满足路径规划的要求，要选择与当前起点在同一条路径上的点。
3. 改变下一个点的选择范围：原来是 按照直角坐标系划分4个区域，从某一个区域当中选择下一个点；现在改成以起点和终点的连线为基准，向左向右分别45°（共90°）区域内寻找下一个点。

具体做法：
对于第一点：通过对节点集合矩阵求并集，来选择出满足要求的节点。
对于第二点：对于道路数据库中的每一个点，添加一个所在路段编号。选择节点时，增加一步：筛选与当前起点具有相同路段编号的点。（即具有相同路段编号，则说明这些点在同一条道路上）
对于第三点：搜索范围从之前的方形区域改为扇形区域；遍历每个点，保留距离在搜索范围内且夹角也在范围内的点。