改进的A*算法的路径规划(2)

 子节点优化选择策略

(1)子节点选择方式

为了找到从起始点到终点的路径,需定义一种可以选择后续节点的方式。在 A*算法中两种常见的方法为4-邻接(见图5-7(a) 和8-邻接(见图5-7(b)),  但考虑到 在复杂越野环境上,我们希望智能车辆允许更多的自由运动来更好规避危险,因此

本文选择16-邻接(见图5-7(c))。

如图5-8所示,4-邻接规划的路径具有很多的直角拐点且路径最长,其次是8- 邻接规划的路径,而16-邻接规划的路径平滑、拐点数少、路径短,适合复杂越野

环境智能车的需求。

改进的A*算法的路径规划(2)_第1张图片

在越野栅格地图中,我们使用节点之间的距离作为全局评估函数中真实代价  g(n)  的计算成本。规定:上下左右移动成本为1;对角45°移动成本为  2;  其余方向移动成本为  5。考虑到计算机对小数处理速度慢于处理整数,则将成本 扩大10倍,最后上下左右移动成本为10;对角45°移动成本为14;其余方向移动成本为22。

(2)优化子节点选择

传统 A* 算法在子节点选取上,仅考察子节点周围是否为障碍物,而未考察子 节点与障碍物位置的相关性,从而规划出路线存在斜着通过障碍物栅格顶点的问 题,导致车辆可能与障碍物发生碰撞。因为本文中所构建环境模型具有更危险的威

胁物存在,所以优化了子节点的选择规则。如图5-9,为16个子节点分布图。本文结合越野环境栅格地图R,    设计的子节点选择规则为:

1:若子节点4或子节点12具有威胁(在越野环境栅格地图R 中值≥1),则子节 点2、子节点6、子节点3、子节点5或子节点13、子节点9、子节点14、子节点11不作为预选点。

  x = m(x)  # run
  File "/home/xugaoxiang/anaconda3/envs/pytorch1.6/lib/python3.7/site-packages/torch/nn/modules/module.py", line 722, in _call_impl
    result = self.forward(*input, **kwargs)
  File "/home/xugaoxiang/Works/Yolov5-Deepsort-Fastreid/models/yolo.py", line 36, in forward
    self.training |= self.export

2: 若子节点16或子节点8具有威胁,则子节点2、子节点13、子节点15、子节点1或子节点6、子节点9、子节点10、子节点7不作为预选点。

#获得任意节点信息 ,__getitem__()魔法函数作用为当实例化对象map进行map[key]操作上自动调用。
    def __getitem__(self, item):
        return self.data[item]

###############创建点类################
class Point:
    #初始化
    def __init__(self,x,y):
        self.x=x
        self.y=y
        
    #判断是否同一个点
    def __eq__(self, other):
        if self.x==other.x and self.y==other.y:
            return True
        return False

3: 均无具威胁,则不做处理。

优化子节点选择后,规划后的路径避开具有威胁栅格的顶点,避免智能车辆在运动中发生碰撞,更符合实际要求。

改进的A*算法的路径规划(2)_第2张图片

5.3.3 自适应评估函数设计

为权衡复杂越野环境下智能车辆的行驶安全与行车效率之间的关系,规划出 安全可行的行车路径,本文提出改进的 A*算法。传统A*算法的评价函数一般 为:

f(n)=g(n)+h(n)                               

   (5.14) 式中: f(n)   为全局代价函数, g(n)   为真实代价函数, h(n)  为启发函数。针对传统 A*算法计算效率、无法自适应不同任务要求和拐点数量多等问题,因此,本文算 法提出了方向变化惩罚与局部区域复杂度惩罚来自适应调整g(n)  h(n) 的比例系数。

(1)方向变化惩罚

在传统的 A* 算法中,计算节点的真实代价函数g(n) 值时,到邻域任何子节点 并没有产生惩罚。然而实际的车辆行驶中,我们希望车辆保持直线行驶,所以规划 的路径应笔直、更少转弯和转弯的角度应小点等,因此本文引入方向变化惩罚,来 减少路径的无用拐点。

在越野环境中,本文规定智能车的转弯角度范围为0°~90°,将方向变化对车辆行驶的影响近似转换为行驶距离的增加,即:在原本的移动成本上进行惩罚。

 # 描述AStar算法中的节点数据
    class Node:  
        #初始化
        def __init__(self, point, startPoint,endPoint, g=0,w=1,p=1):
            self.point = point  # 自己的坐标
            self.father = None  # 父节点
            self.g = g       # g值,g值在用到的时候会重新算
            
            # 计算h值,采用曼哈顿距离
            #self.h = (abs(endPoint.x - point.x) + abs(endPoint.y - point.y)) * 10  
            
            #采用欧几里得距离
            #self.h = math.pow((math.pow((endPoint.x - point.x),2) + math.pow((endPoint.y - point.y),2)),0.5)*10
            
            #采用对角距离
            pp=(1-p)+0.2*math.exp((math.pow((math.pow((endPoint.x - point.x),2) + math.pow((endPoint.y - point.y),2)),0.5))/(math.pow((math.pow((endPoint.x - startPoint.x),2) + math.pow((endPoint.y - startPoint.y),2)),0.5)))
            Diagonal_step = min((endPoint.x - point.x),(endPoint.y - point.y))
            straight_step = (abs(endPoint.x - point.x) + abs(endPoint.y - point.y)) - 2*Diagonal_step
            self.h  =(straight_step + math.pow(2,0.5)*Diagonal_step)*10*pp
            #print(pp)

方向变化惩罚规则为:

Stepl:计算当前节点与该其父节点的方向 Direction1,方向规定如图5-10(a)所 示。

Step2: 计算当前节点到其子节点的方向 Direction2。

Step3:  计算D_Change=|Direction1-Direction2|,     D_Change>4,   则将方向变化惩罚 D_P 置为无穷大,若D_Change≤4,      则参考表5.2 选择相应的方向变化惩 罚系数,图5-10(b) 已给出相应的例子参考。

Step4:   计算子节点的真实代价函数值。通过方向变化惩罚后,可以使得规划的路径尽量保持直线,减少拐点,使得路径更加高效、符合实际要求。

改进的A*算法的路径规划(2)_第3张图片

改进的A*算法的路径规划(2)_第4张图片

(2)局部区域复杂度惩罚

通过方向变化惩罚后,增加了节点的真实代价值,在A*算法中,真实代价G 大于预估代价 H 时,会增加算法的搜索范围,使得效率降低。所以本小节引入了 局部区域复杂度惩罚,来自适应调节预估代价 H 的值,使得预估代价约等于真实代价,提高算法的效率。

传统的A*算法中,算法本身是未考虑环境的复杂度。经过研究证实,路径规 划算法计算的复杂性和环境搜索空间的规模成正比,例如: A* 算法的复杂性为 o(n²)(n     为节点数),降低搜索空间即可减少遍历的节点数。为此,本文提出了根据局部区域复杂度,来自适应调节节点的搜索空间,以减少算法的时间复杂度。

改进的A*算法的路径规划(2)_第5张图片

(a)局部区域示意图           (b)不同方位局部区域

图5-11 局部区域复杂度定义示意图

如上图5-11(a)所示,若此时当前节点为节点1,它的父节点为父节点1,则可 计算得到当前的方向为3,参考图5-11(b)可获得该方位上的局部区域,观察图5- 11(a)局部区域,明显此区域存在障碍物、威胁物和草地等,在此区域内,希望算法 能够扩大搜索范围,寻找更优路径在避免接触障碍物和威胁物。相反如果该区域不存在障碍物和威胁物体,则希望算法能够缩小搜索范围,提高效率。因此,本文量化了局部区域的信息,设计了威胁率Q。和通过率Q,  具体为:

式中:L。为局部区域,R(i,j)越野环境栅格地图值,δ,为环境敏感度,考虑到越 野智能车辆,对于草地土路能够轻松驶过,所以本文设置为0.5,可根据车辆类型 工作任务灵活选取。n₂,n,    为越野栅格地图中一行和一列均小于δ,的条数, L  D 为局部区域的行和列。Q,描述了环境中威胁程度的多少, Q,越大表明环境存在 越多不可通过的物体。Q,描述了环境中威胁物、障碍物等位置的混乱程度, Q,越 大表明混乱度越小。

#初始化A-start
    def __init__(self, map2d, startPoint, endPoint, passTag=1.0):#map2d地图信息,startPoint起点, endPoint终点, passTag=1.0为不可行驶区域

        # 开启表
        self.openList = []
        # 关闭表
        self.closeList = []
        # 寻路地图
        self.map2d = map2d
        # 起点终点
        if isinstance(startPoint, Point) and isinstance(endPoint, Point):
            self.startPoint = startPoint
            self.endPoint = endPoint
        else:
            self.startPoint = Point(*startPoint)
            self.endPoint = Point(*endPoint)
 
        # 不可行走标记
        self.passTag = passTag
 
    def getMinNode(self):
        """
        获得openlist中F值最小的节点
        :return: Node
        """

局部区域惩罚规则为:

Stepl: 计算当前节点与该其父节点的方向 Direction。

Step2:  根据方向 Direction,  参考图5-11(b)选择区域范围。

Step3: 计算威胁率Q。和通过率Q,。

Step4:  计算节点的预估代价函数值。

值得注意的是,值得注意的是,图5-11(b)中局部区域范围a 的取值,本文采 用正方形区域提高为了编程效率。a 的取值也影响着算法的效率,如图5-12所示,

改进的A*算法的路径规划(2)_第6张图片

(a)时间图

(a)Time to figure

改进的A*算法的路径规划(2)_第7张图片

(b)节点数

(b)Number of nodes in figure

取四个大小分别为30×30,40×40,50×50,60×60具有相同环境分布的栅格地 图,观察到:在不同规模的地图上,随着a 值的增加,算法遍历的节点数都有减少 的趋势,但是当a 值过大时,超过6时,算法的运算时长却有所增加,这是因为随  a 变大局部区域变大,则计算公式(5.15)和公式(5.16)需要更多时间,因此a 值应 该处于3≤a≤6,        本文选择a 为5。

 #判断节点是否在关闭表中
    def pointInCloseList(self, point):
        for node in self.closeList:
            if node.point == point:
                return True
        return False
    
    #判断节点是否在开启表中,是返回该节点
    def pointInOpenList(self, point):
        for node in self.openList:
            if node.point == point:
                return node
        return None
    
    #判断开启表中是否有终点
    def endPointInCloseList(self):
        for node in self.openList:
            if node.point == self.endPoint:
                return node
        return None

(3)自适应评估函数

在获得方向变化惩罚 D_P、 局部区域的威胁率Q。和通过率Q, 在越野环境R下,改进的评估函数可以表示为:

f(n)=R(g(nz)+n*Step)+R,(h(n))

式中:g(ng)为节点n父节点的真实代价值, D_P为方向变化惩罚, Step移动代 价,R(i,j)为在节点坐标(i,j)越野栅格地图值,h(n)为节点n 的预估代价,ε为环 境威胁敏感度, d 为节点到目标点的距离, d 起始点到目标点的距离。值得注意 的是,为了加快计算机处理速度将移动成本增加了10倍,因为在计算预估代价时 也需增加10倍。

因此,综合考虑了真实越野地图的复杂性、方向变化、局部区域复杂度的影响。 方向变化惩罚确保了规划的路线尽可能笔直,并存在较少的拐点;越野栅格地图模 拟了实际的越野情景,让规划的路线更符合实际需要;局部区域复杂度惩罚则提高 了算法的高效性,当环境危险度较低时,则使启发函数权重增加,进而减小搜寻空 间,从而提升搜寻效率,相反当环境危险度高时,则使启发函数权重降低,从而扩大搜寻空间,提升搜寻精度,当环境通过率较低时,表明环境的混乱程度较大,则使真实代价函数权重提高,拓展了搜寻空间,反之降低权权重。将越野环境信息与 智能汽车的位置信息相结合,并针对不同场景,通过权重函数实现自适应调节,从 而自主调整算法的有效搜索空间,不但能够提高计算的高效性与灵敏度,还保证了路径的全局最优。

5.3.4双向Floyd 算法优化路径

Floyd 算法是一种利用动态规划思想寻找多源点之间最短路径的算法之一。根 据之前小节规划出的路径可能存在多余拐点、路径并非最优的情况,因此针对此问 题,本节提出了改进的 Floyd 算法,设计了双向优化处理来实现双向平滑优化;设计了安全距离,来保证优化了路径避免与障碍物威胁物等发生碰撞。

首先增添防碰撞安全距离预警,首先设置的安全距离 D, 接着计算威胁物点到 连线的垂直距离L,  判断安全距离与垂直距离的关系,从而判断优化的路径是否安 全。如图5-13 所示,设点a坐标为(x,ya), 点S 坐标为(xs,ys), 点n的坐标为 (xa,Ya), 则可以计算得到点a 距离直线 S-n的距离 L。安全距离的设置还与地图 位置有关,由于智能越野车辆能够较为轻松驶过道路通行系数小于0.8的道路,因此安全距离具体为:

式中: cell 为单元栅格的长度。因此,优化的路径要保证距离威胁物的距离要满足

L≥D

改进的A*算法的路径规划(2)_第8张图片

 图5-13 双向 Floyd 算法示意图

双向 Floyd 算法优化路径具体步骤为:

Stepl:    从起始点 S  开始,设置 S 为开始点,按步长 k 取下一路径点1,计算 距离 L 与安全距离D 并判断小大,若满足L≥D   则取下一路径点2,直至存在不满 L≥D     的路径点n,   则重新设置点n-1 为开始点,继续取点循环以上步骤直至遇到终点T 循环结束。

#障碍物分布
    def obstacle_dis(self,minF):
        if minF.father==None:
            return 1
        angle=self.Angle(minF.father.point,minF.point)#主方向   
        #print(angle)

        if angle==1:
            x_left=minF.point.x-2
            x_right=minF.point.x+2
            y_up=minF.point.y-5

Step2:  反向 Floyd 算法,将终点T 设置为开始点,按 Stepl 反向遍历路径点 直至遇见起始点 S 循环结束。

Step3: 若正向 Floyd算法优化路径与反向 Floyd 算法优化路径存在交点,则取 两者交集;若不存在交点,则于拐点数与路径长度和小的路径。

QQ767172261

你可能感兴趣的:(算法)