一个不成熟的路径规划

基于带权重的无向图，搜索从初始点到目标点的路径，经过修改，成了一个四不像算法。

下表是节点位置信息

ID	x	y	heuristic-cost-to-go
1	-0.5	-0.5	1.4142
2	-0.09	-0.4	1.0762
3	-0.285	-0.305	1.1244
4	0.0575	-0.225	0.8494
5	-0.0525	-0.0175	0.7604
6	-0.37	0.3	0.8927
7	0.3525	-0.0525	0.5719
8	0.0625	0.255	0.5014
9	-0.1	0.3725	0.6134
10	0.4275	0.195	0.3135
11	0.345	0.3525	0.214
12	0.5	0.5	0

下表是边的信息

ID1	ID2	cost
12	11	0.214
12	10	0.3135
12	8	0.5014
11	10	0.1778
11	9	0.4454
10	7	0.2586
9	8	0.2005
9	6	0.2796
9	5	0.5994
8	4	0.48
7	4	0.3417
5	2	0.179
4	3	0.3517
4	2	0.2289
3	2	0.2169
3	1	0.2903
2	1	0.422
7	5	0.4402
5	4	0.11

在地图上展开：

编写程序

# -*- coding: utf-8 -*-
"""
Created on Tue Jul 17 14:24:41 2018

@author: Administrator
"""

import numpy as np
import pandas as pd
edges=pd.read_csv('edges.csv',header=None,sep=',',comment='#')
nodes=pd.read_csv('nodes.csv',header=None,sep=',',comment='#') 
obstacles=pd.read_csv('obstacles.csv',header=None,sep=',',comment='#')
edges=np.array(edges)
nodes=np.array(nodes)
obstacles=np.array(obstacles)
opend=[1]
closed=[]
#numbers of nodes
n=nodes.shape[0]
#transfer cost to 2D array
cost=np.zeros((n+1,n+1))-1
#numbers of edges
m=edges.shape[0]
for i in range(m):cost[int(edges[i,0]),int(edges[i,1])]=edges[i,2]
for i in range(m):cost[int(edges[i,1]),int(edges[i,0])]=edges[i,2]
#save parent nodes
parent=[]
#destination node number 12th
dest=12
current=opend.pop()
while True:
    closed.append(current)
    
    find=np.where( cost[current] > -1)[0]
    f={}
    opend=[]
    for i in find:
        if (i not in opend) and (i not in closed):
            opend.append(i)
            h=nodes[i-1,3]
            f[(current,i)]=cost[current,i]+h
    print('opend: ',opend)
    
    parent.append(current) 
    for i in f:
        if f[i]==min(f.values()):
            current=i[1]
    if str(dest) in str(f.keys()):
        parent.append(dest)
        break
    print(current,f)
    
    if (dest in closed) or (not opend):
        break
#for i in range(len(parent)):past_cost.append(nodes[i,3])
#for i in range(len(parent-1)):past_cost.append()
#exam whether to remove inter node
removed=[]
for i in range(len(parent)-2):
    if cost[parent[i],parent[i+2]]>-1:
        if cost[parent[i],parent[i+1]]+cost[parent[i+1],parent[i+2]]+nodes[i][3]>cost[parent[i],parent[i+2]]:
            removed.append(parent[i+1])
for i in removed:parent.remove(i)
print(parent)
#save parent nodes to csv
dataframe = pd.DataFrame(np.array(parent).reshape(1,-1))
dataframe.to_csv("path.csv",index=False,sep=',',header=None)

 在vrep中打开；当目标点为12时

目标点为11：

目标点为9：

目标点为7：

目标点为6：

目标点为5

到后面已经没法看了。

后期学到再更新。。。