JAMRIS 2017 Vol 11 No 2

VOLUME 11 N°2 2017 www.jamris.org pISSN 1897-8649 (PRINT) / eISSN 2080-2145 (ONLINE)

Indexed in SCOPUS

JOURNAL OF AUTOMATION, MOBILE ROBOTICS & INTELLIGENT SYSTEMS

Editor-in-Chief

Associate Editor:

Janusz Kacprzyk

Maciej Trojnacki (PIAP, Poland)

(Polish Academy of Sciences, PIAP, Poland)

Statistical Editor: Małgorzata Kaliczynska (PIAP, Poland)

Advisory Board: Dimitar Filev (Research & Advenced Engineering, Ford Motor Company, USA)

Typesetting:

Kaoru Hirota (Japan Society for the Promotion of Science, Beijing Office)

Ewa Markowska, PIAP

Jan Jabłkowski (PIAP, Poland) Witold Pedrycz (ECERF, University of Alberta, Canada)

Webmaster: Piotr Ryszawa, PIAP

Co-Editors: Roman Szewczyk (PIAP, Warsaw University of Technology)

Editorial Office:

Marek Zaremba (University of Quebec, Canada)

Industrial Research Institute for Automation and Measurements PIAP Al. Jerozolimskie 202, 02-486 Warsaw, POLAND Tel. +48-22-8740109, office@jamris.org

Executive Editor:

Copyright and reprint permissions Executive Editor

Anna Ładan aladan@piap.pl

The reference version of the journal is e-version. Printed in 300 copies.

Editorial Board:

Mark Last (Ben-Gurion University, Israel) Anthony Maciejewski (Colorado State University, USA) Krzysztof Malinowski (Warsaw University of Technology, Poland) Andrzej Masłowski (Warsaw University of Technology, Poland) Patricia Melin (Tijuana Institute of Technology, Mexico) Fazel Naghdy (University of Wollongong, Australia) Zbigniew Nahorski (Polish Academy of Sciences, Poland) Nadia Nedjah (State University of Rio de Janeiro, Brazil) Dmitry A. Novikov (Institute of Control Sciences Sciences, Russian Academy of Sciences, Moscow, Russia) Duc Truong Pham (Birmingham University, UK) Lech Polkowski (Polish-Japanese Institute of Information Technology, Poland) Alain Pruski (University of Metz, France) Rita Ribeiro (UNINOVA, Instituto de Desenvolvimento de Novas Tecnologias, Caparica, Portugal) Imre Rudas (Óbuda University, Hungary) Leszek Rutkowski (Czestochowa University of Technology, Poland) Alessandro Saffiotti (Örebro University, Sweden) Klaus Schilling (Julius-Maximilians-University Wuerzburg, Germany) Vassil Sgurev (Bulgarian Academy of Sciences, Department of Intelligent Systems, Bulgaria) Helena Szczerbicka (Leibniz Universität, Hannover, Germany) Ryszard Tadeusiewicz (AGH University of Science and Technology in Cracow, Poland) Stanisław Tarasiewicz (University of Laval, Canada) Piotr Tatjewski (Warsaw University of Technology, Poland) Rene Wamkeue (University of Quebec, Canada) Janusz Zalewski (Florida Gulf Coast University, USA) Teresa Zielinska (Warsaw University of Technology, Poland)

Oscar Castillo (Tijuana Institute of Technology, Mexico)

Chairman - Janusz Kacprzyk (Polish Academy of Sciences, PIAP, Poland) Plamen Angelov (Lancaster University, UK) Adam Borkowski (Polish Academy of Sciences, Poland) Wolfgang Borutzky (Fachhochschule Bonn-Rhein-Sieg, Germany) Bice Cavallo (University of Naples Federico II, Napoli, Italy) Chin Chen Chang (Feng Chia University, Taiwan) Jorge Manuel Miranda Dias (University of Coimbra, Portugal) Andries Engelbrecht (University of Pretoria, Republic of South Africa) Pablo Estévez (University of Chile) Bogdan Gabrys (Bournemouth University, UK) Fernando Gomide (University of Campinas, São Paulo, Brazil) Aboul Ella Hassanien (Cairo University, Egypt) Joachim Hertzberg (Osnabrück University, Germany) Evangelos V. Hristoforou (National Technical University of Athens, Greece) Ryszard Jachowicz (Warsaw University of Technology, Poland) Tadeusz Kaczorek (Bialystok University of Technology, Poland) Nikola Kasabov (Auckland University of Technology, New Zealand) Marian P. Kazmierkowski (Warsaw University of Technology, Poland) Laszlo T. Kóczy (Szechenyi Istvan University, Gyor and Budapest University of Technology and Economics, Hungary) Józef Korbicz (University of Zielona Góra, Poland) Krzysztof Kozłowski (Poznan University of Technology, Poland) Eckart Kramer (Fachhochschule Eberswalde, Germany) Rudolf Kruse (Otto-von-Guericke-Universität, Magdeburg, Germany) Ching-Teng Lin (National Chiao-Tung University, Taiwan) Piotr Kulczycki (AGH University of Science and Technology, Cracow, Poland) Andrew Kusiak (University of Iowa, USA)

Publisher: Industrial Research Institute for Automation and Measurements PIAP

If in doubt about the proper edition of contributions, please contact the Executive Editor. Articles are reviewed, excluding advertisements and descriptions of products. All rights reserved © Articles

1

JOURNAL OF AUTOMATION, MOBILE ROBOTICS & INTELLIGENT SYSTEMS VOLUME 11, N° 2, 2017 DOI: 10.14313/JAMRIS_2-2017

CONTENTS 48

3

Editorial – Robot modelling, perception, and motion synthesis 5

Modeling and Simulation of a Tracked Mobile Inspection Robot in Matlab and V-Rep Software, Mariusz Giergiel DOI: 10.14313/JAMRIS_2-2017/11 12

to the Modelling of Human Robot Motion, " # $ # % DOI: 10.14313/JAMRIS_2-2017/12 21

! "

# # &' ( # ) * DOI: 10.14313/JAMRIS_2-2017/13 31

$ % % ! & % #

" # DOI: 10.14313/JAMRIS_2-2017/14 42

'% ( ) % % ! (&( * # Gyroscope and Magnetometer, +"# , 5 & ' & & & 6 DOI: 10.14313/JAMRIS_2-2017/15

2

Articles

% + ' .% ( / # 9 '" ( # ( , DOI: 10.14313/JAMRIS_2-2017/16 57

+ % 0 1 ( 1 $ ( # + # < & % = DOI: 10.14313/JAMRIS_2-2017/17 67

2 2 % ( / # > ? 9 > ? % # > # DOI: 10.14313/JAMRIS_2-2017/18 75

% 3 ) ') 4+0 .% ( # & ( # 6 , DOI: 10.14313/JAMRIS_2-2017/19 82

' ( &5 ) % % ! ! % % % &(& 6 # + # ?# > ? 9 9 '" ( # ( , ( # " # + # ? % 9 % 'C# E% DOI: 10.14313/JAMRIS_2-2017/20 95

7 ( ' % 8 2 % # ( , F C ( # # 6 , 'H & & 6 % # & > DOI: 10.14313/JAMRIS_2-2017/21

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

N° 2

2017

This issue of the Journal of Automation, Mobile Robotics and Intelligent Systems is devoted to three complementary aspects of current robotics research: modelling of robots, robot perception, and motion synthesis for different types of robots. The main theme is robot motion synthesis, however it takes into account robot perception and diverse aspects of modelling. This complementarity encouraged us to put those topics together into a single issue of JAMRIS, enabling its readers to enjoy wider context of the presented subject. The idea of publishing an issue of JAMRIS devoted to the mentioned topics emerged in the discussions during the 14th National Conference on Robotics organized by the Department of Fundamental Cybernetics and Robotics, Q # ) > U# V # " # & # W? & # XYth till 18th of Septem, Z[X\] # # & , # = "# # = # #" # # ,? # & " , , # V # were gathered within one publication. Hence, a selected group of authors of the most insighting achievements was invited to submit papers describing their research results. It should be stressed that the papers put together in this issue of JAMRIS are by no means a simple translation into English of the conference papers. The results reported here have been updated, described more comprehensively and in a wider context. The new papers were subjected to the regular JAMRIS review procedure. Gratitude should be expressed to all of the reviewers who provided in depth comments enabling many clarifications and overall improvement of the contents of the papers. Below we make a brief overview of the contents of this issue. The paper entitled Modeling and Simulation of a Tracked Mobile Inspection Robot in Matlab and V-Rep Software, , # " # # V #E= = inspection robot. The robot contains two track modules that propel it inside the pipe. These tracks, mounted around the modules containing motors, are pressed onto the inner side of the pipe by arms (called pedipulators), so that the resulting friction is able to provide adequate support and traction. The article presents the mechanical structure of the robot, including the mechanism that enables it to adapt to varying diameters of the inspected pipes, the simulation of its motion, and the tests performed on the prototype of the device. Magdalena # % # = = Reconfigurable Double Inverted Pendulum Applied to Modelling Human Robot Motion, which delves into the motion of humanoids. A simplified model of the humanoid body is introduced. A 15-segment anthropomorphic model is divided into two parts: upper and lower, hence the concept of a double inverted pendulum could be employed. Trajectories of the centres of mass of the two parts have been subjected to the analysis in terms of correlation coefficients. The simplified model was compared with the model allowing free arm movement, showing the influence of arm motions on the overall humanoid movement. The article: A Kinetostatics-Based Study of Uniqueness of Reactions and Drives in Robotics, autored by Marcin &' # ( # ) * # = , # #E # ~ # V #" # # # , ] kinetostatics-based analysis, unlike the previously used methods, which detected only non-uniqueness of reaction forces, enables concurrent detection of both types of uniqueness deficiency. The method is based on the concept derived from linear algebra, i.e. the null space of a coefficient matrix containing the geometry-related parameters of joints and links. Azimuth Angle Determination for the Arrival Direction for an Ultrasonic Echo Signal, by Bogdan Kreczmer, looks into the problem of how to measure the angle of echo arrival from an ultrasonic signal produced by a piezoelectric sonar. The proposed method assumes that the echo is reflected from a single object. The theoretical considerations have been validated by experiments performed using the constructed rotary sonar system. +"# , 5 & ' & & # & 6 # # The Measurement of Displacement with the Use of MEMS Sensors: Accelerometer, Gyroscope and Magnetometer present a method of determining the orientation of manipulator links with respect to a global reference frame by relating the vectors associated with the links to the vectors of the gravitational and magnetic fields. For this purpose each link was equipped with low-cost inertial sensors and a magnetometer. The measurements obtained from those sensors are processed by an Extended Kalman Filter. The paper: A Set of Depth Sensor Processing ROS Tools for Wheeled Mobile Robot Navigation , 9 '" and Janusz Jakubiak, presents a ROS based toolset facilitating the implementation of robot navigation. The presented toolset contains software for: converting 3D depth images into 2D polar scans, removing the ground plane, projecting obstacles, compensation of sensor tilt angle, detection of holes in the ground, and estimation of the height 3

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

N° 2

2017

and the orientation of the sensor. The utility of the created software was validated on a mobile platform equipped with a Kinect sensor. The work entitled On the Application of RGB-D SLAM Systems for Practical Localization of Mobile Robots, writ # , + # < # & % = V # = = , # # when localizing mobile robots using RGB-D SLAM techniques. The work concentrates on the influence of different classes of mobile robots and the environments they operate in, and on the quality of the estimated trajectory obtained from measurements provided by an RGB-D sensor mounted on a mobile platform. The paper concludes that not all RGB-D SLAM architectures presented in the literature perform robustly when applied to physical robots and environments. Moreover, it provides an insight as to why they fail. Wojciech Dudek, Wojciech Szynkiewicz and Tomasz Winiarski present Cloud Computing Support for the MultiAgent Robot Navigation System. The paper discloses how to distribute navigation software between the on-board robot computer and the computational cloud. Thus computationally demanding modules can be located in the cloud supporting many mobile robots. In-cloud robot path planning and global localization have received particular attention. = = , & ( # # 6 , # Path Following for Two HOG Wheels Mobile Robot, describes a mobile robot composed of two hemispheres. It focuses on the kinematics of the device for the purpose of its control along a predefined path. A full kinematics model of the device and its simplification are considered. = + # ?# > ? 9 9 '" ( # ( , ( # " # + # ? % # 9 % 'C# E% = # Selected Topics in Design and Application of a Robot for Remote Medical Examination with the Use of Ultrasonography and Ascultation from the Perspective of the REMEDI Project. The aim of this project is to construct a robot that is able to subject a patient to a medical examination by a remotely located doctor. The fundamental aspect of this system is robot perception, providing remote sensing to the doctor. The physician, by using the robot, is able to conduct an interview and observe the patient, as well as subject him/her to an auscultation and an ultrasound examination, including echocardiography. The paper describes the overall system structure and its components. Moreover it presents the system evaluation by the users. The paper entitled: 2D Microgravity Test-Bed for the Validation of Space Robot Control Algorithms, was writ # , # #" ( , F C ( # # 6 , 'H & & 6 % # # & > ] ‚ , = , # # # V #" , , = , ƒ „ ~ == # = ] & # , thoroughly tested on Earth under microgravity conditions. Autonomous operation of such robots is a necessity, as the acquisition of debris requires quick reactions. In such systems the perception component, based on vision, is of paramount importance. The paper presents the dynamics of a satellite-manipulator system, which is subsequently used for simulation and control of such systems. The vision system supplies the information on the pose of the debris with respect to the satellite. All of the topics addressed in the papers contained in this issue of JAMRIS, and briefly characterized above, are the subject of current deliberations of the scientific community conducting research concerned with robot modelling, perception and motion synthesis. Each of the papers gives a valuable insight into a particular problem, providing its formulation and deriving its solution. This selection of papers reveals the wide scope and diversity of contemporary robotics.

Warsaw University of Technology

4

Articles

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

N° 2

2017

! "# $ Submitted: 16th December 2016; accepted: 18th February 2017

Â’ÂŒÂ‘ÂŠĂ™Čą Â’ÂœÂŁÂŽÂ ÂœÂ”Â’Ç°ČąRÂžÂ”ÂŠÂœÂŁČą Â’Â?”Šǰȹ Â˜Â–ÂŠÂœÂŁČą ž›ŠÂ?Â˜Â ÂœÂ”Â’Ç°Čą ÂŠÂ›Â’ÂžÂœÂŁČą ’Ž›Â?’Ž• DOI: 10.14313/JAMRIS_2-2017/ 1 %&'()*'+ This paper presents modeling and simulation of a tracked mobile robot for pipe inspection with usage of MATLAB and V-REP software. Mechanical structure of the robot is described with focus on pedipulators, used to change pose of track drive modules to adapt to different pipe sizes and shapes. Modeling of the pedipulators is shown with application of MATLAB environment. The models are verified using V-REP and MATLAB co-simulations. Finally, operation of a prototype is shown on a test rig. The robot utilizes joint space trajectories, computed with usage of the mathematical models of the pedipulators. ,-./0(1&+ tracked robot, pipe inspection, kinematics, robot adaptation, V-REP, MATLAB, co-simulation

23 4'(015*'604 In this paper, modeling and simulation of a pipe inspection mobile robot are presented. The work is focused on a motion unit of the robot that can adapt to various shapes and dimensions of pipelines. To realize motion of the robot, modeling and calculations are performed in MATLAB software and simulations are run in MATLAB, connected to V-REP robotic simulator. To validate the simulations, prototype testing in an analogous environment is shown. V-REP (Virtual Robot Experimental Platform) is a general-purpose robot simulation framework, widely used in robotics [4]. It is rich with functions, features and comprehensive APIs for interfacing with virtually any application or device. The most important attribute that affects functionality of the simulator is a distributed architecture, this means each simulation element can be controlled in different ways. These ways include regular Application Programming Interface (API) with available programming languages: Lua, C/C++; remote API with available languages: Lua, C/C++, Python, Java, MATLAB/Octave and Urbi or Robot Operating System (ROS) interface which can advertise or subscribe to ROS topics and offer ROS services. 5 # Π= = # = # bots has been made by some teams. Hansen et. al. proposed a wheeled mobile robot intended for inspection and pipe mapping [8]. Their robot is designed for movement in horizontal pipes and utilizes a forward #" Π] = = visual odometry and build pipe models with high

resolution. Nayak and Pradhan proposed a robot for horizontal and vertical pipes. It is designed to operate in pipes with constant radius [12]. Its construction allows to use only one actuator to pass through pipeline elbows. Sharma et al. proposed an inspection robot with additional functionality, which is obstacles removal and pipe cleaning [13]. They have also tested a possibility of controlling the robot wirelessly. Another approach was presented by ULC company [14]. They designed a robot, called Micro Magnetic Crawler, capable of inspecting any object made of steel. This compact robot uses magnetic attraction, thus can move in any direction, even upside down, on ferromagnetic surfaces. A manually adjustable robot chassis for variable pipe geometry was designed by Inuktun company [9]. The Versatrax robot can be set for motion inside of pipes of various diameters or can move on horizontal surfaces. The research shows that an automatic adaptation of the robot’s motion unit to the pipe shape and diameter has not been investigated by research teams. In this paper, an automatic adaptation system is shown in the context of modeling, simulation and prototype validation.

73 -*8)46*)9 '(5*'5(The mechanical structure of the robot is based on two pedipulators that control pose of the track drive

Fig. 1. Pedipulator mechanism [7]: 1 – track drive module; 2 – front driven ring; 5 – rear driven ring; 3, 6 – sprockets; 4 – front arm; 7 – rear driven arm 5

Journal of Automation, Mobile Robotics & Intelligent Systems

modules. One pedipulator consists of two coaxial, independently driven rings (2, 5), with axes of rotation in the center of the robot body, as depicted in Fig. 1. These rings are driven by servomotors and torques are transferred by internal meshing gear transmissions (3, 6). Arms (4, 7) are mounted to the rings by revolute joints. Other sides of the arms are attached to the track drive module (1). The rear arm (7) is equipped with an additional servomotor. By assembling two pedipulators to the robot body, #Π" , V # # that allows positioning of the track drives to allow robot adaptation to various environments [2]. The robot is equipped with six servomotors, responsible for setting pose of the track drive modules, thus the robot has 8 drives in total. Construction of the robot guarantees that it can operate in pipelines with ac V Π" = = # #V # # where watertightness and dust protection is required.

VOLUME 11,

N° 2

2017

Fig. 2. Kinematic model of pedipulator: r1 – rear ring (5) rotation angle; f1 – front ring (2) rotation angle; r3 – rear arm (7) rotation angle with respect to track drive module (1); r2, f2– unactuated joints rotation angles

:3 ,64-;)'6* 01-9 0< '8- #-16=59)'0( Kinematic modeling of the robot was divided into # = ] Œ ,? V , # # Œ # # # faces. This model was described in [3]. The model, however, does not provide information on the adaptation of the robot’s motion unit to the environment, since motion is described for only one pedipulator pose, dedicated for even surfaces. To complement the modeling approach, a mathematical model of the pedipulators that set pose of the track drive modules was created. In general, the pedipulator structure for adaptation of one track module is a closed kinematic chain # Œ V # # � V ? # among which three are actuated. Their angles of rotations are denoted by and in Fig. 2. The mecha-

nism can be treated as planar, since all the revolute joints axes are parallel. In order to apply an approach widely used in robotics for open kinematic chains, the mechanism was divided into two planar manipulators with two and three degrees of freedom that represent front and rear parts of the pedipulator. Later, it was assumed that the manipulators have to maintain position of their end-effectors in one point, that would Π# # # #] For both manipulators, transformation matrices were created, using standard Denavit-Hartenberg notation [11]. The transformation from end-effector to the base coordinate system for the front (2-DOF) manipulator is presented by equations (1) and (2), and for the rear (3DOF) manipulator by equations (3) and (4). (1)

where: f1 Žf2 – rotation angles of joints 1, 2; af1, af2 – lengths of links 1, 2. (3)

(4)

where: Žr1 Žr2 Žr2 – rotation angles of joints 1, 2, 3; saf1, af2 – lengths of links 1, 2, 3.

6

Articles

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

N° 2

2017

(5)

(6)

Poses of the pedipulators for particular pipe sizes were determined with usage of the CAD model. With usage of the data, joint angular positions were used as #= # # Π# = ? " # -

a

b

Fig. 3. Pedipulator transformation – joint trajectories: mm, mm

tion. The second step was to generate 5th order polynomial trajectories for the front (2-DOF) manipulator , # # # ÂŒ # = ] < Â? #V # ics problem was solved with usage of a numerical approach, based on the Jacobian pseudo-inverse for the rear (3-DOF) manipulator to match position of endeffector of the front manipulator. In this case, analytical formulation of the inverse kinematics problem gives 8 solutions for the entire pedipulator structure. Therefore, additional conditions were used for planar manipulators, as in [11]. Joint rotation ranges were also limited, to comply with the mechanical structure. Finally, oscillations of the manipulator, caused by transitions through singular positions were eliminated. The calculations were done in MATLAB software with Robotics Toolbox [5]. Thus, smooth trajectories for transformation of the robot pedipulators were obtained to realize required motion in the workspace. Original construction of the pedipulator as a closed # # # ÂŒ # three motors to set desired pose of the track drive module. Rotation angles of the positioning drives during transformations are shown in Fig. 3. Initial validation of the model was performed in MATLAB. The pedipulators positions were drawn for every step of the trajectory. In Fig. 4, the kinematic model of both pedipulators is depicted during transformation from neutral pose (all servomotors are in the middle of their operating range) to the pose that allows the robot to move in horizontal pipes with internal diameter of Ă˜210 mm. The MATLAB visualization of the pedipulators model shown in Fig. 4 proves that the trajectory calculation algorithm is properly designed for reconfiguration of the robot chassis. The analyzed closed kinematic chain is highly dependent on the path taken to attain desired pose, thus it is not always possible to use arbitrary start pose and expect satisfactory kinematics results for every goal pose. Articles

7

Journal of Automation, Mobile Robotics & Intelligent Systems

a

b

Fig. 4. Kinematic model of two assembled pedipulators during transformation (white color – free joints, hatched – actuated joints): a – neutral pose, b – pose

:3 0%0' 0'604 6;59)'604& 64 ! "# )41 &0<'/)(In order to validate the mathematical model and trajectory calculations of the robot’s pedipulators, it was necessary to use a simulation environment. As previously stated, V-REP is a versatile and customizable simulation platform that can be easily connected with remote applications such as MATLAB and may be used for various applications [10]. Due to the fact that the robot control algorithms, presented in this paper are developed in MATLAB, a remote API was used. Remote APIs, independently of programming language chosen, offer four operation modes, designed for different tasks (Fig. 5). First mode is a blocking function call that causes an API client to wait for response of the simulator. The second type is a non-blocking function call that could be used only for sending data to the simulator, as it does not wait for a server response. The third type is data streaming, where a client sends message once and a server sends reply regularly to the client. The last mode is synchronous operation that enables synchronization of each simulation step with API client. In this mode, server holds execution of each simulation step, until a trigger from client is sent, thus it is usually the slowest mode of communication. 8

Articles

VOLUME 11,

N° 2

2017

The first step of the simulation procedure was to create a properly defined robot model in V-REP that satisfies all mechanical constraints and can be simulated in a way resembling real operation. The robot model designed in Autodesk Inventor software was imported in the V-REP environment. Next, the model structure was defined in a way that allows creation of joints, motors and ensures interaction with environment. To improve calculation efficiency, the model was composed of visible bodies and dynamically enabled bodies that were simplified using convex hull decomposition. Overview of the robot model is presented in Fig. 6. To improve visual aspects of the simulation, the tracks were modeled using separate segments that move on a predefined path (Fig. 6a). In contrast, for dynamic model, each track was represented by five cylinders with rounded external surfaces that provide steady contact with flat and curved pipe surfaces, whilst assuring simulation results analogous to a tracked drive. This setup proved to be the most reliable for various simulation cases. Natural representation of a track as a set of segments is computationally costly and may lead to simulation instabilities due to rapidly alternating contact points of the track treads. The pedipulator mechanisms were modeled using bodies connected by rotary joints that were set as actuated with position controllers, or not actuated, according to the mechanical design. The robot model created in V-REP was linked with MATLAB using a remote API link. The link allows running simulations directly from MATLAB, with simultaneous data transfer between both programs. The mathematical model created in MATLAB was used to control positions of the servomotors to realize pedipulator transformation trajectories and also for velocity control of the track drive modules of the robot 3D model created in V-REP.

Fig. 5. V-REP API overview [4]

Journal of Automation, Mobile Robotics & Intelligent Systems

a

VOLUME 11,

N° 2

2017

a

b b

Fig. 7. V-REP model of pipe inspection robot: a – pipe run, b – robot driving through pipe elbows in DN315 ! #

Fig. 6. V-REP model of pipe inspection robot: a – visible bodies, b – dynamically enabled bodies (light color) with joints (dark color) Initially, reconfiguration of the pedipulators was verified by V-REP simulations of the robot model placed on a dedicated support that provided unobstructed motion of the track drive modules. Next the robot motion was tested in various environments, such as flat surfaces and pipes with circular crosssection. In order to check operation of the robot in complex pipe structures, a dedicated V-REP scene was prepared (Fig. 7a). The test run contained the following pipe segments: straight 300 mm, 90° bend in 300 mm pipe (Fig. 7b), followed by straight segment, reducer from 300 mm to 242 mm, straight 242 mm segment followed by 30° bend (Fig. 7c) and straight section. A co-simulation of MATLAB and V-REP was prepared to check if the robot would be able to traverse = = #] 6 #ÂŒ " # = = performed automatically based on the mathematical model and motion of the robot in pipe was realized by manual control, with usage of a joystick connected to a MATLAB/Simulink model. In Fig. 7b, the robot VREP model is presented during motion in 90° bend in Ă˜300 mm pipe. At this stage of control system design, operation of passing bends was performed with usage of teleoperation with visual feedback. In Fig. 8 we can observe the robot during simulation in a pipe reducer between Ă˜300 mm and Ă˜242 mm pipes. This process is realized automatically, with V = = = #ÂŒ " tion process. With this approach, servomotors overload is avoided, that could arise from rapid transfer through the reducer by excessive velocity, manually set with teleoperated control. The usage of MATLAB and V-REP co-simulation run in synchronous mode allowed to control the robot 3D model in a similar way as a real prototype, because application of mathemat # V ÂŒ V ]

a

b

c

Fig. 8. V-REP model of pipe inspection robot during re ! "# ! # pipe: a – robot in pipe DN315, b – robot in the reducer, ! #

Fig. 9. V-REP and MATLAB simulation results comparison for transformation from neutral position to pipe Ă˜ $ % b – absolute positioning error V-REP vs MATLAB Articles

9

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

a

N° 2

2017

b

c

& ' " ' *+/0< =>?@>C G to pipe Ă˜242 mm: a – positioning drives angles, b – absolute positioning error V-REP vs MATLAB

Results of the simulations are presented in Fig. 9 and Fig. 10. We can see that the mathematical model trajectories from MATLAB and simulation results from V-REP coincide well and the absolute positioning errors of pedipulators drives do not exceed 0.6° for the analyzed cases. The errors are caused mainly by joint position tolerances of the Bullet dynamics engine, used for the simulation, especially when closed kinematic chain is analyzed. This errors are satisfactory and do not affect proper transformation of the pedipulators between desired poses. V-REP and MATLAB co-simulations proved to be very efficient testing tools for motion analysis of the robot models that allowed optimization of pedipulators transformation trajectory calculation algorithm and development of control system for the robot prototype.

>3 -&'& 0< '8- #(0'0'.=For the testing of the robot’s ability to adapt its pedipulators poses and assessment of prototype mobility inside pipes, a test rig was prepared. It consisted of straight pipes of nominal diameter DN315 mm and DN250 mm, pipe reducer connecting two pipes and two bends, one 90° bend of diameter DN315 mm and second 30° bend in pipe of diameter DN250. The pipe segments were assembled as presented in Fig. 11a, similarly to the simulation scene presented in Fig. 7. The objective of the test procedure was to verify if the robot would be able to traverse the rig. Initially, # # = = #ÂŒ " pose for motion in DN315 mm pipe, with usage of the trajectories for positioning servomotors, computed in MATLAB. Next, the robot was placed at the entry of the pipeline. Control strategy of the robot included 10

Articles

Fig. 11. Tests of the prototype: a – test rig, b – the ! # HG J% c – the robot after completion of the test run

manual operation of track drives by velocity setting and automatic transformation of the robot’s pedipulators. Motion in the test rig included the following steps: drive into the pipe, traverse 90° bend in larger = = " = = # #" #ÂŒ " # # to smaller pipe diameter, get into smaller pipe (Fig. 11b), pass to next bend, traverse 30° bend in smaller pipe, get out of the test rig. The performed test complied with simulation results and the robot completed the experiment successfully (Fig. 11c). In straight pipe segments, motion of the robot without slip was observed, however negotiation of pipe bends implied excessive slip of track drives that were controlled manually. Application of an automatic velocity control during negotiation of bends would provide smoother transitions. The most difÂŒ = # V " = = from DN315 mm to DN250 mm. In this case, simulta# #ÂŒ " # , ‘ = = # the tracks forward motion would be optimal, but the functionality was not available in the current version of control system of the prototype.

?3 @04*95&604& )41 A5'5(- B0(C In this paper, mathematical modeling, simulations and tests of a pipe inspection robot with an adaptable motion unit were shown. Co-simulation prepared in VREP simulation software with MATLAB/Simulink = # #V # # = V , V Π# tools for testing control system design for the inspection mobile robot, featuring two pedipulators with closed kinematic chains. The pedipulators transfor-

Journal of Automation, Mobile Robotics & Intelligent Systems

mation trajectories, generated with usage of math V Π#" = = on a dedicated test rig analogous to simulation environment. The preliminary prototype tests allowed V # = = #Π" # , # # V Π# = # for robot motion. Future work would focus on software implementation for the inspection robot, including a vision system that would serve as an aid for autonomous reconfiguration inside pipelines, depending on the recognized geometry. Moreover, the robot would be simulated during motion and adaptation in vertical pipes. An upgraded electronic board and enhanced software would allow control of the track drive clamp forces exerted by pedipulators extension during motion of the robot inside of vertical pipelines.

VOLUME 11,

[6]

[7]

[8]

–œ˜

[10]

D EF – Department of Robotics and Mechatronics, AGH University of Science and Technology, Cracow, Poland. E-mail: mcisz@agh.edu.pl.

[11]

Mitka – Department of Robotics and Mechatronics, AGH University of Science and Technology, Cracow, Poland. E-mail: mitka@agh.edu.pl

[12]

Tomasz Buratowski – Faculty of Mechanical Engineering and Robotics AGH University of Science and Technology, Poland. E-mail: tburatow@agh.edu.pl

[13] [14]

N° 2

2017

J. Giergiel, M. Giergiel, T. Buratowski, M. Ciszewski, „Pedipulator’s mechanism for positioning of a track drive module, esp. for mobile robotsâ€?, Patent application (description of the invention) no. PL 406656, 2015 (in Polish). P. Hansen, H. Alismail, P. Rander, B. Browning, “Visual mapping for natural gas pipe inspectionâ€?, Int. J. Rob. Res., vol. 34, 2015, 532–558. DOI: 10.1177/0278364914550133. J. Khalilov, A.T. Kutay, “Interfacing Matlab/Simulink with V – REP for A Controller Design for Quadrotorâ€?, Int. J. Eng. Res. Rev., no. 3 , 2015, 42–49. ] &] 9 >] > W, , Modelowanie i sterowanie robotĂłw (Control and modelling of robots), 1st ed., PWN: Warszawa, 2012. (in Polish) A. Nayak, S.K. Pradhan, “Design of a new inpipe inspection robotâ€?, Procedia Eng., no. 97, 2014, 2081–2091. DOI: 10.1016/j.proeng.2014.12.451. Y. Sharma, K. Deepak, P. Kumar, A. Chauhan, “Blockage removal and RF controlled pipe inspection robot (BRICR)â€?, Int. J. Electron. Telecommun., vol. 4, 2015, 62–68. Coppelia Robotics, V-REP Virtual Robot Experimental Platform, 2016, http://www.coppeliarobotics.com Inuktun, Inuktun crawler vehicles, 2015, http:// www.inuktun.com/crawler-vehicles ULC, ULC’S Micro-magnetic crawler, 2014, http://ulcrobotics.com

Mariusz Giergiel – Faculty of Mechanical Engineering and Robotics, AGH University of Science and Technology, Poland. E-mail: giergiel@agh.edu.pl *Corresponding author

"A" "G@" [1] S.R.S. Buss, Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods, Univ. California, San Diego, Typeset Manuscr., vol. 132, 2009, 1–19. DOI: 10.1016/j.neuroscience.2005.01.020. [2] M. Ciszewski, T. Buratowski, M. Giergiel, P. Malka, K. Kurc, “Virtual Prototyping, Design and Analysis of an in-Pipe Inspection Mobile Robot�, J. Theor. Appl. Mech., vol. 52, 2014, 417–429. [3] M. Ciszewski, M. Waclawski, T. Buratowski, M. Giergiel, K. Kurc, “Design, Modelling and Laboratory Testing of a Pipe Inspection Robot�, Arch. Mech. Eng., vol. 62, 2015, 395–408. DOI: 10.1515/meceng-2015-0023. [4] P. Corke, Robotics, Vision and Control: Fundamental Algorithms in MATLAB, Springer, 2011, ISBN 978-3-642-20144-8 DOI: 10.1007/978-3-642-20144-8 –—˜ ‚] 9 ', ] F= ™+ = # ( cobian-based methods of inverse kinematics for serial robot manipulators�, Int. J. Appl. Math. Comput. Sci., vol. 23, 2013, 373–382. DOI: 10.2478/amcs-2013-0028. Articles

11

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

¢ ǰ ¢ £ ǰ Ú !" "#$ %&'(' )# ' %&'()*'+

! ! " # $ % ! & ! " ! ' " ' ,-./0(1&+ ( ' " #

23 4'(015*H04

"# # , # E == E # ] = = # , # ] # , E # , ] # # , V = E # # ] 9 = E # , # = = E , ] #" , E # " # #" " # E # , = # ? ] = E # " 5 # #V & # E 5 & Z X ] 5 & ) "] X # , # #" "

V # " = # # == = ] 5 & E , #" # , # # #" = ¡ ] == #" # , #E X¡ ]

2323 ')'- 0< '8- (' ) # , # , # = = , # ] # " = = , #" ? E = # , # #" X ] > #E # E

# # = , ] , , " # # # # E #

# # , = # # V = # = , E # # # # " # , = ] # , # " # # V = # # # = # = ] V E = #" = E V # # ,? # # " # # , E # ? = , # & # " # E X \ ] # ? V ? # " # #" # #

, ] # == # " # # # " # # #" E = , #V = # , E XY ] # # = = # " E = # # # # ## # # , ] # # ] < #" #E # = , # #" # ? E , # ? # #E # ] + V # E = , # #" , # # , V # " , # # E = , " = # XZ

& ' "' O Q @ S < H@S<J

%= #"E5 #V & # %5 & ) "] Z , # #E " E = # # V # # # XY # # # , Y ] 9 = %5 & , , " # # 5 & ] 5 & # # = # #" ¢ " %5 & = # #] U# 5 & %5 & # # #" = # #" , # = #" = = # # = # # = = #"] # ZX # " # # = # ] + #E V = # # " # E #" # #"] # V # # , , # , V #E

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

V # == = , , E , # # # = # ] V = # = , #" V , # # , # # E ] , = = V = # # # = # #V = # # # # ] % # # = , #" = " " X\ ]

& ' ' O Q O @ S < HO@S<J

" # #" = # V ] £ # # #" 5 & # " # = #]

= E # , # " # #] # X #" = E # " # #" #V = # = # ] # X\ V # 9 5 & ] % = , E # #V " = " " ] #E #" " #" ## #" , # #E V = # # # \ ] " # = , # #" " , # , #V = # , # ¡ # V # # " = ] % = E V # # # #" = # #" = & # # & # X[ == ] # Z[ = # = # , = # # #V = # 5 & # , #V = # ] 5 # #V E = # # # , # # # = # = = # ] 9 , #V = # # 5 & # # , ¢¤ # # ¢ " # ] + , E , # # ]"] " # # = # , ] ¤ V , #V = # E # # # # # =E = == = # , = ¤ ¢¤ # # ¢ " # ] < " #" " # , # E = #" , # = # = # # = # # # ] F# # "# #" # , # V # = , # XX ] % # , V # # , = # #] = # = E # # = = # #" , # E = , # ] 9 #" # # E #" = , , = #] # #" V V # # # # # , ] % = , #E # , # #" = ##

# E # # == , V # ] % #V E " # V # # # # " # # , # #E , V ] = # , = E # " #" V # E #" = #] V , #" = # ,

# = # ] # # V # # = # # ] + V # = = # # E # # ,? #" , E = # # = , # #" V V # = , # ] QV # # V = = # # # # , # V # ] > = , # " # E # # # ¢ # # E # # # #" ## #"] E = = , V # # # , V # # V #" #E = ] 9 # # " # = = , = # #" , # V # " # # # # # # ] # # # = = , # V # #E #" #] + ~ #" #V = # , = # E = # #" # " # # , # # = = ] 9 , #V = # E V #" #" = # = = # = = ] = E = # " # = V # == # # # ] ) # , #E # # #V = # # ] + # # , # " # # # = " #] U# # #" , # = # ] # # = = #V = # #E # , # " # # == = , ] & # # = # # #" # , V #" #" = # ] # V # # # " E , = = ] # # E # # # = , # # # #" E #] # , # = V # # , #] X

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

