Advanced Strategies for Robot Manipulators

Advanced Strategies for Robot Manipulators edited by Seyed Ehsan Shafiei SCIYO Advanced Strategies for Robot Manipulat...

Author: Seyed Ehsan Shafiei

93 downloads 1605 Views 33MB Size Report

This content was uploaded by our users and we assume good faith they have the permission to share this book. If you own the copyright to this book and it is wrongfully on our website, we offer a simple DMCA procedure to remove your content from our site. Start by pressing the button below!
Report copyright / DMCA form

DOWNLOAD PDF

Advanced Strategies for Robot Manipulators edited by Seyed Ehsan Shafiei

SCIYO

Advanced Strategies for Robot Manipulators Edited by Seyed Ehsan Shafiei

Published by Sciyo Janeza Trdine 9, 51000 Rijeka, Croatia Copyright © 2010 Sciyo All chapters are Open Access articles distributed under the Creative Commons Non Commercial Share Alike Attribution 3.0 license, which permits to copy, distribute, transmit, and adapt the work in any medium, so long as the original work is properly cited. After this work has been published by Sciyo, authors have the right to republish it, in whole or part, in any publication of which they are the author, and to make other personal use of the work. Any republication, referencing or personal use of the work must explicitly identify the original source. Statements and opinions expressed in the chapters are these of the individual contributors and not necessarily those of the editors or publisher. No responsibility is accepted for the accuracy of information contained in the published articles. The publisher assumes no responsibility for any damage or injury to persons or property arising out of the use of any materials, instructions, methods or ideas contained in the book. Publishing Process Manager Jelena Marusic Technical Editor Teodora Smiljanic Cover Designer Martina Sirotic Image Copyright higyou, 2010. Used under license from Shutterstock.com First published September 2010 Printed in India A free online edition of this book is available at www.sciyo.com Additional hard copies can be obtained from [email protected]

Advanced Strategies for Robot Manipulators, Edited by Seyed Ehsan Shafiei p. cm. ISBN 978-953-307-099-5

SCIYO.COM WHERE KNOWLEDGE IS FREE

free online editions of Sciyo Books, Journals and Videos can be found at www.sciyo.com

Contents Preface

IX

Chapter 1 The Comparative Assessment of Modelling and Control of Mechanical Manipulator 1 M. H. Korayem, H. N. Rahimi and A. Nikoobin Chapter 2 Hyper Redundant Manipulators 27 Ivanescu Mircea and Cojocaru Dorian Chapter 3 Micro-Manipulator for Neurosurgical Application M. R. Arshad and Ya’akob Yusof

61

Chapter 4 DeLiA: A New Family of Redundant Robot Manipulators Jaime Gallardo–Alvarado

73

Chapter 5 Dynamic Modelling, Tracking Control and Simulation Results of a Novel Underactuated Wheeled Manipulator (WAcrobot) Mohsen Moradi Dalvand and Bijan Shirinzadeh

91

Chapter 6 Kinematics Synthesis of a New Generation of Rapid Linear Actuators for High Velocity Robotics with Improved Performance Based on Parallel Architecture 107 Luc Rolland Chapter 7 Sliding Mode Control of Robot Manipulators via Intelligent Approaches 135 Seyed Ehsan Shafiei Chapter 8 Supervision and Control Strategies of a 6 DOF Parallel Manipulator Using a Mechatronic Approach 173 FJoão Mauricio Rosário, Didier Dumur, Mariana Moretti, Fabian Lara and Alvaro Uribe Chapter 9 Collision Detection and Control of Parallel-Structured Flexible Manipulators Based on Unscented Kalman Filter 197 Yuichi Sawada, YusukeWatanabe and Junki Kondo Chapter 10 On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 217 Jorge Orrante-Sakanassi, Víctor Santibáñez and Ricardo Campa

VI

Chapter 11 Real-Time-Position Prediction Algorithm for Under-actuated Robot Manipulator Using of Artificial Neural Network 249 Ahmad Azlan Mat Isa, Hayder M.A.A. Al-Assadi and Ali T. Hasan Chapter 12 On Nonlinear Control Perspectives of a Challenging Benchmark Guangyu Liu and Yanxin Zhang

261

Chapter 13 A Unified Approach to Robust Control of Flexible Mechanical Systems Using H∞ Control Powered by PD Control 273 Masayoshi Toda Chapter 14 An Improved Adaptive Kinematics Jacobian Trajectory Tracking of a Serial Robot Passing Through Singular Configurations 287 Ali T. Hasan, Hayder M.A.A. Al-Assadi and Ahmad Azlan Mat Isa Chapter 15 Development of Fuzzy-logic-based Self Tuning PI Controller for Servomotor Oyas Wahyunggoro and Nordin Saad

311

Chapter 16 Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 329 Gerasimos G. Rigatos Chapter 17 Design and Control of a Compact Laparoscope Manipulator: A Biologically Inspired Approach 365 Atsushi Nishikawa, Kazuhiro Taniguchi, Mitsugu Sekimoto, Yasuo Yamada, Norikatsu Miyoshi, Shuji Takiguchi, Yuichiro Doki, Masaki Mori and Fumio Miyazaki Chapter 18 Open Software Architecture for Advanced Control of Robotic Manipulators 381 J. Gomez Ortega, J. Gamez García, L. M. Nieto Nieto and A. Sánchez García Chapter 19 Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 397 Y. Cao, Y. W. Li and Z. Huang

Preface Robotic technology has grown beyond the boundaries of imagination during recent decades. Nowadays, it’s not very surprising to see that a robot can hear, see and even talk and a servant robot is not a dream anymore. But now we confront newer challenges such as nano-robots, surgical manipulators and even robots who can make decisions which are employed for underwater or space missions. Amongst the robotic systems, robot manipulators have proven themselves to be of increasing importance and are widely adopted to substitute humans in repetitive and/or hazardous tasks. Modern manipulators have a complicated design and need to do more precise, crucial and critical tasks. So, the simple traditional control methods cannot be efficient, and advanced control strategies with considering special constraints need to be established. In spite of the fact that groundbreaking researches have been carried out in this realm until now, there are still many novel aspects which have to be explored. This book consists of a set of materials that introduces various strategies related to robot manipulators. Although the topics provided here are not in a rational order, they can be divided into three major subjects such as design and modelling, control strategies and applications of robot manipulators. These subjects cover different approaches like dynamic modelling, redundant manipulators, micro-manipulator, parallel manipulator, nonlinear control, intelligent control and many other valuable matters that are addressed here by different authors through 19 chapters. I gratefully acknowledge the contributions made by each of my coauthors. They showed enthusiasm to contribute their knowledge that lead to creation of this book. However, this is not a text book for academic education, the book is addressed to graduate students as well as researchers in the field and I am sure they can benefit from its multidisciplinary chapters. Editor Seyed Ehsan Shafiei Shahrood University of Technology Iran

1 The Comparative Assessment of Modelling and Control of Mechanical Manipulator M. H. Korayem, H. N. Rahimi and A. Nikoobin

Robotic Research Lab, College of Mechanical Engineering, Iran University of Science and Technology Iran 1. Introduction 1.1 Overview In this book chapter a comparative assessment of modelling and control of mechanical manipulator is considered. First, kinematic and dynamic modelling of wide range of mechanical manipulators comprising flexible link, flexible joint and mobile manipulators are considered. Then, open-loop optimal control problem is formulated to control of the obtained system. Finally, some applications of method including motion planning and maximum payload determination are illustrated through the computer simulations. 1.2 Problem statement Mechanical flexibilities can be classified into two categories: Link flexibility and joint flexibility. Link flexibility is a result of applying lightweight structure in manipulator arms designed to increase the productivity by fast motion and to complete a motion with small energy requirement. Joint flexibility arises from elastic behavior of the drive transmission systems such as transmission belts, gears and shafts. Mobile manipulators are combined systems consists of a robotic manipulator mounted on a mobile platform. Such systems are able to accomplish complicated tasks in large workspaces. In particular the greatest disadvantage of mobile robotic manipulators is that most of these systems are powered on board with limited capacity. Hence, incorporating light links can minimize the inertia and gravity effects on links and actuators and it results to decrease the energy consumption in the same motion. Hence, lightweight systems have primary importance in design and manufacturing stages of mobile manipulators. 1.3 Motivation Unfortunately, reviewing of the recent literature on modelling and optimization of flexible and mobile manipulators shows that a very scant attention has been paid to study of model that describes both link and joint flexibility, particularly for mobile manipulators. The main motivation for this study is to present a comprehensive modelling and optimal control of flexible link-joint mechanical mobile manipulators. It can provide an inclusive reference for other researchers with comparative assessment view in the future studies.

2

Advanced Strategies for Robot Manipulators

1.4 Prior work Analyzing of nonlinear dynamic motion of elastic manipulators is a very complex task that plays a crucial role in design and application of such robots in task space. This complexity arises from very lengthy, fluctuating and highly nonlinear and coupled set of dynamic equations due to the flexible nature of both links and joints. The original dynamics of robotic manipulators with elastic arms, being described by nonlinear coupled partial differential equations. They are continuous nonlinear dynamical systems distinguished by an infinite number of degrees of freedom. The exact solution of such systems does not exist. However, most commonly the dynamic equations are truncated to some finite dimensional models with either the assumed modes method (AMM) or the finite element method (FEM). The assumed mode expansion method was used to derive the dynamic equation of the flexible manipulator (Sasiadek & Green, 2004). Dynamic modelling technique for a manipulator with multiple flexible links and flexible joints was presented based on a combined Euler–Lagrange formulation and assumed modes method (Subudhi & Morris, 2002). Then, control of such system was carried out by formulating a singularly perturbed model and using it to design a reduced-order controller. Combined Euler–Lagrange formulation and assumed modes method was used for driving the equation of motions of flexible mobile manipulators with considering the simply support mode shape and one mode per link (Korayem & Rahimi Nohooji, 2008). Then, open-loop optimal control method was proposed to trajectory optimization of flexible link mobile manipulator for a given twoend-point task in point-to-point motion. In finite element method, the elastic deformations are analyzed by assuming a known rigid body motion and later superposing the elastic deformation with the rigid body motion (Usoro et al. 1986). One of the main advantages of FEM over the most of other approximate solution methods to modelling the flexible links is the fact that in FEM the connection are supposed to be clamp-free with minimum two mode shapes per each link (Korayem et al. 2009(a)). This ensures to achieve the results that display the nonlinearity of the system properly. The Timoshenko beam theory and the finite element method was employed to drive the dynamic equation of flexible link planar cooperative manipulators in absolute coordinates (Zhang & Yu, 2004). Dynamic model of a single-link flexible manipulator was derived using FEM and then studied the feed-forward control strategies for controlling the vibration (Mohamed & Tokhi, 2004). Finite element method was used for describing the dynamics of the system and computed the maximum payload of kinematically redundant flexible manipulators (Yue et al., 2001). Then, the problem was formulated for finding the optimal trajectory and maximum dynamic payload for a given point-to-point task. Finally, numerically simulation was carried out for a planar flexible robot manipulator to validate the research work. The review of the recent literature shows that extensive research has been addressed the elastic joints robotic arms (Korayem et al. 2009(b)). However, there is only limited research works have been reported on a comprehensive model that describes both link and joint elasticity (Rahimi et al. 2009). Moreover, in almost all cases, linearized models of the link flexibility are considered which reduced the complexity of the model based controller (Chen, 2001).

The Comparative Assessment of Modelling and Control of Mechanical Manipulator

3

Mobile manipulators have recently received considerable attention with wide range of applications mainly due to their extended workspace and their ability to reach targets that are initially outside of the manipulator reach. A comprehensive literature survey on mobile manipulator systems can be found (Bloch, 2003). A host of issues related to mobile manipulators have been studied in the past two decade. These include for example: dynamic and static stability (Papadopoulos & Rey, 1996), force development and application (Papadopoulos & Gonthier, 1999), maximum payload determination (Korayem & Ghariblu, 2004). However, a vast number of research publications that deal with the mobile manipulators focus on techniques for trajectory planning of such robots (Korayem & Rahimi Nohooji, 2008). Motion planning for mobile manipulators is concerned with obtaining open-loop or closeloop controls. It steers a platform and its accompanying manipulator from an initial state to a final one, without violating the nonholonomic constraints (Sheng & Qun, 2006). In most studies of trajectory planning for mobile manipulators the end effector trajectory is specified and the optimal motion planning of the base is considered (Mohri et al., 2001), or integrated motion planning of the base and the end effector is carried out (Papadopoulos, et al., 2002). However, because of designing limitation or environmental obstacle in majority of practical application of mobile manipulators especially in repetitive applications, the platform must follow a specified pose trajectory. In this case, designer must control the joint motions to achieve the best dynamic coordination that optimize the defined cost function such as energy consumption, actuating torques, traveling time or bounding the velocity magnitudes. Applications for such systems abound in mining, construction or in industrial factories. Optimal control problems can be solved with direct and indirect techniques. In the direct method at first the control and state variables are discretized and the optimal control problem is transcribed into a large, constrained and often sparse nonlinear programming problem, then, the resulting nonlinear programming problem is treated by standard algorithm like interior point methods (Wachter & Biegler, 2006). Famous realizations of direct methods are direct shooting methods (Bock & Plitt, 1984) or direct collocation methods (Hargraves & Paris, 1987). However, direct methods are not yield to exact results. They are exhaustively time consuming and quite inefficient due to the large number of parameters involved. Consequently, when the solution of highly complex problems such as the structural analysis of optimal control problems in robotics is required, the indirect method is a more suitable candidate. This method is widely used as an accurate and powerful tool in analyzing of the nonlinear systems. The indirect method is characterized by a ''first optimize, then discretize'' strategy. Hence, the problem of optimal control is first transformed into a piecewise defined multipoint boundary value problem, which contains the full mathematical information about the respective optimal control problem. In the following step, this boundary value problem is discretized to achieve the numerical solution (Sentinella & Casalino, 2006). It is well known that this technique is conceptually fertile, and has given rise to far-reaching mathematical developments in the wide ranges of optimal dynamic motion planning problems. For example, it is employed in the path planning of flexible manipulators (Rahimi et.al, 2009), for the actuated kinematic chains (Bessonnet & Chessé, 2005) and for a large multibody system (Bertolazzi et al., 2005). A survey on this method is found in (Callies & Rentrop, 2008).

4


1.5 Layout The balance of the remaining of the chapter is organized as follows. Section 2 provides background information about kinematic and dynamic analysis of the flexible mobile robotic manipulators. Hence, assumed mode and finite element methods are introduced and formulated to dynamic modelling of flexible link manipulators. Then, the flexible model is completed by adding the joint flexibility. After that, formulation is extended to comprise the mobile manipulators. Section 3 consists of a brief review of converting the problem from optimal control to optimization procedure with implementing of Pontryagin’s minimum principle. some application examples with the two links flexible mobile manipulator is detailed in this section. Finally, the concluding remarks with a brief summary of the chapter is presented in the last section.

2. System modelling 2.1 Kinematic analysis A mobile manipulator consisting of differentially driven vehicle with n flexible links and n flexible revolute joints is expressed in this section (Fig. 1). The links are cascaded in a serial fashion and are actuated by rotors and hubs with individual motors. The flexible joints are dynamically simplified as a linear torsional springs that works as a connector between the rotors and the links. A concentrated payload of mass mp is connected to the distal link. θn

...

θi

Link-1 θ2

θ0

Y r0

...

(FJ)2

r1

ri

Link-i

Payload

rpayload

Link-n rn

( FJ) n

( FJ) i +1

( FJ) i

θ1

( FJ)1

X Z Fig. 1. A schematic view of a multiple flexible links – joints mobile manipulator The following assumptions are made for the development of a dynamic model of the system. • Each link is assumed to be long and slender. • The motion of each link and its deformation is supposed to be in the horizontal plane. • Links are considered to have constant cross-sectional area and uniform material properties.

The Comparative Assessment of Modelling and Control of Mechanical Manipulator

5

• The inertia of payload is neglected. • The backlash in the reduction gear and coulomb friction effects are neglected. • It is assumed that the mobile base does not slide. The generalized coordinates of the flexible links/joints mobile manipulator consist of four parts, the generalized coordinates defining the mobile base motion qb = (qb 1 , qb 2 ,… , qbnb )T , the generalized coordinates of the rigid body motion of links qr = (q1 , q2 ,… , qn )T and the generalized coordinates that related to the flexibility of the links q f = (q11 , q12 ,…, q1n f , q21 ,…, q2 n f ,…, qn1 ,…, qnn f )T , and the generalized coordinate corresponding to the flexibility of joints q j = (q1 + n , q2 + n ,… , qn + n )T .Where n, nb and n f are number of links, base degrees of freedom and manipulator mode shapes, respectively. The notion of redundancy expresses that the number of generalized coordinates (v) is strictly greater than the (global) degree of freedom (d). Thus, the mechanical system is redundant if d ws[2] > ··· > ws[N]. A random numbers generator is evoked and the resulting numbers ui:N~U[0,1] fall in the partitions of the interval [0,1]. The width of these partitions is wi and thus a redistribution of the particles is generated. For instance, in a wide partition of width wj will be assigned more particles than to a narrow partition of witdh wm. A detailed analysis on the tuning of the resampling procedure in Particle Fitlering has been given in (Rigatos 2009a). 4.2 Distributed Particle Filtering for state estimation fusion The Distributed Particle Filter performs fusion of the state vector estimates which are provided by the local Particle Filters. This is succeeded by fusing the discrete probability

348


density functions of the local Particle Filters into a common probability distribution of the system’s state vector. Without loss of generality fusion between two estimates which are provided by two different probabilistic estimators (particle filters) is assumed. This amounts to a multiplication and a division operation to remove the common information, and is given by (Ong et al. 2008), (Ong et al. 2006) p( x( k )|ZA ∪ZB ) ∝

p( x( k )|ZA )p( x( k )|ZB ) p( x( k )|ZA ∩ZB )

(68)

where ZA is the sequence of measurements associated with the i-th processing unit and ZB is the sequence of measurements associated with the j-th measurement unit. In the implementation of distributed particle filtering, the following issues arise: 1. Particles from one particle set (which correspond to a local particle filter) do not have the same support (do not cover the same area and points on the samples space) as particles from another particle set (which are associated with another particle filter). Therefore a point-to-point application of Eq. (68) is not possible. 2. The communication of particles representation (i.e. local particle sets and associated weight sets) requires significantly more bandwidth compared to other representations, such as Gaussian mixtures. Fusion of the estimates provided by the local particle filters (located at different processing units) can be performed through the following stages. First, the discrete particle set of Particle Filter A (Particle Filter B) is transformed into a continuous distribution by placing a Gaussian kernel over each sample (Fig. 7) (Musso et al. 2001) Kh(x) = h2K(x)

(69)

where K() is the rescaled Kernel density and h > 0 is the scaling parameter. Then the continuous distribution A (B) is sampled with the other particles set B (A) to obtain the new importance weights, so that the weighted sample corresponds to the numerator of Eq. (68) (Fig. 8). Such a conversion from a discrete particle probability distribution functions

∑

N

w(Ai )δ ( x(Ai ) ) ( ∑ i =1wB( i )δ ( x(Bi ) ) ) into continuous distributions is denoted as N

i =1

N

∑w i =1

N

δ ( x(Ai ) ) → p A ( x ) ( ∑wB( i )δ ( xB( i ) ) → pB ( x ))

(i ) A

(70)

i =1

The common information appearing in the processing units A and B should not be taken into account in the joint probability distribution which is created after fusing the local probability densities of A and B. This means that in the joint p.d.f. one should sample with importance weights calculated according to Eq. (68). The objective is then to create an importance sampling approximation for the joint distribution that will be in accordance to Eq. (68). A solution to this can be obtained through Monte Carlo sampling and suitable selection of the so-called ”proposal distribution” (Ong et al. 2008), (Ong et al. 2006)] According to the above, for the joint distribution the idea behind Monte Carlo sampling is to draw N i.i.d samples from the associated probability density function p(x), such that the target density is approximated by a point-mass function of the form p( x )

N

∑w i =1

δ ( x(ki ) )

(i ) k

(71)

Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs

349

Fig. 7. Conversion of the particles discrete probability density function to a continuous distribution, after allocating a Gaussian kernel over each particle where δ ( x (ki ) ) is a Dirac delta mass located at x(ki ) . Then the expectation of some function f (x) with respect to the pdf p(x) is given by I ( f ) = Ep ( x ) [ f ( x )] = ∫ f ( x )p( x )dx

(72)

the Monte-Carlo approximation of the integral with samples is then IN ( f ) =

1 N ∑ f (x( i ) ) N i =1

(73)

where x(i) p(X) and IN( f )→I( f ) for N→∞. since, the true probability distribution p(x) is hard to sample from, the concept of importance sampling is to select a proposal distribution p( x ) in place of p(x), with the assumption that p( x ) includes the support space of p(x). Then the expectation of function f (x), previously given in Eq. (72), is now calculated as I( f ) = ∫ f (x)

p( x ) p( x )dx = ∫ f ( x )w( x )p( x )dx p( x )

(74)

where w(x) are the importance weights w( x ) =

p( x ) p( x )

(75)

Then the Monte-Carlo estimation of the mean value of function f (x) becomes N

I N ( f ) = ∑ f ( x( i ) )w( x( i ) ) i =1

For the division operation, the desired probability distribution is

(76)

350


p( x( i ) ) =

p A ( x( i ) ) pB ( x ( i ) )

(77)

In that case the important weights of the fused probability density functions become w( x( i ) ) =

pA ( x( i ) ) pB ( x ( i ) ) p ( x ( i ) )

(78)

which is then normalized so that ∑ i =1w( x( i ) ) = 1 / N , where N is the number of particles. The next step is to decide what will be the form of the proposal distribution p( x ) . A first option is to take p( x ) to be a uniform distribution, with a support that covers both of the support sets of the distributions A and B. N

p( x ) = U( x )

(79)

Then the sample weights p( x( i ) ) are all equal at a constant of value C. Hence the importance weights are w( x( i ) ) =

pA ( x( i ) ) pB ( x( i ) )C

(80)

Another suitable proposal distribution that takes more into account the new information re ceived (described as the probability distribution of the second processing unit) is given by p ( x ) = pB ( x )

(81)

and the important weights are then adjusted to be w( x( i ) ) =

p A ( x( i ) ) p B ( x ( i ) )2

(82)

5. Nonlinear control for autonomous UAV navigation 5.1 Kinematic model of the UAV For the design of the autonomous navigation system of the UAVs a suitable control scheme has to be chosen. In this control loop there will be processing of the estimated UAV state vector, as obtained through the distributed filtering algorithms which were presented in Sections 2 to 4. To this end, the kinematic model the kinematic model of the UAVs has to be analyzed first. Based on this kinematic model a flatness-based controller will be derived. The UAV dynamics suggest the following structure for constant altitude manoeuvres (Léchevin & Rabbath 2006):

x = vcos(θ ), y = vsin(θ ), θ = u1 v = u2 , h = 0

(83)

where (x,y) is the desired inertial position of the UAV, θ is the UAV’s heading, v is the UAV’s velocity, h is the UAV’s attitude, and u1, u2 are constrained by the dynamic capability of the UAVs namely the heading rate constraint and the acceleration constraint respectively.

351


pA(x)

0.04 0.02 0

0

2

4

6

8

10

6

8

10

6

8

10

x

pB(x)

0.04 0.02 0

0

2

4 x

p(x)

0.04 0.02 0

0

2

4 x

Fig. 8. Fusion of the probability density functions produced by the local particle filters An inertial measurement unit (IMU) of a UAV usually consists of a three axis gyroscope and a three axis accelerometer. A vision sensor can be also mounted underneath the body of the UAV and is used to extract points of interest in the environment. The UAV also carries a barometric pressure sensor for aiding of the platform attitude estimation. A GPS sensor, can be also mounted on the board. The sensor data is filtered and fused to obtain estimates of the desired entities such as platform and feature position (Vissière et al. 2008). 5.2 Differential flatness for finite dimensional systems Flatness-based control is proposed for steering the UAV along a desirable trajectory (Oriolo et al. 2002), (Villagra et al. 2007), (Fliess et al. 1999). The main principles of flatness- based control are as follows: A finite dimensional system is considered. This can be written in the general form of an ODE, i.e.

Si ( w , w , w ,

, w i ), i = 1, 2,

,q

(84)

The quantity w denotes the system variable while wi, i = 1, 2, ··· , q are its derivatives (these and can be for instance the elements of the system’s state vector). The system of Eq. (1) is said to be differentially flat if there exists a collection of m functions y = (y1, ··· ,ym) of the system variables wi, i = 1, ··· , s and of their time-derivatives, i.e. yi = φ ( w , w , w ,

α

, w i ), i = 1,

,m

(85)

such that the following two conditions are satisfied (Fliess et al. 1999), (Rigatos 2008): 1. There does not exist any differential relation of the form R( y , y ,

, yβ ) = 0

(86)

352

2.


which implies that the derivatives of the flat output are not coupled in the sense of an ODE, or equivalently it can be said that the flat output is differentially independent. All system variables, i.e. the components of w (elements of the system’s state vectors) can be expressed using only the flat output y and its time derivatives wi = ψi ( y , y ,

γ

, y i ), i = 1,

(87)

,s

An equivalent definition of differentially flat systems is as follows: Definition: The system x = f ( x , u) , x∈Rn, u∈Rm is differentially flat if there exist relations h : Rn×Rm→Rm, φ: (Rm)r→Rn and ψ: (Rm)r+1→Rm, such that y = h(x , u, u,

, u( r ) ) , x = φ( y , y , , y( r −1) )

and u = ψ( y , y ,..., y ( r − 1) , y ( r ) ) . This means that all system dynamics can be expressed as a function of the flat output and its derivatives, therefore the state vector and the control input can be written as x( y ) = φ ( y(t ), y(t ),..., y ( r ) (t )) and u = ψ( y(t ), y(t ),..., y ( r ) (t )) . It is noted that for linear systems the property of differential flatness is equivalent to that of controllability. 5.3 Differential flatness of the UAV kinematic model It is assumed that the helicopter-like UAV, performs manoeuvres at a constant altitude. Then, from Eq. (83) one can obtain the following description for the UAV kinematics x = vcos(θ ), y = vsin(θ ), θ =

v tan(φ ) l

(88)

where using the analogous of the unicycle robot v is the velocity of the UAV, l is the UAV’s length, θ is the UAV’s orientation (angle between the transversal axis of the UAV and axis OX), and φ is a steering angle. The flat output is the cartesian position of the UAV’s center of gravity, denoted as η = (x,y) , while the other model parameters can be written as: v=± η

⎛ cos(θ ) ⎞ η ⎜ ⎟= ⎝ sin(θ ) ⎠ v

tan(φ ) = ldet(ηη ) / v 3

(89)

These formulas show simply that θ is the tangent angle of the curve traced by P and tan(φ) is the associated curvature. With reference to a generic driftless nonlinear system q , q ∈ Rn , w ∈ Rm

(90)

dynamic feedback linearization consists in finding a feedback compensator of the form

ξ = α (q , ξ ) + b(q , ξ )u w = c(q , ξ ) + d(q , ξ )u

(91)

with state ξ ∈ Rv and input u ∈ Rm, such that the closed-loop system of Eq. (90) and Eq. (91) is equivalent under a state transformation z = T(q, ξ) to a linear system. The starting point is the definition of a m-dimensional output η = h(q) to which a desired behavior can be assigned. One then proceeds by successively differentiating the output until the input appears in a non-singular way. If the sum of the output differentiation orders equals the dimension n + v of the extended state space, full input-state-output linearization is obtained


353

(In this case η is also called a flat output). The closed-loop system is then equivalent to a set of decoupled input-output chains of integrators from ui to ηi. The exact linearization procedure is illustrated for the unicycle model of Eq. (21). As flat output the coordinates of the center of gravity of the vehicle is considered η = (x,y). Differentiation with respect to time then yields (Oriolo et al. 2002), (Rigatos 2008) ⎛ cos(θ ) 0 ⎞ ⎛ v ⎞ ⎟⋅⎜ ⎟ ⎝ sin(θ ) 0 ⎠ ⎝ ω ⎠

⎛x⎞ ⎝y⎠

η =⎜ ⎟=⎜

(92)

showing that only v affects η , while the angular velocity ω cannot be recovered from this first-order differential information. To proceed, one needs to add an integrator (whose state is denoted by ξ) on the linear velocity input ⎛ cos(θ ) ⎞ v = ξ , ξ =α ⇒η = ξ ⎜ ⎟ ⎝ sin(θ ) ⎠

(93)

where α denotes the linear acceleration of the UAV. Differentiating further one obtains ⎛ cos(θ ) ⎞ ⎛ sin(θ ) ⎞ ⎛ cos(θ ) −ξ sin(θ ) ⎞⎛ α ⎞ ⎟ + ξθ ⎜ ⎟=⎜ ⎟⎜ ⎟ θ sin ( ) ⎝ ⎠ ⎝ cos(θ ) ⎠ ⎝ sin(θ ) ξ cos(θ ) ⎠⎝ ω ⎠

η =ξ⎜

(94)

and the matrix multiplying the modified input (α,ω) is nonsingular if ξ ≠ 0. Under this assumption one defines

η is denoted as

⎛ α ⎞ ⎛ cos(θ ) −ξ sin(θ ) ⎞ ⎛ u1 ⎞ ⎜ ⎟=⎜ ⎟⋅⎜ ⎟ ⎝ ω ⎠ ⎝ sin(θ ) ξ cos(θ ) ⎠ ⎝ u2 ⎠

⎛ η1 ⎞ ⎛ u1 ⎞ ⎟=⎜ ⎟=u ⎝η 2 ⎠ ⎝ u2 ⎠

η =⎜

(95)

(96)

which means that the desirable linear acceleration and the desirable angular velocity can be expressed using the transformed control inputs u1 and u2. Then, the resulting dynamic compensator is (return to the initial control inputs v and ω)

ξ = u1cos(θ ) + u2 sin(θ ) v=ξ u 2 cos(θ ) − u1 sin(θ ) ω= ξ

(97)

Being ξ∈R, it is n + v = 3 + 1 = 4, equal to the output differentiation order in Eq. (29). In the new coordinates z1 = x z2 = y z 3 = x = ξ cos(θ ) z 4 = y = ξ sin(θ )

(98)

354


The extended system is thus fully linearized and described by the chains of integrators, in Eq. (29), and can be rewritten as z1 = u1 z2 = u2

(99)

The dynamic compensator of Eq. (97) has a potential singularity at ξ = v = 0, i.e. when the UAV is not moving, which is a case never met when the UAV is in flight. It is noted however, that the occurrence of such a singularity is structural for non-holonomic systems. In general, this difficulty must be obviously taken into account when designing control laws on the equivalent linear model. A nonlinear controller for output trajectory tracking, based on dynamic feedback linearization, is easily derived. Assume that the UAV must follow a smooth trajectory 1

(xd(t),yd(t)) which is persistent, i.e. for which the nominal velocity vd = ( xd2 + y d2 ) 2 along the trajectory never goes to zeros (and thus singularities are avoided). On the equivalent and decoupled system of Eq. (32), one can easily design an exponentially stabilizing feedback for the desired trajectory, which has the form u1 = xd + k p ( xd − x ) + kd ( xd − x ) 1

1

u2 = y d + k p ( y d − y ) + kd ( y d − y ) 1

(100)

1

and which results in the following error dynamics for the closed-loop system ex + k d ex + k p ex = 0 1

1

ey + kd ey + k p ey = 0 2

(101)

2

where ex = x − xd and ey = y − yd. The proportional-derivative (PD) gains are chosen as kp1 > 0 and kd1 > 0 for i = 1, 2. Knowing the control inputs u1, u2, for the linearized system one can calculate the control inputs v and ω applied to the UAV, using Eq. (91). The above result is valid, provided that the dynamic feedback compensator does not meet the singularity. In the general case of design of flatness-based controllers, the following theorem assures the avoidance of singularities in the proposed control law (Oriolo et al. 2002): Theorem: Let λ11, λ12 and λ21, λ22, be respectively the eigenvalues of two equations of the error dynamics, given in Eq. (91). Assume that, for i = 1,2 it is λ11 < λ12 < 0 (negative real eigenvalues), and that λi2 is sufficiently small. If 0 ⎛ xd (t ) ⎞ ⎛ εx ⎞ min t ≥ 0 ⎜ ⎟ ≥ ⎜⎜ 0 ⎟⎟ ⎝ y d (t ) ⎠ ⎝ εy ⎠

(102)

with εx0 = εx (0) ≠ 0 and εy0 = εy (0) ≠ 0 , then the singularity ξ = 0 is never met.

6. Simulation tests 6.1 Autonomous UAV navigation with Extended Information Filtering It was assumed that m = 2 helicopter models were monitored by n = 2 different ground stations. At each ground station an Extended Kalman Filter was used to track each UAV. By


355

fusing the measurements provided by the sensors mounted on each UAV, each local EKF was able to produce an estimation of a UAV’s motion. Next, the state estimates obtained by the pair local EKFs associated with each UAV were fused with the use of the Extended Information Filter. This fusion-based state estimation scheme is depicted in Fig. 2. As explained in Section 2 the weighting of the state estimates of the local EKFs was performed using the local information matrices. The distributed fitering architecture is shown in Fig. 9.

Fig. 9. Distributed Filtering over WSN Next, some details will be given about the local EKF design for the UAV model of Eq. (88). The UAV’s continuous-time kinematic equation is: x(t ) = v(t )cos(θ (t )), y(t ) = v(t )sin(θ (t )), θ (t ) = ω (t )

(103)

The IMU system provides measurements or the UAV’s position [x,y] and the UAV’s orientation angle θ over a sampling period T. These sensors are used to obtain an estimation of the displacement and the angular velocity of the UAV v(t) and ω(t), respectively. The IMU sensors can introduce incremental errors, which result in an erroneous estimation of the orientation θ. To improve the accuracy of the UAV’s localization, measurements from the GPS (or visual sensors) can be used. On the other hand, the GPS on this own is not always reliable since its signal can be intermittent. Therefore, to succeed accurate localization of the UAV it is necessary to fuse the GPS measurements with the IMU measurements of the UAV or with measurements from visual sensors (visual odometry). The inertial coordinates system OXY is defined. Furthermore the coordinates system O′X′Y′ is considered (Fig. 10). O′X′Y′ results from OXY if it is rotated by an angle θ. The coordinates of the center of symmetry of the UAV with respect to OXY are (x,y), while the coordinates of

356


Fig. 10. Reference frames for the UAV the GPS or visual sensor that is mounted on the UAV, with respect to O′X′Y′ are x′i , y′i . The orientation of the GPS (or visual sensor) with respect to OX′Y′ is θ i′ . Thus the coordinates of the GPS or visual sensor with respect to OXY are (xi,yi) and its orientation is θi, and are given by: xi ( k ) = x( k ) + x′i sin(θ ( k )) + y′i cos(θ ( k )) y i ( k ) = y( k ) − x′icos(θ ( k )) + y′i sin(θ ( k ))

(104)

θi ( k ) = θ ( k ) + θi For manoeuvres at constant altitude the GPS measurement (or the visual sensor measurement) can be considered as the measurement of the distance from a reference surface P j. A reference surface P j in the UAVs 2D flight area can be represented by Prj and Pnj , where (i) Prj is the normal distance of the plane from the origin O, (ii) Pnj is the angle between the normal line to the plane and the x-direction. The GPS sensor (or visual sensor i) is at position xi(k),yi(k) with respect to the inertial coordinates system OXY and its orientation is θi(k). Using the above notation, the distance of the GPS (or visual sensor i), from the plane P j is represented by Prj , Pnj (see Fig. 10): dij ( k ) = Prj − xi ( k )cos( Pnj ) − y i ( k )sin( Pnj )

(105)

Assuming a constant sampling period Δtk = T the measurement equation is z(k + 1) = γ(x(k)) + v(k), where z(k) is the vector containing GPS (or visual sensor) and IMU measures and v(k) is a white noise sequence ~N(0,R(kT)). By definition of the measurement vector one has that the output function is γ(x(k)) = [x(k),y(k), θ(k),d1(k)]T. The UAV state is [x(k),y(k), θ(k)]T and the control input is denoted by


357

U(k) = [v(k),ω(k)]T. To obtain the Extended Kalman Filter (EKF), the kinematic model of the UAV is linearized about the estimates xˆ ( k ) and xˆ − ( k ) the control input U(k − 1) is applied. The measurement update of the EKF is K( k ) = P − ( k ) JγT ( xˆ − ( k ))[ Jγ ( xˆ − ( k ))P − ( k ) JγT ( xˆ − ( k )) + R( k )]−1 xˆ ( k ) = xˆ − ( k ) + K ( k )[ z( k ) − γ ( xˆ − ( k ))] P( k ) = P − ( k ) − K ( k ) JγT P − ( k ) The time update of the EKF is P − ( k + 1) = Jφ ( xˆ ( k ))P( k ) JφT ( xˆ ( k )) + Q( k ) xˆ − ( k + 1) = φ ( xˆ ( k )) + L( k )U ( k ) ⎛ Tcos(θ ( k )) 0 ⎞ ⎛ 1 0 − v( k )sin(θ )T ⎞ ⎜ ⎟ ⎜ ⎟ where L( k ) = ⎜ Tsin(θ ( k )) 0 ⎟ and Jφ ( xˆ ( k )) = ⎜ 0 1 − v( k )cos(θ )T ⎟ ⎜ ⎜0 0 ⎟ 0 T ⎟⎠ 1 ⎝ ⎝ ⎠

(106)

while Q(k) = diag[σ2(k),σ2(k),σ2(k)], with σ2(k) chosen to be 10−3 and φ ( xˆ ( k )) = [ xˆ ( k ), yˆ ( k ),θˆ( k )]T ,

γ ( xˆ ( k )) = [ xˆ ( k ), yˆ ( k ),θˆ( k ), d( k )]T , i.e. xˆ ( k ) ⎛ ⎞ ⎜ ⎟ ˆ ( ) y k ⎟ γ ( xˆ ( k )) = ⎜⎜ ⎟ θˆ( k ) ⎜ j ⎟ j j ⎜ P − x ( k ))cos( P ) − y ( k )sin( P ) ⎟ i n i n ⎠ ⎝ r

(107)

In the calculation of the observation equation Jacobian one gets

1 0 0 ⎛ ⎞ ⎜ ⎟ 0 1 0 ⎟ JγT ( xˆ − ( k )) = ⎜ ⎜ ⎟ 0 0 1 ⎜⎜ ⎟⎟ j j j j ′ ′ ⎝ −cos( Pn ) −sin( Pn ) { xi cos(θ − Pn ) − yi sin(θ − Pn )} ⎠

(108)

The UAV is steered by a dynamic feedback linearization control algorithm which is based the flatness-based control analyzed in Section 5: u1 = xd + K p ( xd − x ) + K d ( xd − x ) 1

1

u2 = y d + K p ( y d − y ) + K d ( y d − y ) 2

2

ξ = u1cos(θ ) + u2 sin(θ ) u cos(θ ) − u1sin(θ ) v =ξ, ω = 2 ξ

(109)

358


Under the control law of Eq. (109) the dynamics of the tracking error finally becomes ex + K d ex + K p ex = 0 1

1

(110)

e y + K d ex + K p e y = 0 2

2

where ex = x − xd and ey = y − yd. The proportional-derivative (PD) gains are chosen as Kp1 and Kd1, for i = 1, 2. EIF Master Station

EIF Master Station

15

15

10

10

5

5 Y

20

Y

20

0

0

-5

-5

-10

-10

-15

-15

-10

-5

0

5 X

(a)

10

15

20

-15

-15

-10

-5

0

5

10

15

20

X

(b)

Fig. 11. Autonomous navigation of the multi-UAV system when the UAVs state vector is estimated with the use of the Extended Information Filter (a) tracking of circular reference trajectory (b) tracking of a curve-shaped reference trajectory Results on the performance of the Extended Information Filter in estimating the state vectors of multiple UAVs when observed by distributed processing units is given in Fig. 11. Using distributed EKFs and fusion through the Extended Information Filter is more robust comparing to the centralized EKF since (i) if a local processing unit is subject to a fault then state estimation becomes is still possible and can be used for accurate localization of the UAV, as well as for tracking of desirable flight paths, (ii) communication overhead remains low even in the case of a large number of distributed measurement units, because the greatest part of state estimation is performed locally and only information matrices and state vectors are communicated between the local processing units, (iii) the aggregation performed on the local EKF also compensates for deviations in state estimates of local filters (which can be due to linearization errors). 6.2 Autonomous UAV navigation with Distributed Particle Filtering Details on the implementation of the local particle filters are given first. Each local particle filter provides an estimation of the UAV’s state vector using sensor fusion. The UAV model described in Eq. (103), and the control law given in Eq. (109) are used again.

359


The wki =

measurement

update

w i − p( z( k )|x( k ) = ξ i − ) k N

k

∑ j =1wkj p( z( k )|x( k ) = ξ j− )

of

the

PF

p( x( k )|Z ) = ∑ i =1wki δ N

is

ξi−

( x( k ))

with

k

where the measurement equation is given by zˆ( k ) = z( k ) + v( k )

k

with z(k) = [x(k), y(k), θ(k), d(k)]T, and v(k) =measurement noise. The time update of the PF is p( x( k + 1)|Z ) = ∑ i =1wki δ i ( x( k )) where ξ ki ∼ p( x( k + 1)|x( k ) = ξ i − ) N

ξk

k

and the state equation is xˆ − = φ ( x( k )) + L( k )U ( k ) , where φ(x(k)), L(k), and U(k) are defined in subsection 6.1. At each run of the time update of the PF, the state vector estimation xˆ − ( k + 1) is calculated N times, starting each time from a different value of the state vector ξ ki . Although the Distributed Particle Filter can function under any noise distribution in the simulation experiments the measurement noise was assumed to be Gaussian. The obtained results are given in Fig. 12. DPF Master Station

DPF Master Station

15

15

10

10

5

5 Y

20

Y

20

0

0

-5

-5

-10

-10

-15

-15

-10

-5

0

5

10

15

20

-15

-15

-10

-5

0

5

X

X

(a)

(b)

10

15

20

Fig. 12. Autonomous navigation of the multi-UAV system when the UAVs state vector is estimated with the use of the Distributed Particle Filter (a) tracking of circular reference trajectory (b) tracking of a curve-shaped reference trajectory In the simulation experiments it was observed that the Distributed Particle Filter, for N = 1000 particles, succeeded more accurate state estimation (smaller variance) than the EIF and consequently enables better tracking of the desirable trajectories by the UAVs. This improved performance of the DPF over the EIF is due to the fact that the local EKFs that constitute the EIF introduce cumulative errors due to the EKF linearization assumption (truncation of higher order terms in the Taylor expansion of Eq. (2) and Eq. (4)). Comparing to the Extended Information Filter, the Distributed Particle Filter demands more computation resources and its computation cycle is longer. However, the computation cycle of PF can be drastically reduced on a computing machine with a fast processor or with

360


parallel processors (Míguez 2007). Other significant issues that should be taken into account in the design of the Distributed Particle Filter are the consistency of the fusion performed between the probability density functions of the local filters and the communication overhead between the local filters. The simulation results presented in Fig. 12 show the efficiency of the Distributed Particle Filtering in providing accurate localization for the multi-UAV system, as well as for implementing state estimation-based control schemes. The advantages of using Distributed Particle Filtering are summarized as follows: (i) there is robust state estimation which is not constrained by the assumption of Gaussian noises. The fusion performed between the local probability density functions enables to remove outlier particles thus resulting in an aggregate state distribution that confines with accuracy the real state vector of each UAV. If a local processing unit (local filter) fails the reliability of the aggregate state estimation will be preserved (ii) computation load can be better managed comparing to a centralized particle filtering architecture. The greatest part of the necessary computations is performed at the local filters. Moreover the advantage of communicating state posteriors over raw observations is bandwidth efficiency, which is particularly useful for control over a wireless sensor network.

7. Conclusions The paper has examined the problem of localization and autonomous navigation of a multiUAV system based on distributed filtering over sensor networks. Particular emphasis was paid to distributed particle filtering since this decentralized state estimation approach is not constrained by the assumption of noise Gaussian distribution. It was considered that m UAV (helicopter) models are monitored by n different ground stations. The overall concept was that at each monitoring station a filter should be used to track each UAV by fusing measurements which are provided by various UAV sensors, while by fusing the state estimates from the distributed local filters an aggregate state estimate for each UAV should be obtained. The paper proposed first the Extended Information Filter (EIF) and the Unscented Information Filter (UIF) as possible approaches for fusing the state estimates obtained by the local monitoring stations, under the assumption of Gaussian noises. It was shown that the EIF and UIF estimated state vector can be used by a flatness-based controller that makes the UAV follow the desirable trajectory. The Extended Information Filter is a generalization of the Information Filter in which the local filters do not exchange raw measurements but send to an aggregation filter their local information matrices (inverse covariance matrices which can be also associated to the Fisher Information matrices) and their associated local information state vectors (products of the local Information matrices with the local state vectors). In case of nonlinear system dynamics, such as the considered UAV models, the calculation of the information matrices and information state vectors requires the linearization of the local observation equations in the system’s state space description and consequently the computation of Jacobian matrices is needed. In the case of the Unscented Information Filter there is no linearization of the UAVs observation equation. However the application of the Information Filter algorithm is possible through an implicit linearization which is performed by approximating the Jacobian matrix of the system’s output equation by the product of the inverse of the state vector’s covariance matrix (Fisher information matrix) with the cross-covariance matrix


361

between the system’s state vector and the system’s output. Again, the local information matrices and the local information state vectors are transferred to an aggregation filter which produces the global estimation of the system’s state vector. Next, the Distributed Particle Filter (DPF) was proposed for fusing the state estimates provided by the local monitoring stations (local filters). The motivation for using DPF was that it is well-suited to accommodate non-Gaussian measurements. A difficulty in implementing distributed particle filtering is that particles from one particle set (which correspond to a local particle filter) do not have the same support (do not cover the same area and points on the samples space) as particles from another particle set (which are associated with another particle filter). This can be resolved by transforming the particles set into Gaussian mixtures, and defining the global probability distribution on the common support set of the probability density functions associated with the local filters. Suitable importance resampling is proposed so as to derive the weights of the joint distribution after removing the common information contained in the probability density functions of the local filters. The state vector which is estimated with the use of the DPF was again used by the flatnessbased controller to make each UAV follow a desirable flight path. Comparing to centralized state estimation and control the proposed distributed state estimation and control schemes have significant advantages: (i) they are fault tolerant: if a local processing unit is subject to a fault then state estimation is still possible and accurate, (ii) the computation load is distributed between local processing units and since there is no need to exchange a large amount of information, the associated communication bandwidth is low. In the case of the Extended Information Filter and of the Unscented Information Filter the information transmitted between the local processing units takes the form of the information covariance matrices and the information state vectors. In the case of Distributed Particle Filtering the information transmitted between the local processing units takes the form of Gaussian mixtures. The performance of the Extended Information Filter and of the Distributed Particle Filter was evaluated through simulation experiments in the case of a 2UAV model monitored and remotely navigated by two local stations. Comparing the DPF to the EIF through simulation experiments it was observed that the Distributed Particle Filter, succeeded more accurate state estimation (smaller variance) than the EIF and consequently enabled better tracking of the desirable trajectories by the UAVs. This improved performance of the DPF over the EIF is explained according to to the fact that the local EKFs that constitute the EIF introduce cumulative errors due to the EKF linearization assumption. It was also observed that the Distributed Particle Filter demands more computation resources than the Extended Information Filter and that its computation cycle is longer. However, the computation cycle of the DPF can be drastically reduced on a computing machine with a fast processor or with parallel processors. Other issues that should be taken into account in the design of the Distributed Particle Filter are the consistency of the fusion performed between the probability density functions of the local filters and the communication overhead between the local filters.

8. References [Beard et al. 2002] Beard, R.W.; McLain, T.W., Goodrich, M., & Anderson, E.P. (2002). Coordinated Target Assignment and Intercept for Unmanned Air Vehicles. IEEE Transactions on Robotics and Automation, Vol. 18, No. 6, pp. 911-922, 2002.

362


[Caballero et al. 2008] Caballero, F.; Merino, L., Ferruz, J., & Ollero, A. (2008). A particle filtering method for Wireless Sensor Network localization with an aerial robot beacon. Proc: IEEE International Conference on Robotics and Automation 2006, pp. 28602865, 2008. [Coué et al. 2003] Coué, C.; Pradalier, C. & Laugier, C. (2003). Bayesian Programming MultiTarget Tracking: an Automotive Application. Int. Conf. on Field and Service Robotics, Lake Yamanaka (Japan), July 2003. [Coué et al. 2006] Coué, C; Pradalier, C., Laugier, C., Fraichard, T. & Bessiére, P. (2006). Bayesian Occupancy Filtering for Multitarget Tracking: An Automotive Application. The International Journal of Robotics Research, Vol. 25, No. 1, pp. 19-30, 2006. [Deming & Perlovsky 2007] Deming, R.W. & Perlovsky, L.I. (2007). Concurrent multi-target localization, data association, and navigation for a swarm of flying sensors. Information Fusion, Elsevier, vol.8, no.3, pp. 316-330, 2007. [Fliess et al. 1999] Fliess, M. & Mounier, H. (1999). Tracking control and π-freeness of infinite dimensional linear systems, Dynamical Systems, Control, Coding and Computer Vision, (G. Picci and D.S. Gilliam, Eds.), Vol. 258, pp. 41-68, Birkhaüser, 1999. [Gan & Harris 2001] Gan Q. & Harris, C.J. (2001). Comparison of two measurement fusion methods for Kalman-filter-based multisensor data fusion. IEEE Transactions on Aerospace and Electronic Systems, Vol. 37, No.1, pp. 273-280, 2001. [Gao et al. 2009] Gao, S; Zhong, Y. Zhang, X., & Shirinzadeh, B. (2009). Multi-sensor optimal data fusion for INS/GPS/SAR integrated navigation system. Aerospace Science and Technology, Elsevier, Vol. 13, pp. 232-237, 2009. [Hue et al. 2002] Hue, C.; Le Cadre, J.P. & Pérez, P. (2002). Tracking Multiple Objects with Particle Filtering. IEEE Transactions on Aerospace and Electronic Systems, Vol. 38. No.3, pp. 791-812, 2002. [Ing & Coates 2005] Ing, J. & Coates, M.G. (2005). Parallel particle filters for tracking in wireless sensor networks. IEEE Workshop on Signal Processing Advances in Wireless Communications, SPAWC 2005, art. no. 1506277, pp. 935-939, 2005. [Julier et al. 2000] Julier, S.; Uhlmann, J. & Durrant-Whyte, H.F. (2000). A new method for the nonlinear transformations of means and covariances in filters and estimators. IEEE Transactions on Automatic Control, Vol.45, No.3, pp. 477-482, 2000. [Julier et al. 2004] Julier, S.J. & Uhlmann, J.K. (2004). Unscented Filtering and Nonlinear Estimation, Proceedings of the IEEE, Vol.92, pp. 401-422, 2004. [Léchevin & Rabbath 2006] Léchevin, N. & Rabbath, C.A. (2006). Sampled-data control of a class of nonlinear flat systems with application to unicycle trajectory tracking. ASME Journal of Dynamical Systems, Measurement and Control, vol. 128, No.3, pp. 722-728, 2006. [Lee et al. 2008] Lee D.J. (2008). Unscented Information Filtering for Distributed Estimation and multiple sensor fusion. AIAA Guidance, Navigation and Control Conference and Exhibit, Aug. 2008, Hawai, USA. [Lee et al. 2008] Lee, D.J. (2008). Nonlinear estimation and multiple sensor fusion using unscented information filtering. IEEE Signal Processing Letters, Vol. 15, pp. 861-864, 2008. [Mahler 2007] Mahler, R.P.S. (2003). Statistical Multisource-Multitarget Information Fusion. Artech House Inc. 2007.


363

[Makarenko & Durrant-Whyte 2006] Makarenko, A. & Durrant-Whyte, H. (2006). Decentralized Bayesian algorithms for active sensor networks. Information Fusion, Elsevier, Vol.7, pp. 418-433, 2006. [Manyika & H. Durrant-Whyte 1994] Manyika, J. & Durrant-Whyte, H. (1994). Data fusion and sensor management: a decentralized information theoretic approach. Englewood Cliffs, NJ, Prentice Hall, 1994. [Míguez 2007] Míguez, J. (2007). Analysis of parallelizable resampling algorithms for particle filtering. Signal Processing, Elsevier, Vol. 87, 3155-3174, 2007. [Morelande & D. Mušicki 2005] Morelande, M.R. & Mušicki, D. (2005). Fast multiple target tracking using particle filters. Proc. of the 44th IEEE Conference on Decision and Control, and the European Control Conference 2005, Seville, Spain, December 12-15, 2005. [Musso et al. 2001] Musso, C.; Oudjane, N. & Le Gland, F. (2001). Imrpoving regularized particle filters, In: Sequential Monte Carlo Methods in Practice. A. Doucet, N. de Freitas and N. Gordon Eds., Springer-Verlag 2001, pp. 247-272, 2001. [Nettleton et al. 2003] Nettleton, E.; Durrant-Whyte & H. Sukkarieh, S. (2003). A robust architecture for decentralized data fusion. ICAR03, 11th International Conference on Advanced Robotics, Coimbra, Portugal, 2003. [Olfati-Saber 2005] Olfati-Saber, R. (2005). Distributed Kalman Filter with Embedded Consensus Filters. Proc. 44th IEEE Conference on Decision and Control, pp. 8179-8184, Seville, Spain, 2005. [Olfati-Saber 2006] Olfati-Saber, R. (2006). Distributed Kalman Filtering and Sensor Fusion in Sensor Networks. Lecture notes in control and information sciences, Vol. 331, pp. 157-167. [Ong et al. 2006] Ong, L.L.; Upcroft, B., Bailey, T., Ridley, M. Sukkarieh, S. & Durrant-Whyte, H. (2006). A decentralized particle filtering algorithm for multi-target tracking across multiple flight vehicles. IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, October 2006. [Ong et al. 2008] Ong, L.L.; Bailey, T., Durrant-Whyte, H. & Upcroft, B. (2008). Decentralized Particle Filtering for Multiple Target Tracking in Wireless Sensor Networks. Fusion 2008, The 11th International Conference on Information Fusion, Cologne, Germany, July 2008. [Oriolo et al. 2002] Oriolo, G.; De Luca, A. & Vendittelli, M. (2002). WMR Control Via Dynamic Feedback Linearization: Design, Implementation and Experimental Validation. IEEE Transactions on Control Systems Technology, Vol. 10, No.6, pp. 835852, 2002. [Ren & Beard 2004] Ren, W. & Beard, R.W. (2004). Trajectory tracking for unmanned air vehicles with velocity and heading rate constraints. IEEE Transactions on Control Systems Technology, Vol. 12, No. 5, pp. 706-716, 2004. [Rigatos & Tzafestas 2007] Rigatos, G.G. & Tzafestas, S.G. (2007). Extended Kalman Filtering for Fuzzy Modeling and Multi-Sensor Fusion. Mathematical and Computer Modeling of Dynamical Systems, Vol. 13, No 3, Taylor and Francis, 2007. [Rigatos 2008] Rigatos, G.G. (2008). Autonomous robots navigation using flatness-based control and multi-sensor fusion. Robotics, Automation and Control, (P. Pecherkova, M. Fliidr and J. Dunik, Eds), I-Tech Education and Publishing KG, Vienna Austria, pp. 394-416, 2008.

364


[Rigatos 2009a] Rigatos, G.G. (2009). Particle Filtering for State Estimation in Nonlinear Industrial Systems. IEEE Transactions on Instrumentation and Measurement, Vol. 58, No. 11, pp. 3885-3900, 2009. [Rigatos 2009b] Rigatos, G.G. (2009). Sigma-point Kalman Filters and Particle Filters for integrated navigation of unmanned aerial vehicles. Intl. Workshop on Robotics for Risky Interventions and Environmental Surveillance, RISE 2009, Brussels, Belgium, Jan. 2009. [Rigatos & Zhang 2009] Rigatos, G. & Zhang, Q. (2009). Fuzzy model validation using the local statistical approach. Fuzzy Sets and Systems, Elsevier, Vol. 60, No.7, pp. 882904, 2009. [Rosencrantz et al. 2003] Rosencrantz, M.; Gordon, G., Thrun, S. (2003). Decentralized data fusion with distributed particle filtering. Proceedings of the Conference of Uncertainty in AI (UAI), Acapulco, Mexico, 2003. [Särrkä 2007] Särrkä, S. (2007). On Unscented Kalman Filtering for state estimation of continuous-time nonlinear systems. IEEE Transactions on Automatic Control, Vol.52, No.9, pp.1631-1641, 2007. [Shima et al. 2007] Shima, T.; Rasmussen, S.J. & Chandler, P. (2007). UAV team decision and control using efficient collaborative estimation. Journal of Dynamic Systems, Measurement and Control, Transactions of the ASME, Vol. 129, No 5, pp. 609-619, 2007. [Singh & Fuller 2001] Singh, L. & Fuller, J. (2001). Trajectory generation for a UAV in urban terrain using nonlinear MPC. Proceedings of American Control Conference, pp. 23012308, 2001. [Thrun et al. 2005] Thrun, S.; Burgard, M. & Fox, D. (2005). Probabilistic Robotics, MIT Press, 2005. [Vercauteren &Wang 2005] Vercauteren, T. &Wang, X. (2005). Decentralized Sigma-Point Information Filters for Target Tracking in Collaborative Sensor Networks. IEEE Transactions on Signal Processing, Vol.53, No.8, pp. 2997-3009, 2005. [Villagra et al. 2007] Villagra, J.; d’Andrea-Novel, B., Mounier, H. & Pengov, M. (2007). Flatness-based vehicle steering control strategy with SDRE feedback gains tuned via a sensitivity approach. emphIEEE Transactions on Control Systems Technology, Vol. 15, pp. 554-565, 2007. [Vissière et al. 2008] Vissière, D.; Bristeau, P.-J., Martin, A.P. & N. Petit (2008). Experimental autonomous flight of a small-scaled helicopter using accurate dynamics model and low-cost sensors. Proceedings of the 17thWorld Congress The International Federation of Automatic Control Seoul, Korea, July 2008. [Watanabe & Tzafestas 1992] Watanabe, K. & Tzafestas, S.G. (1992). Filtering, Smoothing and Control in Discrete-Time Stochastic Distributed-Sensor Networks, In: Stochastic Large- Scale Engineering Systems (S.G. Tzafestas and K. Watanabe Eds), pp. 229-252, Marcel Dekker, 1992. [Zhang et al. 2005] Zhang, Campillo, Q.F., Cérou, F. & F. Legland (2005). Nonlinear fault detection and isolation based on bootstrap particle filters, Proc. of the 44th IEEE Conference on Decision and Control, and European Control Conference, Seville Spain, Dec. 2005.

17 Design and Control of a Compact Laparoscope Manipulator: A Biologically Inspired Approach Atsushi Nishikawa1, Kazuhiro Taniguchi2, Mitsugu Sekimoto3, Yasuo Yamada3, Norikatsu Miyoshi3, Shuji Takiguchi3, Yuichiro Doki3, Masaki Mori3 and Fumio Miyazaki3 1Shinshu

University University 3Osaka University Japan

2Kogakuin

1. Introduction Laparoscopic surgery is a technique where surgical tools and a laparoscope are inserted into the patient’s body through small holes in the abdomen, and the surgeon carries out the surgery while viewing the images from the laparoscope on a TV monitor (see Fig. 1(left)). Laparoscopic surgery has grown rapidly in popularity in recent years, not only because it is less invasive and produces less visible scarring, but also because of its benefits in terms of healthcare economy, such as shorter patient stays. The most important characteristic of this technique is that the surgeon performs the operation while watching the video image from the laparoscope on a monitor instead of looking directly at the site of the operation. Thus, an important factor affecting the safety and smoothness of the operation is the way in which the video images are presented in a field of view suitable for the surgical operation. Manipulation of the laparoscope is not only needed for orienting the laparoscope towards the parts requiring surgery, but also for making fine adjustments to ensure that the field of view, viewing distance and so on are suitable for the surgical operation being performed. A camera assistant operates the laparoscope according to the surgeon’s instructions, but must also make independent decisions on how to operate the laparoscope in line with the surgeon’s intentions as the surgery progresses. Consequently even the camera assistant that operates the laparoscope must have the same level of experience in laparoscopic surgery as the surgeon. However, not many surgeons are skilled in the special techniques of laparoscopic surgery. It is therefore not uncommon for camera assistants to be inexperienced and unable to maintain a suitable field of view, thus hindering the progress of the operation. To address this problem, laparoscope manipulating robots are expected as a substitute for the human camera assistant and have already been made commercially available (see Fig. 1(right)). However, there are several problems to be solved: 1. Hardware problems: A large apparatus sometimes interferes with the surgeon. The setting and repositioning is awkward. Furthermore, the initial and maintenance costs are expensive.

366


Fig. 1. Laparoscopic surgery. (left) Conventional laparoscopic surgery where the laparoscope is operated by a human camera assistant. (right) Robot-assisted surgery where the laparoscope is operated by a laparoscope manipulator. 2.

Software problems: It is difficult to build and implement the accurate laparoscope manipulating model and consequently the conventional systems may not always offer the optimal view that the surgeon wants. In this chapter, we will introduce a biologically inspired approach to the development of a new laparoscope manipulating robot to overcome those problems.

2. Related works Laparoscope manipulators have been developed in the last fifteen years and there are at least 27 kinds of laparoscope controlling robots which are commercialized or published in refereed articles as of September 2009 (Taniguchi et al. (2010)). Some of them have already been made commercially available and are in widespread use. These include AESOP made in the US by Computer Motion Inc. (now known as Intuitive Surgical Inc.) (Sackier & Wang (1994)), EndoAssist made in the UK by Armstrong Healthcare Ltd. (now known as Prosurgics Inc.) (Aiono et al. (2002)), LapMan made in Belgium by Medsys s.a. (Polet & Donnez (2004)), and Naviot made in Japan by Hitachi Co.,Ltd. (Tanoue et al. (2006)). Although these commercialized manipulators have various merits such as stable view and reduction of need for medical staff, several problems have been noted. First, the bulky manipulator and the supporting arm often interfere with the surgical procedures. Second, the setting and detaching of the robot is frequently awkward, causing an extension of the time required for the operation. Furthermore, the initial and maintenance costs are expensive. In addition to such hardware problems, they usually must be controlled by the operating surgeon himself/herself using a human-machine interface such as an instrumentmounted joystick, foot pedal, voice controller, or head/face motion-activated system. This is an additional task that distracts the surgeon’s attention from the main region of interest and may result in frustration and longer surgery time. To free the surgeon from the task of controlling the view and to automatically offer an optimal and stable view during laparoscopic surgery, several automatic camera positioning systems have been devised (Casals et al. (1996), Wei et al. (1997), Wang et al. (1998), Nishikawa et al. (2003), Ko & Kwon (2004), Nishikawa et al. (2006)). These systems visually extract the shape and/or position of the surgical instrument from the laparoscopic images in

Design and Control of a Compact Laparoscope Manipulator: A Biologically Inspired Approach

367

real time, and automatically manipulate the laparoscope to always center the tip of the instrument in the displayed image. Such systems are based on the simple idea that the projected position of the distal end of the surgical tool corresponds to the surgeon’s region of interest in a laparoscopic image. Besides centering on the most interesting area, there is an additional and important factor that defines a good image of the surgical scene—zooming ratio (Nishikawa et al. (2008)) — that corresponds to the depth of insertion of the laparoscope along its longitudinal axis. The pioneering studies of fully automatic camera positioning systems defined the zooming ratio as a “uniform” function of the estimated distance between the tip of the tool and the laparoscope (Wei et al. (1997)) or the area ratio between the visible tool and the whole image (Casals et al. (1996)). Although these approaches may completely remove the surgeon’s camera control burden, they may not provide the specific view that the surgeon wants, because the most appropriate zooming ratio varies widely during surgery. The best zooming ratio depends on both the surgical procedure/phase and the habits/preferences of the operating surgeon. For this reason, most of the instrument tracking systems recently developed (Wang et al. (1998), Nishikawa et al. (2003), Ko & Kwon (2004), Nishikawa et al. (2006)) have abandoned the idea of systematic control of zooming parameters; instead, the surgeon is required to define the parameters preoperatively or adjust them intraoperatively through conventional human-machine interfaces, which again means an extra control burden for the surgeon.

3. Hardware design: analogy to human muscular structure We developed a compact and lightweight robot manipulator, named P-arm (Sekimoto et al. (2009)), in collaboration with Daiken Medical Co., Ltd., Japan. 3.1 Parallel mechanism There are several parallel robots (Kobayashi et al. (1999), Tanoue et al. (2006), Pisla et al. (2008)), which operates a laparoscope through the incision point on the abdominal wall of the patient. These systems have “less than 4” DOF and set up the laparoscope “outside” the parallel mechanism. Unlike the previous systems, the proposed manipulator is composed of a Stewart-Gough platform equipped with “six” linear actuators arranged in parallel “around” the laparoscope (see Fig. 2). This novel mechanism has an analogy to human muscular structure in which many extensors and flexors interact with each other; the rigid laparoscope corresponds to a bone of the human body and the linear actuators correspond to the muscles attached to the bone. This bio-inspired structure enables both the manipulator itself and the space necessary for operating the manipulator to be simple and small. The size of the P-arm is 120 mm in maximum diameter and 297.5 mm in length. Consequently, the manipulator can avoid interference with the surgeon’s work during surgery. The Stewart-Gough platform has 6 DOF, whereas laparoscope movements are kinematically restricted to 4 DOF, due to the constraints imposed by operating through the incision point. In our case, even when two of the six actuators stop and are dislocated, the manipulator works safely because the system uses the remaining four actuators to produce constrained 4 DOF motion. Thus, our laparoscope manipulating robot based on the use of Stewart-Gough platform architecture provides both flexibility and accuracy while maintaining safety.

368


(a)

(b)

Fig. 2. Compact and lightweight laparoscope manipulator, named P-arm. (a) The P-arm is composed of a Stewart-Gough platform equipped with six linear hydraulic actuators. (b) The P-arm can hold a general laparoscope and can be supported by the conventional instrument holder. 3.2 Hydraulic actuators The “artificial” muscles of the manipulator, that is, the linear actuators, are driven by hydraulic pressure transmitted via tubes connecting to the water cylinders in the controller unit. The actuator, tube, and the cylinder containing water were assembled en bloc and packaged in sterilized condition for clinical use. Also, the materials that were as inexpensive as possible were selected for all the parts of the manipulator including the actuators among those suitable for medical use and sterilization. All of the previously developed robots had to be wrapped in a sterilized plastic bag preoperatively, because the robot itself was not suitable for sterilization. The proposed manipulator was designed to be disposable and to be provided in a sterilized condition to make the preparation for the operation easy and quick and lessen the maintenance cost of the robot. Furthermore, materials in the manipulator were also selected in consideration of their weight. The actuator, which was mainly made of polycarbonate, weighed only 30 g. In total, the manipulator weighed only 580 g. The light weight allows that the manipulator to be fixed to the operating table with a conventional slim instrument holder. This makes the setting and repositioning of the manipulator easier and quicker. Also, the operating table can be tilted without repositioning the manipulator.


369

The actuators are attached to the manipulator using a permanent magnet. Therefore, when excess force is applied to the manipulator, the actuator is readily dislocated so it does not injure the patient. In addition, even when two of the six actuators are dislocated, the manipulator works safely as discussed above. The dislocated actuators can be easily reattached to the manipulator. Furthermore, in the case of an emergency, the robot can be stopped promptly by the emergency stop system, which is controlled by a circuit independent of the operating system. 3.3 Results The robot was evaluated by performing the following three types of operations using a living swine: a laparoscopic cholecystectomy, a laparoscopic anterior resection of the rectum, a laparoscopic distal gastrectomy (Sekimoto et al. (2009)). As a result, it worked steadily for all the operations, without interfering with the surgeon’s work (see Fig. 3). Also,

Fig. 3. View of an in vivo experiment (laparoscopic cholecystectomy) using a living swine. The P-arm and its supporting arm were so small that they did not interfere with the surgeon’s work.

370


it contributed to shortening the setting and detaching time. The setting times were 66, 93, 104 seconds and the detaching times were 24 and 17 seconds, respectively. Wagner reported the setting time of 2 minutes for AESOP and 5.3 minutes for EndoAssist (Wagner et al. (2006)). Compared with these results, the P-arm was considered to be superior. The facility of the system is essential for the robot to be accepted by surgeons.

4. Software design: Use of biological fluctuation Recent studies revealed that biological systems did not require the precise environmental model but rather made use of “fluctuation” in order to adapt to the environment. This adaptation mechanism can be represented by the following equation (Kashiwagi et al. (2006)): G G dx G G = f ( x ) × activity + η dt

(1)

G G G G where x and f ( x ) are the state and the dynamics of the system, and η indicates noise G (fluctuation). A scalar variable activity indicates the fitness of the state x to the environment G G and controls the behavior of the system. The term f ( x ) × activity becomes dominant in the above equation when the variable activity is large, and the state transition becomes G deterministic. On the other hand, the noise η becomes dominant G G when activity is small, and the state transition becomes probabilistic. If the function f ( x ) has several attractors, the G state of the system x is entrained into one attractor when activity is large, while the behavior of the system becomes like a random walk when activity is small. The variable G activity is designed to be large G G (small) when the state x is suited (not suited) to the environment. The function f ( x ) is designed to have several attractors and updated in realG time based on the present activity information such that the state x may efficiently become suited to the environment. As a result, the state of the system is entrained into an attractor that is suited to the environment and activity becomes large. Otherwise activity remains small and the system searches for a suitable attractor by a random walk. G By letting the state x be the desired position of the tip of the right-hand surgical instrument in terms of laparoscopic camera coordinates, we developed a novel laparoscope positioning system that did not require any precise camera manipulating models (Nishikawa et al. (2009a)).

4.1 Design of activity In order to find the activity–the most important factor for offering the specific view that the surgeon wants during laparoscopic surgery, a number of in-vitro laparoscopic cholecystectomy tests were performed. For each test, a swine liver with a gallbladder was placed in a training box and the gallbladder was removed by an operating surgeon with the use of the laparoscope robot P-arm controlled through a joystick interface by a camera assistant (another surgeon). In order to gather the positional relationship between the right and left surgical instruments and the laparoscope during surgery, a 3D tracking system (Polaris Accedo, NDI Corporation) was used. As a result, it was found that the velocity of the tip of the left-hand instrument and the velocity of the tip of the laparoscope had a high correlation (the cross correlation coefficient between the two was +0.765, (Nishikawa et al. (2009b))). We hypothesized that, at least in case of laparoscopic cholecystectomy, the camera


371

assistant changed the field of view when the magnitude of the acceleration of the tip of the left-hand instrument was large, and employed the following equation as the activity: activity =

1 N

activity i =

N −1

∑ activity n=0

1 N

(2)

i −n

N −1

∑p n=0

(3)

i −n

⎪⎧0 if vi − vi − 1 > K pi = ⎨ ⎪⎩ 1 if vi − vi − 1 ≤ K

(4)

where i means time, N indicates the positive number for calculating the moving average. vi indicates the magnitude of the velocity of the tip of the left-hand instrument at time i, and K(> 0) is a threshold value. 4.2 DesignGof attractors G In Eq. 1, f ( x ) must have several attractors. Fukuyori et al. (2008) pointed out that the attractor should be adaptively allocated where the activity becomes large. Based on this concept of “adaptive attractors”, we regard the position of the tip of the right-hand instrument at time j as the center of the j-th attractor with the magnitude of Ci−j × activityj at time i(≥ j), and employ the following equation to design attractors: G G f (x) =

⎡ ⎤ B ⎢ i− j ⎥ C activity × × j ⎢ ⎥⋅ G G2 i− j j ≤ i , C × activity j > M ⎢ rj − x + A ⎥ ⎣ ⎦

∑

G x( t )

t =0

=

1 N

N −1

G

∑ ri −n

G G rj − x G G rj − x

(5)

(6)

n=0

where i means the present time, N indicates the positive number for calculating the moving G average. The vector rj represents the position of the tip of the right-hand instrument at time j. A, B, and C are all the positive constants: the parameters A and B respectively set the range and power of attractors, and the parameter C(0) is a threshold for ignoring weak attractors. The term

G whose center is rj .

B acts like the Gaussian function G G 2 rj − x + A

4.3 Results We implemented this bio-inspired method on our robotic laparoscope positioner described in section 3. Fig. 4 shows the overview of our automatic laparoscope positioning system. The position/pose of the three tools: the right and left instruments and the laparoscope can be obtained simultaneously by the commercial 3D tracking system, Polaris Accedo (NDI Corporation) (See Blasinski et al. (2007) and Nishikawa et al. (2008) for the details). Then

372


Fig. 4. Overview of automatic laparoscope positioning system. The proposed system uses “fluctuation” to determine and update in real-time the desired position of the tip of the G righthand instrument, x , during surgery. G G both ri (the position of the tip of the right-hand instrument) and vi (the velocity of the tip of the left-hand instrument) are estimated in terms of laparoscopic camera coordinates, and G G activity and f ( x ) are calculated from Eqs. 2 and 5 respectively. As a result, we can determine and update also in real-time the desired position of the tip of the right-hand G G G instrument, x , during surgery, by substituting the resulting values: activity and f ( x ) into Eq. 1 and solving the Eq. 1 numerically (e.g., by the Runge-Kutta method) under the initial condition given by Eq. 6. To validate the proposed system, a number of in-vitro laparoscopic cholecystectomy tests were performed. For each test, a swine liver with a gallbladder was placed in the training box and the gallbladder was removed by an operating surgeon with the support of the laparoscope robot P-arm controlled by Eq. 1. As a result, our system successfully and automatically controlled the position of a laparoscope during all the operations (Figs. 5–10).

5. Concluding remarks A compact and lightweight laparoscope manipulator was developed. Also, a novel method for controlling the position of a laparoscope was inspired by biological systems dynamics. Our approach opens potential applications to skill transfer and adaptive behavior in medicine.


373

Fig. 5. View of an in vitro experiment (laparoscopic cholecystectomy) using a swine liver with a gallbladder (1/6). (left) a surgeon and the laparoscope robot P-arm, (mid) image from the laparoscope, (right) visualization of attractors as the contour map on the image plane.

374




375


376




377


378




379

6. Acknowledgements This research was supported by “Special Coordination Funds for Promoting Science and Technology: Yuragi Project” of the Ministry of Education, Culture, Sports, Science and Technology, Japan, and Grant-in-Aid for Scientific Research (A) of the Japan Society for the Promotion of Science (Representative Researcher: Atsushi Nishikawa, Project Number 19206047). We cordially appreciate the cooperation of Mr. Takeharu Kobayashi, Mr. Kouhei Kazuhara, Mr. Takaharu Ichihara, and Mr. Naoto Kurashita of Daiken Medical Co. Ltd. Also, we would like to express our sincere gratitude that the surgeons of Osaka University Hospital agreed to help our research.

7. References Aiono, S.; Gilbert, J. M.; Soin, B.; Finlay, P. A. & Gordan, A. (2002). Controlled trial of the introduction of a robotic camera assistant (Endo Assist) for laparoscopic cholecystectomy. Surgical Endoscopy, Vol. 16, No. 9, September 2002, 1267–1270, ISSN 0930-2794 (Print) 1432-2218 (Online) Blasinski, H.; Nishikawa, A. & Miyazaki, F. (2007). The application of adaptive filters for motion prediction in visually tracked laparoscopic surgery. Proceedings of the 2007 IEEE International Conference on Robotics and Biomimetics (ROBIO2007), pp. 360–365, Sanya, China, December 2007, IEEE Press Casals, A.; Amat, J. & Laporte, E. (1996). Automatic guidance of an assistant robot in laparoscopic surgery. Proceedings of the 1996 IEEE International Conference on Robotics and Automation, pp. 895–900, Minneapolis, USA, April 1996, IEEE Press Fukuyori, I.; Nakamura, Y.; Matsumoto, Y. & Ishiguro, H. (2008). Flexible control mechanism for multi-DOF robotic arm based on biological fluctuation. In: From Animals to Animats 10, 22-31, Springer, ISBN 978-3-540-69133-4 Kashiwagi, A.; Urabe, I.; Kaneko, K. & Yomo, T. (2006). Adaptive response of a gene network to environmental changes by fitness-induced attractor selection. PLoS ONE, Vol. 1, No. 1, December 2006, e49 Ko, S. Y. & Kwon, D. S. (2004). A surgical knowledge based interaction method for a laparoscopic assistant robot. Proceedings of the 13th IEEE International Workshop on Robot and Human Interactive Communication (ROMAN 2004), pp. 313–318, Kurashiki, Japan, September 2004, IEEE Press Kobayashi, E.; Masamune, K.; Sakuma, I.; Dohi, T. & Hashimoto, D. (1999). A new safe laparoscopic manipulator system with a five-bar linkage mechanism and an optical zoom. Computer Aided Surgery, Vol. 4, No. 4, 1999, 182–192 Nishikawa, A.; Asano, S.; Fujita, R.; Yamaguchi, S.; Yohda, T.; Miyazaki, F.; Sekimoto, M.; Yasui, M.; Takiguchi, S.; Miyake, Y. & Monden, M. (2003). Selective use of face gesture interface and instrument tracking system for control of a robotic laparoscope positioner. In: Medical Image Computing and Computer Assisted Intervention –MICCAI2003, 973-974, Springer, ISBN 978-3-540-20464-0 Nishikawa, A.; Ito, K.; Nakagoe, H.; Taniguchi, K.; Sekimoto, M.; Takiguchi, S.; Seki, Y.; Yasui, M.; Okada, K.; Monden, M. & Miyazaki, F. (2006). Automatic positioning of a laparoscope by preoperative workspace planning and intraoperative 3D instrument tracking. MICCAI2006 Workshop Proceedings, Workshop on Medical Robotics: Systems and Technology towards Open Architecture, pp. 82–91, Copenhagen, Denmark, October 2006

380


Nishikawa, A.; Nakagoe, H.; Taniguchi, K.; Yamada, Y.; Sekimoto, M.; Takiguchi, S.; Monden, M. & Miyazaki, F. (2008). How does the camera assistant decide the zooming ratio of laparoscopic images? Analysis and implementation. In: Medical Image Computing and Computer Assisted Intervention –MICCAI2008, 611-618, Springer, ISBN 978-3-540- 85989-5 Nishikawa, A.; Yamada, Y.; Taniguchi, K. & Miyazaki, F. (2009). Automatic endoscope positioning algorithm based on biological fluctuation. Proceedings of the 5th Asian Conference on Computer Aided Surgery (ACCAS2009), p. 125, Changhua, Taiwan, July 2009 Nishikawa, A.; Yamada, Y.; Toda, S.; Sekimoto, M.; Miyoshi, N.; Takiguchi, S.; Doki, Y.; Mori, M. & Miyazaki, F. (2009). Analysis of Decisions by Camera Assistants on the Field of View for Laparoscopic Surgery and Its Application to Automatic Positioning of a Laparoscope. Proceedings of the 21st International Conference of the Society for Medical Innovation and Technology (SMIT2009), pp. 99–100, Sinaia, Romania, October 2009 Pisla, D.; Plitea, N. & Vaida, C. (2008). Kinematic modeling and workspace generation for a new parallel robot used in minimally invasive surgery. In: Advances in Robot Kinematics: Analysis and Design, Lenarcic, J & Wenger, P, (Ed.), 459–468, Springer, ISBN 978-1-4020-8599-4 (Print) 978-1-4020-8600-7 (Online) Polet, R. & Donnez, J. (2004). Gynecologic laparoscopic surgery with a palm-controlled laparoscope holder. The Journal of the American Association of Gynecologic Laparoscopists, Vol. 11, No. 1, February 2004, 73–78 Sackier, J. M. & Wang, Y. (1994). Robotically assisted laparoscopic surgery. From concept to development. Surgical Endoscopy, Vol. 8, No. 1, January 1994, 63–66, ISSN 0930-2794 (Print) 1432-2218 (Online) Sekimoto, M.; Nishikawa, A.; Taniguchi, K.; Takiguchi, S.; Miyazaki, F.; Doki, Y. & Mori, M. (2009). Development of a compact laparoscope manipulator (P-arm). Surgical Endoscopy, Vol. 23, No. 11, November 2009, 2596–2604, ISSN 0930-2794 (Print) 14322218 (Online) Taniguchi, K.; Nishikawa, A.; Sekimoto, M.; Kobayashi, T.; Kazuhara, K.; Ichihara, T.; Kurashita, N.; Takiguchi, S.; Doki, Y.; Mori, M. & Miyazaki, F. (2010). Classification, design and evaluation of endoscope robots. In: Robot Surgery, Seung Hyuk Baik, (Ed.), 1–24, INTECH, ISBN 978-953-7619-77-0 Tanoue, K.; Yasunaga, T.; Kobayashi, E.; Miyamoto, S.; Sakuma, I.; Dohi, T.; Konishi, K.; Yamaguchi, S.; Kinjo, N.; Takenaka, K.; Maehara, Y. & Hashizume, M. (2006). Laparoscopic cholecystectomy using a newly developed laparoscope manipulator for 10 patients with cholelithiasis. Surgical Endoscopy, Vol. 20, No. 5, May 2006, 753– 756, ISSN 0930-2794 (Print) 1432-2218 (Online) Wagner, A.; Varkarakis, I. M.; Link, R. E.; Sullivan, W. & Su, L. M. (2006). Comparison of surgical performance during laparoscopic radical prostatectomy of two robotic camera holders, EndoAssist and AESOP: a pilot study. Urology, Vol. 68, 2006, 70–74 Wang, Y. F.; Uecker, D. R. &Wang, Y. (1998). A new framework for vision-enabled and robotically assisted minimally invasive surgery. Computerized Medical Imaging and Graphics, Vol. 22, No. 6, December 1998, 429–437. Wei, G. Q.; Arbter, K. & Hirzinger, G. (1997). Real-time visual servoing for laparoscopic surgery. Controlling robot motion with color image segmentation. IEEE Engineering in Medicine and Biology Magazine, Vol. 16, No. 1, January-February 1997, 40–45, ISSN 0739-5175

18 Open Software Architecture for Advanced Control of Robotic Manipulators J. Gomez Ortega, J. Gamez García, L. M. Nieto Nieto and A. Sánchez García

System Engineering and Automation Department at Jaén University, Spain 1. Introduction

So far, the robotic applications has been dominated by proprietary based hardware and software devices developed for industrial applications with a large volume manufacturing, like the automotive and electronics industries. Then, the main goal of the automation technologies has been an optimized robot design for precise assembly tasks, resulting in complex systems with a reduced flexibility. Traditional robotic applications have a fixed configuration, with the advantages of high accuracy and a well studied kinematics. However, since recent years, the number of service robots in our daily life environments is increasing. There are many new applications, i.e. teleoperation, human-robot-collaborative works, etc. that require reconfigurable hardware and expansibility to accomplish new working modes in not-structured scenarios and notintensive manufacturing tasks. However, these systems must meet diverse user requirements and integrate different hardware and software systems developed for a particular proprietary platform. As a result, many different researchers have solved similar issues with non-interchangeable products, working from scratch each time, adapting the traditional industrial robots platform for the new applications. Today, a new robotic system is an integration of different processors and hardware platforms manufactured by different vendors, controlled by software modules developed using different programming languages and different communication protocols. In addition, as robotic manipulator is expected to accomplish more complex tasks, it needs the integration of multiple sensors working with different time bases and bandwidths (Gamez et al., 2009; Luo et al., 2002), and new capabilities are needed that traditional control technology of current industrial robots is not offering. In order to solve these problems different open robotics platforms have been presented. 1.1 Open robotic platforms: an overview From the definition of an Open Systems (IEEE 1003.0), an Open (Robotic) Platform should "provide capabilities that enable properly implemented applications to run on a variety of platforms from multiple vendors interoperate with other system applications and present a consistent style of interaction with the user". This leads to the following properties: • Portability of the software, to reuse it in other platforms with minor changes.

382


•

Reusability is an issue that should be addressed from the beginning of the development process, identifying common problems that could be solved with reusable solutions and shared within the robotics community. • Extensibility to change or add several component (of hardware or software) to the system from different vendors. • Adaptability/dynamic reconfigurability, providing mechanism of easy adaptation of its parameters according to the application requirements. • Interoperability refers to the ability to support interchange of information between robotic modules designed by different vendors, providing effective communication and working in a coordinated manner. In particular it relies on the network and communication protocols that must provide effective real-time communication among distributed components, independently of the system specific particularities. Diverse approaches have been proposed to achieve these capabilities. Some solutions use Matlab/Simulink and Real Time Workshop to generate control applications for robotic systems with a proprietary operating systems (Gamez et al., 2007), but with the disadvantage of a limited interoperability. Today, most of the research and robotic applications developed based on proprietary hardware used a layered software architecture. This approach typically includes a standard based middleware to provide integration, efficient communication, interoperability, abstraction of software components, also providing portability. At the top level different reusable software components are used. In the low level layer, the hardware is controlled by drivers developed to run on a proprietary RTOS. However, since last decade, developers have a growing interest on developing open source applications based on Linux RTOS. Thus, vendors are offering commercial-grade Linux operating systems (Saravanan et al., 2009; Gamez et al. 2009). Another approach based on hardware modularity can support integration of new components from various vendors. The corresponding software must provide a well defined interface to provide easily integration between interconnected devices, and the capabilities of extensibility and modification (Xuemei & Liangzhong, 2007). As the hardware is always vendor-dependent, the integration of different devices may be difficult due to incompatibility reasons. To overcome this problem, some hardware standards have been proposed, however this method is considered too restrictive to achieve reusability of existing hardware (Hong et al. 2001). 1.2 Related research In recent years, an increasing number of initiatives have been presented: • OROCOS (Open Robot Control Software) project (OROCOS, 2010), is a European initiative for providing free software project to develop advanced robotics applications. The project supports different C++ libraries for creating control applications over different proprietary operating systems (e.g. Win32, Mac OS). Also includes the RealTime Toolkit (RTT) library for writing hard real-time control applications in C++ for Linux based systems, and tools from contributors to generate components using RealTime Workshop from Matlab/Simulink. To achieve reusability, the framework supports standard component interfaces and CORBA for interoperability between distributed components over a network. Some others not real-time projects have derived from OROCOS, like ORCA (ORCA, 2010) and SmartSOFT (Schlegel, 1999). • RT-Middleware (from Robot-Technology) (Ando et al., 2006; Chishiro et al. 2009) is a CORBA based software platform for robot system integration developed in Japan, with

Open Software Architecture for Advanced Control of Robotic Manipulators

383

the participation of the Japan Robot Association (JARA). One of the objectives of the project is to simplify the construction of customized robot combining selected RTcomponents. In recent years, the Object Management Group (OMG) (OMG, 2008) started a standardization process for these RT-components to achieve interoperability, interconnectivity and integration of components from different manufacturers. • In Korea, the Open Platform for Robotic Services (OPRoS) (Park & Han, 2009) is another open software project promoted to unify different robots platforms. The framework includes standardized components, an integrated development environment (IDE) and a simulation and testing environment. OPRoS supports CORBA and the Universal Plug and Play (UPnP) (Ahn et al., 2006) standards for modular integration. The operational scheme employs a server-client model to interact with the robot system as a target robot, and external servers for heavy computation. • The Coupled Layered Architecture for Robotic Autonomy (CLARAty) (Nesnas et al., 2003) was initiated in the NASA to provide a software framework to develop advanced robotic technologies for robotic platforms employed in other NASA programs. Unlike others architectures, CLARAty is a two-level architecture were the system decomposition allows for intelligent behavior at low levels while the structure of the different levels is maintained. In this scheme, the high level Decision Layer sends command to the Functional Layer and in a client-server model, and the Functional Layer provides different levels of abstractions to achieve adaptation of the reusable components to the hardware of different robots. Also, the Decision Layer provides a unified representation of activity plans based on a declarative model. • For the Mobile and Autonomous Robotics Integration Environment (MARIE), the main goal was to provide a common component-based middleware to reuse and interconnect different programming environments (Cote et al., 2006). The framework followed a oneto-many interaction model between different components to coordinate the interaction within a virtual shared space, and allowing each component to use its own communication protocol. • MIRO (Utz et al., 2002) is a CORBA based middleware organized in three layers: a device layer provides object-oriented interface abstraction for the hardware, and a service layer provides CORBA interface services between the device layer and the top layer. This layer provides reusability and easy integration in an object oriented framework. • In recent years, several RT-Linux based open projects are developed: RTOC (Xu & Jia, 2006) is a RT-Linux based architecture based upon the OSACA model (OSACA, 1996) that can be ported to not PC-based platforms. In its layered model, a database stores universal application modules for control, path planning, etc. Other Linux based platforms use ST-RTL to generate control applications from Simulink models (Ostrovrsnik et al., 2003). Xenomai (Xenomai, 2010) is another Linux-based Real Time operating system used to develop robot control systems using open source and standardized communications protocols (Sarker et al. 2006). The remainder of this paper is organized as follows. Firstly, a brief explanation of the necessity of these platforms is introduced in Section 2. Later, Section 3 describes the hardware structure. In this section the main characteristics of both hardware configurations are presented. In Sec. 4, the software structure is presented, while Sec. 5 presents experimental results which validate the performance of the proposed architecture. Finally, Discussions and Conclusions are presented in Section 6 and 7, respectively.

384


2. Why the necessity of these robotic platforms It has been long recognized that multisensor-based control is an important problem in robotics (Gamez et al. 2008), the need to take advantage of multiple sensors in controlling a system becomes increasingly important. On the other hand, to the purpose of getting an adequate interaction between the manipulator and its environment, force/position feedback control is necessary, above all, if the environment where the robot wants to interact is unknown or changing (Gamez et al., 2005). In general, given the classical hierarchical control structure of a robot microcomputer controller (Groover, 2008) (Fig. 1), the possibilities of control or the integration of new sensors into the setup, are not offered nowadays by the robot manufacturers.

Fig. 1. Classical hierarchical control structure of a robot microcomputer controller. A representative example of implementation of a force/position controller could be the impedance controller (Hogan, 1985). The purpose is to ensure that the manipulator is able to operate in a non-ideally structured constrained environment while maintaining contact forces within suitable limits. A description of this system is sumarized in fig. 2:

Fig. 2. Impedance controller structure. However, an intrinsic problem occurs when trying the application of this control algorithm, if only a wrist force sensor has been used, in a dynamic situation, where the manipulator is


385

moving in either free or constraint space, the interaction forces and moments at the contact point, and also the noncontact ones, are measured by this sensor (Gamez et al., 2004). Furthermore, the magnitude of these dynamics disturbances cannot be ignored when large accelerations and fast motions are considered (Khatib & Burdick, 1986), when the manipulator carries out tasks with heavy tools (Johansson & Robertsson, 2003), or when the environment is not perfectly known (not allowing the use of switching strategies that compensate for the free space phase). To solve this problem, the integration of different sensors such as a force/torque and a acceleration sensor could be use to solve this problem (Gamez et al., 2008; Kroger et al., 2007); however, fusion of data from multiple sensors into a robust and consistent model meets some difficulties such as measurements with different time bases (Luo et al., 2002) or noise and incompleteness of sensor data (Larsson et al., 1996). Another problem could be to easily connect these sensors, which are from diverse manufacturers, to the hardware setup (Gamez et al. 2009). Thus, observing these problems, it can be guessed why a complex dynamic system, such as robotic manipulator, is demanding new and highly sophisticated capabilities that traditional control technology of current industrial robots is not offering (Wills et al., 2001).

3. Hardware elements of the platform This section describes the hardware components that convert this platform in a nonconventional one from an industrial point of view. Also, we will describe the necessity of these elements that were used to test and validate new control concepts for manipulators that interacts with an unknown environments. In this point, it is necessary to point out that two different hardware configurations, and thus two software structure, were carried out. In both cases, the experimental setup contained the following elements: an anthropomorphic 6DOF Stäubli RX60 industrial manipulator and a CS8 controller, a Phantom 6D Haptic Device, a vision system composed of two cameras, a 6-DOF ATI wrist force/torque sensor, a 3-DOF capacitive accelerometer , a 3-DOF gyroscope, a special purpose end effector and the teach pendant, an acquisition board integrated in the robot controller, a workcell and a number PCs to mainly develop software and to collect data. 3.1 Old hardware configuration Initially we designed a hardware scheme that had the structure shown in Figure 3 (Gamez et al., 2009). The kernel of this architecture is the CS8 controller PC. It is in charged of the high-level operations (execution of the path planner, trajectory generation, sercos communication, etc.), and also of reading external sensors such us the wrist force and torque sensor or the acceleration sensor. These elements were connected to the open PCI slot in the controller PC. With this structure, software modules for collecting data where mainly resident in this PC. The main advantage of using a PC-based standard interface is that it ensures that the extensibility and scalability are available. Therefore, the hardware and software components can be integrated or replaced easily. Due to proprietary reasons, the operating system running on this PC is VxWorks (WindRiver, 2005), which allows easy integration of many commercially available add-on peripherals such as acquisition boards, ethernet boards, etc.. It also provides deterministic

386


context switching, timeliness, support for open standard. The external sensors used to model the environment, and thus to make the robot capable of interacting with it, are: a ATI wrist force/torque (f/t) sensor (MINI SI80-4) where the f/t strain gauge signals are conditioned using an intermediate module, called supply board, and later transmitted through a DAQ acquisition board which processes the strain gauge information and offers it through the PCI slot. A 3D accelerometer, which was attached to the end-effector of the manipulator and a 3D gyroscope of CFX Technology (an UCG-TX model). These two last sensors were also read by the same acquisition board.

CS8 Controller Teach Pendant

DAQ (PCI)

CPU Pentium 32 MB RAM

Haptic Device PC

USB Board I/O Ethernet

Servo Controllers (original units)

Force and acc. sensors + Vision Sensors

Ethernet

Host PC

External PC for Computer Vision

Fig. 3. Hardware configuration for the old system. Regarding the vision system, the cameras are connected directly to a dedicated computer vision PC. Later, the image is processed and the required information -normally a vector with coordinates of positions and orientations- sent to the controller PC through ethernet. The haptic device is a PHANTOM 3/6DOF with six degrees of freedom in position and force feedback in three translational degrees of freedom. The 3.0/6 DOF has a range of motion approximating full arm movement pivoting at the shoulder. This device is connected to an extra PC via the parallel port (EPP) interface. The sample time of the haptic device is higher than the controller loop (1 Khz against 250 Hz.), and since there is no physical distance between them, the delay is one controller sample time at maximum. The bandwidth of all the mentioned sensors apart from the vision sensor is 250 Hz. This sample time has been chosen in order to synchronize the sensor readings with the robot control loop. The bandwidth of the vision sensor is smaller (around 30 Hz), because of the high computational effort required and the camera speed. Shortly, we are going to change these cameras to new ones with a bandwidth of 120 frames/sec.


387

3.2 New hardware configuration The main drawbacks that can be found in the former robotic system are: • The master PC, where a huge number of applications are running -sensor readings, control algorithm execution, robot movements- , is placed in the controller PC, so this structure is subject to a PC that in few years is antiquated and cannot be changed (without the manufacturer collaboration). • A great part of the code running in the controller is unknown -belongs to the manufacturer- and sometimes it occurs problems because of the inconsistency between the original code and the experimental one (as uncontrolled modifications of some common data). These problems are difficult to solve, again, without a close collaboration with the manufacturer. Also, while the experimental code is more complex and bigger, the inconsistencies are more probable. • The time synchronization is easier and more robust if we have an external master PC that configures and controls all the sensors, the control algorithms and the actuation system. To solve the problems that the first configuration presented, a new hardware and software structure is being developed. Similar to first one, the main difference can be found in where the main executions are carried out. Figure 4 shows a scheme of this new configuration. MASTER PC Haptic Device DAQ (PCI) Parallel Port

UP TO DATE COMPUTER

Vision Sensors

Force and acceleration sensors CS8 CONTROLLER

IEEE 1394

Ethernet

Fig. 4. Hardware configuration for the new system. With respect to the old system, this new system introduces the following changes: • The operating system is based on LINUX with a real time framework called XENOMAI (Xenomai, 2010). • The vision sensors are read through a IEEE 1394 port placed in the master PC. From the experimental tests, it was checked that the computational effort required by a normal vision system processing does not bother to the rest of the tasks. • The haptic device is connected directly to the Master PC through a parallel port. • The external sensors, i.e. the wrist force sensor, the accelerometer, or other sensors, are connected to a Data Acquisition Board plugged into a PCI slot of the Master PC.

4. Software structure In this section, we describe a component-based control software architecture developed in order to get a robust and easy-to-maintain experimental robotic platform. Two fundamental

388


goals were established for the architecture: first, it should standardize functions that are common across sensors and open robotic platforms; second, the architecture should enable design by composition. Since the most interesting configuration is the new one, we limit this section to its the description. Further information about the old software configuration can be found in (Gamez et al. 2010). 4.1 Layer architecture and component definitions Although the software structure of the experimental setup contains basically two PCs: the master PC and the controller PC, it consists of a hierarchy of components that are divided into four main layers proposed originally by (Nilsson & Johansson, 1999): lowest layer, middle layer, high layer and end-user layer. Each layer contains different types of components, which are classified depending on their functionality (Fig. 5). This components are related, in the major cases, to a block or system of the hardware structure. The four layers are: 1. Lowest layer: whose components correspond to those ones closer to the physical environment. Examples are the different sensor components or the joint control components. 2. Middle layer: Components can use the information of the lowest layer and the high layer. Examples could be a virtual sensor component or a manipulator control component. 3. High layer: Trajectory generator components. 4. End-user layer: Task planner components.

Fig. 5. Structure of the components developed for the platform.


389

The end-user layer describes the task to be carried out in terms of final positions, orientations and velocities of the robot end-effector. Different components have been developed and they are used depending if the task to be carried out is in open space, with constraint motion or with both. In addition, another component has been designed which is in charge of controlling the haptic device. The inputs to the components of this layer can be the reference position-orientation of the robot TCP, the desired contact forces exerted by the manipulator to the environment or even vision features. Currently, these inputs can only be defined off-line, not taking the most significant advantage of on-line programming, that is, the robot can be programmed in accordance with the actual position of equipment and pieces of these modules. The high-level layer is compound basically of two components with the functions of a path planner. This planner generates trajectory set points for the robot, according to motion command which it receives from task specification. The commands these components offer to their lower layer can be either the joints trajectory or the Cartesian trajectory of the robot end-effector. It is necessary to point out that both the original task planning and the original trajectory generator developed by Stäubli were not used in this platform due to proprietary reasons. For our applications, the components designed for the special-purpose planner calculate the joint coordinates from the Cartesian references solving the inverse kinematics on line (Gamez, 2006). In this sense, a second component has been developed to reduce the computational cost of the previous block if necessary. Specifically, it consists of the decomposition of the robot geometric structure into two subsystems: one for position and one for orientation. This option offers an analytic solution that simplifies the singularities problem. Furthermore, a number of restrictions have been imposed to prevent special singularities such as shoulder and wrist ones. Although the developed trajectory is not robust, the resultant workspace is acceptable for most of the practical cases. Currently, these components are used from the former configuration and, in the future, we expect to improve them using more sophisticated trajectory generators than can be found, for instant, in (Bruyninckx, 2001). For the middle-level layer, and from an engineering point of view, we note that tailoring the motion control requires control engineering competence while application support does not (Nilsson & Johansson, 1999). Although is therefore reasonable and appropriate to define two different sub-layers for these types of programming: application control layer, (movement constraints, tool mass, etc.) and control layer (to configure the control loop, tunes the gains, etc.), this level is built, on the one hand, using manipulator control components. On the other hand, other kind of components that are used in this layer are the virtual sensor components. These elements allowed the application of sensor fusion strategies in a structured way. Both components are designed with Simulink and the Real Time Toolboox of Matlab. Using the property that any Simulink control model is an interconnection of signals (reference commands, position feedback, velocity feedback, torque feedback, sensors feedback) and mathematical operations, a generic block has been designed with a predefined number of inputs and outputs. Inside each block, one can implement different control algorithms, or sensor fusion strategies, combining a high-performance language for technical computing with a fast prototyping of the robotic platform since all the inputs and outputs are readable and writable.

390


In the lowest layer dedicated to the sensor, each one is modeled by a component that contains basically two parts: one is for data structure building and the other is for sensory data sharing. One of the function of the sensor components is to process the information of a specific sensor and to provide a unified sensory data bank manager. The main advantage of this manager is that it can directly offer the calibrated sensor data. Furthermore, sensor data must be shared with every necessary function in the software architecture. Another important function of the sensor components, and on the rest of components, is to stamp a time when a set of measurements, or interaction, is obtained. It helps to obtain a history of the events.

Fig. 6. Low level structure of the joint controllers. Regarding the joints control sub-layer, it uses a low level interface designed by Stäubli Robots (Pertin & Bonnet, 2004); in fact, this is the only software module that remains from the original Stäubli system. This level obeys the structure presented in Fig. 6 and its mission is to allow the low level control of each joint. Although three different components were defined in the previous software structure (given their possibility of control: torque, position and velocity controller), only a generic one is considered in this structure. This sub-layer is placed currently in the robot controller PC and we are working on how to define the component automatically -in terms of torque, position or velocity-, given the programming of manipulator control component. 4.2 Middleware In our case, we have to different software contexts: this one placed at the controller PC and the second one running on the master PC. In the controller PC, where the component of the joints control sub-layer is running, to guarantee that the shared memory constraints are fulfilled, the system has to protect itself from invalid memory accesses that otherwise could compromise the system. In this case, to avoid this problem, between the component and the monitoring task, the tasks are synchronized following a structure "top to bottom" where the maximum priority is given to the joints control task. The operating system running on this PC was VxWorks (Wind-River, 2005). Another problem was to synchronize different components that are placed in a master PC with interconnections with external systems and a Real Time Linux operating system. The solution selected was to choose XENOMAI (Xenomai, 2010) with the RTNet (Real Time


391

Network) package. For our case, the synchronization scheme is not based on a master clock (as it was in the former configuration, where the it followed a "top to bottom" structure). Each component has its own clock, updating data with their respective bandwidth. Currently, we are implementing a middleware using concepts similar to those ones defined in OROCOS project (Bruyninckx, 2001). In a middle-long term, our intention is to obtain a user-friendly API that allows fast and easy prototyping. Figure 7 shows the block diagram of the hardware and software communications differentiating between the master PC and the controller PC. It can be guessed from this figure that each component communicates with other ones mainly through shared memory, or through Ethernet depending on where it is placed.

Fig. 7. Block diagram of the hardware-software communications.

5. Experimental validation Different experiments have been carried out to validate the performance of the proposed architecture, noting that these results are obtained from the old hardware configuration. They consisted in the application of a compliant motion controller where the environment

392


information was obtained fusing different sensors. In particular, for the case shown in this paper, the sensors used were a force/torque sensor, an accelerometer and the joint sensors. The objective of this integration was to develop a force observer capable of estimating, accurately and from the f/t sensor measurements -which reflect the contact forces, the inertial ones and the gravity forces- , the contact force exerted by a manipulator to its environment (Gamez et al., 2008). To carry out this test, some of the components that were described before in the previous section were used.

Fig. 8. Force measurement from the wrist sensor ATI (upper-left), force observer output (upper-right), accelerometer output (lower-left) and real and measured position of the robot tip for y-axis (lower-right). The results obtained applying the force/position controller, where the information of the force observer is used, are presented in Fig. 8. The experiment consisted of a movement in the axis z of three phases: an initial movement in free space (from t =5s to t = 6.2s), a contact transition (from t =6.2s to t = 6.4s) and a movement in constrained space (from t =6.4s to t = 9s). Apart from the force compensation shown in Fig. 8, it can be also compared how the observer eliminates the inertial effects and the noise introduced by the sensors. Note the time lag between the filtered signal and the original one. It was because the selection of the gains made the poles of observer to be quite near the unit-circle (Gamez, 2006). The force control loop applied was an impedance controller (Hogan, 1985).


393

6. Discussion The construction of the open robotic system developed in the framework of this work was necessary because, as it is well known, industrial manipulators do not offer, with an appropriate bandwidth, the possibility of integrating either advanced control algorithms or new sensors into the software-hardware architecture. This fact forced the research community to extend an industrial manipulator architecture in order to get a completely open one in both senses: hardware and software. The proposed platform was designed considering a multi-layer structure that simplified the integration of external functionality in several ways. The first one consisted of offering different interfaces where the user was capable of reading all the parameters and variables, besides modifying the commanded signals, with a considerable bandwidth. The second one pretended to avoid the limitation of the industrial robots where the current methodology is to control exclusively the position without considering high level strategies for task decision making, or without taking into account new sensors that could improve the environment modelling. Certainly, a pending aspect of this platform is the the man-machine interaction. New solutions in the area of software technology have to be included in order to create a more friendly-interface that permits to modify easily the requirements of the system, especially for the experiment generation. Perhaps, creating pseudo intelligent task interpreters, as a experimental interface, will play an important role. Furthermore, it has to be pointed out that the design solutions have been driven following a trade-off between mass products (paying attention to the cost) and standardization requirements (leading edge technology). On the other hand, the proposed architecture was based on consolidated open robotic platforms, specially on those ones developed in Lund University (Sweden) and Leuven University (Belgium). These platforms have been developed during several decades and have accumulated a great deal of experience, representing an excellent paradigm for initial developments. In addition, a narrow collaboration with the company of the manipulator robot has existed, what allowed access to internal functions and hardware what would be impossible in other conditions. To conclude this section, the development of this kind of platforms does not only prove to be useful for testing advance control algorithms, but also it is necessary to emphasize the necessity of building such systems since, from a robotic research point of view, and mainly, from a robotic manufacturers overview, it helps to increase the development speed opening up the systems for third party.

7. Conclusions This work describes an experimental platform that allows the implementation of modelbased and sensor-based control algorithms in robotic manipulator. Particulary, this new system allows to easily integrate new sensors and advance control algorithms in an Stäubli industrial 6-dof robot using a component-based software methodology. Based on a component-based development approach, two possible configurations were described, explaining why the original structure was modified migrating to a new one where the Master PC was different to the controller PC. It is also explained how the fact of using this paradigm allowed an easy reconfiguration of the robotic platform, demonstrating

394


that the use of components -i. e. Sensor components- , that are independent of the context, allowed as well an important restructuration of the new robotic architecture. The resulting architecture has been designed, among other objectives, to allow different sensors to be easily switched and rewired depending on the new sensor fusion or control strategy that must be tested. Together with the component-based development approach, a software structure of layers has been proposed to facilitate the design, configuration and testing of new control algorithms and sensor fusion techniques. This structure allows systems of components, with standardized interfaces, to be connected while abstracting away implementation details of components. Eventually, a number of experiments were performed to validate the performance of the proposed architecture and its capacity of allowing a fast and easy implementation of advance control algorithms in non-structured environments.

8. References Ahn, S. C., Lee, J.-W., Lim, K.-W., Ko, H., Kwon, Y.-M. & Kim, H.-G. (2006). Requirements to UPnP for Robot Middleware, Proc. of the 2006 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), Beijing, pp. 4716 -4721. Ando, N., Suehiro, T., Kitagaki, K. & Kotoku, T. (2006). RT (Robot Technology)-Component and its Standarization, SICE-ICASE International Joint Conference, 2006, pp. 26332638. Bruyninckx, H. (2001). Open robot control software: the OROCOS project. Proc. of the 2001 IEEE Int. Conf. on Robotics and Automation (ICRA), pp. 2523–2528. Chishiro, H., Fujita, Y., Takeda, A., Kojima, Y., Funaoka, K., Kato, S. & Yamasaki, N. (2009). Extended RT-Component Framework for RT-Middleware, IEEE International Symposium on Object/Component/Service-Oriented Real-Time Distributed Computing, 2009 (ISORC), pp. 161-168. Cote, C., Brosseau, Y., Letourneau, D., Raievsky, C. & Michaud, F. (2006). Robotic Software Integration Using MARIE, International Journal of Advanced Robotic Systems, 2006, pp. 055-060. Gamez, J., Robertsson, A., Gomez, J. & Johansson, R. (2004). Sensor fusion of force and acceleration for robot force control. Int. Conf. Intelligent Robots and Systems (IROS 2004), 2004, pp. 3009–3014. Gamez, J., Robertsson, A., Gomez, J. & Johansson, R. (2005). Force and acceleration sensor fusion for compliant robot motion control. IEEE Int. Conf. on Robotics and Automation (ICRA2005), 2005, pp. 2709 - 2714. Gamez, J. Sensor Fusion of Force. (2006). Acceleration and Position for Compliant Robot Motion Control. Phd thesis, Jaen University, Spain, 2006. Gamez, J., Gomez, J. Nieto, L. & Sanchez Garcia, A. (2007). Design and validation of an open architecture for an industrial robot control, IEEE International Symposium on Industrial Electronics (IEEE ISIE 2007), 2007, pp. 2004–2009. Gamez, J., Robertsson, A., Gomez, J. & Johansson, R. Sensor fusion for compliant robot motion control. IEEE Trans. on Robotics, 2008, pp. 430–441. Gamez, J., Gomez, J., Sanchez, A. & Satorres, S. (2009). Robotic software architecture for multisensor fusion system. IEEE Trans. on Industrial Electronics, 2009, pp. 766–777. Groover, M. P. (2008). Automation, Production Systems and Computer-Integrated Manufacturing. Pearson Education, Upper Saddle River, New Jersey, USA, 2008.


395

Hogan, N. (1985). Impedance control: An approach to manipulation, parts 1-3. J. of Dynamic Systems, Measurement and Control. ASME, 1985, pp. 1–24. Hong, K. Kim, J. Huh, C., Choi, K. & Lee, S. (2001). A pc-based open robot control system: PC-ORC. IEEE International Symposium on Industrial Electronics, ISIE 2001. 2001, pp. 1901 –1906. Johansson, R. & Robertsson, A. (2003). Robotic force control using observer-based strict positive real impedance control. IEEE Proc. Int. Conf. Robotics and Automation, 2003, pp. 3686–3691. Khatib, O. & Burdick, J. (1986). Motion and force control of robot manipulators. IEEE Int. Conf. Robotics and Automation, 1986, pp 1381– 1386. Kröger, T., Kubus, D. & Wahl, F. (2007). Force and acceleration sensor fusion for compliant manipulation control in 6 degrees of freedom. Advanced Robotics, 2007, pp. 1603– 1616. Larsson, U., Forsberg, J. & Wenersson, A. (1996). Mobile robot localization: integrating measurements from a time-of-flight laser. IEEE Trans. Industrial Electronics, 1996, pp. 422–431. Luo, R., Yih, C. & Su, K. (2002). Multisensor fusion and integration: approaches, applications, and future research directions. IEEE Sensors J.,2002, pp. 107–119. Nesnas, I., Wrigh, A., Bajracharya, M., Simmons, R. & Estlin, T. (2003). CLARAty and Challenges of Developing Interoperable Robotic Software, Proceedings of the 2003 IEEE/RSJ. Intl. Conference on Intelligent Robots and Systems, 2003, pp. 2428 – 2435. Nilsson, K. & Johansson, R. (1999). Integrated architecture for industrial robot programming and control. J. Robotics and Autonomous Systems, 1999, pp. 205–226. OMG Robotic Technology Component Specification, formal/08-04-04 edition. Object Management Group, 2008. ORCA, http://orca-robotic.sourceforge.net/, 2010. OROCOS-Simulik, http://www.orocos.org/simulink/, 2010. OSACA, Open System Architecture for Controls within Automation Systems, ESPRIT III Project 6379/9115, 1996. Ostrovrsnik, R., Hace, A. & Terbuc, M. (2003). Use of open source software for hard realtime experiments, IEEE International Conference on Industrial Technology, 2003, pp. 1243 – 1246. Park, H. & Han, S. (2009). Development of an Open Software Platform for Robotics Services, ICCAS-SICE Int. Joint Conference, 2009, pp. 4773 – 4775. Pertin, F. & Bonnet des Tuves, J. (2004). Real time robot controller abstraction layer. Proc. Int. Symposium on Robots (ISR), Paris, France, March 2004. Saravanan, K., Thangavelu, A. & Rameshbabu, K. (2009). A middleware architectural framework for vehicular safety over vanet (InVANET). International Conference on Networks & Communications, 2009, pp. 277 – 282. Sarker, M., Kim, C., Cho, J. & You, B. (2006). Development of a Network-based Real-Time Robot Control System over IEEE 1394: Using Open Source Software Platform, IEEE International Conference on Mechatronics, 2006, pp. 563 – 568. Schlegel, C. & Worz, R. (1999). The Software Framework {SMARTSOFT} for implementing Sensorimotor Systems, Proc. IEEE Int. Conf. Intelligent Robots and Systems, 1999, pp 1610-1616.

396


Utz, H., Sablatnög, S., Enderle, S. & Kraetzschmar, G. (2002). MIRO - Middleware for mobile robot applications, IEEE Transactions on Robotics and Automation, 2002, pp. 493 – 497. Wills, L., Kannan, S., Sander, S., Guler, M., Heck, B., Prasad, J., Schrage, D. & Vachtsevanos, G. (2001). An Open Platform for Reconfigurable Control, IEEE Control Systems Magazine, 2001, pp. 49 – 64. Wind-River. VxWorks: Reference Manual. Wind River Systems, 2005. Wind-River Linux. http://www.windriver.com/products/linux/, 2010. Xenomai: Real-Time Framework for Linux. http://www.xenomai.org/, 2010. Xu, H. & Jia, P. (2006). RTOC: A RT-Linux Based Open Robot Controller, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006, pp. 1644 – 1649. Xuemei, L. & Liangzhong, J. (2007). Study on control system architecture of modular robot. Proc. of the 2007 IEEE Int. Conf. on Robotics and Biometrics, 2007, pp. 508 – 512.

19 Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 1School

Y. Cao1, Y. W. Li2 and Z. Huang2

of Mechanical Engineering, Jiangnan University, 1800 Lihu Avenue, Wuxi, Jiangsu, 214122, 2Robotics Research Center, Yanshan University, Qinhuangdao, Hebei, 066004, China 1. Introduction During the past two decades, parallel manipulator system has become one of the research attentions in robotics. This popularity has been motivated by the fact that parallel manipulators possess some specific advantages over serial manipulators, i.e., higher rigidity and load-carrying capacity, better dynamic performance and a simpler inverse position kinematics, etc. Among various manipulators, the best-known is the Gough-Stewart Platform (GSP) that was introduced as a tire performance (Gough 1956-57) and an aircraft simulator (Stewart 1965). One of the important problems in robot kinematics is special configuration or singularity. As to parallel manipulators, in such configurations, the end-effector keeps at least one remnant freedom while all the actuators are locked. This transitorily puts the end-effector out of control. Meanwhile, the articular forces may go to infinity and cause mechanical damages. Determination of the special configurations of the six-DOF Gough-Stewart parallel manipulators is a very important problem. It is one of the main concerns in the analysis and design of manipulators. The singularity analysis of parallel manipulators has attracted a great deal of attention in the past two decades. Hunt (1983) first discovered a special configuration for this manipulator that occurs when the moving triangle-platform is coplanar with two legs meeting at a vertex of the triangle, and all the six segments associated with six prismatic actuators intersect a common line. Fichter (1986) discovered a singularity of the parallel manipulator. That occurs when the moving platform rotates ψ =±π/2 around Z-axis, whatever the position of the moving platform is. That mechanism has a triangular mobile platform and a hexagonal base platform. It may be named a 3/6-GSP. Huang and Qu (1987) and Huang, Kong and Fang (1997) also studied the singularity of the parallel manipulator, whose moving and basic platforms are both semi-regular hexagons (6/6-GSP). It also occurs whenψ= ±π/2. Merlet (1988, 1989) studied the singularity of the six-DOF 3/6-GSP more systematically based on Grassman line geometry. He discovered many new singularities including 3c, 4b, 4d, 5a and 5b. 3c occurs when four lines of the six legs intersect at a common point; 4b occurs when five lines are concurrent with two skew lines; 4d occurs when all the five lines are in one plane or pass through one common point

398


in that plane; 5a is in general complex; 5b occurs when the six segments cross the same line. Based on line geometry, wrench singularity analyses for platform devices have been presented by Collins & Long (1995), and Hao & McCarthy (1998). Gosselin and Angeles (1990) pointed that singularities of closed-loop mechanisms can be classified into three different groups based on the Jacobian matrices. This classification was further discussed by Zlatanov, Fenton and Benhabib (1994, 1995). Zlatanov, Bonev and Gosselin (2002) discussed constraint singularities. Ma and Angeles (1991) studied architecture singularities of parallel manipulators. Kong (1998) also discussed architecture singularities of the general GSP. McAree and Daniel (1999) discussed the singularity and motion property of a 3/3-parallel manipulator. Karger and Husty (1998), Karger (2001) described the singular positions and self-motions of a special class of planar parallel manipulators where the platform is similar to the base one. It is shown that it has no self-motions unless it is architecturally singular. Kong (1998), Kong and Gosselin (2002) also studied self-motion. Chan and Ebert-Uphoff (2000) studied the nature of the kinematic deficiency in a singular configuration by calculating the nullspace of the Jacobian matrix. Di Gregorio (2004) studied the SX-YS-ZS Structures and Singularity. Many researches dealt only with isolated singular points in space. However, in the practical configuration space of parallel manipulators the singularity configuration should be a continuous singularity curve or even be high-dimension surface. One of the main concerns is further to find out its singularity loci and their graphical representations, as well as the structure and property of the singularity loci. That is of great significance in a context of analysis and design since it allows one to obtain a complete picture of the location of the singular configurations in the workspace. For a given practical application, it is therefore easy to decide whether the singularities can be avoided. Sefrioui and Gosselin (1994, 1995) studied singularity loci of planar and spherical parallel mechanisms. Wang and Gosselin (1996, 1997) used the numerical method to study the singularity loci of spatial four- and fiveDOF parallel manipulators. Collins and McCarthy (1997, 1998) studied singularity loci of the planar 3-RPR parallel manipulator, and 2-2-2 and 3-2-1 platforms and obtained cubic singularity surfaces. For the six-DOF Gough-Stewart Platform, however, the singularity expression generally is quite complicated, and difficult to analyze. Recently, Wang (1998) presented a method to analyze the singularity of a special form of the GSP and derived corresponding analytical singularity conditions. Di Gregorio (2001, 2002) also discussed the singularity loci of 3/6 and 6/6 fully-parallel manipulators. In particular, Mayer St-Onge and Gosselin (2000) analyzed the Jacobian matrix of general Stewart manipulators by two different new approaches. They derived a simpler explicit expression from the Jacobian matrix, and pointed out that the singularity locus of the general Gough-Stewart manipulator should be a polynomial expression of degree three. They also gave the graphical representations of the singularity loci. For practical application, we want to obtain a simpler algebra expression of the singularity loci, their accurate graphical representations and know whether it consists of some typical geometrical figures. But this is very difficult for the Gough-Stewart manipulator. Huang et al. (1999, 2003) studied the singularity kinematics principle of parallel manipulators, and proved a new kinematics sufficient and necessary condition to determine the singularity. Using this method he discovered the characters of singularity locus of the 3/6-GoughStewart platform firstly. It shows that the singularity locus of the 3/6-Gough-Stewart platform is resolvable and consists of two typical geometrical graphs, a plane and a

