CN113934219B - 一种机器人自动避障方法、***、设备及介质 - Google Patents

一种机器人自动避障方法、***、设备及介质 Download PDF

Info

Publication number
CN113934219B
CN113934219B CN202111539782.2A CN202111539782A CN113934219B CN 113934219 B CN113934219 B CN 113934219B CN 202111539782 A CN202111539782 A CN 202111539782A CN 113934219 B CN113934219 B CN 113934219B
Authority
CN
China
Prior art keywords
particles
particle
coordinates
robot
coordinate
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.)
Active
Application number
CN202111539782.2A
Other languages
English (en)
Other versions
CN113934219A (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.)
Areson Technology Corp
Original Assignee
Areson Technology Corp
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 Areson Technology Corp filed Critical Areson Technology Corp
Priority to CN202111539782.2A priority Critical patent/CN113934219B/zh
Publication of CN113934219A publication Critical patent/CN113934219A/zh
Application granted granted Critical
Publication of CN113934219B publication Critical patent/CN113934219B/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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • 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
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS

Landscapes

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

Abstract

本发明公开了一种机器人自动避障方法、***、设备及介质，方法包括：获取机器人的初始坐标及目标点的目标坐标；初始化粒子滤波器，确定各粒子的初始坐标；计算当前时刻各障碍物的坐标；将粒子朝随机方向移动特定距离，确定各粒子移动后的坐标；计算各粒子与各障碍物的斥力场，以及与目标点的引力场；确定各粒子的权重；确定当前时刻的下一时刻机器人移动到达的坐标；判断机器人是否到达目标点，若是，结束，若否，将所确定的当前时刻的下一时刻机器人移动到达的坐标作为当前时刻机器人的当前坐标，继续确定当前时刻的下一时刻机器人移动到达的坐标。本发明结合粒子滤波及人工势场，避免局部极值及目标不可达的问题，有效实现机器人自动避障。

Description

