利用影像與雷射測距儀來求解自走式機器人之定位與週遭地圖建立

全文

(1)國立高雄應用科技大學電機工程系碩士論文. 利用影像與雷射測距儀來求解自走式機器人之定位與週遭地圖建立 SIMULTANEOUS LOCALIZATION AND MAP BUILDING FOR MOBILE ROBOT USING VISION AND LASER RANGE FINDER. 研究生：蘇郁椉指導教授：王冠智. 中華民國九十五年七月.

(2) National Kaohsiung University of Applied Sciences. SIMULTANEOUS LOCALIZATION AND MAP BUILDING FOR MOBILE ROBOT USING VISION AND LASER RANGE FINDER. by Eden Y. C. Su Advised by Luke K. Wang. A Thesis Submitted to Institute of Electrical Engineering in Partial Fulfillment of the Requirements for the Degree of Master in Electrical Engineering. Kaohsiung, Taiwan, Republic of China July 2006 © Eden C.W. Hsueh 2006.

(3)

(4)

(5) Simultaneous Localization and Map Building for Mobile Robot Using Vision and Laser Range Finder Student：Eden Y. C. Su. Advisor：Dr. Luke K. Wang. Institute of Electrical and Control Engineering National Kaohsiung University of Applied Sciences. ABSTRACT A novel technique, utilizing only a single Charge-Coupled Device (CCD) camera and a Laser Range Finder (LRF), is presented to resolve the task of Simultaneous Localization and Map Building (SLAM) for indoor mobile robot navigation. Instead of using sophisticated, computationally intensive technique like occupancy grid, and line search for the construction of robot's environment, a feasible alternative, using solely reflected LRF’s projected spots in image plane as the measurements, is formulated to update robot’s pose. The Extended Kalman Filter (EKF) has become a standard technique used in a number of nonlinear system dynamics and measurement process. This approach is used to process the information acquired by the sensors mounted on the mobile robot. However, in recently years, a more robust filtering scheme, so-called Unscented Kalman Filter (UKF), is adopted to accomplish the tracking job. We use line segments as the basic element for the purpose of localization and to build the map. Line segments do provide considerable geometric information about the scene that can be used for accurate and fast localization. The proposed scenario can be applied to indoor robot navigation as well as outdoor’s due to its. i.

(6) distinctive features, namely, simplicity to install and cost reduction.. Keywords : mobile robot, SLAM, vision. ii.

(7) 利用影像與雷射測距儀來求解自走式機器人. 之定位與週遭地圖建立. 學生：蘇郁椉. 指導教授：王冠智博士. 國立高雄應用科技大學電機工程系碩士班. 摘. 要. 本文提出一嶄新技術求取戶外自走式機器人之定位與週遭地圖 (Simultaneous Localization and Map Building, SLAM)建立，而所使用感測器只是一個 CCD 相機與一個雷射測距儀(Laser Range Finder)。不使用複雜且大計算量，如格子佔據(Occupancy Grid)，與線搜(Line Search)來建立機器人週遭環境。我們唯用投射於牆上之雷射測距儀光點所映對影像平面上之點即可達成機器人位置更新。擴張型卡門濾波器(Extended Kalman Filter, EKF)已經成為在許多非線性的系統動態與量測方程式使用方面的一種標準技術，這方法被用來處理裝置在自走式機器人身上的感測器所獲得的資訊。然而，在近幾年，一個強靭之濾波器，即非察覺型卡門濾波器(Unscented Kalman Filter, UKF)來對付高度非線性的系統動態與量測方程式。我們使用線段作為定位和地圖建立的基本元素，而線段確實提供相當多的幾何資訊關於可能用於準確和快速定位的地方。本文所提出之技術不但可用於室內也可適用於戶外導航任務，最突顯之特性即為低成本與易安裝。. iii.

(8) 關鍵詞 : 自走式機器人，SLAM，視覺. iv.

(9) 誌. 謝. 在這兩年的碩士班研究生涯中，首先要非常感謝我的指導老師王冠智教授的耐心指導，不論在課業上的學習或是待人處世方面都使我能獲得更寬廣的想法與收穫。此外特別感謝口試委員蕭飛賓教授、萬欽德教授和黃國源教授對於學生論文內容的細心指導與建議，使本論文更加的周詳與完善。回顧在這兩年的學習過程中，承蒙謝勝治教授與蔡繁仁教授在生活上與課業上的指導也令我受益良多，還有感謝學長黃國禎、同學潘政宏、學弟陳建文及大學部學弟們的鼓勵與協助，讓我的研究工作能更加的順利，當然也要感謝許多老師與同學的幫助，使研究能夠持續不斷的進行。最後，感謝父母的默默支持和兄姊的關懷，對他們的感激無法以言語來形容，故僅以此論文獻給所有關心並支持我的人。蘇郁椉謹誌於高雄應用科技大學電機系中華民國九十五年七月. v.

(10) CONTENTS ABSTRACT ..................................................................................................................... i 摘要 ............................................................................................................................. iii 誌謝 .............................................................................................................................. v CONTENTS ................................................................................................................... vi LIST OF FIGURES AND TABLES ........................................................................... viii CHAPTER 1 INTRODCTION ..................................................................................... 1 1.1 Motivation ......................................................................................................... 1 1.2 Thesis Outline ................................................................................................... 3 CHAPTER 2 FUNDAMENTAL CONCEPTS............................................................. 4 2.1 Simultaneous Localization and Map Building (SLAM)................................ 4 2.1.1 The Dynamic Localization Problem..................................................... 5 2.1.1a Local Techniques ......................................................................... 6 2.1.1b Global Techniques ....................................................................... 9 2.1.2 Map Models.......................................................................................... 12 2.1.2a Grid-Based Approach................................................................ 12 2.1.2b Topological Approach................................................................ 13 2.1.2c Feature-Based Approach........................................................... 14 2.2 Coordinate Frame Transformations ............................................................. 16 2.3 Direction Cosine Matrix................................................................................. 18 2.4 Euler Angles .................................................................................................... 18 2.5 3-D to 2-D Perspective Projection ................................................................. 19 2.6 Transformation between Coordinate Frames and Camera Coordinate ... 21 2.7 The SLAM System Description ..................................................................... 22 CHAPTER 3 ATTITUDE AND LOCATION ESTIMATION METHOD .............. 26 3.1 Extended Kalman Filter................................................................................. 26 3.2 The Unscented Transform ............................................................................. 28 3.3 The Unscented Kalman Filter ....................................................................... 30 3.4 Pose Estimation Using EKF and UKF .......................................................... 32 3.4.1 State Assignment.................................................................................. 33 3.4.2 System (Dynamic) Model .................................................................... 34 3.4.3 Measurement (Sensor) Model............................................................. 35 CHAPTER 4 ENVIRONMENT MAP REPRESENTATION .................................. 37. vi.

(11) 4.1 Extraction of Line Segments.......................................................................... 38 4.1.1 Clustering ............................................................................................. 38 4.1.2 Splitting ................................................................................................ 39 4.2 Line Fitting...................................................................................................... 40 4.2.1 Linear Regression ................................................................................ 40 4.2.2 An Alternative Line Parameterization............................................... 41 CHAPTER 5 SIMULATION RESULTS.................................................................... 44 CHAPTER 6 SUMMARY AND CONCLUSION ...................................................... 60 REFERENCES ............................................................................................................. 61. vii.

(12) LIST OF FIGURES AND TABLES Figure 1 Relationship between two coordinate frames .................................. 17 Figure 2 The perspective projection for imaging geometry ........................... 20 Figure 3 The schematics of robot and its environment .................................. 25 Figure 4 The relationship between camera and LRF...................................... 25 Figure 5 Bloke diagram of the UT ................................................................. 29 Figure 6 Mean and covariance propagations.................................................. 30 Figure 7 Mobile robot coordinate system....................................................... 34 Figure 8 Extracting start point and end point ................................................. 38 Figure 9 Split step of clustering region .......................................................... 39 Figure 10 Result of clustering region ............................................................. 40 Figure 11 Parameterization of a line segment ................................................ 42 Figure 12 The true and estimated of states in x- and y-direction using EKF . 48 Figure 13 The true and estimated of state in z-direction and theta using EKF ................................................................................................................ 48 Figure 14 The true and estimated of the image points (u, v) using EKF........ 49 Figure 15 Measurement errors and innovation sequence of image points (u, v) using EKF ....................................................................................... 49 Figure 16 The estimation errors of states in x- and y-direction using EKF ... 50 Figure 17 The estimation errors in z-direction and theta using EKF ............. 50 Figure 18 The error covariances of states in x- and y-direction using EKF... 51 Figure 19 The error covariances in z-direction and theta using EKF............. 51 Figure 20 The true and estimated of states in x- and y-direction using UKF. 52 Figure 21 The true and estimated of state in z-direction and theta using UKF ................................................................................................................ 52 Figure 22 The true and estimated of the image points (u, v) using UKF ....... 53 Figure 23 Measurement errors and innovation sequence of image points (u, v) ................................................................................................................ 53 Figure 24 The estimation errors of states in x- and y-direction using UKF ... 54 Figure 25 The estimation errors in z-direction and theta using UKF ............. 54 Figure 26 The error covariances of states in x- and y-direction using UKF .. 55 Figure 27 The error covariances in z-direction and theta using UKF ............ 55 Figure 28 The two lines used for simulation .................................................. 56. viii.

