Motion Planning and Coordination for Robot Systems ... - IEEE Xplore

248

IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS—PART B: CYBERNETICS, VOL. 41, NO. 1, FEBRUARY 2011

Motion Planning and Coordination for Robot Systems Based on Representation Space Jianbo Su, Senior Member, IEEE, and Wenlong Xie

Abstract—This paper proposes a general motion planning and coordination strategy for robot systems. The representation space (RS) of a robot system is constructed to describe the distributions of system attributes. The reachable area in the RS, denoting the attribute set that the system can be of, indicates the system’s ability to accomplish tasks. Moreover, it also describes the influences of the internal and external constraints on the system’s capability. Task realization is transformed to finding a trajectory in the RS for the system attributes to transit along under constraints. Meanwhile, the realizable conditions of a prescribed task by the robot system of specific configurations are discussed. If the task is realizable, the optimal strategy for task execution could further be figured out. Otherwise, it could be transformed to be realizable via task reassignment or system reconfigurations so that a connected path could be found for the transition of the system attributes from the starting point to the goal in the RS. The proposed scheme contributes to designing, planning, and coordination of the robotic tasks. Experiments on path planning of a robot manipulator and formation movement of a multirobot system, as well as coordination of a mobile manipulator system, are conducted to show the validity and generalization of the proposed method. Index Terms—Attribute transition, representation space (RS), robot system, task planning, task realizability.

I. I NTRODUCTION

P

ATH PLANNING for a robotic manipulator is aimed at finding an obstacle-free path from a starting position to a specified goal position under certain constraints. It is generally completed in configuration space (CS) or task space (TS). CS planning [1]–[3], also called joint-space planning, is done at joint level and widely employed due to its real-time advantage. TS planning [4]–[6] is done at Cartesian space and enables planning to be visualized. However, the joint trajectories are essential for task implementation, which surely leads to heavy computational burden in exploiting inverse kinematics of the robot. A multirobot system (MRS) is able to perform complex tasks that might not be done by a single robot alone. Coordinative robots can accomplish a variety of tasks, such as hunting, object transportation, and environment exploration. So far, there are three strategies for multirobot coordination [7]: the Manuscript received June 8, 2009; revised December 8, 2009 and May 10, 2010; accepted May 12, 2010. Date of publication July 8, 2010; date of current version January 14, 2011. This work was supported in part by the National Natural Science Foundation of China under Grant 60935001 and in part by the Program for New Century Excellent Talents in University of MOE of China under Grant NCET-06-0398. This paper was recommended by Associate Editor J. Wang. J. Su is with the Department of Automation, Shanghai Jiao Tong University, Shanghai 200240, China (e-mail: [email protected]). W. Xie is with the Shanghai Academy of Spaceflight Technology, Shanghai 201108, China (e-mail: [email protected]). Digital Object Identifier 10.1109/TSMCB.2010.2051025

leader–follower scheme, the behavioral method, and the virtualstructure (VS) technique. In the leader–follower scheme [8], [9], one robot is designated as the leader, while others act as followers to keep definite relations with respect to the leader. This strategy has been adopted for a variety of applications due to its simplicity in control. However, it is generally an open-loop strategy, which may inherently lead to failure for some kind of task, such as formation navigation of an MRS in a dynamic environment [25]. In the behavioral method [10]–[12], several desired primitive behaviors are prescribed in advance for each robot, probably including obstacle avoidance, collision avoidance, formation maintenance, and moving to goal, etc. Each robot calculates a weighted average of these primitive behaviors according to their relative importance to the task so that its desired motion to fulfill the prescribed task is acquired. The control strategy using this method is simple. However, the behavioral method does not have a deterministic definition for the group behavior, which makes it difficult to analyze mathematically and guarantee the stability of the group. The VS technique [13] treats each robot as one point embedded in an imaginary structure. In this approach, a virtual force is exerted to the VS, whose position and orientation vary, consequently. It is easy to define the formation task, and it can implement trajectory tracking with high precision. Unfortunately, the restriction of moving in VS results in limited application of this approach. A mobile manipulator is composed of a mobile platform and a manipulator. The former extends the system’s workspace, while the latter offers much flexible operation functionality. The control of a mobile manipulator mainly includes two aspects: motion planning and coordinative control [26]. Motion planning is usually done in CS [3], [14], [15] or TS [16]–[18], where two essential issues, i.e., storage space and computation time, should also be considered [15]. Generally, there are two types of control strategies proposed so far in the literature [19]. One is the decentralized control of the mobile platform and the manipulator, and the other is the unified control of both. These motion-planning and coordination methods have already had a wide range of applications. However, they are only applicable to the tasks that are implicitly realizable. What kind of tasks could be achieved and how to handle those unachievable tasks, by a specific robot system, have seldom been addressed so far. For a given task and a robot system of definite configuration, two important issues should first be considered: 1) whether the given task can be realized by the system; designing task-oriented motion planning and coordination strategies

1083-4419/$26.00 © 2010 IEEE

SU AND XIE: MOTION PLANNING AND COORDINATION FOR ROBOT SYSTEMS BASED ON RS

