张浩杰 苏治宝 HERNANDEZ D E 苏 波

(1.中国北方车辆研究所兵器地面无人平台研发中心， 北京 100072； 2.德国移动技术有限公司， 伯恩 53757)

0 引言

移动机器人集成了传感器、定位与深度学习、高精度地图、路径规划、障碍物检测与避障、机械控制、能耗与热管理、系统集成与优化等多项关键技术和基础理论，在军用和民用领域具有巨大的发展前景。全电驱动机器人具有零排放、噪声小、高能源利用率和控制简单准确等优势[1]，越来越多的研究机构将其应用于智能工厂[2]、物流配送[3]、安防、精准农业[4]和军事巡逻侦察[5]等领域。随着机器人自主能力的不断提升，它们势必在室外环境中执行更多的任务。然而，由于受容量及重量的限制，机器人单次携带的能源是有限的，如电池或者燃料等，有时甚至很难返航进行能源补给，如在外太空执行任务的星球探测车。因此，能源因素限制了机器人连续执行任务的持久性。为了使机器人在能量有限或者补给不足的情况下更高效地执行更多的任务，降低其运动过程中的能耗变得尤为重要，并逐渐成为研究热点。

基于能耗最优的路径规划方法最早应用于无人飞行器[6]及无人潜水器[7-8]，由于这类平台在运动过程中受风流或水流干扰较大，能耗最优的路径规划方法可以有效地借助风流及水流的运动[9]，降低机器人在运动过程中的能耗。随着对移动机器人在室外越野环境执行任务的需求增加，移动机器人的能耗最优路径规划方法也逐渐成为研究热点。BRODERICK等[10]采用油-电混合驱动系统可以降低部分能耗，然而一旦驱动系统确定，在机器人运动过程中只有通过路径规划、速度规划降低能耗。JONES等[11]提出了一种在不确定性、强扰动环境下的能耗最优路径规划框架，在该框架下采用迭代优化算法进行路径规划。该优化算法在路径通行消耗估价中加入了能耗，改进随机轨迹优化算法(Stochastic trajectory optimization for motion planning，STOMP)[12]为能耗最优随机轨迹优化算法(Energy-efficient stochastic trajectory optimization，EESTO)，通过不断地迭代优化获得能耗最优的路径。当不确定性环境扰动引起环境信息更新后，该方法将根据设计的不同路径对比准则选择机器人在当前环境状态下的能耗最优路径。与文献[11]中的方法不同，文献[13-14]将能耗最优的路径规划过程分为两层进行，首先，通过传统的路径规划方法生成一系列路径，如启发式搜索算法[15-16]、RRT算法[17-18]等，其次，在路径优化过程中按照设计的能耗模型估算执行这些路径时的能耗，最终获取能耗最优路径。这类方法的不同之处在于所采用的能耗模型，MEI等[13]采用六次多项式曲线建立电机的能耗模型，通过线性转换描述平台速度与驱动电机速度之间的关系，进而推算平台的能耗，在能耗计算过程中考虑了转弯、加速、减速等运动。同样，也可以通过平台执行路径时与地面的摩擦力计算能耗[14]，这种能耗模型仅考虑了机械运动能耗，而未涉及发热损失能耗。

本文提出一种基于改进实时动态(Anytime Dynamic A*， AD*)算法的移动机器人低能耗最优路径规划方法。该方法利用建立的机器人运动学模型和能耗模型，采用基于采样的模型预测算法生成优化轨迹簇，将执行路径时的距离成本消耗和能耗融进优化轨迹簇。依据优化轨迹簇中的节点连接关系，改进启发式搜索算法，以实现在路径规划过程中获得机器人的能耗最优路径。

1 机器人动力学模型及能耗模型

1.1 机器人动力学模型

机器人的动力学模型能够更好地表达机器人的控制输入、运动速度、加速度、机器人与地面的作用力等特征。尤其当机器人工作在室外环境时，在有起伏坡度的地形条件下，通过动力学模型能够更加真实准确地估算机器人跟踪路径的能耗。定义机器人坐标系xoy和大地坐标系XOY，在机器人坐标系中，x轴的正方向指向机器人的前方，y轴正方向指向机器人的左方，z轴正方向指向机器人的上方，而大地坐标系为固定坐标系。对于差速转向的机器人而言，其相对机器人坐标系的平移速度和角速度由两侧电动机的驱动旋转产生。图1所示是机器人以恒定的平移速度和角速度绕瞬时转动中心运动示意图。

