AStar 路径规划之初级二

1、A* 中 g，h函数使用两点之间五次样条的弧长，并且根据需要进行微调。

2、五次多项式的末状态为（l,0,0），所以中间过程两点进行连接的时候是水平的，最好的结果还是应该获得各个轨迹点后，再对该轨迹点进行重新拟合。或者在查找的过程中就已经考虑到中间的状态不是水平的。

3、如果模型为低速模型，那这个轨迹应该是可以使用的。

# -*- coding: utf-8 -*-
"""
Created on Fri Dec 27 11:02:55 2019

@author: leizhen.liu
"""
from scipy.integrate import quad
import numpy as np
import matplotlib.pyplot as plt

class VehicleState:
    def __init__(self,s,l,vpl,apl):
        self.s = s
        self.l = l
        self.vpl = vpl
        self.apl = apl


class trajectoryCost:
    def __init__(self,startVehicleState,endVehicleState,totals,totall,detas,detal):
        self.startVehicleState = VehicleState(0,0,startVehicleState.vpl,startVehicleState.apl)
        self.endVehicleState = VehicleState((endVehicleState.s - startVehicleState.s)*detas,(endVehicleState.l - startVehicleState.l)*detal,endVehicleState.vpl,endVehicleState.apl)
        self.sorg = startVehicleState.s * detas
        self.lorg = startVehicleState.l * detal
        self.id = str(startVehicleState.s + startVehicleState.l * totals) + 'id'+ str(endVehicleState.s + endVehicleState.l * totals)
        self.matP = np.zeros((6,1)) 
        self.arcLength()


    def calQuintic(self):
        s = self.endVehicleState.s
        matS = np.mat([[1,0,0,0,0,0],
                         [0,1,0,0,0,0],
                         [0,0,2,0,0,0],
                         [1,s,s**2,s**3,s**4,s**5],
                         [0,1,2*s,3*s**2,4*s**3,5*s**4],
                         [0,0,2,6*s,12*s**2,20*s**3]])
    
        #ju zhen ni pan duan
        if np.linalg.det(matS)<0.001:
            self.matP = np.mat([0,0,0,0,0,0]).T
            return
    

        matL = np.mat([0,self.startVehicleState.vpl,self.startVehicleState.apl,
                         self.endVehicleState.l,self.endVehicleState.vpl,self.endVehicleState.apl])

        self.matP =  matS.I * matL.T 
                

        
    def f(self,s):
        return  np.sqrt(1+ (self.matP[1] +2*self.matP[2]*s+3*self.matP[3]*s**2+4*self.matP[4]*s**3+5*self.matP[5]*s**4)**2)
        
    def arcLength(self):
        self.calQuintic()
        self.cost,err = quad(self.f,self.startVehicleState.s, self.endVehicleState.s)
        
    def show(self):
        s = list()
        l = list()
        print('stop =',self.endVehicleState.s)
        for i in np.arange(0,self.endVehicleState.s,0.3):
            s.append(i + self.sorg)
            sp = np.mat([1,i,i**2,i**3,i**4,i**5])
            l0 = sp*self.matP

            l1 = np.asarray(l0)
            l.append(l1[0][0] + self.lorg)

        plt.plot(s,l,'-')    
            
        

# -*- coding: utf-8 -*-
"""
Spyder Editor

This is a temporary script file.
"""

import numpy as  np
import matplotlib.pyplot as plt
import math
import queue
import time
import copy
import huchang

MaxsSearch  = 5

detaWidth = 0.3
totalWidth = 6.0   #road width
detaLength = 3.0   
totalLength = 60.0 # road predict length
vehicleHarfWidth = 0.8

# s,l half width,half height /m
obstacle = np.array([[40,4,0.3,0.3],
                     [16,1.5,0.6,0.3],#[30,2.5,0.3,0.3]
                     ])

numberOfL = np.int(totalWidth /detaWidth)
numberOfS = np.int(totalLength /detaLength)

class StrPosition:
    def __init__(self,s,l,vpl =0.0,apl =0.0):
        self.s = s
        self.l = l
        self.vpl = vpl
        self.apl = apl


class StrReachablePosition:
    def __init__(self,parentNode,reachNode):
        self.parentNode = parentNode
        self.reachNode = reachNode
        self.f =999
        self.g =999
        self.h =999
        