is meaningful only if the task is realizable, and 2) if a prescribed task cannot be accomplished by the robot system of definite configuration, how to make it accomplishable via task adjustment and/or system reconfiguration; namely, it is essential to find a strategy to transform an unrealizable task to be realizable with the available system capability. The transformation condition should first be figured out before the concrete schemes to realize that the task could be designed. In this paper, we propose a general motion planning and coordination strategy for robot systems. The representation space (RS) of a robot system is constructed in response to the system capability and the task requirements. Reachable scope of the RS, showing the system ability to accomplish tasks, is figured out by the system’s internal and external constraints. Task execution is considered a transition of the system attributes from the initial representation to the goal in its RS. Hence, motion planning and coordination are rendered as a trajectorysearching problem in RS with which the realizable conditions for a given task are explored. The optimal strategy for task fulfillment is also investigated according to the given performance index. If the task is unrealizable, it could be transformed to be realizable by adjusting the system’s configurations and/or task constraints. Also, the transformation conditions could further be investigated. The remainder of this paper is organized as follows. Section II describes the general theory of motion planning and coordination for robot systems after construction of the system’s RS. Three different tasks, path planning for a robot manipulator, formation movement of an MRS, and coordination task for a mobile manipulator, are exemplified to show the effectiveness and generalization of the proposed method in Sections III–V, respectively. Conclusions are given in Section VI. II. M OTION P LANNING IN RS When performing a given task, the positions, velocities, and accelerations of the robot system (one or several robots) vary continuously. If we construct a space, called RS, with representation variables related to the attributes of the robot system and the prescribed task, the attributes of the system at any time could be denoted as a vector in the RS. Then, the task execution could be considered a transition of the system attributes in the RS. A. RS Model (RSM) Constructing an RS with all representation variables will lead to a space of high dimensionality. Therefore, we will only include those variables that are directly related to the task requirements and system characteristics, ignoring those of less interest. For example, direction, size, position, velocity, and acceleration of each robot are all available attributes of the system. If the task concerns little on other attributes except position, then the representation of the robot’s position is just sufficient to construct the system’s RS. Hence, the concept “RS” is used here to indicate all attributes of a specific robot system to realize an assigned task. It is certainly not the same

249

as the CS and/or the TS. In CS and TS, the variables are normally “real variables” (spatial or joint positions, velocities, and accelerations of the robot) having certain physical senses. While in RS, besides these variables, any attributes of the system as well as the tasks that relate to task realization are possible dimensionalities of the space. For instance, the link length of a manipulator, seldom appearing in CS, is probably one of the dimensions in RS, if it is critical to task realization. The formation center of an MRS determined by positions of all component robots might also be a dimension considered in its RS as well. There are many ways to formalize the RS, so long as the system attributes and task definitions can be revealed by the representation variables chosen. For a given task, suppose the number of variables chosen is m. An m-dimensional RS ζ = (ζ1 , . . . , ζm ) ∈ Rm can be established, with each variable having its range ζi min ≤ ζi ≤ ζi max ,

(i = 1, . . . , m).

(1)

The initial attribute (representation) ζ 0 of the robot system and the goal ζ g when the robot system has accomplished the given task are, respectively, specified as 0 ζ 0 = ζ10 , . . . , ζm g ζ g = (ζ1g , . . . , ζm ).

(2)

B. Reachable RS (RRS) The RS generalizes the TS and CS of the robot system. The pose and position of a robot concerned in the TS, as well as the joint variable concerned in the CS, are all possible attributes of the robot system related to a specific task realization. Thus, all of them might be different dimensions forming the RS of the robot. A robot system may be subject to three kinds of constraints: physical constraints, obstacle constraints, and task constraints. Mechanical restriction and motion capability of the robots are physical constraints, such as moving ranges of joints or the size and shape of a mobile robot, etc. Obstacles in the robot workspace will also limit the activity of the robot. In addition, the task to be achieved may impose constraints on the robots. For instance, the gripper or joints of a manipulator is required to move along a predefined path, or an MRS should move in a prerequisite formation. Physical constraints are inherent in the robot system and are also called internal constraints, while obstacle constraints and task constraints are external constraints. In the RS R of a robot system, the internal and external constraints result in unreachable subspaces of the system attributes, called RS obstacles (RSO) and denoted as A A = Ap + Ao + At = Api + Aoj + Atk (3) i

j

k

where Ap , Ao , and At are the unreachable areas in the RS resulting from the physical constraints, the obstacle constraints, and the task constraints, respectively. Let A denote the reachable areas; we have R = A ∪ A. Then, the procedure of the

250


rithm as an example to illustrate how to find out the trajectory in the RS. The cost function of A∗ algorithm is ˆ fˆ(n) = gˆ(n) + h(n)

(5)

where gˆ(n) is the minimal cost from the initial node to node n, ˆ h(n) denotes the cost estimation from node n to the goal node, and fˆ(n) is the lowest cost estimation from the initial node to the goal node passing through node n. According to [20], the A∗ algorithm is capable of finding the optimal path conditionally. Let Fig. 1. Example of the RS with m = 3.

0

n

gˆ(n) = min C(ζ , ζ ) = min

n−1

C(ζ i , ζ i+1 )

(6)

i=0

robot system to accomplish a given task corresponds to a transition trajectory of the system attributes from ζ 0 to ζ g within the reachable area A. The reachable as well as the unreachable areas of the RS can be evaluated as follows. 1) Determine the range of each attribute variable ζi, ζi min ≤ ζi ≤ ζi max , i = 1, 2, . . . , m. 2) Specify a sampling interval Ii (i = 1, 2, . . . , m) for each variable and fix the values of the first m − 1 variables in order from ζ1 to ζm−1 . Then, ζm is sampled by Im within its range [ζm min , ζm max ]. It is reachable if the condition ζ ∈ A is satisfied; otherwise, it is unreachable. 3) Similar to that of Step 2, fix the values of the first m − 2 variables and determine whether the system attribute set is reachable or not at every sampling point, with the last two variables varying their ranges. By repeating these steps, all the reachable and unreachable areas in the RS can be determined eventually. Fig. 1 shows an example of the RSM with m = 3. The RS is normally of high dimension, leading to large storage space. Therefore, only the reachable (or unreachable) areas could be considered, depending on the constraint conditions. In order to reduce the computational complexity and apply it in dynamic environment, we combine offline RS construction with online dynamic modification. By this means, the onlinecomputation burden will sharply be reduced. C. Trajectory Searching in the RS The given task is realizable if the system satisfies the following conditions: 0 (ζ ∈ A) ∩ (ζ g ∈ A) (4) ∃ P ⊂ A, P = {ζ 0 , . . . , ζ g } which means that the initial attribute representation ζ 0 of the system before the task is conducted and the final attribute ζ g when the task is realized are both in the reachable area A. Meanwhile, there should exist a connectable path from ζ 0 to ζ g within the reachable area of the RS. Many kinds of searching methods can be employed to search for the attribute transition trajectory, such as depth first, breadth first, A∗ , D∗ , best first, sampling-based motion planning [24], and bidirectional search, etc. Here, we take the A∗ [20] algo-

