New hierarchical method for path planning of large-scale robots

M. H. Korayem, H. Vatanjou and V. Azimirad

Robotic Lab., Mechanical Engng. School, IUST, Tehran, Iran
hkorayem@iust.ac.ir

Abstract - In this paper, a new coordination method based on non-linear hierarchical control for large-scale robots is presented. The large-scale system is considered as combination of subsystems so that each subsystem has interaction with others. The procedure is to use interaction prediction principle with optimal control for each subsystem. At the first level, applying optimal control principle to each subsystem with guessed interaction variables results in Two Point Boundary Value Problem (TPBVP). By solving TPBVP the new interaction parameters are generated. At the second level, the resulted interactions are exchanged between subsystems and the interaction variables are improved within interaction prediction principle. Difference between guessed and achieved interaction in each step is used in general cost function to coordinate subsystems. Hence continuing the algorithm causes to converging the interactions to each other. The new method results in less time by parallel processing for each subsystem, and has less sensitivity for different initial guess in comparison with centralized use of optimal control for large-scale robots because of using small sized sub-systems which is a step toward real-time planning of mobile manipulators, also the proposed method has the ability to solve problems with inseparable cost functions which is an important benefit for robots path planning in presence of obstacles and specified path for end-effector. The method is simulated and verified by previous work in this area. The simulation results show effectiveness of proposed method for large-scale robots. The approaches validity is checked via simulations and experiments with a 2-link nonholonomic mobile manipulator named Scout.

Keywords - Hierarchical Control; Large-scale Systems; Optimal Control; Mobile Robot; Nonholonomic.

NOMENCLATURE

M(q) : Inertial matrix of manipulator.

:Centrifugal and coriolis matrix of manipulator.

τ, U: control inputs to joints or wheels (N.m, N).

q: generalized coordinates of manipulator.

X_i: state variable of i'th sub-system.

: head angle of nonholonomic mobile base (rad).

θ_r, θ_l: angular displacement of right and left wheels respectively (rad).

θ₁, θ ₂: angular displacement of first and second joints of manipulator respectively (rad).

Zⁱ: interaction vector of i'th sub-system.

J: cost function.

k: weight of convergence.

l_i : inseparable part of cost function of each sub-system.

h_i: separable part of sub-system's cost function.

h: overall cost function.

H: hamiltonian of general system.

R,Q: weighting matrixes of states and control variables.

η: states related to velocities.

λ_i: adjoin vector of i^th sub-system.

x*: desired state vector.

p_i: lagrange multiplayer of i'th sub-system.

I. INTRODUCTION

Today's robotic applications go toward using high DOFs robots with complex construction. They are desired because of much larger workspace and high amount of manipulability but as the order of system increases, the path planning and control become more difficult, because of high amount of dynamic equations, constraints and dynamic interactions. High DOF robots as Large-scale systems are which contain a number of interdependent components with special functions, share resources, and are governed by a set of interrelated constraints and goals (Mesarovic et al., 1970).

Although during the past decades, a great deal of attention has been given to the problem of motion planning of robots, but a few authors have studied the complex robotic systems (Sadati and Babazadeh, 2006). Different methods are used for optimal path planning of robotic manipulators can be categorized into two main groups; direct methods and indirect methods (Chettibi et al., 2004). Generally, direct methods result in approximate solution, and they are not suitable for systems with a large number of DOFs because they are quite inefficient due to the large number of parameters involved and may cause to numerical explosion (Korayem et al., 2009). On the other hand, indirect methods are based on Pontryagin maximum principle (PMP) which solves the optimal control problem exactly (Korayem et al., 2009). The optimality conditions are expressed as a set of differential equations and lead to a two point nonlinear boundary value problem (TPBVP). This boundary values problem is solved by numerical techniques. But solving nonlinear boundary value problems are numerically ill conditioned problems (Kirk, 1970). The main difficulty of these problems comes from sensitivity to initial guess and long time of computations. Also, as the order of the system increases, these difficulties become more critical.

