改进A*算法的机器人能耗最优路径规划方法

张浩杰, 张玉东, 梁荣敏, 杨甜甜

(1. 北京科技大学自动化学院工业过程知识自动化教育部重点实验室, 北京 100083;2. 中国兵器科学研究院兵器技术创新中心, 北京 100089)

0 引 言

随着移动机器人技术的不断成熟,其在敌区监视、救援、边境巡逻、采矿运输和智慧农业等领域正发挥着越来越重要的作用[1-3]。移动机器人大多采用电池作为动力源,具有零排放、噪声小、高能源利用率和控制简单准确等优势[4],因此在单次能源补给下的运行时间很大程度上依赖于其能源效率。然而,在机器人执行任务的一些复杂场景很难实现能源快速补给,比如火星车着陆后巡视探测[5],而在一些其他场景则期望其在一次能源补给下能工作更长时间,从而降低能源补给的频率,比如工业生产中的货物运输调度[6]和室外未知区域协同探索与地图构建[7]等。因此,移动机器人在有限的能源供给下最大程度地执行任务变得至关重要。

移动机器人可以通过选用高效率电机、低功耗传感器和先进的能量管理系统等手段降低其运动过程中的能耗[8-9],然而,一旦这些固件确定,其能耗主要受自主规划系统影响[10]。最早开展能耗最优路径规划的研究工作可追溯到二十世纪九十年代,当时通过在移动机器人路径规划算法中引入能耗模型进行优化,该能耗模型考虑了施加在机器人上的外力,并且将任意两点之间的通行成本定义为克服摩擦力和重力影响的能量值[11]。然后,采用A*算法从路径子空间中选择适当的路径段,从而获得通行能耗最优路径。基于这个思想,后期开展了一系列移动机器人能耗最优路径规划相关的研究工作[12]。

通过在地形表面离散点之间设定权重连接关系,Sun等提出了一种移动机器人能耗最小路径规划方法,在某些假定条件下获得了能耗高效路径组合的上限值和下限值[13]。然而,该方法无法保证获得的路径是全局能耗最优的。Saad等提出了一种合并能耗模型和运动距离的组合路径矩阵,并将其用于Dijkstra算法进行路径规划[14-15],这样的能耗模型很难获得,其限制了路径规划算法进行能耗优化的实际应用。通过构建距离和能量损耗之间的关系模型,文献[16]将其代入传统A*算法的代价函数,进而提出了一种考虑能耗约束的ECA*(energy constraint A*)算法,以解决在资源有限情况下的能耗最优路径规划问题。根据地形表面的局部不规则性和高度差考虑机器人在其上运动时的能量消耗,Zakharov等提出了一种用于三维地图环境中机器人能耗最优路径规划的LRLHD-A*(local roughness local height difference A*)算法[17],仿真测试表明该算法所规划的路径能耗比A*算法降低约1.3%～6.3%。在智能制造系统中总生产能耗是不可忽略的,Zhang等以运输距离和能耗作为两个优化目标建立了自动导引车的能耗最优规划模型[18],进一步采用粒子群优化方法对该模型进行求解,实验结果表明运输任务的执行顺序对能耗有显著影响,从而验证了该模型的有效性。然而,由于环境信息对移动机器人的定位过程具有多方面的影响,Zhang等将定位和能耗效率标准作为参数化轨迹的成本函数,提出了一种改进的海豚群算法,以生成更多的能效轨迹[19]。该方法能够有效地提高能源效率,并减少了沿生成轨迹运动时的定位误差。然而,在动态环境中进行路径规划的能耗是巨大的,为保证移动机器人在避障的同时兼顾能源-时间之间的平衡关系,Sangeetha等提出了一种基于增益的高效动态绿色蚁群优化算法[20],通过基于增益函数的高效信息素增强机制,减少了路径规划过程中的总能耗,并通过在三维动态环境中的仿真实验,验证了该方法的有效性。