i+1 i C(ζ i , ζ i+1 ) = f α1 ζ1i+1 − ζ1i , . . . , αm ζm (7) − ζm m 2 ˆ h(n) =

βj ζjn − ζjg . (8) j=1

The minimal-cost path can be derived whenever the cost function is given. Normally, we take α1 = · · · = αm = 1 and β1 = · · · = βm = 1. The RS may be composed of different kinds of variables. Suppose that the attribute representation ζ n of the system changes to ζ n+1 along the direction in accordance with ζi after a time period, and the corresponding path cost is Ci (ζ n , ζ n+1 ). The sampling interval should satisfy C1 (ζ n , ζ n+1 ) = C2 (ζ n , ζ n+1 ) = · · · = Cm (ζ n , ζ n+1 ). (9) By doing so, the actual cost from ζ n to ζ n+1 will be proportional to the Euclidean distance of the two points in the RS. Then, gˆ(n) can be substituted by the shortest distances from the start node to the current one gˆ(n) = min

n−1 m

ζji − ζji+1

2

.

(10)

i=0 j=1

It is noted that the searching direction in reachable space is subject to the physical constraints and the task constraints. Inherent attributes of the system confine the robots to certain specific motion styles. Moreover, the given task may have special requirements for the robot system. Path planning should accordingly be consistent with them. Motion planning and coordination for a robot system to perform tasks could be analyzed by the proposed RS method. After constructing the RS of a robot system, the reachable area of the system attributes can be figured out. The given task can explicitly be defined in the RS, which can further be investigated if realizable or not. Realization strategy for the given task is thus explored in the RS, as well as the optimal solution, if there exists. D. Transformation of Unrealizable Task The given task would be unrealizable if at least one condition in (4) is not satisfied, i.e., either the start and/or the goal


251

attribute representation of the system is not in the reachable area of the RS, or there is no connecting path for the attribute transition in the reachable area of the RS. To make the task realizable, the system capabilities and/or the task constraints should be modified so that the RS of the system is reformed. Suppose a new dimension of the RS, denoted by ζm+1 , is added to the RS by modification of either the system capabilities or the task constraints. The RS of the system is then reconstructed ζ = (ζ1 , . . . , ζm , ζm+1 ) ∈ Rm+1 .

(11)

According to the variation range [ζm+1 min , ζm+1 max ] for ζm+1 and its sampling interval Im+1 , reachable areas of the system attribute can be calculated with ζ 0 and ζ g relocated in the new RS. Suppose that the system satisfies (4) now. In order to get the minimum changing range of ζm+1 , take αm+1 = βm+1 αi , βi (i = 1, . . . , m), and the path acquired is Pˆ , Pˆ ⊂ A. Define Pˆi (i = 1, . . . , m + 1) as the variation set of the ith attribute in Pˆ Pˆi = ζi0 , . . . , ζig .

(12)

Qm+1 min and Qm+1 max are, respectively, the minimum and maximum values of ζm+1 in Pˆ Qm+1 min = min{Pˆm+1 } (13) Qm+1 max = max{Pˆm+1 }. Thus, the transformation condition for an unrealizable task to be a realizable one is (ζm+1 min ≤ Qm+1 min ) ∩ (ζm+1 max ≥ Qm+1 max ).

(14)

Following [1], a projection to ζm+1 is conducted in order to evaluate the improvement to the previous RS after adjustment of the system configurations and/or task constraints. Let πi be a point projection denoting representation subspace when ζi is equal to a specified value πi (ζ) = (ζ1 , . . . , ζi−1 , ζi+1 , . . . , ζm+1 ).

(15)

i

[ai ,bi ] (ζ) is the projection for point sets on interval ai ≤ ζi ≤ bi i

(ζ) = {πi (ζ) | ζ ∈ S

ζi ∈ [ai , bi ]} .

Fig. 2. RSM of a two-link manipulator in a 2-D environment.

while the latter deals with the joints. Both of them are subsets of the representations of the system. A. RSM Note that when designing a task, the positions and poses of the end effector are straightforward for task description. The RSM can thus be defined as ζ = (x, y, z, θ, α, β)

(17)

where x, y, and z denote the position components of the end effector in the world coordinate system, and θ, α, and β are the pose components with respect to the axes. Thus, a 6-D RS is built up. However, this method does not limit the representations of the system attributes to be positions and/or poses. If, for example, the length of a link is a key attribute involved in a task realization, it is then a component of the system attribute, which is also a representation or dimension in the RS of the system. Fig. 2 is a two-link revolute manipulator in a 2-D environment. Its corresponding RS is 3-D ζ = (x, y, θ)

(18)

where (x, y) is the coordinate of the reference point at the end of link 2 and θ is the angle of link 2 with respect to the horizontal axis.

(16)

[ai ,bi ]

After projection, m+1 [Qm+1 min ,Qm+1 max ] (ζ), we can obtain the representation subspace on interval [Qm+1 min , Qm+1 max ] for ζm+1 , whereas the previous RS could be regarded as a subspace after projection m+1 [Q,Q] (ζ) when ζm+1 ≡ Q. By comparing the differences of the reachable areas between the two subspaces, we can evaluate the improvement for system capability. III. T RAJECTORY P LANNING OF ROBOT M ANIPULATOR This section shows how the trajectory planning of a robot manipulator can be investigated with the help of the proposed RS. It has generally been conducted in TS or CS, either of which can be considered an instance of the RS. The former considers the positions and orientations of the manipulator,