, ,

& ' ' H J O Q U % H J W

2373 I05%9- 4J-('-1 #-41595; )& ) 01-9 0< ) E5;)4 01. > , #V = # ) "] # V # "E # = # # # #E = XZ ] & # V # ] # # = = #" # 5 & # %5 & = E # # , = # #" # 5 & # = #" # # %5 & ] + V #" #" # # E # # # # #" = E , ] = = # # # V # "# # # # , E # = , "# # ] F = # ) "] ] E #" # V # " # = E V ] > = = " , =E # # V # # E # # " V #" " # ]

73 @04&61-(-1 01-9& )41 0J-;-4'& 7323 E5;)4 01. 01-9 # # , # # ) " , ] , = # #

X]\ " # ] " ] E #" = # #" = # # = ] ¤ # , E V X " # # # X "E # Z " # Z " # # Z " # , # X " # = X " # " Z "E # # Z " # Z " # ] ) " # = # #" # = XZ "# ] 7373 6;=96K-1 01-9 X E " # E # , = # # ) "] , = ZE " # E XY

# == # = , ## , E V = # ? # # = V &Q5&¢ = # ] = , # " # = V ] == = # #" " # ] 5 #" = #E # ~ # , # #E # = ? # ] 5 #" == = #E ~ # , # = V &Q5&¢ = # # = # # ] % = E # , #V = # # #" #V = # # # #

== , # = , ] 73:3 4)9.&-1 0J-;-4'& = = = # E # V # # , = # # " E = # ] V # E

# # , = = # # ]"] # # ] Q V # # # # #

# == , V # # E # ] % # #" £ # # = ] & # # #" , #V = # V #" E

#" ) "] ] % " # ) "] , = # # # # #" = # # = # E ]

:3 4)9.&6& 0< @-4'-( 0< )&& ()L-*'0(6-& U #" # " , £ # # =E ? # , = # E ] #V " # # # V # ] % = # , , #V = # # #" # ] F, # "

# # , # # # = # ? # E , #V = # E V #" # = V # # #" # = #]

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~•

ƒ „

ƒ,„ ,

ƒ „

ƒ „

Eâ—¦

‡~„

& ' ' < X $ H J Y W% H J H WJ% H J Y W% H J H WJ' C

Z = X W

W

Z = X W W

5 # # ƒ „ E #"

0 rCoM =

i Σni=1 mi rm , n Σi=1 mi

ƒX„

mi iE " # # ri

E # # " , # ] 9 V #

, # " # = # # ) "]  # = , #" # # " # # # # = # –XZ˜] F, # = # ? E # V " # # Z9 # # " ƒ V „] ) = =

= # , " # V ]

£ # =  9 V # E #" = # ,? , ] = V # # , E , # ? ] ) = = # ? = # E # == = , ] + , # #" # E , " # , ƒ) "]  Âƒ,„„ == = = # ƒ E = # # = –XZ˜„ E "# ] F# # #" # # # " # # ?

= # # == = , , # ] = # = # # , #V = # ]

X—

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

& ' #' ? \ Z = X W

W $ Y W H J% Y W H J% W HW

J W H

J

:323 ()L-*'0(6-& @0;=)(6&04 64 '8- 6;- I0;)64 & # ? , V # # # ) "] Y , ) "] Y V #" ) "] Y , # , ) "] Y # # V #" ) "] Y ] ¤ #E # = # # # V ? , " # V ] ? = # = ? = , , # # ? == =

, # ] # #" # # , # ? ] ? E = # == = , #" # # ] == V # E # # # ) "] Y] ? == =

, # = , # = # # ] 9 #" V # , ? == = , # = , #E ] # # " V #" ? , V # # # V # , "" # , # ? ] % = # , # # # = ? == , = ] # # " V #" # # #] # # # V V # # V # #" #] X\

# , # ? =E = # = , # # ) "] \] #" # , V # # # , "" # # = # # E # " , E , = # ] :373 -9)H04 -'/--4 '8- ()L-*'0(6-& 64 -(;& 0< @0(! (-9)H04 % , V , V # ~ # , # # ] + #" E ? = , V E , x # ? == = , # V , y #V " # = # # V , #" & # # # ] E # # ρ = ,

Σn (xi − x ¯)(y − y¯) i ρ = n i=1 , Σi=1 (xi − x ¯)2 Σni=1 (yi − y¯)2

Z

xi # yi V ? E # iE = # # x ¯ y¯ # V

x ¯=

1 xi , n i=1

y¯ =

1 yi . n i=1

Y

n

n

# # # #E , # # == = , # E = , = #

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

, ,

E◦

~

& ' ^' O _

Y $ H J Y W% H J H WJ% H J Y W% H J H WJ

? ' "' < U Y ` ( , V #" # 6 # , 6 # V #" #

¢ #" ¢ V # = # = # # # # # # ) "] Y # # ) "] ] , "" # # , # V # , = ] #" # = # # E V # , E # # # V # #E # ? ]

2 ρ1 = 0.9701 ρ2 = 0.9255 ρ3 = 0.9972 ρ4 = 0.9878 , , =

# = # # , = # E # # # , # =

# # # , # # E # # , #] # # # " E # " # V ] , # = E V # = E , = # #

X¡

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

== = ] > V # # , V # == # E # V # V V ] ## #" , # # , "" # E V #" # # ] E # # , # # " , V #" # # ] 9 # # # , # # V # # = E V # , # # ] :3:3 -9)H04 -'/--4 ()L-*'0(6-& #(-&-4'-1 04 *)'! '-(=90'& = " V # # ) "] \ ~ # E # #" # # # E V # ] % = ) "] \ = # " E E # , # ? # == =

, ] F# # = # , = # = # # V = E # # == = , # E ] )

# = = # E # == , yi = # # = #

, xi ] Q = # xi yi # # iE ] & = # # ) "] \ # = # = # # # # E #" = ) "] Y] > # # = , V # #

== = V #] E = # ) "] Y , V == #" V # = ] # # " # # # " # = # , = # E ] :3>3 4)9.&6& 64 -(;& 0< G04!16;-4&604)9 M5)4HH-& # # # # " V #" # # #E # # ~ # = # ] & # # " # #" == = = , V #" = , ] < #E # # = # #" = # = # = # V #"

ki =

d0i · 100¥, l

d0i = # # # iE # # = # " , # l #" == = = # ] " E V # # # = # = # # = E V = # # , " = # #" #

# = # #" ] 5 # == = E = = # = = V # # ) "] ] E = # # 9 = ) "] ¡ # #" # #" # # E = # = # #" ] +# #" = # ) "] ¡ # # E # = # = # #" = , # #" −\¦ + ¦ , ] # X

E◦

~

k #" #" # , E # \ ¥ # ¡ ¥] # # E V # # = , # # = # # ¡[¥

" # # ? # ] = E # , Y¥ # = # #" ~ X [][Y ] % = # # = = ) "] Y , ] +# #" = # # ) "] ¡ , # # # = # = #E #" == = , # #" −\¦ +\¦] k #" #" # , # Z[¥ # Z\¥] # = V # # == =

, # = # , ZY¥ = # ? # = V ] = # # \¥ E # = # #" [] , , [][ ] % = # # = E = ) "] Y , ]

>3 -&59'& +# = # # # # "# E # # , # = # ? =E = # = , # , E V # " , # " V #" # , # # # V #" ] ? # = # # # = ] #" # , # ? # = # E V # # ? = # , # V # ] , V # , #" # " ] " V # , "" # # # , # ? V # E V #" ] == # # # # ] ¤ V E # # " # [] #] +# = # # # Y = V #" , #V = # ] < #E # # # " V = # V =

= # , V ]

?3 @04*95&604& # # = "# #

V # ] > # V #" ? == , # # ? , V # # ] # , # = # 9 = , #V = # E # " # #" # V # ] + E # = # V # , , = #" & # # ] F, # = E = # # # " ] # # # " E #" # #" # # E V # == # = , E = # , # # ] # #" E

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

, ,

& ' q' / Y Y $ H J W% H J

W

# # , " ~ #E ] F # = # #" = E # " 5 & # , =

# ] U #" # = = , # # V # #" # # = = , ] = E # , # #" # , # # #" = V # # = # E #" E = #] % = E = # , # = # ] # " V # # , # = E , # # V # # ] = # # # , = , V # # # ,? #" , ]"] # # = , # = E V V # " # #" , # ] QV # V = = # " # # # # # # # ] 9 , #V = # ~ E = # V = # , V # V # ] # #V " # = # 9 = " V # #] < = , V # E V , V # # = ] # V # E V #" = # # # , = #E # " # ] = # = = # # " #" #

# 9 = , # ]

@,GFB "IN" "G == , 9 # # ) & # + # Q#" # E #"] > # # = ] " % = # #" # = E # # & # 9 "# < # E & # # 9

D EF ( ) < ) = > U# E V # " ) & # + # Q#" # #" # + # E # +== # < ? ZY > [[E\\ E § ]= ] ]=

] ]= ] ]= ¨ ,¨& # ¨ " E " # E ] ( = $ ) = ∗ > U# V

# " ) & # + # E Q#" # #" # + # # +== # < ? ZY > [[E\\ E

§ ]= ] ]=

] ]= ] ]= ¨ ,¨& # ¨ # ]E #E% ] ' > ? = > U# V # E " ) & # + # Q#E " # #" # + # # +=E = # < ? ZY > [[E\\ E § ]= ] ]=

] ]= ] ]= ¨ ,¨& # ¨= ]E E # ] ∗

= # #"

X

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

"A" "G@" X ¤ ] ¤ ] ¤ ©] # ] ¢ V = # ¤ # # , ¢] # !""# $%%% $ & ' ( * + / " X X ZX XZZ\] 9F X[]XX[ ¨6F F ]X ]\¡¡Z ] Z ? %] # )] # ] © ] ¤ E ¤] ¢ 9 5 # #V & # + = #" , = #" = # " # #¢] # 0

* 344! $%%%5&67 $ $ & ' * 6 / ¤ Z[[X Z ZY\] 9F X[]XX[ ¨ 6F%]Z[[X] ¡ \ ]

# 6] ) 6](] ¢% # "" # # #" # = ¢ 7 /8 0 V ] X¡ # ] X [ X¡] 9F X[]X[[¡¨ )[[X ¡¡\[]

Y > # #" &] ] F # 9]Q] ¢¤ " E%= ¤ E # 6 ## #" " # 9E%5 & ¢] # $%%%5&67 $ ( $ & ' * 6 / 6F% ( = # Z[X X Y XY[] 9F

X[]XX[ ¨ 6F%]Z[X ]\\ ¡[ ] ] < " # +] 5 #" ¤] % # E " ©] ¢% #" & " U #" Q# # 5 # #V & # ¢ $%%% 9 / * %

( V ] Z # ] Z[[ X [Z X X ]9F

X[]XX[ ¨ Q]Z[[ ] X [] \ # ] ¤ " # (] ¢ # % ¤ E # 6 ## #" #" # #V & # # 6 # # = % ~ # ¢ % ( 8 5+ < 6$==&+0> 6 /8 / /( 8 + / Z[X[ X XX] ¡ ¤ ] + ª # 5] +] "E # ] ¢¤ # = #

# # E " #" # , # ¢ > ( / < / 6 Z[XY V ] XY XZ XY\] 9F X[]X[X\¨?] V]Z[XY][¡][[Y] = ]<] > " ¤] ¢& +E + " E ) # % # " ¤ # 6 , # V ¢ $ 7 > / ( * & ' V ] X # ] Z Z[X\ X ZX] 9F

¨X[]XXYZ¨%[ZX Y \X [[ ¡X] & (] (] 9 # V %] +] ¢6 E V = & # + % = ¤ E # & ¢] # 0

* ?th $%%%( &+6 $ > / * & ( ' ¤ # Z[[\ Z[[\ Z[[ Z[¡] 9F

X[]XX[ ¨ ¤6]Z[[\] ZX ] X[ 6] # %] 9 6] ] ¢+ E # E & " " # # # , , #¢] # $%%%(&+6 !@th $ > / * & ' ¤ # Z[X Z[X % \ Y[] 9F X[]XX[ ¨¤U +E <F 9%]Z[X ]¡ \ Y¡ ] Z[

MFCLD< ~~

E◦

~

XX & # ¤] ¤ (]¤] Y 6 # 9]+] 5 E , # 9]Q] ¢ # # # # #" # # #" # ## #"¢ 7 %Q8 ( / 9 V ] ZXZ # ] Y Z[[ Z Y] 9F X[]XZYZ¨? ,][ZY Z¡] XZ = ¨¨ ] ] ¨, ¨ X /8 Y 9 * 6 / 0 / X 3(X 6 * 9 , Z[X\] X ? %] # ] ¢% # , = E # # "" # V # # == E # # #V = # ¢] # 0

* !""! $%%% $ & ' * + / % E # + X X XY[ XYXX] 9F X[]XX[ ¨6FE F ]X X]X X XX] XY

# 6] ) 6](] ¢% # "" # # #" # = ¢ 7 /8 0 V ] X¡ # ] X [ X¡]9F X[]X[[¡¨ )[[X ¡¡\[]

X % F] £ > , &] ] Z ¢ E + ¤ # > #" & E # & = #" # Q = # ¢ +* * & ' V ] ZZ # ] \ ¡ Z[[ E\XX] 9F

X[]XX\ ¨X \ [ « [ Z \] X\ ? %] # )] # ] © ] ¤ E ¤] ¢ 9 # #V = # = #" , = E #" = # " # #¢] # 0

* Z 344! $%%%5&67 $ $ & ' * 6 / Z[[X Z ZY\] 9F

X[]XX[ ¨ 6F%]Z[[X] ¡ \ ] X¡ 5 ]9] 5 ] ¢¤ # , # #" # #V = # = # # , , E # V # ¢ 7 ( 0 V ] Y[ # ] Z[[Z XXXX XXZY] 9F X[]XXX ¨?= ]Z[[X][X [¡¡] X # ] ¢ = " " # E "" # ¢ 9 ' V ] ¡Y # ] X \ Z\ Z¡ ] 9F

X[]X[[¡¨ )[[\ ZZZ¡] X + +] 9] ¢¤ #E # = # = > #" 6 , ¢ $%%% + / ( V ] # ] Z[XY XX XX [] 9F

X[]XX[ ¨ + ]Z[XY]ZZ YZ] Z[ ] ¤ 9] 6] ¢ #" # # #V = #E # #"¢ # & V ] YX # ] Z Z[X Y] 9F

X[]X[X\¨?]" = ]Z[XY]X[][Z ] ZX +] 9] ¢ # # " # #V = # # " + # E #" = = V ¢ > / < / 6 V ] Z\ # ] Y Z[[¡ \X¡ \ \] 9F

X[]X[X\¨?] V]Z[[¡][Y][[ ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

* + +(, + % 5 6 8 ++ + + +

¿ ǰ £ ­ £ !" "#$ %&'(' )# " %&'()*'+ ) " * " $

+ ! " " * ( " * $ / 0 $ 1 " " * 2 ! " 2 0

,-./0(1&+ 1 " *

23 4'(015*H04 6 # # # # , E ] # # # # # = # V ] == # #E # V "

9F) # # = = E Z Z ] % # #" = #E # # = # # Z ]"] #E E # = 956 " " , = E # = # # # = ] +=E = # # # # V # ~ E # # , V ] ¤ V #E # # , , # # # ] # " # == E ] # = # = = # E # # # , # " # # V #E # ¨ # # # #

# # # # # ] == ]"] # Y ¡ Z[ ] # #" # == , ~ V # # ] # # #" # #

# , = " , , ] = = V " , # ] 6 E " , = # # = # # , , # # ] ( # E # # , # E " , ## , # ~ # , # # # # E ] # # # # #

# = # #

# , #" # ¡ X Z ] V , = # # # # # , = , # #" ] ] = == #V # , ( , # # # , = ]"] XX XZ ] = , # #E # ~ # # # E # " , ]"] # , # = # = # , # "E # ] 6 # # #E # ~ # , E # # V # ]

, # # # , , ¡ X Z ] U# # # # , #V V " #" # " = E # ¡ X ZZ ] ¤ V ? # # , # ~ E # = # #E # ~ # " , E # # ¡ X Z ] # # ~ # # # # , ] #E = # " , ¡ X Z # # = Y ] % # ( , #

# # ] V , # , # #

# E ,

# ( , # , ? # # # # # # ~ # ? # # ] < = E = V = # # # = == # , # # E #] # = = # # ~ # V #" ] = E = # # #E # ~ # # # # E

# ]"] # # " , # E # X # = V = # ZY # ~ #" # # ? # ] # # # # V # V #" # #E # ~ ] E V = # #" = #E V #" # ~ # = , , " # # ~ # ] + # # , # , # # ] E, " ) 9 X ] % == # " E , # #

# # = , # = V == E # # ] # #" # E, # #

# # = , # X[ X XY ] = # # # XY < # # # ZX

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

MFCLD< ~~•

# 6 , = = –X[˜] ‚# ÂŒ V Â? = " # # , # " == –¥ Âœ Xœ˜ ƒ # E # V „ # # # = –œ˜ # # V # # # = ] < = # # # #" = E

# # # # " ] ] % # Z = # # #  # ~ # # , # # = # Y = � = # # — # # # # ]

73 ,64-'0&')H*& % #" = # # # E # # ~ # ] ‚# = # V = E # # ## , ] # V E ƒ # # � # „ # = V ƒ? # # „ #" # , # E ] = E, " ƒ) 9„ –X˜] % , ~ # ~ , ~ E # , #] QV #

m ~ , ~ # , # ƒ m ÂŹ 3p = # m ÂŹ 6p # = # p # , , # # „] ) , i ~ # V #" ⎧ ⎪ ( jik ­3djik ) ÂŹ @ 3bi ­3i ­ ⎪ ⎪ ⎪ ⎨ k j [Ëœ k ( jik ­3djik ) ­(jik ­(djik ] ­ ⎪ ⎪ ⎪ j ⎪ ⎊ k ­Ëœ Ci (3bi ­3i ) ­(bi ­(i ÂŹ @,

ZZ

ƒX„

Eâ—Ś

‡~„

] < # ~ # = E # # , ] % , ~ # ~ , ~ # , # # #" mĂ—n 5nĂ—1 ÂŹ mĂ—1 ,

ƒ „

# V 5nĂ—1 # # # # # E

# # V #" ƒ jik (jik 3djik # (djik „ mĂ—n ÂŒ # Â? # # #" " E ~ # ƒ k # Ci „ # mĂ—1 # # #" ] V ~ # ƒ „ # # jik ÂŹ E ijk (jik ÂŹ E(ijk 3djik ÂŹ E3dijk # (djik ÂŹ E(dijk ] ‚ # #" # ~ # = # # V 5 , # , Â? # #" Â? # , ] # # = V ]

:3 G599&=)*- -'801 V = # # ƒ

= # # „ V 5 # ~ # #" = , = ] X„ 9 # # = , Â? mĂ—n ] = # = Â? /nĂ—(n−r) # # # = # # V = ## #" # = –X— X\˜] < Â? / ,

, # #" ]"] E( # Q # # % #" ÂŁ 9 = # ƒ%ÂŁ9„ –X\˜] E

V , = # # E # # + 5+ ÂŽ #V # # # = , , = #" # # %ÂŁ9 –XÂĄÂ˜] < # = # # # V ƒ# = Â? / = „ # # # ~ # # ~ ] F E # # Â? / # Â? E # # #E # ~ # V ] ‚ E = # = # = Â? # # #E # # # ~ V ] ) # # = E # ~ # # = # # = E = , # # V # ~ ]

Œ ~ # ~ # ~ , # ~ # ~ ~ , ] V i ∈ {1, 2, . . . , p} # # � = #" , j ∈ {0, 1, . . . , p}: j = i # # � , #" # #" , ƒ , # # # # # 0„ k # # � = #" ? # jik # (jik # # # ~ ƒ , j � # , i # ? # k„ = E V ] 3bi # (bi # , i ƒ # ~ = V „ 3djik V #" (djik V #" ~ 3i V # E # #" # #" � # #E , i (i �E # ~ #" # , i ˜ k # ˜ Ci E = E # V ? # k ƒ k „ # #

, i ƒ Ci „ = V ] < E Â? ƒ # V = [rx ry rz ]T „ ÂŒ # ⎥ ⎤ 0 −rz ry Ëœ = ⎣ rz 0 −rx ⎌ . ƒZ„ −ry rx 0

Z„ % , # # # S ÂŹ {xΡ } Ρ âˆˆ {1, . . . , n} # ~ # ] < , , # * * / ] ) # , ,E = ÂŒ ] ] V 5S # # #" = # # xΡ , Â? S = # #"

5S ƒ # # � „ # , � /S # " , # = � /]

‚ , = # ~ , ~ E # # # # # # # ƒ E = # , # # „ # V #"

Y„ QÂ? # # , Â? S ] ‚ , E Â? # # 5S , # ~ E # ƒ, S 5S ÂŹ S Â? # #„]

 Â„ # " # # # # S /S ÂŹ @.

ƒY„

‚ # # ÂŒ # , # # S 5S ÂŹ S # ~ # ƒ ## , # # ~ # 5S „]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

# = , , # E S # V # # "# # # ] < # ~ # # , = V E # , S V # ] # = # # = = == = # = # # 5S # # E = # # # , S ] ¤ # # ~ # S 5S = # ~ #

V 5S ]

>3 "O);=9-& # V V = E , = V # E = = # " == ¡ X # # # E = # # V # # # = E ] ) = " = # #" E # E, " ) 9 # E , , = # # #E " # # # ~ # # ] % ,E ~ # V 5 # # E # = / # # ] & # # , # # = ] >323 N(6==-( /6'805' *'5)H04 # = = # " == # = V # E # ¡ X ] ] " ] # = # # ) "] X # ) 9 # # ) "] Z] # # # " , ## , ? # ] < ? # V # # #" # = # # ] V # # " 9F) # # ] = # # , == # # # # ~ # # = ] < # " = V # # ~ # # , # # # ( , # ]"] ¡ X ]

& ' ' & + W Y = # " , = # # ] ] = , =] + #" ) 9 = # # ) "] Z ~ # # ~ , , # ~] X

E , X ⎧

0 ⎪ E 11 S12Dt E 13Br ­3b1 ¬ @ ⎨ 1 S01Bt

˜ Bt 01 S01Bt E˜ Dt 11 S12Dt E˜ Br 13Br ­ ⎪ ⎩ ­M01Bt EM12Dt ­˜ C1 3b1 ­Mb1 ¬ 0 E , Z ⎧

1 ⎪ ­ 11 S12Dt E 24Cr ­3b2 ¬ @ ⎨ 0 S02Ct

˜ Ct 10 S02Ct ­˜ Dt 11 S12Dt E˜ Cr 24Cr ­ ⎪ ⎩ ­M02Ct ­M12Dt ­˜ C2 3b2 ­Mb2 ¬ 0 E , 13Br E 34Er ­3b3 ¬ @ ˜ Br 13Br E˜ Er 34Er ­˜ C3 3b3 ­Mb3 ¬ 0 E , Y 24Cr ­ 34Er ­3b4 ¬ @ ˜ Cr 24Cr ­˜ Er 34Er ­˜ C4 3b4 ­Mb4 ¬ 0

& ' "' x Y # = # E , , ϕ4 ¬ E π6 rad ) "] X ]

\

¡

S01Bt S02Ct # S12Dt # # ? # # V = = # ? # ] V # k # E ] = # = # = # ? # # # = ] ) E = 34Er # # #" , 3 4 # ? # = # E V E # #" ,, V # # = = ? # r V ? # t # # ? # c # ? # ] + E T T T V [0 1] [1 1] # [1 0] # E # # == = # Z

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

] V # = # E = , V # ˜ i ¬ [−riy rix ] i ¬ [rix riy ]T ] ~ # , # # = , ~] ] ¤ # E , # # ~ # ⎧

0 ⎪ S01Bt E 11 S12Dt E 13Br ¬ E3b1 ⎪ 1 ⎪

1

1 ⎪ ⎪ ⎪ ⎪ 0 S02Ct ­ 1 S12Dt E 24Cr ¬ E3b2 ⎪ ⎪ ⎪ E ¬ E3b3 ⎪ ⎪ ⎪ 13Br 34Er ⎪ ⎪ 24Cr ­ 34Er ¬ E3b4 ⎪ ⎪

⎪ ⎨˜ 0 S Dt 11 S12Dt E˜ Br 13Br ­M01Bt ­ Bt 1 01Bt E˜ . ⎪ EM12Dt ¬ E˜ C1 3b1 EMb1 ⎪ ⎪

⎪ ⎪ ˜ Ct 10 S02Ct ­˜ Dt 11 S12Dt E˜ Cr 24Cr ­M02Ct ­ ⎪ ⎪ ⎪ ⎪ ⎪ ­M12Dt ¬ E˜ C2 3b2 EMb2 ⎪ ⎪ ⎪ ⎪ ⎪ ˜ Br 13Br E˜ Er 34Er ¬ E˜ C3 3b3 EMb3 ⎪ ⎪ ⎪ ⎩˜ Er 34Er ¬ E˜ C4 3b4 EMb4 Cr 24Cr ­˜ < ~ # #"

# = ~ # ~ , # , ~ # ~ ~ E , ] F,V ~ # , , ] V # E # = # # # # #" # # # # " E # # #" = E # # ] ~ # , # # # ] # # # # V # # # 5 # # = # #" 5 # # ~ # " E # V E ] # = V # # # 5 #"

512×1 ¬ S01Bt S02Ct S12Dt T13Br T24Cr T34Er M01Bt M02Ct M12Dt

T

,

X[

# 12×12 # ⎤ ⎡ 0

@2×1 E 2×2 @2×2 @2×2 @2×1 @2×1 @2×1 E 11 1

⎥ ⎢ 1 1 @2×2 E 2×2 @2×2 @2×1 @2×1 @2×1 ⎥ ⎢ @2×1 0 1 ⎥ ⎢ ⎢ @2×1 @2×1 @2×1 2×2 @2×2 E 2×2 @2×1 @2×1 @2×1 ⎥ ⎥ ⎢ ⎢ @ @2×1 @2×1 @2×2 2×2 2×2 @2×1 @2×1 @2×1 ⎥ ⎥ ⎢ 2×1

¬⎢ ⎥, ⎢˜ Bt 01 ˜ Dt 11 E˜ Br @1×2 @1×2 1 0 E1 ⎥ 0 E ⎥ ⎢ ⎢ 0 ˜ Ct 1 ˜ Dt 1 @1×2 E˜ Cr @1×2 0 1 1 ⎥ ⎥ ⎢ 0 1 ⎥ ⎢ ˜ Br @1×2 E˜ Er 0 0 0 ⎦ 0 0 ⎣ 0 0 0 0 @1×2 ˜ Cr ˜ Er 0 0 0 XX @i×j i × j # 2×2 # # 2×2] # r ( ) ¬ 11] # = /12×1 = # # = , #" E T XZ / ¬ [@1×9 • • •] , • # # # #E # /] < # = / " V # # , V , # E = # #" # # " #] ZY

E◦

~

< # # ] # ,] X # = # # # # # , E S = # ] ¤ # = , , , S # /S # E " # # # Y ] # ,] Z , # = # ] < # # 6

,, V # U # #" # # ~ # < # #" # #E # ~ E # ] + # # # 6 # S # = = # # , # " # # , S # ~ # # , # # S 5S E = # ~ # V 5S , #" E # ] ? ' "' O Y % #

Q # 5

# #" S

6 # Bt

S01Bt M01Bt

X X[

6 # Ct

S02Ct M02Ct

Z XX

6 # Dt

S12Dt M12Dt

XZ

6 # Br

13Br

Y

6 # Cr

24Cr

\ ¡

6 # Er

34Er

? ' ' / W Y E # Y V

6 #

S

6

6 # Bt Ct Dt

= @

Z

<

6 # Br Cr Er

=@

Z

U

% #

< # = # ~ # # # " ] QV # # # # V ? # # ~ # # E # ? # # ## , # ~ E # ] < # # = V = E , # ¡ X ] >373 *'5)'-1 N(6==-( # = # = # " == ] #V " # # = V = V # ~ # # V ? # Cr] ¤ # E # = V E

# # # ) "] X ) 9 " == = # # ) "] ] # = E # = V ] # = V # # E # 5 # , # # V #" ~ E

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

? ' ' / W E # Y V

6 #

S

6

6 # Bt Ct Dt

= @

Z

<

6 # Br Cr Er

=@

Z

U

9 V Cr

=@

X

U

% #

# ? # Cr # , " # ~ # ] V # ~ # # E

# " V # = V = ] ¤ # " , " E, # ]

& ' ' & + W

>3:3 FJ-()*'5)'-1 N(6==-( = # , = # # 5

513×1 ¬ S01Bt S02Ct S12Dt T13Br T24Cr T34Er M01Bt M02Ct M12Dt Md24Cr

T

.

X

) V # 12×13 ] % # XZ # V 5 # = V = XZ # # # = V E

= # # # = ] # # # #" 13 ¬ [@1×9

E1

0

T

1] .

= = # # V E = # " == ] # # = V = ] # # #" # = # # ) "] X] +# # # # ? # Bt XE9F) # # ] < ) 9 " == , E # == , X # # # ) "] Y]

XY

< # r ( ) ¬ 12 # # E = /13×1 = # #" E T / ¬ [@1×9 • • • 0] . X # ,] # " # ,] X = E V ] ] # = # # # # , S ] E

V ,] Y # # ] + = # ? ' ' O % #

Q # 5

# #" S

& ' ' & + W

6 # Bt

S01Bt M01Bt

X X[

6 # Ct

S02Ct M02Ct

Z XX

# = E # # = V = # V # # E # 5 #"

6 # Dt

S12Dt M12Dt

XZ

6 # Br

13Br

Y

6 # Cr

24Cr

\ ¡

6 # Er

34Er

9 V Cr

Md24Cr

X

# 9F) # # V V #"

514×1 ¬ S01Bt S02Ct S12Dt T13Br T24Cr T34Er T

M01Bt M02Ct M12Dt Md24Cr Fd01Bt ] ,

X\

Fd01Bt V V #" == # # # ? # Bt =

# = ] # 12 × 14] X # Z

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

= V = XY # , # T T T 1 1 @1×6 ˜ Bt @1×3 . X¡ 14 ¬ 0 0 V # r ( ) ¬ 12] # = /14×2 = # #" #" / ¬ [@2×1

T

/1 ] ,

X

/1 , 2×13 # # # # # ] < = V # = # # # = V = , # ] , # \ # # # E ~ # # # = V ] + E ? ' #' O % #

Q # 5

# #" S

6 # Bt

S01Bt M01Bt

X X[

6 # Ct

S02Ct M02Ct

Z XX

6 # Dt

S12Dt M12Dt

XZ

6 # Br

13Br

Y

6 # Cr

24Cr

\ ¡

6 # Er

34Er

9 V Cr

Md24Cr

X

9 V Bt

Fd01Bt

XY

& ' #' x Y

# ~ # # # E V #" # # = #

T 5π T " ¬ [ϕ01 ϕ12 ϕ23 ] ¬ 0 3π rad] 4 6 + z # # E = ] ¤ # = , k ]T V ? # (kdijk ¬ [0 0 Mdijkz k k # ? # kijk ¬ [Sijkx Sijky 0]T # k k T 3dijk ¬ [0 0 Fdijkz ] , = ? # k k V # # (kijk ¬ [Mijkx Mijky 0]T ] + V # # # 5 #"

Ar Ar Br M01Ary | T14Br M14Brx 546×1 ¬ T01Ar M01Arx

? ' ^' / W

% #

# 6 # Y S V

T Er Er Fr Fr M23Ery | T26F r M26F T23Er M23Erx rx M26F ry | 37Gr

6

6 # Bt Ct Dt Br Cr Er

= @

Z

<

9 V Cr Bt

= @

X

<

= V #" # ? # Cr # Bt # #E # ~ ] V # V #" # #E # ~ # # # # #" ] < #" # E # # ~ # , V #] >3>3 -1541)4' )46=59)'0( # == E , = # # # = E # # ] # # ) 9 # # ) " ] # \ E = V ] # # V # , ## , # # ? # V # V # E # ] # = 9F) ] Z\

Br Cr Cr Dr Dr M14Bry | T12Cr M12Crx M12Cry | T25Dr M25Drx M25Dry |

Gr Gr Hc Hc Hc Hc Ic M37Grx M37Gry | S45Hcx S45Hcy M45Hcx M45Hcy | S67Icx T Hc Ic Ic Ic Ic Ar S67Icy M67Icx M67Icy | Md01Arz | Fd45Hcz | Fd67Icz , X

| = # = V , E ] # 42×46 # r ( ) ¬ 42] E " # , 1 = # ] = # #"

~ # ~ , , # 1F 3×46 ¬ [ 3×3 @3×2 E 3×3 @3×2 E 3×3 @3×33 ] ,

Z[

3×3 # # 3×3] = # #" ~ # ~ , E ~ V

Ar 1M 3×46 ¬ ˜

0Ar $ E 0Cr $

E˜ Br

E 0Br $

@3×28

0Ar )

E˜ Cr @3×2 ,

ZX

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

,] ¡ # = # # # # , S ] E = = # # ,] ] < # # # # ] ] # ~ # V #"

,V # # = # # # ~ # # # V ? # = # A # , # , # # = ] ¤ # , # ~ # ] ? ' q' O

& ' ^' & + W

$ ¬ 3×2 = V E ¬ [1 0 0]T # ¬ [0 1 0]T 0k # E #

# " ,

# ? # k \ ] < # #" # " , #" ~ , ~ # , ] % , ~ # = # #" # = / 46×4] #"

% #

Q # 5

# #" S

6 # Ar

Ar 01Ar M01Arx Ar M01Ary

X

6 # Br

Br 14Br M14Brx Br M14Bry

\ X[

6 # Cr

Cr 12Cr M12Crx Cr M12Cry

XX X

6 # Dr

25Dr Dr M25Drx Dr M25Dry

X\ Z[

6 # Er

Er 23Er M23Erx Er M23Ery

ZX Z

6 # F r

Fr 26F r M26F rx Fr M26F ry

Z\ [

6 # Gr

Gr 37Gr M37Grx Gr M37Gry

X

6 # Hc

Hc S45Hcx Hc S45Hcy Hc M45Hcx Hc M45Hcy

\

6 # Ic

Ic Ic S67Icx S67Icy Ic M67Icx Ic M67Icy

Y[ Y

9 V Ar

Ar Md01Arz

YY

9 V Hc

Hc Fd45Hcz

Y

9 V Ic

Ic Fd67Icz

Y\

? ' z' / W

% # / ¬ [@4×6 /1 @4×1 /2 @4×1 /3 @4×1 /4 @4×1 /5 @4×1 /6 @4×1 /7 @4×1 /8 @4×1 /9 @4×1 /10 @4×1 T

/11 @4×1 /12 @4×2 /13 @4×2 /14 @4×3 ] ,

ZZ

/i i ¬ 1, 2, . . . 14 , # # #" # # # # " = V E = ] V (/1 )4×1 (/2 )4×2 (/3 )4×1 (/4 )4×2 (/5 )4×1 (/6 )4×2 (/7 )4×1 (/8 )4×2 (/9 )4×1 (/10 )4×2 (/11 )4×1 (/12 )4×3 (/13 )4×2 (/14 )4×1 ]

# 6 # Y S V

6

6 # Ar

=@

U

6 #

Br Cr Dr Er F r Gr

= @

<

6 # Hc Ic

= @

Y

<

9 V Ar Hc Ic

=@

X

U

Z¡

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

>3?3 FJ-()*'5)'-1 -1541)4' )46=59)'0( = # V #E # # = ] # # = E = # # ) "] ¡ # ) 9 # # ) "] ] # , #" =E = # # #" , #

# # # = # # = V = ] ¤ # # # V # E # Y # = ]

& ' q' x Y # ~ # = # = E # # = = Y]Y] V E # # # 5 #"

Ar Ar Br 561×1 ¬ T01Ar M01Arx M01Ary | T14Br M14Brx Br Cr Cr Dr Dr M14Bry | T12Cr M12Crx M12Cry | T25Dr M25Drx M25Dry | Fr T Er Er Fr M23Ery | T26F r M26F T23Er M23Erx rx M26F ry | 37Gr Gr Hc Hc Hc Hc Ic Gr M37Grx M37Gry | S45Hcx S45Hcy M45Hcx M45Hcy | S67Icx Ic Ic Ic Ar Hc Ic S67Icy M67Icx M67Icy | Md01Arz | Fd45Hcz | Fd67Icz | T18Jr Jr Jr Kr Kr Lc Lc M18Jrx M18Jry | T29Kr M29Krx M29Kry | S89Lcx S89Lcy T Lc Lc Lc M89Lcx M89Lcy | Fd89Lcz , Z

# Y\ # = V = ] # = # #" V , # " = E V = ] V 54×61 # # r ( ) ¬ 54] % , ~ # # = /

61×7 , # = ] #" / ¬ [@7×5 /1 @7×1 /2 @7×1 /3 @7×1 /4 @7×1 /5 @7×1 /6 @7×1 /7 @7×1 /8 @7×1 /9 @7×2 /10 T

@7×2 /11 @7×1 /12 @7×1 /13 @7×2 /14 ] . Z

ZY

& ' z' & + W

V , /i i ¬ 1, 2, . . . 14 # # # (/1 )7×5 (/2 )7×1 (/3 )7×7 (/4 )7×1 (/5 )7×2 (/6 )7×1 (/7 )7×2 (/8 )7×1 (/9 )7×3 (/10 )7×2 (/11 )7×1 (/12 )7×1 (/13 )7×11 (/14 )7×2 ] % # # V 5 # # = V = # # # # # ,] ¡ == , ] V # E " # #" # " V # # ,] ] ) # = = #E # ,] X[] < # # # E # ] ] # # V ? # A # # E ~ # # # # # V E # # # #E # ~ # V #" E # = ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

? ' {' > Y

% #

Q # 5

# #" S

6 # Jr

Jr 18Jr M18Jrx Jr M18Jry

Y¡ X

6 # Kr

29Kr Kr M29Krx Kr M29Kry

Z \

6 # Lc

Lc Lc S89Lcx S89Lcy Lc M89Lcx Lc M89Lcy

¡ \[

Lc Fd89Lcz

\X

9 V Lc

# 6 # Y S V

~

# ] # # #" , V # # ] ¤ # # # E ~ # V #" , = " # # ~ # ]

@,GFB "IN" "G , # == , < E # % # # & # # " # # ] 9Q EZ[XZ¨[¡¨ ¨% ¨[ ] > # > ? # & # , # ]

D EF

? ' " ' / W

% #

E◦

6

( A= ∗ 9 V # # # 6 , # + # # +== # ) & # + # Q#E " # #" > U# V # " < E ? ZY [[ \\ > & # E = E § ]= ] ]= ] C $ 3 D $ = 9 V # # # 6 , # + # # +== # ) & # + # Q#E " # #" > U# V # " < E ? ZY [[ \\ > & # E ? E § ]= ] ]= ]

6 # Ar

=@

U

6 #

Br Cr Dr Er F r Gr Jr Kr

= @

<

∗

6 # Hc Ic Lc

= @

Y

<

"A" "G@"

9 V Ar Ic

=@

X

U

9 V Hc Lc

= @

X

<

?3 @04*95&604& = = = , # #E # ~ # ? # # # V # # E # , # , # " #" E = , #" # # #] + # # E, == , # V = # = # V # ~ # ? # # # V #" E ] = # # , E , = V # # V E # # ] = # # # # # # = , E # #" # ~ # ] == , , = # # = ] == V = V , # # " == ¡ X E # # # = # # V #E # # = ] # " # " ? # # # # # E # # # # = , # ] # V # # #" V #" # E # ] , = # # = Y] V E # " == # #E # ~ #

= # #"

X 6] ] # # (] ] < , 6

Y < ( %

X [ \ %* E ¤ Z[XX] Z %] V # ] F # ] 9] > ] # E 6 # # # = ¢] # ] % E # # F] , ] 68 > *' & ( ' ] %= #" E£ " # ¤ , " Z[[ 9F X[]X[[¡¨ ¡ E E Y[E [ [XE ¯XZ] Q] %] # # 6] #" #" # # # # # , ¢ & ' V ] X # ] X ¡ \ 9F

X[]X[X¡¨%[Z\ ¡Y¡ ¡[[[\¡Z] Y (] ] ( ± # # ] 9] ± E5 ± = E , # # # # # # #" # # ~ E # # # # # E # # # # ¢ < ' * 6 / X / V ] [ # ] Z[X XX YX 9F

X[]X[[¡¨ XX[YYE[X E E¡] 6] ) # # 9] Q] F #] 9 # ¢] #

] % # # F] , ] 68 > *( ' & ' ] %= #" E£ " # ¤ E , " Z[[ 9F X[]X[[¡¨ ¡ E E Y[E [ [XE ¯ ] \ (] ) * # ] > ? / *] Z < * ' > E # < E # # > Z[[ ] ¡ (] ) * # ] > ? F# # ~ E V , # = , E Z

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

E◦

~

# # # # # # E , # # ? # ¢ < / * < ( V ] Y\ # ] Z[XX XZ Y 9F

X[]X[X\¨?] ]Z[X[]XX][[ ]

X ] > ? ( # 6 # ) # , % 6 # # # # ¢ < ' *

6 / X / V ] XY # ] X Z[[ Z Y\ 9F

X[]X[[¡¨ XX[YYE[[ E \¡E[]

5] # V &] ) # (] ] % # & FV # & # ) #" % #" ? #" % # # # ¢ < ' * 6 / X / V ] XZ # ] Y Z[[Y X¡ Y 9F X[]X[[¡¨ XX[YYE[[YE Z ZEX]

Z[ ] > ? ( # # # " , # = # # #E # ¢ < / * < V ] YY # ] XZ Z[[ ZZ\ ZZ¡ 9F

X[]X[X\¨?] ]Z[[ ][¡][[ ]

+] ª + # V V # # = = # # # # # # # %E V

# ¢ < ( ' * 6 / X / V ] X\ # ] Y Z[[\ [ [ 9F X[]X[[¡¨ XX[YYE[[\E [Z E[] X[ ] &' # (] ) * ] # ? # # E # ± ? # =' ± , * # E ¢] # ] #± # ] #± ] 0 ( ^8 ' Z / ! & < & # > ?] Q # ] ] X ] F # > E # & # > ? > Z[X\ # # XY] ? # # ? 6 E , XY] 6 & # ± ? & # % =E , XYth X th Z[X\] XX ] &' # (] ) * = # # E = # # , E # ¢ 7 * +88 * < ( V ] Y # ] Y Z[X\ X X XY[Y 9F

X[]X \ Z¨? E= ] Y]Y]X X] XZ ] &' # (] ) * = # E # , # E # # # # ¢ + < ( %

V ] \ # ] X Z[X\ XXZ 9F X[]X X ¨ #"EZ[X\E[[[ ] X ] &' # (] ) * ] # # E " , # # # # ¢] # _ 7 $ < ' * 6 / X / `$<6X 34!?{] # ± # Z th ( # Znd Z[X\ Q # +, & , ] XY ] &' (] ) * # &] ] % V ,

