Control of Kinematically Redundant Robotic Manipulators for Orthopedic Surgery
Andre´ Ribeiro da Silva McGuire
˜ para obtenc¸ao ˜ do Grau de Mestre em Dissertac¸ao
ˆ Engenharia Mecanica
Juri ´ Presidente:
˜ Rogerio ´ Professor Joao Caldas Pinto
Orientador:
Professor Jorge Manuel Mateus Martins
Co-Orientador:
Professor Jose´ Manuel Gutierrez Sa´ da Costa
Vogal:
˜ Professor Rui Cortesao
Novembro - 2010
Do not wait; the time will never be “just right”. Start where you stand, and work with whatever tools you may have at your command, and better tools will be found as you go along. Napoleon Hill
Este trabalho reflecte as ideias dos seus ˜ nao ˜ autores que, eventualmente, poderao ´ coincidir com as do Instituto Superior Tecnico.
Abstract The aim of the present work is to develop an impedance controller for rigid and flexible robots with kinematic redundancy, within the context of Hip Resurfacing Arthroplasty (HRA). We first introduce the current trends in surgical robotics, where a flexible redundant robot arises as a promising solution. HipRob, a seven-degree of freedom flexible link manipulator is introduced. Modeling techniques for flexible manipulators are presented and various symbolic tools are compared, from which the MECANISMO toolbox [Martins, (2007)] is chosen to generate both a rigid and flexible model of the system. Force control schemes are discussed, and an impedance controller with inner motion control is implemented for the rigid model. For the flexible model, a Closed Loop Inverse Kinematics with Redundancy resolution scheme is used associated to an impedance controller. A suitable complacency environment for HRA is presented, where the impedance is proportional to the distance to a contact point. A redundancy resolution scheme based on joint impedance control is proposed. The controller reconfigures the manipulator based on internal force measurements. A controller for suppressing the elastic vibrations is developed, based on feedback of the modal deflection rates for each link. Simulated results for both models show the effectiveness of the proposed systems. This applies for both direct application of force to the robot end effector and for an applied force at an intermediate point of the structure. The vibration controller is shown to be effective at increasing the damping of link vibrations.
Keywords: Medical Robotics, Flexible manipulators, Impedance control, Kinematic Redundancy, Closed Loop Inverse Kinematics.
v
vi
Resumo ˜ e´ desenvolver um controlador de impedancia ˆ ˆ O principal objectivo desta dissertac¸ao para robos ˆ ´ ´ r´ıgidos e flex´ıveis com redundancia cinematica, dentro do contexto da robotica cirurgica, nomeadamente ´ ˜ descritas as tendencias ˆ a cirurgia de artroplastia parcial da anca. Inicialmente sao actuais dentro da ´ ´ ˜ flex´ıvel e redundante se apresenta como promissora. HipRob, um robotica medica, onde uma soluc¸ao ´ ˜ para novo manipulador flex´ıvel com sete graus de liberdade e´ introduzido. As tecnicas de modelac¸ao ˆ flex´ıveis sao ˜ discutidas, e apos ´ uma analise ´ ´ robos comparativa de ferramentas simbolicas, a toolbox MECANISMO [Martins, (2007)] e´ escolhida para gerar um modelo r´ıgido e outro flex´ıvel. ˆ ˜ Dentro dos controladores de forc¸a, o controlador de impedancia com controlo interno de posic¸ao ˜ e´ escolhido para o modelo r´ıgido. No caso flex´ıvel, e´ desenvolvido um controlador de e orientac¸ao ˆ ´ ˜ de redundancia. ˆ impedancia associado a´ cinematica inversa em anel fechado com resoluc¸ao Um ambiˆ ente de complacencia e´ igualmente apresentado para a cirurgia referida, onde a rigidez e´ proporcional ˆ a` distancia ao ponto de contacto. ˆ de redundancia ˆ ˆ Um esquema de resoluc¸ao baseado em controlo de impedancia ao n´ıvel das juntas ´ e´ proposto. O controlador reconfigura o manipulador quando actuado por forc¸as internas. E´ tambem ˜ desenvolvido um controlador para aumentar o amortecimento de cada elo baseado na realimentac¸ao ˜ modais. das velocidades de deformac¸ao ˜ ´ Simulac¸oes com ambos os modelos comprovam a eficacia dos sistemas propostos, tanto para a ˜ directa de uma forc¸a sobre o elemento terminal do robo, ˆ como para uma forc¸a aplicada aplicac¸ao ´ ˜ demonstra ser efectivo no aumento do num ponto intermedio da estrutura. O controlador de vibrac¸ao amortecimento dos elos flex´ıveis.
Palavras chave:
´ ´ ˆ ˆ Robotica Medica, Manipuladores Flex´ıveis, Controlo de impedancia, Redundancia
´ ´ Cinematica, Cinematica Inversa
vii
viii
Acknowledgments First of all, I would like to thank my supervisor, Dr. Jorge M.M. Martins, for introducing me to the field of robotic manipulators, and for the passion he has when imagining the ramifications of any project in real world applications. I would also like to thank my co-supervisor, Dr. Jose´ Sa´ da Costa, for accepting and supporting the work done during this dissertation. A special thanks goes out to Pedro Pires, Pedro Teodoro and Tiago Rita, for all the help and brainstorming that has certainly enriched the work done here. Also, to the staff at the Control, Automation and Robotics lab goes a thank you for help in all aspects of practical implementation. To all my friends, especially the “Basement Group” and Tiago, for all the years of company and friendship without which the time spent here would have certainly been boring. To my mother and family, a thank you for all the support, encouragement, and incredible faith in my abilities. You have shaped me into the man I am today, and this work is a byproduct of that. Finally, to Francisca, thank you for never letting me give up, for showing me that dreams can be reached, and that nothing is impossible.
˜ para a Ciencia ˆ This work was supported by FCT-Fundac¸ao e a Tecnologia, project PTDC/EMECRO/099333/2008: HipRob - Robot-Assisted and Ultrasound-Guided Navigation for Hip Resurfacing Arthroplasty
ix
x
Contents Contents
xi
List of Figures
xiii
List of Tables
xv
Notation
xvii
1 Introduction
1
1.1 Medical Robotics and Computer Assisted Surgery . . . . . . . . . . . . . . . . . . . . . .
1
1.1.1 Technical Paradigms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
1.1.2 Orthopedic Surgery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
1.2 Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
1.3 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
2 Modeling Flexible Manipulators
9
2.1 Modeling Flexible Links . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
2.1.1 Kinematics of a Flexible Beam . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
10
2.1.2 Dynamic Model of a Flexible Link . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
2.2 Flexible Multibody Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
2.2.1 Joint Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
2.2.2 Rigid Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
2.2.3 Serial Multibody System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
2.3 Computational Strategies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
2.3.1 Symbolic Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
16
2.3.2 Software Tools . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
3 Force Control of Flexible Manipulators
21
3.1 Interaction Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
21
3.2 Impedance Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
22
3.2.1 Impedance Control with Inner Motion Control . . . . . . . . . . . . . . . . . . . . .
23
3.3 Analysis of Redundancy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
24
3.3.1 Redundancy Resolution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
25
xi
3.3.2 Impedance Control with Redundancy Resolution . . . . . . . . . . . . . . . . . . .
27
3.4 Flexible Manipulators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
3.4.1 Vibration Suppression . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
32
3.4.2 Tracking control for flexible manipulators . . . . . . . . . . . . . . . . . . . . . . . .
33
3.4.3 Closed Loop Inverse Kinematics with Redundancy Resolution
36
. . . . . . . . . . .
4 Implementation
39
4.1 System Description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
39
4.1.1 HipRob . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
39
4.1.2 Simulation and Control Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
41
4.1.3 Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
43
4.2 Implementation of Controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
45
4.2.1 Rigid Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
46
4.2.2 Flexible Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
49
4.3 Final System Configuration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
54
5 Simulation Results
57
5.1 Rigid Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
58
5.1.1 Redundancy resolution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
58
5.1.2 Impedance Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
60
5.2 Flexible Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
62
5.2.1 Vibration Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
62
5.2.2 Inverse Dynamics with Redundancy Resolution . . . . . . . . . . . . . . . . . . . .
64
5.2.3 PD controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
67
5.2.4 CLIK-R . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
68
6 Discussion and Conclusions
71
6.1 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
71
6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
72
6.3 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
72
Bibliography
75
Appendix
81
A Test Results
83
A.1 Tests on Rigid Redundancy Resolution . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
83
A.2 Tests on Vibration Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
84
A.3 Tests on Flexible Inverse Dynamics with Redundancy Resolution . . . . . . . . . . . . . .
85
A.4 Tests on joint PD controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
86
A.5 Tests on CLIK-R . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
87
xii
List of Figures 1.1 Tradeoff between procedural role and robot autonomy. . . . . . . . . . . . . . . . . . . . .
3
1.2 Prosthetic implants in THR and HRA.
. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4
1.3 Stress distribution in a femur. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
1.4 Mechanical jig position/orientation procedure. . . . . . . . . . . . . . . . . . . . . . . . . .
5
2.1 Flexible beam kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
10
2.2 Joint kinematics
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
2.3 Planar Slider-Crank Mechanism and Linear Graph Representation . . . . . . . . . . . . .
18
2.4 Symofros modules overview. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
2.5 MECANISMO Simulink block library. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
20
3.1 Spatial impedance control with inner motion control. . . . . . . . . . . . . . . . . . . . . .
24
3.2 Spatial impedance control with redundancy resolution. . . . . . . . . . . . . . . . . . . . .
28
3.3 Generic system block diagram. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
31
3.4 Velocity feedback control scheme. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
33
3.5 Block diagram of velocity feedback control with inverse dynamics control. . . . . . . . . .
33
3.6 CLIK methodology diagram. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
35
3.7 Block diagram for closed loop inverse kinematics scheme with redundancy resolution. . .
36
3.8 Block diagram for impedance control of a flexible redundant system. . . . . . . . . . . . .
37
4.1 HipRob - The IST 7-Dof Flexible Manipulator . . . . . . . . . . . . . . . . . . . . . . . . .
40
4.2 Hiprob interacting with a human operator. . . . . . . . . . . . . . . . . . . . . . . . . . . .
40
4.3 Identification of bodies and joint reference frames used for describing the system. . . . .
41
4.4 VRML model used for simulation visualization. . . . . . . . . . . . . . . . . . . . . . . . .
43
4.5 Complacency environment associated with the manipulator end-effector. . . . . . . . . . .
44
4.6 Implementaiton of robot model. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
45
4.7 Implementation of impedance controller and inner motion controller. . . . . . . . . . . . .
46
4.8 Implementation of inverse dynamics with redundancy resolution for the rigid model of the robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
47
4.9 Block diagram of pseudoinverse subsystem. . . . . . . . . . . . . . . . . . . . . . . . . . .
47
4.10 Block diagram of the redundancy subsystem. . . . . . . . . . . . . . . . . . . . . . . . . .
48
4.11 Implementation of impedance controller considering only end effector position/orientation.
49
xiii
4.12 Implementation of the CLIK-R scheme.
. . . . . . . . . . . . . . . . . . . . . . . . . . . .
50
4.13 Error Calculation subsystem block diagram. . . . . . . . . . . . . . . . . . . . . . . . . . .
50
4.14 Redundancy subsystem block diagram. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
50
4.15 Joint based PD controller. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
51
4.16 Implementation of inverse dynamics controller for flexible robotic systems. . . . . . . . . .
51
4.17 Implementation of inverse dynamics with redundancy resolution block for the flexible model. 52 4.18 Implementation of velocity feedback vibration controller. . . . . . . . . . . . . . . . . . . .
53
4.19 Block diagram with the implementation of the complete system for a rigid model. . . . . .
54
4.20 Block diagram with the implementation of the complete system for a flexible model. . . . .
55
5.1 Initial configuration for simulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
57
5.2 Reference acceleration and applied force used for validation of redundancy resolution. . .
59
5.3 Comparison of the end effector position and joint trajectories using rigid redundancy resolution.
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
59
5.4 Variation of end effector position when a step force of 10N is applied to the end effector perpendicular to the drill direction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
60
5.5 Comparison of the end effector position and joint trajectories using impedance control with rigid redundancy resolution. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
61
5.6 Comparison of the tracking abilities of the inverse dynamics with vibration controller and without. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
63
5.7 Comparison of controlled and uncontrolled vibrations for link 1. . . . . . . . . . . . . . . .
63
5.8 Evolution of end effector Cartesian position under pseudoinverse with inverse dynamics control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
64
5.9 Reference acceleration and force profiles used in flexible redundancy tests. . . . . . . . .
65
5.10 Comparison of the end effector position and joint trajectories using flexible redundancy resolution: Tracking. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
66
5.11 Comparison of the end effector position and joint trajectories using flexible redundancy resolution: Applied force. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
66
5.12 Reference joint positions and velocities used for testing PD controller with respective result. 68 5.13 Simulation results for CLIK end effector position tracking. . . . . . . . . . . . . . . . . . .
69
5.14 Force signal measured by the auxiliary sensor. . . . . . . . . . . . . . . . . . . . . . . . .
69
5.15 Simulation results for redundancy resolution with CLIK-R. . . . . . . . . . . . . . . . . . .
70
A.1 Variation of joint coordinates based on reference end-point acceleration and applied force. 83 A.2 Comparison of controlled and uncontrolled flexible link vibrations. . . . . . . . . . . . . . .
84
A.3 Variation of joint coordinates based on reference end-point acceleration. . . . . . . . . . .
85
A.4 Variation of joint coordinates based on joint position and velocity under PD control. . . . .
86
A.5 Variation of joint coordinates when actuated by a force at the auxiliary sensor. . . . . . . .
87
xiv
List of Tables 1.1 Complementary Strengths of Robots and Humans. . . . . . . . . . . . . . . . . . . . . . . 2.1 Physical meaning of parameters in Jourdain’s Principle
2
. . . . . . . . . . . . . . . . . . .
11
4.1 Properties of flexible elements used for modeling and simulation . . . . . . . . . . . . . .
42
4.2 Geometric and dynamic parameters used for modeling rigid elements . . . . . . . . . . .
42
5.1 Controller parameters used for redundancy resolution . . . . . . . . . . . . . . . . . . . .
58
5.2 Controller parameters for impedance control with inner motion control. . . . . . . . . . . .
60
5.3 Gain parameters used in vibration control. . . . . . . . . . . . . . . . . . . . . . . . . . . .
62
5.4 Controller parameters for joint PD controller. . . . . . . . . . . . . . . . . . . . . . . . . . .
67
5.5 Controller parameters for CLIK-R. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
68
xv
xvi
Notation The following notation is used throughout this work.
Acronyms AMM
-
Assumed Mode Method
CAD/CAM
-
Computer Aided Design/Computer Aided Manufacturing
CAOS
-
Computer-Assisted Orthopedic Surgery
CI
-
Composite Inertia
CT
-
Computerized Tomography
CLIK
-
Closed Loop Inverse Kinematics
CLIK-R
-
Closed Loop Inverse Kinematics with Redundancy resolution
DOF
-
Degree of Freedom
FE
-
Finite Element
HRA
-
Hip Resurfacing Arthroplasty
MBS
-
Multibody Systems
MIS
-
Minimally Invasive Surgery
MRI
-
Magnetic Resonance Imaging
MuT
-
Multibody Toolbox
ODEs
-
Ordinary Differential Equations
PD
-
Proportional Derivative
PDEs
-
Partial Differential Equations
Pose
-
Position and Orientation
THA
-
Total Hip Replacement
RFR
-
Rigid-Flexible-Rigid
VRML
-
Virtual Reality Modeling Language
xvii
List of Symbols Chapter 2 {OI , XI YI ZI }
Inertial reference frame
{On , Xn Yn Zn }
Body reference frame
{Ok , Xk Yk Zk }
Cross section reference frame
{Onb , Xnb Ynb Znb }
Reference frame attached to the last cross section of the beam T
Kk = [K1 K2 K3 ]
Curvature vector with components along the cross section reference frame
[Φrn+1,n Φen+1,n ]T
Vector of matrix operators that transform generalized (rn ) and elastic (en ) velocities expressed in the current body reference frame into the following body’s reference frame
α
Pure torsion angle
αk
Angular acceleration of the cross section reference frame
β0
Inertial body of reference
β1 (βN )
First (last) body in a chain
βn0 , Σβn0
Body and its surface in undeformed configuration
ϕ12 , ϕ13
Shear angles defined by the classic theory of elasticity
γ2 , γ2
Pure shear deflections
ρA
Beam mass per unit of length
ρp
Specific mass of the beam at point p
σ
Vector of Piola-Kirchoff stresses acting on a cross section
υ2 , υ3
Pure bending deflections
υ 2x , υ 3x , αx , γ 2x , γ 3x
Shape function vectors
υ 2t , υ 3t , αt , γ 2t , γ 3t
Vectors of generalized coordinates
ωk
Angular velocity of the cross section reference frame
ω n (ω ˙ n)
Absolute linear angular velocity (acceleration) of βn relative to βn−1 , expressed in the body reference frame
ap
Absolute (relative to the inertial frame) acceleration vectors
ak
Linear acceleration of the cross section reference frame
E
Young’s Modulus
an
Absolute linear acceleration of βn relative to βn−1 , expressed in the body reference frame
An (Arf rn )
Absolute generalized acceleration vector of the body reference frame (nth RFR element), which includes linear plus gravitational acceleration and transverse angular acceleration
fs
External force vector applied on the beam surface
xviii
Fn
External generalized force vector expressed in the body reference frame
FΣn
Generalized force vector applied on the boundary of the beam
g
Gravity acceleration vector
Gp
Green-Lagrange strain-displacement relation
Hnv (qnw ) (Hnω (qnw ))
Translation (Rotation) Jacobian matrices of joint n
J
Geometrical moment of inertia tensor of a cross section relative to the cross section reference frame, but expressed in the moving reference frame
Jn (Jn+1 )
Inward (Outward) joint of body βn
Ken
Load vector resulting from linear elastic forces
Men
Mass matrix of the flexible beam
Mrf rn
Mass matrix of the RFR body
Nen
Generalized nonlinear inertial force vector
N en,r
Generalized nonlinear inertial vector
Nrf rn
Generalized nonlinear inertial vector of the RFR body
rn−1,n
Inter-body position vector h
qen+1 = qTKn+1
qTϕn+1
iT
Rn/n−1
Global vector of elastic coordinates Orthogonal rotation matrix that describes the rotation of body βn−1 relative to body βn−1
Trf rn
Externally applied force vector of the nth RFR element
vp
Absolute (relative to the inertial frame) velocity vectors
vk
Linear velocity of the cross section reference frame
vn
Absolute linear velocity of βn relative to βn−1 , expressed in the body reference frame
Vn (Vrf rn )
Generalized velocity vector of the body reference frame (nth RFR element)
Chapter 3 γ
Joint velocity vector available for redundancy resolution
λ
Damping factor
φn
Joint acceleration vector lying in the null space of the Jacobian matrix
ω(q)
User defined joint task function for local optimization
ω ˙ i (xi , t)
Transversal deformation rates from link i at distance xi from the base of the beam
∆x
Cartesian position/orientation
ae = [ap ao ]T
End effector acceleration in Cartesian coordinates
c
Vector of applied moments to the end effector
µ
xix
c
dc
Quaternion orientation error
C
Manipulator matrix of centrifugal and Coriolis forces
en
Null space velocity error
h = [f , µ]T
Vector of contact forces/moments exerted by the end-effector on the environment
h∗
Auxiliary force sensor inputs
I
Identity matrix
J , J e and J c J
T∗
J
†
Manipulator, main task and additional task Jacobian matrix Jacobian matrix of the manipulator up to the element where the auxiliary force sensor is placed Pseudoinverse of the Jacobian matrix
J#
Dynamically consistent pseudoinverse
K MI ,K DI ,K PI
Positive definite matrices defining the virtual inertia, damping and stiffness of the robotic manipulator system
K M q , K Dq , K P q
Positive definite matrices defining the virtual inertia, damping and stiffness of the joints
K P ,K D
Positive definite matrices defining the proportional and derivative gains for the joint PD controller.
m
Dimension of the task space
M (q)
Rigid manipulator mass matrix
n
Dimension of the joint space (number of DOF)
N (q, q) ˙
Non-linear vector which includes the vector of Coriolis and centrifugal torques, the vector of friction torques and the vector of gravitational torques
pc
Position of the reference frame expressed in the inertial frame
Q = [η, ]
Quaternion representation of the orientation
q(t)
Joint space trajectory
q˙ 0
Joint velocity vector
q˙ eij (t)
Modal deflection rates associated with modal functions ϕij (xi )
q¨0
Additional joint acceleration vector
r
Degree of redundancy
Rc
Rotation matrix defining the orientation of the compliant frame
T
Vector of driving torques
xc
Compliant frame
x(t)
Operational space trajectory
xx
Chapter 1
Introduction The field of robotics is expanding. Robots have left the controlled environment of factories and warehouses they were initially developed for, and are making their way into the highly dynamic, unconstrained world of humans. Their ability to replace, supplement or transcend human performance has had a profound influence on many fields of our society, spanning fields such as agriculture, military, oceanographic exploration and education. The excellent geometric accuracy of robots, associated with their ability to integrate several different sources of information, has led to their natural implementation in medicine, more specifically in surgery. Patients demand greater precision, less and minimally invasive procedures, and faster recovery times. The increasing life expectancy and aging-associated diseases, associated with a need for reducing costs and increasing efficiency have opened the door for new and innovative solutions in the medical robotic industry. The U.S. market for medical robotics and computer-assisted surgical equipment was worth an estimated $648 million in 2008. The market is projected to reach $1.5 billion by 2014, a compound annual growth rate of 17.7% (Source: BCC Resarch [McWilliams, (2009)]). As the field grows, so do the requirements to product developers. Increasing safety constraints demand use of compact, slim and lightweight mechanisms. This implies dealing with structural flexibility. Surgeons demand simple and intuitive interaction, while keeping control of the entire procedure. Successful robots must be easily manipulated, and less intrusive to the surgical workflow.
1.1
Medical Robotics and Computer Assisted Surgery
The field of robotics in medicine is relatively new, with the first clinical application of a robot to neurosurgery in 1985 [Kwoh et al., (1988)]. Since then, research centers around the world have developed a multitude of robotic products, encompassing areas such as orthopedics, radiology, urology, cardiothoracic and ophthalmology. Detailed descriptions of past and current systems can be found in [Cleary & Nguyen, (2001)], [Wang et al., (2006)] or [Gomes, (2010)]. Early attempts consisted mainly of modifying industrial robots for a given procedure, but concerns have arisen over the safety of such mechanisms. The current trend in surgical robotics is that systems 1
will no longer be adaptations of industrial robots, but increasingly systems will be dedicated to a single task, with some autonomous capabilities and an extreme level of safety [Riviere et al., (2006)]. The acceptance of medical robots comes from their ability to significantly improve surgeon’s technical capability, by exploiting the complementary strengths between humans and robotic devices, as described in Table 1.1. Their intended function is not to replace the surgeon, but to support the surgeon with enhanced dexterity, visual feedback, and information integration. Table 1.1: Complementary Strengths of Robots and Humans [Taylor & Joskowicz, (2003)].
Strengths
Limitations Prone to fatigue and inattention
H U M A N S
Excellent judgment
Tremor limits fine motion
Excellent hand-eye coordination
Limited manipulation and dexterity outside natural scale
Excellent dexterity (at natural ”human” scale)
Cannot see through tissue
Able to integrate and act on multiple information sources
Bulky end-effectors
Easily trained
Limited geometric accuracy
Versatile and able to improvise
Hard to keep sterile Affected by radiation, infection
R O B O T S
Excellent geometric accuracy
Poor judgment
Untiring and stable
Hard to adapt to new situations
Immune to ionizing radiation
Limited dexterity
Operate at many different scales of motion and payload
Limited hand-eye coordination
Integrate multiple sources of numerical and sensor data
Limited haptic sensing
Possibility of less or minimally invasive surgery (MIS)
Limited ability to integrate and interpret complex information
Surgical robots should be considered more as smart surgical tools that enable human surgeons to treat individual patients with improved efficacy, greater safety and less morbidity [Taylor, (2006)]. Another reason for promoting medical robots is found when you factor in the experience of the surgeon, which can influence the outcome of the procedure. Studies have shown [Moorthy et al., (2004)] that the learning curve for surgeons that take advantage of robotic systems is improved when compared to regular surgery.
1.1.1
Technical Paradigms
Due to the vast range of computer assisted systems applied to surgery, there is a need to categorize these systems. In the revised literature, they are many different classifications, of which the following are most used. • Primary mode of operation: Surgical CAD/CAM systems or Surgical Assistants. This approach is the most common for product developers ([Kazanzides et al., (2008)], [Hager et al., (2008)] and [Fichtinger et al., (2008)]). However, it can be seen that many surgical robots display characteristics from both categories, since they are not mutually exclusive. • Application: Cardiology, orthopedics, laparoscopy, etc. This selection is an artificial decoupling, because the technology that provides the solution is not necessarily related to the procedure that defines the problem. 2
• Role: Active, Restricted or Passive. This classification focuses on how the system interacts with the patient and the surgeon. Since robot autonomy and surgeon’s control are inversely proportional, there is a tradeoff in current systems, depicted in Figure 1.1. An active-role robot assisting in a surgery implies significant interaction with the patient and supervision from the surgeon. On the other hand, a highly autonomous robot can only perform actions that are either low risk or that are a small part of the overall procedure.
Autonomy
- Imaging Systems
Passive
- Guidance Systems - Co-Manipulation - Teleoperation - Manual instruments
Restricted
Active
Robot Role Figure 1.1: Tradeoff between procedural role and robot autonomy. Procedural role indicates the level of responsibility and involvement the robot has with the patient during a procedure. Robot role in a procedure scales up with greater duration, scope, invasiveness and risk, which decreases the level of autonomy. Adapted from [Camarillo et al., (2004)].
Imaging systems such as computerized tomography (CT) or magnetic resonance imaging (MRI) have a great deal of autonomy since they have have little interaction with humans. Computer-aided guidance systems, such as Accuray’s CyberKnife [Adler et al., (1997)] have a more active role but still have no direct physical contact with the patient. In co-manipulation control systems, such as John Hopkins’ Steady Hand [Taylor et al., (1999)], the Acrobot system [Jakopec et al., (2001)] and more recently the DLR MIRO [Hagn et al., (2008)], manipulation of the surgical instrument is shared with the robot who is armed with force sensors. The procedure is controlled by the surgeon, however, the robot restricts motion into sensitive areas by use of active constraints called virtual fixtures. This type of interface can also filter unwanted and often unavoidable tremors in the surgeon’s hand. In teleoperated systems, of which the most widely known is Intuitive Surgical’s daVinci system [Ballantyne & Moll, (2003)], the robot acts as the surgeons eyes and arms. They are controlled using a master/slave methodology by means of a control console with enhanced imaging and haptic interface. The robot’s end effectors are small and fit through incisions in the patient’s body enabling MIS. The interface also allows adjustment of the surgeon’s movements, for precision at small scales. Current research projects are mainly focusing on providing the physicians with smarter tools for complex procedures by means of integrated force control. Popular strategies for hands-on surgery are based on impedance controllers as they provide an ergonomic and safe way to interact human and robotic operators [Haidegger et al., (2009)]. 3
1.1.2
Orthopedic Surgery
Orthopedic procedures often require geometric precision, both in bone removal and in the placement of prosthetics. Contrarily to soft tissues, bone is rigid and does not alter its shape once fully grown. Preoperative scans such as X-ray or CT are common and procedures are planned in advance. These properties have made orthopedic surgery a prime candidate for the implementation of medical robots. Also, as most procedures are not life threatening, there has been less skepticism over the implementation of these systems. Although most surgeons are satisfied with the outcome of conventional techniques [Jaramaz et al., (2006)], pressure to improve efficiency, implement less invasive procedures by reducing exposure of bony structures has enabled research into the area of Computer-Assisted Orthopedic Surgery (CAOS). There are many different applications for robotics in orthopedics. This work will give focus to joint replacement techniques, more specifically Hip Resurfacing Arthroplasty (HRA). The most common hip arthroplasty procedure is Total Hip Replacement (THR), where the femoral head is completely removed, and a prosthetic implant is inserted along the bone shaft. As can be seen in Figure 1.2a, this procedure is highly invasive, and concerns have arisen over the long-term outcome in younger or more active patients. In HRA, there is no need to sever the bone. The femur head is milled and a smaller implant is placed, covering the surface (Figure 1.2b). This procedure has the advantage of preserving high amounts of femoral bone, superior postoperative joint stability and lower rates of articular wear [Kruger et al., (2009)]. ¨
(a)
(b)
Figure 1.2: Prosthetic implant in THR (a), and in HRA (b) (From www.hipresurfacingnewyork.com); The THR implant requires removal of the femoral head while the HRA preserves original bone structure.
However, the surgical technique of HRA is complex and demanding for the surgeon, as the implant must be correctly aligned to resemble normal bone strain. Stress analysis of the resurfaced head (Figure 1.3) show that a slight deviation of the alignment can lead to significant variations in stress distribution. Studies have shown [Mont et al., (2001)] there is a link between the accuracy of component placement and femoral neck notching, fracture and revision rates. In cases of unsuccessful surgery, it is still possible to perform the more invasive THA. Conventional HRA surgery uses a complex mechanical jig technique seen in Figure 1.4 to determine the correct position and alignment of the implant’s stem. Experienced hip surgeons need to undertake over fifty procedures in order to place the implant consistently 4
Figure 1.3: Stress distribution in a) Healthy femur; b) Varus orientation of the prosthesis; c) Normal orientation of the prosthesis; d) Valgus orientation of the prosthesis [Teodoro et al., (2008)].
within 5 degrees of the intended position as mechanical jig positioning can produce highly variable pin positions [Kluge, (2009)].
Figure 1.4: Mechanical jig position/orientation procedure (Smith & Nephew - Birmingham Hip Resurfacing System - Protocol Papers)
Due to the complexity of the procedure, computer assisted hip resurfacing systems were developed to assist the placement of the femoral implant. In some systems, such as BrainLAB’s Vector Vision CAS guidance system [BrainLAB, (2005)], the computer guides the surgeon through visual information, R but final position and orientation still depends on human accuracy. The Acrobot Navigation system
mentioned above has also been applied to hip resurfacing [Barrett et al., (2007)], using virtual constraints to guide the surgeon to the predetermined entry point. When compared with the conventional mechanical jig technique [Resubal & Morgan, (2009)], the use of CAS techniques yields a more accurate and precise placement of the femoral component. This procedure reduces variability in the deviation of the implant when compared to the preoperative plan. Improvements in tracking systems should allow even less invasiveness of the procedure by reducing bone exposure. The drive for minimal access arthroplasty continues, fueled by patient demand and pressure to reduce costs. Technology such as an active constraint robot might enable this process without unacceptable risks of inaccuracy [Davies et al., (2006)]. 5
1.2
Objectives
This work comes to complement a series of works in the subject of medical robotics and control of ´ flexible robot manipulators carried out at Instituto Superior Tecnico (IST). The purpose of this work is then to build on existing work, and develop a robotic system that enhances the surgeon’s ability in the Hip Resurfacing Arthroplasty surgery. The system should provide an interface with the surgeon within the models of cooperative control schemes described above. As the robot is meant to be placed in a human environment, the system is lightweight, and has the approximate dimensions of the human arm making it easily operated either at its tip or at the intermediate links. The objectives of this dissertation can then be described as follows: • Develop a model for flexible robot manipulators. • Design and implement vibration control strategies. • Design and implement a controller to explore redundancy. • Implement impedance control.
1.3
Contributions
The main contributions of this work can be stated as: • Analysis of existing software tools for modeling of flexible robots. • Modeling and analysis of a new robotic platform at IST: HipRob. • Expansion of previous works on impedance control for flexible, kinematically redundant robots. • Proposal of a new kinematic redundancy resolution scheme to be used with Closed Loop Inverse Kinematics of flexible manipulators. • Proposal of a novel user defined task function for solving kinematic redundancy. These contributions constitute one more step towards the development of a prototype manipulator system at IST, to be used in orthopedic HRA surgeries.
6
Outline In Chapter 2, an overview of modeling methods is given, followed by an extensive formulation of the dynamic model of a flexible open chain robotic manipulator, based on the Assumed Mode Method with Rayleigh-Ritz discretization. At the end of the chapter a comparative analysis is made on existing computational modeling tools for flexible robotic manipulators, with focus on symbolic modeling approaches.
Chapter 3 is the subject of interaction control for robotic manipulators. A brief overview of force control methods is given, followed by a description of the impedance control scheme with inner motion control. A thorough analysis of kinematic redundancy, it’s applications in robotics and existing solutions is given, and a novel secondary task function is presented. The final section is a study on vibration control of flexible manipulators, where a vibration controller based on positive velocity feedback is developed. A Closed Loop Inverse Kinematics with Redundancy resolution scheme is also proposed for applications in flexible systems.
In Chapter 4 HipRob, the robotic system which is the platform for this work, is presented, along with the parameters used for generating the model. The Impedance environment in which the robot will operate is also presented. The main aspects of implementation of the controllers presented in Chapter 3 is given, and the final system configuration is described for a rigid and a flexible model of the system.
In Chapter 5 the most significant simulation results for are shown, aimed at validating each controller separately and their connection in the overall system design.
In Chapter 6 the results shown are discussed and conclusions of the work presented are drawn. Future improvements that could enrich the work developed during this dissertation are proposed.
7
8
Chapter 2
Modeling Flexible Manipulators As stated above, the current trend in robotic manipulation designed for human interaction is to develop lightweight systems that are easily and safely wielded by the user. Lightweight robot manipulators have several advantages, such as reduced size, power consumption, faster speeds and accelerations and improved payload-to-weight ratio. However, reducing link weight leads to a natural loss of stiffness, a measure of the resistance of an elastic body to deformation by an applied force. The manipulator will thus vibrate due to elastic deformation. As such, the modeling and control methodologies are significantly more complex than when dealing with rigid systems. In this chapter a formulation for modeling flexible links is presented, followed by a dynamic model architecture for robotic manipulators with both rigid and flexible links. Afterwards, an overview is made of the computational tools required for model generation and simulation.
2.1
Modeling Flexible Links
Flexible manipulators are both nonlinear and infinite dimensional, and as such the design of suitable controllers poses a considerable challenge. Problems arise due to lack of sensing capabilities, vibration due to system flexibility and reduction end-effector precision [Piedboeuf et al., (1993)]. Dynamic models of flexible manipulators are described either by partial differential equations (PDEs) or by finitedimensional ordinary differential equations (ODEs) through some kind of approximation [Tokhi et al., (2008)]. These can be classified as: • Lagrange’s equation and modal expansion (Ritz-Kantrovitch). • Lagrange’s equation and finite element (FE) method. • Euler-Newton equation and modal expansion. • Euler-Newton equation and FE method. • Singular perturbation and frequency domain techniques. 9
The formulation shown here is based on the work by [Martins, (2007)] and [Sa´ da Costa et al., (2008)], which uses Jourdain’s Principle, or the Principle of Virtual Powers, with a Rayleigh-Ritz expansion of the elastic variables.
2.1.1
Kinematics of a Flexible Beam
This methodology makes use of a body reference frame shown in Figure 2.1 to describe the kinematics of the flexible links, thus separating rigid body motion of the beam from elastic deformation. The inertial reference frame is designated {OI , XI YI ZI }, and the body reference frame, designated as {On , Xn Yn Zn }, is rigidly attached to the neutral axis and describes rigid body motion. The cross section reference frame, {Ok , Xk Yk Zk } is located at the intersection of the beam neutral axis with the cross section, and is directed according to the symmetry axis of the cross section. The reference frame attached to the last cross section of the beam is {Onb , Xnb Ynb Znb }.
3.2 Deformation Assumptions Figure 2.1: Flexible beam kinematics
Assumptions are a critical part of the modeling process because the the applicability and the limits of the model depend on them. The following assumptions are the basis for the chosen reference frame. 1. Plane beam cross sections before deformation remain plane after deformation. 2. Bending and shear deformation are considered (Timoshenko beam theory). 3. The beam neutral fiber does not suffer extension. 4. The beam neutral axis in the undeformed configuration is a straight line. 5. The beam cross sections are of constant specific mass, and are symmetrical relative to their principal axis. 6. Local deformation is small, but displacement is not. The shear strains and bending strains are considered to be small, accounted for up to the first order. A flexible beam can then be modeled as a group of rigid cross sections distributed across an inextensible neutral axis, subject to to elastic forces and moments due to small local shear and bending. 10
2.1.2
Dynamic Model of a Flexible Link
The dynamic model of a single flexible link can be derived by applying Jourdain’s Principle which is stated as follows: Z βn0
δvpT ap ρp dβn0 +
Z βn0
˙ Tp σdβn = δG 0
Z βn0
δvpT gρp dβn0 +
Z Σβn0
δvpT fs dΣβn0
(2.1)
Where the first term on the left is the virtual power of the inertial force, the second term is the virtual power of the internal elastic stress, the terms on the right are the virtual power of the gravitational force and the virtual power of the external forces applied on the surface of the body. The physical meaning of the variables in equation 2.1 can be seen in Table 2.1. Table 2.1: Physical meaning of parameters in Jourdain’s Principle
βn0 , Σβn0
Body and its surface in undeformed configuration
vp , ap
Absolute (relative to the inertial frame) velocity and acceleration vectors
Gp
Green-Lagrange strain-displacement relation
σ
Vector of Piola-Kirchoff stresses acting on a cross section
g
Gravity acceleration vector
ρp
Specific mass of the beam at point p
fs
External force vector applied on the beam surface
Applying the kinematic conditions for beam deformation described in section 2.1.1, equation 2.1 can be rewritten in order to separate linear (shear) and angular (bending) motions of the cross section. • The inertial force term can be simplified into: Z Z L e k Jω)dX1 δvpT ap ρp dβn0 = ρA δvkT ak + ρA δω Tk (Jαk + ω βn0
(2.2)
0
where ρA is the beam mass per unit of length, vk and ak are respectively the linear velocity and acceleration of the cross section reference frame, ω k and αk are respectively the angular velocity and acceleration of the cross section reference frame. The term J represents the geometrical moment of inertia tensor of a cross section relative to the cross section reference frame, but expressed in the moving reference frame. • The elastic force term is expanded into: Z Z L T ˙ δ Gp σdβn0 = δ ϕ˙ 12 GAϕ12 + δ ϕ˙ 13 GAϕ13 + δ K˙ 1 GJϕ1 + δ K˙ 2 EI2 ϕ2 + δ K˙ 3 EI3 K3 dX1 βn0
(2.3)
0
where the shear angles ϕ12 and ϕ13 are defined by the classic theory of elasticity, E represents Young’s Modulus and G = E/2. The vector Kk = [K1 K2 K3 ]T is the curvature vector with components along the cross section reference frame. The terms GAϕ12 and GAϕ13 are the elastic forces due to deformation, and the terms GJK1 , EI2 K2 and EI3 K3 elastic moments due to deformation. • The gravitational force term becomes: 11
Z
δvpT gρp dβn0 =
βn0
Z
L
ρA δvkT gdX1
(2.4)
0
The placement of the cross section reference frame at the center of mass of the cross section makes gravity only affect its linear motion. Thus, it is possible to account for the contribution of the gravitational acceleration by simply subtracting the acceleration force vector g from the linear acceleration of a cross section. • The external force term can be expressed by: Z Z Z T T δvp fs dΣβn0 = δvp fn dAn + An
Σβn0
Σβ n0
δvpT fΣβ n
Z 0
dΣβ n + 0
An
δvpT fnb dAnb
(2.5)
where the boundary of the beam has been separated into three parts, referring to the first cross section (equation 2.6), the lateral faces of the beam (Equation 2.7), and the last cross section (equation 2.8). Z δvpT fn dAn = δvnT Fn + δω Tn Mn (2.6) An
Z Σβ n0
Z An
δvpT fΣβ n dΣβ n = 0
0
Z 0
L
δvkT FΣβ n + δω Tk MΣβ n dΣβ n 0
0
(2.7)
0
δvpT fnb dAnb = −δvnTb Renb Fn+1 − δω Tnb Renb Mn+1
(2.8)
Note that in a real application, the reference frame attached to the last cross section of the beam, {Onb , Xnb Ynb Znb }, would coincide with the first cross section of the following body {On+1 , Xn+1 Yn+1 Zn+1 }. The continuous model thus generated is difficult to implement both analytically and computationally. Current analysis and control theory is well established for finite dimensional systems [Martins et al., (2002)], but tackling the continuous model directly is still a considerable challenge. To obtain the system of ODEs that describes the dynamics of the flexible beam, a spatial discretization of the elastic variables in equations 2.2,2.3,2.4 and 2.5 is performed according to the Rayleigh-Ritz approximation. This method is also known as the Assumed Mode Method (AMM). To simplify notation, the terms that are dependent on the elastic variables are rewritten as such: Z X1 Z ξ υ2 = K3 dηdξ = υ 2x (X1 )T υ 2t (t) (2.9) 0
Z
0 X1
Z
υ3 = 0 X1
Z α=
ξ
K2 dηdξ = υ 3x (X1 )T υ 3t (t)
(2.10)
0
K1 dξ = αx (X1 )T αt (t)
(2.11)
0
Z
X1
γ2 =
ϕ12 dξ = γ 2x (X1 )T γ 2t (t)
(2.12)
ϕ13 dξ = γ 3x (X1 )T γ 3t (t)
(2.13)
0
Z γ3 =
X1
0
where υ2 and υ3 are pure bending deflections, α is the pure torsion angle and γ2 and γ3 represent pure shear deflections. υ 2x , υ 3x , αx , γ 2x and γ 3x are shape function vectors and υ 2t , υ 3t , αt , γ 2t and γ 3t are vectors of generalized coordinates. Applying these variables one can define the following global vector of elastic coordinates: T . qen = qTKn .. qTϕn = υ T2t υ T3t αTt 12
.. . γ T2t γ T3t
T (2.14)
which leads to the resulting form for the discretized equations of a flexible beam: Men,rr Men,re An I ΦTrn+1,n Fn+1 + Nen (Vn , q˙ en ) + Ken (qen ) = Fn + FΣn − ¨ en ΦTen+1,n MTen,re Men,ee q 0
(2.15)
or M en,rr MTen,re
I ΦTrn+1,n Fn+1 + (Vn , q˙ en , qen , FΣn ) = Fn − ¨ en ΦTen+1,n Men,ee q Nen,e 0
Men,re
An
Nen,r
(2.16)
where Men is the Mass matrix of the flexible beam, Vn is the generalized velocity vector of the body reference frame, An is the absolute generalized acceleration vector of the body reference frame, which includes linear plus gravitational acceleration and transverse angular acceleration, and Fn is the external generalized force vector expressed in the body reference frame. Nen , Ken and FΣn represent respectively the generalized nonlinear inertial force vector, the load vector resulting from linear elastic forces of the flexible beam and the generalized force vector applied on the boundary of the beam. For simplicity in representation these terms have been grouped under the same vector N en,r . The vector [Φrn+1,n Φen+1,n ]T is a vector of matrix operators that transform generalized (rn ) and elastic (en ) velocities expressed in the current body reference frame {On , Xn Yn Zn } into the following body’s reference frame {On+1 , Xn+1 Yn+1 Zn+1 }. Their respective transposes perform the opposite reference frame transformation.
2.2
Flexible Multibody Systems
Study of flexible manipulators is a part of the research into multibody system (MBS) dynamics. Current mechanical design considers a series of flexible links and rigid components connected by joints in a serial chain. In this work, once again based on [Martins, (2007)] and [Sa´ da Costa et al., (2008)], it is assumed that rigid bodies are the support for actuators at the joints, and have significant mass. A robot manipulator is then modeled as a series of Rigid-Flexible-Rigid elements. The first body in a chain is designated as the base body β1 , while the last body is βN . The inertial body of reference is β0 . Joints are referred to in similar fashion, with the inward joint of body βn being Jn and the outboard joint Jn+1 .
2.2.1
Joint Kinematics
The number of degrees of freedom (DOF) in a joint is a measure of the mobility between the two bodies it connects. A robot floating in space would have complete freedom, and as such the base joint would have 6-DOF. On the other hand, two bodies welded together have no mobility between them and as such are connected by a 0-DOF joint. Figure 2.2 represents two rigid bodies connected by a single joint. The inter-body position vector rn−1,n , describes the position of the second body reference frame {On , Xn Yn Zn } relative to the first {On−1 , Xn−1 Yn−1 Zn−1 }. The rotation of body βn relative to body βn−1 is given by the orthogonal rotation matrix Rn/n−1 . 13
Figure 2.2: Joint kinematics [Martins, (2007)].
The absolute linear (vn ) and angular (ω n ) velocity of βn relative to βn−1 , expressed in {On , Xn Yn Zn } is given by:
RT = n/n−1 ωn 0 vn
RTn/n−1˜ rTn/n−1 RTn/n−1
H + nv ω n−1 0 vn−1
0 Hnω
q˙ nv q˙ nω
⇔ Vn = Φrn,n−1 Vn−1 + Hn q˙ n
(2.17)
where Hnv (qnw ) and Hnω (qnw ) are respectively the translation and rotation Jacobian matrices of joint n. Similarly, the absolute linear (an ) and angular (ω ˙ n ) accelerations are given by: an an−1 RTn/n−1 RTn/n−1˜ rTn/n−1 Hnv 0 q ¨nv = + ω ˙n 0 RTn/n−1 ω ˙ n−1 0 Hnω q ¨nω ^ ˙ nv q˙ nv RTn/n−1 ω ˜ n−1 ω ˜ n−1 rn−1,n + (2(RTn/n−1 ω n−1 ) + ω ˜ n/n−1 )Hnv q˙ nv + H + ˙ q˙ (RT ^ ω )H q˙ +H n/n−1
n−1
nw nw
nw nw
⇔ An = Φrn,n−1 An−1 + Hn q ¨n + Nn
2.2.2
(2.18)
Rigid Components
The dynamic equation for a rigid body is determined by applying the Principle of Virtual Powers in similar fashion as equation 2.1. Disregarding the elastic terms, this leads to: Mrn An + Nrn (ω n ) = Fn − ΦTrn+1,n Fn+1
(2.19)
Assuming massless joints, the generalized external force vector applied by actuators becomes: Tnv HTnv 0 Fn = ⇔ Tn = HTn Fn (2.20) T Tnω 0 Hnω Mn
2.2.3
Serial Multibody System
As stated above, a serial manipulator is modeled as a series of rigid-flexible-rigid (RFR) bodies, where the rigid bodies are seen as supports for the actuators. 14
Let us consider a rigid body βn stiffly connected (0-DOF) to a flexible body βn+1 , who in turn is also stiffly connected to another rigid body βn+2 . This means that actuated joints will only exist at the extremities of the RFR element. These three components form a building block for the modeling of a serial manipulator. The goal is to obtain recursive dynamic equations that allow calculation of the system parameters.
The generalized coordinates of the RFR body are the absolute linear and angular velocities of rigid h iT body βn , given by Vn = vn ω n and the global vector of elastic coordinates of flexible body βn+1 , h iT qen+1 = qTKn+1 qTϕn+1 . The dynamic equation for the nth RFR element in a serial chain is then given by:
An
+ Nrf rn Mrf rn q ¨en+1
Fn (Φrn+3,n+2 Φrn+2,n+1 Φrn+1,n )T = − 0 (Φrn+3,n+2 Φen+2,n+1 )T
0 Fn+3 0 0
⇔ Mrf rn Arf rn + Nrf rn = Frf rn − ΦTrf rn+1,n Frf rn+1
(2.21)
where the mass matrix Mrf rn and the nonlinear inertial force vector Nrf rn of the RFR body are given by:
Mrf rn
Nrf rn
Mrn 0 ΦTrn+1,n Men+1,rr Φrn+1,n ΦTrn+1,n Men+1,re + = 0 0 MTen+1,re Φrn+1,n Men+1,ee (Φrn+2,n+1 Φrn+1,n )T Mrn+2 Φrn+2,n+1 Φrn+1,n (Φrn+2,n+1 Φrn+1,n )T Mrn+2 Φen+2,n+1 + ΦTen+2,n+1 Mrn+2 Φrn+2,n+1 Φrn+1,n ΦTen+2,n+1 Mrn+2 Φen+2,n+1
Nrn ΦTrn+1,n (Men+1,rr Nn+1 + Nen+1,r ) + = MTen+1,re Nn+1 + Nen+1,e 0 (Φrn+2,n+1 Φrn+1,n )T (Mrn+2 (Φrn+2,n+1 Nn+1 + Nen+2 ) + Nrn+2 ) + ΦTen+2,n+1 (Mrn+2 (Φrn+2,n+1 Nn+1 + Nen+2 ) + Nrn+2 )
(2.22)
(2.23)
The generalized velocity, Vrf rn , acceleration, Arf rn , and externally applied force Trf rn vectors of the nth RFR element are: 15
Φ Φ Φ = rn,n−1 rn−1,n−2 rn−2,n−3 q˙ en+1 0 Vn
Φrn,n−1 Φen−1,n−2 0
H + n q˙ en−2 0 Vn−3
0 I
q˙ n q˙ en+1
⇔ Vrf rn = Φrf rn,n−1 Vrf rn−1 + Hrf rn q˙ rf rn
(2.24)
Φrn,n−1 Φrn−1,n−2 Φrn−2,n−3 Φrn,n−1 Φen−1,n−2 An−3 H = + n q ¨en+1 0 0 q ¨en−2 0 Nn + Φrn,n−1 (Φrn−1,n−2 Nn−2 + Nen−1 ) + 0 An
0 q ¨n q ¨en+1 I
⇔ Arf rn = Φrf rn,n−1 Arf rn−1 + Hrf rn q ¨rf rn + Nrf rn
T HT n = n 0 0
(2.25)
0 Fn I 0
⇔ Trf rn = HTrf rn Frf rn
(2.26)
The final system for a determined serial RFR body manipulator can then be determined from equations 2.21, 2.24, 2.25 and 2.26, which are solved using elimination methods such as the Articulated Body method or the global dynamics Composite Inertia (CI) method. Applying the CI method the joint space dynamic equation for a flexible manipulator becomes: (HTrf r ΦTrf r Mrf r Φrf r Hrf r ) q ¨ + HTrf r ΦTrf r (Mrf r Φrf r Nrf r + Nrf r ) = T | | {z } {z } M
(2.27)
N
where the product Φrf r Hrf r is a Jacobian Matrix. This solution forms the basis for solving forward and inverse dynamics for a chain of RFR bodies.
2.3
Computational Strategies
The development of the final system of dynamic equations of a flexible multibody system is naturally determined computationally. Extensive reviews of such strategies are available in the literature, such as in [Wasfy & Noor, (2003)]. In this work a small number of available solutions for model generation will be analyzed.
2.3.1
Symbolic Modeling
The expressions for obtaining the dynamic equations in section 2.2 can be obtained using either numeric or symbolic methods. A symbolic approach consists of deriving a mathematical model of the system in terms of parameters and time dependent variables in symbolic form, before running simulation. Some of the advantages of symbolic modeling are [Piedboeuf et al., (2002)]: • Simplification of the equations: removing null expressions or trigonometric simplifications can significantly reduce the amount of computational time required in simulation. 16
• Model generated only once: numerical approaches require the model to be generated at each simulation step, which can greatly increase the weight of calculations during simulations. In a symbolic approach, the modeling is decoupled from the simulation. • Auxiliary variables: avoid repeated computation of the same terms. However, the numerical approach has been the main tool used for investigation in constrained multibody systems, as the symbolic implementation of these methods can be difficult. Also, increasing the degrees of freedom (DOF) of the system exponentially increases the complexity of the symbolic expressions, which can lead to memory management problems and difficulty of handling complex expressions [Moore et al., (2002)]. Some authors [McPhee et al., (2002)] seek to overcome this problem by use of dummy Jacobian matrices for symbolic formulation, which are evaluated during the numerical simulation.
2.3.2
Software Tools
Due to the complexity of the equations generated in modeling flexible manipulators, current processing power is unable to efficiently simulate a fully numerical model in real time. For this reason, most R control-oriented numerical modeling tools, such as the Robotics Toolbox for Matlab [Corke, (1995)] or
SimMechanics1 do not support models with flexible elements. As such, the study of flexible robots has led to the development of several symbolic modeling tools, that allow simulation and control of complex systems with many degrees of freedom. The following is an overview of three existing symbolic modeling tools and their underlying principles.
Dynaflex/MapleSim Dynaflex2 is a computer package for generating the kinematic and dynamic equations for multibody mechanical systems, with only a description of the system as input. It was initially developed for the computer algebra package MapleTM , and has recently been incorporated within the physical modeling tool MapleSim3 . The framework uses Linear Graph theory to represent the system topology. A linear graph is a collection of edges: a line segment which has at its endpoint a vertex (or node). The topology of the linear graph is completely defined when one specifies which edges are connected to each and every vertex of the graph. From it, an incidence matrix is constructed, containing a complete topological description of the physical system [McPhee, (1996)]. An example of a linear graph of a multibody system is shown in Figure 2.3. The nodes are used to represent reference frames in the system, while edges represent physical elements that connect these frames. The inertia effects of the bodies are represented by edges that originate at a Datum node and terminate at the center of mass. 1 http://www.mathworks.com/products/simmechanics/ 2 http://real.uwaterloo.ca/compute.htm 3 http://www.maplesoft.com/products/maplesim/index.aspx
17
e 62 e 23 e22
Y
J2 C1
T J1
e 24 α2
e 21
C2 α3
α1
X
s
e 11
e12
e61
J3
e 63
T Datum
e64
Figure 2.3: Planar Slider-Crank Mechanism and Linear Graph Representation [Shi & McPhee, (2002)].
From the linear graph, sets of topological equations can be generated that relate the through (Force) and across (Velocity) variables. They are based on: the vertex postulate, which states that for each node of the graph, the oriented sum of through variables associated with the edges connected to the node must be zero; and on the circuit postulate, which states that for each closed loop (circuit) of the graph, the oriented sum of across variables associated with the edges involved in the loop must be zero [Sass et al., (2004)]. The kinematic equations that describe the system are derived from the circuit equations, while the full dynamic model of the system is extracted from a single expression for the Virtual Work of the entire system [Shi & McPhee, (2000)]. Dynaflex also offers the ability of selecting the variables that will appear in the final equation. This allows different models to be constructed from the same graph, and is also suitable for generating symbolic equations for complex systems. However, a critical drawback of this program is that it is only capable of generating equations considering first order strain-displacement relationships in the kinematics of the flexible links. This limitation affects the applicability of the program, as some displacements can only be captured if second order relationships are considered [Piedboeuf, (1996)].
SYMOFROS SYMOFROS4 stands for Symbolic Modeling of Flexible Robots and Simulation. It is a general purR pose program based on Maple , developed by the Canadian Space Agency (CSA) to simulate and
control complex space manipulators. The program generates dynamic and kinematic equations for multi-link flexible manipulators through a recursive formulation based on Jourdain’s Principle, which considers bending in two directions, torsion and foreshortening of the flexible beam. Formulation of the dynamic equations is described in detail in [Piedboeuf, (1998)], while detailed description of the algorithm’s intrinsic functions can be found in [Piedboeuf, (1996)]. R The algorithm is implemented in a series of modules depicted in Figure 2.4. In the Simulink environ-
ment, the topology of the system is described using a graphical interface. In SYMOFROS, a mechanism 4 http://www.softsim.ca/symofros/index.html
18
is described through an object oriented approach. Two main objects considered are: generalized body, composed of a rigid or flexible body and a joint; or a closure which is used to implement closed kinematic loops [Piedboeuf et al., (1999)]. SYMOFROS MATLAB & SIMULINK Graphical Interface
MATLAB & SIMULINK
MAPLE
Model Generator
Symbolic Modelling
C Code Generator
MATLAB Interface
SIMULINK NRT Simulation
SIMULINK RT WORKSHOP Real-Time Code Generation
RT Simulation
Figure 2.4: Symofros modules overview (Adapted from [McPhee et al., (2002)]).
A symbolic model generator then computes the dynamic and kinematic equations of the bodies and R joints in Maple , which are optimized and written in C Code in the form of functions [Moore et al.,
(2003)]. The model can then be simulated in real time in Simulink using Matlab’s Real-Time toolbox. Forward and inverse dynamics can be calculated by use of function blocks available in the SYMOFROS library. This tool is attractive for generating the model of a robotic manipulator, since it allows generation of the dynamic equations without great knowledge of symbolic manipulation from the user. The included SIMULINK blocks also enable fast and simple controller setup and simulation. However, one limitation of this software is the assumption of Bernoulli’s hypothesis: direct shear strains are ignored, which means that beam sections remain plane and perpendicular to the neutral axis. This means that only slender beams can be accurately modeled [Piedboeuf, (1998)]. Support for the SYMOFROS package has been discontinued, and the software is incompatible with R R recent versions of Matlab or Maple . It has evolved into a new package called the Multibody Toolbox
(MuT).
MECANISMO Another symbolic methodology, developed by [Martins, (2007)], is a spatially recursive scheme for deriving a global optimized symbolic model of the flexible manipulator. Its formulation is based on the Principle of Virtual Powers described in sections 2.1 and 2.2. The kinematic assumptions for modeling flexible links are the ones described in section 2.1.1. The algorithm is spatially recursive working from base to tip and solves kinematics, dynamics and differential kinematics. The recursivity of Φ, N and V allows for the use of intermediate auxiliary variables that replace current expressions and are only substituted in the final loop. The procedure has R been implemented in a Matlab toolbox called MECANISMO, depicted in Figure 2.5. R The description of the system topology is made via Matlab M-file where the number of bodies,
their type (rigid or flexible), geometric (lenght, center of mass) and dynamic properties (mass, inertia moments, Young’s modulus) are defined in a structure of consecutive bodies. Afterwards, the symbolic R model is manipulated and optimized in the Maple workspace, written in C code and finally exported to R Simulink via S-functions.
19
Figure 2.5: MECANISMO Simulink block library.
The blockset also provides as an output the displacements (xk) and orientations (Rk) of specific cross-section reference frames of the flexible links, which can be used as input for the Virtual Reality TM
Modeling Language (VRML) model through Matlab’s Virtual Reality Toolbox
. Forward dynamics, a
useful solution for control theory, can be obtained by applying an LU solver to the mass and non-linear matrices to calculate the system accelerations, followed by double numerical integration. One of the main advantages of this methodology is the assumption of Timoshenko beam theory, which includes shear deformation up to the first order. As such, this tool can accurately model nonslender beams.
The above described tools generate models of a system according to different principles. It is difficult to determine off-hand the best solution for a given application, and comparative simulations should be run to ascertain which approach is more suited. Unfortunately, we have been unable to obtain licenses for the MuT package, and thus the MECANISMO toolbox has been chosen for model generation.
20
Chapter 3
Force Control of Flexible Manipulators Chapter 1 illustrated the need for co-manipulation control strategies for surgical robots. Active constraints are the most used solution, and are based on force control methods. In this chapter an overview of interaction control strategies is given, with focus on indirect force control. A description the principles of impedance control is given, followed by an analysis of redundancy and its implications in control methodologies. An impedance control scheme with redundancy resolution is presented for rigid manipulators. Finally, a study on the implications of force control to flexible manipulators is made. A vibration suppression controller and a Closed Loop Inverse Kinematics with Redundancy resolution scheme are proposed.
3.1
Interaction Control
Control of the interaction between the manipulator and environment is a fundamental requirement for the success of a manipulation task. The use of motion control alone is a candidate for failure, as most interaction tasks place the manipulator into an environment that is difficult to describe in detail [Siciliano et al., (2009)]. Contact forces may increase due to lack of precision and lead to overall system instability [Kazerooni et al., (1986)]. Force control arises as a more natural strategy for handling unconstrained manipulation tasks. Interaction control strategies can be grouped in two categories [Villani & Schutter, (2008)]:
Direct force control - The contact force between the manipulator and the environment is directly controlled to a desired value. This requires an explicit model of the interaction task, and the user must specify the desired moment and contact force respecting the constraints of the environment. The most common schemes are: • Strict force control • Hybrid force/motion control - A special strategy which aims at controlling the motions along unconstrained task directions and force/moment along constrained task directions. 21
Indirect force control - Force control is achieved via motion control, without explicit closure of a force feedback loop. The deviation of the end-effector motion from the desired reference is related to the contact force through a mechanical impedance. Common implementations are: • Compliance control - If only the static relationship between end-effector position and orientation (pose) deviation is considered, also named stiffness control [Salisbury, (1980)]. The robot is under a position control scheme. • Impedance control - If both pose and velocity deviation of the end-effector are considered. The robot is under the action of an inverse dynamics control law in the operational space. This work is not meant to be a comparative analysis of different force control schemes, which can be found in most classical robotics textbooks such as [Siciliano & Villani, (1999)]. The focus of this dissertation will be on indirect force control, more specifically in Impedance Control with Inner Motion Control. It has been determined in a previous work by [Feio, (2008)] that this controller is adequate for the proposed tasks.
3.2
Impedance Control
Impedance control, as the name implies, is a control strategy based on manipulating the impedance of the robotic system, a measure of opposition to motion of a structure subjected to a force. Unlike position-based compliance control schemes, in impedance control the goal is to achieve a desired dynamic behavior by relating velocities to forces acting on the mechanical system [Hogan, (1984)]. The robot manipulator then acts as a mass-spring-damper system with adjustable parameters, and reacts to motion deviation by generating forces. Impedance control is often referred to as admittance control [Villani & Schutter, (2008)] when the robot control reacts to interaction forces by imposing a deviation from the desired motion. In this work only the term impedance control will be used. Let us consider the dynamic model for a rigid manipulator subject to external forces: T = M (q)¨ q + N (q, q) ˙ + J (q)T h
(3.1)
where T is the vector of driving torques, M (q) is the rigid manipulator mass matrix and N (q, q) ˙ is a non-linear vector which includes the vector of Coriolis and centrifugal torques, the vector of friction torques and the vector of gravitational torques. J is the manipulator Jacobian matrix and h = [f , µ]T is the vector of contact forces/moments exerted by the end-effector on the environment. Applying the well known inverse dynamics control scheme leads to T = M (q)y + N (q, q) ˙
(3.2)
y = J −1 (q).(ae − J˙(q, q) ˙ q) ˙
(3.3)
with y given by
22
where ae = [ap ao ]T is the end effector acceleration in Cartesian coordinates. Using this methodology control is made directly at the end-effector acceleration level, which allows a better understanding of the physical meaning of the parameters. The aim of an impedance controller is to establish a mass-spring-damper relationship between the Cartesian position/orientation ∆x and the Cartesian force/moment h, according to: h = K MI ∆¨ x + K DI ∆x˙ + K PI ∆x
(3.4)
where K MI ,K DI ,K PI are positive definite matrices defining the virtual inertia, damping and stiffness of the system. In [Siciliano & Villani, (1999)], it is shown that it is difficult to successfully guarantee good disturbance rejection while keeping a low desired impedance with a single controller. This is related to the fact that in order to increase the gains towards achieving a null tracking error, the associated stiffness will also increase.
3.2.1
Impedance Control with Inner Motion Control
To achieve decoupling between position control and impedance control, [Siciliano & Villani, (1999)] propose an Impedance Controller with Inner Motion Control, depicted in Figure 3.1. The methodology makes use of a compliant frame xc which is modified by the impedance controller and serves as input to the motion controller. In view of this intermediate frame, equation 3.4 is rewritten as: K MP I ∆p¨dc + K DP I ∆p˙ dc + K PP I ∆pdc = f
(3.5)
for the position impedance control, with ∆pdc = pd − pc . Notice that pc can be obtained by successive integration of the above equation, and the associated position control signal is calculated by: ap = p¨c + K DP ∆p˙ ce + K PP ∆pce
(3.6)
Defining the orientation of the robot end-effector presents a number of challenges due to representation singularities. To avoid such occurrences the following expressions make use of a quaternion representation of the orientation, Q = [η, ], presented in [Chiaverini & Siciliano, (1999)]. Accordingly, equation 3.4 is rewritten in terms of end effector orientation as: K MOI ∆c ω ˙ dc + K DOI ∆c ω dc + K 0 POI c dc = cµ
(3.7)
with ∆c ω ˙ dc = RTc (ω d − ω c )
(3.8)
K 0 POI = 2ηdc .c dc K POI
(3.9)
where c µ is the vector of applied moments to the end effector, c dc is the quaternion orientation error and Rc is the rotation matrix defining the orientation of the compliant frame. Similarly to equation 3.7, ωc and Rc and can be obtained by successive integration of the above equation. The orientation control signal is then defined according to: ao = ω ˙ c + K Do .∆ω ce + K P o .Re .e ce 23
(3.10)
The control scheme for the spatial impedance controller with inner motion control is shown in picture 3.1. It can be clearly seen that using this methodology, the impedance control is defined as an outer control loop applied to a manipulator already under an inverse dynamics control law with pose control.
pd , R d p˙ d , ω d p¨ d , ω˙ d
SPATIAL IMPEDANCE
pc , R c p˙ c , ω c p¨ c , ω˙ c
POS & ORIENT CONTROL
ae
INVERSE DYNAMICS
pe , R e p˙ e , ω e
τ
MANIPULATOR & ENVIRONMENT
f ,μ q˙ q
DIRECT KINEMATICS
Figure 3.1: Spatial impedance control with inner motion control [Siciliano & Villani, (1999)].
The control scheme presented above has been developed for rigid, non-redundant robots, for which the Jacobian matrix is square and of full rank. However, when the robot is redundant the number of columns in the Jacobian matrix exceeds the number of rows, and the matrix is non-invertible. The following section is an analysis of redundant manipulators, and presents a control scheme for spatial impedance control of redundant manipulators.
3.3
Analysis of Redundancy
Kinematic redundancy occurs when a manipulator possesses more degrees of freedom than the minimum number required to execute a given task. Although most manipulators possess a suitable structure, reduced manipulability due to joint limits, kinematic singularities or obstacles in the workspace may reduce the extent of their applications. When a manipulator is redundant, the inverse kinematic problem admits a number of solutions. This implies that, given a constant end effector pose, it is possible to induce internal motions of the structure without affecting the end effector. This property makes redundant manipulators much more versatile, as the additional DOF can be used to fulfill other user-defined tasks. It is considered as a major characteristic in performing tasks that require dexterity comparable to the human arm [Patel & Shadpey, (2005)]. The degree of redundancy for a robot manipulator is defined as: r =n−m
(3.11)
where n is the dimension of the joint space (number of DOF) and m is the dimension of the task space.
Common uses for redundant degrees of freedom include: • Obstacle avoidance: Use redundant degrees of freedom to avoid collisions or enable reaching objects blocked by obstacles. 24
• Minimization of power consumption: Configurations that minimize effects of gravitational forces reduce required power to the actuators. • Joint limit avoidance: Avoiding mechanical joint limits keeps the manipulator in configurations that maximize its manipulability. • Singularity avoidance: Kinematic singularities occur when, for a given configuration, the Jacobian matrix loses rank. In such cases the manipulator is unable to move along a direction of the task space. Avoidance of these situations can be made by maximizing the manipulability measure q det(J J T ), which is a measure of the proximity to a kinematic singularity. • Task compatibility: In motion control, the velocity ellipsoid, v Te (J J T )− 1v e , is used to determine the ability of the end effector to arbitrarily change position or orientation. In force control applications, the force ellipsoid, hT (J J T )−1 h, can be used to maximize the transfer ratio from motors to the end effector in a given direction.
3.3.1
Redundancy Resolution
The kinematic control problem can formulated as: for a given trajectory in the operational space x(t), find a joint space trajectory q(t) such that f (q(t)) = x(t) . It is easily seen that for a redundant manipulator, the inverse kinematics equation admits multiple solutions, as there is more than one configuration of joints q that generate the same desired end-effector position/orientation [Siciliano, (1990)]. The following schemes are based on an instantaneous or local resolution of redundancy. Global implementations have been proposed but at an increase in computational cost that makes them unsuitable for on-line control applications [Patel & Shadpey, (2005)]. Cartesian controllers generate commands that are expressed in the operational space. In case of a redundant robotic manipulator, the inputs need to be projected into joint space. This can be done on two levels, depending on the application requirements and chosen controller. For redundancy resolution at the velocity level, a first order differential kinematics equation (3.12) is used, while redundancy resolution at the acceleration level requires second order differential kinematics (equation 3.13). x˙ = J q˙
(3.12)
x ¨ = J q¨ + J˙q˙
(3.13)
The Jacobian matrix J m×n can be seen as a linear mapping of Rn into Rm . For a redundant manipulator, J has more rows than columns and the above equations represent under-determined systems of equations.
Redundancy resolution at the velocity level The most common method for redundancy resolution is based on the Moore-Penrose pseudoinverse of the Jacobian matrix, defined as: J † = J T (J J T )−1 25
(3.14)
for which the first order inverse kinematics equation can be written as: q˙ = J † (q)x˙
(3.15)
The ability of the pseudoinverse to provide a meaningful solution in the least squares sense regardless of whether equation 3.12 is under-specified, square or over-specified makes it the most attractive technique in redundancy resolution [Patel & Shadpey, (2005)]. The pseudoinverse solution at velocity level minimizes joint velocities, and thus stabilizes internal motion of the manipulator. However, the approach has a number of limitations: • The solution does not guarantee generation of joint motions which avoid singular configurations. In the proximity of singularities the norm of the solution becomes very large. A solution to this problem can be found if a modified Jacobian matrix is used that is non singular in the entire workspace, such as the damped least-square inverse of the form J ∗ = J T (J J T + λ2 I)−1 . The solution generated is an inverse kinematic approximation which limits velocity in the neighborhood of singularities. Increasing the damping factor λ provides good behavior but at a cost of reduced accuracy near singular points. • Joint motions do not preserve the repeatability and cyclicity conditions: a closed path in Cartesian space does not imply a closed path in joint space. This can become a problem for industrial applications as the robot is expected to autonomously perform the same task repeatedly. • The extra degrees of freedom are not used to satisfy any additional user-defined tasks. An important concept that must be introduced here is that of the null space of the Jacobian matrix, which is defined as the set of all vectors q for which J.q = 0. The natural conclusion is that a joint velocity vector q˙ 0 which belongs to the null space of the Jacobian will generate internal motions of the manipulator without influencing the end effector. The joint velocity vector can then be complemented by an additional term belonging to the null space of the Jacobian matrix, and equation 3.15 is rewritten as: q˙ = J † (q)x˙ + [I − J † (q)J (q)]q˙ 0
(3.16)
where I is the identity matrix and q˙ 0 is a joint velocity vector. The projection operator (I − J † J ) selects the components of q˙ 0 in the null space of J , and thus only generates internal motion of the structure. A common approach for defining additional tasks is to optimize a scalar cost function h(q) by use of the gradient projection method. The velocity vector available for redundancy resolution is defined as q˙ 0 = (∂h/∂q)T , and the solution acts as a gradient optimization method which converges to a local minimum of the cost function. Another alternative is to use task space augmentation, by imposing an additional constraint task to be executed simultaneously with the main task. The subsequent augmented Jacobian matrix becomes J = [J e J c ]T , where J e and J c denote respectively the main task and additional task Jacobian. The main advantage of this method is that the manipulator kinematics have been expanded, and equation 3.15 is no longer undetermined. As a consequence, the manipulator ceases to be redundant. This 26
method however has major drawbacks namely: the dimension of the additional task must be equal to the degree of redundancy r, making the approach not applicable for tasks not active at all times; If either of the Jacobians are at a singular point, or if there is a conflict between both tasks and both Jacobians become linearly dependent, the extended Jacobian loses rank which may lead to instability.
Redundancy resolution at the acceleration level Dynamic control of redundant manipulators in task space, such as compliance control, requires computation of joint accelerations. In these circumstances, redundancy should be explored on an acceleration level. Recalling equation 3.13 for the second order differential kinematics, the respective inverse kinematics equation is written as: q¨ = J † (q)[¨ x − J˙(q)q] ˙
(3.17)
Analogously to equation 3.16, an additional joint acceleration vector q¨0 can be projected into the null space of the Jacobian to satisfy additional user defined constraints, leading to: q¨ = J † (q)[¨ x − J˙(q)q] ˙ + [I − J † (q)J (q)]¨ q0
(3.18)
The dynamic approach to solving redundancy resolution has one important limitation that must be accounted for: just as the pseudoinverse solution for velocity minimizes null space velocity components, the solutions for acceleration are aimed at minimizing the null space joint accelerations. However, joint velocities that belong to the null space of the end effector Jacobian are not controlled. This may result in the so-called “self-motion” effect, which can lead to internal instabilities [Patel & Shadpey, (2005)]. A number of solutions have been proposed to counteract this limitation. In [Natale et al., (1999)], redundancy at the acceleration level is solved while stabilizing null space velocities. This methodology is implemented in an impedance control scheme which is the subject of the following section.
3.3.2
Impedance Control with Redundancy Resolution
The solution presented here is similar to the impedance controller with inner motion control described in section 3.2.1, with the inclusion of an additional term for redundancy resolution. Recalling equation 3.2 for the inverse dynamics of a robotic manipulator: T = M (q).y + N (q, q) ˙
(3.19)
y = J # (q).(ae − J˙(q, q). ˙ q) ˙ + φn
(3.20)
where y is now given by:
The term φn denotes a joint acceleration vector which is lying in the null space of the Jacobian matrix, which can be assigned to control the redundant degrees of freedom. Also, J # is the dynamically consistent pseudoinverse defined by: J # (q) = M −1 (q)J T (q)[J (q)M −1 (q)J T (q)]−1 27
(3.21)
This approach has the advantage of producing no null space accelerations when the end effector is subject to an external force or moment. This property takes additional relevance in the context of interaction control schemes. The method of redundancy resolution will depend on the choice of the joint space acceleration vector φn . A suitable choice is then to define φn as to ensure stabilization of null space velocities while optimizing an additional task functions. The first step is to define a null space velocity error en , so that it converges asymptotically to zero. Consider en = [I − J # (q)J (q)](γ − q) ˙
(3.22)
where γ is a joint velocity vector available for redundancy resolution. Choosing #
φn = (I − J # J )[γ˙ − J˙ J (γ − q) ˙ + B −1 (K n en + Cen )]
(3.23)
where K n is a positive definite matrix and C is the manipulator matrix of centrifugal and Coriolis forces, leads to asymptotic stabilization of internal velocities en → 0. The manipulator now has stable behavior when dealing with kinematic redundancy. The final step is to choose the appropriate use of the redundant degrees of freedom, for which a common choice is:
γ = kγ M
−1
∂ω(q) ∂q
(3.24)
where kγ is a signed scalar and ω(q) is a user defined task function that is locally optimized. The block diagram depicting the impedance control scheme with redundancy resolution is shown in Figure 3.2.
φn
pd , R d p˙ d , ω d p¨ d , ω˙ d
SPATIAL IMPEDANCE
pc , R c p˙ c , ω c p¨ c , ω˙ c
POS & ORIENT CONTROL
ae
INVERSE DYNAMICS
pe , R e p˙ e , ω e
REDUNDANCY RESOLUTION
τ
MANIPULATOR & ENVIRONMENT
f ,μ q˙ q
DIRECT KINEMATICS
Figure 3.2: Spatial impedance control with redundancy resolution [Natale et al., (1999)].
In this work, a novel task function is presented, aimed at augmenting the interaction the manipulator has with the environment. Assuming that additional force measurements are available at an intermediate point of the kinematic chain, redundancy can be explored to allow reconfiguration of the structure to 28
accommodate the needs of the user. The objective is then to generate internal joint motions based on force measurements, which can be accomplished by applying a joint based impedance controller according to: J T ∗ h∗ = K M q ∆¨ q cr + K Dq ∆q˙ cr + K P q ∆q cr
(3.25)
where h∗ is the auxiliary force sensor inputs, J T ∗ is the Jacobian matrix of the manipulator up to the element where the auxiliary force sensor is placed. The terms K M q , K Dq and K P q are gain matrices associated to the inertial, damping and stiffness components. The choice of parameters will naturally depend on the required application, but should in general be chosen to have a critically damped behavior in order to avoid oscillations. The task function ω(q) to optimize is then given by:
ω(q) =
1 (q − q c )2 2
(3.26)
where q c can be obtained through successive integration of equation 3.25. With this scheme, a manipulator that is given a constant Cartesian reference position and orientation, but who is actuated at an intermediate point by an external force, will generate dynamic joint motion without affecting the end-effector. With this methodology it is possible to expand the impedance controller described in the previous section to redundant manipulators. However, the approach is still only valid for rigid manipulators. The following section describes a methodology that allows application of the preciously described algorithms for flexible manipulators.
3.4
Flexible Manipulators
Tip tracking control is an essential part of any Cartesian control scheme. Control of flexible link manipulators has the difficulty of non-minimum phase property due to the finite speed of propagation of the mechanical wave along the links since the joint actuators are non-colocated. The transfer function between the input (torque) and the output (end-effector position) is non-minimum phase, even for a single-link. When multiple links are considered, this problem increases in complexity due to the distributed flexibility. The system is also underactuated, as the number of controlled variables is strictly less than the number of the mechanical degrees of freedom. The dynamic behavior of a flexible link manipulator can be considered as a collection of rigid body dynamics and flexible dynamics. Thus, control stratagems for these systems are derived to account both for rigid body motion, developed within the standard rigid manipulator control, and flexible motion which falls under the subject of vibration control of flexible structures. Vibration control schemes can be classified into two generalized categories [Tokhi et al., (2008)]: • Passive Control - Uses the absorption property of matter and is realized by a fixed change in the physical parameters of the structure. 29
• Active Control - Uses the principle of wave interference by artificially generating a destructive anti-source that interferes with the disturbances and reduces the level of vibration. Active control techniques are subdivided into feedforward (open-loop) and feedback (closed-loop) approaches:
Feedforward control Feedforward control involves altering the reference input based on the inherent physical properties of the system, so that system vibrations at response modes are reduced. This method does not require any additional sensors, but does not adapt to any changes in the dynamics of the system once the input is generated. The most common methods are forcing functions, command shaping techniques, computed torque control and bang-bang control. Forcing functions include use of a Fourier expansion to reduce peaks of the frequency spectrum at discrete points. In command shaping techniques, a forcing function is generated that attempts to minimize system vibrations, which can include use of filtering techniques that attempt to extract the input energy around the natural frequencies of the system [Mohamed et al., (2005)]. Common problems arise due to long response time, instability due to un-reduced modes and robustness in case of change in dynamics. In the computed torque approach, the input is generated by inversion of a model of the system. This technique suffers from a generality of problems owing to system inaccuracy, trajectory modeling or sensitivity to system parameters. Bang-bang control uses single and multiple switch bang-bang (on-off) functions that act as input to the system. These functions require an accurate selection of switching time, as a minor modeling error can lead to excitation of natural frequencies and subsequent increase in residual vibration. The major drawback of the feedforward control schemes is their inability to cope with disturbances to the system or parameter variations.
Feedback control Contrastingly to feedforward techniques, feedback control uses direct measurements and estimates of the system to alter the reference input, attempting control of rigid body motion while suppressing vibration suppression. Sensors that are commonly used are strain gauges that measure flexural mode shapes or accelerometers. These, however, provide only local information and thus can lead to inaccuracies in end point tracking. If every sensor is located at the same location of the actuator, the system is collocated and inherently stable. If, on the other hand, the sensors are located away from the actuator, the system is noncollocated. Control schemes where the end position is measured directly such as [Tso et al., (2003)] lead to more accurate tip tracking but suffer from non-minimum phase problems and stability issues. In [Mohamed et al., (2005)], strain gauges are located close to the hub of the manipulator to avoid problems associated with non-collocated control. 30
Hybrid control It is common practice to combine both methodologies and apply a hybrid controller composed of feedforward torques to maneuver the flexible manipulator along the nominal trajectory while feedback torques are used to minimize deviation [Kilicaslan et al., (2010)]. Some authors [Singhose & Seering, (2008)] suggest the use of command generation techniques to provide the input to a system controlled by both a feedforward and a feedback contribution. A block diagram of a generic system controlled by a hybrid controller is depicted in Figure 3.3.
Feedforward controller + Desired performance
Command generator
∑
System +
+
∑
Feedback controller
_
Figure 3.3: Generic system block diagram [Singhose & Seering, (2008)].
Studies in vibration suppression of single or two-link flexible manipulators have been extensive, and various kinds of control techniques have been developed. Linear control, optimal control, adaptive nonlinear control, sliding mode control, inverse dynamics method and singular perturbation approach are some of the many examples present in the literature [Lee et al., (2001)]. Surprisingly, in the revised literature, very little work has been published regarding impedance control of multi-link flexible robots. These focus mainly in situations where the manipulator’s end effector is given a constant position and orientation, where the effects of inertial terms can be dismissed [Siciliano, ¨ (1998)]. Other works resolve impedance on joint level, such as [Albu-Schaffer & Hirzinger, (2002)], where a flexible joint model is assumed. Reference positions and actuating forces are translated to joint space, and impedance is resolved at joint level. To accomplish this each motor needs to be instrumented with an individual torque sensor. When considering redundant manipulators with flexible links, [Nguyen et al., (1992)] proposed a method for using self motion to increase structural stiffness through dynamic coupling, effectively damping the flexural modes. This method however, does not allow using the redundant degrees of freedom for any additional user-defined tasks. An interesting approach to flexible manipulator force and position control has been proposed in [Siciliano & Villani, (2000)] and [Siciliano & Villani, (2001)], based on singular perturbation theory. If the link stiffness is high, a two-time scale model of the flexible manipulator can be derived, consisting of a slow subsystem describing the rigid body dynamics and a fast subsystem that describes flexible motion. A composite control strategy is then adopted, where a slow controller is developed to guarantee tracking accuracy for an equivalent rigid manipulator and a fast controller is used to stabilize vibrations. 31
3.4.1
Vibration Suppression
In this work a simple approach is pursued. If link stiffness is high, the dynamics associated to rigid body motion are much slower than the dynamics of the flexible links. Thus the controller setup is based on an inverse dynamics scheme that deals with rigid body motion complemented with an inner control loop that deals with vibration supression.
Inverse Dynamics for Flexible Manipulators One of the most common type of schemes used in dynamic control of robotic manipulators is the inverse dynamics controller. Spatial impedance control (section 3.2) uses inverse dynamics (equation 3.2) to generate the necessary torque inputs to the system. The inverse dynamics control law for flexible manipulators can be deduced from equation 2.27 describing the dynamic model of a flexible manipulator subject to an external force: M rr M er
Nr τ JT q¨r = r − r h + J Te q¨e Ne 0 M ee
M re
(3.27)
where the mass matrix M has been grouped under sub-matrices representing the rigid, elastic and coupling terms. The same separation has been applied to the non linear terms N and the Jacobian matrix J , and thus the rigid terms from the top equation are isolated from the flexible ones. Notice that as the only actuators considered are the joint motors, the torque vector only applies to the rigid terms. The inverse dynamics control law is then given by: i h T −1 T q¨r − M re M −1 M rr − M re M −1 ee N e + N r − M re M ee J e h + J r h = τ r ee M er
(3.28)
This controller however, is designed solely for control of the rigid degrees of freedom, and does not eliminate vibrations of the flexible links. It is then necessary to include an additional feedback control loop to enhance the damping of the system.
Direct Velocity Feedback The controller described here is based on Direct Velocity Feedback methods [Preumont, (2002)]. The objective is to generate control inputs that act as viscous damping and effectively dissipate energy associated with elastic vibration. Assuming that link modal deflections and deflection rates are measurable through the use of strain gauges, it is possible to calculate the velocity of deformation for each link using: ω ˙ i (xi , t) =
mi X
ϕij (xi )q˙ eij (t)
(3.29)
j=i
where ω ˙ i (xi , t) represents the transversal deformation rates from link i at distance xi from the base of the beam. The term q˙ eij (t) are modal deflection rates associated with modal functions ϕij (xi ) [Pires et al., (2009)]. 32
The control input can be obtained by applying a gain to these deformation rates, and using the resulting control signal in a positive feedback loop. The input signal to the system becomes an error signal that increases damping in each individual link. τ ref
τ
+
τvib
MANIPULATOR & ENVIRONMENT
.
ω
Gain
Modal functions
q˙ q q˙ e
Figure 3.4: Velocity feedback control scheme.
By affecting only link vibration, instead of attempting to eliminate tip-vibration, this methodology attempts to avoid some of the problems associated with non-minimum phase common in flexible manipulator control. Combining the system sketched in Figure 3.4 with an inverse dynamics control law leads to an inner velocity feedback control loop, shown in Figure 3.5.
ae
INVERSE DYNAMICS
τr
τ
+
MANIPULATOR & ENVIRONMENT
f ,μ q˙ e q
Vibration control
Figure 3.5: Block diagram of velocity feedback control with inverse dynamics control.
Note that as the only actuators present are the joint motors, the commands generated by the vibration controller conflict with the optimal torques calculated by the inverse dynamics block. However, due to the difference in time scales of the rigid body motion and the flexible vibration, the system asymptotically follows the desired trajectory while suppressing the undesired vibrations.
3.4.2
Tracking control for flexible manipulators
As described above, one of the main issues with position control of flexible manipulators is the non minimum phase properties of the system due to the presence of zeros of the open loop transfer function in the right half-plane. This means that Cartesian position error-based techniques are poised to fail, as these zeros become poles for the closed loop and lead to unstable system behavior. Recent studies have begun to countermand the non-minimum phase issue, but are currently being developed only for single link manipulators [Pires et al., (2010)]. 33
Closed Loop Inverse Kinematics Another approach, proposed in [Siciliano, (1998)], is based on a Closed Loop Inverse Kinematics (CLIK) scheme that determines the required joint coordinates that correspond to a desired tip position. The control is then made at joint level which does not suffer from the above mentioned problems. The basis for CLIK control is the adoption of a Jacobian matrix obtained by correcting the equivalent rigid arm Jacobian with an additional term for static deflections due to gravity and additional deflections caused by contact forces. Recalling once again equation 3.27 for the inverse dynamics of the system: M rr M er
τ JT g q r + r = r − r h ge 0 J Te qe ke
N 0r 0 q¨r + + 0 N 0e M ee q¨e
M re
0
(3.30)
where the nonlinear force vector N has been separated into N 0 , which contains the friction, centrifugal and Coriolis effects; the gravitational force vector g and the stiffness matrix k. Assuming second order flexural model displacements and first order torsional modal displacements, the stiffness matrix of link i can be determined using Pires et al. [(2009)]: Z
Li
kiy = 0
Z
Li
kiz = 0
Z kiτ = 0
Li
ϕ00i1 ϕ00i1 Ei Iiz ϕ00i2 ϕ00i1
ϕ00i1 ϕ00i2
ϕ00i1 ϕ00i1 Ei Iiy ϕ00i2 ϕ00i1
ϕ00i1 ϕ00i2
ϕ00i2 ϕ00i2
ϕ00i2 ϕ00i2
dx, i = 1, ...nl
dx, i = 1, ...nl
Ei Iix ϕ0iτ ϕ0iτ dx, i = 1, ...nl 2 k iy ki = 0 0
0 kiz 0
0
0 kiτ
(3.31)
where nl is the number of flexible links in the system and ϕ00ij denotes the second spatial derivative of the modal function ϕij . The global stiffness matrix for the robotic manipulator is then given by: k1 ke = 0
0 ..
. k nl
(3.32)
From equation 3.30, it is then possible to calculate the deflection variables from the static deflection variables and the deflection induced by the applied force using: T q e = −k−1 e (J e h + g e )
34
(3.33)
The basis for the CLIK control methodology is to determine the rigid joint coordinates that correspond to a given end effector position. This is accomplished by use of the transpose of the rigid arm Jacobian, according to: q˙ r = J Tr K p (P d − P )
(3.34)
where K p is a positive definite matrix and P d − P is the end effector tracking error. The closed loop denomination is due to the corrective feedback term K p (P d −P ) which makes it a recursive methodology where the tracking error is upper-bounded [Pires et al., (2009)]. The block diagram for the CLIK algorithm is shown in Figure 3.6. Pd
.
+
.
qr
qr = J Kp (pd - p) -
T r
∫
qr
P h Jr
qe calculator
Direct Kinematics
qe , qr
Figure 3.6: CLIK methodology diagram [Pires et al., (2009)].
To allow implementation with an inverse dynamics control law (equation 3.28), the joint positions and velocities are fed into a simple joint PD controller which determines the required joint acceleration vector according to: q¨r = K D (q˙ r − q) ˙ + K P (q r − q)
(3.35)
where K D and K P are gain matrices that represent respectively the proportional and derivative components. The application of the CLIK methodology assumes a number of considerations namely: • The system is considered to be in a static regime, thus inertial effects are considered to be null. The system must evolve at a low speed. • Only small deformations are considered. The gravitic term g r is considered to be only a function of q r . • There is a small position/orientation error between the real system and CLIK. Accordingly, the CLIK method has a number of limitations: This methodology is not compatible with the dynamic behavior associated with the impedance controller described in section 3.2.1, as only the desired end effector pose is used for control purposes. By only acting upon the end effector position, the control scheme becomes a compliance controller, as described in section 3.1. However, there are currently no second order inverse kinematics schemes developed for flexible manipulators. We will maintain the nomenclature of impedance control for consistency and comparison with the rigid body case. 35
The lack of a reference acceleration for the joint PD controller implies that perfect dynamic compensation can never be achieved. This technique is not compatible with the redundancy resolution scheme analyzed in section 3.3.2, since for a constant end effector position CLIK returns constant reference joint coordinates. Any change in joint velocities caused by the redundancy resolution would be considered as a disturbance and corrected by the joint PD controller. The following section presents a modification to the closed loop inverse kinematics scheme for flexible manipulators which enables the use of redundant degrees of freedom.
3.4.3
Closed Loop Inverse Kinematics with Redundancy Resolution
As was stated above, the CLIK method does not allow using the redundant degrees of freedom for any additional tasks. This happens because the resolution of redundancy occurs after the controller has defined the necessary joint coordinates for a given end effector position and orientation. In this work we propose a novel solution to this limitation, which implements redundancy resolution within the CLIK scheme. The basis for this Closed Loop Inverse Kinematics with Redundancy resolution (CLIK-R) is to project a joint velocity vector q˙ 0 into the null space of the rigid Jacobian, by use of the pseudoinverse solution: q˙ red = [I − J † (q)J (q)]q˙ 0
(3.36)
where once again, q˙ 0 is a joint velocity vector available for redundancy resolution. This contribution is then added to the output of the standard CLIK. The block diagram for this scheme is presented in Figure 3.7.
Pd
∫
.
+
qr
.
-
qr = J Tr Kp (pd - p)
P
+
qr
.
Redundancy Resolution
Jr
qred
h
qe calculator
Direct Kinematics
qe , qr
Figure 3.7: Block diagram for closed loop inverse kinematics scheme with redundancy resolution.
As the projection onto the null space is based on a rigid Jacobian matrix, there will be a slight deviation of the end effector position and orientation, due to the static deflection of the flexible links due to gravity. However, as the redundant joint velocity vector is determined within the feedback loop of CLIK, the error is treated as a disturbance and corrected. One of the advantages of this scheme is that redundancy is solved at the velocity level, which leads 36
to stabilization of internal velocities, and thus does not suffer from instabilities associated with null space dynamics. The joint velocity vector can be used to optimize a user defined task function, similarly to what was described in section 3.3.2. The objective is to generate internal motion of the structure based on measurements from an additional force sensor present at an intermediate point of the structure. The associated joint impedance control law is then given by: J T ∗ h∗ = K M q ∆¨ q cr + K Dq ∆q˙ cr
(3.37)
where h∗ is the auxiliary force sensor inputs and J T ∗ is the Jacobian matrix of the manipulator up to the element where the auxiliary force sensor is placed. The terms K M q , K Dq are gain matrices associated to the inertial and damping coefficients. Making the joint velocity vector q˙ 0 = q˙ c , which is obtained from successive integration of equation 3.37, leads to the desired internal dynamic behavior. In this application, the reference velocities and accelerations are zero when there no force is applied. Notice that unlike the joint impedance controller developed for the rigid case (equation 3.25), the proposed controller in this case does not include a stiffness component. This is to do with the fact that the redundancy control scheme described earlier requires a joint position vector, which is then optimized. As such, it is necessary to include a joint position loop in the impedance controller to ensure stability. In this application, however, the impedance controller directly outputs a joint velocity vector, and as such a stiffness component is not required. It is important to mention that the applied joint velocity vector for redundancy resolution must not violate the original CLIK assumptions, and as such the system should evolve at a low speed. The final block diagram for a flexible robotic system with impedance control, closed loop inverse kinematics with redundancy resolution, and vibration control is depicted in Figure 3.8.
.
pd , R d
SPATIAL IMPEDANCE
pc , R c
qr ,qr CLIK-R
..
PD Controller
qr
INVERSE DYNAMICS
τr
+
τ
MANIPULATOR & ENVIRONMENT
Vibration control
Figure 3.8: Block diagram for impedance control of a flexible redundant system.
37
q˙ e
q q˙ f ,μ
38
Chapter 4
Implementation In this chapter the main aspects regarding the implementation of the controllers presented in Chapter 3 are discussed. Initially a description of the robotic manipulator in study is given, as well as the kinematic and dynamic model used in this work. A suitable complacency environment for the impedance controller is presented. Two models are generated for a flexible and rigid system, and are implemented in Simulink. The final system configurations for both systems are shown.
4.1
System Description
The proposed solution for computer-assisted hip resurfacing arthroplasty consists of a flexible redundant manipulator, armed with a force sensor at it’s tip. The surgeon holds the manipulator’s end effector, which is controlled using the previously described spatial impedance controller under a constrained environment. This section describes the manipulator and its modeled kinematic and dynamic properties. A description of the compliant environment used for hip resurfacing arthroplasty is also given.
4.1.1
HipRob
HipRob is IST’s 7-DOF flexible manipulator (Figure 4.1a), developed for research in lightweight, spatially redundant manipulators. It consists of three lightweight carbon fiber links, 7 revolute joints actuated by Harmonic Drive servos with gear reduction ratios of 100. The structure was designed with dimensions and kinematics that resemble a human arm (Figure 4.1c). The circular cross section of the links enables high stiffness while reducing structural load. This reduces end-effector vibration caused by joint acceleration, and allows instrumentation of the links for full state measurement. The circular cross section also reduces deformation effect by shear strains. The upper extremity (Figure 4.1b) weighs less than 4kg, and has a maximum payload of around 1.5Kg in its entire working range. A six-DOF force sensor placed at its tip provides measurement of interaction of the end effector with the environment. The characteristics described above make HibRob an extremely useful tool for studies in humanrobot interaction at IST. 39
junta 4 inclusivé Figura 2.41 Extremidade do manipulador, apartir da
Joint 7 Link 3
Joint 6 Link 2
Joint 3
Joint 5
Joint 4
Joint 2
Link 1
Joint 1
(a)
(b)
(c)
Figure 4.1: HipRob - The IST 7-Dof Flexible Manipulator; a): Full view b): Detail of the upper extremity (joints 4 to 7); c): Representation of manipulator kinematics. [Rita, (2010)]
In the HRA surgical environment, a manipulator such as HipRob can be used to hold the surgical drill required for perforation of the femoral head, while the surgeon is still holding the instrument’s handle. This scenario is depicted in Figure 4.2. One can notice that unlike common anthropomorphic manipulators, HipRob is not equiped with a spherical wrist, lacking the final roll degree of freedom. This however is not a limitation in this scenario, as this final rotation is concentric with the drill direction. As such, this final degree of freedom can be left non-actuated, and the surgeon may choose the orientation that is most comfortable.
Figure 4.2: Hiprob interacting with a human operator.
40
4.1.2
Simulation and Control Model
The manipulator model has been generated recurring to the MECANISMO toolbox presented in section 2.3. The robot is modeled as a serial chain consisting of a base frame, seven rigid revolute joints, three flexible links, three rigid links and a rigid end effector. Figure 4.3 displays the joint reference frames and bodies used for describing the system. All reference frames are parallel and are located along the X-axis of the base frame. This allows a description of the flexural movements of the links according to the notation used in Chapter 2. X N M L K J
A
Joint 1
B
Joint 2
C
Rigid Body 1
D
Flexible Body 1
E
Joint 3
G
F
Joint 4
F
G
Rigid Body 2
E
H
Flexible Body 2
I
Joint 5
J
Joint 6
K
Rigid Body 3
L
Flexible Body 3
M
Joint 7
N
Robot End Effector
O
Base Frame
I
H
D C A, B
O Y
Z
Figure 4.3: Identification of bodies and joint reference frames used for describing the system. The manipulator is designed as a series of three flexible links, each preceded by a pair of joints and a rigid element. The final joint affects only the rigid end-effector.
41
The geometric and dynamic properties of the flexible elements can be seen in Table 4.1. The values for dynamic properties shown here are reference values for standard carbon-fiber tube. Length (m) Flex 1
0.06
Flex 2
0.27
Flex 3
0.11
Specific Mass
Young’s
Cross Section
Inertia Tensor of
Deformation modes
(Kg/m3 )
Modulus
Area (m2 )
Cross Section
(vy vz tx sy sz)
9e-4
1.7061e − 7 0 0 0 8.5307e − 8 0 0 0 8.5307e − 8
[2 2 1 0 0]
1520
150e9
Table 4.1: Properties of flexible elements used for modeling and simulation
The last column in Table 4.1 denotes the type and number of flexural deformation modes considered for modeling and simulation purposes. All links have been modeled with 2 flexural modes for each of the directions perpendicular to the neutral fiber and 1 torsional mode. Shear effects have not been considered. The modal functions used are cubic polynomial Hermite functions for the flexural modes and linear polynomial Hermite functions for the torsional mode, and are given by: 3 2 − 2 Lx ϕi1 (x) = 3 Lx i i 2 3 ϕi2 (x) = Lx − Lx i i ϕiτ (x) = Lx
(4.1)
i
where the subscripts 1 , 2 and
τ
represent whether the function is relative to the first flexural, second
flexural or torsional mode respectively. The term x is the distance along the neutral fiber from the base of the link, and L is the length of the link. The properties of the rigid components of the structure were R model (Rita [(2010)]), and are displayed in Table 4.2. extracted from a SolidWorks
Distance to next referential (m)
Mass (Kg)
First mass
Inertia Matrix
moment
Axis of rotation
J1
[0.8845;0;0]
-
-
-
x
J2
[0;0;0]
30
[0;0;0]
[0.6 0 0;0 0.6 0;0 0 0.3]
z
R1
[0.145;0;0]
5
[0;0;0]
[0.4 0 0;0 0.4 0;0 0 0.2]
-
J3
[0.157;0;0]
5
[0.165;0;0]
[0.2 0 0;0 0.2 0;0 0 0.1]
y x
J4
[0.1935;0;0]
3.02
[0.258;0;0.002]
[0.007802 0 3.3e-4;0 0.04805 0;3.3e-4 0 0.04559]
R2
[0.071;0;0]
0.237
[0.00612;0;0]
[1.982 0 0;0 3.3983 0;0 0 3.3983]
-
J5
[0.0755;0.0;0.0]
0.853
[0.0602;0.0;-0.00637]
[5.629e-4 0 4.862e-4;0 0.0049964 0;4.862e-4 0 0.004951]
z x
J6
[0.102;0.0;0.0]
0.579
[0.0405;0.0;-0.00546]
[5.519e-4 0 1.833e-4;0 0.00374 0;1.834e-4 0 0.0034585]
R3
[0.066;0.0;0.0]
0.138
[0.003828;0.0;0.0]
[7.485e-5 0 0;0 1.957e-4 0;0 0 1.957e-4]
-
J7
[0.0655;0.0;0.0]
0.524
[0.03369;0.0;-0.00327]
[ 2.422e-4 0 2.2488e-4;0 0.002541 0;2.249e-4 0 0.002513]
z
End
[0.065;0.0;0.0]
0.152
[0.00584;0.0;-0.00371]
[1.923e-4 0 9.22e-005;0 4.958e-4 0;9.22e-005 0 3.721e-4]
-
Table 4.2: Geometric and dynamic parameters used for modeling rigid elements
It is worth remarking that the dynamic properties displayed in Tables 4.1 and 4.2 are based on models of the system, and as such are only approximations. A thorough identification of the system would be necessary before application of control methodologies in physical experiments. To test the different properties and behaviors of the controllers described in the previous chapter, two models of the system were considered. One flexible model, with the properties described above, and 42
a rigid analogous, where the flexible links were replaced by equivalent rigid elements, with the same inertial and geometric properties.
Virtual Reality This work focuses on three-dimensional space controllers, where not only joint space variables but also Cartesian coordinates are involved in the simulation process. It is natural to assume that when complex trajectories are planned, visualization of the simulation results can become troublesome by simply analyzing end-point and joint trajectories. Examining the behavior of the robot in a three-dimensional world can be extremely helpful in the interpretation of simulation results. For this reason, a virtual reality model has also been generated for visualization of simulation results by use of the SimulinkTM 3D Animation Toolbox. The model is constructed using virtual reality modeling language (VRML), and is shown in Figure 4.4. The model includes not only rigid degrees of freedom resulting from joint rotation, but also allows visualization of deformation of the links.
Figure 4.4: VRML model used for simulation visualization.
4.1.3
Environment
As the manipulator is meant to be controlled within a constrained environment, the nature of the procedure will determine the restrictions on the end-effector’s pose. Considering the hip resurfacing arthroplasty procedure described in section 1.1.2, a natural selection is to consider an increase in the stiffness of the manipulator as the drill bit approaches the previously determined drill point. Motions that cause the end-effector’s position to move in a direction that is normal to the drill direction will also lead to an increase in manipulator stiffness. 43
R In some applications of virtual fixtures such as the Acrobot system, the area where the robot is
actuating is throughly defined and classified into zones [Jakopec et al., (2003)]. The applied impedance on the end effector will depend on which zone it is currently being used in. While this is essential for knee procedures for which the system was developed, in other applications such complex descriptions are not required. In this work, the goal is to make the HRA procedure as simple as possible, and an accurate description of the affected zone is not essential. In [Feio, (2008)], a complacency environment has been proposed based solely on the predetermined position and orientation of the desired orifice. The working principle, depicted in Figure 4.5, is based on a mass-springer-damper system that acts upon the position and orientation of the end effector when it is driven away from the correct drill direction. In order to ensure smoothness in the robot’s movement and to provide a more comfortable interface for the surgeon, the robot should become stiffer as it approaches the point of perforation. The natural frequency of the system, defined as: r ωn =
KP KM
(4.2)
will increase as the end effector is driven towards the drill point. However, there is no increase in impedance if the end effector is aligned with the direction of the drill point and is moved along the drill direction. It is also noticeable that a rotation around the axis that defines the drill direction will not provoke any increase in manipulator stiffness.
Figure 4.5: Complacency environment associated with the manipulator end-effector [Feio, (2008)].
The environment can thus be described as a virtual cone that guides the surgeon’s hand and consequently the drillbit to the correct perforation point, with the best orientation, in an intuitive and userfriendly way.
44
4.2
Implementation of Controllers
After obtaining the model of the system, it is necessary to implement the various controllers described in Chapter 3. The objective is to make the flexible redundant system respond to an applied force at its tip with a compliant behavior that varies according to the distance to a pre-determined drill point. As this is an operational space scheme, inverse kinematics are required to translate control signal to joint-space. If a force is applied at an intermediate point, a redundancy resolution controller will re-configure the manipulator to provide a more comfortable position for the user, without affecting the end-effector position. In the flexible model, damping of the vibrating links is artificially increased by flexural velocity feedback. Each controller has been implemented as independent Simulink block that is analyzed individually. The blocks are then combined to give shape to the final system configuration.
Robot Model The robot model has been implemented using the MECANISMO toolbox, with parameters defined in section 4.1.1 and in depicted in Figure 4.6. Joint accelerations are calculated by applying an LDL solver on the mass and non-linear acceleration matrices, followed by double numerical integration. The initial values for the integration blocks are the initial robot joint velocities and joint positions, respectively. The resulting joint positions and orientations are then fed back into the model at each simulation step.
M qdq
Q Rtip Flexivel M*ddq+N=0
1 In Torques
S
SX=B (LDL') X
N Q Rtip
Q2J
xtip Vtip
J
uT
Matrix Multiply
-1 Gain
B
[m x m] LDL Solver
1 s
1 s
Integrator
Integrator1
1 q 2 dq
2 TipForces
Nacctip
InForces
3 ddq
Rk xk
Terminator
S-Function
Figure 4.6: Implementaiton of robot model.
Although the MECANISMO block provides information regarding the end effector position, orientation, speed, and acceleration, these properties are not used at this point. This is due to the fact that this block simulates the actual mechanical system, from which only the joint coordinates are available from motor-based sensors. For implementation in an experimental setup, it is only necessary to replace the robot model block by the actual inputs and outputs of the real system. This implementation is the same for both the rigid and the flexible model of the system. The following sections focus on each of the two models separately, as the associated controllers are also different.
45
4.2.1
Rigid Model
The system of controllers for a redundant rigid robotic manipulator consists of an impedance controller with inner motion control and an inverse dynamics controller with redundancy resolution. Impedance Controller The nature of an impedance controller, by only determining end-effector behavior, makes it applicable to any robotic manipulator with the necessary DOF. In [Feio, (2008)] an impedance controller has been designed for an anthropomorphic six DOF manipulator for application in hip resurfacing arthroplasty. This controller has been adapted to comply with the kinematics of HipRob. The Simulink block diagram for the impedance controller with inner motion control is depicted in Figure 4.7. The controller receives from a force sensor information regarding the forces applied at the robot’s end effector. From the robot model it is possible to obtain the joint coordinates and velocities and from direct kinematics the end effector position, orientation and velocity is extracted. Information regarding the drill location and desired orientation is introduced in the block drill parameters, which feeds references into both the translational and rotational impedance controller. Controller gains are adjusted using the orientation gains block or the position gains block contained within the translational impedance subsystem. 3 Xtip 4 Rtip
xe
[Rtip] [Vtip]
2 Vtip drill point drill direction
xtip
Vtip_Normal
Vtip_Normal_e
Rtip
Vtip_tangent
Vtip_Tangent_e
vtip
xc
a_translational
xc
Tip Forces
xc'_Normal
x _normal_c'
drill point
xc'_Tangent
x_tangent_c'
drill direction
drill matrix Rd
xc''
translational impedance
xc ''
position control
drill parameters [Rtip]
1 a
Rtip
1 TipForces
[Vtip] orientation gains
R_tip
KMor
Mo
KDor
Do
KPor
Ko drill matrix Rd Tip Forces
vtip
Rc
Rc
Wc
Wc
Wc'
rotational impedance
a_rotational
Wc'
orientation control
Figure 4.7: Implementation of impedance controller and inner motion controller [Feio, (2008)].
As was described in the previous section, the impedance associated with the end effector increases as the manipulator approaches the drill point. This variation is determined according to equation 4.2 as: 25 ω = , MDPD > 0.1 n 100MDPD−0.1 (4.3) ω = 25 , MDPD ≤ 0.1 n
where the parameter MDPD stands for Minimum Drill Point Distance and is a measure of the distance between the end effector and the drill location, taken along the drill direction. The values chosen here for the parameters should be modified according to the user’s preference. 46
Redundancy Resolution As was defined in section 3.3.2, the redundancy resolution scheme consists of modifying the inverse dynamics control law by inclusion of an additional acceleration term. Figure 4.8 shows the Simulink block diagram corresponding to the implementation of an inverse dynamics control scheme for rigid manipulators, complemented with a redundancy resolution block. The HipRob block (grey) has been generated with the MECANISMO toolbox, and provides the dynamic information of the system based on current joint position and velocity. f
4 aux sensor
ddq
3 ddq
dq phi
q J M^-1
[J]
J#
M^-1
J
phi M
J#
Matrix Multiply
[accel]
PseudoInverse M
Matrix Multiply
N
1 q
qdq
Q
2 dq
1 Tau
[J]
Q
uT
J
Rtip
Rtip
Flexivel xtip M*ddq+N=0
6
Rtip
[accel]
dJdq
InForces
Nacctip
Matrix Multiply
2 TipForce
a
Vtip
Ground
5 TipForces
Nacctip
Rk xk S-Function
Figure 4.8: Implementation of inverse dynamics with redundancy resolution for the rigid model of the robot.
The dynamically consistent pseudoinverse (equation 3.21) has been implemented in a separate pseudoinverse subsystem, whose inputs are the mass and Jacobian matrices. Figure 4.9 shows details of the subsystem. Due to the bad conditioning of the Jacobian matrix, it was necessary to include an additional pseudoinverse (Simulink function) block to eliminate instabilities when calculating the term [J (q)M −1 (q)J T (q)]−1 . The block also exports the inverse of the mass matrix, which will be used for redundancy resolution.
2 M
1 M^-1
General Inverse (LU) LU Inverse
1 J
u
Matrix Multiply
T
Matrix Multiply
Matrix Multiply Pseudoinverse X A (SVD) Pseudoinverse
Figure 4.9: Block diagram of pseudoinverse subsystem.
47
2 J#
The redundancy resolution scheme from section 3.3.2 has been developed for rigid manipulators, and thus receives as input the inverse of the mass matrix, the Jacobian and pseudoinverse of the Jacobian. The inputs for the joint impedance controller are the joint positions, velocities and accelerations, as well as the auxiliary sensor measurements. Figure 4.10 shows the Simulink block diagram for redundancy resolution. The top part of Figure 4.10 shows the joint impedance controller used to calculate the joint velocity vector γ, while the bottom part is the determination of the joint space acceleration vector φn . Notice that even in the absence a an applied force at the intermediate point, the scheme still uses information regarding current joint velocities to stabilize internal motion. [mass_inv]
6 M^-1 q Jrw4
u
T
Matrix Multiply
dq
4 q
J4
2 ddq
1 f 3 dq
U
U
Y ddqI
Selector4
Y
KMI_Inv
KDI
Y
U
25
Selector3
1 s
1 s
Integrator
Selector1
Kgama Matrix Multiply
qI
Integrator1
dqI
u
(0 0 0) qI
KPI
[gamma]
du
Matrix Multiply
Derivate
u
[mass_inv] Matrix Multiply
Matrix Multiply
eye( )
5 J
1 phi
du
Derivate1
7 J#
[gamma]
Identity Matrix1 Matrix Multiply
Matrix Multiply
20 Kn
en
Figure 4.10: Block diagram of the redundancy subsystem.
In this implementation, it has been assumed that the auxiliary force sensor is placed immediately before the second flexible link (see Figure 4.3). For this reason, the Jacobian matrix is calculated based on a reduced model of the system consisting of only the first 4 joints (yellow). Similarly, the inputs are filtered so only the first four joint positions, velocities and accelerations are selected for the joint impedance controller.
48
4.2.2
Flexible Model
As expected, the controller setup for the flexible model is more complex than is the case for the rigid manipulator. Accordingly, the system is composed of an impedance controller, a CLIK-R scheme with joint PD control, a flexible inverse dynamics controller and a vibration suppression controller. For comparison purposes, a redundancy resolution similar to that developed for the rigid model is also applied.
Impedance Controller As was described in section 3.4.2, the control methodology for flexible manipulators only considers as input a reference end effector pose. As such, the impedance controller that is used for the rigid body case is modified to comply with these requirements. The block diagram for the impedance controller used for modifying the reference for the flexible model is depicted in Figure 4.11. 2 Xtip
xtip
Tip Forces xc drill point
2 Xc
drill point drill direction
drill direction
drill matrix Rd
3 Rtip
drill parameters orientation gains 1 TipForces
1 TipForce
translational impedance
R_tip
KMor
Mo
KDor
Do
KPor
Ko
Rc
3 Rc
drill matrix Rd Tip Forces
rotational impedance
Figure 4.11: Implementation of impedance controller considering only end effector position/orientation.
As explained previously, this scheme also differs from what was implemented for the rigid model in terms of position control. The inner motion control loop has been removed, and is replaced by a joint based controller which will be explained below. It is noticeable, however, that the determination of the compliant frame is still determined by equations 3.5 and 3.7, with the exception that the only terms that are extracted from this are the ones referring to position and orientation.
Closed Loop Inverse Kinematics with Redundancy resolution The CLIK-R scheme has been implemented in Simulink in a manner similar to what was described in Figure 3.7, and the corresponding Simulink block diagram is depicted in Figure 4.12. The inputs to the system are the desired position and orientation, which are compared to the actual end effector position in the error calculation subsystem (Figure 4.13) and the resulting error is used to generate reference joint velocities. Similarly to what was defined in section 3.2.1, a quaternion representation of the orientation error has been used. 49
3
Pd
Pd
2
4
dq
Rd
Rd
Pd-P
Pd-P
P
1 s
dv
J
dq=Jt.Kp.(Pd-P)
Rtip
1 q
Integrator 2
Error Calculation 1
TipForces
f
aux sensor
3 TipForce J
qrd
dq0
5
q
delta
q
Rtip
Redundancy
Jr
TipForces
4 Xtip
q
Rtip Ptip
Direct Kinematics
Figure 4.12: Implementation of the CLIK-R scheme.
By integration, the joint coordinates are obtained which are used in conjunction with the forces that are applied at the end effector to determine the static deflection variables. The full state vector is then used for direct kinematics, and to determine the rigid arm Jacobian. The outputs of the system are the current end effector position and orientation, which will be used for feedback to the impedance controller. The joint velocities and coordinates are also exported to the be used in a joint PD controller.
3 P 1 Pd
1 Pd-P
4 Rtip
u
T
Transpose
Matrix Multiply
2 Rd
Matrix Multiply
DCMbe
q
Matrix Multiply
-1 Gain
Direction Cosine Matrix to Quaternions
EpslonReal
Matrix Multiply1
Figure 4.13: Error Calculation subsystem block diagram.
The Redundancy subsystem (Figure 4.14) receives as input the rigid Jacobian of the robot manipulator, as well as the measurements of the intermediate force sensor. Similarly to what was implemented for the rigid body case, it is assumed that the auxiliary force sensor is placed immediately before the second flexible link and thus the current joint coordinates are used to calculate the Jacobian matrix of a reduced model of the system consisting of only the first four joints.
2
J
J
PseudoInverse
3 q
Jrw4
q
u
Identity Matrix
Matrix Multiply
T
Matrix Multiply
J4
Matrix Multiply
1 f
eye( )
J#
KDI
KMI^-1
ddqI
1 s Integrator1
dqI [0;0;0]
Figure 4.14: Redundancy subsystem block diagram.
50
1 dq0
PD controller The outputs of the CLIK-R scheme are reference joint velocities and coordinates, q r and q˙ r , which are compared with the real values, q r and q˙ r (Figure 4.15). The PD controller calculates the necessary joint acceleration vector that will serve as reference to the inverse dynamics controller. The purpose of the PD controller is to enable effective disturbance rejection, and provide robustness to unmodeled system dynamics. 1 qr KP 3 q
1 ddq
2 dqr KD 4 dq
Figure 4.15: Joint based PD controller.
Inverse Dynamics The implementation of the inverse dynamics controller for a flexible manipulator is shown in Figure 4.16. The HipRob block (grey) has been generated with the MECANISMO toolbox, and provides the dynamic information of the system based on current joint position and velocity. However, the block exports data in the order described in Figure 4.3. To implement the block matrices described in equation 3.27 for the inverse dynamics of flexible manipulators, it is necessary to rearrange the terms of the mass, non-linear and Jacobian matrices. 3 ddq Mrr M
General Inverse
Mee
Mee-1
(LU)
[Mee_inv]
Mre
Matrix Multiply
M Mer N
1
N
q
qdq
Q
2 dq
Matrix Multiply
Nr Ne
Matrix Multiply
[Mee_inv]
[Mre]
1
Rtip Flexivel M*ddq+N=0
xtip Q
Jr
Vtip
InForces
Tau
[Jr] Matrix Multiply
4 Rtip
Rk
T
TipForces
Nacctip
Ground
u
[Mre]
Je
[Mee_inv] u
T
Matrix Multiply
xk HipRob 2 Tip Forces
Figure 4.16: Implementation of inverse dynamics controller for flexible robotic systems.
51
For comparison purposes, an inverse dynamics scheme with redundancy resolution similar to what was implemented above has also been developed for the flexible model case. Figure 4.17 shows the Simulink block diagram corresponding to the implementation of an inverse dynamics control scheme for rigid manipulators, complemented with a redundancy resolution block. 6 faux
f
3 ddq
ddq dq q
phi
J M^-1
[Jr]
J#
M^-1
J
Redundancy M
J#
Matrix Multiply
PseudoInverse [accel] Mrr
General Inverse
Mee
M
Mee-1
(LU)
[Mee_inv]
Mre
Matrix Multiply
M Mer
N
1 q
Nr
N
qdq
Q
2 dq
Ne
Rtip Flexivel xtip M*ddq+N=0
[Jr] uT
Jr
Q
1 Tau
Matrix Multiply
TipForces 5
Nacctip
Ground
Matrix Multiply
[Mee_inv]
[Mre]
Q2J2JrJe1
Vtip
InForces
Matrix Multiply
[Mre]
Je
Rtip
[Mee_inv]
Rk
u
xk
Matrix Multiply
T
ae
HipRob Rtip
4 [accel]
dJdq Nacctip
Figure 4.17: Implementation of inverse dynamics with redundancy resolution block for the flexible model.
The redundancy resolution scheme from section 3.3.2 has only been developed for rigid manipulators, and thus receives as input the inverse of the rigid mass matrix, the rigid Jacobian and rigid pseudoinverse of the Jacobian. The inputs for the joint impedance controller are the rigid joint positions, velocities and accelerations, as well as the auxiliary sensor measurements. The redundancy subsystem is the same as the one shown in figure 4.10.
52
Vibration Controller The vibration controller has been implemented to resemble the diagram in Figure 3.4. At the top of the figure, joint coordinates and velocities are used to determine the Jacobian matrix of the system at the base of each of the three flexible links (yellow). For each link, the elastic velocity coordinates are extracted and the rate of deformation is calculated based on modal functions (red), which were determined at a point xi = 0.1 × Li to ensure they are located before the inflection point of the second vibration mode. The three components of deformation are calculated based on the reference frame of each link, multiplied by the corresponding Jacobian transpose and by a gain factor. 2
q
0.8
100 1000
Matrix Multiply
u
T
Jrw2
q dq
Transpose J2 q
u
T
Jrw4
1
dq
0.1
1 tau 0
30 10
0.8
Transpose2
30 Matrix Multiply
q
u
10
1
dq
J4
T
Jrw6 dq
Transpose3 J6
10 0.1
10 0.2
-1 gain
10
10
-KPhiVectorlink1
Matrix Multiply -1
10
gain1
100
-1 gain2
-K-
Select Rows
PhiVectorlink2
-KPhiVectorlink3
Figure 4.18: Implementation of velocity feedback vibration controller.
The controller setup has been designed assuming that the deformation of each link can be controlled by actuating on all the joints that precede them. This method is an approximation since, for control purposes, it does not consider dynamic coupling between the vibration of the link and the motion of the bodies that follow it. Looking at the kinematics of HipRob, there are two actuated joints between each flexible link, and as such the first link is controlled by the first two joints, the second link is controlled by the first four joints and so on. As there is no flexible link that is located after the last joint no control input is generated. It can then be seen that each pair of joints affects different links. Subsequently, a weighting factor associated to each link has been implemented to decouple their effects on the joint control signals.
53
4.3
Final System Configuration
All of the blocks described above have been connected forming two final system layouts. These systems naturally differ in the nature of the controllers they employ.
Rigid Model The final system configuration for the rigid model is visible in Figure 4.19. The Main Sensor and Auxiliary Sensor blocks are responsible for generating reference signals for the forces actuating respectively on the main and auxiliary sensors and are constructed based on third order polynomial functions. The Direct Kinematics block is based on a model of the system like the one used in Figure 4.6.
Aux Sensor Forces q
Force Generator1
q Tau
dq
Torque
ddq
Tip Forces
dq
aux sensor
Force Generator
TipForces Vtip
TipForces TipForce
Xtip Rtip
Impedance control with inner motion control
a
TipForce
TipForces ddq
a
HipRob
Inverse Dynamics with Redundancy Resolution
Rtip
q
Xtip Vtip
dq
Direct Kinematics
Figure 4.19: Block diagram with the implementation of the complete system for a rigid model.
Following the path of the reference signal through the system it can be seen that, starting from a static position, a force applied to the end effector enters the Impedance control with inner motion control block, which generates end-effector accelerations. These accelerations are fed into the Inverse dynamics with Redundancy Resolution block, along with information from the auxiliary sensor, and reference motor torques are determined and fed into HipRob. The resulting joint and modal variables are also used in a feedback loop back into the inverse dynamic block. The final feedback loop is closed after direct kinematics are applied and the resulting end-effector Cartesian position, orientation and velocity are inserted into the position controller.
54
Flexible Model The final system configuration for the flexible model is visible in Figure 4.20. The functions used to generate force signals are similar to the ones used in the rigid model case.
Aux Sensor Forces q
Force Generator1
Tau
dqr
Tip Forces aux sensor
Force Generator
TipForces
TipForce
Rc
Rtip
Impedance Control
dq
TipForces
TipForce Xc
Xtip
q
Pd
Xtip
ddq ddq
q dq
q
In Forces
dq
qr
Tip Forces TipForces
TipForce
Joint PD Controller
dq
TipForces
TipForces
Inverse Dynamics
HipRob
Rtip
Rd
dq
CLIK Redundancy
tau
q
VibControl
Figure 4.20: Block diagram with the implementation of the complete system for a flexible model.
Analyzing the controller disposition it is clear that there are two distinct sets of controllers. The Impedance controller, together with the closed loop inverse kinematics with redundancy resolution scheme, are responsible for generating the reference inputs to another system composed by the joint PD controller, the inverse dynamics and vibration controller. Their function is to ensure that the robot model follows the reference inputs as closely as possible, while still increasing damping of flexible vibration of the links. Once again we follow the path of the reference signal, starting from a static position. If a force is applied to the robots end effector, the impedance controller will change the reference pose for the CLIK, which calculates the corresponding joint coordinates. If a force is applied at an intermediate point of the structure, the CLIK-R scheme generates a velocity vector belonging to the null space of the Jacobian matrix that reconfigures the manipulator without modifying the end effector pose. The joint coordinates and velocities are exported into the joint PD controller, which generates a joint acceleration vector based on the error between the reference and current joint coordinates/velocities. The inverse dynamics block then uses this signal to generate the required torques that are fed into the flexible model. The resulting joint coordinates and velocities are fed back into both the PD and inverse dynamics controller. The flexible modal deformations serve as input to the vibration controller which generates an additional torque vector that is added to the contribution from the inverse dynamics block.
55
56
Chapter 5
Simulation Results To validate both implemented systems, a series of simulations were performed to test each controller independently as well the overall rigid and flexible system. Both systems have been constructed starting with the robot model and successively closing feedback loops as each new controller is introduced. This chapter presents the most relevant results for each of the implemented systems. All simulations were performed in the Simulink environment with variable step ode23s (stiff/mod. Rosenbrock) solver. The initial conditions for simulation were chosen as ones that would be similar to a possible working position in a real environment. The initial configuration chosen is depicted in Figure 5.1.
Figure 5.1: Initial configuration for simulation.
As has been done throughout this work, the following sections analyze each of the implemented systems separately. The different nature of each controller makes direct comparison of the results difficult, however the function of both systems is identical. 57
5.1
Rigid Model
The system configuration for the rigid model is the one shown in Figure 4.19, consisting of an inverse dynamics with redundancy resolution controller and an impedance controller with inner motion control.
5.1.1
Redundancy resolution
The aim of the redundancy resolution controller is to enable the projection of a user defined velocity vector into the null space of the Jacobian matrix, generating internal motion without affecting the endeffector position. In order to test the functionalities of the proposed controller, a simple test was made. The robotic manipulator starts from an static position and receives a reference end effector acceleration (Figure 5.2a), describing a Cartesian trajectory. Simultaneously, a varying force is applied at an intermediate point, which persists after the desired end-effector trajectory is complete (Figure 5.2b). The controller parameters chosen are displayed in table 5.1. KMq
1/300 × I 3×3
K Dq
300 × I 3×3
KP q
100 × I 3×3
Kγ
25
Kn
20 × I 7×7
Table 5.1: Controller parameters used for redundancy resolution
The choice of parameters has been made to ensure that the behavior of the system is critically damped, to avoid oscillations in the internal joint velocities. The parameter Kγ acts as a scaling factor that affects the magnitude of the control action leading to internal motion. This term has been chosen in a manner that allows a moderate displacement of the joints when the applied force nears the 10N range. This force was assumed to be approximate to the amount of force sensed when a hand pushes at the robot. The parameter Kn is responsible for stabilization of internal velocities. Increasing this parameter leads to faster stabilization but an increased control signal, which may lead to instabilities. The choice of Kn has been made by attempting to minimize both the control signal and the stabilization time. Notice that although the desired end-effector trajectory is complete 3 seconds after the start of the simulation, the applied force is active for 5 seconds. Figure 5.3 shows a comparison of test results for end effector position joint coordinates, for one system where the reference acceleration has been given and another where both the reference acceleration and the applied force were given. Due to similarity in results, the information regarding joint trajectories has been limited to the two most representative joints, respectively joint 1 and joint 4 of the robot manipulator. The remaining joint trajectories are found in Appendix A.1. 58
Reference end effector acceleration (x direction)
0.2
4
0.15
3.5 3
0.05 Force (N)
Acceleration (rad/s2)
0.1
0 −0.05
2.5 2 1.5
−0.1
1
−0.15 −0.2
Force applied to the system (z direction)
4.5
0.5
0
1
2
3
Time (s)
4
5
6
0
7
0
1
2
3
(a)
Time (s)
4
5
6
7
(b)
Figure 5.2: Reference acceleration (a) and applied force (b) used for validation of redundancy resolution.
End effector position
0.2
Force + Accel Accel Reference
1.2 1
0
1
2
3
4
5
6
7
y (m)
x 10
−0.1
4.9
Force + Accel Accel
0
1
2
3
0
1
2
3
4
5
6
7
4
5
6
7
4.85 4.8
−1.35
0
1
2
3
4
5
6
7
−1.4 J4 (rad)
−0.5109 z (m)
0.1 0
−3
4.95
End effector position
0.3
J1 (rad)
x (m)
1.4
−0.511
−1.5 −1.55
−0.511 −0.5111
−1.45
0
1
2
3
Time (s)
4
5
6
7
(a)
−1.6
Time (s)
(b)
Figure 5.3: Comparison of the end effector position (a) and joint trajectories (b) using rigid redundancy resolution with reference acceleration and applied force.
Looking at Figure 5.3a it is noticeable that, for both systems, reference tracking is perfectly achieved. The minor errors that occur, and that are more visible in the bottom two plots are due to accumulation of numerical error. An important factor to notice is that the end-effector x position varies only until second 3 of the simulation, which coincides with the end time of the acceleration trajectory. This effect is also seen in Figure 5.3b: when observing the joint trajectories for the system without force applied, as expected, the variation in configuration ends when the reference in acceleration also becomes zero. If, on the other hand, the system with an applied force is analyzed, the end a reference in acceleration does not lead to stagnation of joint coordinates. The presence of the applied force until second 5 in the simulation changes the configuration. However, this change during seconds 3-5 does not reflect into a change in the end effector position. 59
5.1.2
Impedance Controller
The final step in implementing the impedance controller with redundancy resolution in the rigid system is to close the position feedback loop. This will not only make the system more robust to disturbances, but also enables modifying the reference position of the end effector by applying a force to the robot’s tip. The first step is to determine the controller gains that produce an acceptable response to perturbations. To accomplish this, a test is made where the end effector’s initial position does not coincide with the predetermined drill direction. The controller will then correct the end effector position according to the controller gains specified. The gains are adjusted until a critically damped response is obtained. The drill parameters and controller gains used for position and orientation control are shown in table 5.2. K MP I
I 3×3
K DtangentI
25 × I 3×3
K DpI
50 × I 3×3
K P pI
625 × I 3×3
K Dp
50 × I 3×3
KP p
100 × I 3×3
K MOI
2 × I 3×3
K DOI
I 3×3
K POI
I 3×3
K Do
15 × I 3×3
KP o
50 × I 3×3
Table 5.2: Controller parameters for impedance control with inner motion control.
The following test serves to verify that the impedance controller changes input references to the position controller when a force is applied to the end effector. The system starts from a static initial position and a step force perpendicular to the drill direction is applied. Figure 5.4 shows the change in controller reference and subsequent end effector position over time. Variation of end effector position
x (m)
0.82 0.8 0.78
End effector position 0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
y (m)
0.23 0.22 0.21
z (m)
−0.51 −0.52 −0.53
Time (s)
Figure 5.4: Variation of end effector position when a step force of 10N is applied to the end effector perpendicular to the drill direction.
60
It is clearly seen that the presence of an applied force at the end effector changes the compliant frame associated with the impedance controller. As this system is currently implemented in continuous time, there is perfect dynamic compensation and the system follows the reference trajectory perfectly. Similar results have been achieved for tests performed on the system’s response to a variation in orientation. The final step for validation of the implementation of the rigid impedance controller with inner motion control and redundancy resolution is to apply a force at the auxiliary sensor. It has been previously demonstrated that the implemented redundancy resolution controller is able to reconfigure the manipulator without changing the end-effector position. The following test is meant to validate the controller when a position and orientation feedback control loop is implemented. The system begins at an initial position with a slight offset in relation to the drill direction. After 1 second, a force is applied to the intermediate force sensor with the profile similar to Figure 5.2b. Variation of end effector position Joint coordinates −1.3
0.792 −1.4
0.79
0
1
2
3
4
5
6
J4 (rad)
x (m)
0.794
7
−1.6
0.218 y (m)
−1.5
−1.7
0.216 0.214
0
1
2
3
4
5
6
0
1
2
3
0
1
2
3
4
5
6
7
4
5
6
7
0.65
7
0.6 J6 (rad)
z (m)
−0.5186 −0.5188 End effector position −0.519
0
1
2
3
4
5
6
0.55 0.5 0.45
7
0.4
Time (s)
Time (s)
(a)
(b)
Figure 5.5: Comparison of the end effector position (a) and joint trajectories (b) using impedance control with rigid redundancy resolution with null end effector force and applied force at the intermediate sensor.
As can be seen from Figure 5.5, the system corrects the initial displacement, placing the end effector on the drill direction. After one second of simulation time, the robot changes its configuration by effect of the force applied at an intermediate point of the structure, but does the position of the end effector remains constant.
61
5.2
Flexible Model
As was stated in the previous chapters, control of flexible manipulators is a much more complex subject than the rigid counterpart. Firstly, active vibration control is required to dampen the effects of the elastic links. The non-minimum phase characteristics make tip position feedback control unfeasible, and a closed loop inverse kinematics scheme and a PD controller is added. A consequence of this implementation is that the impedance controller acts only upon the end effector position and orientation.
5.2.1
Vibration Controller
As was mentioned in section 3.4.1, the need for implementing the vibration controller comes from the elastic motion of the links that the inverse dynamics controller is unable to dampen. A key factor in this implementation is the controller gains used in the controller. The highly coupled nature of the dynamics of the 3D multi link system makes tuning these parameters a sensitive procedure. The controller gains for the vibration controller were determined manually to maximize flexural damping while maintaining system stability. the methodology for tuning the proportional gains was to start from the bottom joints, which affect the entire system and the first link directly. Once acceptable vibration was obtained, the gains relating to the following links were tuned independently. The final parameters chosen are shown in Table 5.3. Joint 1 Link 1
Link 2
Link 3
Gain
100
Weight Gain
Weight
Joint 3
1000
30
30
10
0.1 10
Joint 4
Joint 5
-
0.8
Weight Gain
Joint 2
10
-
0.8 10
10
0.1
Joint 6
10
10
0.2
100 1
Table 5.3: Gain parameters used in vibration control.
To test the performance of the vibration controller, a reduced system consisting only of an inverse dynamics block, which receives reference joint accelerations, and a vibration controller are used. The system does not include PD control or inverse kinematics at this point, to validate the joint-acceleration tracking of the inverse dynamics controller. In this test a simple linear joint acceleration trajectory is given as reference to two systems, one with vibration control and one without. Figure 5.6 shows the resulting robot joint accelerations and the effect on the elastic vibrations of link 1 is shown in Figure 5.7. The results shown here for the first link are similar to those obtained for the other links, which are displayed in Appendix A.2. 62
Reference Signals
4
Joint acceleration ( rad/s 2 )
3
2
1
0
-1
-2
-3
-4 0
1
2
3
Time (s)
4
5
6
7
(a) Joint accelerations without vibration controller
4
4
3
3
2 1 0
2 1 0
-1
-1
-2
-2
-3
-3
-4
-4 -5
Joint accelerations with vibration controller
5
Joint acceleration ( rad/s2 )
Joint acceleration ( rad/s2 )
5
0
1
2
3
Time (s)
4
5
6
-5
7
0
1
2
3
(b)
Time (s)
4
5
6
7
(c)
Figure 5.6: Comparison of the tracking abilities of the inverse dynamics with vibration controller and without. a) reference joint acceleration; b) robot joint accelerations with inverse dynamics controller; c) joint accelerations with inverse dynamics and vibration controllers.
Torsion (x axis) (rad/s)
−3
5
Link 1 deformation rates
x 10
0 −5
0
1
2
3
4
5
6
7
1
2
3
4
5
6
7
1
2
3
4
5 6 7 Inv. Dyn. Inv. Dyn. & Vib. Cont.
Flexion (x−y plane) (m/s)
−5
2
x 10
0 −2
0
Flexion (x−z plane) (m/s)
−5
5
x 10
0 −5
0
Time (s)
Figure 5.7: Comparison of controlled (using vibration controller) and uncontrolled vibrations for link 1.
63
Analyzing the previous figures, it can be seen that the inverse dynamics controller for flexible links can accurately track a given trajectory in joint space. Any deviations from the above can then be attributed to the vibration control signal. Figure 5.6c clearly demonstrates the effect of vibration control on the joint accelerations. This result was expected since both controllers share the same actuators, as described in section 3.4.1. Albeit having fluctuations, the joint accelerations track the desired accelerations asymptotically. In terms of vibration suppression efficacy, it is evident that the application of the vibration controller greatly increases the damping of the system, when compared to a simple inverse dynamics control law. The peaks in Figure 5.7 occur at discontinuities of the reference acceleration, which excite the natural frequencies of the system. It is worth noticing that the model of the system does not include structural damping due to friction, and as such the uncontrolled vibrations are undamped. This however does not invalidate the effectiveness of the implemented vibration suppression control. Having validated the effect of vibration control, the following tests omit displaying information regarding the deformation rate of the flexible links.
5.2.2
Inverse Dynamics with Redundancy Resolution
As a manner of comparison with the rigid model case, the system shown in Figure 4.17 was tested. Initially, the dynamically consistent pseudoinverse is introduced to examine the response of the system to a reference end-effector acceleration. End effector position 1.3
Reference Inverse Dynamics Inverse Dynamic & Vibration control
x (m)
1.25
1.2
1.15
1.1
1.05 0
-0.02
y (m)
-0.04 -0.06 -0.08
Reference Inverse Dynamics Inverse Dynamic & Vibration control
-0.1 -0.12 -0.14 -0.705 -0.71
z (m)
-0.715 -0.72 -0.725 -0.73
Reference Inverse Dynamics Inverse Dynamic & Vibration control
-0.735 -0.74
0
1
2
3
Time (s)
4
5
6
7
Figure 5.8: Evolution of end effector Cartesian position under pseudoinverse with inverse dynamics control.
64
A test was performed with a reference signal similar to the one shown in Figure 5.6a, given in the x component of Cartesian acceleration. The results for end effector position are shown in Figure 5.8. In terms of position, the reference is a polynomial trajectory that leads to an increase of 0.3m in the x coordinate, while maintaining the initial y and z coordinates. Results show that using a regular inverse dynamics control law, the reference position is followed closely, although some deviation occurs due to end-effector vibration. When introducing vibration control, the influence on input torques to the manipulator leads to a significant end effector position tracking error. This effect is related to the perturbation caused by the vibration controller on the torque inputs that is required to dampen the vibration of the system. Is is worth noticing that so far the end effector position control remains open loop, as there is no motion controller present at this time. Afterwards, the redundancy controller is introduced to manage the spare degrees of freedom. As was previously observed, the presence of the vibration control loop affects the inverse dynamics in a manner that jeopardizes end effector trajectory tracking. Subsequently, the introduction of the redundancy control loop in the system should not improve its tracking ability, as the motion control is still open loop. However, the following tests are meant to validate the implementation in terms of using the redundant degrees of freedom to change configuration when a force is applied at an intermediate point of the system. Similar tests as the ones that were used to provide validation of the controller in the rigid body case (section 5.1.1) are used here. The reference force and accelerations used for testing are shown in Figure 5.9. Reference end effector acceleration (x direction)
0.15
7
0.1
6
0.05
5
0 −0.05
4 3
−0.1
2
−0.15
1
−0.2
0
1
2
3 4 Time (s)
5
Force applied to the system (z direction)
8
Force (N)
Acceleration (rad/s2)
0.2
6
0
7
0
1
2
3 4 Time (s)
5
6
7
Figure 5.9: Reference acceleration and force profiles used in flexible redundancy tests.
In a first test, the system is given only a reference end effector acceleration corresponding to a trajectory in the Cartesian space. Results for the end effector position and joint coordinates are shown in Figure 5.10 . The joint coordinates shown here are the most representative, and the remaining joint coordinates can be found in Appendix A.3.
65
End effector position
1.4
1.2
1.2 1.1 0
1
2
3
4
5
6
7
J3 (rad)
x (m)
Inv. Dyn. & Vib. Cont. Inv. Dyn.
1.3
1.3
1
Joint coordinates
1.4
1.1 1 0.9 0.8 0.7
0.3
y (m)
0.2
0
1
2
3
0
1
2
3
4
5
6
7
4
5
6
7
0.1 0 −0.1
0.8 0
1
2
3
4
5
6
7 0.7
InvDyn & Vib Reference InvDyn
−0.2 z (m)
J6 (rad)
0
−0.4
0.5
−0.6 −0.8
0
1
2
0.6
3
Time (s)
4
5
6
0.4
7
Time (s)
Figure 5.10: Comparison of the end effector position and selected joint trajectories using flexible redundancy resolution with only reference acceleration.
Analyzing the system response, it is clear that the implemented redundancy resolution scheme is unable to provide accurate tracking of the end effector position in open loop. The results here differ from the ones achieved using only the pseudoinverse (Figure 5.8) approach. When introducing the vibration controller, the system loses all tracking abilities, and the joint velocities do not stabilize as the controller attempts to eliminate flexible vibration. In second test, a force is applied to the system which is given a constant end effector reference position. The results can be seen in Figure 5.11. End effector position
1.1
Inv. Dyn. & Vib. Cont. Inv. Dyn.
1.5
1 0.9
Joint Coordinates
1.6
0
1
2
3
4
5
6
J3 (rad)
x (m)
1.2
7
1.4 1.3
−0.2
1
2
3
0
1
2
3
4
5
6
7
4
5
6
7
−1
0
1
2
3
4
5
6
7
−0.7 z (m)
0
−0.1
InvDyn + Vib Reference InvDyn
−0.75 −0.8
0
1
2
−1.2
J4 (rad)
y (m)
0
−1.4 −1.6
3
Time (s)
4
5
6
7
−1.8
Time (s)
Figure 5.11: Comparison of the end effector position and selected joint trajectories using flexible redundancy resolution with constant reference position and an applied force.
As expected, using the inverse dynamics controller with redundancy resolution the system changes its configuration only during the time the force is applied, while maintaining the given reference position. The tracking error is due to the static deflection of the links which acts as a disturbance. When including the vibration controller, the system displays a much greater position error due to the effect of the control action on the joints. 66
The above results shown that implementation of an outer control loop is essential for guaranteeing accurate end effector pose tracking. The unfeasibility of direct end effector position feedback control requires introduction of joint based controllers and closed loop inverse kinematics.
5.2.3
PD controller
Considering once again the flexible system consisting of a vibration suppression controller and an inverse dynamics controller, the next step is to introduce the PD controller that will guarantee accurate joint trajectory tracking. A key aspect for the implementation of PD controller is the choice of controller gains. Attending to the fact that the transfer function for the controller is given by a second order system, the controller gains can be related according to equation 5.1: K = ω2 P n K = 2 ξ ω D
n
√ ⇔ KD = 2 ξ KP
(5.1)
The requirements are that the system be critically damped to reduce oscillations. However, the system must also be fast to follow the desired trajectory accurately. The tuning of parameters was then to define a proportionate gain that generates satisfactory behavior. The final value was defined as a compromise between speed and required control input, as the generated torques should not exceed the maximum of the motors associated with each joint. Due to dynamics that are not fully compensated and the presence of disturbances caused by the vibration controller, the damping factor ξ was chosen higher than unity. The final values for the controller gains used in the joint PD controller are shown in table 5.4 KP ξ KD
40 × I 7×7 1.2 √ 2 × ξ × 40 × I 7×7
Table 5.4: Controller parameters for joint PD controller.
To test the controller, a system consisting of a joint PD, an inverse dynamics block and a vibration controller were given a reference position and velocity, depicted in Figure 5.12a. These references were equally fed to all joints on the system. It can be seen that the given reference trajectories are quite aggressive, which is intended to ensure the performance of the controller even at high speeds. In the application in mind, it is expected that the joint velocities will be low at all times. Note that the CLIK methodology also requires a slow evolution of the system. Figure 5.12b shows resulting joint trajectories for the first two joints, which have been displayed here for compactness. The remaining joint trajectories can be found in Appendix A.4. Analyzing the simulatino results, it can be seen that even though the damping factor has been chosen to make the system overdamped, there is still a slight overshoot in the joint positions. This fact is caused in one part by the relatively high speeds involved in this test, added to the fact that the controller does not take into consideration a full dynamic compensation (reference joint accelerations are not used for control purposes). 67
Reference inputs
4
Position (rad) Velocity (rad/s)
3 J1 (rad)
2
3
1
−1
2
0
1
2
0
1
2
3
4
5
3
4
5
6
1.5
5
1 0.5 0
Actual trajectory Reference
0
2.5
J2 (rad)
Joint trajectories (rad − rad/s)
3.5
Joint coordinates
4
4 3 2
0
1
2
Time (s)
3
4
1
5
(a)
Time (s)
(b)
Figure 5.12: a) Reference joint positions and velocities used for testing PD controller; b) Resulting joint coordinates.
Having completed PD tuning, the flexible robotic system is now controlled in joint space. The next step is to generate reference inputs that provide adequate behavior in Cartesian space, as well as deal with the kinematic redundancy of the structure.
5.2.4
CLIK-R
The nature of the implemented closed loop inverse kinematics scheme with redundancy resolution makes it independent of any of the other controllers. The CLIK-R scheme has two functions. Firstly, for a given reference position and orientation trajectory, it calculates the required joint velocity and position trajectories. Secondly, if force is applied at an intermediate point of the structure the robot should change its configuration without affecting the end effector position. The control parameters for the CLIK-R can be found in table 5.5. Similarly to what was described for the rigid body case, the joint impedance controller gains were determined so the internal motions would be critically damped without any oscillation. It was also desired to have a low joint inertia as to allow reconfiguration with little effort from the user. K MI
0.1 × I 7×7
K DI
0.8 × I 7×7
Table 5.5: Controller parameters for CLIK-R.
It is logical then that two situations are required to test the functionalities of this system. On a first test, the objective is to determine whether the controller is capable of generating joint coordinates that ensure a null end-point tracking error. The system is given as an input a trajectory in the operational 68
space, and no force is applied at the intermediate point of the system or at the end effector. Figure 5.13 shows the simulation results comparing the reference and end effector position. End effector position
x (m)
1.5
CLIK Reference
1 0.5
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
y (m)
0.216 0.2159 0.2158
z (m)
−0.5187 −0.5188 −0.5189
Time (s)
Figure 5.13: Simulation results for CLIK end effector position tracking.
Analyzing the obtained results, it can be seen that the implemented CLIK-R methodology is able to effectively track a reference trajectory in the operational space and convert it into joint coordinates. It is noticeable that there is a small steady state error between the end effector position calculated by CLIK-R and the desired position. In another simulation, the system is given a constant end effector pose, and a force (Figure 5.14) is applied at the intermediate force sensor. The results can be seen in Figure 5.15, where the information regarding the first two joints is given. Information regarding all joints can be found in Appendix A.5. Intermediate force measurements 0 −1
Force (N)
−2 −3 −4 −5 −6 −7 −8
0
1
2
3
4
5
6
Time (s)
Figure 5.14: Force signal measured by the auxiliary sensor.
69
7
End effector position
0.2
0.7903 0.7902
Joint coordinates
0.3
0
1
2
3
4
5
6
7
J1 (rad)
x (m)
0.7904
0.1 0 −0.1
0.216 0.2155
1
2
3
4
5
6
7
−0.517 z (m)
0
1
2
3
4
5
6
7
0
1
2
3 4 Time (s)
5
6
7
−0.2 0
CLIK Reference
−0.518
−0.4 J2 (rad)
y (m)
0.2165
−0.6 −0.8 −1
−0.519
0
1
2
3 4 Time (s)
5
6
7
(a)
−1.2
(b)
Figure 5.15: Simulation results for redundancy resolution with CLIK-R. a) End effector position; b) Resulting joint coordinates.
When looking at the simulation results, it is noticeable that the effect of the force applied at the intermediate sensor causes a reconfiguration of the manipulator. Moreover, the end effector position remains constant throughout the whole process of reconfiguration, albeit having a slight steady state error. Having completed the validation of the inverse kinematics scheme, the problems associated with non minimum phase systems are avoided, and the full system is complete. As the impedance controller implemented for flexible systems is the same as the one implemented for the rigid case, results will not be shown here. It has been proven earlier (Figure 5.4) the ability of the impedance controller to modify the reference compliant frame based on measurements from the sensor placed at the robot’s tip. The flexible redundant robot manipulator is now controlled. If a force is applied at the end effector of the robot manipulator, the compliant frame which is generated by the impedance controller is modified. Subsequently, this new reference is converted by CLIK-R into a joint trajectory, and the joint PD controller ensures tracking of this new reference. If at any time a force is also applied at an intermediate point of the structure, the CLIK-R generates an additional velocity vector that does not affect the end effector position. The PD controller once again corrects the joint coordinates to comply with this new reference. The output of the PD controller serves the Inverse dynamics controller, which generates required torques. The vibration suppression controller actively increases damping of elastic velocities during at all times.
70
Chapter 6
Discussion and Conclusions In this chapter the performance of the implemented controllers is analyzed, as well as the response of the overall system. Conclusions are made regarding the developed system, and future works that might enrich the work developed here are propsed.
6.1
Discussion
The first aspect worth mentioning is the fact that the system is implemented in continuous time, and no disturbances are considered. This factor has been made to simplify the design procedure, as it has been shown in previous works that the impedance controller with inner motion control is capable of effective disturbance rejection. When considering the rigid system, the redundancy resolution scheme is shown to accurately change the configuration of the robot whilst having no effect on the end effector position. This effect is valid both for constant end effector position and for position tracking. The implementation of the impedance controller with inner motion control is seamless, and it is possible to modify the end effector reference position by an actuating force. The gains have been chosen as to guarantee acceptable response of the system, but impedance gains associated both with impedance controller and redundancy resolution should be modified to accommodate the preferences of the user. When looking at the flexible system, it is clear that the implementation of the vibration controller effectively increases structural damping of the flexural links. The corresponding required joint accelerations have a large frequency, which may be difficult to achieve due to limited bandwidth of the actuators. This implementation, however, leads to a deterioration of open loop position tracking when compared to a simple inverse dynamics control. The introduction of the redundancy resolution controller to the flexible case enables changing the system configuration when a force is applied at an intermediate point, with small tracking error. However, the inclusion of the vibration controller perturbs the commands generated by the inverse dynamics, and the effect is that the robot changes its final configuration. This limitation is critical since there can be no end point feedback position control, since the non-minimum phase properties of the system would lead to an unstable closed loop. 71
Alternatively, the solution which makes use of the closed Loop inverse kinematics scheme shows great promise, as it ensures system stability and provides accurate end point tracking. It requires, however, that the system evolve at a low speed, which is not a serious limitation in the application at study in this work. The implementation of the redundancy resolution to CLIK also shows interesting results, as it effectively generates internal motions without affecting the end effector. The fact that these motions are generated before the corrective term in CLIK guarantees small position error during the reconfiguration.
6.2
Future Work
This work is part of a project spanning for several works in the control of flexible robots, and as such is always at best incomplete. The improvements that the author thinks should be considered in the follow up projects are: • In the rigid body case, the system requires additional studies regarding robustness in disturbance rejection. • The vibration gains that were used for the vibration controller were determined by manual tuning. For a more effective vibration damping the gains should be selected by using an optimal approach by solving the Riccati equations, or using an LQR regulator. This would require obtaining the model of the system in state space. The inclusion of piezoelectric actuators enables other types of controllers, without the disturbance effect on the inverse dynamics control signal. • Still in the vibration controller, the measurement of modal deformation rates is sometimes diffucult in a practical sense, due to high sensor noise. As such, a controller based on deformation such as Positive Position Feedback could be a more fitting solution. • As for the redundancy resolution, the aspect of placing a sensor at an intermediate point of the structure may not be viable. A solution for the flexible system is to extract the force applied at any point of the structure by measuring the deformation this force induces on the flexible links. • Ultimately, the system should be implemented on the actual physical system.
6.3
Conclusions
In this work, the extensive area of human robot interaction is approached, with focus on developing medical robots that assist surgeons in complex, highly precise procedures. A flexible, kinematically redundant solution under co-manipulation control seems to be the formula to success in a rapidly growing market. The proposed solution is HipRob, the 7 degree of freedom flexible link manipulator present at IST. The lightweight design, geometry and kinematics that resemble the human arm make it a logical solution to assist humans in unconstrained environments. 72
A comprehensive study on the modeling of flexible link manipulators using the Assumed Mode Method with Rayleigh Ritz discretization has been presented. A comparative review of symbolic robotic modeling tools has been made, from which the MECANISMO toolbox has been chosen to generate the model of the system. Interaction control schemes have been analyzed, and two systems, one rigid and one flexible, have been proposed. For the rigid case, an impedance controller with inner motion control and redundancy resolution has been proposed. For the flexible system, a solution based on a closed loop inverse kinematics scheme has been modified to include redundancy resolution. a PD controller with inverse dynamics generates desired torques for the motors. For redundancy resolution, a novel task function has been developed, which controls the redundant degrees of freedom by generating inner motion commands based on an applied force at an intermediate point of the system. This controller has been applied to both models of the system with good results. A vibration control scheme has been introduced in the flexible model of the system to eliminate structural vibration. The results show effective damping in static conditions, but at a cost of reduced accuracy in trajectory tracking. In light of the obtained results the control methodologies presented in this dissertation seem a promising solutions for interaction control of a robotic manipulator with a human operator.
73
74
Bibliography Adler, J R, Chang, S D, Murphy, M J, Doty, J, Geis, P, & Hancock, S L. (1997). The Cyberknife: a frameless robotic system for radiosurgery. Stereotact Funct Neurosurg, 69(1-4 Pt 2), 124–128. ¨ Albu-Schaffer, A., & Hirzinger, G. (2002). Cartesian impedance control techniques for torque controlled light-weight robots. Pages 657–663 of: Proc. IEEE Int. Conf. Robotics and Automation ICRA ’02, vol. 1. Ballantyne, G, & Moll, F. (2003). The daVinci telerobotic surgical system: The virtual operative field and telepresance surgery. Surg. Clin. North Am., 83, 1293–1304. Barrett, A. R W, Davies, B. L., Gomes, M. P S F, Harris, S. J., Henckel, J., Jakopec, M., Kannan, V., y Baena, F. M. Rodriguez, & Cobb, J. P. (2007). Computer-assisted hip resurfacing surgery using the acrobot navigation system. Proc Inst Mech Eng H, 221(7), 773–785. BrainLAB. (2005) (Winter). Fast Growth and Innovations for Computer-Assisted Hip Resurfacing. Tech. rept. Camarillo, David B, Krummel, Thomas M, & Salisbury, Kenneth J Jr. (2004). Robotic Technology in surgery: past, present and future. The American Journal of Surgery, 188(October). Chiaverini, S., & Siciliano, B. (1999). The Unit Quaternion: a Useful Tool for Inverse Kinematics of Robot Manipulators. Systems Analysis, Modelling and Simulation, 35, 45–60. Cleary, K, & Nguyen, C. (2001). State of the art in surgical robotics: clinical applications and technology challenges. Computer Aided Surgery: Official Journal of the International Society for Computer Aided Surgery, 6(6), 312–28. Corke, P.I. (1995). A computer tool for simulation and analysis: the Robotics Toolbox for MATLAB. Pages 319–330 of: Proc. National Conf. Australian Robot Association. Davies, B, Jakopec, M, Harris, SJ, Rodriguez Y Baena, F, Barrett, A, Evangelidis, A, Gomes, P, Henckel, J, & Cobb, J. (2006). Active-Constraint Robotics for Surgery. Proceedings of the IEEE, 94(9), 1696– 1704. ´ (2008). Controlo da Impedancia ˆ ˆ Manipuladores para Aplicac¸oes ˜ em Cirurgia OrFeio, Jose. de Robos ´ ´ ´ topedica. M.Phil. thesis, Instituto Superior Tecnico - Universidade Tecnica de Lisboa. In Portuguese. 75
Fichtinger, G, Kazanzides, P, Okamura, A, Hager, G, Whitcomb, L, & Taylor, R. (2008). Surgical and interventional robotics: Part II - Surgical CAD-CAM Systems. IEEE Robotics & Automation Magazine, 15(3), 94–102. Gomes, P. (2010). Surgical Robotics: Reviewing the past, analysing the present, imagining the future. Robotics and Computer-Integrated Manufacturing. Hager, G., Okamura, A., Kazanzides, P., Whitcomb, L., Fichtinger, G., & Taylor, R. (2008). Surgical and interventional robotics: Part III - Surgical Assistance Systems. IEEE Robotics & Automation Magazine, 15(4), 84–93. ¨ ¨ Hagn, U, Nickl, M, Jorg, S, Passig, G, Bahls, T, Nothhelfer, A, Hacker, F, Le-Tien, L, Albu-Schaffer, A, Konietschke, R, Grebenstein, M, Warpup, R, Haslinger, R, Frommberger, M, & Hirzinger, G. (2008). The DLR MIRO: a versatile lightweight robot for surgical applications. Industrial Robot: An International Journal, 35(4), 324–336. ´ B., Kovacs, ´ ´ (2009) (August 12-14). Force Sensing and Force Control Haidegger, T., Benyo, L., & Benyo. for Surgical Robots. Pages 413–418 of: Proceedings of the 7th IFAC Symposium on Modelling and Control in Biomedical Systems. Hogan, Neville. (1984). Impedance Control: An Approach to Manipulation. Pages 304–313 of: Proc. American Control Conf. Jakopec, M, Harris, S J, y Baena, F Rodriguez, Gomes, P, Cobb, J, & Davies, B L. (2001). The first clinical application of a ”hands-on” robotic knee surgery system. Computer Aided Surgery, 6(6), 329– 339. Jakopec, M., Rodriguez y Baena, F., Harris, S. J., Gomes, P., Cobb, J., & Davies, B. L. (2003). The hands-on orthopaedic robot ”acrobot”: Early clinical trials of total knee replacement surgery. IEEE Transactions on Robotics and Automation, 19(5), 902–911. Jaramaz, Branislav, Hafez, Mahmoud A, & DiGioia, Anthony M. (2006). Computer-Assisted Orthopaedic Surgery. Proceedings of the IEEE, 94(9), 1689–1695. Kazanzides, P, Fichtinger, G, Hager, G D, Okamura, A M, Whitcomb, L L, & Taylor, R H. (2008). Surgical and Interventional Robotics - Core Concepts, Technology, and Design. IEEE Robotics & Automation Magazine, 15(2), 122–130. Kazerooni, H., Sheridan, T., & Houpt, P. (1986). Robust compliant motion for manipulators, part I: The fundamental concepts of compliant motion. IEEE Journal of Robotics and Automation, 2(2), 83–92. ¨ ¨ Kilicaslan, S., Olzg olren, M., & Ider, K. (2010). Hybrid force and motion control of robots with flexible links. Mechanism and Machine Theory, 45, 91–105. Kluge, Wolfram H. (2009). Computer assisted hip resurfacing. Orthopaedics and Trauma, 23(3), 210– 215. 76
Kruger, S., Zambelli, P. Y., Leyvraz, P-F., & Jolles, B. M. (2009). Computer-assisted placement technique ¨ in hip resurfacing arthroplasty: improvement in accuracy? Int Orthop, 33(1), 27–33. Kwoh, Y S, Hou, J, Jonckheere, E A, & Hayati, S. (1988). A robot with improved absolute positioning accuracy for CT guided stereotactic brain surgery. IEEE Transactions on Biomedical Engineering, 35(2), 153–160. Lee, T. H., Ge, S. S., & Wang, Z. P. (2001). Adaptive robust controller design for multi-link flexible robots. Mechatronics, 11, 951–967. Martins, J. M. (2007). Modelling Identification and Control of Flexible Manipulators. PhD, Instituto ´ Superior Tecnico (TU Lisbon). Martins, J. M., Botto, M. A., & Sa da Costa, J. (2002). Modeling of Flexible Beams for Robotic Manipulators. Multibody System Dynamics, 7, 79–100. McPhee, J J. (1996). On the Use of Linear Graph Theory in Multibody System Dynamics. Nonlinear Dynamics, 73–90. McPhee, J.J., Shi, P., & Piedbœ uf, Jean-Claude. (2002). Dynamics of Multibody Systems Using Virtual Work and Symbolic Programming. Mathematical and Computer Modelling of Dynamical Systems, 8(2), 137–155. McWilliams, Andrew. (2009). Medical Robotics and Computer-Assisted Surgery. Market Research Report HLC036C. BCC Research. Mohamed, Z., Martins, J. M., Tokhi, M. O., Sa´ da Costa, J., & Botto, M. A. (2005). Vibration control of a very flexible manipulator system. Control Engineering Practice, 13(3), 267 – 277. Aerospace IFAC 2002. Mont, MA, Rajadhyaksha, AD, & Hungerford, DS. (2001). Outcomes of limited femoral resurfacing surgeries compared with total hip arthroplasty for osteonocrosis of the femoral head. J. Arthroplasty, 16(8), 134–139. Moore, B., Kovecses, J., & Piedboeuf, J.C. (2003) (July 1-4). Symbolic Model Formulation for Dynamic Parameters Identification. In: Ambrsio, Jorge A.C. (ed), Multibody Dynamics. Moore, Brian, Piedboeuf, J.C., & Bernardin, Laurent. (2002). Maple as an automatic code generator? Maple Summer Workshop. Moorthy, K, Munz, Y, Liddle, A, Dosis, A, Martin, S, Rockall, T, & Darzi, A. (2004). Objective comparison of the learning curves of laparoscopic and robotic surgery using motion analysis. Surg. Endosc., 18, 282. Natale, C., Siciliano, B., & Villani, L. (1999). Spatial impedance control of redundant manipulators. Pages 1788 –1793 of: Robotics and Automation, Proceedings. 1999 IEEE International Conference on, vol. 3. 77
Nguyen, L. A., Walker, I. D., & DeFigueiredo, R. J. P. (1992). Dynamic control of flexible, kinematically redundant robot manipulators. IEEE Transactions on Robotics and Automation, 8(6), 759–767. Patel, R. V., & Shadpey, F. (2005). Redundant Manipulators: Kinematic Analysis and Redundancy Resolution. Chap. 2, pages 7–33 of: Control of Redundant Robot Manipulators. LNCIS, no. 316. Berlin-Heidelberg: Springer-Verlag. Piedboeuf, J. (1998). Recursive Modeling of Serial Flexible Manipulators. The Journal of the Astronautical Sciences, 46(1), 1–24. Piedboeuf, J., Dovon, M., Langois, P., & L’Archevque, R. (1999). SYMOFROS: A Flexible Dynamics Modeling Software. In: Proc. 5th International Symposium on Artificial Inteligence, Robotics and Automation in Space. Piedboeuf, J. C. (1996). Modeling Flexible Robots with Maple. MapleTech, 3(1). Piedboeuf, J.C., Farooq, M., Bayoumi, M. M., Labinaz, G., & Argoun, M. B. (1993). Modelling and control of flexible manipulators-revisited. Pages 1480–1483 of: Proc. 36th Midwest Symp. Circuits and Systems. ¨ Piedboeuf, J.C., Kovecses, J., Lange, C., Moore, B, & Gonthier, Y. (2002). Advanced Modeling Techniques for Robotic Manipulators. In: Tutorial Session at the 2002 IEEE Conference on Robotics & Automation. Pires, P., Teodoro, P., Martins, M., & Sa´ da Costa, J. (2009). Position and Force Control of a Flexible Robot Manipulator For Orthopedic Surgery. Proceedings of the European Control Conference 2009, August, 3347 – 3352. Pires, P., Teodoro, P., Martins, M., & Sa´ da Costa, J. (2010). Position Control of the Non-Minimum Phase Flexible Robotic Link System. Submited for publication. Preumont, A. (2002). Vibration control of active structures: An introduction. 2nd edn. Solid Mechanics and its Applications. Kluwer Academic. Resubal, Jose Rafael E, & Morgan, David A F. (2009). Computer-assisted vs conventional mechanical jig technique in hip resurfacing arthroplasty. The Journal of arthroplasty, 24(3), 341–50. ˆ Manipuladores de Baixo Peso com Redundancia ˆ Rita, Tiago. (2010). Projecto Integrado de Robos ´ ´ Cinematica e Elos Inteligentes. Tech. rept. Instituto Superior Tecnico. In Portuguese. Riviere, C. N., Gangloff, J., & De Mathelin, M. (2006). Robotic Compensation of Biological Motion to Enhance Surgical Accuracy. Proceedings of the IEEE, 94(9), 1705–1716. Sa´ da Costa, J., Martins, J. M., & Botto, M. A. (2008). Classical mechanics approach of modelling multilink flexible manipulators. Chap. 3 of: Tokhi, M.O., & Azad, A.K.M. (eds), Flexible Robot Manipulators - Modelling Simulation and Control. Control Engineering Series, no. 68. IET. 78
Salisbury, J. Kenneth. (1980). Active stiffness control of a manipulator in cartesian coordinates. Pages 95–100 of: Proc. 19th IEEE Conf. Decision and Control including the Symp. Adaptive Processes, vol. 19. Sass, L., McPhee, J., Schmitke, C., Fisette, P., & Grenier, D. (2004). A Comparison of Different Methods for Modelling Electromechanical Multibody Systems. Multibody System Dynamics, 12(3), 209–250. Shi, Pengfei, & McPhee, John. (2000). Dynamics of Flexible Multibody Systems Using Virtual Work and Linear Graph Theory. Multibody System Dynamics, 355–381. Shi, Pengfei, & McPhee, John. (2002) (August). DynaFlex User’s Guide. Systems Design Engineering , University of Waterloo, Canada. Siciliano, B. (1990). Kinematic Control of Redundant Manipulators: A Tutorial. Journal of Intelligent Robotic Systems, 3, 201–212. Siciliano, B. (1998). An inverse kinematics scheme for a flexible arm in contact with a compliant surface. Pages 3617 – 3622 of: Decision and Control, 1998. Proceedings of the 37th IEEE Conference on, vol. 4. Siciliano, B., & Villani, L. (1999). Robot Force Control. Kluwer Academic. Siciliano, B., & Villani, L. (2000). Parallel force and position control of flexible manipulators. IEE Proceedings in Control Theory and Applications, 147(6), 605 –612. Siciliano, B., & Villani, L. (2001). Two-time scale force and position control of flexible manipulators. Pages 2729 – 2734 vol.3 of: Proceedings 2001 ICRA IEEE International Conference on Robotics and Automation, vol. 3. Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Advanced Textbooks in Control and Signal Processing. Springer. Singhose, W.E., & Seering, W.P. (2008). Control of flexible manipulators with input shaping techniques. In: Flexible Robot Manipulators - Modelling Simulation and Control. IET. Taylor, R. (2006). A Perspective on Medical Robotics. Proceedings of the IEEE, 94(9), 1652–1664. Taylor, R, & Joskowicz, L. (2003). Standard Handbook of Biomedical Engineering and Design. New York: McGraw-Hill. Chap. Computer-Integrated Surgery and Medical Robotics, pages 29.3–29.45. Taylor, R, Jenson, P, Whitcomb, L, Barners, A, Kumar, R, Stoianovici, D, Gupta, P, Wang, Z, deJuan, E, & Kavoussi, L. (1999). A steady-hand robotic system for microsurgical augmentation. International Journal of Robotics Research, 18, 1201–1210. Teodoro, P, Pires, P, Martins, J, & Sa´ da Costa, J. (2008). Resurfaced femoral head finite element analysis: Mapping von-mises stress distribution in three different prosthesis orientations. Tech. rept. IST, CSI-IDMEC. 79
Tokhi, M. O., Azad, A.K.M, Pota, H.R., & Senda, K. (2008). Flexible Manipulators - an overview. Chap. 1 of: Tokhi, M.O., & Azad, A.K.M. (eds), Flexible Robot Manipulators - Modelling Simulation and Control. Control Engineering Series, no. 68. IET. Tso, S.K., Yang, T.W., Xu, W.L., & Sun, Z.Q. (2003). Vibration control for a flexible-link robot arms with deflection feedback. International Journal of Non-Linear Mechanics, 38, 51–62. Villani, L., & Schutter, J. (2008). Force Control. In: Siciliano, B., & Khatib, O. (eds), Springer Handbook of Robotics. Springer. Wang, Y, Butner, S E, & Darzi, A. (2006). The Developing Market for Medical Robotics. Proceedings of the IEEE, 94(9), 1763–1771. Wasfy, Tamer M, & Noor, Ahmed K. (2003). Computational strategies for flexible multibody systems. Applied Mechanics Reviews, 56(6), 553.
80
Appendix
81
Appendix A
Test Results Tests on Rigid Redundancy Resolution
0 −0.2
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0 −0.5 −1
1.5 1 0.5
−1.2 −1.4 −1.6
J7 (rad)
J6 (rad)
J5 (rad)
J4 (rad)
Joint positions
0.2
J3 (rad)
J2 (rad)
J1 (rad)
A.1
1.7 1.6 1.5
0.8 0.6 0.4
1 0.5 0
0
1
2
3
Time (s)
4
5
6
Force + Accel Accel
7
Figure A.1: Variation of joint coordinates based on varying end-point acceleration for 3 seconds and applied force during 5 seconds.
83
A.2
Tests on Vibration Control Link 1 deformation rates
x 10
−3
0 −5
0
1
2
3
4
5
6
5 0 −5
7
0
x 10
0 −2
0
1
2
3
4
5
6
7
1
2
3
Time (s)
4
5 6 7 Inv. Dyn. Inv. Dyn. & Vib. Cont.
Flexion (x−z plane) (m/s)
Flexion (x−z plane) (m/s)
x 10
0
3
4
5
6
7
1
2
3
4
5
6
7
1
2
3
5
6
7
0
−5
0 −4
0 −5
2
x 10
5
−5
5
1 −4
Flexion (x−y plane) (m/s)
Flexion (x−y plane) (m/s)
−5
2
Link 2 deformation rates
x 10 Torsion (x axis) (rad/s)
Torsion (x axis) (rad/s)
−3
5
x 10
5 0 −5
0
(a)
4
Inv. Dyn. Inv. Dyn. & Vib. Cont.
(b) −3
Torsion (x axis) (rad/s)
Time (s)
2
Link 3 deformation rates
x 10
0 −2
0
1
2
3
4
5
6
7
1
2
3
4
5
6
7
1
2
3
4
5 6 7 Inv. Dyn. Inv. Dyn. & Vib. Cont.
Flexion (x−y plane) (m/s)
−5
2
x 10
0
−2
0
Flexion (x−z plane) (m/s)
−5
5
x 10
0 −5
0
Time (s)
(c)
Figure A.2: Comparison of controlled (using vibration controller) and uncontrolled flexible link vibrations.
84
A.3
Tests on Flexible Inverse Dynamics with Redundancy Resolution
J1 (rad)
Joint coordinates 0.4 0.2
J2 (rad)
0
J3 (rad)
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
1 0 −1
1.5 1 0.5
J4 (rad)
0
−1 −1.5
J5 (rad)
−2
2.5 2
J6 (rad)
1.5
0.8 0.6
J7 (rad)
0.4
1 0.5 0
Time (s)
7 Inv. Dyn. & Vib. Cont. Inv. Dyn.
Figure A.3: Variation of joint coordinates based on a reference end-point acceleration during 3 seconds.
85
Tests on joint PD controller
J6 (rad)
J5 (rad)
J4 (rad)
J3 (rad)
J2 (rad)
J1 (rad)
Joint coordinates
J7 (rad)
A.4
4 2 0 0
0.5
1
1.5
2
2.5
3
3.5
Actual trajectory Reference 4 4.5 5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
0
0.5
1
1.5
2
2.5 Time (s)
3
3.5
4
4.5
5
4 2 0
4 2 0
4 2 0
4 2 0
4 2 0
4 2 0
Figure A.4: Variation of joint coordinates based on joint position and velocity under PD control.
86
Tests on CLIK-R
0.5 0 −0.5
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0 −1 −2
2 1.8 1.6
−1 −1.5 −2
J6 (rad)
J5 (rad)
J4 (rad)
J3 (rad)
J2 (rad)
J1 (rad)
Joint coordinates
J7 (rad)
A.5
1.7 1.6 1.5
1 0.5 0
−0.2 −0.4 −0.6
Time (s)
Figure A.5: Variation of joint coordinates when actuated by a force at the auxiliary sensor.
87
88