对于分层的路径规划过程而言,其首先通过传统的路径规划方法生成一系列的路径[21],其次,根据移动机器人的转向控制方法,将局部路径优化算法与能耗模型相结合以预测跟随这些路径的能耗[22],比如人工势场法[23]和基于采样的模型预测方法[24]等。这类方法的不同之处在于所采用的能耗模型及其相关联的路径优化算法,Gupta等基于滑动转向移动机器人的输出扭矩限制建立了其动力学模型,并据此获得了能耗模型[25]。通过将该能耗模型与模型预测优化方法结合,提出了一种有效生成动态合理的能耗最优轨迹方法。之后,Jaramillo-Morales等提出了一种考虑移动机器人驱动电机动力学参数的能耗模型[26],并采用该模型预测不同加速度及负载下生成轨迹的能耗。Xie等提出在移动机器人速度规划过程中采用能耗模型进行评估,以最小化能耗作为轨迹规划的目标实现了能耗优化[27]。由于机器人运动过程中的动力学约束及其与地形间的交互作用极其复杂,传统方法构建的能耗模型精度低,Visca等利用少量地形感知数据,通过深度元学习算法训练生成了更为精确的能耗模型[28]。然而,由于环境中存在大量的动态障碍物,在对传统路径规划方法生成的路径进行跟踪控制时很难获得较好的效果,最终导致非能耗最优运动。为避免这种情况发生,Ajanovic等在路径跟踪优化阶段考虑动态障碍物约束[29],利用目标成本地图和最优速度轨迹树重新规划调整能耗最优路径。

由以上分析可知,在路径优化过程中所采用的能耗模型越准确,其对不同轨迹的能耗估计越精确。在我们的前期研究工作中,通过分析移动机器人在运动过程中的机械能耗和发热能耗,建立了一个简单的能耗模型进行路径能耗计算,通过将距离成本和能耗成本融入启发式搜索算法的节点评估函数,从而获得考虑能耗约束的全局路径[30]。然而,所建立的简单能耗模型中部分参数很难估算,在本文中,我们进一步优化了能耗模型,以此离线估算机器人运动基元的能耗,进而提出了一种改进A*算法的移动机器人能耗最优路径规划方法,保证生成一条全局能耗最优路径。

1 移动机器人能耗模型

以四轮差速驱动移动机器人为研究对象,仅考虑其沿x、y方向的平移及绕z轴的横摆运动,忽略其他方向的运动,如图1所示。

图1 四轮差速驱动机器人运动学约束Fig.1 Kinematic constraints of four-wheel differential drive robot

以符号v表示机器人的平移速度,符号ω表示其横摆角速度,符号θ表示机器人的横摆角,它应满足的运动学约束条件为

(1)

在移动机器人运动过程中,控制器通过控制左右侧驱动电机的转速ωr和ωl,以使移动机器人达到期望的运动速度v和ω,因此机器人运动速度和左右侧驱动电机转速之间满足:

(2)

式中:

其中，L为机器人的轮距;r为轮子的半径。

移动机器人以电池作为动力源,假定电池电压为Us,驱动器通过输出不同占空比脉宽调制(pulse width modulation, PWM)信号调节左右侧驱动电机的电枢电压Ur和Ul,进而实现对驱动电机的转速控制。因此,左右驱动电机的电枢电压可以表示为

(3)

式中:Dl和Dr分别为左右侧驱动电机的PWM信号占空比。

电机的电枢电压大部分用于驱动线圈转动,有少部分用于电机内阻分压,可将式(3)表示为

(4)

式中:Ra为电机内阻;Kb为电机的反电动势系数;g为电机减速器的减速比。

将式(4)进行化简,可得

(5)

式中:

由式(5)可知,一旦移动机器人的电池、电机及减速器等固件确定后,Kb、g、Us、Ra均为常值,因此α为常值。当机器人负载恒定,以恒速运动时电机转矩为常值,根据电机转矩与电枢电流成正比例关系可知[31],此时电枢电流ir和il均为常值,则β为常值矩阵。然而,由于电枢电流很难实时测量,文献[32]的研究结果表明,α和β的值可以通过最小二乘法拟合近似。

将式(2)代入式(5),消去ωr和ωl,可得

(6)

在移动机器人全局能耗最优路径规划过程中,路径能耗由电机运动能耗评估,因此本文仅对电机进行能耗分析建立机器人的能耗模型,忽略其他能耗的影响。移动机器人左右侧驱动电机的功率可表示为

(7)

将式(5)代入式(7),消去il和ir,可得

(8)

机器人左右侧驱动电机的总能耗为

(9)

如果以D=[DrDl]T表示左右侧电机PWM信号占空比矢量,u=[vω]T表示机器人的控制矢量,将式(2)代入式(9)整理后,可得移动机器人的能耗模型为

(10)

因此,由式(10)表示的移动机器人能耗模型可估算其在控制矢量u作用一定周期下的能耗。

2 能耗运动基元集构建

在移动机器人路径规划过程中,运动基元确定了路径搜索时节点之间的连接关系,这种连接可以是一对一或一对多的父节点与子节点从属关系[33]。以sms(xms,yms,θms)和smg(xmg,ymg,θmg)表示运动基元的起始状态和目标状态,为了简化运动基元的计算,假定在单个运动基元生成时机器人的平移速度v和横摆角速度ω均为常值[34],而总的运动时间设为常值T。对于每一条运动基元,机器人先做连续的直线运动周期tl,之后做连续的曲线运动周期T-tl,如图2所示。直线运动周期tl满足0≤tl≤T。当tl=0时,运动基元为曲线;当tl=T时,运动基元为直线;当0

图2 单个运动基元的运动分析Fig.2 A motion primitive’s kinematic analysis

根据式(1)表示的机器人运动学约束条件可知,在直线运动过程中机器人位姿可表示为

(11)

式中:0≤t≤tl。

在曲线运动过程中,根据机器人的平移速度v和横摆角速度ω获得其曲率半径r,即

(12)

因此,在曲线运动过程中机器人的位姿可表示为

(13)

式中:γ(t)=ω(t-tl)+θms,tl

机器人在T时刻到达运动基元的目标状态smg,将其代入式(13),若求解获得常值v、ω和tl,则表明在sms和smg之间存在有效的运动基元,否则不存在运动基元。因此,将v、ω和tl分别代入式(11)和式(13),即可获得机器人在运动周期T内的运动基元,之后,采用式(10)估算该运动基元的能耗。在给定多组目标状态后可计算得到多个运动基元及其能耗,这些运动基元的集合被称为能耗运动基元集,它包含多组起始状态和目标状态之间的运动基元。

设定栅格地图分辨率为0.1 m,角分辨率为π/8,周期T=1.0 s,起始状态为(0,0,0),目标状态分别为(8,0,0)、(-5,0,0)、(8,1,1)、(8,-1,-1)、(6,2,2)、(6,-2,-2)、(-8,-1,1)、(-8,1,-1)、(2,2,4)和(2,-2,-4),则按式(11)和式(13)生成的运动基元集如图3所示,生成每一条运动基元所采用的机器人速度(v,ω)见图3中基元末端参数值。

图3 状态(0,0,0)的部分能耗运动基元Fig.3 Some energy consumption motion primitives for state (0,0,0)

3 改进A*算法的能耗最优路径规划

基于改进A*算法的能耗最优路径规划问题可以表示为在有向图G=(S,E)上按照设定的优化目标搜索获得一条从起始状态到目标状态的最优路径。其中,S是离散状态空间中所有节点s={x,y,θ}的集合,而E是有向图中节点状态之间连接关系的集合,即能耗运动基元集。