一种机器人自动避障方法、***、设备及介质
技术领域
本发明涉及人工智能领域，更具体地，涉及一种机器人自动避障方法、***、设备及介质。
背景技术
机器人定位和路径规划是一种有着广泛应用领域的技术。在实际应用中，不同于行人路径规划，机器人在路径规划上，由于要考虑避让可能存在的移动障碍物，通常需要进行全局规划结合局部路径规划，才能保证机器人安全到达目的地。
目前在机器人局部路径规划方面，主要有人工势场法、模糊逻辑控制法、遗传算法和窗口滚动法等几种主要方法，其中使用最为最广泛的则是人工势场法，它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动，规划出平滑且较为安全的路径，但是人工势场存在以下几种缺陷：
1、容易陷入局部最优问题，导致机器人在局部震荡或停滞；
2、在使用过程中，若目标点附近有障碍物，则可能导致目标不可达的问题。
发明内容
本发明旨在克服上述现有技术的至少一种缺陷（不足），提供一种机器人自动避障方法、***、设备及介质，用于解决机器人路径规划中，人工势场法容易陷入局部最优的问题及目标点附近有障碍物导致目标不可达的问题。
本发明采取的技术方案是，一种机器人自动避障方法，包括以下步骤：
获取机器人的初始坐标及目标点的目标坐标；
根据所述机器人的初始坐标初始化粒子滤波器，以确定所述粒子滤波器中各粒子的初始坐标；
获取当前时刻机器人的当前坐标，并根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标，具体包括：
检测当前时刻机器人与各障碍物的距离及方位角，并根据所述当前时刻机器人的当前坐标、所述当前时刻机器人与各障碍物的距离及方位角，计算当前时刻各障碍物的坐标；
将所述粒子朝随机方向移动特定距离，并根据所述各粒子的初始坐标，确定各粒子移动后的坐标，所述特定距离为机器人在当前时刻到下一时刻的时间内移动的距离；
根据所述各粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的斥力场，并根据所述各粒子移动后的坐标、所述目标坐标，计算各粒子与目标点的引力场；
根据所述斥力场、所述引力场，确定各粒子的权重；
根据所述各粒子移动后的坐标、所述各粒子的权重，确定当前时刻的下一时刻机器人移动到达的坐标；
判断机器人是否到达目标点，若是，则结束，若否，则将所确定的所述当前时刻的下一时刻机器人移动到达的坐标作为所述当前时刻机器人的当前坐标，继续根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标。
本发明中，通过粒子滤波算法结合人工势场的思想，利用人工势场的斥力场与引力场计算粒子的权重，替代计算综合人工势场梯度，优化了机器人局部路径规划，避免陷入局部极值问题，从而导致机器人在局部震荡或停滞，同时，通过粒子滤波器中大量有限定移动范围的粒子模拟，实现对于机器人移动路径的指引，避免了传统人工势法在目标点附近有障碍物时目标不可达的问题。另一方面，本发明将粒子滤波的思想反向应用于机器人的动态避障问题中，把传统的被动预测应用于机器人移动过程的主动引导，扩展了粒子滤波的应用范围。
进一步的，获取机器人的初始坐标及目标点的目标坐标，还包括：获取可通行区域以及不可通行区域；
根据所述各粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的斥力场，并根据所述各粒子移动后的坐标、所述目标坐标，计算各粒子与目标点的引力场，具体包括：
根据所述各粒子移动后的坐标，统计无效粒子，所述无效粒子包括碰撞到各障碍物的粒子，以及移动后的坐标位于所述不可通行区域的粒子；
比较无效粒子的数量是否等于所述粒子滤波器中粒子的数量，
若无效粒子的数量等于所述粒子滤波器中粒子的数量，则重新初始化粒子滤波器，根据重新初始化后的所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标；
若无效粒子的数量不等于所述粒子滤波器中粒子的数量，则将所述粒子滤波器中无效粒子之外的粒子作为有效粒子；
根据所述各有效粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各有效粒子与各障碍物的斥力场，并根据所述各有效粒子移动后的坐标、所述目标坐标，计算各有效粒子与目标点的引力场；
根据所述斥力场、所述引力场，确定各粒子的权重，具体包括：
根据所述斥力场、所述引力场，确定各有效粒子的权重；
根据所述各粒子移动后的坐标、各粒子的权重，确定当前时刻的下一时刻机器人移动到达的坐标，具体包括：
根据所述各有效粒子移动后的坐标、各有效粒子的权重，确定当前时刻的下一时刻机器人移动到达的坐标。
本发明中，通过粒子滤波器中的粒子移动，模拟确定机器人各时刻移动的坐标，以完成对于机器人移动路径的指引，但是在移动过程中，可能存在部分粒子碰撞到障碍物，或者移动到机器人活动的不可通行区域内，这些粒子无法引导机器人进行有效避障，丧失引导作用，为无效粒子，故应当首先在粒子滤波器中应进行该部分粒子的统计剔除，使用无效粒子以外的有效粒子，进行机器人下一时刻移动到达坐标的计算，以实现对机器人进行有效避障路径引导。具体的，在粒子滤波器中，当无效粒子的数量等于粒子滤波器中粒子的数量，则代表粒子滤波器中所有粒子均失效，没有有效粒子的存在，则需要重新初始化粒子滤波器，使用重新初始化后的粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标，而当无效粒子的数量不等于所述粒子滤波器中粒子的数量，则代表所述粒子滤波器中仍旧存在有效粒子，故继续使用有效粒子进行后续确定下一时刻机器人移动到达的坐标等步骤，而具体的，当无效粒子的数量不等于所述粒子滤波器中粒子的数量时，存在两种情况：无效粒子的数量小于所述粒子滤波器中粒子的数量，且不为零，则代表粒子滤波器中既存在无效粒子，也存在有效粒子，则两者差值，则为有效粒子数量；无效粒子的数量等于零，则代表粒子滤波器中粒子全部为有效粒子，可全部用于执行后续步骤。
进一步的，根据所述各粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的斥力场，具体包括：
根据所述各粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的距离；
根据各粒子与各障碍物的距离，采用以下公式，计算各粒子与各障碍物的斥力场：
Figure 518325DEST_PATH_IMAGE001
其中，
Figure 931989DEST_PATH_IMAGE002
表示第k个粒子与第i个障碍物的斥力场，
Figure 693927DEST_PATH_IMAGE003
表示斥力因子，
Figure 902055DEST_PATH_IMAGE004
表示第k个粒子与第i个障碍物的距离，
Figure 480935DEST_PATH_IMAGE005
表示设定的斥力距离阈值，
Figure 698290DEST_PATH_IMAGE006
表示第k个粒子与各障碍物斥力场；
根据所述各粒子移动后的坐标、所述目标坐标，计算各粒子与目标点的引力场，具体包括：
根据所述各粒子移动后的坐标、所述目标坐标，计算各粒子与目标点的距离；
根据各粒子与目标点的距离，采用以下公式，计算各粒子与目标点的引力场：
Figure 311805DEST_PATH_IMAGE007
其中，
Figure 690833DEST_PATH_IMAGE008
表示第k个粒子与目标点的引力场，
Figure 147223DEST_PATH_IMAGE009
表示引力因子，
Figure 778055DEST_PATH_IMAGE010
表示k个粒子与目标点的距离。
进一步的，根据所述斥力场、所述引力场，确定各粒子的权重，具体包括：
根据所述斥力场、所述引力场，采用以下公式，计算各粒子的权重因子：
Figure 370711DEST_PATH_IMAGE011
其中，
Figure 61586DEST_PATH_IMAGE012
表示第k个粒子的权重因子，
Figure 474113DEST_PATH_IMAGE013
表示第k个粒子与各障碍物的斥力场，
Figure 33270DEST_PATH_IMAGE014
表示第k个粒子与目标点的引力场；
采用以下公式，对所计算的各粒子的权重因子做归一化处理，确定各粒子的权重：
Figure 355798DEST_PATH_IMAGE015
其中，
Figure 342209DEST_PATH_IMAGE016
表示第i个粒子的权重因子，
Figure 648556DEST_PATH_IMAGE017
表示第k个粒子的权重。
进一步的，根据所述各粒子移动后的坐标、所述各粒子的权重，确定当前时刻的下一时刻机器人移动到达的坐标，具体包括：
根据所述各粒子移动后的坐标、所述各粒子的权重，采用以下公式计算的机器人下一时刻移动到达的坐标：
Figure 480246DEST_PATH_IMAGE018
其中，
Figure 47494DEST_PATH_IMAGE019
表示第i个粒子在当前时刻t移动后的坐标，
Figure 80172DEST_PATH_IMAGE020
表示第i个粒子的权重，
Figure 467291DEST_PATH_IMAGE021
表示计算的机器人下一时刻t+1移动到达的坐标；
判断所计算的机器人下一时刻移动到达的坐标，是否处于可通行区域：
若是，则将所计算的机器人下一时刻移动到达的坐标，确定为当前时刻的下一时刻机器人移动到达的坐标；
若否，则将所述权重最大的粒子所对应的移动后坐标，确定为当前时刻的下一时刻机器人移动到达的坐标。
进一步的，将所确定的所述当前时刻的下一时刻机器人移动到达的坐标作为所述当前时刻机器人的当前坐标，继续根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标，具体包括：
将所确定的所述当前时刻的下一时刻机器人移动到达的坐标作为所述当前时刻机器人的当前坐标；
判断无效粒子的数量是否为零，
若是，则继续根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标，
若否，则对所述粒子滤波器进行粒子重采样，得到重采样后的粒子滤波器，继续根据重采样后的所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标。
本发明中，当无效粒子的数量不为零，且之前判断了无效粒子的数量不等于粒子滤波器中的粒子数量，则代表所述粒子滤波器中存在数量不为零的无效粒子，相比原来初始化的粒子滤波器中的粒子数量而言，移动后的有效粒子数量减少，而随着机器人的不断移动，有效粒子数量持续减少，将出现粒子匮乏状况，在进行下一时刻机器人移动到达坐标计算时，可能由于粒子匮乏状况，导致计算不准确或得到的结果不是最优结果，或者出现类似传统人工势场法在目标点较远处，引力过大导致碰撞障碍物的问题，从而无法引导机器人很好完成避障行为，因此，本发明在这种情况出现时，对粒子滤波器进行粒子重采样，及时补充粒子数量，保障下一时刻机器人移动到达坐标计算过程中有足够的粒子，不会出现粒子匮乏，从而有效提升本发明中机器人避障路径规划的准确性；另一方面，当无效粒子的数量等于零时，则代表粒子滤波器中不存在无效粒子，所有粒子均为有效粒子，故无需补充粒子，继续以所述粒子滤波器中的粒子，确定当前时刻的下一时刻机器人移动到达的坐标。
进一步的，对所述粒子滤波器进行粒子重采样，具体包括：
将各有效粒子按对应的权重从大到小排序，所述排序中前若干个有效粒子形成第一重采样粒子集合；
以当前时刻机器人的当前坐标为中心，随机生成若干个粒子，随机生成的粒子形成第二重采样粒子集合；
将第一重采样粒子集合和第二重采样粒子集合的粒子作为所述粒子滤波器中的粒子，以完成粒子重采样。
本发明中，将两种不同的重采样粒子集合的粒子加入粒子滤波器，以完成粒子重采样，有效保障了重采样时粒子的多样性，而其中，第一重采样粒子集合收入权重排序较前的若干粒子，而权重大代表对于机器人的引导作用较大，故该种重采样方式可有效优化最终对于机器人的引导结果，从而得出更优化的路径规划。
另一方面，本发明采取的另一种技术方案为，一种机器人自动避障***，通过上述避障方法实现机器人自动避障，包括：
初始坐标获取模块，用于获取机器人的初始坐标及目标点的目标坐标；
粒子滤波器初始化模块，用于根据所述机器人的初始坐标初始化粒子滤波器，以确定所述粒子滤波器中各粒子的初始坐标；
机器人移动到达坐标确定模块，用于获取当前时刻机器人的当前坐标，并根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标；
所述机器人移动到达坐标确定模块，具体包括：
障碍物坐标计算模块，用于检测当前时刻机器人与各障碍物的距离及方位角，并根据所述当前时刻机器人的当前坐标、所述当前时刻机器人与各障碍物的距离及方位角，计算当前时刻各障碍物的坐标；
粒子移动后的坐标确定模块，将所述粒子朝随机方向移动特定距离，并根据各粒子的初始坐标，确定各粒子移动后的坐标，所述特定距离为机器人在当前时刻到下一时刻的时间内移动的距离；
斥力场及引力场计算模块，用于根据所述各粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的斥力场，并根据所述各粒子移动后的坐标、所述目标坐标，计算各粒子与目标点的引力场；
权重确定模块，用于根据所述斥力场、所述引力场，确定各粒子的权重；
机器人移动到达坐标确定模块，用于根据所述各粒子移动后的坐标、各粒子的权重，确定当前时刻的下一时刻机器人移动到达的坐标；
目标点到达判断模块，用于判断机器人是否到达目标点，若是，则结束，若否，则将所确定的所述当前时刻的下一时刻机器人移动到达的坐标作为所述当前时刻机器人的当前坐标，继续根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标。
另一方面，本发明采取的另一种技术方案为，一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述避障方法的步骤。
另一方面，本发明采取的另一种技术方案为，一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述避障方法的步骤。
与现有技术相比，本发明的有益效果为：
本发明通过粒子滤波算法结合人工势场的思想，利用人工势场的斥力场与引力场计算粒子的权重，替代计算综合人工势场梯度，优化了机器人局部路径规划，避免陷入局部极值问题，从而导致机器人在局部震荡或停滞，同时，通过粒子滤波器中大量有限定移动范围的粒子模拟，实现对于机器人移动路径的指引，避免了传统人工势法在目标点附近有障碍物时目标不可达的问题；另一方面，本发明将粒子滤波的思想反向应用于机器人的动态避障问题中，把传统的被动预测应用于机器人移动过程的主动引导，扩展了粒子滤波的应用范围。
附图说明
图1为本发明的方法流程图。
图2为本发明的***结构图。
标记说明：初始坐标获取模块100，粒子滤波器初始化模块110，障碍物坐标计算模块120，粒子移动后的坐标确定模块130，无效粒子统计模块140，无效粒子数量比较模块150，斥力场及引力场计算模块160，权重确定模块170，机器人移动到达坐标计算模块180，机器人移动到达坐标确定模块190，目标点到达判断模块200，无效粒子数量判断模块210，重采样模块220。
具体实施方式
本发明附图仅用于示例性说明，不能理解为对本发明的限制。为了更好说明以下实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
实施例1
如图1所示，本实施例提供一种机器人自动避障方法，包括以下步骤：
S1、获取机器人的初始坐标及目标点的目标坐标；
具体的，本实施例中，步骤S1中获取机器人的初始坐标为
Figure 102671DEST_PATH_IMAGE022
，目标点的目标坐标为
Figure 662441DEST_PATH_IMAGE023
，此外，还包括获取机器人预加载活动场景的全局地图信息，地图信息包括：所采用的坐标***、地图边界及其多边形坐标序列信息、可通行区域和不可通行区域及其多边形坐标序列信息，以及获取机器人初始航向角
Figure 990654DEST_PATH_IMAGE024
，机器人匀速移动速度
Figure 740436DEST_PATH_IMAGE025
；
S2、根据所述机器人的初始坐标初始化粒子滤波器，以确定所述粒子滤波器中各粒子的初始坐标；
具体的，本实施例中，步骤S2包括：
设定粒子滤波器中的粒子数量n；
以机器人的初始坐标为
Figure 179507DEST_PATH_IMAGE026
为圆心，R为半径的圆范围内，随机分布生成n个粒子，确定各粒子的初始坐标为
Figure 596713DEST_PATH_IMAGE027
，且每个粒子初始权重均设定为1/n；
S3、检测当前时刻机器人与各障碍物的距离及方位角，并根据所述当前时刻机器人的当前坐标、所述当前时刻机器人与各障碍物的距离及方位角，计算当前时刻各障碍物的坐标；
具体的，本实施例中，获取当前时刻机器人的当前坐标为
Figure 830248DEST_PATH_IMAGE028
，当前时刻地图中障碍物数量为m，通过机器人自带的传感器，如激光传感器、毫米波雷达传感器，检测当前时刻机器人与各移动障碍物的距离
Figure 457539DEST_PATH_IMAGE029
、方位角
Figure 310088DEST_PATH_IMAGE030
，并根据当前时刻机器人的当前坐标
Figure 972014DEST_PATH_IMAGE031
、当前时刻机器人与各障碍物的距离
Figure 517396DEST_PATH_IMAGE032
及方位角
Figure 100824DEST_PATH_IMAGE033
，通过以下公式计算当前时刻各障碍物的坐标：
Figure 147277DEST_PATH_IMAGE034
其中k表示第k个障碍物，方位角是指，在当前时刻，以机器人当前坐标
Figure 273496DEST_PATH_IMAGE031
为起点，以各障碍物坐标为终点的连线在全局地图上的指向，计算所得的当前时刻各障碍物坐标为
Figure 114413DEST_PATH_IMAGE035
；
S4、将所述粒子朝随机方向移动特定距离，并根据所述各粒子的初始坐标，确定各粒子移动后的坐标，所述特定距离为机器人在当前时刻到下一时刻的时间内移动的距离；
具体的，本实施例中，获取当前时刻机器人的当前坐标为
Figure 326083DEST_PATH_IMAGE028
，当前时刻机器人与目标点的方位角为
Figure 645069DEST_PATH_IMAGE036
，将粒子朝随机方向移动具体为，以方位角
Figure 16007DEST_PATH_IMAGE036
为基准，将所有粒子朝
Figure 637613DEST_PATH_IMAGE037
方向移动
Figure 726791DEST_PATH_IMAGE025
个单位，其中
Figure 583889DEST_PATH_IMAGE038
对于每个粒子来说是一个随机变量，确定的各粒子移动后的坐标为：
Figure 941490DEST_PATH_IMAGE039
；
S5、根据所述各粒子移动后的坐标，统计无效粒子，所述无效粒子包括碰撞到各障碍物的粒子，以及移动后的坐标位于所述不可通行区域的粒子；
具体的，本实施例中，步骤S5中统计无效粒子包括：
初始化一个无效粒子集合S；
设定距离阈值
Figure 124210DEST_PATH_IMAGE040
，当粒子与任意一个障碍物距离小于
Figure 44893DEST_PATH_IMAGE041
时，认为该粒子碰撞到障碍物，将该粒子加入到无效粒子集合S中；
对于每个粒子，运用地图拓扑算法，将移动后坐标位于全局地图中的不可通行区域中的粒子，加入到无效粒子集合S中；
统计无效粒子集合S中粒子数量
Figure 971260DEST_PATH_IMAGE042
；
S6、比较无效粒子的数量是否等于所述粒子滤波器中粒子的数量，若是，返回步骤S2，若否，则执行步骤S7；
S7、将所述粒子滤波器中无效粒子之外的粒子作为有效粒子；
具体的，本实施例，在粒子滤波器中，当无效粒子的数量等于粒子滤波器中粒子的数量n时，则代表粒子滤波器中所有粒子均失效，没有有效粒子的存在，则需要返回初始化滤波器步骤，重新初始化粒子滤波器，使用重新初始化后的粒子滤波器，继续执行后续步骤，而当无效粒子的数量
Figure 192157DEST_PATH_IMAGE042
不等于所述粒子滤波器中粒子的数量n时，则代表所述粒子滤波器中仍旧存在有效粒子，故可以继续使用有效粒子继续执行后续步骤，而具体的，当无效粒子的数量
Figure 280199DEST_PATH_IMAGE042
不等于所述粒子滤波器中粒子的数量n时，存在两种情况：
无效粒子的数量
Figure 78391DEST_PATH_IMAGE042
小于所述粒子滤波器中粒子的数量n，且不为零，则代表粒子滤波器中既存在无效粒子，也存在有效粒子，则两者差值
Figure 683815DEST_PATH_IMAGE043
，则为有效粒子数量；
无效粒子的数量
Figure 883853DEST_PATH_IMAGE042
等于零，则代表粒子滤波器中粒子全部为有效粒子，可全部用于执行后续步骤。
S8、根据所述各有效粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各有效粒子与各障碍物的斥力场，并根据所述各有效粒子移动后的坐标、所述目标坐标，计算各有效粒子与目标点的引力场；
进一步的，步骤S8中计算各有效粒子与各障碍物的斥力场，具体包括：
根据所述各有效粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的距离；
根据各粒子与各障碍物的距离，采用以下公式，计算各粒子与各障碍物的斥力场：
Figure 18162DEST_PATH_IMAGE044
其中，
Figure 303650DEST_PATH_IMAGE045
表示第k个粒子与第i个障碍物的斥力场，
Figure 571820DEST_PATH_IMAGE046
表示斥力因子，
Figure 501730DEST_PATH_IMAGE047
表示第k个粒子与第i个障碍物的距离，
Figure 931574DEST_PATH_IMAGE048
表示设定的斥力距离阈值，
Figure 579724DEST_PATH_IMAGE049
表示第k个粒子与各障碍物斥力场；
进一步的，步骤S8中计算各有效粒子与目标点的引力场，具体包括：
根据所述各有效粒子移动后的坐标、所述目标坐标，计算各有效粒子与目标点的距离；
根据各有效粒子与目标点的距离，采用以下公式，计算各有效粒子与目标点的引力场：
Figure 651586DEST_PATH_IMAGE007
其中，
Figure 295057DEST_PATH_IMAGE050
表示第k个有效粒子与目标点的引力场，
Figure 302327DEST_PATH_IMAGE052
表示引力因子，
Figure 31248DEST_PATH_IMAGE010
表示k个有效粒子与目标点的距离。
S9、根据所述斥力场、所述引力场，确定各有效粒子的权重；
进一步的，步骤S9具体包括：
根据所述斥力场、所述引力场，采用以下公式，计算各粒子的权重因子：
Figure 779237DEST_PATH_IMAGE053
其中，
Figure 542794DEST_PATH_IMAGE012
表示第k个粒子的权重因子，
Figure 580020DEST_PATH_IMAGE054
表示第k个粒子与各障碍物的斥力场，
Figure 937183DEST_PATH_IMAGE014
表示第k个粒子与目标点的引力场；
采用以下公式，对所计算的各粒子的权重因子做归一化处理，确定各粒子的权重：
Figure 85268DEST_PATH_IMAGE055
其中，
Figure 844276DEST_PATH_IMAGE056
表示第i个粒子的权重因子，
Figure 52404DEST_PATH_IMAGE057
表示第k个粒子的权重；
S10、根据所述各有效粒子移动后的坐标、所述各有效粒子的权重，采用以下公式计算的机器人下一时刻移动到达的坐标；
Figure 21497DEST_PATH_IMAGE058
其中，
Figure 114218DEST_PATH_IMAGE059
表示第i个粒子在当前时刻t移动后的坐标，
Figure 586787DEST_PATH_IMAGE060
表示第i个粒子的权重，
Figure 106761DEST_PATH_IMAGE061
表示计算的机器人下一时刻t+1移动到达的坐标；
S11、判断所计算的机器人下一时刻移动到达的坐标，是否处于可通行区域，若是，执行步骤S12，若否，执行S13；
S12、将所计算的机器人下一时刻移动到达的坐标
Figure 297571DEST_PATH_IMAGE062
，确定为当前时刻的下一时刻机器人移动到达的坐标，继续执行步骤S14；
S13、将所述权重最大的粒子所对应的移动后坐标，确定为当前时刻的下一时刻机器人移动到达的坐标，继续执行步骤S14；
S14、判断机器人是否到达目标点，若是，则结束，若否，执行步骤S15；
S15、将所确定的所述当前时刻的下一时刻机器人移动到达的坐标作为所述当前时刻机器人的当前坐标；
S16、判断无效粒子的数量是否为零，若是，返回步骤S3，若否，执行步骤S17；
S17、对所述粒子滤波器进行粒子重采样，得到重采样后的粒子滤波器，再返回步骤S3；
进一步的，步骤S16具体包括：
将各有效粒子按对应的权重从大到小排序，所述排序中前若干个有效粒子形成第一重采样粒子集合；
以当前时刻机器人的当前坐标为中心，随机生成若干个粒子，随机生成的粒子形成第二重采样粒子集合；
将第一重采样粒子集合和第二重采样粒子集合的粒子作为所述粒子滤波器中的粒子，以完成粒子重采样。
具体的，本实施例中，设置第一重采样粒子集合S1中粒子数量为n1，第二重采样粒子集合中粒子数量为n2，
当无效粒子集合S中粒子数量
Figure 53038DEST_PATH_IMAGE042
为偶数时，
Figure 521059DEST_PATH_IMAGE063
，
当无效粒子集合S中粒子数量
Figure 70989DEST_PATH_IMAGE042
为奇数时，
Figure 890041DEST_PATH_IMAGE064
。
实施例2
如图2所示，本实施例提供一种机器人自动避障***，包括：
初始坐标获取模块100，用于获取机器人的初始坐标及目标点的目标坐标；
具体的，本实施例中，初始坐标获取模块100获取机器人的初始坐标为
Figure 183619DEST_PATH_IMAGE065
，目标点的目标坐标为
Figure 896360DEST_PATH_IMAGE023
，此外，还包括获取机器人预加载活动场景的全局地图信息，地图信息包括：所采用的坐标***、地图边界及其多边形坐标序列信息、可通行区域和不可通行区域及其多边形坐标序列信息，以及获取机器人初始航向角
Figure 758137DEST_PATH_IMAGE024
，机器人匀速移动速度
Figure 657960DEST_PATH_IMAGE025
；
粒子滤波器初始化模块110，用于根据所述机器人的初始坐标初始化粒子滤波器，以确定所述粒子滤波器中各粒子的初始坐标；
具体的，本实施例中，粒子滤波器初始化模块110确定粒子滤波器中各粒子的初始坐标，具体包括：
设定粒子滤波器中的粒子数量n；
以机器人的初始坐标为
Figure 633524DEST_PATH_IMAGE026
为圆心，R为半径的圆范围内，随机分布生成n个粒子，确定各粒子的初始坐标为
Figure 466351DEST_PATH_IMAGE027
，且每个粒子初始权重均设定为1/n；
障碍物坐标计算模块120，用于检测当前时刻机器人与各障碍物的距离及方位角，并根据所述当前时刻机器人的当前坐标、所述当前时刻机器人与各障碍物的距离及方位角，计算当前时刻各障碍物的坐标；
具体的，本实施例中，障碍物坐标计算模块120获取当前时刻机器人的当前坐标为
Figure 358084DEST_PATH_IMAGE031
，当前时刻地图中障碍物数量为m，通过机器人自带的传感器，如激光传感器、毫米波雷达传感器，检测当前时刻机器人与各移动障碍物的距离
Figure 886148DEST_PATH_IMAGE032
、方位角
Figure 521529DEST_PATH_IMAGE033
，并根据当前时刻机器人的当前坐标
Figure 84228DEST_PATH_IMAGE066
、当前时刻机器人与各障碍物的距离
Figure 412442DEST_PATH_IMAGE032
及方位角
Figure 427802DEST_PATH_IMAGE033
，通过以下公式计算当前时刻各障碍物的坐标：
Figure 601295DEST_PATH_IMAGE034
其中k表示第k个障碍物，方位角是指，在当前时刻，以机器人当前坐标
Figure 143134DEST_PATH_IMAGE031
为起点，以各障碍物坐标为终点的连线在全局地图上的指向，计算所得的当前时刻各障碍物坐标为
Figure 517615DEST_PATH_IMAGE035
；
粒子移动后的坐标确定模块130，将所述粒子朝随机方向移动特定距离，并根据各粒子的初始坐标，确定各粒子移动后的坐标，所述特定距离为机器人在当前时刻到下一时刻的时间内移动的距离；
具体的，本实施例中，粒子移动后的坐标确定模块130获取当前时刻机器人的当前坐标为
Figure 879326DEST_PATH_IMAGE031
，当前时刻机器人与目标点的方位角为
Figure 997455DEST_PATH_IMAGE036
，将粒子朝随机方向移动具体为，以方位角
Figure 128222DEST_PATH_IMAGE036
为基准，将所有粒子朝
Figure 798238DEST_PATH_IMAGE067
方向移动
Figure 788190DEST_PATH_IMAGE025
个单位，其中
Figure 569065DEST_PATH_IMAGE038
对于每个粒子来说是一个随机变量，确定的各粒子移动后的坐标为：
Figure 695284DEST_PATH_IMAGE039
；
无效粒子统计模块140，用于根据所述各粒子移动后的坐标，统计无效粒子，所述无效粒子包括碰撞到各障碍物的粒子，以及移动后的坐标位于所述不可通行区域的粒子；
具体的，本实施例中，无效粒子统计模块140统计无效粒子，具体包括：
初始化一个无效粒子集合S；
设定距离阈值
Figure 536201DEST_PATH_IMAGE040
，当粒子与任意一个障碍物距离小于
Figure 872504DEST_PATH_IMAGE041
时，认为该粒子碰撞到障碍物，将该粒子加入到无效粒子集合S中；
对于每个粒子，运用地图拓扑算法，将移动后坐标位于全局地图中的不可通行区域中的粒子，加入到无效粒子集合S中；
统计无效粒子集合S中粒子数量
Figure 329506DEST_PATH_IMAGE042
；
无效粒子数量比较模块150，用于比较无效粒子的数量是否等于所述粒子滤波器中粒子的数量，若是，通过粒子滤波器初始化模块110重新初始化粒子滤波器，使用重新初始化后的粒子滤波器，继续执行后续步骤，若否，将所述粒子滤波器中无效粒子之外的粒子作为有效粒子；
具体的，本实施例，在粒子滤波器中，当无效粒子的数量等于粒子滤波器中粒子的数量n时，则代表粒子滤波器中所有粒子均失效，没有有效粒子的存在，则需要返回初始化滤波器步骤，重新初始化粒子滤波器，使用重新初始化后的粒子滤波器，继续执行后续步骤，而当无效粒子的数量
Figure 434865DEST_PATH_IMAGE042
不等于所述粒子滤波器中粒子的数量n时，则代表所述粒子滤波器中仍旧存在有效粒子，故可以继续使用有效粒子继续执行后续步骤，而具体的，当无效粒子的数量
Figure 322049DEST_PATH_IMAGE042
不等于所述粒子滤波器中粒子的数量n时，存在两种情况：
无效粒子的数量
Figure 411228DEST_PATH_IMAGE042
小于所述粒子滤波器中粒子的数量n，且不为零，则代表粒子滤波器中既存在无效粒子，也存在有效粒子，则两者差值
Figure 268326DEST_PATH_IMAGE043
，则为有效粒子数量；
无效粒子的数量
Figure 369137DEST_PATH_IMAGE042
等于零，则代表粒子滤波器中粒子全部为有效粒子，可全部用于执行后续步骤。
斥力场及引力场计算模块160，用于根据所述各有效粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各有效粒子与各障碍物的斥力场，并根据所述各有效粒子移动后的坐标、所述目标坐标，计算各有效粒子与目标点的引力场；
进一步的，斥力场及引力场计算模块160中计算各有效粒子与各障碍物的斥力场，具体包括：
根据所述各有效粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的距离；
根据各粒子与各障碍物的距离，采用以下公式，计算各粒子与各障碍物的斥力场：
Figure 551857DEST_PATH_IMAGE068
其中，
Figure 738118DEST_PATH_IMAGE069
表示第k个粒子与第i个障碍物的斥力场，
Figure 664486DEST_PATH_IMAGE003
表示斥力因子，
Figure 885383DEST_PATH_IMAGE070
表示第k个粒子与第i个障碍物的距离，
Figure 973425DEST_PATH_IMAGE071
表示设定的斥力距离阈值，
Figure 771617DEST_PATH_IMAGE072
表示第k个粒子与各障碍物斥力场；
进一步的，斥力场及引力场计算模块160中计算各有效粒子与目标点的引力场，具体包括：
根据所述各有效粒子移动后的坐标、所述目标坐标，计算各有效粒子与目标点的距离；
根据各有效粒子与目标点的距离，采用以下公式，计算各有效粒子与目标点的引力场：
Figure 111462DEST_PATH_IMAGE007
其中，
Figure 311499DEST_PATH_IMAGE008
表示第k个有效粒子与目标点的引力场，
Figure 570442DEST_PATH_IMAGE052
表示引力因子，
Figure 731296DEST_PATH_IMAGE010
表示k个有效粒子与目标点的距离。
权重确定模块170，用于根据所述斥力场、所述引力场，确定各有效粒子的权重；
进一步的，权重确定模块170确定各有效粒子权重，具体包括：
根据所述斥力场、所述引力场，采用以下公式，计算各粒子的权重因子：
Figure 999467DEST_PATH_IMAGE073
其中，
Figure 663797DEST_PATH_IMAGE074
表示第k个粒子的权重因子，
Figure 624800DEST_PATH_IMAGE075
表示第k个粒子与各障碍物的斥力场，
Figure 132005DEST_PATH_IMAGE076
表示第k个粒子与目标点的引力场；
采用以下公式，对所计算的各粒子的权重因子做归一化处理，确定各粒子的权重：
Figure 828301DEST_PATH_IMAGE077
其中，
Figure 737352DEST_PATH_IMAGE078
表示第i个粒子的权重因子，
Figure 479043DEST_PATH_IMAGE079
表示第k个粒子的权重；
机器人移动到达坐标计算模块180，用于根据所述各有效粒子移动后的坐标、所述各有效粒子的权重，采用以下公式计算的机器人下一时刻移动到达的坐标；
Figure 739123DEST_PATH_IMAGE080
其中，
Figure 83516DEST_PATH_IMAGE081
表示第i个粒子在当前时刻t移动后的坐标，
Figure 988018DEST_PATH_IMAGE082
表示第i个粒子的权重，
Figure 25245DEST_PATH_IMAGE061
表示计算的机器人下一时刻t+1移动到达的坐标；
机器人移动到达坐标确定模块190，用于判断所计算的机器人下一时刻移动到达的坐标，是否处于可通行区域，若是，将所计算的机器人下一时刻移动到达的坐标
Figure 382408DEST_PATH_IMAGE062
，确定为当前时刻的下一时刻机器人移动到达的坐标，若否，将所述权重最大的粒子所对应的移动后坐标，确定为当前时刻的下一时刻机器人移动到达的坐标；
目标点到达判断模块200，用于判断机器人是否到达目标点，若是，结束，若否，将所确定的所述当前时刻的下一时刻机器人移动到达的坐标作为所述当前时刻机器人的当前坐标，并继续进行无效粒子数量判断模块210的判断；
无效粒子数量判断模块210，用于判断无效粒子的数量是否为零，若是，重新通过障碍物坐标计算模块120计算当前时刻各障碍物的坐标，若否，通过重采样模块220对所述粒子滤波器进行粒子重采样，得到重采样后的粒子滤波器，再重新通过障碍物坐标计算模块120计算当前时刻各障碍物的坐标；
重采样模块220，用于对所述粒子滤波器进行粒子重采样，得到重采样后的粒子滤波器；
进一步的，重采样模块220粒子重采样步骤具体包括：
将各有效粒子按对应的权重从大到小排序，所述排序中前若干个有效粒子形成第一重采样粒子集合；
以当前时刻机器人的当前坐标为中心，随机生成若干个粒子，随机生成的粒子形成第二重采样粒子集合；
将第一重采样粒子集合和第二重采样粒子集合的粒子作为所述粒子滤波器中的粒子，以完成粒子重采样。
具体的，本实施例中，设置第一重采样粒子集合S1中粒子数量为n1，第二重采样粒子集合中粒子数量为n2，
当无效粒子集合S中粒子数量
Figure 796072DEST_PATH_IMAGE083
为偶数时，
Figure 414135DEST_PATH_IMAGE063
，
当无效粒子集合S中粒子数量
Figure 763208DEST_PATH_IMAGE083
为奇数时，
Figure 466721DEST_PATH_IMAGE064
。
实施例3
本实施例提供一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述实施例1中机器人自动避障方法的步骤。
实施例4
本实施例提供一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述实施例1中机器人自动避障方法的步骤。
显然，本发明的上述实施例仅仅是为清楚地说明本发明技术方案所作的举例，而并非是对本发明的具体实施方式的限定。凡在本发明权利要求书的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (9)

