CN115903816A - 一种低能耗移动机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种低能耗移动机器人路径规划方法及推荐系统包括，利用栅格法构建路径规划环境地图并确定起始位置和目标位置；利用ESA*算法进行静态环境下的机器人路径规划，并对规划路径进行转折惩罚、剪枝和平滑处理，获取全局节能路径；提取全局节能路径的子目标点生成子目标点序列，利用IDWA算法在相邻的子目标点间进行局部路径规划，直至目标点；利用全局节能路径偏离评价和航向角自适应调整策略对融合算法进行优化。本发明能部署应用于配置不高的移动机器人设备上，能够对移动机器人完成路径规划，并考虑时间、长度、能量消耗等以确定机器人能够在有限能量下完成尽可能多的任务；能够快速规划合理路径，降低车间能耗，并节省人力支出。

Description

一种低能耗移动机器人路径规划方法

技术领域

本发明涉及机器人路径规划技术领域，尤其涉及一种低能耗移动机器人路径规划方法。

背景技术

机器人移动作业过程中，能源容量的多少从根本上决定了其执行给定任务的持久性，目前主要通过增加能源容量与补给次数完成，但提升效果有限且增加了资源成本。在能源有限、补给不足时，移动机器人可通过节能、有效的路径规划方法，减少能源的损耗，提升复杂环境下的作业能力。

常见的路径规划方法有遗传算法、A*算法、快速随机树搜索法、动态窗口法(Dynamic window approach，DWA)、人工势场法等，其中A*算法应用最广泛；A*算法常用于静态环境下最优路径的求解，其根据代价函数在静态环境下选择一条从起点到终点距离最优的路径；在A*算法的基础上，研究者们先后提出了改进算法例如EA*、LPA*等；如何在能量有限或补给不足的情况下，延长机器人在复杂环境下的作业时间和提升其动态避障能力，成为了当下一个重要的研究问题。

发明内容

本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

鉴于上述现有存在的问题，提出了本发明。

因此，本发明提供了一种低能耗移动机器人路径规划方法，能够解决复杂狭窄环境下机器人节能路径的规划与动态避障的问题。

为解决上述技术问题，本发明提供如下技术方案，一种低能耗移动机器人路径规划方法，包括：

利用栅格法构建路径规划环境地图并确定起始位置和目标位置；

利用ESA*算法进行静态环境下的机器人路径规划，并对规划路径进行转折惩罚、剪枝和平滑处理，获取全局节能路径；

提取全局节能路径的子目标点生成子目标点序列，利用IDWA算法在相邻的子目标点间进行局部路径规划，直至目标点；

利用全局节能路径偏离评价和航向角自适应调整策略对融合算法进行优化。

作为本发明所述的低能耗移动机器人路径规划方法的一种优选方案，其中：所述利用ESA*算法进行静态环境下的机器人路径规划，包括：基于移动机器人能耗情况构建移动机器人的总能耗方程，表示为：

E_robot＝(1+k)E_motion＝(1+k)[mg sin(φ)*s+μmgcos(φ)*s]

其中，m为机器人总质量,g为重力加速度,μ为摩擦系数；φ为坡度且取值范围为：s为移动距离；k为设备能耗E_equipment与运动能耗E_motion的比例系数。

作为本发明所述的低能耗移动机器人路径规划方法的一种优选方案，其中：所述利用ESA*算法进行静态环境下的机器人路径规划，还包括：构建ESA*算法基于能耗成本的路径搜索新准则，表示为：

E(k)＝g′(k-1)+A_E(k-1，k)+A_E(k，goal)

其中， E(k)为移动机器人行驶到路径节点k的总能耗；g′(k-1)为节点k的父节点的能耗代价；A_E(k-1，k)为节点k-1到节点k间的真实能耗代价；A_E(k，goal)为节点k到目标点的预估能耗代价。

作为本发明所述的低能耗移动机器人路径规划方法的一种优选方案，其中：所述转折惩罚，包括：引入惩罚因子的能耗评价函数，表示为：

其中，p为转弯惩罚因子，ψ为扩展节点与动态转角基准的角度。

作为本发明所述的低能耗移动机器人路径规划方法的一种优选方案，其中：所述剪枝处理，包括：对初始路径节点进行三角剪枝处理，进一步剔除冗余节点、减少转折次数与转折角度，具体步骤为：

当ESA*算法初步确定节能路径生成点时，进行路径节点剪枝处理，从初始路径节点集N{n_i,1≤i≤m}的起点依次选三个节点记为三角点Q、中间节点M以及末点E并进行三角形判定；若不是三角形，中间节点M为冗余点，删除，更新N；反之，连接QE，对线段QE进行障碍物判定，若无障碍物，中间节点M为冗余点，删除，更新N；否则，不更新N；当路径节点集N更新时，三角点Q不变，在节点集N中，从三角点Q重新开始依次对一节点进行剪枝优化，直至目标点n_m；而当初始路径节点集N不更新时，将中间节点M更新为三角点Q，同理在节点集N中以新三角点Q重新开始依次对下一节点剪枝优化，直至目标点n_m。