B. RRS Without Obstacle The manipulator shown in Fig. 2 has limited capability due to the inherent physical constraints. With the method proposed, we analyze its ability to achieve tasks. Reachable areas of the RS can be calculated according to the three steps mentioned earlier. Since the end-effector’s pose has sole corresponding configuration in joint space, the reachable space can also be constructed with respect to the joints so that the computational complexity could be reduced. The position and pose of the end effector of the robot manipulator could be described by joints q1 and q2 x = Bx + L1 cos q1 + L2 cos(q1 + q2 ) y = By + L1 sin q1 + L2 sin(q1 + q2 ) θ = q1 + q 2

(19)

252


Fig. 3. RRS of the manipulator.

where (Bx , By ) is the base position of the manipulator and L1 and L2 are, respectively, the lengths of the two links. The movement range of each joint is 0 ≤ qi < 2π (i = 1, 2), and the sampling intervals are I1 = 1◦ and I2 = 2◦ . Suppose (Bx , By ) = (1, 1) and L1 = L2 = 0.5, the RRS of the manipulator is shown in Fig. 3(a), which is the surface of a bending cylinder composed of a sequence of circles. It is easy to see from Fig. 3 that the reachable areas are smaller than the unreachable ones considering the constraints among links. In fact, only the reachable areas in RS are practically calculated for the sake of reductions of computation cost and storage space. We show the whole RS here to give a clear illustration of the system attributes. Fig. 3(b) shows the reachable areas of the representations (attributes) of the robot system when θ = 0, π/2, π, and 3π/2, respectively, which are four circles. From (19), we can have x = C1 + L1 cos q1 y = C2 + L1 sin q1

(20)

where C1 and C2 are constants if θ = q1 + q2 is assigned a definite value qc C1 = Bx + L2 cos(q1 + q2 ) = Bx + L2 cos qc C2 = By + L2 sin(q1 + q2 ) = By + L2 sin qc .

(21)

In this case, the reachable space is a circle centered at (C1 , C2 ) with a radius L1 , as shown in Fig. 3(b). The whole RRS of the manipulator is, hence, a surface composed of a sequence of planar circles with identical radius but different centers, as shown in Fig. 3(a). Thus, we know that the manipulator could only achieve the tasks when the corresponding attributes of the system are on the surface of the bending cylinder. Continuous-path tasks, such as arc welding and flame cutting, have special constraints on the intermediate attitudes of the robot system. In Fig. 2, the manipulator is required to move from S to G along the contact surface with the second link perpendicular to it. The initial attribute representation is ζ S = (1.36, 1.86, π/2), and the goal is ζ G = (0.8, 1.86, π/2) with the task constraint At : θ = π/2. Realizability of this task

could first be analyzed with the help of the RS of the robot. The trajectory of this task in the RS is a line segment from ζ S to ζ G in the plane θ = π/2, as shown in Fig. 3. It is easy to see that ζ S is on the reachable surface, while ζ G is inside the cylinder and NOT on the surface. Consequently, the task cannot be achieved by the manipulator. With the proposed RS of a robot system, realizability of a task can first be investigated before the task is assigned to the robot system. This is more essential compared with the CS or TS, although all of them can suggest concrete strategies to accomplish the task. C. RRS With Obstacles If there are obstacles in the workspace, those obstacle locations cannot be reached by the manipulator, which means that some poses of the manipulator related to those locations would be unachievable. Mapping to the RS of the system, some previously reachable regions in the RS are now unreachable due to the obstacles. In Fig. 4(a), there are two rectangular obstacles (O1 and O2 ) and a circular one (O3 ). The RRS is shown in Fig. 4(b). Three obstacles correspond to three unreachable regions. The robot arm L1 collides with the obstacle O1 when q1 is near 0. Hence, the corresponding representation of the system is unreachable despite the variations of q2 , which corresponds to the blank areas Ao1 in Fig. 4(b) (without O1 , the reachable space will be a closed surface.). Similarly, the arm L2 collides with the obstacle O2 when q1 and q2 are near π/2 and 0, respectively. Then, the regions nearby ζ = (1, 2, π/2) are unreachable corresponding to the blank areas Ao2 (a hole in the surface). Furthermore, the arm L2 collides with the obstacle O3 when q1 and q2 are near 3π/4 and 0, respectively. Accordingly, the regions nearby ζ = (0.29, 1.71, 3π/4), denoted as Ao3 , are unreachable in Fig. 4(b). Hence, the RRS of the manipulator in this case is reduced due to these obstacles, and its ability to achieve tasks is diminished. D. RS Trajectory Searching Suppose the manipulator is to move from S to g1 , as shown in Fig. 4(a); ζ S = (1.68, 1.68, 1.05), and ζ g1 = (0.66,


Fig. 4.

RRS of the manipulator with obstacles.

Fig. 5.

CS path planning.

253

Fig. 6. RRS when x = 0.5.

1.92, 2.09). We would like to plan the shortest path for the end effector to achieve the task. Let

C(ζ i−1 , ζ i ) = (xi − xi−1 )2 + (y i − y i−1 )2 (22)

ˆ h(n) = (xn − xg )2 + (y n − y g )2 + (θn − θg )2 . (23) Without obstacles, the straight line from ζ S to ζ g1 in the RRS obviously represents the optimal way for the robot to achieve the given task. Due to the presence of O2 , the optimal path of the straight line will go through Ao2 . In this case, the system attribute has to transit away from Ao2 along the solid line shown in Fig. 4(b). In order to avoid collision with the obstacles, a safe distance Lsaf = 0.05 is employed in path searching. Fig. 5 is the CS path planning for the same task, where the minimal joint displacement is considered. The corresponding attribute transition trajectory of the robot system is denoted by the dashed line in Fig. 4(b). The same planning result can be derived in RS if we take the minimum joint movement as the planning index, which is shown in Fig. 5. Therefore, we can see that the proposed RS of the robot system is the extension of its CS. The manipulator might have different ways to execute the same task. In Fig. 6(a), the manipulator is required to move from A to B along the straight line x = 0.5. The cross section

