CN114089743A - 一种配电室移动机器人的轨迹优化方法 - Google Patents

Abstract

本发明提供了一种配电室移动机器人的轨迹优化方法，包括如下步骤，设一估价函数：f(n)＝g(n)+h(n)，所述g(n)为配电室移动机器人从节点n到终点的轨迹函数，所述h(n)为配电室移动机器人从当前位置到节点n之间的函数，对所述配电室移动机器人当前位置跟节点n位置之间设置横纵坐标，因此所述h(n)可进行如下表示：

其中，d₁(n)和d₂(n)分别表示的是配电室移动机器人当前的节点位置跟目标点的位置之间横坐标的差值以及纵坐标的差值。

Description

一种配电室移动机器人的轨迹优化方法

技术领域

本发明涉及一种配电室移动机器人的轨迹优化方法。

背景技术

随着“无人值守变电站”的提出，配电室无人值守也被提上的议程，解决配电室配电室移动机器人路径规划问题，使配电室移动机器人能在配电室中按预定点位自主移动，在这个能够自主移动的载体上装载其他例如：相机，红外热成像仪，温湿度检测器，局部放电检测仪器等检测设备的条件下，对配电室环境设备进行实时远程监控。达到解放劳动力，降低人工巡检遇到突发危险的可能性，提高职工安全。目前配电室移动机器人应用于室外变电站的较多，配电室这种小型环境的配电室移动机器人应用较少，在劳动力成本急剧上升、机器人成本下降、安全性意识不断增强的背景下，配电室配电室移动机器人路劲规划的研究，使机器人代替人工巡检成为可能，突破电力系统配电环节一大瓶颈，填补国内外同类产品空白。

A*算法是一种启发式搜索算法，在配电室移动机器人路径规划上虽然被广泛使用，通过估价函数对搜索空间进行评估，避免了搜索的盲目性，大大提高了搜索效率。A*算法一般用于静态全局规划，但是规划出来的路径不利于真实机器人行驶，也不符合人类常规思维下的行驶路径，针对规划出来的路径拐点多、转折次数多等问题，进行如下的算法优化。

发明内容

基于背景技术存在的问题，本发明提出了一种配电室移动机器人的轨迹优化方法。

本发明采用了如下方案：

一种配电室移动机器人的轨迹优化方法，包括如下步骤，设一估价函数：f(n)＝g(n)+h(n)，所述g(n)为配电室移动机器人从节点n到终点的轨迹函数，所述h(n)为配电室移动机器人从当前位置到节点n之间的函数，对所述配电室移动机器人当前位置跟节点n位置之间设置横纵坐标，因此所述 h(n)可进行如下表示：

其中，d₁(n)和d₂(n)分别表示的是配电室移动机器人当前的节点位置跟目标点的位置之间横坐标的差值以及纵坐标的差值。

优选地，当配电室移动机器人从当前位置A点到目标位置D点之间具有障碍物时，令L(A,D)为A点到D点之间的距离，在A点与D点之间找到B 点使得L(A,D)<L(A,B)+L(B,D),将移动轨迹设置为从A点到B点最后到达D 点；若能在B点与D点之间找到C点使得L(A,C)+L(C,D)<L(A,B)+L(B,D),则配电室移动机器人将去除B点，将移动轨迹优化为从A点到C点最后到达D 点。

优选地，还包括如下步骤：所述估价函数为时间的函数，所述配电室移动机器人的轨迹按路径点分成m段，每小段路径拟合为一多项式曲线，则对第n段路径有：

其中N为多项式的个数，p_i多项式的系数，t为时间；对于整条路径则有

通过求导可得任意时刻t的位置P＝f(t)、速度v＝f'(t)并施约束约束多项式曲线经过起、止点，以及导数约束，约束方式如下：

式中x_i为第i个路径点，m段多项式对应m+1个路径点，同时施加连续性约束，使不同段的多项式曲线在路径点处位移、速度和加速度连续，约束方式如下：

其中i＝1,2,…,m-1&k＝1,2,3；在约束条件下求得整条路径函数的最小值则完成对所述配电室移动机器人轨迹的优化。

优选地，还括如下步骤：假设配电室移动机器人从起点开始，平稳加速到最大允许速度时均速运动，接近终点时再平稳减速，则加速度函数为：

其中

w_j为期望的最大j值，v_max为最大允许速度，速度从静止加速到最大允许速度的时间为2π/w_v，为使加速度不大于给定值a_max，令

到达最大允许速度前的位移函数为：

将初始路径乘一常系数，即s′_i＝w_ss_i，并令 s_init＝s_prol(2π/w_v)，对于首段位移的时间分配，有：

末段位移时间的计算与首段位移同理，对于首、末段之间位移s′_i的时间分配则有：

