Motion Planning Method for Obstacle Avoidance of 6-DOF Manipulator Based on Improved A* Algorithm

WANG Shou-kun (汪首坤), ZHU Lei (朱 磊)

Key Laboratory of Intelligent Control and Decision for Complex System, Beijing Institute of Technology, Beijing 100081, China

Motion Planning Method for Obstacle Avoidance of 6-DOF Manipulator Based on Improved A*Algorithm

WANG Shou-kun (汪首坤)*, ZHU Lei (朱 磊)

KeyLaboratoryofIntelligentControlandDecisionforComplexSystem,BeijingInstituteofTechnology,Beijing100081,China

The conventional A*algorithm may suffer from the infinite loop and a large number of search data in the process of motion planning for manipulator. To solve the problem, an improved A*algorithm is proposed in this paper by the means of selecting middle points and applying variable step segments searching during the searching process. In addition, a new method is proposed for collision detection in the workspace. In this paper, the MOTOMAN MH6 manipulator with 6-DOF is applied for motion plan. The algorithm is based on the basis of the simplification for the manipulator and obstacles by cylinder enveloping. Based on the analysis of collision detection, the free space can be achieved which makes it possible for the entire body to avoid collisions with obstacles. Compared with the Conventional A*, the improved algorithm deals with less searching points and performs more efficiently. The simulation developed in VC++ with OpenGL and the actual system experiments prove effectiveness and feasibility of this improved method.

manipulator;collisiondetection;A*algorithm;variablestepsegmentssearching;OpenGL

Introduction

Motion planning for obstacle avoidance is to design a safe path from the start point to the interest point. Taking a manipulator for an example, many factors should be considered in motion planning for obstacle avoidance, including the uncertainty and dynamic characters of work space, the real-time and optimal performance of algorithm, and the motion capacity of the manipulator. Many researchers dedicate to developing an efficient motion planner for manipulators. The classic path planning algorithms can be classified into three types: geometrical model searching, cell decomposition and potential fields[1]. In the geometrical model searching method, the main idea is a way of representing the free configuration space and connectivity, such as the roadmap method and rapidly-exploring random tree (RRT). It’s composed of a network of free paths. Hrabar proposed a method of probabilistic road map (PRM) in three-dimension space on the basis of roadmap[2]. The main idea of PRM is to generate path points randomly according to a certain probability distribution in the search space. Lavalle proposed the method of RRT based on probability sampling[3]. But the path is not optimal. In a method of cell decomposition proposed by Scheurer and Zimmermann[4], free space is divided into grids, and an available path is composed by the grids which are not occupied by the obstacles. Csiszaretal. applied the potential field method in industrial robot collision avoidance[5]. In the potential field algorithm, the manipulator motion is defined by the action of artificial potential field. But the method is easy to fall into local minimum point[6].

In addition to previous classic methods, other types of methods can be used for motion planning. Jailletetal.[7]proposed a method of free space based on Configuration-Space(C-space) Cost-maps. In the C-space, the motion path can be figured out by heuristic search algorithm. On the basis of C-space, other methods often applied analytic algorithm and heuristic algorithm in the traditional path search, such as Dijkstra’s algorithm and A*algorithm[8-10]. A*was developed to combine heuristic approaches like Best-First-Search and formal approaches like Dijkstra’s algorithm[11]. The algorithm is often used in two-dimension games to find the shortest path. A*algorithm is the most popular choice for path finding, because it’s fairly flexible and can be used in a wide range of contexts. A*is like other graph-searching algorithms in that it can potentially search a huge area of the map. Whereas A*algorithm is not always ideal and sometimes it will get stuck in an infinite loop[12]. At the same time large amount of invalid path searching can lead to the algorithm in low efficiency.

To deal with the problem of manipulator motion planning and improve the efficiency of conventional A*algorithm, an improved A*algorithm is proposed. The collision model of this manipulator is firstly introduced, including simplification for the manipulator and obstacles. Then collision detection is researched by simplifying the obstacles and evaluating the safety distance. Based on the analysis above, variable step segments search is proposed. In the end, the simulation and experiment results prove the algorithm admissible.

