Motion Planning for a Mobile Manipulator with Redundant ... - CiteSeerX

International Journal of Information Technology

Vol. 11 No. 11 2005

Motion Planning for a Mobile Manipulator with Redundant DOFs De Xu1,2, Huosheng Hu2, Carlos A. Acosta Calderon2, and Min Tan1 1

The Key Laboratory of Complex System and Intelligence Science, Institute of Automation, Chinese Academy of Sciences, Beijing 100080, China 2 Department of Computer Science, University of Essex, Colchester CO4 3SQ, UK [email protected], [email protected], [email protected], [email protected]

Abstract This paper investigates how to track a desired trajectory by a mobile manipulator that has redundant degrees of freedom (DOFs). Based on the analysis of its kinematics models, a motion planning method is proposed. The position and orientation of the end-effector is decomposed into two parts. In the first part, the manipulator contributes sub-vectors projected on the Z-axis in the world frame, including position and orientation. In the second part, the mobile base and the manipulator move along the direction of the desired path and reach the sub-vectors on axes X and Y in the world frame respectively. Simulated results are presented to show the effectiveness of the proposed approach.

Keyword: Mobile manipulator, Redundant DOFs, Insufficient DOFs, Path planning, Trajectory tracking.

I. Introduction A mobile manipulator consists of a mobile base and a manipulator [1], which represents several advantages and various constraints [2]. The most important feature of a mobile manipulator is the flexible operational workspace in contrast with the limited workspace of a fixed manipulator. This feature endows a mobile manipulator with the ability to operate in a large scale of operation [3], such as handling and transporting parts from one place to another. However, there is an intrinsic problem with a mobile manipulator: the total number of degrees of freedom (DOFs) is generally greater than six DOFs. How to deal with the redundant DOFs attracts much attention in the robotics community recently [1-10]. Seraji [4] presented an approach to motion control of a mobile manipulator, in which both mobility and manipulation were put on the same frame with an equal treatment using the combined Jacobian matrix. Tanner et al [5] proposed a motion planning method for multiple cooperating mobile manipulators. Huang et al [6] introduced the Zero Moment Point to path planning for a mobile base, and to orientation planning for a manipulator with 5 DOFs. The orientation of the end- effector is not concerned except for avoiding instability in mechanics. Papadopoulos and Poulakakis [7] presented a model-based control and planning method for a mobile manipulator. Perrier et al [8] presented a global approach to motion generation for a non-holonomic mobile manipulator. Both homogeneous 1

De Xu, Huosheng Hu, Carlos A. Acosta Calderon, and Min Tan Motion Planning for a Mobile Manipulator with Redundant DOFs

matrices and dual quarter- nions are respectively employed to generate a point-to-point trajectory for a mobile robot. Sugar and Kumar [3][9] provided a framework and algorithm for cooperating mobile manipulators to hold and transport large objects. Matsikis et al [10] proposed a behavior coordination manager based on Bayesian Belief Networks for a mobile manipulator to estimate the effectiveness of its behaviors. It should be noticed all these mobile manipulators have enough DOFs such that the mobile base and the manipulator can be decoupled in the control design. However, the mobile manipulator concerned in this paper consists of a differential driven mobile robot, Pioneer II, and a 5-DOF manipulator Pioneer Arm (PArm). It is impractical to decouple them since the 5-DOF PArm itself cannot satisfy some desired positions and orientations of its endeffector. Hence, it is a real challenge to deal with the problem of insufficient DOFs for the PArm and redundant DOFs for the mobile manipulator, i.e. the integration of the Pioneer II and the PArm. The rest of the paper is organized as follows. The configuration of the mobile manipulator and its task are described in Section 2. The models for the mobile manipulator are investigated in Section 3. In Section 4, a new strategy to deal with the motion control of the mobile manipulator is introduced based on its kinematics models. The method of freedom assignment for the mobile base and manipulator is given, and the resolve of joint angles for the PArm is deduced. The position assignment of the mobile base is also presented in this section. Simulation results are shown in Section 5. Finally, Section 6 concludes the paper.

II. The Configuration of the Mobile Manipulator and Its Task The sketch of the mobile manipulator is as shown in Fig. 1(a). The manipulator PArm is on the top of the mobile base Pioneer II. A gripper is mounted on the end of the PArm, namely the end-effector in the rest of this paper.

d4

d1

a1

O0

a2

O1

Door

O4, O5 O6

θ2 O2, O3

θ2+θ3

Y6 Z6

Zm Om

X6 Ym

Y6

X6

Z6 (b)

Xm(a)

Fig.1. The mobile manipulator and its task. The configuration sketch of the mobile manipulator is shown in (a). The principle sketch of opening a door is shown in (b). Assume that the robot’s local frame Om is at the center of the main axis connecting two driven wheels of the mobile base, and the PArm is located at (mx, my, mz) in the robot local frame. O0 is the base frame of the manipulator, and O6 is the end-effector’s frame. O1 to O5 are the joint frames of the manipulator. a1, a2, d1 and d4 are the inherent parameters of the manipulator. More detail about the frame assignment can be found in [11][12].

