基于环境分层方法的双足机器人路径规划研究_闵伟伟

路径规划新算法

第30卷第2期2013年2月计算机应用研究

ApplicationResearchofComputersVol．30No．2Feb．2013

基于环境分层方法的双足机器人路径规划研究

闵伟伟，刘国栋，莫栋成

（江南大学物联网工程学院教育部重点实验室，江苏无锡214122）

摘

要：为了实现双足机器人在障碍环境中的路径规划，提出一种将三维环境分层的方法，用两个截面将环境

分为高于机器人身高障碍层、低于机器人抬脚高度障碍层和中间障碍层。首先在中间障碍层进行机器人轨迹规划，再根据机器人各种步态的不同损耗构建代价函数，把规划好的轨迹放到最底层进行规划修改，最终得到双足机器人在规划路径上代价最小的一系列连续的动作，通过计算机仿真实验验证了方法的有效性。关键词：双足机器人；路径规划；环境分层；轨迹规划；代价函数中图分类号：TP242．6

文献标志码：A

文章编号：1001-3695（2013）02-0389-03

doi：10．3969/j．issn．1001-3695．2013．02．019

Pathplanningforbipedrobotsbasedonenvironmentstratification

MINWei-wei，LIUGuo-dong，MODong-cheng

（KeyLaboratoryofMinistryofEducation，SchoolofInternetofThingsEngineering，JiangnanUniversity，WuxiJiangsu214122，China）

Abstract：Inordertoachievepathplanningamongobstaclesenvironmentforbipedrobot，thispaperproposedamethodthatdi-videdthethree-dimensionenvironmentintothreeparts：theobstacleslayeraboverobot’sheight，theobstacleslayerbelowro-smovingfootheight，andthemiddleobstacleslayerbetweentheabovetwosections．Firstofall，maketrajectoryplanninginbot’

themiddleobstacleslayer，andstructurecostfunctionbasedonthedifferenttransferoflossesduetothedifferentgait，thenplacedthemiddle-tierplanningtrajectoryontothebelowlayerforplanningandmodifying．Eventually，itgotaseriesofcontinu-ousactionofminimumcostforBipedRobotspathplanning．Computersimulationexperimentshowsthatthismethodiseffective．

Keywords：bipedrobots；pathplanning；tieredoftheenvironment；trajectoryplanning；costfunction

双足机器人是机器人研究领域最前沿的问题之一，而双足机器人研究的重点是如何使双足机器人能稳定地行走。步行是人与大多数动物所具有的移动方式，是自然界经过漫长进化过程所选择出来的最有效的移动方式，而双足步行又是生物界步行方式中自动化程度最高、最为复杂的动作，是其他步行结

［1］［2］

构所无法比拟的。路径规划技术是双足机器人研究中的也是人工智能与机器人学的重要结合点。路径一个重要部分，

规划的主要内容是通过某种分析标准，在存在一些障碍物的环境内，找到一条从起始状态（包括位置和姿态）到达目标状态（包括位置和姿态）的没有碰撞的路径。双足机器人路径规划具备以下五个特性，即环境模型的多维性及复杂性、运动学及动力学模型的复杂性、稳定步行约束条件的苛刻性、路径规划的离散性和规划问题的高度复杂性。

本文提出一种将三维模型分层的方法，采用分步规划的方法将双足机器人路径规划分成两部分。

［3］

平坦的物体表面；第二种是机器人可以直接跨越的障碍物；第

三种是机器人可以从障碍物内部穿过的障碍物，如高脚的桌子；第四种是机器人不可逾越的障碍物。

由于三维环境的复杂度高，机器人对环境的处理要占用较对垂直单元进行高度方向的分多的资源。为了克服这个问题，层采样，每组采样数据提供了每个截面间的环境信息，将不同截面间的地图信息按照空间关系依次排列，便可组成环境特征的三维模型。本文用两个截面对地图进行分层处理，如图1所示，最上层（A层）的高度大于机器人行走的最大高度H，最下层（C层）的高度小于机器人的最大抬脚高度h。

将B层进行水平单元和垂直单元的栅格化处理，得到一

40列，个三维的栅格图，假设栅格图水平方向上有40行、垂直block（x，y，z）方向上有k格，每个栅格由坐标（x，y，z）表示，

y，z）的信息，表示栅格（x，反映了三维空间中每个栅格的占有有率，

k=

1环境模型