图1 机器人绕转动中心运动示意图Fig.1 Sketch of robot performing a circular turn

在图1中，以(vx,ωz)表示机器人在机器人坐标系中沿x轴的平移速度和绕z轴的角速度，则其在运动过程中的状态转移方程为

(1)

式中r——机器人驱动轮半径，mm

B——机器人宽度，mm

ωl、ωr——左、右两侧驱动轮的角速度，rad/s

对于给定的采样控制量(vx,ωz)，对式(1)求解，可得两侧驱动轮的转动角速度ω为

(2)

参考MEI等[13]对机器人在起伏路面运动时的非线性动力学约束的研究成果，将机器人的动力学模型以车轮状态表示为

(3)

其中

τ=[τlτr]T

式中M——质量矩阵

G(q)——克服的重力

q——左、右驱动轮的角位移矢量

τ——左、右驱动轮的电动机输出扭矩

1.2 机器人能耗模型

机器人在运动过程中的能耗包含机械能量消耗和电动机的电阻线圈发热能耗，本文对这两部分能耗进行分析，从而建立机器人的能耗模型。将机器人左右两侧的驱动系统简化为图2所示的等效电路，图中Ul和Ur是左、右两侧电动机驱动器的输出电压，il和ir是左、右等效电路中的电流。

图2 机器人电动机驱动系统等效电路Fig.2 Circuit diagram for motor driving system of robot

在图2中，等效电路包括电池、电动机驱动器和电动机线圈电阻Re。电动机和电动机驱动器之间的关系如图3所示，电动机驱动器采用电流调速的方式使左右车轮达到期望速度。当电动机驱动器向电动机提供电流驱动时，电动机转动并消耗能量。相反地，当外在扭矩驱动电动机转动时，电动机产生反电动势，储存能量。

图3 机器人速度闭环控制系统原理框图Fig.3 Closed-loop control system for robot

从图2及图3可知，机器人的能耗主要包括电动机输出扭矩驱动机器人运动的能耗及电动机线圈的发热能耗，因此，它的能耗模型可以表示为

P=ωτ+I2Re

(4)

式中P——机器人能耗

I——电动机线圈电流

以扭矩τ表示电动机的驱动电流

(5)

式中KT——扭矩常量系数g——传动比

η——电动机的传动效率

假设左、右车轮的能耗效率分别为ηl和ηr，则左、右车轮的能耗模型可分别表示为

(6)

(7)

将式(5)代入式(6)、(7)，可将左、右车轮的能耗Pl和Pr表示为扭矩函数，即

(8)

(9)

式(8)、(9)中τl和τr可由式(3)计算获得。

因此，根据式(8)、(9)可知，机器人在运动过程中的总能耗E(t)可以表示为

(10)

2 基于运动学约束和能耗模型的优化轨迹簇

2.1 机器人运动学约束分析

给定起始状态下，机器人的运动学约束能够对可达到的下一个位置状态进行限制。机器人在运动过程中既可平移，又可转动，因此，它共有6个自由度，分别以位置量(x,y,z)和转动量(φ,ψ,θ)表示，其中φ、ψ、θ分别为侧倾角、俯仰角、横摆角。如果以z=[xyz]T和ψ=[φψθ]T表示机器人在大地坐标系中的位移矢量和角度矢量，根据欧拉变换关系，可得机器人的运动学模型为

(11)

(12)

式中，[vxvyvz]T是机器人相对于机器人坐标系的线速度，而[ωxωyωz]T是机器人相对于机器人坐标系的角速度，c表示cos函数，s表示sin函数，t表示tan函数。

给定机器人的起始状态，将采样控制量作用于起始状态，则根据式(11)、(12)表示的机器人运动学模型可以估计它的下一时刻可达到的状态。在本文中，为了优化计算，不考虑机器人的俯仰和侧倾，即φ=ψ=0。以Xinput=[vxvyvzωz]T作为机器人运动学模型的输入量，而Xout=[xyzθ]T作为输出状态量，则式(11)、(12)可合并为

(13)

因此，给定机器人的输入量，即可通过式(13)获得机器人在控制周期内的可达到的状态量。

2.2 SBMPO算法预测生成优化轨迹簇

