Ming Cong and Bo Fang School of Mechanical Engineering, Dalian University of Technology Dalian, 116024, China congm@ * This work is supported by national natural science fund #50675027to Ming Cong Abstract - This paper presents a multisensor system for combining measurements from ultrasonic sensors and navigation for robot mowers. The proposed sensing system enables robot mowers to mapping unknown environments. It is important for an autonomous robot mower to explore its surroundings in performing the task of localization and navigation for mowing. Because of the complexity of the environment, one simple kind of sensors is not sufficient for robot mower to accomplish these tasks. We develop a robot mower equipped with DSP TMS320F2812 as its CPU. The sensing system integrates with ultrasonic sensors, infrared sensors, collision sensors, encoders, a temperature sensor and an electronic compass. A method of high accuracy ultrasonic ranging technology based on wavelet transform is reported to improve the measurement precision of ultrasonic sensors. Simulation studies show that the proposed multisensor fusion method is very effective for the navigation of robot mowers. Experimental results indicate that this sensing system based on generalized auto-correlation method for obstacle detection and localization shows great potential for providing a high performance-to-price ratio and robust solution for robot mowers in dynamic working condition. Index Terms - multisensor fusion, ultrasonic sensors, robot mower, mapping, navigation I. INTRODUCTION Lawn mowing is considered by many to be one of the most boring and tiring routine tasks. The environmental robots are needed urgently to perform the task. Some predictions indicate that the robot mowers will be one of the most promising personal robot applications and have substantial market in the world. Therefore, the concept of Intelligent Robot Mower (IRM) had been proposed for the first time in 1997’s annual conference of the OPEI (Outdoor Power Equipment Institute) [1]. The robots mainly face to the general families to help the busy people and the hypodynamic old folks save the payments for hiring labours, also remove people from noise, pollen and danger of mowing blade. The robot mowers serve for home care as the outdoor mobile robots, actually kind of intelligent mechatronics devices for environment clean-up [2][3]. The important thing is that the robot mowers are representative of some area-covering environmental robots used not only for indoor floor cleaning as in [4] but also in hazardous environments such as removing landmines, cleaning up radiant points and prospecting for resources etc. The robot mowers get great challenges differing from indoor mobile robots. The robot mowers use sensors to understand environments as well as their real-time states for obstacle avoidance, map building, location and navigation in the whole work area. Because of the complexity of the environment, one simple kind of sensors is not sufficient for robot mower to accomplish these tasks. It is necessary to combine the observed sensor data coming from different sensors to reduce the uncertainties of the robot in any working environment. To merge the information from the various sensors, robust and real-time sensor fusion is required [5]. In cases of sensor error or failure, multisensor fusion can also reduce uncertainty in the information and increase its reliability. A sensing system of low cost, low power consumption, high performance is described. The detecting range of ultrasonic sensors is 0.3m~5m, they provide good range information. However, uncertainties in ultrasonic sensors caused by the specular reflection from environments make them less attractive. The detecting range of infrared sensors is 0.02m~1m, they can detect the obstacles within the ultrasonic sensor’s blind zone. In order to satisfy the needs of robot mowers for the low cost and high accuracy ranging technology, the research on the high accuracy ultrasonic ranging technology based on wavelet transform (WT) is reported to improve the measurement precision of ultrasonic sensors. Measurement data gathered from the sensing system are integrated to avoid the robot mower from unknown obstacles and plan an optimum, reliable and realizable plan completely coverage of entire working area. Finally, simulation studies and experimental results show the effectiveness of the sensing system for the navigation, obstacle detection and localization of robot mowers. II. SYSTEM HARDWARE OF IRM The IRM uses DSP TMS320F2812 as its CPU, including four units: vehicle system, cutting system, sensing system and control system. The sensing system is used to collect the external dynamic information of the working environment for obstacle avoidance, map building, navigation and localization. It is also used to detect vehicle system’s movement parameters and cutting mechanism’s working status. The controller compares the acquired information with the database, and then sends out revisory and accurate command to the robot to perform its tasks. The hardware of the IRM is shown in Fig. 1. Multisensor Fusion and Navigation for Robot Mower * 978-1-4244-1758-2/08/$25.00 ? 2008 IEEE. 417 Proceedings of the 2007 IEEE International Conference on Robotics and Biomimetics December 15 -18, 2007, Sanya, China Fig. 1 Hardware overview of IMR The robot must be physically strong, computationally fast, behaviourally accurate and safety. It should have the ability to perform on its own, and required no human intervention during the whole or most part of the mowing period. The IRM is modularized designed and each unit of the IRM is relatively independent. Modularized design makes the maintenance much easier. Any broken unit of the IRM can be replaced directly without influencing the functions of other units. III. SENSING SYSTEM A. Ultrasonic Sensor Unit Because ultrasonic sensors can provide good range information based on the time of the flight (TOF) principle, mainly due to their simplicity and relatively low cost, they have been widely used in mobile robots for obstacle avoidance, map building and so on. This type of external sensor is very good in obstacles distance measurement. The main lobe of the sensitivity function is contained within an angle of 20 degrees, as shown in Fig. 2 [6]. A number of tests showed that the range accuracy of the sensors is in the order of ±2cm. Fig. 2 Typical intensity distribution of an ultrasonic sensor On IRM, we set up a sensor array which consists of 12 ultrasonic sensors spaced 30 degrees apart. The ultrasonic signals can cover all the space around and satisfy the space requirement about which robot can detect the environmental signals. Classical techniques used in ultrasonic transducers are based on TOF measurement, which calculates the distance of the nearest reflector using the speed of sound in air and the emitted pulse and echo arrival times. The distance d to a reflected object is calculated by ()2dct=× (1) where c is the speed of sound, and t is the time-of-flight. The TOF method produces a range value when the echo amplitude first exceeds the threshold level after transmitting, ignoring a second echo from a further reflector. The ultrasonic sensor unit includes a trigger pulse generation unit, a multi-channel selection unit and an echo receiving unit. A sensor interface circuitry designed to send and receive ultrasonic sound pulses catches always the first returning echo. The range data related to an object is considered to be on the conic axes even if it is located off the axes. The ultrasonic wave typically has a frequency between 40 and 180 kHz, and the frequency of the ultrasonic sensors used in the system is 40 kHz. The beam angle is 20 degrees. The 40 kHz PWM pulse is generated by the general-purpose timer unit of DSP. To drive the transmitter effectively and not to bring much vibration, an 8 cycle burst of ultrasound at 40 kHz is sent out once a time. When the ultrasonic pulse is emitted, the sensor will experience “ringing”. Ringing caused by the transmitted pulse can cause the receiver to detect a false echo. This problem is solved by not enabling the capture interrupt of DSP until a delay interval has passed. This means that the ranger can not detect an object whose distance from the sensor is less than half the distance that sound travels during the delay interval. This is the blind zone of the ultrasonic sensor, as shown in Fig. 3. Trigger pulse Emitted signal Received signal TOF Blind zone Echo Fig. 3 The sketch map of ultrasonic transmission and reception B. Infrared Sensor Unit and Other Sensors To overcome the ultrasonic sensor’s blind zone, infrared sensors are added. The infrared sensors can detect obstacles within 20cm, which patch up the problem caused by the blind zone problem of ultrasonic sensors. This unit has 16 infrared sensors. Each infrared range finder has a conic view of 6 degrees which is the main lobe of the sensitivity function. This sensor has a useful measuring range of a target up to about one meter with high accuracy. A number of tests showed that the range accuracy of the sensors is in the order of ±lcm. In order to save the DSP’s resource, 16 infrared sensors are connected with DSP TMS320F2812’s data interface 418 instead of the IO interface. This kind of architecture can also read the sensors’ status at the same time, ensuring the real- time capability of the system. A sensor interface circuitry designed to send and receive infrared pulses catches always the first retuning echo to process its amplitude. Robot mower works in an outdoor environment, where the temperature changes rapidly. The changing of temperature will affect the speed of sound. Therefore, a temperature sensor is used to guarantee the precision of the ultrasonic sensor. Collision sensor is a group of sensitive swatches, which used to prevent the damage caused by unexpected collision. Because moist environment do harm to the circuit of the IRM, humidity sensors are introduced to detect the humidity of the environment. Although these sensors are not absolutely necessary for an autonomous robot mower, they can provide helpful functions to make the work availability and safety. IV. SENSOR-BASED NAVIGATION A. Mapping As seen in Fig. 4, a reference direction x is defined and the robot coordinates are shown as R x , R y . By the help of an electronic compass built in on the robot [7], the angle i θ , which is the ith sensor’s angle from the 1st sensor, can be easily measured. Actually if only the angle S θ (heading angle of the robot) is measured, other sensor angles can be found as iSi β θθ=+ (2) where i β is the angle to the our world coordinate center. The number of maximum sensor group on the ultrasonic ring is n, and the radius is r (in our system n=12 and r=0.25m). The distance between the origin and the center of the ring is R, and reference angle to the center isΩ . The reference position of the robot's center is ( R x , R y ). The distance from the origin to object which is detected by the ith sensor data on the two dimensional plane is called i R . Now let i dm denote measured value which is combined data from the ultrasonic and infrared sensors, for the exact distance i R . There will be an error i δ between these values as iii dm d δ=+. (3) In this work we naturally assume that i δ is a uniform random variable in the range of (-W, W). Here W denotes the maximum distance measurement error. Here the problem is, given R x , R y , r, 12 ,,, n θθ θg34 , and 12 ,,, n dm dm dmg34 , to estimate the coordinates of the occupied cells i x and i y (or equivalently i R ) in most efficient way. The equations involving the detected object can be written as 222 (( )cos()(( )sin() iR i i R i i Rxrd yrdββ=++ +++ (4) 22 2 ()2()(cos()sin() iii RR rd rdx yββ=++ + + + 22 2 ( ) 2( )cos( ) ii R R R rd rd β=++ + + Ω? (5) y x x y |? R R |? g455 d d O Fig. 4 The robot position on x-y section The equations involving the robot due to the object can be written as 222 (( )cos()(( )sin() iiii i Rxrd yrdββ=?+ +?+ (6) 222 2 ()2()(cos()sin() ii i ii ii i Rxy rd rdx yβ β=+++ ? + + If we define the positions as: [ ] [ ] 11 ,, , TT in Ppp p xy==g34 , then we have [ ] 22 2 ( ) 2( ) cos( ), sin( ) ii ii ii R R rd rd y Pββ=++ ?+ (7) After the inserting the 2 i R in 2 R , [][] ( ) cos( ) cos(),sin() iiii rd R y Pβββ++ Ω? = (8) Here again we have n such equations. And we write them in matrix form [][] i mAP= (9) And if we introduce new matrix as [][ ] () cos(),sin() iiii LPβββ= and [] [ ] 0,0φ = , then (10), can be written as 111 1 2 cos( ) ( ) () cos( ) ( ) R nRn nn rdm R L p L rdm R L p ββφφφ φβφ φ φφ φ βφφφβ ++ Ω? ?g170g186g170 g186g170g186 g171g187g171 g187g171g187 ??? g171g187g171 g187g171g187 g171g187g171 g187g171g187?= ?? g171g187g171 g187g171g187 ?? ? g171g187g171 g187g171g187 g171g187g171 g187g171g187 ++ Ω? ? g172g188g172 g188g172g188 Here if we perform the least squares estimate for i P , we obtain 1 ()( ) TT lsq i PAAAm ? = (11) Thus we find the best squares estimate of the positions. B. Simulation Studies Sensor-based navigation has been tested with simulation to shown the usefulness of this sensor fusion method in the two environments respectively as shown in Fig. 5 and Fig. 6. The mower has been primarily tested in a structured laboratory as shown in Fig. 5. Start at (0.3m, 0.5m, 0degree), a virtual 419 robot was driven around a virtual square corridor one time. The walls in the artificial environment are denoted by the real map. The entire vehicle is self-contained. It has a maximum travel speed on 0.4 m/s. The laboratory area was surveyed out to a 10cm grid with accuracy better than about 1cm. To extract the mapping, a start and goal points were presented. The robot position and orientation were established by the electronic compass [8]. Fig. 5 Data collection and navigation result in structured environment The result in Fig. 5 demonstrates the mapping quality and the usefulness of this sensor fusion method. In the tests, we find that the average error (ε ) in estimating the position of the obstacles in the environment was in the range of [-0.2, 0.2]m. In the simulations we see that () lsq i P in (11), obtained does not satisfy () ilsqi RP= which actually should. In the case a better estimate for the positions can be given as ()() () i ei lsqi lsq i R PP P = (12) In this case, estimate for the angle i Ω does not change but the estimate for distance i R is scaled to it best estimate. Therefore for the position, the distance estimate i R remains the same as before, while the least squares estimate works only for the angle i Ω . Simulations show that this way produces more accurate results. Fig. 6 The simulation result of wall-following behavior Wall following was selected for the initial problem domain because it is a fairly simple problem to set up and evaluate [9]. It also lays the groundwork for more complex problem domains, such as maze traversal, mapping and complete coverage path planning which is used on lawn mowing and vacuuming. The simulation result of wall- following behavior shown in Fig. 6, and the experimental result in Fig. 6 demonstrate that the IRM have the capability to perform its mowing task in unstructured environment. The program of sensor-based navigation simulation in Fig. 5 is given below. Sub Main Dim PI,Fcr,Fct,X_target,Y_target,X,Y As Single Dim X_grid, Y_grid, i, j, C As Integer Dim Frx,Fry,d, dist_targ, rot, Fx, Fy As Single Dim Fcx,Fcy, Rx,Ry As Single PI=3.1415927 Fcr=1 Fct=1 X_target=GetMarkX(0) Y_target=GetMarkY(0) SetCellSize(0,0.1) 'Set cell size 10 cm x 10 cm SetTimeStep(0.1) 'Set simulation time step of 0.1 seconds Do ' Start main loop X=GetMobotX(0) 'Present mobot coordinates (in meters) Y=GetMobotY(0) X_grid=CoordToGrid(0,X) ' indexes of cells where the Y_grid=CoordToGrid(0,Y) ' mobot center is MeasureRange(0,-1,3) ' Perform a range scan and update ' the Certainty Grid (max. cell value=3) Frx=0 Fry=0 ' Each ocuppied cell inside the windows of 33 x 33 cells ' applies a repulsive force to the mobot. For i=X_grid-16 To X_grid+16 For j=Y_grid-16 To Y_grid+16 C=GetCell(0,i,j) If C>0 Then d=Sqr((X_grid-i)^2+(Y_grid-j)^2) If d>0 Then Frx=Frx+Fcr*C/d^2*(X_grid-i)/d Fry=Fry+Fcr*C/d^2*(Y_grid-j)/d End If End If Next Next dist_targ=Sqr((X-X_target)^2+(Y-Y_target)^2) Fcx=Fct*(X_target-X)/dist_targ Fcy=Fct*(Y_target-Y)/dist_targ Rx=Frx+Fcx Ry=Fry+Fcy rot=RotationalDiff(0,X+Rx,Y+Ry) 'shortest rotational difference between 'current direction of travel and direction of vector R SetSteering(0,0.5,3*rot)'mobot turns into the direction of R 'at constant speed and steering rate 'proportional to the rotational difference StepForward Loop Until dist_targ<0.1 'Loop until mobot reaches the target End Sub 420 V. ULTRASONIC RANGING TECHNOLOGY BASED ON WT Unfortunately, the practical received multi-echoes has time-varying property and is a typical non-stationary signal because the influence of the environmental complexity and the noise. Furthermore, the noise mixed in the ultrasonic pulse- echo is Non-Gaussian white noise but colored noise, and correlated with the target echo. The TOF method can not be used directly in such conditions. Referencing the generalized correlation method for estimation of time delay [10], we put forward the generalized auto-correlation method for estimation of time-of-flight based on wavelet transform [11] and present in Fig. 7. Fig. 7 Delay estimation of generalized auto-correlation based on WT Where ()t? is the mother wavelet and () a t τ ? is the daughter wavelet. The coefficient α is the scale (or scaling factor) andτ is the time displacement. The wavelet transform of the signal ()x t is ()yt . Actually this is a filtering process of the ultrasonic echo using a multitude of bandpass filters of equal Q , which is equivalent to the whitening filter of the generalized correlation method for estimation of time delay, in order to eliminate the input noise which can influence the following processing. () yy R τ can be found as ( ) [ ( ) ( )] ( ) [ ( ) ( )] yy xx a a R Eyt yt R t t t ττ ττ??=?=??? As there has the relationship of Fourier transform between auto-correlation function () yy R τ and his power spectrume: 2 () [()] ()()() ()() yy yy xx xx GFRG aaG aωτωωωωω ? ==ΨΨ=Ψ We obtain the generalized auto-correlation function as Last, the peak values of () yy R τ are detected to accomplish the estimation of TOF and calculate the real ultrasonic velocity. Fig. 8 Noisy ultrasonic echo Fig. 9 Denoised echo using WT Fig. 10 Auto-correlation function () yy R τ Fig. 11 Peak detection The noisy ultrasonic echo is shown in Fig. 8, and the denoised ultrasonic echo by wavelet transform is shown in Fig. 9. It is obvious that the noise mixed in the ultrasonic echo is effectively eliminated after WT operation. The auto- correlation operation () yy R τ of the denoised ultrasonic echo is shown in Fig. 10. Fig. 11 shows the envelope of () yy R τ through Hilbert transform. As we can see, if the abscissa of every peak point is determined, the estimation of TOF g109 ND can be calculated. Considered the attenuation of the ultrasonic echo and the demand of the high precision in practice, only the former four echoes are used to estimate the TOF. The values of the TOF estimation are g110g110 g109 g108 g109 g109 3,2, ,,2,3DDDDDD??? , which are symmetrical to the x-axis. Using this method, the estimation of the ultrasonic