2


Vol. 11 No. 11 2005

The task of the mobile manipulator is to open a door, as shown in Fig. 1(b). The end-effector catches the doorknob in the upward orientation. In other words, the Z6-axis in the end-effector frame is upward. The Y6-axis is the direction from the doorknob to the turning axis of the door. When the end-effector opens the door, its trajectory is a piece of circle arc. This task can be described as: following the desired trajectory and orientation by a mobile manipulator. How to deal with its redundant DOFs is the key factor to ensure the PArm and the mobile base working reasonably.

III. The Model of the Mobile Manipulator Assume that ci denotes for cos(θi), si for sin(θi), c23 for cos(θ2+θ3), s23 for sin(θ2 +θ3), c for cos, and s for sin. The transform Aj between two neighboring frames, i.e. Oj-1 and Oj, can be obtained from Denavit-Hartenberg parameters. Therefore, the transforms A1-A6 can be obtained. Then the position and orientation of the end-effector [11] are indicated as v v v v ⎡n o a p ⎤ 0 T6 = A1 A2 A3 A4 A5 A6 = ⎢ (1) ⎣ 0 0 0 1 ⎥⎦

[

v here n = n x

ny

[

]

T v nz , o = o x

oy

]

[

T v oz , a = ax

ay

az

]

T

[

v , p = px

py

]

T

pz .

A. The Model of Mobile Robot Pioneer II Pioneer II is a mobile robot with two differential driven wheels and a caster. W1 and W2 represent the left and right wheel separately, along the forward direction of the mobile robot in the view of bird’s eye. The world frame Ow is selected at a fixed position on the floor, which is taken as absolute reference frame. Assume that Omi indicates the coordinate frame Om of the robot at i-th sampling. ri denotes its turning radius in the period between the i-th and i+1-th samplings, which is relative to W1. θ i represents its direction angle at the i-th sampling. l is the distance between W1 and W2. The position and orientation of frame Om in the world frame at the i+1-th sampling, wTmi+1, can be derived from wTmi according to homogeneous transformation. Suppose wTmi is as shown in Eq. (2). ⎡cθ i − sθ i 0 p xi ⎤ ⎢ i ⎥ sθ cθ i 0 p iy ⎥ w i ⎢ (2) Tm = ⎢ 0 0 1 0⎥ ⎢ ⎥ 0 0 1⎦ ⎣ 0 ⎡cθ ⎢ i +1 sθ w i +1 Tm = ⎢ ⎢ 0 ⎢ ⎣⎢ 0

i +1

− sθ cθ

i +1

i +1

0 0

0 0 1 0

p p

i +1 x i +1 y

0 1

⎡ i i i i ⎤ ⎢c(θ + α ) − s (θ + α ) ⎥ ⎢ ⎥ = ⎢ s (θ i + α i ) c(θ i + α i ) ⎥ ⎢ ⎥ ⎢ 0 0 ⎦⎥ ⎢ 0 0 ⎣

0 0 1 0

