一种移动机器人三维路径规划优化算法

禹建丽¹，程思雅¹，孙增圻²，Kroumov V³

(1. 中原工学院 电子信息学院，河南 郑州，4510007；

2. 清华大学 计算机系国家智能技术与系统重点实验室，北京，100084；

3. 日本冈山理科大学 工学部电子工学科，日本 冈山，700-0005)

摘  要：对移动机器人在三维工作环境中障碍物位置和形状已知条件下的全局路径规划问题进行研究。机器人的初始路径取为出发点到目标点的直线路径，引入人工神经网络结构和模拟退火温度定义路径能量函数；根据多面体形障碍物的形状特征设定各边界面不等的模拟退火初始温度，并且对路径点位于障碍物内、外的不同情况建立不同的运动方程；提出一种基于神经网络结构能量函数的路径规划算法及其优化算法，对所提路径规划算法进行仿真研究。研究结果表明，该算法是一种有效的移动机器人三维路径规划算法；算法计算简单，不存在组合爆炸问题；可避免路径规划的某些局部极小值问题；优化算法能够规划出移动机器人最短避障路径，并且可加快路径规划收敛速度。

关键词：全局路径规划；能量函数；神经网络；模拟退火

中图分类号：TP242.6         文献标识码：A         文章编号：1672-7207(2009)02-0471-07

An optimal algorithm of 3D path planning for mobile robots

YU Jian-li¹, CHENG Si-ya¹, SUN Zeng-qi², Kroumov V³

(1. Department of Electronics and Information, Zhongyuan University of Technology, Zhengzhou 450007, China;

2. Department of Computer Science and Technology, State Key Laboratory of Intelligent Technology and Systems,

Tsinghua University, Beijing 100084, China;

3. Department of Electronic Engineering, Faculty of Engineering, Okayama University of Science,

1-1 Ridai-cho, Okayama 700-0005, Japan)

Abstract: Global path planning was studied for a moving robot in a 3D environment filled with obstacles whose shapes and positions were known. An aggressive algorithm for path planning was presented. The obstacles were described by an energy function defined using neural networks. Different initial simulated anneal temperatures of each surface of objects can be set according to the shape of them. The different path generating equations were used, depending on the path points inside or outside the obstacles, which allows high speed of the calculations and fast convergence. The simulation results show that the computation is simple, some local minimum problems can be avoided, and the constructed path is optimal and piecewise linear.

Key word: global path planning; energy function; neural network; simulated annealing

移动机器人路径规划是指在有障碍物的工作环境中寻找一条从给定出发点到目标点的运动路径，使机器人在运动过程中能安全、无碰撞地绕过所有的障碍物，并且在保证安全的条件下寻找最短避障路径。路径规划分为环境已知的全局路径规划和环境未知(或部分未知)的局部路径规划。全局路径规划主要有路线图法(Roadmap)^[1-3]、单元分解法(Cell decomposition)^[4]、人工势场法(Potential field)^[5-6]、粒群算法^[7-8]等。这些研究主要解决移动机器人在二维工作环境中的路径规划问题。近年来，人们对三维空间中工作的移动机器人的研究不断深入，如研究了飞行机器人、水下机器人、登月探测机器人、蛇形机器人、爬壁机器人等。三维空间的路径规划问题是此类机器人研究的基本问题，也是反映机器人智能水平的重要标志之一^[9]。Faverjon等^[10-11]提出了用八叉树进行三维空间路径规划的方法，但此算法存在“组合爆炸”问题。人工势场法是建立目标、障碍物和机器人的势能场模型，机器人只需沿着势能场的梯度方向移动就可以找到避障路径，其优点是快速、高效，但存在局部极小值问题且不适于寻找最短避障路径^[12-13]。禹建丽等^[14]给出了一种路径规划算法，该算法计算简单，收敛速度快，能避免某些局部极小值且能规划出最短避障路径，但仅适用于二维路径规划。在此，本文作者给出一种基于神经网络结构能量函数的移动机器人三维路径规划算法。该算法不存在“组合爆炸”问题，可根据障碍物的形状设置各表面的不同初始模拟退火温度，解决某些局部极小值问题；此外，对位于障碍物内部和外部的路径点采用按不同运动方程移动的方法，可使规划出的避障路径最短，且可提高路径规划速度。

1  碰撞罚函数

1.1  问题描述

本文讨论三维全局路径规划。机器人的工作环境是三维空间，在机器人工作环境中存在静止且位置和形状已知的多面体形障碍物。将机器人模型化为球形机器人，障碍物尺寸根据机器人的半径及运行安全性要求进行相应“膨化”处理^[15]，使“膨化”后的障碍物边界为安全区域，且各障碍物之间不相交。机器人路径由出发点、目标点及出发点到目标点之间机器人经过的有限个路径点组成。路径规划的目的是在机器人三维工作环境中寻找从出发点到目标点的最短避障路径。

