基于快速进行平方算法的巡检机器人路径规划研究

武 刚,许家元,邸 迪

(沈阳大学信息工程学院,辽宁 沈阳110044)

0 引言

近年来,随着产业智能化程度的不断提高,各国对于智能化无人系统的研究在不断深入,与其相关的机器人产业也迎来了发展的黄金时期[1]。巡检机器人作为一种无需人工驾驶即可自主移动的机器人[2],可以在工厂和制造设施中执行巡检任务,可以检查生产线的设备是否运转正常、是否存在故障,以及检测可能的安全隐患[3]。

作为机器人产业的重要组成部分,巡检机器人拥有复杂的自动引导系统,而路径规划部分是其自动引导系统中的关键。近年来,国内外学者在机器人的路径规划研究中采用了多种算法,包括Dijkstra算法[4]、A*算法[5]、RRT算法[6]、粒子群优化算法[7]、蚁群算法[8]、人工势场法[9]和快速行进平方法[10-11]等。

快速行进平方法(fast marching square method,FMS)克服了人工势场容易陷入局部最小值的问题,对比粒子群算法与蚁群算法,该算法所得的规划路径的一致性较好,相较于Dijkstra算法和A*算法等其他方法,快速行进平方法具有使用简单、响应速度快等优点,近几年被广泛用于解决无人驾驶装备的路径规划问题上。

然而,在将快速行进平方法应用到实际的场景时仍存在不足。具体而言,规划路径距障碍物越远,路径的安全性就越高,但长度也会相应增加,此时对于截止阈值参数Sat的调整没有一个合适的评判标准,仍需要进一步的研究探讨。

针对上述问题,本文设计了一种改进快速行进平方法,添加路径评价函数,综合考虑路径的长度、机器人安全性与灵活性等因素,对截止阈值参数Sat进行量化调整,以产生适用于当前环境下的最优路径。

1 基本原理

1.1 快速进行法

快速行进平方法是快速行进法(fast marching method,FMM)的一种变体。其在保持快速行进法的主要优点的同时,通过2次使用快速行进法来规划路径[12-13],弥补了快速行进法规划路径安全性不足的缺点。图1显示了这2种算法的不同之处。

图1 快速行进法与快速行进平方法规划的路径差异

快速行进法是由Sethian提出用来对物理波界面Γ进行跟踪建模的数值算法,是水平集方法的1个特例[14]。该方法已应用于不同的研究领域,包括计算机图形学、医学成像、计算流体动力学、图像处理及轨迹计算等,可以迭代求解二维或多维界面的传播问题。

其描述波面运动的Eikonal方程为

(1)

式中: (x,y)为该点的位置坐标;T(x,y)为波界面到达(x,y)所需的时间;F(x,y)为波在该位置的扩展速度。

通过离散化梯度T(x,y)能够在空间中每点(x,y)迭代求解程函方程。将机器人所处的环境空间进行离散,通过式(2)和式(3)简化所要求解的点(x,y)处的梯度。

T1=min(T(x-Δx,y),T(x+Δx,y))

(2)

T2=min(T(x,y-Δy),T(x,y+Δy))

(3)

式中:Δx、Δy为离散空间的x和y方向上的间距。

再将式(2)和式(3)代入程函方程并平方,得到离散化形式,即

(4)

从初始界面T(x,y)=0开始,进行迭代求解式(4),在后续的迭代过程使用前次迭代中已求解过的点来计算其临近点的T值,直至所有点都被求解完成。

1.2 快速进行平方法

Garrido等在快速行进法的基础上,提出了一种名为快速行进平方法的新算法,该算法有着较好的路径平滑度与安全距离。

算法将获取的地图信息通过合适的分辨率转化为二进制地图,如图2a所示。

图2 快速行进平方法路径规划地图与势场

首先,将图中所有障碍物栅格均被视为波源,波同时从障碍物边界扩展,地图中每个栅格的速度值都通过时间值来反馈,这样就可以得到对应的速度势场W(x),如图2b所示。

其次,将机器人的目标点用作波源,从目标点开始扩展,直至到达起始点。利用上一步建立的速度势场与传播波速度可以得到时间势场D(x),如图2c所示。

在建立速度势场的过程中,通过改变重要截止阈值参数Sat来调整速度值与时间值的映射关系,进而影响机器人运动范围,此处Sat(x)的取值区间为wpi∈[0,Sat)→0。Sat值越大,障碍物的边界范围越大,实现了机器人在路径规划的过程中与障碍物时刻保持安全距离的目的。

2 改进快速行进平方法

为了更直观地理解不同的截止阈值参数Sat对实际规划路径的影响,如图3所示,本文任意选取2个不同的Sat,通过对不同Sat值下规划的路径与速度势场W(x)比较可见,当Sat值调整的过小时,所规划的路径离障碍物距离较小,增加了机器人行驶的危险性,而Sat值调整的过大又会使所规划的路径较长,降低了机器人行驶的效率。