(13) Figure 29 Scans taken at real line 1................................................................ 56 Figure 30 Scans taken at real line 2................................................................ 57 Figure 31 Scanned measurement data of lines ............................................... 57 Figure 32 Result of line segment 1................................................................. 58 Figure 33 Result of line segment 2................................................................. 58 Figure 34 The actual and estimated line segments ......................................... 59. Table 1 The system parameters....................................................................... 44. ix.

(14) CHAPTER 1 INTRODCTION. 1.1 Motivation In recent years, there has been a great deal of interest in the use of mobile robots for automation. These machines have the potential to carry out many tasks which are considered undesirable or difficult for humans. In order to accomplish given tasks, a mobile robot should be able to determine its pose (orientation and location), i.e. to localize itself in map-based localization; a mobile robot decides its pose from the correlation between a given global environment map and measurement data from exteroceptive sensors such as video finder [Dudek and Zhang, 1996], ultrasonic range finder [Elfes, 1987], infrared range finder [Cox, 1991], laser range finder, and so on. The simultaneous localization and map building (SLAM) problem for a mobile robot is described as a robot starts at an unknown location as well as unknown environment, it can persistently build the map of the environment, and use this map to calculate the robot’s position with respect to the reference frame. An airborne SLAM scheme for an Uninhibited Aerial Vehicle (UAV), using a single vision camera and inertial measurement unit (IMU), is presented [Kim and Sukkarieh, 2003]. Both pose and the locations of ground landmarks are identified instead of the map construction of the environment. An outdoor mobile robot navigation may use vision to detect landmarks and Differential Global Positioning System (GDPS) to compute its initial position as well as orientation [Kotani et al., 1998]. A real-time implementation of. 1.

(15) SLAM, putting aside some entries of state estimation error covariance matrix during filter processing, was presented [Guivant and Nebot, 2001]; efforts were all being concentrated upon filter computation scheme, and the trade-off was to define some relative local frames associated with landmarks. Reductions on computational complexity were fully illustrated. Mobile robot’s precise localization and map building using laser range finder is one of the popular research fields for mobile robot autonomous navigation in indoor environment [Dubrawski and Siemiatkowska, 1998; Einsele, 2000]. Ultrasonic range sensors were once widely used in the earlier mobile robot navigation stage primarily due to its low-cost. But ultrasonic systems have some evident and still unresolved problems such as cross talk, specular reflection, low angular resolution and limited sampling rate. Almost all of these problems are resolved in laser range finder systems. In recent days, laser sensors are increasingly popular for the quality of data and the competitive cost. At the same time, the vision system is also a common choice for mobile robot platform for its versatility and effectiveness in processing and identifying the image of the static objects and obstacles [Yan et al., 2003]. Both LRF and CCD camera are mounted at the mobile robot. The data sequence recorded is these reflected LRF’s projected spots via CCD camera imaging process. Those identified centers of imaged points are stored and served as the measurement portion of the EKF/UKF. No other complicated image processing techniques are needed. Map maintenance or even map pruning is not considered hereafter. Only the function of map construction is realized. Map building and localization are two fundamental problems in mobile robotics,. 2.

(16) and they are very closely connected. Building an accurate environment map for a mobile robot is important and essential. There are basically two types of maps suitable for mobile robot. A popular map building method is the occupancy grid, first introduced by Moravec and Elfes [Moravec, 1988; Elfes, 1989]. Occupancy grid represents the environment as a two dimensional array of cells, and each cell holds a value which indicates the probability of being occupied. Occupancy grid map is easy to build and useful for environment modeling even with imprecise range sensor like ultrasonic rangefinders. But it is hard to be used directly for localization and path planning due to its inaccuracy. Another type of map is based on geometric primitives; line segment is the most widely used feature. Geometric primitive-based map has been described by many researches [Vandorpe et al., 1996]. The method is presented where a set of short line segments approximate the shape of almost any kind of environment. One major drawback of a map, composed out of line segments only, is that small objects like table legs or other narrow poles are not modeled.. 1.2 Thesis Outline Chapter 2 illustrates the coordinate transformations between different frames; the SLAM system description is also presented. Standard EKF and UKF technique is described in Chapter 3, and its associated equations are listed. In Chapter 4, the map building based on line segment is described. Chapter 5 simulates the SLAM scenario. Chapter 6 consists of summary and conclusion.. 3.

(17) CHAPTER 2 FUNDAMENTAL CONCEPTS. 2.1 Simultaneous Localization and Map Building (SLAM) The problem of localization given a map of the environment or estimating the map of knowing the mobile robot position is known to be a solved problem, and in fact applied in many research and industrial applications [Durrant-Whyte , 1996]. A much more fundamental problem is when both the map and the mobile robot position are not known. This problem is usually referred as Simultaneous Localization and Map Building (SLAM) [Guivant et al., 2000]. When absolute position information is not available, it is still possible to navigate with small errors for long periods of time. The SLAM algorithm addresses the problem of a mobile robot with known kinematics, starting at an unknown position and moving through an unknown environment populated with some type of features. The algorithm uses dead reckoning and observation to estimate the position of the vehicle and to build and maintain a navigation map. With appropriate planning, the mobile robot will be able to simultaneously navigate and build a relative map of the environment. If the initial position is known with respect to a global reference frame or if absolute position information is obtained during the navigation task, then the map can be registered to the global frame. If not, the mobile robot can still navigate while exploring and incorporating new areas to the map.. 4.

(18) 2.1.1 The Dynamic Localization Problem Robot localization has been widely acknowledged as one of the most fundamental problems in mobile robotics. The aim of localization is to estimate the position and orientation (briefly, pose) of a robot in the environment, given a map of the surrounding space and sensor data. Most successful mobile robot systems to date utilize localization techniques, since knowledge of the robot pose is essential for a broad of robotic tasks. Although investigated extensively during the last three decades, the localization problem is still an active field of research because of the lack of a single, generally appropriate method. As it comes out from surveying the vast body of literature on mobile robot positioning, to date three is no universally valid solution. The reader interested in the most common positioning systems is referred to [Borenstein et al., 1996] and reference therein, where commercially available sensors and integrated systems for mobile robot localization are thoroughly reviewed. Despite the heterogeneity of the solutions found in literature, the localization methods proposed so far can roughly be categorized into local and global techniques [Fox et al., 1999]. Tracking, or local, techniques incrementally update the robot position. They require the knowledge (at least approximate) of the initial robot pose of the autonomous vehicle and they cannot typically recover from a possible failure of the robot position tracking. On the contrary, global techniques are designed to estimate the robot pose even under global uncertainty, i.e. when the initial robot configuration is completely unknown. Clearly, techniques falling in the latter category are more powerful than the former, paying the price of higher computational and storage requirements. For instance, they are able to face the so-called “wake-up robot problem”,. 5.

(19) associated with finding an estimate of the pose from scratch, as well as the “kidnapped robot problem”, in which a robot is carried to an arbitrary location during its operation. Global localization methods do not need any kind of initialization of the estimate and can cope with circumstances in which the robot is likely to experience serious position errors.. 2.1.1a Local Techniques The simplest method for tracking the position of an autonomous vehicle is dead reckoning. Derived from “deduced reckoning” of sailing days, it denotes a mathematical procedure for determining the current position of a robot by some previous position through known course and velocity information, over a given length of time. In its easiest implementation, such a technique is usually referred to as odometry, since the position of the vehicle along its path is directly provided by some on-board “odometer.” Optical encoders, directly coupled to the wheel axes, are so far the most common sensors suited to this task. Because of the easiness of the computation involved and the inexpensiveness of the sensory system required, practically all mobile platforms are supplied with odometric modules. A more elaborate dead reckoning implementation is given by the Inertial Navigation Systems (INS) [Leonard and Durrant-Whyte, 1992]. Position estimate is computed by integrating measurements of rate of rotation and linear acceleration, provided by gyroscopes and accelerometers. Both odometry and INS are prone to systematic and stochastic errors. The former are mainly due to uncertainty in the kinematic model of the vehicle or inaccuracy in the sensor model. For example, the distance between the wheel axes, as well as the wheel radii, are known only up to a. 6.

