CN113359714B - 基于粒子滤波算法的巡检机器人动态路径规划方法及装置 - Google Patents

基于粒子滤波算法的巡检机器人动态路径规划方法及装置 Download PDF

Info

Publication number
CN113359714B
CN113359714B CN202110574036.0A CN202110574036A CN113359714B CN 113359714 B CN113359714 B CN 113359714B CN 202110574036 A CN202110574036 A CN 202110574036A CN 113359714 B CN113359714 B CN 113359714B
Authority
CN
China
Prior art keywords
path
inspection robot
local
particle filter
filter algorithm
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
CN202110574036.0A
Other languages
English (en)
Other versions
CN113359714A (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.)
State Grid Jiangsu Electric Power Co Ltd
Electric Power Research Institute of State Grid Jiangsu Electric Power Co Ltd
Original Assignee
State Grid Jiangsu Electric Power Co Ltd
Electric Power Research Institute of State Grid Jiangsu Electric Power Co Ltd
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 State Grid Jiangsu Electric Power Co Ltd, Electric Power Research Institute of State Grid Jiangsu Electric Power Co Ltd filed Critical State Grid Jiangsu Electric Power Co Ltd
Priority to CN202110574036.0A priority Critical patent/CN113359714B/zh
Publication of CN113359714A publication Critical patent/CN113359714A/zh
Application granted granted Critical
Publication of CN113359714B publication Critical patent/CN113359714B/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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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

Abstract

本发明公开了一种基于粒子滤波算法的巡检机器人动态路径规划方法、装置、设备及计算机可读存储介质，在进行巡检机器人动态路径规划操作同时采用粒子滤波算法进行环境态势分析，以保证规划过程对于动态突发障碍物具有一定的预见性；巡检机器人每执行一个规划步长，将基于传感器所获取的当前环境信息，采用粒子滤波算法对障碍物及巡检机器人的状态进行一次预测更新，根据更新后的环境信息，在每个路径滚动窗口内迭代执行预设次数粒子滤波算法优化局部路径，在提高了算法估计准确度的同时，加快了最优解的收敛过程；将粒子滤波算法与滚动优化策略结合，采用滚动窗口对路径分段局部规划，在保证规划过程预见性的同时减少了全局路径规划的计算量。

Description

