Proceedings oi 2004 IEEElRY International Conference on Intelligent Robots and Systems September 28 -October 2,2004, Sendai, Japan

Personal Navigation System

Jari Saarinen, Jussi Suomela, Seppo Heikkila, Mikko Elomaa and Aame Halme
Helsinki University of Technology Automation Technology Laboratory PLSSOO, 02015 HUT, Finland. Email: Jari.Saarinen@hut.fi

Abstract This paper presents a human dead-reckoning system far beaconless indoor positioning. The system is based on traditional dead-reckoning Senson like compass, gyro, and accelerometers. Due to the difficult kinematics of a human, there are no ready solutions for the odomehy. This problem is solved by using a self-made stride length measurement unit and laser odomehy. AU the sensors are Integrated to a complete system including sensor fusion. The fnnctiondity of the integrated system is verified with tests io an office environment. Finally the test results are analyzed.
Keywords - Pprsonal Navigofion; indoor localisation:
dead reckoning.

I. INTRODUCTION The search and rescue type of missions have gained increasing attention in the past years. Utilizing robotics in these missions can decrease the execution time of the mission and save human lives. The fire-fighting scenario is one of the most common rescue situations. Despite of their name the f r e fighters are not always fighting against the fue; most of their tasks are sorting out false alarms i.e. they check the alarming building in order to fmd out what sensor in which area gave the alarm and was there a real reason for the alarm. Other typical searching and mapping missions for fire fighters are accidents with leaking gases or liquids and - of c o m e - the traditional smoke diving in a buming building.

entities. The aim of the project is to map a totally or partially unknown environment by human and robotic explorers and generate a common map - presence fiom the mapped data. One of the main topics is localization, which is the common denominator for the both entities in order to generate the feeling ofpresence. The accurate position of all crewmembers on a map gives a clear view of the situation to the operator. The exploring entities w l know the position of the others as i l well as their own and - before all - an accurate position makes it possible to map the environment. The a priori map, position of all entities and the real-time mapping data are generated to a model or a representation of the environment. Both the robots and humans can use this representation for navigation and updating their sensor information. The challenges of the work are how to generate a common presence, which both humans and robots can understand, how to map the environment and finally bow to localize a human without a priori map or ready installed beacons. [I]

Il. PERSONAL NAMGATION SYSTEM

The Personal Navigation s s e (PeNa) is a system ytm for localizing a human in indoon. The system calculates the position from an initial position without using a map or external landmarks. Thus the PeNa system m the current state can be referred in robotics terms as a deadreckoning system. The system does not use any extemal information sources for the position update. At the moment a map is neither needed nor generated during a mission. Therefore the performance of the system is time dependant; the longer the mission continues the poorer is the position accuracy (i.e. the position uncertainty is unbounded). The PeNa hardware (see fig.1 and fig.2) includes: batteries, power conversions, Stride Length Measurement Unit (SiLMLI), a fiber optic gyro, a compass, SICK laser scanner and a laptop. All the hardware is mounted in a backpack. The laptop is installed in the front to serve also as a display for user interface. The existing PeNa hardware is a demonstrational prototype, thus the outlook should not be considered for the real firefighting conditions.

Presently a typical fire-fighting scenario includes several lire-fighterpairs and a mission coordinator. The cwrdinator stays outside the building and gives instluctions via a short wave transmitter. Usually there is a rough map of the building in hand, but not a detailed one. As in many other applications the more knowledge can be got from the situation the better. According to tbe 6re fighters, the knowledge of the positions of the fire fighting crew is crucial both for the safety and the mission control. For localization, the 6ame of reference is also needed and in some cases there is no a priori map available for the mission. This means that the localization system has to have the capability to use an existing map of the building or to map the unknown environment during the mission. European Community supported project "PeLoTe" (Building Presence through Localization for Hybrid Telematic Systems) is studying a search and rescue concept based on cooperative human and robotic

possible. While walking, the body of the gyro is continuously swinging around the all three axes due to the human movement. Thus, the gyro is not measuring exactly in the correct plane, which generates extra driA to the beading estimate. The Laser scanner is the SICK LMSZOO. The laser is measuring distances in 2D plane. The device is set to measure a 180 degree sector with 0,5 degree resolution. The compass is a 3D orientation module, with 3D magnetometers, 3D gyros and 3D accelerometers. The module is used as a gyro compensated compass. The self-made Stride Length Measurement Unit (SiLh4U) measures the distances between the ankles. SiLMu is an ultrasound-based device that measures the distance based on Time-of-Flight. The length measurement is done continuously at 60Hz rate. The ultmsonic transmitter is placed on the other foot and receiver to another (see Fig. 3). Microcontroller calculates the distance between the legs and stores the value to the memory. The value is constantly requested by the CPU, which makes tbe higher-level calculations. Reflectors are used in order to widen the transmitting and receiving sectors. For the stride length measurement a beam angle of almost 180 degrees is needed. The reflectors are done from one half of a cone, cut in two from top to bottom (Fig. 3).

Figure 1. PcNa squipmcnt

m.

HARDWARE

.The backpack is a standard biking equipment. The Frame is made of aluminum. The solid h m e is needed in order to maintain the same frame of reference between the different sensors. The solid Frame also supports the equipment and thus the load is easier to carry. The total weight of the system is approx. 14kg without the laptop. The power subsystem consists of two 12V 4Ah lead-acid batteries connected in serial to provide 24V DC. The average power consumption of the PeNa system is less than 30W.

Figure 3. The Stride Lmgth Mcawemml unit

All the sensors are communicating via serial ports. There is an own thread for each sensor and the synchronization of the data is done by using timestamps and buffering. The fact that the data is accessed through a serial port causes some time differences. However, using locks in the code bas minimized the crucial time differences.

IV. METHODS
A.
Heading estimation The heading is estimated independently by fusing the compass and the gyroscope measurements. The fusion is done with a Kahnan filter. In indoors the electric fields and steel structures causes disturbances to the magnetic field. Thus in short term use the compass error can be up to k40 degrees. The fiber optic gyro provides a good short-term accuracy, but is influenced by drift in the long-term use. The typical drift of the gym is approximately 2 degimiu including also the additional drifis caused by the movement of the rotation axis during walking.

Figure 2. The backpack and the hmdwwe

The gyro in PeNa system is Hitachis Fiber Optic Gyroscope HOFG-X. The gyroscope measures the angular velocity of the beading angle. There is no compensation between the other axes. This means that the gyroscope bas to be installed as horizontally as

213

The prediction of the movement is difficult, since there are no control inputs for the human movement. Thus the prediction is done with the gym input:

time stamp is the exact time when the foot lands on the ground and stops moving forward.

X = X, + o * dt , P = P+ Q'dt

(1)

where theXpor is the updated angle estimate, Xpri is the predicted angle estimate, P is the covariance of the estimate, o is the angular speed during time dt and Q is the variance of the gym measurement. The update is calculated fiom the following Kalman equations: e=z-X,
Figure 4. Stride calculationprinciple

S=P+R K=PIS P=P-K*S*K X, = X + K * e , ,

(2)

When a step is completed, the pmgram calculates the stride length hy using the (4).
stride = Jmax"in2
(4)

where the z is the compass measurement and R is the compass variance. Since the only estimated state variable is the angle, all the variables are scalars. The Q/R ratio was experimentally tuned. Too much reliability on the compass causes the filtered value to follow the compass too much. On the other hand, too low reliability causes drift in the long-term use. For the tests a ratio of 0.00001 was used During long test n m s this value w e d out to be too small. However, the short-term angle estimate is better. Calculating The Step Length The consecutive distance measurements form a pattern that represent a sine wave with a DC offset. The measurements horn microcontroller are digitally filtered with a fmt order Chebyshev hand pass filter. The filter is used to reduce outliers 60x11 the data. The transfer function of the filter is:
0.1339 - 0.13392-' H ( z )= 1 - 1.6052z-' + 0.7321z-' '

As illustrated in the Fig. 4, a step forms a rightangled triangle. The minimum leg is measured when the foot passes the other foot. The maximum distance from foot to foot is the hypotenuse.
The accuracy of the SiLMU was tested separately. All the tests were performed by walking along a line. The real waked distance was 55m. The measured total distance varied on both sides of the real distance. The variation seems to be completely random. The fmt measurement did provide an error far bigger than the average, but there was found no particular reason for this. The average error on the 55-meter distance was 0,70 meten. This gives an error percentage of about 1,28 %. The maximum error was 3 meters, the m o r percentage being 5,45X. The complete set of results can be seen in Table 1.
Tablc I Measurement rcdu from the stand-alone test for S i u l l l

B.

The filter was designed with Matlab FDATool. The pass bandwidth of the filter is 1.5 Hz and the three decibel cut-off frequencies of the filter are at 1.25 Hz and 2.75Hz. The frequency band corresponds to normal walking. The PC p r o p m detects the minimum and the maximums of the panem and uses these to recognize a step and to calculate the length of a step. The step is considered to be completed when after increase in the distance between the feet, the distance skxts to decrease again and a new temporary minimum is found. This happens when the distance is at least 15 cm smaller than the last maximum. The last maximum and the m e minimum before the maximum are stored and used in the calculation of the stride length. The time when the last m a x i " was found is also available and can be used as a time stamp for the step (see Fig. 4). Thus the

C. Approximation o the upper body location f An approximation for the upper body location is calculated after each successful measurement. The last

214