SBMPO算法融合了基于采样的路径规划算法和模型预测控制算法思想生成轨迹[20]，以避免基于采样的路径规划算法的计算复杂等瓶颈问题。该算法集成在模型预测控制算法框架中，因此，对于给定的起始状态xcurrent和目标状态xgoal，SBMPO算法对起始状态执行控制，并重复优化计算两个状态之间的最优路径，直至到达目标状态xgoal。在算法实施过程中，SBMPO算法生成一系列满足输入约束的控制状态空间采样，将采样控制量作用于系统模型，以生成与当前节点状态相关联的邻近节点。按照距离最短、时间最快或者能耗最少等期望目标，评估计算生成的邻近节点与当前节点之间通行消耗值。通过将采样控制量连续作用于通行消耗值最低的节点，直至目标状态xgoal被扩展搜索，从而生成从起始状态到目标状态的优化轨迹。

采用SBMPO算法对机器人控制状态空间进行采样，将采样控制量作用于机器人的运动学模型，以生成在一定仿真时间内的预测优化轨迹簇。以xi=(0,0,0,θi)表示机器人在起始位置的离散初始状态，i=1,2,…,N，位置为坐标原点，其中，N为机器人在大地坐标系中横摆角等额离散的数目。在机器人的控制状态空间u，采用拟随机采样生成控制量，将控制量作用于式(13)表示的机器人运动学模型，根据它的初始状态量xi生成在仿真时间[t1,t2]内的输出状态xo(t)，其算法流程如下：

whilei=1 &&i<=Ndo

xi←start

repeat

SBMPO(xi,xo(t),B)

如果xo(t)有效，则存储

until 生成B条有效轨迹

i++

end while

对于给定的机器人初始状态xi，采用SBMPO算法生成输出状态，如果输出状态有效，则存储，直至对每一个初始状态生成B条有效轨迹。对于每一个给定的起始状态xi，这些输出状态序列组成了与初始状态关联的轨迹T(xi)={xo(t1),xo(t1+Δt),…,xo(t2)}，其中xi=xo(t1)，Δt为时间步长。

对于采样生成的轨迹T(xi)，根据感知模块提供的环境地图的栅格分辨率优化轨迹，将其末端状态xo(t2)优化至栅格中心，这些与初始状态关联的轨迹簇，组成了用于能耗最优规划的优化轨迹簇。图4所示是机器人在初始状态xi=(0,0,0,0)时采用SBMPO算法生成的优化轨迹簇示例。在该优化轨迹簇生成中，设定的地图栅格分辨率为0.2 m，采用的控制量采样空间为(vx,vy,vz)∈[0,0.5]和ωz∈[0,π/8]。从图4中可以看出，基于SBMPO算法生成的优化轨迹簇包含7条有效轨迹，分别以不同的颜色显示，这些轨迹是与机器人起始状态相关联的可到达的最优路径，既有在XOY平面的平移，也有沿Z轴方向的梯度运动位移。

图4 采样空间生成的轨迹簇Fig.4 Trajectory cluster generated by sampling

对于轨迹簇中的每一条局部轨迹T(xi)，其通行成本消耗主要包括距离成本消耗和能量成本消耗两部分，即轨迹起始状态和终止状态之间的消耗c(xo(t1),xo(t2))。距离成本消耗可以通过机器人的初始状态和末端状态之间的欧几里得距离计算[21]，而能量成本消耗依据生成该局部轨迹时的采样控制量，按照式(2)推算左、右驱动轮的角速度ωl和ωr。将机器人的左、右驱动轮的角速度代入式(8)、(9)计算左、右驱动轮的能耗，而在采样控制周期内的总能耗按式(10)计算获得。因此，轨迹簇中每条轨迹的总通行成本消耗为距离成本消耗和能耗成本消耗之和，即

c(xo(t1),xo(t2))=α‖xo(t1),xo(t2)‖+

(14)

式中α——机器人在平坦路面移动欧几里得距离1 m时的能耗

‖xo(t1),xo(t2)‖——节点xo(t1)与xo(t2)之间的欧几里得距离

μ——地面摩擦因数

g——重力加速度

m——机器人质量，kg

xo,z(t1)、xo,z(t2)——节点xo(t1)与xo(t2)在z轴方向的位移

通过上述采样、预测、优化轨迹过程，这些轨迹簇组成了节点与节点间的连接有向图，这些节点间的通行成本消耗可以按照距离成本消耗和能耗成本两部分加权计算获得，见式(14)。

