CN114166235A - 一种基于优化a-star算法的全局动态平滑路径规划方法 - Google Patents

Abstract

本发明公开一种基于优化A‑STAR算法的全局动态平滑路径规划方法，前期将障碍物进行矩形化处理，无障碍区域应用传统A*算法寻路，在遇到障碍物时，采用优化后的A*算法，在初次完成路径规划后，按照所选取的路径点，加到路径列表进行计算，然后应用人工势场法进行路径平滑处理。本发明不但能够保证搜索到的路径具有平滑性和实时避障能力，还在确保全局最优的基础上提高了寻路效率。

Description

一种基于优化A-STAR算法的全局动态平滑路径规划方法

技术领域

本发明涉及动态路径规划技术领域，尤其涉及一种基于优化A-STAR算法的全局动态平滑路径规划方法。

背景技术

A*算法是一种经典的启发式算法，能够在静态环境中高效地求出从起始点到目标点的最优路径，这种算法程序简单，易于实现。目前，传统的A*算法在运行过程中，需要对Open list和Close list两个列表进行维护。A*在寻路过程中需要计算的当前位置的周围节点，当遇到障碍物时，就会在起点附近重新进行寻路，由此重复，直到搜索宽度超过障碍物宽度，由此再继续向前寻路，因此会产生大量的无用节点需要计算，导致运算量过大，占用内存过大，寻路时间过长等问题。并且在规划好的路径中存在着不必要的转折点，传统A*算法没有对路径进行平滑处理，因此规划好的路径转折剧烈，不利于机器人进行路径跟踪。

发明内容

本发明的目的在于提供一种基于优化A-STAR算法的全局动态平滑路径规划方法。

本发明采用的技术方案是：

一种基于优化A-STAR算法的全局动态平滑路径规划方法，其包括路径规划部分和路径平滑处理部分，具体步骤如下：

路径规划部分：

步骤10，构建环境地图,将地图上障碍物区域内的栅格上标注为-1并将-1的坐标放入障碍物列表，并基于起始点和终点构建启发式函数：

F(m)＝G(m)+H(m)

其中，Fm表示起始点到任意节点的估价值，Gm表示起始点到某一个节点的实际代价，Hm表示起始该点到终点的估计代价值；

步骤11，分别创建一个开启列表和关闭列表，并将起始点添加到开启列表中，开启列表用来存放当前点的相邻节点，关闭列表用来存放开启列表中F值最小的节点，即下一步的父节点；

步骤12，检查开启列表是否为空列表；是则，结束寻路并进入路径平滑部分；否则，继续执行步骤13；

步骤13：判断是否遇到障碍物；是则，将当前点的8个相邻节点按照顺序分别放进开放列表中，将对应的代价值F数据分别进行标记并执行步骤14；否则，执行步骤15；

步骤14，将开放列表的元素分别与障碍物列表的元素进行对比，当确定有相同的点时，将对应的点及其所有数据都设置成无穷大；

步骤15：对开启列表中以F值的大小进行排序以得到F值最小的点，将F值最小点放入关闭列表中，然后在开放列表里面删除对应的F值最小点；

步骤16：判断目标点(即终点)是否到在关闭列表中；是则，结束寻路并进入路径平滑部分；否则，执行步骤17；

步骤17，判断当前点的相邻点是否在关闭列表；是则，忽略此当前点的相邻点并执行步骤12；否则，执行步骤18；

步骤18，判断当前点的相邻点是否在开放列表里；是则，用当前点的G值重新检查当前点的相邻点，并更新G、F值和父节点后执行步骤12；否则，将当前点的相邻点添加到开放列表里面，并重新计算F和G的值，把当前点作为父节点执行步骤12；

具体地，以当前点为父节点，依次判断开放列表里面的相邻点是否能成为下一个父节点，更新的是开放列表中挑选出来的下一个父节点。更新父节点就是将上一个父节点的坐标及其参数换成当前的。

具体地，不同与步骤13遇到障碍物时，一次性直接无差别的将周围的8个节点放入开放列表。步骤18中寻路已经开始进行，一边往前走一边将相邻点添加到开放列表里面。

父节点在步骤12中没有作用，步骤12的作用就是判断是否继续循环还是进行下一步。

路径平滑部分：

步骤20，获取路径列表并计算获取路径列表的行数，并设置n的初始值为1；n表示路径点列表(path)里面的索引，n是几代表了就是路径点里面的第几行的坐标。