Structure and Property of the Singularity Loci of Gough-Stewart Manipulator

399

hyperbolic paraboloid, for the special orientations: φ=±30°, ±90°, or ±150°. However, the singularity locus expression of degree three is irresolvable, and the locus graph in infinite parallel principal sections includes a parabola, four pairs of intersecting straight lines and infinite hyperbolas for the general orientations: φ≠±30°, ±90°, and ±150°. For the singularity loci of the 6/6-GSP which is a more general structure form and widely used in practice, its graphical representations of the singularity loci for different orientations are quite various and complex. Huang and Cao (2005) analyzed the singularity loci both in 3-D space and in the principal-section on which the moving platform lies. The singularity locus equation of this class of Gough–Stewart manipulators in three-dimensional space is also irresolvable, and the curves in infinite parallel principal sections of the singularity loci also contains one parabola, four pairs of intersecting straight lines, and infinite hyperbolas. We also found out an incredible phenomenon, in that special configuration six lines associated with the six extensible links of the 6/6-Gough-Stewart manipulator can intersect the same common line and the remnant instantaneous motion of the manipulator is a pure rotation. All the above-mentioned analyses are only about positional singularity when the orientation of the moving platform is specified and invariable. On the other hand, there is a need to further find out the orientation-singularity space when the position of the moving platform is specified and invariable. Some researchers began to study the issue, such as Pernkopf and Husty (2002); Cao, Huang & Ge (2006). Of course, for this topic there is still much work to be done in depth.

2. The kinematics principle and linear-complex classification 2.1 The classification of singularity by linear-complex A general algebraic equation for a linear complex (Hunt 1978; Ball 1900) is: a1 P + a2Q + a3 R + a4 L + a5 M + a6 N = 0

(1)

where the six coefficients denote a twist screw $m = ( a1 a2 a3 ; a4 a5 a6 )

(2)

a1 a4 + a2 a5 + a3 a6 a12 + a22 + a32

(3)

$ = (L M N ; P Q R )

(4)

LP + MQ + NR = 0

(5)

Its pitch is hm = Its reciprocal screw satisfying Eq. (1) is

and we have

where $ denotes a line vector. The infinite line vectors satisfying Eq. (1) composed a line complex.

400


$m

α

Fig. 1. Linear complex In a linear complex (Hunt 1978), those lines that pass through any pole must all lie in the same polar plane; those lines that lie in any polar plane must all intersect the same point. Fig. 1 shows the pole and polar plane of a linear complex with the pencil of lines in α. All the lines that pass through the pole are normal to the helix. The linear complex can be divided into three parts according to its pitch hm: when hm is finite and nonzero, it is a general linear complex; when hm=0，this is the first special linear complex, in which all the coaxial helices collapse into homocentric circles with a common axis $m and all the lines of the complex intersect $m or parallel to it; and when hm=∞, this is the second special linear complex, in which all the lines of the complex comprise planar fields of lines in all planes normal to the direction $m, and $m is no longer occupying a specific line. The last two forms are associated respectively with pure rotation and pure translation. All singularities of the Gough-Stewart parallel mechanism belong to the linear-complex singularity. From this point of view, the singularity can be divided into three kinds with different instant output motion: (1) The general Linear-Complex Singularity. The possible motion of end-effector is a twist with hm is finite and nonzero; (2) The First Special Linear-Complex Singularity. The possible motion of end-effector is a pure rotation with hm=0; (3) The Second Special Linear-Complex Singularity. The possible motion of end-effector is a pure translation with hm=∞. 2.2 The kinematic principle of singularity First of all, let us discuss the velocity relationship of three points in a moving body. The following issue is to introduce the principle of a novel method analyzing the singularities of parallel manipulators (Huang et al. 1999; 2003; Ebert-Uphoff et al., 2000; Kong and Gosselin 2001). Let us consider any non-collinear three points in a rigid body, and then we may deduce the following theorem: Theorem 1: Three velocities of three points in a moving body have three normal planes at the corresponding three points. In general, the three planes intersect at a common point, and the intersecting point necessarily lies in the plane determined by the three points.


401

Fig. 2 The velocity relationship of three non collinear points in a moving body Theorem 2: When three velocity directions of three points in a rigid body are given, then three normal planes of the three velocities are determined. If the intersecting point of the three planes lies in the plane determined by the three points, the three velocities can determine a twist; otherwise, the given velocities are improper and cannot determine a twist of that body. The thinking of the velocity analysis in the proof of Theorem 2 itself is also useful for singularity study of the 3/6-GSP. The 3/6-GSP is a typical manipulator which many authors paid attention to. The 3/6-GSP is represented schematically in Fig.3. It consists of a mobile platform B1B3B5, equilateral triangle; a base platform C1…C6, semi-regular hexagon; and they are connected via six extensible prismatic actuators. When all the legs of 3/6-GSP are locked, the three normal planes of three velocities VB1, VB3 and VB5 are respectively B1C1C2, B3C3C4 and B5C5C6 (Fig.3). According to Theorem 2, we may educe the following deduction to determine the singularity of 3/6-GSP. Let us firstly define a “Star-frame C-B1B3B5” in the moving platform. It is constructed by using three ray lines passing three points, B1, B2 and B3, of the triangle B1B3B5 and intersecting at a common point C called the center of Star-Frame.

a) A 3/6–Stewart manipulator Fig. 3. A 3/6-Stewart parallel manipulator

b) Its top view

402


Theorem 3: A necessary and sufficient condition that the three velocities of three points in a rigid body can express that the body has a possible twist motion is that the intersecting point of three normal planes of the three velocities lies in the plane determined by the three points.

3. Structure and property of singularity loci of 3/6-Gough-Stewart for special orientations The kinematics method can determine the singularity of the manipulator. If the six extensible legs of the GSP are locked and the mechanism has an instantaneous freedom, the manipulator is singular. Now, let us firstly discuss the kinematics properties of the typical singularity structures including singularities: 3c, 4b, 4d, 5b (Merlet 1988; 1989) and others. 3.1 Singularity hyperbola equation derived in an oblique plane Our task is to find the whole singularity loci of the GSP and identify their structure and property. It is of an important and difficult issue. Here three Euler angles φ, θ and ψ are used to represent orientation of the mobile in terms of a rotation φ about Z-axis, then a rotation θ about the new Y′-axis, and finally a rotation ψ about the new Z″-axis. In order to find the whole singularity loci and solve the issue, we first study the singularity equation in a special plane (Huang et al. 2003). The issue is divided into two parts: (1) When the first Euler angle φ is equal to one of the following values, ±30°, ±90°, and ±150°, it is a special orientation cases and easier to analyze. (2) When φ is any value with the exception of ±30°, ±90°, and ±150°, this is the general case. Now, we solve the equation for singularity curve of the 3/6-GSP in a certain plane while the orientation of the mobile is provisionally set to φ=90°, ψ=0and θ is any finite nonzero value. The parameters of the parallel manipulator are as follows. The circumcircle radius of the basic hexagon platform is Ra, and the one of the triangle mobile is Rb; β0 denotes the central angle of the circumcircle of the basic hexagon corresponding to side C1C2. Point P is the geometric center of the mobile (Fig. 3). The stationary frame O-XYZ is fixed to the base and the moving frame P-X′Y′Z′ is attached to the mobile. Fig. 4 shows the position after the mobile rotates (90°, θ, 0). The oblique plane in which the moving platform lies intersects the basic plane at line UV, which is parallel to axis X. For the orientation, B1P(Y′)is parallel to A5A1 (X).At first, providing that point P is located at a special point C0 in the perpendicular bisector of UV, and the distance from O2 to point C0 is equal to that between point O2 and A3, then we deduce that C0B3 and A3A1 intersect at point V, and C0B5 and A3A5 intersect at U. In that case, the mechanism is singular according to Deduction 2. The included angle between the oblique plane and the basic one is θ. In order to conveniently express the oblique plane below, we call it θ-plane. Let us suppose that the mobile translates to the position B11B31B51 in θ-plane and line B11P intersects line O2C0 at C. If line B31C intersects A1A3 at point V, and line C1B51 intersects A3A5 at point U, the mechanism is also singular (Deduction 2). We can prove that the center of Star-frame always lies in line O2C0 for the orientation. In general, the singularity is a general-linear-complex singularity. Based on the analysis above, we study the singularities of 3/6–GSP when the mobile translates arbitrarily in θ-plane. The coordinates of point C0 and O2 with respect to the fixed frame are (0, Y0, Z0) and (0, u, 0), respectively. The frame O2-xyz is attached to θ-plane. It should be noticed that angle θ as shown in Fig. 5 about the Y′-axis is negative.


X' B3

y P C

403

B1

B51 B3 Y' Y A3

B1 C0 P B5 X x V A1 O O θ O 2 A5

U

Fig. 4. θ- Oblique plane for the orientation (90°, θ, 0) The coordinates of points P, C, B31 and V with respect to O2-xyz are P : ( x , y , 0) C : (0, y , 0) Rb 3 , y+ Rb , 0) 2 2 3 Z0 V : (− , 0, 0) 3 sin θ Considering O2C0=O2A3, we can obtain B31 : ( x −

(6)

O2O1 − OO1 + OA3 = O2C 0

(7)

namely

3 Ra cos( β 0 / 2) − Z0

cosθ Z − Y0 = − 0 sin θ sin θ

(8)

In the right-angled triangle ΔO1O2C0, we obtain Y0 − u = −Z0

cosθ sin θ

(9)

Solving Eqs. (8) and (9) for Y0 and Z0, we obtain Y0 = u(1 − cosθ ) + 3 Ra cos( β 0 / 2)cosθ Z0 = u sin θ − 3 Ra cos( β 0 / 2)sin θ

(10)

Provided that the coordinates of an arbitrary point in line B31Vare ( xx , y y , 0) , its equation is written as

404


3 R Rb xx − x + b 2 2 = 3 3 Z0 R −y − − −x+ b Rb 2 3 sin θ 2

yy − y −

(11)

Since point C lies in line B31V, substitute the coordinates of point C ( xx = 0 and y y = y ) into Eq. (9) and simplify as xy −

Rb ZR y− 0 b =0 2 2 sin θ

(12)

Substituting Eq. (10) into Eq. (11) and eliminating Z0 , yield xy −

Rb (3 Ra cos( β 0 / 2) − u)Rb =0 y+ 2 2

(13)

Eq. (13) denotes a hyperbola and is independent of the Euler angle θ. The coordinates of its center are (Rb/2,0), and its vertical and horizontal asymptotes are x= Rb/2, y=0 This is an important conclusion, as we have known that the singularity equation of GSP in 3dimension space is a polynomial expression of degree three. However, equation (13) is only a quadratic equation in the special θ-plane. Eq. (13) only contains variables x and y, so it denotes the positions of point p when the mechanism is singular. The equation is termed the equation of the singularity curve in θ-plane. When orientation of the mobile is given by three Euler angles (-90°, θ, 0), the singularity equation can also be obtained in θ-plane with respect to the frame O2-xyz, the same as in Fig. 4. xy +

Rb (3 Ra cos( β 0 / 2) − u)Rb =0 y− 2 2

(14)

When parameters of the mechanism are set to Ra = 2 , Rb = 1 , β 0 = 900 and u = −2 , the

hyperbolas denoted by Eqs. (13) and (14) are illustrated in Fig. 6(a). Since the result comes from the above-mentioned Theorem and satisfies the necessary and sufficient condition of singularity, so that there is no any singularity except the points on hyperbolas in that θ-plane.

(a) For the orientation ( ±900 θ 0) Fig. 5. The singularity curve in θ- plane

(b) For the orientation ( ±900 θ 600 )


405

3.2 The singularity equation derived in three-dimensional space Eqs. (13) and (14) are deduced by geometric method in the oblique plane. By Theorem 3, we can analyze the distribution properties of the singularities of 3/6-GSP in three-dimensional space. The coordinates of point Bi (i=1, 2, 3) of the mobile are denoted as Bi':(Bix', Biy', Biz') in the moving frame, and Bi:(Bix, Biy, Biz) in the fixed frame; the coordinates of point Cj are denoted as (Cjx, Cjy, Cjz) in the fixed frame. The transformation matrix T of the moving frame with respect to the fixed one can be written using Euler angles φ, θ and ψ as ⎡ cos φ osθ cosψ − sin φ sinψ − cos φ cosθ sinψ − sin φ cosψ cos φ sin θ X ⎤ ⎢sin φ cosθ cosψ + cos φ sinψ − sin φ cosθ sinψ + cos φ cosψ sin φ sin θ Y ⎥ ⎥ [ T ] = ⎢⎢ − sin θ cosψ Z ⎥ sin θ sinψ cosθ ⎢ ⎥ 0 0 1 ⎢⎣0 ⎥⎦

(15)

where(X, Y, Z) are the coordinates of point p with respect to the fixed frame. The coordinates of point Bi in the mobile with respect to the fixed frame are

⎧Bix ⎫ ⎧Bix′ ⎫ ⎪B ⎪ ⎪B′ ⎪ ⎪ iy ⎪ ⎪ iy ⎪ = T [ ] ⎨ ⎬ ⎨ ⎬ , i = 1, 2, 3 B ⎪ iz ⎪ ⎪Biz′ ⎪ ⎪1 ⎪ ⎪1 ⎪ ⎩ ⎭ ⎩ ⎭

(16)

3.2.1 Singularity equation for orientation (90°, θ, 0) When three Euler angles are 900, θ and 0, respectively, from Eq. (16), we can obtain coordinates of three points Bi (i = 1, 2, 3) in the mobile with respect to the fixed frame. Thus

three equations of three normal planes B1C1C2, B3C3C4 and B5C5C6 and the one that the mobile belongs to can be written by the coordinates of the three corresponding points. The equation of plane B1C1C2 is x − B1 x

y − B1 y

z − B1 z

C 1 x − B1 x C 1 y − B1 y

C 1 z − B1 z = 0

C 2 x − B1 x C 2 y − B1 y

C 2 z − B1 z

(17)

where x, y and z are the coordinates of moving point in plane B1C1C2 with respect to the fixed frame. Substituting coordinates of points B1, C1 and C2 into the above equation, we obtain Z y −Y z = 0

(18)

Similarly, the equation of plane B3C3C4 can be obtained (-3Rb sin θ + 2 3Z )x + (2 Z - 3 Rb sin θ )y + ( - 2Y + 6 Ra cos(β0 / 2) - 3Rb cosθ + 3 Rb - 2 3X )z - 6Z Ra cos(β0 / 2) + 3 3 Ra Rb sin θ cos(β0 / 2) = 0

(19)

406


The one of plane B5C5C6 is: (-3Rb sin θ − 2 3Z )x + (2 Z + 3Rb sin θ )y + ( − 2Y + 6 Ra cos(β0 / 2) + 3 Rb cosθ − 3 Rb +2 3X )z − 6Z Ra cos(β0 / 2) − 3 3 Ra Rb sin θ cos(β0 / 2) = 0

(20)

The one of plane B1B3B5 is (sin θ )y+(cosθ )z − (sin θ )Y − (cosθ )Z = 0

(21)

Note that, the equations of these planes are on the same condition that point P (X, Y, Z) is located at some point and the orientation is denoted by three Euler angles (90°, θ, 0). Solving Eqs. (18), (19) and (20) for x, y and z, then substituting them into Eq. (21) and eliminating x, y and z, we obtain

[(sin θ ) Y + (cosθ ) Z] [ 2 XZ + Rb (sin θ )Y + Rb (cosθ )Z − Rb Z − 3Rb Ra sin θ cos( β0 / 2)] = 0 (22) According to Theorem 3, Eq. (22) denotes the singularity locus of point P for the orientation (90°, θ, 0). Obviously, it includes a plane and a conicoid. The plane equation is (sin θ )Y + (cosθ ) Z = 0

(23)

Eq. (23) denotes that singularity locus of point P is a plane containing line C1C2 or A5A1, namely, X-axis. As the plane and plane B1B3B5 denoted by Eq. (23) have the same normal vector, and when plane B1B3B5 translates and coincides with plane expressed by Eq.(23), the configuration is singular. The case belongs to the Hunt’s singularity and is the first speciallinear-complex singularity explained in Case 5. Eq. (23) shows that the mechanism is singular, wherever point P locates in the plane. The conicoid equation is 2 XZ + Rb (sin θ ) Y + Rb ((cosθ ) − 1)Z − 3Rb Ra sin θ cos( β 0 / 2) = 0

(24)

When θ is constant, Eq. (24) denotes a hyperbolic paraboloid and we will explain later. Eq. (28) also represents a hyperbolic paraboloid. 3.2.2 Singularity equation for the orientation (±90°, θ, ψ)

3.2.2.1. The Derivation of the Equation For the orientation (±90°, θ, ψ), the transformation matrix T is ⎡ −c − d 0 ⎢bd − bc a [ T ] = ⎢⎢ − ad ac b ⎢ 0 0 ⎣⎢0

X⎤ Y ⎥⎥ Z⎥ ⎥ 1⎦⎥

(25)

where a = sin θ ; b = cosθ ; c = sinψ ; d = cosψ Using the same method above, the equations of the three normal planes can be obtained.

(26)


( acRb − Z )y + (bcRb + Y )z = 0 3( 3 adRb − acRb − 2 Z )x + ( 3 adRb − 2 Z − acRb )y + (2Y + 2 3X + 3bdRb − 3cRb − 3dRb − bcRb − 6 Ra cos( β 0 / 2))z + 3Ra cos( β 0 / 2)(2Z + acRb − 3adRb ) = 0 3( acRb + 2 Z + 3adRb )x − (2 Z + 3 adRb + acRb )y + (2Y − bcRb − 6 Ra cos( β 0 / 2) − 3bdRb − 3cRb + 3dRb − 2 3X )z + 3 Ra cos( β 0 / 2)(2 Z + acRb + 3adRb ) = 0

407

(27) (28)

(29)

The equation of plane B1B3B5 is a y + b z − aY − b Z = 0

(30)

Solving Eqs. (27), (28) and (29) for x , y and z , and then substituting them into Eq. (30), the singularity equation is

[(sin θ )Y + (cosθ )Z] ( e Z 2 − f XZ + gYZ + h X − iY + j Z + k ) = 0

(31)

Eq. (31) shows that the singular loci include a plane and a conicoid. The plane equation is the same as Eq. (25). It also represents that in this case all the six lines cross a common line. This case belongs to the first special-linear-complex singularity. The quadratic equation is e Z 2 − f XZ + gYZ + h X − iY + j Z + k = 0

(32)