1.一种机器人自动避障方法，其特征在于，包括以下步骤：
获取机器人的初始坐标及目标点的目标坐标；
根据所述机器人的初始坐标初始化粒子滤波器，以确定所述粒子滤波器中各粒子的初始坐标；
获取当前时刻机器人的当前坐标，并根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标，具体包括：
检测当前时刻机器人与各障碍物的距离及方位角，并根据所述当前时刻机器人的当前坐标、所述当前时刻机器人与各障碍物的距离及方位角，计算当前时刻各障碍物的坐标；
将所述粒子朝随机方向移动特定距离，并根据所述各粒子的初始坐标，确定各粒子移动后的坐标，所述特定距离为机器人在当前时刻到下一时刻的时间内移动的距离；
根据所述各粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的斥力场，并根据所述各粒子移动后的坐标、所述目标坐标，计算各粒子与目标点的引力场；
根据所述斥力场、所述引力场，确定各粒子的权重，具体包括：
根据所述斥力场、所述引力场，采用以下公式，计算各粒子的权重因子：
Figure 235035DEST_PATH_IMAGE001
其中，
Figure 237626DEST_PATH_IMAGE002
表示第k个粒子的权重因子，
Figure 777323DEST_PATH_IMAGE003
表示第k个粒子与各障碍物的斥力场，
Figure 472746DEST_PATH_IMAGE004
表示第k个粒子与目标点的引力场；
采用以下公式，对所计算的各粒子的权重因子做归一化处理，确定各粒子的权重：
Figure 979951DEST_PATH_IMAGE005
其中，
Figure 520654DEST_PATH_IMAGE006
表示第i个粒子的权重因子，
Figure 429704DEST_PATH_IMAGE007
表示第k个粒子的权重；
根据所述各粒子移动后的坐标、所述各粒子的权重，确定当前时刻的下一时刻机器人移动到达的坐标；
判断机器人是否到达目标点，若是，则结束，若否，则将所确定的所述当前时刻的下一时刻机器人移动到达的坐标作为所述当前时刻机器人的当前坐标，继续根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标。
2.根据权利要求1所述的一种机器人自动避障方法，其特征在于，获取机器人的初始坐标及目标点的目标坐标，还包括：获取可通行区域以及不可通行区域；
根据所述各粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的斥力场，并根据所述各粒子移动后的坐标、所述目标坐标，计算各粒子与目标点的引力场，具体包括：
根据所述各粒子移动后的坐标，统计无效粒子，所述无效粒子包括碰撞到各障碍物的粒子，以及移动后的坐标位于所述不可通行区域的粒子；
比较无效粒子的数量是否等于所述粒子滤波器中粒子的数量，
若无效粒子的数量等于所述粒子滤波器中粒子的数量，则重新初始化粒子滤波器，根据重新初始化后的所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标；
若无效粒子的数量不等于所述粒子滤波器中粒子的数量，则将所述粒子滤波器中无效粒子之外的粒子作为有效粒子；
根据所述有效粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算有效粒子与各障碍物的斥力场，并根据所述有效粒子移动后的坐标、所述目标坐标，计算有效粒子与目标点的引力场；
根据所述斥力场、所述引力场，确定各粒子的权重，具体包括：
根据所述斥力场、所述引力场，确定有效粒子的权重；
根据所述各粒子移动后的坐标、各粒子的权重，确定当前时刻的下一时刻机器人移动到达的坐标，具体包括：
根据所述有效粒子移动后的坐标、有效粒子的权重，确定当前时刻的下一时刻机器人移动到达的坐标。
3.根据权利要求2所述的一种机器人自动避障方法，其特征在于，根据所述各粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的斥力场，具体包括：
根据所述各粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的距离；
根据各粒子与各障碍物的距离，采用以下公式，计算各粒子与各障碍物的斥力场：
Figure 30450DEST_PATH_IMAGE008
其中，
Figure 24950DEST_PATH_IMAGE009
表示第k个粒子与第i个障碍物的斥力场，
Figure 369344DEST_PATH_IMAGE010
表示斥力因子，
Figure 883633DEST_PATH_IMAGE011
表示第k个粒子与第i个障碍物的距离，
Figure 920859DEST_PATH_IMAGE012
表示设定的斥力距离阈值，
Figure 137077DEST_PATH_IMAGE013
表示第k个粒子与各障碍物斥力场；
根据所述各粒子移动后的坐标、所述目标坐标，计算各粒子与目标点的引力场，具体包括：
根据所述各粒子移动后的坐标、所述目标坐标，计算各粒子与目标点的距离；
根据各粒子与目标点的距离，采用以下公式，计算各粒子与目标点的引力场：
Figure 285161DEST_PATH_IMAGE014
其中，
Figure 903225DEST_PATH_IMAGE015
表示第k个粒子与目标点的引力场，
Figure 111352DEST_PATH_IMAGE016
表示引力因子，
Figure 814866DEST_PATH_IMAGE017
表示k个粒子与目标点的距离。
4.根据权利要求3所述的一种机器人自动避障方法，其特征在于，根据所述各粒子移动后的坐标、所述各粒子的权重，确定当前时刻的下一时刻机器人移动到达的坐标，具体包括：
根据所述各粒子移动后的坐标、所述各粒子的权重，采用以下公式计算的机器人下一时刻移动到达的坐标：
Figure 501062DEST_PATH_IMAGE018
其中，
Figure 989943DEST_PATH_IMAGE019
表示第i个粒子在当前时刻t移动后的坐标，
Figure 368972DEST_PATH_IMAGE020
表示第i个粒子的权重，
Figure 559782DEST_PATH_IMAGE021
表示计算的机器人下一时刻t+1移动到达的坐标；
判断所计算的机器人下一时刻移动到达的坐标，是否处于可通行区域：
若是，则将所计算的机器人下一时刻移动到达的坐标，确定为当前时刻的下一时刻机器人移动到达的坐标；
若否，则将所述权重最大的粒子所对应的移动后坐标，确定为当前时刻的下一时刻机器人移动到达的坐标。
5.根据权利要求2所述的一种机器人自动避障方法，其特征在于，将所确定的所述当前时刻的下一时刻机器人移动到达的坐标作为所述当前时刻机器人的当前坐标，继续根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标，具体包括：
将所确定的所述当前时刻的下一时刻机器人移动到达的坐标作为所述当前时刻机器人的当前坐标；
判断无效粒子的数量是否为零，
若是，则继续根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标，
若否，则对所述粒子滤波器进行粒子重采样，得到重采样后的粒子滤波器，继续根据重采样后的所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标。
6.根据权利要求5所述的一种机器人自动避障方法，其特征在于，对所述粒子滤波器进行粒子重采样，具体包括：
将各有效粒子按对应的权重从大到小排序，所述排序中前若干个有效粒子形成第一重采样粒子集合；
以当前时刻机器人的当前坐标为中心，随机生成若干个粒子，随机生成的粒子形成第二重采样粒子集合；
将第一重采样粒子集合和第二重采样粒子集合的粒子作为所述粒子滤波器中的粒子，以完成粒子重采样。
7.一种机器人自动避障***，其特征在于，通过权利要求1至6中任一项所述方法实现机器人自动避障，包括：
初始坐标获取模块，用于获取机器人的初始坐标及目标点的目标坐标；
粒子滤波器初始化模块，用于根据所述机器人的初始坐标初始化粒子滤波器，以确定所述粒子滤波器中各粒子的初始坐标；
机器人移动到达坐标确定模块，用于获取当前时刻机器人的当前坐标，并根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标；
所述机器人移动到达坐标确定模块，具体包括：
障碍物坐标计算模块，用于检测当前时刻机器人与各障碍物的距离及方位角，并根据所述当前时刻机器人的当前坐标、所述当前时刻机器人与各障碍物的距离及方位角，计算当前时刻各障碍物的坐标；
粒子移动后的坐标确定模块，将所述粒子朝随机方向移动特定距离，并根据各粒子的初始坐标，确定各粒子移动后的坐标，所述特定距离为机器人在当前时刻到下一时刻的时间内移动的距离；
斥力场及引力场计算模块，用于根据所述各粒子移动后的坐标、所述当前时刻各障碍物的坐标，计算各粒子与各障碍物的斥力场，并根据所述各粒子移动后的坐标、所述目标坐标，计算各粒子与目标点的引力场；
权重确定模块，用于根据所述斥力场、所述引力场，确定各粒子的权重；
机器人移动到达坐标确定模块，用于根据所述各粒子移动后的坐标、各粒子的权重，确定当前时刻的下一时刻机器人移动到达的坐标；
目标点到达判断模块，用于判断机器人是否到达目标点，若是，则结束，若否，则将所确定的所述当前时刻的下一时刻机器人移动到达的坐标作为所述当前时刻机器人的当前坐标，继续根据所述粒子滤波器，确定当前时刻的下一时刻机器人移动到达的坐标。
8.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至6中任一项所述方法的步骤。
9.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至6中任一项所述的方法的步骤。
CN202111539782.2A 2021-12-16 2021-12-16 一种机器人自动避障方法、***、设备及介质 Active CN113934219B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111539782.2A CN113934219B (zh) 2021-12-16 2021-12-16 一种机器人自动避障方法、***、设备及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111539782.2A CN113934219B (zh) 2021-12-16 2021-12-16 一种机器人自动避障方法、***、设备及介质