complete step is held as a reference and the upper body location is appro-&nated with half of the perpendicular disoncc to the other foot using (5).
r q / Z r m , : -mi":
2

m,,&j/e =

(5)

causes all the time movement in the heading angle. These changes between headings are usually h m 5 to 10 degrees, but the worst case can be up to 40 degrees (when tuming rapidly). The motion between scans is usually 10-30cm in the heading direction. Another difficult error source is the floor (and ceiling) reflections, which occur when the sensor is pointing too low (or high).

The reference point IS held on the foot that touches the ground. The reference point changes when a new completc step is recognized. The position estimate is cdculolcd by using a Kalman filtered heading md stride length measurements. The main idea is that Ihe heading is fixed when the human has both legs on the floor. This happens when the step has been taken. The new location is calculated fur each angle measurement by finding nearest stride measurement. If 3 new nep was uken, the position is updated permanently. Otherwise only a tempurary estimation for the position is calculated. The process of position estimation is illustrated in Fig 5 .

To test the feasibility of using a laser scanner for the human localization a simple algorithm was selected. The algorithm is very similar to the one presented in [4], except the matching is done with raw data instead of evidence grids. The main idea of the algorithm is described below:
Generate a set of poses, relative to the reference scan. 2. Transform the current scan according to the pose. 3. For every point in the current scan find the nearest neighbor in the reference scan. If the neighbor is closer than a threshold, increase the bit count. 4. Afler going tbmugh all the poses, select the pose that has the biggest hit value. For the computing speed reasons the reference scan is sorted according to the x-coordinate (helps to find the closest neighbor), In many cases the reference scan holds enough information for matching between many successive future scans. The benefit of not changing the reference scan every time is that the m r given hy the algorithm is relative to the reference scan. Therefore the error is not accumulated while scans are matched against a "static" reference scan. The decision for changing the reference scan is done according to the number of pairs found between scans. [Z]
1.

Figure 5. Location estimatiao using SiLMU and angle mca~u~cmsntF.

Finally the Least Squares (LSQ) estimation is done for the best estimate. It is expected that the nearest neighbors in the best estimate are correct and LSQ is used to refine the pose estimate [3]. This algorithm proved to be robust for outliers in the data, but due to the nature of the algorithm the speed is a problem. Using the initial position estimates ffom S L M U and the heading fiom the Kalman filter the speed can be increased significantly. Currently the search angle is 4 degrees (increment 0.6 degrees) and the search area is 60x40cm (increment 6cm). The reason of still having such a wide search area is because of improper synchronization of the laser and the gyro, and because the S L M U can give error measurements ffom time to time. Currently only 180 points from a laser scan are used in the scan matching. This reduces the computation time significantly, but also causes some more drifl to the position estimate. The matching gives an update to the position, which is integrated to give the final estimate. The heading result from the Kalman filter is always maintained, and thus it is not integrated after matching.

D. Laser Scan Matching

