CN112857384B - 基于改进启发函数的a*算法的移动机器人路径规划方法 - Google Patents

Abstract

本发明提出了一种基于改进启发函数的A*算法的移动机器人路径规划方法，旨在保证能规划出最短路径的前提下，通过减小路径的累计转折角度，提高移动机器人路径规划的效率，实现步骤为：构建移动机器人路径规划场景模型；初始化参数并计算起始标记节点A_start的代价值；移动机器人对Close集合C_k进行更新；移动机器人基于改进启发函数的A*算法计算节点P的邻域节点的代价值；移动机器人对Open集合O_k、代价值集合F和路径树T进行更新；移动机器人判断路径规划是否成功；移动机器人对路径树T'进行回溯以得到路径规划结果。

Description

基于改进启发函数的A*算法的移动机器人路径规划方法

技术领域

本发明属于路径规划技术领域，涉及一种移动机器人路径规划方法，具体涉及一种基于改进启发函数的A*算法的移动机器人路径规划方法。

背景技术

移动机器人在执行任务的过程中，往往需要从一个位置移动到另一个指定的位置。在这一过程中，如何让移动机器人快速、安全地到达指定位置，是任务执行能否成功的关键之一，这就涉及到移动机器人在执行任务过程中所使用的路径规划方法。随着人们的不断研究，很多优秀的算法被提出并应用于解决实际问题，如Dijkstra算法、A*算法、蚁群算法和遗传算法等。

A*算法于上世纪七十年代首次提出[Peter E.Hart,Nils J.Nilsson,BertramRaphael.Correction to“A Formal Basis for the Heuristic DeterminationofMinimum Cost Paths”,1972,:28-29.],A*算法综合了BFS(广度优先搜索)和Dijkstra(深度优先搜索)算法的优点，是一种启发式的搜索算法。A*算法利用启发函数减小搜索空间，提高搜索的酰氯，因此，启发函数在A*算法中发挥着关键的作用。同时，由于传统A*算法在路径规划时没有考虑到环境中的障碍信息的影响，因此使用传统A*算法规划出的路径存在折线多、累计转折角度大的问题。

为了解决A*算法存在的问题，提高算法的效率，人们对A*算法进行了很多的改进。例如，申请公布号为CN105844364A，名称为“基于启发函数的服务机器人最优路径规划方法”的专利申请，公开一种基于启发函数的移动式服务机器人的路径规划方法，该方法提出了包含方向和距离两个因素的启发函数，利用启发函数，减小搜索空间，提高搜索效率。但该方法在设计启发函数中的方向因素时，仅仅考虑了起始目标点和终止目标点，没有考虑到环境中的障碍物位置信息，规划出的路径存在转折角度过大的问题，导致路径规划的效率较低。

发明内容

本发明的目的在于克服上述现有技术存在的缺陷，提出一种基于改进启发函数的A*算法的移动机器人路径规划方法，旨在保证能规划出最短路径的前提下，通过减小路径的累计转折角度，提高移动机器人路径规划的效率。

为实现上述目的，本发明采取的技术方案包括如下步骤：

步骤1)构建移动机器人路径规划场景模型：

(1a)采用栅格法将移动机器人所在的二维直角坐标系XOY平面划分为C×R个栅格，得到栅格集合Δ，每个栅格Δ_c,r代表一个节点，其中：

其中，C和R分别表示栅格集合的行数和列数，C≥2，R≥2，Δ_c,r表示第c行第r列个栅格，1≤c≤C，1≤r≤R；

(1b)将Δ中M个障碍物所在的栅格作为障碍物节点，将其余C×R-M个栅格作为移动机器人可以通行的可行节点，并对M个障碍物节点，以及可行节点中移动机器人的起始节点和目标节点进行标注，得到由障碍物标记节点集合B＝{B_k|1≤k≤M}，以及坐标为(x_start,y_start)的起始标记节点A_start和坐标为(x_end,y_end)的目标标记节点A_end组成的移动机器人路径规划场景模型，其中M＞1，B_k表示第k个障碍物标记节点；

步骤2)初始化参数并计算起始标记节点A_start的代价值：