# # #V # = , = # # ¢] # 3!st $ ( < * * < * + ( / * & ' `<<+& 34!?{] ' E ? & # + " Z th % = , Xst Z[X\ 9F X[]XX[ ¨ +6]Z[X\]¡ ¡ [¡ ] X ] % #" $ * | + ' [ _ %* > E , " & Z[[ ] X\ ] % #" # ] | + ' [ = * [ * =06 > , " & X ¡] X¡ # # # = " + 5+ ² E ² 5 # + " , ² +# + 5+ ® =] X 5]E>] & ' + } < 6 * 0 < 8 > # # X ] [

MFCLD< ~~

ZX ] > ? # (] ) * ( # # # E " , , # # # # # ¢ 9 0 + * / 6 6 V ] \[ # ] Z[XZ \X¡ \Z\ 9F X[]ZY¡ ¨VX[X¡ E[XZE[[¡ E ] ZZ ] > ? # (] ) * = # % ¤ # #" 6 # # #E # # , % % # ¢ 7 /8 * \ X ( / V ] # ] Z Z[X [ZX[[¡ X 9F

X[]XXX ¨X]Y[[\ ] Z ] > ? # (] ) * % V , E # # " , #E # # # # # # ¢ < ' * 6 ( / X / V ] [ # ] Z Z[X X X¡X 9F X[]X[[¡¨ XX[YYE[X E ZE[] ZY (] > (] > #" ] 5 # 5] > #" & # +# # +== # 6 # # + E & # = #"¢ 7 $ * & ' 6 / V ] [ # ] Z Z[[¡ X\ X [ 9F X[]X[[¡¨ X[ Y\E[[¡E X EY] Z (] > (] > #" 5] > #" # ] 5 9 # # # = # E9F) = # = # # # ¢ < / * < ( V ] YY # ] Y Z[[ Y 9F

X[]X[X\¨?] ]Z[[ ][Y][[Z]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

9

6 + ; % !

£ !" "#$ %&'(' )# ! %&'()*'+ 2 " " " $ % " $ 0 ,-./0(1&+ " 3

23 4'(015*H04 # #" # ~ = # , , V V #E , # #] # E # # , #" # V =

] # = V # E # # V 9F+ ] # E #" # # ,? , #"

" F) # "# # , # #" 9F+ = , #" = E , # ZE9 # E9] # ZE9 # E #" # ] 9F+ # E9 V # #" # , = ] # = = ZE9 # ] == ] = # V # " # = , # # # # = # E = # V # # # = V , # , #" = = # # E ] V # " = = = = # # # == #" "# V #] # == , # = #" = "E # # 9F+] # #] # V = # == =E = #" = , # # , E # V , "" # "# V E #" ] # == = # # X[ XX ] = = # " #" 9F+ = , # = # = # = V V # = = ] = = " # # # # ³ # % # = E == = , # #" "# E V # # # # , # V ³ % # Y , = = # # = E , # , " , # E # , # V , "" #

V #" "# ³ # % # E # # # # # # # E , ³ % # \ # # V , #

= = ³ # % # ¡ E = # = # # ³ % # " E V # # # ]

73 -9)'-1 B0(C > # #" # #" # # 9F+ #" # , # F) # , == ] ¤ V = # "# # # , ] E == , # = , ,? = # # # # ,? "# #] Q E = # == ZE9 = # # Y X¡ ] = E , #" = # # ] V E , # Y , "# E # #

# V # # = # " #" ] E = # # XY # == , # E9 ] +# E " E9 # , # # = # # ¡ X\ ] #" # # = # = # # # 1◦ ] + # E V # " == # # # # = # ] = , "# # ZX ] = = # #" # # #" % # = \[[ # # =E # ] = = # = # , # , # = # # # " " # V = , "# , ,? ] # = # # ] # #

= # " # #] E , " # # ] V # # = # # , ] #"

V # # , = E #" #" # == ] , # V = E F) # # ~ # = , # [ ¤ # # XZ μ ] F) # , = #" XE, # V # = ] # % # = \[[ # # # , " [ ] E # = # V # , ] > # # V # # , V #" ] # #E X

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

" = # = = # # # # ] > # #" # V 9F+ U% U = % "# # E # , ] # Z[ == E #" V # # # #

#V #E # ] + 5+< # E ] # # # , = #" V V # # = ] = , # #" = # # ,? # # ] # X E "# E £5% = = E # ] "# V # # "# # # = # # #= , #" #" # #" "# ] = #" = E= # " = E = , # = V # V E # #= , E #" #" ] Q % # " = , E = # ] # # Q % = # = # ] = # # , #" " # E # # #E, ] = E # # # , #" = # # # # , # # # , # E # E , #"] %= # X ] = # == # , #" # ~

# E9 = ] , # #" , E , # , #" = # #

V = , , # E9 # E # # = # = " V =E = #" ] +# == , # = E #" "# , # # V = # # \ ] , # "# E = # #" = ] # == = "# #" #" =

= # # = = # #] # # = = # # E = ] == , # #" V "

9F+ # # #V V E V = # = # V ] == E = # # = = = ,

= # , # # , # "

9F+ #]

Z

MFCLD< ~~

E◦

~

= , , # V "# E #" ] # = , , V , = , ] # #" # # , ] > # , # V # = # # "# "# V # # " ,

V # , = # V ] # # # #E " "# V # # #" V ) "] X # , ] + #"

& ' "' > V # # , = , t01 # # V , # # "# # , V R0 # #

# , R1 ] # #" E # , # # # #" φ

φ = #

va t01 s = # . b b

X

s # V # V R1 # # V R0 ) "] X va = V # b # , # V R0 # R1 ] :323 ;%6P56'. 0< #8)&- 86Q I-'-(;64)H04 # # # #" Q~] X # , =E = # V #" λ " # ~ # "# # # b < λ2 ] " = b , "" = ## , # ) "] Z ]

:3 I-'-(;64)H04 0< 6P4)9 ((6J)9 I6(-*H04

& ' ' > |

+ #" "# # , # = # # # E # = # V # # , # , = #" E #" # ] #

V # # = ] # # E # # ,? # # #" # # V , = # " ] ¤ V , "" # , # V = # E

= , # # #" # # V s ) "] ] & = = # = E ~ # Y[ ¤ ] # # # # #" V , ]¡ ] # # , # V ## , " # Y] ] U# # V , # E # = , # #] # X[ XZ XY

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

& ' ' > φ s

# X\ ] # = E V %& [Y[YU9 , Y ] ¤ E V # # V = = # E V , # ] > # # #" # # " = #

, " # Q~] X # , # # " # # , # , # " #" " = V 0.5λ] # V V , E = # # # # #" φ E , " # # ] ) " Y # = # # # #" 0◦ # 30◦ = V ] # #E # = # # " V # " = ] ) # # #" φ = 0◦ , " E

,

& ' ' >

Y φs Y φ } J 0◦ % J 30◦ # == # b > λ] ¤ V φ = 30◦ , " # , # b ≈ 0.65λ] # φ = 90◦ , " E # == # b = 0.5λ] " = # # ) "] Y # , "" " = , # E V , " # # V # ]

~

,

& ' #' *

Y φs 0}' " Y φ b } "" % "#

" # ]"] # E # #" ~ −20◦ #" = V " = b = 11 = , # Q~] X {−22◦ , 20◦ }] ) #" # = E V " = b = 15 = , E # {−48◦ , −11◦ , 20◦ , 68◦ }] # = # # # ) "] \] # E

E◦

,

& ' ^' 0~ } 20◦ ' Y b } "" % "# # = V # E # #" ] # # == # , == ] # #" #" =

V = , = ] V = # V

, = # , # # ] ) # # E # #" V , # ) "] ¡ ] # = = V

:373 ;%6P56'. "96;64)H04 > # = #" " # ) "] Y # , # #" #" # ~ # " V # " = # # # #" V ] 9 V # V #" = = # # , #" # # [−70◦ , 70◦ ] = = # # # E #" ] ¤ V # #E # V #" ] # #" V , " E # # # #" φ " #" = , #" φ # #" ] # ) "] " " V #

~ #] V = , E # Q~] X # # V # # #" φ] " = " = E # XX # X ] " # " V # # #" = # V #" ]

& ' q' ? W # # = # "# ] # # #" "# #" ) "] ] = V = E # # V

# # # #" , = V V # " = , # V ] # # # = # # = , = V " # #E " #" # # V ] # ? = , # , V = = # # ) "] \ ## ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

V = V {R0 , R1 } # {R0 , R2 }] # V # t01 # t02 ] #" ~ # 0 = A #(ωt01 − kx b1 ) Z 0 = A #(ωt02 − kx (b1 + b2 ))]

& ' z' @ Y W +

W

, = ## V # # = ] # = # # # V , # = , " ] ) " , # # # # V = V ] = = # #

& ' {'

Y b } "" "# # # # # # # ]

>3 4*61-4' 4P9- I-'-(;64)H04 + #" "# # , # # " E V # # # , == , #" # # S(t) = A #(ωt + γ), A = "# ω #" ~ # # γ = "# ] > # # E #" ZE9 # V = # # ) "] ¡ V "# , # # , E , # # R0 ] # "# V , V # , = ⎧ ⎨ S0 (t) = A #(ωt + γ ) S1 (t) = A #(ωt − kx b1 + γ ) ⎩ S2 (t) = A #(ωt − kx (b1 + b2 ) + γ ) kx x

# V V =]

"# # , E V # = E # # V R0 ] = # = # # , V # "# V , V " V # V ] # # "# V , R1 # R2 # E #" " "# V , R0 V [] + # = , E V R1 R2 # # #" " "E # V , = V V V [] , # # = Y

# #" E " Q~] Z ~ V #

2πn1 = ωt01 − kx b1 2πn2 = ωt02 − kx (b1 + b2 )] n1 # n2 # " # , ] #" # E #

ω=

2π # φ 2π # λ = va Ta , # kx = Ta λ

Ta = V "# Q~] # , # # #" n1 λ = va t01 − b1 # φ n2 λ = va t02 − (b1 + b2 ) # φ]

~ # φ , # ] # E # , # V " V 1λ φ = # va t01b−n 1 Y va t02 −n2 λ φ = # b1 +b2 ] # = , φ # # = , # #" V n1 n2 ] > # b1 = b2 ~ # V # # # # # , # = V n1 # n2 ] # #" # # # # , va t01 − n1 λ va t02 − n2 λ = . b1 b1 + b2

¤ V # # # #] , " # ] = , E # = V #] + = V # , " # , V , #" = E = " = b1 # b2 ] # #" [nmin , nmax ] # = (n1 , n2 ) , ] = E # # #" φ , #

X Q "# # # = E # t01 # t02 ] Z ) # = n1 , n2 ∈ [nmin , nmax ] # # Q~] ] U #" Q~] Y # φ] # # # # # Q~] , # E #" va t01 − n1 λ va t02 − n2 λ < ε. \ − b1 b1 + b2 V ε # , V

t01 # t02 # ] Q b1 # b2 # # , # # # #" , # = ] # # ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

?3 05(*-& 0< "((0(& + # # " , # # , # # # # # ] # # #

# # # , , E #" # "# ] # # # , # #" ] # E # # V # # # ] ) # E # , , # = ] # ] # = # == E V ] # = # # ] = # V # "# ] U# # # # # # #

#V # # , ]"] , , # , # # = # V # # # , "# # ] E # # # # ,? # , # # X ] # # V = #] > # E #" # # #" # "# # , V R1 # E # V R0 # , = Δt01 = Δtn + Δtr + τ Δtn # , # Δtr # # τ # , = #" = # V V = E # V ] ]"] E #" "# , # # , , # , ] ¤ V = # = E # # % # ¡ # E # # # = # # # # # V V # # " " , ] ?323 8- "((0( 0< '8- #9)4-/)J- &&5;=H04 V τ = # # = E # # # # #" # #

V ] ) ZE9 "# # = # # , E V ] # " # V = = " = #] > "# # , = # E = = " = E V ] #" "# = # V V "# , # E # V # = = # V ] # # , = E #" = # "# = # V ] # E #" V #

# R0 # R1 # = # d "# S # # V , "" # # = # # ) "] X[] # V # = = " = # V = , # V ] # s = 0 = ) "] X ] + #" V = # # V R1 V "# , # = ]

V # # Δs τ= va

MFCLD< ~~

E◦

~

Δs # , # V # = = # V # V # = # # va = # V ] > # = #" = " = # # Δs

& ' " ' > ~ Y W W Y

= , #" Δs =

d 2 + b2 − d ≈

b2 2d

d b.

¡

b # , # V # d # , # "# S # E V R0 ] # # Δs = #" = # # V = # V ] Q~] ¡ # # V = # # # # ] Q = Δs V #" # # # # # ) "] XX] " V " = b # XX # ZZ ] ) # #" , V []

& ' ""' Δs Y = Δs = E ] == = = E

# ~ " = b , # V E , # b # # # Δs ) "] XX # ) "] XX, ] = b # E Δs # , # # = ) "] XZ # ) "] XZ, ] = V E

& ' " ' Δs Y b } '# "'# Y W Δs # # # " X ~ , 2.92 μs] = # = # b #

" # , [ Δs #

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

, , " "# = # [] " ] ? = # # = # V ] ) #" # # ,

Δt01 Δtn + Δtr . # #" #

V = = # # ) "] ¡ b1 # b2 < 30 # , # , = ] # # # Δt , ] ?373 4)**5()*. 0< R6;5'8 I-'-(;64)H04 # = # Δt

# φ # # Q~] X # , = ] U #" # == E # # # # #" φ # = Δt #" , # va Δt. Δφ = b 1 − sin2 φ

= # " = # # ) "] X ] == # V

φ # # " = b # Y # XZ ] = # = # = # Δt = 1 μs] # #"

& ' " ' ~ Y Δφ φ Y ' ? Δφ Δt = 1 μs b' ? W "" Y W'

E◦

~

# ) "] X # # # Δt 2 μs] = = E # = , # # # , # ] # = V # # = = # # ) "] X ] , #

,

& ' "#'

Y b } "" "# ' S ' S Y μs% μs = = # #" # # # V # = ] "" = , , # # # #" φ] # #" V = μs # " # V ] E # # # #" = ) "] X , ] S323 -15*64P )&-964 # , # E # b # V = , E ] #" # # = V = # "# V = # V # = = " # = # # , V = E , #" V # "E # ] # = #" #" #E " # # # , V , , V ) "] X\ ] # V # # #

V Δφ # " ] # , # Δφ " = b , "" ] , Δφ #V = = # b] # ) "] XY # # # ] & ' "^' >

& ' " ' ~ Y Δφ φ Y Y b' ? Δφ Δt = 1 μs φ' ? W 0◦ 40◦ Y W'

S3 0%5&'4-&& > # #" # # Q~] # #" Δt , " V # V = , # Δφ # # #" = , V #" φ] # " = , # #

V = # # ) "] ] " # \

# , , ] # V , # ] , # μs = ] + #" V # , # T2 # T3 Y #E " = = , # Q~] X , # ] S373 0%5&'4-&& "&H;)H04 = # = = # #" # V φ # # # # #" #] ¤ V # # #E #] " = # # = E V , # # = , = = , "" = , V # # = # # ] , Q~] \ # , , V n1 #

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

? ' "' / W

& ' "q'

Y b } "" ' S ' S

Y μs n2 = # # # # V == #" E = ] # Q~] \ # # , # = # ] # ) "] X = = # # {Pn1 , Pn2 } # {Pn , Pn } E 1

2

b1 b2 #" φ ◦ ε Δt01 μs # Δφ ◦ Δφ ◦

XX X ±50 [][ ±@FJ ±1.2 ±1.8

XX Y ±50 [][ ±KFL ±2.2 ±3.5

Y XX ±60 []Y ±KFN ±4.7 ±11.4

Y XX ±50 [] ±7FN ±7.8 ±14.4

> # #" # # # #" # = , # = # # ε] # # # # #" # # , # #" # #" ) "] X ]

= # = V n1 n2 # n1 n2 = V ] # , # = #

& ' "z' ? Y ~ W 0}' ^% n1 n2 ] Q~] \ = {Pn1 , Pn2 } , # = ] ¤ V = {Pn , Pn } # = # #] 1 2 # # # = , V Δt # # V ε #" = # = , # = #

T3 "O=-(6;-4'& # = # = # #

= ] , # #" #

,? "# = #" ] ,? , , # # #" #" # # # # #

,? ] # # # "

# ,? # V E #" "# "# , " , ] = # E # # "# V # # = E = # ] + = # # = # # ) "] X ] #

X ) # φi ∈ [φmin , φmax ] E = t01 # t02 " V # # φi E ) Δti , Δtj ∈ [−Δt, Δt] E 9 # n1 # n2 t01 + Δti # t02 + Δtj ] E # # # # ε # "

= X ] E # # # ~ = V V E ε # Δt = # E # ] 6 # V ε # Δt # # =] E F # #

=] Z # Δt # " = X ] U #" = = # = " = b1 # b2 ] = #E # ,] X] # V # #" # E # # Δφ , # # φ = 0◦ ] V , E # #" #" ] # E ~ # Q~] # " = E # # ) "] X ]

& ' "{' O # = # ~ == = = = # # ) "] Z[ ] ) V # # # # # , # = # # ] # # ## #" V ) "] Z[ , = , #" # # , 1◦ ] ~ # E "# , Y[ ¤ ] # V E #" "# ]¡ ] " , X ] E # # # E , = # # ) "] X ] > # # # ,

#"

# E # # " , # # = # # ) "] ZX # , # #" α

# # # # #" ¡

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

,

& ' ' ~ % Y

& ' ' ? W 0}' {

& ' ' @ Y W W Y

,

& ' "' Z

W ~

W % φ

W Y α

W ' φ # "# V φ = −α.

#" # # # #" [−50◦ , 50◦ ]] Q~] # E # " # " # # ) "] ZZ] # , # , = #" # ## # X X] # Z ] # # # " #" V , ) "] Z ] U #" # "# V # , #" = # ] = ε 0.3] # # , == #" ] , # = # ) "] ZY ] %

, # #" # =

]" #" # #" E # # XZ ] = == E ] # " # , # #" # #" # # # # ,? #" ] ¤ V # = # # = # # # V # # ] < V , V #"

# # # # #" [−40◦ , 40◦ ] # , "" #

, "

= ] Q E # = X[ ] ) E # # V # σ = ] # = # # = E == 3σ = , E # #] # # # 0.33 μ ] # , X

# # ] # = , # ) "] ZY] E # V #" = # # ) "] Z ] ) # #" # # #" # # V V = # = E " # " ] # #" [−20◦ , 20◦ ] V "

] V E # # 3σ , 2◦ ] # # # # #" V # "E # # # , # = E #] ) "] Z\ = = = # # E # # #" # ] #" # # V # # E # = # # # = # E # ,? # #" # ] = V # = # V # Q # X = =

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

,

MFCLD< ~~

E◦

~

,

& ' ' /

$ " % "# %

= , , == = # E = # , # ,? ] = , # # # , , # V " # #" # #" # ]"] # Z X X ] ¤ E V # V # , #" # V # V " E # , , ] = V # # = # , , # = , ] #" # # #

# V V , # , # , E = #" ) "] Z\ # Z ] + # = # = # "# #] E # # #" = # E # , #" # # " # #" ) "] Z¡ ] ¤ V # #"

#" [−20◦ , 20◦ ] # , # " # # # " V , # # , # # ,? ] E # # # #" # V # # # Q~] ] " = # # ) "] X # # # = # #" ] = # E

& ' #' /

$ " % "# %

& ' ^' > Y

# #" # #E ] < V # , ]

U3 @04*95&604 = # V # " = # == E = , = = = E # # #

#" # , # E #" V # #" #" ] , # = = E # # #" # ] = =

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

@,GFB "IN" "G == , Q = # E # Q & # % # # ¤ " Q E # # # , QU )&¡ E\X[ [Z = E ? 6 9 6 9 "# # # & # # # Z[XY Z[X\ " #E # # # # # = ? ]

& ' q' / Y Y Y

D EF $ 9 = # , # E # 6 , ) Q # > U# V % # # # " ] ( # E " XX¨X¡ [E ¡[ > & # E

, " #] §= ] ]= ]

"A" "G@"

& ' z' / Y [−20◦ , 20◦ ]

Y

# #" "# V # # V = # = # V # # , = #E # # ] # # V , ] = E # , Z μ ] ¤ V # , , E ~ # ] # = V = # # , # = # = V = , V "

E # # ] = # E = #" # , # #" #" # = # , # E V , # # E V ] = , # , E # ] # # = , # E # #] " ] # = # == = #" # = E V #" ,? ] #

# # E V #" # # # # ] > # # # , = , , # ] + = # # ,? = # 9F+ E # # "

] # # V #" # E V # # = , E # #] # , E # # ] # = # # = ] # # # # # = , "" ] Y[

X ] +] == # 6] Q ## E #" % #" = #"E = # # , #" #E " # #" = = ~ # ¢ * 6 / $} & 0 8 [ $%%% V ] # ] Y Z[[\ ¡\ ¡ X[]XX[ ¨ % ]Z[[ ] \X ] Z ] # (] 5 # & , , E # # #" + = = == ¢] # 0 Z _ $ Z ( +* * & ' & X X XZ ¡ XZ Y X[]XX[ ¨ +6]X X]ZY[ ¡¡] +] ¤ +] ¤ == &] % "" +] > # &] > , # # E == #¢] # 0

* @ + ( + <5$%%% $ < ( ' /8 * \ < © <© U%+ X \ X[]XXY ¨ X Y X] X Y¡\] Y +] ¤ # 5] # ) " E # #" # ¢] # $ & ' * 6 ( / [ 344!Z 0

* Z 344! $%%%5&67 $ ( V ] Z[[X XYY\ XY X X[]XX[ ¨ 6F%]Z[[X] ¡¡X Y] ] ¤ # ] # (] ) # 9 E " "# = #" == # = # # , #"¢ + ( + V ] # ] X Z[XY Z¡ [ X[]ZY¡ ¨ EZ[XYE[[[ ] \ +] ] ] + " # ¤] ] ¤ #" 9F+ # V = E # ¢] # 0 $ % / & 6 /8 / = # Z[X Y Y ¡] ¡ (] ( # ] (] U # +] ¤ # # )] + V (] # Q] % # U #" & + # E E " V E "# # # E9 #¢ & ' [ $%%% V ] ZX # ] Z[[ [ ZY X[]XX[ ¨ 6F]Z[[ ] X ¡ ] 5] # # 6] , , # " # # #¢ $ Z 7Z & ' & Z V ] XY # ] Y X Z X X[]XX¡¡¨[Z¡ \Y [XY[[Y[X]

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

–œ˜ 9] ] ™F,? # # , #V E # # , #" # #" ÂŒ # ¢] ‘ > U# E V % # # # " Z[X\] –X[˜

] ™9 # # V #

# # "# ZE9 ¢] ‚# ] #± # ] #± ] 0 ^8 ' Z[X\  [ Âž XZ ƒ # & „]

–XX˜

] ™& # # #" # V #¢] ‚# 3!st $ ( < * * < * + / * & ' `<<+&{ Z[X\ X[¡\žX[Â&#x;X X[]XX[Âœ¨ +6]Z[X\]¡Â—¡Â—ZÂ&#x;¡]

MFCLD< ~~•

Eâ—¦

‡~„

‚# 34!€ $ $ * 0 ( * $ * \ Z[XY œœžX[Z X[]XX[Âœ¨Â‚&‚<]Z[XY]¡Z¡Â—— Z]

–XZ˜ 6] ™& = # # = ¢ $%%% Z & ' Z + / Z V ] X¡ # ] — Z[[X ¡Â&#x;¡Âž¡¡[ X[]XX[Âœ¨¡[]Âœ\Y\¡Â—] –X Â˜ (] (] 5 # # ¤] )] 9 # E> ™ , E , # , #" " , # ¢ $%%% Z & ' Z + / Z V ] ¡ # ]  XœœX  ¡\ž Â&#x;Z X[]XX[Âœ¨¡[]Â&#x;Â&#x;XY¡] –XY˜ ¤] ] 5 # 5] # ™+ =  9 # # , , ¢] ‚# & ' * + / [ !""@Z 0

* [ !""@ $%%% $ ( V ]  Xœœ—  [X—ž [Z[ X[]XX[Âœ¨6F F ]Xœœ—]—Z—¡XZ] –X—˜ ¤] &] V # +] Q ™¤ " # = #" # ¢] ‚# 0 Z !"#@ $%%% $ Z Z & ' Z + / Z XÂœÂ&#x;— XX\žXZX X[]XX[Âœ¨6F F ]XÂœÂ&#x;—]X[Â&#x;¡ X\] –X\˜ +] F (] U # +] ¤ # # ] (] ( E # # ] & ™U # # ÂŒ # #  E9 # E ÂŒ , # = ¢ $ / * < ( / [ $%%% V ] —Â&#x; # ] Âœ Z[[Âœ  [ Xž [YX X[]XX[Âœ¨ ‚ ]Z[[Âœ]Z[X\Â&#x;Z[] –X¡Â˜ ¤] & # ] + # # (] ] £] E = # ™+ " E # # , # E = = #¢ $%%% Z & ' Z + / Z V ] Âœ # ] X Xœœ  \žYÂ&#x; X[]XX[Âœ¨¡[]ZX[¡Âœ ] –XÂ&#x;˜

] % # (] 5] ™ # " & E = # # # , , ¢] ‚#

0 Z $ Z 8 $ & ' 6 ( / Y"~ = # Xœœ X—œžX\—]

–Xœ˜ (] % +] # # ¤] & # ™ E , #  E9 # #" = E #

# V " #¢ & ' [ $%%% ( V ] ZÂœ # ] X Z[X X\XžX¡X X[]XX[Âœ¨ 6F]Z[XZ]ZZZX X ] –Z[˜ (] ¤] ©] % # %] ™+ # V #E # #" #E E V #¢] ‚# ( [ 344@Z (344@( _ Z 344@ $%%% ?3nd V ] X Z[[— YZYžYZÂ&#x; X[]XX[Âœ¨£Q Q )]Z[[—]X——¡ÂœY\] –ZX˜ ] > # ¤] % # ™5 #" ,E ? # # , # # # E # #" = #  9 # ¢] YX

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

N° 2

2017

I $ D " + N Submitted: 2nd January 2017; accepted: 14th April 2017

Â?Â—Â’ÂŽÂœÂŁÂ”ÂŠČą Â˜Â‹Â’ÂŽÂ›ÂœÂ”ÂŠÇ°Čą ÂŽÂœÂŁÂŽÂ”Čą ˜Â?ÂœÂżÂ?”˜ ÂœÂ”Â’Ç°Čą Š ÂŽĂ™Čą ˜›¢£ÂŠĂ™ÂŠÇ°Čą ’˜Â?›ȹ Š”˜ ÂœÂ”Â’ DOI: 10.14313/JAMRIS_2-2017/15 %&'()*'+ The paper presents the method of determining the global orientation of links of the measuring arm by gauging the angles of the links relative to the vector of the gravitational and magnetic field using inertial sensors. A method of using Kalman filter to average the results is presented as a test on two-links measuring arm equipped with accelerometers and magnetometers placed on each of the links and analysis of measurement results in terms of repeatability. There was demonstrated the ability of creating kinematic chain. Instead of determining the position of the final link on the basis of the measurement of angles in relation to the previous links from the end to the base of the arm, it is possible to define links global orientation by measuring angles of links in reference to the vectors of the gravity field and the magnetic field, in global coordinate system. ,-./0(1&+ Kalman filter, MEMS sensors, measuring arm

23 4'(015*'604 Determination of kinematic chain last point’s location based on location of each link constitutes an issue in which usage of precise encoders (optical or magnetic) is common in measuring angle of joint’s rotation. This solution is expensive, however it provides high accuracy of results obtained. Specified, mutual location of individual links in each direction is not dependent from the attachment position of encoders because they directly indicate angular position of rotary axis of exact degree of freedom. They are not used in solutions as this in case of small size devices. The research on solution to miniaturise measuring arm in limited workspace began with the measurement with usage of inertial sensors [1]. However, all of the inertial measuring systems are exposed to integration errors: small acceleration and velocity measurement errors are being integrated in increasingly large velocity errors which magnify themselves to even more significant position errors [3], [2], [1]. New position is calculated based on previous calculated location, measured acceleration and angular velocity. Those errors cumulate approximately proportional to time, because the primary position constitute an input value. For that reason, the position must be periodically adjusted to another additional type of navigation system. 42

However it is possible to create kinematic chain, which eliminates determining position of the last link, based on measurements of angles related to previous links from the end to the base, Instead, it is possible to determine the global orientation by measuring angles of links in respect to the vectors of the gravity field and the magnetic field, which are in global coordinate system. The paper presents measuring method with a simple two-links measuring arm, equipped with accelerometers and magnetometers placed on each of the links and analysis of measurement results in terms of repeatability. In chapter II, the analysis of kinematic structure and method of using Kalman filter to average the results will be presented, in chapter III research stand and in chapter IV measurements results will be presented and whole will be summed up in chapter V.

73 8-0(. 7323 ,64-;)'6* '(5*'5(Unequivocal definition of position of the link its initial point requires defining its orientation in basic coordinate system which is connected to an immovable base. To describe kinematic structure of measuring arm (Fig. 1) the following parameters has been used: X1,Y1,Z1 – global coordinate system located on link 1, X2,Y2,Z2 – coordinate system attached to link 2, X3,Y3,Z3 – coordinate system attached to link 3. Vector L=[LX,LY,LZ] has been specified by combin #" ? # ‚ # ‚‚‚ # #" #" Ž2 between L vector and X2 ž � V Ž3 between L vector and X3 V ŽZ between L vector and X1Y1 = # ŽY between X1 – axis vector and L vector measured in X1Y1 plane,

Fig. 1 Measuring arm’s kinematic chain – plane X1Z1 and X1Y1

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

 as rotation of the plane defined by initial points of joints around L vector. Links location is determined by L vector defined as:

N° 2

2017

In cylindrical coordinate system L vector: Z¸¸5z; P=atan2(Ly, Lx). If vector of the gravity G equal to:

(1) where: – matrix of X2,Y2,Z2 coordinate system rotation - connected with link 2,

(14)

(2)

and vector of the magnetic field M, with the base of the measuring arm oriented towards Earth’s magnetic pole:

– matrix of X3,Y3,Z3 coordinate system rotation – connected with link 3,

(15)

(3) R¡ – rotation around X axis by  angle

values of gravitational acceleration and magnetic field measured by sensor attached to 2 link equals: (4)

(16) (17)

RŽ ž # # � , ŽZ angle And for 3 link we receive: (5)

(18) (19)

RÂŽŠ ž # # Š Â? , ÂŽY angle (6)

RÂŽZ ž # # Š Â? , ÂŽ2 angle (7)

RÂŽ Âž # # Š Â? , ƒEÂŽ3) angle (8)

Basing on the law of cosines we set: (9) (10) (11) and

Based on equations (1–19) and selected data read by sensors direct dependence of L vector can be received. It can be seen that there is more available data than required to perform calculations and hence this redundancy can be used to increase accuracy of measurement. Despite the fact that the data read by sensors are highly noised, it is possible to use the following measurements as a static measurement because the arm does not perform dynamic movements. However it should be remembered that each sensor has different accuracy described as the noise of measurement covariance matrix and his uniqueness should be included while gauging the real location of arm. The references to Kalman filtration can be seen in bibliography [5–8]. It includes all of foregoing factors and sets the estimate of the state vector having the smallest covariance error.

7373 D&)P- 0< '8- "O'-41-1 ,)9;)4 A69'-( '0 J-()P- '8- ')'- -*'0( While constructing Kalman filter, the state vector X, the output vector Z, the control vector U, discrete equation of state, the output equation, covariance matrix Q the noise of the process wQ, covariance matrix R the noise of measurement wR, the initial state X0 and error covariance matrix of its estimation P0 should be defined as follows: The state vector X of the system:

(12) (13)

(20) The output vector Z of the system: Articles

43

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

N° 2

2017

(28) (29) (21)

(30) (31) where:

The control vector of the system is assumed as #" ¡ #"

(22) determined on the base of gyroscope sensors attached to both links. From the methodological point of view more accurate would be to join this velocity to vector of state and values of measurements to vector of measurement. However, proposed solution is less demanding for computing reasons and due to consideration of gyroscope’s measurement uncertainty, the noise of the process covariance matrix gives almost the same results. For variables defined this way, the equation of state is defined as: (23)

(32)

Fk-1 is the state transition model which is applied to the previous state x šX, ;

(33)

Hk is the observation model which maps the true state space into the observed space. For example (34):

,

and the measurement equation is defined as: (24) where relation h(X) combining direction of the gravity and the magnetic field read by sensors with variable of the state is non-linear and defined on the base of (1–19) equations. Due to the fact that measurement is assumed to be static where L vector remains constant and where change of  angle is unknown and rotated manually, the noise covariance matrices Q4x4 was assumed as zeros except (4,4) element, where substituted cubed standard deviation of radial velocity measurement. The noise of the measurement covariance matrix R12x12 has been defined as diagonal matrix with cubed standard deviation of individual sensors on the main diagonal of the matrix. In this system we use equations of Kalman filter in stage of prediction – the prediction of the state at the timestep k based on the state estimate and control from previous timestep: (25) (26) in stage of filtering – the updating of the state estimator and error covariance matrix of the state based on the measurement inputs at the timestep: (27) 44

Articles

(34)

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

N° 2

2017

73>3 -&-)(*8 To conduct the tests, the research stand (Fig. 2) mechanism has been assembled using four links: (1, 2, 3, 4) connected with two joints (no. I, III) of 3DoF and joint (no. II) of 1DoF. Individual links have been manufactured as 3D prints of VeroClear material. On each link, the sensor AltIMU 10 (5) has been placed, # # #" E Â? ƒ #" ÂşZ¸" resolution 0.06 mg/LSB), three-axis magnetometer (scale range Âą2 gauss, resolution 0.06 mgauss/LSB) and three-axis gyroscope (scale range Âą245 dps, resolution 0.004 dps/LSB) [9]. There can be seen that discussed measuring arm with fixed first and last joint has an additional degree of freedom: rotation of arm around axis between both spherical joints. During tests, two methodologies were used: measuring arm was fixed or was forced to perform slow oscillations around axis between both spherical joints. From many conducted tests of the arm, in this paper the following ones were described: 1. Measurement of base position „0â€?: 5 measure # # # # ¡ #gle.

b)