Mohri et al. have used indirect method for trajectory planning of mobile manipulators to achieve minimum effort trajectory in point-to-point motion (Mohri et al., 2001). To solve the problem, they have used a method based on the order of priority. But their method is very time consuming because the whole dynamic of nonholonomic mobile manipulator, as a large scale system, is used at the same time in one computing system. Korayem et al. introduced an approach for computing maximum load carrying capacity of holonomic 3-link mobile manipulators (Korayem et al., 2009). They used the augmented Jacobin concept for solving redundancy constraints. But the process takes long time and so same problems were existed for their work.

Some authors used evolutionary algorithms for path planning of mobile manipulators for example; a method for collision avoidance of a mobile manipulator introduced by Ko et al. (2006). This algorithm based on modification of planned trajectory at every sampling time using elastic force and potential field force the method gives smooth and adaptive trajectory in environments with stationary, also moving obstacles but the derived path is not optimal and also dynamic constraints not considered in the approach.

A fexible stochastic scheme for point-to-point trajectory planning of nonholonomic mobile manipulators is proposed by Haddad et al. (2009). The method takes into account the stability constraints as well as dynamic and kinematic constraints. Also it considers obstacles in environment, but resulted path is not an optimal one.

Fuzzy adaptive Kalman filter hired as an approach for real time planning of an autonomous mobile manipulator by Najjaran and Goldenberg (2007). The approach contains map building and path planning of mobile manipulators. The efficiency of the method has been verified by experiments. Korayem et al. (2010) introduced an algorithm for finding maximum load carrying capacity of mobile manipulator in presence of obstacle. They considered stability criteria in the path planning process.

One of the most common ways for optimization and control of high DOF robots is hierarchical configuration, in which a given unit-level coordinates or controls the units on the level below and in turn is controlled by the units above it (Wismer, 1971). Sadati and Babazadeh (2006) proposed a two level gradient based approach that decompose the large-scale system. They also used gradient updating method for generating new coordination variables which uses linearization formula and for n states, there may be as many as 2n gradient processes attempting to converge which is difficult for large-scale systems (Smith and Sage, 1973). But in their proposed method, the cost function must be separable and there is no guarantee for complete satisfaction of final positions of states (Sadati and Babazadeh, 2006).

A type of combined pseudo model coordination/prediction principle first time proposed by Singh et al. (1976). They used the method for optimal controlling of a synchronous machine. But there is not any work before for of path planning of large-scale robotic systems by optimal control. In this method interactions considered as a functions of states and beside, the potential field term related to difference of predicted and achieved states in each steps added to performance index of overall system but according to dynamic of robot system interaction are functions of states and input controls. So this method is not applicable to hierarchical optimal control of robot manipulators directly. Also in this method the cost function considered as separable one, which in the case of path planning in presence of obstacles this assumption is not held any more.

In this paper an algorithm for optimal path planning of nonholonomic mobile manipulators will be presented. The algorithm uses the idea as presented in Singh et al. (1976), but a number of basic changes are done in order to be applied in path planning of mobile manipulator. First the interaction terms considered as a functions of control inputs as well as state variables. Second the cost function is considered in more general form of non-separable and separable cost function which is suitable for the case of path planning in present of obstacles and also path planning for specified path of end-effector because it contains some non separable combination of states.

The rest of paper is configured as below. In section II, hierarchical dynamic modeling of nonholonomic mobile manipulator is considered, a decomposition scheme and also interactions between sub-systems is presented. In section III, hierarchical optimal control problem of robotic systems is stated. Section IV belongs to new hierarchical algorithm for finding optimal path. In section V simulation results for a nonholonomic mobile manipulator with serial link is presented. Section VI belongs to experimental verification of algorithm and finally conclusions represents in section VII.

II. DYNAMIC MODELING

Assuming a robotic system as a large-scale system comes from high amount of subsystems containing links, joints, and also the complicated nonlinear interaction between subsystems. Successful design of large complex systems invariably involves decomposition of the system into a number of smaller subsystems each with its own goals and constraints (Smith and Sage, 1973). In this section, dynamics mobile manipulators are formulated considering subsystem decomposition in order to become useful for hierarchical optimization. The most versatile mode of operation for mobile manipulator is simultaneous use of manipulator and mobile platform; in another word it must be taken into account the coordinated dynamic model of platform and manipulator.