Publications (2)

Publication Number Publication Date
CN113934219A CN113934219A (zh) 2022-01-14
CN113934219B true CN113934219B (zh) 2022-04-05

Family

ID=79288976

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111539782.2A Active CN113934219B (zh) 2021-12-16 2021-12-16 一种机器人自动避障方法、***、设备及介质

Country Status (1)

Country Link
CN (1) CN113934219B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115235479B (zh) * 2022-09-23 2022-12-06 江西省智能产业技术创新研究院 自动导引车的定位方法、装置、可读存储介质及电子设备

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102411371A (zh) * 2011-11-18 2012-04-11 浙江大学 一种基于多传感器服务机器人跟随***和方法
US10710579B2 (en) * 2017-06-01 2020-07-14 Waymo Llc Collision prediction system
CN108762264B (zh) * 2018-05-22 2021-05-04 重庆邮电大学 基于人工势场与滚动窗口的机器人的动态避障方法
CN111515932A (zh) * 2020-04-23 2020-08-11 东华大学 一种基于人工势场与强化学习的人机共融流水线实现方法
CN112577491A (zh) * 2020-12-14 2021-03-30 上海应用技术大学 一种基于改进人工势场法的机器人路径规划方法

Also Published As

Publication number Publication date
CN113934219A (zh) 2022-01-14