(20) certain degree of accuracy. The latter are the resulting effect of the noise corrupting the measurements. Unfortunately, the integration process, underlying the principle of dead reckoning, leads to error accumulation. Although off-lone calibration procedures, aiming at minimizing the effect of systematic errors on odometric systems, have been proposed [Borenstein and Feng, 1996a], unless additional information from some absolute position sensing mechanism is used, the position error grows without bound, as the robot moves on. Nevertheless, dead reckoning still represents an effective solution for tracking a mobile robot, as far as short-rang navigation is concerned. Since stochastic errors cannot be compensated off-line, filtering is usually introduced to estimate the kinetic quantity to be integrated. For example, a remarkable improvement in the performance of a low cost INS can be achieved if error models of the inertial sensors are generated and included in an Extended Kalman Filter (EKF) [Barshan and Durrant-Whyte, 1995]. Odometric techniques and INS can also be exploited together. In [Borenstein and Feng, 1996b] a method using encoders and gyroscope during different time intervals, is proposed. The aim is to compensate for non systematic odometry errors and, at the same time, to keep the tracking free from the drift associated with the gyroscope. A complementary Kalman filter can be effective to estimate the vehicle’s attitude from the accelerometer signal in low frequency motion and from the gyro signal in high frequency motion. To overcome the long-term drift associated with dead reckoning problem, the most common approach consists of periodically reset ting the estimation error, on the basis of some kind of absolute measurements, i.e. taking with respect to an external reference frame. Mobile robots are usually equipped with several sensors, to avoid the limitation. 7.

(21) of a single measurement system. They are generally distinguished by proprioceptive and exteroceptive sensors. For those sensors whose measuring quantities related to the robot itself (encoders, gyroscopes, accelerometers, compasses), belong to the former while proximity sensors (infrared, sonars, laser range finders, millimeter wave radars), CCD camera, stereocams, and all other sensors performing measurements on the external environment fall into the latter. Fusion of the two kinds of information is usually obtained via some filtering technique, the EKF being the most used [Bar-Shalom and Fortmann, 1988]. In this framework, the uncertainty on robot pose is modelled as an unimodal Gaussian distribution, whose mode represents the pose estimate and whose covariance matrix gives a measure of the estimation uncertainty. [Leonard and Durrant-Whyte 1991] developed a localization algorithm employing EKF to match beacon observations (extracted from sonar scans of the surroundings) to a priori map, given in terms of cylinders, corners, and planes. EKF is used to fuse angular measurements related to light sources, detected using a CCD camera, with the information coming from the odometry. [Cox, 1991] matches distance taken by infrared sensors to a segment description of the robot’s environment, while [Crowley, 1989] explicitly represents the uncertainty on the vehicle position as well as the uncertainty inherent in the sensing process. Line segments extracted by a belt of ultrasonic range devices are used to correct the robot pose estimates by matching, via EKF, the extracted feature to a model of the environment. In [Neira et al., 1999], a specialized version of the EKF, called SPfilter, is developed to estimate the location of a mobile robot from a set of partial and inprecise observations of features in the robot’s environment. All the methods based on the EKF generally models the uncertainty affecting the. 8.

(22) measurement process and the vehicle dynamics as zero-mean, white Gaussian noise. Real-world uncertainties seldom satisfy these hypotheses. Moreover, even when these assumptions are fulfilled, the kalman filter is guaranteed to converge, but the EKF is not. Special caution is considered when neglecting strongly correlated noise or systematic errors. In those cases, the Kalman filter estimates tend to be overoptimistic [Julier and Uhlmann, 1997]. These considerations motivated the adoption of alternative data fusion techniques. To cope with non-white, non-Gaussian noise, [Hanebeck and Schmidt, 1996] propose a set-theoretic approach to the problem of tracking a mobile robot based on angular measurements. Under the hypothesis of bounded errors, information fusion is obtained via set intersections. This work has been later extended to a mixed stochastic/set-theoretic framework], in which the ellipsoidal approximations of the admissible robot poses are uncertain in a statistical sense. In [Jaulin et al., 2000] the problem of robot localization, under the hypothesis of bounded errors, is addressed via interval analysis, the optimal solution being computed via set inversion. The main drawback of this technique is the excessive computational burden, making the proposed algorithm unsuitable for real-time applications.. 2.1.1b Global Techniques Depending on the representation of the robot’s surroundings, several global localization methods have been developed. One of the most common descriptions of the robot’s environment consists in a map that are given in terms of landmarks. According to the definition of [Levitt and Lawton, 1990], a landmark is a distinctive visually event, defining a single direction and being visually reacquirable. More generally, a landmark. 9.

(23) denotes a remarkable feature of the environment which, depending on the sensory system, the robot is to successfully recognize in successive time instants. They are usually distinguished in artificial and natural landmarks. Natural landmarks can be thought of as objects or features that are already present in the environment and have a function other than robot navigation. Artificial landmarks are specially designed objects or markers placed in the environment with the sole purpose of enabling robot navigation. The latter kind of landmarks requires an engineering of the environment, while the former calls for efficient algorithms of feature extraction. Apart from the qualitative approach by [Levitt and Lawton, 1990], most techniques exploit angular and/or distance measurements with respect to landmarks having known position a prior, to compute an estimate of the current robot pose. [Sugihara, 1988] proposes a localization method based on the relative directions of natural landmarks (vertically oriented parts of fixed objects such as doors, desks and wall junctions) extracted from a single image, whose complexity is O( n3 log (n) ). Where n is the number of landmarks in the map. [Krotkov, 1989] extends the above technique by taking into account errors affecting angular measurements, performed via CCD camera. In both approaches, the algorithm is formulated as a search in the Interpretation Tree [Grimson and Lozano-Perez, 1987]. In order to reduce the computational burden of those methods, [Sutherland and Thompson, 1994] suggested a way of choosing the best triple of landmarks (supposed to be distinguishable), with respect to which localize the robot, exploiting relative angular measurements and implicitly assuming bounded noise. Representing the landmark coordinates as a complex number, [Betke and Gurvits, 1997] proposed a least-squares estimation technique, whose complexity is linear in the number of landmarks. Also in. 10.

(24) this case, angular measurements with respect to distinguishable landmarks are processed. More recently, inspired by previous results based on Interpretation Tree, [Mouaddib and Marhic, 2000] developed a localization methods based on goniometrical observations, taken by a panoramic camera, of indistinguishable landmark. The robot’s evolution field is iteratively subdivided into rectangles, as long as they contain at least one admissible landmark/measurement matching, until the desired estimation quality is achieved. To perform an efficient search in the Interpretation Tree and reduce the computational burden, the authors impose angular constraints. The same heuristics was already proposed in [Atiya and Hager, 1993], where a set-based algorithm, for solving the matching problem and computing the robot pose using a stereocam, is presented. A major characteristic of this technique is its ability to provide worst-case, distribution-free error analysis, since bounded measurement noises are assumed. A different description of the environment is represented by the occupancy grids. Developed by [Moravec and Elfes, 1985], they discretized the robot’s space into cells, marked by a score ranging from -1 to 1, representing the belief that the corresponding region is occupied (-1 denotes a certainly empty cell while 1 denotes a certainly occupied one). In this setting, the robot pose can be estimated by matching a local map, built from the current sensor readings, against a prior known map. A measure of the goodness of the match between two maps, and hence a trial displacement and rotation, is found by computing the sum of products of the corresponding cells [Elfes, 1987]. Among other probabilistic methods, those proposed in [Thrun et al., 2000a] have gained remarkable attention. These techniques represent a first application of Particle Filters to mobile robotic problems [Thrun et al., 2002a]. The main idea is to. 11.