l αi ⎤ )(r i + ) s ⎥ 2 2 2⎥ αi i l αi ⎥ i i (3) p y + 2c(θ + )(r + ) s 2 2 2⎥ ⎥ 0 ⎥ 1 ⎦ p xi − 2 s (θ i +

αi

The distance between frame Omi+1 and frame Omi is 2( r i + l / 2) s (α i / 2) , and the rotation angle between Ymi+1 and Ymi is αi. Then wTmi+1 can be derived as (3). More detail is available in [13]. 3


Of course, Eq. (3) can be rewritten as a group of iteration equations that represent positioning with Odometry for the mobile robot [14].

B. The Model of the Mobile Manipulator The position and orientation of the end-effector in the world frame can be derived from homogeneous transform according to the position and orientation of the mobile robot in the world frame, that of the end-effector in the manipulator’s base frame, and the transform between the mobile robot frame and the manipulator’s base frame. The kinematics of the mobile manipulator can be described in Eq. (4). T6i = wT0i 0T6 = wTmi mT0 0T6

w

⎡− nx sθ i − n y cθ i ⎢ nx cθ i − n y sθ i ⎢ = ⎢ nz ⎢ 0 ⎣⎢

− ox sθ i − o y cθ i

− a x sθ i − a y cθ i

ox cθ i − o y sθ i

a x cθ i − a y sθ i

oz 0

az 0

− ( p x + m y ) sθ i − ( p y − mx )cθ i + p xi ⎤ ⎥ ( p x + m y )cθ i − ( p y − mx ) sθ i + p iy ⎥ (4) ⎥ p z + mz ⎥ 1 ⎦⎥

where the up index i denotes the i-th sampling, wT6i denotes the position and orientation of the end-effector in the frame Ow, wT0i is the position and orientation of frame O0 expressed in frame Ow. mT0 as Eq. (5) is the position and orientation of the frame O0 in frame Om. ⎡0 − 1 ⎢1 0 m T0 = ⎢ ⎢0 0 ⎣⎢0 0

0 mx ⎤ 0 my ⎥ ⎥ 1 mz ⎥ 0 1 ⎦⎥

(5)

here [mx my mz]T is the offsets of frame O0 in axis X, Y, and Z of frame Om separately.

IV. Deal with the Insufficient and Redundant DOFs Problem A. The Freedom Assignment for the Mobile Robot and the Manipulator As explained previously, the PArm has 5-DOFs, and the Pioneer II robot has 3-DOFs. Obviously, the mobile manipulator is with redundant DOFs. If the links of the PArm have enough length, or the desired moving range of the end-effector is adequate, the mobile manipulator will have 8-DOFs. In fact, the link length is limited and the desired range of the end-effector is large [15-20]. Therefore, how to distribute the desired 6-DOFs to the manipulator and the mobile robot is a key factor for the mobile manipulator to satisfy the desired position and orientation of the end-effector in an efficient and simple manner. It is easy to find that the position and orientation of the Pioneer II base have no contribution to the position and sub-vectors of orientation for the end-effector in Z-axis of the world frame. Therefore, the manipulator can only satisfy these desired position and sub-vectors of orientation. This means that these 3-DOFs must be completely handed over to the manipulator. Other 3-DOFs should be the interaction results between the manipulator and the mobile base.

4


Vol. 11 No. 11 2005

B. Position and Orientation in Z-axis Suppose the desired position and orientation of the end-effector in frame Ow is v ⎡ wn T =⎢ ⎣0

w

where n = [w nx v

w

ny

w

]

T v nz , o =

[o w

w x

v o 0

w

i 6

oy

w

]

T v oz , a =

v a 0

v p⎤ 1 ⎥⎦

w

[a

w

w

w x

ay

(6)

w

]

T v az , p =

[

w

px

w

py

w

]

T

pz .

nz, woz, waz and wpz are determined by the joint angles θ2-θ5 of the manipulator. Considering redundancy, the assignment θ5=0 will be a good selection to simplify the realization of the position and orientation of the end-effector in the world frame. Hence, Eq. (7) is deduced from Eq. (4) and Eq. (6). ⎧⎪ w nz = c23c4 , woz = −c23s4 (7) ⎨w ⎪⎩ a z = − s23 , wpz − mz = −d 6 s23 − d 4 s23 − a2 s2 + d1 w

θ23 has two candidate solutions denoted as θ23-1 and θ23-2 from s23=-waz. Submitting s23=-waz to the last equation in (7), two candidate solutions for θ2, i.e. θ2-1 and θ2-2, are presented. Submitting θ23 to other equations in (7), θ4 is determined according to the sign of c23. Note that c23=0 is satisfied if and only if waz=±1. In this case, θ4 can be assigned to any value in the working range of joint 4. However, it had better be evaluated with the consideration for θ1 and θ i.

C. Position and Orientation in X and Y axes The orientation angle θ i of the mobile base can be calculated using the last position and the current desired one of the end-effector, which is a known variable. Therefore, 0T6 is a known matrix as shown in Eq. (8). T6 = wT0i

0

−1 w

T6i

⎡− wnx sθ i + wn y cθ i ⎢ w − nx cθ i − wn y sθ i =⎢ w ⎢ nz ⎢ 0 ⎢⎣

− wox sθ i + wo y cθ i − wox cθ i − wo y sθ i w

oz 0

− wax sθ i + wa y cθ i − wa x cθ i − wa y sθ i w

az 0

−( wp x − p xi ) sθ i +( wp y − p iy )cθ i − m y ⎤ ⎥ −( wpx − pxi )cθ i −( wp y − p iy ) sθ i + mx ⎥ (8) w ⎥ p z − mz ⎥ 1 ⎥⎦

The resolving of θ1 is based on Eq. (8) according to the values of θ4 and θ23. If c4=0 and c23≠0, the terms nx and ax can be employed to find θ1. If c4=0 and c23=0, the terms nx and ox are employed. If c4≠0 and c23≠0, the terms oy and ay are used. If c4≠0 and c23=0, the terms ny and oy are selected. The solution of θ1 is obtained as Eq. (9) in the condition θ5=0. ⎧atan[nx sig ( s4 ), a x /c23 ], ⎪atan[n sig ( s ), − o sig ( s s )], ⎪ x x 4 23 4 θ1 = ⎨ ⎪ atan[a y /c23 ,− a x s23 s4 /(c23c4 ) − ox /c4 ], ⎪⎩atan[n y c4 sig ( s23 ) − o y s4 sig ( s23 ),−n y s4 − o y c4 ],

5

if c4 = 0, c23 ≠ 0 if c4 = 0, c23 = 0 if c4 ≠ 0, c23 ≠ 0 if c4 ≠ 0, c23 = 0

(9)


where atan(y, x) is a function to calculate the arc tangent of y/x, the signs of both arguments x and y are used to determine the quadrant of the result. The solutions for θ1-θ4 can be selected from the candidates according to the actual range of the joint angles and criterions in [11]. If θ1 is out of the working range, it is assigned to a maximum value. In other words, if θ1>θ1max then θ1=θ1max; if θ1