初始化用于存放已经生成但是还没有被访问过的节点的Open集合O_k、用于存放已经访问过的节点的Close集合C_k，以及用于记录路径并以起始标记节点A_start为根节点的路径树T，初始化栅格集合Δ中每个栅格Δ_c,r代表的节点的代价值集合F＝{F(Δ_c,r)|F(Δ_c,r)＝C×R}，并将A_start加入到集合O_k中，同时计算起始标记节点A_start的代价值F(A_start)；

步骤3)移动机器人对Close集合C_k进行更新：

移动机器人检索Open集合O_k中的每一个节点，并在代价值集合F中查找节点的代价值，找出O_k中代价值最小的节点P，并将P加入到Close集合C_k中，得到更新后的Close集合C_k'；

步骤4)移动机器人基于改进启发函数的A*算法计算节点P的邻域节点的代价值：

(4a)移动机器人初始化节点P的邻域节点N＝{N_i|1≤i≤8}，N_i表示第i个邻域节点；

(4b)移动机器人基于改进启发函数的A*算法计算节点P的每个邻域节点N_i的代价值F_P(N_i)：

F_P(N_i)＝G(N_i)+H(N_i)+V(N_i)

其中，G(N_i)、H(N_i)和V(N_i)分别表示N_i的实际代价、预估代价和障碍代价，

表示坐标点P所受到的势场力，||·||表示取模值，θ(N_i)表示P点和邻域节点N_i点所连成的直线N_iP与

之间的夹角，

表示目标标记节点A_end对P点的吸引力，

表示障碍物标记节点B_k对P点的排斥力，α和β分别表示

和

的权重，且α≥1,β≥1，

表示对所有属于集合B的元素B_k进行求和，x_p和y_p分别表示P点的横坐标和纵坐标，x_start和y_start分别表示起点A_start的横坐标和纵坐标，

和

分别表示移动机器人路径规划场景模型中X轴的方向向量和Y轴的方向向量，x_Bk和y_Bk分别表示B_k的横坐标和纵坐标，m表示B_k的距离阈值，m≥2；

步骤5)移动机器人对Open集合O_k、代价值集合F和路径树T进行更新：

(5a)移动机器人对Open集合O_k进行更新：

(5a1)移动机器人将节点P从Open集合O_k中移除，并令i＝1；

(5a2)移动机器人判断N_i是否存在于Open集合O_k中，若是，执行步骤(5a3)，否则，将N_i加入Open集合O_k，并执行步骤(5a3)；

(5a3)移动机器人判断i＝8是否成立，若是，得到更新后的Open集合O_k'，否则，令i＝i+1，并执行步骤(5a2)；

(5d)移动机器人令代价集合F中N_i的代价值F(N_i)＝min{F(N_i),F_P(N_i)}，得到更新后的代价值集合F'，其中，min{}表示取最小值；

(5e)移动机器人将N加入到路径树T中并作为P的叶子节点，得到更新后的路径树T'；

步骤6)移动机器人判断路径规划是否成功：

移动机器人检查集合O_k'中是否已经包含目标标记节点A_end，若是，执行步骤(7)，否则，令T＝T'，F＝F'，O_k＝O_k'，并执行步骤(3)；

步骤7)移动机器人对路径树T'进行回溯以得到路径规划结果：

移动机器人对路径树T'从目标标记节点A_end回溯至起始标记节点A_start，得到路径点集合{A_start,...,A_P,...,A_end}，并将这些路径点的连线作为移动机器人的规划路径。

本发明与现有技术相比，具有如下优点：

本发明采用基于改进启发函数的A*算法计算每个邻域节点的实际代价、预估代价和障碍代价，实际代价能够保证规划出最短的路径，包含距离因素的预估代价和包含障碍物信息的障碍代价构成每个邻域节点的启发代价，在障碍代价中构造了目标标记节点对移动机器人的吸引力和障碍物标记节点对机器人的排斥力，通过设计“虚拟力”的方式，让障碍代价中包含了方向信息，从而让启发代价包含更多启发信息，缩小了搜索空间，能够避免规划出的路径存在转折角度过大的缺陷，与现有技术相比，在保证规划路径最短的前提下，有效提高了移动机器人路径规划的效率。

附图说明

图1是本发明的实现流程图。

图2是本发明构建的移动机器人路径规划场景模型图。

图3是本发明的路径规划结果图。

具体实施方式

下面结合附图和具体实施例，对本发明作进一步的详细描述。

参照图1，本发明包括如下步骤：