步骤21，将临时起点坐标放入路径列表，并将临时起点的行数赋值为n，将第n+2行作为临时终点；

具体地，临时起点就是人工势场法寻路的时候的起点，如何确定的就是以N为准，N为几的时候，对应的路径点列表(path)的第N行就是临时起点。

步骤22，判断n的值是否等于路径列表的行数-1【length(path)-1】；是则，将临时起点设为n行点，临时终点设为终点，重置步长和迭代次数后执行步骤23；否则，设置步长和迭代次数j的值，并设j的初始值为1后执行步骤23；

步骤23，依照设定步长进行一次行走，并将所行走的路径点记录至记录列表中；

步骤24，基于人工势场法计算引力、斥力和合力以确定下一步移动方向；

步骤25，判断当前点到临时起点的欧式距离是否大于当前点到n+1点的欧式距离；是则，变更临时起点n＝n+1并执行步骤26；否则，迭代次数j加1后执行步骤23；

步骤26，判断当前点是否到达终点；是则，将记录列表里的所有坐标点采用最小二乘法拟合得到对应的路径函数并结束规划；否则，执行步骤21。

进一步地，步骤10中构建环境地图时，将地图作格栅化处理，将障碍物进行矩形化处理。

进一步地，步骤10中采用欧氏距离定义从起始点到达当前点的代价估计函数G(m)：

其中，M_x，M_y为当前点的横、纵坐标，S_x，S_y为起始点的横纵坐标。

进一步地，步骤10中采用欧氏距离定义从当前点到达目标点的代价估计函数H(m)：

其中，E_x，E_y为目标点的横、纵坐标。

进一步地，步骤14中建立next list以存放相邻8个节点与父节点的坐标关系以及代价F关系，以此依次进行循环，对当前父节点周围得8个节点进行扫描；

进一步地，步骤14中建立一个中转矩阵，里面依次存放当前点的周围节点、代价值F、代价值G和父节点的坐标。

进一步地，步骤24的计算步骤如下：

步骤24-1，计算引力势场：设机器人所在位置为(X,Y)^T，X为机器人位置向量，X_d为目标点位置向量，则引力势场模型为：

其中，(X-X_d)²为机器人到目标点的相对距离，并定义(X-X_d)²＝|(X-X_d)²|+|(Y-Y_d)²|，K为引力势场常量；

步骤24-2，计算引力：机器人所受引力为引力势场负梯度方向，即：

F_aat(X)＝-grad(U_att)＝-K(X-X_d)²

其中，F_aat(X)为机器人所受引力；

步骤24-3，计算斥力势场模型为：

式中：γ(X,X_g)表示机器人到障碍物的欧几里得距离，K_r表示斥力场常数；γ₀为以障碍物为中心的斥力场影响范围；γ为机器人与障碍物的距离，

具体地，当γ≥γ₀时，机器人的距离γ与所受斥力F_rep成线性关系；而当γ≥γ₀时，由于机>器人不再受斥力场的影响，其所受斥力F_rep将变为0。

步骤24-4，计算斥力：机器人所受斥力为斥力势场负梯度方向，即为：

步骤24-5，计算合力：合力计算公式如下：

其中，n为所在位置所受斥力影响的障碍物个数。

进一步地，步骤26中最小二乘法拟合曲线的步骤如下：

步骤26-1，设定回归值与实际值方差表达式：

φ(x)＝a₀+a₁x+a₂x²+…+a_kx^k

其中，a是多项式的待定系数，n是多项式的最高阶次(在此处里最佳阶次为6)，x为路径点的横坐标。

步骤26-2，计算得到各点到曲线的距离之和为：

步骤26-3，对回归方程中的待定系数求偏导得到：

步骤26-4，通过矩阵运算得到系数矩阵A以得到了拟合曲线，矩阵运算如下

其中，A就是对应的系数矩阵，X*为横坐标矩阵的转置，Y为纵坐标矩阵。

本发明采用以上技术方案，先是在传统的A*算法的基础上进行改进。前期将障碍物进行矩形化处理，无障碍区域应用传统A*算法寻路，在遇到障碍物时，采用优化后的A*算法，在初次完成路径规划后，按照所选取的路径点，加到路径列表进行计算，然后应用人工势场法进行路径平滑处理。因为前期已经用优化后的A*算法寻路完毕，所以也避免了人工势场法陷入局部最优的问题。为此，这样不但能够保证搜索到的路径具有平滑性和实时避障能力，还在确保全局最优的基础上提高了寻路效率。

