Variational-Vector Calculus Formulation for Dynamics of Nonholonomic Mobile Manipulator Systems Qing Yu
I-Ming Chen
Robotics Research Center, School of Mechanical & Production Engineering Nanyang Technological University, Singapore 639798
[email protected],
[email protected] Abstract This paper studies the dynamic modeling of a nonholonomic mobile robotic manipulator that consists of a serial manipulator and an autonomous wheeled mobile platform. Variational-Vector Calculus Method and Forward Recursive Formulation for dynamics of multibody systems were employed to establish the governing equation of the mobile manipulator system. The resulting dynamic equation is in state-space presentation and can be used for the purpose of simulation and control design. The detailed flowchart of the simulation implementation is illustrated.
1. Introduction In recent years, mobile manipulator has become a subject of interest because of their mobility and dexterity. Typically, a mobile manipulator consists of a multi-link manipulator and a mobile platform. Dynamic modeling of mobile manipulators has been addressed by a number of researchers. Dubowsky and Tanner [1] studied the compensation of dynamic disturbance to the manipulator caused by the vehicle motion. They derived a set of linearized equations of motion for a 3-DOF manipulator with the inertial of the vehicle. Yamamoto and Yun [2] focused their research on the modeling and compensation of the dynamic interaction of mobile manipulators. They used a modular approach to derive motion equations of a multi-link mobile manipulator. A mobile robot manipulator can be considered as the integration of two subsystems: a manipulator and a mobile platform. Saha and Angeles [3] studied dynamics of a rolling robot with conventional wheels. A multi-link manipulator is a spatial open-loop multibody system. Recursive formulation was regarded as the fast and effective way to establish the dynamic equation of an openloop multibody system due to its state-space formulation and high numerical efficiency. Combining the dynamic modeling methods of the two subsystems, we will establish dynamic equation of the mobile manipulator system in state-space formulation based on Variational-Vector Calculus Method [4]. The approach fully utilizes the existing motion equations of the manipulator and the mobile platform, and introduces additional terms that account for the dynamic interactions between the manipulator and the mobile platform. Consequently, every term in the resulting motion equation retains a clear physical meaning, in particular, the terms
representing the inertia and force/moment interaction between the two subsystems.
2. System Definition Fig. 1 shows the configuration of the mobile manipulator under investigation rolling on conventional wheels. The chassis, or the base of platform, is depicted as a triangular plate in the figure. Despite its shape, the approach applied here can be used easily to other nonholonomic mobile manipulators. Two driving wheels that are connected to the chassis use revolute joints. The axes of the joints pass through points O1 and O2. The third wheel is mounted on a rotary bracket. The manipulator mounted on the mobile platform is a spatial open-loop multibody system with N links. We adopt the following assumptions in modeling the mobile manipulator system.
d
Zp P
Yp Xp
Q
D
ρd
ψ
Zb Yb
O1
θ1
θ3
Ob O2 l
Z0
θ2
Xb
O3
e
Y0 O
X0
Fig. 1 A mobile manipulator. q
There is no slipping between the wheel and the floor. The vehicle can not move sidelong to maintain the nonholonomic constraint. q The manipulator is rigidly mounted on the platform. q The motion of the platform is confined in O-X0Y0 plane and no translations along the global Z0 axis. In Fig. 1, O-X0Y0Z0 is the global reference frame, P-XpYpZp and Ob-XbYbZb is the chassis reference frame and bracket reference frame, respectively. Let l be the distance
between the actuated wheels, e be the distance between P and Q, d be the distance between O3 and Zb axis, respectively. Let ra and rc be radius of driving wheels and caster wheel, respectively. Denote ϕ , θ 1 , θ 2 , θ 3 and ψ as the orientation angle of the chassis, rotate angles of driving wheels, rotate angle of the caster wheel and orientation angle of the bracket relative to the chassis, respectively. Due to the space limitation, this paper can not give the detailed derivation of the dynamic equation of the mobile manipulator system, the detailed derivation and the explanation of each expression or symbol can be found in reference [5].
3. Kinematics of Mobile Manipulator The angular velocity of the chassis cab be given by ω P = ω P Z P = ϕ&Z P According to the nonholonomic constraints and the noslipping assumption, the velocities of the driving wheels are given directly r&O1 = − raθ&1YP , r&O2 = − raθ&2YP . The angular velocity of the chassis can be written in terms of θ& and θ& 1
(
2
)
ra & & θ1 − θ 2 l Furthermore, the velocity of the chassis can be expressed as r r&P = − a θ&1 + θ&2 YP 2 T Define θ a = [θ1 θ 2 ] to be the driving wheel rate, the ω P = ϕ& =
(
)
generalized velocity of the chassis v P = [r&P global reference frame can be written as P P v P = A v P ' = A T P 'θ& a = T P θ& a
ω P ] in the T
P
where A is the orientation of chassis.
[
]
T
Define π P = r P 0 0 ϕ to be the generalized coordinate of the chassis, one can obtain v P = π& P = T P θ& a (1) T
Note that the generalized coordinate of the chassis π P can not be solved directly due to the nonholonomic constraints. Differentiating Equation (1) with respect to time, the generalized acceleration of the chassis is v& P = T P θ&&a + T& P θ& a (2) The angular velocities of the driving wheels is ω 1 = θ&1 X P + ω P Z P , ω 2 = θ&2 X P + ω P Z P
[
Now, let v1 = r&O1
]
[
]
T T ω1 and v 2 = r&O2 ω 2 be the generalized velocities of the respective driving wheel, they can be given as below in the global reference frame AP L ' P v i = A v i ' = P i θ& a = T i θ& a i = 1, 2 (3) A R i ' The generalized accelerations of driving wheels relative to the global reference frame become P v& i = A v& i ' = T i θ&&a + T& i θ& a , i = 1, 2 (4) From Fig. 1, we have the relationship − rcθ&3Yb + d (ω P + ψ& )X b = r&P − ω P eX P (5)
To determine θ&3 , we take inner product of both sides of Equation (5) with Yb . Likewise, ψ& can be determined by taking inner product of both sides of Equation (5) with X b . Then, we get the following equation relating the active wheel rate and the passive wheel rate: θ& u = Hθ& a
where θ u = [θ 3 ψ ] is the passive wheel rate The velocity of the caster wheel can be obtained directly by the no-slip assumption r&3 = −rcθ&3Yb and the angular velocity of the caster wheel can be written conveniently in the bracket fixed reference frame as ω 3 = θ&3 X b + (ω P + ψ& )Z b We now can find the generalized velocity of the caster T wheel v3 = [r&3 ω 3 ] in the global reference frame as T
Ab L ' L b v 3 = T 3 θ& a , T 3 = 3 = A T 3 ' = b 3 R 3 A R 3 '
(6)
b
where A is the orientation of the bracket. The velocity of the bracket is equal to that of point Q on the chassis, i.e. r&b = r&Q = r&P + ω P × PQ and the angular velocity of the bracket is the combination of ω P and ψ& : ω b = ω P + ψ&Z b = (ω P + ψ& )Z b = ω b Z b By applying Equation (1), we can find the generalized velocity vector of the bracket in the global reference frame as AP L ' P v b = T b θ& a , T b = A T b ' = P b (7) A R b ' Differentiating Equations (6) and (7) with respect to time, respectively, one can obtain the generalized acceleration of the caster wheel and the bracket in the global reference frame: v& 3 = T 3 θ&&a + T& 3 θ& a , v& b = T b θ&&a + T& b θ& a (8) Applying forward recursive formulation, the kinematics of a multi-link manipulator can be derived. Now, we consider 0 two bodies B j and Bi connected by joint h i . Let, e be the j
i
global reference frame and e and e be the body fixed reference frames with A
j
i
and A
denoting as their 0
orientations relative to the global reference frame e , respectively. Frame e
h0
h
and e are joint reference frames
on B j and Bi , q and p are their origins, respectively. j
Vectors rj and ri are the displacements of frame e and e
i
relative to frame e . Define ρ q and ρ p to be the location 0
vectors of joint hi described in their respective body p
q
reference frame, respectively; C j and C i h0
to be the h
orientations of joint reference frame e and e relative to their body fixed reference frame, respectively. Let vector v hi be the displacement vector of point p with respect to
h
h
point q; D i be the orientation of frame e with respect to h0
frame e . With the aforementioned notations, one can realize that the translation and the orientation of body Bi v relative to body B j are uniquely determined by hi and h
Di . Let q i , be the relative joint coordinate of joint h i , we can obtain the following equations in frame e
h0
( )
hi ' = H i ' qi , D i ' = Di ' qi h
h
h
Ω
V i ' = H i ' q& i , Ω i ' = H i ' q& i V
o
o
Ω
V i ' = H i ' q&&i + ζ i ' , Ω i ' = H i ' q&&i + η i ' V
o
h
Ω
V
with respect to e 0 , respectively. D i ' , H i ' , H i ' , H i ' ,
ζ i ' and η i ' are called Relative Kinematic Matrices and they depend on the type of the joint. ji j i Let A be the orientation of frame e relative to frame e and A
h0
be the orientation of frame e
h0
relative to global
0
reference frame e , one can find that pT
A = C j Di C i , A0 = A Cj Based on forward recursive formulation, the orientation of i j ji body Bi is given by A = A A and the recursive ji
q
h
h
j
q
0
relationship between r i and r j in frame e is
r i = r j + A ρ q '+ A 0 H i ' q i − A ρ p ' j
[
T Define v α = r&α
h
ω
]
T T α
h
)
(
N
i =1
i =1
)
(10)
For body Bi , M i is the generalized mass matrix, f i contains the centrifugal force, Coriolis force, the gravitational force, and the applied external forces, v i is the generalized velocity. All terms are determined in the same reference frame, namely the global reference frame. By applying the mobile platform kinematics, the first term in Equation (10) can be expanded as 5 T T P P (11) ∑ δ v − M v& + f == δ θ& − M θ&& + F
(
i
i
i
)
[
a
1
1
a
]
where
o
h
(
5
T T ∑ δ v i − M i v& i + f i + ∑ δ v i − M i v& i + f i = 0
i =1
Here, h i ' , V i ' , Ω i ' , V i ' and Ω i ' are called relative displacement, relative velocity, relative angular velocity, h relative acceleration and relative angular acceleration of e h
wheel, and the chassis, numbered from 1 to 5, in the same order. The variational form of equation of motion for the mobile manipulator system can be written as
T
5
P
5
i =1
T
i =1
T
i =1
The recursive relationship for the multi-link manipulator system described by Equation (9) can be rewritten as N v = G T θ& + ∑ G y& i
P
iP
a
ik
k =1
(
k
)
N
(
v& i = G ip T P θ&&a + T& P θ& a + ∑ G ik &y& k + g ik k =1
The recursive matrices are B k < Bi B k < Bi T ij g jk T ij G jk G ik = U i Bk = Bi , g ik = β i B k = Bi 0 0 B B B ⊄ k i k ⊄ Bi and v P is the generalized velocity of chassis. Substituting Equations (11) and (12) into Equation (10), the dynamic equation of the mobile manipulator system can be simplified as
( ) ( = δ θ& [− M θ&& − M &y& + F ] + δ y& [− M θ&& − M &y& + F ] = 0 N
T
i =1
body Bα (α = i, j ) and y i = q i , we have the following (9)
Equation (9) represent recursive relationship of velocity and acceleration of two adjacent bodies. The matrices T ij , U i and β i are called Kinematic Recursive Matrices.
4. Dynamic Equation of the System Variational-Vector Calculus Method and forward recursive formulation for open-loop multibody systems can be utilized to formulate the dynamic equation of the mobile manipulator system. The proposed method presents the dynamic interactions between the mobile platform and the manipulator become explicitly in the equation. It leads to the inclusion or exclusion of dynamic terms in the equation of motion so that the complexity of a controller is maintained within a manageable limit without sacrificing the performance of the overall system. The mobile platform has 5 major components: three wheels (two driving wheels and one caster wheel), the bracket carrying the caster
(12)
)
∑ δ v − M i v& i + f i + ∑ δ v − M i v& i + f i
to be the generalized velocity of
recursive relationship v i = T ij v j + U i y& i , v& i = T ij v&& j + U i &y& i + β
P
5
i
5
M 1 = ∑ T i M i T i , F 1 = ∑ T i f i − ∑ T i M i T& i θ& a
T
i =1
T
P
a
P m
a
T
m P
)
P
m
(13)
m
a
T T Here y = y 1 L y N the manipulator and P P P M = M1 + M 2
T
is the generalized coordinate of
F = F 1 + F 2 − wm − t m , F = F m − wP − t P P
P
P
N
(
P
P
m
m
m
m
)
M 2 = T P ∑ G iP M i G iP T P P
T
i =1
T
N N P T T T mT M m = T P ∑ G iP M i G i1 L ∑ G iP M i G iN = M P i =1 i =1 P (i )
N
P (i )
tm = ∑ tm P
i =1
M
m
N
T
T
l =1
G M i G i1 L G Ti1 M i G iN = M M M T G T M G L G iN M i G iN i iN i 1 T i1
= ∑Mi , M i i =1
N
, t m = T P G iP M i ∑ g il m
m
N GT M G N GT f ∑ i 1 i1 i i iP i =1 i∑ =1 m m & & M θ , M = wP T Fm = N T P a N T ∑ G iN M i G iP ∑ G iN f i i =1 i =1 N GT M g il i1 i l∑ =1 N m m (i ) m (i ) , tP = M tP = ∑tP N T i =1 G iN M i ∑ g il l = 1
we need to determine the content of the matrix s . The simulation procedure can be divided into two modules, namely kinematic analysis module and dynamic assembly module. State Variables of Mobile Manipulator z
State Variable of Mobile Platform θ , θ , θ& , π
P
a
M 2 is the additional mass matrix of the mobile platform
u
a
State Variable of Manipulator y , y&
P
P
due to the inertia of the manipulator; M m is the inertia interaction between the mobile platform and the P manipulator; w m is the manipulator force applying to the mobile platform due to the motion of the mobile platform. The relative motion of the manipulator interacts on the P P mobile platform through t m . And F 2 is the force applied to the mobile platform due to the centrifugal forces, Coriolis forces, and the external forces on the manipulator. m m Similarly, M is the mass matrix of manipulator; M P is the inertia interaction between the mobile platform and the m manipulator; w P is the centrifugal force applied to the
Kinematics of Mobile Platform Kinematics of Chassis Kinematics of Driving Wheels, Caster Wheel and Bracket
Kinematic Parameters of Mobile Manipulator
Fig. 2 Kinematic analysis module
m
manipulator due to the motion of mobile platform; t P m
contains relative motion of the links. Finally, F m represent the centrifugal force and Coriolis due to the motion of the manipulator and the external applied forces applied to the manipulator. Since θ& a and y& are independent variables, terms inside the parentheses in Equation (13) must be zero, i.e., M &x& = F (14) where M P M mP F P θ a = M = m , F m , x = y m F M P M is the generalized mass matrix, the generalized force, and the generalized coordinate of the mobile manipulator system, respectively.
[
]
T
T T T T Let z = x π P θ u x& be the state variable of mobile manipulator system. Account for the nonholonomic constraints, the first order governing equation of the mobile manipulator system can be expressed as z& = s (z ) (15) where x& T θ& s = P& a Hθ a −1 M F
5. Implementation The dynamic model developed in this paper is for the purpose of computer simulation of mobile manipulator system. To automatically generate integrate Equation (15),
Kinematics of Manipulator
Fig. 2 shows the flowchart of the kinematic analysis module. This module uses state variables of mobile manipulator as the input and divided them into two groups, one for the kinematics of mobile platform and the other for the kinematics of manipulator. Note, here the chassis is the base for either the kinematics of mobile platform and that of manipulator. For caster wheel and bracket, one must determine the transformation matrices H and H& first and then calculates the passive joint rates. The generalized velocity and generalized acceleration of the caster wheel can be obtained after the kinematic analysis of the bracket is finished since the orientation of bracket is needed to present them in the global reference frame. To perform kinematic analysis of manipulator, we need a recursive procedure from the base arm to the tip arm. For each arm, we first obtain the relative kinematic matrices in the global reference frame. Then, applying the method described in section 3, the kinematic parameters of each arm and the kinematic recursive matrices can be obtained. The dynamic assembly module is designed to assemble the dynamic equation of the mobile manipulator system automatically. The assembly procedures for the mobile platform and the manipulator can be performed simultaneously and they are illustrated in Fig. 3. The contributions of the mobile platform to the dynamic equation of the mobile manipulator system can be described as the following assembly procedure: P P T P P T T M = M + T M T , F = F + T f − T M T& θ& 1
1
i
i
i
1
1
i
i
i
i
i
i = 1,L ,5
a
The assembly procedure of the manipulator is much more complicated. First, the recursive matrices in Equation (12) should be calculated recursively from the current arm backward to the base arm. To improve the computational efficiency and based on the fact that G ij = 0 and g ij =0 if
Kinematics of Mobile Manipulator
Dynamics of Single Body M i , wi , f
B j > Bi , the assembly procedure can be re-arranged as the following P P T T M 2 = M 2 + T P G iP M i G iP T P
Mm = Mm +TP P
P
T
( [G
)
T iP
T
)
0 L 0
P
M
m
P 2
T P
T iP
P m
P m
T P
T iP
M i , wi , f
0 i
No Assembly of i=5
Bl ≤ Bi
F = F + T G f i , t = t + T G M i ∑ g il P 2
Dynamics of Single Body
P
M 1 ,F1
]
B j ≤ Bi
G ij , g ij
i
Assembly of
M i G i1 L G iP M i G ii P P T T w m = w m + T P G iP M i G iP T& P θ& a
(
Recursive Matrices
0
P
M 2 ,M P 2
G T M i G L G T M i G i1 i1 ii i1 M M M m 0 =M + G T M G L G T M G i ii ii ii i i1 0 0
Acknowledgment This work is supported by ACRF 7/97 Ministry of Education of Singapore and NSTB.
, wm ,tm
m P
P
m m
P
i=N Yes
Assembly of System Generalized Mass Matrix and Force Vector
M ,F
Assembly of First Order Dynamic Equation
z& = s (z )
Numerical Integration
Fig. 3 Dynamic assembly module
References [1].
6. Conclusion This paper presents a general method to establish the dynamic equation of a mobile manipulator system. We divide the mobile manipulator system into two subsystems, namely the mobile platform and the multi-link manipulator. Variational-Vector Calculus Method is adopted to establish the closed form of the equation of motion of the mobile manipulator and the resulting dynamic equation is in statespace representation. Applying this approach, it is easy to distinguish interactions between the mobile platform and the manipulator. One can simplify a controller without sacrificing the desired performance by neglecting less influential terms. Direct application of this dynamic module is in the design and implementation of the simulation kernel for dynamic simulation, motion plan, and control design of the mobile manipulator system.
m
m P
No
G iT1 M i G iP G iT1 f i M M G T M G G T f m m m m w P = w P + ii i iP T& P θ& a , F m = F m + ii i 0 0 M M 0 0 Bl ≤ Bi T G M ∑ g il i i 1 l =1 M Bl ≤ Bi T m m G M g ∑ i ii tP = tP + il l =1 0 M 0
,M
F ,w ,t , F
Yes
l =1
P m
[2].
[3].
[4].
[5].
N.A.M. Hootsmans, The motion control of manipulators on mobile vehicles, Ph.D. dissertation, Massachusetts Institute of Technology, Cambridge, MA, 1992. S. Dubowsky and A.B. Tanner, A study of the dynamics and control of mobile manipulators subjected to vehicle disturbances, Proc. 4th Int. Symp. of Robotics Research, Santa Cruz, CA, 1987, pp. 111-117. Y. Yamamoto and X. Yun, A modular approach to dynamic modeling of a class of mobile manipulators, International Journal of Robotics and Automation, Vol. 12(2), 1997, pp. 41-48. S.K. Saha and J. Angeles, Kinematics and Dynamics of a three-wheeled 2-DOF AGV, IEEE Int. Conf. on Rob. & Autom., Piscataway, NJ, Vol. 3, 1989, pp. 1572-1577. Qing Yu and I-Ming Chen, Variational-Vector Calculus Formulation for Dynamics of Nonholonomic Mobile Manipulator Systems, Technical report, School of Mechanical & Production Engineering, Nanyang Technological University, 2000.