步骤1)构建移动机器人路径规划场景模型：

(1a)采用栅格法将移动机器人所在的二维直角坐标系XOY平面划分为C×R个栅格，得到栅格集合Δ，每个栅格Δ_c,r代表一个节点，其中：

其中，C和R分别表示栅格集合的行数和列数，C≥2，R≥2，Δ_c,r表示第c行第r列个栅格，1≤c≤C，1≤r≤R，本实施例中，C＝39,R＝39。

(1b)将Δ中M个障碍物所在的栅格作为障碍物节点，将其余C×R-M个栅格作为移动机器人可以通行的可行节点，并对M个障碍物节点，以及可行节点中移动机器人的起始节点和目标节点进行标注，得到由障碍物标记节点集合B＝{B_k|1≤k≤M}，以及坐标为(x_start,y_start)的起始标记节点A_start和坐标为(x_end,y_end)的目标标记节点A_end组成的移动机器人路径规划场景模型，其中M＞1，B_k表示第k个障碍物标记节点，本实施例中，M＝495。

参照图2，本实施例构建的移动机器人路径规划场景模型包含39×39个栅格；图中的黑色方块表示障碍物标记节点，一共有495个障碍物标记节点，这些障碍物标记节点构成本实施例中的障碍物标记节点集合B；图中的白色方块表示可行节点，一共有39×39-495个可通行节点；图中菱形块所在的节点为标注的起始标记节点A_start，坐标为(24,36)；图中圆形所在的节点为标注的目标标记节点A_end，坐标为(4,35)。

步骤2)初始化参数并计算起始标记节点A_start的代价值：

初始化用于存放已经生成但是还没有被访问过的节点的Open集合O_k、用于存放已经访问过的节点的Close集合C_k，以及用于记录路径并以起始标记节点A_start为根节点的路径树T，初始化栅格集合Δ中每个栅格Δ_c,r代表的节点的代价值集合F＝{F(Δ_c,r)|F(Δ_c,r)＝C×R}，并将A_start加入到集合O_k中，同时计算起始标记节点A_start的代价值F(A_start)；

F(A_start)计算公式为：

F(A_start)＝|x_start-x_end|+|ystart-yend|

其中，x_start和y_start分别表示A_start的横坐标和纵坐标，x_end和y_end分别表示A_end的横坐标和纵坐标。

本实施例中，A_start的坐标为(24,36)，A_end的坐标为(4,35)，因此，本实施例中的F(A_start)＝|24-4|+|36-35|＝21。

步骤3)移动机器人对Close集合C_k进行更新：

移动机器人检索Open集合O_k中的每一个节点，并在代价值集合F中查找节点的代价值，找出O_k中代价值最小的节点P，并将P加入到Close集合C_k中，得到更新后的Close集合C_k'；

步骤4)移动机器人基于改进启发函数的A*算法计算节点P的邻域节点的代价值：

(4a)移动机器人初始化节点P的邻域节点N＝{N_i|1≤i≤8}，N_i表示第i个邻域节点；

(4b)移动机器人基于改进启发函数的A*算法计算节点P的每个邻域节点N_i的代价值F_P(N_i)：

F_P(N_i)＝G(N_i)+H(N_i)+V(N_i)

其中，G(N_i)、H(N_i)和V(N_i)分别表示N_i的实际代价、预估代价和障碍代价，

表示坐标点P所受到的势场力，||·||表示取模值，θ(N_i)表示P点和邻域节点N_i点所连成的直线N_iP与

之间的夹角，

表示目标标记节点A_end对P点的吸引力，

表示障碍物标记节点B_k对P点的排斥力，α和β分别表示

和

的权重，且α≥1,β≥1，

表示对所有属于集合B的元素B_k进行求和，x_p和y_p分别表示P点的横坐标和纵坐标，x_start和y_start分别表示起点A_start的横坐标和纵坐标，

和

分别表示移动机器人路径规划场景模型中X轴的方向向量和Y轴的方向向量，x_Bk和y_Bk分别表示B_k的横坐标和纵坐标，m表示B_k的距离阈值，m≥2，本实施例中，α＝5,β＝2，m＝2。

本发明对A*算法改进为:在代价函数中加入了障碍代价，改进后的代价函数包括实际代价G(N_i)、预估代价H(N_i)和障碍代价V(N_i)，将H(N_i)和V(N_i)的和H(N_i)+V(N_i)作为改进A*算法中的启发函数，这一启发函数不仅考虑了距离因素，还考虑了障碍物的信息，包含更多启发信息。实际代价G(N_i)和预估代价H(N_i)，计算公式分别为：

其中，x_p和y_p分别表示坐标点P的横坐标和纵坐标，x_Ni和y_Ni分别表示邻域节点N_i的横坐标和纵坐标，x_end和y_end分别表示A_end的横坐标和纵坐标，min{}表示取最小值。

例如，在本实例中，当P的坐标为(22,35)，N_i的坐标为(23,35)，G(P)的值为16时，可以计算出实际代价

预估代价

步骤5)移动机器人对Open集合O_k、代价值集合F和路径树T进行更新：

(5a)移动机器人对Open集合O_k进行更新：

(5a1)移动机器人将节点P从Open集合O_k中移除，并令i＝1；

由于节点P已经完成了搜索，因此将节点从O_k中移除，以确保移动机器人下一次不会再搜索到此节点。

(5a2)移动机器人判断N_i是否存在于Open集合O_k中，若是，执行步骤(5a3)，否则，将N_i加入Open集合O_k，并执行步骤(5a3)；

(5a3)移动机器人判断i＝8是否成立，若是，得到更新后的Open集合O_k'，否则，令i＝i+1，并执行步骤(5a2)；

如果i＝8，说明移动机器人对N中所有的元素都完成了判断，那么，对O_k的更新也就完成了，因此得到更新后的Open集合O_k'，否则，还需要对其他元素进行判断。

(5d)移动机器人令代价集合F中N_i的代价值F(N_i)＝min{F(N_i),F_P(N_i)}，得到更新后的代价值集合F'，其中，min{}表示取最小值；

(5e)移动机器人将N加入到路径树T中并作为P的叶子节点，得到更新后的路径树T'；

步骤6)移动机器人判断路径规划是否成功：

移动机器人检查集合O_k'中是否已经包含目标标记节点A_end，若是，令T＝T'，并执行步骤(7)，否则，令T＝T'，F＝F'，O_k＝O_k'，并执行步骤(3)；

若集合O_k'中已经包含目标点A_end，说明移动机器人已经搜索到了目标标记节点A_end，已经成功找到了一条从起始标记节点到目标标记节点的路径；否则，还需要继续进行搜索，因此需要用更新后的Open集合O_k'、代价值集合F'和路径树T'替换原来的Open集合O_k、代价值集合F和路径树T，并开始下一次搜索。

步骤7)移动机器人对路径树T'进行回溯以取路径规划结果：

移动机器人对路径树T'从目标标记节点A_end回溯至起始标记节点A_start，得到路径点集合{A_start,...,A_P,...,A_end}，并将这些路径点的连线作为移动机器人的规划路径，本实施例中移动机器人规划的路径如图3所示，图3中的·表示路径点，路径点之间的连线就是本实施例中移动机器人规划的路径，从图中可以看出移动机器人规划出了最短的路径，且路径没有冗余的转折点，总转折角度较小。

Claims (3)

1.一种基于改进启发函数的A*算法的移动机器人路径规划方法，其特征在于，包括如下步骤：

(1)构建移动机器人路径规划场景模型：

(1a)采用栅格法将移动机器人所在的二维直角坐标系XOY平面划分为C×R个栅格，得到栅格集合Δ，每个栅格Δ_c,r代表一个节点，其中：

其中，C和R分别表示栅格集合的行数和列数，C≥2，R≥2，Δ_c,r表示第c行第r列个栅格，1≤c≤C，1≤r≤R；

(1b)将Δ中M个障碍物所在的栅格作为障碍物节点，将其余C×R-M个栅格作为移动机器人可以通行的可行节点，并对M个障碍物节点，以及可行节点中移动机器人的起始节点和目标节点进行标注，得到由障碍物标记节点集合B＝{B_k|1≤k≤M}，以及坐标为(x_start,y_start)的起始标记节点A_start和坐标为(x_end,y_end)的目标标记节点A_end组成的移动机器人路径规划场景模型，其中M＞1，B_k表示第k个障碍物标记节点；

(2)初始化参数并计算起始标记节点A_start的代价值：

