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

对角线距离启发式

对角线距离启发式结合了曼哈顿启发式和欧几里德启发式的两个方面。它的 优点是总是给出目标的实际可能的最小代价，不再需要取平方根，从而使其计算效 率略高于欧几里德距离启发式。启发式值由对角线和直线两部分组成。为求可采取的对角线步数，可使用下列公式：

本文采用的为对角距离。

A* 算法原理实现的一个重要组成部分是开链表OPEN  和闭链表 CLOSE。  开放 列表包含已经到达但尚未访问和扩展的所有节点。关闭列表包含所有已访问和展 开的节点。A* 通过从潜在的后续节点列表，即f(n)   值最低的节点中选择最有希望 的节点移动到后续节点。A* 算法的寻路过程可以简单概括为：

Step1: 如图5-5所示，起始点S, 终 点T, 黑色方框为障碍物。从起始点S 开 始，并把它加入到开链表 OPEN 中。现在开链表 OPEN 里只有一项为起始点 S    后面会慢慢加入更多的项。开链表 OPEN  里的格子是路径可能会是沿途经过的，也有可能不经过。

Step3:  计算开链表 OPEN 中所有节点的真实代价 G,   预估代价 H,  全局评估 值 f; 选 择 最 小 全 局 评 估 值f 的 节 点n,  把 它 从 开 链 表 OPEN   里 取 出 ， 放 到 闭 链 表 CLOSE 中。检查所有与它相邻的节点，忽略在闭链表CLOSE 中或障碍物的节点。

#方向标签
    def Angle(self,point_father,point_son):
        if point_son.x-point_father.x==0 and point_son.y-point_father.y==-1:
            angle=1
            return angle
        if point_son.x-point_father.x==1 and point_son.y-point_father.y==-2:
            angle=2
            return angle
        if point_son.x-point_father.x==1 and point_son.y-point_father.y==-1:
            angle=3
            return angle
        if point_son.x-point_father.x==2 and point_son.y-point_father.y==-1:
            angle=4
            return angle
        if point_son.x-point_father.x==1 and point_son.y-point_father.y==0:
            angle=5
            return angle
        if point_son.x-point_father.x==2 and point_son.y-point_father.y==1:
            angle=6
            return angle
        if point_son.x-point_father.x==1 and point_son.y-point_father.y==1:
            angle=7
            return angle
        if point_son.x-point_father.x==1 and point_son.y-point_father.y==2:
            angle=8
            return angle
        if point_son.x-point_father.x==0 and point_son.y-point_father.y==1:
            angle=9
            return angle
        if point_son.x-point_father.x==-1 and point_son.y-point_father.y==2:
            angle=10
            return angle
        if point_son.x-point_father.x==-1 and point_son.y-point_father.y==1:
            angle=11
            return angle
        if point_son.x-point_father.x==-2 and point_son.y-point_father.y==1:
            angle=12
            return angle
        if point_son.x-point_father.x==-1 and point_son.y-point_father.y==0:
            angle=13
            return angle
        if point_son.x-point_father.x==-2 and point_son.y-point_father.y==-1:
            angle=14
            return angle
        if point_son.x-point_father.x==-1 and point_son.y-point_father.y==-1:
            angle=15
            return angle
        if point_son.x-point_father.x==-1 and point_son.y-point_father.y==-2:
            angle=16
            return angle

Step4:如果那些节点不在开链表OPEN  中，则把它们加入到开链表OPEN 中，

传统A*算法缺点分析

虽然传统的A*算法在一些简单的场景具有一定的有效性，但是实际的用途中， 环境复杂性对于算法实时的要求，传统的A*算法并无法满足要求。只有对传统算 法的局限性进行深入了解分析才能更好的在传统方法之上进行更进一步的改进，

因此本小节深入分析传统 A*算法的局限性和不足，具体有：

(1)栅格地图建模的不足：

