CN114442628A - 基于人工势场法的移动机器人路径规划方法、装置及系统 - Google Patents

基于人工势场法的移动机器人路径规划方法、装置及系统 Download PDF

Info

Publication number
CN114442628A
CN114442628A CN202210079534.2A CN202210079534A CN114442628A CN 114442628 A CN114442628 A CN 114442628A CN 202210079534 A CN202210079534 A CN 202210079534A CN 114442628 A CN114442628 A CN 114442628A
Authority
CN
China
Prior art keywords
robot
obstacle
current position
point
force
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210079534.2A
Other languages
English (en)
Other versions
CN114442628B (zh
Inventor
郭铁铮
王杰
陈巍
杨刚
陈国军
梁浩南
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing Institute of Technology
Original Assignee
Nanjing Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing Institute of Technology filed Critical Nanjing Institute of Technology
Priority to CN202210079534.2A priority Critical patent/CN114442628B/zh
Publication of CN114442628A publication Critical patent/CN114442628A/zh
Application granted granted Critical
Publication of CN114442628B publication Critical patent/CN114442628B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Optics & Photonics (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种基于人工势场法的移动机器人路径规划方法、装置及系统，所述方法包括基于机器人的当前位置和目标位置，以及障碍物信息，计算出机器人当前位置的引力和斥力；基于所述引力和斥力，计算出机器人当前位置受到的合力；当基于机器人当前位置受到的合力判断出机器人陷入局部最优状态时，利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，完成机器人路径规划。本发明能够实现当机器人陷入局部最优情况时，建立规划导引点，利用导引点的额外吸引力，逃离局部极小点，解决局部最优问题。

Description

基于人工势场法的移动机器人路径规划方法、装置及系统
技术领域
本发明属于路径规划技术领域，具体涉及一种基于人工势场法的移动机器人路径规划方法、装置及系统。
背景技术
近年来，机器人进入到一个高速发展时期，全球机器人产业正在发生深刻的变化，产业重心开始向非制造业转移，因此，智能机器人成为全球研究的热点。随着机器人技术的不断发展，智能机器人已经广泛应用于人类生产和生活的各个方面。从简单的扫地机器人到复杂的无人汽车，环境越来越复杂，结构越来越多样化，功能也越来越强大。移动机器人应用前景广阔，应用领域越来越多样化，不仅被用在物流行业，也同时用在服务产业，来帮助人类更好的实现服务工作。运动规划技术作为移动机器人智能自主作业中的必要环节得到广泛关注。
目前，常见的运动规划方法主要有人工势场法、粒子群算法、模糊算法、蚁群算法、神经网络等智能算法，其中，人工势场法具有实时性强、结构简单和反应速度快等突出优点，成为机器人路径规划中应用最多的一种方法。尽管优点突出，但人工势场法也存在规划速度慢和易陷入局部最优等缺陷。在实际测试中，移动机器人遇到如下问题：(1)人工势场法采用等步长规划方法，移动机器人在多障碍物环境下无法完全适用，规划速度慢、算法效率有待提高。(2)当机器人受到合力为零时，规划算法失去下一步前进方向，系统陷入局部最优状态。因此需要一种改进算法解决规划速度慢和易陷入局部最优问题。
发明内容
针对上述问题，本发明提出一种基于人工势场法的移动机器人路径规划方法、装置及系统，当移动机器人陷入局部最优情况时，建立规划导引点，利用导引点的额外吸引力，逃离局部极小点，解决局部最优问题。
为了实现上述技术目的，达到上述技术效果，本发明通过以下技术方案实现：
第一方面，本发明提供了一种基于人工势场法的移动机器人路径规划方法，包括：
基于机器人的当前位置和目标位置，以及障碍物信息，计算出机器人当前位置的引力和斥力；
基于所述引力和斥力，计算出机器人当前位置受到的合力；
当基于机器人当前位置受到的合力判断出机器人陷入局部最优状态时，利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，完成机器人路径规划。
可选地，所述使得移动机器人脱离局部最优状态步骤之后，还包括：
根据预设的自适应调节步长公式，计算出移动机器人前进的步长，直至移动机器人到达目标位置。
可选地，所述自适应调节步长公式的表达式为：
Figure BDA0003485350510000021
式中，step为自适应调节步长，ρ为步长增益系数，n为当前产生斥力作用的障碍物数量，qobs为机器人当前位置与障碍物的距离，dobs为障碍物斥力场作用距离，c为当前路径规划循环次数，cmax为路径规划循环次数上限。
可选地，所述机器人当前位置受到的合力的计算公式为：
Figure BDA0003485350510000022
Figure BDA0003485350510000023
Figure BDA0003485350510000024
式中，F为机器人当前位置受到的合力，Fatt为机器人当前位置的引力，Frep为机器人当前位置的斥力，n为当前产生斥力作用的障碍物数量，qobs为机器人当前位置与障碍物的距离，dobs为障碍物斥力场作用距离，μ为引力增益系数，qg为机器人当前位置与目标位置的距离，
Figure BDA0003485350510000025
为机器人当前位置指向目标位置的单位向量，σ为斥力增益系数，
Figure BDA0003485350510000026
为机器人当前位置指向障碍物的单位向量。
可选地，所述利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，包括以下步骤：
当判断出机器人处于单障碍物环境时，以障碍物为圆心，以安全距离d为半径作圆，对圆心作障碍物和机器人连线的垂直线，交圆于G点，认定G点为导引点，G点提供额外的吸引力F'att，使得机器人逃离局部最优点。
可选地，所述利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，包括以下步骤：
当判断出机器人处于多障碍物环境时，两两障碍物之间计算障碍物避碰距离k；
判断当前路径中k值是否大于机器人车身宽度；
若条件成立，则计算出两障碍物坐标中点作为导引点B，通过导引点B的额外吸引力F'att，使得机器人逃离局部最优点；
若条件不成立，则以最外围障碍物为圆心，以安全距离d为半径作圆，分别对各圆心作障碍物和机器人连线的垂直线，各垂直线交对应圆于G点，选择距离机器人最近的G点为作为导引点B，通过引导点B提供额外的吸引力F'att，使得机器人逃离局部最优点。
可选地，所述G点的计算公式为：
Figure BDA0003485350510000031
式中，O(x,y)为障碍物坐标，G(x,y)为导引点坐标，P(x,y)为机器人当前位置坐标，Gx为导引点G的x坐标，Gy为导引点G的y坐标，Ox为障碍物坐标O的x坐标，Oy为障碍物坐标O的y坐标，d为障碍物安全距离，Px为机器人当前位置P点的x坐标，Py为机器人当前位置P点的y坐标。
第二方面，本发明提供了一种基于人工势场法的移动机器人路径规划装置，包括：
第一计算模块，用于基于机器人的当前位置和目标位置，以及障碍物信息，计算出机器人当前位置的引力和斥力；
第二计算模块，用于基于所述引力和斥力，计算出机器人当前位置受到的合力；
规划模块，用于当基于机器人当前位置受到的合力判断出机器人陷入局部最优状态时，利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，完成机器人路径规划。
可选地，所述使得移动机器人脱离局部最优状态步骤之后还包括：
根据预设的自适应调节步长公式，计算出移动机器人前进的步长，直至移动机器人到达目标位置。
第三方面，本发明提供了一种基于人工势场法的移动机器人路径规划系统，包括：
处理器；
存储器，其上存储有可在所述处理器上运行的计算机程序；
其中，所述计算机程序被所述处理器执行时实现如第一方面中任一项所述的方法。
与现有技术相比，本发明的有益效果：
本发明提出一种基于人工势场法的移动机器人路径规划方法、装置及系统，当移动机器人陷入局部最优情况时，建立规划导引点，利用导引点的额外吸引力，逃离局部极小点，解决局部最优问题。
本发明还能够实现移动机器人准确规避障碍物，更快到地达指定位置。
附图说明
为了使本发明的内容更容易被清楚地理解，下面根据具体实施例并结合附图，对本发明作进一步详细的说明，其中：
图1为本发明一种实施例中单障碍物三角导航方法示意图；
图2是本发明一种实施例中多障碍物三角导航方法示意图；
图3是本发明一种实施例中等步长路径规划；
图4是本发明一种实施例中自适应步长路径规划；
图5是本发明一种实施例中单障碍物局部最优情况；
图6是本发明一种实施例中单障碍物局部最优势场分布；
图7是本发明一种实施例中单障碍物三角导航法；
图8是本发明一种实施例中多障碍物局部最优情况；
图9是本发明一种实施例中多障碍物局部最优势场分布；
图10是本发明一种实施例中多障碍物三角导航法；
图11是本发明一种实施例中路径规划流程图；
图12是本发明一种实施例中实际测试环境
图13是本发明一种实施例中实验环境栅格地图；
图14是本发明一种实施例中地图规划路径；
图15是本发明一种实施例中小车实验场地测试。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明的保护范围。
下面结合附图对本发明的应用原理作详细的描述。
实施例1
在机器人运行过程中，主要有两种情况出现局部最优问题。(1)由于障碍物势场作用范围关系，可能出现只有单个障碍物势场产生斥力作用，且斥力方向与引力方向共线，导致机器人在当前位置徘徊，当合力为零时，停止运动。(2)机器人遇到多个障碍物作用时，产生的合斥力与引力共线时，导致机器人在当前位置徘徊，当合力为零时，停止运动。
针对上述两种情况，本发明实施例中提出三角导航法来解决局部最优问题，下面以两种情况分别进行讨论。
情况(一)单障碍物
机器人路径规划运动方向由合力提供，当斥力与引力共线，机器人在当前位置来回振荡或者停止运动，需要提供额外力使机器人逃离局部最优点。为此，本发明实施例中提出以三角导航法确定导引点，利用导引点提供吸引力，引导机器人逃离局部最优点，图1为单障碍物三角导航法。
如图1所示，O(x,y)为障碍物坐标，G(x,y)为导引点坐标，P(x,y)为机器人当前位置坐标，以障碍物为圆心，以安全距离d为半径作圆，对圆心作障碍物和机器人连线的垂直线交圆于G点，将G点作为导引点，G点提供额外的吸引力F'att，帮助机器人逃离局部最优点。
利用三角导航法计算导引点公式如下：
Figure BDA0003485350510000051
式中，O(x,y)为障碍物坐标，G(x,y)为导引点坐标，P(x,y)为机器人当前位置坐标，Gx为导引点G的x坐标，Gy为导引点G的y坐标，Ox为障碍物坐标O的x坐标，Oy为障碍物坐标O的y坐标，d为障碍物安全距离，Px为机器人当前位置P点的x坐标，Py为机器人当前位置P点的y坐标。
由此可见，根据已知的障碍物坐标O(x,y)和机器人当前位置坐标P(x,y)，可根据三角导航法求得导引点G(x,y)。
情况(二)多障碍物
机器人遇到多个障碍物且机器人合力为零情况，机器人陷入局部最优，出现来回振荡或停止现象，需要提供额外力使采摘机器人逃离局部最优。为此，本发明实施例中提出两种解决方法，第一种是从多障碍物中间B点通过，第二种是从多障碍物最外围G点通过，图2为两种方法的示意图。
如图2所示，O(x,y)、O1(x,y)为障碍物坐标，B为O、O1两障碍物坐标中点，第一种方法，通过确定障碍物避碰距离k(即障碍物与障碍物之间避碰长度)是否大于机器人宽度，判断机器人能否安全通过，若条件成立(即机器人能安全通过)，计算出两障碍物坐标中点作为导引点B，通过导引点B的额外吸引力逃离局部最优点。若第一种方法不可行，则采取第二种方法，即三角导航法，在多障碍物最外围确定导引点G，通过G点产生的吸引力逃离局部最优点，具体地，分别以最外围障碍物为圆心，以安全距离d为半径作圆，对圆心作障碍物和机器人连线的垂直线，各垂直线交对应圆于G点，选择距离机器人最近的G点为作为导引点B，通过引导点B提供额外的吸引力F'att，使得机器人逃离局部最优点。根据已知的障碍物坐标O(x,y)和机器人当前位置坐标P(x,y)，可由三角导航法求得导引点G(x,y)坐标。需要注意的是，在多障碍物情况下计算导引点G(x,y)坐标时，障碍物坐标O(x,y)为与距离机器人最近的G点对应的障碍物的坐标。
经典人工势场法采用等步长路径规划方法，存在算法规划效率低、运算时间长等问题，在复杂环境下无法完全适用。从实际测试看，在少障碍物简单环境中，可适当增大步长，减少计算的路径点，增加机器人的移动速度，从而达到提高路径规划效率和降低系统运算时间的目的。在多障碍物环境中，通过适当减小步长，降低机器人移动速度，从而减低碰撞概率。为此，本发明中基于障碍物数量、机器人与障碍物距离和迭代次数这三因素，提出自适应调节步长法，使得步长step与障碍物数量成反比，障碍物数量越多，步长越小；与障碍物距离成正比，机器人距离障碍物越近，步长越小，碰撞风险越低；与迭代次数成正比，迭代次数越多，步长越大，目标收敛速度越快。
综上所述，本发明实施例中提供了一种基于人工势场法的移动机器人路径规划方法，包括以下步骤：
基于机器人的当前位置和目标位置，以及障碍物信息，计算出机器人当前位置的引力和斥力；
基于所述引力和斥力，计算出机器人当前位置受到的合力；
当基于机器人当前位置受到的合力判断出机器人陷入局部最优状态时，利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态；
根据预设的自适应调节步长公式，计算出移动机器人前进的步长，直至移动机器人到达目标位置。
在本发明实施例的一种具体实施方式中，所述自适应调节步长公式的表达式为：
Figure BDA0003485350510000061
式中，step为自适应调节步长，ρ为步长增益系数，n为当前产生斥力作用的障碍物数量，qobs为机器人当前位置与障碍物的距离，dobs为障碍物斥力场作用距离，c为当前路径规划程序循环次数，cmax为路径规划程序循环次数上限。可见，自适应调节步长step与障碍物数量成反比，障碍物数量越多，步长越小；与障碍物距离成正比，机器人距离障碍物越近，步长越小，碰撞风险越低；与迭代次数成正比，迭代次数越多，步长越大，目标收敛速度越快。
在本发明实施例的一种具体实施方式中，所述机器人当前位置受到的合力的计算公式为：
Figure BDA0003485350510000071
Figure BDA0003485350510000072
Figure BDA0003485350510000073
式中，F为机器人当前位置受到的合力，Fatt为机器人当前位置的引力，Frep为机器人当前位置的斥力，n为当前产生斥力作用的障碍物数量，qobs为机器人当前位置与障碍物的距离，dobs为障碍物斥力场作用距离，μ为引力增益系数，qg为机器人当前位置与目标位置的距离，
Figure BDA0003485350510000074
为机器人当前位置指向目标位置的单位向量，σ为斥力增益系数，
Figure BDA0003485350510000075
为机器人当前位置指向障碍物的单位向量。
下面结合图11对本发明实施例中的方法进行详细说明。
步骤1：初始化机器人状态参数，所述状态参数包括引力系数、斥力系数、障碍物作用距离；
步骤2：机器人使用陀螺仪、里程计和激光雷达多信息融合技术扫描实验环境，获取环境信息，所述环境信息包括障碍物位置信息、障碍物数量和实验环境边界信息，建立栅格地图。实验环境如图12所示，图12中纸箱用来模拟障碍物。栅格地图如图13所示，图13中黑色方块为激光雷达获取障碍物信息；
步骤3：设置初始位置和目标位置，所述目标位置(即目标点)在应用过程中，用于被看作引力点，本发明实施例中提及的引力不是目标位置会产生引力，而是在人工势场法原理里目标点产生的虚拟力，获取机器人处于栅格地图中当前位置坐标；
步骤4:计算机器人当前位置的引力Fatt，所述引力Fatt计算公式如下：
Figure BDA0003485350510000076
其中，Fatt为目标位置对机器人的引力，μ为引力增益系数，qg为机器人当前位置与目标位置的距离，
Figure BDA0003485350510000077
为机器人当前位置指向目标位置单位向量；
步骤5：在人工势场原理中，障碍物斥力有作用距离dobs，在作用距离内的障碍物产生斥力作用，为此，本发明实施例中，需要计算机器人当前位置的斥力Frep，在障碍物安全距离dobs以外不受障碍物的斥力作用，所述斥力Frep计算公式如下：
Figure BDA0003485350510000081
其中，Frep为障碍物对机器人的斥力，σ为斥力增益系数，
Figure BDA0003485350510000082
为机器人当前位置指向障碍物单位向量；
步骤6：计算机器人当前位置受到合力大小，合力计算公式如下：
Figure BDA0003485350510000083
其中，n为障碍物数量；
步骤7：判断机器人的合力是否为零，若为零，则进入步骤8。若合力不为零，则进入步骤9；
步骤8：此时，机器人陷入局部最优状态，所受到的合力为0或接近于0，导致机器人停止前行或一定范围内徘徊。如图2所示，此时需要给机器人提供一个额外的逃逸力，脱离局部极小点。利用三角导航法计算导引点位置，产生逃逸力，脱离局部最优状态，进入步骤9，调节机器人移动速度。
所述导引点位置的计算过程具体为：
当判断出机器人处于单障碍物环境时，以障碍物为圆心，以安全距离d为半径作圆，对圆心作障碍物和机器人连线的垂直线，交圆于G点，认定G点为导引点，G点提供额外的吸引力F'att，使得机器人逃离局部最优点。
所述G点的计算公式为：
Figure BDA0003485350510000084
式中，O(x,y)为障碍物坐标，G(x,y)为导引点坐标，P(x,y)为机器人当前位置坐标。
当判断出机器人处于多障碍物环境时，两两障碍物之间计算障碍物避碰距离k。机器人判断当前路径中k值是否大于机器人车身宽度，若条件成立，则计算出可行路径下导引点B，通过导引点B的额外吸引力逃离局部最优点；
若条件不成立，则以最外围障碍物为圆心，以安全距离d为半径作圆，对圆心作障碍物和机器人连线的垂直线，各垂直线交对应圆于G点(如图2中的G1和G2点)，选择距离机器人最近的G点为导引点，引导点提供额外的吸引力F'att，使得机器人逃离局部最优点；
步骤9：机器人根据自适应调节步长公式，计算小车前进的步长，根据合力方向驱动机器人运动，进入步骤10，自适应调节步长公式如下：
Figure BDA0003485350510000091
步骤10：判断小车是否到达目标点，若到达目标点，小车停止运动。若未到达目标点，进入步骤6。
为了验证本发明实施例中路径规划方法的有效性，在本发明实施例的一种具体实施方式中，搭建实验场地，使用机器人进行测试。
首先，根据激光雷达实时扫描数据，获取机器人周边障碍物数量和位置，自动调整机器人的前进步长。图3为经典路径规划，设置起点(0,0)，终点(12,12)，共有12个随机布局的障碍物，全程采用等步长路径规划。图4为本发明实施例中提出的路径规划方法，与图3设置相同，当机器人附近障碍物较多时，机器人规划步长减小，即行驶速度下降，降低了机身碰撞概率。反之，当附近障碍物较少时，机器人规划步长增加，即提高行驶速度，减少系统规划次数，提高路径规划效率。经典势场法完成路径规划所需仿真时间为0.1367s，迭代次数为186。本发明实施例中提出的方法进行路径规划所需时间为0.0765s，迭代次数为137。显然，本发明实施例中提出的方法较经典算法性能有很大提升，系统规划效率提高了44.1％，目标收敛速度提升了26.3％。
当遇到单个障碍物合力为零情况，在采用经典人工势场法时，机器人失去势场力牵引丢失下一步规划路径，机器人停止运动，仿真结果如图5所示，图5中设置障碍物位置(6，6)。图6为势场分布中机器人陷于局部极小点，极小点坐标为(5.62,5.62)，此时机器人具有的势能为115.2J，机器人在极小值点停止运动。
在采用本发明实施例中提出的方法时，机器人陷入局部最优状态时启动三角导航法，生成导引点，导引点提供引力方向，帮助机器人逃离局部最优状态，仿真结果如图7所示。图7中星型为导引点，黑色圆为障碍物(6,6)，本发明实施例中的方法使机器人克服了局部极小值，成功规划到达终点路径。经典势场法陷入局部最优，机器人停止运动，迭代次数为无穷。本发明实施例中的方法解决了局部最优问题，顺利到达目标点，整个过程迭代112次，运算效率高、速度快。
在多障碍物复杂情况下，机器人容易陷入局部最优状态，如图8所示。图8可看出机器人陷入局部最优，设置目标点位置(12,12)，随机分布9个障碍物，图9势场分布中可看出机器人处于局部极小点，位置为(7.91,6.34)，此时机器人势能为83.1J。当机器人在局部最优状态时，采用本发明实施例中提出的三角导航法建立导引点，帮助机器人成功规划出到达终点的路径，路径较平滑，如图10所示。图10中，黑色圆为障碍物，星型为导引点。
多障碍物仿真对比结果表示，经典势场法陷入局部最优，机器人停止运动，迭代次数无穷。可见，本发明实施例中提出的方法能够解决局部最优问题，整个过程迭代180次，实现快速收敛。
如图14所示为机器人在栅格地图中的规划行驶路径示意图。从图中可以看出机器人陷入局部最优，启用本发明实施例中提出的三角导航法规划导引点，成功逃离局部最优状态。从实际测试看，本发明使小车成功逃离局部最优，安全避开障碍物，最终到达目标点，耗费时间更短，实际测试如图15所示。
实施例2
基于与实施例1相同的发明构思，本发明实施例中提供了一种基于人工势场法的移动机器人路径规划装置，包括：
第一计算模块，用于基于机器人的当前位置和目标位置，以及障碍物信息，计算出机器人当前位置的引力和斥力；
第二计算模块，用于基于所述引力和斥力，计算出机器人当前位置受到的合力；
规划模块，用于当基于机器人当前位置受到的合力判断出机器人陷入局部最优状态时，利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，完成机器人路径规划。
在本发明实施例的一种具体实施方式中，所述移动机器人路径规划装置所述使得移动机器人脱离局部最优状态步骤之后，还包括：
根据预设的自适应调节步长公式，计算出移动机器人前进的步长，直至移动机器人到达目标位置。
在本发明实施例的一种具体实施方式中，所述自适应调节步长公式的表达式为：
Figure BDA0003485350510000101
式中，step为自适应调节步长，ρ为步长增益系数，n为当前产生斥力作用的障碍物数量，qobs为机器人当前位置与障碍物的距离，dobs为障碍物斥力场作用距离，c为当前路径规划循环次数，cmax为路径规划循环次数上限。
在本发明实施例的一种具体实施方式中，所述机器人当前位置受到的合力的计算公式为：
Figure BDA0003485350510000111
Figure BDA0003485350510000112
Figure BDA0003485350510000113
式中，F为机器人当前位置受到的合力，Fatt为机器人当前位置的引力，Frep为机器人当前位置的斥力，n为当前产生斥力作用的障碍物数量，qobs为机器人当前位置与障碍物的距离，dobs为障碍物斥力场作用距离，μ为引力增益系数，qg为机器人当前位置与目标位置的距离，
Figure BDA0003485350510000114
为机器人当前位置指向目标位置的单位向量，σ为斥力增益系数，
Figure BDA0003485350510000115
为机器人当前位置指向障碍物的单位向量。
在本发明实施例的一种具体实施方式中，所述利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，包括以下步骤：
当判断出机器人处于单障碍物环境时，以障碍物为圆心，以安全距离d为半径作圆，对圆心作障碍物和机器人连线的垂直线，交圆于G点，认定G点为导引点，G点提供额外的吸引力F'att，使得机器人逃离局部最优点。
在本发明实施例的另一种具体实施方式中，所述利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，包括以下步骤：
当判断出机器人处于多障碍物环境时，两两障碍物之间计算障碍物避碰距离k；
判断当前路径中k值是否大于机器人车身宽度；
若条件成立，则计算出两障碍物坐标中点作为导引点B，通过导引点B的额外吸引力F'att，使得机器人逃离局部最优点；
若条件不成立，则以最外围障碍物为圆心，以安全距离d为半径作圆，分别对各圆心作障碍物和机器人连线的垂直线，各垂直线交对应圆于G点，选择距离机器人最近的G点为作为导引点B，通过引导点B提供额外的吸引力F'att，使得机器人逃离局部最优点。
所述G点的计算公式为：
Figure BDA0003485350510000116
式中，O(x,y)为障碍物坐标，G(x,y)为导引点坐标，P(x,y)为机器人当前位置坐标，Gx为导引点G的x坐标，Gy为导引点G的y坐标，Ox为障碍物坐标O的x坐标，Oy为障碍物坐标O的y坐标，d为障碍物安全距离，Px为机器人当前位置P点的x坐标，Py为机器人当前位置P点的y坐标。
实施例3
基于与实施例1相同的发明构思，本发明实施例中提供了一种基于人工势场法的移动机器人路径规划系统，包括：
处理器；
存储器，其上存储有可在所述处理器上运行的计算机程序；
其中，所述计算机程序被所述处理器执行时实现如实施例1中任一项所述的方法。
以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (10)

1.一种基于人工势场法的移动机器人路径规划方法，其特征在于，包括：
基于机器人的当前位置和目标位置，以及障碍物信息，计算出机器人当前位置的引力和斥力；
基于所述引力和斥力，计算出机器人当前位置受到的合力；
当基于机器人当前位置受到的合力判断出机器人陷入局部最优状态时，利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，完成机器人路径规划。
2.根据权利要求1所述的一种基于人工势场法的移动机器人路径规划方法，其特征在于，所述使得移动机器人脱离局部最优状态步骤之后，还包括：
根据预设的自适应调节步长公式，计算出移动机器人前进的步长，直至机器人到达目标位置。
3.根据权利要求2所述的一种基于人工势场法的移动机器人路径规划方法，其特征在于，所述自适应调节步长公式的表达式为：
Figure FDA0003485350500000011
式中，step为自适应调节步长，ρ为步长增益系数，n为当前产生斥力作用的障碍物数量，qobs为机器人当前位置与障碍物的距离，dobs为障碍物斥力场作用距离，c为当前路径规划循环次数，cmax为路径规划循环次数上限。
4.根据权利要求1所述的一种基于人工势场法的移动机器人路径规划方法，其特征在于，所述机器人当前位置受到的合力的计算公式为：
Figure FDA0003485350500000012
Figure FDA0003485350500000013
Figure FDA0003485350500000014
式中，F为机器人当前位置受到的合力，Fatt为机器人当前位置的引力，Frep为机器人当前位置的斥力，n为当前产生斥力作用的障碍物数量，qobs为机器人当前位置与障碍物的距离，dobs为障碍物斥力场作用距离，μ为引力增益系数，qg为机器人当前位置与目标位置的距离，
Figure FDA0003485350500000015
为机器人当前位置指向目标位置的单位向量，σ为斥力增益系数，
Figure FDA0003485350500000021
为机器人当前位置指向障碍物的单位向量。
5.根据权利要求1所述的一种基于人工势场法的移动机器人路径规划方法，其特征在于，所述利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，包括以下步骤：
当判断出机器人处于单障碍物环境时，以障碍物为圆心，以安全距离d为半径作圆，对圆心作障碍物和机器人连线的垂直线，交圆于G点，认定G点为导引点，G点提供额外的吸引力F'att，使得机器人逃离局部最优点。
6.根据权利要求1所述的一种基于人工势场法的移动机器人路径规划方法，其特征在于，所述利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，包括以下步骤：
当判断出机器人处于多障碍物环境时，两两障碍物之间计算障碍物避碰距离k；
判断当前路径中k值是否大于机器人车身宽度；
若条件成立，则计算出两障碍物坐标中点作为导引点B，通过导引点B的额外吸引力F'att，使得机器人逃离局部最优点；
若条件不成立，则以最外围障碍物为圆心，以安全距离d为半径作圆，分别对各圆心作障碍物和机器人连线的垂直线，各垂直线交对应圆于G点，选择距离机器人最近的G点为作为导引点B，通过引导点B提供额外的吸引力F'att，使得机器人逃离局部最优点。
7.根据权利要求5或6所述的一种基于人工势场法的移动机器人路径规划方法，其特征在于：所述G点的计算公式为：
Figure FDA0003485350500000022
式中，O(x,y)为障碍物坐标，G(x,y)为导引点坐标，P(x,y)为机器人当前位置坐标，Gx为导引点G的x坐标，Gy为导引点G的y坐标，Ox为障碍物坐标O的x坐标，Oy为障碍物坐标O的y坐标，d为障碍物安全距离，Px为机器人当前位置P点的x坐标，Py为机器人当前位置P点的y坐标。
8.一种基于人工势场法的移动机器人路径规划装置，其特征在于，包括：
第一计算模块，用于基于机器人的当前位置和目标位置，以及障碍物信息，计算出机器人当前位置的引力和斥力；
第二计算模块，用于基于所述引力和斥力，计算出机器人当前位置受到的合力；
规划模块，用于当基于机器人当前位置受到的合力判断出机器人陷入局部最优状态时，利用三角导航法计算出导引点位置，产生逃逸力，使得机器人脱离局部最优状态，完成机器人路径规划。
9.根据权利要求8所述的一种基于人工势场法的移动机器人路径规划装置，其特征在于，所述使得移动机器人脱离局部最优状态步骤之后还包括：
根据预设的自适应调节步长公式，计算出移动机器人前进的步长，直至移动机器人到达目标位置。
10.一种基于人工势场法的移动机器人路径规划系统，其特征在于，包括：
处理器；
存储器，其上存储有可在所述处理器上运行的计算机程序；
其中，所述计算机程序被所述处理器执行时实现如权利要求1至7中任一项所述的方法。
CN202210079534.2A 2022-01-24 2022-01-24 基于人工势场法的移动机器人路径规划方法、装置及系统 Active CN114442628B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210079534.2A CN114442628B (zh) 2022-01-24 2022-01-24 基于人工势场法的移动机器人路径规划方法、装置及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210079534.2A CN114442628B (zh) 2022-01-24 2022-01-24 基于人工势场法的移动机器人路径规划方法、装置及系统

Publications (2)

Publication Number Publication Date
CN114442628A true CN114442628A (zh) 2022-05-06
CN114442628B CN114442628B (zh) 2023-12-01

Family

ID=81370040

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210079534.2A Active CN114442628B (zh) 2022-01-24 2022-01-24 基于人工势场法的移动机器人路径规划方法、装置及系统

Country Status (1)

Country Link
CN (1) CN114442628B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115079702A (zh) * 2022-07-18 2022-09-20 江苏集萃清联智控科技有限公司 一种混合道路场景下的智能车辆规划方法和系统
CN115229772A (zh) * 2022-08-23 2022-10-25 深圳市越疆科技有限公司 机器人及其控制方法、装置、设备、存储介质、机械臂

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105629974A (zh) * 2016-02-04 2016-06-01 重庆大学 一种基于改进型人工势场法的机器人路径规划方法及系统
CN107357293A (zh) * 2017-07-31 2017-11-17 上海应用技术大学 移动机器人路径规划方法和系统
CN110879592A (zh) * 2019-11-08 2020-03-13 南京航空航天大学 一种基于逃逸力模糊控制的人工势场路径规划方法
CN112577491A (zh) * 2020-12-14 2021-03-30 上海应用技术大学 一种基于改进人工势场法的机器人路径规划方法
CN112684709A (zh) * 2020-12-25 2021-04-20 长安大学 一种集群跟踪运动学建模方法、系统、设备及存储介质
CN113932810A (zh) * 2021-07-22 2022-01-14 全图通位置网络有限公司 基于多源地理信息模型的城市轨道三维导航地图优化方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105629974A (zh) * 2016-02-04 2016-06-01 重庆大学 一种基于改进型人工势场法的机器人路径规划方法及系统
CN107357293A (zh) * 2017-07-31 2017-11-17 上海应用技术大学 移动机器人路径规划方法和系统
CN110879592A (zh) * 2019-11-08 2020-03-13 南京航空航天大学 一种基于逃逸力模糊控制的人工势场路径规划方法
CN112577491A (zh) * 2020-12-14 2021-03-30 上海应用技术大学 一种基于改进人工势场法的机器人路径规划方法
CN112684709A (zh) * 2020-12-25 2021-04-20 长安大学 一种集群跟踪运动学建模方法、系统、设备及存储介质
CN113932810A (zh) * 2021-07-22 2022-01-14 全图通位置网络有限公司 基于多源地理信息模型的城市轨道三维导航地图优化方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
HENDRI HIMAWAN TRIHARMINTO等: "Framework transformation for local information on artificial potential field path planning", 《2016 8TH INTERNATIONAL CONFERENCE ON INFORMATION TECHNOLOGY AND ELECTRICAL ENGINEERING (ICITEE)》, pages 1 - 6 *
任学干，等: "基于改进势场蚁群算法的AGV路径规划", 《南京工程学院学报(自然科学版)》, vol. 19, no. 01, pages 36 - 41 *
杨勇;丁勇;黄鑫城;: "改进APF与Bezier相结合的多无人机协同避碰航路规划", 电光与控制, no. 11, pages 40 - 45 *
耿双乐;罗顺心;: "逃离传统势场法局部稳定点策略", 计算机与数字工程, vol. 47, no. 04, pages 823 - 862 *
郑利平;程亚军;路畅;廖婷;: "质心Power图下覆盖路径规划算法", 系统仿真学报, vol. 29, no. 5, pages 1120 - 1124 *
闫琳宇: "室内动态环境下移动小车的路径规划技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 5, pages 140 - 445 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115079702A (zh) * 2022-07-18 2022-09-20 江苏集萃清联智控科技有限公司 一种混合道路场景下的智能车辆规划方法和系统
CN115229772A (zh) * 2022-08-23 2022-10-25 深圳市越疆科技有限公司 机器人及其控制方法、装置、设备、存储介质、机械臂
CN115229772B (zh) * 2022-08-23 2023-07-18 深圳市越疆科技股份有限公司 机器人及其控制方法、装置、设备、存储介质、机械臂

Also Published As

Publication number Publication date
CN114442628B (zh) 2023-12-01

Similar Documents

Publication Publication Date Title
CN112631294B (zh) 一种移动机器人智能路径规划方法
CN114442628A (zh) 基于人工势场法的移动机器人路径规划方法、装置及系统
Shiltagh et al. Optimal path planning for intelligent mobile robot navigation using modified particle swarm optimization
Ardiyanto et al. Real-time navigation using randomized kinodynamic planning with arrival time field
Shiltagh et al. Path planning of intelligent mobile robot using modified genetic algorithm
CN113341984A (zh) 基于改进rrt算法的机器人路径规划方法和装置
US20220097736A1 (en) Vehicle Control Method and Apparatus, Storage Medium, and Electronic Device
Chen et al. An improved artificial potential field based path planning algorithm for unmanned aerial vehicle in dynamic environments
CN113359756B (zh) 一种基于栅格法实现全向移动机器人避障路径实时规划的方法
CN114755373B (zh) 一种基于多机器人编队的空气污染源预警定位方法
Rohrmuller et al. Probabilistic mapping of dynamic obstacles using markov chains for replanning in dynamic environments
Kumar et al. Path planning and control of mobile robots using modified Tabu search algorithm in complex environment
CN114564008A (zh) 基于改进A-Star算法的移动机器人路径规划方法
CN108227718B (zh) 一种自适应切换的自动搬运小车路径规划方法
Guo et al. Research on path planning of mobile robot with a novel improved artificial potential field algorithm
Do et al. Safe path planning among multi obstacles
CN116718190A (zh) 一种长距离密集人群场景下的移动机器人路径规划方法
CN113934219B (zh) 一种机器人自动避障方法、系统、设备及介质
Ranade et al. Quadcopter obstacle avoidance and path planning using flood fill method
Maoudj et al. Q-learning-based navigation for mobile robots in continuous and dynamic environments
CN109000653B (zh) 一种多维空间多载运工具路径智能优化方法
Meng et al. A navigation framework for mobile robots with 3D LiDAR and monocular camera
Qi et al. Optimal path planning for an unmanned aerial vehicle under navigation relayed by multiple stations to intercept a moving target
Shiltagh et al. A comparative study: Modified particle swarm optimization and modified genetic algorithm for global mobile robot navigation
Wu et al. Virtual Force Vector Control Method of Intelligent Vehicle Path Planning

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant