Application of Robot Navigation Technology for Civil Robot Development

Transcription

1 Safety, Reliability and Risk of Structures, Infrastructures and Engineering Systems Furuta, Frangopol & Shinozuka (eds) 2010 Taylor & Francis Group, London, ISBN Application of Robot Navigation Technology for Civil Robot Development Hyun Myung Dept. of Civil and Environmental Engineering, KAIST, Korea, Seungmok Lee Dept. of Civil and Environmental Engineering, KAIST, Korea, Haemin Jeon Dept. of Civil and Environmental Engineering, KAIST, Korea, Bum-Joo Lee Dept. of Electrical Engineering, KAIST, Korea, ABSTRACT: In implementing autonomous navigation systems, localization and map building of mobile robots are key technologies. Owing to many efforts in the past decade it has proved that simultaneous localization and map building (SLAM) is possible. Features or landmarks obtained from sensor measurements are registered and associated in SLAM if they are sustainable and correctly matched ones. To have such features it is essential to use a sensor system that provides precise range measurements. Typical sensors for range measurement include the laser range finder (LRF), ultra sonic, or vision sensor. A LRF provide very accurate and long range measurements while measurements from ultrasonic sensors are shorter and coarse due to cone shaped beam patterns. In this paper, a structured light system which combines laser and vision is considered for use with structural displacement measurement. We propose multiple dual laser-vision system to be used as a structural displacement measurement. The proposed dual laser-vision system is quite similar to the structured light system but it solves accurate relative pose instead of range map of the environment. Some preliminary simulation was done to show the effectiveness of the system for the long span structural displacement monitoring. Another good application example such as LBS (Location-Based Service) for U-City (ubiquitous city) will be introduced in the later part. Without initially specifying the locations of ubiquitous sensors, SLAM technique simultaneously estimates the locations of beacons, robot, and even humans. 1. INTRODUCTION As civil infrastructures are exposed to various external loads such as traffic, earthquakes, gusts, and possible wave loads, Structural health monitoring has become an important research topic for continuous assessment and evaluation of structural safety and integrity. Conventional approaches use accelerometer, strain gauge, PZT sensors, etc. The displacement measurement, though important, was not popular due to inaccessibility and the huge size of the civil infrastructures. One of the most frequently employed approaches uses Linear Variable Differential Transformer (LVDT), a contact-type sensor for displacement measurement that needs stable reference points underneath, which is not so practical to be used. Thus the development of structural health monitoring system that measures the displacement of the structure using vision or laser-based mobile robot system can be very helpful. In solving this, we can use the modified structured light system which is widely used in robot navigation. The problem of Simultaneous Localization And Map-building, known as SLAM, has gained great attention in the mobile robotics research area. SLAM focuses on the problem of self-localization while building a map of an unknown environment from a sequence of noisy sensor measurements obtained from a moving robot. SLAM is considered to be a core capability for autonomous mobile robots operating in unknown environments. The dominant approach to the SLAM problem 2552

2 was introduced as a form of Extended Kalman Filter (EKF) Smith et al., 1990, Dissanayake et al., In the last decade, this approach has found widespread acceptance in field robotics. An alternative approach to the SLAM problem has been introduced that factors the SLAM posterior into a localization problem and K independent landmark estimation problems conditioned on the robot pose estimate. This algorithm, called Fast- SLAM Montemerlo et al., 2002, uses a modified particle filter for estimating posterior over the robot path. Each particle possesses K independent Kalman filters that estimate the landmark locations conditioned on the particle s path. FastSLAM has been demonstrated with up to 100,000 landmarks, problems far beyond the reach of the EKF. 2D laser range finders, sonar sensors and cameras are the most popular sensors in indoor SLAM implementations. They can be used separately (Gutmann and Schlegel, 1996) or in combination (Castellanos and Tardos, 1999). Time of flight laser sensors such as SICK laser provide very long and accurate range measurements. Although some error models of TOF type laser sensors were proposed the range measurements are accurate enough for segmentation without preprocessing. On the contrary sonar sensors have wide beam patterns hence inducing large uncertainty. In the SLAM system developed earlier, we used a structured light range sensor system that consists of a B/W camera and a line laser projector (Myung et al., 2004). The sensor provides medium range measurements (up to 4 meters). To use the structured light like system in a long distant observation, it is recommended to put the camera in the observation area not in the projecting area. By formulating this kind of configuration, we will introduce the dual laser-vision system for displacement measurement. In Section 2, the structured light range sensor is introduced for SLAM purpose. Section 3 describes the structural health monitoring system that estimates the 6-DOF displacement using laser and vision sensors. Some preliminary simulation results will be shown to demonstrate the applicability of the proposed system. In Section 4, the application of robot navigation technique to LBS (Location-Based Service) for ubiquitous city will be introduced. Without initially specifying the locations of beacons or ubiquitous sensors, SLAM technique can simultaneously estimate the locations of beacons and robot. Moreover the locations of humans can be estimated with the help of SLAM techniques. 2. STRUCTURED LIGHT SYSTEM FOR SLAM 2.1 Obtaining 2D range profiles In this section we discuss the characteristics of the SL sensor implemented in our SLAM system. As shown in Figure 1, several steps are involved in obtaining a 2D range profile from the laser pattern captured by camera. The first step is to correct the camera image that has been distorted by the lens. The OpenCV libraries of Zhang s method (Zhang, 2000) provide camera s intrinsic and extrinsic parameters as well as lens distortion model coefficients. Scale factors of the intrinsic parameters and all of the extrinsic parameters are used later in mapping the laser regions in the image to range profiles in 2D plane. The following equation is the relationship between real and distorted image coordinates in the model: ˆx = x + x[k 1 r 2 + k 2 r 4 ]+[2p 1 xy + p 2 (r 2 +2x 2 )], ŷ = y + y[k 1 r 2 + k 2 r 4 ]+[2p 1 xy + p 2 (r 2 +2y 2 )], where x and y are ideal, and ˆx and ŷ are real distorted image physical coordinates. The radial distance r is defined as r 2 = x 2 + y 2. Hence the distortion model is characterized by the four coefficients k 1, k 2, p 1 and p 2. The corrected image is then convolved with a kernel to determine the laser region. The kernel is designed in consideration of the thickness of the laser pattern in the image. After the convolution only those pixels whose convolution values are above a threshold remain as the candidate laser regions. Then the image is looked into column by column so that only one laser region is selected in each column. If there are more than one region in a column, the largest region is selected. Finally, in each column, the region is averaged in row values to be mapped in 2D plane. Figure 2 shows the relationship between the camera frame Σ XC Y C Z C and that of the laser projector Σ XP Y P Z P. Camera longitudinal displacement along Y p is annotated with P Y, and angular 2553

3 Image Corrected Image Correction of lens distortion Image processing (detection of laser region) laser region Triangulation for 2D mapping 2D range map Figure 1. Process to obtain 2D range profiles Figure 2. Configuration of camera and laser projector displacement along X p is with α. The following equations are used to map the laser pattern in the image to the 2D plane. x p y p y c z p [ ] x c = = y p = cos α sin α 0 sin α cos α [ ] xs sx f z y s c sy f x c y c z c + 0 P Y 0 is the most well known to find corners from a 2D range scan (Tardos, 2002). One of the difficulties in using the algorithm is to find a right threshold with which the segmentation is done correctly. If the threshold is large, the process will terminate prematurely, and if it is small segmentation will occur even at small difference in the range profile. To find the correct threshold, the sensor characteristics must be considered. Figure 3 shows the nonlinear and discrete characteristics of structured light range sensor. The range resolution decreases with range nonlinearly as shown in the figure. Range resolution (mm) In the equation z p is the longitudinal distance to the laser pattern while x p is the lateral distance with respect to the origin of Σ XP Y P Z P. Laser pattern coordinates [x s y s ] T are the coordinates in the image frame (in pixels) subtracted by the image center. Intrinsic parameters such as image center coordinates and scale factors along image x ( sx ) f and y ( sy ) axes are obtained from the camera f calibration stage. 2.2 Corner Point Extraction from 2D Range Profiles For SLAM it is necessary to find features in a space with which the mapping and localization is possible. In our SLAM algorithm we use corner points as the features. The split and merge method Figure 3. Range resolution along range distance (mm) The threshold in our algorithm is set proportional to the range resolution at each range. Hence, in addition to the 2D range profiles, the range resolution at each range is calculated together in the range calculation of previous subsection, and the following triplets are used for corner point detection: [x p (k) z p (k) r(k)] T (1) where x p (k), z p (k) are the lateral and longitudinal 2554