1 Collision Model for Manipulator and Obstacles

1.1 Model and simplification for manipulator

MOTOMAN MH6 manipulator with 6-DOF is researched in this paper. The direct kinematic resolution of MH6 manipulator can be described by the Denavit-Hartenberg(D-H) method[13]. In Fig.1, [x0, y0, z0] is the base coordinate system, while [xi, yi, zi](i=1, 2, …, 6) represents the coordinate system of each rotation joint in the manipulator. For example, [x6, y6, z6] is the coordinate system of the end-effector. Combining Fig.1 and the MH6 manipulator, the D-H parameters can be obtained

Fig.1 D-H representation of MH6 manipulator

as shown in Table 1.θirepresents the angle between the two adjacent jonits,αirepresents the angle between the two adjacent coordinates,aiis the length of link rod, anddiis the distance between the two adjacent link rods.

Table 1 D-H parameter

Jointθi(°)αi(°)ai/mmdi/mmRange/(°)1Θ1-90152(L1)0-1701702Θ2180614(L2)0-180653Θ3-90155(L3)0-831804Θ4900-640(L4)-1801805Θ5-9000-1351356Θ61800-95(L6)-360360

Considering the irregularity of the manipulator, it can be simplified to reduce the calculative quantities, and the simplification can be divided into four parts as shown in Fig.2.

Fig.2 Simplified manipulator into four parts

In Fig.2, the big arm of the manipulator is represented by part 1. The part of the forearm, the wrist joint, and the motor between the big arm and the forearm are represented by parts 2, 3, 4 respectively. Moreover, Biand Ei(i=1, 2, 3, 4) represent the start point and end point respectively on each part. These four parts are similar to cylinder, and they can be enveloped by cylinder to simplify the calculation. In this way, the active parts are enveloped by the cylinder completely, despite the posture and position of the manipulator. Then the positions of these special cylinders can be captured, and shapes are unchangeable.

Given the position and posture of the end point, the joint angles can be captured by means of inverse kinematic solution. Moreover, the coordinates of each cylinder at the end point along the axis direction can be calculated by means of forward kinematic and the D-H parameter in Table 1.

1.2 Simplification for obstacles

Three-dimensional obstacles have irregular shapes, and the difficulty of calculation can be reduced using regular object to envelop the obstacle. In the present study, the cylinder object is used to envelop the obstacles as shown in Fig.3.Pb(xb,yb,zb) andPe(xe,ye,ze) are the coordinates of the terminal points of the cylinder, andrbis the radius.

Fig.3 Geometry envelope for obstacle with cylinder

1.3 Collision detection in the workspace

Based on the simplification of obstacles and the manipulator, the collision detection can be transformed into the calculation of the position relationship between cylinders. Due to the simplification, the complexity of the model can be simplified[14]. Furthermore, the relationship between the manipulator and obstacles can be expressed by the minimum distance between the segments in the space. For instance, when the distance between cylinders along their axis directions is smaller than the sum of radii of cylinders, the collision between cylinders will happen. Taking segmentsABandCDas an illustration, the relationship betweenABandCDcan be expressed in two ways. WhenABandCDare coplanar, the position relationship is shown in Fig.4.

在技术出口管制方面，一是可以通过知识产权评议，定期评估和调整我国禁止出口和限制出口技术的范围与种类，禁止与国防安全相关的技术出口，限制国内产业发展的关键技术和核心技术出口；二是应当在技术出口活动中建立知识产权审查机制。对于限制出口类技术的出口，应当实行事前的知识产权审查，经审查无异议，中外双方才能签订技术出口合同。

Fig.4 Position relationship between coplanar segments AB and CD

WhenABandCDare not coplanar, the position relationship is shown in Fig.5.

Fig.5 The common vertical line of non-coplanar segments in three ways