通过采用上述技术方案，本发明可以取得以下技术效果：通过对传统的A*搜索路径算法仿真，发现其存在拐点多、路径弯折程度大等问题，因此本文使用了基于静态环境的A*算法改进，对全局规划算法进行平滑处理和轨迹优化。在对路径平滑处理上，针对启发函数存在搜索时间较长问题提出一个新的的启发函数，为了让启发函数更加接近真实值，该函数采用曼哈顿距离和欧式距离结合的方式设置；利用Floyd算法优化传统A*算法搜索路径存在的拐点多、路径弯折程度大等问题。并利用多项式进行轨迹优化，使路径更适合机器人运动。在MATLAB下仿真改进后的A*算法与传统A*算法，对比分析结果可知，两者规划时间差别不大，但改进后的路径更加平滑，路径长度更短。

附图说明

为了更清楚地说明本发明实施方式的技术方案，下面将对实施方式中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本发明改进估值函数的示意图。

图2为本发明Floyd算法的示意图。

图3为本发明进行多项式优化后躲避障碍物的示意图。

图4为进行估值函数改进的配电室巡检机器人路径规划流程图。

图5为未进行估值函数改进的配电室移动机器人轨迹示意图。

图6为进行估值函数改进的配电室移动机器人轨迹示意图。

图7为实际操作本发明轨迹优化方法后配电室移动机器人轨迹示意图。

图8为配电室巡检机器人ROS的A*算法路径规划。

图9为配电室巡检机器人改进后的A*算法路径规划。

具体实施方式

为使本发明实施方式的目的、技术方案和优点更加清楚，下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。因此，以下对在附图中提供的本发明的实施方式的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施方式。基于本发明中的实施方式，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的设备或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征“之上”、“上方”和“上面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”包括第一特征在第二特征正下方和斜下方，或仅仅表示第一特征水平高度小于第二特征。

实施例

以下仅是本发明的优选实施方式，本发明的保护范围并不仅局限于下述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。

A*算法是一种启发式搜索算法，在配电室移动机器人路径规划上虽然被广泛使用，通过估价函数对搜索空间进行评估，避免了搜索的盲目性，大大提高了搜索效率。A*算法一般用于静态全局规划，估价函数可表示为：

f(n)＝g(n)+h(n) (1)

但是规划出来的路径不利于真实机器人行驶，也不符合人类常规思维下的行驶路径，针对规划出来的路径拐点多、转折次数多等问题，进行如下的算法优化。

A*算法中启发函数h(n)对路径搜索结果影响很大。设节点n到目标点距离的真实值为H(n)。当h(n)<H(n)时，路径搜索空间大、节点较多，搜索效率比较低，但最终能够搜索出最优解；当h(n)>H(n)时，路径搜索空间小、节点较少，在搜索速度和效率方面有所提升，但是在大多数情况下规划出来的路劲并不多最优解；当h(n)＝H(n)时，路径搜索将会沿着最短路径节点序列展开搜索，搜索效率变高，但是在实际情况中很难实现。在栅格地图中，A*算法采用八连接方法建立栅格间的连接关系和路径扩展。该特点决定了曼哈顿距离和欧氏距离的启发函数都不是最佳选择。为了让启发函数 h(n)更加接近于真实值H(n)，本文结合曼哈顿距离和欧氏距离各自的特点，设置出一个启发函数，公式如所示：

上式中，d₁(n)和d₂(n)分别表示的是配电室移动机器人当前的节点位置跟目标点的位置之间横纵坐标的差值。从说明书附图1可看到，本文设置的启发函数更接近于真值H(n)，其中NG为欧式距离，NO+GO为曼哈顿距离。

Floyd算法借鉴动态规划的思路搜寻最短路径，是用来解决两点之间的最优距离问题的算法。使用Floyd算法和A*算法结合的方法以减少路径长度，符合实际应用需求。Floyd算法原理说明书附图2所示：

L(A,D)为A、D两点间的距离，如说明书附图2，A、D存在障碍物，可设L(A,D)＝+∞，L(A,D)＝A->D。B点是A点和D之间已规划出的节点：

若

L(A,D)<L(A,B)+L(B,D) (3)

则

L(A,D)＝L(A,B)+L(B,D) (4)

R(A,D)＝A->B->D (5)

在B、D线段上插入点C：

若

L(A,C)+L(C,D)<L(A,B)+L(B,D) (6)

则

L(A,D)＝L(A,C)+L(C,D) (7)

R(A,D)＝A->C->D (8)

去除C点，A到D的优化路径表示为一段优化后的弧形路径 R(A,D)。采取Floyd算法优化A*算法规划的路径能够删除多余的点，更加适合配电室移动机器人行驶。