The localization by using laser range finder is widely researched in the field of mobile robotics (e.g. [3], [SI and [6]). The results with ZD laser scanners have been very promising and this advocates trying the same methods for the human localization and mapping. The transfer of algorithms from robots is not straightfonvard. In mobile robots the sensor is usually installed statically ou the top of the robot, which is not possible in the case of a man. The sensor can have very different orientation in the different stages of the movement. This means that the sensor is not measuring in a plane and the fitting of consecutive measurements can be impossible. Another difference is the lack of accurate odometry. Even the robotic odometry is suffering from inaccuracies in the long-term use; between two scans it is usually accurate. The heading difference is in the common situations the most significant source of errors. The waking

215

V. TESTS AND RESULTS The purpose of the tests was to figure out the performance and the functio~lity the PeNa system of and ensure that the dead reckoning data is accurate enough for future development like SLAM. The tests were performed during one-day period and all the tests were reported without selection.
The integrated system was tested hy walking a closed path. Tbis does not give the absolute error estimate, but it gives some approximation of the error. The closed path usually filters out some of the drifts from the fmal position, thus the results are s h o w as full route tracks. All the data is processed in real-time while waking. The laptop computer has a 1066 M H z Peutium Ill Mobile CPU and Windows XP operating system. The test area was the 2nd floor of the TUASbuilding in Helsinki University of the Technology. The whole test area is illustrated in Fig. 6. The filled area is the comdor neiwork and the black dot marks are the starting and ending points of the test m . s

distance error was only 0,5m hut due to the heading error the total error was 2,7m. The short room and corridor test includes one rather big laboratory room and a small corridor (Fig. 7). The test measures the short-term accuracy in a complicated environment. The result shows that the short-term accuracy is maintained well.

- - u. l- --3 *c -_i*- -- 3

kt

.~

-8.

Figure 7. Short Corridor and mom test

In Fig. 8 is illustrated a simple comdor walk (4 in Tahle 2). The test result looks nice hut in the end the heading error has accumulated up to 8 degrees.

Figure 6. The map of me tcst area

The calculated parameters are: Length of the path Time Elapsed End-Start Error RelativeEnor Estimated heading error The results from the tests are collected in the Table
2.
Table 2. The results of the PeNa tmb. R m c m rwm and C means corridor.

. q
-U .to

L
0

1 -

11 1

yl

4a

(0

F i w 8. Simple Corridor Walk

In Fig. 9 is illustrated a coverage walk (5& in Table 2) that covers all the corridors visible in Fig. 6. The bottom right comdor is actually an open space. The result looks very nice, although in the end the heading error has again influenced quite much to the result.

The reference walk was measured similarly as the

SiLMU tests. The real length of the path was 110m. The

216

as map-based localization, SLAM and beacon based localization. The heading estimation can and should still be improved The planned improvement is to fuse all the information from all the sensors through a position filter. Currently the "fusion" is done only through the scan-matching algorithm and in the separate heading estimator. The goal of the project is to develop a localization system that supports localization of multiple entities in the same frame of reference. It is expected that some kind of a priori map of the building will he at band In the near fume the aim is to develop and test map based localization for PeNa. The great challenge of the work is to coupe with p d a l a priori map. For t h i s reason also some SLAM implementations will be tested The dead reckoning information provides a good base for both of these algorithms. Localization is also supported by operator based position correction. The work will be carried on in the PeLoTe-project, which ends on the spring 2005. ACKNOWLEDGEMENTS The work of all authors has been also supported within the ET-2001-FET framework under project no. 38873 "PeLoTe". The support is gratefully acknowledged. REFERENCES
Suomcls I, S-en I, Halmc A, Harmo P (2003): Proscebgs of O l n htcractivc Building of Presence, The 4' Intmtional nie Conference on Field And Sswioe Robotics,July 1 4 16,2W3. Saarinen J, Marl R, Emen P, Suomela 1, Reucil (2004):

I ...-."?LLk:-,",,'; f " ,rLZ..-?L...-L-,.22 .. ..' .. .

L-~
I)

Figure 9. Long conidor walk

The coverage test (Fig. IO) was done to see the PeNa functionality in large-scale environments. The duration of the test was over 20min and the length of the walked path was almost 60Om. The covered area included all the corridors of the laboratory and all the rooms (all which were accessible for the author). The total number of rooms visited w s about 40, and about a in the half of the cases the doors had to he opened. In general this is a very challenging test for any localization system. Fig. IO shows the outward journey. The rest of the information is left out in order to clarify the figure. In the end the heading is totally lost.

Figure 10. The eovmgc rrplorahan of Ihc antomtion laboratory

VI.

CONCLUSIONS AND F'UTURE WORK

From the very beginning of the PeLoTe project the PeNa demonstrator has been designed to he a deadreckoning system to support the human localization in indwr conditions. The methods have been transferred mainly from robotics and have been under extensive testing and evaluation. Currently all the methods have been integrated to work in real-time. The performance is not yet comparable to the state-of-the-art in robotics, but it has to he kept in mind that the problem is much more challenging when the localized entity is a human being. Like in robotics the heading estimate is the greatest source of inaccuracy. The test results presented in this paper show that the PeNa system provides accurate short-term localization data. As in all dead-reckoning systems, drifts affect the long-term position estimate. The important thing is that the PeNa can estimate the position in all kinds of indoor environments without loosing the accuracy. This makes the result feasible to he used with other methods, such

Sensors And Melhods For Human Deod Reckoning. The 8' Canfcrensc of lntslligcnt Autonomous Syacm, March 10.14, 2004, Amnerdam. L q F. and Milios, E. (1994): Robot Pose Estimation in Wnbtown Environments by Matching Z Range Sum, IEEE D Computer Vision and Panem Recognition Conference (CWR), pp. 935-938. Sehultz A. and Ad- W.(1998). Conrimour loculi:alion wing evidence grids. Proceedings of the B E E Intmationsl Confer" on Robotics and Automation, Leuwen, Bslgiym, may 1988. pp2833-2839. G u t " I.S., Schlegel C. (1996), A M O S Compuison of Scan Matching Approachss for Self-Localization in Indoor Enrimnmcnts, Roc. of Ihs 1st Euromicro Workshop on Advanced Mobile Robots (EUROBOT96), IEEE Computer Society Press, pp. 6147, 1996 MA& R and PFcuEil, L. (2000): Building a ZD Environment Map from Laser Raogc-Findcr Data, Proceedings of the IEEE lotclligcnt Vehicles Symposium 2W0, D u b o m , Michigan, USA, pp. 290-295, ISBN 0-7803-6363-9

217