a)

Fig. 2. Measuring arm model – a) CAD model and, b) 3D printed model

2. Measurement of displacement in X direction, by 2.0 mm in range to 10 mm from position „0â€?: 5 measurements with and without oscillations # ¡ #" ] 3. Measurement of displacement in Z direction, by 3.2 mm in range to 16.0 mm from position „0â€?: 5 measurements with and without oscillations # ¡ #" ]

Table1. The base position measurement’s accuracy

with osc.

without osc.

The base position „0�

Ref. meas. [mm]

Middle meas. [mm]

Âź [mm]

½ [%]

Âż [mm]

|L| [mm]

61.82

61.44

Âą0.57

0.9

0.23

R [mm]

57.06

56.54

Âą0.79

1.4

0.12

P [rad]

1.58

1.19

Âą0.42

26.7

0.06

Z [mm]

-23.79

-24,04

Âą0.56

2.4

0.12

|L| [mm]

61.82

61.61

Âą0.28

0.4

0.23

R[mm]

57.06

56.76

Âą0.40

2.7

0.13

P [rad]

1.58

1.12

Âą0.48

28

0.01

Z [mm]

-23.79

-23.97

Âą0.35

2

0.16

Table 2. Accuracy of displacement’s measurement along Z axis

with osc.

without osc.

Displacem. along Z axis [mm]

Ref. meas. [mm]

Middle meas. [mm]

Âź [mm]

½ [%]

Âż [mm]

-3.11

-3.11

-2.88

Âą0.36

2.39

0.08

-6.24

-6.24

-5.95

Âą0.33

2.18

0.03

-9.34

-9.34

-9.28

Âą0.10

0.67

0.03

-12.51

-12.51

-12.18

Âą0.41

2.77

0.08

-15.65

-15.65

-15.70

Âą0.08

0.53

0.02

-3.11

-3.11

-3.07

Âą0.08

0.6

0.04

-6.24

-6.24

-6.14

Âą0.25

1.7

0.14

-9.34

-9.34

-9.28

Âą0.14

0.9

0.08

-12.51

-12.51

-12.38

Âą0.24

1.6

0.07

-15.65

-15.65

-15.71

Âą0.12

0.8

0.06 Articles

45

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

Before attempting to accomplish measurements, the calibration of sensors has been performed to define indication values of individual vector’s components in respect to the gravity field and the magnetic field of the Earth. Data from sensors have been read for 20 s with sampling frequency equal to 100 Hz, which allow to receive 2000 samples. Reference measurement has been performed with the usage of digital caliper with accuracy of 0.01 mm. Definition of length of links (l2) and (l3) has been based on the CAD model.

Using Matlab software, functions, describing each data presented in section 2, have been created. During tests, huge variety of results connected with arm rotation around vertical axis has been noticed. According to that fact, the results were presented in cylindrical coordinate system as presented:

:3 5;;)(.

It can be seen that value of error is slightly smaller for the measurements with oscillations than for the case without oscillations, however the differences are not significant. Due to the high value of absolute error of measurement P #" ƒŸº[ YZ „] ) #" ments has been focused on determining accuracy of measuring Z and R parameters. Results of measurements described in Table 2 show increased accuracy of measurements in model with applied oscillations

#" ‘ # # ¡] + there can be seen increase of accuracy of displacement measurement in relation to displacement mea-

Simple research stand has been set as basic twolink measuring arm. Global orientation of its links has been defined by measuring angles of joints in relation to vectors of the gravitational and magnetic field in global coordinate system. The paper presents the method of measurement with usage of the measuring arm equipped with accelerometers and magnetometers placed on each of the measuring arm links. Also there is presented analysis of system’s kinematics and method of using Kalman filter to average the results. Analysis of measurements, in terms of repeatability, shows measurement’s accuracy on level of ¹0.5 mm along Z and R direction was described. The results obtained from the magnetometer are strongly affected by external magnetic fields. The optained accuracy in the radial and vertical direction

Table 3. Accuracy of displacement’s measurement along R axis

Without osc. with osc. 46

Articles

2017

surements described in Table 1. Similar results presented in table 3 were achieved for change of location in r direction. During the measurements, magnetic sensor’s measurement has the biggest influence on changes of = # # Ž #] ) � = # , rupted by ferromagnetic materials or devices containing magnets like mobile phone. On the other hand, accuracy of position for measurements in the radial and vertical direction does not exceed ¹ 0.5 mm. Relative error of displacement measurements has been appointed as relation between maximal absolute error and range of displacement which is 10 mm. Very small value of standard deviation of measurements is worth to be noticed. It may indicate that prevailing cause of error is constituted by reference measurement, yet only future research can confirm this hypothesis.

73>3 -&59'&

Displacem. along R axis [mm]

N° 2

Ref. meas. [mm]

Middle meas. [mm]

Âź [mm]

½ [%]

Âż [mm]

-2

-1.99

-1.77

Âą0.25

2.5

0.01

-4

-3.90

-3.45

Âą0.45

4.5

0.02

-6

-5.72

-5.75

Âą0.04

0.4

0.01

-8

-7.45

-7.35

Âą0.10

1.0

0.02

-10

-9.07

-9.22

Âą0.16

1.6

0.01

-2

-1.99

-1.76

0.32

3.2

0.07

-4

-3.90

-3.44

Âą0.47

4.7

0.02

-6

-5.72

-5.63

Âą0.14

1.4

0.03

-8

-7.45

-7.21

Âą0.24

2.4

0.01

-10

-9.07

-9.13

Âą0.08

0.8

0.02

Journal of Automation, Mobile Robotics & Intelligent Systems

was the result of a high repeatability and accuracy of accelerometers. These conclusions were an impulse to seek another, limited cost, method of measurement, which would allow to get a high accuracy not only in two but in all six degrees of freedom. It is possible to determine the joint angle based on data from the accelerometer placed on two links combined with rotary joint. It’s rotation axis should always be inclined from the vertical direction. For a plurality of such links connected in series of the pivot joints and with triaxial accelerometer placed on each of links, we can get a measurement of the bending angle of each of the joints. Of course, as in the previous case the rotation axis of joints should be inclined from the vertical direction.

Fig. 3. 6D arm a) CAD model; b) 3D printed model

In such a case it is possible to create kinematic chain with six degrees of freedom, measuring both the orientation and the position of the end effector in a coordinate system associated with the initial link. Fig. 3 presents a CAD model and 3D printing views of such arm. The arm consists of seven links connected with six rotary joints. In following research measuring algorithm will be developed along with the studies on the accuracy of arm with six degrees of freedom.

D EF Agnieszka Kobierska – Lodz University of Technology, Institute of Machine Tools and Production Engineering, 1/15 B. Stefanowskiego Street, 90-924 Lodz, Poland. E-mail: agnieszka.kobierska@p.lodz.pl. 1 $ = A = ) = Y – Lodz University of Technology, Institute of Machine Tools and Production Engineering, 1/15 B. Stefanowskiego Street, 90-924 Lodz, leszek.podsedkowski@p.lodz.pl

VOLUME 11,

N° 2

2017

"A" "G@" [1] P. Cheng, B. Oelmann, “Joint-Angle Measurement Using Accelerometers and Gyroscopes—A Surveyâ€?, IEEE Transactions On Instrumentation And Measurement vol. 59, no. 2, 2010, 404–414. DOI: 10.1109/TIM.2009.2024367. [2] M. El-Gohary, J. McNames, “Human Joint Angle Estimation with Inertial Sensors and Validation with A Robot Armâ€?, IEEE Transactions On Biomedical Engineering, vol. 62, no. 7, 2015, 1759–1767. DOI: 10.1109/TBME.2015.2403368. [3] M. Quigley et al., “Low-cost Accelerometers for Robotic Manipulator Perceptionâ€?. In: 2010 IEEE/ RSJ International Conference on Intelligent Robots and Systems, October 18–22, 2010, 6168–6174. DOI: 10.1109/IROS.2010.5649804. [4] Y. Wang, B. Pitzer, “Manipulator State Estimation with Low Cost Accelerometers and Gyroscopesâ€?. In: ed. Philip Roan, Nikhil Deshpande, 2012 IEEE/ RSJ International Conference on Intelligent Robots and Systems, October 7–12, 2012, 4822–4827. DOI: 10.1109/IROS.2012.6385893. [5] M. Xu, N. Fan, Z. Wang, “Study on Extended Kalman Filtering for Attitude Estimation of Micro Flight Vehicleâ€?. In: Third International Conference on Measuring Technology and Mechatronics Automation, vol. 3, 2011, 457–460. DOI: 10.1109/ICMTMA.2011.685. [6] H. Zhao , Z. Wang, “Motion Measurement Using Inertial Sensors, Ultrasonic Sensors, and Magnetometers With Extended Kalman Filter for Data Fusionâ€?, IEEE Sensors Journal, vol. 12, no. 5, 2012, 943–953. DOI: 10.1109/JSEN.2011.2166066. [7] ] ) * 5] & ' +] , ™9 fusion using Fuzzy Logic techniques supported , ÂŒ > " #" ƒ)5 >„¢ International Journal of Fuzzy Systems, vol. 18, no. 1, 2016, 72-80. DOI: 10.1007/s40815-015-0095-3. [8] 5] & ' ¤] & ' Ă‚ =tive optimal Finite Impulse Response smoothing for off - line estimation of the states of the system described by discrete state space model using Extended Output Vectorâ€?, Methods and Models in Automation and Robotics MMAR 2015, 1179–1184. DOI: 10.1109/MMAR.2015.7283984. [9] &] $ ™U #" ÂœE Â? % # & surgical Robot Master Angular Position Determinationâ€?, Solid State Phenomena, vol. 199, 2013, 356–361.

) [ $ [ – Lodz University of Technology, Institute of Electronics , 211/215 Wolczanska Street, 90-924 Lodz, pawel.poryzala@p.lodz.pl = ) = – Lodz University of Technology, Institute of Machine Tools and Production Engineering, 1/15 B. Stefanowskiego Street, 90-924 Lodz, piotr.rakowski@edu.p.lodz.pl *Corresponding author Articles

47

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

% % + ++ % = + > ? ! !

Ù ¿ ǰ £ !" "#$ %&'(' )# @ %&'()*'+ $ 4! " $ $ )56 ! 1 " ! )56 " 1 $ 4! ! 7 89 :9 2 "

" $ ! )56

" " " $ ! " " ! $ " "

! ) &6 ; $ ! < * ! 4 = $ < ) 9 ! ! 4! ! ,-./0(1&+ )> 9 = )56 "

23 0HJ)H04 + # # V " # , , E = V = #" ] QV = E = ~ # # , , # , #E V # # = # #E V # ] #E # , , , V # ] # E ## = # , #" # # #" ] V E " ## ] F# = = ## % # ¤ # # #" = # ] # == # = # ## #" # #E # , # # = E # # # = # ## ] =E = # # ,? , V , ## #" = # , " , ] # # V " # , # #E # ## # #E #] +# = # #" # , , == E # Z[X[ # # «, \[ & % # = # ] > # " V , V ~ " E # # # , , " = ] # # == # # , ,E Y

? #" # "# # # " = E #" # # "# # 9 == #" X X ] # # # == #

, # V " # == #" # , E # XX XZ Z[ Z Y ] F# "# = = E QQQ¨6%( 6F% # # # Z[XY = # 6 , < V " # # ¢] ) E #" # # = # # E # # = , E ] , #" # V # #

«, F# X ZZ +%U% « # # 9( # Q9 # 6 % # Z ] + = # = E , # V " # = V #" , # # # V = E 9 # # # ] > # #" # = # # V " # X Z # #E # #" # #" = = , ## = ## ] ¤ V , , # # , = # = # , # # == ## #

, # = = , # V " #] + = # # , # "# V = , = # , "" # " # , ## ]"] # \[ # V # = # = # V \ [[ = # = E # ¤ U6 E[Y ## ] E " # = #" E = # ] ¤ V "E # # #

, = , # # ] # = = " E # #" # # ] + # == # = # Z9 = # = ? #

V # # =] = = = #

V E = # 6 9 = ? & / < * X ( X ]

# = E = # = # , #

, # V " #] = E # # 6F% 6 , F= #" % X E # ~ # # , E # ] + V # V = #

V , 6F% = " # E #" ~ # 6 9 , = ] # = #"

#" ~ # = , # #V # # ] V # = " E

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

# # # # # # #" = # #" # # , #" # ] # = V , " # # , ] # # " # = " # , , == # ]

V

"# = V E = #" = " = V #" # E # , # , " # # # # # # #" ] " #

# = " E # , " # V # # " V , ]

= V # # # E = " # #

]

V , = ¯# V¯

= " X[ # # , # " 6F% # V " # ] = " , # #E = # %9 # ] = = # , # == = " = #" , = V # # " E V , ] = # , E # = = # " E ] = = # # E V = # = " "# # = #" # # 6F% = " = = ] & #" # V # " = " # # # #" # #

# # V " ] = = " # # Z E , = # = # =

# = ¯# V¯

= " # = # , = V # # E ] = = # # ]

73 009 I-&*(6=H04 7323 4'(015*H04

MFCLD< ~~

E◦

~

# #

] = " * 8 8 = # # V # # # # # = " # ] = # # , " # = # # = " ] ) "] X = # ## # , # = # = " ## #" # # " E # # # ] # # " #" , # V , # E # # # # ] 7373 I-='8 ;)P- '0 #9)4)( ;)P- @04J-('-( # # V " # = " # 6F% ~ #= V #] " = # = = , # , ] V , # 6F% = " = V #" #V # * 8( / [ # 8 * V # #" ~ # 6 9 = ? ] # = = #V = # , = " # = # # " # ] # # " # = # # , # #

# , E ] # = , # # # E # #E # = #] # = # , = " = # # = # #

= # # # = #"

= # V " #] # V # V = E # = # # = = ] # E , , V # # = " " # V # = " # #E #" # # = # ] 9 #E V # = # # X , # # # = " 6F% Z9 # | 6 " ] E " # # # # # V ## #" #" ] E = # , # = # # = " , = # * 8( / # # #" # # # # , # " 6F% Z9 # V " # 5 % # ] #= = #

= " n # m # # " # E # (cx , cy ) ) "] Z ] + = #" # "

xi m & ' "' > Y

W = #

, , #

# V " # , = ] = " #V = = " = # " = V #" # E # = # # V " # ] #

* #" # " E V , # # #" E

n

yi

(cx , cy ) (i, j)

,

S & ' ' O

" V # = # = # = ? # ,? = #E Y

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

& ' ' Y Y # ,? zi # # xi # = # S] #V # E , # = # # E # # V # #

# , ] , #"

= " # V # " = zi = min(z0,i , z1,i , . . . , zn,i ),

X

zj,i # # i # # j = " ] # # E # # # # # , = # # , # V " #" #

#" = # ] ) V # # , == # ~ # , #" # ] # V # = # = # 1 Z xi = (i − cx ) zi , fx fx #" # = E # ] ) # # V E = # # # #" i # " # , = # # ) "] ] = # E # = # ~ # # # #" # # , ] #" , # V E = # δ = # # # , = " # , (jmin ) #"

jmin − cy − 12 . δ=θ n−1 #" # # #" # α # , " V # , d = l #(

#( π2 − α − δ) π − α − δ) = z . 2 #( π2 − δ)

Y

# #" , # # == # # = , # = #

" # # , ] = # # #" # # , V # " V # # = " ]

# # " " # V = = # ] 9 # # V " # = # , # = " ] % # = [

& ' ' Y

# , # # V # ~ #" = #" # = # # V ZX ¤ " # # ZY #" = # = # # V Z\

6+<%+ Z ] > # # # # # E " # # , # V E == # ] ¤ V # = , # # " # = # == #" # = " ] U V , = E = = # , Z # #

= # Z¡ ] # \ = = = # = # # # = # " E , #" " # ] " # V = # # = " # # # V εg " ] V = # # " # ] V " # # = " # = # # "# E # # # # # " # ] + = # , " V # " V " ] = # ~ # # # # α # " h = # # , V " # ] ) "] Y = # " # = # " # V δ = θ/2] E ? = # # # π2 − δ − εg . Pg = (x, z) | z ≥ h π2 − δ − α 9 # #" #" # " # # # V " # E # εg ] 73:3 I-'-*'0( 0< G-P)HJ- F%&')*9-& # # " V , # # " # ) "] ] = # = " = ,

# = 6F% # V " #] ~ = = = # #E # , " = #

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

MFCLD< ~~•

= # #" #" # V # " # ] # , , # E #" = " = # E # " # = # Â? #

Îľg ] # # " V , = # " E V # , # Ď€2 − δ + Îľg . ƒ\„ Pc = (x, z) | z ≼ h Ď€2 − δ − Îą 9 V , # # = # = Â? " = " V # ~ , bI,J bs ] ) , # , = # nI,J , E #" #" # " V , nI,J = Ăƒ{Pc |i, j ∈ bI,J }.

ÂƒÂĄÂ„

‚ # " V # , # , = # Â? V nth # = # # , # " V , ] ¤ # ÂŒ #

# " V , = # Pobs = {bI,J | nI,J ≼ nth } .

ƒÂ&#x;„

73>3 -4&0( "9-J)H04 )41 69' "&H;)'0( & # 5 , ƒ& 5„ –ZÂĄÂ˜ = V

# = # V # # #E " ] ¤ V

# = # # , ~ = # Œ

# " ] ‚# # # # # " # = #

] ‚ = = E #] = = # #" Îą # V # h V " # ] ‚ # = # #" = ÂŒ Â? ] 6+<%+ ƒ6+< %+ = # # „ " –œ XY˜] " = " # # , E

# = # #V #E # –¥   ˜] " # = # = # #

= # ] # # E = # # , #" " # , # = " # ÂŒ #" , E # # # " = , E , = # , #" " # ] " V # #" = , # # #" V # [hmin , hmax ] # = , #E " #" [Îąmin , Îąmax ]] # V E # # # ]

& ' #' > Y Y

Eâ—Ś

‡~„

ÂŒ # Z]Z] = # # 6+<%+ " " # = # = E " V # , # Ď€ − δ Ď€ 2 ƒœ„ rmin = hmin 2 − δ − Îąmax # Ď€2 − δ = hmax Ď€2 − δ − Îąmin

ƒX[„

P = {(x, y, z) | z ∈ [rmin , rmax ]}

ƒXX„

rmax

# = # # = , # zi,j V

(i, j) = # = " –Xœ˜ 1 1 (xi,j , yi,j ) = (i − cx ) zi,j , (j − cy ) zi,j . fx fy ƒXZ„ " # = 6+<%+ " " # = # ~ E # n â—Ś m Îą = , ƒX Â„ | n| ¡ |m| h= √

A2

|D| , + B2 + C 2

ƒXY„

n = [A, B, C] m = [A, B, 0]] # = # ~ # #" # " #E = # # ] #" = E V # , ,

= V # = # # #" # " # V ]

:3 -(6K*)H04 6F% # # # = ÂŻ# VÂŻ

= " , = 6 E 9 = ? ] " = ? – XÂĄ ZÂ&#x;˜

, , Â? # E #] + , = # "# ~ = # ] E = = # = , = E ## # = , = ~ =E = # # # V " # ] + = # , # E = V # # #" # # ] :323 -4&0( #9)*-;-4' 9 #" # # = # # # , # # # # # ] F# # #" # # == # = V

#E # # #" , # , V # Œ V ] F,? # # V , # ] ) "] \ # � = #" # # # = V

#" ] ‚# " # E # dm # , Ď€ θ − âˆ’Îą , ƒX—„ dm = (h − h1 ) # 2 2 —X

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

30o

MFCLD< ~~

E◦

~

0,5 m

1,4 m

# # # = # # = E # # Y Z ]

1,11m 1,27m & ' ^' S x Y Y

obstacle

:373 #(0*-&&64P 6;- @0;=)(6&04 = V # = " = # = #" * 8( / = " ] = " #E V = " Z9 ## ] E , X # = # # \[ # # = " \Y[×Y [ = ] = E # = , # = " = # = # t1 # = #" # # t2 ] # ( = " = #" #" E , # " # V = # # # , # ] E = # ~ == $ (@@44 # X\ 6+ ] ? ' "' Y

S

=

# # & ' q' x h1 # " # E # # 0 ≤ h1 ≤ h ]

#" # # # , # , " # #] # = # # # E # == # , V

V # ] = = # # , , = , ] > # # # # # # =

,E ? # = = ) "] ¡ ] " # ,? # " V # # θ θ − α l1 # − α , X\ hmax = h + sgn 2 2 l1 # ,? V # = #" " # = # ] # , # #" E " , , "" " , ] ) , = , = E # # # # # , #" E # " # # ] ¤ V , # E # , #] + # # # , # #"

#" = " # , # V # V = # # #E # # # ] % Z

= " #

'% X Z X

3

t1 \ ]

t2 \ ]

#

#

]X[ ]¡¡ Z]\Y Z] \

[] []¡Z [] \ []

<¨+

X ] X

<¨+

F# # = " # V # # 3 ' Q ] # '

3 V # , " 3 Z , # V # 6F% ] #V # = " X Z[×X[ [ , , , # = " " # &U # &U = # "# # = " #V # ] :3:3 -&'& /6'8 0%69- #9)V0(;& = " , = V = # 6 9 = ? , = # 6 9 = = = ) "] X ] = ~ == # \[ = # # ¤ U6 E[Y5« ## ] 6 , # = #" # 6F% # "

# U, # XY][Y 5 %] = = # # " # # V " # = = # # X\ X¡ ] # # , # V V " # = " ] E , # = " # # ) "] ] F# # , V " # V # E ,

# # , # = #

, ] + # , #" " # V # #] , # E

# , ## ] # = V # ( , #V , #E # # # , = ] ) "] X[

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

& ' z' ? Z

/ = W

& ' {' Y ~ $ / C % H _ |J H _ J ' C

' & %

# # V " # E ] + = # , = E , V ## = # ] # # = # # # = #E # = = #" , ] > = # = ## = # , , , E ) "] X[ ] , E " = # E

MFCLD< ~~

E◦

~

& ' " ' @ Y $ / C % Y W

#" , = , V , # V " # # E , V # ] % = # V = E # V # " E # # V εg ] = E # # ) " ] XX # XZ] # # = , V " Y[ X X ] # = Y = #" V # # #" (0.45m, 15o ) (0.8m, 40o ) (1.2m, 50o ) (1.2m, 60o )] V " # E εg [ Z[ # = ] # , # # εg V # #" , " # , # ~ # == = #] , , V " #" , V , ] & , V = #" " V εg # δ # " # ,?

] = == # ~ # εg V = = E # # , # = "

, , V ] ) = # # " V

, ] ) "] X = # # " # # #" ] " , "# # " V , # # = " ] ,

# , # =] # # # # # = E " == 0.5 # E 15◦ # # ]

>3 @04*95&604& = = = #

V =

# = , " = # # # E # V " # , = ] ( = " = #E 6F% # V " # = " ~

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

& ' ""' Y εg $ |

h = 0.45 m% α = 15o %

h = 0.8 m% α = 40o

| 6 " ] = # = " = # # == # * 8 ( / ] # # # , E #" # #" # V , # #" # # # " # = E #"] + # = " "# # = #" \ # \[ ] * # # # , E " # V = # , = = ## #"] V # , # V E " # # # #"

# #

]

* 8 8 = == # ] = V #

= # # * ] = , %9

= # # # V , 6F% E # ]

V # 6 9 = E ? = # = ? ] E , , #" # # # # # #" # # # ] = # # # # " V , E ] # # , , , # # , # # , = = #E # #" V #" # Y

MFCLD< ~~

E◦

~

& ' " ' Y εg $ |

h = 1.2 m% α = 50o %

h = 1.2 m% α = 60o

& ' " ' 0~ Y $ / C % Y W

% W ] V , # # # # # E # *

# , E ] # , = V , # # = #" # E ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

@,GFB "IN" "G == , Q = # E # )&¡ E\X[ [Z = ? 6 9 6 E 9 "# # # E # # , & #

% # # ¤ " Q #

D EF ( % [ ) A 9 = # , # # 6 E , ) Q # > U# V # " ] > , Ä > = #± " Z¡ [E ¡[ > & # E ] " §= ] ]= ] C $ C = =∗ 9 = # , # # 6 , ) Q # > U# V # " ] > , Ä > = #± " Z¡ [E ¡[ > & # E ? # ]? , §= ] ]= ] ∗

= # #"

"A" "G@" X 6 9 QU & ? ¢] ?iiT,ffrrrX`2K2/B@T`QD2+iX2m] Z ©] (] 5 ¤] Å # ] 5 # ] % # )] 5 # ] ] # +# +£ # # == #" , # 6 # ¢] # &] ] &Q< 9] )] 5 < ] $ < +

/8 * 34!? ? #" &6 # Z[X\ [ ] ] + # (] ( , ] 9 '" ] #± E ] % #, " ] # ] " 9] % ' ± # E% #± ] ( # >] E +] > #± # , E , # # 9 "# #E = # , = # ¢] # 34!? "th $

> / 6 / $ `>6${ Z[X\ ¡\ Z] 9F X[]XX[ ¨¤% ]Z[X\]¡ Z \XZ] Y ] " ] + 6 + & 8 < 8

&=9(X 6 & * 0 ' + '

&=9(X X Z¡ YY] %= #" # # # & , #" Z[XY] (] ] £ 9 = , # E

, , # # # V " #¢] #

0

* $%%% $ & ' * + / Z[XZ X\ ¡ X¡[Z] \ ] ## +] E # %] ¤] +] + # = # #" 9 5 9+6 #

#V # # ¢ 7 + / [ < ' & ' * $ 6 / V ] ¡ # ] Z Z[X ZY Z ] ¡ 9] ## (] Q , " ] 5 #" ## +] < ª E 9 ¤ " # = # # # = # + V # # E "#¢ ~X & Z Z[XX Z X Z X ] 9F

X[]X[[¡¨ 96 ][Z Z[XX ] &] V, ] )

] 6F% = " = # E ¯ ¯ #¢] ?iiT,ffrBFBX`QbXQ`;f TQBMi+HQm/niQnH b2`b+ M]

MFCLD< ~~

E◦

~

%] ] >] © & # QV E # 6+<%+ ) ¢] # 0

*

9 < Z[[ X]X X]XZ] 9F X[] ZYY¨ ]Z ] X] X[ ] 9 '" ] < V " #

¢] ?iiT,ffrBFBX`QbXQ`;f/2Ti?nM pniQQHb] XX )] Q# (] ¤ (] % 9] >] E " E == #" # ",E ¢ $%%% & ' V ] [ # ] X Z[XY X¡¡ X ¡] 9F X[]XX[ ¨ 6F]Z[X ]ZZ¡ YXZ] XZ )] Q# (] ¤ <] Q#" (] % 9] E >] " +# V # 6 E 9 %5+ ¢] # & ' * + / `$ &+{[ 34!3 $%%% $ Z[XZ X\ X X\ \] X &] ) # ] 9] 6 " 6] E # ] ¤ 6] % " # VZ , , # V " # QV # # E #"¢] # 34!@ $ +* * & ' `$ +&{ Z[X Y] 9F

X[]XX[ ¨ +6]Z[X ]¡Z XY ] XY ] ) 6] 6 # = # #E + = " #" == E # " # # E " = ¢ // Z + < V ] ZY # ] \ X X X ] 9F X[]XXY ¨ \\ ] \ Z] X (] ¤ # 5] % 9] « (] % # Q# # = V # # #E + V ¢ $%%% ' ( V ] Y # ] Z[X X X X Y] 9F

X[]XX[ ¨ © ]Z[X ]ZZ\ ¡ ] X\ (] ( , ] 9 '" +] # 9 V E = # , = = # , ¢] # 0 3!st $ < * * < * + / * & ' `<<+&{ Z[X\ XX ¡ XXYZ] 9F

X[]XX[ ¨ +6]Z[X\]¡ ¡ Z ] X¡ (] ( , ] 9 '" ] % #± # # = = # 6 9 , , = E ¢] # 0 34th $ < * * < * + / * & ' `<<+&{ Z[X ] X +] , +] # 6] 6 ] /8 ( * < | &=9(X 6 ( = ¢ 9 9 = # £ # E # # 5 # ¤ ¢] %= #E " Z[XY] X ] # %] +] % %] % +] +] +, 5] # #V # 9 = Z9 = #

%5+ ¢] # $%%% "th $ 6 0 ( * +88 ` 60+{ Z[X ZY¡ Z X] Z[ ] # %] +] % +] E & # # # # Z9 # E # # == #" %5+ # ~ ¢ 6 ( V ] XY # ] XZ Z[XY Z \ Z ¡] 9F

X[] [¨ XYXZZ \ ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

ZX 9] Æ Æ )] ] # = # # #" # 6 E9 # ¢] # 5] 6] ± ] #, Q] ] $ / 6 * 6 ( / 34! Z[XY] 9F X[]X[[¡¨ ¡ E E X E[ Y\ E\¯ ] ZZ Q] 5 ¤] ]E+] ] 5 # &] # ) Q = # # VZ % # 6 #" #"¢ $60&6 ( $ + 0 // [ & / 6 * 68 $ / 6 Z[X X[[] 9F X[] X Y¨ = V E«5E E>YE EZ[X ] Z &] ,' 9] % , # #" , ¢ 7 + / [ < ( ' & ' * $ 6 / V ] ¡ # ] Z Z[X Y ] ZY ] F %] " ] # , ¤] # & # " # # " = # # # == # ¢] # 0 Z $%%% $ Z Z & ' * + / V ] Z Z[[X ZXZ[ ZXZ ] 9F

X[]XX[ ¨6F F ]Z[[X] Z Z[] Z +] F V %] #" ] > ª # ] 9 E # U #" # # V " # #E , , ¢] # 0

* 3 th $/ * /8 \ * Z[XZ [ XY] 9F

X[]XXY ¨ZYZ \]ZYZ Z] Z\ %] FÇ (] %] ## +] ¤ # #" ] ## E ) 9 = # , #"

+ = # = # " # # == # ¢] # !!th $%%%(&+6 $ > ( / * & ' Z[XX ] 9F X[]XX[ ¨¤ E # ]Z[XX]\X[[ \] Z¡ & 5] & # 5 , ¢] ?iiT,ffTQBMi+HQm/bXQ`;] Z +] & ] ] % #± 9] % ' ± # E % #± >] +] > #± ] " ] +] +V # Q] 6 5] V #

+] ) ] + # (] ( , ] ( # "# # # #¢] # \ Q < * << &3! Z[XY] Z <] 6 , (] #" ] ] % ~ ] E # ¤] )] 5 +# , # #E # # # 9 = # # # # # ¢] # & Z[XZ] 9F

X[]X[\X¨ ¡ [¡ YYXZ Z ][ \] [ ] 6 ] 6F% = " = E " ¯ ¯ #¢] ?iiT,ffrBFBX`QbXQ`;f /2Ti?BK ;2niQnH b2`b+ M] X 6F%] 6 , F= #" % ¢] ?iiT,ffrrrX`QbXQ`;] Z ] > ] + # Z¢] ?iiTb,ff;Bi?m#X +QKf+Q/2@B BfB BnFBM2+ik Z[XY Z[X ] ] ©] © #" >] ) ª # & # # # = # ¢] # X 8 / 0 // [ 9 Z[X[] \

MFCLD< ~~

E◦

~

Y ] #" # % # # Q ¢ $%%% < < * Z[XZ Y X[] %] " )] & # # +] 9 ] ] <" # %] + E , + ## = , , # # # , == # È¢] # & ' * 6 % / `& 6%{[ 34!3 $%%% $ ( 6 /8 / Z[XZ XYY XY ] 9F

