Far planner之 障碍物的图搜索

        我们知道，最短路径搜索算法比如A*、Djstra等经常是用在grid地图，也就是栅格地图，当我们把路径搜索算法运用到由node组成的障碍物graph的时候，有很多问题是要进行思考的，当我在完成这个障碍物graph的最短路径搜索的时候，我也想明白了一些far planner里的东西，所以干脆就来说一说好了。

        

        

        如上图所示，这个是一个由Node10出发，到达Node452的一个路径规划。我们知道，对于Far planner而言，它的障碍物contour_graph并不是完全沿着实际障碍物的，而是会有一点膨胀区域，所以用它生成的contour_graph来导航是完全没有问题的（之后我会在改进一下way_point，让它更合理化，而不是直接用障碍物的node）。

        算法主体如下：

        1、给定起点start坐标（x1,y1）和终点坐标（x2,y2)

        2、在contour_graph上找到离起点坐标最近的contour_node，并当作near_start，同理终点。

        3、把起点坐标当作cur_node 以半径R范围进行搜索，生成一个有效的node_list

        4、在这些有效的node_list里进行next_node的启发式搜索

        5、当终点出现在node_list时，由终点开始返回它的父节点，得到path

        是不是听起来还蛮简单的，这里主要涉及到几个问题：

        1：如何判断它在R范围内的node是有效的

        2：如何进行搜索，它的启发值要怎么设计

         对于起点node10而言，以半径R进行搜索，就会得到类似于下面的示意图：

        其中我们可以看到，有效的node只有[9,11,12,21,17]，而无效的node有[16,695]，这些无效的node怎么来的呢，比如node16，它和node10的连线会被[9,695]连成的障碍物墙壁打断，如果放任不管，那就会穿墙。

         再比如node695，它虽然不会穿墙，但是它本身就是往障碍物里钻的，我们也不能选它

         有了以上两个结论，我们就要来设计去排除掉我们的干扰node，在far_planner里也有一样的设计。对于第一个问题而言，far_planner里有一个do_intersect函数来判断交叉，我们也效仿它写了一个增强的。它的主要用途就是判断从cur移动到next的这个线段<cur,next>是否和别的障碍物组成的线段<A,B>相交。        

         第二个问题，涉及到角度问题，far_planner给的是一个固定的角度，让不同的顶点类型有不同的阈值，我这里改了一下，用角平分线来做。

        解决第二个问题需要用到它的surf_dirs 和 free_direct_type 也就是顶点的方向向量和它的顶点几何类型（比如凹、凸、PILLAR、UNKnown）。对于node10而言，它是一个convex_Node 也就是凸点，我们用它的方向向量来做角平分线。

        

         这样我们就会得到Surf与Topo的夹角α，对于凸点，任意给to_node，只要它与Topo_dir的夹角大于α，即说明它是有效的（就不会往内部走了）ps.感谢俺的女朋友，让俺豁然开朗

 

         换算为cos（因为cos在（-180，0）单调，在（0，180）也单调，就是我的cos(β)要小于cos(α)，这也是far_planner在处理graph上的方法，对于纯graph而言，更多情况下的判断是更贴近于数学几何的。

         这里结束之后，就是主程序对路径的查找了，用的是基于a*进行魔改一下的路径搜索：

         写这个是用来打标签的，因为我可以random一个起始点和终点，让它给出路径，这个路径就是我的标签tag ~ 还没有完全写完 random的部分需要判断random_point在不在contour障碍物内部，所以我还得继续完善，但是孩子想去玩了。。。先鸽了算了~