(25) approximate the posterior of the robot pose by means of a fixed number of sample poses, or particles, generated according to probabilistic models of the robot dynamics and of the measurement process. Appealing properties of this approach are that it is able to deal with arbitrary distributions and the update of the estimates, given the observations can be carried out in constant time (it depends only on the cardinality of the particle set) and its accuracy increases with the availability of computational resources.. 2.1.2 Map Models Obviously, the SLAM techniques found in literature, strongly depends on the environment description adopted. Nonetheless, all the proposed SLAM solutions can be roughly categorized into three main approaches: grid-based, topological, and feature-based. The reader interested in the most common environment representations is referred to [Thrun et al., 2002b] for a comprehensive overview of the state of the art in robotic mapping.. 2.1.2a Grid-Based Aapproach The grid-based techniques represent the simplest approach to map building. They adopt probably the most intuitive environment representation, i.e. an occupancy grid. Instead of using a set of lines or points to represent objects in the mapping environment. The space is discretized into cells, labelled with a value denoting the probability for that portion of space to be occupied. From the sensor data, the robot creates a local grid, related to its current pose. A search through a set of previously acquired certainty grids is performed to find the best match. The configuration of the robot giving the highest. 12.

(26) correlation is chosen as the robot pose estimate. Once the vehicle is localized, the local grid is merged into the global one, reducing its uncertainty. In [Thrun et al., 1998] a probabilistic grid of the environment is build, supposing that the robot occasionally observes a small number of landmarks (such as hallway intersections, corners, dead ends). The SLAM problem is posed as a maximum likelihood estimation problem, where both the landmark locations and the robot pose have to be estimated.. By. employing suitable probabilistic models for the robot motion and the sensory system, the likelihood of the robot and landmark positions is maximized using the Baum-Welch algorithm. This techniques are particularly appealing for their simplicity and easiness of implementation. A successful application of this approach to the autonomous navigation of a robot acting as a museum tour-guide, has been presented in [Thrun et al., 2000b]. The main drawback of the grid-based approach relies on its intrinsically high computational burden and storage requirement. Moreover, the effectiveness of those methods strongly depends on the accuracy of the sensory system’s model, since misinterpreted noisy data may lead to inconsistent map management.. 2.1.2b Topological Approach Driven by the observation that human beings are able to successfully navigate, in spite of the poor accuracy of their sensory systems and in presence of frequently incomplete knowledge, topological techniques adopt a qualitative description of the surrounding environment, rather than building a metrically accurate map. The environment is represented by a set of distinctive landmarks and places. In [Kuipers and Byun, 1991], a hierarchical description of the spatial environment, is proposed.. 13.

(27) Distinctive places and paths are defined by their properties at the control level, and serve as the nodes and arcs of the topological model. During the exploration, place matching is done using global topological constraints as well as local metrical comparison, in order to discriminate new places from already visited ones. [Choset and Nagatani, 2001] presented a method that exploits the topology of the robot’s free space, embodied into a generalized Voronoi graph (GVG), which encodes some metric information about the robot’s environment as well. [Mataric, 1992] proposes a behavior-based control scheme, which constructs a topological map of the environment from sonar and compass data In general, those methods are appropriate for navigation in simple environments and for creating models of human and animal cognition. However, for many operations, metrically accurate maps, that are globally referenced, are necessary in order to accomplish the goal of the mission.. 2.1.2c Feature-Based Approach In the feature-based approach, the environment is mapped in terms of a set of remarkable features (landmarks), whose positions have to be estimated, together with the robot pose. Feature-based maps can explicitly store a set of points, lines, features or etc. that may be perceived by a mobile robot, along with their position. The first attempts to phrase the SLAM problem in an estimation-theoretic framework can be found in the pioneering words by [Smith and Cheeseman, 1986] and [Durrant-Whyte, 1988], who posed the statistical basis of environment representation. At the same time, [Chatila and Laumond, 1985], [Moutarlier and Chatila, 1989] and [Ayache and Faugeras,. 14.

(28) 1989] started to address the problem of visual navigation via Kalman filtering. As noticed later [Smith et al., 1990], those trends of research had much in common, all showing that the estimation errors of landmark locations are necessarily correlated, since they are based on relative measurements taken from the same uncertain positions (the robot pose). As recently proved by [Dissanayake et al., 2001] for the linear case, in the limit, the landmark estimates are fully correlated. Therefore, maintaining the full state covariance matrix is of paramount importance, as far as map consistency is concerned. As a matter of fact, simply neglecting landmark correlation leads to overoptimistic estimates, as shown in [Uhlmann et al., 1997] and [Castellanos et al., 1997]. Unfortunately, when exploring large areas, densely populated of features, the computational burden required by the update of the full covariance matrix can rapidly become too cumbersome for real-time operations. For instance, if n landmarks have to be estimated, the storage requirements of the EKF grows as n2 and the computational complexity of the measurement update is O(n3), although it can be reduced to O(n2) exploiting the specific structure of the SLAM problem. The cost involved in the management of the full state covariance matrix circumscribes the application of such techniques to middle-sized environments (map-scaling problem). Most recent works are aimed at finding suboptimal solutions, trade off between computational complexity and estimation accuracy. The Covariance Intersection method, proposed by [Uhlmann et al, 1997], is able to perform the map update in constant time, making worst-case assumptions about the correlation (i.e. full correlation) among the state variables, while preserving the consistency of the estimates. Unfortunately, this approach may lead to unnecessary conservative estimates, with slow rate of. 15.

(29) convergence. A simple strategy for reducing the cost of a SLAM algorithm is to fix the maximum number of features to be mapped, as suggested in [Dissanayake et al., 2000a], and “forgetting” far landmarks. This choice is clearly efficient, but a remarkable amount of information is discarded. In fact, when a previously removed landmark is sensed again, its estimate must be reinitialized from scratch in order to preserve map consistency. Another active research topic involves the development of hierarchical techniques, which decompose large maps into smaller submaps.. 2.2 Coordinate Frame Transformations Coordinate frame transformation between 3-D orthogonal Cartesian coordinate frames can be represented by three components: rotations, translations, and scaling. The relationship between the inertial coordinate frame {i} and the body coordinate frame {b}, as shown in Figure 1 is described by the Helmert transformation given in equation 2.1 [Hofmann-Wellenhof et al., 1997].. 16.

(30) Z → →. rb. z. ri. y →. c. {i}. x. {b} Y. X. Figure 1 Relationship between two coordinate frames. →. →. →. ri = c + i Rb rb. (2.1). where i. Rb : 3 × 3 ratation matrix from {b} to {i} →. c : represents the coordinate of origin of {b} expressed in {i}. →. →. ri and rb are three-element vectors consisting of the 3-D point coordinates in the form. [ x y z]. T. . The ratation matrix has the property of orthogonality. (R) i. b. −1. = ( i Rb ). T. (2.2). and, consequently, i. Rb ( i Rb ) = I 3×3 T. (2.3). A 4-by-4 homogeneous matrix may be written:. 17.

(31) → ⎤ ⎡→⎤ ⎡→ ⎤ ⎡ bR 0 ⎤ ⎡I c r × 3 1 i ⎥ ⎢ ri ⎥ ⎢ b⎥ = ⎢ ⎥ ⎢ 3×3 ⎣⎢1 ⎦⎥ ⎣ 01×3 1 ⎦ ⎣⎢03×3 1 ⎦⎥ ⎣⎢1 ⎦⎥ → ⎡b ⎤ ⎡→⎤ b R R c i ⎥ ⎢ ri ⎥ =⎢ i ⎢⎣ 01×3 1 ⎥⎦ ⎢⎣1 ⎥⎦. ⎡→⎤ = Ti ⎢ ri ⎥ ⎢⎣1 ⎦⎥. (2.4). b. where the 4-by-4 matrix bTi is defined as follow → ⎡b ⎤ b − R R c⎥ i i Ti = ⎢ ⎢⎣ 01×3 1 ⎥⎦. b. (2.5). 2.3 Direction Cosine Matrix The rotation matrix R is a 3x3 matrix, consisting of nine unique parameters.Each of these parameters is the cosine of the angle between axes of two coordinate’s rotation. For attitude determination, the rotation matrix R is often referred to as the direction matrix. Direction cosine matrices can completely describe the orientation of one coordinate frame to another without singularities, unlike the Euler angle parameterization. Because the direction cosine matrix by itself does not contain trigonometric functions. Its computation speed and accuracy can be favorable. The main disadvantage of the direction cosine parameterization is that it consists of nine parameters with comparison to three parameters of Euler angle representation.. 2.4 Euler Angles Mathematical formulation show that a maximum of three successive rotations along the coordinate axes were necessary to rotate one coordinate frame into another. 18.