of its reachable space is shown in Fig. 6(b), which is a closed curve. The initial representation of the system attribute is ζ A = (0.5, 1.87, 2π/3), and the goal is ζ B = (0.5, 0.13, 4π/3). Typically, there are two paths for the system attributes to transfer from ζ A to ζ B . One is clockwise labeled as “1,” the other is counterclockwise labeled as “2.” Since the two joints are both revolute joints, let cwi (i = 1, 2) denotes clockwise turning of the joint i, and acwi denotes the counterclockwise one. Then, the two paths in the RS correspond to two different ways to execute the task in workspace 1. A ⇒ (cw1 , acw2 ) ⇒ (acw1 , cw2 ) ⇒ B or 2. A ⇒ (acw1 , cw2 ) ⇒ (cw1 , acw2 ) ⇒ B.

(24)

In case 1, the two links turn initially in the manner (cw1 , acw2 ), namely, link 1 turns clockwise and link 2 counterclockwise, and then (acw1 , cw2 ). Meanwhile, the end-effector’s attitude increases first and then falls down. While in case 2, the two links turn first in the manner (acw1 , cw2 ) and then (cw1 , acw2 ). The end-effector’s attitude falls down at first and then increases. This example shows that we can understand all possible strategies for a task to be realized by a specific robot system with the proposed RS. The real strategy can then be figured out under some further considerations.

254


Fig. 7. Reachable space after the robot-system structure is reconfigured.

E. Solution to Unrealizable Task Suppose the manipulator is required to move to a goal location g2 , ζ g2 = (0.38, 1.94, 2.44). We can take advantage of the RS of the system to analyze the realizability of the task. Here, the advantage of RS against CS is thoroughly demonstrated. The CS-based method has the implicit assumption that the given task is realizable. The goal state can even not be denoted by joint variables so that the task is unable to be represented in CS. Note that the attribute of the robot system in the goal location is far away from the reachable areas in its RS. This implies that the task is unrealizable due to the limitation of the manipulator’s ability. In this case, we choose, for instance, to attach an additional prismatic joint L3 to the end of L2 , shown in Fig. 7(a). The system’s RS is correspondingly reconstructed with a new set of system attributes ζ = (x, y, θ, q3 ).

(25)

Let q3 ∈ [0, 1], then ζ 0 = (1.68, 1.68, 1.05, 0), ζ g2 = (0.38, 1.94, 2.44, 0.2). The goal state now falls into the reachable area in the RS, which means that the task now becomes realizable. Let C(ζ i−1 , ζ i ) 2 = (xi − xi−1 )2 + (y i − y i−1 )2 + 10 q3i − q3i−1 .

(26)

By increasing the cost of q3 (α4 = β4 = 10) in the A∗ algorithm, it will lead to the minimum changing range for the new attribute q3 of the system. Qmax = 0.2 can be determined from (13). Therefore, the transformation condition for task realization is q3 max ≥ 0.2. That is to say that the upper bound of joint q3 should be larger than 0.2. 4Fig. 7(b) is the representation subspace after projection [0,0.2] (ζ). It is noted that the reachable space is extended outward, which means that the manipulator’s ability to accomplish tasks is improved. ζ g2 is inside the reachable area so that there exists a connectable path from ζ S to ζ g2 . The given task is thus realizable.

Fig. 8.

Hardware of the calligraphic robot.

F. Trajectory Tracking The task of trajectory tracking by a robot manipulator planned with the proposed RS is verified by a calligraphic robot developed in our laboratory [21]. The robot is an Adept One robot arm that is an assembly manipulator with four degreesof-freedom (DOFs). However, only two revolute joints are utilized in the task. In order to show clearly the trajectory of the end effector, a writing brush is mounted at the end of the manipulator (Fig. 8). The manipulator tracks the trajectory in Fig. 4. Since the control of the manipulator is done at joint space, inverse kinematics computation is necessary to obtain the joint trajectories, which can be computed from (19). Fig. 9 shows the tracking result in which the manipulator does not collide with the obstacles and can smoothly arrive at the goal position to accomplish the given task optimally. IV. M ULTIROBOT F ORMATION M OVEMENT The multirobot formation task can also be analyzed with the help of the proposed RS method. In this task, the key issue is how to coordinate the robots’ movement in formation. Static obstacles are considered here to extensively show the performance of the proposed RS method. It is straightforward to extend to dynamic-obstacle situation by including an additional dimension of time in the RS. The static-obstacle case is just a


255

where pi = (pix , piy ) is the position coordinate of Ri and can be computed by the attribute set variables ⎧ p1x ⎪ ⎪ ⎨ p1y ⎪ p ⎪ ⎩ 2x p2y

Fig. 9.

= x + L2 cos θ = y + L2 sin θ = x + L2 cos(θ + π) = y + L2 sin(θ + π)

(29)

where l is the distance from Ri to C and l = L/2. Fig. 11(a) shows the RRS for L = 2m in the workspace of 8 m in length and 7 m in width. The gray areas are the RSO that the attribute of the robot system cannot enter into. Without obstacle, the whole RS would be reachable, as shown in Fig. 11(b). The reachable and unreachable areas are both figured out here to give a clear demonstration of the system attributes. Only the reachable areas in RS are practically necessary to be calculated.

Trajectory-tracking result.

C. RS Trajectory Searching

Fig. 10. Multirobot formation task and its RSM.