Eq. (32) is a singularity equation with respect to the fixed frame O-XYZ. When the mobile shown in Fig. 5 rotates an angle ψ about Z″-axis again, its orientation is (90°, θ, ψ). The plane in which the mobile lies is still θ-plane. After the coordinate transformation, the equation of the singularity curve in θ-plane with respect to the frame O2-xyz is ⎧2(sinψ )y 2 + 2(cosψ )xy + Rb sin(2ψ )x + ( −2u sinψ + 6 Ra sinψ cos( β 0 / 2) ⎪ 2 ⎨− Rb cos(2ψ ))y − Rb sinψ + Rb cos(2ψ )(3 Ra cos( β 0 / 2) − u) = 0 ⎪z = 0 ⎩

(33)

It is also a hyperbola. In addition, Eq. (33) is independent of the Euler angle θ. 3.2.2.2. Analysis of the Singularity Property The four invariants Δ , D, I and J of Eq. (32) are f h 2 2 g i 0 0 − 2 6 2 2 2 = Rb sin θ cos 3ψ ≥ 0 Δ= f g j 4 e − 2 2 2 h i j k − 2 2 2 0

0 −

(34)

408


D= 0

−

f 2 g =0 2

0 −

0

0 f 2

g 2

(35)

e

I = 2 sinψ (1 + cosθ ) , J = − sin 2 θ

(36)

The following cases are discussed according to its invariants, in which D is always zero whatever θ and ψ are. 1. If θ≠0, ψ≠±30°, ±90°, and ±150°, then D=0, Δ>0, the singular locus denoted by Eq. (32) is a hyperbolic paraboloid. Generally, the six lines 1, 2, …, 6 belong to a general linear complex when point P locates at the surface. 2. If θ=0, Eq. (32) can be written as 4(sinψ )Z 3 = 0

(37)

When ψ=0 and Z≠0, namely, the orientation is (90°, 0, 0), Eq. (37) is an identical equation and the mechanism is singular whatever the position of point P in threedimensional space is. This is the Fichter’s singular configuration and all the six lines belong to a general linear complex. b. When Z=0, the moving platform and the base are coplanar. The mechanism is also singular whatever Euler angle ψ is. The mechanism holds three remnant freedoms when all the legs are locked. In this case, there exist the first and the second speciallinear-complex singularities. If θ≠0, ψ=±30°, ±90°, or ±150°, then D=0, Δ=0 and J≠0, and the conicoid degenerates into a pair of intersecting planes. For instance, when ψ=±30°, two equations are a.

3.

2 Z − Rb sin θ = 0

(38)

3(sin θ )X − (sin θ )Y − (1 + cosθ )Z − Rb sin θ + 3Ra sin θ cos( β 0 / 2) = 0

(39)

When ψ=-30°, ±90°, or ±150, the conicoid also degenerates into two planes. The singularity cases are similar to the above. 3.2.2.3. Analysis of Other Singularities The singularities discussed above are all for the orientations, (±90°, θ, ψ), of the mobile. In these cases, the intersecting lines between the oblique moving plane and the basic one are parallel to line C1C2 or A1A5, one of the three sides of the triangle A1A3A5. The similar singularities with a plane equation and a quadratic one can also occur when the orientations are as follows 1. The Euler angles are (–150°, θ, ψ) or (30°, θ, ψ) All the intersecting lines between the oblique mobile and the base are parallel to line C3C4 or A1A3. 2. The Euler angles are (150°, θ, ψ) or (–30°, θ, ψ)


409

All the intersecting lines between the oblique mobile and the base are parallel to line C5C6 or A3A5. For the two cases, the singularity equation can also resolve into two parts: one is a plane equation containing the corresponding side CiCj, another is a hyperbolic paraboloid equation, too. When ψ =±30°, ±90°, or ±150°, the hyperbolic paraboloid also degenerates into two planes. However, when the orientation is (φ, θ, ±30°), (φ, θ, ±90°) or (φ, θ, ±150°) in which φ and θ, can be arbitrary values, the singularity locus also consists of two parts: One is a plane; another is also a hyperbolic paraboloid. When point P translates in the plane, two of three points B1, B3 and B5 lie in the basic plane. 3.3 Singularity distribution in three-dimensional space According to the analysis method above, we may easily know the distribution characteristics of the singularity loci of the 3/6-Gough-Stewart manipulator, and draw their singularity surface in three-dimensional space for some different orientations of the mobile in frame O-XYZ. Here, the parameters of the mechanism are set to Ra = 2 , Rm=1m and β0=90°, and the surfaces are shown in Fig. 6.

(a) The orientation (90° 45° 0)

(b) The orientation (90° 30° 60°)

(c) The orientation (90° 45° 30°)

(d) The orientation (45° 25° 30°)

Fig. 6. The singularity loci for 3/6―Stewart parallel manipulator

410


The readers may wonder that the singularity loci are so huge and completed and ask how can the GSP work? In practice, if you notice the position of the origin point of the O-XYZ system in Figures, and the magnitudes of the parameters, Ra = 2 and Rm=1m, you can find that the workspace of the manipulator is smaller relative the singularity loci shown in figures. You can easily design the manipulator making its workspace locate over the singularity loci and avoiding singularity.

X'

B3

Y'

P

B5 Y

A3

B1

ψ y

X

W A1

O

x

V

A5 U Fig. 7. The general case

4. Structure and property of the singularity loci of the 3/6-Gough-Stewart for general orientations When φ takes any value with the exception of ±30°, ±90°, or ±150°, this is the general orientation case of the mobile of the GSP, and the analysis of the singularity loci is more difficult. In this case, UV is not parallel to any side of triangle A1A3 A5, as shown in Fig. 7. 4.1 Singularity equation based on Theorem 3 for general orientations For the most general orientations of the mobile, φ≠±30°, ±90°, and ±150°, the singularity equation can be directly obtained by using Theorem 3. The equation of normal plane B1C1C2 is

x ' − B1 x y ' − B1 y C 1 x − B1 x C 1 y − B1 y C 2 x − B1 x C 2 y − B1 y

z' − B1 z C 1 z − B1 z = 0 C 2 z − B1 z

(40)

where (x′, y′, z′)denotes coordinates of the moving point on plane B1C1C2 in the fixed frame. This gives Fy ' + Gz' = 0

(41)

Similarly, equations of three planes B3C3C4, B5C5C6 and B1B3B5 can be obtained as well. According to Theorem 3, solving the linear equation system of the four planes for intersecting point C, the singularity locus equation for general orientations is as follows


f1 Z 3 + f2 XZ 2 + f3 YZ 2 + f4 X 2 Z + f5 Y 2 Z + f6 XYZ + f7 Z 2 + f8 X 2 + f9 Y 2 + f10 XY + f11 XZ + f12 YZ + f13 Z + f14 X + f15 Y + f16 = 0

411

(42)

where (X, Y, Z)are the coordinates of center point P. It is a polynomial expression of degree three. The equation is still very complicated and difficult to further analyze, but it is very simple in the following special cases. When φ≠±30°, ±90°, and ±150°and ψ is one of the values ±30°, ±90°, or ±150°, Eq. (42) degenerates into a plane and a hyperbolic paraboloid as well. For example, whenψ=90°, the singularity equation is ( 2Z + R b sin θ )( a 11 X 2 + a 22 Y 2 + a 33 Z 2 + 2a 23 YZ + 2a 31 ZX + 2a12 XY + 2a 14 X + 2a 24 Y + 2a 34 Z + a 44 ) = 0

(43)

where these coefficients are listed in the Appendix 2. Eq. (43) indicates a plane and a hyperbolic paraboloid. The first factor forms a plane equation 2Z + R b sin θ = 0

(44)

which is parallel to the basic plane. When point P lies in the plane, the mechanism is singular for orientation (φ, θ, 90°), because points B3 and B5 lie in the basic plane. This is similar to Case 6. All the six lines cross the same line C1C2. 4.2 Singularity analysis using singularity-equivalent-mechanism The singularity locus expression (Eq. (43)) for general orientations has been derived by Theorem 3. But it is still quite complicated, and we are not sure whether it consists of some typical geometrical figures. Meanwhile the property of singularity loci is unknown yet. In order to reply this question, a “Singularity-Equivalent-Mechanism” which is a planar mechanism is proposed. Thus the troublesome singularity analysis of the GSP can be transformed into a position analysis of the simpler planar mechanism. 4.2.1 The parallel case

4.2.1.1 The Singularity-Equivalent-Mechanism In the parallel case, the three Euler angles of the mobile platform are (90°, θ, ψ), while θ and ψ can be any nonzero value. The mobile plane of the mechanism lies on θ-plane (Fig. 5). The corresponding imaginary planar singularity-equivalent-mechanism is illustrated in Fig. 8. Where Rdenotes a revolute pair and P a prismatic pair, triangle B1B3B5 is connected to ground by three kinematic chains, RPP, PPR and RPR. The latter two pass through two points U andV, respectively, while the first one slides along the vertical direction and keeps L1C//UV. Three slotted links, L1, L2 and L3, intersect at a common point C. In order to keep the three links always intersecting at a common point and satisfying Deduction 2, a concurrent kinematic chain PRPRP is used. It consists of five kinematic pairs, where two R pairs connect three sliders. The three sliders and three slotted links form three P pairs. The PRPRP chain coincides with a single point C from top view. Based on the Grübler-Kutzbach criterion, the mobility of the mechanism is two. It is evident that the planar mechanism can guarantee that the three lines passing through three vertices intersect at a common point, and these three lines can always intersect the

412


corresponding sides of the basic triangle. From Deduction 2, every position of the planar mechanism corresponds to a special configuration of the original GSP. So we call it a “singularity-equivalent-mechanism”. Thus the position solution of the planar mechanism expresses the singularity of the original mechanism. 4.2.1.2 Forward Position Analysis of the Singularity -Equivalent-Mechanism The frames are set as the same as in Fig. 5 and Fig. 10. The coordinates of point P in frame O2-xy are (x, y). ψ indicates the orientation of the triangle B1B3B5 in θ,-plane. In order to obtain the locus equation of point P, firstly we can set three equations of three lines passing through the three vertices, and substitute the coordinates of points B1, B3 and B5 into the equations, then (x, y)and ψ can be obtained. X'

B3

y

C

PRPRP Y

'

P

ψ

B1

L1

B5 L2

L3

β

α

U

O2

V

x

Fig. 8. The singularity-equivalent-mechanism for (90°, θ, ψ) Considering that the mobility of this mechanism is two, there need two inputs α andβ. Three equations of three lines CU, CV and CB1 in reference frame O2-xy are respectively Y = ( tan α )( X + a / 2 )

(45)

Y = ( tan β )( X − a / 2 )

(46)

and Y=−

a tan a tan β tan α − tan β

(47)

Solving Eqs. (52), (53) and (54) yields Rb cosψ J 1 − ( 3 Rb sinψ + a )J 3 2( tan α − tan β )

(48)

Rb sinψ J 2 − 3 Rb J 3 cosψ − 2 a tan α tan β 2( tan α − tan β )

(49)

x=

y=

and tanψ =

(tanβ + tanα ) 3tanα - 3tanβ − 2tanα tanβ

(50)


413

where J 1 = tan α − tan β − 2 3 , J 2 = tan α − tan β − 2 3 tan α tan β , J 3 = tan α + tan β , Eqs. (48), (49) and (50) denote direct kinematics of the mechanism. 4. 2. 1. 3. Singularity Equation in the θ- plane Once the orientation (90°, θ, ψ)of the mobile platform is specified, in Fig.10, Euler angle ψ is an invariant. So it only needs to choose one input in this case. From Eq. (65) one obtains tanβ =

tanα ( 3 tanψ -1) 3 tanψ + 2 tanα tanψ + 1

(51)

So the singularity equation in θ- plane for the orientation (90°, θ, ψ) can be obtained from Eqs. (48), (49) and (50) by eliminating parameters α andβ

2(sinψ )y 2 + 2(cosψ )xy + Rb sin(2ψ )x + ( 3 a sinψ − Rb cos(2ψ ))y − Rb2 sinψ + 3 aRb cos(2ψ ) / 2 = 0

(52)

where a = 2(3 Ra cos( β 0 / 2) − u) / 3 . Eq. (52) denotes a hyperbola. Especially, when ψ=±90°, Eq. (52) degenerates into a pair of intersecting straight lines respectively. Two of the four equations are y − R b /2 = 0 , y + R b /2 = 0

(53)

In both cases, two points B3 and B5 lie in line UV. So that four lines are coplanar with the base plane. This is the singularity of Case 6. The similar situation is for ψ=30°,ψ=-150°, ψ=-30° and ψ=150°. 4.2.2 The general case When φ≠±30°, ±90°, and ±150°, the intersecting line UVW between θ- plane and the base one is not parallel to any side of triangle A1A3A5. This is the most general and also the most difficult case.

4.2.2.1 The Singularity-Equivalent-Mechanism Fig. 11 shows the singularity-equivalent-mechanism. The triangle B1B3B5 is connected to the ground passing through three points W, V and U by three RPR kinematic chains. The three points U, V and W, as shown in Fig. 9, are three intersecting points between θ-plane and sides A3A5, A1A3 and A1A5, respectively. Three slotted links L1, L2 and L3 intersect at a common point C. In order to keep the three links always intersecting at a common point, a concurrent kinematic chain, PRPRP, is used as well. Therefore, all the configurations of the equivalent mechanism satisfying Deduction 2 are special configurations of the GoughStewart manipulator. So we can analyze direct kinematics of the equivalent mechanism to find singularity loci of the manipulator. Similarly the mobility of the equivalent mechanism is two, and it needs two inputs when analyzing its position. 4.2.2.2 Forward Position Analysis of the Singularity–Equivalent -Mechanism The frames are set as shown in Fig. 11. Similar to section 4.2.1.2, we may set three equations of three straight lines passing through three vertices, and substitute the coordinates of points B1, B3 and B5 into the equations, then solutions, (x, y)and ψ, can be obtained

414


y X'

B3 PRPRP Y'

C ψ

P

B1

B5 L3

L1

L2

α

x

W

β

V

U

Fig. 9. The singularity-equivalent-mechanism for general case x = -(3Rb sinψ − 2Rb tanα cosψ + 2w tanα + 3 Rb cosψ - Rb tanβ cosψ + 3Rb tanβ sinψ -2u tanβ )/(2tanβ − 2tanα ) y = (-R b tan α sinψ − 3R b tan α cosψ + 3R b tan α tan β cosψ + 2u tan α tan β − 3R b tan α tan β sinψ -2R b tan β sinψ − 2w tan α tan β )/(2 tan β − 2 tan α )

tanψ =

2 3 w tanα -3u tanα tanβ − 3u tanβ tanβ (-2 3 w tanα + 3u tanα -3u)

(54)

(55)

(56)

where u indicates the distance from point U to V, and w the distance from V to W. Substituting Eq. (56) into Eqs. (55) and (54), and eliminating ψ, the relations between (x, y) and the inputs α, β can be obtained. This is direct kinematics of the equivalent mechanism. 4.2.2.3 Singularity Equation in the θ- plane Under a general case, Euler angle φ can be any value with the exception of ±30°, ±90°, or ±150°. From Eq (56) one obtains tan β =

2 3w tan α -2 3w tan α tanψ + 3u tan α tanψ -3u tanψ + 3u tan α + 3u

(57)

In the case of some specified ψ, there are the same three particular situations that is B1 and B5, B1 and B3, or B3 and B5 lie in the line UV, respectively. The singularity loci are three pairs of intersecting straight lines. In order to use the above-mentioned formulas, u and w in Eq. (57) should be calculated in advance. They depend on their relative positions in UV, as shown in Fig. 10. The distance w between V and W is w = WV =

3 Ra cos( β 0 / 2) − 3xV cos φ

(68)

415


Y C

A3 y

φ

O

A5

φ

A1

θ

V

γ

x

W

X

U Fig. 10. The Intersecting Line UW of two planes The distance u between U and V is u = UV =

2 3x V ( 3 + cot φ ) sin φ

(59)

The sign of w is positive when point W is on the right side of V, and it is negative when W is on the left side of V. It is similar for the sign of u. For a given xv, the singularity equation in θ-plane can be obtained by eliminating the parameter α bxy + cy 2 + dx + ey + f = 0

(60)

The two invariants D, δ of Eq. (60) are 0

b/2 d/2

D = b/2 c d/2 e/2

1 e/2 = − ( b 2 f + d 2 c − bde ) 4 f

(61)

and δ=

0 b/2 1 = − b2 < 0 b/2 c 4

(62)

Generally, D≠0 and δ

Advanced Strategies for Robot Manipulators

Robot Manipulators

Robot Manipulators, New Achievements

The kinematics of robot manipulators

Kinematic Analysis of Robot Manipulators

Control of Redundant Robot Manipulators

Robot Manipulators: Modeling, Performance Analysis and Control

Robot manipulators : mathematics, programming, and control : the computer control of robot manipulators

Control of Robot Manipulators in Joint Space

Flexible Robot Manipulators: Modelling, simulation and control

Nonholonomic Manipulators (Springer Tracts in Advanced Robotics)

Control of Robot Manipulators in Joint Space (Advanced Textbooks in Control and Signal Processing)

Adaptive Control of Robot Manipulators: A Unified Regressor-free Approach

Robot Manipulators: Mathematics, Programming, and Control (Artificial Intelligence)

Control of Robot Manipulators in Joint Space (Advanced Textbooks in Control and Signal Processing)

Control of Robot Manipulators in Joint Space (Advanced Textbooks in Control and Signal Processing)

Advanced web-based training strategies

Connors On Advanced Trading Strategies

Connors On Advanced Trading Strategies

Robot Building for Beginners

The Money Manipulators

Beyond Change Management: Advanced Strategies for Today's Transformational Leaders

Mastering Forensic Psychiatric Practice: Advanced Strategies for the Expert Witness

Effective Language Learning: Positive Strategies for Advanced Level Language Learning

Building Knowledge Economies: Advanced Strategies for Development (Wbi Development Studies)

Advanced Nonlinear Strategies for Vibration Mitigation and System Identification

Robot Builder's Sourcebook : Over 2,500 Sources for Robot Parts

Computational Strategies for Spectroscopy

Strategies for Change

Strategies for literacy education

Strategies for Teaching Mathematics

Advanced Strategies for Robot Manipulators

Robot Manipulators

Robot Manipulators, New Achievements

The kinematics of robot manipulators

Kinematic Analysis of Robot Manipulators

Control of Redundant Robot Manipulators

Robot Manipulators: Modeling, Performance Analysis and Control

Robot manipulators : mathematics, programming, and control : the computer control of robot manipulators

Control of Robot Manipulators in Joint Space

Flexible Robot Manipulators: Modelling, simulation and control

Nonholonomic Manipulators (Springer Tracts in Advanced Robotics)

Control of Robot Manipulators in Joint Space (Advanced Textbooks in Control and Signal Processing)

Adaptive Control of Robot Manipulators: A Unified Regressor-free Approach

Robot Manipulators: Mathematics, Programming, and Control (Artificial Intelligence)

Control of Robot Manipulators in Joint Space (Advanced Textbooks in Control and Signal Processing)

Control of Robot Manipulators in Joint Space (Advanced Textbooks in Control and Signal Processing)

Advanced web-based training strategies

Connors On Advanced Trading Strategies

Connors On Advanced Trading Strategies

Robot Building for Beginners

The Money Manipulators

Beyond Change Management: Advanced Strategies for Today's Transformational Leaders

Mastering Forensic Psychiatric Practice: Advanced Strategies for the Expert Witness

Effective Language Learning: Positive Strategies for Advanced Level Language Learning

Building Knowledge Economies: Advanced Strategies for Development (Wbi Development Studies)

Advanced Nonlinear Strategies for Vibration Mitigation and System Identification

Robot Builder's Sourcebook : Over 2,500 Sources for Robot Parts

Computational Strategies for Spectroscopy

Strategies for Change

Strategies for literacy education

Strategies for Teaching Mathematics

Recommend Documents