(32) coordinate frame. The rotation angles are referred as the Euler angles. Let (α,β,γ) be the given set of Euler angles. The rotation about the x, y, and z axes with anglesα, β, andγ can be described by the Rx(α), Ry(β), and Rz(γ), respectively. They are formally defined in Equations (2.6), (2.7), and (2.8). 0 0 ⎤ ⎡1 ⎢ Rx (α ) = ⎢0 cosα -sinα ⎥⎥ ⎢⎣0 sinα cosα ⎥⎦ ⎡ cosβ 0 sinβ ⎤ Ry ( β ) = ⎢⎢ 0 1 0 ⎥⎥ ⎢⎣-sinβ 0 cosβ ⎥⎦ ⎡ cosγ -sinγ 0 ⎤ Rz ( γ ) = ⎢⎢sinγ cosγ 0 ⎥⎥ 0 1 ⎥⎦ ⎣⎢ 0. (2.6). (2.7). (2.8). Equations (2.6), (2.7), and (2.8) give the completely form of the direction cosine matrices for right-handed rotations about the x, y, and z axes, respectively. The product of direction cosine matrices is an orthogonal direction cosine matrix. A single direction cosine matrix through a series of three matrix multiplications can represent the Euler angles. Euler angles are useful for describing the attitude of one coordinate frame with respect to another.. 2.5 3-D to 2-D Perspective Projection A perspective projection model, commonly referred as the pinhole model shown in Figure 2, is used when the lens center coincides with the origin of camera frame. The process of 3-D to 2-D projection relates the image points in image plane to those corresponding points in Cartesian coordinate. In order to realize the aforementioned. 19.

(33) process, there are two steps that we have to accomplish. The first step is to transfer the landmark’s position to the camera coordinate. Next, the image coordinate of the projected landmark is found. If the camera coordinate of a landmark is (XC,YC,ZC) and the associated image coordinates is ( u , v ), then XC ZC. u = fC v = fC. (2.9). YC ZC. (2.10). where [u v]T : image point associated with [X C YC ZC ]T : focal length. fC. By referring to the equations (2.9) and (2.10), the transformation is nonlinear and noninvertible. That is, the relations are unique for 3-D to 2-D, but the 3-D coordinates cannot be determined uniquely from the corresponding 2-D image points.. P(XC,YC,ZC). P( u , v ). OC. f ZC Focal Length. XC. u v. YC Figure 2 The perspective projection for imaging geometry. 20.

(34) 2.6 Transformation between Coordinate Frames and Camera Coordinate The relationship between two orthogonal Cartesian coordinates may be formulated by homogeneous transformation matrix. The coordinate transformation from the world /inertial, i.e., {w}, to the camera coordinate (located at the optical center of the lens){C} can be represented by two consecutive matrix multiplications, that is, wTbbTC , where w. Tb is the transformation matrix between {w} and body frame {b}. Herein, the. Laser-targeted wall point Pw = [Xw Yw Zw]T in {w} can be mapped to camera coordinate {C} via the following equation ⎡XC ⎤ ⎡Xw⎤ ⎢Y ⎥ ⎢ ⎥ ⎢ C ⎥ = CT bT ⎢Yw ⎥ b w ⎢ ZC ⎥ ⎢Zw ⎥ ⎢ ⎥ ⎢ ⎥ ⎣1 ⎦ ⎣1 ⎦. (2.11). where [XC YC ZC]T is the aforementioned wall point expressed in {C} and bTC the transformation between body and camera coordinate, respectively. Basically, each homogeneous transformation matrix T consists of two portions, namely, rotation and translation. The matrices wTb and bTC can be written as. 21.

(35) ⎡ TC = ⎢ ⎣0. b. b. ⎡b11 b12 b13 0 ⎤ ⎢ ⎥ RC d 0 ⎤ ⎢b21 b22 b23 0 ⎥ ⎥ = 0 0 1 ⎦ ⎢b31 b32 b33 0 ⎥ ⎢ ⎥ 0 0 0 1⎦ ⎣ . Rotation. ⎡1 0 0 − x0b ⎤ ⎥ ⎢ ⎢ 0 1 0 − y0b ⎥ ⎢ 0 0 1 − z0 b ⎥ ⎢ ⎥ 0 0 0 1 ⎦ ⎣ . (2.12). Translation. ⎡ w11 w12 w13 x0 w ⎤ ⎢w w w y ⎥ w Tb = ⎢ 21 22 23 0 w ⎥ ⎢ w31 w32 w33 z0 w ⎥ ⎢ ⎥ 0 0 0 1 ⎦ ⎣ . (2.13). Rotation. where b. RC : orientation of {C} with respect to {b} frame. b11 , b12 ,....., b33 : rotation coefficient between {C} and {b} frame w11 , w12 ,..., w33 : rotation coefficient between {b} and {w} frame. [ x0b y0b z0b ] : origin of the body frame T [ x0 w y0 w z0 w ] : origin of the world frame T. Note that the d0 may be obtained from the prior knowledge of robot’s initial pose, or maybe identified if it not given.. 2.7 The SLAM System Description The schematics of the proposed SLAM system is illustrated in Figure 3 and Figure 4. The information for l is provided by LRF, and is defined as the distance between LRF and its targeted wall point. The 3-D wall point with respect to world frame is ⎡ X w ⎤ ⎡ x + l cos β sin(φ + θ ) ⎤ Pw = ⎢⎢Yw ⎥⎥ = ⎢⎢ y + l cos β cos(φ + θ ) ⎥⎥ ⎢⎣ Z w ⎥⎦ ⎢⎣ ( z + Z L ) − l sin β ⎥⎦. 22. (2.14).

(36) where ZL : elevation of the LRF with respect to the camera. φ : LRF's orientation with respect to robot β : tilt angle of the LRF The key idea is to transform the imaged wall point expressed in {C} into the body coordinate and ultimately to the world coordinate. A wall point expressed in {C} can be formulated as. ⎡ PC ⎤ C b ⎡ Pw ⎤ ⎢ 1 ⎥ = Tb Tw ⎢ 1 ⎥ ⎣ ⎦ ⎣ ⎦ C ⎡ Rb C db ⎤ ⎡ b Rw d 0 ⎤ ⎡ Pw ⎤ =⎢ ⎥⎢ ⎥⎢ ⎥ ⎣ 000 1 ⎦ ⎣ 000 1 ⎦ ⎣ 1 ⎦ ⎡ C Rb C db ⎤ ⎡ b Rw Pw + d 0 ⎤ =⎢ ⎥⎢ ⎥ 1 ⎣ 000 1 ⎦ ⎣ ⎦ C b C C ⎡ R R P + Rb d 0 + db ⎤ =⎢ b w w ⎥ 1 ⎣ ⎦ ⎡ XC ⎤ ⇒ PC = ⎢⎢YC ⎥⎥ =C Rb Rot (b z , θ ) Pw + C Rb d 0 + C db ⎢⎣ Z C ⎥⎦. 23. (2-15).

(37) where C. Tb = Rot ( y, β ) Rot ( z , −90D ) Rot ( y, −φ ) Rot ( x,90D ) ⎡ cosβ ⎢ 0 =⎢ ⎢-sinβ ⎢ ⎣ 0. 0 sinβ 0 ⎤ ⎡ cos(-90D ) -sin(-90D ) 0 0 ⎤ ⎢ ⎥ 1 0 0 ⎥⎥ ⎢sin(-90D ) cos(-90D ) 0 0 ⎥ × 0 cosβ 0 ⎥ ⎢ 0 0 1 0⎥ ⎥ ⎥⎢ 0 0 1 ⎦ ⎢⎣ 0 0 0 1 ⎥⎦ 0 0 0⎤ ⎡cos(-φ ) 0 sin(-φ ) 0 ⎤ ⎡1 ⎢ ⎥ ⎢ 0 ⎥ D D 1 0 0 ⎥ ⎢0 cos(90 ) -sin(90 ) 0 ⎥ ⎢ ⎢-sin(-φ ) 0 cos(-φ ) 0 ⎥ ⎢0 sin(90D ) cos(90D ) 0 ⎥ ⎥ ⎢ ⎥⎢ 0 0 1 ⎦ ⎢⎣0 0 0 1 ⎥⎦ ⎣ 0. Tw = Rot ( z , −θ )trans (− x, − y, 0). b. ⎡cos(-θ ) -sin(-θ ) 0 0 ⎤ ⎡1 ⎢sin(-θ ) cos(-θ ) 0 0 ⎥ ⎢ 0 ⎥⎢ =⎢ ⎢ 0 0 1 0⎥ ⎢0 ⎢ ⎥⎢ 0 0 1⎦ ⎣0 ⎣ 0 ⎡cos(-θ ) -sin(-θ ) 0 -x ⎤ ⎢sin(-θ ) cos(-θ ) 0 -y ⎥ ⎥ =⎢ ⎢ 0 0 1 0⎥ ⎢ ⎥ 0 0 1⎦ ⎣ 0. 0 0 -x ⎤ 1 0 -y ⎥⎥ 0 1 0⎥ ⎥ 0 0 1⎦. and C. Rb : orietation of {b} with respect to {C}. Rot (b z , θ ) : a rotation of body coordinate's z-axis with an angle θ C. d b : distance between the origin of {C} and the origin of {b}. 24.

