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

子节点优化选择策略

(1)子节点选择方式

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

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

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

环境智能车的需求。

在越野栅格地图中，我们使用节点之间的距离作为全局评估函数中真实代价函数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)  # runFile "/home/xugaoxiang/anaconda3/envs/pytorch1.6/lib/python3.7/site-packages/torch/nn/modules/module.py", line 722, in _call_implresult = self.forward(*input, **kwargs)File "/home/xugaoxiang/Works/Yolov5-Deepsort-Fastreid/models/yolo.py", line 36, in forwardself.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=xself.y=y#判断是否同一个点def __eq__(self, other):if self.x==other.x and self.y==other.y:return Truereturn False

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

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

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_stepself.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: 计算子节点的真实代价函数值。通过方向变化惩罚后，可以使得规划的路径尽量保持直线，减少拐点，使得路径更加高效、符合实际要求。

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

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

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

(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-startdef __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 = startPointself.endPoint = endPointelse:self.startPoint = Point(*startPoint)self.endPoint = Point(*endPoint)# 不可行走标记self.passTag = passTagdef 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)时间图

(a)Time to figure

(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 Truereturn False#判断节点是否在开启表中，是返回该节点def pointInOpenList(self, point):for node in self.openList:if node.point == point:return nodereturn None#判断开启表中是否有终点def endPointInCloseList(self):for node in self.openList:if node.point == self.endPoint:return nodereturn 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。

图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 1angle=self.Angle(minF.father.point,minF.point)#主方向   #print(angle)if angle==1:x_left=minF.point.x-2x_right=minF.point.x+2y_up=minF.point.y-5

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

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