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

1.本发明涉及机器人路径规划技术领域，特别是涉及一种基于粒子滤波算法的巡检机器人动态路径规划方法、装置、设备以及计算机可读存储介质。


背景技术：

2.在大规模电力监测系统中，智能巡检机器人因其灵活的控制运行方式、不受天气因素影响等优点，逐渐代替人工巡检完成户外高压设备的巡检任务。对于机载能源有限的巡检机器人而言，既要在执行任务时快速准确地选取的合理路径，还要求当出现动态障碍物、检测任务突发变动时，能根据机器人当前位置，实时、准确、快速寻找一条最优路径并到达任务点进行监测。因此动态路径规划是智能巡检机器人在工作过程中需要解决的关键问题。
3.动态路径规划是机器人、无人驾驶、航空航天等领域的研究热点，其目的是在动态时变场景中按照某种评价指标对从起点到终点的目标路径进行在线实时调整。目前国内外许多研究者们针对巡检机器人的动态路径规划开展了大量研究，现有路径规划方法主要可分为图搜索算法、采样规划算法、智能优化算法、人工势场法等几类。其中，图搜索算法对环境反应迅速、搜索直接，但随着环境规模的增大，算法实时性会较差；采样规划算法搜索能力强、且对地图的预处理没有要求，但搜索时盲目性大，在高维环境下或动态环境中会存在计算复杂度高、易陷入死区等问题；智能优化算法具有很强的鲁棒性和适应性，但由于本身存在早熟收敛和局部搜索能力较弱等问题，同样易出现局部最优解和陷入死锁；人工势场法同样存在局部最优的问题，且引力场的设计是算法能否成功应用的关键。虽然现有的方法在机器人路径规划中已有较为广泛的应用，但面对动态复杂环境依然存在自身局限性。
4.综上所述可以看出，针对巡检机器人在电力监测系统中执行任务时面对的多种动态变化的任务场景，如何满足路径规划实时性和安全性的要求是目前有待解决的问题。


技术实现要素：