sampling at any time moment of a dynamic-obstacle case. The attribute transition trajectory in a dynamic-obstacle case is an integration of a series of static-obstacle trajectories when the static obstacle is in the locations where the dynamic obstacle moves to at the sampling moments. A. RSM Since the formation demands the relative positions of the robots between each other, the RS of the whole robot system can be constructed with its position attributes. Given two robots R1 and R2 shown in Fig. 10, the distance L between them is a constant value. Define the attribute set ζ of the system as being the centroid coordinate (x, y) of the formation and the angle θ between the horizontal line and the line passing through R1 and the formation center C (see Fig. 10) ζ = (x, y, θ).

(27)

Note that this definition does not limit the robot number to be two. If the formation has more robots, the same set of system variables x, y, and θ could be adopted with different definitions, as shown in Section IV-F. B. RRS Without loss of generality, suppose that the workspace is a rectangle with two square obstacles in it. Each robot cannot enter into the obstacle areas O, namely pi ∈ / O,

(i = 1, 2)

(28)

Consider the shortest path, namely, the smallest sum of distances traveled by all robots. I1 = I2 = 0.05 m, I3 = 3◦ , ζ 0 = (2.5, 1.5, 0), and ζ g = (6, 5.5, π/4). The optimal trajectory for attribute transitions derived is shown by the real line in Fig. 11(a) (a safe distance Lsaf = 0.2 m is taken into account in path searching). Fig. 11(b) shows the transition trajectory of the system attribute when there is no obstacle in the workspace. It is obviously a straight line from ζ 0 to ζ g . Fig. 12 shows another way for RS construction and its RRS. It takes R1 as the reference robot whose position coordinate is (x1 , y1 ). The RS can be built up by (x1 , y1 ) and θ21 [the angle between the horizontal line and the line passing through R2 and R1 , shown in Fig. 12(a)]. Compared with Fig. 11(a), the reachable space, the initial and the goal attribute, and the transition trajectory are all different. However, the true motions of the robot system to conduct the task induced by the optimal solutions from both RRSs are the same.

D. Solution to Unrealizable Task Suppose the configuration of the system is changed. The length for each obstacle is 3 m, and the distance between them is 1.2 m. There are three robots with all relative distances L = 1.5 m. The starting and the goal attributes of the system are ζ 0 = (1.5, 1.5, 0) and ζ g = (5.5, 5.5, π/2), respectively. Fig. 13 shows the RRS, where there is no connectable path from ζ 0 to ζ g in reachable areas so that the task is unrealizable. Neither the leader–follower scheme nor the behavioral method or the VS technique mentioned in Section I is able to show whether the given task is realizable or not. Modify the task constraint by making the distance L between robots changeable during the movement and reconstruct the system’s RS by adding new variable L, ζ = (x, y, θ, L). Let L ∈ [0.5, 1.5] and I4 = 0.05 m. Then ζ 0 = (1.5, 1.5, 0, 1.5) and ζ g = (5.5, 5.5, π/2, 1.5). The connectable path could be found out now in the new RS. Take α4 = β4 = 10; we get Qmin = 1.15 m from (13). Thus, the transformation condition

256


Fig. 11. RRS of the formation task.

Fig. 12. Different RSM and its RRS.

Fig. 14.

Connectable path after modifying the task constraint.

F. Extension to Multiple Robot Formation

Fig. 13. No connectable path in the RRS.

for the task realizationis Lmin ≤ 1.15 m. Fig. 14 is the subspace after projection 4[1.15,1.5] (ζ), where the reachable areas of the RS are enlarged and the connectable path exists. Hence, the task is now realizable, whose realization process is denoted by a transition trajectory of the system attributes, shown in Fig. 14 by the solid line from ζ 0 to ζ g .

E. Attribute Trajectory Tracking Based on the optimal transition trajectory shown in Fig. 11(a), the desired trajectory for each robot can be computed from (29), which is divided into a sequence of subgoals [23]. All robots move simultaneously toward their subgoals in turn until, eventually, the task is accomplished. The tracking process is shown in Fig. 15.

The RS method shown in (17) can be extended to the task with multiple robots in formation. Fig. 16 shows another task executed by three mobile robots. The workspace is similar to the case in Fig. 10 except that only one circular obstacle of radius 0.5 m is located at (3.5, 3.5). The formation is supposed to move from ζ 0 = (2.5, 1.5, 0) to ζ g = (5, 5.5, 0), avoiding the circular obstacle. The attribute variables of the system are defined in Fig. 16(b), and the RSO is shown in Fig. 16(c). The position coordinate pi = (pix , piy ) of robot Ri can be computed by the attribute set variables ⎧ p1x = x + l cos θ ⎪ ⎪ ⎪ p ⎪ 1y = y + l sin θ ⎪ ⎨ p2x = x + l cos(θ + 2/3π) (30) p2y = y + l sin(θ + 2/3π) ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ p3x = x + l cos(θ − 2/3π) p3y = y + l sin(θ − 2/3π) where l is the distance from the robot √ to the formation centroid, as shown in Fig. 16(b), and l = 3/3L. It is easy to see that (30) is similar to (29) in its formation, although there are more parameters involved. If the distance between the robots is variable, it can be included to reconstruct the RSM as an additional dimension. Fig. 16(c) shows the transition trajectory in RS for the system attributes to realize the formation-movement task. It can be mapped by (30) to the practical movement of each of the robot in the system.


257

Fig. 15. Formation-movement process of an MRS.

Fig. 16. RS method for three robots in formation-movement task.

effector (Ex , Ey , Ez ) has the following relations with attribute variables (x, y, θ, θ1 , θ2 ): ⎧ ⎨ Ex = x + L1 cos θ1 cos θ + L2 cos(θ1 + θ2 ) cos θ (32) E = y + L1 cos θ1 sin θ + L2 cos(θ1 + θ2 ) sin θ ⎩ y Ez = L1 sin θ1 + L2 sin(θ1 + θ2 )