3 AD*算法与低能耗的最优路径规划

3.1 AD*算法

AD*算法是基于传统A*算法的改进算法，通常简称为AD*算法，主要用于解决环境信息改变及时间约束下的最优路径规划问题。与A*算法不同，它在估价函数中对启发值增加了一个比例因子ζ(ζ≥1)，其修正后的估价函数可以表示为

f(x)=g(x)+ζh(x)

(15)

式中g(x)——从起始节点到当前节点x的真实通行成本

h(x)——从当前节点x到目标节点的估算通行成本，也称为启发值

AD*算法在进行路径搜索过程中共维持了OPEN表、CLOSED表和INCONS表3个表。OPEN表中存储待被搜索的节点，CLOSED表中存储已经被搜索过的节点，而INCONS表中存储由于比例因子ζ改变或节点之间平移消耗改变后导致出现状态不一致的节点(v≠g)。AD*算法搜索节点的先后顺序由节点的f值决定，通过将搜索过程中出现的状态不一致的节点重新移至OPEN表中, 实现算法的实时性和增量性，关于该算法的理论分析参见文献[22]。

3.2 节点评估函数

在采用低能耗最优路径规划方法进行路径搜索过程中，对于每一个搜索节点x，生成x的节点集合称为x的父节点集P(x)，而由x生成的节点集合称为x的子节点集S(x)。以x′∈P(x)为到达x节点的最优父节点，则节点x评估函数中的g(x)项可表示为

g(x)=g(x′)+c(x′,x)

(16)

式中，c(x′,x)为节点x′与节点x之间的真实通行成本消耗，即这两个节点之间的优化轨迹成本消耗，可按式(14)计算。

节点x的估价函数中的h(x)为节点x到目标节点xgoal的估算消耗成本，该估算消耗成本为两个节点之间的欧几里得距离成本消耗和机器人克服重力的能耗[23]之和，即

h(x)=α‖x,xgoal‖+μmg(xz-xgoal,z)

(17)

将式(16)、(17)代入式(15)，可得节点x的评估函数为

(18)

3.3 基于AD*算法与低能耗模型的最优路径规划算法

本文在研究过程中采用AD*算法思想，沿用其在路径搜索过程中管理节点的OPEN表、CLOSED表和INCONS表，设计基于AD*算法和低能耗模型的最优路径规划算法(称为最优路径规划改进AD*算法)，其算法流程如下：

(1)xcurrent←start。

(2)g(xcurrent)=rhs(xcurrent)=∞;g(xgoal)=∞;rhs(xgoal)=0;ζ=ζ0。

(3) 将xcurrent和xgoal放入OPEN表中。

(4) repeat。

(5) 从OPEN表中按照评估函数移出估值最小的节点x。

(6) 如果g(x)>rhs(x)

g(x)=rhs(x)。

(7) 将x放入CLOSED表中。

(8) 根据轨迹簇找到x的子节点集S(x)，并更新子节点的通行消耗成本(距离和能耗)。

(9) 否则g(x)=∞。

(10)根据轨迹簇找到x的子节点集S(x)，并更新子节点的通行消耗成本(距离和能耗)。

(11)untilxgoal被搜索，生成能耗最优路径。

(12)如果环境地图变化

对所有关联节点，更新其通行消耗成本。

(13)对所有关联节点，按照g(xi)和rhs(xi)关系更新xi的搜索表所属关系。

(14)将INCONS表中节点移至OPEN表中。

(15)重复步骤(2)～(9)，生成环境变化后的能耗最优路径。

所提出的基于AD*算法和能耗模型的最优路径规划方法与传统AD*路径规划方法的不同之处在于其在搜索过程中使用的节点关联关系及节点通行成本评估函数。采用该算法进行路径搜索过程中，从机器人的起始状态开始，根据式(18)节点评估值的计算方法，不断将估价值最小的节点进行搜索，按照轨迹簇找到其相关的子节点，并将子节点不断放进OPEN表或INCONS表进行搜索，直至搜索到目标节点，路径搜索终止，从而得到了从起始状态到目标状态的能耗最优路径。

该方法在路径规划过程中，充分考虑了每个节点与父节点及子节点之间的距离成本消耗和能耗成本，因此，搜索获得的路径为能耗最优的路径。