def nodeCost(startPosition,endPosition,node2nodeCost):
    begin = startPosition.s + startPosition.l*numberOfS
    end = endPosition.s + endPosition.l*numberOfS
    if startPosition.s > endPosition.s:
        begin = copy.deepcopy(end)
        end = startPosition.s + startPosition.l*numberOfS
        Position =copy.deepcopy(startPosition)
        startPosition = copy.deepcopy(endPosition)
        endPosition = copy.deepcopy(Position)
        
    ikey = str(begin) +'id'+str(end)

    if startPosition.s == 2 and startPosition.l == 11:
        print('ikey',ikey)
        
    
    if node2nodeCost.get(ikey,0) == 0 :
        startState =  huchang.VehicleState(startPosition.s,startPosition.l,startPosition.vpl,startPosition.apl)
        endState = huchang.VehicleState(endPosition.s,endPosition.l,endPosition.vpl,endPosition.apl)
        traject = huchang.trajectoryCost(startState,endState,numberOfS,numberOfL,detaLength,detaWidth)
        node2nodeCost[ikey] = copy.deepcopy(traject)

        
    return node2nodeCost,node2nodeCost[ikey].cost
        
def findInQueue(closeMap,node):
     ikey = node.reachNode.l * numberOfS + node.reachNode.s
     if closeMap.get(ikey,0) == 0:
         return False
     return True


def refreshMap(node,openList):
      ikey = node.reachNode.l * numberOfS + node.reachNode.s
      openList[ikey] = node
      return openList
      
def updateNode(node,openList,closeMap):
    #print("updateNode-------------",node.reachNode.l,node.reachNode.s)
    ikey = node.reachNode.l * numberOfS + node.reachNode.s
    if openList.get(ikey,0) != 0:
        nodet = openList[ikey]
        if(nodet.g > node.g):
            openList[ikey] = node
    else:
         if findInQueue(closeMap,node) == False:
             openList[ikey] = node
    return copy.deepcopy(openList)

def delMap(node,openList):
    ikey = node.reachNode.l * numberOfS + node.reachNode.s
    if openList.get(ikey,0) == 0  or ikey == 0:
        return openList
    del openList[ikey] 
    return openList


def environmentShowGraph(environment,startIndex,endIndex):

    shape = environment.shape
    row = shape[0]
    col= shape[1]   
    os = list()
    ol = list()
    for rowi in range(startIndex,endIndex):
        for coli in range(col):
          x = np.array([0,col-1])
          y = np.array([rowi,rowi])
          x1 = np.array([coli,coli])
          y1 = np.array([0,(row -1)])
          plt.plot(x,y,'b-')
          plt.plot(x1,y1,'b-')
          if environment[rowi][coli] >0.1:
              os.append(coli)
              ol.append(rowi)
    plt.plot(os,ol,'o','r')


def environmentShow(environment,startIndex,endIndex):

    shape = environment.shape
    col= shape[1]   
    os = list()
    ol = list()
    for rowi in range(startIndex,endIndex):
        for coli in range(col):
          x = np.array([0,totalLength])
          y = np.array([rowi*detaWidth,rowi*detaWidth])
          x1 = np.array([coli*detaLength,coli*detaLength])
          y1 = np.array([0,totalWidth])
          plt.plot(x,y,'b-')
          plt.plot(x1,y1,'b-')
          if environment[rowi][coli] >0.1:
              os.append(coli*detaLength)
              ol.append(rowi*detaWidth)
    plt.plot(os,ol,'o','r')



def createObstacle(environment,obstacle,startIndex,endIndex):
    
    eshape = environment.shape
    erow = eshape[0]
    ecol= eshape[1]
    
    oshape = obstacle.shape
    for orowi in  range(oshape[0]):

        start_l = math.floor( (obstacle[orowi][1]-obstacle[orowi][2]) / detaWidth) - startIndexOfL
        end_l = math.ceil((obstacle[orowi][1]+obstacle[orowi][2]) / detaWidth) + startIndexOfL
        
        start_s = math.floor(( obstacle[orowi][0] -obstacle[orowi][3])/detaLength)
        end_s = math.ceil(( obstacle[orowi][0] + obstacle[orowi][3])/detaLength)
        #print("obstacle s range",start_s,end_s)
        for obsl_i in range(start_l,end_l+1):
            for obss_i in range(start_s,end_s+1):
                if obsl_i>=0 and obsl_i < erow and obss_i >= 0 and obss_i< ecol:
                    environment[obsl_i][obss_i] = 1.0
                    
            
def createStopPosition(x,startIndexaOfL,endIndexOfL):
    stopPosition = np.zeros((endIndexOfL - startIndexOfL,2))
    for index in range(endIndexOfL-startIndexaOfL):
        stopPosition[index][0] = x
        stopPosition[index][1] = index
    return stopPosition