Fig. 17. RSM of the mobile manipulator.

V. M OBILE -M ANIPULATOR TASK P LANNING The RS method can also be applied to task planning of the mobile-manipulator system. Pose variables of the mobile robot and joint variables of the manipulator are all used to construct the RS of the whole system. The mobile robot has three DOFs of position and direction (x, y, θ), while the manipulator may have n DOFs (θ1 , θ2 , . . . , θn , n is the total link number). Thus, the overall dimension of the system’s RS is 3 + n. This way of construction is applicable for the task by multilink manipulators mounted on a mobile platform. Fig. 17 shows a typical case, a two-link manipulator mounted on a mobile robot. We construct the system’s RS with pose variables of the mobile robot (x, y, θ) and joint variables of the manipulator (θ1 , θ2 ), shown in Fig. 17 ζ = (x, y, θ, θ1 , θ2 ).

(31)

The mobile robot and the manipulator are both prevented from entering into the obstacle areas. The position of the end

by which the system’s reachable space could be figured out. Suppose the mobile manipulator is required to go through a door in a workspace of a 2-m square (Fig. 18). The initial attribute of the system is ζ 0 = (0.5, 0.5, 0, π/6, π/6), and the goal attribute is ζ g = (1.5, 1.5, π/2, 5π/12, π/12). The sampling intervals for attribute variables are empirically chosen to be I1 = I2 = 0.05 m, I3 = I4 = 5◦ , and I5 = 10◦ , respectively. The shortest path for the whole system, namely, the minimum summation of the distances traveled by the mobile robot and the manipulator, is considered. Since the system satisfies (4), the task is realizable with the attribute transition trajectory Pˆ obtained by the method proposed in Section II-B. Then, the corresponding trajectories of the mobile robot and the end effector in the workspace can be computed from (32). The 0 g , Pmr are the initial results are shown in Fig. 18(a), where Pmr g 0 are those and goal positions of the mobile robot, and Pef , Pef of the end effector, respectively. In terms of different task requirements, the corresponding trajectories can be derived through modification of the cost function. Consider the individual shortest path of the mobile robot and the end effector; the resultant trajectories of the system in the workspace are shown in Fig. 18(b) and (c), respectively. It is easy to see that the task requirements and realization process are very convenient to adjust, which illustrates the adaptability and versatility of the proposed scheme and method.

258


Fig. 18. Trajectories of the mobile robot and the end effector to go through a gate in different indexes.

It is worthwhile to mention that the computational cost of RS construction and trajectory searching for attribute transition is specifically being paid attention to in the experiments. All programs are compiled in C++ language and run in a PC of 2.0-GHz CPU with 512-MB RAM. The total time spent in the mobile manipulator task is less than 0.5 s, which is normally acceptable. VI. C ONCLUSION This paper has proposed a general motion planning and coordination strategy for robot systems. The RS of a robot system is defined, and the reachable areas of the system attributes are figured out, by which the system’s ability to achieve tasks can be analyzed. Consequently, whether the given task can be accomplished depends on whether the system satisfies the prescribed conditions evaluated in its RS. Moreover, if the task is evaluated to be realizable by a robot system of definite configurations (internal constraints) under specific environment or task constraints (external constraints), the optimal strategy for the robot system to accomplish the task can hence be obtained in the RS. The proposed method has been validated through three different tasks: the manipulator path planning, the multirobot formation movement, and the mobile manipulator task planning, which implies that the proposed RS is a universal strategy for different robots in different kinds of task planning. Furthermore, the proposed RS method could help to transform an unrealizable task to be realizable by a specific robot system by modifying its configurations and/or task constraints. The transformation condition is also figured out. This contributes to clarify the design and plan of tasks for a robot system of definite functionality with definite configurations. The proposed method is general enough and can adapt to any single or multiple robot systems as well as tasks under any environments. Future work lies in applying the proposed RS method to more complicated tasks. Reduction of the computational cost by taking advantage of the RS method is also under consideration. Other path-planning methods, such as randomized-samplingbased motion-planning methods, which might be of higher efficiency for obtaining attribute transition trajectories in RS, will further be investigated.

