（本来想做个标题党的，可怕被喷哈哈哈）【对了，这个程序是基于python写的，但思路想法并不局限于python】
这是我们大创的一个课题（机器人的路径规划问题），写这篇博客呢，主要是想总结一下自己写代码的过程，也给一些有需要的人提供一些借鉴。（前期可能不会附上代码，等我大创项目结题了有需要就附上哈哈，不过可能比较久，因为是国家级要两年时间结题呢）
首先我们来谈谈什么是路径规划，顾名思义（好吧，是根据常识理解），和导航的功能差不多，给定你的出发地和目的地，我能给你展示一条最好的路径（路程最短 or 时间最短）避开障碍物从出发地到达目的地。（是不是很简单？是的，但可拓展的东西太多了）
这是目前做“好”的程序，黑色矩形是障碍物，蓝色线段的两端是我们给程序输入的起始点和终止点，程序反馈给我们最优路径是蓝色的线段。
好哒，那我们开始着手做这个项目，把这个项目分成几个步骤吧，首先要有地图，然后地图上有障碍物，程序要能识别这个地图，程序含有路径规划算法，在我们给程序输入两个点之后程序能返回给我们一个路径，并把路径在地图上画出来。其实真正的路径规划问题只有路径规划算法所涉及到的那一点东西：得到两个点输出路径。但是一个对一个项目而言，就需要程序前面的地图绘制地图识别以及程序后面路径绘制，这样看起来才更像是一个完整的项目。
于是我把程序分成了三个部分，第一个部分是绘制地图/绘制路径的程序，第二个部分是地图数据处理的程序，第三个部分是路径规划算法的程序，最后我们用一个main程序讲这三个部分调用，这样就可以分步来完成了，并且哪一个部分出了问题我们直接修改相应的程序就好了，并且可以单独对某个部分进行测试以检查程序。
第一部分的画图程序，我选择了不用花太多时间学习的turtle库来完成（其实主要是当初只知道turtle库，而且turtle库里的乌龟运动更像是机器人行走，但用它来画图还是有点不大合适，但它可以模仿机器人运动过程就很perfect！）；
可以看到画出的障碍物很贴合实际（哈哈哈哈哈至少不都是矩形），但因为是模型，也考虑到后面的算法实现难易程度，我在后面的障碍物设置中都将障碍物设置成了矩形。这里的障碍物是我用二维坐标在turtle库内置的坐标系下绘制的。
第三个部分路径规划算法（先讲第三个部分，因为第二个部分是针对算法类型做的预数据处理，让地图信息转换成算法可以分析的数据），路径规划算法我采用了最简单的递归寻址，是基于狄克斯特拉算法的一种变异算法吧（其实这个算法源于我在学习小甲鱼的python课程时记下的（推广一波小甲鱼，可惜没有广告费！），后来发现和狄克斯特拉算法很像，下面附上我参考的原始代码：）
def searchPath(graph, start, end):
results = []
__generatePath(graph, [start], end, results)
results.sort(key=lambda x:len(x))
return results
def __generatePath(graph, path, end, results):
current = path[-1]
if current == end:
results.append(path)
else:
for n in graph[current]:
if n not in path:
__generatePath(graph, path+[n], end, results)
def showPath(results):
print('The path from ', results[0][0], 'to' , results[0][-1], 'is :')
for path in results:
print(path)
if __name__=='__main__':
graph = {'a':['b','c','d'],
'b':['e'],
'c':['d','f'],
'd':['b','e','g'],
'e':['d'],
'f':['d','g'],
'g':['e']}
r1 = searchPath(graph, 'a', 'g')
showPath(r1)可以看到，这里的graph是预先设定好的一个“点可以到达的所有点”的字典，而程序所要做的就是递归遍历这个字典，从start遍历到end指导所有的点都遍历过为止，然后对路径进行排序，就能得到最短路径了。与狄克斯特拉不同的是，这里没有赋予权重，每个节点到下一个节点的成本是一样的，最优路径就成了长度最短（所经历的点的数量最少）的路径了。而我们实际需要的是一个像狄克斯特拉一样节点间计算权重的算法，这个权重就是距离的大小，所以要对这个算法进行修改才能使用。
下面就来说说怎么修改（这里是一个重点噢），首先我们的graph肯定不可能是像 a,b,c,d...这个样子的，因为这并不是地图信息的有效表示方法，那什么是地图信息有效的表示方法呢，我猜是坐标吧！因为后面要计算距离，这时中学学的计算二维坐标系下两点的距离公式√((x-x)+(y-y))就派上用场了，那感情好，就用坐标(x,y)来代替a,b,c..了，整个字典就是这个样子了：
graph = {
(80, 380): ((0, 380), (80, 270), (180, 200), (270, 200), (180, -70), (100, -100), (100, -300)),
(80, 270): ((0, 270), (80, 380), (180, 200), (270, 200), (180, -70), (100, -100), (-100, -100), (-350, -100), (100, -300), (-100, -350)),
(0, 270): ((80, 270), (0, 380), (80, 380), (180, 200), (270, 200), (-100, -100), (100, -100), (180, -70), (-350, -100), (100, -300), (-100, -350)),
(0, 380): ((80, 380), (0, 270), (-100, -100), (-350, -100), (-100, -350)),
(270, 200): ((180, 200), (80, 270), (80, 380), (270, -70), (0, 270)),
(270, -70): ((200, -100), (180, -70), (100, -100), (200, -300), (270, 200), (-100, -100), (80, 270), (-350, -100)),
(180, -70): ((200, -100), (100, -100), (270, -70), (180, 200), (-100, -100), (270, 200), (80, 270), (0, 270), (80, 380), (-350, -100)),
(180, 200): ((270, 200), (80, 270), (0, 270), (80, 380), (180, -70), (100, -100), (-100, -100), (-350, -100), (-100, -350)),
}我们得到了可用的graph之后，再对算法稍加改动，让程序计算出每条路径的路程，并根据路程长短排序，就可以了。
import math
def __searchPath(graph, start, end):
results = []
__generatePath(graph, [start,0], end, results)
results.sort(key=lambda x:x[-1])
return results
def __generatePath(graph, path, end, results):
current = path[-2]
if current == end:
results.append(path)
else:
for n in graph[current]:
if n not in path:
distance = path[-1] + math.sqrt((n[0]-path[-2][0])**2 + (n[1]-path[-2][1])**2) #增量计算不同路径的距离
__generatePath(graph, path[:-1]+[n,distance], end, results)
def showPath(results):
print('The path from ', results[0][0], 'to' , results[0][-1], 'is :')
for path in results:
print(path)
graph = {.......}测试结果如下（我的graph和你们的会一样）：
看到这里，我觉得有必要很明显的说明一下我的路径规划思路了：我们把障碍物的顶点作为狄克斯特拉算法中的节点，这些节点都是机器人可以到达的地方，不用于A*算法将整个地图划分成小格，每个没有障碍物的小格都是机器人可以到达的地方，这样就可以把上面这个算法用上了，是不是很巧妙呢，嗯我也这么觉得！那大家应该理解为什么先讲第三部分的程序了吧，对第二部分的程序功能也应该有模糊的想法了叭（emmm应该有的吧、只要多思考）。
第二部分的程序要做的就是给路径规划算法提供graph，这个graph是从地图信息中获取并处理得到的。如果说路径规划算法是我改写别人的算法，是一个很简单的开发过程的话，那么这个部分，你必须要认真仔细定睛的看看了，不然你对我的评价就只能是盗码贼，山寨了！我可不想你这么认为我！因为第二部分的程序，完全是自己原创，从0到1的质变，花了我好几个马原课的时间呢！和好几次约未来女朋友自习的时间呢！（当时的未来咦嘻嘻嘻嘻嘻~突然变态、）所以，【第二部分是重头戏！！！！！】
言归正传，第二部分的内容对于当时的我来说，实现难度还是蛮大的叭，程序要解决的是给它一张地图信息（点集），它能反馈给我们各个点所能到达的所有点（其实就是graph），好像也不难？啊？什么？不难？那你一定是大佬了！！反正俺是整了挺久的，因为障碍物是真的是障碍物，给定一个障碍物，在地图上任选一个观测点（因为能看到的点一定也是可以直线到达的点）最多只能看到障碍物的三个顶点（假定障碍物是矩形），光是这一点就很难实现呢，更何况一张地图上有多个障碍物，还要考虑前一个障碍物遮挡后一个障碍物（存在全遮挡和半遮挡问题），结合下面一个图理解一下吧
大家可以明显的看到，我采用了可视的方法来等效替代可直线到达这个较为抽象的概念（好吧，并没有那么明显哈哈），这是因为我们人要走到某个障碍物的顶点时，首先要能看到这个顶点，然后目不转睛的盯着这个顶点，直线走一定的距离就到了，第二部分程序也正是模仿了人的这一行为，能看到的点就是可直线到达的点，比起收集可直线到达的点这么抽象的东西，收集可以看到的点就显得容易多了，话虽这么说，可，路漫漫其修远兮……设计一个算法能兼顾所有的情况就成了大难题了。
深夜我辗转反侧，日思夜想，白发丛生，终于想到了一个比较可行的解决方法，一言难尽，请用图说话！
（啊，作图累死个人，就凭这一点，你必须给我点个赞，且不说你看懂没看懂！当然还是希望你能看懂，不然没得了意思）
这个方法的巧妙之处在于，抽象了一个障碍物角度区间，角度区间内的每一个角度都对应一个距离长度，对于任何一个障碍物顶点在已知障碍物角度区间上，该顶点和观测点的连线构成一个角度，只要该角度所对应的距离长度大于障碍物顶点距观测点的距离，那么障碍物顶点就是可视的，反之是不可视的，即便在有很多障碍物的情况下，我们只要得到了障碍物角度区间，就可以对每一个障碍物顶点进行判断其是否可视，将可视的点保存下来，不可视的点丢弃，就能生成一个graph用于第三步的程序运行。（但在获取所有障碍物角度区间和判断障碍物顶点是否可视的过程中，我们应当考虑把障碍物当成一个整体（类似于面向对象中类一样的东西），并且在判断的过程中优先和距离较近的障碍物角度区间进行比较，否则（如果和最远的障碍物角度区间进行比较）那么除了该障碍物上的不可视点以外，所有的其他障碍物上的所有点都可视，因为所有点距离观测点的距离都小于角度区间下指定角度所对应的距离）
三个部分都写完之后，再写一个main函数，在第一部分程序绘制好地图之后将障碍物的坐标信息传递给第二部分的数据处理程序，该程序对坐标信息处理传递给下一个程序graph，第三部分程序根据整个graph运行算法，返回main函数最优路径（应当是一系列路径的点集），main函数将最优路径的点集传给绘图程序（第一部分），绘图程序绘制路径，就OK了！
其实，认真思考的朋友们一定发现了一个问题，那就是，第一部分绘图程序是怎样绘制地图的呢？其实目前情况下，这个地图构建过程还是通过我手工输入的障碍物的点的坐标来完成的，程序还不能识别给定地图图片，自动生成坐标这个样子，目前我也在寻找其他的方法（之前有想过用pillow分析地图图像灰度，来判断道路这个样子，估计有点麻烦吧，现在把目光转向了osm文件），期待我的更新吧、（据悉可能要很久，因为要打游戏还要陪npy，哈哈哈哈哈我太水了！）（回头补上一张牛逼一点（障碍物多一点的）的成品图，第一张图有点太简单了，怪不好意思的。）