作为本发明所述的低能耗移动机器人路径规划方法的一种优选方案，其中：所述平滑处理，包括：利用动态弦定弧过渡法对转折点进行平滑处理，生成光滑连续、低耗能的路径，具体步骤为：

计算转折角度并作其角平分线T_iT_i′，在角平分线T_iT_i′上任选一个点为弦点C_i；

过弦点作角平分线T_iT_i′的垂线，与转折边线段T_i-1T_i、T_iT_i+1相交于两点h_i、h_i′，得到弦线h_ih_i′；

对弦线h_ih_i′障碍物评定，若有障碍物，利用C_i′＝C_i+λ|C_iT_i|动态更新弦点，并重新获取弦线；否则，过点h_i作线段T_i-1T_i的垂线交角平分线T_iT_i′于点O_i，计算平滑路径的曲率|O_ih_i|^-1；式中，C_i′为新弦点，λ动态更新因子，|C_iT_i|为线段C_iT_i的长度；

判断平滑路径的曲率是否满足安全、平滑性的要求，若不满足则返回弦点获取步骤再次执行，若满足，则以点O_i为圆心，半径|O_ih_i|作劣圆弧劣圆弧为所求平滑路径。

作为本发明所述的低能耗移动机器人路径规划方法的一种优选方案，其中：所述利用IDWA算法进行局部路径规划，包括：设计能耗评价子函数对模拟轨迹进行能耗评价，能耗评价子函数表示为：

其中，v_i、ω_i为动态窗口中第i个采样点的速度、角速度；为动态窗口第i个采样点的线加速度与角加速度；m为机器人总质量。

作为本发明所述的低能耗移动机器人路径规划方法的一种优选方案，其中：所述利用IDWA算法进行局部路径规划，还包括：构建全局节能路径偏离评价子函数Globdist(v,ω)，表示为：

其中，是全局规划路径上的样点坐标，是局部运动轨迹上的样点坐标，N是采样点的总数目。

作为本发明所述的低能耗移动机器人路径规划方法的一种优选方案，其中：所述利用IDWA算法进行局部路径规划，还包括：在IDWA算法的评价函数中加入全局节能路径偏离评价子函数Globdist(v,ω)形成最终的评价函数，表示为：

G(v,ω)＝σ[αhead(v,ω)+βdist(v,ω)+γvel(v,ω)+δEnergy(v,ω)+εGlobdist(v,ω)]

其中，αhead(v,ω)、dist(v,ω)、vel(v,ω)、Energy(v,ω)、Globdist(v,ω)分别为方位角、距离、速度评价、能耗、偏置子函数；σ为平滑函数；α、β、γ、δ、ε为各评价子函数的加权系数。

作为本发明所述的低能耗移动机器人路径规划方法的一种优选方案，其中：所述航向角自适应调整策略，包括：计算子目标点之间的倾斜角度并将其转化为弧度，对两子目标点之间的路径线段进行动态障碍物检查，若存在障碍物，则判断机器人与动态障碍物间左右区域内是否有静态障碍物，若左侧区域有静态障碍物，则以理想航向角yaw_idea为基准向右偏离设定的航向角偏离阈值yaw_out进行航向角调整，并更新机器人状态参数，将其作为下一状态参数；反之，则向左偏离yaw_out进行航向角调整；若不存在障碍物，则根据机器人状态参数获取机器人的实时航向角yaw_now，将yaw_now与yaw_idea进行比较，若设定的航向角误差阈值yaw_in≤|yaw_now-yaw_idea|，则利用航向角动态调整公式进行调整；反之，则不再进行航向角调整；

航向角动态调整的公式表示为：

其中，yaw_new为动态调整的航向角，下一状态的起始航向角；κ为机器人转弯能力系数。

本发明的有益效果：本发明提出了基于能耗成本优化策略的全局节能路径规划方法、基于动态基准转折惩罚、三角剪枝与弦定弧过渡法相结合的路径优化方法，保证全局路径最优；通过全局节能路径偏离评价和航向角自适应的调整，避免移动机器人局部规划中产生冗余路径，陷入局部解；本发明能部署应用于配置不高的移动机器人设备上，能够对移动机器人完成路径规划，并考虑时间、长度、能量消耗等以确定机器人能够在有限能量下完成尽可能多的任务；本发明能够快速规划合理路径，降低车间能耗，并节省人力支出，由于该路径规划方法有自动避障的功能，因此，工人在车间正常作业移动时无需考虑机器人的运动路线，方便工人作业；本发明可以直接在移动机器人上运行，无需额外的硬件设施，且使用简单方便，降低了使用成本，非常利于在生产车间、物流车间等场所的广泛使用，具有极高的可推广性。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：

图1为本发明一个实施例提供的一种低能耗移动机器人路径规划方法的方法流程图；