4 仿真实验

图5 机器人实验平台Fig.5 Experimental platform of robot

为了验证所提出的最优路径规划方法的有效性，本文根据机器人实验平台的性能参数，利用机器人操作系统(Robot operating system，ROS)中的仿真软件Gazebo生成模拟的移动实验平台，并随机搭建了20个室外场景进行仿真验证。实验平台为四轮驱动机器人，如图5所示。实验平台的硬件主要是由工控机、UTM-30LX-EW型激光雷达、CCD摄像头和遥控手柄组成。机器人的工作模式分为遥控模式和自主模式两种，在遥控模式下，操作者可以通过遥控手柄控制机器人运动，而在自主模式下，机器人需要根据探测到的环境信息进行自主运动。

机器人实验平台的主要性能参数如表1所示，在Gazebo中按照表1的参数模拟机器人平台进行仿真测试。

表1 机器人平台性能参数Tab.1 Main specifications of robot

4.1 仿真场景设计

在仿真测试中，通过仿真软件Gazebo搭建了20多个室外测试环境，如图6所示，尺寸约为60 m×60 m。仿真测试环境中根据表1的机器人可通行性能设定了一定的起伏及坡度。

图6 仿真测试场景示例Fig.6 Example of scenario for simulation

在这些模拟测试环境中，机器人平台通过携带的多种激光传感器和立体视觉传感器完成对模拟环境的探测，并生成三维栅格地图，如图7所示。栅格分辨率设定为20 cm×20 cm×20 cm，该部分工作由环境感知模块完成，而所输出的三维栅格地图是本文研究工作的基础。

图7 三维栅格地图Fig.7 3D grid map

4.2 结果分析

分别采用AD*算法在图7所示的三维栅格地图上进行距离成本最优路径规划和利用本文所提出的规划方法进行能耗最优路径规划，以进行规划结果的对比分析。整个仿真计算在装有Ubuntu 16.04系统的计算机上运行，该计算机的配置为Intel(R) Core(TM) i7-4790 CPU，主频为3.60 GHz。在仿真实验过程中，所采用的用于计算能耗的关键参数如表2所示。

表2 仿真实验关键参数Tab.2 Key parameters for simulation

对于给定的起始状态和目标状态，分别采用AD*算法进行距离成本最优路径规划和本文所提出的能耗最优规划方法进行路径规划。根据这两种方法获得路径上的节点连接关系，计算路径上节点之间的欧几里得距离之和，即路径长度。同时，按照式(14)计算路径节点之间的能耗之和，即从起始状态到目标状态的能量消耗之和。仿真实验结果的统计分析见表3，分别对这两种方法规划生成路径的长度和能量消耗进行对比分析。从表3可以看出，本文所提出的移动机器人能耗最优规划方法与距离最优规划方法相比，在整个路径规划过程所需要的规划时间基本一样，规划路径的长度增加了3.7%，而能量消耗约降低了16.1%。

表3 仿真实验结果Tab.3 Simulation results

图8所示是移动机器人能耗最优规划方法及距离最优规划方法规划的路径在三维栅格地图中的表示，其中紫色为距离最优路径，而绿色为能耗最优路径，其相对应在仿真环境中的显示如图9所示。从图8、9也可看出，这两条路径基本接近，而路径能耗则降低了约16.1%。

图8 三维栅格地图中的能耗最优及距离最优路径Fig.8 Energy optimal and distance optimal paths in 3D grid map

图9 仿真场景中的能耗最优及距离最优路径Fig.9 Energy optimal and distance optimal paths in simulation scenario

5 结束语

针对机器人在室外环境执行任务时每次供给的能源(电池/燃料)有限，有时甚至很难实现补给等问题，本文提出了一种基于AD*算法的低能耗移动机器人最优路径规划方法。该方法通过构建机器人的动力学模型及其在运动过程中的能耗模型，实现对路径的能耗估计。结合机器人的运动学模型，采用基于采样的模型预测算法生成优化轨迹簇，这些轨迹簇融合了距离成本消耗和能耗模型估算的能耗成本。改进AD*算法，将路径能耗融入路径搜索算法的节点评估函数，根据轨迹簇中的节点连接关系和三维环境地图进行在线规划，从而获得能耗最优路径。最后，通过仿真测试证实了所提出的规划方法可以有效地降低能耗，提高执行任务的效率。