基于粒子滤波算法的巡检机器人动态路径规划方法及装置
技术领域
本发明涉及机器人路径规划技术领域，特别是涉及一种基于粒子滤波算法的巡检机器人动态路径规划方法、装置、设备以及计算机可读存储介质。
背景技术
在大规模电力监测系统中，智能巡检机器人因其灵活的控制运行方式、不受天气因素影响等优点，逐渐代替人工巡检完成户外高压设备的巡检任务。对于机载能源有限的巡检机器人而言，既要在执行任务时快速准确地选取的合理路径，还要求当出现动态障碍物、检测任务突发变动时，能根据机器人当前位置，实时、准确、快速寻找一条最优路径并到达任务点进行监测。因此动态路径规划是智能巡检机器人在工作过程中需要解决的关键问题。
动态路径规划是机器人、无人驾驶、航空航天等领域的研究热点，其目的是在动态时变场景中按照某种评价指标对从起点到终点的目标路径进行在线实时调整。目前国内外许多研究者们针对巡检机器人的动态路径规划开展了大量研究，现有路径规划方法主要可分为图搜索算法、采样规划算法、智能优化算法、人工势场法等几类。其中，图搜索算法对环境反应迅速、搜索直接，但随着环境规模的增大，算法实时性会较差；采样规划算法搜索能力强、且对地图的预处理没有要求，但搜索时盲目性大，在高维环境下或动态环境中会存在计算复杂度高、易陷入死区等问题；智能优化算法具有很强的鲁棒性和适应性，但由于本身存在早熟收敛和局部搜索能力较弱等问题，同样易出现局部最优解和陷入死锁；人工势场法同样存在局部最优的问题，且引力场的设计是算法能否成功应用的关键。虽然现有的方法在机器人路径规划中已有较为广泛的应用，但面对动态复杂环境依然存在自身局限性。
综上所述可以看出，针对巡检机器人在电力监测系统中执行任务时面对的多种动态变化的任务场景，如何满足路径规划实时性和安全性的要求是目前有待解决的问题。
发明内容
本发明的目的是提供一种基于粒子滤波算法的巡检机器人动态路径规划方法、装置、设备以及计算机可读存储介质，以解决现有技术中所提供的动态路径规划算法，针对巡检机器人在电力监测系统中执行任务时面对的多种动态变化的任务场景，具有自身局限性，存在实时性差且易出现局部最优解的问题。
为解决上述技术问题，本发明提供一种基于粒子滤波算法的巡检机器人动态路径规划方法，包括：S101：对巡检机器人任务场景创建地图，建立路径滚动窗口；S102：利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；S103：根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；S104：根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；S105：在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；S106：在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；S107：判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行S102-S107直至所述巡检机器人到达所述任务终点。
优选地，所述根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点包括：
根据所述巡检机器人的当前位置与任务终点，确定所述路径巡检窗口内的基准点；
根据所述基准点，确定所述巡检机器人的局部任务点，从而确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点。
优选地，所述根据所述巡检机器人的当前位置与任务终点，确定所述路径巡检窗口内的基准点包括：
确定所述巡检机器人的当前位置与所述任务终点的连线，选取所述连线与所述路径滚动窗口的交汇点作为所述基准点。
优选地，所述根据所述基准点，确定所述巡检机器人的局部任务点包括：
以所述基准点为中心，在预设生成范围内选取所述巡检机器人的局部任务点；
其中，所述预设生成范围服从标准差为
Figure GDA0003645507840000021
的高斯分布，L0
为全局路径长度，Lk为当前时刻的剩余待规划路径长度，δmax为所述预设生成范围的最大标准差。
优选地，所述在所述路径滚动窗口内建立局部坐标系包括：
以所述巡检机器人的当前位置与所述基准点的连线作为坐标系横轴，在所述路径滚动窗口内建立局部坐标系，以便采用一组N维向量表示一条路径样本X＝[x0，x1，…，xD]，其中，D＝L/s为一次滚动规划的步数，L为所述路径滚动窗口的大小，s为规划步长，N＝D+1，x0为所述巡检机器人的当前位置，x1为所述巡检机器人的局部路径点，xD为所述巡检机器人的局部路径终点。
优选地，所述根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集包括：
根据所述更新后的环境信息和所述局部路径起止点，采用正交化算法在所述局部坐标系中生成所述巡检机器人的初始路径样本集。
优选地，所述在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步包括：
S701：将所述初始路径样本集输入至粒子滤波算法估计路径样本，并记录所述粒子滤波算法的当前迭代次数；
S702：根据路径评价函数计算每个路径样本的权值，并根据所述每个路径样本的权值估计当前最优路径样本；
S703：判断所述当前迭代次数是否达到预设次数；
S704：若所述当前迭代次数未达到所述预设次数，则在所述路径滚动窗口内对所述巡检机器人的局部路径点进行重采样，得到更新后的路径样本集与当前迭代次数，返回执行S702；
S705：若所述当前迭代次数达到所述预设次数，则输出所述当前最优路径样本作为目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步。
本发明还提供了一种基于粒子滤波算法的巡检机器人动态路径规划装置，包括：
建立模块，用于对巡检机器人任务场景创建地图，建立路径滚动窗口；
获取模块，用于利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；
预测模块，用于根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；
确定模块，用于根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
生成模块，用于在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；
迭代模块，用于在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；
判断模块，用于判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行所述获取模块至所述判断模块的操作步骤直至所述巡检机器人到达所述任务终点。
本发明还提供了一种基于粒子滤波算法的巡检机器人动态路径规划设备，包括：
存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现上述一种基于粒子滤波算法的巡检机器人动态路径规划方法的步骤。
本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述一种基于粒子滤波算法的巡检机器人动态路径规划方法的步骤。
本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法，在进行动态路径规划操作同时将采用粒子滤波算法进行环境态势分析，在实际应用中，巡检机器人的运行环境存在多种类型的障碍物，为保证路径规划过程对于动态突发障碍物具有一定的预见性，所述巡检机器人每执行一个规划步长，将基于当前传感器所获取的环境信息，采用粒子滤波算法对动态障碍物及机器人的状态进行一次预测更新；进一步根据所估计的障碍物及机器人的位置和移动速度采用改进粒子滤波算法调整局部路径，即在所述路径滚动窗口中迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，得到目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步。本发明所提供的方法，在每个路径滚动窗口内迭代计算粒子滤波，既达到了搜索收敛的优化效果，又可保证对可行路径估计的准确度。本发明将改进的粒子滤波算法与滚动优化策略结合，采用路径滚动窗口对路径分段局部规划，以多步规划、单步执行的方式完成路径的在线实时规划，在保证规划过程预见性的同时一定程度上减少了全局路径规划的计算量。本发明能有效应对巡检机器人在执行电力系统监测时面对的多种动态变化的任务场景，且满足其路径规划的实时性与安全性的要求。
附图说明
为了更清楚的说明本发明实施例或现有技术的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法的第一种具体实施例的流程图；
图2为本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法的第二种具体实施例的流程图；
图3为本发明实施例提供的一种基于粒子滤波算法的巡检机器人动态路径规划装置的结构框图。
具体实施方式
本发明的核心是提供一种基于粒子滤波算法的巡检机器人动态路径规划方法、装置、设备以及计算机可读存储介质，可有效应对巡检机器人在执行电力系统监测时面对的多种动态变化的任务场景，针对环境中遇到的动态障碍物能在线实时调整路径，在保证规划路径安全性和可行性的同时，对环境具有一定的预见性。
为了使本技术领域的人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参考图1，图1为本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法的第一种具体实施例的流程图；具体操作步骤如下：
步骤S101：对巡检机器人任务场景创建地图，建立路径滚动窗口；
步骤S102：利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；
步骤S103：根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；
设所述任务场景中的障碍物和所述巡检机器人运动模型如下：
Figure GDA0003645507840000051
其中，Xk为目标k时刻的状态向量，Xk-1为k-1时刻的状态向量，Fk为k时刻状态转移矩阵，wk为k时刻的过程噪声，Zk为目标k时刻的观测向量，Hk为k时刻的观测矩阵，vk为k时刻的观测噪声。
在算法初始阶段，按照高斯均匀分布在目标状态空间中生成初始样本。由已知的状态先验概率P(X)得到样本均值
Figure GDA0003645507840000061
和方差P，进一步基于状态统计量产生服从高斯均匀分布的随机样本
Figure GDA0003645507840000062
i＝1,2,...,N，其中randn为与状态量同维度的高斯分布随机数。
更新粒子权值，通过传感器测量得到环境中障碍物和巡检机器人包含位置和速度的观测信息
Figure GDA0003645507840000063
后，结合目标观测信息和观测模型，根据权值更新公式计算粒子新的权值：
Figure GDA0003645507840000064
其中，xk为目标k时刻在x方向上的位置信息，
Figure GDA0003645507840000065
为目标k时刻在x方向上的速度信息，yk为目标k时刻在y方向上的位置信息，
Figure GDA0003645507840000066
为目标k时刻在y方向上的位置信息，
Figure GDA0003645507840000067
为k时刻第i个粒子，
Figure GDA0003645507840000068
为k-1时刻第i个粒子，Zk为k时刻的观测信息，
Figure GDA0003645507840000069
为k-1时刻第i个粒子的权值。
计算粒子加权和，获得状态估计：
Figure GDA00036455078400000610
对粒子集进行重采样，优选权值大的粒子，保留一部分小权值粒子。根据粒子权值从粒子集
Figure GDA00036455078400000611
重新抽取N个粒子
Figure GDA00036455078400000612
并令
Figure GDA00036455078400000613
建立新的粒子集
Figure GDA00036455078400000614
预测目标状态，根据目标运动的状态方程预测下一时刻目标状态Xk (i)。
步骤S104：根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
对每一时刻的路径滚动窗口，基准点根据所述巡检机器人的当前位置、任务终点和所述路径滚动窗口大小综合确定。确定所述巡检机器人的当前位置与所述任务终点的连线，选取所述连线与所述路径滚动窗口的交汇点作为所述基准点。
根据所述基准点，确定所述巡检机器人的局部任务点，从而确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点。其中，所述局部任务点在以所述基准点为中心的预设生成范围内选取。假定所述预设生成范围服从标准差为δ的高斯分布，随着规划过程不断向所述任务终点推进，所述局部任务点的预设生成范围将逐渐减小，其调整过程可表示为：
Figure GDA0003645507840000071
其中，L0为全局路径长度，Lk为当前时刻的剩余待规划路径长度，δmax为所述预设生成范围的最大标准差。
步骤S105：在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；
假设所述路径滚动窗口的大小为L，规划步长为s，则一次滚动规划的步数为D＝L/s，相应的可得到的样本维数为N＝D+1。以所述巡检机器人的当前位置与所述基准点的连线作为坐标横轴，对所述路径滚动窗口建立局部坐标系，使每个局部路径点只在对应的垂线上下滑动，则局部路径可以一组坐标点表示，每个路径样本即代表一条局部待选路径X＝[x0，x1，…，xD]，其中，x0为所述巡检机器人的当前位置，x1为所述巡检机器人的局部路径点，xD为所述巡检机器人的局部路径终点。
步骤S106：在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；
步骤S107：判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行S102-S107直至所述巡检机器人到达所述任务终点。
由于实际应用中，巡检机器人的运行环境存在多种类型的障碍物，包括静态障碍、移动障碍、禁行区、突发障碍等，为保证规划过程对于动态突发障碍物具有一定的预见性，本发明实施例在进行动态路径规划操作同时采用粒子滤波算法进行环境态势分析，巡检机器人每执行一个规划步长，将基于当前传感器所获取的环境信息，采用粒子滤波算法对障碍物及机器人的状态进行一次预测更新，进一步根据所估计的障碍物及机器人的位置和移动速度，采用改进的粒子滤波算法调整局部路径，即通过设置迭代次数，在每个路径滚动窗口内重复执行粒子滤波算法估计路径样本，搜索目标局部路径，既达到了搜索收敛的优化效果，又可保证对可行路径估计的准确度。
基于上述实施例，在本实施例中，根据巡检机器人当前位置和当前路径滚动窗口内的基准点，采用正交化思想生成初始路径样本，引入迭代计算机制，在每个滚动窗口内迭代执行粒子滤波估计路径样本，根据路径评价函数计算每个路径样本的权值，并估计最优路径样本。
请参考图2，图2为本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法的第二种具体实施例的流程图；具体操作步骤如下：
步骤S201：对巡检机器人任务场景创建地图，建立路径滚动窗口；
步骤S202：根据传感器获取的当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；
步骤S203：根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
步骤S204：在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，采用正交化算法在所述局部坐标系中生成所述巡检机器人的初始路径样本集；
步骤S205：将所述初始路径样本集输入至粒子滤波算法估计路径样本，并记录所述粒子滤波算法的当前迭代次数；
步骤S206：根据路径评价函数计算每个路径样本的权值，并根据所述每个路径样本的权值估计当前最优路径样本；
步骤S207：判断所述当前迭代次数是否达到预设次数；
步骤S208：若所述当前迭代次数未达到所述预设次数，则在所述路径滚动窗口内对所述巡检机器人的局部路径点进行重采样，得到更新后的路径样本集与当前迭代次数，返回执行S206；
步骤S209：若所述当前迭代次数达到所述预设次数，则输出所述当前最优路径样本作为目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；
步骤S210：判断所述巡检机器人是否到达所述任务终点，若所述巡检机器人到达所述任务终点，则完成所述巡检机器人的动态路径规划操作。
根据所述巡检机器人的当前位置和所述基准点，采用正交化思想生成初始路径样本
Figure GDA0003645507840000081
引入迭代计算机制，在每个滚动窗口内迭代执行粒子滤波估计路径样本，根据路径评价函数计算每个路径样本的权值，并估计当前最优路径样本
Figure GDA0003645507840000091
判断所述路径滚动窗口内的粒子滤波算法是否满足迭代结束条件，若当前迭代次数达到预设迭代上限，则结束迭代计算过程并输出所述当前最优路径样本作为目标局部路径
Figure GDA0003645507840000092
确定所述巡检机器人下一步待执行路径点；否则重采样更新路径样本集
Figure GDA0003645507840000093
并继续下一轮迭代计算。
为提升粒子滤波算法对状态量的估计准确度、加快最优解的收敛过程，本实施例引入了启发式优化的迭代计算机制对粒子滤波算法的计算过程进行改进，通过设置迭代次数，将在每个路径滚动窗口内重复执行粒子滤波估计路径样本，当达到所设定的终止条件，才会停止重复计算并更新局部路径，路径滚动窗口则继续向前推进。
本发明实施例所提供的方法，能够有效应对巡检机器人在电力监测系统中执行任务时面对的多种动态变化的任务场景，针对环境中遇到的动态障碍物能在线实时调整路径，在保证规划路径安全性和可行性的同时，对环境具有一定的预见性。
请参考图3，图3为本发明实施例提供的一种基于粒子滤波算法的巡检机器人动态路径规划装置的结构框图；具体装置可以包括：
建立模块100，用于对巡检机器人任务场景创建地图，建立路径滚动窗口；
获取模块200，用于利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；
预测模块300，用于根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；
确定模块400，用于根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
生成模块500，用于在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；
迭代模块600，用于在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；
判断模块700，用于判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行所述获取模块至所述判断模块的操作步骤直至所述巡检机器人到达所述任务终点。
本实施例的基于粒子滤波算法的巡检机器人动态路径规划装置用于实现前述的基于粒子滤波算法的巡检机器人动态路径规划方法，因此基于粒子滤波算法的巡检机器人动态路径规划装置中的具体实施方式可见前文中的基于粒子滤波算法的巡检机器人动态路径规划方法的实施例部分，例如，建立模块100，获取模块200，预测模块300，确定模块400，生成模块500，迭代模块600，判断模块700，分别用于实现上述基于粒子滤波算法的巡检机器人动态路径规划方法中步骤S101，S102，S103，S104，S105，S106和S107，所以，其具体实施方式可以参照相应的各个部分实施例的描述，在此不再赘述。
本发明具体实施例还提供了一种基于粒子滤波算法的巡检机器人动态路径规划设备，包括：存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现上述一种基于粒子滤波算法的巡检机器人动态路径规划方法的步骤。
本发明具体实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述一种基于粒子滤波算法的巡检机器人动态路径规划方法的步骤。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上对本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法、装置、设备以及计算机可读存储介质进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (7)