图3 不同Sat值下的规划路径与速度势场

在传统的路径规划方法中,评价函数通常只考虑路径的长度,即从起点到终点的直线距离。这种评价函数的设计主要基于最小化总移动距离的理念,以提高效率。传统的评价函数可以表示为

F(path)=length(path)

(5)

式中:F(path) 为路径的评价函数值;length(path) 为路径的长度函数,它计算从起点到终点的直线距离。

这种评价函数在解决许多简单的路径规划问题时表现良好,尤其在起点和终点接近直线,且不存在复杂障碍物的场景中。然而,在存在障碍物或起点和终点之间存在复杂路径的情况下使用快速行进平方法,仅考虑路径长度可能会导致规划出的路径不是最安全或最有效率的。

针对这一问题,设计出了一个适用于快速行进平方法的评价函数。该函数受机器人行驶距离、与障碍物的安全距离以及沿路径行驶的角度变化量3个因素影响。通过评价函数来综合评价所规划的路径是否适合机器人在当前环境下行驶,从而选择出最优的截止阈值参数Sat进行路径规划,使得机器人能够高效且安全地行驶。

总体路径评价函数表达式为

E=k1(1-L′)+k2(1-P′)+k3D′

(6)

机器人行驶距离L′反映了机器人在路径上移动的总距离,体现了路径的长度,其表达式为

(7)

式中:m为在规划路径上航路点的数量。

行驶角度变化量P′通过航路点转向角的总和,度量了机器人沿路径行驶过程中方向的变化程度,其表达式为

(8)

障碍物的最短距离D′通过计算航路点与障碍物的离散点的最短距离,衡量了机器人与障碍物之间的安全距离,体现了路径的可行性,其表达式为

(9)

式中:(xobs,yobs)为环境中障碍物离散点坐标。

这3个参数具有不同的量纲,如长度、角度和距离等,因此需要进行无量纲化处理,以便用于评价路径。通过结合数量级分析将这3个参数进行归一化处理,从而使它们的量纲相同。这样,这3个参数就可以在路径评价函数中平等考虑,以便机器人能够更好地选择最佳路径。

在权重系数方面,k1、k2和k3之和为1,这是为了使得路径评价函数能够应用于不同类型的机器人。如对于小型机器人,由于其功耗相对较低,重点是规划路径要短,以满足行驶效率;而对于大型机器人而言,其体积更大,操控能力更差,需要充分考虑规划路径与障碍物的最短距离和行驶角度变化等因素,以确保其安全行驶。因此,通过根据不同类型的机器人进行权重系数的调整,可以使路径评价函数更加适用于各种场景,并有效帮助机器人选择最优路径。

3 仿真及分析

3.1 仿真模型参数设置

根据上文对快速行进平方法的改进,通过计算机进行仿真模拟,软件仿真环境为MATLAB R2022b,硬件处理器配置为Intel(R) Core(TM) i7-8750H CPU 2.20 GHz,RAM配置为8.00 GB (7.88 GB 可用),Windows 11(64位操作系统)。此外,为了验证规划路径的合理性,本文建立机器人运动学模型,并通过跟踪控制验证路径是否符合机器人的运动学特性。

首先,本文所使用的仿真地图如图4所示,电子地图文件中深色的区域为模拟巡检环境的墙壁以及柱子等建筑实体,浅色区域巡检环境的机器设备、安全门及堆叠的物品,白色区域为巡检通行区域。通过使用MATLAB将电子地图文件转化为大小为418×720的二进制地图文件,将图中不可通行区域,在二进制地图中显示为黑色,可通行区域显示为白色,起始点与目标点分别用叉号与星号表示。

图4 仿真场景

其次,对路径进行跟踪仿真,所使用的机器人底盘结构如图5所示,呈菱形结构,左右两侧为控制机器人运动状态的驱动轮,前后为保持机器人平衡的万向支撑轮,通过对左右两轮差速驱动完成前进、后退、转弯等移动操作[15]。图中,X-Y为环境坐标系,XR-YR为底盘坐标系,以质心C为底盘坐标系的原点,θ为2个坐标系之间的夹角。

图5 机器人底盘结构

假设环境坐标系中机器人的位姿矢量为ξi=(x,y,θ)T,底盘坐标系中机器人的位姿矢量为ξR=(xR,yR,θR)T,二者通过正交旋转矩阵R(θ)来反映映射关系。

(10)

机器人在行驶过程中符合运动学规律,因此满足

vL=ωLr,vR=ωRr

(11)

(12)