{

int（H－h）/aint［（H－h）/a］+1

（H－h）%a=0（H－h）%a≠0

（1）（2）（3）

E=｛（x，y，z）|0≤x≤40，0≤y≤40，0≤z≤k，x，y，z∈Z｝

环境模型的表示是机器人地图构建中的一个重要问题，针

对三维场景，最常用的表示方法包括几何法和高程图法。由机器人自身所携带的传感器对环境进行信息捕获并进行处理，其中包括目标、边界和障碍物，得到它们的形状、尺寸和位置等数据，利用这些几何数据建立三维模型，环境的复杂程度对建模处理过程起决定性作用。假定双足机器人所在环境中存在四种障碍物，第一种是机器人的脚可以放上去的平面，如矮小且

收稿日期：2012-07-18；修回日期：2012-08-25

block（x，y，z）=

{

01

y，z）为自由栅格，（x，y，z）∈E栅格（x，

y，z）为障碍栅格，（x，y，z）∈E栅格（x，

%为取余运算，a为栅格的边长，E为所其中：int为取整运算，

y，z）的值，有栅格的集合。假设根据block（x，最终可建立B层

的高程图，如图2所示。将B层的高程图投影到β平面，将C层投影到γ平面，得到两个二维地图，如图3和4所示。

作者简介：闵伟伟（1987-），男，江苏宝应人，硕士研究生，主要研究方向为人工智能与机器人（cayman_3@126．com）；刘国栋（1950-），男，教授，博导，主要研究方向为人工智能与机器人系统；莫栋成（1989-），男，江苏盐城人，硕士研究生，主要研究方向为双足机器人规划．

路径规划新算法

·390·

计算机应用研究第30卷

［4～7］

双足机器人路径规划就转换成在两个环境中的规划问题。

在B层，双足机器人就相当于平面移动机器人，规划问题就变使机器人在成了找到一条从给定起点到终点的连续运动轨迹，运动过程中安全、无碰撞地避开所有障碍物，且所走路径最短，所得到的轨迹为双足机器人运动足迹的连线。在C层，虽是三维环境中的规划问题，但可根据机器人的步态损耗将B层规划好的轨迹放到C层的投影平面上进行机器人的步态规划，即得到双足机器人的连续动作

。

，给定左由栅格。采用基于栅格法的机器人路径蚁群算法

上角和右下角分别为起始栅格和目标栅格，要求机器人从起始绕过障碍栅格，找出一条通往目标栅格的最短路径。栅格出发，

40列，0），如图8所示，栅格有40行、初始栅格为（0，目标40）；τ［（xi，yj）］yj+1）表示栅格（xi，yi）到栅格为（40，→（xi+1，

yi+1）的信息素；p［（xi，yj）→（xi+1，yj+1）］（t）表相邻栅格（xi+1，

yj）选择下一个可达栅格（xi+1，示蚂蚁在t时刻处于栅格（xi，

yj+1）的转移概率；Q表示问题的最优解。利用蚁群算法求解机器人路径问题即为如下优化问题：

（xi，yj）→（xi+1，yj+1）］=τ［

{

c0

（xi+1，yj+1）为可到达栅格否则

（5）

其中：c为常数。

p［（xi，yj）→（xi+1，yj+1）］（t）=

（xi，yj）→（xi+1，yj+1）］（t）ηβ（xi+1，yj+1）τα［

（xi，yj）→（xi+1，yj+1）］（t）ηβ（xi+1，yj+1）∑τ［

（6）

p∈P

（xi，yj）→（xi+1，yj+1）］（t）表示在t时刻栅格（xi，yi）到其中：τ［

yi+1）路径上残留的信息量；α、相邻栅格（xi+1，β表示信息素和yj+1）=C/d（xi+1，yi+1），C为常数，距离信息的权值；η（xi+1，

d（xi+1，yi+1）表示栅格（xi+1，yi+1）中心到目标栅格的距离。

2机器人模型及步态

算法步骤如下：

a）设定蚂蚁的数量m、最大迭代次数N，初始化各栅格的信息素，0）。将蚂蚁置于初始栅格（0，

b）每只蚂蚁根据式（4）选择下一个路径栅格，若此栅格到其相邻栅格路径上的信息素值均为0，则返回到上一个栅格，并将其置为障碍栅格。

c）重复b），40）。直到所有蚂蚁都到达目标栅格（40，d）计算各蚂蚁的路径长度L，记录当前最优解。e）进行信息素全局更新。

f）若所有蚂蚁收敛到一条路径或达到最大迭代次数，则结束循环，否则转到b）。

g）输出全局最优解Q。

为了让机器人在不同的环境内有效地避障，将机器人简化

将机器人简化成圆柱体模型，圆柱体成不同的模型。在B层，

的半径随不同的动作取不同的值。图5为机器人的前向运动

模型，图6为机器人的侧向运动模型。机器人投影到β平面为一个圆，为保证机器人能避开所有的障碍物，取圆的半径为机器人运动时的最大半径：

R=max（R1，R2）

（4）

为了实现栅格法的路径规划算法，将障碍物的边界向外扩

展R的长度，机器人可看做质点，尺寸大小忽略不计。在C层进行机器人的步态规划，将离散的步态组合起来就得到了双足机器人的整体路径规划。本文将机器人简化成三维线性倒立摆模型，根据这个模型得到机器人的各种步态。模型如图7所示。

双足机器人在行走过程中可以离散化为以下几种步态gait=｛forward，backward，turn-left，turn-right，up-bench，down-bench，side-left，side-right｝。其中，forward表示平地前进，

backward表示平地后退，turn-left表示左转，turn-right表示右up-bench表示上台阶，down-bench表示下台阶，side-left表转，

［4］

side-right表示横着向右走。示横着向左走，

由上面的算法可得到双足机器人在B层的规划路径。

在C层，将双足机器人近似为一个集中了所有质量的点如图9所和链接该质点与支撑点的无质量的腿组成的倒立摆，

相当于机器人膝关节的示。在伸缩力f的作用下腿可以伸缩，

弯曲或者伸展而产生质心的上下运动。

y和z三个方向上的分量：伸缩力f可以分解为x、

fx=（x/r）ffy=（y/r）ffz=（z/r）f

（7）（8）（9）

其中：r为支撑点和质心之间的距离，质心只受伸缩力和重力

的作用，因此运动方程为

Mx=（x/r）fMy=（y/r）fMz=（z/r）f－Mg

．．

．．．．

（10）（11）（12

）

3路径规划

ky决定平面的斜率，定义约束面z=kxx+kyy+zc，其中kx、zc决定其高度。为了使质心在约束面上运动，其加速度应该保

有了环境模型和机器人模型及动作就可以进行路径规划

了。首先对B层进行规划，由上面的定义得到栅格的尺寸和位置，如图8所示，图中黑色阴影表示障碍栅格，剩下的表示自

路径规划新算法

第2期

持与约束面的法矢量垂直，即

［f

闵伟伟，等：基于环境分层方法的双足机器人路径规划研究

表2

z

r

??

－Mg］?

??

·391·

三条路径比较

移动代价16．53212．03913．074

66．71863．40069．612

()f()f()

xr

yr

Mgrf=

zc

－kx－ky1

?????

路径平均最短路径/格

=0（13）

ⅠⅡⅢ

从而解出（14）

显然，通过施加与腿长r成正比的伸缩力f就可以将质心控制在约束面上运动。将式（14）代入式（10）和（11），求得质心在水平方向上的运动方程如下：

．．

由表2可得，路径Ⅱ的移动代价最小，因此选择路径Ⅱ为

定义通过机器人的行走路径。双足机器人通过动作改变状态，

［11］

动作b，机器人的状态由s变为s'，则有

s'=succ（s，b），b∈gait

（25）

x=y=

g

xzcgyzc

（15）（16）

．．

因此可得知质心在水平方向的运动与约束面的斜率无关。由台阶的参数可求得机器人上台阶的步态，模型如图10所示。同理，根据机器人三维线性倒立摆模型可求得双足机器人的各种步态。由双足机器人各种步态模型定义双足机器人的步态代价分别为

f（forward）=k1s1f（backward）=k2s2f（turn-left）=k3θ1f（turn-right）=k3θ2f（up-bench）=k4zcxf（down-bench）=k5zcxf（side-left）=k6s3f（side-right）=k6s4

（17）（18）（19）（20）（21）（22）（23）（24）

机器人从起始状态A到目标状态J由下面一系列动作组

B=succ（A，forward），C=succ（B，up-bench·for-合而成，

ward），D=succ（C，turn-left·forward），E=succ（D，down-F=succ（E，turn-left·forward），G=succ（F，bench·forward），up-bench·down-bench），H=succ（G，forward），I=succ（H，up-bench·forward·down-bench），J=（I，forward）。双足机器人在三维障碍环境中的路径规划即为按照图中的运动轨迹由下

bench·forward·面的动作实现的：J=succ（A，forward·up-turn-left·forward·down-bench·forward·turn-left·for-ward·up-bench·down-bench·forward·up-bench·for-bench·forward）。ward·down-

5结束语

s2、s3、s4分别为机器人前进、其中：s1、后退、左横移、右横移的

距离；θ1和θ2分别为机器人落足的左转角度和右转角度；zc为机器人上台阶时的质心高度；x为机器人走过台阶的水平长

k2、k3、k4、k5和k6为各子函数的权重系数，度；k1、有k4＞k5＞k2＞k6＞k3＞k1。

本文设计的双足机器人分步规划方法，用两个截面将三维

将中间层的高程图投影到水平面上，模型进行垂直方向分层，

用蚁群算法进行机器人轨迹规划，得到机器人双足轨迹的连线，将规划好的轨迹放到底部障碍层，使用三维线性倒立摆模

用相应的步态进行底层规划，型进行双足机器人的步态规划，

将各离散的步态连续起来得到最终的双足机器人路径规划结

果。仿真实验结果表明了提出的方法行之有效。参考文献：

［1］范强．双足竞步机器人设计及其步态规划研究［D］．淄博：山东

2009．理工大学，［2］BORENSTEINJ，KORENY．Thevectorfieldhistogram-fastobstacle

avoidanceformobilerobots［J］．IEEEJournalofRoboticsandAu-1991，7（3）：278-288．tomation，

［3］CHESTNUTTJ，KUFFNERJ，NISHIWAKIK，etal．Planningbiped

navigationstrategiesincomplexenvironments［C］//ProcofIEEEIn-ternationalConferenceonHumanoidRobotics．2003：322-329．［4］BONABEAUE，DORIGOM，THERAULAZG．Swarmintelligence

fromnaturaltoartificialsystems［M］．Oxford：OxfordUniversityPress，1999．

［5］DORLGOM，CAROGD．Antcolonyoptimization：anewmeta-heu-ristic［C］//ProcofCongressonEvolutionaryComputation．1999：

4仿真实验

对B层的蚁群算法进行仿真实验，参数分别为：蚂蚁数目

m=30，最大迭代次数N=50，信息素和距离信息的权值α=1、β=2，常数C=100。实验结果如图11所示。

为了验证算法的效果，本文进行了大量的仿真实验，在

*［8］

40×40的栅格环境下，、GA［9］、RRT［10］和本文分别使用A

的ACO算法进行移动机器人的路径规划，最终比较结果如表1所示。从实验结果发现ACO算法比其他算法搜索到最优解的能力强。

表1

比较项平均最短路径/格平均运行时间/s

A*N/A＞300

四种算法比较

GA93．7149．1

RRT69．295．3

ACO63．4101．5

1470-1477．

［6］邓高峰，张雪萍，刘彦萍．一种障碍环境下机器人路径规划的蚁群

J］．控制理论与应用，2009，26（8）：879-883．粒子群算法［［7］赵云涛，王京，蔺风琴，等．一种用于连续域寻优的蚁群算法［J］．2008，15（3）：242-249．控制工程，［8］LESTERP．A*pathfindingforbeginners［EB/OL］．（2005-07-18）．［2009-08-20］．http：//www．gamedev．net：80/ref-erence/arti-cles/article2003．asp．

［9］唐国新，陈雄，袁杨．基于改进遗传算法的机器人路径规划［J］．

2007，28（18）：4446-4449．计算机工程与设计，［10］国海涛，朱庆保，徐守江．基于栅格法的机器人路径规划快速搜．南京师范大学学报：工程技术版，2007，7（2）：索随机树算法［J］

58-61．

［11］张彤，J］．计算机工肖南峰．仿人机器人实时路径规划方法研究［

2009，45（27）：4-6．程与应用，

将得到的轨迹放到C层的环境模型内，如图12所示，路径Ⅱ

为B层规划的轨迹，路径Ⅰ和Ⅲ为修改的路径。机器人的步态参k2=0．6，k3=0．4，k4=0．8，k5=0．7，k6=0．5，zc=数为：k1=0．2，

0．3。分别计算三条路径的长度和移动代价，如表2所示

。