- Email: [email protected]
AN ARCHITECTURE FOR FORCE AND IMPACT CONTROL
AN ARCHITECTURE FOR FORCE AND IMPACT CONTROL1 Zotovic Ranko*, Ángel Valera Fernández*, Pedro José García Gil*, Ángel Llosá Guillén* * Departament of Systems Engineering and Control Universidad Politecnica de Valencia P.O. Box. 22012, E-46071, Valencia, Spain. e-mail: rzotovic, giuprog, [email protected], [email protected]
Abstract: An architecture for force and impact control is proposed. Its objectives are at first to control the robot in free motion until the contact with the environment is made. Next, it should make the transition phase as smooth as possible. Finally, it should switch to force control in order to assure the tracking of the force reference. An absolute priority is to protect the system against the dangerous peaks of force. Also, loss of contact should be avoided when possible. The performance is improved by taking care of the accumulation of elastic energy in the robot. Sampling period is changed depending on the state of the system. An experimental platform was developed in order to test the architecture. Copyright © 2006 IFAC Keywords: force control, impact control, impact, supervision, switching, discontinuous control.
1. INTRODUCTION The control architectures are widely studied in mobile robotics in the last decades. In this period they made an important evolution.
Usually they consisted of several behaviours (goal attraction, obstacle avoidance, wall following, etc.), whose outputs were combined (behaviour fusion) and/or alternated (behaviour arbitration) to obtain the final path of the vehicle.
At first, in the sixties, the hierarchical paradigm in terms of the Sense - Plan – Act primitives was proposed. The results were unsatisfactory due to the slow response.
Nevertheless, the reactive architectures had some limitations and the relatively complicated tasks needed planning and therefore deliberative levels to achieve it.
In the mid-eighties a revolution has been made with the first reactive architecture by Brooks (1986). The introduction of the reactivity made faster the response of the robots to the unpredictable events, allowing their application in unknown or changing environments.
An improvement was made with the hybrid paradigm. Several hybrid architectures have been proposed, to mention Konolige (1996). It is a combination of the hierarchical and the reactive paradigm, keeping the advantages of both of them. The hybrid architecture consists of a lower, faster, reactive layer and one or more higher, deliberative layers. The output of the higher layers is the input to the lower one where it is treated like one behaviour more.
1
This work has been partially funded by FEDER-CICYT project with reference DPI2005-08732-C02-02 financed by Ministerio de Educación y Ciencia
At least one layer above the reactive is necessary in order to select which behaviours are active (arbitration) or to interpolate the effect of different behaviours (fusion).
The force control has some similarities with the mobile robotics. At first, a robot should perform different actions in different cases: control the speed in free motion, try to make the transient phase to constrained motion as smooth and as short as possible, protect the robot by any means against dangerous peaks of force, try to avoid the contact losses if possible, and finally, when the transient oscillations are sufficiently damped, try to reach the force reference value. At second, a priori knowledge of the environment characteristics (mechanic and geometric) may be wrong or inexistent, just like in mobile robotics. Otherwise it would be easy to execute a force control task. Third, the reaction of the controller must be very fast, otherwise the robot could damage itself and/or the environment. This specially true in the impact instant, when there is a very abrupt change in the dynamics of the system. Fourth, several different sensor may be used. For the realisation of this article, only force, position and velocity sensors where used. There is a possibility to add others like artificial vision, accelerometers, distance sensors, etc. In this paper, an architecture for force control, or at least its lower level, is proposed. In the second section its objectives are explained. The third section is dedicated to the description of the architecture. The fourth to the description of the experimental platform. The final one to the conclusions. The detailed explanation of the architecture would need much more space then it is allowed. For this reason, the descriptions were abbreviated and only the most relevant results are graphically represented. The data for the experiments (like reference of the force, maximum force, etc...) are chosen rather for test the general performance of the system than for a particular application. It is assumed that in the real applications they will be defined by the user according to the characteristics of the task. This article is limited to the case of a one degree of freedom robot. 2. OBJECTIVES
the robot, the environment or the tool. 2. The velocity in free motion should not be too low in order to avoid the excessive duration of the task. 3. The contact losses should be avoided as possible. They make the execution of the task slower, provoke wear of the mechanics and even can make the system unstable, 4. If it is not possible to avoid losses of contact, their number should be limited. In addition to these initial requirements, the experiments demonstrated the need to limit the accumulation of the elastic energy in the robot and the environment, when it is possible. It is useful to limit the peaks of the force, stabilise the system and make the convergence faster. Another, objective was to verify the effect of the use of different sampling periods in free and constrained motion. 3. THE ARCHITECTURE The architecture of the system consists of several different states the system can adopt depending on the actions to be taken. It can be only in one state at the time. The following possible states were identified: • • • • • •
Approximation to the environment Impact control Force control Dangerous value of force (red zone) Dangerous value of force (yellow zone) Danger of loss of contact
Each of these states may have a different control strategy, controller, reference value, and/or sampling period. Following, a few words will be told about each state. 3.1 Approximation to the environment. In order to achieve force control, the robot must be in contact with the environment, which is not the case in the beginning of the task. The purpose of this state is to take the robot towards the environment until the contact is achieved. It is also used in the case of contact losses.
Before starting the design of the architecture, its objectives were defined. Following, they are enumerated in order of importance and briefly explained.
During this state, the robot is in free motion. The dynamics of the system is relatively slow, allowing a lower sampling frequency. The value of 200 Hz was used. For all the other states the sampling frequency was set to 8 kHz because the dynamics is much faster in constrained motion.
1. The force should never exceed a given maximum value. It is a matter of security because otherwise an irreversible damage could be caused to
The main dilemma for this state was between position and velocity control. The former required precise knowledge of the position of the environment: if this
distance was overestimated, the impact could be very dangerous. If it was underestimated, the robot did not reach the environment. With the velocity control the contact is assured and the impact does not depend on the position of the environment. The velocity control has also some other advantages like the possibility of easily reducing the speed in the proximity of the environment, or to assign lower values of reference speed in the case of contact losses.
Similar ideas where proposed in Armstrong, et al., 1997, Armstrong and Wade (2000), Armstrong, et al., 2001 and Xu et al., 1995.
3.2 Impact control In order to abbreviate the task, the velocity in free motion should be as high as possible. It means that in the instant of the impact, the kinetic energy of the system is relatively high. This kinetic energy is transformed in elastic potential energy of the environment and the robot once the contact is made. Due to the very high stiffness of the materials used in industrial applications, the system is highly underdamped in constrained motion. High frequency oscillations occur and very dangerous peaks of force can be reached. The impact is, although very brief, the most dangerous part of the task. Due to the very short duration, an adaptive control may not be efficient for the protection of the system. The purpose of the impact control state is not to track a reference value. It is to dissipate as fast as possible the kinetic energy of the robot acquired in free motion. The following control law was applied during the impact control: u = K p ( Frefi − f ) − K v x
(1)
Where u is the control action, Kp is the proportional constant of the force, Frefi is the force reference value during the impact phase, that may be (or not) smaller than the nominal value, Kv is the velocity constant and x is the robot speed. No integrator because it would increase the peaks of force. On the other hand it is unnecessary because tracking the force reference is not the purpose of this state. In order to improve the performance, the proportional constant is switched between two values according to the following law: K p1 if Kp = 0 if
( Frefi − f ) x < 0 ( Frefi − f ) x > 0
higher then the reference) and a the same time it keeps penetrating more (which is reflected by a positive velocity), then the system should be braked, what is obtained assigning a high value to the proportional constant. On the contrary, if the robot has penetrated too deep and is retiring, there is no need for a proportional control action since the system is leaning towards the reference value by itself. The application of a proportional gain would accelerate the robot and its speed would be higher when it reaches the reference value, making also higher the next peak of force.
(2)
This allows a faster convergence of the system and even may make it stable if it is not. The demonstration of this switching law is beyond the scope of this article. The basic idea is the following: if the robot has penetrated too much in the environment (which is reflected by a value of force
The validity of the switching criteria from equations (2) is experimentally verified. The results are represented in the following figures. 8000 7000 6000 5000 4000 e cr o F
3000 2000 1000 0 -1000
0
500
1000
1500 Samples
2000
2500
3000
Fig. 1. Graphical representation of the force without (solid line) and with (dotted line) switching the proportional constant according to (2). The force is measured in grams-force instead of Newtons in order to obtain better resolution. 3
2.5
2
et at S
1.5
1
0.5
0
0
500
1000
1500 Samples
2000
2500
3000
Fig. 2. Graphical representation of the state without (solid line) and with (dotted line) switching the proportional constant according to (2). State zero corresponds to free motion, one to impact control and two to force control.
3.4 Dangerous value of force (red zone)
5 4
This state is the ultimate protection of the system against the peaks of force that could damage the robot, the environment or the tool, whichever is the most fragile.
3 2 n oi t c A l or t n o C
1 0 -1 -2 -3 -4 -5
0
500
1000
1500 Samples
2000
2500
3000
Fig. 3. Graphical representation of the control action without (solid line) and with (dotted line) switching the proportional constant according to (2). The graphics show that the system is unstable if there is no switching of the proportional constant. The system enters the impact control phase but doesn’t leave it. On the contrary, if the switching of the proportional constant is made, the system enters the next state after a relatively brief time lapse. The transition to the next state (force control) should be made after the impact is dumped enough. To determinate when this has occurred the following criterion was used: the peaks of the velocity were detected in every period of oscillation. When the peak was lower than 10% of the impact velocity, it was assumed that enough energy was dissipated by the impact controller and that the system had more or less reached a steady state. 3.3 Force control The purpose of this state is to track the reference value of force. This state is reached when the transient phase is finished and there is no more danger of high peaks of force that could damage the system. Since the force regulation by itself is not the purpose of this article, and taking into account the lack of space, it will not be further analysed. This subject was studied in detail in Volpe and Khosla, 1993. Just to mention that an integrator should be used in order to reach the reference value. Although the fact that this state has been reached means the impact has already been damped, the possibility of dangerous peaks of force still exists due to irregularities in the environment or vibrations. The monitoring of the system should continue in order to activate any of the other states if necessary.
The idea of this state is very simple: if the force exceeds a threshold value, to apply the maximum control action in order to stop the robot. Given the elasticity of the robot, this threshold value of the force should be considerably lower value that can provoke mechanical damage. Once the robot starts retiring, the maximum negative control action there is not necessary anymore. On the contrary, it could cause a high velocity of the robot in the opposite direction and subsequent loss of contact. For this reason, when the force derivative becomes negative, the control is set to a pure active damping. It has been observed that the performance can be improved if taking into account the accumulation of the elastic potential energy in the robot. It was assumed that the robot is compressing if the velocity is positive while the force derivative is negative. This would mean that the two extremes of the robot are going towards its center. If the velocity and the force derivative have the same sign, it is not possible to determinate if the robot is compressing or expanding. The final control action is defined by the following rules: − u max u = − k v x − u max
if if if
f > Fmax & df > 0 f > Fmax & df < 0 & x < 0 f > Fmax & df < 0 & x > 0
(3)
Where umax is the maximal control action allowed by the actuator, Fmax the force threshold that activates this state, and df the force derivative. The meaning of the first case is simply that if the force exceeds the potentially dangerous threshold and at the same time it is increasing, the maximum possible control action should be applied in order to stop the robot as fast as possible. The second and the third case occur when the robot, or at least the part where the force sensor is, starts retiring from the environment. When this happens, the force begins to decrease, i.e. its derivative becomes negative. In some experiments it has been observed that the velocity of the motor was positive while the measured force was decreasing. It meant that one extreme of the robot was going forward while the other is getting backward. Consequently, the robot was compressing and accumulating elastic energy that could possibly increase the oscillations of the system in the future. That corresponds to the third case in equation (3). It
doesn’t happen frequently, but some examples were observed. Usually, when the robot is retiring from the environment is that the motor is also going backwards. This corresponds to the second case of equation (3) and a pure active damping is applied. Not that if the force derivative and the robot derivative have the same sign, it will be assumed that it is impossible to determinate if the robot is compressing or expanding. It must be emphasized that the sign of the force derivative was extremely hard to estimate due to the noise in the signal from the force sensor. A 20th order filter has been used. The value of the derivative was not estimated, jut its sign.
3.6 Danger of loss of contact The purpose of this state is to avoid losses of contact if possible because they may lead to stability loss, wear of material and increased duration of the task. The system enters this state when the measured force falls under a given minimum threshold and its derivative is negative. This threshold value was set empirically to 50 grams. To quit this state, the force must exceed another threshold value, set to 80 grams or if the contact is lost. In the first case, the system switches to the previous state. In the second, to the free motion state. The control action will be different if the robot is retiring than if it is penetrating back into the environment. In the first case ( df < 0 ) a constant value was chosen for the control action:
The purpose of this state is to avoid, when possible, entering the red zone and if not al least to make the transition smoother. The reasons for this are that the maximum control action may reduce the time of life of the motor, and the abrupt changes in control action increase the wear of the mechanical components.
ucl was set to 1 Volt empirically. If the robot is penetrating in the environment ( df > 0 ), two cases were differentiated: 1.
The system enters this state if the force exceeds a threshold called fmin. First of all, if the force derivative is increasing while the system is in the yellow zone, the control action is obtained as an interpolation of the control actions of the impact control and the red zone. u = α ( K p ( Fref − f ) − k v x ) + β ( −u max )
2.
(8)
Where k is a constant.
Fmax − f α = Fmax − Fmin < f < Fmax : β = f − Fmin F −F min max
(5)
From the point view of robot architecture, the yellow zone could be controlled as a behaviour fusion between the impact control and the red zone.
The control action will be zero when the system exits this state avoiding the unnecessary generation of kinetic energy. Following is represented an experiment where the system goes thru all the states. 2500
2000
When the robot is retiring from the environment, like in the previous state, there are two cases: 1.
If the force is low (below 50 grams), there is a high probability of loss of contact so the control law is according to (7). If the value of force is higher than 50 grams, the control law is selected in order to push back the robot slower as it approaches the limit of 80 grams. f − 50 2 u = k 1 − (80 − 50)
(4)
Where:
Fmin
(7)
u = ucl
3.5 Dangerous value o f force (yellow zone)
If the velocity is positive, it will mean that the robot is compressing and the control action will be according to (4).
1500
e cr o F
1000
500
0
2. If the velocity is negative, the control should be a pure damping in order to dissipate the kinetic energy of the system:
u = − k v x
(6)
-500
0
100
200
300 Samples
Fig. 4. Evolution of the force.
400
500
600
5. CONCLUSIONS
5 4.5
A control architecture is proposed for tasks with constrained motion. Several states were defined for free motion, impact attenuation, force tracking and contact loss avoidance.
4 3.5 3 et at S
2.5 2 1.5 1 0.5 0
0
100
200
300 Samples
400
500
600
Fig. 5. Evolution of the state. It can be observed that the system enters the protection stress (3 and 4) limiting the peak of force. Also, state 5 is activated. In this example it avoids successfully the loss of contact. In the transition from state 5 to state 1, there are successive switchings from one state to the other. Finally the system reaches the state 2 (force control), meaning the transient phase is finished and the kinetic energy acquired during free motion is mostly dissipated. 4. EXPERIMENTAL PLATFORM An experimental platform was developed in order to verify the previous conclusions. It consist of a Quanser lineal axe on which was mounted a JR3 force/torque sensor. In order to measure the speed, a Maxon tachometer was coupled to the axe. The system is controlled by a PC under the real- time operating system MARTE (Aldea M. and González M., 2001). The control routines were programmed in ADA while the drivers for the data acquisition boards were made in C. The measurements were realized impacting the robot against a book, in order to reduce the risk of force overshoots that could damage the system.
Fig. 6. Lineal Axe with force sensor.
In each state a different control action is applied depending on if the robot is penetrating in the environment or rebounding. In this way the unnecessary accumulation of the elastic energy of the environment is avoided. Also, in some states different control actions were used if the robot was compressing or expanding, in order to reduce the accumulation in the robot. A different sampling period was used in free and constrained motion because of the huge difference in the characteristics of the system in the two cases. An experimental platform was developed in order to verify empirically the assumptions. REFERENCES Aldea Rivas M. and Gonzalez Harbour M. (2001) MaRTE OS: An Ada Kernel for Real-Time Embedded Applications. Proceedings of the International Conference on Reliable Software Technologies, Ada-Europe- Leuven, Belgium, Lecture Notes in Computer Science, LNCS 2043, ISBN:3-540-42123-8, pp. 305,316 Armstrong B., J. McPherson, and Y. Li (1997) Stability of Nonlinear PD control. Applied Mathematics and Computer Science, 7, (2), pp. 101–120. Armstrong B., B. Wade (2000) Nonlinear PID control with partial state knowledge: damping without derivatives. Inter. Journal of Robotics Research, 19, 8, pp. 715–731. Armstrong B., D. Neevel D and T. Kusik (2001) New results in NPID control: Tracking, integral control, friction compensation and experimental results. IEEE Trans. on Control Systems Technology, 9 (2), pp. 399–406. Brooks R.(1986). A robust layered control system for a mobile robot. IEEE journal of Robotics and Automation (2), 1, pp. 14-23. Konolige, K., K. Myers, , E. Ruspini and A. Saffiotti, (1996). The Saphira Architecture: A Design for Autonomy. Journal of Experimental and Theoretical AI. 9, pp 215-235. Volpe R. and P. Khosla (1993) A Theoretical and Experimental Investigation of Explicit Force Control Strategies for Robot Manipulators. IEEE Transactions on Automatic Control- Noviembre 38 (11). Xu Y., J.M. Hollerbach J. M. and D. Ma (1995) A nonlinear PD controller for force and contact transient control. IEEE Control Systems Magazine, 15, (1), pp. 15–21.
Abstract: An architecture for force and impact control is proposed. Its objectives are at first to control the robot in free motion until the contact with the environment is made. Next, it should make the transition phase as smooth as possible. Finally, it should switch to force control in order to assure the tracking of the force reference. An absolute priority is to protect the system against the dangerous peaks of force. Also, loss of contact should be avoided when possible. The performance is improved by taking care of the accumulation of elastic energy in the robot. Sampling period is changed depending on the state of the system. An experimental platform was developed in order to test the architecture. Copyright © 2006 IFAC Keywords: force control, impact control, impact, supervision, switching, discontinuous control.
1. INTRODUCTION The control architectures are widely studied in mobile robotics in the last decades. In this period they made an important evolution.
Usually they consisted of several behaviours (goal attraction, obstacle avoidance, wall following, etc.), whose outputs were combined (behaviour fusion) and/or alternated (behaviour arbitration) to obtain the final path of the vehicle.
At first, in the sixties, the hierarchical paradigm in terms of the Sense - Plan – Act primitives was proposed. The results were unsatisfactory due to the slow response.
Nevertheless, the reactive architectures had some limitations and the relatively complicated tasks needed planning and therefore deliberative levels to achieve it.
In the mid-eighties a revolution has been made with the first reactive architecture by Brooks (1986). The introduction of the reactivity made faster the response of the robots to the unpredictable events, allowing their application in unknown or changing environments.
An improvement was made with the hybrid paradigm. Several hybrid architectures have been proposed, to mention Konolige (1996). It is a combination of the hierarchical and the reactive paradigm, keeping the advantages of both of them. The hybrid architecture consists of a lower, faster, reactive layer and one or more higher, deliberative layers. The output of the higher layers is the input to the lower one where it is treated like one behaviour more.
1
This work has been partially funded by FEDER-CICYT project with reference DPI2005-08732-C02-02 financed by Ministerio de Educación y Ciencia
At least one layer above the reactive is necessary in order to select which behaviours are active (arbitration) or to interpolate the effect of different behaviours (fusion).
The force control has some similarities with the mobile robotics. At first, a robot should perform different actions in different cases: control the speed in free motion, try to make the transient phase to constrained motion as smooth and as short as possible, protect the robot by any means against dangerous peaks of force, try to avoid the contact losses if possible, and finally, when the transient oscillations are sufficiently damped, try to reach the force reference value. At second, a priori knowledge of the environment characteristics (mechanic and geometric) may be wrong or inexistent, just like in mobile robotics. Otherwise it would be easy to execute a force control task. Third, the reaction of the controller must be very fast, otherwise the robot could damage itself and/or the environment. This specially true in the impact instant, when there is a very abrupt change in the dynamics of the system. Fourth, several different sensor may be used. For the realisation of this article, only force, position and velocity sensors where used. There is a possibility to add others like artificial vision, accelerometers, distance sensors, etc. In this paper, an architecture for force control, or at least its lower level, is proposed. In the second section its objectives are explained. The third section is dedicated to the description of the architecture. The fourth to the description of the experimental platform. The final one to the conclusions. The detailed explanation of the architecture would need much more space then it is allowed. For this reason, the descriptions were abbreviated and only the most relevant results are graphically represented. The data for the experiments (like reference of the force, maximum force, etc...) are chosen rather for test the general performance of the system than for a particular application. It is assumed that in the real applications they will be defined by the user according to the characteristics of the task. This article is limited to the case of a one degree of freedom robot. 2. OBJECTIVES
the robot, the environment or the tool. 2. The velocity in free motion should not be too low in order to avoid the excessive duration of the task. 3. The contact losses should be avoided as possible. They make the execution of the task slower, provoke wear of the mechanics and even can make the system unstable, 4. If it is not possible to avoid losses of contact, their number should be limited. In addition to these initial requirements, the experiments demonstrated the need to limit the accumulation of the elastic energy in the robot and the environment, when it is possible. It is useful to limit the peaks of the force, stabilise the system and make the convergence faster. Another, objective was to verify the effect of the use of different sampling periods in free and constrained motion. 3. THE ARCHITECTURE The architecture of the system consists of several different states the system can adopt depending on the actions to be taken. It can be only in one state at the time. The following possible states were identified: • • • • • •
Approximation to the environment Impact control Force control Dangerous value of force (red zone) Dangerous value of force (yellow zone) Danger of loss of contact
Each of these states may have a different control strategy, controller, reference value, and/or sampling period. Following, a few words will be told about each state. 3.1 Approximation to the environment. In order to achieve force control, the robot must be in contact with the environment, which is not the case in the beginning of the task. The purpose of this state is to take the robot towards the environment until the contact is achieved. It is also used in the case of contact losses.
Before starting the design of the architecture, its objectives were defined. Following, they are enumerated in order of importance and briefly explained.
During this state, the robot is in free motion. The dynamics of the system is relatively slow, allowing a lower sampling frequency. The value of 200 Hz was used. For all the other states the sampling frequency was set to 8 kHz because the dynamics is much faster in constrained motion.
1. The force should never exceed a given maximum value. It is a matter of security because otherwise an irreversible damage could be caused to
The main dilemma for this state was between position and velocity control. The former required precise knowledge of the position of the environment: if this
distance was overestimated, the impact could be very dangerous. If it was underestimated, the robot did not reach the environment. With the velocity control the contact is assured and the impact does not depend on the position of the environment. The velocity control has also some other advantages like the possibility of easily reducing the speed in the proximity of the environment, or to assign lower values of reference speed in the case of contact losses.
Similar ideas where proposed in Armstrong, et al., 1997, Armstrong and Wade (2000), Armstrong, et al., 2001 and Xu et al., 1995.
3.2 Impact control In order to abbreviate the task, the velocity in free motion should be as high as possible. It means that in the instant of the impact, the kinetic energy of the system is relatively high. This kinetic energy is transformed in elastic potential energy of the environment and the robot once the contact is made. Due to the very high stiffness of the materials used in industrial applications, the system is highly underdamped in constrained motion. High frequency oscillations occur and very dangerous peaks of force can be reached. The impact is, although very brief, the most dangerous part of the task. Due to the very short duration, an adaptive control may not be efficient for the protection of the system. The purpose of the impact control state is not to track a reference value. It is to dissipate as fast as possible the kinetic energy of the robot acquired in free motion. The following control law was applied during the impact control: u = K p ( Frefi − f ) − K v x
(1)
Where u is the control action, Kp is the proportional constant of the force, Frefi is the force reference value during the impact phase, that may be (or not) smaller than the nominal value, Kv is the velocity constant and x is the robot speed. No integrator because it would increase the peaks of force. On the other hand it is unnecessary because tracking the force reference is not the purpose of this state. In order to improve the performance, the proportional constant is switched between two values according to the following law: K p1 if Kp = 0 if
( Frefi − f ) x < 0 ( Frefi − f ) x > 0
higher then the reference) and a the same time it keeps penetrating more (which is reflected by a positive velocity), then the system should be braked, what is obtained assigning a high value to the proportional constant. On the contrary, if the robot has penetrated too deep and is retiring, there is no need for a proportional control action since the system is leaning towards the reference value by itself. The application of a proportional gain would accelerate the robot and its speed would be higher when it reaches the reference value, making also higher the next peak of force.
(2)
This allows a faster convergence of the system and even may make it stable if it is not. The demonstration of this switching law is beyond the scope of this article. The basic idea is the following: if the robot has penetrated too much in the environment (which is reflected by a value of force
The validity of the switching criteria from equations (2) is experimentally verified. The results are represented in the following figures. 8000 7000 6000 5000 4000 e cr o F
3000 2000 1000 0 -1000
0
500
1000
1500 Samples
2000
2500
3000
Fig. 1. Graphical representation of the force without (solid line) and with (dotted line) switching the proportional constant according to (2). The force is measured in grams-force instead of Newtons in order to obtain better resolution. 3
2.5
2
et at S
1.5
1
0.5
0
0
500
1000
1500 Samples
2000
2500
3000
Fig. 2. Graphical representation of the state without (solid line) and with (dotted line) switching the proportional constant according to (2). State zero corresponds to free motion, one to impact control and two to force control.
3.4 Dangerous value of force (red zone)
5 4
This state is the ultimate protection of the system against the peaks of force that could damage the robot, the environment or the tool, whichever is the most fragile.
3 2 n oi t c A l or t n o C
1 0 -1 -2 -3 -4 -5
0
500
1000
1500 Samples
2000
2500
3000
Fig. 3. Graphical representation of the control action without (solid line) and with (dotted line) switching the proportional constant according to (2). The graphics show that the system is unstable if there is no switching of the proportional constant. The system enters the impact control phase but doesn’t leave it. On the contrary, if the switching of the proportional constant is made, the system enters the next state after a relatively brief time lapse. The transition to the next state (force control) should be made after the impact is dumped enough. To determinate when this has occurred the following criterion was used: the peaks of the velocity were detected in every period of oscillation. When the peak was lower than 10% of the impact velocity, it was assumed that enough energy was dissipated by the impact controller and that the system had more or less reached a steady state. 3.3 Force control The purpose of this state is to track the reference value of force. This state is reached when the transient phase is finished and there is no more danger of high peaks of force that could damage the system. Since the force regulation by itself is not the purpose of this article, and taking into account the lack of space, it will not be further analysed. This subject was studied in detail in Volpe and Khosla, 1993. Just to mention that an integrator should be used in order to reach the reference value. Although the fact that this state has been reached means the impact has already been damped, the possibility of dangerous peaks of force still exists due to irregularities in the environment or vibrations. The monitoring of the system should continue in order to activate any of the other states if necessary.
The idea of this state is very simple: if the force exceeds a threshold value, to apply the maximum control action in order to stop the robot. Given the elasticity of the robot, this threshold value of the force should be considerably lower value that can provoke mechanical damage. Once the robot starts retiring, the maximum negative control action there is not necessary anymore. On the contrary, it could cause a high velocity of the robot in the opposite direction and subsequent loss of contact. For this reason, when the force derivative becomes negative, the control is set to a pure active damping. It has been observed that the performance can be improved if taking into account the accumulation of the elastic potential energy in the robot. It was assumed that the robot is compressing if the velocity is positive while the force derivative is negative. This would mean that the two extremes of the robot are going towards its center. If the velocity and the force derivative have the same sign, it is not possible to determinate if the robot is compressing or expanding. The final control action is defined by the following rules: − u max u = − k v x − u max
if if if
f > Fmax & df > 0 f > Fmax & df < 0 & x < 0 f > Fmax & df < 0 & x > 0
(3)
Where umax is the maximal control action allowed by the actuator, Fmax the force threshold that activates this state, and df the force derivative. The meaning of the first case is simply that if the force exceeds the potentially dangerous threshold and at the same time it is increasing, the maximum possible control action should be applied in order to stop the robot as fast as possible. The second and the third case occur when the robot, or at least the part where the force sensor is, starts retiring from the environment. When this happens, the force begins to decrease, i.e. its derivative becomes negative. In some experiments it has been observed that the velocity of the motor was positive while the measured force was decreasing. It meant that one extreme of the robot was going forward while the other is getting backward. Consequently, the robot was compressing and accumulating elastic energy that could possibly increase the oscillations of the system in the future. That corresponds to the third case in equation (3). It
doesn’t happen frequently, but some examples were observed. Usually, when the robot is retiring from the environment is that the motor is also going backwards. This corresponds to the second case of equation (3) and a pure active damping is applied. Not that if the force derivative and the robot derivative have the same sign, it will be assumed that it is impossible to determinate if the robot is compressing or expanding. It must be emphasized that the sign of the force derivative was extremely hard to estimate due to the noise in the signal from the force sensor. A 20th order filter has been used. The value of the derivative was not estimated, jut its sign.
3.6 Danger of loss of contact The purpose of this state is to avoid losses of contact if possible because they may lead to stability loss, wear of material and increased duration of the task. The system enters this state when the measured force falls under a given minimum threshold and its derivative is negative. This threshold value was set empirically to 50 grams. To quit this state, the force must exceed another threshold value, set to 80 grams or if the contact is lost. In the first case, the system switches to the previous state. In the second, to the free motion state. The control action will be different if the robot is retiring than if it is penetrating back into the environment. In the first case ( df < 0 ) a constant value was chosen for the control action:
The purpose of this state is to avoid, when possible, entering the red zone and if not al least to make the transition smoother. The reasons for this are that the maximum control action may reduce the time of life of the motor, and the abrupt changes in control action increase the wear of the mechanical components.
ucl was set to 1 Volt empirically. If the robot is penetrating in the environment ( df > 0 ), two cases were differentiated: 1.
The system enters this state if the force exceeds a threshold called fmin. First of all, if the force derivative is increasing while the system is in the yellow zone, the control action is obtained as an interpolation of the control actions of the impact control and the red zone. u = α ( K p ( Fref − f ) − k v x ) + β ( −u max )
2.
(8)
Where k is a constant.
Fmax − f α = Fmax − Fmin < f < Fmax : β = f − Fmin F −F min max
(5)
From the point view of robot architecture, the yellow zone could be controlled as a behaviour fusion between the impact control and the red zone.
The control action will be zero when the system exits this state avoiding the unnecessary generation of kinetic energy. Following is represented an experiment where the system goes thru all the states. 2500
2000
When the robot is retiring from the environment, like in the previous state, there are two cases: 1.
If the force is low (below 50 grams), there is a high probability of loss of contact so the control law is according to (7). If the value of force is higher than 50 grams, the control law is selected in order to push back the robot slower as it approaches the limit of 80 grams. f − 50 2 u = k 1 − (80 − 50)
(4)
Where:
Fmin
(7)
u = ucl
3.5 Dangerous value o f force (yellow zone)
If the velocity is positive, it will mean that the robot is compressing and the control action will be according to (4).
1500
e cr o F
1000
500
0
2. If the velocity is negative, the control should be a pure damping in order to dissipate the kinetic energy of the system:
u = − k v x
(6)
-500
0
100
200
300 Samples
Fig. 4. Evolution of the force.
400
500
600
5. CONCLUSIONS
5 4.5
A control architecture is proposed for tasks with constrained motion. Several states were defined for free motion, impact attenuation, force tracking and contact loss avoidance.
4 3.5 3 et at S
2.5 2 1.5 1 0.5 0
0
100
200
300 Samples
400
500
600
Fig. 5. Evolution of the state. It can be observed that the system enters the protection stress (3 and 4) limiting the peak of force. Also, state 5 is activated. In this example it avoids successfully the loss of contact. In the transition from state 5 to state 1, there are successive switchings from one state to the other. Finally the system reaches the state 2 (force control), meaning the transient phase is finished and the kinetic energy acquired during free motion is mostly dissipated. 4. EXPERIMENTAL PLATFORM An experimental platform was developed in order to verify the previous conclusions. It consist of a Quanser lineal axe on which was mounted a JR3 force/torque sensor. In order to measure the speed, a Maxon tachometer was coupled to the axe. The system is controlled by a PC under the real- time operating system MARTE (Aldea M. and González M., 2001). The control routines were programmed in ADA while the drivers for the data acquisition boards were made in C. The measurements were realized impacting the robot against a book, in order to reduce the risk of force overshoots that could damage the system.
Fig. 6. Lineal Axe with force sensor.
In each state a different control action is applied depending on if the robot is penetrating in the environment or rebounding. In this way the unnecessary accumulation of the elastic energy of the environment is avoided. Also, in some states different control actions were used if the robot was compressing or expanding, in order to reduce the accumulation in the robot. A different sampling period was used in free and constrained motion because of the huge difference in the characteristics of the system in the two cases. An experimental platform was developed in order to verify empirically the assumptions. REFERENCES Aldea Rivas M. and Gonzalez Harbour M. (2001) MaRTE OS: An Ada Kernel for Real-Time Embedded Applications. Proceedings of the International Conference on Reliable Software Technologies, Ada-Europe- Leuven, Belgium, Lecture Notes in Computer Science, LNCS 2043, ISBN:3-540-42123-8, pp. 305,316 Armstrong B., J. McPherson, and Y. Li (1997) Stability of Nonlinear PD control. Applied Mathematics and Computer Science, 7, (2), pp. 101–120. Armstrong B., B. Wade (2000) Nonlinear PID control with partial state knowledge: damping without derivatives. Inter. Journal of Robotics Research, 19, 8, pp. 715–731. Armstrong B., D. Neevel D and T. Kusik (2001) New results in NPID control: Tracking, integral control, friction compensation and experimental results. IEEE Trans. on Control Systems Technology, 9 (2), pp. 399–406. Brooks R.(1986). A robust layered control system for a mobile robot. IEEE journal of Robotics and Automation (2), 1, pp. 14-23. Konolige, K., K. Myers, , E. Ruspini and A. Saffiotti, (1996). The Saphira Architecture: A Design for Autonomy. Journal of Experimental and Theoretical AI. 9, pp 215-235. Volpe R. and P. Khosla (1993) A Theoretical and Experimental Investigation of Explicit Force Control Strategies for Robot Manipulators. IEEE Transactions on Automatic Control- Noviembre 38 (11). Xu Y., J.M. Hollerbach J. M. and D. Ma (1995) A nonlinear PD controller for force and contact transient control. IEEE Control Systems Magazine, 15, (1), pp. 15–21.