5.本发明的目的是提供一种基于粒子滤波算法的巡检机器人动态路径规划方法、装置、设备以及计算机可读存储介质，以解决现有技术中所提供的动态路径规划算法，针对巡检机器人在电力监测系统中执行任务时面对的多种动态变化的任务场景，具有自身局限性，存在实时性差且易出现局部最优解的问题。
6.为解决上述技术问题，本发明提供一种基于粒子滤波算法的巡检机器人动态路径规划方法，包括：s101：对巡检机器人任务场景创建地图，建立路径滚动窗口；s102：利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；s103：根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；s104：根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；s105：在所述路径滚动窗口内建立局部坐标系，根据所述更新
后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；s106：在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；s107：判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行s102
‑
s107直至所述巡检机器人到达所述任务终点。
7.优选地，所述根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点包括：
8.根据所述巡检机器人的当前位置与任务终点，确定所述路径巡检窗口内的基准点；
9.根据所述基准点，确定所述巡检机器人的局部任务点，从而确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点。
10.优选地，所述根据所述巡检机器人的当前位置与任务终点，确定所述路径巡检窗口内的基准点包括：
11.确定所述巡检机器人的当前位置与所述任务终点的连线，选取所述连线与所述路径滚动窗口的交汇点作为所述基准点。
12.优选地，所述根据所述基准点，确定所述巡检机器人的局部任务点包括：
13.以所述基准点为中心，在预设生成范围内选取所述巡检机器人的局部任务点；
14.其中，所述预设生成范围服从标准差为的高斯分布，l015.为全局路径长度，l
k
为当前时刻的剩余待规划路径长度，δ
max
为所述预设生成范围的最大标准差。
16.优选地，所述在所述路径滚动窗口内建立局部坐标系包括：
17.以所述巡检机器人的当前位置与所述基准点的连线作为坐标系横轴，在所述路径滚动窗口内建立局部坐标系，以便采用一组n维向量表示一条路径样本x＝[x0，x1，
…
，xd]，其中，d＝l/s为一次滚动规划的步数，l为所述路径滚动窗口的大小，s为规划步长，n＝d+1，x0为所述巡检机器人的当前位置，x1为所述巡检机器人的局部路径点，x
d
为所述巡检机器人的局部路径终点。
[0018]
优选地，所述根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集包括：
[0019]
根据所述更新后的环境信息和所述局部路径起止点，采用正交化算法在所述局部坐标系中生成所述巡检机器人的初始路径样本集。
[0020]
优选地，所述在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步包括：
[0021]
s701：将所述初始路径样本集输入至粒子滤波算法估计路径样本，并记录所述粒子滤波算法的当前迭代次数；
[0022]
s702：根据路径评价函数计算每个路径样本的权值，并根据所述每个路径样本的权值估计当前最优路径样本；
[0023]
s703：判断所述当前迭代次数是否达到预设次数；
[0024]
s704：若所述当前迭代次数未达到所述预设次数，则在所述路径滚动窗口内对所述巡检机器人的局部路径点进行重采样，得到更新后的路径样本集与当前迭代次数，返回执行s702；
[0025]
s705：若所述当前迭代次数达到所述预设次数，则输出所述当前最优路径样本作为目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步。
[0026]
本发明还提供了一种基于粒子滤波算法的巡检机器人动态路径规划装置，包括：
[0027]
建立模块，用于对巡检机器人任务场景创建地图，建立路径滚动窗口；
[0028]
获取模块，用于利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；
[0029]
预测模块，用于根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；
[0030]
确定模块，用于根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
[0031]
生成模块，用于在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；
[0032]
迭代模块，用于在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；
[0033]
判断模块，用于判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行所述获取模块至所述判断模块的操作步骤直至所述巡检机器人到达所述任务终点。
[0034]
本发明还提供了一种基于粒子滤波算法的巡检机器人动态路径规划设备，包括：
[0035]
存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现上述一种基于粒子滤波算法的巡检机器人动态路径规划方法的步骤。
[0036]
本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述一种基于粒子滤波算法的巡检机器人动态路径规划方法的步骤。
[0037]
本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法，在进行动态路径规划操作同时将采用粒子滤波算法进行环境态势分析，在实际应用中，巡检机器人的运行环境存在多种类型的障碍物，为保证路径规划过程对于动态突发障碍物具有一定的预见性，所述巡检机器人每执行一个规划步长，将基于当前传感器所获取的环境信息，采用粒子滤波算法对动态障碍物及机器人的状态进行一次预测更新；进一步根据所估计的障碍物及机器人的位置和移动速度采用改进粒子滤波算法调整局部路径，即在所述路径滚动窗口中迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，得到目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步。本发明所提供的方法，在每个路径滚动窗口内迭代计算粒子滤波，既达到了搜索收敛的优化效果，又可保证对可行路径估计的准确度。本发明将改进的粒子滤波算法与滚动优化策略结合，采用路径滚动窗口对路径
分段局部规划，以多步规划、单步执行的方式完成路径的在线实时规划，在保证规划过程预见性的同时一定程度上减少了全局路径规划的计算量。本发明能有效应对巡检机器人在执行电力系统监测时面对的多种动态变化的任务场景，且满足其路径规划的实时性与安全性的要求。
附图说明
[0038]
为了更清楚的说明本发明实施例或现有技术的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0039]
图1为本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法的第一种具体实施例的流程图；
[0040]
图2为本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法的第二种具体实施例的流程图；
[0041]
图3为本发明实施例提供的一种基于粒子滤波算法的巡检机器人动态路径规划装置的结构框图。
具体实施方式
[0042]
本发明的核心是提供一种基于粒子滤波算法的巡检机器人动态路径规划方法、装置、设备以及计算机可读存储介质，可有效应对巡检机器人在执行电力系统监测时面对的多种动态变化的任务场景，针对环境中遇到的动态障碍物能在线实时调整路径，在保证规划路径安全性和可行性的同时，对环境具有一定的预见性。
[0043]
为了使本技术领域的人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0044]
请参考图1，图1为本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法的第一种具体实施例的流程图；具体操作步骤如下：
[0045]
步骤s101：对巡检机器人任务场景创建地图，建立路径滚动窗口；
[0046]
步骤s102：利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；
[0047]
步骤s103：根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；
[0048]
设所述任务场景中的障碍物和所述巡检机器人运动模型如下：
[0049][0050]
其中，x
k
为目标k时刻的状态向量，x
k
‑1为k
‑
1时刻的状态向量，f
k
为k时刻状态转移矩阵，w
k
为k时刻的过程噪声，z
k
为目标k时刻的观测向量，h
k
为k时刻的观测矩阵，v
k
为k时刻的观测噪声。
[0051]
在算法初始阶段，按照高斯均匀分布在目标状态空间中生成初始样本。由已知的状态先验概率p(x)得到样本均值和方差p，进一步基于状态统计量产生服从高斯均匀分布的随机样本其中randn为与状态量同维度的高斯分布随机数。
[0052]
更新粒子权值，通过传感器测量得到环境中障碍物和巡检机器人包含位置和速度的观测信息后，结合目标观测信息和观测模型，根据权值更新公式计算粒子新的权值：
[0053][0054]
其中，x
k
为目标k时刻在x方向上的位置信息，为目标k时刻在x方向上的速度信息，y
k
为目标k时刻在y方向上的位置信息，为目标k时刻在y方向上的位置信息，为k时刻第i个粒子，为k
‑
1时刻第i个粒子，z
k
为k时刻的观测信息，为k
‑
1时刻第i个粒子的权值。
[0055]
计算粒子加权和，获得状态估计：
[0056][0057]
对粒子集进行重采样，优选权值大的粒子，保留一部分小权值粒子。根据粒子权值从粒子集重新抽取n个粒子并令建立新的粒子集
[0058]
预测目标状态，根据目标运动的状态方程预测下一时刻目标状态
[0059]
步骤s104：根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
[0060]
对每一时刻的路径滚动窗口，基准点根据所述巡检机器人的当前位置、任务终点和所述路径滚动窗口大小综合确定。确定所述巡检机器人的当前位置与所述任务终点的连线，选取所述连线与所述路径滚动窗口的交汇点作为所述基准点。
[0061]
根据所述基准点，确定所述巡检机器人的局部任务点，从而确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点。其中，所述局部任务点在以所述基准点为中心的预设生成范围内选取。假定所述预设生成范围服从标准差为δ的高斯分布，随着规划过程不断向所述任务终点推进，所述局部任务点的预设生成范围将逐渐减小，其调整过程可表示为：
[0062][0063]
其中，l0为全局路径长度，l
k
为当前时刻的剩余待规划路径长度，δ
max
为所述预设生
成范围的最大标准差。
[0064]
步骤s105：在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；
[0065]
假设所述路径滚动窗口的大小为l，规划步长为s，则一次滚动规划的步数为d＝l/s，相应的可得到的样本维数为n＝d+1。以所述巡检机器人的当前位置与所述基准点的连线作为坐标横轴，对所述路径滚动窗口建立局部坐标系，使每个局部路径点只在对应的垂线上下滑动，则局部路径可以一组坐标点表示，每个路径样本即代表一条局部待选路径x＝[x0，x1，
…
，x
d
]，其中，x0为所述巡检机器人的当前位置，x1为所述巡检机器人的局部路径点，x
d
为所述巡检机器人的局部路径终点。
[0066]
步骤s106：在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；
[0067]
步骤s107：判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行s102
‑
s107直至所述巡检机器人到达所述任务终点。
[0068]
由于实际应用中，巡检机器人的运行环境存在多种类型的障碍物，包括静态障碍、移动障碍、禁行区、突发障碍等，为保证规划过程对于动态突发障碍物具有一定的预见性，本发明实施例在进行动态路径规划操作同时采用粒子滤波算法进行环境态势分析，巡检机器人每执行一个规划步长，将基于当前传感器所获取的环境信息，采用粒子滤波算法对障碍物及机器人的状态进行一次预测更新，进一步根据所估计的障碍物及机器人的位置和移动速度，采用改进的粒子滤波算法调整局部路径，即通过设置迭代次数，在每个路径滚动窗口内重复执行粒子滤波算法估计路径样本，搜索目标局部路径，既达到了搜索收敛的优化效果，又可保证对可行路径估计的准确度。
[0069]
基于上述实施例，在本实施例中，根据巡检机器人当前位置和当前路径滚动窗口内的基准点，采用正交化思想生成初始路径样本，引入迭代计算机制，在每个滚动窗口内迭代执行粒子滤波估计路径样本，根据路径评价函数计算每个路径样本的权值，并估计最优路径样本。
[0070]
请参考图2，图2为本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法的第二种具体实施例的流程图；具体操作步骤如下：
[0071]
步骤s201：对巡检机器人任务场景创建地图，建立路径滚动窗口；
[0072]
步骤s202：根据传感器获取的当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；
[0073]
步骤s203：根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
[0074]
步骤s204：在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，采用正交化算法在所述局部坐标系中生成所述巡检机器人的初始路径样本集；
[0075]
步骤s205：将所述初始路径样本集输入至粒子滤波算法估计路径样本，并记录所述粒子滤波算法的当前迭代次数；
[0076]
步骤s206：根据路径评价函数计算每个路径样本的权值，并根据所述每个路径样本的权值估计当前最优路径样本；
[0077]
步骤s207：判断所述当前迭代次数是否达到预设次数；
[0078]
步骤s208：若所述当前迭代次数未达到所述预设次数，则在所述路径滚动窗口内对所述巡检机器人的局部路径点进行重采样，得到更新后的路径样本集与当前迭代次数，返回执行s206；
[0079]
步骤s209：若所述当前迭代次数达到所述预设次数，则输出所述当前最优路径样本作为目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；
[0080]
步骤s210：判断所述巡检机器人是否到达所述任务终点，若所述巡检机器人到达所述任务终点，则完成所述巡检机器人的动态路径规划操作。
[0081]
根据所述巡检机器人的当前位置和所述基准点，采用正交化思想生成初始路径样本引入迭代计算机制，在每个滚动窗口内迭代执行粒子滤波估计路径样本，根据路径评价函数计算每个路径样本的权值，并估计当前最优路径样本判断所述路径滚动窗口内的粒子滤波算法是否满足迭代结束条件，若当前迭代次数达到预设迭代上限，则结束迭代计算过程并输出所述当前最优路径样本作为目标局部路径确定所述巡检机器人下一步待执行路径点；否则重采样更新路径样本集并继续下一轮迭代计算。
[0082]
为提升粒子滤波算法对状态量的估计准确度、加快最优解的收敛过程，本实施例引入了启发式优化的迭代计算机制对粒子滤波算法的计算过程进行改进，通过设置迭代次数，将在每个路径滚动窗口内重复执行粒子滤波估计路径样本，当达到所设定的终止条件，才会停止重复计算并更新局部路径，路径滚动窗口则继续向前推进。
[0083]
本发明实施例所提供的方法，能够有效应对巡检机器人在电力监测系统中执行任务时面对的多种动态变化的任务场景，针对环境中遇到的动态障碍物能在线实时调整路径，在保证规划路径安全性和可行性的同时，对环境具有一定的预见性。
[0084]
请参考图3，图3为本发明实施例提供的一种基于粒子滤波算法的巡检机器人动态路径规划装置的结构框图；具体装置可以包括：
[0085]
建立模块100，用于对巡检机器人任务场景创建地图，建立路径滚动窗口；
[0086]
获取模块200，用于利用传感器获取当前时刻的环境信息，所述环境信息包括所述任务场景中障碍物和所述巡检机器人的位置与移动速度；
[0087]
预测模块300，用于根据所述当前时刻的环境信息，采用粒子滤波算法预测所述任务场景中动态障碍物和所述巡检机器人下一时刻的位置和移动速度，得到更新后的环境信息；
[0088]
确定模块400，用于根据所述巡检机器人的当前位置及任务终点，确定所述巡检机器人在所述路径滚动窗口内的局部路径起止点；
[0089]
生成模块500，用于在所述路径滚动窗口内建立局部坐标系，根据所述更新后的环境信息和所述局部路径起止点，在所述局部坐标系中生成所述巡检机器人的初始路径样本集；
[0090]
迭代模块600，用于在所述路径滚动窗口内迭代执行粒子滤波算法估计路径样本，直至迭代次数达到预设次数，输出目标局部路径，以便所述巡检机器人根据所述目标局部路径前进一步；
[0091]
判断模块700，用于判断所述巡检机器人是否到达所述任务终点，若否，则将所述路径滚动窗口向前推进一步，循环执行所述获取模块至所述判断模块的操作步骤直至所述巡检机器人到达所述任务终点。
[0092]
本实施例的基于粒子滤波算法的巡检机器人动态路径规划装置用于实现前述的基于粒子滤波算法的巡检机器人动态路径规划方法，因此基于粒子滤波算法的巡检机器人动态路径规划装置中的具体实施方式可见前文中的基于粒子滤波算法的巡检机器人动态路径规划方法的实施例部分，例如，建立模块100，获取模块200，预测模块300，确定模块400，生成模块500，迭代模块600，判断模块700，分别用于实现上述基于粒子滤波算法的巡检机器人动态路径规划方法中步骤s101，s102，s103，s104，s105，s106和s107，所以，其具体实施方式可以参照相应的各个部分实施例的描述，在此不再赘述。
[0093]
本发明具体实施例还提供了一种基于粒子滤波算法的巡检机器人动态路径规划设备，包括：存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现上述一种基于粒子滤波算法的巡检机器人动态路径规划方法的步骤。
[0094]
本发明具体实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述一种基于粒子滤波算法的巡检机器人动态路径规划方法的步骤。
[0095]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0096]
专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
[0097]
结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(ram)、内存、只读存储器(rom)、电可编程rom、电可擦除可编程rom、寄存器、硬盘、可移动磁盘、cd
‑
rom、或技术领域内所公知的任意其它形式的存储介质中。
[0098]
以上对本发明所提供的基于粒子滤波算法的巡检机器人动态路径规划方法、装置、设备以及计算机可读存储介质进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。