Design of a bilateral position/force master slave teleoperation system with nonidentical robots

Transcription

1 Design of a bilateral position/force master slave teleoperation system with nonidentical robots P.W.M. van Zutven DCT 28.8 Supervisors CINVESTAV (Mexico D.F.): Dr. A. Rodriguez Angeles Dr. C. A. Cruz Villar Supervisor TU/e Prof. Dr. H. Nijmeijer DCT report Eindhoven University of Technology Department Mechanical Engineering Dynamics and Control Group Eindhoven, September 28

2 Preface This report is a result of a master traineeship of three months in Mexico City, Mexico at the mechatronics department of el Centro de Investigacion y Estudios Avanzados del Instituto Politecnico Nacional (CINVESTAV del IPN). Special thanks go to Dr. A. Rodriguez Angeles and Dr. C.A. Cruz Villar from CINVESTAV for their support and supervision during this project and feedback on the progress reports and on the final report. Besides that I would like to thank the several students from this institute who helped me to understand the robots and to build the experimental setup. Finally I would like to thank Prof. Dr. H. Nijmeijer from the dynamics and control group of the Eindhoven University of Technology for his support during the project and feedback on my final report.

3 Abstract In this research a bilateral master slave teleoperation system with two non identical robots is designed and investigated through simulations and experiments. Two already existing robots are identified kinematically and dynamically. Forward kinematics and inverse kinematics, describing the end effector position and orientation are determined using the robots configurations. Dynamical identification has been done using the frequency response of the master and slave robot. Transfer functions have been fitted through the frequency response data to obtain the system transfer function matrices. These matrices are used to design a bilateral master slave teleoperation system. It is a bilateral system because the slave robot imitates the position and orientation of the master robot whereas the master is imitating the reaction force applied to the slave when it is in contact with the environment. Therefore a master controller and a slave controller are developed to ensure high performance. The master controller consists of two parts, namely a PI force controller that controls the force feedback and an internal model controller that compensates for the master robot dynamics. The master robot dynamics should be compensated so that an operator, handling the master robot, actually does not feel this robot. The slave controller also uses a PI force controller to ensure that the force, which is applied by the operator to the master robot, is also applied to the environment. The second part of the slave controller is a position controller tuned with the slave robot transfer functions so that high performance can be reached. Because no force sensor is available two types of virtual environments are developed to calculate the forces when one of the robots is in contact with these environments. The first type of virtual environment is using an impedance model which models the environment as a mass-springdamper system, especially useful for modeling soft environments that can be penetrated. The second type is a holonomic constraint model which models the environment as if the robot is attached to it. This type is for modeling rigid environments whereas penetration is impossible. With both models the robots can move tangential to the environment while applying force to it. For simulation purposes an operator is designed as a PID position and force controller so that it can generate torques on the master robot to move it along smooth desired trajectories that are designed using Bezier polynomials. Eventually the designed teleoperation system is simulated and implemented in an experimental platform. 2

4 Table of contents Preface... Abstract... 2 Introduction System identification Kinematics Master kinematics Slave kinematics Dynamics Master dynamics Slave dynamics Teleoperation system design Virtual environment Impedance model Holonomic constraint model Master controller Force controller Internal model control Slave controller Position controller Force controller Operator Simulations Bezier polynomials Impedance model Holonomic constraint model Experiments Impedance model Holonomic constraint model Virtual environments comparison Conclusions and recommendations Conclusions Recommendations References

5 Appendix A Slave inverse kinematics derivation... 4 Appendix B Simulink schemes for frequency response measurements Overall scheme Controller scheme Appendix C Simulink schemes for simulations and experiments Master robot Slave robot Forward kinematics master robot Forward kinematics slave robot Inverse kinematics slave robot Virtual environment using impedance model Virtual environment using holonomic constraint model Master controller Slave controller using impedance model as virtual environment Slave controller using holonomic constraint model as virtual environment Operator Total scheme using impedance model Total scheme using holonomic constraint model