X[]XX[ ¨6F%Q]Z[XZ]\Y[Z\X ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

B,( %C %5+ + C 9 + !

ǰ Ù ǰ £¢ £¢Ú !" "#$ %&'(' )# ) %&'()*'+ $ " " )> 9 6 ? 3 " 6? * 3 " @ C ! * " D ! * " )> 9 6? ! )> 9 * G0

H H " )> 9 ; " " 6? ! 0 ( !

,-./0(1&+ 6? )> 9 "

23 4'(015*H04 6 # = " # # # #" % # 5 # # == #" = , # V , # # #E # = E # " # = #E , # E , = " # %5+ ] # E = # %5+ " , # # V , # = , , # =E # # " = E = # #

%5+ = , ] # # E 6 E9 6 # 9 = # E %5+ = V V # E , = #" = # Z ] + 6 E9 # # , = E # # , , V # = # = #" = ZY ] ¤ E V # # " # # %5+ E V V # , V # 6 E 9 %5+ # X ] +# # # # %5+ = , E V , 6 E9 ] , # = # # # # # Y ZX ZZ ¡ ] # V # V #V V 6 E9 ~ # ~ , # # #

« # # V # # = E 6 E9 " XY ] = V E # %5+ " # # , # # E

# , # , # " # # #

# V # # # E # #" ] V # = E # # V , , V , # E ] = = , V , 6 E 9 , # #" , , ] # U 6 E9 # ~ # # , # # # & # , ] ¤ V # ~ # # # ,? = E # # = # #" # # # #" # , V %5+ " # = ] " % # XZ # # 6 E9 ~ #" # # # # , , , " # ? , # , "# #" Z9 #

= # , E #" = V #" , XZ , V , E E E %5+ \ Z ] > # = = V #" # = =

V = , , # ¡ X\ Z V # # = %5+ # # E , , ? #] % Y = V V # V %5+ V #" = V ] E+ Z # V V # F6 E%5+ ZZ = #" E # = # #" V # " E, == ] # # 6 E9E, V = # = V # = E, E # == V #" = , #E ] # # " #

, # = , V #" # = E #" # == %5+ # = #E , , #] # = = = # V 6 E9 %5+ E # # #

# E , , # #" # # # = = X¡ ] > # = , E , , 6 , 5 , & # #± U# V # " &U ] F# ~ E #" , # = ,

X # , # #E # # E "" , V #" = # = ] ~ # == E # , # ~ # U 6 E9 #E # , # # # , # # , E = ] ¡

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

%5+ # # = V # Â?= = # ] > , E V E, == # = , , # # =E = # # , # " # #" # , # # = # # # # = # – X˜] = # #E , %5+ &U %5+ –Y˜ F6 E%5+ Z –Z Â˜ <Š¯6 9 –œ˜ # 6 E9 %5+ VZ –XX˜] ÂŒ &U %5+ V = , 6 , 5 , &U = E ÂŒ # V , , ] # #" = # "# ÂŒ # E # == = , %5+ 6 E9 ] %5+ E # # = , 6 E9 V ] 6 E9 V E = # = = , # 6 E9 # # = , # #) 5 " % E == , # # = = # E ) # ] + # = #E ] + ~ # = , E V , ] > # #" E , ~ # V = # # # – Â—˜ , ~ V # # V #V # # = , # 6 E9 " , &U %5+ ] &U %5+ V = # , = # E # # #" # # V # Â?= # ÂŒ # # V = # &U %5+ # E V # # # , , , #E " #" # ] = =

# Z , Œ = # V E # #  "

, # , ~ # V # ~ V E # Y , # # # # — # = = ]

MFCLD< ~~•

Eâ—Ś

‡~„

 E9 = # = #" ~ E # –X[˜] # E # # V ÂŒ , == #" 6+<%+ == ] F# = = # Q # E # " V # ] # " # # # #" # , = ] ÂŒ # # # # # # = = = # ƒ #E „ ] , " 6 E9 ÂŁF E " = # # ) "] X] keypoint detection

descriptors

head-to-tail concatenation of estimates

matching of descriptors

depth data association

RANSAC model test

estimation of the sensor pose

re-estimation of the sensor pose

SIMPLE VISUAL ODOMETRY

& ' "' C

€ /‡C+ *Š W 7373 #D &U %5+ = = = " ] ) 6 " # � = # F6 E –Z\˜ # "

E , # = # # V # V " # # = # ÂŒ # –ZÂ&#x;˜] = # Â?E = # = V , E " V # = # V #" # = E " –XÂ&#x;˜] ‚# = # # – XÂ&#x;˜ # # # %5+ , # # # –Y˜]

a

f1

x1

b x1

x2

x3

f3

f2

x4

x2

f6

f1

f3

f2

x1

f7

x5

x4

x3

c x5

f5

f4

x2

f4

f5

f6

x3

x4

x5

f7

73 .&'-;& )41 8-6( (*86'-*'5(-& 7323 N !I F #V " # E #" V # ~ # ? # ÂŁ F ÂƒÂŁF„ –ZÂĄÂ˜] ‚# = # %5+ ÂŁF = == # , # ] ‚# E #V " # V = # # # ~ V #" ? # = 6 E9 ÂŁF = = # , # = = , # = V , # F= # ÂŁ , –X\˜] 6 E9 ÂŁF E # = # # 6 " # # E = V ] = # = # #  E9 = #

= # #" = " ] < Â? # E # [ , ]T ∈ %Q( ) , # E —Â&#x;

& ' ' = € Q O = H J' & + O@>= H J% C > X O@>= H J' @ € %

%

€ % € ' 0 W = == " = E, %5+ , Z9 %5+ = #" #E # –X ˜ , # = # " = # = # �= #

= ] ‚# &U %5+ , # =  E9 = # E # , = V == #" ] + # # –  ˜ %5+ = , # , # # # # # " = ] " # # # # = V #

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

# % # ƒ% „ = E , = #E , V , E # ] # , V # V ÂŒ = V , 5 # V , ƒ) "] Z „] ¤ V %5+ , # " = " # # = , V == # V , Â?= # = #V # # ]

V # = V " " = #E # = E, %5+ " # ƒ # �= =„ # = # " = = # # , # ƒ) "] Z,„] # # # , = = # E , V E ƒ # # " # ) "] Z,„] # # = #

= ] ] # , # E # # # # = , = # " E , V E ]

= # # # %5+ # , ? E ] # # = # # " = # # ) "] Z, , , #" ] # " = # # &U %5+ = # # # –Y˜ # –\˜ # #E # # # ~ "2 , –Z[˜ = " = # # ] ‚ �= E V " # , , #E #" = # = # = # ƒ) "] Z „] " = # #

# V 5 = # #" # = # = # #" = # ] ij ∈ R3 " E = # # # #" 6 E9 # # , # iE = # jE = # ] # # # # = E # , # # Â? Ί # , E # , #V #" V # Â? = E # –\˜] &U %5+

= E , ~ # #" # #" 6 E9 E = # #" Â?= E # = ] == # + ? # ƒ +„ ÂŒ # V % = , – \˜ # == # E V E %5+ –ZZ˜ # 6 E9E, # # –ZX˜] ¤ V # = # # , # = + " # == # # &U %5+ # &U %5+ Q # # = E

# # # V #E # + E= ? # # " # ] + # , " # # = V , # = E # # &U %5+ ] E = # = # ÂŒ E # ÂŁF = = # E E #" # = ] , , # 5 E # " –X˜ # # 6 " –XÂ&#x;˜] # , #E = = = # E # == # V %5+ –ZZ˜] + # V # V F6 = –Z\˜ # =] F# , E

Eâ—Ś

MFCLD< ~~•

‡~„

6 " #" # E # # , = , #E " = # = # # # V –Y˜] # = V , # E E = #" = E , V "# Œ # # #" # " # #" , V #] SLAM BACK-END keypoint detection

terminate tracking ?

Lucas-Kanade tracking

SLAM FRONT-END

match RANSAC model test

matching of descriptors

2

go optimization engine

NO estimation of the sensor pose

features projected from the map

YES

depth data association

computing constraints adding new features

factor graph

features, desriptors, poses

no match

& ' ' C

€ <ˆ? O@>= W &U %5+ # V # " E = #" ƒ) "]  „ # "

ÂŒ # # = E = ] V # # E # = # #" 6 E9 = #" # #" ƒ = #" ÂŁF„ # #" , # # # = # , E # #" = # = # #" = # " = ] # # # Â?E #" # ÂŒ # V # ]"] # # = # , "2 ] &U %5+ # # # E # #] &U %5+ = #E V , ¤ ,X ] 73:3 F ! 7 F6 E%5+ Z # V # V E %5+ = # # –ZZ˜] F6 E%5+ " # %5+ + #E = # = # = #] # V E # –Z Â˜ # V 6 E9 V = = , # #" = = = # # # V E , ] " = = # = # # F6 E%5+ Z g 2 o , # &U %5+ ] + V V

, V # # E # # , E # ] = # # , E # F6 E%5+ Z # = = # %5+ #" # # #" = # # # = #" = #" # E= ? # ] , # # # E , # #" = , # = # # = = # ]"] # #" E #" , # = # #" 6 E9 # ] V # F6 E%5+ Z

= # = # #" == # E , = "# # == #" , "E E # ~ –ZZ˜] #"

= # , E # # # " = —œ

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

V "

= # E ] # F6 E%5+ Z E F6 # # #

# = = # = #" # # # #

= = ] ¤ E V #" = # # #" # # = # F6 E %5+ Z # V #E # F6 E%5+ V # ,

# #" # # , ] 73>3 @@GWX N I " = # # = E # # # = # E #" = # #

# E # = # V # F6 E ] F# , = # # #E # # = " , = E9 = # ] Q "# # E # = # = , V # E ] = # , # V # =

= V & # & " ] # #" # #] # = V ~ #"

#" = " # # E # # = = # ] , E # # = # # = =E = # # =

# ] # # =] # <©¯6 9 # # E # # = " = = # V , ] = # "# # = V ~ = = , #" #E # ] ¤ V # , # # #V " %5+ # E = , # # E #E E, == # ] 73?3 N !I J7 " = # # XX = # # = # ] % &U %5+ = # # =E , # # = = " = = ] # # E # E, E V # = E # , # V 6 E9 ] E # = # £F 6+<E %+ == == ] E # # = = E V # = ,

= ] E #" V "

= # #" #E # , # # == ] ¤ V

= = == # E, = "# #] = # ? , # , £F ,

= # # = " = #E # = g 2 o , ] # 6 E9 %5+ VZ = , V # ¨ = = ] # = # %U6) ¨ = Z = # F6 \[

MFCLD< ~~

E◦

~

# F6 # #

= ] # = # # # # ¡ ] 73S3 ,64A5 )(P- *)9 " V , # & 5 , Z # = #E E = # # # # E

¡ " # # # = V V E # E # # # ) # # = ] # #E V #

# = # # 6 E9 # # # ] # V E = #V # # , 3 V ] = #" # % "# 9 # ) # # %9) # # " = & &U # ] # = E # = V =

& " ] # V = # , # # # = # , == #" " = ] 9 # # # = # # #" E # V = # # # V # E # == ]

:3 @04*-=' )41 -'801090P. 0< "O=-(6;-4'& # = # E # # , , # = # V , # = ] #V E V , #" E , Z ] # # = # , # # , # = , #E 7`jnHQM;nQ77B+2n? mb2?QH/ ~ # U 6 E9 # ] ~ # = # # ] X E , # , #" ZY \ E V # # V , # , V # #

# #

) "] Y ] = # # ] Z E ~ # TmiFFnR X ¡ , # , , # # V #" # , = , # X ] + E ? # = #" # , =

# # #"

= V , # " , # = # ) "] Y, ] 9 # = # # ] V , # " # E "" #" , + « # &6F 5 V # ] , V #" = # = , = #" ? E " ~ = ) "] Y ] " ? # " = # = , ] + " # # # " = # ? # &U %5+ , # = # # ] # # K2bbQ`knk ~ # X [[ , # #" = " , # # = [][ ¨ ] # ? 6 E9 , , , , # E V # &U # &U Z ] V " E # E

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

a

MFCLD< ~~•

1.5 1 0.5 0 2.5 2 1.5 1 0.5

x [m]

0 -0.5 -1 -1.5 -2

-3

-2.5

-2

-1.5

-0.5

-1

0

0.5

1

1.5

y [m]

b start 2

1

stop

z [m] 1.5 0.5

0 y [m]

0.5

-2

0 x [m]

-0.5

c

-1

start/stop -0.8 -0.6 -0.4 -0.2 x [m] 0

0.2

ƒZ„

-3

z [m] 0.2 -1.0

# # , # + Q V ? E 6 %Q ƒX„ # ] <

, # + Q ? V , "E # = = #" ƒX„ , Œ # #" # E # # # , # E " = # = # #" ? – —˜] % 6&Q iE " V # ,

−1 −1 −1 " 'i 'i+1 . = ('" 'i+1 &6&Q i i )

-1 1

‡~„

" " '" = {'" 1 , '2 , . . . , 'n } # # ' = {'1 , '2 , . . . , 'n } # , n = E ] # 'i # '" i " V # YĂ—Y " # E # = + Q iE

−1 = '" 'i , ƒX„ &+ Q i i

start stop

z [m]

Eâ—Ś

0.4

0.6

0 -0.2 -0.4 -0.6 -0.8 -1 y [m] -1.2 -1.4 -1.6

& ' ' ‡ X $ x H J% H J%

H J # #" ,

# ~ E = V # , , # # ,? ƒ , „] # E # , #

# E

# #

E # # , # " =E = = , # – [˜] , # = #V V #" E # " E #" # # # V # " " EŒ , # , " # ? E ] &U # " # E # # , # " # 6 E9 #E, #

, –Xœ˜] V V ? E #" +, ? E Q ƒ+ Q„ # 6 V & Q ƒ6&Q„ == ] # E V , # # # – —˜ # # # # , # ] + Q V Q # # , # E = # #" = # # " # ? ] + Q E # # = = # ? ] ) E ? 6

# %~ Q ƒ6 %Q„ + Q ] 6&Q V # V # # # , # V 6 E9 # ? ] + #" V ? " #

#" # # # = &6&Q i , # # # 6&Qt(i) # 6&Qθ(i) = V ] 6 %Q 6&Qt 6&Qθ ? = = V = ƒZ„ # ] + " # Â?= # # , ? # E # # , = # # " , = #] U# # = = " # , E, %5+ ## , # = ## #" ,? "# #] = # == #" – Â&#x;˜ = E #" " # # = E %5+ ] # V # ÂŒ # # #V # # == #" = Â? #"  E9 = 6 E9 ~ # ? V ] # = = #" ) ) E # , # – Z˜] ) ) #

ÂŒ # = # V # # F = " –X—˜] ¤ V # # F = V Â? # # # , # # , V ,? E = = #

" # # 6 E9 V ] == # # E # # &U # # , , = # = ] = V # # ) ) # , # #" " # % "# 9 # ) # # –Â&#x;˜ # # = # = # #] #" = # E = # , E # #"]

>3 "O=-(6;-4')9 -&59'& 6 ÂŒ Â?= # = V # , # , # , E , ] ‚# Â?= # %5+ #" = # ƒ&U %5+ F6 E%5+ Z <Š¯6 9 6 E 9 %5+ VZ„ V # ? + Q # X[ ƒ# = # # ,] X„] %5+ #" = E # = ] ] &U %5+ ƒ) "] —,„ # \X

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

F6 ¯%5+ Z ) "] V V " ? E # + Q V #E # # # # # #" ]

a

b

c

d

e

f

MFCLD< ~~

E◦

~

" E , V # = #" "# # ? ] 6 E9 %5+ VZ # , V ? # = # ] ==

= # #" 6 , # , E = , # # ) "] \ ] ¤ E V = #" # 6 E9 %5+ VZ

= # , # # = # # # #E #" ) "] \ ] + Q V # E ? # # , # # ,] X] &U %5+ ) "] \, V + Q

, X[ #" +E E =

= ] ¤ V # F6 E %5+ Z ) "] \ V # , ? # ] , E = == # E,

= E # ? # # " # # ,

= = # , F6 E%5+ Z V # # "

] #) 5% ) "] \ V

# = ? , #" #" #]

a

b

& ' #' 0 Y X 7`jnHQM;nQ77B+2n?Qmb2?QH/$ / C+ * H J% < ? O@>= H J% /C+O@>= H J% ZZ! / C H J% / C+ O@>= H J% x & @O H J " V # " F6 E%5+ Z E = , , # " E F6 ] E, # =

= # <©¯6 9 #" # # E #" ) "] # 6 E9 %5+ VZ = " = = # ) "] " E # , # +E =E = ? # = # = # = # ] #) 5 " % E # #) 5% = + Q E , " ## #" # # , E V # ? #" # #" #" # # # = # E , # ) "] ] ? # E V , = 6 E9 £F ) "] V #"

V " + Q # # 6&Q = , ] # = # #" , V

= #" # %5+ E ] £ = # E #" V 6&Qt ,] X = E " + Q , ) "] \ ] + + Q V , V <©¯6 9 ) "] \ "" = # # # V = E # = V #

= #E, == = # = #" \Z

c

d

e

f

& ' ^' 0 Y X TmiFFnR } $ / C+ * H J% < ? O@>= H J% /C+O@>= H J% ZZ! / C H J% / C+ O@>= H J% x & @O H J = # # , # #V " = E , # # # # , # "" , E # # # V , == " ] , 6 " ~ # # # # # , # V ] # = 6 E9 £F #E # # # #" ? ) "] ¡ ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

? ' "' Z

X W H>?0 /=O0J Y H/<0 /=O0J W 5 # 6 E9 £F &U %5+ F6 E%5+ Z <©¯6 9 6 E9 %5+ VZ

=] X 6&Qt [][Z [][[ [][[Y [][Z [][ [

=] X 6&Qθ ◦ X[] [] []Z X][¡ ]

=] X + Q X][ [][Z [][[ []X[\ [][

=] Z 6&Qt [][XX [][X\ [][[Y [][

+ F6 E%5+ Z # , V ? #" # #" = # # ~ # V , 6 E " == # ) "] ¡ ] F6 E%5+ Z E # " E= ? # E , #" # # # = # %5+ "# #" = ] # E # V # , # E #" ] &U %5+ #" 6 E9 V # , ? ) "] ¡, ] + <©¯6 9 ) "] ¡ # 6 E9 %5+ VZ ) "] ¡ , # + Q E "# # " ] #) 5% # = # 6 " , # V # = # = # ] ¤ V

#) 5% # V # ? E #" , # # ) "] ¡ ]

a

b

c

d

e

f

& ' q' ? X Y K2bbQ`knk } $ / C+ * H J% < ? O@>= H J% /C+O@>= H J% ZZ! / C H J% / C+ O@>= H J% x & @O H J + Q ? # # # ,] X == # , ~ E V # # == #" ] > ) ) # = = 6 E9 " , &U %5+ # ) "] ] + " # #

=] Z 6&Qθ ◦ []Y [] []XZ X]

=] Z + Q []\XZ []X[Y [][X [] Z

=] 6&Qt [] XY [][YX []XX []XX\

=] 6&Qθ ◦ ZZ] ]\\ \] X[] Y

=] + Q X] Y [][\ [] [¡ []ZX[

" " # # = V # XY V , ~ # E #

,? = # = " # = # # ? #] "" # = " &U %5+ ? , == V E

, , # # E = # ]

?3 @04*95&604& = # = # Y V # # 6 E9 %5+ # = #E V E E E #" # #" # # E , , ] = # = , , # # #= , # # = E # #" , = # ] 9 # # #V # = #

V , = # = # " , # , # E # = # = # # F6 E%5+ Z ] & , =

# = # # "

#" = # , #V # # # # « # = # #" ] %5+ E = #" # = #) 5% E # , # , , E #] == "# " #E == # # # = # ¨ , # = = # ] +=E = # # , # # #" = "

" &E, == ## # V =] # = # &U %5+ # V # # = # ] £F , #" # = # # # # # = ]"] #" , == " , V , # ] + V # V #" # #" # = # # #" ] ¤ E V # = &U %5+ " V # ? # F6 E%5+ Z ] ) V = # &U %5+ E = # #"

= , # == # E, = "# # # = V #" # " # = # # # # # # = \ ] # E # \

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

a

b

MFCLD< ~~

E◦

~

D EF

= =∗ & # #± U# V E # " # # # # # Q#" # #" ] & + \[E \ & # #± & # E

#E ] ] § ]= ]= # #]= ] ( % [ / ) = & # #± U# V # " # # # # # Q#" # #" ] & + \[E \ & # #± & # E E ]# §= ]= # #]= ] = $ $ ? = & # #± U# V # E " # # # # # Q#" # E #" ] & + \[E \ & # #± & # E

= ] = # §= ]= # #]= ] ∗

= # #"

"A" "G@" X %] ] ¢5 E # Z[

# + # #" ¢ $ Z 7 /( 8 V ] \ # ] ZZX Z Z[[Y] 9F

X[]X[Z ¨ £ % ][[[[[XXZ[ ]XX¡¡ ] ]

c

Z ¤] +] Q ] 5] £ #

¢%= E = , %U6) ¢ /8 * $/ * ( * V ] XX[ # ] Z[[ Y\ ] 9F

X[]X[X\¨?] V ]Z[[¡][ ][XY] 9] ] < &] % = #± ¢F# = # = E, 6 E9 V # E V " # ¢ # /8 + 34! 9] Z ] 5< % [[Y %= #E " Z[X Y[¡ YZ ] 9F X[]X[[¡¨ ¡ E E X E X\ [ EX¯Z ]

& ' z' & & ~ / C+ W < ? O@>=$ 7`jnHQM;nQ77B+2n? mb2?QH/ H J% TmiFFnR H J% K2bbQ`knk H J = # # # , , # # = == # #" V , 6 E9 # ]

*C40/9-1P-;-4'& == , < # % # # & # # " # Z[X ¨[ ¨ ¨% ¡¨[X ] > +] == , & # #± U# V # " ) Q Q#" E # #" " # 9% ¨[X Y]

G0'-& X ?iiTb,ff;Bi?m#X+QKfG_JSlhfSlhaG Jfi`22f`2H2 b2 Z 9 = , V , ?iiT,ffH`KXTmiXTQxM MX THfTmibH Kf + V ?iiT,ffH`KX+B2XTmiXTQxM MXTHfD K`BbK TXrKp Y + V = ¨¨ ] ]= ]= # #]= ¨? ] V

\Y

Y 9] ] < &] % = #± ¢+ E =E, 6 E9 %5+ , , ¢ # & ' 34!@} +* & ' 5] &] 6

Z ] + % YX %= #" Z[X Y ] 9F X[]X[[¡¨ ¡ E E X EZ¡XY EX¯YX] 9] ] < &] % = #± ¢QV E #" =E, 6 E9 %5+ # # # #" , ¢ # + / [ & ( ' * < / 6] % E Z ] + % YY[ %= #" Z[X\ Y\ Y X] 9F X[]X[[¡¨ ¡ E E X EZ ¡E ¯YZ] \ 9] ] < &] % = #± ¢ E = V #" E, 6 E9 %5+ , #" = # # = # E ¢] # 0 Z $%%% $ Z Z & ' * + / % Z[X\ XZ¡ XZ Y] 9F

X[]XX[ ¨ 6+]Z[X\]¡Y ¡Z ] ¡ &] É Æ± Ê (] ) " ¢F# # # == #" 6 E9 # # = #" , # " # ¢] # 0 Z $%%% $ Z Z 6 / [ < [ * ' = Z[X\ ZZ¡ ZZ¡ ] 9F

X[]XX[ ¨% ]Z[X\]¡ YY ¡¡]

] ] 5 V ¢+ V , #" = #" " ¢

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

0 Z 3~rd Z /8 = 8 * $ ( 6$==&+0> < F # X \ [ XZ] 9F X[]XXY ¨Z ¡X¡[]Z ¡Z\ ] ] 9 # V 6] £ # (] « ¢) V

# == #" 6 E9 ¢] #

0 Z $%%% $ Z Z & ' + / Z[X ¡[Y ¡XX] 9F X[]XX[ ¨ E 6+]Z[X ]\\ [ ] X[ 9] >] Q"" +] 5 6] ] ) ¢Q #" E9 " , # # = # ? " ¢] < * +8( 8 V ] # ] \ Z¡Z Z [ X ¡] 9F

X[]X[[¡¨ [[X [[ [[Y ]

MFCLD< ~~

E◦

~

* +88 V ] Z # ] X Z[X¡ \X ¡Y] 9F

X[]X[[¡¨ [[X E[X\E[ [ZE\] Z[ 6] ª ] ¤] % ] # " >] " ¢"2

+ " # " =

= #¢ & ] QQQ # ] # ] # 6 , Ë + # % #" Z[XX \[¡ \X ] 9F

X[]XX[ ¨ 6+]Z[XX] ¡ Y ] ZX 6] (] % 9] ¢% , =E, , # ? # 9 # # 6 E9 ¢ # 0 & } = 0& 34! «] ( #" Z ] 5< % ¡ %= #" Z[XY Y \ ] 9F X[]X[[¡¨ ¡ E E X EXX¡ ZEZ¯ ]

XX )] Q# (] ¤ (] % 9] >] E " ¢ E9 == #" # 6 E9 $%%% Z & ' V ] [] # ] X Z[XY X¡¡ X ¡] 9F X[]XX[ ¨ 6F]Z[X ]ZZ¡ YXZ]

ZZ 6] E+ (] ] ] # (] 9] ± ¢F6 E%5+ + V # E # %5+ ¢ $%%% Z & ' ( V ] X # ] Z[X XXY¡ XX\ ] 9F

X[]XX[ ¨ 6F]Z[X ]ZY\ \¡X]

XZ ] ) # ¤] ( ## # ] (] (] 5 # ¢ % # ¢ # ] ( # 6 , 6 V ] Z # ] XY Z[X X\ X\ ] 9F X[]XX¡¡¨[Z¡ \Y X [ [ ]

Z 6] E+ (] 9] ± ¢F6 E%5+ Z +#

= #E %5+ # # 6 E9 « V = = # « V X\X[][\Y¡ VX Z[X\]

X ] 6] ª ] % # >] " ¢+ # " = E, %5+ ¢ $%%% $ 8 6 / < V ] Z # ] Y Z[X[ X Y ] 9F

X[]XX[ ¨ %]Z[X[] Z ]

ZY ] < &] % = #± ¢Q = # V E # #" , E # E # # ¢ ( + / ( [ < ' & ' $ 6 / V ] ¡ # ] Y Z[X YZ X] 9F X[]XY X ¨(+ 6 %¯YE Z[X ¨Y ]

XY +] ¤ # ] > # (] 9] 9 # +] (] 9 V # ¢+ , # 6 E9 V 9 E # # # %5+ ¢ $%%% $ Z Z & ' ( + / ¤ #" #" Z[XY X ZY X X] 9F X[]XX[ ¨ 6+]Z[XY]\ [¡[ Y] X +] ¤ # #" ] F] > ] ## ] % E # >] " F = +# # = , E , 9 == #" , # + # 6 , V ] Y # ] Z[X X Z[\] 9F X[]X[[¡¨ X[ XYE[XZE ZXE[] X\ +] ¢ = # = # E # = " # 6 E 9 ¢ # + / [ & ' * < / 6] % Z ] + % YY[ %= #" Z[X\ \[ \ZZ] 9F X[]X[[¡¨ ¡ E E X EZ ¡E ¯ ] X¡ +] ] < &] % = #± ¢F# 6 E9 %5+ , , E #¢ \ 0 ( # ] X 0 8 ' Z Z[X\ ¡ \ # & ] X ] ] < 6] & ## +] % &] % = #± ¢Q # 6 E9 = #" E, E # , E , ¢ $ Z 7 +88 * < / * /8 6 V ] Z\ # ] X Z[X\ \ ¡ ] 9F X[]X X ¨ EZ[X\E[[[ ] X ] ] < +] % ] ) &] % = #± ¢ V # V # E V " # " # 6 E9 E # # E" # # # ¢ <

Z & # 5 , ?iiT,ffTQBMi+HQm/bX Q`;f Z\ Q] 6 , £] 6 , ] # " ] E ¢F6 # # # V % )

%U6)¢ QQQ # ] # ] # = E £ # # Z[XX Z \Y Z ¡X] 9F X[]XX[ ¨ £]Z[XX]\XZ\ YY] Z¡ 9] % )] ) # ¢£

& [ # # # ¢] $%%% & ' + / < V ] X # ] Y Z[XX [ Z] 9F X[]XX[ ¨ 6+]Z[XX] Y Z ] Z +] % ] ] ) ] 9 " ¢Ì = V # = # E # = # # , # V E " #¢ 7 + / [ < ' & ' $ 6 / V ] ¡ # ] X Z[X XX Z[] Z +] % ] ] ) ] 9 " ¢ " # V # #

V %5+ # " ¢ 7 + / [ < ' & ' $ 6 ( / V ] ¡ # ] Z Z[X Y\ X] [ +] % +] #± ] ] ) ] 9 " ¢ , # E E " # V # V " # , # E #"¢ $ Z 7 +* * & ' 6 ( / V ] XX # ] Z[XY] 9F X[] ¡¡Z¨ Y¡X] X &] % = #± ¢ , , #

# #" Ȣ #

+ / 34! Z $ + / [ & ( ' * < / 6] % E \

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

Z ] + % [ %= #" Z[X¡] 9F

X[]X[[¡¨ ¡ E E X E Y[YZE ]

~

£ Z[XZ ¡ [] 9F X[]XX[ ¨ E 6F%]Z[XZ]\ ¡¡ ]

Z )] % #, ª (] % 9] ¢Ì£ E 9 == #" # E # &U¢] #

0 Z $%%% $ Z Z & ' + / ¤ #" #" Z[XY Z[ZX Z[Z ] 9F X[]XX[ ¨ E 6+]Z[XY]\ [¡XZ¡]

\

¤] % (] ] ] # +] (] 9 V # ¢£ %5+ > È¢ $/ * /8 V ] [ # ] Z Z[XZ \ ¡¡] 9F

X[]X[X\¨?] V ]Z[XZ][Z][[ ]

¡ ] > # ] ¤] ( ## # ] ) E # (] (] 5 # (] ] 9 # ¢6 E " E # 6 E9 %5+ V E #¢ $ Z 7 & ' & ( V ] Y # ] Y Z[X \Z\] 9F

X[]XX¡¡¨[Z¡ \Y XY X[[ ]

Y ¤] % | * '

6|+< & 9 9 # = E " 5 # # Z[XZ] (] % <] Q#" )] Q# >] " 9] ¢+ , # V #

6 E9 %5+ ¢] # 0 Z $%%%5&67 $ Z Z $ & ' 6 /

\\

E◦

] "" &] )] 5 # 6] ] ¤ +] >] ) " ,, # ¢Ì # ? # # # ¢ # + / } * 0 5< % X %= #" Z[[[ Z ¡Z] 9F X[]X[[¡¨ E Y[EYYY [E¡¯ZX]

+] > ] # ] % #± >] E = Q # " # # 9 = #" 6 E9 # # ] ( # +== E # = % # V ] Z\ # ] X Z[X\ XZZ] 9F X[]X X ¨ EZ[X\E[[[¡]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

E E % ( ? %5+

ǰ £¢ £ǰ £ !" "#$ %&'(' )# F %&'()*'+ $ " $ " ) (( ! 1 $ ) (( ! 1 4! < "

" " ! 0 " " 1 " 1 $ " " ) ((

" J ! ! 1 & " ! )56 ! 1 G0

& 5 0 " 1 " 1

,-./0(1&+ " "

23 4'(015*H04 % V # , = # # #V E # # # == # V ] F#

, V , , E # # V " # # # #V # # ] % # = # ~ , E , # #" E # #V #E # = , #" = ## #" # # # ] # #V # # #" , #E # = # = E= ## # = # # V # ] , V # # , " " # , # # #V # # ] # # = #" # ~ " = # = # V " = = # # # " X\ ] < V " # # V , , # # # # = # , " = #" " ] V , V , E V = = = # ] + V \ = # # , E

= #] #" = # = # ,

E " " = " " ? E = V E = #" = " = #" E # #" = ## #"

E V 6 , 5 # #" #" ? #

# = , E ¤ # = # " # V # # # #" # V ] + #" , , V

# , # V " #] XZ = # 2 + = V %5+ V%5+ ] # # #V # # = , #" #" V = # = " = 6 E9 # = , ] # X # # == E = #" , # #" , E # , = # ] # = E " = #" # , , = E # ] = # = #" # # # # # " # # # # # ] = = , # = #" #E # , # , # , # E ] 9+V # X = ? V = , # 6F% 6 , F= #" % Y XX # # # # ¤

= X ] = ) %5+ ) % E # 5 # +# == #" " # = E ] 6 = = X[ # E # , = #" #V # # , # 6F% ] #V # # #E ? " # # # # # E V #" , # , ] # # , E # # # , # > ,% ] +, V = #" , = E , # # = ] # # V # " ] # # == , # # = # = # " = , , = ] + #" V # " = #" #

, " E = # " E V #" , = == ,

# # # # #E, , = ] # # , == , , = # == E # # " = #" V ] > E = = , #" = # , , # = , ] # # # , ,

E " = " E = # # # E ~ = # V V E # ,? \¡

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

E , # , ] = 6+&& # # # , # ž = E E V ] ) # V " # E # = , == # ] % == # ƒ6+== „ # , = # , # #E Â?= # = " #" 6+&& +&‚ # # ] > # E , ~ # , , # 6+== # # = 6+&& +&‚ = #E #" # = ÂŒ , = # = E # #] 6+&& # V " # = 6+&& # # , # V " # , 6+&& ] F == , # ÂŒ , E " # –XÂ&#x;˜ # = " # , # V " # # E ] # # # = # ] ‚# –Z˜ , #E, = # V " # = # ] ‚# E = # "# V = # # Â? E # = = 6+&& # V " # ] + = ÂŒ # # #" = # # % #" 5 #" " ƒ% 5„ –—˜ ] ) # % # Z " #

6+&& = # ] ‚# % #  ]X # # # ~ , V ƒ , „

# V " # ] % #  ]Z , = # # = # , , E � #] # # , # , # # , # % #  ] ] % #" % # Y V # V " # = # # # V Œ # # � = ] % " V # # % # —]

73 ## .&'-; '(5*'5(9 V = # V , , # E = , # , # # = # " 6+&& = ? –œ XÂĄÂ˜] > " V #E, = #" # # , # #" # = = , = #] ‚# = # # V " # = E , E " # , # ] +" # ƒacore „ = # #, , = E # = # , # E , # = ] 9 = # #" # # acore , # # " #

V 6 = +" # ƒarep „ E # ] 5 = == # ž 9 # +" # ƒadyn „ = # # ) "] X] ‚ = E # # , == # == E , # 6+&& % # arep ƒ = ÂŹ [„] > ~ , E ƒ = ÂŹ X„ acore # == = E adyn 6+&& ƒ = ÂŹ Z„] ‚ , E == # ž 6+== = " # ž +" # ƒacloud „ # 9 # +" # acloud \Â&#x;

‡~„

# # ƒ6+&& = „ # adyn # 6 , = ƒ = ÂŹ  „] < Â? adyn E " # # V 6 , = # #" arep # acloud V = # , = # ƒ = ÂŹ Y„] ) # # ÂŒ # adyn # = # #" acloud # ƒ = ÂŹ —„] 9 = ÂŒ # " E # 6+&& = # # –XÂĄÂ˜] Robot platform

RAPP platform Repository Agent 0

Compilation

E , == # V # , = E

Eâ—Ś

MFCLD< ~~•

RApp

1

2

Core Agent user

RAPP store RApp

RApp

RApp

RApp

Command

Application Download Service

Download o RApp

Application Download Service

3 Launch

RApp Dynamic Agent Operate

RApp Cloud Agent

4

Launch

Destroy

5 Time

& ' "' @ W /> W >

