NAVIGATION SYSTEM OF AN OUTDOOR SERVICE ROBOT WITH HYBRID LOCOMOTION SYSTEM

NAVIGATION SYSTEM OF AN OUTDOOR SERVICE ROBOT WITH HYBRID LOCOMOTION SYSTEM Jorma Selkäinaho, Aarne Halme and Janne Paanajärvi Automation Technology Laboratory, Helsinki University of Technology, Espoo, Finland Abstract: The paper deals with the navigation system of a hybrid service robot intended for outdoor applications. The system uses different navigation sensors depending the current situation and task being done. The main sensors are laser range finder, GPS and heading gyro. Keywords: autonomous navigation, hybrid locomotion, outdoor robotics 1 INTRODUCTION Service robots intended to work outdoors are supposed to travel relative long distances on variable terrain. Navigation requirements are demanding, because the robot should do accurate navigation close to the work positions and long distance navigation when traveling between them. Navigation system must be reliable and easy to use by both the user directly and by other tasks in the software system. WorkPartner shown in Fig. 1, is a service robot presently under development at HUT Automation Technology Laboratory [ Halme et al., 2001]. It is a centaur-like general purpose service robot intended to work in urban environment. The working area includes mainly outdoor places, like yards, parks etc, but also covered areas like big halls or storages. GPS is a natural positioning system when available, but because availability is not always guaranteed and accuracy is not enough for all needs, other means have been considered, too. These are a laser range finder, heading gyro and odometry. Because of the relatively complex kinematics of the robot mobility system with different locomotion modes, odometry is not a trivial thing. Fusing sensor data from different sensors and logically changing between different configurations of the navigation system are the ways how variable situations can be managed. Although navigation requirements may vary during mission, we start from the fact that 3D pose estimation is mostly needed. Besides radio beacon system, like GPS or gsm-network based systems in the future, other artificial beacon based systems cannot be considered, because this would restrict too much the use of the robot. Therefore, only natural landmarks can be utilized. The robot has a stereo camera system available, but it was decided to use a scanning laser range finder, because distances to suitable structures can vary much as well as the illumination conditions from dark to sunshine. Cameras are, however, used when controlling the tasks of the two hand manipulator system. The accuracy requirements depends much on the tasks included in the mission, but one can say that a typical accuracy needed for successful task execution with the two hand manipulator is few centimeters, and half a meter when traveling. 2 NAVIGATION UNIT Workpartner must know its attitude in order to be able to control stability. The pitch and roll angles of the front body are measured by accelerometers. Fig. 1 The workpartner robot. In the beginning of the project, a three axial magnetometer was chosen as the primary nondrifting heading sensor. It has been successfully used inside a personal car after iron calibration. However, it turned to be too sensitive to disturbances originating from electrical motor currents when installing on the robot. Odometry is commonly used on flat surface navigation. However, the WorkPartner will operate on difficult surfaces where conventional odometry is not reliable. Also changes in locomotion mode causes problems to make odometry accurate. Therefore,