6 Introduction Teleoperation systems are systems in which more than one individual system is involved. As individual system one can think of a robot, an electric circuit, a virtual environment, etc. Such systems may interact with each other and in some cases trying to synchronize. Teleoperation systems can be roughly divided in two classes, unilateral and bilateral. Unilateral means that one robot is sending data to another but is not receiving anything from that other robot; whereas bilateral means that the robots are really interacting with each other. All robots in a bilateral teleoperation system are sending and receiving data to and from each other. Teleoperation systems are already investigated in the past. A lot of information about teleoperation systems can be found. For example global research has been done to determine control strategies and experiment with different kinds of teleoperation control (Flemmer, 24) and (Marcassus, et al., 26). Sometimes new control strategies for teleoperation systems are developed (Lee, et al., 993) and (Yokokohji, et al., 994). Another part of the research is more in depth on specific robots. But most of this research uses analytical dynamic expressions (Alise, et al., 27), (Cortes Martinez, 27), (Rodriguez Angeles, et al., 27), (Hashtradi-Zaad, et al., 22) and (Kwon, et al., 2). Teleoperation research using transfer functions or frequency response data is rare (Yan, et al., 996). That is one of the reasons why this research has been done. Another is that teleoperation control and synchronization is applied more and more in all kinds of advanced robotic platforms. Applications where teleoperation is used, researched and further developed are for example in hospitals, at dangerous and unreachable places. In hospitals teleoperation is used during medical surgery. The doctor is handling a robot or joystick while watching a screen, whereas a robot is actually inside a patient imitating the movements of the doctor. Operating in this way decreases the change on infections and complications. What one can think of as dangerous places are for example war situations, where soldiers need to disarm bombs or fly over hostile areas or advanced environmental research near volcanoes or tornados. It is better to use unmanned vehicles that are driven from a safe distance, to do this kind of jobs. Unreachable places are for example the deep sea or other planets like Mars. With teleoperation system it is possible to discover these places without actually going there, although big time delays can decrease the value of those teleopation systems. This research includes the design of a bilateral position/force master slave teleoperation system for two existing non identical robots. In the research the design will be tested using simulations and experiments. During simulations and experiments virtual environments will be designed as if the robots are actually interacting with their surroundings. A measure for performance is called transparency, the better the slave robot imitates the master position and orientation and the better the master is imitating the applied force to the slave, the higher is the transparency of the system. To further increase the transparency of the system, a dynamical compensation controller will be developed for the master robot, so that an operator will not feel that it is handling the robot. Goals for this research are formulated in the following way. - Identify kinematic and dynamic properties of an existing master and slave robot using frequency response functions - Design a bilateral position/force master slave teleoperation system with dynamical compensation using two different virtual environments - Test the design in simulations 5

7 - Implement teleoperation design in real robot platform These subjects will be discussed in the report in the same order as stated here. In chapter 2 the system identification using frequency response functions is described. This is followed by the teleoperation design in chapter 3. After that the simulation and experimental results will be discussed in chapters 4 and 5. Finally a comparison is made between the two developed virtual environments to weight there advantages and disadvantages in chapter 5.3 and conclusions and recommendations are written in chapter 6. 6

8 2 System identification System identification is an important part of this research, because without the knowledge of the system to be controlled it is impossible to reach a high performance. System identification can be done in different ways, for example by deriving analytical expressions using Lagrange or Newton-Euler equations which already has been done for the master (Cortes Martinez, 27) and slave system (Muro Maldonado, 27). However in this report frequency response functions are used to retrieve information about the systems to be controlled. But first the kinematics of the master and slave system is derived, which is necessary in the total bilateral master slave teleoperation system, as we will see in chapter Kinematics The kinematics describes the position of the end effector with respect to the joint angles and vice versa. The first is called forward kinematics, the second inverse kinematics. In case of the master robot only the forward kinematics is necessary in a teleoperation system, where the slave side needs both the direct and inverse kinematics. In the next two paragraphs these are derived for the master and slave robot. 2.. Master kinematics The master system is a parallel robot developed and already described (Cortes Martinez, 27). It is a 3-degree of freedom robot consisting of four bars forming a closed loop chain. Joints are actuated by motors and joint angles are measured through encoders. The kinematics is already known, but in this research the coordinate system is adapted to overlap with the slave coordinate system so that no coordinate transformation is needed. Therefore new kinematics should be derived. In Figure 2. the master robot is depicted with all necessary parameters. Figure 2. Master robot 7

9 All parameters marked with a are for the master, the ones marked with a are for the slave. As can be seen, the coordinate system is placed at a certain height above the base of the robot. This is because in that way the coordinate system of the master and slave overlap and no extra coordinate transformation is needed. The position of the end effector can in this way directly be used by the slave robot. The forward kinematics can be derived from Figure 2. as such. = cos + cos sin = sin + sin = cos + cos cos = (2.) Parameters used for the lengths of the links of the master robot are shown in Table 2.. Table 2. Master link lengths Slave kinematics The slave system is a planar robot developed and already described (Muro Maldonado, 27). It is a 2 degree of freedom robot consisting of 3 links and 3 joints, so it is redundant. The joints are actuated by motors and joint angles are measured by encoders. Forward and inverse kinematics are already known, but for a different coordinate system, so these need to be adapted. In Figure 2.2 the slave robot is shown with all its important parameters. Figure 2.2 Slave robot The orientation of the end effector, which is determined by the 3th link of the slave robot, can be chosen arbitrarily because the slave robot has a redundant link. This orientation is defined as the angle between the third link and a vertical line. Possible choices for the orientation are = or = 2 in which the third link is vertical and horizontal respectively. A more advanced 8