A. Nonholonomic mobile manipulator

A mobile manipulator composed of a serial manipulator and a mobile platform has a fixed-base manipulator due to the mobility provided by the mobile platform. But it is difficult to control, since it has high redundancy, nonholonomic constraints of mobile platform, and dynamic interactions between the manipulator and the mobile platform (Yamamoto and Yun, 1996). Recent tendency is to make mobile robots much more independent in path planning level from origin, means to do the path planning level online, fast and independently. Complicated and large-scale dynamic equations are two main hinders toward this goal. In this work a differentially driven mobile manipulator is considered. A manipulator mounted on a non-holonomic wheeled mobile platform which has some driving and castor wheels, the driving wheels are independently driven by motors. The schematic of a redundant 2 degrees of freedom manipulator with attached coordinates to its links is shown in Fig. 1.

Fig. 1: Nonholonomic mobile manipulator

There are three constraints in motion for nonholonomic mobile platform, the first one is that platform must move in direction of axis of symmetry, and other two constraints are the rolling constraint not allowing them to slip. Three Eqs. (1 to 3) show the constraints according to non-holonomy.

(1)

(2)

(3)

The system is decomposed into two subsystem; nonholonomic platform and holonomic manipulator.

In the case of mobile base (first subsystem), dynamic equations are in the form (Yamamoto and Yun, 1996);

(4)

where M_v1 and V_v1 are the mass inertia and the velocity dependant of the platform, respectively, M_v2 and V_v2 represents the inertial term and centrifugal and coriolis terms due to the presence of the manipulator, τ_v is the input torque to the vehicle, E_v is a constant matrix, λ denotes the Lagrange multipliers corresponding to the kinematic constraints, and R _v represents the inertia matrix which reflects dynamical effect of the arm motion on the vehicle (Yamamoto and Yun, 1996).

Also dynamic equation for the mounted arm is:

(5)

where q_r denotes n-dimensional Lagrangian coordinates of the manipulator, M_r is the inertia matrix, V_r1 represents Coriolis and centrifugal terms, V_r2 denotes centrifugal and coriolis terms caused by the angular motion of the platform, τ_r is the input torque/force for the manipulator, and R_v is the inertial matrix which represents the effects of the vehicle dynamical on the manipulator (Yamamoto and Yun, 1996). Next, the dynamic equations of subsystems could be written in state space form; for mobile base this equations becomes (Yamamoto and Yun, 1996):

(6)

where

(7)

(8)

(9)

where

(10)

In these equations:

(11)

Also;

is a (5*1) vector of Lagrange coordinates of nonholonomic base also;

is of order (2*1), also M_v1is of order (5*5) and S is of order (2*5), then X¹ in Eq. (6) is order (7*1) and extracting orders of Z₁, N₁, N₂ the order of right hand side of Eq. (6) is of order (7*1).

And for manipulator arm state space equation is:

(12)

where

(13)

(14)

(15)

here

is of order (2*1), M_r is of order (2*2), V_r1 and V_r2 are of order (2*1) so X² in Eq. (12) is of order (4*1) and order compatibility in this equation holds.

III. PROBLEM STATEMENT

Although there may be an infinity of different ways of transforming a given optimization problem into a multilevel problem, they are all essentially combination of two different approaches which may be termed as model-coordination method and the goal coordination method (Jamshidi, 1983). The model-coordination method converts the integrated optimization problem into two level problems by fixing the interaction variables. But the goal-coordination method cuts the interaction variables in order to create a first level problem which can easily be decentralized or decomposed into independent sub-problems. The second level of this method attempts to force the first-level independent sub-problems to arrive at a solution for which the interaction balance principle holds (Wismer, 1971). Considering the robot dynamic in the following form:

(16)

It is possible to define this system as a nonlinear interconnected system:

(17)

where z_i is the vector of interactions between sub-systems and could be calculated from the dynamic equation:

(18)

However in the case of robotic systems if, subsystems compose of consecutive joints and links interaction terms is not only function of states but also function of control variables. Then, Eq. (18) becomes:

(19)

