lianganmobilerobot

S TATE U NIVERSITY OF N EW Y ORK AT B UFFALO Mechanical and Aerospace Engineering Department.

MAE 505 ROBOTICS

FINAL PROJECT

NAME: LENG-FENG LEE

TAO

GAN

DATE: 15 DEC 2003.

MAE505 Robotic Final Project

In this final project, we studied two journal papers and simulated some of the results based on the theories developed in the papers. These two papers studied wheeled-mobile robot, and focus on the modeling, redundancy resolution, and motion control of wheeled mobile robot (WMR). The two papers that we discussed here are:

1. B. Bayle, J-Y. Fourquet and M. Renaud, “Nonholonomic Mobile Manipulators: Kinematics, Velocities and Redundancies,” Journal of Intelligent and Robotic Systems Volume 36 , Issue 1 (January 2003) Pages: 45 – 63.

2. Yoshio Yamamoto and Xiaoping Yun, “Coordinating Locomotion and Manipulation of a Mobile Manipulator,” IEEE Transactions on Automatic Control, Vol. 39, No. 6, June 1994, pp. 1326-1332.

In the following paragraphs, we discussed each paper in the following manner and our focus will be on the formulation of wheel mobile robot. The first paper uses Reduced Velocities Kinematics Formulation whereas the second paper focuses on the Lagrangian Modeling. In this report, we will mainly focus on the formulation that we use in coding the simulation and discuss some of the setting that we used in the simulation.

1st paper: Nonholonomic Mobile Manipulators: Kinematics, Velocities and Redundancies

Introduction:

Mobile manipulator refers to robots that combine capabilities of locomotion and manipulation. The arrangement of the wheels and their actuation determine the holonomic or nonholonomic nature of this locomotion system. In the following paragraph, we will discuss the modeling of the wheeled mobile robot. Along with the formulation, we use an example (where this example will be used in the simulation of the result) to show how we can applied the concepts in a WMR.

Modeling:

We will use the following planar WMR with a RR links attached on its mobile platform as an example in our formulation and simulation.

A planar mobile manipulator.

1. Modeling of joint space of the Platform.

To define the platform configuration is equivalent to define the configuration of a rectangle on a plane. Its generalized coordinates are then three in number: two for the position and one for the orientation.

It is important to notice that whether the platform is holonomic or not, all the three values are reachable.

2. Modeling of joint space of the Robot Arms.

The robotic arms are mounted on the platform. It is made of n revolute or prismatic joints. Its generalized coordinates are n in numbers.

3. Modeling of joint space of the entire wheel-mobile robot.

Using the formulation in 1 and 2 above, we can now determine the

Thus, in our example, the joint space of the WMR becomes:

=

4. Modeling of the Task Space of the WMR.

The task space is define in terms of the location of the EE the characterized the position of the platform and the orientation of the fix frame attached on the platform itself.

In our example, this becomes:

5. Kinematics of the WMR.

We can now write the velocity kinematics of the WMR as:

Where J(q) is the Jacobian matrix. However, this equation express only a part of the constraints acting on , and it do not reveal specific cases concerning the admissibility of operational velocity-the non-holonomic constraint. Next, we will include the nonholonomic constraint in the velocity equation.

6. Nonholonomic Constraint.

Rolling without slipping condition gives:

This equation is not integrable or nonholonomic, and we can define:

To add this nonholonomic constraint in the equation, we write:

where: is the mobility control vector, and

Note, however, this constraint in written in for the mobile platform.

This constraint restrict the movement in the y-axis of the mobile platform, and only allow movement in the x-axis direction and also rotation about z-axis of the mobile platform. Which shown in the following diagram.

Diagram showing the feasible moving direction of WMR. We now want to write this constraint for the WMR.

Recalled, that this constraint is written for the platform as:

and we can write it for the WMR as:

with

we then introduce a vector, for the joint space of the WMR, where

and finally, the constraint acting on the WMR is then:

with

and

we can write:

which is the Reduced Direct Velocity Kinematics (RDVK).

In our example, we have:

The Forward Kinematic:

and the Jacobian matrix:

To write in the RVDK,

We have:

The task space and the joint space is then related by using:

with:

Note that, the velocity redundancy is that:

7. Step of solving for Matlab coding.