Similar Documents

Publication Publication Date Title
Hennes et al. Multi-robot collision avoidance with localization uncertainty
Althoff et al. Safety assessment of robot trajectories for navigation in uncertain and dynamic environments
CN112631294A (zh) 一种移动机器人智能路径规划方法
CN113341984A (zh) 基于改进rrt算法的机器人路径规划方法和装置
CN110488818B (zh) 一种基于激光雷达的机器人定位方法、装置和机器人
CN106919181A (zh) 一种无人机避障方法
CN109933067A (zh) 一种基于遗传算法和粒子群算法的无人艇避碰方法
CN111381600A (zh) 一种基于粒子群算法的uuv路径规划方法
CN110705385B (zh) 一种障碍物角度的检测方法、装置、设备及介质
CN113934219B (zh) 一种机器人自动避障方法、***、设备及介质
CN112540609A (zh) 路径规划方法、装置、终端设备及存储介质
CN109885046B (zh) 一种基于粒子滤波的移动机器人定位加速方法
Alves et al. Localization and navigation of a mobile robot in an office-like environment
CN115509231A (zh) 一种机器人跟随避障方法、装置及存储介质
WO2018068446A1 (zh) 一种追踪方法及追踪设备、计算机存储介质
CN108196540A (zh) 一种利用二阶梯度信息改善人工物理避障轨迹平滑度的方法
WO2022232415A1 (en) Reactive collision avoidance for autonomous vehicles considering physical constraints
CN114442628A (zh) 基于人工势场法的移动机器人路径规划方法、装置及***
CN112799385A (zh) 一种基于引导域人工势场的智能体路径规划方法
CN108490429B (zh) Tws雷达多目标跟踪方法及***
CN113988196A (zh) 一种机器人移动方法、装置、设备及存储介质
Doellinger et al. Environment-aware multi-target tracking of pedestrians
CN117419716A (zh) 无人机三维路径规划方法、***、存储介质和电子设备
Bis et al. Velocity occupancy space: Robot navigation and moving obstacle avoidance with sensor uncertainty
Song et al. Critical rays self-adaptive particle filtering SLAM

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