初始化用于存放已经生成但是还没有被访问过的节点的Open集合O_k、用于存放已经访问过的节点的Close集合C_k，以及用于记录路径并以起始标记节点A_start为根节点的路径树T，初始化栅格集合Δ中每个栅格Δ_c,r代表的节点的代价值集合F＝{F(Δ_c,r)|F(Δ_c,r)＝C×R}，并将A_start加入到集合O_k中，同时计算起始标记节点A_start的代价值F(A_start)；

(3)移动机器人对Close集合C_k进行更新：

移动机器人检索Open集合O_k中的每一个节点，并在代价值集合F中查找节点的代价值，找出O_k中代价值最小的节点P，并将P加入到Close集合C_k中，得到更新后的Close集合C_k'；

(4)移动机器人基于改进启发函数的A*算法计算节点P的邻域节点的代价值：

(4a)移动机器人初始化节点P的邻域节点N＝{N_i|1≤i≤8}，N_i表示第i个邻域节点；

(4b)移动机器人基于改进启发函数的A*算法计算节点P的每个邻域节点N_i的代价值F_P(N_i)：

F_P(N_i)＝G(N_i)+H(N_i)+V(N_i)

其中，G(N_i)、H(N_i)和V(N_i)分别表示N_i的实际代价、预估代价和障碍代价，

表示坐标点P所受到的势场力，||·||表示取模值，θ(N_i)表示P点和邻域节点N_i点所连成的直线N_iP与

之间的夹角，

表示目标标记节点A_end对P点的吸引力，

表示障碍物标记节点B_k对P点的排斥力，α和β分别表示

和

的权重，且α≥1,β≥1，

表示对所有属于集合B的元素B_k进行求和，x_p和y_p分别表示P点的横坐标和纵坐标，x_start和y_start分别表示起点A_start的横坐标和纵坐标，

和

分别表示移动机器人路径规划场景模型中X轴的方向向量和Y轴的方向向量，

和

分别表示B_k的横坐标和纵坐标，m表示B_k的距离阈值，m≥2；

(5)移动机器人对Open集合O_k、代价值集合F和路径树T进行更新：

(5a)移动机器人对Open集合O_k进行更新：

(5a1)移动机器人将节点P从Open集合O_k中移除，并令i＝1；

(5a2)移动机器人判断N_i是否存在于Open集合O_k中，若是，执行步骤(5a3)，否则，将N_i加入Open集合O_k，并执行步骤(5a3)；

(5a3)移动机器人判断i＝8是否成立，若是，得到更新后的Open集合O_k'，否则，令i＝i+1，并执行步骤(5a2)；

(5d)移动机器人令代价集合F中N_i的代价值F(N_i)＝min{F(N_i),F_P(N_i)}，得到更新后的代价值集合F'，其中，min{}表示取最小值；

(5e)移动机器人将N加入到路径树T中作为P的叶子节点，得到更新后的路径树T'；

(6)移动机器人判断路径规划是否成功：

移动机器人检查集合O_k'中是否已经包含目标标记节点A_end，若是，执行步骤(7)，否则，令T＝T'，F＝F'，O_k＝O_k'，并执行步骤(3)；

(7)移动机器人对路径树T'进行回溯以得到路径规划结果：

移动机器人对路径树T'从目标标记节点A_end回溯至起始标记节点A_start，得到路径点集合{A_start,...,A_P,...,A_end}，并将这些路径点的连线作为移动机器人的规划路径。

2.根据权利要求1所述的基于改进启发函数的A*算法的移动机器人路径规划方法，其特征在于，步骤(2)中计算起始标记节点A_start的代价值F(A_start)，计算公式为：

F(A_start)＝|x_start-x_end|+|y_start-y_end|

其中，x_start和y_start分别表示A_start的横坐标和纵坐标，x_end和y_end分别表示A_end的横坐标和纵坐标。

3.根据权利要求1所述的基于改进启发函数的A*算法的移动机器人路径规划方法，其特征在于，步骤(4b)中的N_i的实际代价G(N_i)和预估代价H(N_i)，计算公式分别为：

其中，x_p和y_p分别表示坐标点P的横坐标和纵坐标，

和

分别表示邻域节点N_i的横坐标和纵坐标，x_end和y_end分别表示A_end的横坐标和纵坐标，min{}表示取最小值。

Images