直线路径不利于机器人的运动，在上一小节路径规划的基础上利用多项式进行轨迹优化处理。将多项式各阶导数作为轨迹的位置、速度(v)、加速度(a)和加加速度(j)等参数，单段多项式难以适应较为复杂的环境，因此使用多段高阶多项式可以更好地在各种场景进行轨迹优化。为了能限制加速度导数，使用五次多项式。使用多段多项式平滑路径，首先将轨迹按路径点分段，设共分成m段。每小段路径拟合为一多项式曲线，则对第n段路径有

式中p_i—多项式系数，t—时间。

对于整条路径，则有

由式(10)，通过求导可得任意时刻t的位置P＝f(t)、速度v＝f′(t)等参数并施加约束。约束多项式曲线经过起、止点，以及导数约束。

式中x_i为第i个路径点。

m段多项式对应m+1个路径点，约束的放宽对解的质量有重要影响。每个路径点只限制了一段多项式曲线，因此需施加连续性约束，使不同段的多项式曲线在路径点处位移、速度和加速度连续：

可根据需要选取最小化目标函数的导数阶数，此处以最小化j为目标以增加运动平稳性，因此轨迹优化问题将转换为如下最优化问题

该优化问题属于QP问题。优化后的一些物理量可能超过预设值，可对优化后的时间比例乘一系数

式中M—速度等物理量预期值。以此控制相关物理量的最高值，这不会影响优化的效果。

优化时的轨迹存在重新碰撞障碍物的可能，可通过约束多项式端点的方式避开障碍物。参考说明书附图3，当某段轨迹被检测到碰撞后，进行连线检查，当有连线未经障碍物时，将该连线端点，及上一经过障碍物的连线的其中一个端点，3个点作为路径点按前述方法做平滑，得到新的平滑路径点作为轨迹的约束，使轨迹经过该点。

上述QP问题，实际上可以看作是寻找最优时间分配问题。通常先给定一个时间分配初值，再用迭代的方法寻找最优解。对于时间初值的给定，目前有采用均速运动推算时间，即按路径长度比例分配时间的方法，虽然能收敛到最优解，但缺乏合理性，初值与最优解偏差较大，将增大迭代难度，增加运算时间。合理的时间初值可以大大减少迭代次数。

针对起止为静止状态的时间分配，设配电室移动机器人从起点开始，平稳加速到最大允许速度时均速运动，接近终点时再平稳减速。若以最小化j为目标，则加速期间加速度应有一个平缓的变化率，因此借助余弦函数设加速度函数为

其中

式中w_j—期望的最大j值，v_max—最大允许速度。且速度从静止加速到最大允许速度的时间为2π/w_v。为使加速度不大于给定值a_max，令

到达最大允许速度前的位移函数为：

优化后的轨迹往往长于初始轨迹，因此将初始路径乘一常系数，即s′_i＝w_ss_i，并令s_init＝s_prol(2π/w_v)，对于首段位移的时间分配，有：

末段位移时间的计算与首段位移同理。对于首、末段之间位移s′_i的时间分配则有：

在保证路径安全性的前提下，为了尽可能提高配电室巡检机器人路径规划的效率，本文对A*算法进行了改进，说明书附图4显示了路径规划的改进A*算法流程图。

为了验证改进后A*算法全局路径规划的效果，在MALAB下对该算法进行了仿真实验。说明书附图5-6为机器人的环境地图，其中圆圈表示障碍物；取点(1,1)为配电室移动机器人起始点；取点(29,29)为配电室移动机器人目标点。说明书附图5为传统A*算法路径规划图，实线线段为其规划出的路径；说明书附图6为改进A*算法路径规划图，实线线段为其规划出的路径。

仿真结果表明，在相同的环境地图下，A*算法改进前后，规划出的路径有较大的提升。改进后的A*算法在搜索过程中不仅节点变少，提高了搜索时间。而且拐点变少，在拐点处进行了平滑处理，并对路径进行轨迹优化。实验证明改进优化过后的算法规划的路径不仅路径距离更加简短而且更加适合配电室移动机器人的运动。

巡检机器人主控使用英伟达Tegra K1，四核Cortex A15 2.2GHz处理器。主控计算机上运行Linux操作系统，在此基础上安装ROS机器人操作系统。建图使用Gmapping算法建立如说明书附图7所示栅格地图，同时播放数据集。在同一局域网下用一台Windows10笔记本电脑安装人机交互软件，作为远程控制端。膨化安全距离设置为0.12m，maxv＝0.15m/s，maxa＝0.15m/s²，w_j＝0.5，w_s＝1.05，起始点为(0.2,0.78)，终点为(1.02， -5.34)，路径亦具有良好的平滑性。