In this equation x_i is the n_i dimensional state vector of the ith subsystem. u_i is the m_i dimensional control vector and z_i is the n_i dimensional vector of interactions. Each subsystem has its independent control input but some of them have no independent control input (Yamamoto and Yun, 1996). The cost function is:

(20)

As its clear in the Eq. (20), the cost function is written assuming that the cost function of the original system is the sum of the costs of the subsystems and the cost of each subsystem is only function of its stats and controls, this limitation of the Singh's algorithm (Singh et al., 1976) in the case of robots path planning makes problems when the cost function is not separable; for example in the case of path planning in present of obstacles and path planning in specified end-effecter path. So it is better to write the cost function in this form:

(21)

IV. HIERARCHICAL OPTIMAL CONTROL APPROACH

In order to solve problem, first optimal control for subsystems is defined as Eq. (22):

(22)

Subject to

(23)

In which, l_i is function of non-separable terms in object function, which is a function of states and controls of all subsystems. This situation comes for example in the case of path planning in presence of obstacles. In Eq. (22) J_i is defined, so:

(24)

As stated in Ko et al. (2006) and Najjarian and Goldenberg (2007) for making the sub-system problems independent from each other, the cost function of each subsystem most be only a function of it's states and controls. In Singh et al. (1976) the cost function is considered as a separable one for avoiding this problem, but in the case of robots path planning, avoiding obstacles and specifying path for end-effector cause to existing non linear coupled terms of states. For solving this conflict, states and controls in the inseparable part of cost function (l_i), are replaced by predicted values for states and controls (x^* and u^*). So the algorithm gets the ability to solve more general problems with inseparable cost function. To solve the problem, the new cost function is defined as Eq. (25):

(25)

In which, last term is added to make the states to go toward desired ones (Singh et al., 1976). k is a positive constant and I_i is a n_i dimensional identity matrix. x^* is a n_i dimensional "desired" state vector. Substituting x=x^* in motion equations, results in desired control vectors. Now by substituting x^* and u^* into Eq. (19) and also resulting interconnection into Eq. (17), it can be separated as N independent subsystems, then the problem becomes in form of Eq. (26):

Subject to

(26)

It should be noted that when x=x^* then this new problems solution and that of the original problem are identical. The problem then is to iteratively find x_i^* (i=1,2,…,N) such that it approaches x_i. In order to do this, write the Hamiltonian as:

(27)

where is the n_i dimensional adjoin vector and p_i is n_i dimensional Lagrange multiplier which has been added to satisfy the constraint x=x^* at the optimum.

Now for optimality conditions to hold, the differentiation of H with respect to its variables must be vanish so, the two level algorithms are as follow:

Step 1: Guess x^*k and p^k at level 2 and send to level 1 and set j = 1.

Step2: Substitute guessed values for x^*k into dynamic equations to get u^*k at level 2 and send to level 1.

Step 3: For given x^*k , u^*k , p solve N independent minimization problems given by the minimization of H_i. This leads to a two point boundary value problem when the following operations are performed:

(28)

Step 4: Produce a new "prediction" of x^*, p by "reinvesting" the last value (Singh et al., 1976) into the conditions:

(29)

(for updating coordination parameters)

These conditions give x=x^* and another equation for p, then:

(30)

Step 5: Substitute resulted x into dynamic motion equations to get optimal controls

Step 6: If the R.H.S. of Eq (21) is approximately the same as the L.H.S., stop, otherwise go to step 2.

As told before, previous works had some weaknesses which are not usable for motion planning of robots (Najjarian and Goldenberg, 2007), but below modifications make the purposed algorithm suitable for motion planning of large-scale robots:

1- Considering the interaction vector as functions of states and controls

2- Considering the cost function as an inseparable function of states and controls (which is more general form) to make possibility for path planning in presence of obstacles and path planning for specified end-effector motions.

V. SIMULATION RESULTS

A. Nonholonomic Mobile Manipulator with two link serial manipulator

A non-holonomic mobile manipulator is considered as a case study, the system is decomposed into two subsystems; a nonholonomic base and a 2-link manipulator. Using the algorithm presented in previous section and centralized method the optimal path for robot is achieved, and the results are compared. Table 1 shows the physical parameters of robot.

Table 1: WMM parameters and inertia properties