def getHCost(startPosition,endPosition,node2nodeCost):
    
     node2nodeCost,cost = nodeCost(startPosition,endPosition,node2nodeCost)
     cost0 = (startPosition.l - endPosition.l)*detaWidth - (startPosition.s - endPosition.s)*detaLength *0.1
     return node2nodeCost,cost + cost0 *0.1

def getGCost(node,endPosition,node2nodeCost):
    node2nodeCost,cost = nodeCost(node.reachNode,endPosition,node2nodeCost)
    cost0 = (startPosition.l - endPosition.l)*detaWidth - (startPosition.s - endPosition.s)*detaLength *0.1
    return node2nodeCost, node.g + cost+0.01*cost0

def getFCost(node):
    return node.g+node.h

def checkcollision(environment,startPosition,endPosition):

    minl = min(startPosition.l,endPosition.l)
    maxl = max(startPosition.l,endPosition.l)
    mins = min(startPosition.s,endPosition.s)
    maxs = max(startPosition.s,endPosition.s)
    for s0 in range(mins,maxs+1):
        for l0 in range(minl,maxl+1):
            if environment[l0][s0] >0.1:
                return False
    return True
            


def reachPoints(openList,environment,lastNode,stopPosition,closeMap, startIndexOfL,endIndexOfL,node2nodeCost):
    print("reachPoints-------------")
    parentNode = lastNode.reachNode
    nexts = parentNode.s + 1
    reachNode = StrPosition(nexts,0)
    node = StrReachablePosition(parentNode,reachNode)
    
    shape = environment.shape
    node.parentNode = parentNode
    print("nexts,s of env",nexts , shape[1],numberOfS,numberOfL)
    maxsearchOfs = min(nexts+MaxsSearch,numberOfS)
    for s in range(nexts,maxsearchOfs):
        for l in range(startIndexOfL,endIndexOfL):
            reachNode.s = s
            reachNode.l =l
            if checkcollision(environment,parentNode,reachNode):
                node.reachNode = reachNode
                node2nodeCost, node.g = getGCost(lastNode,reachNode,node2nodeCost)
                node2nodeCost, node.h = getHCost(reachNode,stopPosition,node2nodeCost)
                node.f = getFCost(node)
                openList = updateNode(node,openList,closeMap)  
    return openList,node2nodeCost
    
def freshCloseList(node,openList,closeList,closeMap):
    print("freshCloseList-------------")
    ikey = node.reachNode.l * numberOfS + node.reachNode.s
    closeList.put(node)
    closeMap[ikey] =node.parentNode.l * numberOfS + node.parentNode.s
    return delMap(node,openList),closeList,closeMap    
     
 
def getMinFFromOpenList(openList):
    print("getMinFFromOpenList-------------")
    #find min node
    start = StrPosition(0,0)
    end = StrPosition(0,0)
    noden = StrReachablePosition(start,end)
    if  len(openList) == 0:
        return noden,False
    minf =999999 
    for nodet in openList.items():
        if minf >nodet[1].f:
            minf = nodet[1].f
            noden = nodet[1]
            
    print("min",minf,noden.reachNode.s,noden.reachNode.l)       
    return noden,True
 


def AStart(environment,startPosition,stopPosition,startIndexOfL,endIndexOfL):
    print("AStart-------------")
    node2NodeCost = dict()
    #put startnode to queue
    node = StrReachablePosition(startPosition,startPosition)
    node.g = 0
    node2NodeCost, node.h = getGCost(node,startPosition,node2NodeCost)
    node.f = getFCost(node)
    openList = dict()
    openList = refreshMap(node,openList)
    print("dict len",len(openList))
    print("reachPosition",node.reachNode.s,node.reachNode.l)
    time.sleep(1)
    closeList = queue.Queue()
    closeMap = dict()
   #search 
    while node.reachNode.s != stopPosition.s or  
        node.reachNode.l != stopPosition.l and len(openList)!=0:
        # update reachabe node 
        #print("node.reachNode.s,stopPosition.s,node.reachNode.l,stopPosition.l",node.reachNode.s,stopPosition.s,node.reachNode.l,stopPosition.l)
        openList,node2NodeCost = reachPoints(openList,environment,node,stopPosition,closeMap,startIndexOfL,endIndexOfL,node2NodeCost)

        ##-------------
        '''
        if node.reachNode.s ==  2 and node.reachNode.l == 7:
            for nodet  in openList.items():
                print("openlist",nodet[0],nodet[1].reachNode.s,nodet[1].reachNode.l,nodet[1].f)
        '''
        ##----------------
        #time.sleep(5)
        node,flag = getMinFFromOpenList(openList)
        if flag == False:
            print("openlist null")
            return closeList,closeMap,node2NodeCost,False
        else:
           openList,closeList,closeMap = freshCloseList(node,openList,closeList,closeMap)
          ##-------------
        #print("closemaplen",len(closeMap))
        #for nodet2  in closeMap.items():
        #    print("closemap",nodet2[0],nodet2[1])
        ##----------------
             
        if node.reachNode.s == stopPosition.s and node.reachNode.l == stopPosition.l:
            return closeList,closeMap,node2NodeCost,True
    print("openlist null",len(openList))
    return closeList, closeMap,node2NodeCost,False     
  
    