1.一种基于粒子滤波算法的巡检机器人动态路径规划方法，其特征在于，包括：
S101：对巡检机器人任务场景创建地图，建立路径滚动窗口；
S102：利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；
S103：根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；
S104：根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
S105：在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；
S106：在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；
S107：判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行S102-S107直至所述巡检机器人到达所述任务终点；
所述根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点包括：
根据所述巡检机器人的当前位置与任务终点，确定所述路径巡检窗口内的基准点；
根据所述基准点，确定所述巡检机器人的局部任务点，从而确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
所述根据所述巡检机器人的当前位置与任务终点，确定所述路径巡检窗口内的基准点包括：
确定所述巡检机器人的当前位置与所述任务终点的连线，选取所述连线与所述路径滚动窗口的交汇点作为所述基准点；
所述根据所述基准点，确定所述巡检机器人的局部任务点包括：
以所述基准点为中心，在预设生成范围内选取所述巡检机器人的局部任务点；
其中，所述预设生成范围服从标准差为
Figure FDA0003645507830000011
的高斯分布，L0为全局路径长度，Lk为当前时刻的剩余待规划路径长度，δmax为所述预设生成范围的最大标准差。
2.如权利要求1所述的方法，其特征在于，所述在所述路径滚动窗口内建立局部坐标系包括：
以所述巡检机器人的当前位置与所述基准点的连线作为坐标系横轴，在所述路径滚动窗口内建立局部坐标系，以便采用一组N维向量表示一条路径样本X＝[x0，x1，…，xD]，其中，D＝L/s为一次滚动规划的步数，L为所述路径滚动窗口的大小，s为规划步长，N＝D+1，x0为所述巡检机器人的当前位置，x1为所述巡检机器人的局部路径点，xD为所述巡检机器人的局部路径终点。
3.如权利要求2所述的方法，其特征在于，所述根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集包括：
根据所述更新后的环境信息和所述局部路径起止点，采用正交化算法在所述局部坐标系中生成所述巡检机器人的初始路径样本集。
4.如权利要求1所述的方法，其特征在于，所述在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步包括：
S701：将所述初始路径样本集输入至粒子滤波算法估计路径样本，并记录所述粒子滤波算法的当前迭代次数；
S702：根据路径评价函数计算每个路径样本的权值，并根据所述每个路径样本的权值估计当前最优路径样本；
S703：判断所述当前迭代次数是否达到预设次数；
S704：若所述当前迭代次数未达到所述预设次数，则在所述路径滚动窗口内对所述巡检机器人的局部路径点进行重采样，得到更新后的路径样本集与当前迭代次数，返回执行S702；
S705：若所述当前迭代次数达到所述预设次数，则输出所述当前最优路径样本作为目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步。
5.一种基于粒子滤波算法的巡检机器人动态路径规划装置，采用如所述权利要求1～4任一项所述的一种基于粒子滤波算法的巡检机器人动态路径规划方法，其特征在于，包括：建立模块，用于对巡检机器人任务场景创建地图，建立路径滚动窗口；获取模块，用于利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；
预测模块，用于根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；
确定模块，用于根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
生成模块，用于在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；
迭代模块，用于在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；
判断模块，用于判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行所述获取模块至所述判断模块的操作步骤直至所述巡检机器人到达所述任务终点。
6.一种基于粒子滤波算法的巡检机器人动态路径规划设备，其特征在于，包括：
存储器，用于存储计算机程序；
处理器，用于执行所述计算机程序时实现如权利要求1至4任一项所述一种基于粒子滤波算法的巡检机器人动态路径规划方法的步骤。
7.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至4任一项所述一种基于粒子滤波算法的巡检机器人动态路径规划方法的步骤。
CN202110574036.0A 2021-05-25 2021-05-25 基于粒子滤波算法的巡检机器人动态路径规划方法及装置 Active CN113359714B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110574036.0A CN113359714B (zh) 2021-05-25 2021-05-25 基于粒子滤波算法的巡检机器人动态路径规划方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110574036.0A CN113359714B (zh) 2021-05-25 2021-05-25 基于粒子滤波算法的巡检机器人动态路径规划方法及装置