图2为本发明一个实施例提供的移动机器人运动过程的受力情况示意图；

图3为本发明一个实施例提供的基于地面水平的邻接矩阵A_φ构建示意图；

图4为本发明一个实施例提供的动态转角基准示意图；

图5为本发明一个实施例提供的路径三角剪枝示意图；

图6为本发明一个实施例提供的路径转折平滑处理示意图；

图7为本发明一个实施例提供的节能路径偏离评价子函数示意图；

图8为本发明一个实施例提供的航向角调整情况示意图，图8(a)为随机航向角示意图，图8(b)为航向角陷入局部解示意图，图8(c)为航向角产生累计误差示意图；

图9为本发明一个实施例提供的路径转折点平滑处理结果图；

图10为本发明一个实施例提供的路径平滑处理方法的性能指标对比图；

图11为本发明一个实施例提供的实验仿真环境示意图，图11(a)为15*15的仿真环境，图11(b)为30*30的仿真环境，图11(c)为60*60的仿真环境；

图12为本发明一个实施例提供的不同算法的路径规划轨迹示意图，图12(a)为15*15仿真环境下的不同算法路径规划轨迹，图12(b)为30*30仿真环境下的不同算法路径规划轨迹，图12(c)为60*60仿真环境下的不同算法路径规划轨迹；

图13为本发明一个实施例提供的多指标归一化分析结果图，图13(a)为15*15仿真环境下多指标归一化分析结果图，图13(b)为30*30仿真环境下多指标归一化分析结果图，图13(c)为60*60仿真环境下多指标归一化分析结果图；

图14为本发明一个实施例提供的空旷环境下规划的路径轨迹示意图，图14(a)为ESA*算法规划的路径轨迹，图14(b)为IDWA算法规划的路径轨迹，图14(c)为传统融合算法规划的路径轨迹，图14(d)为改进融合算法规划的路径轨迹；

图15为本发明一个实施例提供的密集环境下规划的路径轨迹示意图，图15(a)为ESA*算法规划的路径轨迹，图15(b)为IDWA算法规划的路径轨迹，图15(c)为传统融合算法规划的路径轨迹，图15(d)为改进融合算法规划的路径轨迹；

图16为本发明一个实施例提供的融合算法静态环境下的避障结果对比图，图16(a)为传统融合算法的避障过程，图16(b)为改进融合算法的避障过程；

图17为本发明一个实施例提供的融合算法动态环境下的避障结果对比图，图17(a)为传统融合算法的避障过程，图17(b)为改进融合算法的避障过程。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明，显然所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护的范围。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。

本发明结合示意图进行详细描述，在详述本发明实施例时，为便于说明，表示器件结构的剖面图会不依一般比例作局部放大，而且所述示意图只是示例，其在此不应限制本发明保护的范围。此外，在实际制作中应包含长度、宽度及深度的三维空间尺寸。

同时在本发明的描述中，需要说明的是，术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一、第二或第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。

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

实施例1

参照图1～8，为本发明的第一个实施例，该实施例提供了一种低能耗移动机器人路径规划方法，包括：

S1：利用栅格法构建路径规划环境地图并确定起始位置和目标位置。

具体的，获取图像的大小，画出水平与竖直横线形成网格图像，定义障碍物的位置，定义起始位置与目标位置，最终生成栅格地图。

S2：利用ESA*算法进行静态环境下的机器人路径规划，并对规划路径进行转折惩罚、剪枝和平滑处理，获取全局节能路径。

具体的，基于移动机器人能耗情况构建移动机器人的总能耗方程，表示为：

E_robot＝E_equipment+E_motion＝∫_tP_equipmentdt+∫_tP_motiondt  (1)

其中，P_motion表示运动过程中的功率耗损，P_equipment表示机器人内阻、传感器等电子设备的功率耗损。

需要知道的是，移动机器人的能量损耗主要为传感器等电子设备工作消耗的能量和运动过程中所消耗的能量(忽略传递到空中的热能)。

更进一步的，假设移动机器人在道路坡度不变且路面粗糙程度相同的情况下匀速行驶，由图2移动机器人运动过程的受力情况可知，移动机器人在运动过程中克服牵引阻力所损耗的能量可表示为：

W＝P_motion*Δt＝F_traction*Δs＝(F_f+F_air+F_g)*L  (2)

其中，W为运动过程中的总功；F_traction为运动过程中的总牵引力；F_f、F_air、F_g分别为运动过程中的摩擦力、空气阻力以及重力斜面水平分力；L为移动距离。

更进一步的，鉴于移动机器人在运动过程中始终保持匀速行驶且速度低，忽略空气阻力的影响，因此，由式(1)和(2)可知机器人在运动过程中的总能耗E_robot可以表示为：

E_robot＝(1+k)E_motion＝(1+k)[mgsin(φ)*s+μmgcos(φ)*s] (3)

其中，m为机器人总质量,g为重力加速度,μ为摩擦系数；φ为坡度且取值范围为：s为移动距离；k为设备能耗E_equipment与运动能耗E_motion的比例系数。