(38) Figure 3 The schematics of robot and its environment. Figure 4 The relationship between camera and LRF. 25.

(39) CHAPTER 3 ATTITUDE AND LOCATION ESTIMATION METHOD A number of approaches have been proposed to address the SLAM problem. One solution is Bayesian estimate approach that does not assume Gaussian probability distributions. Another solution is the Kalman filter that is the most common approach [Dissanayake et al., 2001]. Kalman filter gives a robust, optimal, and recursive state estimation to fuse redundant information from different sensors, assuming Gaussian noise on both system and measurement processes. The Extended Kalman Filter (EKF) was proposed by Smith, Self, and Cheesman [smith et al., 1990] who developed an EKF approach for building a stochastic map of spatial relationships. It is the most popular algorithm of linear Kalman filter and the linearization techniques for nonlinear system. However, In recently years, the Unscented Kalman Filter (UKF) has been proposed and adopted as an alternative to the EKF. Both EKF and UKF algorithms deal with nonlinear dynamic equations, but the linearization technique for UKF algorithm is unnecessary.. 3.1 Extended Kalman Filter The extended Kalma filter (EKF) has been widely used for state estimation of nonlinear stochastic systems. The technique of the EKF uses the linearization of the nonlinear functions by truncating the second and higher order of derivatives in the Taylor series expansion. In the EKF, the state transition and observation models need. 26.

(40) not be linear functions of the state, but may instead be (differentiable) functions. The EKF consists of two parts which are state equation and measurement equation. state equation : x ( t ) = f ( x ( t ) , t ) +w ( t ) ,. w ( t ) ~ N ( 0, Q ). (3.1). v ( t ) ~ N ( 0, R ). (3.2). measurement equation : z ( t ) = h ( x ( t ) , t ) +v ( t ) ,. where the function f is a nonlinear function of the state and w is a zero mean Gaussian noise having spectral density matrix Q. h is a nonlinear function of the measurement and v is a zero Gaussian noise having spectral density matrix R. To use the Kalman filter to a nonlinear system, it must be linearized [Brown and Hwang, 1992]. At each time step the Jacobian is evaluated with current predicted state. These matrices can be used in the Kalman filter equations. This process essentially linearizes the nonlinear function around the current estimate. The EKF algorithm consists of two updates which are time update and measurement update. time update : Pk +1 ( − ) = Ak Pk AkT + Qk. (3.3). xˆk ( − ) = f ( xk −1 ( + ) , k − 1). (3.4). measurement update : xˆk +1 ( + ) = xˆk +1 ( − ) + K k +1 ( zk +1 − zˆk +1 ). (3.5). Pk +1 (+ ) = ( I − K k +1 H k +1 ) Pk +1 ( − ) K k +1 = Pk +1 (−) H. T k +1. ( H k +1 Pk +1 (−) H. (3.6) T k +1. + Rk +1 ). −1. (3.7). The xˆk +1 (−) and xˆk +1 (+ ) are prior state estimation and posterior state estimation in time index k+1, respectively. The Pk +1 (−) and Pk +1 (+ ) are prior error covariance. 27.

(41) matrix and posterior error covariance matrix in time index k+1, respectively. The linearizations of system and measurement equations are Ak = Hk =. ∂f ∂x. x = xˆk ( − ). ∂h ∂x. x = xˆk ( − ). (3.8) (3.9). 3.2 The Unscented Transform The Unscented Transformation (UT) is relatively a new method for predicting means and covariances in nonlinear system [Julier et al., 1995]. It represents random variables with a set of sigma points are deterministically chosen using the prior means and covariances, and propagates through system dynamics. −. Consider a n-dimensional random variable x with mean x and covariance Px, propagating though an arbitrary nonlinear function y = f ( x) . The unscented transform creates 2n+1 sigma vectorsχand weights W, given by. χ0 = x. ( χ = x −( χi = x + i. W0( m ) = W0( c ) =. ( n + λ ) Px )i ,. ( n + λ ) Px )i ,. i = 1,..., n i = n + 1,..., 2n. λ. (3.10). (n + λ ) λ. (n + λ ). Wi ( m ) = Wi ( c ) =. + (1 − α 2 + β ) 1 , i = 1,..., 2n 2(n + κ ). where λ = α 2 (n + κ ) − n is a scaling parameter. α determines the spread of the sigma. 28.

(42) points around x and is usually set to a small positive value. κ is a secondly scaling parameter which is usually set to 0, and β is used to incorporate priori knowledge of the distribution of x. ( (n + λ ) Px )i is the i th row (or column) of the matrix square root. These sigma vectors are propagated through the nonlinear function to yield a set of transformed sigma points,. ζ i = f ( χ i ), i = 0,..., 2n. (3.11). And the mean and covariance for y are approximated using a weighted sample mean and covariance of the transformed sigma points as follows: −. 2n. y = ∑ Wi ( m )ζ i. (3.12). i =0. 2n. −. −. Py = ∑ Wi ( c ) (ζ i − y )(ζ i − y )T. (3.13). i =0. Figure 5 illustrates the steps in performing the UT.. Figure 5 Bloke diagram of the UT [Haykin, 2001] A simple comparative example of the sigma-point approach is shown in Figure 6. 29.

(43) [Seyoon et al., 2002] for a 2-dimensional system: the plot (a) shows the true mean and covariance propagation using Monte Carlo sampling; the plot (b) show the performance of the UT (note that only 5 sigma-points are needed for the 2D case); and the plot (c) show the results using a linearization approach as would be done in the EKF. The superior performance of the sigma-point approach is clearly evident.. (a) Actual sampling. (b) UT. (c) First-order linearization. Figure 6 Mean and covariance propagations: (a) The true mean and covariance, (b) UT (5 sigma points for 2D cases), and (c) EKF’s linearization approach [Haykin, 2001]. 3.3 The Unscented Kalman Filter The unscented Kalman filter (UKF) was first proposed by Juiler to address nonlinear state estimation in the context of control theory [Juiler et al., 1995]. The. 30.

(44) algorithm uses a set of carefully chosen sample points to capture mean and covariance of the system. The UKF bypasses the linearization issues of the EKF. Instead of approximating a random process by linearizing the nonlinear functions in the system, UKF approximates the distribution of the random state variable by the deterministically picked sigma points to more accurately capture the mean and covariance of the nonlinear transformation of the random variables. This scheme can also be treated as a statistical linear regression. The UKF is an extension of UT to the Kalman filter framework [Van Der Merwe and Wan, 2001; Tak et al., 2002], and it uses the UT to implement the transformations for both nonlinear process (time update or prediction) and sensor models (measurement update or correction). The standard UKF algorithm is shown as follows: 1. Initialize the state mean and covariance matrix:. xˆ0 = E[ x0 ] for. P0 = E[( x0 − xˆ0 )( x0 − xˆ0 )T ]. (3.14). k ∈ {1,..., ∞} .. 2. Calculate sigma point χ i ,k −1 using equation (3.10). 3. Time update equations (Prediction):. ζ i ,k k −1 = f ( χ i ,k −1 ) , i = 0,..., 2n 2n. xˆk− = ∑ Wi ( m )ζ i ,k k −1 i =0. 2n. Pk− = ∑ Wi ( c ) (ζ i , k k −1 − xˆk− )(ζ i ,k k −1 − xˆk− )T + Qk i =0. ηi ,k k −1 = h(ζ i ,k k −1 ), i = 0,..., 2n 2n. zˆk− = ∑ Wi ( m )ηi ,k k −1 i =0. 31. (3.15).