R EFERENCES [1] T. Lozano-Perez, “A simple motion-planning algorithm for general robot manipulators,” IEEE J. Robot. Autom., vol. RA-3, no. 3, pp. 224–238, Jun. 1987. [2] K. Sun and V. J. Lumelsky, “Motion planning for three-link robot arm manipulators operating in an unknown three-dimensional environment,” in Proc. 30th Conf. Decision Control, Brighton, U.K., Dec. 1991, pp. 1019–1026. [3] K. Tchon and J. Jakubiak, “A repeatable inverse kinematics algorithm with linear invariant subspaces for mobile manipulators,” IEEE Trans. Syst., Man, Cybern. B, Cybern., vol. 35, no. 5, pp. 1051–1057, Oct. 2005. [4] G. Antonelli, S. Chiaverini, M. Palladino, G. P. Gerio, and G. Renga, “Cartesian space motion planning for robots: An industrial implementation,” in Proc. 4th Int. Workshop Robot Motion Control, Jun. 17–20, 2004, pp. 279–284. [5] X. Xu and Y. Chen, “A method for trajectory planning of robot manipulators in Cartesian space,” in Proc. 3rd World Conf. Intell. Control Autom., Jun. 28–Jul. 2, 2000, pp. 1220–1225. [6] A. Ibeas and M. D. L. Sen, “Robustly stable adaptive control of a tandem of master–slave robotic manipulators with force reflection by using a multiestimation scheme,” IEEE Trans. Syst., Man, Cybern. B, Cybern., vol. 36, no. 5, pp. 1162–1179, Oct. 2006. [7] W. Kowalczyk, “Multi-robot coordination,” in Proc. 2nd Workshop Robot Motion Control, Oct. 18–20, 2001, pp. 219–223. [8] J. P. Desai, J. Ostrowski, and V. Kumar, “Controlling formation of multiple mobile robots,” in Proc. IEEE Int. Conf. Robot. Autom., Leuven, Belgium, May 1998, pp. 2864–2869. [9] T. Suagr and V. Kumar, “Decentralized control of cooperating mobile manipulators,” in Proc. IEEE Int. Conf. Robot. Autom., Leuven, Belgium, May 1998, pp. 2916–2921. [10] X. Yun, G. Alptekin, and O. Albayrak, “Line and circle formation of distributed physical mobile robots,” J. Robot. Syst., vol. 14, no. 2, pp. 63– 76, 1997. [11] T. Balch and R. C. Arkin, “Behavior-based formation control for multirobot teams,” IEEE Trans. Robot. Autom., vol. 14, no. 6, pp. 926–939, Dec. 1998. [12] G. Baldassarre, V. Trianni, M. Bonani, F. Mondada, M. Dorigo, and S. Nolfi, “Self-organized coordinated motion in groups of physically connected robots,” IEEE Trans. Syst., Man, Cybern. B, Cybern., vol. 37, no. 1, pp. 224–239, Feb. 2007. [13] M. A. Lewis and K. H. Tan, “High precision formation control of mobile robots using virtual structures,” J. Auton. Robots, vol. 4, no. 4, pp. 387– 403, Oct. 1997. [14] J. Ward and J. Katupitiya, “Mobile manipulator motion planning towards multiple goal configurations,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Beijing, China, Oct. 9–15, 2006, pp. 2283–2288. [15] J. Ward and J. Katupitiya, “Free space mapping and motion planning in configuration space for mobile manipulators,” in Proc. IEEE Int. Conf. Robot. Autom., Rome, Italy, Apr. 10–14, 2007, pp. 4981–4986. [16] K. Nagatani, T. Hirayama, A. Gofuku, and Y. Tanaka, “Motion planning for mobile manipulator with keeping manipulability,” in Proc. IEEE/


[17]

[18]

[19]

[20] [21]

[22]

[23]

[24] [25]

[26]

RSJ Int. Conf. Intell. Robots Syst., Lausanne, Switzerland, Oct. 2002, pp. 1663–1668. G. Oriolo and C. Mongillo, “Motion planning for mobile manipulators along given end-effector paths,” in Proc. IEEE Int. Conf. Robot. Autom., Barcelona, Spain, Apr. 2005, pp. 2154–2160. Z. Li, S. S. Ge, and A. Ming, “Adaptive robust motion/force control of holonomic-constrained nonholonomic mobile manipulators,” IEEE Trans. Syst., Man, Cybern. B, Cybern., vol. 37, no. 3, pp. 607–616, Jun. 2007. J. Tan and N. Xi, “Unified model approach for planning and control of mobile manipulators,” in Proc. IEEE Int. Conf. Robot. Autom., Seoul, South Korea, May 21–25, 2001, pp. 3145–3152. N. J. Nilsson, Artificial Intelligence: A New Synthesis. San Mateo, CA: Morgan Kaufmann, 1998. K. Zhang and J. Su, “RTOS-based software architecture for multisensor fusion system,” in Proc. 5th Asian Control Conf., Melbourne, Australia, Jul. 20–23, 2004, pp. 906–913. C. S. Lin, P. R. Chang, and J. Y. S. Luh, “Formulation and optimization of cubic polynomial joint trajectories for industrial robots,” IEEE Trans. Autom. Control, vol. AC-28, no. 12, pp. 1066–1073, Dec. 1983. J. R. T. Lawton, R. W. Beard, and B. J. Young, “A decentralized approach to formation maneuvers,” IEEE Trans. Robot. Autom., vol. 19, no. 6, pp. 933–941, Dec. 2003. S. M. LaValle, Planning Algorithms. Cambridge, U.K.: Cambridge Univ. Press, 2006. X. Chen and Y. Li, “Smooth formation navigation of multiple mobile robots for avoiding moving obstacles,” Int. J. Control Autom. Syst., vol. 4, no. 4, pp. 466–479, Aug. 2006. Y. Li and Y. Liu, “Real-time tip-over prevention and path following control for redundant nonholonomic mobile modular manipulators via fuzzy and neural-fuzzy approaches,” Trans. ASME, J. Dyn. Syst. Meas. Control, vol. 128, no. 4, pp. 753–764, Dec. 2006.

259

Jianbo Su (SM’04) was born in Jiangsu Province, China, in 1969. He received the B.S. degree in automatic control from Shanghai Jiao Tong University, Shanghai, China, in 1989, the M.S. degree in pattern recognition and intelligent system from the Institute of Automation, Chinese Academy of Sciences, Beijing, China, in 1992, and the Ph.D. degree in control science and engineering from Southeast University, Nanjing, China, in 1995. After a two-year postdoctoral research with the Institute of Automation, Chinese Academy of Sciences, he joined the faculty of the Department of Automation, Shanghai Jiao Tong University in 1997, where he is currently a Full Professor. His research interests include robotics, pattern recognition, and human–machine interaction. In these areas, he has published three books, more than 170 technical papers, and is the holder of 15 patents. Dr. Su is a member of the Technical Committee of Networked Robots, IEEE Robotics and Automation Society, and a standing committee member of the Chinese Association of Automation. He served as the General Cochair of the IEEE International Conference on Machine Learning and Cybernetics in 2004. Wenlong Xie was born in Hebei Province, China, in 1977. He received the B.S. and M.S. degrees in electrical engineering and automation from the Hebei University of Technology, Tianjin, China, in 2000 and 2003, respectively, and the Ph.D. degree in control theory and engineering from Shanghai Jiao Tong University, Shanghai, China, in 2008. He is currently an Engineer with the Shanghai Academy of Spaceflight Technology, Shanghai. His research interests include multirobot systems, motion coordination, and path planning.