附图说明

以下结合附图和具体实施方式对本发明做进一步详细说明；

图1为传统A*算法寻路示意图；

图2为本发明一种基于优化A-STAR算法的全局动态平滑路径规划方法的路径规划流程示意图；

图3为本发明一种基于优化A-STAR算法的全局动态平滑路径规划方法的路径平滑流程示意图；

图4为物理学引力场模型示意图；

图5为人工势场模型示意图；

图6为本发明一种基于优化A-STAR算法的全局动态平滑路径规划方法的寻路效果示意图；

图7为人工势场法路径平滑示意图；

图8为本发明最小二乘法拟合路径示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图对本申请实施例中的技术方案进行清楚、完整地描述。

路径规划是自主移动机器人领域的关键技术之一，针对这一技术目前已经开展了大量的研究工作。自主移动机器人的路径规划可以分为静态路径规划和动态路径规划。其中A*算法、Dijkstra算法等目前主要应用于已知的静态场景中，而人工势场法、D*算法、动态窗口法等则大多应用于环境未知动态场景中。同时，随着近几年人工智能的迅速发展，蚁群算法、遗传算法、神经网络算法等智能算法也开始广泛的应用于移动机器人的路径规划中。

如图1所示，A*算法是一种全局路径规划算法,最开始是由P.E.Hart、N.J.Nilsson和B.Rapheal提出的一种新的启发式算法，是为了弥补Dijkstra算法和早期的搜索算法的不足所提出的。A*算法从起点开始，使用定义的启发式函数计算领域节点的代价估计值，并选择代价估计值最小的节点作为其扩展方向，重复这一过程直到扩展到目标点，生成最终路径。

人工势场法是将移动机器人所在的环境模拟成物理学中的“引力”，称为虚拟势场，如图4或5所示，正电荷相当于障碍物，对小车擦产生斥力，负电荷相当于目标点，对小车产生引力。引力场和斥力场经过综合距离等因素，经过运算得到移动方向。移动机器人借助于引力场的作用，逐渐向目标点靠近，与此同时，斥力场作用于移动机器人上，使其巧妙地绕过障碍物。

如图2至图8之一所示，本发明公开了一种基于优化A-STAR算法的全局动态平滑路径规划方法，其包括路径规划部分和路径平滑处理部分，具体步骤如下：

路径规划部分：

步骤10，构建环境地图,将地图上障碍物区域内的栅格上标注为-1并将-1的坐标放入障碍物列表，并基于起始点和终点构建启发式函数：

F(m)＝G(m)+H(m)

其中，Fm表示起始点到任意节点的估价值，Gm表示起始点到某一个节点的实际代价，Hm表示起始该点到终点的估计代价值；

具体地，在前期初始化的过程中，会将障碍物节点设置成-1，并且将所有障碍物节点放入数组，所以，机器人进行寻路的过程中，当前点在对周围的8个节点进行搜索时，会先对8个节点依次和障碍物节点进行对比，检测是否遇到了障碍物。

步骤11，分别创建一个开启列表和关闭列表，并将起始点添加到开启列表中，开启列表用来存放当前点的相邻节点，关闭列表用来存放开启列表中F值最小的节点，即下一步的父节点；

步骤12，检查开启列表是否为空列表；是则，结束寻路并进入路径平滑部分；否则，继续执行步骤13；

步骤13：判断是否遇到障碍物；是则，将当前点的8个相邻节点按照顺序分别放进开放列表中，将对应的代价值F数据分别进行标记并执行步骤14；否则，执行步骤15；

假设当前点的坐标是(x，y)则周围的节点为：(x-1，y+1)，(x-1，y)，(x-1，y-1)，(x，y+1)，(x，y-1)，(x+1，y+1)，(x+1，y)，(x+1，y-1)。

步骤14，将开放列表的元素分别与障碍物列表的元素进行对比，当确定有相同的点时，将对应的点及其所有数据都设置成无穷大；

步骤15：对开启列表中以F值的大小进行排序以得到F值最小的点，将F值最小点放入关闭列表中，然后在开放列表里面删除对应的F值最小点；

步骤16：判断目标点是否到在关闭列表中；是则，结束寻路并进入路径平滑部分；否则，执行步骤17；

步骤17，判断当前点的相邻点是否在关闭列表；是则，忽略此当前点的相邻点并执行步骤12；否则，执行步骤18；