Suppose that initially the non-holonomic mobile base with platform and load is at a point with p0 (x = 0.75m, y = 0.2m, φ = 0 rad, θ1 = 1.554 rad, θ2 = 1.998 rad), so the coordinate of End-effector is calculated (xe = 0.3m, ye = 0.5m). The end-effector must reach to final point Pf (xe = 2.45m, ye = 0.5m) while carrying the maximum load, as well as the mobile base reaches the final point (x = 1.6 m, y = 0.5m, φ = 0 rad) at tf=1.9s.

The penalty matrices are chosen to be: Q= diag(1), R= diag(1) and k=diag(1000). Figure 2 shows the results.

Figure 2. Comparison of hierarchical method with centralized method in path plannig of mobile manipulator with a serial link

As it is seen, there is an acceptable coincidence between hierarchical and centralized algorithm results. This high level coincidence could be achieved by setting parameter k (Eq. 24) to relatively high amount.

B. Two arm nonholonomic mobile manipulator in point-to-point motion

The most critical problem of path planning for large scale robots is dependence of results to initial guess. The problem to be solvable, the initial guesses of states must be near to the answer, but this problem can be solved by using hierarchical method. In order to study the effect of various initial guesses, another simulation is done and the robustness of purposed method in various initial guess (which is necessary for solving boundary value problem) is studied. Hence by changing the initial guesses for simulation discussed in section 3-1 for two cases of hierarchical and centralized, the robustness of hierarchical algorithm investigated in comparison with centralized method. As shown in Fig. 3. Using a poor set of initial guesses, the results from hierarchical algorithm not changed, but in the case of centralized algorithm the results become fluctuant and unstable.

Figure 3 Comparison of centralized and hierarchical methods robustness

The reason for this phenomenon is the size of dynamic equations and their coupling interactions. In the case of hierarchical algorithm the dynamic equations are in lower dimension in comparison with centralized algorithm and also the equations are less coupled with together. This is one of benefits of purposed hierarchical algorithm. In another word, purposed hierarchical method is the most effective approach in the case of complicated systems like mobile manipulators, where less sensitivity to initial guesses and also other parameters (like weighting multipliers) is desired.

Other main benefit of hierarchical algorithm is less time of computations compared to centralized methods in the case of parallel calculation (Smith and Sage, 1973); hence it is a powerful capability for systems in which, on-line path planning is required.

VI. EXPERIMENTAL STUDY

In order to verify simulation results with experiment, a mobile manipulator named Scout is used. This robot is available in IUST Robotic Research Lab. The mobile

base is a differential drive and two 3DOF manipulators are mounted. Actuators of base and manipulator are all DC motors and are connected to computer using Wi-Fi system. This robot is shown in Fig. 4. Due to software limitations, two motors of manipulator are used. Table 2 shows the physical specifications of Scout.

Figure 4. Schematic view of modeled Scout robot

Table 2: Physical parameters of Scout robot

The original system decomposes to two subsystems containing nonholonomic base as first subsystem and right manipulator as second subsystem. The right manipulator carries load from initial point of (-0.18 m, -0.08 m, 0.05 m) to second point of (1.70 m, 1.455 m, 0.3 m) at 3.2 s. using kinematic equations of motion in first and second points can be calculate as below:

p0 (x = 0m, y = 0m, φ = 0 rad, θ1 = -Π rad, θ2 = -30×Π/180 rad)

pf (x = 1.5 m, y = 1.5 m, φ = 0.84 rad, θ1 = -Π/4 rad, θ2 = Π/4 rad)

Also cost function's parameters are considered in this form:

Simulation results for this case study are shown in Fig. 5.

Figure 5. Simulation results for Scout robot

Scout is planned to track the optimal path calculated in simulation level. Plotted data from sensors of joints and wheels and their comparison with simulation results shows that they are in good coincidence. Experimental results from sensors of joints and wheels and their comparison with simulation are shown in Fig. 6.

Figure 6. Comparison of simulation results with experiments

VII. CONCLUSION