(45) 4. Measurement update equations (Correction): 2n. Pzk zk = ∑ Wi ( c ) (ηi , k k −1 − zˆk− )(ηi , k k −1 − zˆk− )T + Rk i =0 2n. Pxk zk = ∑ Wi ( c ) (ζ i , k k −1 − xˆk− )(ηi ,k k −1 − zˆk− )T i =0. K k = Pxk zk Pz−k 1zk. (3.16). xˆk = xˆk− + K k ( zk − zˆk− ) Pk = Pk− − K k Pzk zk K kT where Qk : process noise covariance matrix Rk : measurement noise covariance matrix pzk zk : measurement correlation matrix pxk zk : cross-correlation matrix K k : kalman gain xˆk : updated state Pk : update state covariance matrix zk : current measurement. 3.4 Pose Estimation Using EKF and UKF The pose estimation method is based on the EKF and UKF algorithms. The EKF is derived from a stochastic state-space representation of the mobile robot model and of the measured process. This technique uses some statistical relation to link the detected features positions from the uncertain sensor information and a simple mobile robot motion model in order to form an optimal estimate of the mobile robot’s pose. The EKF is applicable to this problem since the dynamics is highly nonlinear as well as its measurement function. Furthermore, instead of truncating nonlinear dynamics to first order as the EKF’s,. 32.

(46) the UKF approximates the distribution of the state with a finite set of points. Since the nonlinear dynamics are used without approximation, it is much simpler to implement and has better results; its performance has been analytically shown to be similar to a truncated second order EKF without the need to calculate Jacobians or Hessians of the dynamics [Juiler et al., 1995]. Both states and system parameters can be estimated in the UKF and EKF algorithm. Then the EKF and UKF equations for kinematics equations of attitude estimation are derived.. 3.4.1 State Assignment The typical kinematics model of a mobile robot can be obtained from Figure 7. The steering control θ and speed νare used with the kinematics model to predict the position of the mobile robot. And lL is the distance between wheel axels. Rear wheel drive car model: x = ν cos θ y = ν sin θ z = 0. θ = ν. (3.17). tan φ lL. φ = ω. Reduced model: x = ν cos θ y = ν sin θ z = 0 θ = ω. (3.18). 33.

(47) Figure 7 Mobile robot coordinate system. The state assignment consists of mobile robot position and orientation (θ). x = [x y z θ ]. T. (3.19). 3.4.2 System (Dynamic) Model The discrete time dynamic model can be written as xk +1 = f ( xk ) + Wk. (3.20). Wk is the process noise and assumed to be additive Gaussian noise with zero mean and covariance Q. the system model is following ⎡ xk +1 ⎤ ⎡ xk + τν cos θ ⎤ ⎢ y ⎥ ⎢ y + τν sin θ ⎥ ⎢ k +1 ⎥ = ⎢ k ⎥ +W k ⎢ zk +1 ⎥ ⎢ ⎥ zk ⎢ ⎥ ⎢ ⎥ ⎣θ k +1 ⎦ ⎣ θ k + τω ⎦. (3.21). where τ is the sampling period; ν is the velocity of robot; ω is the angular velocity of. 34.

(48) robot. The linearization of equation (3.21) leads to. F ( x) =. ∂f k ( x) ∂x x = xk. ⎡1 ⎢0 =⎢ ⎢0 ⎢ ⎣0. 0 1 0 0. 0 − τν sin θ ⎤ 0 τν cos θ ⎥⎥ ⎥ 1 0 ⎥ 0 1 ⎦. (3.22). 3.4.3 Measurement (Sensor) Model The measurement model is zk = h ( xk ) + vk. (3.23). The measurement noise vk is a zero mean, Gaussian noise with covariance matrix Rk . The nonlinear function h ( xk ) comprises the 3-D transformation and perspective projection functions. Consequently, the measurement model follows as ⎡ ⎢ ⎡uk ⎤ ⎢ ⎢v ⎥ = ⎢ ⎢ k⎥ ⎢ ⎢⎣lk ⎥⎦ ⎢ ⎢ ⎢ ⎣. ⎤ ⎥ ⎥ ⎥ YC f ⎥ + Vk ZC ⎥ 2 2 2⎥ ( x − X w ) + ( y − Yw ) + ( z + Z L − Z w ) ⎥ ⎦ f. XC ZC. where the time index k has been omitted for concise expression and. (3.24). ( X w , Yw , Z w ) is the. vector Pw. The linearization of the nonlinear function h results in ⎡ H11 0 H13 H14 ⎤ ∂hk ( x ) = ⎢⎢ 0 H 22 H 23 H 24 ⎥⎥ H k ( x) = ∂x x = x k ⎢⎣ 0 0 0 H 34 ⎥⎦. 35. (3.25).

(49) where H11 =. f z + Z L − l sin β. H13 =. f (− x − l cos β sin(φ + θ )) ( z + Z L − l sin β ) 2. H14 =. f (l cos β cos(φ + θ )) z + Z L − l sin β. H 22 =. f z + Z L − l sin β. H 23 =. f (− y − l cos β cos(φ + θ )) ( z + Z L − l sin β ) 2. H 24 =. f (−l cos β sin(φ + θ )) z + Z L − l sin β. −1 1 2 2 2 2 ⎡ ⎤ H 34 = ⎣(−l cos β sin(φ + θ )) + (−l cos β cos(φ + θ )) + (l sin β ) ⎦ × 2 [ (−2l cos β sin(φ + θ ))(−l cos β cos(φ + θ ))] + [ (−2l cos β cos(φ + θ )). (l cos β sin(φ + θ )) ] The required elements of the EKF are given above. An initial estimate x0 is required based on prior knowledge. An initial error covariance matrix P0 is dependent on the prior knowledge, and must be specified. Process and measurement noise covariance matrices QK and Rk must be also specified. As iteration proceeds, the derived partial derivatives with respect to each state variable are calculated to determine the linearization equations for the EKF.. 36.

(50) CHAPTER 4 ENVIRONMENT MAP REPRESENTATION. Environment maps, especially structured indoor one, are extensively used in mobile robots navigation for the tasks like map-based positioning or global path planning. Building an accurate environmental map for a mobile robot is important and essential. There are basically two types of maps suitable for mobile robot. The occupancy grids represent the environment as a two-dimensional array of cells, where each cell holds a value which indicates the probability of being occupied. Occupancy grid map is easy to build and useful for environment modeling even with imprecise range sensor like ultrasonic rangefinders. But, it is hard to be used directly due to its inaccuracy. The geometric primitive based map has been explored by many researchers [Gonzalea et al., 1994]. Line segment is the most widely used feature. Since not every object provides line segments, these maps cannot give a closed and connected region like the occupancy grid maps. A map represented by a set of geometric primitives is enough to describe most of structured indoor environments. It extracts geometric primitives from sampling points and represents them with detail geometric primitives. If line segments in environmental map are arranged in order, navigation algorithm based on map will be more efficient and more time-saving.. 37.

(51) 4.1 Extraction of Line Segments To represent environment with line segments, some linear fitting technologies are used to extract line segment from a set of sampling points that have been transformed into Cartesian coordinate system. To extract line segments from range points, we use recursive line splitting method [Matsuo et al., 1999; Xu et al., 2002], which is reliable and fast.. 4.1.1 Clustering All the points in each scan can be classified into different clustered regions by selecting threshold value Dthreshold 1 appropriately. Distance d between two successively points are calculated. Thus each region consists of a set of points from start point to end point. Because the density of sampling points is different at different position, adaptive clustering is adopted to more accurately classify the points into different regions, as shown in Figure 8.. Figure 8 Extracting start point and end point. 38.

(52) 4.1.2 Splitting Some clustering regions are linear and can be fitted directly with a line segment. Some clustering regions are nonlinear. It probably consists of several linear regions and must be split into sub-regions that are suitable for a good linear fitting. Each clustered region is tested whether it is linear or not. As to each clustered region, the orthogonal distance of every point to the line, which is generated by connecting the start and the end point of the sets of points that belong to the same region, is computed. The point with the maximum orthogonal distance towards the line is singled out, and the point is the corner point. If the maximum distance D is less than a prescribed threshold value Dthreshold 2 , this region or this set of points has good linear feature. If the maximum distance is larger than the threshold, this region or this set of points is split into two pieces, which are formed by connecting the endpoints of the previous region with this newly found point. The region, too, is split into two sub-regions at the point possessing the maximum orthogonal distance as shown in Figure 9 and Figure 10.. Figure 9 Split step of clustering region. 39.

(53) Figure 10 Result of clustering region. 4.2 Line Fitting Each linear region or sub-region consists of a set of sampling points. These points are not well aligned and scatter arbitrary. A line segment is used to approximate each linear region. Line segments are selected through best fitting of all points within the linear regions. This approach to be illustrated is the linear regression method (LRM) [Vandorpe et al., 1996]. Probably it is one of most common and well known approach for line extraction, which objective is to solve the minimum error along the vertical axis.. 4.2.1 Linear Regression The intermediate line parameters are (m, q) from the line equation y = mx + q (regression y to x) or (s, t) from the line equation x = sy + t (regression x to y). The choice between (m, q) and (s, t) parameters depends on the slope of the line in the global. 40.