1.2  碰撞罚函数

1条路径的碰撞罚函数定义为各路径点的碰撞罚函数之和，而1个点的碰撞罚函数定义为它对各个障碍物基于神经网络结构定义的罚函数之和。图1所示为1个点到1个多面体形障碍物的罚函数的神经网络。神经网络为前向型三层网络，由输入层、隐层和输出层构成。输入层的3个神经元分别为给定路径点的坐标x，y和z，隐层的每个神经元相应于障碍物的1个边界面的不等式限制条件，输入层和隐层的连接权系数就等于不等式中x，y和z前面的系数，隐层每个神经元的阈值等于相应不等式中的常数项。隐层到输出层的连接权为1，输出层神经元的阈值取为不等式的个数减去0.5后的负数。

图1  1个点到1个障碍物的罚函数的神经网络

Fig.1  Obstacle description network

该连续网络的运算关系为，

各神经元的激发函数为“S”形函数，即

可以根据障碍物的形状，设定各边界面的初始温度，这样，对于一些不对称图形的障碍物可避免路径规划收敛到局部极小值。图2所示为点到1个长方体形障碍物的罚函数的神经网络，其中，w_xm，w_ym和w_zm及神经元的阈值如下：

；

；

；

；

。

图2  点到障碍物的罚函数的神经网络举例

Fig.2  An example of obstacle description network

整条路径的碰撞罚函数定义为：

由于各神经元的激发函数为“S”形函数，所以，越靠近多面体中心位置的点，其罚函数值越大。

2  三维路径规划算法

这里给出基于神经网络结构能量函数的三维路径规划算法(以下称B算法)，可规划出机器人避障的可行性路径。

2.1  路径能量函数

对路径点P(x_i, y_i, z_i) (i=1, 2, …, N)，定义

路径的能量函数定义为：

2.2  路径规划算法

初始路径一般取为由出发点到目标点的直线路径，初始路径点是该直线上均匀分布的点列。路径能量函数是所有路径点的函数，通过移动每个路径点，使其朝着能量减少的方向运动，最终便能获得总能量最小的路径。为此，求E对t的导数得：

。     (12)

其中：L₀=L_N=0。取

，

，

，    (13)

则有

＜0。    

其中：η为正常数。

由方程(13)解得路径点运动方程为：

；

；

。   (15)

其中：

；          (16)

；      (17)

i=2, 3, …, N-1。

因为能量函数E=E_l+E_c是由路径长度平方和路径罚函数2部分能量组成，路径点按照方程(15)运动，将朝着尽量远离障碍物且使路径较短的位置移动。由于模拟退火温度在开始时较高，路径点移动到远离障碍物的位置，随着温度减小，路径的长度逐渐减小，最后，收敛到避障的可行性路径。此算法称为三维路径规划B算法。

2.3  三维路径规划算法仿真实验

按照三维路径规划B算法，用Visual C++ 6.0编写计算机运行程序，其中路径点运动微分方程用4阶龙可库塔方法求解，进行路径规划仿真实验，仿真实验结果用Matlab软件绘图予以显示。

图3所示为用三维路径规划B算法进行路径规划的仿真实验结果。其中：机器人的工作环境中有1个障碍物，s是机器人运动的出发点，g为目标点。路径点数取为40，运算参数是：η=0.1，β₀=0.5，β_m=0.5 (m=1, 2, 3, 4, 5, 6)。

(a) 路径规划仿真实验结果; (b) 路径点由障碍物内逐渐向障碍物外移动的过程

图3  B算法仿真实验结果

Fig.3  Simulation results of B algorithm

图4所示为机器人的工作环境里有2个障碍物时，用三维路径规划B算法进行路径规划的实例。路径点数为40，运算参数是：β₀=1.8，β_m=1.8，η=0.1。

图4  存在2个障碍物时B算法仿真实验

Fig.4  Simulation of B algorithm with two obstacles

由图3和图4可以看出，用B算法进行路径规划时，障碍物内的路径点逐渐向障碍物外移动，移出障碍物外以后，还会继续向远离障碍物的方向移动。因此，规划出的路径是避障的尽可能短的可行路径，但不是最短避障路径，而且影响路径规划的收敛速度。

3  三维路径规划优化算法

3.1  三维路径规划优化算法

为了规划最短避障路径，加快路径规划收敛速度，路径规划时对于路径点P(x_i, y_i, z_i) (i=2, 3, …, N-1)，判断其是否在避障物内部。即若对于任意的m (m=1, 2, …, M)，都有

＞0，       (18)