def getTrack(closeMap,startPosition,stopPosition):
    startkey =  startPosition.l * numberOfS + startPosition.s
    endkey = stopPosition.l * numberOfS + stopPosition.s
    
    ikey = closeMap[endkey]
    slist = list()
    llist = list()
    track = queue.deque() 
    slist.append(stopPosition.s)
    llist.append(stopPosition.l)
    track.append(np.int(endkey))
    #print('s,l',endkey,stopPosition.s,stopPosition.l)
    
    while ikey != startkey:
        s =  np.int(ikey % numberOfS)
        l =  np.int((ikey - s) / numberOfS)
        #print('s,l',ikey,s,l)
        track.append(np.int(ikey))
        ikey = closeMap[ikey]
        slist.append(s)
        llist.append(l)
        
    slist.append(startPosition.s)
    llist.append(startPosition.l)
    track.append(np.int(startkey))
    #print('s,l',startkey,startPosition.s,startPosition.l)
    
    return copy.deepcopy(slist),copy.deepcopy(llist),copy.deepcopy(track)

def showTrajackPointGraph(slist,llist):
    plt.plot(slist,llist,'o')
    

def showTrajackPoint(slist,llist):
    slistn  = list()
    llistn = list()
    for s in slist:
        slistn.append(s*detaLength)
        print('s',s*detaLength)
    for l in llist:
        llistn.append(l*detaWidth)
        print('l',l*detaWidth)
        
    plt.plot(slistn,llistn,'o')   

def showTrack(track,node2nodeCost):
    start = track.pop()
    print('node2nodeCost',len(node2nodeCost))
    while len(track) !=0:
        end = track.pop()
        key = str(start) + 'id' + str(end)
        #print('key',start,end) 
        trackvalue = node2nodeCost[key]
        trackvalue.show()
        start = end
    
    
def showCloseList(closeList):
    while not closeList.empty():
        node = closeList.get()
        plt.plot(node.reachNode.s*detaLength,node.reachNode.l*detaWidth,'r*')

def showCloseListGraph(closeList):
    while not closeList.empty():
        node = closeList.get()
        plt.plot(node.reachNode.s,node.reachNode.l,'r*')   
    

if __name__ == '__main__':

    # vehicle to road region  of l 

    startIndexOfL = np.int(vehicleHarfWidth / detaWidth)
    endIndexOfL = numberOfL - np.int(vehicleHarfWidth / detaWidth)
     
    environment = np.zeros((numberOfL,numberOfS))
    createObstacle(environment,obstacle,startIndexOfL,endIndexOfL)
    environmentShow(environment,startIndexOfL,endIndexOfL)
    
    #simple end a*--------------------------------------------

    startPosition = StrPosition(0,12,0.1,0)
    stopPosition = StrPosition(18,8)
    closeList,closeMap,node2NodeCost,Flag = AStart(environment,startPosition,stopPosition,startIndexOfL,endIndexOfL)
    
    
    
    if Flag == True:
        slist,llist,track = getTrack(closeMap,startPosition,stopPosition)
        
        for t in track:
            print('result track',t)
        
        
        showTrajackPoint(slist,llist)
        showTrack(track,node2NodeCost)
    else:
        print("no route-----------------")
        showCloseList(closeList)

    #multi end a*------------------------------------
    '''
    stopPosition = createStopPosition(numberOfS-1,startIndexOfL,endIndexOfL)  
    startPosition = StrPosition(0,9)

    shapeOfEnd = stopPosition.shape
    for i  in  range(0,shapeOfEnd[0]):
        stop = StrPosition(stopPosition[i][0],stopPosition[i][1])
        closeList,closeMap,node2NodeCost,Flag = AStart(environment,startPosition,stop,startIndexOfL,endIndexOfL)
    
        if Flag == True:
            slist,llist,track = getTrack(closeMap,startPosition,stop)   
            for t in track:
                print('result track',t)
        
            showTrajackPoint(slist,llist)
            showTrack(track,node2NodeCost)
        else:
            print("no route-----------------")
            showCloseList(closeList)
        #showCloseList(closeList)   
    '''