10 choice is =atan in which the absolute distance between the origin and the end effector is as large as possible for all choices of and. The forward kinematics of the slave robot is given by the following equations. = sin + sin + + sin + + = cos + cos + + cos + + = (2.2) For the slave robot the inverse kinematics is also needed when designing a teleoperation system. This is defined in the following way, for a complete derivation, see Appendix A. =atan sin cos acos + 2 = acos + 2 = (2.3) Parameters used for the lengths of the links of the slave robot are shown in Table 2.2. Table 2.2 Slave link lengths Dynamics The dynamics of the two robots can be derived using Lagrange or Newton-Euler equations. But in this research it has been chosen to measure the frequency response of the robots to keep the models of the robots simple and linear. Although the existing robots are not linear in general, this method is a good alternative to investigate if it is possible to design a teleoperation system with linearized system models. The frequency responses can be fitted by transfer functions to obtain dynamical models which can be used in simulations. Measuring frequency response functions is a known good and an already documented method (Boot, 23) and (Elling, 25). Generally frequency responses can be measured in two ways, open loop and closed loop. In most cases closed loop is preferable; open loop can be dangerous because you cannot know how the system reacts on the applied input. In Figure 2.3 the general scheme is shown for measuring closed loop frequency responses. In this scheme, is the controller and is the system to be measured. Signal is a noise signal, which acts as a disturbance and signals and are the input and output signals to and from the robots. d C u H y Figure 2.3 General scheme for measering closed loop frequency responses 9

11 From the signals, and the frequency response of system can be determined in the following two ways. If the controller is known, then it suffices to calculate the sensitivity function () from which the system response can be calculated. = = + = (2.4) The other way is to calculate the process sensitivity () too and divide this by the sensitivity () from which the system () remains. = = + = + + = (2.5) The first way is more common because in general the controller is known or otherwise can be measured too. Using this information frequency responses are measured for the master and slave robot and fitted using routine frsfit from the DIET toolbox in Matlab (Steinbuch) to obtain dynamical models Master dynamics The frequency response of the master robot is measured using both methods described earlier. In Appendix B Simulink schemes for a closed loop measurement can be found. One by one each of the three links of this robot is actuated while all links are measured to determine if the links of the system are decoupled. During the measurements a sample rate of 4 Hz. is used for seconds to retrieve enough data. The controllers are measured off line in a similar way to be able to also calculate the frequency response function through the process sensitivity and compare this with the sensitivity method. After the frequency responses are retrieved, the data are fitted to second order transfer functions. It has been chosen to use second order transfer functions. Higher order transfer functions could also be used, but this would make the design of the teleoperation system more difficult. In Figure 2.4, Figure 2.5 and Figure 2.6 the frequency responses and fitted transfer functions are shown for the first, second and third link of the master robot respectively. As can be seen in these figures, the measured data of both methods overlap exactly, so both methods give the same results. From that it can be concluded that the measurements are correct. The system behaves as a second order system with for low frequencies a slope of - and for high frequencies a slope of -2. Also some resonant/anti-resonant peaks can be found and from this one might conclude that the system is not critically damped. The total fitted open loop transfer matrix for the master robot is as such = (2.6)

12 Because slow inputs are used, the links which are not actuated do not move. This means that the off-diagonal terms are zero or can be neglected. So the robot is totally decoupled when using slow inputs and therefore consists of three separate SISO systems. Magnitude [db] Phase [º] -2 2 Frequency [Hz] Figure 2.4 Frequency response and transfer function fit of first link of master robot Magnitude [db] Phase [º] -2 2 Frequency [Hz] Figure 2.5 Frequency response and transfer function fit of second link of master robot

13 Magnitude [db] Phase [º] -2 Figure 2.6 Frequency response and transfer function fit of third link of master robot Slave dynamics 2 Frequency [Hz] In a similar way the dynamics of the slave robot is determined. One by one each link is actuated and all links are measured. A sample rate of 4 Hz is used for seconds. Frequency responses are calculated in both ways and fitted with second order transfer functions. Again can be seen that the measured data of both methods overlap exactly, thus both methods give the same results. From this it can be concluded that the measurements are correct. Just like the master robot, the slave robot behaves like a second order system with for low frequencies a slope of - and for high frequencies a slope of -2. There are no resonant peaks visible and therefore this is a critically damped system. Magnitude [db] Phase [º] -2 2 Frequency [Hz] Figure 2.7 Frequency response and transfer function fit of first link of slave robot 2