则路径点P(x_i, y_i, z_i)在避障物内部，否则，P(x_i, y_i, z_i)不在避障物内部。

由于能量函数E=E_l+E_c，在路径点运动方程(15)中，一项是使路径点远离障碍物的改变量，另一项是使路径点缩短路径长度的改变量。对于障碍物外的路径点(或从障碍物内移出的路径点)，运动方程改为下列方程：

          (19)

其中：i=2, 3, …, N-1。

即障碍物外的路径点或从障碍物内移出的路径点，只需按照减少路径长度的方向移动，不再向远离障碍物的方向移动，从而使路径能快速收敛到避障的最短路径。

3.2  三维路径规划优化算法步骤

三维路径规划优化算法称之为B+算法，其算法步骤如下。

步骤1  输入出发点P(x₁, y₁, z₁)及目标点P(x_N, y_N, z_N)的坐标，对于t=0，初始路径一般取为出发点到目标点的直线上均匀分布的点列。当x₁≠x_N时，

     (20)

其中：i=2, 3, …, N-1。

步骤2  对于路径点P(x_i, y_i, z_i) (i=2, 3, …, N-1)，检测其是否在障碍物内。

a. 若P(x_i, y_i, z_i)在障碍物内，则按照运动方程(15)移动。

b. 若P(x_i, y_i, z_i)在障碍物之外，则按照运动方程(19)移动。

步骤3  重复执行步骤2，直到路径收敛为止。

3.3  三维路径规划优化算法仿真实验

图5所示是利用三维路径规划优化算法B+进行路径规划的实例。图5中的路径规划是在机器人运行环境与图3中对应的运行环境完全相同的条件下进行的，即障碍物位置和几何形状及路径的出发点和目标点位置完全相同。运算参数为：对方程(15)，η=0.1；对方程(19)，η=10；模拟退火初始温度均为1.9，规划出的避障路径是一条折线形的最短避障路径。图5(b)所示为应用B+算法进行路径规划中路径点移动的过程，可以看出，障碍物内的路径点逐渐向障碍物外移动，移出障碍物外后不再继续向远离障碍物的方向移动，从而规划出了最短的避障路径。

(a) 图3所示环境应用B+算法所得路径规划仿真实验结果；(b) 应用B+算法所得路径点由障碍物内逐渐向障碍物外的移动过程

图5  B+算法仿真实验结果

Fig.5  Simulation results of B+ algorithm

图6所示是图3和图5所示的仿真实验中两者路径规划收敛速度的比较结果，其中，横轴是路径规划算法计算循环次数，纵轴为路径长度。图6中，实线是图5所示B+算法仿真实验的收敛速度曲线，虚线是图3所示B算法的仿真实验的收敛速度曲线，前者的收敛速度快于后者，说明B+算法不仅可以规划出最短的避障路径，还可以有效地加快路径规划收敛速度。

1—图5中B+算法仿真实验收敛速度；2—图3中B算法仿真实验收敛速度

图6  路径规划B算法和B+算法收敛速度比较

Fig.6  Comparison of convergence velocities for algorithm B and B+

图7所示是机器人在1个台形障碍物上运动的路径规划B+算法仿真结果。路径点数为30，η为0.5(对方程(15))和0.1(对方程(19))。由于台形物体呈不对称的立体状，各表面的模拟退火初始温度β_m(m=1, 2, …, 6)分别为0.9, 0.9, 0.9, 0.9, 0.5和0.5，其中台形的上表面和下表面的初始温度为0.5，从而避免了由于物体不对称而可能造成的路径规划局部极小值问题。仿真结果表明，规划出的路径是最短的折线形可行路径。

图7  存在台形障碍物时B+算法仿真实验结果

Fig.7  Simulation results of B+ algorithm with platform obstacle

图8所示是机器人在有3个障碍物工作环境中利用三维路径规划B+优化算法进行路径规划的仿真实验结果，障碍物的几何形状与位置已知。路径点数为40，η取0.1(对方程(15))和50(对方程(19))。仿真结果表明，B+优化算法在多个障碍物的复杂环境中也能规划出最短的避障路径，而且计算速度与图7对应条件下的计算速度基本相同，不存在“组合爆炸”问题。

图8  存在3个障碍物时B+算法仿真实验结果

Fig.8  Simulation results of B+ algorithm with three obstacles

4  结  论

a. 对于在三维工作环境中障碍物位置和形状已知条件下的移动机器人的初始路径取为出发点到目标点的直线路径，引入人工神经网络结构和模拟退火温度定义路径能量函数，利用最速下降法建立路径点运动方程，使路径点朝着能量函数减小的方向运动，提出一种基于神经网络结构能量函数的路径规划B算法。B算法不存在“组合爆炸”问题；此外，可根据多面体形障碍物的形状特征设定各边界面不等的模拟退火初始温度，避免局部极小值问题。