更进一步的，构建基于距离的邻接矩阵A_D与基于能量的邻接矩阵A_E，以能量矩阵A_E(i，j)作为ESA*算法的搜索准则，表示为：

A_E(i，j)＝E_robot＝(1+k)[sin(φ)+μ_i,jcos(φ)]mgA_D(i，j) (4)

其中，μ_i,j为位置i到位置j的平均摩擦系数。

应说明的是，构建基于距离的邻接矩阵与基于能量的邻接矩阵是为了表示复杂环境下的机器人移动距离和能量消耗。

更进一步的，如图3所示，构建基于地面水平的邻接矩阵A_φ(i，j)，表示为：

A_φ(i，j)＝φ_j-φ_i (5)

其中，A_φ(i，j)表示位置i到位置j的坡度大小以及坡度状态，相邻位置i与j数值差的绝对值代表着对应区域间的坡度，正负表示移动机器人爬坡与下坡两种状态。

更进一步的，获取ESA*算法基于能耗成本的路径搜索新准则，表示为：

E(k)＝g′(k-1)+A_E(k-1，k)+A_E(k，goal) (6)

其中，αhead(v,ω)、dist(v,ω)、vel(v,ω)、Energy(v,ω)、Globdist(v,ω)分别为方位角、距离、速度评价、能耗、偏置子函数；σ为平滑函数；α、β、γ、δ、ε为各评价子函数的加权系数。

更进一步的，在节点扩展中引入基于动态转角基准的转弯惩罚因子；

具体的，如图4所示，先由父节点与当前节点的坐标关系确定机器人的移动方向并以移动方向为转角基准；然后根据扩展节点与转角基准的位置关系，在能耗评价函数中利用转弯惩罚因子对其进行惩罚处理。

引入惩罚因子的能耗评价函数表示为：

其中，p为转弯惩罚因子，ψ为扩展节点与动态转角基准的角度。

更进一步的，对初始路径节点进行三角剪枝处理，进一步剔除冗余节点、减少转折次数与转折角度；

具体的，如图5所示，当ESA*算法初步确定节能路径生成点时，进行路径节点剪枝处理，从初始路径节点集N{n_i,1≤i≤m}的起点依次选三个节点记为三角点Q、中间节点M以及末点E并进行三角形判定；若不是三角形，中间节点M为冗余点，删除，更新N；反之，连接QE，对线段QE进行障碍物判定，若无障碍物，中间节点M为冗余点，删除，更新N；否则，不更新N；当路径节点集N更新时，三角点Q不变，在节点集N中，从三角点Q重新开始依次对一节点进行剪枝优化，直至目标点n_m；而当初始路径节点集N不更新时，将中间节点M更新为三角点Q，同理在节点集N中以新三角点Q重新开始依次对下一节点剪枝优化，直至目标点n_m。

应说明的是，本发明通过设置两阶段优化方法：一阶段通过引入惩罚因子对路径节点进行转折惩罚，二阶段通过对初始路径节点进行三角剪枝处理，解决了ESA*算法因冗余节点多、路径多转折、转折角度大带来额外能量耗损的问题。

更进一步的，利用动态弦定弧过渡法对转折点进行平滑处理，生成光滑连续、低耗能的路径；

应说明的是，路径的转折角，让机器人频繁启停，会导致速度交替变换，造成能耗大量损耗，从而影响机器人作业的持久性，因此需要对转折点进行平滑处理来降低损耗。

具体的，如图6所示，在一个可选的实施例中平滑处理的具体步骤为：

(1)计算转折角度并作其角平分线T_iT_i′，在角平分线T_iT_i′上任选一个点为弦点C_i；

(2)过弦点作角平分线T_iT_i′的垂线，与转折边线段T_i-1T_i、T_iT_i+1相交与点h_i、h_i′，得到弦线h_ih_i′；

(3)弦线h_ih_i′障碍物评定，若有障碍物，根据式(8)动态更新弦点，转至步骤(2)，否则，转步骤(4)；

C_i′＝C_i+λ|C_iT_i| (8)

其中，C_i′为新弦点，λ动态更新因子，|C_iT_i|为线段C_iT_i的长度；

(4)过点h_i作线段T_i-1T_i的垂线交角平分线T_iT_i′于点O_i，计算平滑路径的曲率|O_ih_i|^-1，若满足安全、平滑性的要求，转步骤(5)，否则根据式(8)更新弦点，重复步骤(2)～(4)。

(5)以点O_i为圆心，半径|O_ih_i|作劣圆弧劣圆弧为所求平滑路径。

需要知道的是，弦线|h_ih_i′|越短，移动机器人通过就越安全，不易与障碍物发生碰撞，但路径平滑性不足；为此，动态弦定弧过渡法可根据安全性、平滑性等不同需求生成相对应的平滑路径。

S3：提取全局节能路径的子目标点生成子目标点序列，利用IDWA算法在相邻的子目标点间进行局部路径规划，直至目标点。