为了验证改进后A*算法全局路径规划的效果，进一步进行对比实验。实验的参照对象为ROS自带的A*算法，该算法启发式函数采用的是曼哈顿距离，并通过插值的方式进行路径的优化，说明书附图8为ROS的A*算法路径规划的路线图。本文改进的A*算法规划的路径，如说明书附图9所示。

仿真结果表明，在相同的环境地图下，改进后的A*算法规划时间变长了，这是由于路径的平滑处理及轨迹优化的增加而造成的。但所增加的时间在可接受的范围内，对配电室巡检机器人日常的巡检工作没有太大影响。 A*算法改进后，规划出的路径有较大的提升，形成了一条光滑的曲线。从图9可以看出改进后的A*算法在搜索过程中不仅节点变少，而且拐点变少，经过路径的轨迹优化，减少拐点的弯折程度，提高了路径的平滑度。表1是仿真实验对比数据，从表4-1可见证明改进后的A*算法规划的路径距离更短。结合图9和表1的仿真结果，改进后的A*算法规划的路径更加适合巡检机器人的运动。

表1仿真实验对比数据

本发明通过对传统的A*搜索路径算法仿真，发现其存在拐点多、路径弯折程度大等问题，因此本文使用了基于静态环境的A*算法改进，对全局规划算法进行平滑处理和轨迹优化。在对路径平滑处理上，针对启发函数存在搜索时间较长问题提出一个新的的启发函数，为了让启发函数更加接近真实值，该函数采用曼哈顿距离和欧式距离结合的方式设置；利用Floyd 算法优化传统A*算法搜索路径存在的拐点多、路径弯折程度大等问题。并利用多项式进行轨迹优化，使路径更适合机器人运动。在MATLAB下仿真改进后的A*算法与传统A*算法，对比分析结果可知，两者规划时间差别不大，但改进后的路径更加平滑，路径长度更短。

Claims (4)

1.一种配电室移动机器人的轨迹优化方法，其特征在于，包括如下步骤，设一估价函数：f(n)＝g(n)+h(n)，所述g(n)为配电室移动机器人从节点n到终点的轨迹函数，所述h(n)为配电室移动机器人从当前位置到节点n之间的函数，对所述配电室移动机器人当前位置跟节点n位置之间设置横纵坐标，因此所述h(n)可进行如下表示：

其中，d₁(n)和d₂(n)分别表示的是配电室移动机器人当前的节点位置跟目标点的位置之间横坐标的差值以及纵坐标的差值。

2.一种如权利要求1所述的配电室移动机器人的轨迹优化方法，其特征在于，当配电室移动机器人从当前位置A点到目标位置D点之间具有障碍物时，令L(A,D)为A点到D点之间的距离，在A点与D点之间找到B点使得L(A,D)<L(A,B)+L(B,D),将移动轨迹设置为从A点到B点最后到达D点；若能在B点与D点之间找到C点使得L(A,C)+L(C,D)<L(A,B)+L(B,D),则配电室移动机器人将去除B点，将移动轨迹优化为从A点到C点最后到达D点。

3.一种如权利要求1所述的配电室移动机器人的轨迹优化方法，其特征在于，包括如下步骤：所述估价函数为时间的函数，所述配电室移动机器人的轨迹按路径点分成m段，每小段路径拟合为一多项式曲线，则对第n段路径有：

其中N为多项式的个数，p_i多项式的系数，t为时间；对于整条路径则有

通过求导可得任意时刻t的位置P＝f(t)、速度v＝f'(t)并施约束约束多项式曲线经过起、止点，以及导数约束，约束方式如下：

式中x_i为第i个路径点，m段多项式对应m+1个路径点，同时施加连续性约束，使不同段的多项式曲线在路径点处位移、速度和加速度连续，约束方式如下：

其中i＝1,2,…,m-1&k＝1,2,3；在约束条件下求得整条路径函数的最小值则完成对所述配电室移动机器人轨迹的优化。

4.一种如权利要求3所述的配电室移动机器人的轨迹优化方法，其特征在于，包括如下步骤：假设配电室移动机器人从起点开始，平稳加速到最大允许速度时均速运动，接近终点时再平稳减速，则加速度函数为：

其中

w_j为期望的最大j值，v_max为最大允许速度，速度从静止加速到最大允许速度的时间为2π/w_v，为使加速度不大于给定值a_max，令

到达最大允许速度前的位移函数为：

将初始路径乘一常系数，即s′_i＝w_ss_i，并令s_init＝s_prol(2π/w_v)，对于首段位移的时间分配，有：

末段位移时间的计算与首段位移同理，对于首、末段之间位移s′_i的时间分配则有：

Images