4 distance to the laser pattern found at column k of the image, and r(k) is the range resolution at that point. (k is the index of image column and in our system k [0 639].) In addition to the location of corners, the start and end angles that make the corner are also calculated to ease the data association of the SLAM routine. If occlusion occurs and only one angle is available then the other will be annotated with a wildcard symbol. Hence each feature point consists of the following quadruplets. [x p z p θ 1 θ 2 ] T (2) 3. STRUCTURAL HEALTH MONITORING SYSTEM 3.1 Dual laser-vision system Recently structural health monitoring has become an important research topic for continuous assessment and evaluation of structural safety and integrity. The development of structural health monitoring system that measures displacement of the structure using mobile robot system can be very helpful. A possible candidate for the displacement measurement of the long structure is illustrated in Figure 4. Figure 5 shows the configuration of the proposed dual laser-vision system which corresponds one module in the overall system described in Figure 4. Laser sensor A composed of at least two point lasers projects its parallel beam to the distant screen B. The distance can be quite large such as 100 meters or so. In screen B, the image of two points are captured by camera B, and we can obtain the two image points. The laser sensor B also projects its two points to the distant screen A. We can also observe two distinct image points in screen A. It is also possible to place the camera at the front side of the screen instead of the backside. We observe overall four different image points in 2D screens and can obtain 8 scalar values. Using this observed values, the relative displacement, i.e., the translation x, y, z, and rotation angle θ, φ, ψ around x-axis, y-axis, and z-axis, are to be estimated. The kinematics defines the geometric relationship between the observed data M = [ A O, A Y, B O, B Y ] T and estimated pose P = [x, y, z, θ, φ, ψ] T. To derive the kinematics, the transformation matrices A T B and B T A can be used. As can be seen in equation (4), the transformation matrix consists of rotation about x, y, z-axis and translation in x, y, z-axis. A T B = T (x, y, z)r(x, θ)r(y, φ)r(z,ψ) = x y 0 cos θ sin θ z 0 sin θ cos θ cos φ 0 sin φ sin φ 0 cos φ cos ψ sin ψ 0 0 sin ψ cos ψ Using the transformation matrix A T B, we can obtain the kinematic equation after some mathematical manipulations. In each screen, we assume the laser sensor is installed a distance of Y S away in y-axis direction. If we consider the laser projected from B to screen A, the screen coordinate A O can be obtained from the following equation: A O = A T B 0 Y S Z AB 1 (4) As z =0on the screen A in coordinate frame A, we let z component of A O vector to zero to obtain Z AB. By substituting the obtained Z AB to equation (4), we can obtain the actual screen coordinate A O of the projected laser beam from B. Inthe same way, we can easily obtain A Y, B O, and B Y. This yields the following kinematic equation (5): By using the steepest descent method, the estimation of P can be obtained as follows: ˆP (k +1)= ˆP (k)+j + p (M(k) ˆM(k)) (6) where J p = M is the Jacobian of the kinematic p equation and J p + is the pseudo-inverse of the Jacobian. 3.2 Simulation results This section describes the simulation result. In the simulation, we set the true values for (x, y, z, θ, φ, ψ) = (0.1, 1.02, 100, 0.001, 0.002, 0.003). Using the distance of z = 100 meters, the estimated value converged (3). 2555