:3 G)J6P)H04 .&'-; :323 .&'-; -Y56(-;-4'& )41 -(J6*-& 9 , V , "# # Œ # " # = � , V # # , V # # # # ] # == # , "# = # # , #" # ] % = # # V " E # V # V ƒ) "] Z„] + = # ƒ V  „ # V " # = " # ƒ V Z„] Q = # " # "# #" = Œ # = E # # # Z] > = # # # ƒ V X„ "#

V = # # = # #] ¤ V = # # = # # V , " V # # # # � E # # V ] RAPP navigation system (level 3) Agent

Agent

Agent

component1 component2 component3 (level 1)

component4 component5 (level 1)

component6 (level 1)

(level 2)

(level 2)

(level 2)

& ' ' @ W /><< Y W + #" 6+&& = Π# # V " # = # V = , # V " # ~ V = , # 6+&& ] + # , # E V " # "# = = # ,

V # # V " #

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

E T i?SH MMBM;k/ ž = # #E = " #V # # E ;2ih` Mb7Q`K ž # " V # , ? #

= " V # = # ~

E # E ;2iJ `F2`J T ž # # = E K `F2`GQ+ HBx iBQM ž # # # # ,

E # E `2H iBp2GQ+ HBx iBQM ž #" # =E # # # , =

, # # =

E +QKTmi2*m``2MiSQb2 ž # # , = # " ,

# #" = +QKTmi2:HQ# HSQb2 V # `2H iBp2GQ+ HBx iBQM V

‡~„

~ " = # = ƒK `F2`GQ+ HBx iBQM„ # #‘ ƒ+QKTmi2:HQ# HSQb2 ;2iJ `F2`J T„] navigation <<Include>>

<<Include>>

<<Include>>

<<Include>> path_planning

global_localization

localization robot_motion

<<Include>> <<Include>> getTransform mapBuilding <<Include>>

<<Include>>

getMarkerMap pathPlanning

<<Include>>

<<Include>> markerLocalization

E +QKTmi2:HQ# HSQb2 ž # , " , = # #" # = # E # , ,

# # K `F2`GQ+ HBx iBQM V # E #

Eâ—Ś

MFCLD< ~~•

<<Require>>

common_motion

<<Include>> computeGlobalPose <<Include>>

moveAlongPath relativeLocalization <<Require>> <<Require>>

<<Include>>

computeCurrentPose <<Require>>

<<Require>> <<Include>> lookAtPoint

<<Include>>

ƒ „ ¤ V

E HQQF iSQBMi ž # , = # " V # # " ,

#

RAPP navigation system

E KQp2 HQM;S i? ž V , , #" " V # = dynamic_agent

E +QKKQMnKQiBQM ž " = , E V # ƒ V , #" V , E V V , ? # # # E , = Œ # = „]

cloud_agent

<<allocate>>

<<allocate>>

getMarkerMap

path_planning <<allocate>>

<<allocate>>

# V " # , V E ƒ) "]  ƒ „„] > #" # V � , = # # ƒ" E = „ # = � V = = Œ # V ƒ = „] V # " , = = V # " # E ~ ] > #"

E *Q`2 b2`pB+2b ž = # # , = E ~ , , = E _2TQbBiQ`v b2`pB+2b ž # , #" , V " = # # , == # # E ~ " = # E *HQm/ b2`pB+2b ž # , #" , V " = = Œ # ~ " = # E E .vM KB+ b2`pB+2b ž # , #" , V " = ] Q , V " = , # E = # #" " # , # V " # V # # ) "]  ƒ,„] 5 #

, , # " = ÂŒ V ] > = ÂŒ # # # ÂƒĂ…6E „ , " , # # QÂ? # # ) V #] = Â? V = # V , = = # # ƒ`2H iBp2GQ+ HBx iBQM ;2ih` Mb7Q`K +QKTmi2*m``2MiSQb2„ # " , # " = ÂŒ ] F#

repository_agent

markerLocalization

computeGlobalPose

<<allocate>> mapBuilding

core_agent

<<allocate>>

<<allocate>> <<allocate>>

computeCurrentPose

robot_motion getTransform

<<allocate>> relativeLocalization

ƒ,„ 9 , # V # " # 6+&& E

& ' ' Z Y W

:373 .&'-; @0;=04-4'& = # # Â? V = # V " E # V # " E V # , = ÂŒ # # # , # # = # #] > # = # # # Â? = # # V # # " # , # ] = #" = # # = #" , # " # ] 6 = # = # # , V ] ÂŁ E = # = # # " #" = # = V ] # E , = # # = # V " # # # , " # = # # ~ ] \Âœ

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

#" # # # , V E # V = # # # , #" " # # # ) "] Y] + acore = E Œ # # = # # = # V # # , = = # acore <+F , # � = ] T i?SH MMBM; = � V � #" = # #

E 88†/ 8† ž #V # # = # = V arep = # # E 88† / 83* ž = # # = " V # , = # # " V #

# # = # " =] = # # #ÂŒ " = = E ' †8 ž #E = #" # 88† / 83* # ~ # " , = ] + E = # # , # # # 6F% = " ž " E , ÂŻ= ## –Â&#x;˜ , # " E +Ă? # 9 ? ‘ ] ‚ = = #" " V # " " % = & #E # " E 88†8 †8 ž V 8 0 E V ~ # = # #" V E 88† / 83* 88†/ 8† ( ' †8 = # # ] ;HQ# HnHQ+ HBx iBQM V , V , = V = # # E # ~ " #

E ÂŒ * X ž = # K `F2`GQ+@ HBx iBQM V # acloud " # E / † ž = # + Tim`2AK ;2 V # acore " # Âł V " , E E / † ž = # ;2ih` Mb7Q`K E V # acore " # E ž = # +QKTmi2:HQ# HSQb2 # ;2iJ `F2`J T V # E # , #E adyn " # ƒ6+==„] `Q#QinKQiBQM = Â? V Â? , #" = # # # acore " #

E / † ž = # # , # +&‚ # acore " # , ³ # acore # , E Q † ž = # # ~ #E acore # , , ³ # acore V , E ' † ž " acore =E # V # # , , E acore # , ] /8 0 V # " / V # / † = E # # ] V ž `2H iBp2GQ+ HBx iBQM ž � ,

= # #" = E # # # acore " # ,

E † ž " , # E = ƒ ÂŻ = „ = , E # # V # ÂŻ , ÂĄ[

MFCLD< ~~•

Eâ—Ś

‡~„

E ' † ž #" † # = , ƒ V

, # = „ # V # E ¯ , ] = # # = � # V " # E = # # ) "] Y] RAPP navigation system

dynamic_agent

repository_agent

cloud_agent

Arep-control_subsystem : : : :

Adyn-control_subsystem parts localization : RAPP dynamic API : hazard-detection RApp

parts rapp_path_planning rapp_map_server rapp_costmap2d global_planner

Acloud-control_subsystem parts qrCodeDetection : RAPP dynamic API

core_agent

real_receptors

virtual_receptors

parts : camera : bumpers : sonars

parts : camera_server : obstacle_detector

real_e ectors

Acore-control_subsystem

parts : motors : IMU

parts : estimator_server : move_server

virtual_e ector parts : execution_server : state_server : robot_localization

& ' ' Y W /><< Â? ÂŽ :3:3 @(0&& @0;=04-4' @0;;546*)H04

, " # # 6+&& –XÂĄÂ˜ ## # , # = E # # ] # # #" " # , , E # = ÂŒ , # , # # # = ÂŒ , # " # ] ) 6+&& = ÂŒ E # # # , # " # # = , = " # #

#] , V # #

# # = # # , # =

V , # ## # ]> = # " # # # , E = # # ~ = , ] 9 , = #V V , E ~ # # , # = E # # ] + # # = #" #" E # = # , # E # # # ~ –¥ XY˜

V = , # # , E # , = # # ] ‚# % # = # = # # # # # � = V = #] F= # E = � V # ~ = Œ # # , # = # # � # = # V ]

= � # V " # V = E #" ž T i?nTH MMBM; # ;HQ# HnHQ+ HBx iBQM] Œ # ž T i?nTH MMBM; V ž = E # arep # ] V #

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

# # E " # # # ] ¤ V arep # " # V # # , # # # " E #

E # # # # arep #" 6& ~ ¨ = # # E , = , ~ V = # ] V " , # # # # , # = # # #" T i?nTH MMBM; V Â? # = # # ) "] —] + ) "] Y # ) "] — arep = E path_planning repository_agent rapp_path_planning

rapp_map_server rapp_costmap2d

global_planner

MFCLD< ~~•

Eâ—Ś

‡~„

# # 88†8 †8 = # #

# 6+&& ,? ] # = Â? # V " # V V # " = #" ;HQ# HnHQ+ HBx iBQM V ] += V # ,

= #" " # ž acloud adyn # acore ] = # # = # , V � # 6+== ƒadyn „] ‚ " # # V # # " , # = # ] # " # = # V = # # ) "] \] + ;HQ# HnHQ+ HBx iBQM V ~ # # , = = # # acore " # � = = # #" <+F , # # acore ]

algorithm_config costmap_config confPlanningSeq obs_map_path

global_localization [true]

map plan_seq

[false]

costmap sendMapToCost map

loadMap plan collision-free pa

status

moveHead

head limit calculateCost map

map

[false]

makePlan

qrCodeDetected

[true] - qrCode pose

requestMakePlan makePlan_request

path

captureImage

getTransform

camera pose image

robot_type

planning_algorithm

grid_map_ID

start_pose

goal_pose

planned_path

qrCodeLocalization

qrCode pose qrCodeDetection

& ' #' S Y arep ‹ ~ Y 88†/ 8† 88† / 83* ' †8 # 88†8 †8 = E # # ] 9 #" arep # # = " V # # , = # # # # # ] adyn V , # #" ~

arep ] ~ # V =

E , ÂŻ = ž ‚9 , = ÂŒ # , # # E = ## #"ÂŻ " ž ‚9 # V , = #E # #" " E " ÂŻ =¯‚9 ž ‚9 = # = #V E # # = E # E ÂŻ= ž # = ~ = E " ÂŻ= ž # = ~ = ] < Â? 88†8 †8 = # # # = ~ # #ÂŒ " == = ƒ E # # „ # # = # # ] + = # " = #" # V ƒHQ / bT2+B7B2/ K T„ ` TTnK Tnb2`p2` = # # ] = # + H+mH i2 +QbiK T # V = = # # 88† / 83* = # # ] < Â? K F2SH M # V = = # #

' †8 = # # " = 88† / 83* = # # # # ] + ~ # 88†8 †8 = # # E ~ K F2SH M # V = # = ] ) # K F2SH M V =

QRmap

& ' ^' > Y W

‹

Y !>Š

adyn " # V � # , ~ #" acore = ƒ + Tim`2AK ;2 # V „ # # # # ,

# ƒ ;2ih` Mb7Q`K # V „] < Â? adyn # , # acloud [`*Q/2.2i2+iBQM V ] V = # , ÂŒ # #" Ă…6E # = # # # = Ă…6E # ,

# # # = # adyn ] < Â? # # , #" #V , adyn # V ƒ [`*Q/2GQ+ HBx iBQM„] V " = E = Â’&/ 8 ÂŒ # = Ă…6E # " ,

# ] ) # = , # " , = #" = # # # Ă…6E ,

# ] ¤ V [`*Q/2.2i2+iBQM V # Ă…6E # , # = adyn ~ acore

#" , # # ƒ ~ KQp2>2 / V „] # # � = = # ] ,

= # # E # Ă…6E # # = , Â? # # ƒ * / ÂŹ „] ÂĄX

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

>3 G)J6P)H04 .&'-; ;=9-;-4')H04 )41 -! (6K*)H04 = # , V #" # " = # # = #" # ] + acore = # # , = acore <+F , = # ] £ E = # , #" , = <+F~ ] > E # # , # & ' 8 6 / 6F% XX

E ;HQ# HnTH MM2` ;HQ# HnTH MM2` = E # #

MFCLD< ~~

E◦

~

6+== = # #" <+F , = acore ] # #V # #

= # ¨ # = #¨ ] + 6+== " V # , = # # = #E , # #" # # = # ]"] E # # ] # V # , V E # # # # # = #] V , # # # # # # " = " # , ]

E `Q#QinHQ+ HBx iBQM `Q#QinHQ+ HBx@ iBQM = # # E +QbiK Tnk/ ~ == 6+&& = = "E # ` TTn+QbiK Tnk/ = # # E E K Tnb2`p2` # # #E E #" = , = # # ` TTnK Tnb2`p2` = # # ] + # # , # , # = , # ~ ¨ = # = E # = # # # #" ¤ & = ] # , = 6F% V ~ E ¨ = # # ¤ & ~ E ¨ = # ] £ # # , #" V = #" = 6+== E #] # Z # V # # = # 6+==] # ~ # E V ~

X * 8 = # = , # # V +QKKQMnKQiBQM = V ] , = E = # # # #] Z ' #

;HQ# HnHQ+ HBx iBQM V ] # , # = ] * * 8 V # #V # #

8 8 # T i?nTH M@ MBM; V ] adyn ~ #E = #

acore ~ # ) "] ¡ ] , / + 0 #

KQp2 HQM;S i? V `Q#QinKQiBQM = V ] , V # # = ] + 0 # HQQF iSQBMi V `Q#QinKQiBQM = V ] , = # # E # = # ] > * # +?2+F> x `/ V # arep V ] # # ,? 6+== ) "] ¡ , ] ' # ;HQ# HnHQ+ HBx iBQM V ] ¡Z

& = ## #" 8 8 V # #

acore #] = # " # , # # , # , # ,E = # V , , #

, <+F , #" # V

?iiTb,ff pBK2QX+QKfR9Ryd38dd #"

& ' q' * Q Y ~ Y

# = # E , = = # # E = , ↔ # # ] = = ## , arep # E , = # # # #V # # #E ]

E # " , # E = = V # #" = E # E # # , # = # = ] " # , # , = # = X # # V # <+F , ] = # # = ?iiTb,ff;Bi?m#X+QKf` TT@T`QD2+if ] = , 6F% # " V # # # # ¯ " # <+F ,

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

?3 5;;)(. # = = , , # V " # E , ] , # , " # # # , , # , # , # = = #" = # , , # = ] ) = = , E #" # " # , , # V ] & # # V " # V == = ~ # # = , # , # # E # = #" = ] #" <+F , V = E #" # , , = # = , ~ = V ] # " #

= # , #"

, = = , # = # = = # #E = , # #" = ] # = = # # E = # #E V , = = ## #" # " , #]

= # # # # ,

= # , = # E , = ] # V " # # E " #E V # V E , = == ] = = = # # V <+F , ] # = # V E V = = # ~ = # , , , = ~ == #E, = # = ] # # = = # = , =E = # V = # ] = # ~ E V = V , E V # ] 6 , == # = " # # # # = # # # = , # 6F%]

*C40/9-1P-;-4'& # , )&¡ , V & E ? 6+&& # +" # < ] \X[ Y¡ # , Q = # #] == , & # % # # ¤ " Q E # # # Z[XY Z[X\ " # # E # # # # # = ? ]

D EF

. 6 % =∗ # # # E = # Q#" # #" > U# V # E " [[ \\ > ] < ? X ¨X E

? ] ] §" ]

= ¨¨ ] , ] ]= ] ]= ¨] . 6 % $ = ) $ # # # = # Q#" # #" > U# V # " [[ \\ > < ? X ¨X E >]% # § ]= ] ]= ] ' $ . = # # # = E # Q#" # #" > U# V # "

MFCLD< ~~

E◦

~

[[ \\ > < ? X ¨X E # E §" ] ] ∗

= # #"

"A" "G@" X 6] + " £] Q# 5] #", #" >] « ? # ] # )] )] #" +] %] ] 9] #" # ] >] 9+V # + = #" E V , ¢] # & ' * + ( / `$ &+{[ 34!4 $%%% $ ( Z[X[ [ Y [ ] Z >] 9 ] # >] % # # ] > # 9 , <+F , # E V " # # # =E = #¢] # 3! $%%% $ ( < * * < * + / ( * & ' [ <<+&Y34!? Z[X\ YZ Y¡ X[]XX[ ¨ +6]Z[X\]¡ ¡ Z\Y] >] 9 >] % # # ] > # < 6 , < V " # % % 9 V E = # # # +" # E + 6+&& & ¢] # 6] % ] #± E # ] #± ] & +* + / [ & ' * < ( V ] YY[ Z[X\ \Z \ X[]X[[¡¨ ¡ E E X EZ ¡E ¯ Y] Y F] %] 6] ) # #] 6 , F= #" % ¢] ?iiT,ff`QbXQ`;f] F# # ³ X[E+= E Z[X\ ] %] ) # +]

# 6] % # + 8 ( * 6 <|} / / * ( Z ~ * *Z Q V " # ## Z[X ] \

] %] & &] +,, # ] , " + V # , # #¢ $%%% + / 6 * %

V ] XZ # ] Z Z[X Y[ X[]XX[ ¨ +%Q]Z[XY]Z ¡\Y Z]

¡ 6] 5] # 6] 9] £ # * 6 } + /( 8 = * 6 * /8 > E & , #" Z[X[] Q] EQ== # Q] " ] )

] E # ] # " # 6 E , # V " # # # #

#V # # ¢] # $ & ' * + ( / Z[X[] &] + V , " V

== == ¢] # 6] % ] #± # ] #± ] 0 + / [ & ' * < Z Z 3 & ' ( Z V ] X Z[X X¡X X¡ ] X[ ] # ? 9] ¤ # 6] 9 +# # ] > , 6 = + , = E ¢ $%%% + / 6

* %

V ] XZ # ] Z Z[X Y X Y X[]XX[ ¨ +%Q]Z[XY]Z Z \] XX ] Å " ] # ] (] ) ] )

(] 5 , 6] > # +] ©] <" 6F% # = #E ¡

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

6 , F= #" % ¢] # $ &+ ( 8 8 V ] # ] ]Z Z[[ ] XZ (] 6 (] V # (] ] ] # Z + +

= V #" # == #"¢ & ' * + / 6 / V ] \Z # ] Y Z[XY Y[X YX ] X (] % ± #E Æ &] Î#Ï " )] 9] 6Æ # 9] " " E #Ï + # E, , # V " # # #" E " = #"¢ $%%% + ( / 6 * %

V ] XZ # ] Z Z[X YYY Y Y X[]XX[ ¨ +%Q]Z[X ]ZY[ ] XY (] % # + V # # # E ¢ && V ] , ¨X[XX]X Z Z[X[] X ] % V ¤] #" %] 6 # 6] #

= , ¢] # 34!4 $%%% 3? /8 / / / * `<66 { Z[X[ X X[] X\ ] # # ] # ,Ð # " = #" # = # , ¢] #

344" $%%%5&67 $ $ ( & ' * 6 / Z[[ YZ\X YZ\\] X¡ ] #± >] % # ] ) " ] % # ] # >] = ] % #± ] #± E # (] ) " 6 # " , # E = , ¢] # ] E ] !4 $ 8 & ' < * `& < { Z[X X [ X X[]XX[ ¨6 ]Z[X ]¡ZX ¡ZY] X ] #± ] # # ] > # + E "# #" # V # , ¢] # !"( $%%% $ ( < * * < * + / * & ' [ <<+& Z[XY X XY X[]XX[ ¨ +6]Z[XY]\ ¡ X¡]

¡Y

MFCLD< ~~

E◦

~

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

G H =H B > + "

Ù ǰ £¢Ú !" "#$ %&'(' )# K %&'()*'+ $ " G* ! ! ! " 2 ! 1 " ! ! " ! $ ! 1 " D * $ 0

! H N !

" 1 ! $ " ,-./0(1&+ + 5 " >

! !

23 4'(015*H04 # # # = # # # # E , V #" # # = ¤ =E 9 V %= ¢ X[ XX ] ~ == ¤ = F # # E , ¤F # = ## #" E = ] % # , # == # V , = # # , E , ~ == V X == E ] # # E # V #" #" = ## #" E = , ~ #" # = = # # = ## #" V = # = V # # " V ] V = #" = , # # = # , #" , #" #" = #" # # = = ## #" = ] " = ## #" = # , =E = # , V Z Y ] ¤ = , E #" #" , # # # # , # V # = # # ] # , = # = # == ~ , #] , # # # # , E , = , #" ,? V #E # ] # = = , E

, , ~ == ¤F ] + #" , V # # = # V # # = = # = = # " ] = = = # #" ] # # # # E , #" # # # #" = #] % # Z = V , # % # = # , # E " ] 6 # = # #" , , V # # % # Y] % # # = = ] > V # # # , #" " #

# # V q(t) q(t) ˙ ∈ Rn # , # ,E ? # , l < n = # # ] #E # = # & # A(q)q˙ = 0,

X

A(q) ∈ Rl×n # # # ] ) , , # # # # # = # # # #" E # = #" # # = , ] # , , #" & # # # X # , , , # m q˙ = G(q)η = gi (q)ηi , Z i=1

q ∈ R η ∈ R m = n − l # V , # # V V = V # G(q) # V gi (q) = ## #" # A(q) n

m

A(q)gi (q) = 0, i = 1, . . . , m.

73 0%0' ,64-;)H*& 01-9& # # # # #E = ## #" = R V , ) " X ] = = # 2l] Q = # # = # # # # , , = # = # # = = #E ## #" = #E X # , , #" # " # = ## #" E = # # # # = # #

] % # # # E = #" , #" , #" = = ## #" # E ] V # = , V # ] ¡

Eâ—Ś

‡~„

2 AM G = T rans(X, x)T rans(Y, y)T rans(Z, R) Rot(Z, θ0 )T rans(Y, −2l).

ƒ\„

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

MFCLD< ~~•

# = # PKi i = 1, 2 = E " # �= # = # #" # #" = # ⎛

Mi PK i

⎞ 0 = � 0 ⎠, −R

ÂƒÂĄÂ„

# = V # , #" # # i AH Mi = Rot(X, Ď•i )Rot(Y, θi )Rot(Z, Ďˆi ).

& ' "' = �Š‡ Q Y

‚# # = #

, ΠΠ#" , , E V # # = Π# = #E # #" = , = , V # V , ] 7323 A599 ,64-;)H*& 01-9 > , V # E # = # ~ == " , #

E # ] , #ÂŒ " # , , # # ) " X , #ÂŒ " # V q = (x, y, θ0 , Ď•1 , θ1 , Ďˆ1 , Ď•2 , θ2 , Ďˆ2 ) ∈ R9 , T

ƒY„

x y θ0 , , , = # #

# # # Ď•i θi Ďˆi i = 1, 2 # # = # #" = = V ] #" , # = " E = # = # ] V # #V # # #E # #" = #

# ÂŒ Â? , , # " # E = # ] Â? = = # # = # # ÂŤ Â? = = # # = = # Â? ## #" E = # ] # # " ,

# " V # 1 AM G

ÂĄ\

= T rans(X, x)T rans(Y, y)T rans(Z, R) Rot(Z, θ0 ),

ƒ—„

ƒÂ&#x;„

G04809040;6* @04&'()64'& V , # E , V #

# #" # = # = # PKi i = 1, 2 = " # ] ‚ " V #" = #Œ " # = = # # = E ] V = V = # , ~ # V = # # #" = # PMi i = 1, 2] , # # = # # # E = V = # = # ƒ V " ,

# „ �= # # #" = #

# ] ⎛

⎞ R(θË™i Ď•i − ĎˆË™ i θi # Ď•i ) Mi ⎠, i = 1, 2. PË™ K =âŽ? −R(Ď•Ë™ i + ĎˆË™ i # θi ) i 0 ƒœ„ V = # #" = # Â?E = , V # # #" = # " V # ,

M1 PË™ M 1

M2 PË™ M 2

⎞ ⎛ xË™ θ0 + yË™ # θ0 = âŽ?yË™ θ0 − xË™ # θ0 ⎠, 0

⎛ ⎞ xË™ θ0 + yË™ # θ0 + 2lθË™0 ⎠, = âŽ? yË™ θ0 − xË™ # θ0 0

ƒX[„

ƒXX„

= V ] + = # V ƒœ„ ƒX[„ # ƒœ„ ƒXX„ " V ~ # # # E # # # #" # = =E # # , # # & Œ # ƒX„

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

⎡

−cθ0 ⎢ s θ0 ⎢ ⎣−cθ0 s θ0

−sθ0 0 0 Rcϕ1 −Rcθ1 sϕ1 −cθ0 0 −R 0 −Rsθ1 −sθ0 −2l 0 0 0 −cθ0 0 0 0 0

⎞ x˙ ⎜ y˙ ⎟ ⎜ ⎟ ⎤ ⎜ θ˙0 ⎟ ⎜ ⎟ 0 0 0 ⎜ϕ˙ 1 ⎟ ⎥ ⎜ ⎟ 0 0 0 ⎥ ⎜ θ˙1 ⎟ = 0, ⎟ ⎦ 0 Rcϕ2 −Rcθ2 sϕ2 ⎜ ⎜ψ˙ 1 ⎟ ⎟ ⎜ −R 0 −Rsθ2 ⎜ϕ˙ 2 ⎟ ⎜ ⎟ ⎝ θ˙2 ⎠ ψ˙ 2 XZ # ,, V # sα = # α # cα = α]

MFCLD< ~~

E◦

~

⎛

,64-;)H*& 01-9 > V #" # ¤F , , E # Z #

# V = # # ] % # E # # , " #

# V q(t) = n = 9 # # & # XZ # A(q) = l = 4 # V # # m = n − l = 5 = # # ] < , # , # , ] ¤ V # = # # \ # E # # #" ϕ˙ 1 θ˙1 ψ˙ 1 # ϕ˙ 2 θ˙2 ψ˙ 2 V ] # V # , # ] # V , # # V = E , ] , # # # #

= ## #" V

#" # V = # # ϕ˙ 1 , θ˙1 , ψ˙ 1 , ϕ˙ 2 , θ˙2 ] > # V # # = ## #" V = ## #" V , = E V =] +# # ~ # #E # XZ , # # # Z ⎧ x˙ = Rsθ0 η1 + Rcθ0 cϕ1 η2 + R(sθ0 sθ1 − cθ0 cθ1 sϕ1 )η3 ⎪ ⎪ ⎪ ⎪ y ˙ = −Rcθ0 η1 + Rsθ0 cϕ1 η2 − R(cθ0 sθ1 + sθ0 cθ1 sϕ1 )η3 ⎪ ⎪ ⎪ ⎪ θ˙0 = R (− θ2 sϕ η1 − cϕ η2 + cθ sϕ η3 − ⎪ 2 1 1 1 ⎪ 2l ⎪ ⎪ − θ2 sθ1 sϕ2 η3 + θ2 sϕ2 η4 + cϕ2 η5 ) ⎪ ⎪ ⎨ ϕ˙ 1 = η1 . θ˙1 = η2 ⎪ ⎪ ˙ ⎪ = η ψ ⎪ 1 3 ⎪ ⎪ ⎪ ϕ˙ 2 = η4 ⎪ ⎪ ⎪ ⎪ θ˙2 = η5 ⎪ ⎪ ⎪ ⎩ ψ˙ = 1 (η + # θ η − η ) 2 1 1 3 4 s θ2 X , # # # sθ2 = 0 # V # V #" , " # " # ] 7373 6;=96K-1 ,64-;)H*& 01-9 " , V # = #E # #" = , V #" #" Z V ,

& ' ' 0}

& ' ' O Q Q Y

# ~ V # ) " Z ] % # ,E V # # # ,

¤F X Z , ~ == #" V , , , = ## #" #" ψui #" #" θui # # E rui i = 1, 2 " = E # , , V # , = #" # #" = E = # # ] % # # " # V , # ) " q = (x, y, θ0 , θu1 , ψu1 , θu2 , ψu2 , ru1 , ru2 )T ,

XY

x, y , , = # θ0 # E # θu1 , θu2 # #" ψu1 , ψu2 = # #" ru1 , ru2 ] "Y56J)9-4' B8--9 V # , ~ == ~ V # # # # # , #

E # , #" = #

# , #" # ~ V # ] F# # V = #" ϕi θi = ## #" = # # # = # #" # = ] = E # ~ V # ¡¡

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

rui ] + #" # # rui # #" # θui # = # ψui # , = ⎧ 2 2 2 ⎪ ⎨ rui = R ## ϕi + $ϕi # θi θi , X θui = # # θi # ϕi ⎪ ⎩ ψui = ψi R # = # ϕi θi # ψi , # " #] #V # E # X ⎧ #% $ 2 R2 −rui ⎪ ⎪ 2 #2 θ ⎨ ϕi = ± R2 −rui ui r X\ . ui θ = ± # R | # θui | ⎪ ⎪ ⎩ i ψi = ψui "# # X\ = , θui #" = # # ~ # # ] U #" X\ # , # # = #" = , , # = # E Z[ " # V #" # = E X ] G04809040;6* @04&'()64'& +" # , V # #" # = ] ) # " # V XY # # E #" # = 1 " V # x˙ # (θ0 + θu1 ) − y˙ (θ0 + θu1 ) = 0 . x˙ (θ0 + θu1 ) + y˙ # (θ0 + θu1 ) − ru1 ψ˙ u1 = 0 X¡ % 2 # " ⎧ x˙ # (θ0 + θu2 ) − y˙ (θ0 + θu2 )+ ⎪ ⎪ ⎨ + 2l # (θu2 )θ˙0 = 0 . x˙ (θ0 + θu2 ) + y˙ # (θ0 + θu2 )+ ⎪ ⎪ ⎩ + 2l (θu2 )θ˙0 − ru2 ψ˙ u2 = 0 X # # # , " V # & # X ⎡ 0 0 0 0 s0u1 −c0u1 ⎢c0u1 s0u1 0 0 −r 0 u1 ⎢ ⎣s0u2 −c0u2 2lsu2 0 0 0 0 0 c0u2 s0u2 2lcu2 0 ⎛ ⎞ x˙ ⎜ y˙ ⎟ ⎜ ⎟ ⎤ ⎜ θ˙0 ⎟ ⎟ 0 0 0 ⎜ ⎜ θ˙u1 ⎟ ⎜ ⎟ 0 0 0⎥ ⎥ ⎜ψ˙ u1 ⎟ = 0, ⎟ 0 0 0⎦ ⎜ ⎜ θ˙ ⎟ u2 ⎟ −ru2 0 0 ⎜ ⎜ψ˙ ⎟ ⎜ u2 ⎟ ⎝ r˙ ⎠ u1 r˙u2 X su2 = # θu2 cu2 = θu2 s0w = # (θ0 + θw ) c0w = (θ0 + θw ) w ∈ {u1, u2}] ,64-;)H*& 01-9 + # # # # , " #

E # V q(t) = n = 9 # & ¡

MFCLD< ~~

E◦

~

# A(q) = l = 4 # # V # # m = n − l = 5 = # # ] +" # = V E

# # # #= # #

# , # , # , θ˙u1 θ˙u2 ψ˙ u1 ψ˙ u2 r˙u1 r˙u2 # , , V ] # V V r˙u1 r˙u2 # # V # ,E V ] # # #"

# E # = , , # # # # = , # # # #" θu1 , θu2 # # = ## #" ψu1 = ## #" #" ψu2 , = E ] # ~ # # # X ~ # # # G(q) # #" # Z # ⎧ x˙ = (θ0 + θu1 )ru1 η2 ⎪ ⎪ ⎪ ⎪ y ˙ = # (θ0 + θu1 )ru1 η2 ⎪ ⎪ ⎪˙ # (θu1 −θu2 ) ru1 ⎪ ⎪ θ 0 = ⎪ # θu2 2l η2 ⎪ ⎪ ˙ ⎪ ⎨ θu1 = η1 . Z[ ψ˙ u1 = η2 ⎪ ⎪ ˙ ⎪ = η θ u2 3 ⎪ ⎪ ⎪ # θ ru1 ⎪ η2 ⎪ ψ˙ u2 = # θu1 ⎪ u2 ru2 ⎪ ⎪ ⎪ r˙u1 = η4 ⎪ ⎩ r˙u2 = η5 + # X # # sθ2 = 0 # V #" , " # " # # ] < V #" Z[ # V #" = #" = , " # X

# # V = , = # # , # # V X\

" # #] = # # , # % # ] # #" #" E

# # # Z[ #" , , # = # , , , #" = ## #" = #" , # = = = ## #" = # ] # # ~ # # E , ? # , V , = ## #" = #" # # # , V # # # = ## #" = ] > , # V = E # , == # ~ V E # = ## #" = # V X\ ¤F = ## #" = ] # = # # , #" # E #" #" E = ¤F ]

:3 @04'(09 9P0(6'8; # ¤F , , = "# # #"

#" , ¡ ] # # #" ~ # = # , = = # # # # # ) " Y = # = # M ,

# (yc , sc )T ] > θc , #" #E " # #" # = = # P (sc ) #

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

& ' ' < Y Y cc (s) # #" = V V θ˙c (sc (t)) = cc (sc (t))s˙c (t).

ZX

V V V = s " E V # gc (s)

c˙c (s(t)) = gc (s(t))s(t). ˙

ZZ

# # = # # # , " # # #" # " V # # ] < ~ # = E Z[ , #" , , # , # ⎧ cθ −θc +θ ⎨ s˙c = 01−cc ycu1 ψ˙ u1 Z y˙ = sθ0 −θc +θu1 ψ˙ u1 ⎩ ˙c ˙ θ0 = σ ψu1 1 ( # θu1 − # θu2 θu1 ) #V σ = 2l

# , # # # #] + #" ¡ # # = , ψ˙ u1 = v # # # # = # # ] > # = # ~ E # Z , # = # ⎧ c −θc +θu1 s˙c = v θ01−c ⎪ c yc ⎪ ⎪ ⎪ y ˙ = vs ⎪ c θ −θ 0 c +θu1 ⎪ ⎪ ⎪ ˙0 = vσ ⎪ θ ⎪ ⎪ ⎪ ⎨ θ˙u1 = η ˙ (1−2lσ # θu1 )−2lσ˙ θu1 , ZY θ˙u2d = θu1(2lσ− # ⎪ θu1)2 + 2 θu1 ⎪ ⎪ ˙ ⎪ ˙ (1−2lσ # θu1 )−2lσ˙ θu1 ⎪ ⎪ − kθu2 (θu2 − θu2d ) θ = θu1(2lσ− # ⎪ θu1)2 + 2 θu1 ⎪ u2 ⎪ ⎪ ˙ ψ = v ⎪ ⎪ ⎩ ˙ u1 ψu2 = v (2lσ − # θu1 )2 + 2 θu1

η # σ˙ = # #E = # # " # V # # (sc , yc , θ0 , θu1 , θu2d , θu2 , ψu1 , ψu2 )T θu2d , #" # V , ³ kθu2 # #E# " V " #] % # " = = # ¡ # V #" # # # , # " # V # ] # # # # E # # # yc " #

E◦

~

#" θ = θ0 − θc , #" # , # , , # # # = #" # #" == V θd ] ) ,

#" # , V # E " = = # ¡ ] , # # ZY # , # Y ⎧ cθ+θu1 s˙ = v 1−c ⎪ ⎪ c yc ⎪ c ⎪ ⎪ y˙c = vsθ+θu1 ⎪ ⎪ cθ+θu1 ⎪ ⎪ ) θ˙ = v(σ − cc 1−c ⎪ ⎪ c yc ⎪ cθ+θu1 cθ+θu1 ⎪ ˙ ⎪ = v [y θ u1 c ⎪ 1−cc yc 1−cc yc (gc sθ+θu1 − kpy )+ ⎪ ⎪ ⎪ + s (c s ⎪ θ+θu1 c θ+θu1 − ⎪ cθ+θu1 ⎪ ⎪ − k ⎪ vy cθ+θu1 sign( 1−cc yc )) + cc ] − vσ ⎪ ⎪ ⎪ ⎪ θ˙u2d = θ˙u1 (1−2lσ # θu12)−2lσ˙2 θu1 ⎪ ⎪ (2lσ− # θu1) + θu1 ⎨ ˙θu2 = θ˙u1 (1−2lσ # θu12)−2lσ˙2 θu1 − kθ (θu2 − θu2d ) , u2 (2lσ− # θu1) + θu1 ⎪ ⎪ ˙ ⎪ ψ = v ⎪ u1 ⎪ ⎪ 2 2 ⎪ ⎪ ψ˙ u2 = v (2lσ ⎪ & − # θ'u1 ) + (θu1 ⎪ c c ⎪ θ+θ θ+θ u1 u1 ⎪ σ˙ = v 1−cc yc 1−cc yc −kpθ θ˜ + gc + ⎪ ⎪ ⎪ & ⎪ cθ+θu1 ⎪ ⎪ +σ ⎪ ⎪ 1−cc'yc yc gc cθ+θu1 + kpy sθ+θu1# + ⎪ $() ⎪ cθ+θu1 ⎪ ⎪ +s c − c + k s sign ⎪ θ+θu1 c θ+θu1 v θ+θ ⎪ ⎪ ' ( y # u1 $) 1−cc yc ⎪ c c ⎪ θ+θ θ+θ u1 ⎩ −kvθ σ − cc 1−ccu1 yc sign 1−cc yc Z = (θ + θu1 ) sθ+θu1 = cθ+θu1 # (θ + θu1 ) θ˜ = θ − θd # # E kθu2 , kpy , kvy , kpθ , kvθ # #E# " V " # # v # # = ## #" V ] # # ¡ " Z # yc # # # θ˜ = #V " ] # # # # " # X # , # Z , # # X ] # =E = #" # " Z # , # # ? = Z[ ] < # = X\ # # ? ? " # X ] ) # V E V = # , = # # E " # ? " V # E " # ] % = , # ¢ # # ] + = E = , # # #

# V # E

= == #" ]

>3 @0;=5'-( 6;59)H04& = # = = #E = V # == # = ] + # = = (xd (s), yd (s))T V # 5 ? V xd (s) = # 2s , Z\ yd (s) = (s + π4 ) # # ] # # V l = 0.1 # R = 0.03] " Z # #V " # ˜ = E (yc , θ) = ] ¤ " ¡

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

e 0.1

E◦

~



ex 10

20

30

40

ey50

0.6 t

1

0.4

2

0.2

0.1 0.2

0.2

10

20

30

40

50

t

0.4 0.6

0.3  1.0

 0.1

1

0.5

4 10

20

30

40

50

1

0.2

2

0.3 t

0.4 0.5

0.5

0.6 0.7

1.0  1.0

 

20

30

40

50

30

40

10.5 t

0.5

50

t

 1  2

11.0

5 10

20

11.5

2

0.5

10

10.0 9.5 9.0

1.0

& ' #' Z

0

10

20

y

30

40

50

t

1.0

= # # #" # # = = #" E # , # , = # # = , # ex = x − xd ey = y − yd ] + # = \ ] ) # = V E # kθu2 = 1 kpy = kvy = kpθ = kvθ = 500 #" θd = π2 − 0.4 V E v = 0.2 # # # # q(0) = = (x(0), y(0), θ0 (0), θu1 (0), φu1 (0), θu2 (0), φu2 (0)) (0, 0, − π2 , π2 − 0.4, 0, π2 − 0.4, 0)T ] % #" Z[ E Z # # #" # # # ¢ , # # # ) " # \] # " = # # , , = # (x, y)T # ] + # = = #" #V " V # V # V ] % , V # = # = = " # , #" # # # ? # # # = # ¢ # ~ # = V # #" # [

0.5

1.0

0.5

0.5

1.0

x

0.5

1.0

& ' ^' Z

H Y J

# # ¢ # # ] # #" # " # #V " # # V # , ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

= , # # "# #" # # # ¢ " # # ] " = = # V , # # == # # = , V ]

?3 @04*95&604 # = = # E , , ~ == ¤F E V # = # , V E = # # = # ] , # = = E # = ¤F , E , #" V , ] E # , # ¤F

# # ~ E V #

# ] E = = # = # X Z E , , # " # ] = # # " , # # = # ] " = # ] + # , # E , V # = # #" E = # ] > = # E # # = V # # = # , # " ] < # E # # , ] ) = , E # # ¢ # # , #V " E ] ) == , # V # E " , # ]

@,GFB "IN" "G = == , > U# V % # # # " # E = ? ]

G0'-& X = # # # # " ] Z # # " # #" , = ## #" # = ] # = # " #" # ] Y & # # # , # Z[ # # # ¡ # ¡ # # # # V , , # #E # # ]

MFCLD< ~~

E◦

~