navigation is based on matching successive laser scans in the vicinity of buildings and trees where GPS navigation is not working. On open areas GPS navigation can be used whereas laser scanner does not work because of the lack of targets. GPS and laser scanner are complementary methods that support each other. Odometry is considered as an optional method. The velocity and heading are measured moving by using a GPS receiver. Required velocity is approximately 0.1 m/s. Velocity and heading measurements are based on Doppler effect of signals from at least 3 GPS satellites. The velocity measurement accuracy is about 0.1 m/s. When there is less than three satellites available for the position fix then dead reckoning based on heading gyro and laser mapping is used. This is especially the case when the robot is operating indoors. Natural landmarks are detected by using a 2D-laser scanner. It is installed in the manipulator. Other navigation sensors are installed on a navigation unit. The navigation unit includes a 400 MHz Pentium computer in PC104 size. Additionally a 12 bit A/D converter card in PC104 size is used for reading the Murata ENV piezogyro and two accelerometers. The laser scanner and GPS receiver communicate through 2 serial ports on motherboard. The operating system used is QNX. The operation commands to the computer controlling the articulation angle, leg movements and wheel rpms are sent through Ethernet cable. 3 SENSOR FUSION Indoors or when less than 3 GPS satellites are on sight the navigation is based on heading gyro and laser scan matching. On structured environments wall lines can be identified and used for navigation [Jensfelt, 1999] and [Pears, 2000]. In our case the maps are made on laser range measurements from unstructured objects like bushes and tree trunks. Successive laser scan matching is used to measure the change in position and heading. The assumption behind this approach is that the time interval, and hence the displacement of the robot, between successive scan lines is small. This reduces the ambiguity associated with this type of approach because scenery changes very little between successive scans, ensuring high overlap and limiting effects of occlusion. Further, for small time intervals the displacement can be predicted with high accuracy together with the vehicle model and GPS based odometry. This information can be used to constraint the search for matches, which also decreases ambiguity and the amount of computation needed. The laser range finder measures distance and bearing to an obstacle that is not beyond 30 meters. On every scan total 361 measurements at half a degree intervals are obtained. The laser range finder measures the distance and bearing on a coordinate frame that is fixed on the front part of the robot. When the robot is moving same obstacles will be seen from different viewing points. In order to build a map, a static map coordinate system must be chosen. A natural choice for the map coordinate system is the position and heading of robot front body when it takes the first laser scan. This means that the 361 ranges and bearings of a laser scan are transformed from polar coordinates to cartesian coordinates. A map consists of these 361 x,y-pairs. The next laser scan is taken on the front body coordinates of moving robot. The transition and rotation of the robot body coordinate system between two successive viewing points can be found by matching the corresponding laser maps in cartesian coordinates. The matching is based on rotating and translating the most recent map and then measuring the goodness of fit between two successive maps. A hit is obtained if the distance between one point in previous map and any point in the last map is less than 0.2 meters. This value comes from the fact that the resolution of the laser range finder is 0.26 meters at 30 meters distance. The number of hits are maximized by a direct search algorithm. The searched area in x and y directions is from 0.4 meter to 0.4 meter in 0.2 meter steps around the predicted position. The searched heading is from 6 degrees to 6 degrees in 1 degree steps around the predicted heading. The predicted position and heading are obtained from GPS velocity and heading message. When GPS measurements are not available, heading gyro is used for predicting the heading. Odometry can be used for predicting the travelled distance. The robot is able to initialize its position and heading relative to a map made earlier in the same area. The local initialization map consists of 361 pairs of x and y coordinates. The x-axis points to east and y-axis points to north. Since the piezogyro drifts a continuous calibration of the reference voltage corresponding to the zero angular velocity is needed. At indoor use the heading increment obtained from map matching is used to update the reference voltage. At outdoor environment the GPS heading value is used for reference voltage calibration. Reference voltage update

is restricted to cases when robot is not making any fast heading changes. The reference voltage is initialized in the rest state when the workpartner has not moved yet. Odometry can be used for computing the predicted position. In outdoor environment the NMEA VTG message is used to compute the predicted position and heading. The position estimate drifts slowly and hence the NMEA GGA message is used to make small corrections to position estimates. The correction gain is equal to 0.02/HDOP where HDOP is horizontal dilution of precision obtained from GPS receiver. When NMEA GGA is used the GPS coordinates of the starting point should be known with some accuracy. The route points to goal are picked from a map (Fig. 5) showing the current working environment. This map has been joined from consecutive laser scan lines when the robot has moved in the working area. When navigating the robot orientates towards to the current route point until the distance from current position to the next route point is less than distance between current and next route point. The laser scanner is also used for reactive obstacle avoidance. When an obstacle appear at a distance less than 0.2 m from side of planned trajectory on 9 s forward in time a turn to other direction is commanded. When an obstacle appears at a distance of less than 3 m in front of robot a zero velocity command is send to the main computer. The simple navigation model that explains the movements projected on a horizontal plane fixed on the center of front body coordinates is represented (1). x& 1 = x4 *sin( x3) x& = x * cos( x ) 2 4 3 (1) where x 1 is position to east, x 2 is position to north, x 3 is heading and x 4 is velocity. The laser range finder defines origin of the coordinates. 4 NAVIGATION EXPERIMENTS Some experiments have been performed at the vicinity of a three floors high university office building [Selkäinaho et al., 2000]. WorkPartner robot was driven manually along a 6 meters wide road. Figure 2 shows the route computed by integrating the velocity and heading obtained from a GPS receiver. The origin of the local coordinates is 60.11 N and 24.49 E. After 308 meters long route the position drift caused by Euler integration is equal to -0.5 meters north and 1.7 meters east. Figure 3 shows the GPS position fixes obtained at the same route. There exist position jumps that are over 10 meter. Because metal plates cover of the university building some multipath fixes did occur. The route computed from velocity and heading measurements is much more accurate than that obtained from position message (NMEA GGA). 460 450 440 430 420 410 400 390 380 370 180 200 220 240 260 280 Fig. 2 Workpartner route in meters integrated from GPS velocity and heading. Tests were also made to evaluate the odometry information obtained from matching successive laser scans. The search area used in finding the match was from 0.5 m to 0.5 m sideways, -0.6 m to 0.9 m in heading direction and from 20 degrees to 20 degrees in heading displacement. The position was searched at 0.1 m steps and heading in steps of one degree. 460 450 440 430 420 410 400 390 380 370 180 200 220 240 260 280 Fig. 3 Workpartner route in meters plotted from GPS position messages. After the best estimate for displacement was found, the estimate was fine-tuned by minimizing the sum of squared angle and range displacements of the matched points with quasi-newton method. Matching of two 361-point scans took under a half- second on 533 MHz Pentium III. The laser data was gathered from a route along the side of the university building, corresponding roughly to

lower portion of the route in figures 2 and 3. A total of 220 laser scans was gathered and integrated together. Figure 4 shows the route obtained from the displacement estimates. One can see a drift of some meters as the starting and ending position differ. The environment included bushes, group of young trees and the wall of university building. 10 0-10 -20-30 -40-50 -60-70 -40-30 -20-10 0 10 20 30 40 Fig. 4 A route computed by matching successive laser scans. Place is same as the left wing of route in Fig. 2. At the third experiment autonomous navigation was tested. The robot first initialized its position and heading by comparing the laser scanner readings to a map made by laser scanner earlier in the same area. The navigation unit read a route map given in local laser map (Fig. 5) coordinates. The navigation task was to autonomously move to a trailer placed about 50 meters away in the yard and pick a cardboard box and return it to start position. The task was started without any GPS readings because the building blocked many satellites. Successive laser scan matching was used. After about one minute the robot was able to see enough satellites and it was predicting new poses based on GPS and then matching successive laser scans until to the trailer. 5 LASER MAPPING A mobile robot can build a map of its environment when it takes laser range measurements from natural beacons like bushes and tree trunks on its route. Map building requires that the range and bearing measurements taken from different viewpoints are rotated and translated to the same coordinates. In the coordinate transformations accurate position and heading of the laser range finder is needed. However, any cumulative error in pose estimation causes distortion in the map. [Lu and Milios, 1995] have shown that taking every pose where a laser scan has taken as an estimated state the problem can be avoided. In the following computation in real time is required and the old pose estimates are not updated with new laser scan data and some drifting error will accumulate to the pose. However, it is possible to correct old pose estimates in real time, but this requires some development work An exact laser based map could be made by taking only one laser scan with a long range model like Riegl 3D laser range finder which is reaching up to 350 m with 0.17 degrees angular resolution [Forsman, 2001]. A Sick laser range finder installed in the manipulator was used to form a map of a university building neighborhood. The map build from range measurements from several viewpoints is named as global map and the map build from range measurements from one viewpoint is named as local map. A laser scan consists of 361 range measurements corresponding bearing values from 0 degrees to 180 degrees in laser coordinates. Successive local map matching was used to compute incremental change in position and heading of robot. Then the last local map was transformed to global map coordinates and compared to the global map. If the number of common points in the two successive local maps was over a predefined threshold then new points last local map which are at least 0.2 m apart from old global map points is added to the updated global map. Spurious points that exist only in one laser scan are not filtered out from the map. The map (Fig. 5) was used to define a route to the goal of the working task. The map was in the form of coordinates of occupancy grid points. Occupancy grid size used in map building was equal to 0.2 m and only coordinates of occupied cells were stored. When the map is matched accurately from several consecutive scan lines it can be used for position and heading estimation. Fig. 5 A 2D map build from laser range measurements at different successive viewpoints.

The laser scanner is operating on horizontal plane fixed on the robot manipulator. Because the robot is moving in pitch and roll and the manipulator is pitching the resulting depth picture is partially 3-dimensional. 6 CONCLUSION AND FUTURE WORK Navigation sensors used in workpartner robot have been presented in this paper. Some early test results have been shown, too. The ordinary GPS receiver became an accurate velocity and heading sensor on May 1 st, 2000 when USA government finished to disturb the position signal. The NMEA VTG message of the receiver gives non- drifting heading information better than most optical gyros and the velocity information is comparable or better in accuracy to ground speed radar. Since the GPS receiver needs at least three satellites at sight other sensors must be also used. A piezogyro was used as a secondary heading sensor. Workpartner articulation angle and velocity commands can be used to compute the velocity when GPS satellites are blocked. A laser scanner offers an alternative method for position and heading measurement relative to natural landmarks. It also carries out the task to detect obstacles in the environment. By connecting pitch and roll information to a 2-dimensional laser scanner a partially 3- dimensional view can be obtained. The above presentation shows some early tests that will be followed by new ones in near future. Methods like iterative closest point [Madhavan et al.,1998] or particle filters will be used. Particle filters have been successfully used at indoor environment [Thrun et al., 2000]. Environmental Models. Licentiate thesis, Royal institute of technology, Stockholm. 129 p. [Lu and Milios, 1995] Feng Lu and Evangelos Milios. Optimal Global Pose Estimation for Consistent Sensor Data Registration. IEEE International Conference on Robotics and Automation. Pp. 93-100. [Madhavan et al., 1998] Ray Madhavan, M. Dissanayake and Hugh Durrant-Whyte. Autonomous Underground Navigation of an LHD using a Combined ICP-EKF Approach. Proceedings of the 1998 IEEE International Conference on Robotics & Automation, Leuven. Pp. 3703-3708. [Pears, 2000] Nick Pears. Feature extraction and tracking for scanning range sensors. Robotics and Autonomous Systems 33. Pp.43-58. [Selkäinaho et al., 2000] Jorma Selkäinaho and Janne Paanajärvi. On tuning of Kalman filter. International Symposium on Robotics and Automation, Monterrey, 2000. 5 p. [Thrun et al., 2000] Sebastian Thrun, Dieter Fox, Wolfram Burgard and Frank Dellaert. Robust Monte Carlo Localization for Mobile Robots. Artificial Intelligence. ACKNOWLEDGEMENTS This work has been supported by a grant from Sandvik/Tamrock Ltd. REFERENCES [Forsman, 2001] Pekka Forsman. Feature based Registration of 3D Perception Data for Indoor and Outdoor Map Building. Proceedings of the 3 rd International Conference on Field and Service Robotics, Espoo, Finland. [Halme et al., 2001] Aarne Halme, Ilkka Leppänen, Sami Salmi, and Sami Ylönen. Hybrid Locomotion of the WorkPartner Service Robot. Proceedings of the 3 rd International Conference on Field and Service Robotics, Espoo, Finland. [Jensfelt, 1999] Patric Jensfelt. Localization using Laser Scanning and Minimalistic