A new coordination method for solving path planning problem of large-scale robots based on non-linear hierarchical control presented. The hierarchical modeling and subsystem decomposition for two group of robotic systems a) serial link fix robots and b) nonholonomic mobile manipulator discussed. Applying optimal control principle with guessed interaction variables to each subsystem resulted in Two Point Boundary Value Problem (TPBVP) and by solving TPBVP the new interaction parameters are generated from new states and controls of sub-systems. The resulted interactions are exchanged between subsystems and the interaction variables are improved within interaction prediction principle. Difference between guessed and achieved interaction in each step is used as a constraint and potential field in general cost function to coordinate subsystems. Simulation results of implying new method on a nonholonomic mobile manipulator with a serial link are compared with centralized method and there was an acceptable adjustment between them. But the drawbacks of numerically solving of (TPBVP) are reduced due to decomposing it into sum smaller equations. In another simulation the effect of changing the initial guesses on solving the hierarchical and centralized method studied. The simulation results showed that the purposed method is a) much more robust in changing initial guesses and b) faster than centralized algorithm when parallel calculation is used.

REFERENCES
1. Chettibi, T., H.E. Lehtihet, M. Haddad and S. Hanchi, "Minimum cost trajectory planning for industrial robots," European Journal of Mechanics, 23, 703-715 (2004).         [ Links ]
2. Haddad, M., S Hanchi and H.E. Lehtihet, "Point-to-point trajectory planning of wheeled mobile manipulators with stability constraint, extension of the random-profile approach," European Journal of Mechanics, 28, 477-493 (2009).         [ Links ]
3. Jamshidi, M, Large-Scale Systems; Modeling and Control, North Holland, (1983).         [ Links ]
4. Kirk, D.E., Optimal control theory, An Introduction, Prentice-Hall Inc. (1970).         [ Links ]
5. Ko, N.Y., R.G. Simmons and D.J. Seo, "Trajectory modification using elastic force for collision avoidance of a mobile manipulator," Lecture Notes in Computer Science,4099, 190-199 (2006).         [ Links ]
6. Korayem, M.H., A. Nikoobin and V. Azimirad, "Maximum load carrying capacity of mobile manipulators: optimal control approach," Robotica, 27, 147-159 (2009).         [ Links ]
7. Korayem, M.H., V. Azimirad, A. Nikoobin and Z. Boroujeni, "Maximum load-carrying capacity of autonomous mobile manipulator in an environment with obstacle considering tip over stability," The International Journal of Advanced Manufacturing Technology, 46, 811-829 (2010).         [ Links ]
8. Mesarovic, M.D., D. Macko and Y. Takahara, Theory of hierarchical multilevel systems, Academic Press, New York (1970).         [ Links ]
9. Mohri, A., S. Furuno, M. Iwamura and M. Yamamoto, "Sub-optimal trajectory planning of mobile manipulator," Proceedings of the IEEE International Conference on Robotics and Automation, Seoul, Korea, 2, 1271-1276 (2001).         [ Links ]
10. Najjarian, H. and A. Goldenberg, "Real-time motion planning of an autonomous mobile manipulator using a fuzzy adaptive Kalman filter," Robotics and Autonomous Systems, 55, 96-106 (2007).         [ Links ]
11. Sadati, N. and A. Babazadeh, "Optimal control of robot manipulators with a new two-level gradient-based approach," Journal of Electrical Engineering, 88, 383-393 (2006).         [ Links ]
12. Singh, M.G., M. Hassan and J.L. Calvet, "Hierarchical optimization for non-linear systems with application to a synchronous machine," International Journal of Systems Science, 7, 1041-1051 (1976).         [ Links ]
13. Smith, N.J. and A.P. Sage, "An introduction to hierarchical systems theory," Journal of Computer and Electrical Engineering, 1, 55-71 (1973).         [ Links ]
14. Wismer, D.A., Optimization methods for large-scale systems, with applications, McGraw-Hill (1971).         [ Links ]
15. Yamamoto, Y. and X. Yun, "Effect of the dynamic interaction on coordinated control of mobile manipulators," IEEE Transaction on Robotics and Automation, 12, 816 - 824 (1996).         [ Links ]

Received: November 10, 2009
Accepted: September 10, 2010
Recommended by Subject Editor: Jorge Solsona