式中:vL为左侧驱动轮的线速度;vR为右侧驱动轮的线速度;ωL为左侧驱动轮的角速度;ωR为右侧驱动轮角速度;r为驱动轮半径;l为驱动轮轮距;ω为机器人角速度;v为机器人线速度。

机器人任务的起点和目标点分别设置为(25,372)和(439,169)。在仿真中使用的机器人参数如表1所示。

表1 机器人参数

3.2 路径规划仿真

为了验证所使用算法的有效性,在静态环境下,本文采用不同的截止阈值参数Sat进行了仿真对比实验。如图6所示,对选取的Sat值为0.1、0.3时的速度势场与时间势场对比。当Sat值为0.1时,通过观察可以发现,障碍物对周围区域的影响较小,在规划路径时模拟机器人与障碍物的距离较远,机器人的可行区域范围较广,所规划路径时将较为平直。当Sat值提高到0.3时,此时障碍物的边界范围因Sat值的增大而增大,模拟机器人与障碍物的距离较近,机器人的可行区域缩小,为了保证机器人行驶的安全性,规划路径时会比Sat值为0.1时更加曲折。

图6 不同Sat值下的速度势场与时间势场

随着Sat值的增加,W(x)的障碍物区域不断增加,因此基于W(x)使用梯度下降法得到的路径也会随之变化。通过图7,对模拟场景进行路径规划,可以更加直观地发现,这些路径与障碍物的最短距离逐渐增加,增强了路径的安全性。

图7 不同Sat值下的规划路径

由上述的仿真测试,如图8所示,本文对不同Sat值下的规划路径参数进行对比分析。根据图8c可知,使用快速行进平方法规划路径时,当Sat=0.1时,距离地图区域的最短距离为0.84 m,而Sat=0.3时距离地图区域的最短距离1.39 m,说明越大的Sat值距离障碍物距离越大。由于巡检环境的工作区域常常在墙壁附近,这对于机器人行驶来说是不安全的。为了保证机器人的行驶安全性,应尽可能远离障碍物,因此需要选择较大的Sat值。但图8b的结果显示,随着Sat值的增大,规划路径的长度从839.04 m增加到985.52 m,增长率为14.85%。规划机器人行驶距离增加必然会导致规划时间的增加,进而影响机器人的运输效率与经济性。为此在考虑机器人安全性的同时也需要考虑规划路径的长度。

图8 不同Sat值下的规划路径参数

根据本文提出的路径评价函数(式(6)),选取的多个Sat值进行仿真模拟,通过计算得到的评价函数结果如表2所示。

表2 不同Sat 值下规划所得路径的评价函数值

由于本文使用的巡检机器人具有体型小、质量轻、灵活性好的特点,权重系数分配如下:k1=0.4、k2=0.3、k3=0.3。通过表2的对比,评价函数的最优值在Sat=0.15时取得,其值为0.492 6。

因此,在综合考虑机器人行驶的安全性、经济性和控制复杂性后,对于本文选取的地图区域,Sat=0.15是最合适的阈值参数。可以以此Sat值下规划所得的路径作为机器人在本地图区域从任务执行的起点到目标终点行驶的全局路径。

为了进一步评估新算法的性能,在原有路径规划的基础上引入了PID控制器,将其与原算法进行对比分析。在仿真实验中,采用相同的实验条件和参数设置,并对2种算法的路径规划结果进行比较。

首先,本文对比了2种算法在路径长度与安全性方面的表现,如图9所示。

图9 原算法与改进算法规划路径对比

实验结果表明,在保障巡检车辆行驶安全性的同时,本文算法下,机器人能够规划出更短的路径。与原算法相比,本文提出的算法规划出的路径长度降低了10.75%,与此同时,这表明本文算法在路径规划效率方面具有优势。

此外,本文还对比了2种算法在平滑度与操作性方面的表现,如图10所示。

图10 原算法与改进算法机器人速度与航向角偏差

实验结果表明,本文算法规划出的路径在平滑度方面表现更优。与原算法相比,本文提出的算法规划出的路径航向角偏差更加小,速度变化更加平缓,这有利于机器人的平稳行驶。

综上所述,通过与原算法生成的机器人轨迹的对比分析发现,本文提出的新算法在路径长度、安全性和平滑度方面均具有优势。这些结果表明,本文的算法充分利用无人巡检机器人的移动特征,具有重要的实际应用价值。

4 结束语

针对目前快速行进平方法在实际场景中,对截止阈值参数Sat值的选取没有合适的评价尺度这一问题,本文提出了一种改进快速行进平方法。通过引入路径评价函数,并对机器人行驶距离、机器人行驶角度变化量、与障碍物的距离进行无量纲化处理,以获得当前环境下最优的Sat。经过仿真实验的验证,改进后的快速行进平方法可根据Sat值生成适用于不同的车型与环境特点的最优路径。