5 Figure 4. Displacement monitoring using multiple dual laser-vision system Figure 5. One unit of dual laser-vision system M = ( cos θ sin ψ sin θ sin φ cos ψ z sin φ + x cos θ cos φ)/(cos θ cos φ) (cos ψ + z sin θ + y cos θ)/ cos θ 1/10( 9 cos θ sin ψ 9 sin θ sin φ cos ψ 10z sin φ +10x cos θ cos φ)/(cos θ cos φ) 1/10(9 cos ψ +10z sin θ +10y cos θ)/ cos θ (x cos θ cos ψ + cos φ sin ψ x sin θ sin ψ sin φ + y sin ψ cos φ)/(cos θ cos φ) (x cos θ sin ψ cos ψ cos φ + x sin θ cos ψ sin φ y cos ψ cos φ)/(cos θ cos φ) 1/10(10x cos θ cos ψ + 9 cos φ sin ψ 10x sin θ sin ψ sin φ +10y sin ψ cos φ)/(cos θ cos φ) 1/10(10x cos θ sin ψ 9 cos ψ cos φ +10x sin θ cos ψ sin φ 10y cos ψ cos φ)/(cos θ/ cos φ) (5) to true values very rapidly. As it can be seen in Figure 6, it converged only after 10 iterations. When there exists a random unform measurement noise, the estimation also quickly converged to the true value as can be seen in Figure 7. We also tested the effect of dynamic disturbance in x-axis direction by applying x = 0.01 sin(0.1t) with the same amount of random uniform measurement 2556

6 Figure 7. Simulation result for random uniform measurement noise of [ 0.001, 0.001]. Figure 6. Simulation result for (x, y, z, θ, φ, ψ) = (0.1, 1.02, 100, 0.001, 0.002, 0.003) noise as in the previous simulation. As can be seen in Figure 8, x direction estimation quickly tracks the dynamic displacement. 4. LBS USING SLAM 4.1 Conventional beacon-based LBS The conventional beacon-based localization techniques usually incorporates trilateration like the same principle used by GPS (Global Positioning System). The most common methods are Timeof-Arrival (ToA) and Time-Difference-of-Arrival (TDoA). The example system is shown in Figure 9. To perform those kind of method, the locations of each beacon should be known prior to localization of a tag. But it would be very hard to determine the beacon location if the number of beacons grows rapidly for use in large scale. The accuracy of localization in this case would be determined by the accuracy of beacon locations. The SLAM (Simultaneous Localization and Mapping) technique used in robot navigation can be used to solve this problem. The autonomous mapping capability of SLAM can estimate the beacon locations without the need to initially specifying the exact beacon locations. The structure of SLAM will be discussed in detail. 4.2 SLAM-based LBS Since only the range data are used in SLAM for LBS, researchers sometimes call it a range-only SLAM technique. Some implementations of rangeonly SLAM can be found in Djugash et al., 2005 and Kurth, The structure of particle filter based SLAM is depicted in Fig.10 (Myung et al., 2004). The basic SLAM structure is the same as that of FastSLAM (Montemerlo et al., 2002) which is also known as a Rao-Blackwellized particle filter. The reader is recommended to refer to the original FastSLAM paper for detailed explanation (Montemerlo, 2003). Here a per-particle maximum likelihood data association combined with Monte Carlo data association is used. An observation (range data from a beacon) is associated with a beacon of the same ID. It should be noted that the term landmark and beacon location are interchangeable throughout the paper since the beacon based SLAM is considered. Initially, the beacon location is initialized with the aid of dead reckoning of the robot. The robot installed with a tag, moves and receives the range data from each beacon. And as the distance traveled can be calculated using the odometry of robot wheels, the beacon location can be initialized using trilateration method. The importance weight of each particle defined by the equation (7) is related to the Mahalanobis 2557