b. 对位于障碍物内部和障碍物外部路径点建立不同的运动方程，改进B算法，得到基于神经网络结构能量函数的三维路径规划B+优化算法。B+优化算法可使规划出的避障路径达到最短避障路径，而且可以加快路径规划收敛速度。

c. 路径规划B算法是移动机器人在三维工作环境中一种有效的避障可行路径规划算法，而B+算法则是一种避障且路径最短的最优路径规划算法。

参考文献：

[1] Lozano-Pérez T. Spatial planning: A configuration space approach[J]. IEEE Transactions on Computers, 1983, 32(2): 108-120.

[2] Barraquand J, Latombe J C. Robot motion planning: A distributed representation approach[J]. The International Journal of Robotics Research, 1991, 10(6): 628-649.

[3] Lengyel J, Reichert M, Donald B, et al. Real2time robot motion planning using rasterizing computer graphics hardware[C]// International Conference on Computer Graphics and Interactive Techniques. Texas, 1990: 327-335.

[4] Latombe J C. Robot motion planning[M]. Norwell: Kluwer Academic Publishers, 1991.

[5] Khatib O. Real-time obstacle avoidance for manipulators and mobile robots[J]. The International Journal of Robotics Research, 1986, 5(1): 90-98.

[6] Barraquand J, Langlois B, Latombe J. Numerical potential field techniques for robot path planning[J]. IEEE Transactions on Systems, Man and Cybernetics, 1992, 22(2): 224-241.

[7] 李枚毅, 蔡自兴. 基于粒群行为与克隆的移动机器人进化路径规划[J]. 中南大学学报: 自然科学版, 2005, 36(5): 739-744.
LI Mei-yi, CAI Zi-xing. Evolutionary path planning based on swarm behavior and clone for mobile robot[J]. Journal of Central South University: Science and Technology, 2005, 36(5): 739-744.

[8] YU Jin-xia, CAI Zi-xing, ZOU Xiao-bing, et al. Research on the calibration method for the heading errors of mobile robot based on evolutionary neural network prediction[C]//WANG Jun, LIAO Xiao-feng, YI Zhang. Advances in Neural Networks-ISNN 2005: Second International Symposium on Neural Networks. Chongqing: Springer, 2005: 265-270.

[9] 蔡自兴. 智能控制及移动机器人研究进展[J]. 中南大学学报: 自然科学版, 2005, 36(5): 721-726.
CAI Zi-xing. Advance in research of intelligent control and mobile robots[J]. Journal of Central South University: Science and Technology, 2005, 36(5): 721-726.

[10] Faverjon B, Tournassound P. A local based approach for path planning of manipulators with a high number of degrees of freedom[C]//Proceedings of the IEEE International Conference on Robotics and Automation. Raleigh, 1987: 1152-1159.

[11] Faverjon B. Obstacle avoidance using an octree in the configuration space of a manipulator[C]//Proceedings of the IEEE International Conference on Robotics and Automation. Atlanta, 1984: 504-512.

[12] 况 菲, 王耀南. 基于混合人工势场-遗传算法的移动机器人路径规划仿真研究[J]. 系统仿真学报, 2006, 18(3): 774-777.
KUANG Fei, WANG Yao-nan. Robot path planning based on hybrid artificial potential field/genetic algorithm[J]. Journal of System Simulation, 2006, 18(3): 774-777.

[13] 肖本贤, 余 雷, 李善寿, 等. 逃逸人工势场法局部极小值策略的研究[J]. 系统仿真学报, 2007, 19(19): 4495-4503.
XIAO Ben-xian, YU Lei, LI Shan-shou, et al. Research of escaping local minima strategy for artificial potential field[J]. Journal of System Simulation, 2007, 19(19): 4495-4503.

[14] 禹建丽, Kroumov V, 孙增圻, 等. 一种快速神经网络路径规划算法[J]. 机器人, 2001, 23(3): 201-205.
YU Yian-li, Kroumov V, SUN Zeng-qi, et al. Fast algorithm for path planning based on neural network[J]. Robot, 2001, 23(3): 201-205.

[15] Lozano-Pérez T, Wesley M A. An algorithm for planning collision-free paths among polyhedral obstacles[J]. Comm of the ICM, 1979, 22(10): 560-570.

收稿日期：2008-04-20；修回日期：2008-06-28

基金项目：国家自然科学基金资助项目(10572125)；河南省自然科学基金资助项目(0611052500)

通信作者：禹建丽(1960-)，女，河南洛阳人，博士，教授，从事智能控制和智能机器人等研究；电话：0371-67698847；E-mail: yjl837@163.com