"A" "G@" X Q] + #] © V # V # , V , ¢] QQQ %= ?iiT,ffbT2+i`mKXB222XQ`;f miQK iQMf `Q#QiB+bf/BvfvQmp2@M2p2`@b22M@ @/`Bp2@ bvbi2K@HBF2 @i?Bb@#27Q`2 ( Z[XX] Z ] # ] ¤ = " , V ¤F ¢] ?iiT,ffrrrX i2+?+2Mi2`XBMfkyRRfydf?2KBbT?2`B+ H@ ;BK# H2/@r?22H@/`Bp2X?iKH] ] ] > ] # # ] % # ] ( & ' %= #" E£ " < © # ] % <( U%+ X \] Y &] ( # ] +# ¤F , E , , V ¢] > U# E V % # # # " Z[X ] # & E ] &] ( # # 6] #± ] V ¤F , , ¢] # ] #± # ] #± ] 0 & ' V E X[ XXY] F # > # & E # > ? Z[X\] # & ] \ > ¢] ?iiT,ffrrrXrQH7` KX +QKfK i?2K iB+ f] ¡ +] # ] % #] ? #" # E = # E #"E E , , ¢] # 6 = Z[ ¡ # < E # 9 6 Q# # ~ Q Q# + E ~ X ] ] 6 , #± ] , , ¤F ¢] > U# V % # # # " Z[XX] # & ] ] % # & #" # EV #" , , # , E , ¢] # $ +* * & ' * /8 V ] X X Z X]X X] ] X[ U# # # ¤ = V = ¢ < ( * > * V ] # ] X Z ¡ & V , # # ?iiT, ff#HQ;XKQ/2`MK2+? MBtX+QKf?2KBbT?2`2@ /`Bp2@bT22/bi2`f] XX > = ] ¤ " ¢] ?iiT,ff2MX rBFBT2/B XQ`;frBFBf>2KBbT?2`B+ Hn QKMB/B`2+iBQM Hn;BK# H2/nr?22H]

D EF ) [ C = ) # E = % # U# V > ] ) E ( E XEX ¡ > & # E

=] ]? # §" ] ] ( $ ? = ∗ 9 = # , # # 6 , > U# V % # # # " ] ( # " XX¨X¡ [E ¡Z > E & # E , ] # §= ] ]= ]= ] ]= ] ∗

= # #"

X

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

MFCLD< ~~

E◦

~

% = + + ;L H + 6 +

5 + + N #

£¢ £ ǰ £ Ú ǰ R £ ǰ ǰ Ù ¿ ǰ £ ǰ £

ǰ £ ǰ ǰ Ù Ú £¢ ǰ £ £¿ï Ȭ Ú £¢ !" "#$ %&'(' )#' %&'()*'+ $ " 0 " 2 " ! ! " " 0 "

$ ! ) 9 ) 9 " G O ' ) 2 " Q 7 ! 1 ( " ! ' " ! * $ " " 6 * ! " 7 " " ,-./0(1&+ 0 " 4! '

23 4'(015*H04 # # = #" , # # ZZ Z Z\ ] # # , , E == # # V , # " #" #E # #" ] < # = # # , " Y[ ZZ , # , ZY X # Z[ = # # \ # # ZX , == # # # E , E= Y = = # # # # #" , # = ] ) [

, == # # " = # E # , # # , E \ ¡ ] 9 #" # , E # # = # # E # " = # # ] 6 , #" #" # = # , ] V = # # " ¨# ¨= # # # #E #" , # = ] # = = # # , # # = = = # # ] # = # = # = # " #E # V V E # # #" ] "# # # = , > ¤ F " # # # Z

" = = ] # # E V # #" = E , # = = " E " ] "# # # , " # = # = " #" ] V = = = V " = " , # # ] + , # # # # V , V # = E # # # # " = # # # # # ] #" # # # 6 9 = E ? ] , # " E , , # # E "# V V # , # V = ] + E " , # # = Y # # " = <%X$& 9 YX E = " = # <%| X YZ , # # " = V = # # = # # =E = # "# ] = # # = , 6 9 E # # # # # E # # " = # #] < E = # # #" = # # V , # # ] # % # Z # =

] < # % # , = # # = # # # ] # % # Y # " = # ] ) # # % # = # = = # # # #E " V # V , # ] #E # # # % # \] F " # # # # # V "

£E # " V = #" E # , # # £9 ZZ[\] " # = ? 6 E 9 ]

73 8- .&'-; 1-) # #" 6 9 = ? = E # # Z # ] # = # = # , E # # # # # # #] + # # # E # , V # # # V = # E # # # # " = # #] # # = # # V # # = = E

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

#] ~ # # # # ) "] X] + # = , 6 9 # # E = # # = = # ] F# "# # # = ) "] X # , = # # "#

= # # ] F# , ) "] X , = ## # #" = E # = # ] , ~ =E = # V 9 ## EV E # #] = , = # = # " # = = ] # = # ,

# = # = = # # # " #" #" # # = # E #" # , = #"]

,

MFCLD< ~~

E◦

~

] + #" E ~ # # # , = # # #" # , # E , = # V ] ~ # V , # # V , # # #E , # = = 6 9 , # = # # # = # ] & ~ # # = # = V # , # = ## ] ) = E = # #

E "# , # = # E "# E # # # E = = # , = E # = # = # V E #? ] # = # = # E # # # 6 9 ] E # , # # # # , ]

:3 - -I6 .&'-; @0;=04-4'& & ' "' > W

~ Y $ + Y ~ Y % + ~ Y Y '

# # = # = V , V = #

# , # V # # # # == # # V # #] # #" , , = # # = # # # ## , # , ]"] #¨ E , # V " # , = E

# # # # # = # ]"] = = # #" , = # # # # = # ] + , " # #" = ? # = E , # # V E , " # = # ] V # ~ # = ] % = E # # Z X ] # X # " E # , V = # 6 9 , # # == # # E ] Z = # " " # #" # # = V ] = = == # # #

# ~ # # #E # V = E = V # , V # # # V # # #" " = E # #" = # V ~ ## E

+ # , # ~ # ~ # # =

, # # , # = # # # 6 9 = ? = , V = ] , # # # # = # # # #" = = # # ] # E , # , , # = E # # # # # # # #] = # # = # # # [ ] , = # # "# # # X $ # 6 9 , = # ] # # ) "] X # # " = # #" " 6 9 # ) "] Z] " # ) "] Z # V % 5 # # , " V = # =E # "# = == 6 9 [ ] 6 9 , # # = #" = # # , # # # = # #

# # = " = # V E # # #" E # # " # , # # = # # #" #" , , # E # = = # # , , ] Q # # = # # # , # #" , " = , # ) "] Z] 6 9 , # # ) "] ] "# # # # # ) "] Y] " = # ) "] Z] # # = V #" , E V 9 V # " # ¨V E " = # V E # # #" "

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

MFCLD< ~~•

Eâ—Ś

‡~„

DiagUI Control System

ReMeDi Robot Control System

Force/Torque Sensor

Palpation Effector Sensors

Telepresence module

Telepresence module

Assistant GDiagUI

Emergency button

Dead man Switch

Power System User Interface

Head/Gaze Tracker

Keyboard, 3D Mouse

Haptic Interface Control System

Power System Unit

Palpation Effector Control System

Audio System

Camera

3D Vision System

DiagUI Interface and Decision Maker

Robot CentralControl System

Arm Control System

Palp. effector Drive Unit

Microphones

Kinect

Stethoscope

Patient Body Pose Estimation Acquisition and Management of Vision and Audio Signals

Docking Station

Assistant Interface (Touchscreen)

Patient Site

USG Control Panel Leap Sensor

Palpation Interface Control System

GDiagUI Palpation Interface Sensors

Arm Drive Unit

Pain Level Estimator

Visualisation/ Tomography Map p

Palpation Interface Drive Unit

Assistant PBs and Indicators

Perception p Module Point Cloud Preprocessing

Haptic Interface Drive Unit

Laser sensors

USG Device

Emergency Button

Kinect

Mobile Platform Control & Navigation System

Screen

Bumper

Head Control System

Foot Switches

Proximity sensors

Blood preasure, temperature sensor

Head Drive Unit

LoudSpeakers

Platform Drive & Locking Unit

Cameras

USG Probe

Diagnostician Site

& ' ' @ / = Y W % Â? ÂŽ

, # , # == # = # = = #] :323 )46=59)'0(

& ' ' / = $ % X = # # "# # # #

, = # # # E ]

& ' ' ˆS$ % = # # 6 9 , ] ? # # 6 , ] ‚ , # # E = # == # , ] = # " # = = # # –ZÂĄÂ˜] ‚# # = # # Â? # # # # , # –Z˜ # – Â&#x;˜] # # Â&#x;Y

+ # = = # # #" # "# #‘ ] # "# #‘ # # # V "# # , # #V # # # # ] > # 6 9 , # = # E , "# #" " , E V # # = –XÂ&#x;˜ + 6Q+ ƒ = # , # 5 , # = # # # 6 E 9 = ? „] F# " # " # Â?= # " % +6+ = # = # ] ‚ \ 9F) # "# = = #] % " E # Âœ[ #"] #" V # ¥— ] # =

# X[[ < ] #E # , V # # #" # = , # = # = = , # E ] , # # = # ] # = "# # Â? E # # # # ) "] — # , + 6Q+ # " # # "# " # ] Â? # Y[ <] # = #E # ÂĄ 9F) ƒ) "] —ƒ,„„ = ? # ] #" = , #" = = #] # "# = , # # = = , = #" V # = , # # = = #E #] E # = # # == # # = = # # , = "# V , + 6Q+] # , E # ( # # # , + 6Q+] &U = # #" %  Z)Y # ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

,

& ' #' / = $

% Y % Y H SJ

" ( # # , = # # % # Y]X] 9 = # # " E , , # ( # # # & = ] == # " ,

# = # # V , # #E = # # # # = # # Z ] # # & " " U9& = ] , # % # Y]X] + E ~ ¨ # , E # ] + # # = = # = # # # ) "] ] = # = # = E # V V , Z ] " = #E "# # V # = E # # #V # # ] # # # # #" # = # # # Z ] , V # # # = # E #" # # = ] # #" # # E # # # = ] # # # # " # # #E V # = # # # = , # " E # # = = #" #" = # # # # "# # V "

V , #" # #] # # # # # # # = # ~ E , ] , = # ) "] E # #" #" # # = , ] # # , # "# #" # , # # # = # = E , # = = , ]

MFCLD< ~~

E◦

~

= # # # , "# E # # , V # # # # V ] % # " V = , = # # = # ] ,

& ' ^' / = Y $ W Y

% / C+ % Y %

X W Y H SJ # # ) "] \ # , # # ) "] # # E # # = # V # Q9 # # E ] + = ) "] \ , ? , E = # ] V V Z 9F) # # = # # # # ] #E " = # V # ~ #E # # # #"] # #E V # # # # #" "# # V #" ? # ) "] \ # # ) "] Y ] , =

V # # #" "# = # ] + = = V # # #" 6 E 9 # # ) "] ¡ # # X¡ ]

,

:373 E-)1 )41 61-0 @04<-(-4*64P .&'-;

& ' q' / = W $ % Y %

+ 6 9 , " V

# # #" # = # , E # # = # , E # 9 "U " # ,

# = " E ) "] ¡ = # ) "] ¡ , # # # ) "] ¡ = V ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

8 8 / ) "] ¡ , = # # " # # V ## ] V V * ( 8 / # # # # # ## # # = = , ] +# # # # # E = # # " # V ## ] = E # 8 / ) "] ¡ ] # # V # # #" = , E = = " # = E # "# # " # #" # = ] # # # # E V # # #" # = # # ~ "# # ~ , = # # ] # E 8 / == = E = , #" ] # # V # # # # #" = E # # # # # =

= , , # , ] # = , = # # E # # , ] # = " E # = # #" # = E # #] * 8 / ) "] ¡ = ] # # = # # # # ] V # # " E # V V # 8 8 /] # # # # V = # V # , # # , ] = " V 9 "U , # ] V E = # # = # # V # ~ # ¨ # = , , # , # E # ] V = # , Å # % X][ , ] :3:3 0%69- )&> = ~ = # V , = ## # = V # # ~ # # E = # #" , # #V V = ## ] ~ # , ~ == E= = , , ) "] ) "] # ) "] Z #" # E # E # V # ] # " ~ # X # #

# # #

E V #" , # , = #" # # E #

(* / E = # #" , # = # # = = # = , # , #" , , ( * / ] = = , # # E = # ) "] ] = , # # # % # ]Z \

MFCLD< ~~

E◦

~

& ' z' / =

,

& ' {' =

$ %

# # ? ] + #E # # , # = # # , # # , V # # E # #

] # # = # # # V , # ] (* / # # = # #" = = # # = # V E #" # = # , ~ =E # # ] =

# # = V ] # E # # = V # " ? # , # = # = ) "] ] # (* / = # #" = # ~ ] # #" # , # E # # , V , # = = ] # = # ~ # # E = ## #V V # # = # # # #

# #" " # # = , = # #" # #" ) "] ,

#" = # " # ] = # # E = # = # # = E

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

V # ] = = ## #" # 6F% ;HQ# HnTH MM2` , ] E # # ~ # # ‘ # E V # # #

,

# , = #" # # ] == #

#" = # # = = = # = # Â? # # = V # # = # ] "# = V V = V = # = = # –Xœ˜ # –X[˜ # # #ÂŒ " # #E # ] # = = 6 9 = E # , , ] # # = ÂĄ[Ă—ÂĄ[ח[ ] ‚ Â? V X]X ¨ ] " = Y[ " , = Y— "] = V # ~ == = # # # ] E = # # " # # , # # " # ] # # ## ¤ U% EX[5ÂŤ = " # # , #" #  \[ # # # E , , # # # , ] ## "# # # , E #] # # # # = = , E #" = = # , # V , ## ƒ , V ## #" = # „] E # # = V E #

= , #" # ] # = ,

# ( # # ƒ # E , + 6Q+„ # # ‚ ÂŤ = ] ÂŒ E ƒ V # E „] # # V " # # # , # 6F% ] V # , Â? # # # , E #] V # # ## ] ‚ # E ƒ ) "] X[„] # #

# #" Π# ] , # # , # #"

V ] V , #E # # = = # #" Π# ]

,

MFCLD< ~~•

Eâ—Ś

‡~„

# # V E , # –XY˜] ‚ V = # # Â?= # = # # –X— X\˜] # Â?E = = # ] # , , , "" # #" ÂŒ # #" E # V , % # =] # V # = # # ) "] X[, —ZחZ ] # = = # # –œ˜] :3>3 5=-(J6&0(. @04'(099-(& 0< - -I6 0%0' )41 I6! )PD .&'-; @0;=04-4'& , V 6 9 = V , " E V # 6 , # #E % ƒ6 %„ # 9 "U‚ 9 # ƒ99 „ ƒ ) "] Z„] # 6 9 , # 9 "U‚ = V # 6 % = 99 ] ‚# = E #V V # # # , E # #" # = , , # = Â? E # # = #" ] 0%0' @-4'()9 @04'(09 .&'-; 6 , # # % ƒ ) "] Z„ = V 6 9 , ] # # "# " E # # # # "# V = # # 6 % E # # , =

# # = = # # = ] # = # , "# 6 %ÂŻ% #= "# E V # ] % # 6 % "# # # ƒ ) "] XX„ =

# # 6 8 ƒ # , ? =

#„ + ƒ, #" � #" # = #„ 6 * ƒ � #" # #„ # _ ƒ V # " # , # =

# E V = # # # „] [EMG_OK && Fault_CLR] Startup

[Trans2Failure] 2 [PowerManager_Status==POWER_ON&&... EMG_OK && Arm_Status==READY &&‌ MobilePlatform_Status==READY &&‌ Head_Status==READY &&‌ PalpationEffector_Status==READY &&‌ PerceptionModule_Status==READY]

Active

Failure [Trans2Failure]

[PowerManager_Status==POWER_OFF]

[PowerManager_Status==POWER_OFF] Shutdown

Trans2Failure= ~EMG_OK||Arm_Status==FAILURE||Head_Status==FAILURE||‌ MobilePlatform_Status==FAILURE||PalpationEffector_Status==FAILURE||‌ PerceptionModule_Status==FAILURE]

& ' " ' = W $ % = = # = # E

& ' ""' /ZOO$ Q 6 % ÂŒ # # # # # = # # ) "] XX] Q Â&#x;ÂĄ

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

= # , V , ] +

= # # = Â? E Â? # # = # V ] % , +

# # � # # = = Œ , #" = # V # ] # , #" , # # ƒ # # % #  ]Z„ # = # ~ , # # # = # ] = # 6 % # , # # – [˜ ] I6)PD I-*6&604 )C-( 9 "U‚ 9 # ƒ ) "] Z„ Œ # # ƒ)% „] #

99 = V # # # = E V 9 "U‚ == #] 99 "# # # , #" % )

, Â?] ) # " Â?= ­­ , # # F6F F% = # # ] 99 = # # ) "] XZ] ÂŒ # # # #

$ & 6 * # _ ] 6 ## #" # # = # = # E V 6 /& ‘ %Q ( _ & * ‘ 6 # $ %Q /]

[Initialize2Shutdown]

[Shutdown2Initialize]

[Initialize2Failure]

[Initialize2Running]

Failure [Running2Failure]

[Running2Shutdown] [Failure2Shutdown] Shutdown

& ' " ' =$ Q 99 # = ] V E #" # " # , # # # # # # #" # 99 = # # # = Â? # # # 6 9 , # 9 "U‚ = # # ] #= 99 ## ? 9 "U‚ = # # # 6 %] E " # # , # = V , E V ] 99 " = # " # = # V ] # ÂŒ # # $ # 99 , 9 "U‚ = # # #" =] + # E # # # Â? ,E 6 /& ‘ & ] ‚# 99 #" # , E = # # , = # ] > # # # ÂŒ 99 & ¨ & ( Â&#x;Â&#x;

Eâ—Ś

‡~„

* ] = # E #" 99 = # # # & ¨ 6 ] ‚# 6 9 , # 9 "U‚ # , = ƒ, # = „] > # = = , ƒ ] ] 6 9 = „ 99 & ¨ $ %Q /] # E Â? # # # ] > # = ƒ ] ] 6 9 = „ 99 #" & ¨ 6 ] 6 9 # = # =

Â? # # # #" # # # E " = ] #" #" , # = Â?E # # # # # 9 "U‚ E & ¨ 6 /& ‘ & ¨ & ( * & ¨ 6 # & ¨ $ %Q /] + #" # #ÂŒ " # ƒ = , #" == V #" # " = #E , „ 99 # & ¨ 6 /& ‘ ] 9 "U‚ # # ~ = # # , # ~ , # E Â? # # = ]

>3 4'-P()'-1 - -I6 &.&'-; % = # # = # % #  V , # # " # # V ] ‚# # Œ E , # , # " # # # � = # # " 6 9 # � # # # #]

Initialize

Running

MFCLD< ~~•

>323 4'-P()H04 <();-/0(C ) V = = # V 6 9 E # % # Â " # E , , = # = E # # = # # = # E ] = # # V # = Â? E = # ~ # = # ] V = # # , #" # = # # V = , #V V = E # # , # # ,

# ] ) # # " E # 6 9 = , # " V # V == = # # +== E #" , V = # –XZ˜ == == ] ) = = # " # 6 9 , # , , # E = # # = # , = E # # ] 6 = # # Œ = = # # ƒ „ V = , # V = E # ] < , E = "# = E #" # # =E # == == 6 9 # # = Œ # # % #  # = # ) "] Z] = # # , , � Œ # # E # # # ] +, = # # Œ # # ] %= E Œ # , = # # # # E Œ # # # ƒ # # ,

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

# , = # # # # # #E # #] QV # = # # = E # = ©+ 5 # # = E # = ] ©+ 5 # E , # , = V # E # " # E # " ) "] X XY ] V

& ' " ' ? / = W H & ' J

MFCLD< ~~

E◦

~

6F% # F6F F% , E = #" E 5 # ­« # ) 6 F% == #" E # # 6 # # #E V V #" # # & E , V ] = # , ~ == # # , # # # V # # # # # # , , V # , =

# = ~ # " #" # # E # # # ] E = = " V # E ~ " = # = # # , V = E #" V # # "# # # #" # # #] ¤ V = # # , " ] # 6 9 # & = #" = E # , # ] 9 "U ,

# & = # = # V ] # 6 9 , V

& = # # #

7 # # & * XX ] ( # # E "] X\ # "] X¡ " = # = , E V # # 55 # # = XZ 9 E V # == # # # = # # Z # E = V ]

& ' " ' ? > H

W % O Y ' O Y ' J

END-SWITCH

BUMPER

EMERGENCY BUTTON CIRCUIT

& ' "^' Z

ACCREA PAWER UNIT

GPIO

UART

SPI

RMII

TIMER 1

MAX3097

TIMER 2

MAX3097

TIMER 3

MAX3097

TIMER 4

MAX3097

GPIO

SPI

PROGRAMMER USB

DC INCREMENTAL MOTOR ENCODER

DC DRIVE 1

UART<-->RS485

ETHERNET RMII<-->PHY

©+ 5 # , , # # # # " , # V # # ] # ~ # = E # = # # # , # , # = = # = V , = V # # # , #" ] 6 9 # " #

# # # # #V # # V = # , E # ] " # # = # E " #

= # # ) "] X ]

#E

STM32F1

ENCODER INTRFACE MODULE 1

SWD

STM32F407VG MMC

JOINTS CONTROLLER

& ' "q' Z

+ Y

W

& ' "#' ? Y

# " ,

+ # " 55 , # = # # # # % Z)Y[¡£ = , X # # # , V # " ~ # X ¤ ³ Z # # " E V # " Q # ³ E # # 9 V = # " U+6 ³

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

# Y ~ #" # # " %& ] & * "# , V ) "] X ]

,

& ' "z' /? $ % # # V E = # , # # ] , E " = # ) "] X

MFCLD< ~~

E◦

~

V V == , = # # # == # # 6 9 , ] & # # E # # , F6F F% Z] Y # 6F% Y\ # " ¤ , 5 # U, # XZ][Y XY][Y E #E # 5 # # « # Z]\]Y Y ] E , # == # E E V ¡ E ~ E # (6 Y # # # ¨F % # \Z\ X # V [ ] F6F F% "# E = # # = # # V

# # E " ] 6F% #V " = E # # #" ] # # #E " # 6F% # # # E # == # # , "# # E = # ] # F6F F% , # " ] # , # = V # = V , == # ] % # ) 6 F% YY ##

= # E = E #" , V E

#" == # ] E E = # # # , # # " 6 E # Y¡ # # = # # # Q # # ] >373 4'-P()'-1 - -I6 &.&'-;

& ' "{' ? /? # # # 6 # V # # , V # #] , = V #" # V ¨F =

Y # " #= Z # " = Z " #E = Z " = Z ~ # # 6 # # # # ] , E = = == # # = # # , # E ] # # & * V # , , = = # # # == = E #] 9 = # #" # V E , # # # V # , E #" #" # V # E ] & * # 6 9 ==E # " # # # V " # # = # #, ] & * # 7 , V # 6 9 = ? # == # " #

) "] X V = # # # " # 6 9 , E ] 7 , # #

9 V # # = # # V # "# E E #" # = # # ] & * # = # # , # V , " , # & # V # ] # #" "# = = V # , = # " # # # QU = ? U6F # 5 6Q ] ¤ E [

,

& ' ' S / = W H

~ Y Y $ S% / = # " 6 9 # ) "] Z[] ) "] Z[ 9 "U # " U# V 5 , # " = # #] # # ) "] Z[ , = # , # E # # = # # # = V E # # # ] #" 9 " E #" " # # # " # = E # = , # = = # ]

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

" ~ # , = V # # = # = , ] # # , " # ] , # # # # , # = , # = # , ] V # # #" # == # " # # , = # # # # ] = # = , # ¨ # ] = , # =E # ] V # # E V # = , ] # #" # # = , = , = # ] & = = E # # = # # ] > # " , # # = , , ~ # , ]

?3 D&-(&d -J)95)H04 6 9 = # # % # Y]Z # = # V # V V # E # #" #" U# V 5 , # # & # ] + # " = ? V E # , # # % # Z] = # # V , # V E # , , X " = # # # = Y ] # , # E = # # = = V V %U% # = ~ ## ] E V # # , # , # V # # ] # V #

¢ # E ¢ = ] V # # , , = = E V # V # # #" == E # # # = ] # " # = # # # , # = 6 9 , E # # V , = # # #" # = # ] ¤ V = E = # = # # , = = # # # # # V # 6 9 ] # 6 9 , V V , # # = # ] £ = # # V , " V # # " # # # #" # #" # #V V # = ? ] =

# " #E " # # = V = # # # E # # # = V # = # V E = ## ] % # # = #

# "# # # E # = # , # #]

MFCLD< ~~

E◦

~

S3 @04*95&604& = # # = , # # 6 9 # E V = = # #] # # V , # #

= # # # # " = E # # # ] # V # E # V , V # # # # # #] # = E = # #" #"] 9 V = # 6 9 , E = # "

= # " #" " #" = E # ~ # # # V # = ]

@,GFB "IN" "G == , Q = # E # )&¡ E\X[ [Z = ? 6 9 6 E 9 "# # # E # # , & #

% # # ¤ " Q #

D EF

$ $ ∗ 9 = # , # # 6 , > U# V % # # # E " ] > , Ä > = #± " Z¡ [E ¡[ > E & # E ] # §= ] ]= ] ( $ 2% ) ? = 9 = # E , # # 6 , > U# V % # # # " ] > , Ä > = E #± " Z¡ [E ¡[ > & # E

] # §= ] ]= ] ^ = $ 2% 6 = 9 = # , # # 6 , > U# V % # # # E " ] > , Ä > = #± " Z¡ [E ¡[ > E & # E ] ?# §= ] ]= ] . 6 % = 9 = # , # # 6 , > U# V % # # # E " ] > , Ä > = #± " Z¡ [E ¡[ > E & # E ? ] §= ] ]= ] ] ]= ] ( % [ ) A 9 = # , # # 6 E , > U# V % # # # " ] > , Ä > = #± " Z¡ [E ¡[ > & E # E ] " §= ] ]= ] C $ C = = 9 = # , # # 6 E , > U# V % # # # " ] > , Ä > = #± " Z¡ [E ¡[ > & E # E ? # ]? , §= ] ]= ] ( $ C = 9 = # , # # 6 E , > U# V % # # # " ] > , Ä > = #± " Z¡ [E ¡[ > & E # E ]? # §= ] ]= ] $ 9 = # , # # 6 , > U# V % # # # E " ] > , Ä > = #± " Z¡ [E ¡[ > E & # E , " #] §= ] ]= ] = + # # " 9 E = # 5 , # U# V # " ] < , Z[E\X 5 , # & # E

] # §= ,]= ] X

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

[ 6 ? $ = + 6Q+ Q#" # #" ] ¤ E # Z[ Z[EXY 5 , # & # E , #E § ] ] ] ] $ $A_ = ? $ = 9 = # " U# V 5 , # ] ] ( " Z[EXY 5 , # & # E

] # # § ,]= ] ∗

= # #"

"A" "G@" X ] + # (] ( , ] 9 '" ] #± E ] % #, " ] # ] " 9] % # E% # ] ( # >] E # +] > #± # , , # # 9 "# # = # , = # ¢] # 0

* " $ > / 6 / $ `>6${ Z[X\ ¡\ Z X[]XX[ ¨¤% ]Z[X\]¡ Z \XZ] Z ] +] +V # )] + # (] ] ( # E £ " # Q] 6 +# = " V , ¢] # 0

* $%%% % 8 < * 6 /8 / Z[X Z Z Z ¡]

MFCLD< ~~

~

0

* 3! $ ( < * * < * + / * & ' `<<+&{ Z[X\ XX ¡ XXYZ X[]XX[ ¨ +6]Z[X\]¡ ¡ Z ] X[ (] ( , ] 9 '" # ] % #± # # = = # 6 9 , , = ¢] # 0

* 34 $ ( < * * < * + ( / * & ' Z[X ] XX ] ( # ] 0 ' / ' V X =E 6 # = # # # " # " ± E = ' E= " " ¡ [] F # > # & # > ? Z[X\] XZ ] ( # # ] #± # % + E #V " # # # + " # # Q = , & 6 ¢ 9 0 + * / 6 6 V ] \ # ] Z[X \\¡ \¡ ] X ] # ] # 6 , # , X¢ + / %

( V ] # ] X Z[XX] XY

] U # #" # =E = # " "# #¢] # 0 (

* # $ > / 6 / $ `>6${ Z[X XYX XY¡ X[]XX[ ¨¤% ]Z[X ]¡X¡[\ ¡]

Y (] ] (] 5] % " +] ] 9 # 6] )] > # "# # = # = E

# # = = = # + E V ¢ 7 & ' & X ( 8/ V ] [ # ] Z[X \X ]

X

] & # # E #" # V #¢] # 0

* 3! $ ( < * * < * + / * & ' `<<+&{ Z[X\ X[¡\ X[ X]

(] "# E 9] ] 6 # ¤] E # # , == E # + V ¢ $%%% & ' V ] X # ] \ Z[X XZ\X XZ [ X[]XX[ ¨ 6F]Z[X ]ZY [[]

X\

] ] 0 ' / ' V Z

0 \ 0 Z % = 9 # # V E # # # "# ZE9 [ XZ] F # > # & # % = E , Z[X\] # & ]

X¡

] )] 9] % ' ± # E % #± ] + # # ] % #± £

# # #" == # , ¢ 7 < * $/ * > $ / V ] Z[X X X[]

6] +] 6 , # % # 6 9 # ¢ 7 & ' V ] Z[XZ Z[XZ X[]XX ¨Z[XZ¨Y[X\X ]

\ Q] 9 " #" 5] U , # &] # %] (] ± = (]E5] "# # )] & # Q] 9 E , ¤ == # # # , == # ¢] # 0

*

$%%%5&67 $ $ & ' * 6 / V ] Z X \Y] ¡ >] 9 ] 0 ^8 ' V X = & == = #" V ~ # # E = #" 5 # « # X X[[] F # > E # & # > ? Z[X\]

X +] # ] % # # ] # # = E V = # "# ¢] # 0

( * $ < ( * & ' [ 6 + `<%& ( 6 +34! { Z[XY ZXY ZX ]

] ] (] ? ] # # ] E , # 6] £ "# # # ] ] ] #E # F , 6 , = " # E , = < #E # V # # , # #"¢] # 0

* @ $ ( 9 / * 9 / * ( %

Z[XX X ]

X +] # ] 0 ' / 8 $ Z % Z = U # = * , #* # =' Ä # ] & E # 5 , Z[X ]

(] ( , ] 9 '" # +] # 9 V = # , = = # , ¢] #

Z

E◦

Z[ ] == # ] # # £] 6 # ] E ] $ & ' * +88 =E % % # < V " # F = E 6 , F F6F \¡] %= #" Z[XX] ZX ] %] ¤ # ¤] < ©] ©] % E %] # %] ¤ 9 V = #

AgmjfYd g^ 8mlgeYlagf DgZad] IgZgla[k ¬ @fl]dda_]fl Jqkl]ek

# #"E # , , # # # ¢] # 0

*

$%%%5&67 $ $ & ' * 6 / Z[X[] ZZ ] < ] 0 ^8 ' Z 0 / / ( * / ' = & E , " # Z¡ Z Y] > # # ? * # ± Z[[ ] Z ] < ] < * & ' & 8 = 6 E , # & ¡ X\] % 6 Z[XZ] ZY ] < ] ? # 6] 6 # + # E , = # E

= V = ¢ < * ( * 9 %

* /8 V ] Y Z[[¡ ¡ [[] Z +] & ] ] % # 9] % # E % # >] +] > # ] " ] +] +V # Q] 6 5] V

+] ) ] + # (] ( , # ] ( # "# # # #¢] # \ Q < * << &3! Z[XY] Z\ 5] & ' & ' / * Z 9 * > # < E # # > Z[X[] Z¡ Q] 6 +] ) == )] (] ] ( # # ] +] +V # Q# # = " #E # # #¢] #

0

* $%%% 6 /8 / ~X $ ( Z[X X¡ X [] Z ] % ] +] +V # +] ) &] = # ] " = # #E EV #" #" V V , ¢] #

0

* &67 $

$ & ' * 6 / Z[[ Y \ Y\[Z] Z

] % # 5] % V 5] £ # # ] F E

& ' Z < * [ 0 * %= #" E£ " 5 # # Z[[ ]

MFCLD< ~~

E◦

~

] % #, " ] # <] # " ] E " ] + # ]

" # )] 9] % # E% # ] 6 # +] > # 9 "# #" # # " = + E # E # = # # # #" , ¢] # 0

* $%%% $ 6 /8 / & ' * > / $ // `& (<+\ 34!?{ < © U%+ Z[X\] Y 9] % ' ± # E% #± ] % #± 6] E >] 6] # ] ( E # ] & ? = ± E # = = # " ,

# ? # # ?# ? "# # ?¢] ?iiT,ff+QM72`2M+2XK2/B+ H`Q#QibXTHfrT@ +QMi2MifmTHQ /bfkyRefyRf`272` iny9XT/7 Z[X\] 6] ¤] +] # ] ) #" # &] 9 E ] > *' & ' = 6 , E # = E # " % " ] %= #" Z[[ ] \ ] U ] ) # # E # (] ] & 6] +## )] % # V # ] " E # ] 0 ' / ' = + , V # = E, , # E V #"] F # > # & # > ? Z[X[] ¡ +] £ (] &] #~ # ] # )] & + # , E " = ¢ $%%% & ' * + / V ] X # ] Z[[ ZZ Z\] (] ] (] £ " ] +] +V # Q] 6 # ] " + = #E # # V , ¢] # 0

*

$%%% % 8 < * 6 /8 / Z[X Y\Z Y\ ] 6 9 E = ? , ¢] ?iiT,ff rrrX`2K2/B@T`QD2+iX2mf]

] % #± +] # # ] + # 5 " E "# , E ¢] # 0

* 3! $ ( < * * < * + ( / * & ' `<<+&{ Z[X\ Z[[ Z[ X[]XX[ ¨ +6]Z[X\]¡ ¡ X ]

Y[ £ # % " % # , ¢] ?iiT,ffrrrXBMimBiBp2bm`;B+ HX +QKfT`Q/m+ibf/ pBM+Bnbm`;B+ Hnbvbi2Kf Z[XY]

X ] % #, " ] Q] ] # ] " 9] % # E% # ] ( # E >] 6] ] # +] > # 6 , # ¢] # 0

* $ ( > / 6 / $ Z[XY Z¡\ Z X]

YZ # , ]¢] ?iiT,ff ?iiT,ffrrrX /2+?Qi2+?X+QKfT`Q/m+ibf]

[

Z ] % #, " ] ] # ] " 9] % # E% # ] ( # >] E # +] > # = #" = = # , # ¢] # 0

* + & ( ' 8 34! Z[XY YZ Y\]

YX , # ?iiT,ffrrrXK2/B`Q#Xb2]

, ]¢]

Y £ 6 , ¢] ?iiT,ffrrrXp;Q+QKX+QKf] YY _

& 6 #" # ]¢] rrrX7`22`iQbXQ`;]

?iiT,ff

Y & 6 ( 8 & ' 6 ¢] ?iiT, ffrrrXQ`Q+QbXQ`;] Y\ & 6 ( & ' 8 6 /¢] ?iiT,ffrrrX `QbXQ`;] Y¡ & ( > * & ( / \ & ( / | Q¢] ?iiT,ffrrrX`iM2iXQ`;]

AgmjfYd g^ 8mlgeYlagf• DgZad] IgZgla[k  @fl]dda_]fl Jqkl]ek

–YÂ&#x;˜ ™ˆ / ( & ( / _ / | Q¢] ?iiT,ffrrrXt2MQK BXQ`;] –Yœ˜ ™(6 E+Â? 5 # " ¢] ?iiT, ffrrrXD`jX+QKf] –—[˜ < $ X+Â’ < (_ 6 < ] –—X˜ $ / [ 6 < * ?3?[ 0 $ < $5‰ 9 * ( # Z[[Y] ?iiT,ffrrrXb2MbQ` vX+QKf/QrMHQ /bf K MnekenRXyX8XT/7]

ÂœY

MFCLD< ~~•

Eâ—Ś

‡~„

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

N° 2

2017

7I e @ Submitted: 18th January 2017; accepted: 18th February 2017

Š”ž‹ȹ •Žïǰȹ Š—ȹ ’—Â?Â›ÂŠÂŒÂ”Â’Ç°Čą Â˜Â–ÂŠÂœÂŁČą ¢Â‹ÂžÂœÇ°ČąRÂžÂ”ÂŠÂœÂŁČą ÂżÄŒ¢Â”Ç°Čą ›£Ž–¢ÂœĂ™ÂŠ ȹ ÂŠÂœÂŁÂ”Â’ÂŽ Â’ÂŒÂŁÇ°Čą ŠÂ?Â˜ÂœĂ™ÂŠ ȹ ˜Œ£¢Â?Ă™Â˜ ÂœÂ”Â’Ç°Čą Â˜Â–ÂŠÂœÂŁČą ÂŠÂ›ÂŒÂ’ĂšÂœÂ”Â’Ç°Čą ÂŠÂ›Â˜Â•Čą ÂŽ ÂŽÂ›¢Â—Ç°Čą ’˜Â?›ȹ Â˜Â•ÂŠĂšÂœÂ”Â’ DOI: 10.14313/JAMRIS_2-2017/21 Abstract: The utilization of satellites equipped with robotic arms is one of the existing strategies for Active Debris Removal (ADR). Considering that the time intended for on-orbit capturing manoeuvres is strictly limited, any given space robot should possess a certain level of autonomy. This paper is about the control of on-orbit space robots and the testing of such objects in laboratory conditions. The Space Research Centre of the Polish Academy of Sciences (CBK PAN) possesses a planar air bearing microgravity simulator used for the testing of advanced control algorithms of space robots supported on air bearings. This paper presents recent upgrades to the testing facility. Firstly, the base of the space robot is now equipped with manoeuvre thrusters using compressed nitrogen and therefore allowing for position control of the entire system. Secondly, a signal from an external vision system, referencing the position and orientation of the robot’s parts is used by the control system for the closed loop control. Keywords: space debris, Active Debris Removal, Kessler syndrome, microgravity simulator, space robot, robotic arm, manipulator, control algorithm