14 Magnitude [db] Phase [º] -2 2 Frequency [Hz] Figure 2.8 Frequency response and transfer function fit of second link of slave robot Magnitude [db] Phase [º] -2 2 Frequency [Hz] Figure 2.9 Frequency response and transfer function fit of third link of slave robot For the slave robot, the total fitted open loop transfer function matrix looks like the following = (2.7) Because slow inputs are used, the links which are not actuated do not move. This means that the off-diagonal terms are zero or can be neglected. So the robot is totally decoupled when using slow inputs and therefore consists of three separate SISO systems. With the system identifications derived in this chapter, controllers can be designed for the bilateral master slave teleoperation system. 3

15 3 Teleoperation system design After the systems are identified a teleoperation system can be designed. In the past more research about teleoperation systems has been done. Global research has been done to determine control strategies and experiment with different kinds of teleoperation control (Flemmer, 24) and (Marcassus, et al., 26). More in depth research has been done on specific robots, most of them using analytical dynamic expressions (Alise, et al., 27), (Cortes Martinez, 27), (Rodriguez Angeles, et al., 27),(Hashtradi-Zaad, et al., 22) and (Kwon, et al., 2) but also using frequency response functions (Yan, et al., 996). Teleoperation systems always consist of more than one robot, interacting with each other. There are two ways of interaction possible, unilateral and bilateral. In a unilateral way one robot is sending data, the others are receiving; bilateral means that all robots are sending and receiving. In this report we focus on two robots interacting in a bilateral way sending and receiving position and force information to and from each other. The master robot, which is moved by an operator, sends its position to a slave robot that tries to move to the same position. As long as the slave robot is not in contact with the environment this teleoperation is unilateral, because no force is present. However, when the slave robot comes in contact with the environment, a reaction force originates. This force is sent to the master robot that tries to give feedback to the operator with the same force. In that case a bilateral teleoperation system is active. The general scheme of a bilateral position/force teleoperation system as developed in this report is shown in Figure 3.. Operator Master Controller Master Robot Slave Controller Slave Robot Master Virtual Evironment Slave Virtual Evironment Figure 3. General bilateral master slave teleoperation scheme Seven important parts can be distinguished in this scheme. The master and slave robot of course, furthermore a master and slave controller, an operator and two virtual environments. The master and slave robot are identified in chapter 2 and are described in transfer function matrices and respectively. The equations describing the two robots in this way are: = + + = + + = + = + (3.) 4

16 In these equations,,,, are torques applied by the operator, master controller, master virtual environment, slave controller and slave virtual environment respectively. The forces from the virtual environments, and, can be mapped to torques using a Jacobian or depending on the robot and virtual environment used. Four parts remain to be developed. The virtual environment is simulated using two different approaches, an impedance model and a holonomic constraint model. Second a master controller needs to be developed which controls the force the slave robot feels such that the operator feels the same, combined with a dynamical compensation controller so that the operator does not feel the master robot and transparency is improved. Also a slave controller needs to be designed that needs to imitate the positions of the master robot with a position controller combined with a force controller if it is in contact so that it is pushing on the environment just as hard as the operator does on the master robot. Finally, for simulation reasons an operator needs to be developed that controls certain desirable trajectories on the master robot. 3. Virtual environment A virtual environment is needed because no force sensor is available, neither in simulations, nor during experiments. The virtual environment calculates forces that are present when one or both robots are in contact with the environment. These forces react on the robots and need to be controlled by controllers such that the robots are not pushing too hard or too soft on the environment. Two different models for the virtual environment are developed, an impedance model and a holonomic constraint model. 3.. Impedance model An impedance model describes an environment as a mass-spring-damper system. When a robot penetrates the environment a reaction force originates with amplitude depending on the inertia, stiffness and damping of the environment (Lewis, et al., 993) and (Spong, et al., 989). In Figure 3.2 a graphical representation is shown of a robot pressing on an environment with inertia, stiffness and damping. Figure 3.2 Graphical representation of an impedance model The resulting force due to penetrating an environment can be calculated using the following equation for master and slave respectively. = + + > = (3.2) 5

