（Visual Navigation）路径规划算法（一）Dijkstra算法

• 上文总结了基于深度相机的见图过程
  （Visual Navigation）深度图像转点云
  （Visual Navigation）点云转地图信息
• 得到了机器人能够理解的地图信息 那下一步就是导航了 下面来介绍一下路径规划的算法

轨迹规划

Dijkstra算法

综述

具体实现过程

一

• 寻找一条v1-v6的最短路径
• 其中open list为未收录节点 closed list为已收录节点

二

• 此时可以到达的节点为④（代价为1）和②（代价为2）故此时到达节点④的距离小一点 故将④节点放入closed list
  
  三
• 更新此时可到达的节点（③②⑥⑦）并更新节点到①节点的距离总和
• 以⑦为例（⑦是④的可到达节点 故代价为①-④-⑤的代价为 1+4=5）其他节点如是

四

• 接着继续收录代价最小的节点并更新距离
  
  
  五
• 直到收录到目标节点⑥

• 至此算法循环结束
• 但此时只算出了最短路径 最短路径为①-④-⑦-⑥怎么都得到的呢

伪代码

机器人如何记录最短路径

• 节点第一次出现时 会记录使自己出现的父节点
• 当节点距离更新时 该节点也会记录使自己距离变化的当前被收录节点并更改父节点为此 eg：使⑥节点出现的节点为④节点 故前面⑥的父节点为④ 当收录到节点③时 ⑥节点的距离变更为8 故此时⑥的父节点更换为③节点 最后⑥节点的距离变更为6 则⑥的父节点变更为⑦节点
• 故可以找出最终路径为①-④-⑦-⑥
  

下一篇介绍Dijkstra改进 A*算法