- Email: [email protected]
Measuring the 3D-position of a walking vehicle using ultrasonic and electromagnetic waves
Sensors and Actuators 75 Ž1999. 131–138
Measuring the 3D-position of a walking vehicle using ultrasonic and electromagnetic waves J.M. Martın P. Gonzalez-de-Santos ´ Abreu ) , R. Ceres, L. Calderon, ´ M.A. Jimenez, ´ ´ Instituto de Automatica Industrial (Departments of Automatic Control and Sensory Systems), CN-III Km 22.8, 28500 Arganda del Rey, Madrid, Spain ´ Received 31 March 1998; received in revised form 4 November 1998; accepted 15 December 1998
Abstract The knowledge of the true position of a walking machine Žrover. in an inertial reference frame is a problem of paramount importance for realistic application of mobile robots. The points of the robot follow three-dimensional trajectories even on even terrain. When no position feedback is possible, a good knowledge of the dynamical behaviour of the rover is needed to get an estimation of the position and orientation of the robot. There already exist some sophisticated optical systems which track the path followed by specific Žlighted. points of the robot. Although these systems can provide very precise measurements, they cannot cover larger areas with the same precision. This paper presents a laboratory prototype capable of measuring the position of a four-legged walking robot using a combination of electromagnetic ŽEM. and ultrasonic ŽUS. waves produced by a spark-generator, avoiding any physical link between the robot and the environment.The 3D position is obtained from range data that can be estimated from the travel time of the acoustical wave from the sparking point to three static receivers. The EM wave is used for synchronization. The system provides real time data to operate in a wider space than the optical systems. After the processing and filtering of the signal, a final precision better than 1 mm is reached in a work range of about 5 m. The track-data obtained by the position meter is used to know the dynamic behaviour of the robot and to study the improvement introduced by the use of inclinometers. q 1999 Elsevier Science S.A. All rights reserved. Keywords: Ultrasonic sensors; Mobile robots; Absolute position
1. Introduction The 3D position of a mobile robot in a workspace has a special importance for walking machines where the internal foot forces produce foot slippage, the minimisation of weight and the high collision risk led to the use of flexible materials and slender links that are prone to flexure. The consequence of these phenomena is that the positions of the different points of the machine are not well known. Therefore, in the case of walking robots, it is of great interest to analyse the actual movement of the different components in order to improve the movement algorithms and the overall design, reducing instabilities and energy consumption. There are optical systems that, using stereo vision with ultrahigh-speed video recording and off-line data processing, can be used to track moving points with high precision; nevertheless they cannot maintain the precision in ) Corresponding author. Tel.: q34-1-871-1900; Fax: 34-1-871-7050; E-mail: [email protected]
large measuring areas, for instance, the Selspot II, from Selcom. The present development deals with a new 3D measurement real time system, able to record the trajectories followed by a mobile robot without physical connection. The system has been used to track the path of the points of a walking machine. The main goal of the measurement system is to study the behaviour of the vehicle under different control strategies. Ultrasonic sensors are widely used in mobile robot applications for obstacle detection and map elaboration from range data. In such cases the US transmitter and receiver are installed on board and the measurement of the time of fly ŽTOF. spent by the waves to travel from the transmitter up to one target and back to the receiver is used to calculate the distance to the target. In this application the principle of location measurement is different. Combining the distance estimation obtained by measuring the TOF’s of an ultrasonic wave from one transmitting point up to three different receiving points, the position of the emitting point can be calculated in
0924-4247r99r$ - see front matter q 1999 Elsevier Science S.A. All rights reserved. PII: S 0 9 2 4 - 4 2 4 7 Ž 9 8 . 0 0 3 7 8 - 1
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
132
Fig. 1. Ža. General system. Žb. Principle of measurement. T s spark generator, R 1 , R 2 , and R 3 s ultrasonic receivers.
reference to the frame system formed by the three receivers as is suggested by some authors in other applications w1x. Although the principle of measurement is different, the technique used is also pulse–echo and the strategies developed to improve the estimation of distances can also be applied here. In the system described below, a spark plug is used as a source of EM and US waves. Using the big difference in propagation speed of both kinds of waves Ža rate of 1:10 6 ., we can consider the transmission of the electromagnetic wave as instantaneous, using it as the trigger signal Ž t s 0. for measuring the TOF of the ultrasonic wave from the sparking point up to three different receivers, knowing the instant of the US emission without need of any physical connection between the robot and the environment. Finally the distances from the transmitter to the three receivers can be estimated, knowing the speed of ultrasonic waves ŽFig. 1a.. A measurement system has been developed to acquire and process the signals in real time, providing information about the dynamic behaviour of different parts of the robot structure, showing the 3D path of the chosen point.
2. Measurement principle The sparking point is located on the robot, at the point we want to track. This generator produces an electric arc at constant time intervals of about 40 timesrs Žpulse rate frequency.. This start of the arc produces an electromagnetic emission and also an overpressure in the air generating acoustic waves of many wide bands of frequencies. An
antenna, placed in the RF receiver and connected to the central processing unit, detects the electromagnetic emission that is used as a start-signal for the TOF measurement. The value of the PRF is chosen as a compromise between a maximum number of measurements and a time large enough between samples to allow damping of the secondary reflections of the previously transmitted US waves. Having an estimation of the distances from the transmitter up to the three receivers Ž d1 , d 2 , d 3 in Fig. 1b. we can formulate the following equations: 2
2
2
2
2
2
2
2
2
d12 s Ž x y rx 1 . q Ž y y r y 1 . q Ž z y rz 1 .
d 22 s Ž x y rx 2 . q Ž y y r y 2 . q Ž z y rz 2 . d 32 s Ž x y rx 3 . q Ž y y r y 3 . q Ž z y rz 3 .
where Ž x, y, z . are the coordinates of the emitting point and Ž rx n , ryn , rz n . the coordinates of each receiver. Resuming, we have: d i2 s
Ý Ž a y ra i . 2 asx , y , z
These are the equations of the surfaces of three spheres with centres in the receivers r 1 , r 2 and r 3 and radii d i . The solutions are the intersections of the three spheres. If they do not have the same centre and their radii fulfil the conditions that they have the same common points, the intersection would be two points and we will have therefore two possible solutions for the position of the emitting point. As we will see later, one of the solutions can be rejected.
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
For simplicity we can choose our coordinates frame with origin in receiver 1, while the second receiver is placed on the x-axis and the third receiver is contained in the x–z plane; then we have that rx 1 s ry1 s rz 1 s ry 2 s rz 2 s ry 3 s 0. and we find two possible solutions for the coordinates of the emitting point: 2
2
d12 s Ž x . q Ž y . q Ž z . 2
2
2
2
2
d 22 s Ž x y rx 2 . q Ž y . q Ž z .
2
d 32 s Ž x y rx 3 . q Ž y . q Ž z y rz 3 .
2
With the solution: xs
zs
d12 y d 22 q rx 22 2 rx 2 d12 y d 32 q rz 32 2 rz 3
(
y s " d12 y x 2 y z 2 The second solution of the system is symmetrical on the xz-plane and implies one of the limitations of the system; we can only measure on the hemispace y ) 0, which is also the practical case, because it is very difficult to get omnidirectional receivers and in our system the receivers cannot get a signal from the opposite hemispace. 2.1. Influence of the distance between receiÕers in the measurement As we have shown in previous studies w2x, the physical limit for estimating distances by measurement of the TOF of acoustical waves comes from the movements of the transmitting media: the air. This causes an absolute error that is mainly proportional to the measuring distance Žnot for very short ranges less than 10 cm, where other problems as right detection of the TOF or interference phenomena have a bigger influence.. Looking at the solutions, we can feel that the distance between receivers would have a big influence on the precision. To analyse this with detail we look at the error Ž E x . in the estimation of the x-coordinate as a consequence of an error in the estimation of distances, then we have: Ex s
d1 rx 2
Ed 1 q
d2 rx 2
nate, we search for the optimal distance between receivers Ž rx .. That can be done by looking at the point where the first derivative of E x by rx becomes null. That happens when: rx s 2 y The same conclusion can be extracted for the estimation of the z-coordinate. Thus, the most adequate distance between receivers would be twice the distance of the robot to the measuring plane. The error in the estimation of the y-coordinate, would have a different dependence of the distance, its precision decreases when the distance between sensors increases and would be optimal when the distance between sensors is minimal. On the other hand large distances between sensors imply a calibration problem. In the results presented here, we choose a distance of 1 m that we can calibrate well with our instruments and that would be optimal for measurements taken at 0.5 m from the plane formed by the sensors.
3. Walking-machine test bed The RIMHO walking machine has been developed and used for study and testing purposes w3x. It has four legs based on a pantographic mechanism Žsee Fig. 2.. Each leg is driven by three independent electrical actuators. The body is a structure of 700 mm = 700 mm = 250 mm. Legs and body are made of aluminium and including the electronics weigh 60 kg and it can transport about 40 kg. The twelve actuators of the machine let it move in quite different manners, having six degrees of freedom. It can perform different periodic gaits Žleg sequence. such as waves and discontinuous gaits w4,5x. For the tests described in this paper we have considered the crab wave gait w6x. This is a continuous gait in which the body follows a straight line, at constant speed, forming an angle Žthe crab angle. with the longitudinal axis of the body. The experiments were conducted to detect the actual path followed
Ed 2
Supposing that the relative error Ž e d s Edrd . is the same for both measurements and that it is constant for each distance, we have: Ex s
d12 q d 22 rx 2
ed
Choosing for simplicity that the robot is at an equal distance from each receiver Ž d . and at a given y-coordi-
133
Fig. 2. The-four legged walking robot ŽRIMHO..
134
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
by the vehicle when a theoretical change of the crab angle is requested. The changes in the altitude of the vehicle are also studied, specially the variations that occur when a leg changes its supporting status.
4. The measurement system The measurement system ŽFig. 3. consists of two separate devices: the spark-generator with its associated electronics on board of the robot and the receivers with the processing device and computer placed in the work space.
we get acoustical energy at almost any frequency Ždue to the abruptness of the discharge., even in the audible range. Although the acoustical level in the audible range stays far below the inconvenience level. The EM wave generated at the instant that the electric arc breaks down has also a main component at 40 kHz. We use a simple coil as EM receiver, which gives a good signal for the operation distance we want to measure. A quartz clock of 20 MHz combined with digital timers produces at regular intervals a very stable pulse that amplified in current and voltage is finally applied to a transformer that would produce the high voltage for the poles where the electrical discharge would take place.
4.1. The spark-generator 4.2. The US-reception The generation of both waves Želectromagnetic and acoustical. is not produced by a continuous electric arc, but by the changes in electron current and therefore in air pressure. This corresponds to the instants that the arc begins and stops, because we use one single pulse for excitation. The frequency spectrum of the acoustical waves produced by this pulse depends mainly on the width of the pulse and the distance between electrodes. Both are chosen to match the frequency behaviour of the piezoelectric transducers used as receivers which have their maximal sensitivity at 40 kHz. A study with a wide-band microphone, 5 Hz–200 kHz ŽBruel Kjaer 4138., has shown this dependency and has been used to adjust the system in order to get the maximal energy at 40 kHz. In every case
The three ultrasonic receivers are mounted on the vertex of a fixed triangle, making it easier to calibrate the distances between receivers and to conserve the validity of the calibration. In each receiver we have also mounted a pre-amplifier in order to minimize the influence of the noise produced in the wires. This kind of transducer Žflexible bimorph ceramics., is not suitable for the intrinsic temperature correction proposed in Ref. w7x, as is explained in Ref. w8x. Therefore we use one temperature sensor placed near the US sensors, in order to correct for the variation in the speed of sound with temperature. The signal of the temperature sensor is passed to the same acquisition board.
Fig. 3. The measurement system.
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
The signal coming from the US receivers passes through four different analog processing stages Žsee Fig. 4.: gathering by a pre-amplifier of 40 dB placed near the receivers, band-pass filtering ŽB. at the resonance frequency of the transducers, rectification of the negative part of the signal ŽC. and low-pass filter ŽD. in order to demodulate the signal, extracting the envelope. In the lower side of Fig. 4. we can see a typical acoustical signal and the results of the different processing stages. The bandpass filtering improves the signal allowing us to operate with a very good signal-to-noise ratio even at large distances Ž60 dB at 1 m.. Although the system is designed and tested at distances up to 6 m which was considered enough for testing purposes, it can be easily extended to operate at greater ranges. The rectification and the use of a second-order low-pass filter allows us to obtain the envelope with minimal deformation. Comparing it to AM demodulation in RF applications, the difficulty here is the relative proximity in frequencies between both signals: the envelope and the carrier. The obtained envelope is applied to an ArD converter that digitises the signal. With a sampling rate of 100 kHz and a multiplexer for the signal from each channel the acquired data is stored in the memory of a PC, where the digital processing of the signal takes place.
4.3. Digital processing of the signals The measurement of the precise moment of the arrival of the ultrasonic signal is subject to some uncertainty.
135
Even when only one pulse is produced at the spark generator, due to the inertia and narrow band of the transducers, the acoustical wave train received shows a considerable increasing time as show in Fig. 4. The conventional method of detecting the time of arrival using a fixed threshold introduces a big error due to the variation in amplitude with distance w9x: exp Ž ya x . A s A0 x Where x is the distance, a the attenuation coefficient and A 0 is a constant. A dynamic threshold w10x that depends on the time transcurrent from the emission can also introduce an important error due to the variation of the signal amplitude with the angle, transducer, or air movement. On the other hand in this application where the wave is directly transmitted from the emitter to the receiver, we can consider the shape of the wave as constant and try to identify its position on the time axis after digitising the signal. As shown in Ref. w11x, the least-square method using a reference signal would be the most accurate and noise-invariant method to identify the time position of the signal. On the other hand this method needs a lot of calculations and needs a previous calibration process for the wave-form. When we have a high signal-to-noise ratio, other methods can reach similar precision with a drastic reduction in the calculation needs. The algorithm used here makes an estimation of the maximum of the signal Ž Vmax in Fig. 5. using three points around the instant of maximal amplitude Ž t max ., looking for the intersection between the previously digitised envelope of the acoustical wave and the value of half the
Fig. 4. Stages of the analog processing.
136
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
Fig. 5. Determination of the time of arrival.
maximum Ž Vmaxr2.. This point is chosen because it is approximately the point of maximum derivative and therefore the point where an error in amplitude implies the least error in time estimation. For estimating the instant of intersection we consider the signal as a straight line in a short interval around the half value and use the least-square method to find the final point where in the straight line f Ž t 0 . s Vmaxr2. This method allows us to reach a high resolution in time, even better than the sampling time of the signal. This method implies an offset Ž te y t 0 ., because t 0 is not the actual instant of arrival of the signal. But, if the form of the signal does not suffer significant variations, this offset can be considered as constant and can be calibrated once at the beginning.
The result of the measurements shows that the final error is rather a consequence of the air movements in the room w2x than from the measurement method used. Finally a configurable low pass digital filter of first order is implemented to minimize the influence of fast varying error influences. The configuration allows to adapt the filter to the expected speed of the effect Žmovement. we want to measure.
Fig. 6. Error calibration.
Fig. 7. X – Y measurements of the trajectory and the planned path.
5. Results: measurements with the RIMHO walking machine. To estimate the precision of the final system, the sparking device has been put on a test bed Žbank. as used in
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
optical applications. It presents a mechanical positioning precision of 0.1 mm in the tree axis, x, y and z. The range of the axis x and y are 1.5 m while in the z-axis the range is only 0.5 m. To test the precision at large ranges, we have changed the distance between the test bed and the receivers. We must take into consideration that our main interest consists in the measurement of the relative motion of the robot parts. Fig. 6 gives a summary of the calibration measurements. The standard deviation of 100 samples at different distances between test bed and receivers. We have confirmed that the error depends mainly on the distance. Most of the measurement were taken at a distance of 3 m with an error of less than 1 mm. This precision can be considered sufficient for analysing real time effects and even for getting a good estimation of the speed of the moving point calculated as the derivative of the position w6x. This range of measurements is considerably larger than what can be reached with optical cameras which present focusing problems. Algorithms for omnidirectional displacements and altitude control were incorporated into the standard baggage of the RIMHO ŽFig. 2.. To demonstrate the effectiveness of the movement control algorithms, some experiments were performed measuring the position of the centre of the reference frame of the robot using the system described. Fig. 7, shows both: requested path Žstraight line. and actual trajectory of the emitting point corresponding to the crab angles a s y58 and a s q108. Abrupt changes in the position of the sparking point are due to the flexion and foot force redistribution phenomena in the walking machine. It can be observed that the trajectory of the sparking point deviates from the straight line a maximum of 30 mm which can be considered acceptable while the orientation control was not active in this test. The RIMHO has touch-sensors on each foot and as a first approach, after detecting contact with the ground it moves its shoulder to maintain a constant altitude from the ground, due to the difference in forces at the instant of contact and the moment of desired altitude together with the phenomena of redistribution of forces and flexure of the legs. This movement normally does not correct all that is necessary in order to maintain the same altitude. Looking for a solution with the most simple sensors, we have equipped the robot with inclinometers allowing the robot to recover after each leg movement the original altitude. In this case we have shown, using the 3D measurement system, how the information from the inclinometer would be sufficient to maintain the same altitude Žsee Fig. 8.. In this figure we can also see another detail. The vertical marks represent the moments that special events occur: l when a leg is lifted and p when it is placed, while the accompanying numbers identify the different legs. The figure illustrates that changes are more significant when front legs Žnumbers 1 and 2. are lifted than when the rear legs are changed. This result can be expected from the
137
Fig. 8. Variations in altitude. Discontinuities when the legs support the robot or are lifted.
position of each foot in the wave gait and the force redistribution phenomenon. In each case we can see that the altitude of the emitting point remains steady around the origin with variations of about 15 mm.
6. Conclusions An innovative contactless system has been developed to track the 3D position of a moving point. It has been designed for tracking the different parts of a mobile robot, with respect to the environment. The system uses the EM wave and the US wave produced by a sparking punctual device to measure its position. The use of analog and digital processing techniques has led to a precision that can be consider sufficient for analysing the movement of the robot Žin the laboratory. in order to improve the strategies and algorithms of movement control. The simplification of processing algorithms and the electronic circuits used, makes it possible to measure, process, and monitor the effects in real time, with a measurement rate of 40 samplesrs. The possibility of monitoring more than one point Žsequentially. as a way of calculating the orientation of the machine is being studied for future applications. A new system is also being developed, in collaboration with an industrial mobile robot firm, which increases the measuring range. It can also be used for guidance purposes in known environments. In this system the spark generator is substituted by other US transmitter and traditional RF– FM-transmission in order to avoid ignition danger and EMI generation.
References w1x Ch. Delepaut, L. Vanderdorpe, Ch. Eugene, Ultrasonic three-dimensional detection of obstacles for mobile robots, 16th International Symposium on Industrial Robots, 8th International Conference on Industrial Robot Technology, 30 September–2 October, 1986, Brussels, Belgium, pp. 483–490.
138
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
w2x S. Ros, J.M. Martın, ´ T. Freire, L. Calderon, ´ Digital techniques improve range measurement with ultrasound sensor, Sensors and Actuators A 32 Ž1992. 550–554. w3x M.A. Jimenez, P. Gonzalez-de-Santos, M.A. Armada, A four-legged ´ ´ walking test bed, 1st IFAC International Workshop on Intelligent Autonomous Vehicles, Hampshire, UK, April 1993, pp. 8–13. w4x P. Gonzalez-de-Santos, M.A. Jimenez, Generation of discontinous ´ ´ gaits for quadrupped walking vehicles, Journal of Robotic Systems 12 Ž9. Ž1995. 599–611. w5x P. Gonzalez-de-Santos, M.A. Jimenez, Path tracking with ´ ´ quadrupped walking machines using discontinuous gaits, Computers and Electronic Engineering 21 Ž6. Ž1995. 383–396. w6x M.A. Jimenez, Generacion ´ ´ e Implementacion ´ de Modos de Caminar Ondulatorios para Robots Caminantes, Tesis Doctoral, Universidad de Cantabria, June 1994. w7x J.M. Martın, ´ R. Ceres, L. Calderon, ´ Ultrasonic ranging gets thermal correction, Sensor Review 9 Ž1. Ž1989. 153–155. w8x C. Cai, P.P.L. Regtien, A versatile ultrasonic ranging system, Sensor Review 13 Ž1. Ž1989. 22–25. w9x J.M. Martın, ´ R. Ceres, A comparative study of several approaches for high resolution range measurements using ultrasonic sensors, International Workshop on Sensorial Integration for Industrial Robots ŽSIFIR 89., University of Zaragoza, Nov. 1989, pp. 206–211. w10x J.D. Fox, B.T. Khuri-Yakub, G.S. Kino, High frequency acoustic wave measurement in air, Proceeding of the IEEE 1983 Ultrasonic Symposium, October 31-2, Atlanta, GA, pp. 581–584. w11x J.J. Anaya, C. Fritsch, J.M. Martın, ´ A comparative study of several approaches for high resolution range measurements using ultrasonic sensors, International Workshop on Sensorial Integration for Industrial Robots ŽSIFIR 89., University of Zaragoza, Nov. 1989. Jose M. Martın ´ Abreu was born in 1958 in Isla Cristina, Spain. He graduated in physics from the Universiteit van Amsterdam in 1982 and received the doctoral degree in 1990 from the Universidad Complutense de Madrid. He has developed many research activities in the field of automation of processes and especially on the study of sensors Žfocused on ultrasonic sensors. and their processing and application. He has published many scientific papers and holds several patents. He has also participated in different national and international scientific programmes and congresses.
Ramon ´ Ceres was born in 1947 in Jaen, ´ Spain. He graduated in physics Želectronic. from Universidad Complutense de Madrid in 1971 and received the PhD degree in 1978. After a first stay for one year, in the LAAS-CNRS in Toulouse, France, be has been working at the Instituto de Automatica Industrial ŽIAI., a dependent of the Spanish National ´ Council for Science Research. For the period 1990–1991 he worked in an electronics company ŽAutelec. as R&D director. Since the beginning, Dr. Ceres has developed research activities on sensor systems applied to different fields such as continuous process control, machine tools, agriculture, robotics and disabled people. On these topics he has published more than 80 papers and congress communications, and he has several patents in industrial exploitation. At present Dr. Ceres belongs to the Spanish Delegation of the IMT ŽBrite-Euram. Committee, being deputy director of the IAI. Leopoldo Calderon ´ was born in 1947 in Lumbrales, Spain. He graduated in physics from the Universidad de Sevilla in 1974 and received the doctoral degree in 1984 from the Universidad Complutense de Madrid. Since 1974 Dr. Calderon ´ has been working in the Instituto de Automatica ´ Industrial developing many research activities in the field of automation of processes and especially on the study of sensors Žfocused on ultrasonic sensors. and their processing and application. As a consequence of this activity, Dr. Calderon ´ has published many scientific papers and is author of different patents. He has also participated in different national and international scientific programmes and congresses. Marıa received her BS in physics and PhD degrees from the ´ A. Jimenez ´ University of Cantabria ŽSpain. in 1986 and 1994, respectively. In 1987, she joined the Department of Automatic Control at IAI-CSIC. Her PhD thesis was focused on the generation of wave gaits and adaptability to irregular terrains. Her research interests include configuration, simulation, and implementation of autonomous walkers. Pablo Gonzalez ´ de Santos is a research scientist at the Spanish Council for Scientific Research ŽCSIC.. He received his PhD from the University of Valladolid in 1986. Since 1981 he has been involved actively in the design and development of industrial robots and special robotic systems as well. His work during the last 9 years has been focused on walking machines participating in the development of seven different projects in this area.
Measuring the 3D-position of a walking vehicle using ultrasonic and electromagnetic waves J.M. Martın P. Gonzalez-de-Santos ´ Abreu ) , R. Ceres, L. Calderon, ´ M.A. Jimenez, ´ ´ Instituto de Automatica Industrial (Departments of Automatic Control and Sensory Systems), CN-III Km 22.8, 28500 Arganda del Rey, Madrid, Spain ´ Received 31 March 1998; received in revised form 4 November 1998; accepted 15 December 1998
Abstract The knowledge of the true position of a walking machine Žrover. in an inertial reference frame is a problem of paramount importance for realistic application of mobile robots. The points of the robot follow three-dimensional trajectories even on even terrain. When no position feedback is possible, a good knowledge of the dynamical behaviour of the rover is needed to get an estimation of the position and orientation of the robot. There already exist some sophisticated optical systems which track the path followed by specific Žlighted. points of the robot. Although these systems can provide very precise measurements, they cannot cover larger areas with the same precision. This paper presents a laboratory prototype capable of measuring the position of a four-legged walking robot using a combination of electromagnetic ŽEM. and ultrasonic ŽUS. waves produced by a spark-generator, avoiding any physical link between the robot and the environment.The 3D position is obtained from range data that can be estimated from the travel time of the acoustical wave from the sparking point to three static receivers. The EM wave is used for synchronization. The system provides real time data to operate in a wider space than the optical systems. After the processing and filtering of the signal, a final precision better than 1 mm is reached in a work range of about 5 m. The track-data obtained by the position meter is used to know the dynamic behaviour of the robot and to study the improvement introduced by the use of inclinometers. q 1999 Elsevier Science S.A. All rights reserved. Keywords: Ultrasonic sensors; Mobile robots; Absolute position
1. Introduction The 3D position of a mobile robot in a workspace has a special importance for walking machines where the internal foot forces produce foot slippage, the minimisation of weight and the high collision risk led to the use of flexible materials and slender links that are prone to flexure. The consequence of these phenomena is that the positions of the different points of the machine are not well known. Therefore, in the case of walking robots, it is of great interest to analyse the actual movement of the different components in order to improve the movement algorithms and the overall design, reducing instabilities and energy consumption. There are optical systems that, using stereo vision with ultrahigh-speed video recording and off-line data processing, can be used to track moving points with high precision; nevertheless they cannot maintain the precision in ) Corresponding author. Tel.: q34-1-871-1900; Fax: 34-1-871-7050; E-mail: [email protected]
large measuring areas, for instance, the Selspot II, from Selcom. The present development deals with a new 3D measurement real time system, able to record the trajectories followed by a mobile robot without physical connection. The system has been used to track the path of the points of a walking machine. The main goal of the measurement system is to study the behaviour of the vehicle under different control strategies. Ultrasonic sensors are widely used in mobile robot applications for obstacle detection and map elaboration from range data. In such cases the US transmitter and receiver are installed on board and the measurement of the time of fly ŽTOF. spent by the waves to travel from the transmitter up to one target and back to the receiver is used to calculate the distance to the target. In this application the principle of location measurement is different. Combining the distance estimation obtained by measuring the TOF’s of an ultrasonic wave from one transmitting point up to three different receiving points, the position of the emitting point can be calculated in
0924-4247r99r$ - see front matter q 1999 Elsevier Science S.A. All rights reserved. PII: S 0 9 2 4 - 4 2 4 7 Ž 9 8 . 0 0 3 7 8 - 1
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
132
Fig. 1. Ža. General system. Žb. Principle of measurement. T s spark generator, R 1 , R 2 , and R 3 s ultrasonic receivers.
reference to the frame system formed by the three receivers as is suggested by some authors in other applications w1x. Although the principle of measurement is different, the technique used is also pulse–echo and the strategies developed to improve the estimation of distances can also be applied here. In the system described below, a spark plug is used as a source of EM and US waves. Using the big difference in propagation speed of both kinds of waves Ža rate of 1:10 6 ., we can consider the transmission of the electromagnetic wave as instantaneous, using it as the trigger signal Ž t s 0. for measuring the TOF of the ultrasonic wave from the sparking point up to three different receivers, knowing the instant of the US emission without need of any physical connection between the robot and the environment. Finally the distances from the transmitter to the three receivers can be estimated, knowing the speed of ultrasonic waves ŽFig. 1a.. A measurement system has been developed to acquire and process the signals in real time, providing information about the dynamic behaviour of different parts of the robot structure, showing the 3D path of the chosen point.
2. Measurement principle The sparking point is located on the robot, at the point we want to track. This generator produces an electric arc at constant time intervals of about 40 timesrs Žpulse rate frequency.. This start of the arc produces an electromagnetic emission and also an overpressure in the air generating acoustic waves of many wide bands of frequencies. An
antenna, placed in the RF receiver and connected to the central processing unit, detects the electromagnetic emission that is used as a start-signal for the TOF measurement. The value of the PRF is chosen as a compromise between a maximum number of measurements and a time large enough between samples to allow damping of the secondary reflections of the previously transmitted US waves. Having an estimation of the distances from the transmitter up to the three receivers Ž d1 , d 2 , d 3 in Fig. 1b. we can formulate the following equations: 2
2
2
2
2
2
2
2
2
d12 s Ž x y rx 1 . q Ž y y r y 1 . q Ž z y rz 1 .
d 22 s Ž x y rx 2 . q Ž y y r y 2 . q Ž z y rz 2 . d 32 s Ž x y rx 3 . q Ž y y r y 3 . q Ž z y rz 3 .
where Ž x, y, z . are the coordinates of the emitting point and Ž rx n , ryn , rz n . the coordinates of each receiver. Resuming, we have: d i2 s
Ý Ž a y ra i . 2 asx , y , z
These are the equations of the surfaces of three spheres with centres in the receivers r 1 , r 2 and r 3 and radii d i . The solutions are the intersections of the three spheres. If they do not have the same centre and their radii fulfil the conditions that they have the same common points, the intersection would be two points and we will have therefore two possible solutions for the position of the emitting point. As we will see later, one of the solutions can be rejected.
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
For simplicity we can choose our coordinates frame with origin in receiver 1, while the second receiver is placed on the x-axis and the third receiver is contained in the x–z plane; then we have that rx 1 s ry1 s rz 1 s ry 2 s rz 2 s ry 3 s 0. and we find two possible solutions for the coordinates of the emitting point: 2
2
d12 s Ž x . q Ž y . q Ž z . 2
2
2
2
2
d 22 s Ž x y rx 2 . q Ž y . q Ž z .
2
d 32 s Ž x y rx 3 . q Ž y . q Ž z y rz 3 .
2
With the solution: xs
zs
d12 y d 22 q rx 22 2 rx 2 d12 y d 32 q rz 32 2 rz 3
(
y s " d12 y x 2 y z 2 The second solution of the system is symmetrical on the xz-plane and implies one of the limitations of the system; we can only measure on the hemispace y ) 0, which is also the practical case, because it is very difficult to get omnidirectional receivers and in our system the receivers cannot get a signal from the opposite hemispace. 2.1. Influence of the distance between receiÕers in the measurement As we have shown in previous studies w2x, the physical limit for estimating distances by measurement of the TOF of acoustical waves comes from the movements of the transmitting media: the air. This causes an absolute error that is mainly proportional to the measuring distance Žnot for very short ranges less than 10 cm, where other problems as right detection of the TOF or interference phenomena have a bigger influence.. Looking at the solutions, we can feel that the distance between receivers would have a big influence on the precision. To analyse this with detail we look at the error Ž E x . in the estimation of the x-coordinate as a consequence of an error in the estimation of distances, then we have: Ex s
d1 rx 2
Ed 1 q
d2 rx 2
nate, we search for the optimal distance between receivers Ž rx .. That can be done by looking at the point where the first derivative of E x by rx becomes null. That happens when: rx s 2 y The same conclusion can be extracted for the estimation of the z-coordinate. Thus, the most adequate distance between receivers would be twice the distance of the robot to the measuring plane. The error in the estimation of the y-coordinate, would have a different dependence of the distance, its precision decreases when the distance between sensors increases and would be optimal when the distance between sensors is minimal. On the other hand large distances between sensors imply a calibration problem. In the results presented here, we choose a distance of 1 m that we can calibrate well with our instruments and that would be optimal for measurements taken at 0.5 m from the plane formed by the sensors.
3. Walking-machine test bed The RIMHO walking machine has been developed and used for study and testing purposes w3x. It has four legs based on a pantographic mechanism Žsee Fig. 2.. Each leg is driven by three independent electrical actuators. The body is a structure of 700 mm = 700 mm = 250 mm. Legs and body are made of aluminium and including the electronics weigh 60 kg and it can transport about 40 kg. The twelve actuators of the machine let it move in quite different manners, having six degrees of freedom. It can perform different periodic gaits Žleg sequence. such as waves and discontinuous gaits w4,5x. For the tests described in this paper we have considered the crab wave gait w6x. This is a continuous gait in which the body follows a straight line, at constant speed, forming an angle Žthe crab angle. with the longitudinal axis of the body. The experiments were conducted to detect the actual path followed
Ed 2
Supposing that the relative error Ž e d s Edrd . is the same for both measurements and that it is constant for each distance, we have: Ex s
d12 q d 22 rx 2
ed
Choosing for simplicity that the robot is at an equal distance from each receiver Ž d . and at a given y-coordi-
133
Fig. 2. The-four legged walking robot ŽRIMHO..
134
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
by the vehicle when a theoretical change of the crab angle is requested. The changes in the altitude of the vehicle are also studied, specially the variations that occur when a leg changes its supporting status.
4. The measurement system The measurement system ŽFig. 3. consists of two separate devices: the spark-generator with its associated electronics on board of the robot and the receivers with the processing device and computer placed in the work space.
we get acoustical energy at almost any frequency Ždue to the abruptness of the discharge., even in the audible range. Although the acoustical level in the audible range stays far below the inconvenience level. The EM wave generated at the instant that the electric arc breaks down has also a main component at 40 kHz. We use a simple coil as EM receiver, which gives a good signal for the operation distance we want to measure. A quartz clock of 20 MHz combined with digital timers produces at regular intervals a very stable pulse that amplified in current and voltage is finally applied to a transformer that would produce the high voltage for the poles where the electrical discharge would take place.
4.1. The spark-generator 4.2. The US-reception The generation of both waves Želectromagnetic and acoustical. is not produced by a continuous electric arc, but by the changes in electron current and therefore in air pressure. This corresponds to the instants that the arc begins and stops, because we use one single pulse for excitation. The frequency spectrum of the acoustical waves produced by this pulse depends mainly on the width of the pulse and the distance between electrodes. Both are chosen to match the frequency behaviour of the piezoelectric transducers used as receivers which have their maximal sensitivity at 40 kHz. A study with a wide-band microphone, 5 Hz–200 kHz ŽBruel Kjaer 4138., has shown this dependency and has been used to adjust the system in order to get the maximal energy at 40 kHz. In every case
The three ultrasonic receivers are mounted on the vertex of a fixed triangle, making it easier to calibrate the distances between receivers and to conserve the validity of the calibration. In each receiver we have also mounted a pre-amplifier in order to minimize the influence of the noise produced in the wires. This kind of transducer Žflexible bimorph ceramics., is not suitable for the intrinsic temperature correction proposed in Ref. w7x, as is explained in Ref. w8x. Therefore we use one temperature sensor placed near the US sensors, in order to correct for the variation in the speed of sound with temperature. The signal of the temperature sensor is passed to the same acquisition board.
Fig. 3. The measurement system.
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
The signal coming from the US receivers passes through four different analog processing stages Žsee Fig. 4.: gathering by a pre-amplifier of 40 dB placed near the receivers, band-pass filtering ŽB. at the resonance frequency of the transducers, rectification of the negative part of the signal ŽC. and low-pass filter ŽD. in order to demodulate the signal, extracting the envelope. In the lower side of Fig. 4. we can see a typical acoustical signal and the results of the different processing stages. The bandpass filtering improves the signal allowing us to operate with a very good signal-to-noise ratio even at large distances Ž60 dB at 1 m.. Although the system is designed and tested at distances up to 6 m which was considered enough for testing purposes, it can be easily extended to operate at greater ranges. The rectification and the use of a second-order low-pass filter allows us to obtain the envelope with minimal deformation. Comparing it to AM demodulation in RF applications, the difficulty here is the relative proximity in frequencies between both signals: the envelope and the carrier. The obtained envelope is applied to an ArD converter that digitises the signal. With a sampling rate of 100 kHz and a multiplexer for the signal from each channel the acquired data is stored in the memory of a PC, where the digital processing of the signal takes place.
4.3. Digital processing of the signals The measurement of the precise moment of the arrival of the ultrasonic signal is subject to some uncertainty.
135
Even when only one pulse is produced at the spark generator, due to the inertia and narrow band of the transducers, the acoustical wave train received shows a considerable increasing time as show in Fig. 4. The conventional method of detecting the time of arrival using a fixed threshold introduces a big error due to the variation in amplitude with distance w9x: exp Ž ya x . A s A0 x Where x is the distance, a the attenuation coefficient and A 0 is a constant. A dynamic threshold w10x that depends on the time transcurrent from the emission can also introduce an important error due to the variation of the signal amplitude with the angle, transducer, or air movement. On the other hand in this application where the wave is directly transmitted from the emitter to the receiver, we can consider the shape of the wave as constant and try to identify its position on the time axis after digitising the signal. As shown in Ref. w11x, the least-square method using a reference signal would be the most accurate and noise-invariant method to identify the time position of the signal. On the other hand this method needs a lot of calculations and needs a previous calibration process for the wave-form. When we have a high signal-to-noise ratio, other methods can reach similar precision with a drastic reduction in the calculation needs. The algorithm used here makes an estimation of the maximum of the signal Ž Vmax in Fig. 5. using three points around the instant of maximal amplitude Ž t max ., looking for the intersection between the previously digitised envelope of the acoustical wave and the value of half the
Fig. 4. Stages of the analog processing.
136
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
Fig. 5. Determination of the time of arrival.
maximum Ž Vmaxr2.. This point is chosen because it is approximately the point of maximum derivative and therefore the point where an error in amplitude implies the least error in time estimation. For estimating the instant of intersection we consider the signal as a straight line in a short interval around the half value and use the least-square method to find the final point where in the straight line f Ž t 0 . s Vmaxr2. This method allows us to reach a high resolution in time, even better than the sampling time of the signal. This method implies an offset Ž te y t 0 ., because t 0 is not the actual instant of arrival of the signal. But, if the form of the signal does not suffer significant variations, this offset can be considered as constant and can be calibrated once at the beginning.
The result of the measurements shows that the final error is rather a consequence of the air movements in the room w2x than from the measurement method used. Finally a configurable low pass digital filter of first order is implemented to minimize the influence of fast varying error influences. The configuration allows to adapt the filter to the expected speed of the effect Žmovement. we want to measure.
Fig. 6. Error calibration.
Fig. 7. X – Y measurements of the trajectory and the planned path.
5. Results: measurements with the RIMHO walking machine. To estimate the precision of the final system, the sparking device has been put on a test bed Žbank. as used in
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
optical applications. It presents a mechanical positioning precision of 0.1 mm in the tree axis, x, y and z. The range of the axis x and y are 1.5 m while in the z-axis the range is only 0.5 m. To test the precision at large ranges, we have changed the distance between the test bed and the receivers. We must take into consideration that our main interest consists in the measurement of the relative motion of the robot parts. Fig. 6 gives a summary of the calibration measurements. The standard deviation of 100 samples at different distances between test bed and receivers. We have confirmed that the error depends mainly on the distance. Most of the measurement were taken at a distance of 3 m with an error of less than 1 mm. This precision can be considered sufficient for analysing real time effects and even for getting a good estimation of the speed of the moving point calculated as the derivative of the position w6x. This range of measurements is considerably larger than what can be reached with optical cameras which present focusing problems. Algorithms for omnidirectional displacements and altitude control were incorporated into the standard baggage of the RIMHO ŽFig. 2.. To demonstrate the effectiveness of the movement control algorithms, some experiments were performed measuring the position of the centre of the reference frame of the robot using the system described. Fig. 7, shows both: requested path Žstraight line. and actual trajectory of the emitting point corresponding to the crab angles a s y58 and a s q108. Abrupt changes in the position of the sparking point are due to the flexion and foot force redistribution phenomena in the walking machine. It can be observed that the trajectory of the sparking point deviates from the straight line a maximum of 30 mm which can be considered acceptable while the orientation control was not active in this test. The RIMHO has touch-sensors on each foot and as a first approach, after detecting contact with the ground it moves its shoulder to maintain a constant altitude from the ground, due to the difference in forces at the instant of contact and the moment of desired altitude together with the phenomena of redistribution of forces and flexure of the legs. This movement normally does not correct all that is necessary in order to maintain the same altitude. Looking for a solution with the most simple sensors, we have equipped the robot with inclinometers allowing the robot to recover after each leg movement the original altitude. In this case we have shown, using the 3D measurement system, how the information from the inclinometer would be sufficient to maintain the same altitude Žsee Fig. 8.. In this figure we can also see another detail. The vertical marks represent the moments that special events occur: l when a leg is lifted and p when it is placed, while the accompanying numbers identify the different legs. The figure illustrates that changes are more significant when front legs Žnumbers 1 and 2. are lifted than when the rear legs are changed. This result can be expected from the
137
Fig. 8. Variations in altitude. Discontinuities when the legs support the robot or are lifted.
position of each foot in the wave gait and the force redistribution phenomenon. In each case we can see that the altitude of the emitting point remains steady around the origin with variations of about 15 mm.
6. Conclusions An innovative contactless system has been developed to track the 3D position of a moving point. It has been designed for tracking the different parts of a mobile robot, with respect to the environment. The system uses the EM wave and the US wave produced by a sparking punctual device to measure its position. The use of analog and digital processing techniques has led to a precision that can be consider sufficient for analysing the movement of the robot Žin the laboratory. in order to improve the strategies and algorithms of movement control. The simplification of processing algorithms and the electronic circuits used, makes it possible to measure, process, and monitor the effects in real time, with a measurement rate of 40 samplesrs. The possibility of monitoring more than one point Žsequentially. as a way of calculating the orientation of the machine is being studied for future applications. A new system is also being developed, in collaboration with an industrial mobile robot firm, which increases the measuring range. It can also be used for guidance purposes in known environments. In this system the spark generator is substituted by other US transmitter and traditional RF– FM-transmission in order to avoid ignition danger and EMI generation.
References w1x Ch. Delepaut, L. Vanderdorpe, Ch. Eugene, Ultrasonic three-dimensional detection of obstacles for mobile robots, 16th International Symposium on Industrial Robots, 8th International Conference on Industrial Robot Technology, 30 September–2 October, 1986, Brussels, Belgium, pp. 483–490.
138
J.M. Martın ´ Abreu et al.r Sensors and Actuators 75 (1999) 131–138
w2x S. Ros, J.M. Martın, ´ T. Freire, L. Calderon, ´ Digital techniques improve range measurement with ultrasound sensor, Sensors and Actuators A 32 Ž1992. 550–554. w3x M.A. Jimenez, P. Gonzalez-de-Santos, M.A. Armada, A four-legged ´ ´ walking test bed, 1st IFAC International Workshop on Intelligent Autonomous Vehicles, Hampshire, UK, April 1993, pp. 8–13. w4x P. Gonzalez-de-Santos, M.A. Jimenez, Generation of discontinous ´ ´ gaits for quadrupped walking vehicles, Journal of Robotic Systems 12 Ž9. Ž1995. 599–611. w5x P. Gonzalez-de-Santos, M.A. Jimenez, Path tracking with ´ ´ quadrupped walking machines using discontinuous gaits, Computers and Electronic Engineering 21 Ž6. Ž1995. 383–396. w6x M.A. Jimenez, Generacion ´ ´ e Implementacion ´ de Modos de Caminar Ondulatorios para Robots Caminantes, Tesis Doctoral, Universidad de Cantabria, June 1994. w7x J.M. Martın, ´ R. Ceres, L. Calderon, ´ Ultrasonic ranging gets thermal correction, Sensor Review 9 Ž1. Ž1989. 153–155. w8x C. Cai, P.P.L. Regtien, A versatile ultrasonic ranging system, Sensor Review 13 Ž1. Ž1989. 22–25. w9x J.M. Martın, ´ R. Ceres, A comparative study of several approaches for high resolution range measurements using ultrasonic sensors, International Workshop on Sensorial Integration for Industrial Robots ŽSIFIR 89., University of Zaragoza, Nov. 1989, pp. 206–211. w10x J.D. Fox, B.T. Khuri-Yakub, G.S. Kino, High frequency acoustic wave measurement in air, Proceeding of the IEEE 1983 Ultrasonic Symposium, October 31-2, Atlanta, GA, pp. 581–584. w11x J.J. Anaya, C. Fritsch, J.M. Martın, ´ A comparative study of several approaches for high resolution range measurements using ultrasonic sensors, International Workshop on Sensorial Integration for Industrial Robots ŽSIFIR 89., University of Zaragoza, Nov. 1989. Jose M. Martın ´ Abreu was born in 1958 in Isla Cristina, Spain. He graduated in physics from the Universiteit van Amsterdam in 1982 and received the doctoral degree in 1990 from the Universidad Complutense de Madrid. He has developed many research activities in the field of automation of processes and especially on the study of sensors Žfocused on ultrasonic sensors. and their processing and application. He has published many scientific papers and holds several patents. He has also participated in different national and international scientific programmes and congresses.
Ramon ´ Ceres was born in 1947 in Jaen, ´ Spain. He graduated in physics Želectronic. from Universidad Complutense de Madrid in 1971 and received the PhD degree in 1978. After a first stay for one year, in the LAAS-CNRS in Toulouse, France, be has been working at the Instituto de Automatica Industrial ŽIAI., a dependent of the Spanish National ´ Council for Science Research. For the period 1990–1991 he worked in an electronics company ŽAutelec. as R&D director. Since the beginning, Dr. Ceres has developed research activities on sensor systems applied to different fields such as continuous process control, machine tools, agriculture, robotics and disabled people. On these topics he has published more than 80 papers and congress communications, and he has several patents in industrial exploitation. At present Dr. Ceres belongs to the Spanish Delegation of the IMT ŽBrite-Euram. Committee, being deputy director of the IAI. Leopoldo Calderon ´ was born in 1947 in Lumbrales, Spain. He graduated in physics from the Universidad de Sevilla in 1974 and received the doctoral degree in 1984 from the Universidad Complutense de Madrid. Since 1974 Dr. Calderon ´ has been working in the Instituto de Automatica ´ Industrial developing many research activities in the field of automation of processes and especially on the study of sensors Žfocused on ultrasonic sensors. and their processing and application. As a consequence of this activity, Dr. Calderon ´ has published many scientific papers and is author of different patents. He has also participated in different national and international scientific programmes and congresses. Marıa received her BS in physics and PhD degrees from the ´ A. Jimenez ´ University of Cantabria ŽSpain. in 1986 and 1994, respectively. In 1987, she joined the Department of Automatic Control at IAI-CSIC. Her PhD thesis was focused on the generation of wave gaits and adaptability to irregular terrains. Her research interests include configuration, simulation, and implementation of autonomous walkers. Pablo Gonzalez ´ de Santos is a research scientist at the Spanish Council for Scientific Research ŽCSIC.. He received his PhD from the University of Valladolid in 1986. Since 1981 he has been involved actively in the design and development of industrial robots and special robotic systems as well. His work during the last 9 years has been focused on walking machines participating in the development of seven different projects in this area.