17 = + + > = (3.3) As environment a flat horizontal surface has been chosen at position, and are the vertical positions of the master and slave end effector respectively. These equations are only valid when the robot is penetrating the surface of course. Only one force direction is taken into account, namely only the force acting perpendicular to the environment. This is because it is very difficult, maybe even impossible, to determine the parameters of inertia, stiffness and damping in all possible directions. This means that the end effector is only allowed to touch the environment perpendicular. In simulations and experiments this is also easier to achieve. The advantage of an impedance model is that it is very simple and that the computational effort is very low when calculating the force. However a disadvantage is that the robot really needs to penetrate the environment to originate a force and that the force is dependent on the position. This makes it harder for an operator to control the system because in general it will not be possible to reach the desired position and force at the same time. Only the desired position can be reached with a force error or the desired force is reached with a position error. A holonomic constraint method does not have these disadvantages, but is more difficult and takes more computational effort to solve Holonomic constraint model A holonomic constraint is a constraint in position for the end effector of a robot. It is modeled as if the robot is attached to the environment and only can move tangent to it. Due to this restriction, a degree of freedom disappears and therefore a reaction force is originated. A holonomic constraint only occurs when the robot is in contact with an environment. The position, velocity and acceleration are constrained to directions tangent to the environment only, while the force is constrained in a perpendicular direction. A holonomic constraint is rigid and the robot cannot penetrate the environment (Liu, et al., 999) and (Liu, et al., 997). In Figure 3.3 a graphical representation of a holonomic constraint is shown. Figure 3.3 Graphical respresentation of a holonomic constraint model To calculate the resulting force, the constraints need to be added to the equations describing the master and slave robot. For the master and slave the following constraints are used respectively. = = sin + sin = = cos + cos + + cos + + (3.4) In these formulas is again the position of the environment vertical direction, i.e. the same horizontal plane that is used in the impedance model. Adding these equations to the dynamic equations of the master and slave robot results in the following systems, which can be used to calculate the reaction force from the environment. 6

18 = + + = + + = = + = + = (3.5) (3.6) By defining = and = it means that the robots are in contact with the environment and that the position, velocity, acceleration and force are constrained. In practice however the differential equations could become too stiff and one may run into computational problems. Therefore a small boundary layer around the environment is defined and the constraints to solve become = and =. The total system is now a Differential Algebraic Equation (DAE) and can be solved by using the following expressions (Ortega Aguilera, 26) and (Shampine, et al., 999). =+2 + = =+2 + = (3.7) In this model, and are called Lagrange multipliers representing the force and and are parameters that represent the stiffness and damping of the virtual environment. To calculate the reaction force from the environment, simply integrate and with respect to time. = = (3.8) The advantage of a holonomic constraint model is that the robot cannot penetrate the environment and that the force is not dependent on the position. Therefore it is possible for an operator to reach the desired position and force. Rigid environments can be used in this way, something impossible for an impedance model. A disadvantage however is mentioned earlier; the model is more difficult and computationally harder to solve because of the DAE that needs to be solved. 3.2 Master controller The master controller is used in two ways. First it takes care of the force feedback from the slave when that robot is in contact with an environment. It needs to compare the force calculated by the slave virtual environment and the force calculated by the master virtual environment. Its function is to make sure the force calculated by the master virtual environment is the same as the one coming from the slave to achieve force feedback with high transparency. That is also the reason why the master side also needs a virtual environment, without virtual environment it is impossible to know what force is acting on the master at the moment and therefore it is impossible to know what force is felt by the operator. The second part of the master controller is a dynamical compensation controller. To increase transparency it is important that the operator does not feel that it is handling the master robot. Dynamics included in the master system are for example gravity, inertia and friction. There already exist researches for dynamical compensation, even in teleoperation systems(checcacci, et al., 22) and(goto, et al., 26), but these are all based on Newton Euler or Lagrange system 7

19 equations. A method usable with transfer functions is the internal model control method (Tham, 22), which uses an inverse model of a system as controller to compensate for the systems dynamics. This can be written in an expression as follows. = + (3.9) With the master torques which will be sent to the master robot, the torques calculated by the internal model controller and the torques calculated by the force controller Force controller The force part of the master controller receives a force from the slave virtual environment ( ) and master virtual environment ( ). It uses a PID force controller to control the force the operator is feeling as force feedback. It is controlling the master force by using the slave force as set point. = + + (3.) The parameters, and are controller gains that can be tuned for performance. It should be noted that is often chosen very small or even zero because the derivative of the force is not present or very noisy. Because the forces are expressed in the Cartesian coordinate system, the resulting controller output needs to be multiplied with a Jacobian to map the joint torques. This Jacobian is different for the two virtual environments. When using the impedance model it is the Jacobian of the master robot configuration, denoted by. = cos + cos cos = sin sin cos (3.) sin sin cos But when a holonomic constraint model is used it should be the Jacobian of the constraint, denoted by. = = cos (3.2) cos Internal model control The dynamics of the master robot are compensated to increase the transparency. When the operator does not feel that it is handling the master robot the transparency of the system is increased. There are several ways to compensate the dynamics of a robot. Most of them use dynamic relationships to calculate compensation controllers (Checcacci, et al., 22) and (Goto, et al., 26), but using the identified frequency responses of the robots it is possible to use the internal model control method (Tham, 22). In Figure 3.4 the general scheme for an internal model controller is shown. 8