具体地，假设当前点的坐标是(x，y)则周围的节点为：(x-1，y+1)，(x-1，y)，(x-1，y-1)，(x，y+1)，(x，y-1)，(x+1，y+1)，(x+1，y)，(x+1，y-1)。

步骤18，判断当前点的相邻点是否在开放列表里；是则，用当前点的G值重新检查当前点的相邻点，并更新G、F值和父节点后执行步骤12；否则，将当前点的相邻点添加到开放列表里面，并重新计算F和G的值，把当前点作为父节点执行步骤12；

进一步地，步骤10中构建环境地图时，将地图作格栅化处理，将障碍物进行矩形化处理。

进一步地，步骤10中采用欧氏距离定义从起始点到达当前点的代价估计函数G(m)：

其中，M_x，M_y为当前点的横、纵坐标，S_x，S_y为起始点的横纵坐标。

进一步地，步骤10中采用欧氏距离定义从当前点到达目标点的代价估计函数H(m)：

其中，E_x，E_y为目标点的横、纵坐标。

进一步地，步骤14中建立next list以存放相邻8个节点与父节点的坐标关系以及代价F关系，以此依次进行循环，对当前父节点周围得8个节点进行扫描；

进一步地，步骤14中建立一个中转矩阵，里面依次存放当前点的周围节点、代价值F、代价值G和父节点的坐标。

路径平滑部分：路径平滑策略的核心思想是：因为在前期进行寻路的过程中本发明对路径点进行了标注，在传统的人工势场法的应用中，只是将最终目标点作为唯一目标点，在路径平滑的过程中，本发明要求按照前期规划的路径进行行走，人工势场法在局部进行使用，因此从第三个关键点开始，直到最后点都会依次成为起点和目标点，也极大的避免了人工势场法容易陷入局部最优的问题，同时也为小车留出了足够的行走空间。因为使用的是栅格地图，所以平滑优化后得到的路径点是坐标集合，所以采用最小二乘法进行路径拟合，路径平滑效果如图7和图8所示：

路径平滑部分的具体步骤如下：

步骤20，获取路径列表并计算获取路径列表的行数，并设置n的初始值为1；

步骤21，将临时起点坐标放入路径列表，并将临时起点的行数赋值为n，将第n+2行作为临时终点；

临时起点就是人工势场法寻路的时候的起点，如何确定的就是以n为准，n为几的时候，对应的路径点列表(path)的第n行坐标就是临时起点。

步骤22，判断n的值是否等于路径列表的行数-1【length(path)-1】；是则，将临时起点设为n行点，临时终点设为终点，重置步长和迭代次数后执行步骤23；否则，设置步长和迭代次数j的值，并设j的初始值为1后执行步骤23；

步骤23，依照设定步长进行一次行走，并将所行走的路径点记录至记录列表中；

步骤24，基于人工势场法计算引力、斥力和合力以确定下一步移动的方向；

步骤25，判断当前点到临时起点的欧式距离是否大于当前点到n+1点的欧式距离；是则，变更临时起点n＝n+1并执行步骤26；否则，迭代次数j加1后执行步骤23；

步骤26，判断当前点是否到达终点；是则，将记录列表里的所有坐标点采用最小二乘法拟合得到对应的路径函数并结束规划；否则，执行步骤21。

进一步地，因为在寻路的过程中，已经将路径点记录在路径列表(path list)中，为此，人工势场法的优化处理在此基础上进行。人工势场法主要分为障碍物的斥力Fr，方向由障碍物指向小车；目标点的引力Fa，方向是由小车指向目标点。

步骤24的计算步骤如下：

步骤24-1，计算引力势场：引力势场随着小车与目标点的距离变化而变化，引力场与移动车辆和目标点之间的距离成正比，引力势场的计算公式如下：

式中，U_att(x)为目标对小车产生的引力势场，K为引力场的作用系数，(P_S,P_E)为起点到终点的欧式距离；

步骤24-2，基于引力势场计算引力，引力为引力势场的负梯度，引力的计算公式如下：

步骤24-3，计算获取斥力场的大小，斥力场的大小与小车和目标点的距离成反比，斥力场的计算公式如下：

公中，U_rep(x)为障碍物的斥力场，K_rep为斥力场作用系数，ρ(p,p_obs)为小车与障碍物之间的欧式距离，p₀为障碍物斥力的临界距离，当小车与障碍物的距离大于p₀时，此时小车受到的斥力为零；