(54) reference frame. If given n measurement points pw = ( xwi , ywi ) being fitting into the extracted line, the regression parameters S x , S y , S xx , S yy , and S xy are updated with these new points, and we defined the following regression parameters, n. n. n. i =1. i =1. 2 S x = ∑ xwi , S y = ∑ ywi , S xx = ∑ xwi n. n. i =1. i =1. i =1. S yy = ∑ ywi2 , S xy = ∑ xwi ⋅ ywi. (4.1). To distinguish between (m, q) or (s, t) parameters, T1 and T2 are defined as: T1 = S xx ⋅ n − S x2 , T2 = S yy ⋅ n − S y2. (4.2). where T1 and T2 represent the widths of the cloud of the regression points along the Xand Y-axis. If T1 is larger than T2, the cloud of points lies more horizontally than vertically which makes regression of y to x ( y = mx + q ) favourable. If T1 is smaller than T2, the regression of x to y ( x = sy + t ) is selected. With T = S xy ⋅ n − S x S y , the standard line parameters (m, q) or (s, t) are given by following equations: m= s=. ( S y − mS x ) T ,q= T1 n. (4.3). ( S x − sS y ) T ,t= T2 n. (4.4). 4.2.2 An Alternative Line Parameterization Line segments are represented by parameters ρ L and α L from following line equation, and Figure 11 shows the representation of the basic primitives. The equation of line is. 41.

(55) y sin α L + x cos α L = ρ L. (4.5). where α L is the angle between the x axis and the normal of the line, and ρ L ≧ 0 is the perpendicular distance from the line to the origin.. Figure 11 Parameterization of a line segment. Transform to. ( ρL , α L ). parameters to represent a line is necessary because they. are much more general. Another advantage of this representation is the fact that equation (4.5) is normalized. The following equations are used to calculate ρ and α from (m, q) or from (s, t):. ρL =. q m2 + 1. or ρ L =. t s2 + 1. ⎛. q ⎞ −1 ⎛ −t ⋅ s ⎞ ⎟ or α L = tan ⎜ ⎟ ⎝ t ⎠ ⎝ −q ⋅ m ⎠. α L = tan −1 ⎜. To obtain a line segment, the coordinates. (4.6) (4.7). ( xstart , ystart ) and ( xend , yend ) of the start and. end point have to be determined. The first point of the cluster region which was added to the regression line is projected on the line resulting in the start point. The projection. 42.

(56) of the last point leads to the end point.. 43.

(57) CHAPTER 5 SIMULATION RESULTS. A robot environment is constructed, consisting of a room of size 5m x 5m, a mobile robot is equipped with a CCD camera and a LRF. The LRF provides planar 180 degree scans with an angular step of 0.5 degree. Assumed that the orientation of LRF coordinate and the camera frame are coincided. Therefore, the direction of laser beam will be parallel to the direction of the optical axis with a linear offset in one axis. The first episode of simulation is to conduct pose initialization. Mobile robot is stationary while rotating its camera and LRF to obtain an imaged data sequence of Laser projected spots, emitted from LRF to the wall and then reflected to project onto the CCD’s image plane. In order to evaluate the performance of the proposed pose estimation system, the following EKF and UKF scenarios will be simulated in this chapter. Using the measurement and system equations, the state estimate and its estimation errors can be computed recursively by EKF and UKF techniques. The system parameters are chosen in Table 1. Table 1 The system parameters parameter ν ω. φ. value. comment. 5 cm / s 0.05 cm / s. The velocity of robot 2. 30 deg rees. 44. The angular velocity of robot LRF’s orientation with respect to robot.

(58) β. 45 deg rees. Tilt angle of the LRF. τ ZL. 1 sec. The sampling period. 15 cm. Elevation of the LRF with respect to the floor (world). f. 5 mm. Focal length. Wk. (0, Qk). Process noise. Vk. (0, Rk). Measurement noise. The parameters used in UKF algorithm are chosen as L=4, α=0.05, κ=0, and β =2. The sampling time interval is 1 sec. The measurement noise covariance matrix has been set to R= Diag{10-3 , 10-3 , 10-3 }, and the process noise covariance matrix has been set to Q= Diag{5x10-2 , 5x10-2 , 5x10-2 , 10-4 }. The choice of the Q matrix will affect the accuracy of UKF estimates. The true state of the system is [20 30 40 45] in x-axis, yaxis, z-axis, and orientation, respectively. The initial state is chosen the same as with the EKF’s. They are assumed to be [30 45 40 48]. The true and estimated of states in x-, y-, z-directions and theta using EKF are illustrated in Figure 12 and Figure 13. Figure 14 describes the true and estimated of image points u and v using EKF. Figure 15 shows the measurement errors and innovation sequence of the image points u and v using EKF. Figure 16 and Figure 17 show the state estimation errors of the system. The error covariances of each state are shown in Figure 18 and Figure 19, and all of the error covariances are converged. Figure 20 and Figure 21 describe the true and estimated of states x-, y-, z-direction and theta using UKF. The true and estimated of the image points u and v using UKF are illustrated in Figure 22. Figure 23 shows the measurement errors and innovation sequence of image points u and v using UKF. Figure 24 and Figure 25 show the state. 45.

(59) estimation errors of the system. Comparing the results in Figure 16 ~17 with the Figures 24 ~25, we know that the state estimation errors in the UKF algorithm is smaller than that in the EKF algorithm. Thus, the UKF method can provide more accurate state estimation than EKF method does. Figure 26 and Figure 27 show the error covariances of each state using UKF. As expected, UKF achieves fast convergence rate comparing to EKF’s, owning to its intrinsic property, i.e. no linearizations are required under any circumstance. These sigma points used in UKF are faithfully portraying the nonlinear transformations of true mean and covariance of the process. UKF captures the posterior mean and covariance up to third-order comparing to these EKF’s, achieving only first-order accuracy. Surprisingly, the computational load of UKF stays at the same order as EKF [Eric and van der Merwe, 2000]. Building a line segment map by using adaptive clustering threshold values, the resultant regions can obtain more accurately the real environment. It is more reasonable than clustering with constant threshold, and can avoid some clustering errors, i.e. points from different objects near the scanner are grouped into one while points from same object far from the corner are split into different regions. The threshold for line extraction is established at Dthreshold 1 = 30 cm and Dthreshold 2 = 50 cm . The Figure 28 ~34 illustrates the map building processes. Figure 28 shows the two lines used for simulation. Figure 29 and Figure 30 describe the scan taken at real line 1 and real line 2. All scanned measurement data of lines are shown in Figure 31. Figure 32 ~33 show the result of line segment 1 and line segment 2.. 46.

(60) The parameterization of line segment 1 are ρ L1 = 119.8264 cm and α L1 = 44.7984D ; the parameterization of line segment 2 are ρ L 2 = 12.0180 cm and α L 2 = −42.7858D . The actual and estimated line segments are described in Figure 34.. 47.

(61) Figure 12 The true and estimated of states in x- and y-direction using EKF. Figure 13 The true and estimated of state in z-direction and theta using EKF. 48.

(62) Figure 14 The true and estimated of the image points (u, v) using EKF. Figure 15 Measurement errors and innovation sequence of image points (u, v) using EKF. 49.

(63) Figure 16 The estimation errors of states in x- and y-direction using EKF. Figure 17 The estimation errors in z-direction and theta using EKF. 50.

(64) Figure 18 The error covariances of states in x- and y-direction using EKF. Figure 19 The error covariances in z-direction and theta using EKF. 51.

(65) Figure 20 The true and estimated of states in x- and y-direction using UKF. Figure 21 The true and estimated of state in z-direction and theta using UKF. 52.

(66) Figure 22 The true and estimated of the image points (u, v) using UKF. Figure 23 Measurement errors and innovation sequence of image points (u, v) using UKF. 53.

(67) Figure 24 The estimation errors of states in x- and y-direction using UKF. Figure 25 The estimation errors in z-direction and theta using UKF. 54.

(68) Figure 26 The error covariances of states in x- and y-direction using UKF. Figure 27 The error covariances in z-direction and theta using UKF. 55.

(69) Figure 28 The two lines used for simulation. Figure 29 Scans taken at real line 1. 56.

(70) Figure 30 Scans taken at real line 2. Figure 31 Scanned measurement data of lines. 57.

(71) Figure 32 Result of line segment 1. Figure 33 Result of line segment 2. 58.

(72) Figure 34 The actual and estimated line segments. 59.