Based on the analysis for the position relationship between segments which are simplification for cylinders, a universal method to compute the distance between segments in the space can be achieved. Consider the segment AB asA(x1,y1,z1),B(x2,y2,z2), and the segmentCDasC(x3,y3,z3),D(x4,y4,z4).Pis a point in lineAB, and the coordinate ofP(X,Y,Z) can be expressed as follows:

(1)

ConsiderQas a point in lineCD, and pointQ(U,V,W) can be achieved in the same way:

(2)

The distance betweenPandQcan be calculated by

(3)

Consider

f(s, t)=PQ2=(X-U)2+(Y-V)2+(Z-W)2.

(4)

(5)

The parameterssandtcan be acquired from the equation above. Ifsandtmeet the condition that 0≤s≤1, 0≤t≤1,PQis the shortest distance betweenABandCD. Moreover, compared the distances fromAtoCD,BtoCD,CtoAB, andDtoAB, the minimum is the shortest distance betweenABandCD. After the analysis for the minimum distance between the segments in the workspace, the free space and obstacle space can be distinguished. Moreover, by means of segments as simplification for cylinders, the calculation is simplified.

2 Searching Method Based on A*Algorithm with Variable Step Segments

2.1 Improved A*algorithm based on variable step segments

When the A*algorithm is used in the avoiding obstacles path searching, the following two problems must be faced and resolved[15]. Firstly, if the obstacle has great size while expansion step is set to be small in the manipulator’s movement from the initial point to the end point, the search data will increase greatly, moreover the progress of searching may even submerge into a dead loop. Secondly, if the expansion step is set to be large, although it may protect the process from falling into a dead loop, the searching path may become so unsmooth that the manipulator can not move steadily along such a path.

To search the path in three-dimension space withA*, the cubic expansion method is considered in this paper, which is one method with the same step expansion. Expansion step is the distance between current node and its sequential node in A*algorithm. By this method, 26 sequential nodes are generated by the current node, as shown in Fig.6, where the distances from each successor node to the current node are the same.

Fig.6 The process of attaining sequential nodes with three-dimensional cubic expansion method

In A*algorithm, the evaluation function is the most important. It will guide the searching towards the most promising direction to seek the shortest and optimal path if the evaluation function is chosen properly. In this paper, the evaluation function is the same as traditional A*. Differently, the variable step segments searching method is proposed. The evaluation function is:

f(n)=h(n)+g(n)，

(6)

where f(n) is the evaluation function for the current node n; g(n) represents the actual cost from the initial node to the current node in the state space which is the extent of the priority trends in the search; h(n) is the heuristic function which represents the evaluation about the optimized path from current nodento the end node and the heuristic information is also reflected in h(n). When h(n) keeps zero constantly, the algorithm becomes the Dijkstra algorithm; if h(n) is larger than or equal to the actual distance, it tends to be the method of breadth first search. Therefore, the core of the algorithm focuses on the heuristic function h(n). In this paper, h(n) is equal to Euclidean space distance between two points.

(7)

By using this heuristic function, the distance between the current point to end point equal to a straight line distance, which can ensure that the estimated value is less than or equal to the actual distance between the two points. As shown in Fig.7, the searching method with variable step segments is divided into two steps. Firstly, the extension step is set to be a large value, and the path is searched from the start point to end point by means of A*algorithm. Then one point is selected from the searching path as the middle point, which satisfies that the sum ofS1andS2is the minimum. In general,S1represents the distance from the middle point to the start point and the goal point.S2represents the distance between the middle point and center axis of obstacle. At last, the extension step is set to be a smaller value. To search the paths from the start point to the middle point, as well as the middle point to the end point, A*algorithm is applied again. Taking a cylinder obstacle for an example, the middle point can be achieved as follows.SandErepresent the start point and end point respectively.

Fig.7 The midpoint in the path for cylinder obstacle

