TABLE OF CONTENTS 1.
LITERATURE REVIEW AND PAST WORKS .......................................... ................................................................. ............................... ........ 10
1.1
FISH SWIMMING MODES: ............................................. .................................................................... ............................................. ........................ .. 10
1.2
LIGHTHILL’S WORKS: .......................................... ................................................................. ............................................. ................................... ............. 10
1.3
SIMPLIFIED PROPULSIVE MODEL FOR CARANGIFORM CARANGIFORM PROPULSION: ........................ ........................ 11
1.4
GRAY’S PARADOX: ........................................... .................................................................. ............................................. ...................................... ................ 14
1.5
PAST PROJECTS: .......................................... ................................................................. ............................................. .......................................... .................... 14
1.5.1
ROBOTUNA I, II AND CHARLIE I (MIT): .......................................... ................................................................. ......................... 14
1.5.2
ROBOPIKE (MIT) 1998: ......................... ............................................... ............................................. ........................................... .................... 15
1.5.3
PPF-04 (NMRI): ........................................... .................................................................. ............................................. ................................... ............. 16
1.5.4
ROBOTIC FISH SPC-03 BUAA-CASIA (CHINA): ....................................................... .......................................................16
1.5.5
ROBOTIC EEL - ROBEA PROJECT CNRS (FRANCE):............................................ ................................................. ..... 17
1.5.6
BOXYBOT BIRG-EPFL (SUISS): .......................... ................................................ ............................................. ................................ ......... 18
1.5.7
ESSEX ROBOTIC FISH (GREAT BRITAIN): ........................................... ............................................................... ....................18
1.5.8
ROBOTIC KOI - RYOMEI ENGINEERING – JAPAN:.................................................. .................................................. 19
1.5.9
JESSIKO - ROBOTSWIM - FRANCE: ......................................... ............................................................... ............................... ......... 19
1.5.10
ROBOFISH - UNIVERSITY OF WASHINGTON - U.S.A.: ............................................ ............................................ 20
1.5.11
STINGRAY – KNIFEFISH – NANYANG UNIVERSITY – SINGAPORE:.......................... .......................... 20
1.5.12
TAI-ROBOT-KUN TAI-ROBOT-KUN - UNIVERSITY OF KITAKYUSHU - JAPAN:..................................... JAPAN:.....................................21
1.5.13
POLYMER ACTUATED (PPy) ROBOTIC FISH: ....................... .............................................. .................................... ............. 21
1.5.14
BIOMIMETIC CONTROLLED CURVATURE CURVATURE ROBOTIC PECTORAL FIN: ...................... ...................... 22
2.
SYSTEM MODELLING...................................................... ............................................................................. ............................................. ........................... ..... 23
2.1
MODELLING TECHNIQUES: TECHNIQUES: ............................................ ................................................................... ............................................. ........................ .. 23
2.2
LAGRANGIAN MODELLING:............................................... ...................................................................... ........................................... ....................24
2.2.1
DENAVIT HARTENBERG HARTENBERG NOTATION: .......................................... ................................................................ ........................... ..... 24
2.2.2
REFERENCE FRAMES: .......................................... ................................................................. ............................................. ........................... .....25
2.2.3
MATRICES AND VECTORS: ............................................. .................................................................... ....................................... ................ 25
2.2.4
EQUATIONS OF MOTION: .......................................... ................................................................. ........................................... ....................27
2.2.5
ROBOFISH AND HEAD 3D CONFIGURATION: CONFIGURATION: ........................ ............................................... ................................ ......... 28
2.2.6
KINETIC AND POTENTIAL ENERGIES: ........................................................... .................................................................... ......... 29
2.3
HYDRODYNAMIC MODELLING: ............................................. .................................................................... ....................................... ................ 35
i
2.3.1
ASSUMPTIONS: ........................................... .................................................................. ............................................. ................................... ............. 36
2.3.2
HYDRODYNAMIC FORCES APPROXIMATION: .......................................... ....................................................... ............. 37
2.3.3
PRESSURE ON LINKS: .......................................... ................................................................. ............................................. ........................... ..... 37
2.3.4
APPROACH STREAM PRESSURE: ................................................ ...................................................................... ........................... ..... 38
2.3.5
FRICTION DRAG:....................................... DRAG:............................................................. ............................................. ....................................... ................38
2.3.6
PRESSURE ON PECTORAL FINS:.......................................... FINS:................................................................ ................................... ............. 38
2.3.7
COMPOSITION OF HYDRODYNAMIC FORCES: .................................. ...................................................... ....................39
2.3.8
LIMITATIONS:............................................ ................................................................... ............................................. ...................................... ................ 39
3.
MECHANICAL DESIGN: ........................................... .................................................................. ............................................. ................................... ............. 40
3.1
DIFFICULTIES IN BIOMIMETIC ROBOFISH DESIGN: .................................................... ...................................................... .. 40
3.2
ASSUMPTIONS: ........................................... .................................................................. ............................................. .......................................... ....................41
3.3
SOLIDWORKS MODEL:................................................ ....................................................................... ............................................. ........................... ..... 42
3.3.1
HEAD DESIGN:........................................................ ............................................................................... .............................................. ......................... 42
3.3.2
LINK I DESIGN: .................................. ........................................................ ............................................. .............................................. ......................... 43
3.3.3
LINK II-III DESIGN: ........................................... .................................................................. ............................................. ............................... ......... 43
3.3.4
CAUDAL FIN DESIGN: ............................................................ ................................................................................... ................................ ......... 43
3.3.5
PECTORAL FIN DESIGN: ................................... ......................................................... ............................................. ................................ ......... 43
3.4
CUSTOM VISUALIZATION: VISUALIZATION: .......................................... ................................................................. ............................................. ........................... ..... 44
3.5
DESIGN PARAMETERS: ........................................... .................................................................. ............................................. ............................... .........45
3.6
SOLIDWORKS TO SIMMECHANICS: .................................................. ......................................................................... ............................ ..... 48
3.6.1
ADVANTAGES OF USING SIMMECHANICS: .......................................... .............................................................. ....................48
3.6.2
EXPORTING CAD ASSEMBLIES INTO SIMMECHANICS: ............................. ............................................. ................ 49
3.6.3
REQUIREMENTS REQUIREMENTS FOR CAD EXPORT AND MECHANICAL IMPORT: ............................ ............................ 49
3.6.4
IMPORTING THE XML AND GENERATING A MODEL: .......................................... ............................................... ..... 51
3.6.5
INTRODUCING ACTUATORS AND SENSORS TO PHYSICAL MODEL: ......................... ......................... 52
3.6.6
SIMULATING THE SIMMECHANICS MODEL: ......................................................... .........................................................54
3.6.7
ACTIVATING AND CONTROLLING CONTROLLING ANIMATION RECORDING: ...............................54
3.7
SOLIDWORKS TO VIRTUALREALITY VIRTUALREALITY TOOLBOX: ............................................ ............................................................ ................ 56
3.7.1
VISUALIZING WITH A VIRTUAL REALITY CLIENT: .................................................. .................................................. 56
3.7.2
REPRESENTING BODIES AS VIRTUAL OBJECTS:............................................ ..................................................... ......... 56
3.7.3
ISSUE WITH WRL FILE IMPORT: ............................................. ................................................................... ............................... ......... 58
3.7.4
INTERFACING SIMMECHANICS MODELS WITH VIRTUAL WORLDS: ..................... 58
ii
2.3.1
ASSUMPTIONS: ........................................... .................................................................. ............................................. ................................... ............. 36
2.3.2
HYDRODYNAMIC FORCES APPROXIMATION: .......................................... ....................................................... ............. 37
2.3.3
PRESSURE ON LINKS: .......................................... ................................................................. ............................................. ........................... ..... 37
2.3.4
APPROACH STREAM PRESSURE: ................................................ ...................................................................... ........................... ..... 38
2.3.5
FRICTION DRAG:....................................... DRAG:............................................................. ............................................. ....................................... ................38
2.3.6
PRESSURE ON PECTORAL FINS:.......................................... FINS:................................................................ ................................... ............. 38
2.3.7
COMPOSITION OF HYDRODYNAMIC FORCES: .................................. ...................................................... ....................39
2.3.8
LIMITATIONS:............................................ ................................................................... ............................................. ...................................... ................ 39
3.
MECHANICAL DESIGN: ........................................... .................................................................. ............................................. ................................... ............. 40
3.1
DIFFICULTIES IN BIOMIMETIC ROBOFISH DESIGN: .................................................... ...................................................... .. 40
3.2
ASSUMPTIONS: ........................................... .................................................................. ............................................. .......................................... ....................41
3.3
SOLIDWORKS MODEL:................................................ ....................................................................... ............................................. ........................... ..... 42
3.3.1
HEAD DESIGN:........................................................ ............................................................................... .............................................. ......................... 42
3.3.2
LINK I DESIGN: .................................. ........................................................ ............................................. .............................................. ......................... 43
3.3.3
LINK II-III DESIGN: ........................................... .................................................................. ............................................. ............................... ......... 43
3.3.4
CAUDAL FIN DESIGN: ............................................................ ................................................................................... ................................ ......... 43
3.3.5
PECTORAL FIN DESIGN: ................................... ......................................................... ............................................. ................................ ......... 43
3.4
CUSTOM VISUALIZATION: VISUALIZATION: .......................................... ................................................................. ............................................. ........................... ..... 44
3.5
DESIGN PARAMETERS: ........................................... .................................................................. ............................................. ............................... .........45
3.6
SOLIDWORKS TO SIMMECHANICS: .................................................. ......................................................................... ............................ ..... 48
3.6.1
ADVANTAGES OF USING SIMMECHANICS: .......................................... .............................................................. ....................48
3.6.2
EXPORTING CAD ASSEMBLIES INTO SIMMECHANICS: ............................. ............................................. ................ 49
3.6.3
REQUIREMENTS REQUIREMENTS FOR CAD EXPORT AND MECHANICAL IMPORT: ............................ ............................ 49
3.6.4
IMPORTING THE XML AND GENERATING A MODEL: .......................................... ............................................... ..... 51
3.6.5
INTRODUCING ACTUATORS AND SENSORS TO PHYSICAL MODEL: ......................... ......................... 52
3.6.6
SIMULATING THE SIMMECHANICS MODEL: ......................................................... .........................................................54
3.6.7
ACTIVATING AND CONTROLLING CONTROLLING ANIMATION RECORDING: ...............................54
3.7
SOLIDWORKS TO VIRTUALREALITY VIRTUALREALITY TOOLBOX: ............................................ ............................................................ ................ 56
3.7.1
VISUALIZING WITH A VIRTUAL REALITY CLIENT: .................................................. .................................................. 56
3.7.2
REPRESENTING BODIES AS VIRTUAL OBJECTS:............................................ ..................................................... ......... 56
3.7.3
ISSUE WITH WRL FILE IMPORT: ............................................. ................................................................... ............................... ......... 58
3.7.4
INTERFACING SIMMECHANICS MODELS WITH VIRTUAL WORLDS: ..................... 58
ii
3.7.5
ANIMATING THE VIRTUAL WORLD BODIES: ............................. .................................................... ............................ ..... 59
3.7.6
CONVERTING BODY SENSOR SIGNALS INTO VRML FORMAT: .............................. .............................. 60
4.
SIMULATION AND RESULTS ........................................... .................................................................. ............................................. ........................... ..... 63
4.1
SPEED CONTROL: ............................................. .................................................................... ............................................. ...................................... ................ 63
4.1.1
SPEED CONTROL BASED ON OSCILLATING FREQUENCY: ...................................... ...................................... 63
4.1.2
SPEED CONTROL BASED ON OSCILLATORY AMPLITUDE: ...................................... ......................................64
4.1.3
COMPUTED TORQUE CONTROL METHOD:........................................... ........................................................... ................ 64
5.
FUTURE WORKS AND THINGS TO EXPLORE:............................................ ................................................................... ......................... 66
5.1
OPEN AREAS OF RESEARCH: ............................................................. .................................................................................... ............................ ..... 66
5.2
CENTRAL PATTERN GENERATOR (CPG):............................................ .................................................................. ........................... ..... 67
5.3
CONCLUSION:.......................................... ................................................................ ............................................ ............................................. ......................... .. 67
APPENDIX A: MATLAB CODES ........................................ .............................................................. ............................................. .................................... ............. 68 APPENDIX B: SYSTEM EQUATIONS EQUATIONS AND MATRICES............................................. ................................................................. ....................77 APPENDIX C: REFERENCES REFERENCES .......................................... ................................................................. ............................................. ...................................... ................ 88
iii
LIST OF FIGURES Fig 1.1. Different swimming modes of natural fish locomotion a) BCF (Body Caudal Fin) and b) MPF (Median Pectoral Fin). Shaded areas contribute to thrust generation. ..... 11 Fig 1.2. Physical Model of Carangiform Swimming showing the undulation motion in 2/3
rd
part of posterior body. ................................................................................................... 12 Fig 1.3. Link based body wave fitting. Notice that a 4-link mechanism is used as the model of body wave fitting. ....................................................................................................... 13 Fig 1.4. General Carangiform Swimming Motion. LightHill equation and Tail Phase relationship. .................................................................................................................... 13 Fig 1.6 a) RoboTuna II built in MIT 1994 with lever, pulley and ball bearing mechanisms and b) Improved Rib Cage design for RoboTuna II (MIT) for reduced drag and friction............................................................................................................................. 15 Fig 1.7 RoboPike (MIT) 1998 [12] concept gets its birth in the design board.......................... 15 Fig 1.8 RoboPike (MIT) 1998 swimming in the MIT Towing Tank (on the left) and internal wooden structure of the links with ribs (on the right). .................................................. 16 Fig 1.9 PPF-04 Uni-link robot fish design by NMRI, Japan ....................................................... 16 Fig 1.10 Robotic Fish Chinois SPC-03 developed in BUAA-CASIA China for underwater exploration...................................................................................................................... 17 Fig 1.11 Robotic Eel Angulliform Fish (Robea Project) developed in CNRS France ................. 17 Fig 1.12 BoxyBot Fish developed in BIRG - EPFL (Suiss) ........................................................... 18 Fig 1.13 a) Essex G8 Robotic Fish diving mode in water b) internal structure of the fish, servo locations and water proofing ................................................................................ 18 Fig 1.14 Carp Koi Robotic Fish by Ryomei Engineering - Japan ................................................ 19 Fig 1.15 Jessiko V4 Robotic Fish developed in France by RobotSwim ..................................... 19 Fig 1.16 RoboFish developed by University of Washington U.S.A with only tail joints. .......... 20 Fig 1.17 a) StingRay, b) KnifeFish developed in NTU, Singapore using undulating fin mechanism and c) Arowana Robot, a BCF swimming mode with 2 links....................... 20 Fig 1.18 Tai Robot Kun Robotic Fish with unique exoskeleton design designed in University of Kitakyushu, Japan ....................................................................................................... 21
iv
Fig 1.19 Polymer Actuated Robotic Fish propelled by Conducting Polymer Trilayer Polypyrrole (PPy). ........................................................................................................... 21 Fig 1.20 Isometric, frontal, and top design views of fin system Servos push the ‘push rods’ to controllably bend the ribs (Left) and Rib bending curvature and tip deflection analysis and Rib FEA analysis to design for rib damage prevention (Right) ................... 22 Fig 2.1 DH parameters a
,
,
,
defined for joint i and link (i) . ...................................... 24
Fig 2.2 Multi-link Robotic Fish Configuration with system parameters and joint variables .... 28 Fig 2.3 Forces acting on a swimming fish with ve locity U. Coordinate system used in thesis is also shown ................................................................................................................... 36 Fig 3.1 SolidWorks Model of the Carangiform Fish (BCF Swimming Motion) ......................... 44 Fig 3.2 Individual Assemblies of a) Head with HS 48 5HB Motor, coaxial holder, batteries and HS 5085MG Motors, b) Link 1 with HS 5085MG motor and motor couplings, c) Link 2, d) Right Pectoral Fin Assembly with pectoral shaft and holder, e) Left Pectoral Fin Assembly with pectoral shaft and holder, and f) Caudal Fin Assembly with motor coupling ................................................................................................ 46 Fig 3.3 Individual Parts designed in SolidWorks which are assembled in the different assemblies shown in Fig 3.2............................................................................................ 47 Fig 3.5 Successful translation of CAD Assembly to Physical Modelling XML file from SolidWorks. ..................................................................................................................... 50 Fig 3.4 Exporting CAD Model from SolidWorks into a SimMechanics XML file ....................... 50 Fig 3.6 Complete translation of CAD Assembly into Visualizable model ................................. 50 Fig 3.7 Importing a Physical Modelling XML file into a model. ................................................ 51 Fig 3.8 Import Physical Modelling dialog box option to load the XML model. ........................ 52 Fig 3.9 SimMechanics Subsystem of the imported Physical Model with Actuators and Sensors ............................................................................................................................ 53 Fig 3.10 SimMechanics animation of the physical model as Ellipsoids ................................... 55 Fig 3.11 SimMechanics animation of the physical model as External Animation File (STL) .... 55 Fig 3.12 Virtual Reality Export settings from SolidWorks ........................................................ 56 Fig 3.13.b) VR World created in VR Realm Builder with transforms for each link .................. 57
v
Fig 3.13.a) VR World imported into VR Realm Builder from SolidWorks for each link .......... 57 Fig 3.14.b) VR World created in VR Realm Builder with transforms for each link with inlined url files ................................................................................................................ 57 Fig 3.14.a) VR World created in VR Realm Builder from imported SolidWorks inlined wrl file ................................................................................................................................... 57 Fig 3.15 Body Sensor SimMechanics block added to the Links. Position (x,y,z) and Rotation Matrix [3x3] are enabled to measure the rotation parameters..................................... 59 Fig 3.16 VR Sink Block Parameters dialog box ......................................................................... 60 Fig 3.17 Subsystem connecting Body Sensor block from SimMechanics system to VR Block . 61 Fig 3.18 Virtual Reality Subsystem with VR Sink Block ............................................................ 62 Fig 3.19 Virtual Reality Simulation in Simulink using VR Sink Block. ....................................... 62 Fig 4.1 Simulink block diagram of the whole system with SimMechanics and VR subsystems...................................................................................................................... 63 Fig 4.2 General Block Diagram of the Computed Torque Control Method using the desired joint angles, velocities and accelerations generated using the Trajectory Generator. . 64 Fig 4.3 Functional Block Diagram of the RoboFish Control System using the control method shown in Fig 4.2. ............................................................................................... 64 Fig 4.4 Joint theta values obtained from the Open Loop SimMechanics System.................... 65
vi
LIST OF TABLES Table 1.1. Optimal Swimming Parameters derived by Barett [5] …………………………………………. 13 Table 2.1 Denavit-Hartenberg Parameters for the 4 Link RoboFish with 2 identical Pectoral Fins considering the head is fixed. ………………………………………….…………………………………………………..24 Table 3.1 Custom Visualization of the designed Solidworks Model ……………………………………… 43 Table 3.2 Design Specifications of the Solidworks Model ……………………………………………………. 44 Table 4.1 Optimal values of Oscillating Frequency and Oscillating Amplitude of each of the links and pectoral fins ………………………………………………………………………………………………………….64
vii
viii
ABSTRACT Underwater robots are widely used in the fields of ocean development, ocean investigation, and marine environmental protection. The need for higher efficiency and improved propulsion mechanism for underwater robots essentially requires fish-like movement. The design of energy efficient biomimetic fish like propulsion system is desirable. Fish exhibits a variety of swimming movements, which employ their fin(s) and/or body to produce propulsive and manoeuvring force. One of the main objectives o f the AUV’s (Autonomous Underwater Vehicle) research is intended to investigate various swimming modes: anguilliform, subcarangiform, rajiform, amiiform, and gymnotiform, which might suit biomimetic underwater robot application using undulatory fins as mai n thrust-generator. Proper material selection is inevitable in the design of the flexible membrane. Material density, strength, and water absorption capability are those important criteria for selection. The material is required to have a density near that of water so that no or little work is done to lift and orientate the membrane. The scope of this project is to focus mainly on the biomimetic design of multi mode robotic fish and flexible fins with a specially designed modular fin mechanism. Investigation on the manoeuvrability and control aspect of a multi mode biomimetic fish robot with modular mechanical fin and undulating fins are required.
ix
CHAPTER 1 LITERATURE REVIEW AND PAST WORKS There is currently an increased interest in the use of long range/long duration Autonomous Undersea Vehicles (A.U.V.'s) for oceanographic observation, military surveillance and commercial search missions. Existing A.U.V.'s are relatively small vehicles for three reasons; low cost (fully autonomous vehicles have a significant probability of being lost), ease of deployment (to allow operations from conventional ships), and safety (to minimize the danger to manned ships and installations). They are powered by small rotary propellers driven by electric motors. The propellers typically operate at fairly low efficiencies and suffer from serious lag times in transient response. The space required for the batteries often approaches 70% of the hull volume. These problems lead to short mission times, restricted payloads, and control problems. Consider the fish: highly manoeuvrable and an effortless swimmer, this animal 160 million years in the making is superbly adapted to its watery environment.
1.1
FISH SWIMMING MODES: In the past decades, both scientists and engineers have devoted themselves to
overcome the drawbacks of the conventional vehicles, and these attempts include multiscrew, several rudders, biology-inspired propulsor, and so on. Among them, biology-inspired approaches are recently harder for practical applications, but they are expected to be comparatively effective and feasible. As matters of energy economy and greater locomotion performance are desired in engineered systems, imaginative solutions from nature may serve as the inspiration for new technologies. Several physicomechanical designs in fish evolution have recently inspired robotic devices for propulsion and manoeuvring purposes in underwater vehicles. It is obvious that the potential benefits from biological innovations can be applied to systems operating in water with a high speed, reduced detection, energy economy, and enhanced manoeuvrability. Certainly, various fishes may possess different functions and provide distinct enlightenments. Therefore, it is helpful for us to be conscious of fish’s classification and functions [1].
According to P. W. Webb [2], aquatic locomotion can be classified, in terms of propulsors, into two styles: BCF (body and/or caudal fin) and MPF (median and/or paired fin). Hereinto, BCF locomotion prevails in high speed and superb accelerating ability, while MPF locomotion has a great potential to achieve better manoeuvrability as well as higher efficiency than BCF locomotion.
1.2
LIGHTHILL’S WORKS:
James Lighthill Memorial Paper [3] bears only upon part of the many activities of Sir James Lighthill, namely upon the mathematical theory of the swimming of fish and cetaceans. It is well-known that Sir M.J. Lighthill considered many aspects of fluid mechanics, as follows
10
from his collected papers which are covered by four volumes [4]. Besides his mathematical approach to fish locomotion Lighthill went, as he says himself with the help of colleagues in zoology, deeply into the biological background of the different phenomena that he investigated. Along with Lighthill, Wu must also be mentioned as the author of a number of papers which contribute to the mathematical foundation of the subject.
Fig 1.1. Different swimming modes of natural fish locomotion a) BCF (Body Caudal Fin) and b) MPF (Median Pectoral Fin). Shaded areas contribute to thrust generation.
1.3
SIMPLIFIED PROPULSIVE MODEL FOR CARANGIFORM PROPULSION: It is common in literature [1 + that the passage of a wave underlies the fish’s propulsive
structure. The propulsive wave behaves as lateral curvature in spine and musculature, which begins posterior of head and increases with amplitude as it moves posteriorly. It traverses at a speed greater than the forward speed. A typical swimming mode for carangiform has been presented by Barrett et al. [5] which are composed of two basic components; the fish body represented by a planar spline curve and its lunate caudal tail by an oscillating foil. The spline curve starts from the fish's center of inertia to the caudal joint, which is assumed to take the form of a travelling wave originally suggested by Lighthill.
−− ,
where
1
+
2
2
sin
+
is the transverse displacement of body,
is the body wave number envelope,
=
2
=
2
is the displacement along main axis,
, λ is the body wave length,
is the quadratic wave amplitude envelope,
11
(1.1)
1
is the linear wave amplitude
is the body wave frequency
(
=2
=2 / )
−−
(1.2)
rd
Fig 1.2. Physical Model of Carangiform Swimming showing the undulation motion in 2/3 part of posterior body.
Once given this body-wave, the task of the following analysis is to determine the proper body-wave constants (i.e.,
1, 2,
,
etc.) for a desired swimming motion. In literature [6], a
set of seven key parameters for the kinematic model of RoboTuna's swimming was captured and a genetic algorithm was used to guide the search for an optimal swimming efficiency. Clearly, for various species, dimensions and shapes, there are different parameter sets. Hence, it is a tough task to optimize the fish's swimming efficiency and manoeuvrability in general. For simplicity, a discrete planar spline curve is considered i.e. oscillating frequency f is separated from the body wave function, y_body (x,t). In other words, the travelling body wave is decomposed into two parts: the time independent spline curve sequence y_body (x,i)(i=0,1,2…M-1) in an oscillation cycle, which is described by the Eqn (1.3) and the time
dependent oscillation frequency f [6].
…− ,
where
=
1
+
2
2
sin
+
2
is the variable of spline curve sequence,
= 0,1
1
−−
(1.3)
is the body wave resolution that
represents the discrete degree of the overall travelling wave, which is restricted by the maximum oscillation frequency of servomotors (here, we assume that joints are actuated by servomotors).
12
Fig 1.3. Link based body wave fitting. Notice that a 4-link mechanism is used as the model of body wave fitting.
The caudal fin motion is extremely important in fish propulsion, as it has been analyzed that most of the thrust is produced by it. The angular motion of the caudal fin has a special phase relationship with the body motion [7] described by
=
0
sin
+
−−
(1.4)
David Barrett derived self-propelled optimal values for “RoboTuna” at MIT *5 ] for the kinematic variables in the above equations. The values are listed in the Table 1.1 below.
Fig 1.4. General Carangiform Swimming Motion. LightHill equation and Tail Phase relationship.
After Lighthill’s seminal paper ‘Note on the swimming of slender fish’ *8 ] and the
fundamental paper of Wu ‘Swimming of a waving plate’ *9 ], many other mathematically oriented articles using reactive theories followed. This led to hydrodynamics getting a strong foothold in research on the swimming of fish.
13
1.4
GRAY’S PARADOX:
There is a famous paradox formulated by J. Gray in his article ‘Studies in animal
locomotion VI, The propulsion power of the dolphin’. Gray estimated among other things the power needed for a dolphin of length 1·82 m to swim at a speed of 10·1 m/s. This he did by calculating the dolphin’s resistance by means of a drag coefficient based on a turbulent boundary layer. He found that the required power was possibly about seven times the estimated muscular power available for propulsion. This yields the paradox which is considered by a large number of investigators. The paradox is, however, rather difficult to tackle because of the lack of a common opinion among investigators on the influence of the swimming motion on the resistance of the body. Some opinions are that the resistance of the swimming body can be increased by a factor of three with respect to the resistance of the body when it glides motionless through the water; see for instance M.J. Lighthill’s ‘Large -amplitude elongated-body theory of fish locomotion’ [10].
Variable
Description
Value
U
Swimming Speed (Body Lengths /
0.65
Second) St
Strouhal Number:
0.156
Peak-to-Peak Amplitude of Caudal Fin Tip
0.115
α
Maximum Angle of Attack of Caudal Fin
16.2°
Ф
Phase of Caudal Fin Motion with respect
75 - 95°
λ
Propulsive Wavelength (Body Lengths)
1.27
1
Coefficient of Linear Term
0.00372
2
Coefficient of Quadratic Term
0.002
Table 1.1. Optimal Swimming Parameters derived by Barett [5]
1.5
PAST PROJECTS:
1.5.1
ROBOTUNA I, II AND CHARLIE I (MIT):
The RoboTuna project [11] was started in 1993 with the objective to develop a better system of propulsion for the autonomous underwater vehicles. The tuna was selected as model for its speed (a blue fin tuna can go up to 74 km/h) and its accelerations. It is a question of understanding how a fish can generate enough energy to reach such speeds. This one includes 8 vertebrae and a system of cables which is used as tendons and muscles. The envelope is made up of a fine and flexible layer of foam covered with Lycra to approach the flexibility and smoothness of the tuna skin. RoboTuna was suspended by a mast which is fixed
14
at a system which slides along the tank allowing it to rotate and pulled out of the water for robot maintenance and storage. The mast is also used to pass the cables which connect the robot to the controllers. Thus, the controllers receive information from the sensors in entry and return instructions to RoboTuna. The major structural component of the robot fish is a segmented backbone made up from 8 discrete rigid vertebras which are driven through an elaborate system of pulleys and cable tendons by 6 brushless DC servo motors. These tendon drives are the mechanical analogy of the biological fish's muscles.
Fig 1.6 a) RoboTuna II built in MIT 1994 with lever, pulley and ball bearing mechanisms and b) Improved Rib Cage design for RoboTuna II (MIT) for reduced drag and friction.
1.5.2
ROBOPIKE (MIT) 1998:
Fig 1.7 RoboPike (MIT) 1998 [12] concept gets its birth in the design board.
15
Fig 1.8 RoboPike (MIT) 1998 swimming in the MIT Towing Tank (on the left) and internal wooden structure of the links with ribs (on the right).
1.5.3
PPF-04 (NMRI):
The NMRI (National Maritime Research Institute) developed many projects [13] of robotic fish (series PF and series PPF) with a view to apply, in the future, the capacities of fish to our boats and submarines. The PPF-04 is one small robotic fish of 19 cm and 400 g, remote controlled. Its size makes it possible to test it in a small tank (like a bath-tub). The study carried, inter alia, on the relation between the speed and the amplitude of the oscillations of the caudal fin.
Fig 1.9 PPF-04 Uni-link robot fish design by NMRI, Japan
1.5.4
ROBOTIC FISH SPC-03 BUAA-CASIA (CHINA):
The SPC-03 measures 1.23m length and is stable, very handy, and is controlled remotely by technicians. It can work 2 to 3 hours in immersion, at the maximum speed of 4 km/h. This robotic fish is intended for underwater archaeological exploration but the two persons in charge for the project, Wang Tianmiao (BUAA) and Tan Min (CASE), consider many other uses such as underwater photography, cartography of the underwater flora and fauna, transport of small object, etc,.
16
Fig 1.10 Robotic Fish Chinois SPC-03 developed in BUAA-CASIA China for underwater exploration
Result of several years of research, the robot was tested in August 2004 on the site of a marooned warship. The robotic fish explored a surface of more than 4000 sq.m over 6 hours of immersion. It took many photographs and transmitted to the surface. 1.5.5
ROBOTIC EEL - ROBEA PROJECT CNRS (FRANCE):
The objective of the ROBEA-Eel project [14] was to "design, study and produce a robotic eel able to swim in three dimensions". Certain fish as tuna have a mode of locomotion based on oscillations of the body, whereas the locomotion of anguilliform fish (eel, lamprey...) is based on undulations of the body. Thus, the swimming of eel presents remarkable performances in term of manoeuvrability. It is the high number of internal degrees of freedom of this fish which enables it to thread in the most difficult places of access. The prototype of project ROBEA [14] consists of a stacking of platforms of the kneecap type, imitating the vertebrae of eel. LAG, Laboratoire of automatic of Grenoble, set up the control systems of the movements of the eel (orientation, speed) as well as stabilization in rolling of the robot.
Fig 1.11 Robotic Eel Angulliform Fish (Robea Project) developed in CNRS France
17
1.5.6
BOXYBOT BIRG-EPFL (SUISS):
Fig 1.12 BoxyBot Fish developed in BIRG - EPFL (Suiss)
Developed with the BIRG (Biologically Inspired Robotics Group), the BoxyBot project [15] aimed at the realization of an autonomous robot of various forms and those which uses the fins, like the labriform type and ostraciiform. These fish have a rigid body and a low speed but a great manoeuvrability thanks to their fins. In fish of the labriform type, the pectoral fins are used for the propulsion and the caudal fin is used as rudder. BoxyBot is 25 cm long and can swim up to 0.37 m/s. It can plunge, swim ahead, behind, on the side and carry out gimlets. The speed depends on the amplitude and the frequency of the oscillations on the fins and also size and rigidity of those. 1.5.7
ESSEX ROBOTIC FISH (GREAT BRITAIN):
A fish has various modes of displacement (speed, turns, accelerations and braking) and the challenge of the researchers of Essex was to obtain an autonomous robot-fish G8 [16] which can reproduce all these behaviours and not in one or two instances but in a more or less uniform way. They thus indexed the various behaviours in a library used by the computer to generate varied and unexpected trajectories of stroke. Robotic Fish [16] (50 cm length) is able to curve its body according to a great angle in a much reduced time (approx 90°/0.20sec).
Fig 1.13 a) Essex G8 Robotic Fish diving mode in water b) internal structure of the fish, servo locations and water proofing
Several models were designed, since G1 in 2003 until G8 [16] and G9 in 2005. The researchers continue to work on the improvement of the algorithms of training which make it
18
possible for the robot to generate adaptive behaviours in a changing environment and thus remain unpredictable. 1.5.8
ROBOTIC KOI - RYOMEI ENGINEERING – JAPAN:
Fig 1.14 Carp Koi Robotic Fish by Ryomei Engineering - Japan
A robot-fish inspired by the “carp koi” *17 ] was presented in March 2006 in Japan. It was developed by three companies of which Ryomei Engineering, a subsidiary company of Mitsubishi Heavy Industries, which are already at the origin of the series "Mitsubishi Animatronics", was a key player. The robot, which measures 80 cm and weighs 12 kg, was remote controlled. Its mouth was equipped with sensors being used to control the oxygen concentration in water, information essential if one wants to supervise the health of fish. In a second step, the researchers want to make their robot autonomous. Thanks to its camera, the robot could be sent in recognition to examine the resources present in the depths. It could be also used to inspect the oil platforms to locate and supervise possible damage. 1.5.9
JESSIKO - ROBOTSWIM - FRANCE:
Fig 1.15 Jessiko V4 Robotic Fish developed in France by RobotSwim
Jessiko [18] is one of the smallest robotic fish in the world (20cm/100g). Thanks to its communication potential and its artificial intelligence, Jessiko [18] can swim in a school of 10
19
fish or more, so that they make attractive aquatic and luminous choreographies. At first, it is designed for the events market and aquatic museums. After, it was announced to mass market as a kit to enliven pools during long summer nights. 1.5.10
ROBOFISH - UNIVERSITY OF WASHINGTON - U.S.A.:
Fig 1.16 RoboFish developed by University of Washington U.S.A with only tail joints.
The "Robofish" of the University of Washington [19] measures a half meter long and weighs 3kg. It is highly manoeuvrable and can swim backwards by inverting pectoral fins. Since radio signals travel badly in salt water, a system that allows robots to communicate was studied through this project. During the experiment [20], 3 Robofish broadcasted their headings to each other, and used any information received to adjust their own courses. According to Kristi Morgansen, the group remained coordinated despite about half of all information packets being lost - showing that the system was relatively robust. With the same technique, it will be possible to explore large areas, track a pollution spill, or to report the location of marine animals. 1.5.11
STINGRAY – KNIFEFISH – NANYANG UNIVERSITY – SINGAPORE:
Fig 1.17 a) StingRay, b) KnifeFish developed in NTU, Singapore using undulating fin mechanism and c) Arowana Robot, a BCF swimming mode with 2 links.
Researchers of the School of Mechanical and Aerospace Engineering from Nanyang Technological University study fish propulsion. Their objective is to design and optimize robotic fish using undulating fin mechanisms. Thus, for experiments they designed different types of robotic fish like a Stingray Robot, a Knifefish Robot, an Arowana Robot and more [21].
20
1.5.12
TAI-ROBOT-KUN - UNIVERSITY OF KITAKYUSHU - JAPAN:
Engineers at the University of Kitakyushu have developed one of the most realistic biomimetic robots in the world. This red snapper is actually a robotic fish known as “Tai -robotkun” *22]. Tai-robot-kun weighs 7kg and mimics a real fish swimming silently in the water, and
can go for as long as an hour with a full battery. It has a silicone body covered in realistically hand-painted scales, features a unique propulsion system that allows it to move its tail and drift silently through the water like a real fish.
Fig 1.18 Tai Robot Kun Robotic Fish with unique exoskeleton design designed in University of Kitakyushu, Japan
1.5.13
POLYMER ACTUATED (PPy) ROBOTIC FISH:
Conducting polymer (CP) materials exhibit significant volume change in response to electrical stimulation. The robot is propelled by a trilayer polypyrrole (PPy) polymer actuator. Experiments were conducted to characterize the properties of PPy polymer.
Fig 1.19 Polymer Actuated Robotic Fish propelled by Conducting Polymer Trilayer Polypyrrole (PPy).
Different configurations of actuators were investigated and justified using experimental results [23]. The robotic fish embeds a microcontroller, a Lithium coin cell battery, and necessary circuitry for navigation and control. It cruises using the actuated tail fin. Waterproofing packaging was designed to protect the electronics. This project has
21
successfully demonstrated that PPy polymers can be used to design robotic fish actuators. A self-contained prototype is demonstrated with 10~12 ho urs operation lifetime. 1.5.14
BIOMIMETIC CONTROLLED CURVATURE ROBOTIC PECTORAL FIN:
Fig 1.20 Isomet ric, frontal, and top design views of fin system Servos push the ‘push r ods’ to controllably bend the ribs (Left) and Rib bending curvature and tip deflection analysis and Rib FEA analysis to design for rib dam age prevention (Right)
The pectoral fin is a very complex propulsor, with many shape-changing intricacies. It was found that an actively controlled deforming fin curvature, especially the leading-edge curvature as identified in the bird wrasse, contributes significantly to propulsive ability. To ignore the importance of a controlled fin curvature will only result in designs with reduced operational performance. Only a few designs with flapping pectoral fin with an actively controlled quantitatively specified curvature time-history has been developed. In-depth analysis of the common bird wrasse pectoral fin, fin kinematics, fluid dynamics, and anatomy was done and a parametric study was given to aid in future pectoral fin design. The design and rationale of the physically constructed device that can produce the required fin kinematics was provided.
22
CHAPTER 2 SYSTEM MODELLING
2.1 MODELLING TECHNIQUES: The dynamics of any rigid body can be completely described by the translation of the centroid and the rotation of the body about its centroid. The dynamics equations for the four link system described in Section 3.1 are derived by defining the inertia torques of each link and the reaction torques from the connecting links. This leads to the ability to derive the actuator torques necessary to produce the tail motion that is desired. The links are interdependent in two major respects: 1)
The torque produced on or by a link produces a reaction torque on the other links.
2)
The motion of the links changes the shape of the linkage, which changes the inertia seen by previous links (links closer to the bridge). Therefore, the result for the four link system is a rather complicated interdependent set
of dynamics equations. There are two basic approaches to deriving the dynamics equations, the Newton-Euler and the Lagrangian approach, each having its advantages and disadvantages listed below: METHOD
ADVANTAGES
DISADVANTAGES
Newton-Euler
Uses intuitive concepts of torque
Resultant set of equations
balance and free-body diagram.
not in “closed form”.
Resultant set of equations are in
Energy method approach
“closed form”.
not as intuitive as Newton-
Lagrangian
Euler.
In the Newton-Euler formulation, a “free -body” approach is taken in the classical dynamics sense where the linkage is conceptually disassembled and torques are balanced about each joint pivot, taking into account the torques caused by inertia, centrifugal and Coriolis forces. External forces such as reactions from other joints, gravity and actuator forces are also accounted for. The resultant intermediate equations are not in useful form, however, as they do not involve the actuator torques explicitly, but instead contain the forces of constraint between joints. These must be eliminated by back-substitution to arrive at the useful form called the “closed form” dynamics equations.
Due to the shortcomings in this case for the Newton-Euler approach, the Lagrangian approach was used according to [7]. The Lagrangian approach to dynamics involves energy methods namely; the system is first described by the work and energy stored therein. Deriving
23
the work and energy of the system as a whole is a relatively simple undertaking and although the dynamics equations are still complicated, the closed form dynamics are quickly produced without having to back-substitute to eliminate the constraint forces. Fig 2.1 is a schematic of the RoboFish, showing the relative angular displacement between each link, q i and the kinematic length of each link, l i.
2.2
LAGRANGIAN MODELLING:
2.2.1
DENAVIT HARTENBERG NOTATION:
Every joint is indicated by its axis, which may be translational or rotational. To relate the kinematic information of robot components, a local coordinate frame B is rigidly attached to each link (i) at joint (i + 1) based on the following standard method , known as DenavitHartenberg (DH) method .
Fig 2.1 DH parameters a
,
,
,
− ∗
defined for joint i and link (i) .
− − −
The zi axis is aligned with the (i + 1) joint axis. The x i -axis is defined along the common normal between the
1
and
axes, pointing from the
determined by the right-hand rule,
=
.
1
A DH coordinate frame is identified by four parameters: 1)
Link length
is the distance between
kinematic length of link (i) . 2)
Link twist
1
is the required rotation of the
parallel to the
-axis.
24
and
1
,
to the
-axis. The
,
axes along the
-axis about the
-axis is
.
-axis.
is the
axis to become
3)
Joint distance
, is the distance between
distance is also called link offset. 4)
Joint angle to the
is the required rotation of
-axis.
−
− 1
1 -axis
and
axes along the
about the
−
1 -axis
−
1 -axis.
Joint
to become parallel
The Denavit-Hartenberg parameters of the RoboFish system are tabulated in Table 2.1. a
α
d
θ
Link 1
li
0
0
qi
Link 2
l2
0
0
q2
Link 3
l3
0
0
q3
Caudal
l4
0
0
q4
Left Pectoral
l5
0
q5
Right Pectoral
l6
0
q6
2
−
2
Table 2.1 Denavit-Hartenberg Parameters for the 4 Link RoboFish with 2 identical Pectoral Fins considering the head is fixed.
2.2.2
REFERENCE FRAMES:
F G
Inertial Ground fixed frame of the robotic fish system.
F B
Base frame located at the head center of mass of the robotic fish.
F 0
Zeroth coordinate frame fixed at the head.
F 1
Coordinate frame of the first link.
F i
Coordinate frame of the i link.
2.2.3
MATRICES AND VECTORS:
H(I,j)
Homogeneous transformation from the j frame to the I frame.
H(I,B)
Homogeneous transformation from the B frame to the I frame.
H(B,0)
Homogeneous transformation from the Zeroth frame to the B frame.
H(0,1)
Homogeneous transformation from the first frame to the zeroth frame.
H(0,i)
Homogeneous transformation from the i frame to the zeroth frame.
th
th
th
th
th
th
th
25
T 6
Desired end-effector position or transformation matrix from the sixth frame to the zeroth frame.
JI
Pseudo inertia matrix of link i.
JB
Pseudo inertia matrix of the base.
M
Manipulator inertia matrix.
V
Coriolis-Centripetal matrix.
G
Gravity matrix.
F
Disturbance term matrix.
P
Position of frame F B relative to and projected onto frame F I.
Φ
Rotation of base in frame F I.
r B
Position of frame F 0 relative to and projected onto frame F B.
r I
Position of frame F 0 relative to and projected onto frame F 0.
d i
Position of frame F i relative to and projected onto frame F 0.
r i
Position of point on link i relative to and projected onto frame F 0.
ρi
Position of point on link i relative to and projected onto frame
vi
Velocity of point on link i relative to and projected onto frame F I .
b
Position of point on the base relative to and projected onto frame F B.
b B
Position of point on the base relative to and projected onto frame F I .
v B
Velocity of point on the base relative to and projected onto frame F I .
qi
Generalised coordinates for the links.
V j
Maximum joint velocities.
τ j
Maximum joint torques.
K i
Kinetic energy of link i .
Pi
Potential energy of the link i .
li
Length of link i .
26
Fi.
2.2.4
EQUATIONS OF MOTION:
The Lagrange equation of motion provides a systematic approach to obtain the dynamics equations for robots. The Lagrangian is defined as the difference between the kinematic and potential energies
−
−−
=
(2.1)
The Lagrangian equation of motion for a robotic system can be found by applying the Lagrange equation
− … −− =
where
= 1,2,
,
−−
is the coordinates by which the energies are expresses, and
generalized non potential force that drives
.
(2.2)
is the corresponding
The equations of motion for an n link serial manipulator can be set in a matrix form
+
,
+
=
(2.3)
or in a summation form
( )
+
=1
+
=1
=
=1
( ) is an n x n inertial type symmetric matrix
n
=
+
i=1
1 2
is the velocity coupling vector or Coriolis and Centrifugal force vector
− 1
=
and
2
=1 =1
is the gravitational vector
()
=
=1
The generalized forces of the Lagrange equations are
where
=
th
is the i actuator force at joint i, and
end-effector.
27
+
is the external force system applied on the
2.2.5
ROBOFISH AND HEAD 3D CONFIGURATION:
The configuration of the robotic fish and non-fixed head is as shown in the Fig 2.1. In 3 dimensions, the head has 6 degrees of freedom and the robot body has n degrees of freedom, one for each link. This gives the system 6+n degrees of freedom or one degree of freedom for each generalized coordinate. From Figure 2.1, the frames, matrices and vectors are defined. Homogeneous transformation matrices
(,)
are 4 x 4 matrices which map position
vectors expressed in homogeneous coordinates from one coordinate frame to another. They
consist of a rotation submatrix and a position vector, along with scaling and perspective
information. The generalized coordinate for each link will be denoted as or prismatic
is represented by either
or
. If a link is revolute
respectively. The homogeneous matrix for a
link will reflect whether the link is either translational or rotational. The translational coordinates of the head are defined as
ФФФ
along X, Y and Z axes in the
{
1,
2,
3 }.
Ф
= ( 1 , 2 , 3 , 1) . This corresponds to translations = frame. The rotational coordinates are defined as
These form an Euler angle system, known as a 3-2-3 system, which correspond
to the following current frame rotational sequence.
Fig 2.2 Multi-link Robotic Fish Configuration with system parameters and joint variables
28
2.2.6
KINETIC AND POTENTIAL ENERGIES:
Referring to the Figure 2.1, the position of a point on link i relative to and projected onto frame
is
… − =
( , )
( ,0)
(0,1)
(1,2)
(
1, )
or simplifying,
=
( , )
( ,0)
(0, )
+
,
+
,
The velocity of a point on link i is then
=
or performing the differentiation, ,
=
,0
0,
0,
,
,0
,0
+
0,
,0
0,
It should be noted that all the homogeneous matrices are functions of joint coordinates which are time dependant (eg.
). Assuming the links are rigid we obtain
,
=0
The head is also assumed rigid [24] so it can be written as ,0
Since
,
=0
is a function of the six base generalized coordinates
, and
th
0,
the i link generalized coordinate, we write the above equation compactly as 6
=
,
=
,0
0,
=1
+
,
0,
,0
=1
Knowing that the velocity squared of a vector is
2
=
we can write the kinetic energy of a particle of mass dm on link i at
=
1 2
29
as
is a function of
−− 6
=
1
0,
,
,0
2
0,
,0
+
,
=1 =1
6
1 2
0,
,
,0
0,
,0
0,
,0
,
+
,0
,
+
=1 =1 6
1
0,
,
2
=1 =1
6
1 2
0,
,
,0
0,
,0
,
(2.4)
=1 =1
The total kinetic energy of the links is given by integrating Eqn 2.4 over the mass for each link and then summing as follows
=
=
=1
=1
We also know that the integral of the term matrix
over link i is termed the pseudo inertia
=
For a 3-D rigid body the pseudo inertia matrix can be written as
where
, the mass moment of inertia is written as
2
=
+
2
combining terms and using the fact that
=
(
), the total kinetic energy of
the manipulator can be obtained as 6
,
=
0,
,0
=1 =1 =1
30
0,
,0
,
+
−− 6
6
,
1
,
,0
2
0,
0,
+
,0
=1 =1 =1
0,
1
,
2
0,
,0
,0
,
(2.5)
=1 =1 =1
The potential energy of link i is written as
− =
( , )
( ,0)
(0, )
If the head frame is at the center of mass of the head an equivalent simpler form of the head Kinetic Energy can be written as
=
1
+
2
1 2
Thus the total kinetic energy of the robotic fish system is 6
=
+
,
=
0,
,0
0,
,0
+
,
=1 =1 =1
6
6
,
1
,
,0
2
0,
0,
+
,0
=1 =1 =1
0,
1
,
2
0,
,0
,0
+
,
=1 =1 =1
6
6
1
( , )
( , )
2
=1 =1
Substituting K and P in Lagrange equation stated above where and
is the generalized coordinate
is the generalized force or torque at joint i . The Lagrangian becomes 6
,
=
0,
,0
0,
,0
,
+
=1 =1 =1
6
6
1
,
,
,0
2
0,
0,
,0
+
,
,
,
+
=1 =1 =1
0,
1 2
,
,0
0,
,0
=1 =1 =1
31
+
−− −− ( , )
( ,0)
+
(0, )
(2.6)
( , )
=1
For the link dynamic equations, the link generalized coordinates be represented as the derivative of L with respect to
. Taking
,
6
( , )
=
=
(0, )
(0, )
,0
,
+
=1
,
=
( ,0)
0,
0,
0,
,0
(2.7)
,
=1
=
Combining terms 2 and 3 because of similar indices and also because For p > i,
(0, )
=0
Taking the time derivative of the Eqn 2.7
∂∂ ∂ ∂ ∂∂ ∂∂ ∂∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂∂ ∂ ∂ ∂ ∂∂ ∂ ∂ ∂ ∂ ∂∂ ∂∂ ∂ ∂ ∂∂ ∂∂
d
n
L
dt
=
qi
tr
6
6
2
tr
i=p s=1 t=1 6
i
tr
i=p s=1 m=1 6
n
H
i
tr
i=p s=1 m=1 6
n
6
tr
i=p s=1 t=1 n
H(G,B)
zt zs
H(G,B) zs
H(G,B) zs
H(I,B) zs
G,B
zs
i=p s=1
n
n
6
H(B,0)
tr
i=p k=1 s=1
H(G,B) zs
H(0,i) qm
H(B,0)H(0,i)Ji
H(B,0)H(0,i)Ji
i
6
0,i
H(B,0)
H(0,i) qp
qp
32
qp
qp
T H(0,i)
Ji
qp
T H(0,i)
qm qp T H(0,i)
qp
T H(0,i)
Ji
H(0,i)
Ji
T H(0,i)
2
i=p k=1 i
H
B,0
H(B,0) H(0,i)Ji
tr H(G,B) H(B,0)
n
H
T H(0,i)
qk
Ji
T T H(B,0) H(G,B) zs +
T T H(B,0) H(G,B) zs zt +
T T H(B,0) H(G,B) zs q m +
T T H(B,0) H(G,B) zs qm +
T H(B,0)
T H(G,B)
zt
zs zt +
T T H(B,0) H(G,B) qk +
T H(0,i)
q k
T T H(B,0) H(G,B) qk zs +
(
).
∂ ∂ ∂ ∂ ∂ ∂∂ ∂∂ ∂ ∂∂ ∂∂ − − n
i
i
2
tr H(I,B)H(B,0)
i
i
n
6
i
H(0,i)
tr H(I,B)H(B,0)
qp
i=p k=1 s=1
Ji
qp
i=p k=1 m =1
T H(0,i)
qm q k
T H(0,i)
Ji
q k
2
H(0,i)
tr H(I,B)H(B,0)
T H(0,i)
Ji
qm qp
i=p k=1 m =1 n
H(0,i)
q k
T T H(B,0) H(G,B) q k k q m +
T T H(B,0) H(G,B) q k k q m +
T T H(B,0) H(G,B) q k k zs
(2.8)
For the second term in the Lagrangian equation for the links,
∂∂ ∂ ∂ ∂∂ ∂∂ ∂ ∂ ∂∂ ∂ ∂ ∂ ∂∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂∂ ∂ ∂∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ −− 6
n
L
i
=
pi
H
tr
zs
i=1 s=1 j=1 6
n
i
H
tr
6
6
1
i=1 s=1 t=1 6
n
6
1
i=1 s=1 t=1 n
i
i
i
2
1
i=1 j=1 k=1
n
2
i
2
1
i=1 j=1 k=1
2
H
tr
H
I,B
H
zs
H
tr
H
H
B,0
I,B
H
zs
tr H
I,B
H
I,B
H
B,0
B,0
B,0
G,B
H
i=1
0,i
H
0,i
H
Ji
qj qp
B,0
0,i
qj
H
G,0
qp
qj
H
0,i
H TB,0 HTG,B
H TB,0 H TG,B
qp qj
H T0,i
Ji
0,i
H T0,i
Ji HT0,i HTB,0
qp
HT
B,0
Ji
0,i
H
B,0
n
mi g i H
qp
H
2
tr H
0,i
2
I,B
zs
i=1 s=1 j=1
n
I,B
HTB,0
qj
HT0,i
Ji
q k
2
Ji
zs q j +
zs q j
HTG,B
zs zt t +
zt
HTG,B
zs zt t +
zt
H TB,0 H TG,B k + G, B q j q k
HT0,i
q k q p
T H TB,0 B, 0 H G,B
ri
q j q k k +
(2.9)
Forming the Lagrange equation with Eqns 2.8 and 2.9, after many cancellations and index substitutions, the dynamic equations for the robotic fish system with multiple links are
τ ∂ ∂
obtained [24]. With the generalized coordinate for the link denoted as q i and the generalized force or torque as
i
the equation of motion becomes
n
6
tr
i=p s=1
H(G,B) zs
H(B,0)H(0,i)Ji
33
∂∂ H T0,i qp
HTB,0 HTG,B
zs +
∂∂ ∂∂ ∂∂ ∂∂ ∂ ∂∂ ∂∂ ∂ ∗ ∂ ∂ ∂∂ ∂∂ ∂∂ τ −− n
i
tr H(G,B) H(B,0)
H(0,i) qp
i=p k=1 6
n
6
2
tr
i=p s=1 t=1
n
i
H(G,B)
zt zs
i
H(0,i) qp
i=p k=1 m =1 6
i
2 tr
H(G,B) zs
i=p s=1 m =1 n
mi g i H(G,B) H(B,0)
Ji
H T0,i
qm
H(G,0)
i=1
ri =
zs zt t +
HTB,0 H TG,B
q k q m +
H T0,i
HT0,i
Ji
qk k +
HTB,0 H TG,B
q m q k
H(0,i)
H(B,0)
qp
qp
2
Ji
HTB,0 HTG,B
qk
H(B,0) H(0,i)Ji
tr H(G,B) H(B,0)
n
H T0,i
qp
H TB,0 HTG,B
zs q m +
(2.10)
p
p = 1 gives the equation for link 1 in q 1 th
p = x gives the n equation for link x in q x To obtain the dynamic equations of the head, the Lagrange equation is applied to the Lagrangian function in Eqn 2.6, except this time z w will be the generalised head coordinate
d
∂ ∂ − ∂∂
dt
L
L
zw
zw
−−
= 0
(2.11)
The zero on the right hand side of the equation is because there are no external forces or torques acting on the head. The derivation of the dynamic equation for the base follows similar algebra as for the links, and after simplifications becomes
∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ n
6
H(G,B)
tr
zw
i=p s=1
n
i
tr
i=1 j=1
n
6
6
H(G,B)
tr
zw
i=1 t=1 s=1
n
i
H(G,B) zw
tr
H(G,B) zw
HTG,B
HTB,0
H(B,0)H(0,i)Ji
zt
H T0,i qj
2
H(B,0)H(0,i)Ji HT0,i
i
i=1 j=1 m =1
∂ ∂ ∂ ∂ ∂ ∂ ∂∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂∂ ∂
H(B,0) H(0,i)Ji HT0,i
HTB,0
H(B,0) H(0,i) Ji
34
zw
JB
HTB,0 HTG,B
HTG,B
zs zt
2
+
H(G,B)
HT (0,i)
qm qj
+
H(G,B) zw
H TB,0 H TG,B
H TG,B zt
zt t +
qj +
2
JB
H TG,B G, B
zs zt
qj qm +
zs zt t +
∗ ∂ ∂ ∂∂ 6
n
i
H
2 tr
zw
i=1 j=1 s=1
H(I,B)
mb g i
zw
G,B
∂ ∂ H
B,0
H
0,i
Ji
HT0,i qj
−−
H TB,0 HTG,B zs q j zs +
b=0
(2.12)
st
w = 1 gives the 1 equation for z w th
w = 6 gives the 6 equation for z 6
=
1
2
3
4
5
=
6
1
2
3
1
2
3
The RoboFish system dynamic equations 2.10 and 2.12 have been developed in three dimensions for an n link system connected to a 6 degree of freedom head, assuming there is gravity acting on the system. For a zero buoyant system, gravity would be set to zero since the buoyancy force acting upwards on the fish body cancels the weight force acting downwards. These dynamic equations can also be written in matrix form as
where
+
is the inertia matrix,
,
is the gravity matrix,
is the generalized coordinate. If a disturbance
, is added to the above equation the manipulator dynamic equation becomes
+
Or more compactly,
where
2.3
=
is the Coriolis-centripetal matrix,
is the generalized force (torque) vector, and term,
+
,
,
+
+
+
,
=
=
represents all the non-linear terms.
HYDRODYNAMIC MODELLING: The forces acting on a swimming robotic fish are weight, buoyancy and hydrodynamic
lift in the vertical direction. In the horizontal heading direction, thrust, friction and inertia drag are found. In the classification of fish swimming modes, the robotic fish prototype developed between the sub-carangiform mode and the carangiform mode. Both modes are sorted as BCF locomotion modes. For these two swimming modes, the hydrodynamic model which is related with the wake thrust generated, was found to be associated with the added-mass method [11]. As the
propulsive wave passes backward along the fish, the momentum of the water passing backward is changed by the movement of the fish tail, which causes a reaction force
35
from
water to fish.
is decomposed into a lateral
contributes to overall forward propulsion for fish.
and a thrust component
which
Fig 2.3 Forces acting on a swimming fish with velocity U. Coordinate system used in thesis is al so shown
2.3.1
ASSUMPTIONS:
To simplify the real time challenges, the following preconditions are followed in the simulation environment: • The water in which robotic fishes swim is quasi -steady fluid. Most of fish swimming
mechanism researches are based on this supposition which also makes it easy to build hydrodynamic model. • For a given parameter vector
= { 1 , 2 , , } for the movement of the robotic fish tail, the value and the direction of thrust force acting on the robotic fish are determined with no respect to the velocity of the robotic fish. • The viscous drag is considered as the only resistance when the robotic fish swimming .
The swimming friction drag is supposed as the only resistance against thrust, which is only determined by the travelling wave parameter vector
= { 1,
2,
, }. The longer and
faster the fish, the more resistance it would be encountered. So, in case of a given Ε, there must be a maximum robotic fish velocity
to make the friction drag equal to the thrust.
At the same time, the robotic fish will keep a constant swimming velocity. The friction drag could be calculated using the standard Newtonian equation
where
= 0.5
2
is the drag coefficient which depends on the Reynolds number, S is the wetted
surface area, U is the forward veloc ity of the robotic fish and ρ is the water density. The Reynolds number is defined as
=
36
/
where
is the tail length and
−
2
−
is the kinematics viscosity of water (1.12 mm /s, fresh water
in 60°F). The laminar and the turbulent drag coefficients are 1.328 respectively [25]. In this paper, the drag coefficients coefficients.
0.5
0.2
and 0.074
are set as the sum of two drag
There is a stabile prominent parameter named Strouhal number for BCF movement [39]
− − =
where
=2
1
+
2
2
|
is the tail-beat peak-to-peak amplitude and
=
oscillating frequency. The
/
lies in a specific range (namely 0.25<
velocity swimming. For our robotic fish, compute the maximum velocity calculated. Let thrust force
. Then the maximum viscous drag
equal to
could be
we get
= 1.32 1.328( 8(
)
=
2.3.2
<0.40) for a constant
is about 0.3. Above equation could be used to
2
= 0.5
and
is the
0.5
+ 0.07 0.074( 4(
)
0.2
/
HYDRODYNAMIC HYDRODYNAMIC FORCES APPROXIMATION:
Robot swimming underwater interacted with the fluid flow remains an unresolved problem. In many cases some simplifications are necessary. In order to calculate the resultant forces, a large Reynolds number is applied and all forces acting on a propulsive element are due to the motion of that element in the fluid, i.e. the effects of the fluid’s motion are not considered based on above Assumption 2. The resultant hydrodynamic force perpendicular to the surface of the moving body takes the form [26]:
where
=
− =
| |
/2 is the drag coefficient, ρ is the density of the fluid,
dependent drag coefficient,
is the shape
is the effective area of the element that confronts the fluid, and
is the velocity of the element. This force can be resolved into components which act parallel and perpendicular to the
surface of each element. Then, the robot is acted upon by four types of forces: pressure on links, pressure on pectoral fins, approach stream pressure and friction drag. 2.3.3
PRESSURE ON LINKS:
While oscillating, the hydrodynamic force acting perpendicular to the surface of the link is the thrust force for advancement, which is given by [26]:
⊥ −⊥⊥ ⊥ =
37
|
|
where
⊥
=
−
surface of the
⊥
is the component of the velocity perpendicular to the
link, calculated by substituting the centroid for the barycentre. Likewise,
= 1 /2 is the drag coefficient with effective area of the link. 2.3.4
1 of
flat plate type [26], that is
1
= 1.28.
is the
APPROACH STREAM PRESSURE:
Motion of any object through a stationary fluid causes an increase in pressure in front of it and a decrease behind it. This makes the fluid in front move away and return again behind the object. The different pressures on the two sides also gives a drag force on the object, counteracting the movement, that is, the approach stream pressure acting on the head in opposition to the motion of the RoboFish. Owing to a larger cross-section of the head followed by small-amplitude oscillation of the rear body, the drag forces acting on the other links can be ignored except for the head. The drag force on the head is given by [ 26]:
− = 1
where
⊥ − ∆ ∆ 1
=
1
parallel to the head. 2
= 0.295.
2.3.5
1
1
1
=
= = = 1 1| 1|
is the projection of the velocity vector along the direction
= 1
= 2 /2 is the drag coefficient with is the cross section area of the head.
2
of bullet type [26], i.e.,
FRICTION DRAG: The movement of the robot’s propellin g units through the fluid causes friction drag
parallel to the propelling units, resulting in drag forces which act in opposition to the motion. It is evaluated empirically of the approach stream pressure [26], that is, 2.3.6
= (0.2 ~ 0.5)
= 1 .
PRESSURE ON PECTORAL FINS:
Thanks to pectoral fins mounted on bilateral positions of the head, the robot is more agile to turn or keep balance in water. While oscillating, the pectoral fin produces the perpendicular pressure and parallel friction drag to the surface. The thickness of the pectoral fin is small and the friction drag can be ignored by just considering the pressure perpendicular to the surface which contributes to thrust generation. Similar to the links, the pressure on the left pectoral fin is given by [26]:
⊥ −⊥⊥⊥ ⊥ ⊥ =
where
⊥ 1
,
=
1
1
2
is the shape coefficient using the flat plate type.
pectoral fin.
,
is the effective area of the left
is the normal velocity perpendicular to the surface of the left pectoral fin
which can be calculated from:
=
+
1
38
1
− − − − − =
2
=
2
+
1
1
1
1
The analysis of the right pectoral fin can be similarly available [26]. 2.3.7
COMPOSITION OF HYDRODYNAMIC FORCES:
By resolving all the above forces in the world resolved coordinate system (WRCS), the components of the forces in the direction of X-axis and Y-axis can be rewritten as [26]:
⊥ ⊥ … ⊥ −⊥ … ⊥ ⊥ ⊥ ⊥ =
= 1,2,
=
,
= 1,2,
= 1
=
= 1
1
= 1
=
= 1
1
,
=
1
= ,
=
1
= ,
= 50%
= 1
2.3.8 LIMITATIONS:
In the current version, the hydrodynamic models of turning and up-down movement of the robotic fish are not considered. A simple kinematics model is adopted to compute the turning locomotion and the model of up-down movement is not included temporarily.
39
CHAPTER 3 MECHANICAL DESIGN Based on progress in robotics, hydrodynamics of fishlike swimming, new materials, actuators and control technology, more and more research has focused on the development of novel fish-like vehicles. For the convenience of description we define a robot fish as a fishlie aquatic vehicle that is based on the swimming skills and anatomic structure of a fish primarily the undulatory/oscillatory body motions, the highly controllable fins and the large aspect ratio lunate tail. As a combination of bio-mechanism and engineering technology, the robot fish is a multidisciplinary field that mainly involves hydrodynamics-based control and actuation technology.
3.1
DIFFICULTIES IN BIOMIMETIC ROBOFISH DESIGN: This is a difficult task for a number of reasons. The actual dynamic shape of a swimming
tuna has never been accurately measured, so there is no well defined target to aim for. In any case, the RoboTuna's body is only a low-order copy of a vastly richer biological system, and so it's not clear that attempting to precisely reproduce the biological motion is either practical or desirable. On the other hand attempting to derive the proper motion by purely analytical means is arrested by two problems. The dynamic interaction of the RoboTuna's undulating body with the passing fluid is (currently) too complex to accurately model. And the hyperredundant planar kinematic chain nature of the body itself creates a situation is which there are infinite possible solutions as to how to move the body from one kinematic position to the next. At present, some artificial systems are developed to investigate fish-like locomotion mechanism. In particular few of the unique mechanical designs that are intriguing are 1)
Oscillating foil has been proposed as an alternative propulsor to the conventional screw propeller.
2)
Development of 8-link foil flapping robotic mechanism acquired detailed measurements of the forces on an actively controlled body [28] [29].
3)
Harper et al. proposed the design of optimal spring constant to actuate the oscillating foil [29].
4)
Sean Andrew Mellott (MIT) developed a CAM based mechanism for complaint body biomimetic robot [31]. The advantages of biological propulsion systems are best seen in comparison of fish to
human-built underwater vehicles with similar scale, because the design goals are the same:
40
high cruising speed and acceleration, good manoeuvrability and high efficiency. Some examples of their supremacy are following: 1)
Yellow fin tuna (BCF) can obtain speeds up to 5 body lengths per second or 40 knots while still maintaining superior agility. Man-made devices are able to achieve higher speed levels, but lose most of the controllability.
2)
Some species of BCF can reverse direction without slowing down and with a turning radius only 1/10 of their body lengths while underwater vehicles must reduce their speed by more than 50 percent and their turning radius is at least 10 times larger than the corresponding value for fish.
3)
Chinook and sockeye salmon (BCF) travel over 1400 km and climb nearly 2100 m from the Pacific Ocean as they return to spawn without eating on their journey while artificial underwater vehicles would need many replacement batteries. Moreover, they are probably not even capable for this task.
4)
Pike (BCF) overcomes its prey with short bursts of acceleration that can exceed that of gravity by about 20 times. Though a lot of different actuation mechanisms are suggested, only a few have been
developed into actual working models. This is due to the complexity in the design, efficiency of propulsion, degrees of freedom, modular construction, power utility, water proofing criterion and body mass index of the whole system. Taking all the factors stated above into consideration, BCF swimming mode in Carangiform model is chosen and Servo Motor actuation is preferred.
3.2
ASSUMPTIONS: For the convenience of theoretical analysis, some suppositions are made below:
1)
The RoboFish can be simplified as a five-link serial mechanism coupled with a pair of pectoral fins of feathering motion, and both the pectoral fins are of identical mechanical dimension.
2)
The fluid is stationary, and the forces acting on a single link are due only to the motion of that link.
3)
The deformation of robot’s body can be ignored except the motion of the links a nd
pectoral fins. 4)
The motion is limited in the horizontal plane and the middle positions of pectoral fins are modulated on this plane at the initial moment. The three dimensional dynamic analysis is not considered temporarily.
5)
The pressure differentials in the directions parallel to the moving body are decoupled from pressure differentials perpendicular to the body.
41
3.3
SOLIDWORKS MODEL: Incorporating all of the sophisticated engineering suggested by a live biological tuna into
a man-made robotic vehicle capable of autonomous free swimming and manoeuvring in an ocean environment is a daunting task. Rather than attempt to attain this goal in one giant leap, it has been pursued in a number of shorter pragmatically achievable steps. With all the advantages of the carangiform model that is explained in previous sections, the system design in SolidWorks was started.
From the literature [26], it is found that
maximum forward velocity and higher degree of manoeuvrability can be achieved when the number of links or modes is fixed to be 4. So with this design criterion in mind, the SolidWorks model is developed. Since the system designed is a prototype model, maximum dexterity and multi mode propagation are the design challenges that are taken into account and not the payload and actual usage of the system in real world situations. So the place constraints for sensor systems, gyros, on board processing unit and on board cameras are not considered in this phase of the design. Each part of the model is created separately in SolidWorks with the dimensions given in Table 3.1 below. Separate assemblies are made for each of the item defined in the Table 3.1 and then the model is developed by creating an assembly of all the assemblies. This modular design procedure is followed so that if a design change has to be made in any individual part, it will automatically be carried forward to all the assemblies that use it. The individual assembles are grouped together in the Fig 3.2. 3.3.1
HEAD DESIGN:
Hull design of head structure for the carangiform fish model is designed with dimensions 130 x 50 x 65 mm approximately (tolerance 1%). Material used for the head hull design is acrylic. Wall thickness of 10 mm is maintained throughout the head hull. Considering machining complexity, the head hull part is divided in to 5 sub parts: head part A, head part B, head part C, head part D and head part island. This modularity in the head hull design decreased the machining cost by 70%. Since the developed model is a prototype, the hydrodynamic curvatures of the surface are not taken into design consideration. The front side of the head hull is curved to a radius of 20mm on both sides. The head assembly consists of the 1 Hitec HS 485HB [31] and 2 Hitec HS 5085MG [32] high performance servo motors. HS 485HB is connected to the link 1 connecting shaft and since it drives the whole posterior portion of the fish model, this high torque servo motor is opted. HS 5085MG motors drive the pectoral fins. Due to space constraint and to achieve efficient system architecture, these 2 motors are placed perpendicular to the pectoral fin shafts and coupled with bevel gears to alter the direction of rotation.
42
A unique coaxial shaft holder is designed that supports both the pectoral fins shafts. After many trials, the height of the pectoral fin shaft from the base of head link hull part is fixed as 30mm that is calculated to provide maximum forward thrust to the system. The overall volume of the designed head assembly is 245384.89 cubic millimetres and the overall mass is 455.62 gms where the head hull part alone weighs 285.62 gms. 60% of the system mass is concentrated in the head assembly that provides more stability for the overall system. The mass center of the head assembly is at X = -13.10, Y = -1.84, Z = 0.00. The complete assembly of the head can be seen in Figure 3.2 a). 3.3.2
LINK I DESIGN:
Since water proofing the motors can be easily done using materials like Epoxy Resins, a open body structure is opted for the RoboFish’s links (I,II and III). Since the subsequent links
are lesser weight assemblies, the driving motor is chosen to be HS 5085MG which is much smaller in weight and dimension when compared to HS 485HB. The Link I assembly consists of Head Joint, Head Joint Bottom, Link Design 2, Link Design 2 Bottom, Motor Coupling, Motor Shaft, Link Holder and HS 5085MG. The material used for the design is acrylic. The overall volume of the designed Link I assembly is 46855.69 cubic millimetres and the overall mass is 106.95 gms. Approximate dimension of the Link I assembly is 115 x 20 x 81 mm. Mass center of the assembly is at X = 46.64, Y = 4.48 and Z = -0.00. The complete assembly can be seen in the Figure 3.2 b). 3.3.3
LINK II-III DESIGN:
The mechanical design parameters of both the links II and III are approximately the same. The material used for the design is acrylic. Links II and III assemblies are shown in the Figure 3.2 c) which consists of Joint Bottom, Joint Top, Motor Coupling, Link Design 2, Link Design 2 Bottom, Link Holder and HS 5085HB. The overall mass of the Link II (III) assembly is 78.45 gms and the overall volume is 27624.94 cubic millimetres. 3.3.4
CAUDAL FIN DESIGN:
Caudal assembly shown in the Figure 3.2 f) has Caudal Fin, Caudal Shaft Top and Caudal Shaft Bottom. Weighing around 47 gms, the overall volume of the caudal assembly is 38694.64 cubic millimetres. As already mentioned in Section 3.3.1, hydrodynamic surface design consideration is not taken into account for prototype design and simple spline curves are used for designing the caudal fin surface. 3.3.5
PECTORAL FIN DESIGN:
Pectoral fin assembly as shown in the Figure 3.2 d) and e), consists of Pectoral Fin, Pectoral Shaft, Pectoral Coupling and Bevel Gear. The overall weight of the pectoral assembly is approximately 50 gms and volume is 17222.02 cu.mms. Without taking into account, the hydrodynamics of the fin surface, normal spline tools are used to design the surface of both
43
the pectoral fins. Calculations revealed that the maximum thrust is produced when the dimension of fin is 67.35 x 45 x 10.82 mm which produces a surface area of 8372.47 sq.mms.
3.4
CUSTOM VISUALIZATION: PROPERTY
VALUE
1)
Length
521.82 mm
2)
Width
149.88 mm
3)
Height
106.53 mm
4)
Mass
905.49 gms
5)
Volume
422042.11 cubic millimetres
6)
Material
Acrylic, Aluminium, Silicone Foam
7)
Surface Area
163677.59 millimeters^2
8)
Center of Mass
X = 48.62, Y = 1.25, Z = 2.04 millimetres
Principal axes of inertia and
Taken at the center of mass.
principal moments of
Ix = (1.00, 0.01, 0.04)
inertia: (grams * square
Iy = (-0.01, 1.00, -0.08) Py = 6032018.66
millimetres )
Iz = (-0.04, 0.08, 1.00)
9)
Px = 406991.70 Pz = 6059653.65
Table 3.1 Custom Visualization o f the designed Solidworks Model
Fig 3.1 SolidWorks Model of the Carangiform Fish (BCF Swimming Motion)
44
3.5
DESIGN PARAMETERS:
PARTNAME HEAD
LENGTH(mm)
WIDTH(mm) HEIGHT(mm) WEIGHT(gm) MATERIAL
130
50
65
285.62
ACRYLIC
67.35
45
10.82
24.32
STD
STD
STD
70
ACRYLIC / SILICONE FOAM Hitec HS 485HB
STD
STD
STD
45
40 (MAX)
50 (MAX)
20 (MAX)
100 (MAX)
LINK 1
115.69
20
81.51
61.95
Hitec HS 5085 MG 7.2V 1.6 Ah Ni Mh ACRYLIC
LINK 2
90.69
20
47.5
33.45
ACRYLIC
LINK 3
90.69
20
47.5
33.45
ACRYLIC
CAUDAL FIN TOTAL
114.37
20
106.53
47.38
ACRYLIC / SILICONE FOAM
521.82
149.88
106.53
905.49
PECTORAL FIN HEAD MOTOR PECTORAL MOTOR BATTERIES
Table 3.2 Design Specifications of the Soli dworks Model
45
a)
b)
c)
d)
e)
f)
Fig 3.2 Individual Assemblies of a) Head with HS 485HB Motor, coaxial holder, batteries and HS 5085MG Motors, b) Link 1 with HS 5085MG motor and motor couplings, c) Link 2, d) Right Pectoral Fin Assembly with pectoral shaft and holder, e) Left Pectoral Fin Assembly with pectoral shaft and holder, and f) Caudal Fin Assembly with motor coupling
46
Head Link
Battery
Caudal Joint Top
Joint Bottom
Link Design Bottom
HS 485HB
Bevel Gear
Link Holder
HS 5085HB
Motor Shaft
Head Joint Bottom
Head Joint
Joint Top
Caudal Joint Bottom
Link Design 2
Pectoral Fin
Coaxial Holder
Caudal Fin
Pectoral Shaft
Pectoral Coupling
Motor Coupling
Fig 3.3 Individual Parts designed i n SolidWorks which are a ssembled in the different assemblies shown in Fig 3.2.
47
3.6
SOLIDWORKS TO SIMMECHANICS: SimMechanics toolbox in MATLAB Simulink is very much used in Robotic Systems
simulation. Each link and joint of the Robotic system can be modelled with different utilities in the toolbox. Different types of joints like Revolute, Prismatic, Screw, Six DoF and Spherical, actuators like Body Actuator, Joint Actuator, Variable Mass and Inertia Actuator and Driver Actuator are readily available in the toolbox. Sensor systems can also be modelled with this powerful toolbox using the Joint Sensor, Body Sensor and Constraints and Driver Sensors. Three basic concepts behind the mechanical system modelling in SimMechanics are “ENV” and “RootGround” blocks forms the base of the robot, each link or a joint is associate d with
the system through a coordinate frame that measures position and orientation with respect to the “World” frame, “Base” frame or the “Follower” frame.
3.6.1 1)
ADVANTAGES OF USING SIMMECHANICS:
Provides a modelling environment for building three-dimensional rigid-body mechanical systems. Includes a variety of simulation techniques for analyzing motion and sizing mechanical components
2)
Enables the visualization and animation of mechanical system dynamics
3)
Enables the implementation of high-fidelity, nonlinear plant models in Simulink to support the development and testing of control systems
4)
Provides a SolidWorks translator to enable the use of CAD tools to define mechanical models.
It supports four motion analysis modes: Forward Dynamics: Assigns driving forces and torques to the motion-driving actuators and
calculates the resulting motions of the entire system Inverse Dynamics and Kinematics: Determines the forces and torques that the actuators must
exert to produce user-specified motions Trimming: Identifies the steady-state equilibrium points to be used for linearization and
system analysis Linearization: Extracts a linear model that predicts the system’s response to perturbations in
driving forces, joint and constraint configurations, and initial conditions These modes of analysis enable to test mechanical performance, select proper actuation systems and develop optimal control. SimMechanics automatically creates a 3-D visualization of the mechanical model that can be animated during the simulation. The geometry in the visualization can be generated from
48
the coordinate systems defined in the model, or realistic 3-D geometry can be attached to the bodies in the model. In addition, ellipses representing the mass and inertia of each par t can be displayed. Saving the animation produced during simulation to a separate file helps in further analysis independent of the simulation.
3.6.2
EXPORTING CAD ASSEMBLIES INTO SIMMECHANICS:
Using SimMechanics Link, a SimMechanics model can be automatically generated from a CAD assembly. SimMechanics Link exports critical data and mate information to a file that can be imported by SimMechanics. The mass and inertia of each part in the assembly are converted to rigid bodies in SimMechanics. Geometry from the CAD assembly is automatically converted into geometry files and associated with the proper body in SimMechanics. The mate definitions in the CAD assembly are converted into the appropriate joints in the SimMechanics model. Step 1: MathWorks provides a CAD Translator for SolidWorks and other 3D CAD tools to
convert the existing 3D model into SimMechanics model. The translator is an add-on which can be downloaded from [33]. Step 2: The SimMechanics Link utility is the necessary intermediary that lets you convert CAD
assemblies into SimMechanics models. The intermediate step between CAD assembly and SimMechanics model is exporting an XML file in Physical Modelling format from the assembly. The export also automatically creates STL files containing body geometry information that you need for visualizing the bodies in the model. The SimMechanics importer then converts this XML file into a mechanical model, which references the STL files to visualize the bodies.
3.6.3
REQUIREMENTS FOR CAD EXPORT AND MECHANICAL IMPORT: The full CAD translation is composed of two distinct steps with different requirements.
1) 2)
The first step is the export of the CAD assembly, which creates the Physical Modelling XML file and one or more STL file(s). The second step is the import of the Physical Modelling XML file and generation of the SimMechanics model.
49
Fi 3.4 Ex ortin CAD Model from SolidWorks into a SimMechanics XML file
Fig 3.5 Successful translation of CAD Assembly to Physical Modelling XML file from SolidWorks.
Fig 3.6 Complete translation of CAD Assembly into Visualizable model
50
3.6.4
IMPORTING THE XML AND GENERATING A MODEL: To complete the CAD translation, the Physical Modelling XML file is converted into a
SimMechanics model.
Fig 3.7 Importing a Physical Modelling XML file into a model.
To generate a new model from a physical modelling XML file, command line option or dialog box option can be used. To open the model from command line mech_import(‘cad_assembly.xml’);
Or dialog box option can be used by e ntering the “mech_import” command or “import_physmod” command in MATLAB command window. The XML model can be loaded
from the GUI given below. This creates a SimMechanics model in Simulink with joints, links and environment information from the CAD model. This SimMechanics model can be edited to make any necessary changes to the physical model or can be used as such as a sub system in the Robotic system.
51
Fig 3.8 Import Physical Modelling dialog box option to load the XML model.
3.6.5
INTRODUCING ACTUATORS AND SENSORS TO PHYSICAL MODEL: The SimMechanics model imported will not have any actuators or sensors to drive the
model in a controlled environment. The sensor and actuator blocks described in Section 3.6 can be included here to actuate the joints and measure the joint variables (angular position and velocity) for closed loop control. Tags (local or global) can be used to construct the overall system as a group of individual sub systems that will be easy to understand and also easy to edit.
52
Fig 3.9 SimMechanics Subsystem of the imported Physical Model with Actuators and Sensors
53
3.6.6
SIMULATING THE SIMMECHANICS MODEL:
Once the SimMechanics block is constructed from the physical model, animating it is possible through SimMechanics itself. For the input provided, the kinematics and dynamics can be visualized through the visualization option in SimMechanics. Detailed procedure for setting up the Visualization environment and animation is provided in [34]. To visualize the body motions: 1)
From the Simulation menu, select Configuration Parameters, then the SimMechanics node.
2)
Select Display machines after updating diagram and show animation during simulation. Click OK.
3)
Select Update Diagram from the Edit menu. The SimMechanics visualization window opens.
4)
In the SimMechanics menu of the visualization window, select Machine Display, then Ellipsoids. The display now shows the robot fish’s component bodies as ellipsoids.
5)
Click the Start button. The simulation begins. Observe the robot arm motion in the SimMechanics window. By default the model properties are fed to the two inbuilt animation options, Convex
Hull or Ellipsoid Bodies. This can be changed to “External Graphics File” *3 4] to import body
geometries from external files (STL files) created from SolidWorks translation. However static animation is the default option for SimMechanics Simulation which makes the model fixed to the RootGround. This major disadvantage in the SimMechanics Animation can be overcome by interfacing the SimMechanics Physical Model to VirtualReality toolbox. Even though an actuator can be attached to the RootGround to make the whole model moving in the frame, more options can be ventured using Virtual Reality Toolbox. 3.6.7
ACTIVATING AND CONTROLLING ANIMATION RECORDING:
• Animation recording can be activated by clicking the “ Store Animation in AVI File” button on
the toolbar. The default is deactivated. If it is activated, the AVI File Location file browser opens, as discussed next. • Recording settings can be controlled using the Simu lation menu. Location can be specified and name for your AVI file recording by selecting “ Choose AVI File Location” from the
Simulation menu. If recording is activated, an AVI File Location file browser opens and requires selecting a location and specifying an AVI name. The default AVI name for a model called modelname.mdl is “modelname.avi ”, but it can be changed. Click Save to complete the
AVI files specification. When recording is activated, this AVI file name appears in the bottom middle of the full visualization window, below the display, in the status bar. If the animation is stopped, the recording is stopped and saved as well. There is an option in Recording Settings to compress
54
Fig 3.10 SimMechanics animation of the physical model as Ellipsoids
Fig 3.11 SimMechanics animation of the physical model as External Animation File ( STL)
55
the video file generated. When animation is very long, compressing it gives acceptable video quality at a lower file size.
3.7
SOLIDWORKS TO VIRTUALREALITY TOOLBOX: Solidworks provides a support to Virtual Reality tool by which the CAD model developed
in SolidWorks can be converted to a .wrl file using the Virtual Reality Markup Language (VRML) translation. Few settings that need to be followed up while making this translation are 1)
“Save all components of the assembly in a single file” option should be disabled to
create inlined URL files in the virtual world created. 2)
VRML 97 standard should be selected from the Output as Version drop down list.
3)
Appropriate unit should be selected from the list for the entire model.
Fig 3.12 Virtual Reality Export settings from SolidWorks
3.7.1
VISUALIZING WITH A VIRTUAL REALITY CLIENT:
SimMechanics visualization can be bypassed and a mechanical animation can be created in a virtual world of specific design. A Virtual World can be created and populated with bodies represented as virtual objects using Virtual Reality Modelling Language (VRML). Then interfacing the virtual world with the SimMechanics model is a piece of cake to get any kind of complicated animation. Creating a virtual animation requires a new or existing virtual world for a specific model and an interface between them. The Simulink 3D Animation documentation [34] and VRML books explain how to create virtual objects and assemble them into virtual worlds. 3.7.2
REPRESENTING BODIES AS VIRTUAL OBJECTS:
Each body can be represented by a virtual object encoded in a .wrl file. A master .wrl file should be created to represent the virtual world that refers to body wrl files and to place and orient these bodies in the larger scene. A virtual object ’s position and orientation can be defined with respect to: • The overall virtual world, corresponding to the SimMechanics coordinate system World.
56
• Another body in the machine, corresponding to SimMechanics Body coordinate systems.
Fig 3.13.a) VR World imported into VR
Fig 3.13.b) VR World created in VR Realm
Realm Builder from SolidWorks for each link
Builder with transforms for each link
Fig 3.14.a) VR World created in VR Realm
Fig 3.14.b) VR World created in VR Realm Builder
Builder from imported SolidWorks inlined wrl
with transforms for each link with inlined url files
file
Body references to other bodies can be nested in VRML hierarchies, but at least one body’s position and orientation with respect to the overall virtual world must be defined.
57
Initial states of the bodies should be placed and oriented corresponding to the initial state of the SimMechanics simulation. Each body’s .wrl file contains a hierarchical tree starting with the Transform node. Among Transform’s fields, translation and rotation fields are used to specify the body’s
position and orientation in space. If a body is nested below another body, its position and orientation are defined with respect to the next body up the hierarchy. Creating a virtual world gives great flexibility in representing the machine: • Inclusion or omission of bodies is possible. • If a body should be only translated, omit rotation field from its Transform node. • If a body should be only rotated, omit translation field from its Transform node.
View Points, Background, Directional Light and Navigation Info can be added to enhance the functionalities of the Virtual World created [34]. 3.7.3
ISSUE WITH WRL FILE IMPORT:
When the SolidWorks model is exported into VRML file with the settings mentioned above, all the parts or assemblies in the model are added as inlined URLs. One problem with converting the SolidWorks model is when the wrl file is created, a blank space is prefixed and post fixed to the file name. And when the wrl files inlined are called as per the tree hierarchy as show in Figure 3.13 a) and the files are not getting loaded. To avoid this issue, those blank spaces need to be manually deleted or else after opening the file in V Realm Builder “ save as”
the wrl file as a new file to solve this issue. 3.7.4
INTERFACING SIMMECHANICS MODELS WITH VIRTUAL WORLDS:
To animate a body, the motion has to be measured in SimMechanics simulation and that information has to be exported to the virtual world. This requires connecting Body Sensor blocks to the Bodies that should be animated in the model, then creating an interface that animates the virtual bodies with the body sensor motion signals [35]. Connect the Body Sensors to Body coordinate systems (CSs) on the bodies whose motions should be animated. The Body block reference [35] discusses how to create and configure Body CSs. Extra steps need to be taken to export the signals of a body sensor to your virtual world: 1)
The Body Sensor’s CS reference origin and orientation should follow the body’s defining VRML hierarchy. Example: Define a new Body CS on a body to connect the Body Sensor.
2)
If VRML body’s position is defined with respect to the center of gravity (CG) of a second, neighbouring body in the VRML files, the “Translated from origin of” field of the new
Body CS should be set to the origin of the CG CS of the second body.
58
3)
In the Body Sensor dialog, the [x; y; z] Position check box should be selected if the body’s translational motion needs to be animated.
4)
The [3 x 3] Rotation matrix check box should be selected if the body’s rotational motion needs to be animated.
Fig 3.15 Body Sensor SimMechanics block added to the Links. Position (x,y,z) and Rotation Matrix [3x3] are enabled to measure the rotation parameters.
5)
Choose the coordinate system in which the body motions are measured in the “With respect to coordinate system” pull -down menu.
Choose Absolute (World) or Local
(Body CS). This coordinate system should be the same as the coordinate system used to define the body’s position and orientation in the V RML files.
3.7.5
1)
ANIMATING THE VIRTUAL WORLD BODIES:
Animating the virtual bodies requires interfacing the body sensor signals in the SimMechanics model with the VRML translation and/or rotation fields in the .wrl files. VR Sink block in Simulation 3D Animation block library can be used to accomplish this.
2)
“vrlib” at the command line opens the Virtual Reality Library. VR Sink block can be
dragged and dropped into the model. 3)
While opening the VR Sink dialog box, enter the name of the VRML file that represents the model’s virtual world in the “Source file” field in the World properties area. This is
59
the file that refers to the other .wrl files representing the component bodies of the model. If the virtual world VRML file is not in the same folder as the model, enter the file’s path relative to the model. In the VRML Tree window, the node list of the virtual
world .wrl file appears. 4)
Tree of each component body in the list after expanding shows the body’s check box list. Rotation and/or translation check boxes can be selected as needed for each body. A Simulink input port appears on the block icon for each of these selected check boxes.
Fig 3.16 VR Sink Block Parameters dialog box
3.7.6
CONVERTING BODY SENSOR SIGNALS INTO VRML FORMAT:
Before connecting the Body Sensor Output signals to the VR Sink block, following modifications need to be done for valid use in VRML. • Translational motion signal line should be connected directly from the output port of the Body Sensor to the “node.translation” input port on the VR Sink. The VRML node tree
directly accepts translation motion as a 3-vector signal of rectangular coordinates (x,y,z). The translational motion signal should refer to the same coordinate system used to define the body’s position in the VRML files. • Directly connecting the rotational motion signal line to the VR Sink is not possible. The
Body Sensor output represents orientation with a 3-by-3 rotation matrix R, while VRML
60
accepts orientation represented as the axis-angle 4- vector form * n θ +, where n=(nx, ny, nz) is a 3-vector representing the rotation axis and θ is the rotation angle. In the SimMechanics Utilities library, there is a “RotationMatrix2VR” block. For each rotational motion signal, a “RotationMatrix2VR” block should be added into the model. The
Rotation signal from the Body Sensor block should be connected to the RotationMatrix2VR block. Then the latter block is connected to the corresponding “node.rotation” input port on VR Sink for that body. This block converts the 3-by-3 R matrix signal into the 4-vector VRML form required for animation. SimMechanics model now animates the virtual world using the VRML tools. Animation videos are published in [41].
Fig 3.17 Subsystem connecting Body Sensor block fro m SimMechanics system to VR Block
61
Fig 3.18 Virtual Reality Subsystem with VR Sink Block
Fig 3.19 Virtual Reality Simulation in Simulink using VR Sink Block.
62
CHAPTER 4 SIMULATION AND RESULTS Since the controllability and manoeuvrability of the fish relies on the internal shape (the joint angle) and the oscillating frequency f of the tail for speed, the robot fish’s motion control in a 2-D plane is decomposed into the speed control and the orientation control. For now, speed control of the RoboFish in 2D plane is considered. For an up-down-motioned robot fish, in particular, submerging/ascending control can be implemented in a 3-D workspace.
Fig 4.1 Simulink block diagram of the whole system with SimMechanics and VR subsystems
4.1
SPEED CONTROL:
It is observed that fish in nature uses a combination of frequency and amplitude for speed control. Oscillating frequency , oscillatory amplitude of the posterior body and length of the oscillatory part, are utilized in our method to achieve different swimming speeds [37]. 4.1.1
SPEED CONTROL BASED ON OSCILLATING FREQUENCY:
Substituting as:
∗ for 2
, another form of body-wave equation is easily derived from Eqn 1.1
,
=
1
+
2
63
2
[sin
+2
]
As a general tendency, the swimming speed increases with the oscillating approximate a constant when the desired speed is achieved. 4.1.2
, and
will
SPEED CONTROL BASED ON OSCILLATORY AMPLITUDE:
A second order amplitude envelop
,
produce different body-waves by different values of
1
2
is chosen to
=
1
+
and
2.
In practice, oscillatory-
2
amplitude-based speed control method adjusts the transverse movement at a constant oscillating frequency. Thrust is hence changed and so did the swimming speed. When a closed loop system is formulated, it is better to go for oscillatory frequency control because the joint velocities are directly related to the frequency term of the body wave and speed controller of the 6 Servo Motors can be driven using a trajectory generator which calculates the desired joint parameters from the expected forward velocity and attack angle of the RoboFish using Inverse Kinematic approach on the system dynamics equations. 4.1.3
COMPUTED TORQUE CONTROL METHOD:
In the general Lagrangian system equations Eqn 2.3, computed torque control is the most intuitive method to implement.
Fig 4.2 General Block Diagram of the Computed Torque Control Method using the desired joint angles, velocities and accelerations generated using the Trajectory Generator.
Fig 4.3 Functional Block Diagram of the RoboFish Control System using the control method shown in Fig 4.2.
64
The system is first driven with zero input and gravity acting on it in –y axis. After analyzing and verifying the system response, various combinations of Oscillating Frequencies and Amplitudes for all the six joints (links, caudal and pectoral fins), and the optimized value that provides maximum forward velocity [26] is given in the Table 4.1. st
1 Link
2
nd
Link
rd
3 Link
th
4 Link
Pectoral
(Caudal) Oscillating
1.65
1.65
1.65
1.65
1.65
0.19
0.225
0.275
0.325
0.30
Frequency (Hz) Oscillating Amplitude (rad scaled) Table 4.1 Optimal values of Oscillating Frequency and Oscillating Amplitude of each of the links and pectoral fins
The system is driven with the above given optimal frequencies and amplitudes. The joint theta values are obtained from the SimMechanics joint blocks and plotted below.
Fig 4.4 Joint theta values obtained from the Open Loop SimMechanics System
65
CHAPTER 5 FUTURE WORKS AND THINGS TO EXPLORE Based on this background, consider the fundamental questions rose, by the biological data, and how the RoboFish's experimental results address them: 1)
Can the flow past an undulating body propelled by an oscillating foil be "tuned" such
that the body's drag is reduced and its thrust is enhanced in a beneficial way? Yes, clearly it can be, as both Gray's paradox suggests and the results of the experiments [39] conclusively show, this flow can be altered by the correct body-wave/tail-foil motion to use the hydrodynamics in a beneficial way. 2)
What are the parameters which control this tuning? The parameters which control this tuning are the set of travelling body-wave/tail-foil
control parameters given by: the forward speed, the tail fins maximum angle of attack, the wavelength of the travelling body wave, linear amplitude of the body wave a set of fluid dynamics parameters. These may not be the only or the optimal set of such parameters. As with coordinate or modal systems, there is probably an infinite variety of ways to express the same characteristics, but this particular set is explicitly tied to body dynamics, in such a way as to be easy to observe, to measure, and to comprehend. 3)
What is the maximum benefit that can be achieved? Gray's paradox implies that a seven-fold reduction in drag be achieved. The RoboFish
only experimentally [39] reduced its drag by about half. Obviously Mother Nature is the better engineer. For the sake of argument, assuming the RoboFish has a purely internal mechanical efficiency in the range of 90%, by extension, it can be claimed that its apparent reduction in drag is in fact probably in the range of 60%, but obviously, there may still be a way to go. 4)
Can a man-made (non-biological) system successfully exploit this phenomenon? Yes, it can. But a more appropriate question is: What level of performance can be
achieved by a man-made system, redesigned based on the information collected? These results also raise a host of intriguing new questions such as: What happens at higher speed, what is actually going on in the flow above and behind the body and can this be replicated in a free swimming fish?
5.1
OPEN AREAS OF RESEARCH: In terms of possible direction of future work, three main areas of research stick out.
First, comes the questions such as: What exactly is going on in the flow above and behind
66
the RoboFish as it swims and what is the effect of variations in the body-wave/tail-foil parameters on this flow? Speculating on the answers to these questions is well beyond this discussion, but using flow visualization techniques is clearly the next logical step to take. In addition, the development of an accurate (numerical) model of the flow would be invaluable. Secondly, inspection of the power data revealed the importance of incorporating some form of Tuned-Harmonic-Drive (THD) into the design of any future oscillation system. If the main structural element of future fish can be carefully designed such that its primary model shape corresponds to the wave form required for swimming, it is possible that much better levels of swimming performance can be achieved. Rough calculations indicate that installing a carefully designed THD can reduce the power requirements of a direct drive by half. If this THD can pick up on and be excited by the dynamics of the fish, even greater levels of performance may occur. Thirdly, the RoboFish prototype is designed to really only do one thing, swim in a straight line well. Biological fish add a whole host of amazing manoeuvres in their swimming. They can potentially accelerate at levels exceeding 10 g and turn in less than half a body length at full speed. The most common questions asked are: When are you going to take it off the sled and when will it be free swimming? The level of complexity involved in a free swimming RoboFish is many times greater than in the current design, but is the next logical step in the progression.
5.2
CENTRAL PATTERN GENERATOR (CPG): CPG neural network control imitates motion-controlling mechanism of animals to
organize structures and properties of control network for rhythmic motion. This is a new thought of making use of the achievements in biological cybernetics to improve the performance of robots. Given that the control surface of the multi-fin propulsor obviously performs typically rhythmic motion, namely fin rays’ oscillation about their bases, CPG can be adopted to drive the multi-fin propulsor for RoboFish’s propulsion and manoeuvring.
5.3
CONCLUSION: A free-swimming RoboFish could be used to explore a wide range of efficiency, fast-
starting, sharp turning and three-dimensional manoeuvring issues as well as act as a test bed for a whole host of new marine sensors, actuators and controller technologies. The amount of fundamental new information that could be collected by such a free swimming system is immense. Are there as of yet unknown, "beneficial hydrodynamics" that biological fish exploit to perform their amazing manoeuvres? We believe that the understanding, technology and design expertise is there to build a free-swimming RoboFish.
67
APPENDIX A
APPENDIX A: MATLAB CODES % % % %
DParams_Fish_Withvalues.m M Matrix calculation. Inertial Mass Matrix for the Robotic Fish with 3 Links, 1 Caudal Fin and 2 Pectoral Fins (identical). input vector 'q' has 6 joint theta values
function D = DParams_Fish_Withvalues(q) syms alp a d th ... real ; syms t th1 th2 th3 th4 th5 th6 th1d th2d th3d th4d th5d th6d ... real ; if (size(q,1)+size(q,2) < 7) disp('Not enough arguments to work with.'); D=zeros(6,6); return end T=[cos(th) -sin(th)*cos(alp) sin(th)*sin(alp) a*cos(th); sin(th) cos(th)*cos(alp) -cos(th)*sin(alp) a*sin(th); 0 sin(alp) cos(alp) d; 0 0 0 1]; % System Model Parameters % a1 = 28; % all units are in mm % l = [167.29 167.68 89.99 73.90 56.85 56.85]; l =[14 89.93 64.83 64.84 -33.30 -33.30].*1e-003; m = [61.95 33.45 33.45 47.38 24.32 24.32].*1e-003; % i_head = [1.00 0.00 -0.00; -0.00 1.00 0.00; 0.00 -0.00 1.00]; i_link1 = [0.89 -0.46 0.00; 0.46 0.89 -0.00; -0.00 0.00 1.00]; i_link2 = [0.96 -0.29 0.00; 0.29 0.96 -0.00; -0.00 0.00 1.00]; i_link3 = [0.96 -0.29 0.00; 0.29 0.96 -0.00; -0.00 0.00 1.00]; i_caudal = [0.96 0.00 -0.29; 0.29 -0.00 0.96; -0.00 -1.00 -0.00]; i_pect_l = [0.56 -0.01 0.83; 0.83 -0.03 -0.56; 0.03 1.00 -0.01]; i_pect_r = [-0.56 -0.01 -0.83; 0.83 0.03 -0.56; 0.03 -1.00 -0.01]; dh_link = {a,alp,d,th}; dh_link1 dh_link2 dh_link3 dh_link4 dh_link5 dh_link6 T10 T21 T32 T43 T51 T61
= = = = = =
= = = = = =
{l(1),0,0,th1}; {l(2),0,0,th2}; {l(3),0,0,th3}; {l(4),0,0,th4}; {l(5),-pi/2,0,th5}; {l(6),pi/2,0,th6};
subs(T,dh_link,dh_link1); subs(T,dh_link,dh_link2); subs(T,dh_link,dh_link3); subs(T,dh_link,dh_link4); subs(T,dh_link,dh_link5); subs(T,dh_link,dh_link6);
% Homogeneous Transformation Matrices % T10=T10; T20 = T10*T21;
68
T30 T40 T50 T60
= = = =
T20*T32; T30*T43; T10*T51; T10*T61;
old_th = {th1,th2,th3,th4,th5,th6}; new_th = {'th1(t)','th2(t)', 'th3(t)','th4(t)', 'th5(t)','th6(t)'}; old_thd={'diff(th1(t), t)','diff(th2(t), t)','diff(th3(t), t)' ... ,'diff(th4(t), t)','diff(th5(t), t)','diff(th6(t), t)'}; new_thd={th1d,th2d,th3d,th4d,th5d,th6d}; new_q = {q(1),q(2),q(3),q(4),q(5),q(6)}; % Jacobian Vector v=[th1d th2d th3d th4d th5d th6d]; T10temp = diff(subs(T10(1:3,4),old_th,new_th),t); JD1 = subs(jacobian(subs(T10temp,old_thd,new_thd),v),new_th,old_th); JD1 = subs(JD1,old_th,new_q); T20temp = diff(subs(T20(1:3,4),old_th,new_th),t); JD2 = subs(jacobian(subs(T20temp,old_thd,new_thd),v),new_th,old_th); JD2 = subs(JD2,old_th,new_q); T30temp = diff(subs(T30(1:3,4),old_th,new_th),t); JD3 = subs(jacobian(subs(T30temp,old_thd,new_thd),v),new_th,old_th); JD3 = subs(JD3,old_th,new_q); T40temp = diff(subs(T40(1:3,4),old_th,new_th),t); JD4 = subs(jacobian(subs(T40temp,old_thd,new_thd),v),new_th,old_th); JD4 = subs(JD4,old_th,new_q); T50temp = diff(subs(T50(1:3,4),old_th,new_th),t); JD5 = subs(jacobian(subs(T50temp,old_thd,new_thd),v),new_th,old_th); JD5 = subs(JD5,old_th,new_q); T60temp = diff(subs(T60(1:3,4),old_th,new_th),t); JD6 = subs(jacobian(subs(T60temp,old_thd,new_thd),v),new_th,old_th); JD6 = subs(JD6,old_th,new_q); % D matrix of Transational Motion D_Trans = (m(1).*JD1.'*JD1)+(m(2).*JD2.'*JD2)+(m(3).*JD3.'*JD3)... +(m(4).*JD4.'*JD4)+(m(5).*JD5.'*JD5)+(m(6).*JD6.'*JD6); % D Matrix for Rotational Motion % Rotation Matrices R=[cos(th) -sin(th)*cos(alp) sin(th)*sin(alp); sin(th) cos(th)*cos(alp) -cos(th)*sin(alp); 0 sin(alp) cos(alp)]; R10 R21 R32 R43 R51 R61
= = = = = =
% R10 R20 = R30 = R40 = R50 =
subs(R, subs(R, subs(R, subs(R, subs(R, subs(R,
{th, {th, {th, {th, {th, {th,
alp}, alp}, alp}, alp}, alp}, alp},
{q(1),0}); {q(2),0}); {q(3),0}); {q(4),0}); {q(5),-pi/2}); {q(6),pi/2});
= R10; R10*R21; R20*R32; R30*R43; R10*R51;
69
R60 = R10*R61; % Moment of Inertia Matrices Calculation % 1st link params I11 = (1/12)*m(1)*l(1)^2*i_link1; % 2nd link params I22 = (1/12)*m(2)*l(2)^2*i_link2; % peduncle params I33 = (1/12)*m(3)*l(3)^2*i_link3; % caudal fin params I44 = (1/12)*m(4)*l(4)^2*i_caudal; % right fin params I55 = (1/12)*m(5)*l(5)^2*i_pect_l; % left fin params I66 = (1/12)*m(6)*l(6)^2*i_pect_r; % Jacobian JR1=[0 0 0 JR2=[0 0 0 JR3=[0 0 0 JR4=[0 0 0 JR5=[0 0 0 JR6=[0 0 0 DRot1 DRot2 DRot3 DRot4 DRot5 DRot6
= = = = = =
0.5 0.5 0.5 0.5 0.5 0.5
Matrices 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0; 0 * * * * * *
JR1.' JR2.' JR3.' JR4.' JR5.' JR6.'
for 0 0 0 0 0 0 0 0 0 0 0 0 * * * * * *
Moment 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0;
(R10 (R20 (R30 (R40 (R50 (R60
* * * * * *
of inertia terms 1 0 0 0 0 0]; 1 1 0 0 0 0]; 1 1 1 0 0 0]; 1 1 1 1 0 0]; 1 0 0 0 1 0]; 1 0 0 0 0 1];
I11 I22 I33 I44 I55 I66
* * * * * *
R10.') R20.') R30.') R40.') R50.') R60.')
* * * * * *
JR1; JR2; JR3; JR4; JR5; JR6;
D_Rot = DRot1 + DRot2 + DRot3 + DRot4 + DRot5 + DRot6; % Inertial type symmetric Matrix D = D_Trans + D_Rot; % D inverse calculation % D_inv = inv(D); % AI=inv(A); BI=inv(B); CI=inv(C); DI=inv(D); % inv(A B; C D) = [AI+inv(A*CI*D*BI*A-A) CI+inv(C*AI*B*DI*C-C); % -inv(A*CI*D-B) -inv(C*AI*B-D)]; end
-------------------------------------------------------------------------------------------------------------------------function H = HParams_Fish_Withvalues(q, qd) % clear all; % clc; syms alp a d th ms ls ... real ; syms th1 th2 th3 th4 th5 th6 th1d th2d th3d th4d th5d th6d ... real ; T=[cos(th) -sin(th)*cos(alp) sin(th)*sin(alp) a*cos(th); sin(th) cos(th)*cos(alp) -cos(th)*sin(alp) a*sin(th); 0 sin(alp) cos(alp) d; 0 0 0 1]; % Model Parameters l =[14 89.93 64.83 64.84 -33.30 -33.30].*1e-003; % length in m
70
m = [61.95 33.45 33.45 47.38 24.32 24.32].*1e-003; % mass in Kg new_qd = [qd(1), qd(2), qd(3), qd(4), qd(5), qd(6)]; dh_link = {a,alp,d,th}; dh_link1 dh_link2 dh_link3 dh_link4 dh_link5 dh_link6
= = = = = =
{l(1),0,0,th1}; {l(2),0,0,th2}; {l(3),0,0,th3}; {l(4),0,0,th4}; {l(5),-pi/2,0,th5}; {l(6),pi/2,0,th6};
T10=subs(T,dh_link,dh_link1); T21=subs(T,dh_link,dh_link2); T32=subs(T,dh_link,dh_link3); T43=subs(T,dh_link,dh_link4); T51=subs(T,dh_link,dh_link5); T61=subs(T,dh_link,dh_link6); % Homogeneous Transformation Matrices % T10=T10; T20=T10*T21; T30=T20*T32; T40=T30*T43; T50=T10*T51; T60=T10*T61; % Subs parameters vectors old_th = {th1, th2, th3, th4, th5, th6}; new_thd={th1d,th2d,th3d,th4d,th5d,th6d}; new_q = {q(1),q(2),q(3),q(4),q(5),q(6)}; % Pseudo Moment of Inertia Matrices IP11 = [ 1/3*m(1)*l(1)^2 0 *m(1)*l(1) 0 0 m(1)]; IP22 = [ 1/3*m(2)*l(2)^2 0 *m(2)*l(2) 0 0 m(2)]; IP33 = [ 1/3*m(3)*l(3)^2 0 *m(3)*l(3) 0 0 m(3)]; IP44 = [ 1/3*m(4)*l(4)^2 0 *m(4)*l(4) 0 0 m(4)]; IP55 = [ 1/3*m(5)*l(5)^2 0 *m(5)*l(5) 0 0 m(5)]; IP66 = [ 1/3*m(6)*l(6)^2 0 *m(6)*l(6) 0 0 m(6)];
0 -1/2*m(1)*l(1); 0 0 0 0 ;0 0 0 0 ; -1/2 ... 0 -1/2*m(2)*l(2); 0 0 0 0 ;0 0 0 0 ; -1/2 ... 0 -1/2*m(3)*l(3); 0 0 0 0 ;0 0 0 0 ; -1/2 ... 0 -1/2*m(4)*l(4); 0 0 0 0 ;0 0 0 0 ; -1/2 ... 0 -1/2*m(5)*l(5); 0 0 0 0 ;0 0 0 0 ; -1/2 ... 0 -1/2*m(6)*l(6); 0 0 0 0 ;0 0 0 0 ; -1/2 ...
% H Parameter 2nd Term n=6; for i =1:n s11 = genvarname(['H' int2str(i)]); eval([s11 '=0;']); for j = 1:n s10 = genvarname(['H' int2str(i) int2str(j)]); eval([s10 '=0;']); for k = 1:n s9 = genvarname(['H' int2str(i) int2str(j) int2str(k)]);
71
eval([s9 '= 0;']); r=max([i j k]); for x=r:n temp_1 = []; temp_2 = []; eval(['temp_2 = diff(T' int2str(x) '0,th' int2str(i) ').'';']); eval(['temp_1 = diff(diff(T' int2str(x) '0,th' int2str(k) '),th' int2str(j) ');']); temp_1 = subs(temp_1,old_th,new_q); temp_2 = subs(temp_2,old_th,new_q); eval([s9 ' = ' s9 '+ trace(temp_1 * IP' int2str(x) int2str(x) '* temp_2);']); end eval([s10 '=' s10 '+' s9 '* qd(j) * qd(k);']); end eval([s11 '=' s11 '+' s10 ';']); end eval(['H' int2str(i) ';']); end H=[H1; H2; H3; H4; H5; H6]; end
-------------------------------------------------------------------------------------------------------------------------% GParams_Fish_Withvalues.m % Gravity Vector for the Robotic Fish System. Input term ‘q’ is a 6 element vector % providing the joint theta values to the function. function G = GParams_Fish_Withvalues(q) % clear all; % clc; syms alp a d th ... real ; syms th1 th2 th3 th4 th5 th6 th1d th2d th3d th4d th5d th6d ... real ; T=[cos(th) -sin(th)*cos(alp) sin(th)*sin(alp) a*cos(th); sin(th) cos(th)*cos(alp) -cos(th)*sin(alp) a*sin(th); 0 sin(alp) cos(alp) d; 0 0 0 1]; % l m g
System Model Parameters =[14 89.93 64.83 64.84 -33.30 -33.30].*1e-003; = [61.95 33.45 33.45 47.38 24.32 24.32].*1e-003; = 9.81; % in m/s^2
dh_link = {a,alp,d,th}; dh_link1 dh_link2 dh_link3 dh_link4 dh_link5 dh_link6
= = = = = =
{l(1),0,0,th1}; {l(2),0,0,th2}; {l(3),0,0,th3}; {l(4),0,0,th4}; {l(5),-pi/2,0,th5}; {l(6),pi/2,0,th6};
T10=subs(T,dh_link,dh_link1); T21=subs(T,dh_link,dh_link2); T32=subs(T,dh_link,dh_link3); T43=subs(T,dh_link,dh_link4); T51=subs(T,dh_link,dh_link5); T61=subs(T,dh_link,dh_link6);
72
% Homogeneous Transformation Matrices % T10=T10; T20=T10*T21; T30=T20*T32; T40=T30*T43; T50=T10*T51; T60=T10*T61; % Subs parameters vectors old_th = {th1, th2, th3, th4, th5, th6}; new_q = {q(1),q(2),q(3),q(4),q(5),q(6)}; % Gravitational Matrix for any link i G=[0 -g 0 0]; % Gravity Vector Parameters Calculation from Homogeneous Transformation % Matrices n=6; r11 = [-l(1)/2;0;0;1]; r22 = [-l(2)/2;0;0;1]; r33 = [-l(3)/2;0;0;1]; r44 = [-l(4)/2;0;0;1]; r55 = [-l(5)/2;0;0;1]; r66 = [-l(6)/2;0;0;1]; for i=1:n s14 = genvarname(['G' int2str(i)]); eval([s14 '=0;']); for j = i:n eval([s14 '=' s14 '- m(j) *G*diff(T' int2str(j) '0,th' ... int2str(i) ')*r' int2str(j) int2str(j) ';']); eval([s14 '=subs(' s14 ',old_th,new_q);']); end end G=[G1;G2;G3;G4;G5;G6]; end
-------------------------------------------------------------------------------------------------------------------------% HydroDynamic_Forces.m % Generalized hydrodynamic forces acting on each link are summarized. clear all; clc; l1 = 14; l2 = 89.93; l3 = 64.83; l4 = 64.84; l5 = -33.30; l6 = -33.60; lc = 14; lc1 lc2 lc3 lc4 lc5 lc6
=0.52*14; =0.52*89.93; =0.52*64.83; =0.52*64.84; =0.39*-33.30; =0.39*-33.30;
% lc is a constant. 82 mm syms t t1 t2 t3 t4 t5 t6 t1d t2d t3d t4d t5d t6d ... real ;
73
C1=1.28; % Shape dependent drag coefficient C2=0.295; p=1; % Density of Water % Linear Velocity Calculation % Assumption 1 : the initial position of the first link as (0,0) % Assumption 2 : Centroid of each link l is at the center of the link % (l/2) % Centroid Calculations x1f = (lc1)*cos(t1); y1f = (lc1)*sin(t1); x2f = l1*cos(t1) + (lc2)*cos(t2); y2f = l1*sin(t1) + (lc2)*sin(t2); x3f = l1*cos(t1) + (l2)*cos(t2) + (lc3)*cos(t3); y3f = l1*sin(t1) + (l2)*sin(t2) + (lc3)*sin(t3); x4f = l1*cos(t1) + (l2)*cos(t2) + (l3)*cos(t3) + (lc4)*cos(t4); y4f = l1*sin(t1) + (l2)*sin(t2) + (l3)*sin(t3) + (lc4)*sin(t4); x5f = lc5*cos(t5)*cos(t1) + lc*sin(t1); y5f = -lc*cos(t1) + lc5*cos(t5)*sin(t1); x6f = lc6*cos(t6)*cos(t1) + lc*sin(t1); y6f = lc*cos(t1) + lc6*cos(t6)*sin(t1); old_t = {'t1', 't2', 't3', 't4'}; new_t = {'t1(t)','t2(t)', 't3(t)','t4(t)'}; old_td={'diff(t1(t), t)','diff(t2(t), t)','diff(t3(t), t)','diff(t4(t), t)'}; new_td={'t1d','t2d','t3d','t4d'}; old_tdp={'diff(t5(t), t)','diff(t6(t), t)'}; new_tdp={'t5d','t6d'}; % FORCES ACTING ON LINK ONE %Pressure on link - 1 % Velocity Calculation S1=0.0092100; % effective area of the link-1 in mm^2 mu1per = p * C1 * S1/2; x1f = subs(x1f, old_t, new_t); y1f = subs(y1f, old_t, new_t); v1per = diff(x1f,t)*sin(t1) - diff(y1f,t)*cos(t1); F1per = -mu1per * v1per * abs(v1per); F1per = subs(F1per, old_td,new_td); F1per; % Approach Stream Pressure on link -1 mu1par = p * C2 * S1/2; x1f = subs(x1f, old_t, new_t); y1f = subs(y1f, old_t, new_t); v1par = diff(x1f,t)*cos(t1) + diff(y1f,t)*sin(t1); F1par = -mu1par * v1par * abs(v1par); F1par = subs(F1par, old_td, new_td); F1par; % Frictional Drag on link - 1 F1f = 0.5*F1par; % Composite Dynamic Forces on LINK - 1 F1hydro = F1per + F1par + F1f; F1hydro; % FORCES ACTING ON LINK TWO %Pressure on link - 2 % Velocity Calculation
74
S2=0.00183600; % effective area of the link-1 in mm^2 mu2per = p * C1 * S2/2; x2f = subs(x2f, old_t, new_t); y2f = subs(y2f, old_t, new_t); v2per = diff(x2f,t)*sin(t2) - diff(y2f,t)*cos(t2); F2per = -mu2per * v2per * abs(v2per); F2per = subs(F2per, old_td,new_td); F2per; % Approach Stream Pressure on link -2 mu2par = p * C2 * S2/2; x2f = subs(x2f, old_t, new_t); y2f = subs(y2f, old_t, new_t); v2par = diff(x2f,t)*cos(t2) + diff(y2f,t)*sin(t2); F2par = -mu2par * v2par * abs(v2par); F2par = subs(F2par, old_td, new_td); F2par; % Frictional Drag on link - 2 F2f = 0.5*F2par; % Composite Dynamic Forces on LINK - 2 F2hydro = F2per + F2par + F2f; F2hydro; % FORCES ACTING ON LINK THREE %Pressure on link - 3 % Velocity Calculation S3=0.00183600; % effective area of the link-1 in mm^2 mu3per = p * C1 * S3/2; x3f = subs(x3f, old_t, new_t); y3f = subs(y3f, old_t, new_t); v3per = diff(x3f,t)*sin(t3) - diff(y3f,t)*cos(t3); F3per = -mu3per * v3per * abs(v3per); F3per = subs(F3per, old_td,new_td); F3per; % Approach Stream Pressure on link -3 mu3par = p * C2 * S3/2; x3f = subs(x3f, old_t, new_t); y3f = subs(y3f, old_t, new_t); v3par = diff(x3f,t)*cos(t3) + diff(y3f,t)*sin(t3); F3par = -mu3par * v3par * abs(v3par); F3par = subs(F3par, old_td, new_td); F3par; % Frictional Drag on link - 3 F3f = 0.5*F3par; % Composite Dynamic Forces on LINK - 3 F3hydro = F3per + F3par + F3f; F3hydro; % FORCES ACTING ON LINK FOUR %Pressure on link - 4 % Velocity Calculation S4=0.006000; % effective area of the link-1 in mm^2 mu4per = p * C1 * S4/2; x4f = subs(x4f, old_t, new_t); y4f = subs(y4f, old_t, new_t);
75
v4per = diff(x4f,t)*sin(t4) - diff(y4f,t)*cos(t4); F4per = -mu4per * v4per * abs(v4per); F4per = subs(F4per, old_td,new_td); F4per; % Approach Stream Pressure on link -4 mu4par = p * C2 * S4/2; x4f = subs(x4f, old_t, new_t); y4f = subs(y4f, old_t, new_t); v4par = diff(x4f,t)*cos(t4) + diff(y4f,t)*sin(t4); F4par = -mu4par * v4par * abs(v4par); F4par = subs(F4par, old_td, new_td); F4par; % Frictional Drag on link - 4 F4f = 0.5*F4par; % Composite Dynamic Forces on LINK - 1 F4hydro = F4per + F4par + F4f; F4hydro; % Pressure on pectoral FINS S5 = 0.0027600; % effective area of the left pectoral fin in mm^2 S6 = 0.0027600; % effective area of the right pectoral fin in mm^2 % Left Pectoral Fin muLper = p * C1 * S5/2; x5f = subs(x5f, old_t, new_t); y5f = subs(y5f, old_t, new_t); VLper = diff(x5f,t)*cos(t1)*sin(t5) + diff(y5f,t)*sin(t1)*sin(t5); VLper = subs(VLper, old_tdp, new_tdp); FLper = -muLper * VLper * abs(VLper); FLper; % Approach Stream Pressure on LEFT PECTORAL FIN mu5par = p * C2 * S5/2; x5f = subs(x5f, old_t, new_t); y5f = subs(y5f, old_t, new_t); v5par = diff(x5f,t)*cos(t5) + diff(y5f,t)*sin(t5); F5par = -mu5par * v5par * abs(v5par); F5par = subs(F5par, old_td, new_td); F5par; % Frictional Drag on left pectoral fin F5f = 0.5*F5par; % Right Pectoral Fin muRper = p * C1 * S6/2; x6f = subs(x6f, old_t, new_t); y6f = subs(y6f, old_t, new_t); VRper = diff(x6f,t)*cos(t1)*sin(t6) + diff(y6f,t)*sin(t1)*sin(t6); VRper = subs(VRper, old_tdp, new_tdp); FRper = -muRper * VRper * abs(VRper); FRper; % Approach Stream Pressure on LEFT PECTORAL FIN mu6par = p * C2 * S6/2; x6f = subs(x6f, old_t, new_t); y6f = subs(y6f, old_t, new_t); v6par = diff(x6f,t)*cos(t6) + diff(y6f,t)*sin(t6); F6par = -mu6par * v6par * abs(v6par); F6par = subs(F6par, old_td, new_td); F6par; % Frictional Drag on left pectoral fin F6f = 0.5*F6par; % Composite Dynamic Forces on PECTORAL FINS Fpectoral_hydro = FLper + F5par + F5f + FRper + F6par + F6f; Fpectoral_hydro;
--------------------------------------------------------------------------------------------------------------------------
76
APPENDIX B
APPENDIX B: SYSTEM EQUATIONS AND MATRICES
11 21
=
31 41 51 61
12 22 32 42 52 62
13 23 33 43 53 63
14
15
24
25
34
35
44
45
54
55
64
65
16
46
2
2
=
56 66
3
4
5 6
1
1
26 36
=
3 4
5 6
% Inertial Mass Matrix calculated with length l and mass m vectors from the model
11
= [ m5*(a1*cos(th1) - a1*cos(th1)*cos(th5) + a1*sin(th1)*sin(th5))^2 + m6*(a1*cos(th1) -
a1*cos(th1)*cos(th6) + a1*sin(th1)*sin(th6))^2 + m5*(a1*cos(th1)*sin(th5) - a1*sin(th1) + a1*cos(th5)*sin(th1))^2 + m6*(a1*cos(th1)*sin(th6) - a1*sin(th1) + a1*cos(th6)*sin(th1))^2 + m2*(a1*cos(th1) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2))^2 + m2*(a1*sin(th1) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1))^2 + m4*(a1*cos(th1) + l2*cos(th3)*(cos(th1) *cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1 *cos(th1)*cos(th2) l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4) *(cos(th3) *(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))))^2 + m4*(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3) *(cos(th1)*cos(th2) sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3 *cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1) *cos(th2) sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3) *(cos(th1)*sin(th2) + cos(th2)*sin(th1))))^2 + m3*(a1*cos(th1) + l2*cos(th3)*(cos(th1)*cos(th2) sin(th1) *sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) l1*sin(th1)*sin(th2))^2 + m3*(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1* cos(th2)*sin(th1))^2 +(I31*l1*m1)/24+(I32*l2*m2)/24+(I33*l3*m3)/24+(I34*l4*m4)/24+(I35*l5*m5)/24+(I36*l6*m6)/ 24 +a1^2*m1*cos(th1)^2+a1^2*m1*sin(th1)^2,
12
= m2*(l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2))*(a1*cos(th1) + l1*cos(th1)*cos(th2) -
l1*sin(th1)*sin(th2)) + m2*(l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1))*(a1*sin(th1) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1)) + m3*(l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) l1*sin(th1)*sin(th2))*(a1*cos(th1) + l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2)) + (I32*l2*m2)/24 + (I33*l3*m3)/24 + (I34*l4*m4)/24 + m3*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1))*(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) +
77
l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1)) + m4*(l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1) *sin(th2))))*(a1*cos(th1) + l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) + m4*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2) *sin(th1) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))*(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1) *cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4)*(cos(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))),
= m3*(l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) *(a1*cos(th1) + l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) 13
l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2) *sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2)) + m3*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2) *sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)))*(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1* cos(th2) *sin(th1)) + m4*(l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2) *sin(th1)) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))))*(a1*cos(th1) + l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1) *cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) l3*sin(th4)*(cos(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)))) + m4*(l2*cos(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l3*cos(th4)*(cos(th3) *(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4) *(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))) *(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) -
78
sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))) + (I33*l3*m3)/24 + (I34*l4*m4)/24,
= (I34*l4*m4)/24 + m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3) *(cos(th1)*cos(th2) - sin(th1)*sin(th2))))*(a1*cos(th1) + 14
l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2) + l3*cos(th4) *(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))- l3 *sin(th4) *(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) + m4*(l3 *cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1) *sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))) *(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3) *(cos(th1) *cos(th2) - sin(th1) *sin(th2)) + l1* cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4)*(cos(th3) *(cos(th1) *sin(th2) + cos(th2) *sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))),
= m5*(a1*cos(th1)*sin(th5) + a1*cos(th5)*sin(th1))*(a1*cos(th1)*sin(th5) - a1*sin(th1) + a1*cos(th5)*sin(th1)) - m5*(a1*cos(th1)*cos(th5) - a1*sin(th1)*sin(th5))*(a1*cos(th1) 15
a1*cos(th1)*cos(th5) + a1*sin(th1)*sin(th5)) + (I35*l5*m5)/24, 16
= m6*(a1*cos(th1)*sin(th6) + a1*cos(th6)*sin(th1))*(a1*cos(th1)*sin(th6) - a1*sin(th1) +
a1*cos(th6)*sin(th1)) - m6*(a1*cos(th1)*cos(th6) - a1*sin(th1)*sin(th6))*(a1*cos(th1) a1*cos(th1)*cos(th6) + a1*sin(th1)*sin(th6)) + (I36*l6*m6)/24] 21
= [ m2*(l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2))*(a1*cos(th1) + l1*cos(th1)*cos(th2) -
l1*sin(th1) *sin(th2)) + m2*(l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1))*(a1*sin(th1) + l1*cos(th1)*sin(th2) + l1*cos(th2) *sin(th1)) + m3*(l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2) *sin(th1)) + l1*cos(th1)*cos(th2) l1*sin(th1)*sin(th2))*(a1*cos(th1) + l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1) *sin(th2)) + (I32*l2*m2)/24 + (I33*l3*m3)/24 + (I34*l4*m4)/24 + m3*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2) *sin(th1))*(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1)) + m4*(l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1) *sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) *(a1*cos(th1) + l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2) *sin(th1)) + l1*cos(th1)*cos(th2) -
79
l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1) *sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) + m4*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2) *sin(th1) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))*(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3) *(cos(th1) *cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4)*(cos(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) -
sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))),
= m3*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) sin(th1) *sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1))^2 + 22
m3*(l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1) *sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2))^2 + m2*(l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1))^2 + m2*(l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2))^2 + m4 *(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) + l1 *cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))^2 + m4*(l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) l1*sin(th1)*sin(th2) + l3*cos(th4) *(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4) *(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))))^2 + (I32*l2*m2)/24 + (I33*l3*m3)/24 + (I34*l4*m4)/24,
= (I33*l3*m3)/24 + (I34*l4*m4)/24 + m3*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2 *sin(th3) *(cos(th1)*cos(th2) 23
sin(th1)*sin(th2)))*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2 *sin(th3) *(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1)) + m3*(l2 *cos(th3) *(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))*(l2 *cos(th3) *(cos(th1)*cos(th2) - sin(th1)*sin(th2)) l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1 *cos(th1) *cos(th2) - l1*sin(th1)*sin(th2)) + m4*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2 *sin(th3) *(cos(th1)*cos(th2) sin(th1)*sin(th2)) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2 *sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4) *(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) -
80
sin(th1)*sin(th2))) + l3 *sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))) + m4*(l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1) *sin(th2) )))*(l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))),
= m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1) *sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + 24
cos(th2)*sin(th1))))*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))) + m4*(l3*cos(th4)*(cos(th3) *(cos(th1) *cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3) *(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))))*(l2*cos(th3) *(cos(th1)*cos(th2) - sin(th1)*sin(th2)) l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) + (I34*l4*m4)/24,
25
= 0,
26
=0]
31
= [m3*(l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) +
cos(th2) *sin(th1))) *(a1*cos(th1) + l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2) *sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2)) + m3*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2) *sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)))*(a1*sin(th1) + l2*cos(th3) *(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2) *sin(th1)) + m4*(l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2) *sin(th1)) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3) *(cos(th1)*cos(th2) - sin(th1)*sin(th2))))*(a1*cos(th1) + l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) l1*sin(th1)*sin(th2) + l3*cos(th4) *(cos(th3)*(cos(th1) *cos(th2) - sin(th1)*sin(th2)) -
81
sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3 *sin(th4)*(cos(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) + m4*(l2*cos(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l3*cos(th4)*(cos(th3) *(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))) + l3*sin(th4) *(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1) ))) *(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))) + (I33*l3*m3)/24 + (I34*l4*m4)/24,
= (I33*l3*m3)/24 + (I34*l4*m4)/24 + m3*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2 *sin(th3) *(cos(th1)*cos(th2) 32
sin(th1)*sin(th2)))*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2 *sin(th3) *(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1)) + m3*(l2 *cos(th3) *(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) *(l2 *cos(th3) *(cos(th1)*cos(th2) - sin(th1)*sin(th2)) l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1 *cos(th1) *cos(th2) - l1*sin(th1)*sin(th2)) + m4*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2 *sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2 *sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4) *(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))) + l3 *sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))) + m4*(l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1) *sin(th2) )))*(l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))),
33
= m3*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) -
sin(th1) *sin(th2)))^2 + m3*(l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))^2 + m4*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1) *cos(th2) - sin(th1)*sin(th2)) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3) *(cos(th1)*cos(th2) -
82
sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))^2 + m4*(l2*cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1) *sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))))^2 + (I33*l3*m3)/24 + (I34*l4*m4)/24,
= m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) 34
sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))) + m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))))*(l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)))) + (I34*l4*m4)/24,
35
= 0,
36
=0]
41
= [(I34*l4*m4)/24 + m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) -
sin(th3) *(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))))*(a1*cos(th1) + l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1) *sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) + m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))+sin(th3)*(cos(th1)*cos(th2)-sin(th1)*sin(th2)))+ l3*sin(th4)*(cos(th3)*(cos(th1) *cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))*(a1*sin(th1) + l2*cos(th3)*(cos(th1)*sin(th2)+cos(th2)*sin(th1))+l2*sin(th3)*(cos(th1)*cos(th2)*sin(th1)*sin(th2) ) +l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4) *(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))+sin(th3)*(cos(th1)*cos(th2)-sin(th1)*sin(th2)))+ l3*sin(th4)*(cos(th3)*(cos(th1) *cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))),
83
42
= m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) +
sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l1*cos(th1)*sin(th2) + l1*cos(th2)*sin(th1) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))) + m4*(l3*cos(th4)*(cos(th3) *(cos(th1) *cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) l3*sin(th4)*(cos(th3)*(cos(th1) *sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2))))*(l2*cos(th3) *(cos(th1) *cos(th2) - sin(th1)*sin(th2)) l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l1*cos(th1)*cos(th2) - l1*sin(th1)*sin(th2) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) +
sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) + (I34*l4*m4)/24, 43
= m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) +
sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))*(l2*cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) + l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))) + m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))+sin(th3)*(cos(th1)*cos(th2)-sin(th1)*sin(th2))))*(l2*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l2*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)))-l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2)+cos(th2)*sin(th1))+ sin(th3)*(cos(th1)
*cos(th2)-sin(th1)*sin(th2))))+(I34*l4*m4)/24,
= m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))) + l3*sin(th4)*(cos(th3)*(cos(th1)*cos(th2) 44
sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))^2 + m4*(l3*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l3*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2))))^2 + (I34*l4*m4)/24,
45
= 0,
46
=0]
= [m5*(a1*cos(th1)*sin(th5) + a1*cos(th5)*sin(th1))*(a1*cos(th1)*sin(th5) - a1*sin(th1) + a1*cos(th5)*sin(th1)) - m5*(a1*cos(th1)*cos(th5) - a1*sin(th1)*sin(th5))*(a1*cos(th1) 51
84
a1*cos(th1)*cos(th5) + a1*sin(th1)*sin(th5)) + (I35*l5*m5)/24,
54
= 0, = 0, = 0,
55
= m5*(a1*cos(th1)*sin(th5) + a1*cos(th5)*sin(th1))^2 + m5*(a1*cos(th1)*cos(th5) -
52 53
a1*sin(th1)*sin(th5))^2 + (I35*l5*m5)/24, 56
= 0]
61
= [m6*(a1*cos(th1)*sin(th6) + a1*cos(th6)*sin(th1))*(a1*cos(th1)*sin(th6) - a1*sin(th1) +
a1*cos(th6)*sin(th1)) - m6*(a1*cos(th1)*cos(th6) - a1*sin(th1)*sin(th6))*(a1*cos(th1) a1*cos(th1)*cos(th6) + a1*sin(th1)*sin(th6)) + (I36*l6*m6)/24, 62
= 0,
64
= 0, = 0,
65
= 0,
66
= m6*(a1*cos(th1)*sin(th6) + a1*cos(th6)*sin(th1))^2 + m6*(a1*cos(th1)*cos(th6) -
63
a1*sin(th1)*sin(th6))^2 + (I36*l6*m6)/24] -------------------------------------------------------------------------------------------------------------------------% Coriolis Forces and Velocity Coupling Vector
= - th3d^2*((m4*(2*l2*l3*sin(th3) + l1*l4*sin(th2 + th3 + th4) + 2*l1*l3*sin(th2 + th3) + l2*l4*sin(th3 + th4)))/2 + (l3*m3*(l1*sin(th2 + th3) + l2*sin(th3)))/2) 1
th2d^2*((l1*m4*(2*l3*sin(th2 + th3) + 2*l2*sin(th2) + l4*sin(th2 + th3 + th4)))/2 + (l1*m3*(l3*sin(th2 + th3) + 2*l2*sin(th2)))/2 + (l1*l2*m2*sin(th2))/2) 2*th1d*th3d*((m4*(2*l2*l3*sin(th3) + l1*l4*sin(th2 + th3 + th4) + 2*l1*l3*sin(th2 + th3) + l2*l4*sin(th3 + th4)))/2 + (l3*m3*(l1*sin(th2 + th3) + l2*sin(th3)))/2) 2*th2d*th3d*((m4*(2*l2*l3*sin(th3) + l1*l4*sin(th2 + th3 + th4) + 2*l1*l3*sin(th2 + th3) + l2*l4*sin(th3 + th4)))/2 + (l3*m3*(l1*sin(th2 + th3) + l2*sin(th3)))/2) 2*th1d*th2d*((l1*m4*(2*l3*sin(th2 + th3) + 2*l2*sin(th2) + l4*sin(th2 + th3 + th4)))/2 + (l1*m3*(l3*sin(th2 + th3) + 2*l2*sin(th2)))/2 + (l1*l2*m2*sin(th2))/2) - (l4*m4*th4d^2*(l2*sin(th3 + th4) + l3*sin(th4) + l1*sin(th2 + th3 + th4)))/2 - (l1*m5*th5d^2*sin(th5)*(l1 - l5))/2 (l1*m6*th6d^2*sin(th6)*(l1 - l6))/2 - l4*m4*th1d*th4d*(l2*sin(th3 + th4) + l3*sin(th4) + l1*sin(th2 + th3 + th4)) - l4*m4*th2d*th4d*(l2*sin(th3 + th4) + l3*sin(th4) + l1*sin(th2 + th3 + th4)) - l4*m4*th3d*th4d*(l2*sin(th3 + th4) + l3*sin(th4) + l1*sin(th2 + th3 + th4)) l1*m5*th1d*th5d*sin(th5)*(l1 - l5) - l1*m6*th1d*th6d*sin(th6)*(l1 - l6)
2
= th1d^2*((l1*m4*(2*l3*sin(th2 + th3) + 2*l2*sin(th2) + l4*sin(th2 + th3 + th4)))/2 +
(l1*m3*(l3*sin(th2 + th3) + 2*l2*sin(th2)))/2 + (l1*l2*m2*sin(th2))/2) -
85
th3d^2*((l2*m4*(l4*sin(th3 + th4) + 2*l3*sin(th3)))/2 + (l2*l3*m3*sin(th3))/2) 2*th1d*th3d*((l2*m4*(l4*sin(th3 + th4) + 2*l3*sin(th3)))/2 + (l2*l3*m3*sin(th3))/2) 2*th2d*th3d*((l2*m4*(l4*sin(th3 + th4) + 2*l3*sin(th3)))/2 + (l2*l3*m3*sin(th3))/2) (l4*m4*th4d^2*(l2*sin(th3 + th4) + l3*sin(th4)))/2 - l4*m4*th1d*th4d*(l2*sin(th3 + th4) + l3*sin(th4)) - l4*m4*th2d*th4d*(l2*sin(th3 + th4) + l3*sin(th4)) - l4*m4*th3d*th4d*(l2*sin(th3 + th4) + l3*sin(th4))
3
= th1d^2*((m4*(2*l2*l3*sin(th3) + l1*l4*sin(th2 + th3 + th4) + 2* l1*l3*sin(th2 + th3) +
l2*l4*sin(th3 + th4)))/2 + (l3*m3*(l1*sin(th2 + th3) + l2*sin(th3)))/2) + th2d^2*((l2*m4*(l4*sin(th3 + th4) + 2*l3*sin(th3)))/2 + (l2*l3*m3*sin(th3))/2) + 2*th1d*th2d*((l2*m4*(l4*sin(th3 + th4) + 2*l3*sin(th3)))/2 + (l2*l3*m3*sin(th3))/2) - (l3*l4*m4*th4d^2*sin(th4))/2 l3*l4*m4*th1d*th4d*sin(th4) - l3*l4*m4*th2d*th4d*sin(th4) - l3*l4*m4*th3d*th4d*sin(th4)
= (l4*m4*(l2*sin(th3 + th4) + l3*sin(th4) + l1*sin(th2 + th3 + th4))*th1d^2)/2 + l4*m4*(l2*sin(th3 + th4) + l3*sin(th4))*th1d*th2d + l3*l4*m4*sin(th4)*th1d*th3d + 4
(l4*m4*(l2*sin(th3 + th4) + l3*sin(th4))*th2d^2)/2 + l3*l4*m4*sin(th4)*th2d*th3d + (l3*l4*m4*sin(th4)*th3d^2)/2
5
= (l1*m5*th1d^2*sin(th5)*(l1 - l5))/2
6
= (l1*m6*th1d^2*sin(th6)*(l1 - l6))/2
----------------------------------------------------------------------------------------------------------------------------------% Gravitational Vector
= g*m2*(l1*cos(th1) + l2*cos(th1)*cos(th2) - l2*sin(th1)*sin(th2)) + g*m5*(l1*cos(th1) + (l1*cos(th1)*cos(th5))/2 - (l1*sin(th1)*sin(th5))/2) + g*m6*(l1*cos(th1) + (l1*cos(th1)*cos(th6))/2 - (l1*sin(th1)*sin(th6))/2) + g*m4*(l1*cos(th1) + l3*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) 1
- l3*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*cos(th1)*cos(th2) - l2*sin(th1)*sin(th2) + l4*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l4*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) + g*m3*(l1*cos(th1) + l3*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l3*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*cos(th1)*cos(th2) - l2*sin(th1)*sin(th2)) - (g*l2*m2*(cos(th1)*cos(th2) sin(th1)*sin(th2)))/2 - (g*l5*m5*(cos(th1)*cos(th5) - sin(th1)*sin(th5)))/2 (g*l6*m6*(cos(th1)*cos(th6) - sin(th1)*sin(th6)))/2 + (g*l1*m1*cos(th1))/2 (g*l4*m4*(cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) +
86
sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))))/2 - (g*l3*m3*(cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))/2
= g*m3*(l3*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l3*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*cos(th1)*cos(th2) - l2*sin(th1)*sin(th2)) + g*m2*(l2*cos(th1)*cos(th2) 2
l2*sin(th1)*sin(th2)) + g*m4*(l3*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) l3*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l2*cos(th1)*cos(th2) - l2*sin(th1)*sin(th2) + l4*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - l4*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) - (g*l2*m2*(cos(th1)*cos(th2) sin(th1)*sin(th2)))/2 - (g*l4*m4*(cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))+sin(th3)*(cos(th1)*cos(th2)-sin(th1)*sin(th2)))))/2-g*l3*m3*(cos(th3)*(cos(th1) *cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))/2
3
= g*m3*(l3*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - l3*sin(th3)*(cos(th1)*sin(th2) +
cos(th2)*sin(th1))) + g*m4*(l3*cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) l3*sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + l4*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) l4*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) sin(th1)*sin(th2)))) - (g*l4*m4*(cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))))/2 - (g*l3*m3*(cos(th3) *(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))))/2
= g*m4*(l4*cos(th4)*(cos(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)) - sin(th3) *(cos(th1) *sin(th2) + cos(th2)*sin(th1))) - l4*sin(th4)*(cos(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + 4
sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))) - (g*l4*m4*(cos(th4) *(cos(th3)*(cos(th1) *cos(th2) - sin(th1)*sin(th2)) - sin(th3)*(cos(th1)*sin(th2) + cos(th2)*sin(th1))) - sin(th4)*(cos(th3) *(cos(th1)*sin(th2) + cos(th2)*sin(th1)) + sin(th3)*(cos(th1)*cos(th2) - sin(th1)*sin(th2)))))/2
= g*m5*((l1*cos(th1)*cos(th5))/2 - (l1*sin(th1)*sin(th5))/2) - (g*l5*m5*(cos(th1)*cos(th5) sin(th1)*sin(th5)))/2 5
= g*m6*((l1*cos(th1)*cos(th6))/2 - (l1*sin(th1)*sin(th6))/2) - (g*l6*m6*(cos(th1)*cos(th6) sin(th1)*sin(th6)))/2 6
-----------------------------------------------------------------------------------------------------------------------------------
87
APPENDIX C
APPENDIX C: REFERENCES [1] M.Sfakiotakis, D.M.Lan e, and J.B.C.Davies, “Review of fish swimming modes for aquatic locomotion”, IEEE Journal of Oceanic Engineering 24, pp. 237 – 252 (1999). *2+ P. W. Webb, “Form and function in fish swimming”, Scientific American, vol. 251(1), pp.
58-68, 1984. [3] J.A.Sparenberg, James LightHill’s Memorial Paper 2002, Survey of the mathematical theory of fish locomotion, Journal of Engineering Mathematics vol. 44, Netherlands 2002, pg 395 –448. [4] M. Yousuff Husaini (ed.), Collected Works of Sir James Lighthill. New York: Oxford University Press (1996) 532 pp. [5] David S. Barrett. Propulsive Efficiency of a Flexible Hull Underwater Vehicle, PhD Thesis, MIT (1996). [6] Junzhi Yu, Shuo Wong and Min Tan, Design of a Free-swimming Biomimetic Robot Fish, Proceedings at the 2003 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM 2003), China, pp .95 – 100. [7] Thomas Alan Trapp, Modelling and Control of a Fish-Like Vehicle, Master of Science Thesis, MIT, June 1998. *8+ M. J. Lighthill, “Note on the Swimming of Slender Fish”, Ann. Rev. Fluid Mech., Vol. 44,
NO. 2, pp. 256-301, 1970. [9] T.Y.Wu, Swimming of a waving plate, J. Fluid Mech. 10 (1961) 321 –344. [10] M.J. Lighthill, Large amplitude elongated body theory of fish locomotion. Proc. R. Soc. London B179 (1971) 125 –138. [11] RoboTuna I (robot-tuna), David Barrett, and RoboTuna II, David Beal and Michael Sachinis, MIT, the U.S.A, http://web.mit.edu/towtank/www/Tuna/tuna.html [12] RoboPike, John Kumph, MIT, the U.S.A, http://web.mit.edu/towtank/www/Pike/pike.html [13] PPF-04, Koichi Hirata, NMRI, Japan, http://www.nmri.go.jp/eng/khirata/fish/ [14] Robotic Eel, Robea Project, multi-field team, CNRS, France http://www.lag.ensieg.inpg.fr/fr/projets.php?projet=Anguille [15] Boxybot (from boxfish), Daisy Lachat, BIRG – EPFL, Suiss http://biorob.epfl.ch/boxybot
88
[16] Essex Robotic Fish, Jindong Liu, Huosheng Hu, Dept of Computer Science University of Essex, G.B. http://dces.essex.ac.uk/staff/hhu/Publicity_Articles/Robotic%20Fishes.pdf and http://www.roboshoal.com/? [17] Robotic koi (carp koï), Ryomei Engineering, Japan http://www.youtube.com/watch?v=M7YGEVuJ4mM and http://www.youtube.com/watch?v=3P0aafialbg [18] Jessiko, RobotSwim, France http://www.robotswim.com/ and http://www.youtube.com/watch?v=yO-3s23HHUU [19] RoboFish, University of Washington, U.S.A. http://vger.aa.washington.edu/fish_project.html [20] Kristi A. Morgansen, Benjamin I. Triplett, and Daniel J. Klein, Geometric Methods for Modelling and Control of Free-Swimming Fin-Actuated Underwater Vehicles, IEEE Transactions ON Robotics, VOL. 23, NO. 6, Dec 2007, pp. 1184 – 1199. [21] Stingray, Knifefish and overs, Nanyang Technological University, Singapore, http://www.robotic-fish.net/images/korea231106.pdf http://www.youtube.com/watch?v=FKVNprWTceo [22] Tai-robot-kun, University of Kitakyushu, Japan http://www.youtube.com/watch?v=WINc1mV-L8Y [23] Han Wang, Sungkono Surya Tjahyono, Bruce MacDonald, Paul A. Kilmartin, Jadranka Travas-Sejdic, and Rudolf Kiefer, Robotic Fish Based on a Polymer Actuator, Polymer Electronic Research Centre, University of Auckland, New Zealand. [24] Frederick Michael Carter, Motion Control of Non-Fixed Base Robotic Manipulators, Master of Applied Science Thesis, Mechanical Engineering, University of British Columbia. [25] Jindong Liu and Huosheng Hu, Building a 3D Simulator for Autonomous Navigation of Robotic Fishes, University of Essex, Proceedings of 2004 IEEE/RSJ International Conference on Intelligent Robot and Systems, Japan, pp 613 – 618. *26+ Rui Ding, Junzhi Yu, Qinghai Yang, Min Tan, and Jianwei Zhang , “CPG -based Dynamics
Modelling and Simulation for a Biomimetic Amphibious Robot”, Proceedings of the 2009 IEEE International Conference on Robotics and Biomimetics, Dec 2009, Guilin, China, pg 1657 -1662. [27] S. Guo, Y. Okuda, K. Asaka, A Novel Type of Underwater Micro Biped Robot with Multi DOF, Proceedings of the 2004 IEEE International Conference on Robotics & Automation, pp.4881-4886, 2004.
89