Publications (2)

Publication Number Publication Date
CN113359714A CN113359714A (zh) 2021-09-07
CN113359714B true CN113359714B (zh) 2022-07-08

Family

ID=77527810

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110574036.0A Active CN113359714B (zh) 2021-05-25 2021-05-25 基于粒子滤波算法的巡检机器人动态路径规划方法及装置

Country Status (1)

Country Link
CN (1) CN113359714B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113568435B (zh) * 2021-09-24 2021-12-24 深圳火眼智能有限公司 一种基于无人机自主飞行态势感知趋势的分析方法与系统
CN116954234B (zh) * 2023-09-06 2023-12-01 江苏苏亿盟智能科技有限公司 一种机器人巡检控制方法及系统

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103823466B (zh) * 2013-05-23 2016-08-10 电子科技大学 一种动态环境下移动机器人路径规划方法
CN109959377A (zh) * 2017-12-25 2019-07-02 北京东方兴华科技发展有限责任公司 一种机器人导航定位系统及方法
CN111752275A (zh) * 2020-06-19 2020-10-09 五邑大学 一种用于机器人的自动巡航方法、装置和存储介质
CN112631294B (zh) * 2020-12-16 2022-06-28 上海应用技术大学 一种移动机器人智能路径规划方法

Also Published As