1. Introduction

which is widely used by space agencies and committees (e.g. the Inter-Agency Space Debris Coordination Committee, IADC [17], the Committee on the Peaceful Uses of Outer Space, COPUS [44], the European Space Agency, ESA [11], the National Aeronautics and Space Administration, NASA [26]). Several ideas concerning the Active Debris Removal (ADR) are now being considered [3], [13], [40]. The capturing of large satellites and spent rocket stages [12] by a robotic arm is the most conceptually and technologically advanced solution. In the framework of the Clean Space initiative and the e.Deorbit project, several options are being investigated, e.g. the option of a robotic arm with a dedicated gripper, a huge net and an electrodynamic tether [27] which slows down any object in the Earth’s magnetic field [2], [6], [14]. The idea of a satellite equipped with two manipulators and several de-orbiting kits is presented in [5]. The designing of a service spacecraft equipped with a manipulator is being handled by the Defense Advanced Research Projects Agency (DARPA) in the framework of the Robotic Servicing of Geosynchronous Satellites (RSGS) [15]. Nets [4][29] and harpoons [9] are also considered to capture objects on the orbit. Space debris deorbitation may also be accomplished by a precisely aimed laser beam, which creates a cloud of evaporated material slowing down the object and finally causing its re-entry [16], [41].

2323 )*CP(0541

2373 -&'64P @)=)%696'6-& 0< =)*- 0%0'& @04'(09 .&'-;&

In recent years, many space agencies have become interested in controlling satellites equipped with manipulators. The aim of the DEOS [28] and e.Deorbit [2], [36] projects is the development of a technology capable observing and capturing chosen object from the Earth’s orbit. Such manoeuvres could help to perform certain satellite repairs, replace broken components, refill fuel tanks or remove malfunctioning satellites from the orbit. The reason for such missions is the growing number of defunct manmade objects which remain on the Earth’s orbit as space debris [20] and pose a threat to existing satellite systems, including the International Space Station (ISS) [25]. Onorbit collisions could not only cause a breakdown of active systems, but also increase the number of space debris as well as the likelihood of future collisions. Several strategies are being considered to combat this scenario, known as the Kessler syndrome [20]. Formulating safety regulations regarding space missions is one of the solutions for space debris mitigation,

In this paper, an on-orbit space robot is defined as a satellite and manipulator (also called a robotic arm). The space robot subjected to tests consists of a manipulator and a base. The base represents the satellite. The control of on-orbit space robots is a very complicated task. In order to perform the trajectory correctly, the control system has to take into account that the motion of the manipulator influences the position and orientation of the spacecraft. In subject literature, such behaviour of an object is called “free-floating� in contrast to on Earth “fixed-base� industrial robots. However, it is possible to achieve “fixed-base� conditions on orbit through active positioning and orientation maintenance of the satellite by means of manoeuvre thrusters [8]. In both cases, the control system requires a signal about the actual state of the space robot, which can be measured or estimated using a mathematical model. A manipulator can be actuated with the use of relative measurements (e.g. joint positions or end effec95

Journal of Automation, Mobile Robotics & Intelligent Systems

tor position with respect to target satellite position) or inertial measurements (e.g. GPS). However, inertial measurements are insufficient in the final phase of the capture manoeuvre, because high precision is required. Space robots can be tested in an analogous manner. Until now, tests of the space robot in the Space Research Centre of the Polish Academy of Sciences (CBK PAN) were performed using joint positions and a mathematical model only [31]. In extreme situations, a space robot can be controlled in an open loop, basing only on an initial state and an accurate mathematical model. However, taking into consideration that capturing with a robotic arm is a highly dynamic and risky manoeuvre, it is very important to develop a closed loop control system with signals from specialized sensors [1], [10], [18], [34], [42]. It is very difficult to perform tests of a space robot on the Earth because of the terrestrial gravity. One of the existing opportunities is to reduce the motion of a robot to a plane and use a microgravity simulator with planar air bearings. In CBK PAN tests of 2D motion are performed on a microgravity testbed, consisting of a granite table, 2 m × 3 m wide, flat and precisely levelled, and a space robot supported on air bearings. During recent tests the completion of planned trajectories was investigated in terms of accuracy without feedback on the end effector position (but with feedback on joint positions). All trajectories, including Cartesian ones, were computed in the joint space and then sent to the robot. During the trajectory planning phase optimization methods were often used [31], [33]. In the frame of ongoing RR-SPACE project (PBS3/A3/22/2015) the test bed is being modified in order to develop a semi-autonomous space robot as a platform for the testing of various control algorithms. The modifications consist of fitting a set of manoeuvre thrusters powered by pressurized nitrogen onto the base of the space robot and using an external signal from a vision system in the control system of the robot. The thrusters, made by the Warsaw University of Technology, allow for the motion in the plane, while the vision system provides feedback on inertial positions and orientations of robotic components, therefore closing the master control loop. In this paper, the concept of such a robotic system is presented along with the testing facility and a possible exemplary test. The paper is organized as follows: Section 2 describes 3D nonholonomic multibody system dynamics and control, as well as a point where the control input enters the system. In Section 3, modifications of the test bed are being presented. The implementation of modifications together with possible test scenarios and an exemplary simulation result are presented in Section 4. The paper is concluded in Section 5.

73 =)*- 0%0' I.4);6*& )41 @04'(09 The dynamics of the satellite manipulator system is usually described with a simplifying assumption that the momentum and the angular momentum are equal to zero (e.g., [8], [22], [43]). Such assumption is not valid in the case of a space robot that uses thrusters 96

Articles

VOLUME 11,

N° 2

2017

during the capture manoeuvre. Few authors present dynamic equations of satellite manipulator systems with a non-constant momentum and an angular momentum (e.g. [24], [39]). In this paper, we use the description of space robot dynamics based on [39]. We are considering a space robot equipped with a manipulator that has n rotational degrees of freedom (Fig. 1). Equations presented below are given in the inertial reference frame, CSine.

Fig. 1. A schematic view of a space robot equipped with a manipulator and thrusters

The end effector position can be expressed as: (1)

where rs is the position of the satellite centre of mass, rq is the position of the first kinematic pair of the manipulator in respect to the satellite, li is the position of the i + 1 kinematic pair in respect to the kinematic pair i (all expressed in CSine frame). End effector velocity can be expressed as: (2)

where vs is the linear velocity of the satellite, while `s is the angular velocity of the satellite, Js denotes the Jacobian of the satellite, while JM denotes the Jacobian of the manipulator given in the inertial reference frame, is a n-dimensional vector that contains velocities of manipulator joints. The satellite’s Jacobian is described by: (3)

where ree_s = ree – rs, I denotes the identity matrix, 0 is the zero matrix, the ~ symbol denotes a matrix which is the equivalent of a vector cross-product. The angular momentum of the satellite manipulator system is described by: (4) where L0 denotes the initial angular momentum of the system.

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

Momentum and angular momentum are presented as:

N° 2

2017

is not taken into account as in the short timeframe of the capture manoeuvre, it remains almost constant. The Lagrange equation has the following form:

(5) (10) Matrices H2 and H3 (defined in [31] and [39]) are influenced not only by the state of the manipulator, but also by the state of the satellite. Functions fm and fam on the right hand side of equation (5) describe changes of momentum and angular momentum that are known a priori and depend on external forces Fs and external torques Hs acting on the satellite’s centre of mass: where t0 is the initial time and tk is the final time of the maneuver. Functions fm and fam play a crucial role in the analysis of the space robot equipped with thrusters because these functions allow us to take into account the influence of thrusters on the satellite’s dynamics. If there is an initial momentum and angular momentum of the system and no external forces/torques act on the system, then functions fm and fam are constant. The non-zero right hand side of equation (5) differentiates the approach presented here from the common approach, in which zero momentum and angular momentum is assumed. Taking into account functions fm and fam, the end effector velocity can be described as: (6) The following kinematic relation between the end effector velocity and velocities of manipulator joints can be obtained using: (7) while the linear and angular velocity of the manipulator-equipped satellite is described by: (8) Equation (7) can be used when the manipulator has 6 DoF. If a redundant manipulator is used, then JM is a non-square matrix and it is necessary to use pseudoinverse in equation (7). A transformation matrix between the inertial reference frame and the body-fixed coordinate system is determined through kinematic equations by the angular velocity of the servicing satellite (`s). Equations (7) and (8) are used by the trajectory planning algorithm (in cases where actions of thrusters are known at the trajectory planning stage, before the execution of the manoeuvre). We use Langrangian formalism to derive dynamics equations. To describe the state of the system, we chose the following generalized coordinates [19]: (9) where {s denotes the orientation of the satellite (given by its three Euler angles). The potential energy

where T is the kinetic energy of the system, while is the vector of generalized forces, in which u denotes the vector of control torques applied in the manipulator joints. Modified version of Equation (10) is used to derive the general equations of motion for space robot. These equations can be used to compute control torques u(t) required to perform planned motion of the end effector: (11) where M denotes the mass-matrix, while C denotes the Coriolis matrix (details can be found in [31] and [39]). In the equation (11), potential forces are not taken into account, because the satellite is in the state of free fall.

:3 016<6*)'604& 0< -&'64P .&'-; Former testing system, described in [30], consisted of the base and two-link manipulators, both supported by air bearings for frictionless motion. It allowed for the realisation of pre-planned trajectories both in Cartesian and configuration space with no feedback on the position of the end effector. In order to plan trajectories in Cartesian space, algorithms based on dynamic Jacobian were used to take into account the “free-floating” state of the system and the high ratio of the manipulator mass versus the mass of the base. Results of the tests were then compared with corresponding simulations. There are two modifications of the testing system described in this paper. The first one is related to the space robot control system, specifically to the access of the control system to the inertial position and orientation. The second one affects the satellite body by adding a set of cold gas thrusters which allow its motion in the plane. In the general approach, the control torques in manipulator joints are a composition of feed forward torques uref, calculated in the trajectory planning phase and correction torques ucontr, computed by the control system during motion in real time (12). The measurement of position and velocity of the end effector (ree, vee), which is necessary for the computation of ucontr, can be obtained from the vision system (described in detail in 3.1). Thus, (12) where the position and velocity error are defined as follows: (13) Articles

97

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

(14) The feedback on the position and velocity of the end effector is crucial during capturing manoeuvres. Manoeuvre thrusters, which are added to the base as the second modification (described in detail in 3.2), are responsible for the generation of forces and torques (Fs, Hs) acting on the base centre of mass. Forces Fs and torques Hs are then used in both the dynamic analysis and the control system. In the 2D system, the equation describing the control problem concerning the base can be stated as follows: (15)

where forces acting on the eight cold gas thrusters (the outputs) are: (16) and: Fx, Fy – resultant forces, respectively in x and y direction in the base reference frame (components of Fs in 2D case), Òz – a torque with respect to the z direction (component of Hs in 2D case), A3x8 – a matrix transposing forces at 8 cold gas thrusters to resultant force and torque. The control of the base is formulated as follows:

(17) where A# is the Moore-Penrose pseudoinverse of the matrix A3x8. The use of pseudoinverse allows for the finding of a solution of a minimum Euclidean norm among all possible solutions. It therefore minimizes the flow rate during trajectory performance in the described system. This operation distributes force equally to the specific pairs of thrusters. As a result, the pairs with the same force value and opposite signs are obtained. Therefore, negative values have to be rejected and the others, doubled in order to achieve the same effect on the base.

Fig. 2. Signal transfer in the system 98

Articles

N° 2

2017

In current 2D simulations, matrix A# takes the following form: (18)

where: a = 0.2492, b = 2.2e-4, c = -7.99e-4, d = 0.25022, e = 0.625, g = 0.2508, h = 0.24987. In the current approach, specific control schemes are used: a control scheme based on the dynamic Jacobian inverse [35] for the manipulator and PD controller for the base. Each of them is based on the feedback on the position and orientation of the base and position of the end effector provided by the vision system. Simulations have shown that depending on the type of the base trajectory (line, arc, rotation, line + rotation, etc.) different correcting gains in the PD controller have to be used to ensure good trajectory realization accuracy.

:323 6&604 .&'-; The feedback mentioned in Eq. (13) depends on acquisition time. It is realized by an external vision system delivered by OptiNav, which consists of 3 industrial cameras of 5 MPx resolution and 100 Hz frequency, as well as a PC with dedicated software for visual marker recognition. The results are transferred by wireless communication to the control system on the space robot computer. The space robot on the test bed is extremely sensitive to any force coming from its surroundings. Therefore, the wireless connection for data transfer is used to avoid the influence of cables on the platform motion [37]. In order to correctly use the data obtained from the vision system, we have to ensure that the time instant of image acquisition (taq) is specified in the space robot computer time frame. The necessity of using wireless communication and fulfilling the above-mentioned requirements creates a problem, which is solved in two steps. First of all, the space robot sends a signal to the cameras triggering device, together with a unique time tag, which is then being added to the results of the processing of the acquired image. This ensures that the computer in the space robot can locate the time instant (taq) from which the received data from the vision system comes. Secondly,

Journal of Automation, Mobile Robotics & Intelligent Systems

a synchronization of processors on the space robot and the triggering device is performed by a special Bluetooth protocol with an accuracy of 3 ms. The total delay between sending the triggering signal and receiving the data, which is estimated at 80–100 ms, does not impair the quality of control. This is due to the fact that the data from the vision system is used as a correction of IMU readouts in the Kalman filter. The scheme of the system is shown in Fig. 2. The described system represents a real on orbit case in which the servicing robot approaches the target using GPS navigation in inertial reference frame. During the process of capturing, which is the relative navigation phase, cameras and laser proximity sensors are used. Star trackers could also be used to correct readings from IMU. The test facility can simulate both cases in the presence of two or more objects on the test bed. By default, the vision system works in inertial reference frame but it is possible to compute relative distances and orientations to use them in non-inertial frame. The formulation of the control function ucontr is not a part of this paper.

:373 0%0'6* #9)'<0(; In the framework of the RR-SPACE project, an existing base of a space robot was used. The base was enhanced by mounting 8 cold gas thrusters with a separate gas canister with pressurized nitrogen (Fig. 3). The gasses used in the experiment were

VOLUME 11,

N° 2

2017

chosen for their safety for the space robot and technical operators. The expelled nitrogen does not affect any physical components of the space robot. Cold gas thrusters were mounted on the corners of the base, pointing in perpendicular directions (Fig. 4). Such a location allows for complete position and orientation control. Mass and geometry parameters were chosen as guided by scaling laws. Scaling is usually performed when it is difficult to test the object in 1:1 scale due to both manufacturing costs and testing facility capabilities. Instead, dimensions of the testing mockup can be minimized with the use of scaling laws. Moreover, the obtained upon re-scaling results can be used for the analysis of the full scale object. The equation of scaling law is presented in (19). (19) where: Ps – value after scaling, k – scaling coefficient, w – scaling exponent, P – value before scaling. Several exemplary scaling exponents for specific physical properties are shown in Tab. 1. Tab. 1. Physical properties with scaling exponents [7] Physical property

Scaling exponent

Physical property

Scaling exponent

Distance

1

Inertia

5

Time

1

Velocity

0

Frequency

–1

Acceleration

–1

Force

2

Energy

3

Mass

3

Power

2

In Tab. 2, some assumed parameters of a real space mission are presented together with parameters of a scaled test platform in CBK PAN. Tab. 2. The scaling of the space robot Fig. 3. The space robot. 1 – a gas canister with compressed nitrogen for cold gas thrusters, 2 – a gas canister with compressed air for air bearings, 3 – pressure regulators, 4 – cold gas thrusters, 5 – air bearings, 6 – battery, 7 – electronic box

Fig. 4. Locations of cold gas thrusters on the base of the space robot, rx = ry = 200mm

Parameter

Real space robot (3D)

Space robot in CBK PAN (2D)

Total mass mc [kg]

570.00

66.16

Mass of the base mb [kg]

518.18

60.15

Mass of the manipulator mm [kg]

51.82

6.01

Manipulator length lm[m]

2.50

1.22

Base inertia Ib [kg*m2]

-

2.199

Ratio mm/mb [-]

0.10

0.10

Ratio mm/mc [-]

0.09

0.09

Scaling exponent k

-

0.4878

1/k [-]

-

2.05

The influence of the degree of filling of the gas canister with compressed air for the air-bearings and the gas canister with compressed nitrogen for cold gas thrusters was also analyzed. Several cases were distinguished: (i) both canisters are full, (ii) both canisters are empty, (iii) the canister for air bearings is Articles

99

Journal of Automation, Mobile Robotics & Intelligent Systems

full and the canister for thrusters is empty, (iv) the canister for air bearings is empty and the canister for thrusters is full. The parameters of the base for each of the distinguished cases were evaluated using the CAD model and shown in Tab. 3. Tab. 3. Parameters of the base per analyzed case Case

(i)

(ii)

(iii)

(iv)

Mass [kg]

60.017

59.227

59.617

59.627

x

0.354

0.333

-0.415

1.102

y

-1.279

-1.297

-1.288

-1.288

z

105.361

104.098

104.32

105.147

2.384

2.372

2.378

2.378

CoG position [mm]

Inertia Izz [kg*m2]

Performed analysis showed that the influence of the level of usage of the gasses on the parameters of the base is negligible. The mass of the base changes by 1.3% and the inertia, by 0.5%. The minimal influence should be seen only during experiments conducted in open-loop mode. The influence can be neglected in the closed loop mode. According to tests of the cold gas engine specially designed for experiments, described in details in [21], several parameters can be identified. The nominal thrust of the cold gas engine is 0.846 N, the nominal chamber pressure is 10 bar and the mass flow rate is 1.575 g/s. Some dynamic parameters were also identified. The opening time for the 13.7 W electromagnetic coil is 3.15 ms and the delay in the opening time is 5.5 ms. Therefore, the minimal time of

VOLUME 11,

N° 2

2017

thrust is 15 ms and the maximal frequency (in PWM mode) is 35 Hz. Exemplary characteristics with thrust force and chamber pressure are shown in Fig. 5. and Fig. 6.

>3 -&59'& >323 -&'!%-1 ;=9-;-4')'604 The modifications described in Section 3 were implemented in the test bed system shown in Fig. 7. It consists of a granite table, which is 2 m × 3 m wide, flat and precisely levelled, a space robot mockup and a vision system with dedicated software on an external PC. The space robot can move freely on the surface of the table using air bearings.

Fig. 7. The test-bed. 1 – the air bearing table, 2 – the space robot, 3 – the illumination system, 4 – vision system cameras

>373 "O-;=9)(. -&' *-4)(60 There are several possible aspects that can be investigated in the facility. For example, the influence of a compliant joint with magnetic gear on the accuracy of trajectory realization can be tested. A new robust control system can be developed for this case. The aspect of position and orientation control of the base with 8 manoeuvre thrusters along with trajectory realization may become an area for further research. The most complicated case is when both the base and the end effector have their separate trajectories (Fig. 8). Two phenomena manifest here: one is that the thrusters introduce a non-constant linear and angular momentum into the system and

Fig. 5. Trigger U, thrust force and chamber pressure during a 100 ms’ thrust Base trajectory

End effector trajectory

Fig. 6. Trigger U, thrust force and chamber pressure operating with 10 Hz frequency and a 40 ms’ thrust 100

Articles

Fig. 8. A possible test with trajectories for the base and for the end effector (red – cold gas thrusters, green – one of the components, arrows – thrust forces)

Journal of Automation, Mobile Robotics & Intelligent Systems

the second one is that the motion of the manipulator causes force reactions to act on the base. The correct undertaking of the test is possible due to feedback from the vision system. This case represents the real on orbit case in which a satellite has an active Attitude and Orbit Control System (AOCS), but the system does not maintain the satellite’s position and orientation. The main goal of this activity is to develop a robust testing research platform for various control algorithms of the space robot. The secondary goal is to verify the accuracy of the trajectory performance of the manipulator and to test several control algorithms. The nominal mode of operation of the space robot comprises the realization of two separate trajectories: for the base and the robotic arm at the same time. However, several other modes can be distinguished: • the motion of the base versus the given position and orientation (inertial space), • the realization of the trajectory for the base only, • the motion of the robotic arm versus the given position in configuration space, • the realization of the trajectory for the robotic arm in configuration space, • the motion of the robotic arm to the given point for the end effector (inertial space), • the realization of the trajectory for the end effector only, given in inertial space. The control system on the space robot computer consists of several function blocks: Guidance (which receives data from sensors), Navigation (filters) and the Control function block. Therefore, the space robot is prepared to apply different control algorithms by changing control schemes in the Control function block.

>3:3 6;59)'604 -&59' In the simulation prepared in the MATLAB®/Simulink environment, the manipulator performed the linear trajectory in an inertial space and the cold gas thrusters were used to maintain the position and orientation of the base. This case represents an on orbit situation in which the AOCS controls the state of the satellite and actively counteracts the loads resulting from the manipulator’s movement. Such a case is usually described as “fixed-base” in contrast to the case when the AOCS is turned off completely (the case of “free-floating”). The open-loop control scheme based on the dynamic Jacobian inverse [35] was used for robotic arm movement and the openloop algorithm was used for base stabilization.

VOLUME 11,

N° 2

2017

Tab. 4. Simulation parameters Parameter

Value

Inertia of the base [kg*m2]

2.199

Base and manipulator mass [mb; mm] [kg]

[60.15; 6]

Time of simulation [s]

10

Time of acceleration and deceleration [tp; th] [s]

[3, 3]

Initial end effector position [x; y] [m]

[0.8; 0.7]

Initial end effector velocity [vx; vy] [m/s]

[0, 0]

Final end effector position [x; y] [m]

[0.8; 0.1]

Final end effector velocity [vx; vy] [m/s]

[0, 0]

The parameters used in the simulation are shown in Tab. 4. The motion of the end effector was planned as to have acceleration part of time tp and deceleration part of time th. Fig. 9 (on the left hand side) shows reactions acting on the base centre of mass, resulting from the manipulator’s motion. The task of the stabilization of the base’s position and orientation by counteracting these reactions was performed by a set of 8 cold gas thrusters. The force on each thruster was calculated using equation (17) and shown in Fig. 10. In the presented configuration, the maximal value of force on a single thruster is 0.2 N. The thrust force on a single thruster is 0.86 N. The conclusion is that the stabilization of the base in given conditions should be viable. What is worth noting about thrusters is that they provide an impulse thrust and it is therefore impossible to achieve intermediate values of thrust force. This issue can be resolved by using either short period thrusts or a kind of PWM mode. To check the influence of the impulse operation mode of thrusters on the accuracy of base stabilization, two simulations were performed: • the forces shown in Fig. 10 were applied to the base directly without any change. It means that the thrusters did not work in the impulse mode, • the forces shown in Fig. 10 were calculated to impulse in the PWM mode with a frequency of 5 Hz and a duty cycle depending on the force value. As a result, a total base position and orientation error was obtained (Fig. 9). In the PWM mode, the base orientation error is bigger, with a maximal value of 0.12 deg, compared to 0.08 deg. However, the base position error in the PWM mode is lower, with a maximal value of 1.35*10-4 m compared to 1.55*10-4 m.

Fig. 9. Left: Forces and torque acting on the base center of gravity. Right: Position and orientation errors during the realization of the base trajectory Articles

101

Journal of Automation, Mobile Robotics & Intelligent Systems

VOLUME 11,

0.1

0.06

res_NW res_EN res_SE res_WS

0.2 Force [N]

Force [N]

2017

0.25 res_NE res_ES res_SW res_WN

0.08

0.04 0.02 0

N° 2

0.15 0.1 0.05

0

1

2

3

4

5 Time [s]

6

7

8

9

10

0

0

1

2

3

4

5 Time [s]

6

7

8

9

10

Fig. 10. Forces on cold gas engines necessary for the stabilization of the base in the Cartesian trajectory realization

The simulation showed that the thruster’s impulse operation mode did not affect the accuracy of the base stabilization. Any possible negative effects of impulse mode during trajectory realization should be, however, negligible in experiments performed in closed loop mode with feedback from the vision system.

^ = $ (Aˆ = – Institute of Heat Engineering, Warsaw University of Technology, Nowowiejska 21/25 str., 00-665 Warsaw, Poland.

?3 5;;)(.

[ ) ( $ [ ) = – Space Research Centre of the Polish Academy of Sciences (CBK PAN), Bartycka 18a str., 00-716 Warsaw, Poland.

The context of this paper is the problem of space debris mitigation, as well as select strategies to limit its amount on the Low Earth Orbit (LEO). In this study, the investigation was limited to the phenomenon of control of a space robot equipped with a manipulator arm. In the paper, the recent modifications of the test bed in CBK PAN referencing a microgravity simulation in a plane were presented. The test bed is capable of testing various control systems and performing complex manoeuvres, simulating the capturing of space debris. The vision system provides a position and orientation signal for the closed loop mode, which enables the testing of more complicated cases. Future research will focus on the relative navigation during the final phase of the rendezvous manoeuvre. Tests may be performed with a virtual or real target. Tests related to the formation flight are also being foreseen. The isolation of the space robot from external world is a huge advantage concerning contact tests, e.g. those involving grippers and landers.

$ [ ) $= ) $ – Institute of Heat Engineering, Warsaw University of Technology, Nowowiejska 21/25 str., 00-665 Warsaw, Poland

' $ ? = – Space Research Centre of the Polish Academy of Sciences (CBK PAN), Bartycka 18a str., 00-716 Warsaw, Poland. ) – Space Research Centre of the Polish Academy of Sciences (CBK PAN), Bartycka 18a str., 00-716 Warsaw, Poland. . ? = – Institute of Heat Engineering, Warsaw University of Technology, Nowowiejska 21/25 str., 00-665 Warsaw, Poland. *Corresponding author

"A" "G@" [1]

@,GFB "IN" "G This paper was supported by the National Centre for Research and Development, project no. PBS3/ A3/22/2015.

D EF – Space Research Centre of the Polish Academy of Sciences (CBK PAN), Bartycka 18a str., 00-716 Warsaw, Poland, joles@cbk.waw.pl.

[2]

[3]

[4]

Jan Kindracki – Institute of Heat Engineering, Warsaw University of Technology, Nowowiejska 21/25 str., 00-665 Warsaw, Poland. [5] Tomasz Rybus – Space Research Centre of the Polish Academy of Sciences (CBK PAN), Bartycka 18a str., 00-716 Warsaw, Poland. 102

Articles

] 5 (] 6 , ] % # ] “Controlled Zero Dynamics Feedback Linearization with Application to Free-Floating Redundant Orbital Manipulator�. In: 2013 IEEE American Control Conference, Washington DC, USA, 2013, 1834–1839. DOI: 10.1109/ACC.2013.6580102. Biesbroeck R., “The e.Deorbit Study in the Concurrent Design Facility�, The CleanSpace: Workshop, Darmstadt, September 2012. Bonnal C., Ruault J., Desjean M., “Active Debris Removal: Recent Progress and Current Trends�, Acta Astronautica 85, 2013, 51–60. DOI: 10.1016/j.actaastro.2012.11.009. Botta E., Sharf I., Misra A., Teichmann M., “On the Simulation of Tether Nets for Space Debris Capture with Vortex Dynamics�, Acta Astronautica, 123, 2016, 91–102. DOI: 10.1016/j.actaastro.2016.02.012. Castronuovo M., “Active Space Debris Removal – A Preliminary Mission Analysis and Design�, Acta Astronautica, 69, 2011, 848–859. 10.1016/j.actaastro.2011.04.017.

Journal of Automation, Mobile Robotics & Intelligent Systems

[6] [7] [8]

[9]

[10]

[11]

[12]

[13]

[14]

[15]

[16]

[17]

[18]

[19]

[20]

[21]

Clean Space, “e.Deorbit Implementation Planâ€?, ESA-TEC-SC-TN-2015-007, 2015. Dexler K., Nanosystems: Molecular Machinery, Manufacturing, and Computation, Wiley, 1998. Dubowsky S., Papadopoulos E., “The Kinematics, Dynamics and Control of Free-Flying and Free-Floating Space Robotic Systemsâ€?, IEEE Transaction on Robotics and Automation, vol. 9, no. 5, 1993, 531–543. DOI: 10.1109/70.258046. Dudziak R., Tuttle S., Barraclough S., “Harpoon Technology for the Active Removal of Space Debrisâ€?, Advances in Space Research, 56, 2015, 509–527. DOI: 10.1016/j.asr.2015.04.012. 9 ', ‚] Algorithms of Motion Planning for Nonholonomic Robots FÂŒ # > # & # > ? > XœœÂ&#x;] European Space Agency, International Academy of Astronautics, “Position Paper on Space Debris Mitigation; Implementing Zero Debris Zonesâ€?, Noordwijk, 2006. Felicetti L. et al., “Design of Robotic Manipulators for Orbit Removal of Spent Launchers’ Stagesâ€?, Acta Astronautica, 119, 2016, 118–130. DOI: 10.1016/j.actaastro.2015.11.012. Forshaw J. et al., “RemoveDEBRIS: An In Orbit Active Debris Removal Demonstration Missionâ€?, Acta Astronautica, 2016, 448–463. DOI: 10.1016/j.actaastro.2016.06.018. Hausmann G. et al., “e.Deorbit Mission: OHB Removal Conceptsâ€?. In: ASTRA 13th Symposium on Space Technologies in Robotics and Automation, The Netherlands, 2015. Henshaw C., “The DARPA Phoenix Spacecraft Servicing Program: Overview and Plans for Risk Reduction. International Symposium on ArtiÂŒ ‚# " # ¢ Robotics and Automation in Space (i-SAIRAS), Montreal, 2014. Hou L., Cai Y., Liu J., Hou Ch., “Variable Fidelity Robust Optimization of Pulsed Laser Orbital Debris Removal Under Epistemic Uncertaintyâ€?, Advances in Space Research, 57, 2016, 1698– 1714. DOI: 10.1016/j.asr.2015.12.003 Inter-Agency Space Debris Coordination Committee, “IADC Space Debris Mitigation Guidelinesâ€?, Rev.1, September 2007. ( ', Q] & ] ™ # # Mechanical Systems Modeling and Control: A Free-Floating Space Manipulator Case as a Multi-Constrained Systemâ€?, Robotics and Autonomous Systems, 62, 2014, 1353–1360. DOI: 10.1016/j.robot.2014.04.004. Junkins J. L., Schaub H., “An Instantaneous Eigenstructure Quasivelocity Formulation for Nonlinear Multibody Dynamicsâ€?, Journal of the Astronautical Sciences, vol. 45, no. 3, 1997, 279–295. Kessler D. J. et al., “The Kessler Syndrome: Implications to Future Space Operationsâ€?, Advances in the Astronautical Sciences, 137, no. 8, 2010. # (] ] & &] 'H ]

] > &] ™Q�= # 6 on Low-cost Cold Gas Propulsion for a Space Robot Platform�, Aerospace Science and Tech-

VOLUME 11,

[22]

[23]

[24]

[25]

[26] [27]

[28]

[29]

[30]

[31]

[32]

[33]

[34]

N° 2

2017

nology, 62, 2017, 148–157. DOI: 10.1016/j. ast.2016.12.001. Lindberg R.E., Longman R.W., Zedd M.F., “Kinematic and Dynamic Properties of an Elbow Manipulator Mounted on a Satelliteâ€?. In: Space Robotics: Dynamics and Control, Ed.: Y. Xu and T. Kanade, Springer, USA, 1993. DOI: 10.1007/978-1-46153588-1_1. 'H ] & &] # (] ] “Experimental Research on Hydrogen Peroxide Micro Thruster for Satellite Applicationâ€?. In: & #" ‚ ‚# # # % # ÂŒ Conference „Development Trends in Space Propulsion Systemsâ€?, Warsaw, Poland, 2015, 148– 157, 10.1016/j.ast.2016.12.001. Nanos K., Papadopoulos E., “On the Use of Free-Floating Space Robots in the Presence of Angular Momentumâ€?, Intelligent Service Robotics, vol. 4, no. 1, DOI: 2011, 3–15. 10.1109/ AIM.2010.5695904. ¸NASA Orbital Debris Quarterly News, “Increase in ISS Debris Avoidance Manoeuvresâ€?, Vol. 16, issue 2, April 2012. NASA Procedural Requirements for Limiting Orbital Debris, NPR 8715_006A, 2009. Nishida S. et al., “Space Debris’ Removal System Using a Small Satelliteâ€?, Acta Astronautica 65, 95-102, 2009, 95–102. 10.1016/j.actaastro.2009.01.041 Reintsema D. et al., “DEOS – the German Robotics Approach to Secure and De-Orbit. Malfunctioned Satellites from Low Earth Orbitâ€?, International % = # + ÂŒ ‚# " # 6 , and Automation I Space (i-SAIRAS), 2010. Retat I. et al., “Net Capture System; a Potential Orbital Space Debris Removal Systemâ€?, 2nd European Workshop on Active Debris Removal, Paris, June 2012. Rybus T. et al., “New Planar Air-bearing Micro" V % ÂŁ ÂŒ # %= Robotics Numerical Simulations and Control Algorithmsâ€?, 12th Symposium on Advanced Space Technologies in Robotics and Automation (ASTRA 2013), ESTEC, Noordwijk, The Netherlands, 2013. Rybus T., Seweryn K., “Manipulator Trajectories During Orbital Servicing Mission: Numerical Simulations and Experiments on Microgravity Simulatorâ€?, 6th European Conference for Aeronautics and Space Sciences (EUCASS 2015), KrakĂłw, Polska, 2015, 377–394. Rybus T., Seweryn K., “Planar Air-Bearing Microgravity Simulators: Review of Applications, Existing Solutions and Design Parametersâ€?, Acta Astronautica 120, 2016, 239-259. 10.1016/j.actaastro.2015.12.018 Rybus T., Seweryn K., Sasiadek J., “Application of Trajectory Optimization Method for a Space Manipulator with Four Degrees of Freedomâ€?, 11th AAIA Conference, 2016, 92–101. Rybus T., Seweryn K., Sasiadek J., “Control System for Free-Floating Space Manipulator Based Articles

103

Journal of Automation, Mobile Robotics & Intelligent Systems

[35]

[36]

[37]

[38]

[39]

[40]

[41]

[42]

[43]

[44]

104

on Nonlinear Model Predictive Control (NMPC)�, Journal of Intelligent and Robotic Systems, 2016, 491-509. 10.1007/s10846-016-0396-2. Rybus, T., Barcinski, T., Lisowski, J., Seweryn, K. “Analyses of a Free-Floating Manipulator Control Scheme Based on the Fixed-Base Jacobian with Spacecraft Velocity Feedback� Sasiadek, J.Z. (ed.) Aerospace Robotics II, GeoPlanet: Earth and Planetary Sciences, Springer-Verlag, 2015, 59–69. Scheper M., “e.Deorbit Phase B1�, Clean Space Industrial Days, ESTEC, Noorwijk, The Netherlands, 2016. Seweryn K. et al., “Design and Development of Two Manipulators as a Key Element of a Space Robot Testing Facility�, Archive of Mechanical Engineering, vol. LXII, no. 3, 2015, 377-394. DOI: 10.1515/meceng-2015-0022. ! " #The dynamics of maneuvering of satellites and their connection with a manipulator with non-holistic constraints), PhD thesis, Warsaw University of Technology, 2008. In Polish. Seweryn K., Banaszkiewicz M., “Optimization of the Trajectory of a General Free-Flying Manipulator During the Rendezvous Manoeuvre�, AIAA Guidance, Navigation, and Control Conference and Exhibit, Honolulu, Hawaii, USA, 2008. DOI: 10.2514/6.2008-7273. Shan M., Guo J., Gill E., “Review and Comparison of Active Space Debris Capturing and Removal Methods�, Progress in Aerospace Sciences, 80, 2016, 18–32. DOI: 10.1016/j.paerosci.2015.11.001. Soulard R., Quinn M., Tajima T., Mourou G., “ICAN: A Novel Laser Architecture for Space Debris Removal�, Acta Astronautica, 105, 2014, 192– 200. DOI: 10.1016/j.actaastro.2014.09.004. Ulrich S., Sasiadek J., Barkana I., “Modeling and Direct Adaptive Control of a Flexible-Joint Manipulator�, J. Guid. Control. Dynam., vol. 35, no. 1, 2012, 25–39. DOI: 10.2514/1.54083. Umetani Y., Yoshida K., “Resolved Motion Rate Control of Space Manipulators with Generalized Jacobian Matrix�, IEEE Transactions on Robotics and Automation, vol. 5, no. 3, 1989, 303–314. DOI: 10.1007/978-1-4615-3588-1_7. U# < # F Œ F %= + “Space Debris Mitigation Guidelines of the Committee on the Peaceful Uses of Outer Space�, Vienna 2010.

Articles

VOLUME 11,

N° 2

2017