20 r Inverse system model u System y v System model m Figure 3.4 General internal model control scheme The internal model principle uses a model of the system. This model is subtracted from the system output to obtain the difference of the system and the model. When the inverse of the model is used as controller it is possible to compensate for the dynamics of the system. If the model and system are exactly the same, then the difference is zero and an open loop controller remains. In that case the controller is exactly the inverse of the system and is therefore cancelling the system, resulting in the output being exactly the same as the input reference. In practice it is of course not possible to obtain a model which is exactly describing the real system. As input reference the signal of the master joint angle encoders is used. 3.3 Slave controller Also the slave controller is used in two ways. First, of course, it uses a position controller to imitate the position of the master robot. It receives the master end effector position in Cartesian coordinates and uses the inverse kinematics to calculate the corresponding joint angles. These angles are used as set point for the controller. The second part is a force controller. The force calculated by the master virtual environment is sent to the slave and used as set point to control the slave virtual environment force. This force needs to be controlled because it should reflect the force with which the operator is pushing on the master virtual environment. Pushing too hard can result in damage to the environment and pushing too soft will not result in the desired effect, therefore a slave force controller is also needed. Combining both control parts in an expression results in the following. = + (3.3) With the slave torques which will be sent to the slave robot, the torques calculated by the position controller and the torques calculated by the force controller Position controller The position controller of the slave robot receives a desired end effector position from the master robot, which is calculated with the forward kinematics of the master robot from its joint angles. The slave controller uses this information to calculate the desired torque angles by using the inverse kinematics of the slave robot. The controllers are tuned using the frequency response data to achieve high performance and in that way a high transparency. Controller tuning is described a lot of times and there exist plans for how to design a controller. The frequency response data are loaded with the Matlab toolbox Shapeit (Bruijnen), with which controllers can be tuned in an easy way. All three controllers 9

21 consist of a lead filter in combination with an integrator action and first order low pass filter and are tuned to have a bandwidth of approximately 4 Hz. + = + + (3.4) The parameters,,, and are the tunable parameters for this controller. When using the holonomic constraint model as virtual environment one needs to be sure that the positions, velocities and accelerations are always tangent to the environment when in contact. To achieve this the Joint Space Orthogonalization method (JSOM) is used (Liu, et al., 997). The torques to control the position of the slave robot need to be mapped using a mapping matrix defined as such. = (3.5) Where is the identity matrix and the other part is the transpose of the Jacobian times the so called pseudo inverse of this Jacobian. When in contact, the position controller of the slave robot becomes: = (3.6) In this way it is guaranteed that the robot is only moving tangent to the environment and is not trying to move into it Force controller On the slave side a force controller is also needed to control the force with which the slave robot is pressing on the environment. This controller is again a PID force controller and uses the force from the master virtual environment ( ) as desired force to control the force acting on the slave virtual environment ( ). = + + (3.7) The parameters, and are controller gains that can be tuned for performance. It should be noted that is often chosen very small or even zero because the derivative of the force is not present or is very noisy. Because the forces are expressed in the Cartesian coordinate system, the resulting controller output needs to be multiplied with a Jacobian to map the joint torques of the slave robot. This Jacobian depends on the virtual environment that is used. For the impedance model it is the slave robot Jacobian, denoted by. = cθ = + + (3.8) with cθ =cos θ, cθ θ =cos θ +θ, etc. and =sin, =sin +, etc. 2

22 But when a holonomic constraint model is used, it should be the Jacobian of the slave constraint, denoted by. = = (3.9) 3.4 Operator In simulations it is not possible to interact with the master robot and therefore an operator needs to be designed that acts as a human being. The operator is modeled with two PID controllers, one for position and one for force. The operator uses desired trajectories for position ( ) and force ( ) to control the actual position and force of the master robot. Therefore it receives the master joint angles ( ) and force calculated by the master virtual environment ( ). = (3.2) The parameters,,,, and are controller gains that can be tuned to make sure that the desired trajectories are close to the master robot positions and forces. When the holonomic constraint method is used as virtual environment, it should be noted that trajectories need to be designed that are not trying to penetrate the environment. The four remaining parts, virtual environment, master controller, slave controller and operator, are designed and can be tested in simulations. 2