Publication number Publication date
CN113359714A (zh) 2021-09-07

Similar Documents

Publication Publication Date Title
Xuexi et al. SLAM algorithm analysis of mobile robot based on lidar
CN110244715B (zh) 一种基于超宽带技术的多移动机器人高精度协同跟踪方法
CN111546347B (zh) 一种适用于动态环境下的机械臂路径规划方法
CN113359714B (zh) 基于粒子滤波算法的巡检机器人动态路径规划方法及装置
CN109798896B (zh) 一种室内机器人定位与建图方法及装置
CN110703762A (zh) 一种复杂环境下水面无人艇混合路径规划方法
CN111427047B (zh) 一种大场景下自主移动机器人slam方法
CN112099493A (zh) 一种自主移动机器人轨迹规划方法、系统及设备
CN113359718B (zh) 移动机器人全局路径规划与局部路径规划融合方法及设备
CN112859866A (zh) 机器人滚动路径规划方法、系统、存储介质、设备及应用
Shojaei et al. Experimental study of iterated Kalman filters for simultaneous localization and mapping of autonomous mobile robots
Vonásek et al. RRT-path–a guided rapidly exploring random tree
Gao et al. Multi-mobile robot autonomous navigation system for intelligent logistics
Tang et al. Robot tracking in SLAM with Masreliez-Martin unscented Kalman filter
Doria et al. An algorithm inspired by the deterministic annealing approach to avoid local minima in artificial potential fields
Mohamed et al. Autonomous navigation of agvs in unknown cluttered environments: log-mppi control strategy
CN113359763A (zh) 轮式机器人及动态障碍物轨迹预测方法、装置、存储介质
CN111711432A (zh) 一种基于ukf和pf混合滤波的目标跟踪算法
CN112857379A (zh) 一种基于改进的Gmapping-SLAM地图更新方法及系统
Farag Lidar and radar fusion for real-time road-objects detection and tracking
CN115826586B (zh) 一种融合全局算法和局部算法的路径规划方法及系统
Li et al. Path planning of mobile robot based on improved td3 algorithm
Suzuki Dynamic state estimation in vehicle platoon system by applying particle filter and unscented kalman filter
CN115454061A (zh) 一种基于3d技术的机器人路径避障方法及系统
Wang et al. Research on SLAM road sign observation based on particle filter

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