具体的，设计能耗评价子函数对模拟轨迹进行能耗评价，能耗评价子函数表示为：

其中，v_i、ω_i为动态窗口中第i个采样点的速度、角速度；为动态窗口第i个采样点的线加速度与角加速度；m为机器人总质量。

应说明的是，传统DWA算法通过评价函数对速度空间模拟的轨迹进行评分并择选出最优轨迹；然而，在对轨迹评分中，没有考虑轨迹所消耗的能量，使得最优的轨迹不一定是能耗损耗少的；为此，需特别设计能耗评价子函数，对模拟轨迹进行能耗评价。

IDWA的评价函数表示为：

G(v,ω)＝σ[αhead(v,ω)+βdist(v,ω)+γvel(v,ω)+δEnergy(v,ω)] (10)

其中，αhead(v,ω)、dist(v,ω)、vel(v,ω)、Energy(v,ω)分别为方位角、距离、速度评价、能耗子函数；σ为平滑函数；α、β、γ、δ为各评价子函数的加权系数。

更进一步的，在融合算法的评价函数中加入全局节能路径偏离评价子函数Globdist(v,ω)，表示为：

其中，是全局规划路径上的样点坐标，是局部运动轨迹上的样点坐标，N是采样点的总数目。

经全局路径规划算法ESA*与局部路径规划算法IDWA优化后的评价函数表示为：

G(v,ω)＝σ[αhead(v,ω)+βdist(v,ω)+γvel(v,ω)+δEnergy(v,ω)+εGlobdist(v,ω)]  (12)

其中，ε为Globaldist(v,ω)评价子函数的加权系数。

应说明的是，如图7所示，全局节能路径偏离评价子函数用于计算局部运动轨迹与全局节能路径的距离；在融合算法的评价函数中加入全局节能路径偏离评价子函数是为了避免额外能量损耗，提高安全性，使得融合算法规划的路径更贴近全局节能路径。

S4：利用全局节能路径偏离评价和航向角自适应调整策略对融合算法进行优化。

应说明的是，利用全局节能路径偏离评价和航向角自适应调整策略对融合算法进行优化是为了解决规划的路径与全局节能路径偏离较大、路径冗余、陷入局部解，寻路失败等问题。

还应说明的是，当融合算法在窄空间下规划路径时，正确的航行角对成功躲避动态障碍物、快速到达目标点至关重要；在窄空间中，当机器人与动态障碍物相向而行时，如图8(a)所示，传统融合算法只根据评价函数向左或向右随机地调整航向角，不考虑前方动态障碍物与静态障碍物间的空间机器人能否通过，这样容易导致机器人原地打圈调整航向角或与动态障碍物发生碰撞；当移动机器人与动态障碍物同向而行时，如图8(b)所示，传统融合算法的动态窗口可能会陷入局部解中，只能跟随动态障碍物缓慢前行，耗时且会导致机器人频繁切换运动模式，不利于机器人运动的平稳性；当机器人在相邻子目标点间规划路径时航向角会产生累计误差，如图8(c)所示，会导致路径冗余、陷入局部解；针对上述问题，本发明对机器人状态参数中的航向角采用自适应调整策略来优化。

在一个可选的实施例中，利用自适应调整策略进行优化的具体步骤为：

步骤一：计算子目标点h_i-1、h_i在直角坐标系下的倾斜角度并转化为弧度，计算公式为：

其中，arctan2()函数的值域为[-π,π]； 为相邻子目标点横、纵坐标的增量；为路径线段h_i-1h_i直角坐标系下的倾斜角度；yaw_i-1i为机器人路径线段h_i-1h_i移动的理想航行角yaw_idea；

步骤二：对路径线段h_i-1h_i进行动态障碍物检查，若检测到存在障碍，则转步骤三执行航向角调整策略1，否则，转步骤四执行航向角调整策略2；

步骤三：航向角调整策略1：判断机器人与动态障碍物间左右区域内是否有静态障碍物，若左侧区域有静态障碍物，则以yaw_idea为基准向右偏离yaw_out进行航向角调整，并更新机器人状态参数(x,y,yaw_out,v,w)，将其作为下一状态参数；反之，则向左偏离yaw_out进行航向角调整；yaw_out为航向角偏离阈值，可依实际情况进行设置；执行步骤三转步骤六；

步骤四：航向角调整策略2：根据机器人状态参数(x,y,yaw,v,w)获取机器人的实时航向角yaw_now，将yaw_now与yaw_idea进行比较，若yaw_in≤|yaw_now-yaw_idea|，则跳转步骤五，反之，则不调整航向角，转步骤六；其中，yaw_in为航向角误差阈值，可依实际情况进行设置；

步骤五：若yaw_now大于yaw_i a，则对机器人的航向角进行动态减少，更新机器人状态参数(x,y,yaw_new,v,w)，并将其作为下一状态参数；反之，则动态增大航行角。

航向角动态调整的公式表示为：