To solve the motion rate control using Matlab, we need to express in terms of q , we need to perform the following steps.

A. First determine by:

B. The determine q from by:

33cos()000sin()

000()0

10000100001q q S q ??

???

???

=??

??????

In our example, the S(q) matrix is determined to be:

8. Operational Motion Planning Problem.

The exact solution for the RDVK is given by:

##()()()(()())()t J t t I J t J t z t ηξ=+?

However, in reality, there might be some error between the desired path and the actual path the EE tracing. Thus, it is preferable to include a error function which will asymptotically decrease.

That is, to include: **e e

ξξ

ξξ=?=? , and to asymptotically stabilize the error e(t),

we need: 0e

We += and thus:

*****0()0

()e e

We W W ξξξξξξξ

ξξξ=??+=??+?=?=+?

Substitute into ()(())J t t ξ

+ term and then we will get:

##()()[()(()())](()))()d d t J t t W t t I J t J t z t ηξ

ξξ=+?+?

The first term in the equation is due to the input and the second term is due to internal motion. The author then proposed to use a coordination strategy for the internal motion based on a gradient descent method. That is, let:

()(())T T z t k P H =??

where: ()()[()]H t S t I J J +=?,

substitute ()(())T T z t k P H =??in:

##()()[()(()())](()))()d d t J t t W t t I J t J t z t ηξ

ξξ=+?+?

After simplification, we will get:

**()()(()(()()))T T t J t t W t t kS HH P ηξ

ξξ+=+???

to write in terms of q(t), we multiple the above equation by S(t) and we finally get:

**()()()(()(()()))T q

t S T J t t W t t kHH P ξξξ+=+???

Which is the equation that we used in our simulation.

9. Manipulability Measure as P. In this paper, the author suggests to use the Manipulability Measure as the P function to use in the above equation. There are two type of Manipulability Measure we used in our simulation. One is the well known Manipulability Measure developed by Yoshikawa, and another Manipulability Measure that we used here is suggest by the author, Bayle. Each of them is given in the following:

Yoshikawa’s Manipulability Measure (Two Forms):

A. P =

B. 12...m P σσσ=

Where 12,...m σσσis the singular values determine using Singular Value Decomposition of the Jacobian matrix.

Bayle’s Manipulability Measure:

P =

In our implementation of these two Manipulability Measure in the motion control equation, we found that Yoshikawa’s Manipulability Measure that expressed in

P =form is not computational efficient. So, in the later state, we will only use

Yoshikawa’s Manipulability Measure in the form of 12...m P σσσ=. This is mainly due to calculation of the determinant requires more computational time.

In our example, we also determine the Manipulability in the form of:

(1)p a a P P P αα+=+?

Where p a P +is the Manipulability Measure of the entire WMR. And a P is the Manipulability Measure of the Robot Arm alone.

In other words, p a P +is obtained from the Jacobian Matrix of the entire WMR, and a P is obtained from the Jacobian of the Robot Arm itself.

That is,

p a P +

is obtained form:

and a P is obtained from:

142452451

4245245cos()cos()cos()sin()sin()sin()a L q L q q L q q J L q L q q L q q +++??=????+?+??

P ?is obtained numerically, which will be a 5x1 matrix. Note that, however, because the Jacobian matrix of the WMR or the Jacobian matrix of the robot arm alone is not a function of q1 and q2, the first two term of the P ?matrix is always zeros.

αis a value that give weights to either the Manipulability Measure of the WMR or the Manipulability of the Robot arms alone. We will see how this value affects the simulation result.

10. Simulation Setting:

The following are the simulation setting that we used in our simulation. i. Time span: 0-30secs;

ii. Path Trace:x = t, y = 3*sin(t)+t

iii. Velocity at each point:

1,

3cos()1 x

y t

=

=+

iv. Length of L1 = 5.0 , L2 = 4.0.

v. There is no constraint in the

3

ξdirection.

vi. The W matrix value is determined arbitrarily, which then determines the time constant for the asymptotically decreasing error function.

vii. k value determines the effect of the gradient added to the equation; here, k =1.

viii. The EE initial position of the Manipulator is located at (0,0), unless stated otherwise. Simulation plot (sample):

where:

Attached is the code that used for the simulation (with explaination).

1.Introduction