7 Figure 8. Simulation result for dynamic disturbance of x = 0.01 sin(0.1t) with random uniform measurement noise of [ 0.001, 0.001]. Figure 10. SLAM structure Figure 9. Localization of robot using trilateration. Image taken from DustBot Project(EU). with the same ID previously known. Finally, the resampling stage of the FastSLAM is performed whenever the effective number of particles N eff defined by equation (8) is below some threshold such as half of the number of particles. The importance weight is used as selection probability of each particle. N eff = 1 Ni=1 w 2 i N 2 (8) Once the exact location of each beacon is obtained by using SLAM, the particle filter can be used to estimate the location of each human with a unique tag. It should be noted that in simple environment that needs only several number of beacons, EKFbased SLAM can also be used instead of Fast- SLAM. distance between estimated and observed ranges. 5. CONCLUSIONS 1 w t = 2π Zn,t exp{ 1 2 (z t ẑ n,t ) T [Z n,t ] 1 (z t ẑ n,t )} When there exists a measurement noise, the use of extended Kalman filter (EKF) can be more (7) effective. In this case, as the estimated instantaneous translation and rotation are assumed to be where z t is the measured range, ẑ n,t is the estimated range, and Z n,t is the covariance of range stationary, the system function F is an identity data. Since each beacon can transmit it s unique matrix. And the observation matrix is the same as ID, landmark management routine is much simpler the one derived earlier. We don t need to solve the than that of the landmark initialization algorithm inverse of Jacobian in this EKF scheme. Instead, suggested in Dissanayake et al., The range Kalman gain plays the role of inverse of Jacobian. data can simply be associated with the beacon The advantage of EKF is that we can predict 2558

8 the error covariance and it can quickly track the dynamic behavior. We believe that a lot of robot technology can have wide range of application in civil engineering as well as in home environment. For example, these robot techniques can be used to the monitoring of civil structures such as concrete crack width measurement using structured light range sensor as well as the displacement measurement using laser and vision sensors. Another good application example such as LBS (Location-Based Service) for ubiquitous city was also discussed. Without initially specifying the locations of beacons, SLAM technique simultaneously estimates the locations of beacons, robot, and even the humans. dissertation, tech. report CMU-RI-TR-03-28, Robotics Institute, Carnegie Mellon University. Myung, H., Jung, M.-J., Hong, S.-G., Park, D., Lee, H.- K., and Bang, S Evolutionary simultaneous localization and map-building (SLAM) in home environments using structured light 2d range finder. In Proc. of Simulated Evolution And Learning (SEAL). Busan, Korea. on CD-ROM. Smith, R., Self, M., and Cheeseman, P Estimating uncertain spatial relationships in robotics. Autonomous Robot Vehnicles. Tardos, J. D Data association in SLAM. presentation slides at the KTH Summer School on SLAM. Zhang, Z A flexible new technique for camera calibration. IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11): ACKNOWLEDGMENT This research was supported by a grant (07High Tech A01) from High tech Urban Development Program funded by Ministry of Land, Transportation and Maritime Affairs of Korean government. REFERENCES Castellanos, J. A. and Tardos, J. D Mobile Robot Localization and Map Building: A Multisensor Fusion Approach. Kluwer Academic Publishers. Dissanayake, G., Newman, P., Clark, S., Durrant-Whyte, H.F., and M. Csorba A solution to the simultaneous localisation and map building (SLAM) problem IEEE Transactions of Robotics and Automation, 17(3): Djugash, J., Singh, S., and P.I. Corke, July, Further Results with Localization and Mapping using Range from Radio In Proc. of International Conference on Field and Service Robotics (FSR 05). Gutmann, J.-S. and Schlegel, C Amos: Comparison of scan matching approaches for self-localization in indoor environments. In Proceedings of the 1st Euromicro Workshop on Advanced Mobile Robots. IEEE Computer Society Press. Kurth,D., May Range-only robot localization and SLAM with radio Masters thesis, Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, tech. report CMU-RI-TR Montemerlo, M., Thrun, S., Koller, D., Wegbreit, B FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem. In Proceedings of the Eighteenth National Conference on Artificial Intelligence, AAAI Press. Montemerlo, M. July FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem with Unknown Data Association Doctoral 2559