其中，yaw_new为动态调整的航向角，下一状态的起始航向角；κ为机器人转弯能力系数。

步骤六：航向角调整结束。

更进一步的，利用全局节能路径偏离评价函数对规划路径进行综合评价，利用航向角自适应调整策略对规划路径航向角进行实时判断与调整，最终获取移动机器人全局节能路径。

实施例2

参照图9～17，为本发明的一个实施例，提供了一种低能耗移动机器人路径规划方法及装置，为了验证本发明的有益效果，通过实验进行科学论证。

I.对平滑处理进行有效性验证与分析

ESA*算法能规划出一条能耗低且较为平滑的路径，但路径还存在尖峰，为非连续光滑的曲线，不利于移动机器人的运动跟随且影响机器人运行的平稳性，因此，需对路径的转折尖峰进行平滑处理。

本实施例用贝塞尔曲线平滑法(Bezier curve smoothingmethod，BCSM)、过渡圆弧法(Transition arc method，TAM)以及本发明提出的动态弦定弧过渡法(String-definite arc transition method，SDAM)进行路径平滑对比验证，平滑结果如图10所示，实验数据如表1所示，其中以弯曲能量(Bending Energy,BE)和路径长度能量(The BendingEnergy,TBE)作为机器人路径平滑性的评价指标。

表1路径平滑方法的实验结果对比

由图9可知，在相同的环境下，BCSM、TAM和SDAM都完成了对转折尖峰的平滑处理，得到光滑连续的路径，但从表1和图10中可知：在路径曲率方面，BCSM的路径曲率是最大的，TAM和SDAM的路径曲率相差不大但都远小于BCSM，其中BE和TBE值较BCSM分别减少了92.62％、93.41％，88.89％、90.07％；在路径长度方面，BCSM、TAM和SDAM相较于未平滑处理的路径均减少了，分别减少了1.701％、1.774％、2.101％；由于BCSM平滑处理后的曲率和路径长度是最大的，从而导致TBE指标值较大；通过图9的路径局部细节图可知，TAM平滑的路径会与障碍物发生碰撞，因为过渡圆弧半径的选取原则为转折点最短临边的比例定值，平滑处理过程不能动态改变圆弧半径，而BCSM和SDAM能保证优化路径的安全。

综上分析，SDAM能设计出光滑连续、曲率合适、安全性高的路径。

II.ESA*算法综合性能对比与分析

本实施例构建如图11所示的15*15、30*30、60*60的三种仿真环境，其中网格单元里的数字表示地面摩擦系数μ，绿色方框表示框内的网格单元与地面水平面的坡度为8度，而红色的为12度。在同一环境下使用ACO算法、Dijkstra算法、传统A*算法、基于改进A*与DWA算法融合的温室机器人路径规划(文献[8]算法)、基于能耗优化的节能A*算法(文献[10]算法)与ESA*算法进行机器人的全局路径规划，其规划轨迹如图12所示、性能指标结果如表2所示。为了能更直观地对比ESA*算法与其它方法在这些性能指标上的表现，对性能指标进行归一化以直方图形式进行表示，如图13所示，其中图中的“-”表示数值取反计算，利用Min-Max标准归一化进行计算得到的，若指标值越接近1，则表示该性能越好。

表2移动机器人不同路径规划方法的实验结果对比

从图12的路径轨迹可知，上述算法在复杂度不同的环境下均能成功规划出路径，而从表2和图13中可知，ESA*算法的整体性能比其它算法要好；在能耗与路径长度方面，以路径长度为优化目标的ACO算法、Dijkstra算法、传统A*算法和文献[8]算法规划路径的长度与ESA*算法规划的路径长度相差不多，但在能量方面较ESA*算法要耗费得多，在15*15、30*30与60*60环境中至少要多耗费68.6098J、265.1747J和204.8519J的能量，甚至于更多；与以能耗为优化目标的文献[10]算法相比，ESA*算法在能量损耗与路径长度方面平均减少了6.93％和4.81％，有效提升了路径质量；综合路径能耗与长度得到单位距离能耗，从表2可知，ESA*算法的单位距离能耗明显优于其它算法，尤其是在规模不大的地图中。

在路径的平滑性上，ESA*算法较传统A*算法平均减少了22.72％的转向次数和70.41％的指令次数，同时路径累计转弯角度也平均减少了38.67％，明显提高了路径的平滑性；与文献[8]的路径平滑方法相比较，在转折点数量与指令节点数量相近的情况下，路径平均转折角度从600度减少到404.8度，有效减少了转弯的角度；而与ACO、Dijkstra等其它算法相比，ESA*算法无论在转折点数量、指令节点数量还是转折角度方面都有极大的提高。

综合来看，ESA*算法在静态环境中能为机器人规划出一条低能耗且较为平滑的路径。

III.融合算法的仿真实验与分析