本文所提出的改进A*算法的能耗最优路径规划方法的伪代码如算法1所示。算法1与A*算法不同的是该算法以能耗代价作为节点的启发值,而非距离代价。为了尽可能选取准确的启发值,以提升路径规划效率,在二维离散状态空间(x,y)中先采用Dijkstra算法反向从目标状态向起始状态进行路径搜索(算法1第1行),该搜索过程可以离线进行,并不占用能耗最优路径规划时间。当获取到距离最优路径后,则任一节点s的g(s)值可作为进行正向搜索时的启发值h1(s),然而由于该启发值仅包含距离信息,将其乘以一个权重因子e作为能耗最优路径规划的启发值,即:

h(s)=e·h1(s)

(14)

式中:权重因子e为机器人在恒定平移速度下直线运动1 m的平均能耗值。

与A*算法一致,在能耗最优路径规划算法实施过程中也同样维护了OPEN表和CLOSED表,在搜索过程中优先扩展f值小的节点(算法1第4行)。对于每一个扩展的节点s,根据能耗运动基元集中定义的节点连接关系,找到该节点的所有子节点s′,采用E_cost函数计算节点s和s′之间的通行能耗值,即查询s和s′之间相应运动基元的能耗值,进而更新节点s′的g值(算法1第6、7行)。当目标状态sgoal被扩展时,搜索过程即中止,因此,根据搜索过程中节点之间确定的最优连接关系,即可获得从起始状态sstart到目标状态sgoal的能耗最优路径。