步骤24-4，基于斥力场计算斥力，斥力为斥力场的负梯度，斥力的计算公式如下：

步骤24-5，当地图中存在N个障碍物时，计算所受的合力为：

式中，F_sum(x)为斥力和引力的合力，

为斥力的集合。

进一步地，步骤26中最小二乘法拟合曲线的步骤如下：

步骤26-1，设定回归值与实际值方差表达式：

φ(x)＝a₀+a₁x+a₂x²+…+a_kx^k

其中，a是多项式的待定系数，n是多项式的最高阶次(在此处里最佳阶次为6)，x为路径点的横坐标。

步骤26-2，计算得到各点到曲线的距离之和为：

步骤26-3，对回归方程中的待定系数求偏导得到：

步骤26-4，通过矩阵运算得到系数矩阵A以得到了拟合曲线，矩阵运算如下

其中，A就是对应的系数矩阵，X*为横坐标矩阵的转置，Y为纵坐标矩阵。

由于优化后的A*算法是沿着障碍物走的，所以对小车留出来的行走空间比较小。人工势场法一方面是做路径平滑处理，另一方面是因为障碍物对小车有一个斥力，将小车适当的推离障碍物，为小车留出足够的行走空间。因此，人工势场法有了两个作用，一个是让路径转折没有那么剧烈，另一个就是让小车的通过性更好。此外由于前期用的栅格地图寻路，所以导致路径是由坐标点形成，所以用最小二乘法进行路径拟合。

融合人工势场法和优化A*法的全局动态路径规划方法因其不仅具备实时避障能力，还能保证规划的路径考虑全局最优性，增加平滑度，能够应用于智能物流、智能家居、海上搜救、巡航等场景下的自主移动机器人路径规划。

本发明采用以上技术方案，先是在传统的A*算法的基础上进行改进。前期将障碍物进行矩形化处理，无障碍区域应用传统A*算法寻路，在遇到障碍物时，采用优化后的A*算法，在初次完成路径规划后，按照所选取的路径点，加到路径列表进行计算，然后应用人工势场法进行路径平滑处理。因为前期已经用优化后的A*算法寻路完毕，所以也避免了人工势场法陷入局部最优的问题。为此，这样不但能够保证搜索到的路径具有平滑性和实时避障能力，还在确保全局最优的基础上提高了寻路效率。

显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

Claims (8)

1.一种基于优化A-STAR算法的全局动态平滑路径规划方法，其特征在于：其包括路径规划部分和路径平滑处理部分，具体步骤如下：

路径规划部分：

步骤10，构建环境地图,将地图上障碍物区域内的栅格上标注为-1并将-1的坐标放入障碍物列表，并基于起始点和终点构建启发式函数：

F(m)＝G(m)+H(m)

其中，Fm表示起始点到任意节点的估价值，Gm表示起始点到某一个节点的实际代价，Hm表示起始该点到终点的估计代价值；

步骤11，分别创建一个开启列表和关闭列表，并将起始点添加到开启列表中，开启列表用来存放当前点的相邻节点，关闭列表用来存放开启列表中F值最小的节点，即下一步的父节点；

步骤12，检查开启列表是否为空列表；是则，结束寻路并进入路径平滑部分；否则，继续执行步骤13；

步骤13：判断是否遇到障碍物；是则，将当前点的8个相邻节点按照顺序分别放进开放列表中，将对应的代价值F数据分别进行标记并执行步骤14；否则，执行步骤15；

步骤14，将开放列表的元素分别与障碍物列表的元素进行对比，当确定有相同的点时，将对应的点及其所有数据都设置成无穷大；

步骤15：对开启列表中以F值的大小进行排序以得到F值最小的点，将F值最小点放入关闭列表中，然后在开放列表里面删除对应的F值最小点；

步骤16：判断目标点是否到在关闭列表中；是则，结束寻路并进入路径平滑部分；否则，执行步骤17；

步骤17，判断当前点的相邻点是否在关闭列表；是则，忽略此当前点的相邻点并执行步骤12；否则，执行步骤18；

步骤18，判断当前点的相邻点是否在开放列表里；是则，用当前点的G值重新检查当前点的相邻点，并更新G、F值和父节点后执行步骤12；否则，将当前点的相邻点添加到开放列表里面，并重新计算F和G的值，把当前点作为父节点执行步骤12；

路径平滑部分：