首先要意识到的是处理的是离散数据，而不是现实世界中的“连续”地形。采样 的数字地形图像是真实地形的近似值，应该在一个理想的高分辨率采样。数字地形 图像的分辨率越高，对真实地形的描述越逼真，寻径精度也越高。然而，在分辨率 上存在一个上限，超过这个上限后，道路就不再更加精确，并且会不必要地增加寻 径算法的运行时间。而且传统的建模方式只限定为可行驶区域和障碍物区域，然而 现实世界环境是及其复杂的，例如可行驶区域可区分为不同道路，沙地、草地、土质路面等等；障碍物也区分有树、行人、车辆建筑物等等。

(2)邻域节点选择不足：

为了找到从起始节点到目标节点的路径，我们必须定义一种选择后续节点的 方式。我们可以从一个给定的位置移动到哪里?在现实世界中， 一个人可以朝着喜 欢的任何方向前进，但在数字地形图上，我们的选择更受限制。传统的A* 算法中。有两种常见的方法：4个邻接和8个邻接。4个邻接限制移动在北、南、西、东四个主要风向。8邻接的移动更自由，因为它除了4邻接的方向外，还可以在东北、西北、西南和东南方向移动。

 #障碍物分布
    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
 
            if x_left<0:
                x_left=0
            if x_right>(w-1):
                x_right=(w-1)
            if y_up<0:
                y_up=0
 
            N=0#障碍物数
            s=0#总网格数
            
            for i in range(x_left,x_right+1):
                for j in range(y_up,minF.point.y+1):
                    s=s+1
                    N=N+map2d[i][j]
 
            P0=N/s#障碍率
            #print(P0)
            #print(N)
            #print(s)

(3)算法无法自适应满足不同任务要求：

在不同的任务要求中，有的任务要保证路径的最短，则设计预估代价小于真实 代价，但是效率低下；有的任务要保证效率的高效，设计预估代价大于真实代价，但是规划的路径不是最优。

(4)对于大地图算法计算效率不足：

对于现实的环境场景，可能寻找道路的搜索空间非常大，这意味着必须采取措 施确保内存不会耗尽，或者搜索不会花费过多的时间运行。即使是一个相对较小的300×300像素的地形图也有9万个节点的搜索空间。

5.3 越野环境下的A* 算法

5.3.1 越野环境建模

传统的A*算法的构建方式中最普遍应用的是栅格法，其基本的思路是把智能 车辆的工作空间分割为尺寸一致的网格，并通过数据矩阵来记录环境数据。常规的 栅格算法把物理环境严格区分为自由区域和障碍物区域，从而使得数值矩阵能够 简化为0-1 矩阵，0 为自由空间，1 为障碍物空间。如假设智能车的工作空间为

R×C,M   为数值矩阵，R 表示所有的环境信息，则常规的环境模型可以表示为。

很明显，常规的栅格模型是无法模拟出真实复杂的越野环境，因此本文研究越 野环境的真实场景，建立多层次栅格模型，将越野环境模型细分为障碍物模型，威

胁模型和道路模型，如图5-6所示。

 if angle==5:
            x_right=minF.point.x+5
            y_up=minF.point.y-2
            y_dowm=minF.point.y+2
 
            if x_right>(w-1):
                x_right=(w-1)
            if y_up<0:
                y_up=0
            if y_dowm>(h-1):
                y_dowm=(h-1)
            N=0#障碍物数
            s=0#总网格数

(1)障碍物模型：

障碍物模型即为越野环境中智能车辆无法驶过的区域，如建筑物、森林、山地 等。如图5-6(a)所示，在越野环境中存在障碍物O1-O3, 障碍物之外为安全通行区。

之后的步骤还将添加安全距离，保证智能车能够和障碍物保存一定距离。障碍物模型表示为：

威胁模型：

威胁模型指的是越野环境中可能对智能车造成破坏损伤的存在，如地雷和敌 军等。威胁模型的范围取决于威胁物的风险程度，本文设置威胁等级H 为1～5,则威胁范围为威胁等级 H 的威胁物向外均匀减少至0所覆盖的范围，例： H=3,则威胁范围半径 r 为[3,2,1,0.8,0.6,0.4,0.2,0],每步间隔一个栅格。威胁模型表示

验值，将不同的地表按照属性设置相应的通过系数，如表所示。

本文根据表5.1,将不同地表属性对车辆的通行影响进行量化，则可以将道路

模型可表示为：