算法1 基于改进A*算法的能耗最优路径规划算法1 给定sstart,sgoal,运行反向Dijkstra算法,获得任一状态s的启发值h1(s)2 g(sstart)=0, 其他状态s设定g(s)=∞, h(s)=e·h1(s), OPEN={sstart}3 while sgoal没有被扩展 do4 从OPEN表中移出f值最小的节点s5 根据能耗运动基元集, 找到节点s的所有子节点s'6 if g(s')>g(s)+E_cost(s, s') then7 g(s')=g(s)+E_cost(s, s')8 将s放入CLOSED表, s'放入OPEN表, 更新f(s')=g(s')+h(s')9 end10 end

4 实验结果与分析

为了验证所提出的基于改进A*算法的能耗最优路径规划方法的有效性,本文分别开展了离线地图仿真测试和机器人平台测试。在实验测试过程中,能耗最优路径规划所采用的能耗运动基元集利用图4所示的四轮差速驱动机器人平台参数生成。

图4 移动机器人平台Fig.4 Platform of mobile robot

假定机器人的起始位置为(0,0),对其航向角进行角分辨率π/8离散,可获得16个机器人位姿(0,0,θi),其中,θi=i(π/8) (i=0,1,…,15)。对于任一机器人位姿(0,0,θi),以恒定平移速度v=1.0 m/s和横摆角速度ω=π rad/s作为输入量,按照第2节的方法生成能耗运动基元。之后,结合机器人平台的性能参数,如表1所示,根据式(10)对每条运动基元进行能耗计算,从而获得该机器人实验平台的能耗运动基元集,如图5所示。

表1 机器人平台的性能参数

续表1

图5 位置(0,0)的能耗运动基元集Fig.5 Energy consumption motion primitive set for position (0,0)

4.1 离线地图仿真测试

在离线地图仿真测试过程中,生成了尺寸大小为1 000×1 000、1 500×1 500和2 000×2 000的二维栅格地图各50幅,其分辨率为10 cm×10 cm,如图6所示。所生成的地图上存在大量随机障碍物,选取(0,0,0)为起始状态,而目标状态位于地图右上顶点。

图6 路径规划结果对比Fig.6 Comparison of path planning results

整个仿真测试在装有Ubuntu 18.04系统的计算机上运行,其CPU配置为Intel(R) Core i7-8750H,主频为2.5 GHz,运行内存为8 G。在同一幅栅格地图上,分别使用A*算法[35]、ECA*算法[16]和本文所提出的改进A*算法的能耗最优路径规划方法进行规划。A*算法以生成的距离最优路径上所有运动基元的能耗之和作为该路径的能耗值,而本文所提出的能耗最优路径规划方法采用图5中的能耗运动基元集进行规划,所获得的路径代价即为路径能耗值。这两种算法所规划生成的路径如图6所示,其中黑色实线为本文所提出的规划方法生成的能耗最优路径,路径中有56个转弯运动基元,而黑色虚线为A*算法规划生成的距离最优路径,路径中有98个转弯运动基元。从图6中可以看出,本文所提出的规划方法生成的路径更倾向于避开密集障碍物之间的狭窄通道,用来降低转弯频率,进而减少路径能耗。

对于同尺寸栅格地图,将这3种规划方法生成路径的规划时间、路径长度和路径能耗分别取平均值,对比结果见表2所示。从表2中可以看出,在这3种不同尺寸地图上,本文所提出的能耗最优路径规划方法与A*算法、ECA*算法相比,扩展节点数少、搜索效率更高,在整个路径规划过程中的规划时间明显减少。所获得的路径长度与A*算法及ECA*算法规划生成的路径长度基本接近,而所得路径的能耗值有显著减少,平均约比A*算法及ECA*算法规划生成的路径能耗分别降低28.24%和14.06%。

表2 测试结果数据对比

4.2 机器人实验

本文基于CoppeliaSim软件构建了类似于巷道的仿真场景,如图7所示,以测试验证所提出的能耗最优路径规划方法的有效性。该仿真场景尺寸大小为50 m×50 m,含有半封闭的石质墙体、随机放置的多个木质箱子和建筑物等。

机器人实验在ROS (robot operating system)软件架构上运行,起始位置如图7中左下角红色圆圈所示,而目标位置在目标建筑物左侧。首先采用同步定位与地图构建(simultaneous localization and mapping, SLAM)算法建立仿真环境的二维栅格地图。然后在该地图上设定路径规划的起始状态和目标状态,采用本文所提出的能耗最优路径规划方法和A*算法进行全局路径规划,所生成的路径分别如图8和图9中的绿色实线所示。可以看出,本文所提出的能耗最优路径规划方法生成路径转弯次数少,而A*算法规划生成的路径上存在多次转弯情况,这将导致机器人频繁进行转向运动,因此能耗较大。

图7 巷道仿真场景Fig.7 Simulation scene of roadway

图8 能耗最优路径Fig.8 Energy-efficient path

图9 距离最优路径Fig.9 Distance optimal path

在该实验中,本文所提出的改进A*算法的能耗最优路径规划方法和A*算法生成的路径在长度和能耗上的对比结果如表3所示。从表中可以看出,所提出的规划方法生成的能耗最优路径的长度增加了约10.6 m,然而,由于转弯次数较少,导致路径的总能耗降低了5 621 J,从而表明所提出的能耗最优路径规划方法能够减小路径能耗,更多关于这次实验的结果视频可参看https:∥github.com/robotman801/Energy-optimal-path-planning/releases/tag/Video1.0。

表3 仿真场景测试结果

5 结束语

为了提高移动机器人在运动过程中的能源利用效率,本文对传统A*算法进行改进,提出了一种改进A*算法的移动机器人能耗最优路径规划方法,以规划获得从起始状态到目标状态的全局能耗最优路径。大量离线地图仿真测试和机器人实验结果表明本文所提出的能耗最优路径规划方法能够规划生成全局能耗最优路径,路径的能耗值平均约降低28.24%,而在路径长度上仅有少量增加,可以引导移动机器人朝着低能耗的方向运动。

值得一提的是,全局能耗最优路径是一条引导路径,所获得的路径能耗是移动机器人执行该路径的预估能耗值,而机器人在跟随路径过程中的真实能耗与局部运动规划及控制密切相关。因此,将全局能耗最优路径规划与局部能耗最优轨迹规划结合研究将是后续继续开展的工作。