When a person writes across a board, he/she positions his/her arm in a comfortable writing configuration by moving his/her body rather than reaching out the arm.The same situation happens in many case such as people transporting a heavy object cooperatively. Therefore, when a mobile manipulator performs a manipulation task, it is desirable to bring the manipulator into certain preferred configurations by appropriately planning the motion of the mobile platform.

If the trajectory of the manipulator end point in a fixed coordinate system(world coordinate system) is known a priori, then the motion of the mobile platform can be planned accordingly. If the motion of the manipulator end point is unknown a priori, e.g., driven by a visual sensor or guided by a human operator, the path planning has to be made locally and in real time rather than globally and off line.

This report presents a control algorithm for the platform in the latter case, which takes the measured joint displacement of the manipulator as the input for motion planning and controls the platform to bring the manipulator into a preferred operating region. By using this algorithm, the mobile platform will be able to “understand the intention of its manipulator and respond accordingly.”

Since the mobile platform is subject to nonholonmic constraints, the control algorithm is developed using nonholonomic system theory.

2. Noholonomic Systems

Wheeled mobile robot which we will study in this report is typical examples of

mechanical systems with nonholomic constraints. Although navigation and planning of mobile robots have been investigated extensively over the past decade, the work on dynamic control of mobile robots with nonholonomic constraints is much more recent.

We consider mechanical systems that are subject to m velocity level equality type of nonholonomic constraints characterized by