The search method guarantees that the middle point is in the optimal path. In this method, the infection of the obstacle and the quantity of searching data can be decreased. The method is also beneficial to keep the final searching path smooth by means of dividing the whole search path into several sections. In such searching method with variable step segments, the method of cylinder enveloping detection will be applied to every subsequent point for collision detection using, and if a collision occurs, such node will be abandoned. Finally, the searching path with obstacle avoidance is constructed by the satisfied nodes. The main flow chart of the algorithm is shown in Fig.8, wheresrepresents the start point, andfrepresents the evaluation function.

Fig.8 Block diagram of path searching with variable step segments

3 Simulation Experiment

To verify the method of collision detection with cylinder enveloping and searching path with variable step segments in path planning, a simulation in a virtual platform and an actual experiment in the manipulator system are designed respectively. The three-dimension simulation software platform of 6-DOF of the manipulator is established by OpenGL and visual C++ development environment. Such platform is running on a desktop computer with 3.0 GHz CPU and 2 GB memory.

On this platform, the virtual manipulator is drawn with the same proportion to actual MOTOMAN MH6, in order to reflect the actual results. Moreover, all kinds of parameters, including the location and shape of obstacle, the trace of end point of manipulator and position of searching points, can be set and shown on this simulation platform.

The positions of the start point and end point are set to be (900, 700, 500) and (900, -700, 500) respectively, within the arm base coordinate, with millimeter unit, the same as in the following coordinates. The end posture is vertically downward. The position of the obstacle is set from (500, 0, -550) and (500, 0, 1000), and its radius is 50 mm.

A*algorithm and variable step segments searching method have been simulated for the obstacles avoidance detection, and both of them can direct the manipulator to avoid the obstacle successfully. Comparison of simulation data between these algorithms is shown in Table 2. Obviously, variable step segments searching method spent much less time than traditional A*algorithm in small expansion step.

Table 2 Comparison of simulated data between conventional A*and improved A*algorithm

ParametersandresultsConventionalA*algorithmImprovedA*algorithmbyvariablestepandsegmentsmethodExpansionstep/mm20304050First50then10Time/s62.62326.7618.5152.1622.423Pointnumber—20997688296452296580Quantityofpointduringcollisiondetection—2503412785848510475Lengthofpath/mm—1802.491830.781855.631815.63

According to Table 2, A*algorithm with expansion step 50 is a little faster than variable step segments in searching. For further analysis between variable step segment searching and searching with fixed expansion step 50, comparisons of the paths, the distributions of searching points and the movements of joints are respectively presented in Figs.9-11.

The comparisons above show that variable step segments searching method can reduce the total number of searching points effectively, and guide both each joint and the whole manipulator to move more smoothly and steadily. Figure 12 shows the process of motion plan for the manipulator when a cylinder obstacle in the workspace using OpenGL simulating.

(a) A* algorithm in expansion step 50

(b) Variable step segments searching methodFig.9 Comparison of the final searching paths from top-view

(a) Conventional A* algorithm in expansion step 50 (b) Variable step segments searching method

(a) Conventional A* algorithm in expansion step 50 (b) Variable step segments searching method

(a) (b) (c)

(d) (e) (f)

4 Actual System Experiment

For further verification, variable step segments searching method is experimented on actual manipulator system, and the schematic diagram of path planning by this method is shown in Fig.13, and actual pictures of some special points in a schematic diagram is shown in Fig.14. The white object wrapped with a red tape is designed for an obstacle in this experiment. The method of cylinder enveloping is used to simplify the obstacle, with start point (484, 0, 107), end point (484, 0, 788) and radius 43. The posture of the end of the manipulator is still vertically downward.

Fig.13 Schematic diagram of the path

(a) Point 1 (b) Point 2 (c) Point 3

(d) Point 4 (e) Point 5 (f) Point 6

In the experiment on the actual system, the manipulator moved from the start point to the interest point and avoided the obstacle successfully. The whole process is smooth and continuous. Furthermore, it also proves that the path planning algorithm is fast and efficient in real system, and can be applied in the control of manipulator.