为验证改进融合算法的可行性，在空旷和密集两种复杂度不同的环境下分别用ESA*算法、IDWA算法、ESA*算法与传统DWA未改进的融合算法(传统融合算法)、ESA*算法与IDWA算法改进的融合算法(改进融合算法)作对比仿真实验，其结果如图15～16所示。

由图14(a)和15(a)可知，在空旷与密集环境下，ESA*算法都能规划出连续平滑的全局路径，但由于ESA*算法本身不具备局部路径规划的特性，无法在动态环境中实时躲避随机的动-静态障碍物；在图14(b)和15(b)中可看出，IDWA算法在空旷的环境中能成功抵达目标点，但路径弯曲，不太平滑，当在密集复杂的环境中时，IDWA算法陷入了局部解，路径规划失败。

由图14(c～d)和15(c～d)分析可知，传统融合算法与改进融合算法在空旷环境下都能成功抵达目标点，但传统融合算法规划的路径对全局节能路径的偏离程度明显大于改进融合算法的，容易造成额外能量损耗；在密集环境下，改进融合算法能成功抵达目标点，而传统融合算法因航向角累计偏差陷入局部解，寻路失败。

综合来看，相较于传统融合算法，改进融合算法更贴近全局节能路径，能有效减少额外能量的损耗，自适应调整航向角，避免产生冗余路径以及防止陷入局部解。

IV.融合算法的避障实验与分析

为检验改进融合算法在窄空间中躲避动-静态障碍物的有效性、优越性，分别对传统融合算法与改进融合算法进行仿真对比实验，实验结果如表3所示。

表3融合算法的避障实验结果

在窄空间的静态环境中，在路径上设置三个静态障碍物，以蓝色方格表示。从图16可知，当环境中出现静态障碍物时，传统融合算法和改进融合算法都能成功躲避并抵达目标点，但是传统融合算法的避障路径与全局节能路径偏差大且路径冗余多弯曲；由表3数据可知，改进融合算法、传统融合算法与全局节能路径相比，路径长度分别增加了0.73％、2.83％，改进融合算法在成功躲避障碍物的同时能避免冗余路径的产生，并且改进融合算法在成功躲避障碍物后能更快抵达目标点。

在窄空间的动态环境中，设置了同向、相向以及侧面三种类型的动态障碍物，以黄色方格表示，箭头表示动态障碍物的移动方向。

从图17(a)中可知，当机器人在狭窄环境中与动态障碍物同向而行且动态障碍物的速度小于移动机器人的速度时，传统融合算法会陷入局部解中，只能跟随运动缓慢的动态障碍物移动，不能跳出，耗费大量时间；当移动机器人与动态障碍物相向而行时，移动机器人不能正确调整自己的航向角，原地打圈，造成冗余路径，严重的导致寻路失败。

根据图17(b)和表3中可以看出，改进融合算法在窄空间的动态环境中能有效躲避动态障碍物，自适应调整航向角，避免生成冗余路径，防止陷入局部解。

综上所述，本文改进的融合算法能在窄空间的环境下完成动-静态障碍物的避障，且路径连续平滑、紧贴全局节能路径。

应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。本申请实施例中的方案可以采用各种计算机语言实现，例如，面向对象的程序设计语言Java和直译式脚本语言JavaScript等。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已描述了本申请的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本申请范围的所有变更和修改。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (10)

1.一种低能耗移动机器人路径规划方法，其特征在于：包括，

利用栅格法构建路径规划环境地图并确定起始位置和目标位置；

利用ESA*算法进行静态环境下的机器人路径规划，并对规划路径进行转折惩罚、剪枝和平滑处理，获取全局节能路径；

提取全局节能路径的子目标点生成子目标点序列，利用IDWA算法在相邻的子目标点间进行局部路径规划，直至目标点；

利用全局节能路径偏离评价和航向角自适应调整策略对融合算法进行优化。

2.如权利要求1所述的低能耗移动机器人路径规划方法，其特征在于：所述利用ESA*算法进行静态环境下的机器人路径规划，包括：基于移动机器人能耗情况构建移动机器人的总能耗方程，表示为：

E_robot＝(1+k)E_motion＝(1+k)[mg sin(φ)*s+μmgcos(φ)*s]

其中，m为机器人总质量,g为重力加速度,μ为摩擦系数；φ为坡度且取值范围为：

s为移动距离；k为设备能耗E_equipment与运动能耗E_motion的比例系数。

3.如权利要求1或2所述的低能耗移动机器人路径规划方法，其特征在于：所述利用ESA*算法进行静态环境下的机器人路径规划，还包括：构建ESA*算法基于能耗成本的路径搜索新准则，表示为：

E(k)＝g′(k-1)+A_E(k-1，k)+A_E(k，goal)

其中，

E(k)为移动机器人行驶到路径节点k的总能耗；g′(k-1)为节点k的父节点的能耗代价；A_E(k-1，k)为节点k-1到节点k间的真实能耗代价；A_E(k，goal)为节点k到目标点的预估能耗代价。