23 4 Simulations After the teleoperation system is designed it can be simulated in order to see if the design is correct. Also it is safer to first check the controller in simulations before actually applying it on real robots in experiments. Trajectories are very important in this case, because they need to be smooth when touching the environment, otherwise the robot can start to bounce. Smooth trajectories are designed using Bezier polynomials and implemented in a simulation using the impedance model as virtual environment and a simulation using the holonomic constraint model as virtual environment. 4. Bezier polynomials Trajectories in the simulations and experiments need to be really smooth, otherwise the robot can start to bounce, which is of course undesirable. This bouncing occurs when the robot touches the environment while still moving or even accelerating. Then suddenly there originates a force which cannot be compensated by the force controllers fast enough and the robot is pushed away from the environment again. This behavior can repeat itself several times before the robot finally comes to rest on the environment. Using smooth trajectories which guarantee that the robot touches the environment with a very low velocity and no acceleration, this bouncing behavior will not occur. Smooth trajectories can be designed with Bezier polynomials. Bezier polynomials are a class of polynomials with interesting properties (Faraway, et al., 26). In the case of two variables, position and time, the following equation describes a Bezier polynomial. = (4.) A visualization of a Bezier polynomial is shown in Figure 4.. Figure 4. Example of Bezier polynomial of order 3 The order of a Bezier polynomial is determined by. This value determines through how many points the curve is fitted. Each individual point is called a control point ( ) and the line always starts at and ends at. The other points determine the shape of the line and by choosing those correctly the desired trajectory can be designed which is always smooth. The trajectory is not an equation, but a matrix containing time samples and the belonging coordinates. To create this matrix a linear vector with values between and is needed. For each value in the result of the equation is a coordinate stored in. So in this case contains coordinates of time and position. 22

24 4.2 Impedance model To test if the controller using the impedance model as virtual environment is correct, simulations have been done, before implementing it on the real robots in an experiment. Simulations have been done using Matlab Simulink, the Simulink schemes can be found in Appendix C. Using the Bezier polynomials trajectories for the simulation using the impedance model as virtual environment are designed. Figure 4.2 and Figure 4.3 show the position trajectory and force trajectory respectively. Parameters, and are the desired joint angles for joint, 2 and 3 of the master robot respectively. joint angle [rad] θ dm θ 2dm θ 3dm time [s] Figure 4.2 Position and orientation trajectories in simulation with impedance model as virtual environment 8 force F edm [N] time [s] Figure 4.3 Force trajectory in simulation with impedance model as virtual environment The simulation is started by lifting the master robot about 3 cm. without applying force. Then the master robot is slowly moved towards the environment till it makes contact. After that a 23

25 force is applied of N and 5 N respectively. During the time a force of 5 N is applied the master robot is moved about 6 cm in horizontal direction tangent to the plane. After that the force is released, while the robot is moved back to its original horizontal position. Finally when the reaction force is zero, the robot is lifted from the surface and the whole simulation starts again. The orientation of the master robot is kept vertical during the whole simulation. The parameters used in this simulation are shown in Table 4., Table 4.2, Table 4.3 and Table 4.4. Table 4. Virtual environment parameters Table 4.2 Master control parameters 2 5 Table 4.3 Slave control parameters Table 4.4 Operator control paramaters The errors in position and force are plotted in Figure 4.4 and Figure 4.5 respectively. These errors represent the difference in position and force between the master and slave robot. = = = = (4.2) From these results we can conclude that the simulation works fine. The slave robot cannot follow the master robot perfectly. There are errors visible when the master robot starts to move or push on the environment, but these errors remain very small, in the order of % of the desired trajectory. They also converge to zero very quickly when the master robot stops moving or is pushing with a constant force. The maximum error in position is 2.5 mm, in orientation.6 rad and in force. N. Peaks in the errors occur when touching and releasing from the environment due to the switching behavior of the force at that moment. 24

26 position and orientation error e x [m] e y [m] e φ [rad] time [s] Figure 4.4 Position and orientation error in simulation with impedance model as virtual environment.2.5 force error e F [N] time [s] Figure 4.5 Force error in simulation with impedance model as virtual environment As stated in chapter 3, when using the impedance model the operator cannot reach both the desired position and force at the same time, because the force is dependent on the position. This effect can be seen in Figure 4.6 and Figure 4.7. The errors represent the difference between the desired position and force used by the operator and the actual position and force of the master robot. = = = = (4.3) 25

27 . joint angle error [rad] e o e 2o e 3o time [s] Figure 4.6 Position and orientation error for operator in simulation with impedance model as virtual environment.5. force error e Fo [N] time [s] Figure 4.7 Force error for operator in simulation with impedance model as virtual environment The errors in the position are relatively large compared to the errors in the force, up to 5% of the desired trajectory in position and only % in force respectively. This would mean that the force controller is stronger than the position controller. The force controller forces the robot to penetrate the environment even if the position controller is trying to keep it exactly on the surface. This behavior can be decreased by tuning the controllers in a different way, but both errors can never be zero at the same time. To reach that, the holonomic constraint model as virtual environment is needed. 4.3 Holonomic constraint model The theory for the holonomic constraint model as virtual environment should also be tested before real experiments on a robot can start. The simulation with the holonomic constraint 26