0)(=q

q B (1)

where q is the n -dimensional generalized coordinates, B(q) is an m X n dimensional

matrix with m X n . Since the constraints are assumed to be nonholonomic, (1) is not integrable. It will be assumedthat these constraints are independent. In another words, B(q) has rank m . It is noted that most nonholonomic constraints encountered in mechanical systems, including rolling constraints, are in the form of (1).

Using the Lagrange multiplier rule, the equations of motion of nonholonomically constrained systems are governed by

n T

q B u q E q G q q V q

q M λ)()()(),()(+=++ (2) where M(q) is the n x n dimensional positive definite inertia matrix, ),(q

q V is the n -dimensional velocity-dependent force vector, G(q) is the the gravitational force vector, u is the r -dimensional vector of actuator force/torque, E(q) is the n x r dimensional matrix mapping the actuator space into the generalized coordinate space, and _n is an m -dimensional vector of Lagrange multipliers. It has been established that nonholonomic systems described by the motion equation (2) and constraint equation (1) have a canonical state space representation [1], [2]. The state vector consists of the generalized coordinates q and a set of pseudovelocities

][m

n v v v t ?=…21)(ν , where n is the number of the generalized coordinates and m

is the number of the constraints.

3. Formulation of the problem

A. Constraint Equation

According to the above section we could derive the constraint equation for the mobile platform. The platform has two driving wheels (the center ones) and four passive

supporting wheels (the comer ones). The two driving wheels are independently driven by the dc motors. The following notation will be used in the derivation of the constraint equations and dynamic equation.

There are three constraints.

The first one is the platform must move in the direction of the axis of symmetry, i.e.,

0sin cos =??φφφ d x y

c c (3) where (),c c y x are the coordinates of the center of mass c P in the worl

d coordinat

e system,

and φ is the heading angle of the platform measured from the X axis of the world

coordinateds. The other two constraints are the rolling constraints, i.e., the driving wheels do not slip,

r c c r b y x

θφφφ =++sin cos (4) l

c

c

r b y x

θφφφ =?+sin cos (5) where r r and θθ are the angular displacement of the right and left wheels, respectively.

Letting ),,,(,l r c c y x q θθφ=, the three constraints can be written in the form of

0)(=q

q A (6) which is according to the (1), where

??

??

?

????????????=r b r b d q A 0sin cos 0sin cos 00cos sin )(φφφφφφ (7) It can be shown that among the three constraints, two of them are nonholonomic and the other one is holonomic[3]. In principle, one can always eliminate variable using

holonomic constraints when deriving dynamic equations. Nevertheless, the elimination may be cumbersome in practice. To show the generality of the control method which is able to incorporate both holonomic and nonholonomic constraints, we treat both kinds of constraints in the same way; that is , we do not eliminate variables by using holonomic constraints.

B. Dynamic Equations

We now derive the dynamic equations for the mobile platform. The Lagrage equations of

motion of the platform with the Lagrange multiplier 321,,λλλ are given by

0cos )(sin )cos sin (3

212=+??+?φλλφλφφφφ d m x m c c (8)

0sin )(cos )sin cos (3

212=+?+?+φλλφλφφφφ d m y m c c (9)

0)()cos sin (2

31=?+?+??λλλφφφb d I y x d m c c c (10)

r

r w r I τλθ=+2 (11)

l

l w r I τλθ=+3 (12)

where

w c m m m 2+=

m w c I b d m I I 2)(222+++=

and l τ and r τ are the torques acting on the wheel axis generated by the right and left motors, respectively. These five equations of motion can be written in the vector form as

λτ)()(),()(q A q E q q V q

q M T ?=+ . (13)

This equation is a form of (2) which ignore the gravity element. The matrix )(q A T has

been defined in (7), and the matrices )(q M ,),(q

q V and )(q E are given by

???????????

??

??

?

??=w w c c c c I I I

d m d m d m m d m m q M 0000000000cos sin 00cos 000sin 0)(φφφφ

???????????????

???=000

sin cos ),(22φφφφ d m d m q q V c c , ???????

?

???

??

???=1001

000000

)(q E

Then, we will represent the motion equation (13) and the constraint equation (6) in state space by properly choosing a state vector. To do so, we define a 5 X 2 dimensional matrix )(q S such that 0)()(=q S q A . It is straightforward to verify that the following matrix has the required property

[]??

?

??

??

???????????++?==1001)cos sin ()cos sin ()sin cos ()sin cos ()()()(21c c

d b c d b c d b c d b c q s q s q S φφφφφφφφ

where the constant )2/(b r c =. From the constraint equation (6), q

is in the null space of )(q A . Because the two columns of )(q S are in the null space of )(q A and are linearly

independent, it is possible to express q

as a linear combination of the two columns of )(q S , that is,

υ)(q S q

= (14) The rational behind (14) is to introduce a set of independent velocity variables, υ. Owing to the choice of )(q S matrix, we have

??

????=??????=l r θθ

υυυ 21

Differentiating equation (14), substituting the expression for q

into (13) and premultiplying it by T S , we have

τυυ

=++))()((V t S M t MS S T (15)

Using the state-space vector []

T l

r l r c c T

T T y x q x ],,,,[,θθ

θθφυ ==, we will be able to represent the constraint and motion equations of the mobile platform in state space.

τυ???

??

?+??????=?12)(0MS S f S x T (16)

where ).

()(12V S MS S MS S f T T T ??=?υ This state equation can be further simplified to

u I S x

??

?

???+??????=00υ (17)

by applying the following nonlinear feedback

)(2f u MS S T ?=τ (18)

4. Simulation and Control Algorithm

A. Control Algorithm

According to (17) we could assume a new input u which could linearize the (16).

The problem is what is the u ’s form.

We could differentiate the output

[]T

r r y x q h y ==)( (19)

and with (14) we could have

υυυΦ===??=)()()/)((S J S J q q q h y

h h (20)

Φ is called decoupling matrix.

?

?

????ΦΦΦΦ

==Φ22211211

)()(q S q J h (21)

)sin )(cos )((11φφc r c r x d y b c +??=Φ )sin )(cos )((12φφc r c r x d y b c +++=Φ )cos )(sin )((21φφc r c r x d y b c ++?=Φ )cos )(sin )((22φφc r c r x d y b c +?+=Φ

Then we differentiate the (20) again, we will have

υυ

Φ+Φ=y substitute y

with v which is the linearized feedback

)(1υυυυΦ

?Φ=?Φ+Φ=Φ+Φ

=? v u u v (22)

so we could use the desired path d y to feed back the error y y e d ?=

)()(y y K y y K y v y d p d d d ?+?+== (23)

from (23) we know the v and then we can get the u and then x

, integral x

we can resolve this problem.

We can also present our idea like this

fig.1 or we could do this

fig.2

5.Simulation Result discussion

The mobile platform is initially directed toward positive X axis. Two different paths used for the simulation are shown in figure.3.

fig.3

1)case i) Straight line perpendicular to the X axis or the initial forward direction of

the mobile platform.