4.如权利要求3所述的低能耗移动机器人路径规划方法，其特征在于：所述转折惩罚，包括：引入惩罚因子的能耗评价函数，表示为：

其中，p为转弯惩罚因子，

ψ为扩展节点与动态转角基准的角度。

5.如权利要求1或4所述的低能耗移动机器人路径规划方法，其特征在于：所述剪枝处理，包括：对初始路径节点进行三角剪枝处理，进一步剔除冗余节点、减少转折次数与转折角度，具体步骤为：

当ESA*算法初步确定节能路径生成点时，进行路径节点剪枝处理，从初始路径节点集N{n_i,1≤i≤m}的起点依次选三个节点记为三角点Q、中间节点M以及末点E并进行三角形判定；若不是三角形，中间节点M为冗余点，删除，更新N；反之，连接QE，对线段QE进行障碍物判定，若无障碍物，中间节点M为冗余点，删除，更新N；否则，不更新N；当路径节点集N更新时，三角点Q不变，在节点集N中，从三角点Q重新开始依次对一节点进行剪枝优化，直至目标点n_m；而当初始路径节点集N不更新时，将中间节点M更新为三角点Q，同理在节点集N中以新三角点Q重新开始依次对下一节点剪枝优化，直至目标点n_m。

6.如权利要求5所述的低能耗移动机器人路径规划方法，其特征在于：所述平滑处理，包括：利用动态弦定弧过渡法对转折点进行平滑处理，生成光滑连续、低耗能的路径，具体步骤为：

计算转折角度并作其角平分线T_iT_i′，在角平分线T_iT_i ^′上任选一个点为弦点C_i；

过弦点作角平分线T_iT_i′的垂线，与转折边线段T_i-1T_i、T_iT_i+1相交于两点h_i、h_i′，得到弦线h_ih_i′；

对弦线h_ih_i′障碍物评定，若有障碍物，利用C_i ^′＝C_i+λ|C_iT_i|动态更新弦点，并重新获取弦线；否则，过点h_i作线段T_i-1T_i的垂线交角平分线T_iT_i ^′于点O_i，计算平滑路径的曲率|O_ih_i|^-1；式中，C_i ^′为新弦点，λ动态更新因子，|C_iT_i|为线段C_iT_i的长度；

判断平滑路径的曲率是否满足安全、平滑性的要求，若不满足则返回弦点获取步骤再次执行，若满足，则以点O_i为圆心，半径|O_ih_i|作劣圆弧

劣圆弧

为所求平滑路径。

7.如权利要求6所述的低能耗移动机器人路径规划方法，其特征在于：所述利用IDWA算法进行局部路径规划，包括：设计能耗评价子函数对模拟轨迹进行能耗评价，能耗评价子函数表示为：

其中，v_i、ω_i为动态窗口中第i个采样点的速度、角速度；

为动态窗口第i个采样点的线加速度与角加速度；m为机器人总质量。

8.如权利要求7所述的低能耗移动机器人路径规划方法，其特征在于：所述利用IDWA算法进行局部路径规划，还包括：构建全局节能路径偏离评价子函数Globdist(v,ω)，表示为：

其中，

是全局规划路径上的样点坐标，

是局部运动轨迹上的样点坐标，N是采样点的总数目。

9.如权利要求8所述的低能耗移动机器人路径规划方法，其特征在于：所述利用IDWA算法进行局部路径规划，还包括：在IDWA算法的评价函数中加入全局节能路径偏离评价子函数Globdist(v,ω)形成最终的评价函数，表示为：

G(v,ω)＝σ[αhead(v,ω)+βdist(v,ω)+γvel(v,ω)+δEnergy(v,ω)+εGlobdist(v,ω)]

其中，αhead(v,ω)、dist(v,ω)、vel(v,ω)、Energy(v,ω)、Globdist(v,ω)分别为方位角、距离、速度评价、能耗、偏置子函数；σ为平滑函数；α、β、γ、δ、ε为各评价子函数的加权系数。

10.如权利要求9所述的低能耗移动机器人路径规划方法，其特征在于：所述航向角自适应调整策略，包括：计算子目标点之间的倾斜角度并将其转化为弧度，对两子目标点之间的路径线段进行动态障碍物检查，若存在障碍物，则判断机器人与动态障碍物间左右区域内是否有静态障碍物，若左侧区域有静态障碍物，则以理想航向角yaw_idea为基准向右偏离设定的航向角偏离阈值yaw_out进行航向角调整，并更新机器人状态参数，将其作为下一状态参数；反之，则向左偏离yaw_out进行航向角调整；若不存在障碍物，则根据机器人状态参数获取机器人的实时航向角yaw_now，将yaw_now与yaw_idea进行比较，若设定的航向角误差阈值yaw_in≤|yaw_now-yaw_idea|，则利用航向角动态调整公式进行调整；

反之，则不再进行航向角调整；

航向角动态调整的公式表示为：

其中，yaw_new为动态调整的航向角，下一状态的起始航向角；κ为机器人转弯能力系数。

Images