步骤20，获取路径列表并计算获取路径列表的行数，并设置n的初始值为1，n表示路径列表的第几行的坐标；

步骤21，将临时起点坐标放入路径列表，并将临时起点的行数赋值为n，将第n+2行作为临时终点；其中，临时起点就是人工势场法寻路的起点；

步骤22，判断n的值是否等于路径列表的行数-1，即【length(path)-1】；是则，将临时起点设为n行点，临时终点设为终点，重置步长和迭代次数后执行步骤23；否则，设置步长和迭代次数j的值，并设j的初始值为1后执行步骤23；

步骤23，依照设定步长进行一次行走，并将所行走的路径点记录至记录列表中；

步骤24，基于人工势场法计算引力、斥力和合力以确定下一步移动的方向；

步骤25，判断当前点到临时起点的欧式距离是否大于当前点到n+1点的欧式距离；是则，变更临时起点n＝n+1并执行步骤26；否则，迭代次数j加1后执行步骤23；

步骤26，判断当前点是否到达终点；是则，将记录列表里的所有坐标点采用最小二乘法拟合得到对应的路径函数并结束规划；否则，执行步骤21。

2.根据权利要求1所述的一种基于优化A-STAR算法的全局动态平滑路径规划方法，其特征在于：步骤10中构建环境地图时，将地图作格栅化处理，将障碍物进行矩形化处理。

3.根据权利要求1所述的一种基于优化A-STAR算法的全局动态平滑路径规划方法，其特征在于：步骤10中采用欧氏距离定义从起始点到达当前点的代价估计函数G(m)：

其中，M_x，M_y为当前点的横、纵坐标，S_x，S_y为起始点的横纵坐标。

4.根据权利要求1所述的一种基于优化A-STAR算法的全局动态平滑路径规划方法，其特征在于：步骤10中采用欧氏距离定义从当前点到达目标点的代价估计函数H(m)：

其中，E_x，E_y为目标点的横、纵坐标。

5.根据权利要求1所述的一种基于优化A-STAR算法的全局动态平滑路径规划方法，其特征在于：步骤14中建立next list以存放相邻8个节点与父节点的坐标关系以及代价F关系，以此依次进行循环，对当前父节点周围得8个节点进行扫描。

6.根据权利要求1所述的一种基于优化A-STAR算法的全局动态平滑路径规划方法，其特征在于：步骤14中建立一个中转矩阵，里面依次存放当前点的周围节点、代价值F、代价值G和父节点的坐标。

7.根据权利要求1所述的一种基于优化A-STAR算法的全局动态平滑路径规划方法，其特征在于：步骤24的计算步骤如下：

步骤24-1，计算引力势场：设机器人所在位置为(X,Y)^T，X为机器人位置向量，X_d为目标点位置向量，则引力势场模型为：

其中，(X-X_d)²为机器人到目标点的相对距离，并定义(X-X_d)²＝|(X-X_d)²|+|(Y-Y_d)²|，K为引力势场常量；

步骤24-2，计算引力：机器人所受引力为引力势场负梯度方向，即：

F_aat(X)＝-grad(U_att)＝-K(X-X_d)²

其中，F_aat(X)为机器人所受引力；

步骤24-3，计算斥力势场模型为：

式中：γ(X,X_g)表示机器人到障碍物的欧几里得距离，K_r表示斥力场常数；γ₀为以障碍物为中心的斥力场影响范围；γ为机器人与障碍物的距离，

步骤24-4，计算斥力：机器人所受斥力为斥力势场负梯度方向，即为：

步骤24-5，计算合力：合力计算公式如下：

其中，n为所在位置所受斥力影响的障碍物个数。

8.根据权利要求1所述的一种基于优化A-STAR算法的全局动态平滑路径规划方法，其特征在于：步骤26中最小二乘法拟合曲线的步骤如下：

步骤26-1，设定回归值与实际值方差表达式：

φ(x)＝a₀+a₁x+a₂x²+…+a_kx^k

其中，a是多项式的待定系数，n是多项式的最高阶次(在此处里最佳阶次为6)，x为路径点的横坐标。

步骤26-2，计算得到各点到曲线的距离之和为：

步骤26-3，对回归方程中的待定系数求偏导得到：

步骤26-4，通过矩阵运算得到系数矩阵A以得到了拟合曲线，矩阵运算如下

其中，A表示对应的系数矩阵，X*为横坐标矩阵的转置，Y为纵坐标矩阵。

Images