5 Conclusions

In this paper, an improved A*method is proposed for obstacle avoidance path planning by the means of selecting middle points, applying variable step segments searching and computing the safety distance in collision detection. In addition, to simplify the model the method of cylinder envelopment is used for collision detection. The method is tested on three-dimension simulation platform established based on OpenGL and Visual C++. According to the simulation results, the total number of searching points can be reduced by the method of variable step segments searching. And the manipulator can avoid the obstacle quickly and move smoothly along the planning path. Moreover, the method is verified in an actual manipulator system. Based on the experiment results, it can be concluded that the method proposed in this paper has practical value for manipulators control.

[1] Kunz T, Reiser U, Stilman M. Real-Time Path Planning for a Robot Arm in Changing Environments[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Taipei, Taiwan, China, 2010: 5906-5911.

[2] Hrabar S E. Vision-Based 3D Navigation for an Autonomous Helicopter[D]. USA: University of Southern California, 2006.

[3] Lavalle S M. Planning Algorithms[M]. New York, USA: Cambridge University Press, 2006: 22-58.

[4] Scheurer C, Zimmermann U E. Path Planning Method for Palletizing Tasks Using Workspace Cell Decomposition[C]. IEEE International Conference on Robotics and Automation, Shanghai, China, 2011: 1-4.

[5] Csiszar A, Drust M, Dietz T. Dynamic and Interactive Path Planning and Collision Avoidance for an Industrial Robot Using Artificial Potential Field Based Method[C]. Proceedings of the 9th International Conference Mechatronics, Warsaw, Poland, 2011: 413-421.

[6] Khabit O. Real-Time Obstacle Avoidance for Manipul-ators and Mobile Robots [J].TheInternationalJournalofRoboticsResearch, 1986, 5(1): 90-98.

[7] Jaillet L, Cortés J, Siméon T. Sampling-Based Path Planning on Configuration-Space Costmaps [J].IEEETransactionsonRobotics, 2010, 26(4): 635-646.

[9] Li Q, Xie S J, Tong X H. A Self-adaptive Genetic Algorithm for the Shortest Path Planning of Vehicles and Its Comparison With Dijkstra and A*Algorithm [J].JournalofUniversityofScienceandTechnologyBeijing, 2006, 28(11): 1082-1085. (in Chinese)

[10] Dong Z N, Chen Z J, Zhou R. A Hybrid Approach of Virtual Force and A*Search Algorithm for UAV Path Re-planning[C]. IEEE Conference Industrial Electronics and Applications (ICIEA), Beijing, China, 2011: 1140-1145.

[11] Li D W, Han B M, Han Y. Conversely Improved A Star Route Search Algorithm [J].JournalofSystemSimulation, 2007, 19(22): 5175-5177. (in Chinese)

[12] Stentz A. Optimal and Efficient Path Planning for Partially-Known Environments[D]. Pittsburgh, USA: Carnegie Mellon University, 2006: 15213.

[13] Ding J L, Zhan Q. Inverse Kinematics and Dynamics Optimization Analysis of an Articulated Robot with ADAMS [J].JournalofDevelopment&InnovationofMachinery&ElectricalProducts, 2008, 21(1): 223-234.

[14] Sakahara H, Masutani Y, Miyazaki F. Real-Time Motion Planning in Unknown Environment: a Voronoi-based StRRT (Spa-tiotemporalRRT)[C]. The Society of Instrument and Control Engineers(SICE) Annual Conference, Tokyo, Japan, 2008: 2326-2331.

[15] Ting Y, Lei W I, Jar H C. A Path Planning Algorithm for Industrial Robots [J].Computers&IndustrialEngineering, 2002, 42: 299-308.

Foundation item: National Natural Science Foundation of China (No. 61105102)

TP242.2 Document code: A

1672-5220(2015)01-0079-07

Received date: 2013-08-27

* Correspondence should be addressed to WANG Shou-kun, E-mail: bitwsk@bit.edu.cn