28 model as virtual environment is simulated with Matlab Simulink, the corresponding Simulink schemes can be found in Appendix C. Trajectories for this simulation are slightly different from the ones with the impedance model as virtual environment. This is because in this case the robot cannot penetrate the virtual environment. The trajectories for position and force, designed using Bezier polynomials, are shown in Figure 4.8 and Figure 4.9 respectively. joint angle [rad] θ dm θ 2dm θ 3dm time [s] Figure 4.8 Position and orientation trajectories in simulation with holonomic constraint model as virtual environment 8 force F edm [N] Figure 4.9 Force trajectory in simulation with holonomic constraint model as virtual environment The parameters used in this simulation are shown in Table 4.5, Table 4.6, Table 4.7 and Table 4.8. Table 4.5 Virtual environment parameters time [s] 27

29 Table 4.6 Master control parameters. Table 4.7 Slave control parameters Table 4.8 Operator control paramaters The errors in position and force are plotted in Figure 4. and Figure 4. respectively. These errors represent the difference in position and force between the master and slave robot. Again we can conclude from the results that the simulation works fine. Errors occur when the master robot starts to move or push on the environment, but remain very small, in the order of % of the desired trajectory. They also converge to zero again when the master robot stops moving or is pushing on the environment with a constant force. The maximum error in position is mm, in orientation. rad and in force.3 N except for the peaks that occur when touching and releasing from the environment due to the switching behavior of the force at that moment. More peaks are visible here because the robots cannot penetrate the environment when using the holonomic constraint method as virtual environment. position and orientation error e x [m] e y [m] e φ [rad] time [s] Figure 4. Position and orientation error in simulation with holonomic constraint model as virtual environment 28

30 . -. force error e F [N] time [s] Figure 4. Force error in simulation with holonomic constraint model as virtual environment The operator problem is no longer visible when the operator is trying to reach a desired position and force simultaneously as can be seen in Figure 4.2 and Figure 4.3. The errors represent the difference between the desired position and force from the operator with the actual position and force from the master robot. In this case, the errors are both small (approximately 5% of the desired trajectory) and when the position error shows a peak, for example when the robot is released from the environment, then does the force error too. This means that the force and position in this simulation are decoupled and both controllers are doing what they are designed for..8 e o joint angle error [rad] e 2o e 3o time [s] Figure 4.2 Position and orientation error for operator in simulation with holonomic constraint model as virtual environment 29

31 .25 force error e Fo [N] time [s] Figure 4.3 Force error for operator in simulation with holonomic constraint model as virtual environment 3

32 5 Experiments Experiments on real robots can be done after simulations have shown that the design might work. The designed teleoperation system is implemented on a real experimental platform. The experiment is set up with the master parallel robot virtually in the computer simulation connected with the real slave planar robot. Experiments with the real master parallel robot and a virtual slave running in a computer simulation are already done (Cortes Martinez, 27). So if this setup works, one might expect the total teleoperation system to work in experiments on both real robots. This can be tested in a next research. In the setup discussed in this report, the slave planar robot is connected to a power box already designed and built (Muro Maldonado, 27). The power box is connected to a computer using the Sensoray 626 data acquisition card. The computer is running on Microsoft Windows XP Service Pack 2. Experiments have been done using Mathworks Matlab 27b and Simulink 7 in combination with Real Time Windows Target. Experiments have been done using the ode3 Bogacki Shampine solver with a sample rate of Hz. During experiments almost the same trajectories are used as in the simulations. The trajectories are designed using Bezier polynomials and implemented in experiments with the impedance model as virtual environment and the holonomic constraint model as virtual environment. 5. Impedance model Simulations of the impedance model show that the theory and design work, so real experiments can be done using the slave planar robot. Simulink schemes of the experiment are the same as in the simulations and can be found in Appendix C. The trajectories for position, orientation and force used in this experiment are shown in Figure 5. and Figure 5.2. joint angle [rad] θ dm θ 2dm θ 3dm time [s] Figure 5. Position and orientation trajectories in experiment with impedance model as virtual environment 3

33 8 force F edm [N] Figure 5.2 Force trajectorie in experiment with impedance model as virtual environment For this experiment, the controller gains and virtual environment parameters used are shown in Table 5., Table 5.2, Table 5.3 and Table 5.4. Table 5. Virtual environment parameters Table 5.2 Master control parameters time [s] Table 5.3 Slave control parameters Table 5.4 Operator control paramaters The errors in position, orientation and force are plotted in Figure 5.3 and Figure 5.4 respectively. These errors represent the difference in position and force between the master and slave robot. 32