Autonomous Mobile Robots - Shuzhi Sam Ge, Notas de estudo de Mecatrônica

Baixe Autonomous Mobile Robots - Shuzhi Sam Ge e outras Notas de estudo em PDF para Mecatrônica, somente na Docsity! LA Control Engineering Series Autonomous Mobile Robots Sensing, Control, Decision Making and Applications edited by Shuzhi Sam Ge Frank L. Lewis Autonomous Mobile Robots Sensing, Control, Decision Making and Applications DK6033_half-series-title.qxd 2/23/06 8:37 AM Page A © 2006 by Taylor & Francis Group, LLC Shuzhi Sam Ge The National University of Singapore Frank L. Lewis Automation and Robotics Research Institute The University of Texas at Arlington CRC is an imprint of the Taylor & Francis Group, an informa business Boca Raton London New York Autonomous Mobile Robots Sensing, Control, Decision Making and Applications DK6033_half-series-title.qxd 2/23/06 8:37 AM Page i © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c000” — 2006/3/31 — 16:42 — page x — #10 © 2006 by Taylor & Francis Group, LLC MATLAB® is a trademark of The MathWorks, Inc. and is used with permission. The MathWorks does not warrant the accuracy of the text or exercises in this book. This book’s use or discussion of MATLAB® software or related products does not constitute endorsement or sponsorship by The MathWorks of a particular pedagogical approach or particular use of the MATLAB® software. Published in 2006 by CRC Press Taylor & Francis Group 6000 Broken Sound Parkway NW, Suite 300 Boca Raton, FL 33487-2742 © 2006 by Taylor & Francis Group, LLC CRC Press is an imprint of Taylor & Francis Group No claim to original U.S. Government works Printed in the United States of America on acid-free paper 10 9 8 7 6 5 4 3 2 1 International Standard Book Number-10: 0-8493-3748-8 (Hardcover) International Standard Book Number-13: 978-0-8493-3748-2 (Hardcover) This book contains information obtained from authentic and highly regarded sources. Reprinted material is quoted with permission, and sources are indicated. A wide variety of references are listed. Reasonable efforts have been made to publish reliable data and information, but the author and the publisher cannot assume responsibility for the validity of all materials or for the consequences of their use. No part of this book may be reprinted, reproduced, transmitted, or utilized in any form by any electronic, mechanical, or other means, now known or hereafter invented, including photocopying, microfilming, and recording, or in any information storage or retrieval system, without written permission from the publishers. Danvers, MA 01923, 978-750-8400. CCC is a not-for-profit organization that provides licenses and registration for a variety of users. For organizations that have been granted a photocopy license by the CCC, a separate system of payment has been arranged. Trademark Notice: Product or corporate names may be trademarks or registered trademarks, and are used only for identification and explanation without intent to infringe. Library of Congress Cataloging-in-Publication Data Catalog record is available from the Library of Congress Visit the Taylor & Francis Web site at and the CRC Press Web site at Taylor & Francis Group is the Academic Division of Informa plc. For permission to photocopy or use material electronically from this work, please access www.copyright.com http://www.taylorandfrancis.com http://www.crcpress.com (http://www.copyright.com/) or contact the Copyright Clearance Center, Inc. (CCC) 222 Rosewood Drive, FRANKL: “dk6033_c000” — 2006/3/31 — 16:42 — page vii — #7 Preface The creation of a truly autonomous and intelligent system — one that can sense, learn from, and interact with its environment, one that can integrate seamlessly into the day-to-day lives of humans — has ever been the motivating factor behind the huge body of work on artificial intelligence, control theory and robotics, autonomous (land, sea, and air) vehicles, and numerous other discip- lines. The technology involved is highly complex and multidisciplinary, posing immense challenges for researchers at both the module and system integra- tion levels. Despite the innumerable hurdles, the research community has, as a whole, made great progress in recent years. This is evidenced by technological leaps and innovations in the areas of sensing and sensor fusion, modeling and control, map building and path planning, artificial intelligence and decision making, and system architecture design, spurred on by advances in related areas of communications, machine processing, networking, and information technology. Autonomous systems are gradually becoming a part of our way of life, whether we consciously perceive it or not. The increased use of intelligent robotic systems in current indoor and outdoor applications bears testimony to the efforts made by researchers on all fronts. Mobile systems have greater autonomy than before, and new applications abound — ranging from fact- ory transport systems, airport transport systems, road/vehicular systems, to military applications, automated patrol systems, homeland security surveil- lance, and rescue operations. While most conventional autonomous systems are self-contained in the sense that all their sensors, actuators, and computers are on board, it is envisioned that more and more will evolve to become open net- worked systems with distributed processing power, sensors (e.g., GPS, cameras, microphones, and landmarks), and actuators. It is generally agreed that an autonomous system consists primarily of the following four distinct yet interconnected modules: (i) Sensors and Sensor Fusion (ii) Modeling and Control (iii) Map Building and Path Planning (iv) Decision Making and Autonomy These modules are integrated and influenced by the system architecture design for different applications. vii © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c000” — 2006/3/31 — 16:42 — page xi — #11 Editors Shuzhi Sam Ge, IEEE Fellow, is a full professor with the Electrical and Computer Engineering Department at the National University of Singapore. He earned the B.Sc. degree from the Beijing University of Aeronautics and Astronautics (BUAA) in 1986, and the Ph.D. degree and the Diploma of Imperial College (DIC) from the Imperial College of Science, Technology and Medicine in 1993. His current research interests are in the control of nonlinear systems, hybrid systems, neural/fuzzy systems, robotics, sensor fusion, and real-time implementation. He has authored and co-authored over 200 interna- tional journal and conference papers, 3 monographs and co-invented 3 patents. He was the recipient of a number of prestigious research awards, and has been serving as the editor and associate editor of a number of flagship international journals. He is also serving as a technical consultant for the local industry. Frank L. Lewis, IEEE Fellow, PE Texas, is a distinguished scholar professor and Moncrief-O’Donnell chair at the University of Texas at Arlington. He earned the B.Sc. degree in physics and electrical engineering and the M.S.E.E. at Rice University, the M.S. in Aeronautical Engineering from the University of West Florida, and the Ph.D. at the Georgia Institute of Technology. He works in feedback control and intelligent systems. He is the author of 4 U.S. pat- ents, 160 journal papers, 240 conference papers, and 9 books. He received the Fulbright Research Award, the NSF Research Initiation Grant, and the ASEE Terman Award. He was selected as Engineer of the Year in 1994 by the Fort Worth IEEE Section and is listed in the Fort Worth Business Press Top 200 Leaders in Manufacturing. He was appointed to the NAE Committee on Space Station in 1995. He is an elected guest consulting professor at both Shanghai Jiao Tong University and South China University of Technology. xi © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c000” — 2006/3/31 — 16:42 — page xiii — #13 Contributors Martin Adams School of Electrical and Electronic Engineering Nanyang Technological University Singapore James S. Albus National Institute of Standards and Technology Gaithersburg, Maryland Alessandro Astolfi Electrical and Electronics Engineering Department Imperial College London London, UK Stephen Balakirsky Intelligent Systems Division National Institute of Standards and Technology Gaithersburg, Maryland Anthony Barbera National Institute of Standards and Technology Gaithersburg, Maryland José A. Castellanos Instituto de Investigación en Ingeniería de Aragón Universidad de Zaragoza Zaragoza, Spain Luiz Chaimowicz Computer Science Department Federal University of Minas Gerais, Brazil Jingrong Cheng Department of Electrical Engineering University of California Riverside, California Peng Cheng Department of Computer Science University of Illinois Urbana-Champaign, Illinois Sesh Commuri School of Electrical & Computer Engineering University of Oklahoma Norman, Oklahoma Jay A. Farrell Department of Electrical Engineering University of California Riverside, California Rafael Fierro MARHES Laboratory School of Electrical & Computer Engineering Oklahoma State University Norman, Oklahoma Shuzhi Sam Ge Department of Electrical and Computer Engineering National University of Singapore Singapore Héctor H. González-Baños Honda Research Institute USA, Inc. Mountain View, California xiii © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c000” — 2006/3/31 — 16:42 — page xiv — #14 xiv Contributors Fan Hong Department of Electrical and Computer Engineering National University of Singapore Singapore David Hsu Department of Computer Science National University of Singapore Singapore Huosheng Hu Department of Computer Science University of Essex Colchester, UK Chris Jones Computer Science Department University of Southern California Los Angeles, California Ebi Jose School of Electrical and Electronic Engineering Nanyang Technological University Singapore Vijay Kumar Department of Mechanical Engineering and Applied Mechanics University of Pennsylvania Philadelphia, Pennsylvania Jean-Claude Latombe Department of Computer Science Stanford University Palo Alto, California Steven M. LaValle Department of Computer Science University of Illinois Urbana-Champaign, Illinois Tong Heng Lee Department of Electrical and Computer Engineering National University of Singapore Singapore Frank L. Lewis Automation and Robotics Research Institute University of Texas Arlington, Texas Yu Lu Department of Electrical Engineering University of California Riverside, California Maja J. Matarić Computer Science Department University of Southern California Los Angeles, California Elena Messina Intelligent Systems Division National Institute of Standards and Technology Gaithersburg, Maryland Mario E. Munich Evolution Robotics Inc. Pasadena, California José Neira Instituto de Investigación en Ingeniería de Aragón Universidad de Zaragoza Zaragoza, Spain Jason M. O’Kane Department of Computer Science University of Illinois Urbana-Champaign, Illinois © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c000” — 2006/3/31 — 16:42 — page xix — #19 Contents I Sensors and Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 Chapter 1 Visual Guidance for Autonomous Vehicles: Capability and Challenges . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 Andrew Shacklock, Jian Xu, and Han Wang Chapter 2 Millimeter Wave RADAR Power-Range Spectra Interpretation for Multiple Feature Detection . . . . . . . . . . . . . . . . . 41 Martin Adams and Ebi Jose Chapter 3 Data Fusion via Kalman Filter: GPS and INS . . . . . . . . . . . . . . . . . 99 Jingrong Cheng, Yu Lu, Elmer R. Thomas, and Jay A. Farrell Chapter 4 Landmarks and Triangulation in Navigation . . . . . . . . . . . . . . . . . . 149 Huosheng Hu, Julian Ryde, and Jiali Shen II Modeling and Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 187 Chapter 5 Stabilization of Nonholonomic Systems . . . . . . . . . . . . . . . . . . . . . . . 191 Alessandro Astolfi Chapter 6 Adaptive Neural-Fuzzy Control of Nonholonomic Mobile Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 229 Fan Hong, Shuzhi Sam Ge, Frank L. Lewis, and Tong Heng Lee Chapter 7 Adaptive Control of Mobile Robots Including Actuator Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 267 Zhuping Wang, Chun-Yi Su, and Shuzhi Sam Ge xix © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c000” — 2006/3/31 — 16:42 — page xx — #20 xx Contents Chapter 8 Unified Control Design for Autonomous Car-Like Vehicle Tracking Maneuvers . . . . . . . . . . . . . . . . . . . . . . . . 295 Danwei Wang and Minhtuan Pham III Map Building and Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . 331 Chapter 9 Map Building and SLAM Algorithms . . . . . . . . . . . . . . . . . . . . . . . . . 335 José A. Castellanos, José Neira, and Juan D. Tardós Chapter 10 Motion Planning: Recent Developments. . . . . . . . . . . . . . . . . . . . . . . 373 Héctor H. González-Baños, David Hsu, and Jean-Claude Latombe Chapter 11 Multi-Robot Cooperation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 417 Rafael Fierro, Luiz Chaimowicz, and Vijay Kumar IV Decision Making and Autonomy . . . . . . . . . . . . . . . . . . . . . . . . . 461 Chapter 12 Knowledge Representation and Decision Making for Mobile Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 465 Elena Messina and Stephen Balakirsky Chapter 13 Algorithms for Planning under Uncertainty in Prediction and Sensing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 501 Jason M. O’Kane, Benjamín Tovar, Peng Cheng, and Steven M. LaValle Chapter 14 Behavior-Based Coordination in Multi-Robot Systems. . . . . . . 549 Chris Jones and Maja J. Matarić V System Integration and Applications . . . . . . . . . . . . . . . . . . . . . 571 Chapter 15 Integration for Complex Consumer Robotic Systems: Case Studies and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . 573 Mario E. Munich, James P. Ostrowski, and Paolo Pirjanian © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c000” — 2006/3/31 — 16:42 — page xxi — #21 Contents xxi Chapter 16 Automotive Systems/Robotic Vehicles . . . . . . . . . . . . . . . . . . . . . . . . 613 Michel R. Parent and Stéphane R. Petti Chapter 17 Intelligent Systems. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 655 Sesh Commuri, James S. Albus, and Anthony Barbera © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 3 — #3 Sensors and Sensor Fusion 3 robots possess. These sensors allow robots to obtain a basic set of observations upon which controllers and higher level decision-making mechanisms can act upon, thus forming an indispensable link in the chain of modules that together constitutes an intelligent, autonomous robotic system. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 5 — #5 1 Visual Guidance for Autonomous Vehicles: Capability and Challenges Andrew Shacklock, Jian Xu, and Han Wang CONTENTS 1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.1.1 Context. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.1.2 Classes of UGV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 1.2 Visual Sensing Technology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 1.2.1 Visual Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 1.2.1.1 Passive imaging . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 1.2.1.2 Active sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 1.2.2 Modeling of Image Formation and Calibration . . . . . . . . . . . . . . 12 1.2.2.1 The ideal pinhole model . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 1.2.2.2 Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 1.3 Visual Guidance Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 1.3.1 Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 1.3.2 World Model Representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 1.3.3 Physical Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 1.3.4 Road and Vehicle Following . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19 1.3.4.1 State-of-the-art . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19 1.3.4.2 A road camera model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21 1.3.5 Obstacle Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 1.3.5.1 Obstacle detection using range data . . . . . . . . . . . . . . . 23 1.3.5.2 Stereo vision . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 1.3.5.3 Application examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 1.3.6 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 1.4 Challenges and Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 1.4.1 Terrain Classification. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 5 © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 6 — #6 6 Autonomous Mobile Robots 1.4.2 Localization and 3D Model Building from Vision . . . . . . . . . . 34 1.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 Biographies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 1.1 INTRODUCTION 1.1.1 Context Current efforts in the research and development of visual guidance technology for autonomous vehicles fit into two major categories: unmanned ground vehicles (UGVs) and intelligent transport systems (ITSs). UGVs are primarily concerned with off-road navigation and terrain mapping whereas ITS (or auto- mated highway systems) research is a much broader area concerned with safer and more efficient transport in structured or urban settings. The focus of this chapter is on visual guidance and therefore will not dwell on the definitions of autonomous vehicles other than to examine how they set the following roles of vision systems: • Detection and following of a road • Detection of obstacles • Detection and tracking of other vehicles • Detection and identification of landmarks These four tasks are relevant to both UGV and ITS applications, although the environments are quite different. Our experience is in the development and testing of UGVs and so we concentrate on these specific problems in this chapter. We refer to achievements in structured settings, such as road-following, as the underlying principles are similar, and also because they are a good starting point when facing complexity of autonomy in open terrain. This introductory section continues with an examination of the expectations of UGVs as laid out by the Committee on Army Unmanned Ground Vehicle of the key technologies for visual guidance: two-dimensional (2D) passive ima- ging and active scanning. The aim is to highlight the differences between various the main content of this chapter; here we present a visual guidance system (VGS) and its modules for guidance and obstacle detection. Descriptions concentrate on pragmatic approaches adopted in light of the highly complex and uncer- tain tasks which stretch the physical limitations of sensory systems. Examples © 2006 by Taylor & Francis Group, LLC Technology in its 2002 road map [1]. Next, in Section 1.2, we give an overview options with regard to our task-specific requirements. Section 1.3 constitutes FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 9 — #9 Visual Guidance for Autonomous Vehicles 9 in military scenarios, or there may be too many conflicting sources in a civilian setting. At this point we also highlight a distinction between the terms “act- ive vision” and “active sensors.” Active vision refers to techniques in which (passive) cameras are moved so that they can fixate on particular features [4]. These have applications in robot localization, terrain mapping, and driving in cluttered environments. 1.2.1.1 Passive imaging From the application and performance standpoint, our primary concern is procuring hardware that will acquire good quality data for input to guidance algorithms; so we now highlight some important considerations when specifying a camera for passive imaging in outdoor environments. The image sensor (CCD or CMOS). CMOS technology offers certain advantages over the more familiar CCDs in that it allows direct access to indi- vidual blocks of pixels much as would be done in reading computer memory. This enables instantaneous viewing of regions of interest (ROI) without the integration time, clocking, and shift registers of standard CCD sensors. A key advantage of CMOS is that additional circuitry can be built into the silicon which leads to improved functionality and performance: direct digital out- put, reduced blooming, increased dynamic range, and so on. Dynamic range becomes important when viewing outdoor scenes with varying illumination: for example, mixed scenes of open ground and shadow. Color or monochrome. Monochrome (B&W) cameras are widely used in lane-following systems but color systems are often needed in off-road (or country track) environments where there is poor contrast in detecting travers- able terrain. Once we have captured a color image there are different methods of representing the RGB components: for example, the RGB values can be converted into hue, saturation, and intensity (HSI) [5]. The hue component of a surface is effectively invariant to illumination levels which can be important when segmenting images with areas of shadow [6,7]. circuit captured with an IR camera. The hot road surface is quite distinct as are metallic features such as manhole covers and lampposts. Trees similarly contrast well against the sky but in open country after rainfall, different types of vegetation and ground surfaces exhibit poor contrast. The camera works on a different transducer principle from the photosensors in CCD or CMOS chips. Radiation from hot bodies is projected onto elements in an array that heat up, and this temperature change is converted into an electrical signal. At present, compared to visible light cameras, the resolution is reduced (e.g., 320 × 240 pixels) and the response is naturally slower. There are other problems to contend with, such as calibration and drift of the sensor. IR cameras are expensive © 2006 by Taylor & Francis Group, LLC Infrared (IR). Figure 1.1 shows some views from our semi-urban scene test FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 10 — #10 10 Autonomous Mobile Robots FIGURE 1.1 A selection of images captured with an IR camera. The temperature of sur- faces gives an alternative and complementary method of scene classification compared to standard imaging. Note the severe lens distortion. and there are restrictions on their purchase. However, it is now possible to install commercial night-vision systems on road vehicles: General Motors offers a thermal imaging system with head-up display (HUD) as an option on the Cadillac DeVille. The obvious application for IR cameras is in night driving but they are useful in daylight too, as they offer an alternative (or complementary) way of segmenting scenes based on temperature levels. Catadioptric cameras. In recent years we have witnessed the increasing use of catadioptric1 cameras. These devices, also referred to as omnidirec- tional, are able to view a complete hemisphere with the use of a parabolic mirror [8]. Practically, they work well in structured environments due to the way straight lines project to circles. Bosse [9] uses them indoors and outdoors and tracks the location of vanishing points in a structure from motion (SFM) scheme. 1.2.1.2 Active sensors A brief glimpse through robotics conference proceedings is enough to demon- strate just how popular and useful laser scanning devices, such as the ubiquitous SICK, are in mobile robotics. These devices are known as LADAR and are 1 Combining reflection and refraction; that is, a mirror and lens. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 11 — #11 Visual Guidance for Autonomous Vehicles 11 available in 2D and 3D versions but the principles are essentially similar: a laser beam is scanned within a certain region; if it reflects back to the sensor off an obstacle, the time-of-flight (TOF) is measured. 2D scanning. The majority of devices used on mobile robots scan (pan) through 180◦ in about 13 msec at an angular resolution of 1◦. Higher resolution is obtained by slowing the scan, so at 0.25◦ resolution, the scan will take about 52 msec. The sensor thus measures both range and bearing {r, θ} of obstacles in the half plane in front of it. On a moving vehicle the device can be inclined at an angle to the direction of travel so that the plane sweeps out a volume as the vehicle moves. It is common to use two devices: one pointing ahead to detect obstacles at a distance (max. range∼80 m); and one inclined downward to gather 3D data from the road, kerb, and nearby obstacles. Such devices are popular because they work in most conditions and the information is easy to process. The data is relatively sparse over a wide area and so is suitable for in off-road applications, is caused by pitching of the vehicle on rough terrain: this creates spurious data points as the sensor plane intersects the ground plane. Outdoor feature extraction is still regarded as a very difficult task with 2D ladar as the scan data does not have sufficient resolution, field-of-view (FOV), and data rates [10]. 3D scanning. To measure 3D data, the beam must be steered though an additional axis (tilt) to capture spherical coordinates {r, θ ,φ: range, pan, tilt}. There are many variations on how this can be achieved as an opto- electromechanical system: rotating prisms, polygonal mirrors, or galvono- metric scanners are common. Another consideration is the order of scan; one option is to scan vertically and after each scan to increment the pan angle to the next vertical column. As commercial 3D systems are very expensive, many researchers augment commercial 2D devices with an extra axis, either by deflecting the beam with an external mirror or by rotating the complete sensor housing [11]. It is clear that whatever be the scanning method, it will take a protracted length of time to acquire a dense 3D point cloud. High-resolution scans used in construction and surveying can take between 20 and 90 min to complete a single frame, compared to the 10 Hz required for a real-time navigation system [12]. There is an inevitable compromise to be made between resolution and frame rate with scanning devices. The next generation of ladars will incorporate flash technology, in which a complete frame is acquired simultaneously on a focal plane array (FPA). This requires that individual sensing elements on the array incorporate timing circuitry. The current limitation of FLASH/FPA is the number of pixels in the array, which means that the FOV is still small, but this can be improved by panning and tilting of the sensor between subframes, and then creating a composite image. © 2006 by Taylor & Francis Group, LLC applications such as localization and mapping (Section 1.4.2). A complication, FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 14 — #14 14 Autonomous Mobile Robots 2. Nonlinear optimization techniques account for lens distortion in the camera model through iterative minimization of a determined function. The minimizing function is usually the distance between the image points and modeled projections. In guidance applications, it is common to adopt a two-step technique: use a linear optimization to compute some of the parameters and, as a second step, use nonlinear iteration to refine, and compute the rest. Since the result from the linear optimization is used for the nonlinear iteration, the iteration number is reduced and the convergence of the optimization is guaranteed [18–20]. Salvi [17] showed that two-step techniques yield the best result in terms of calibration accuracy. Calibration should not be a daunting prospect because many software tools are freely available [21,22]. Much of the literature originated from photo- grammetry where the requirements are much higher than those in autonomous navigation. It must be remembered that the effects of some parameters, such as image skew or the deviation of the principal point, are insignificant in com- parison to other uncertainties and image noise in field robotics applications. Generally speaking, lens distortion modeling using a radial model is sufficient to guarantee high accuracy, while more complicated models may not offer much improvement. A pragmatic approach is to carry out much of the calibration off-line in a controlled setting and to fix (or constrain) certain parameters. During use, only a limited set of the camera parameters need be adjusted in a calibration routine. Caution must be employed when calibrating systems in situ because the information from the calibration routine must be sufficient for the degrees of freedom of the model. If not, some parameters will be confounded or wander in response to noise and, later, will give unpredictable results. A common problem encountered in field applications is attempting a complete calibration off essen- tially planar data without sufficient and general motion of the camera between images. An in situ calibration adjustment was adopted for the calibration of the severe but were suitably approximated and corrected by a two-coefficient radial distortion model, in which the coefficients (k1, k2) were measured off-line. The skew was set to zero; the principal point and aspect ratio were fixed in the calibration matrix. The focal length varied with focus adjustment but a default value (focused at infinity) was measured. Of the extrinsic parameters, only the tilt of the camera was an unknown in its application: the other five were set by the rigid mounting fixtures. Once mounted on the vehicle, the tilt was estimated from the image of the horizon. This gave an estimate of the camera calibration which was then improved given extra data. For example, four known points are sufficient to calculate the homographic mapping from ground plane to the image. However, a customized calibration routine was used that enforced the © 2006 by Taylor & Francis Group, LLC IR camera used to take the images of Figure 1.1. The lens distortion effects were FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 15 — #15 Visual Guidance for Autonomous Vehicles 15 constraints and the physical degrees of freedom of the camera, yet was stable enough to work from data on the ground plane alone. As a final note on calib- ration: any routine should also provide quantified estimates of the uncertainty of the parameters determined. 1.3 VISUAL GUIDANCE SYSTEMS 1.3.1 Architecture The modules of a working visual guidance system (VGS) are presented in delving into task-specific processes, we need to clarify the role of VGS within the autonomous vehicle system architecture. Essentially, its role is to capture raw sensory data and convert it into model representations of the environment and the vehicle’s state relative to it. 1.3.2 World Model Representation A world model is a hierarchical representation that combines a variety of sensed inputs and a priori information [23]. The resolution and scope at each level are designed to minimize computational resource requirements and to support plan- ning functions for that level of the control hierarchy. The sensory processing system that populates the world model fuses inputs from multiple sensors and extracts feature information, such as terrain elevation, cover, road edges, and obstacles. Feature information from digital maps, such as road networks, elev- ation, and hydrology, can also be incorporated into this rich world model. The various features are maintained in different layers that are registered together to provide maximum flexibility in generation of vehicle plans depending on mis- sion requirements. The world model includes occupancy grids and symbolic object representations at each level of the hierarchy. Information at different hierarchical levels has different spatial and temporal resolution. The details of a world model are as follows: Low resolution obstacle map and elevation map. The obstacle map consists of a 2D array of cells [24]. Each cell of the map represents one of the follow- ing situations: traversable, obstacle (positive and negative), undefined (such as blind spots), potential hazard, and so forth. In addition, high-level terrain classi- fication results can also be incorporated in the map (long grass or small bushes, steps, and slopes). The elevation contains averaged terrain heights. Mid-resolution terrain feature map. The features used are of two types, smooth regions and sharp discontinuities [25]. A priori information. This includes multiple resolution satellite maps and other known information about the terrain. © 2006 by Taylor & Francis Group, LLC Figure 1.2. So far, we have described the key sensors and sensor models. Before FR A N K L : “dk6033_c001” — 2006/3/31 — 16:42 — page 16 — #16 16 A u to n o m o u s M o b ile R o b o ts Color camera Stereo camera Laser range finder Road model Lane Model Vehicle model Terrain model Color calibration Stereo calibration Vehicle to world coordinates Color segmentation Landmark detection Target tracking Terrain classification Obstacle detection 3D target tracking Terrain analysis Obstacle map fusion Terrain cluster fusion Road map and obstacle map fusion Obstacle map Elevation map Road map Lead vehicle orientation and speed Feature map Sensor Modeling and calibration Task-specific processing Sensor fusion World mapping FIGURE 1.2 Architecture of the VGS. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 19 — #19 Visual Guidance for Autonomous Vehicles 19 TABLE 1.3 Influence of Camera Height on Visibility of Negative Obstacles Visibility of negative obstacle (pixels) trench width w = 1 m Distance, d (m) Camera height h = 2.5 m Camera height h = 4 m 8 21 (0.31 m) 35 (0.5 m) 15 6.8 (0.17 m) 11 (0.27 m) 25 2.5 (0.1 m) 4 (0.16 m) cover a width of 13 m at distance 25 m and possibly detect a ditch {w = 1 m, h = 4 m} by viewing 8 pixels of the ditch. There are several options for improving the chances of detecting an obstacle: Raising the camera. This is not always an option for practical and oper- ational reasons; for example, it makes the vehicle easier to detect by the enemy. Increasing focal length. This has a direct effect but is offset by prob- lems with exaggerated image motion and blurring. This becomes an important consideration when moving over a rough terrain. Increased resolution. Higher-resolution sensors are available but they will not help if a sharp image cannot be formed by the optics, or if there is image blur. The trade-off between resolution and FOV is avoided (at extra cost and fields-of-view and ranges of the sensors on the VGS. Dickmanns [26,27], uses a mixed focal system comprising two wide-angle cameras with divergent axes, giving a wide FOV (100◦). A high-resolution three-chip color camera with greater focal length is placed between the other cameras for detecting objects at distance. The overlapping region of the cameras’ views give a region of trinocular stereo. 1.3.4 Road and Vehicle Following 1.3.4.1 State-of-the-art Extensive work has been carried out on road following systems in the late 1980s and throughout the 1990s; for example, within the PROMETHEUS Programme which ran from 1987 until 1994. Dickmanns [28] provides a comprehensive review of the development of machine vision for road vehicles. One of the key tasks is lane detection, in which road markings are used to monitor the position © 2006 by Taylor & Francis Group, LLC complexity) by having multiple sensors. Figure 1.4 illustrates the different FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 20 — #20 20 Autonomous Mobile Robots 2D ladar mm Radar 3D ladar Stereo imaging 20 m 80 m 200 m FIGURE 1.4 Different subsystems of the VGS provide coverage over different field- of-view and range. There is a compromise between FOV and angular resolution. The rectangle extending to 20 m is the occupancy grid on which several sensory outputs are fused. of the vehicle relative to the road: either for driver assistance/warning or for autonomous lateral control. Lane detection is therefore a relatively mature tech- nology; a number of impressive demonstrations have taken place [29], and some systems have achieved commercial realization such as Autovue and AssistWare. There are, therefore, numerous sources of reference where the reader can find details on image processing algorithms and details of practical implementation. Good places to start are at the PATH project archives at UCLA, the final report of Chauffeur II programme [30], or the work of Broggi on the Argo project [29]. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 21 — #21 Visual Guidance for Autonomous Vehicles 21 The Chauffeur II demonstration features large trucks driving in convoy on a highway. The lead vehicle is driven manually and other trucks equipped with the system can join the convoy and enter an automatic mode. The system incorporates lane tracking (lateral control) and maintaining a safe distance to the vehicle in front (longitudinal control). This is known as a “virtual tow-bar” or “platooning.” The Chauffeur II demonstration is highly structured in the sense that it was implemented on specific truck models and featured inter-vehicle communication. Active IR patterns are placed on the rear of the vehicles to aid detection, and radar is also used. The PATH demonstration (UCLA, USA) used stereo vision and ladar. The vision system tracks features on a car in front and estimates the range of an arbitrary car from passive stereo disparity. The ladar system provides assistance by guiding the search space for the vehicle in front and increasing overall robustness of the vision system. This is a difficult stereo problem because the disparity of features on the rear of car is small when viewed from a safe driving separation. Recently much of the research work in this area has concentrated on the problems of driving in urban or cluttered environments. Here, there are the complex problems of dealing with road junctions, traffic signs, and pedestrians. 1.3.4.2 A road camera model Road- and lane-following algorithms depend on road models [29]. These mod- els have to make assumptions such as: the surface is flat; road edges or markings are parallel; and the like. We will examine the camera road geometry because, with caution, it can be adapted and applied to less-structured problems. For simplicity and without loss of generality, we assume that the road lies in the plane Z = 0. From Equation 1.1, setting all Z coordinates of X to zero is equi- valent to striking out the third column of the projection matrix P in Equation 1.2. There is a homographic correspondence between the points of the road plane and the points of the image plane which can be represented by a 3× 3 matrix transformation. This homography is part of the general linear group GL3 and as such inherits many useful properties of this group. The projection Equation 1.1 becomes x = HX: H ∈ R3×3 (1.7) As a member of the group, a transformation H must2 have an inverse, so there will also be one-to-one mapping of image points (lines) to points (lines) on the road plane. The elements of H are easily determined (calib- ration) by finding at least four point correspondences in general position on 2 The exception to this is when the road plane passes through the camera center, in which case H is singular and noninvertible (but in this case the road would project to a single image line and the viewpoint would not be of much use). © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 24 — #24 24 Autonomous Mobile Robots definition of obstacle is any object that can obstruct the vehicle’s driving path or, in other words, anything rising out significantly from the road surface. Many approaches for extracting obstacles from range images have been proposed. Most approaches use either a global or a local reference plane to detect positive (above the reference plane) or negative (below the reference plane) obstacles. It is also possible to use salient points detected by an elevation differential method to identify obstacle regions [31]. The fastest of obstacle detection algorithms, range differencing, simply subtract the range image of an actual scene from the expected range image of a horizontal plane (global reference plane). While rapid, this technique is not very robust, since mild slopes will result in false indications of obstacles. So far the most frequently used and most reliable solutions are based on comparison of 3D data with local reference planes. Thorpe et al. [22] analyzed scanning laser range data and constructed a surface property map represented in a Cartesian coordinate system viewed from above, which yielded the surface type of each point and its geometric parameters for segmentation of the scene map into traversable and obstacle regions. The procedure includes the following. Preprocessing. The input from a 2D ladar may contain unreliable range data resulting from surfaces such as water or glossy pigment, as well as the mixed points at the edge of an object. Filtering is needed to remove these undesirable jumps in range. After that, the range data are transformed from angular to Cartesian (x-y-z) coordinates. Feature extraction and clustering. Surface normals are calculated from x-y-z points. Normals are clustered into patches with similar normal orientations. Region growth is used to expand the patches until the fitting error is larger than a given threshold. The smoothness of a patch is evaluated by fitting a surface (plane or quadric). Defect detection. Flat, traversable surfaces will have vertical surface nor- mals. Obstacles will have surface patches with normals pointed in other directions. Defect analysis. A simple obstacle map is not sufficient for obstacle ana- lysis. For greater accuracy, a sequence of images corresponding to overlapping terrain is combined in an extended obstacle map. The analysis software can also incorporate color or curvature information into the obstacle map. Extended obstacle map output. The obstacle map with a header (indic- ating map size, resolution, etc.) and a square, 2D array of cells (indicating traversability) are generated for the planner. 1.3.5.2 Stereo vision Humans exploit various physiological and psychological depth cues. Stereo cameras are built to mimic one of the ways in which the human visual system © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 25 — #25 Visual Guidance for Autonomous Vehicles 25 X O2e1 e2 x1 x2 l1 l2 OR OL xL xR X = [x y z]T B f Z e1 e2 FIGURE 1.7 Epipolar geometry is valid for general positions of two views. The figure on the left illustrates the epipolar lines for two frames (1 and 2). However, if the optical axes are parallel and the camera parameters are similar, stereo matching or the search for corresponding features is much easier. The figure on the right illustrates the horizontal and collinear epipolar lines in a left–right configuration with fixed baseline B. (HVS) works to obtain depth information [32]. In a standard configuration, two cameras are bound together with a certain displacement (Figure 1.7). This distance between the two camera centers is called the baseline B. In stereo vision, the disparity measurement is the difference in the positions of two cor- responding points in the left and right images. In the standard configuration, the two camera coordinate systems are related simply by the lateral displacement B (XR = XL + B). As the cameras are usually “identical” (fL = fR = f ) and aligned such that (ZL = ZR = Z) the epipolar geometry and projection equation (x = f X/Z) enable depth Z to be related to disparity d: d = xR − xL = f XL + B Z − f XL Z = f B Z (1.8) where f is the focal length of the cameras. Since B and F are constants, the depth z can be calculated when d is known from stereo matching (Z = fB/d). 1.3.5.2.1 Rectification As shown in Figure 1.7, for a pair of images, each point in the “left” image is restricted to lie on a given line in the “right” image, the epipolar line — and vice versa. This is called the epipolar constraint. In standard configurations the epipolar lines are parallel to image scan lines, and this is exploited in many algorithms for stereo analysis. If valid, it enables the search for corresponding image features to be confined to one dimension and, hence, simplified. Stereo rectification is a process that transforms the epipolar lines so that they are collinear, and both parallel to the scan line. The idea behind rectification [33] is to define two new perspective matrices which preserve the optical centers but with image planes parallel to the baseline. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 26 — #26 26 Autonomous Mobile Robots 1.3.5.2.2 Multi-baseline stereo vision Two main challenges facing a stereo vision system are: mismatch (e.g., points in the left image match the wrong points in the right image) and disparity accuracy. To address these issues, multiple (more than two) cameras can be used. Nakamura et al. [34] used an array of cameras to resolve occlusion by introducing occlusion masks which represent occlusion patterns in a real scene. Zitnick and Webb [35] introduced a system of four cameras that are horizontally displaced and analyze potential 3D surfaces to resolve the feature matching problem. When more than two cameras or camera locations are employed, multiple stereo pairs (e.g., cameras 1 and 2, cameras 1 and 3, cameras 2 and 3, etc.) result in multiple, usually different baselines. In the parallel configuration, each camera is a lateral displacement of the other. For a given depth, we then calculate the respective expected disparities relative to a reference camera (say, the leftmost camera) as well as the sum of match errors over all the cameras. The depth associated with a given pixel in the reference camera is taken to be the one with the lowest error. The multi-baseline approach has two distinctive advantages over the classical stereo vision [36]: • It can find a unique match even for a repeated pattern such as the cosine function. • It produces a statistically more accurate depth value. 1.3.5.2.3 General multiple views During the 1990s significant research was carried out on multiple view geo- metry and demonstrating that 3D reconstruction is possible using uncalibrated cameras in general positions [14]. In visual guidance, we usually have the advantage of having calibrated cameras mounted in rigid fixtures so there seems little justification for not exploiting the simplicity and speed of the algorithms described earlier. However, the fact that we can still implement 3D vision even if calibration drifts or fixtures are damaged, adds robustness to the system concept. Another advantage of more general algorithms is that they facilitate mixing visual data from quite different camera types or from images taken from arbitrary sequences in time. 1.3.5.3 Application examples In this section we present some experimental results of real-time stereo-vision- based obstacle detection for unstructured terrain. Two multi-baseline stereo vision systems (Digiclops from Pointgrey Research, 6 mm lens) were mounted at a height of 2.3 m in front and on top of the vehicle, spaced 20 cm apart. The two stereo systems were calibrated so that their outputs were referred to © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 29 — #29 Visual Guidance for Autonomous Vehicles 29 When sensor outputs are read asynchronously, certain assumptions such as being Linear Time Invariant (LTI) [38] can be made to propagate asynchron- ized data to the upcoming sample time of the control system. Robl [38] showed examples of using first-order hold and third-order hold methods to predict sensor values at desired times. When different resolution sensors are to be fused at the data level (e.g., fusion of range images from ladar and stereo vision), down-sampling of sensor data with higher spatial resolution by interpolation is performed. For sensor fusion at the obstacle map level, spatial synchron- ization is not necessary since a unique map representation is defined for all sensors. Example: Fusion of laser and stereo obstacle maps for false alarm suppression Theoretically, pixel to pixel direct map fusion is possible if the calibra- tion and synchronization of the geometrical constraints (e.g., rotation and translation between laser and stereo system) remain unchanged after calib- ration. Practically, however, this is not realistic, partially due to the fact that sensor synchronization is not guaranteed at all times: CPU loading, terrain differences, and network traffic for the map output all affect the synchroniza- tion. Feature-based co-registration sensor fusion, alternatively, addresses this issue by computing the best-fit pose of the obstacle map features relative to multiple sensors which allows refinement of sensor-to-sensor registration. In the following, we propose a localized correlation based approach for obstacle- map-level sensor fusion. Assuming the laser map Lij and stereo map Sij is to be merged to form Fij. A map element takes the value 0 for a traversable pixel, 1 for an obstacle, and anything between 0 and 1 for the certainty of the pixel to be classified as an obstacle. We formulate the correlation-based sensor fusion as Fij =   Lij Sij = undefined Sij Lij = undefined (a1Lij + a2Si+m,j+n)/(a1 + a2) max(Corr(LijSi+m,j+n)) m, n ∈ undefined Sij, Lij = undefined (1.9) where represents a search area and {a1, a2} are weighting factors. Corr(L, S) is the correlation between L and S elements with window size wc: Corr(LijSi+m,j+n) = wc/2∑ p=−wc/2 wc/2∑ q=−wc/2 Li+p,j+qSi+m+p,j+n+q (1.10) © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 30 — #30 30 Autonomous Mobile Robots The principle behind the localized correlation sensor fusion is: instead of directly averaging Lij and Sij to get Fij, a search is performed to find the best match within a small neighborhood. The averaging of the center pixel at a matched point produces the final fusion map. In case an obstacle map only takes three values: obstacle, traversable, and undefined; the approach above can be simplified as Fij =   Lij Sij = undefined Sij Lij = undefined 1 Lij = 1, Cso > T1, D < T2 1 Sij = 1, Clo > T1, D < T2 0 otherwise (1.11) where T1 and T2 are preset thresholds that depend on the size of the search window. In our experiments a window of size 5 × 5 pixels was found to work well. The choice of size is a compromise between noise problems with small windows and excessive boundary points with large windows. Cso and Clo are obstacle pixel counts within the comparison window wc, for Lij and Sij, respectively, D is the minimum distance between Lij and Sij in : D = min   wc/2∑ p=−wc/2 wc/2∑ q=−wc/2 |Si+m+p,j+n+q − Li+p,j+q|   (m, n) ∈ (1.12) Cso = wc/2∑ p=−wc/2 wc/2∑ q=−wc/2 Si+m+p,j+n+q (1.13) The advantage of implementing correlation-based fusion method is two- fold: it reduces false alarm rates and compensates for the inaccuracy from laser and stereo calibration/synchronization. The experimental results of using above mentioned approach for laser and stereo obstacle map fusion are shown The geometry of 2D range and image data fusion. Integration of sensory data offers much more than a projection onto an occupancy grid. There exist multiple view constraints between image and range data analogous to those between multiple images. These constraints help to verify and disambiguate data from either source, so it is useful to examine the coordinate transformations and the physical parameters that define them. This will also provide a robust framework for selecting what data should be fused and in which geometric representation. © 2006 by Taylor & Francis Group, LLC in Figure 1.9. FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 31 — #31 Visual Guidance for Autonomous Vehicles 31 FIGURE 1.9 Sensor fusion of laser and stereo obstacle maps. False alarm in laser obstacle map (left image, three laser scanning lines at the top of the map), is suppressed by fusion with the stereo vision obstacle map (middle image), and a more reliable fusion result is generated (right image). First, consider the relationship between a data point from the ladar and a world coordinate system. We can transform {r, θ} to a point X in a Cartesian space. A 3D point X will be detected by an ideal ladar if it lies in the plane Z=0 expressed in the sensor’s coordinate system. (This is neglecting the range limits, and the finite size and divergence of the laser beam). If the plane, in the world coordinate system, is denoted asL , the set of points that can be detected satisfy TLX = 0 (1.14) Alternatively we expand the rigid transformation equation and express this as a constraint (in sensor coordinates) XL = GWL X GWL = ( RWL T 0 1 ) (1.15) Only the third row of G [r3i TZ ] plays any part in the planar constraint on the point {X = [X Y Z 1]T}. The roles of the parameters are then explicit: r31X + r32Y + r33Z + TZ = 0 (1.16) However, if the vehicle is moving over tough terrain there will be considerable uncertainty in the instantaneous parameters of R and T . We therefore look at a transformation between ladar data and image data without reference to any world coordinate system. Assuming there are no occlusions, X will be imaged as x on the image planeI of the camera. As X lies in a planeL , there exists © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 34 — #34 34 Autonomous Mobile Robots be classified as “dangerous” or “not dangerous.” Color cameras can be used to perform terrain classification. Color segmentation relies on having a complete training set. As lighting changes, due to time of day or weather conditions, the appearance of grass and obstacle change as well. Although color normalization methods have been successfully applied to the indoor environment, they, to our knowledge, fail to produce reasonable results in an outdoor environment. Similarly, color segmentation can classify flat objects, such as fallen leaves, as obstacles, since their color is different from grass. If dense range measurements in a scene are available (e.g., using ladar), they can be used, not only to represent the scene geometry, but also to characterize surface types. For example, the range measured on bare soil or rocks tends to lie on a relatively smooth surface; in contrast, in the case of bushes, the range is spatially scattered. While it is possible — although by no means trivial — to design algorithms for terrain classification based on the local statistics of range data [39–41], the confidence level of a reliable classification is low. Table 1.4 lists the most frequently encountered terrain types and possible classification methods. 1.4.2 Localization and 3D Model Building from Vision Structure from motion (SFM) is the recovery of camera motion and scene structures — and in certain cases camera intrinsic parameters — from image TABLE 1.4 Terrain Types and Methods of Classification Confidence Terrain type Sensors Classification methods level Vegetable IR/Color camera Segmentation Medium Rocks IR/Color camera Segmentation Medium Walls/fence Camera, stereo, laser Texture analysis, obstacle detection High Road (paved, gravel, dirt) IR/Color camera Segmentation Medium Slope Stereo, ladar Elevation analysis, surface fit High Ditch, hole Stereo, ladar Low Sand, dirt, mud, gravel IR/Color camera Segmentation Medium Water Polarized camera, laser scanner Feature detection, sensor fusion Medium Moving target Camera, stereo Optical flow, obstacle detection, pattern matching High © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 35 — #35 Visual Guidance for Autonomous Vehicles 35 sequences. It is attractive because it avoids the requirement for a priori models of the environment. The techniques are based on the constraints that exist between the multiple views of features. This is a mature area of computer vision that has attracted intensive research activity in the previous decade, prompted by the breakthroughs in multiple view geometry in the early 1990s. Much of the original work was motivated by mobile robotics but soon found more general application such as: the generation of special effects for cinema, scene recovery for virtual reality, and 3D reconstruction for architecture. Here, the theoretical drive has been inspired by the recovery of information from recorded sequences such as camcorders where the motion is general and little can be assumed regarding the camera parameters. These tasks can be accomplished off-line and the features and camera parameters from long sequences solved as a large-scale optimization in batch mode. As such, many would regard this type of SFM as a solved problem but the conditions in vehicle navigation are specific and require separate consideration: • The motion is not “general,” it may be confined to a plane, or restricted to rotations around axes normal to the plane. • Navigation is required in real-time and parameters require continuous updating from video streams as opposed to the batch operations of most SFM algorithms. • Sensory data, from sources other than the camera(s), are usually available. • Many of the camera parameters are known (approximately) beforehand. • There are often multiple moving objects in a scene. Visual guidance demands a real-time recursive SFM algorithm. Chiuso et al. [42] have impressive demonstrations of a recursive filter SFM system that works at a video frame rate of 30 Hz. However, once we start using Kalman filters to update estimates of vehicle (camera) state and feature location, some would argue that we enter the already very active realm of simultaneous local- ization and mapping (SLAM). The truth is that there are differences between SLAM and SFM and both have roles in visual guidance. Davison [43] has been very successful in using vision in a SLAM framework and Bosse [9] has published some promising work in indoor and outdoor navigation. The key to both of these is that they tackle a fundamental problem of using vision in SLAM: the relatively narrow FOV and recognizing features when revisiting a location. Davison used active vision in Reference 4 and wide-angle lenses in Reference 43 to fixate on a sparse set of dominant features whereas Bosse used a catadioptric camera and exploited vanishing points. SLAM often works well with 2D ladar by collecting and maintaining estimates of a sparse set of features with reference to world coordinate system. A problem with SFM occurs when © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 36 — #36 36 Autonomous Mobile Robots features used for reference pass out of the FOV: in recursive mode, there is no guarantee at initiation that features will persist. Errors (drift) are introduced when the reference features are changed and the consequence is that a robot will have difficulty in returning home or knowing that it is revisiting a location. Chiuso has a scheme to reduce this problem but drift is still inevitable. On the other hand, SLAM has to rely on sparse data because it needs to maintain a full covariance matrix which will soon become computationally expensive if the number of data points is not restricted. It can be difficult to associate outdoor data when it is sparse. The two techniques offer different benefits and a possible complementary role. SLAM is able to maintain a sparse map on a large scale for navigation but locally does not help much with terrain classification. SFM is useful for building a dense model of the immediate surroundings, useful for obstacle avoidance, path planning, and situation awareness. The availability of a 3D model (with texture and color) created by SFM will be beneficial for validation of the sens- ory data used in a SLAM framework: for example, associating an object type with range data; providing color (hue) as an additional state; and so on. 1.5 CONCLUSION We have presented the essentials of a practical VGS and provided details on its sensors and capabilities such as road following, obstacle detection, and sensor fusion. Worldwide, there have been many impressive demonstrations of visual guidance and certain technologies are so mature that they are available commercially. This chapter started with a road map for UGVs and we have shown that the research community is still struggling to achieve A-to-B mobility in tasks within large-scale environments. This is because navigating through open terrain is a highly complex problem with many unknowns. Information from the immediate surroundings is required to determine traversable surfaces among the many potential hazards. Vision has a role in the creation of terrain maps but we have shown that practically this is still difficult due to the physical limitations of available sensor technology. We anticipate technological advances that will enable the acquisition of high-resolution 3D data at fast frame rates. Acquiring large amounts of data is not a complete solution. We argue that we do not make proper use of the information already available in 2D images, and that there is potential for exploiting algorithms such as SFM and vision- based SLAM. Another problem is finding alternative ways of representing the environment that are more natural for navigation; or how to extract knowledge from images and use this (state) information within algorithms. We have made efforts to highlight problems and limitations. The task is complex and practical understanding is essential. The only way to make real © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 39 — #39 Visual Guidance for Autonomous Vehicles 39 29. A. Broggi, M. Bertozzi, A. Fascioli, and G. Conte. Automatic Vehicle Guidance: The Experience of the ARGO Autonomous Vehicle. World Scientific, 1999. 30. Chauffeur II Final Report. Technical Report IST-1999-10048 D24, The Promote-Chauffeur II Consortium, July 2003. 31. T. Chang, H. Tsai, S. Legowik, and M. N. Abrams. Concealment and obstacle detection for autonomous driving. In M. H. Hamza (ed.), Proceedings of IASTED Conference, Robotics and Applications 99, pp. 147–152. ACTA Press, Santa Barbara, CA, 28–30 October 1999. 32. T. Okoshi. Three-Dimensional Imaging Techniques. Academic Press, New York, 1976. 33. N. Ayache. Artificial Vision for Mobile Robots: Stereo Vision and Multisensory Perception. The MIT Press, Cambridge, MA, 1991. 34. Y. Nakamura, K. Satoh, T. Matsuura, and Y. Ohta. Occlusion detectable stereo- occlusion patterns in camera matrix. In Proceedings of IEEE International Conference on Computer Vision and Pattern Recognition, San Francisco, CA, pp. 371–378, 18–20 June 1996. 35. C. L. Zitnick and J. A. Webb. Multi-baseline stereo using surface extraction. Technical Report, School of Computer Science, Carnegie Melllon University, November 1996. CMU-CS-96-196. 36. M. Okutomi and T. Kanade. A multiple-baseline stereo. IEEE Transactions on Pattern Analysis and Machine Intelligence, 15: 353–363, 1993. 37. J. Xu, H. Wang, J. Ibanez-Guzman, T. C. Ng, J. Shen, and C. W. Chan. Isodispar- ity profile processing for real-time 3D obstacle identification. In Proceedings of IEEE International Conference on Intelligent Transportation Systems (ITS) Shanghai, China, pp. 288–292, October 2003. 38. C. Robl and G. Faerber. System architecture for synchronizing, signal level fusing, simulating and implementing sensors. In Proceedings of the 2000 IEEE International Conference on Robotics and Automation, San Francisco, CA, pp. 1639–1644, April 2000. 39. J. Huang, A. B. Lee, and D. Mumford. Statistics of range images. In Proceed- ings of IEEE Conference on Computer Vision and Pattern Recognition, vol. 1, pp. 324–331, 2000. 40. M. Hebert, N. Vandapel, S. Keller, and R. R. Donamukkala. Evaluation and comparison of terrain classification techniques from ladar data for autonom- ous navigation. In Twentythird Army Science Conference, Orlando, FL, USA, December 2002. 41. N. Vandapel, D. F. Huber, A. Kapuria, and M. Hebert. Natural terrain classi- fication using 3D ladar data. In Proceedings of IEEE International Conference on Robotics and Automation, New Orleans, LA, USA, Vol. 5, pp. 5117–5122, 26 April–1 May 2004. 42. A. Chiuso, P. Favaro, H. L. Jin, and S. Soatto. Structure from motion caus- ally integrated over time. IEEE Transactions on Pattern Analysis and Machine Intelligence, 24: 523–535, 2002. 43. A. J. Davison, Y. González Cid, and N. Kita. Real-time 3D SLAM with wide-angle vision. In 5th IFAC/EURON Symposium on Intelligent Autonomous Vehicles, Lisboa, Portugal. IFAC, Elsevier Science, 5–7 July 2004. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c001” — 2006/3/31 — 16:42 — page 40 — #40 40 Autonomous Mobile Robots BIOGRAPHIES Andrew Shacklock has 20 years of experience in mechatronics and sensor guided robotic systems. He graduated with a B.Sc. from the University of Newcastle Upon Tyne in 1985 and a Ph.D. from the University of Bristol in 1994. He is now a research scientist at the Singapore Institute of Manufacturing Technology. His main research interest is in machine perception and sensor fusion, in particular for visual navigation. Jian Xu received the bachelor of engineering degree and master engineering degree in electrical engineering from Shanghai Jiao Tong University, China in 1982 and 1984, respectively. He received his Doctor of Engineering from Erlangen-Nuremberg University, Germany in 1992. He is currently a research scientist at the Singapore Institute of Manufacturing Technology, Singapore. His research interests include 3D machine vision using photogrammetry and stereo vision, camera calibration, sensor fusion, subpixeling image processing, and visual guidance system for autonomous vehicle. Han Wang is currently an associate professor at Nanyang Technological University, and senior member of IEEE. His research interests include computer vision and AGV navigation. He received his bachelor of engineering degree from Northeast Heavy Machinery Institute in 1982 and Ph.D. from Leeds University in 1989. He has been a research scientist at CMU and research officer at Oxford University. He spent his sabbatical in 1999 in Melbourne University. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 41 — #1 2 Millimeter Wave RADAR Power-Range Spectra Interpretation for Multiple Feature Detection Martin Adams and Ebi Jose CONTENTS 2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 2.2 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44 2.3 FMCW RADAR Operation and Range Noise . . . . . . . . . . . . . . . . . . . . . . . . 45 2.3.1 Noise in FMCW Receivers and Its Effect on Range Detection. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47 2.4 RADAR Range Spectra Interpretation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48 2.4.1 RADAR Range Equation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 2.4.2 Interpretation of RADAR Noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 2.4.2.1 Thermal noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 2.4.2.2 Phase noise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 2.4.3 Noise Analysis during Target Absence and Presence . . . . . . . 53 2.4.3.1 Power-noise estimation in target absence . . . . . . . . . 53 2.4.3.2 Power-noise estimation in target presence . . . . . . . . 57 2.4.4 Initial Range Spectra Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60 2.5 Constant False Alarm Rate Processor for True Target Range Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 2.5.1 The Effect of the High Pass Filter on CFAR . . . . . . . . . . . . . . . . . 65 2.5.1.1 Missed detections with CFAR . . . . . . . . . . . . . . . . . . . . . 66 2.5.1.2 False alarms with CFAR . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 2.6 Target Presence Probability Estimation for True Target Range Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68 2.6.1 Target Presence Probability Results . . . . . . . . . . . . . . . . . . . . . . . . . . 72 41 © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 44 — #4 44 Autonomous Mobile Robots full predicted range spectra and the results are compared with the measured range bins in the initial stages of a simple SLAM formulation. 2.2 RELATED WORK In recent years RADAR, for automotive purposes, has attracted a great deal of interest in shorter range (<200 m) applications. Most of the work in short-range RADAR has focused on millimeter waves as this allows narrow beam shaping, which is necessary for higher angular resolution [5]. Some of the work to date in autonomous navigation using MMW RADAR is summarized here. Boehmke et al. [8] succeeded in producing three-dimensional (3D) terrain maps using a pulsed RADAR with a narrow beam of 1◦ and high sampling rate. The 1◦ RADAR beam width has a large antenna sweep volume and its physical size is large for robotic applications. The efforts by Boehmke et al. show the compromise between a narrow beam and antenna size, where a narrow beam provides better angular resolution. Steve Clark [9] presented a method for fusing RADAR readings from different vehicle locations into a two-dimensional (2D) representation. The method selects one range point per RADAR observation at a particular bearing angle based on a certain received signal power threshold level. This method takes only one range reading per bin which is the nearest power return to exceed that threshold to the RADAR, discarding all others. Clark [10] shows a MMW-RADAR-based navigation system which utilizes artificial beacons for localization and an extended Kalman filter for fusing multiple observa- tions. The fixed threshold can be used when the environment is known with no clutter.1 However, in a realistic environment (containing features having various RCS) fixed thresholding on raw data will cause an exorbitant number of false alarms if the threshold is low or missed detections if the threshold is too high. Manual assistance is required in adjusting the threshold as the returned signal power depends on various objects’ RCS. This method of feature detection is environment-dependent. Foessel [11] shows the usefulness of evidence grids for integrating uncer- tain and noisy sensor information. Foessel et al. [12] show the development of a RADAR sensor model for certainty grids and also demonstrate the integration of RADAR observations for building 3D outdoor maps. Certainty grids divide the area of interest into cells, where each cell stores a probabilistic estimate of its state [13,14]. The proposed 3D model by Foessel et al. has shortcom- ings such as the necessity of rigorous probabilistic formulation and difficulties 1 Clutter in this research is assumed to be the backscatter from land and is difficult to model. Land clutter is dependent on the type of terrain, its roughness, and dielectric properties. © 2006 by Taylor & Francis Group, LLC features are added together with the RADAR losses. Finally, Section 2.9 shows FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — pau Millimeter Wave RADAR Power-Range Spectra Interpretation 45 in representing dependencies due to occlusion. Jose and Adams [15] show a method of feature detection from MMW RADAR noisy data. 2.3 FMCW RADAR OPERATION AND RANGE NOISE This section gives a brief introduction to the RADAR sensor used in this work and the FMCW technique for obtaining target range. This is necessary for RADAR signal interpretation and for understanding and quantifying the noise in the range/power estimates. This is ultimately used in predicting range bin observations given the predicted vehicle state, in a mobile robot navigation technique it will be shown which noise sources affect both the range and received power estimates, and how each of these is affected. The RADAR unit (from Navtech Electronics) is a 77-GHz FMCW system. The transmitted power is 15 dBm and the swept bandwidth is 600 MHz [16]. The RADAR is shown in Figure 2.1, mounted on a four-wheel steerable vehicle. In Figure 2.2, the input voltage to the voltage control oscillator (VCO) is FIGURE 2.1 A 360◦ scanning MMW RADAR mounted on a vehicle test bed for SLAM experiments within the NTU campus. © 2006 by Taylor & Francis Group, LLC Figure 2.2 shows a schematic block diagram of an FMCW RADAR transceiver. framework — which is one of the goals of this chapter. By analyzing the FMCW FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 46 — #6 46 Autonomous Mobile Robots t V VCO Linearizer Coupler Circulator Antenna Mixer High pass filter Low pass filter Anti aliasing filter FFT FIGURE 2.2 Schematic block diagram of a MMW RADAR transceiver. a ramp signal. The VCO generates a signal of linearly increasing frequency δf in the frequency sweep period Td. This linearly increasing chirp signal is transmitted via the antenna. An FMCW RADAR measures the distance to an object by mixing the received signal with a portion of the transmitted signal [17]. Let the transmitted signal vT(t) as a function of time, t, be represented as vT(t) = [AT + aT(t)] cos [ ωct + Ab ∫ t 0 t dt + φ(t) ] = [AT + aT(t)] cos [ ωct + Ab 2 t2 + φ(t) ] (2.1) where AT is the amplitude of the carrier signal, Ab is the amplitude of the modulating signal, ωc is the carrier frequency (i.e., 2π × 77 GHz), aT(t) is the amplitude noise, and φ(t) is the phase noise present in the signal which occurs inside the transmitting electronic sections. At any instant of time, the received echo signal, vR is shifted in time from the transmitted signal by a round trip time, τ . The received signal is vR(t − τ) = [AR + aR(t − τ)] cos [ ωc(t − τ)+ Ab 2 (t − τ)2 + φ(t − τ) ] (2.2) where AR is the received signal amplitude, aR(t−τ) is the amplitude noise, and φ(t−τ) is the phase noise. The sources of noise affecting the signal’s amplitude consist of external interference to the RADAR system (e.g., atmospheric noise, © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 49 — #9 Millimeter Wave RADAR Power-Range Spectra Interpretation 49 0 20 40 60 80 100 120 140 160 180 200 –15 –10 –5 0 5 10 15 20 25 30 Range (m) P ow er ( dB ) Returns from objects High pass filter gain model FIGURE 2.3 Range spectrum from a MMW RADAR. The X axis is the range (in meters) and the Y axis is the returned power (in decibel). The first reflection is from a corner reflector and the second one is from a concrete wall. Multiple reflections are obtained due to the beam width of the RADAR. The gain model of the high pass filter is also shown in the figure. return of the RADAR spectra decreases near the maximum range (200 m) due to the low pass filter roll-off, which occurs before the high pass filter stage To understand the MMW RADAR range spectrum and to predict it accur- ately, it is necessary to use the RADAR range equation and knowledge of the noise distributions in the RADAR spectrum. A method for predicting the RADAR range spectra is now presented. An introduction is given explaining the relationship between RADAR signal returned power and range. Then, a method for establishing the relationship between the RCS and the range of objects in outdoor environments is shown. A noise analysis during signal absence and presence is then shown. This is necessary for predicting the range bins accur- ately during target presence and target absence. RADAR range bins are then predicted and it will be shown that the results compare reasonably well with actual (recorded) range bins recorded at various robot poses. © 2006 by Taylor & Francis Group, LLC (Figure 2.2). FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 50 — #10 50 Autonomous Mobile Robots 2.4.1 RADAR Range Equation According to the simple RADAR equation, the returned power Pr is proportional to the RCS of the object, σ and inversely proportional to the fourth power of range, R [21]. The simple RADAR range equation is formally written as Pr = PtG 2λ2σ (4π)3R4L (2.7) where Pt is the RADAR’s transmitted power, G is the antenna gain, λ is the wavelength (i.e., 3.89 mm in this case), and L the RADAR system losses. A high 4 drop in received signal power. In an FMCW RADAR, closer objects produce signals with low beat frequencies and vice-versa (Equation [2.5]). Therefore by attenuating low frequencies and amplifying high frequencies, it is possible to correct the range- based signal attenuation [18]. To compensate the returned power loss due to increased range, the high pass filter is modeled in two ways: 1. The bias in the received power spectra is estimated. 2. By modeling the high pass filter with a gain of 60 dB/decade, instead of the usual 40 dB/decade, to comply with the characteristics of the particular RADAR used here. The aim of this is to give a constant received signal power with range. The actual compensation which results in our system was shown in Figure 2.2 where it can be seen that the ideal flat response is not achieved by the internal high pass filter. 2.4.2 Interpretation of RADAR Noise This section analyzes the sources of noise in MMW RADARs and quantifies navigation formulations, observations must be predicted, and for the estimation algorithms to run correctly, the actual observations are assumed to equal the predictions, except that they are corrupted with Gaussian noise. It is therefore the aim of this section to determine the type of noise distributions in the actual received power and range values to determine how the predicted power–range spectra can be used correctly in a robot navigation formulation. RADAR noise is the unwanted power that impedes the performance of the RADAR. For the accurate prediction of range bins, the characterization of noise is important. The two main components are thermal and phase noise. Thermal noise affects the power reading while phase noise affects the range estimate. © 2006 by Taylor & Francis Group, LLC pass filter (shown in Figure 2.2) is used to compensate for the R the noise power in the received range spectra (seen in Figure 2.3). In most robot FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 51 — #11 Millimeter Wave RADAR Power-Range Spectra Interpretation 51 2.4.2.1 Thermal noise Thermal noise is generated in the RADAR receiver electronics. The noise power is given by PN (in Watts), where PN = kT0β (2.8) where k is the Boltzmann constant, T0 is the temperature, and β is the receiver signal (found from the FFT of this signal) is affected by the thermal noise power aR(t−τ), which contributes to A′ in Equation (2.6). It can be shown by analyzing the transition of this thermal (Gaussian) noise through the entire FMCW range detection process that when a target is present (strong received signal) the noise in the power–range spectrum follows a Gaussian distribution. When no target is present (weak or no reflected signal) it will be demonstrated from the results that the noise power follows a Weibull distribution. Therefore measurements with target presence/absence were made to verify these distributions and to quantify the power variance during target absence/presence. 2.4.2.2 Phase noise Another source of noise which affects the range spectra is the phase noise. The phase noise is generated by the frequency instability of the oscillator due to the thermal noise. Ideally for a particular input voltage to the VCO, the output has a single spectral component. In reality, the VCO generates a spectrum of frequencies with finite bandwidth which constitutes phase noise. This is shown in Equation (2.6), where a band of noise frequencies with different phase components, φ(t, τ) affects the desired signal frequency, which corresponds to range. The phase noise broadens the received power peaks and reduces the 3 This introduces noise into the range estimate itself. Experimental data provides insight into the phase noise distribution. For predicting the RADAR range spectra, the peaks at predicted targets are broadened by a small constant amount. This broadening is based on real measurements, which have shown the peaks4 to have widths ranging from 2.5 to 3.5 m. This has been observed from targets, of different RCS, placed at different distances from the RADAR. RADAR swash plate bearing angle. Figure 2.5a shows the entire range bins over the full 200 m range, while Figure 2.5b shows a zoomed view of the spectra obtained from the feature at 10.25 m. From the figures, it is evident that 3 The peaks and skirts shown in Figure 2.4 occur due to the leakage of signals from the transmitter into the mixer through the circulator and also due to the antenna impedance mismatch [11]. 4 © 2006 by Taylor & Francis Group, LLC sensitivity of range detection [11] as shown in Figure 2.4. Figure 2.5 shows 1000 superimposed range bins obtained for the same At their intersections with the high pass filter gain curve shown in Figure 2.3. bandwidth [22]. As shown in Section 2.3, the power in the beat frequency FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 54 — #14 54 Autonomous Mobile Robots the RADAR used in this work, an experimental determination of the power noise distributions is used here. To determine the power bias and variance of the range bins with no targets present, range bins were recorded at a fixed RADAR bearing angle, with no tar- gets present. These were recorded by pointing the RADAR toward the open sky. The mean power and standard deviation of the noisy power–range spectra across the complete range of the RADAR is shown in Figure 2.6. The standard devi- ation of the noise is noticeably less at shorter ranges (<45 m), as the particular RADAR used can only output a minimum received power value of−15 dB, and any received power value less than this, will simply be output as −15 dB. The noise power values significantly increase above the minimum−15 dB at higher ranges due to the higher gain of the high pass (range compensation) filter at higher ranges. Examination of the power distributions obtained at different ranges during target absence, suggests that a suitable approximation to the distributions is 0 20 40 60 80 100 120 140 160 180 200 –15 –10 –5 0 5 10 15 20 25(a) Range (m) P ow er ( dB ) FIGURE 2.6 Mean and standard deviation of the noise during target absence over the complete range of the RADAR. The figures are obtained from noise only range bins by pointing the RADAR toward the sky. (a) Mean power bias as a function of RADAR range. (b) The standard deviation in power as a function of RADAR range. The standard deviation is less at shorter distances due to the lower amplification of the high pass filter at those ranges. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 55 — #15 Millimeter Wave RADAR Power-Range Spectra Interpretation 55 0 20 40 60 80 100 120 140 160 180 200 0 1 2 3 4 5 6(b) Range (m) S ta nd ar d de vi at io n (d B ) FIGURE 2.6 Continued. distributions at arbitrary ranges of 10 and 100 m are shown. The Weibull probability distribution function can be written as f (x) = ξ ψ ( x ψ )ξ−1 e−(x/ψ)ξ , ∀ x > 0 (2.9) where x is the random variable, with scale parameterψ > 0 and shape parameter ξ > 0. The mean of x is µ = ψ (1 + (1/ξ)) − 15 and variance, σ 2 = ψ2 (1+(2/ξ)−ψ2[ (1+(1/ξ))]2), where (· · · ) is the Gamma function [23]. For scaling purposes, in this case the random variable x equals the received power Pr + 15 dB, in order to fit Equation (2.9). For a range of 10 m (Figure 2.7a), suitable parameters for an equival- ent Weibull distribution, ψ and ξ are 0.0662 and 0.4146, respectively.5 At low ranges, this distribution is approximately equivalent to an exponential distribution, with mean, µ = −14.8 dB and variance σ 2 = 0.3501 dB2. For a range of 100 m (Figure 2.7b), suitable Weibull parameters have been obtained as ψ = 26.706 and ξ = 5.333. The distribution has a mean, 5 These values are obtained using Matlab to fit Equation (2.9) to the experimentally obtained distribution of Figure 2.7a. © 2006 by Taylor & Francis Group, LLC the Weibull distribution [23]. This can be seen in Figure 2.7, where power FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 56 — #16 56 Autonomous Mobile Robots –15 –14 –13 –12 –11 –10 –9 –8 –7 –6 –5 0 1000 2000 3000 4000 5000 6000 7000 8000(a) Power (dB) N um be r –15 –10 –5 0 5 10 15 20 25 30 35 0 500 1000 1500 2000 2500(b) Power (dB) N um be r FIGURE 2.7 Experimental estimation of power noise distributions with no targets in the environment. (a) Experimental estimation of the noise distribution obtained from a 10 m distance. The distance has been chosen arbitrarily. (b) Experimental estimation of the noise distribution obtained from a 100 m distance. The distance has been chosen arbitrarily. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 59 — #19 Millimeter Wave RADAR Power-Range Spectra Interpretation 59 0 20 40 60 80 100 120 140 160 180 200 –160 –140 –120 –100 –80 –60 –40 –20 0 20 40 Range (m) R et ur ne d po w er ( dB ) RCS, s = 1000 RCS, s = 0.001 FIGURE 2.9 Expected curves of return power vs. distance for two objects with RCS values of 1000 and 0.001 m2. 0 20 40 60 80 100 120 –100 –50 0 50 100 Range (m) R et ur ne d po w er ( dB ) FIGURE 2.10 Range spectra prediction without range compensation. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 60 — #20 60 Autonomous Mobile Robots 2.4.4 Initial Range Spectra Prediction an object with a known RCS (10 m2) is assumed to be at a distance of 10.25 m. A Monte Carlo method has been used for simulating the noise in the figure. signal presence, and during signal absence Weibull distributions with paramet- ers explained in the previous section have been used. The values are obtained the noise frequency with respect to range which is evident in the real spectra. This mismatch is due to the phase noise throughout the entire range bin. The only during the parts of the range bin which are predicted to have targets, as explained above. During sections of the range bin with no targets (i.e., beyond 11 m in Figure 2.11a) it is not modeled, since this part of the spectra is of little interest in target estimation. A predicted and actual RADAR range spectra, obtained from an outdoor chi-squared test to determine any bias or inconsistency in the power–range bin predictions. The difference between the measured and the predicted range bins is plotted together with 99% confidence interval. The value of 99% bound, = ±16.35 dB, has been found experimentally by recording several [15]. Close analysis of Figure 2.13a shows that the error has a negative bias. This is due to the approximate assumption of the high pass filter gain. For the RADAR used here, the gain of the high pass filter used in the predicted power– range bins was set to 60 dB/decade, as explained earlier. Figure 2.13b shows a chi-squared test on the difference between a measured bin and its predicted error in Figure 2.13b is less biased than Figure 2.13a, a gain of 60 dB/decade with the small bias (Figure 2.13a) is still acceptable as most of the error values are well within 99% confidence limit and also taking the high pass filter effect role into consideration. A method for predicting the RADAR range spectra has been shown here which can be used for predicting observations, based on an estimate of a targets range and RCS value. Clearly a restriction of this method is that as a mobile robot moves with respect to objects within the environment, range bins can only be predicted assuming that the RCS does not change as the RADAR to target angle of incidence changes. In general this is clearly not a valid assumption, but © 2006 by Taylor & Francis Group, LLC noisy power–range bins in target absence (RADAR pointing toward open space) The tools are now complete to simulate/predict RADAR spectra. In Figure 2.10, presence (Figure 2.7 and Figure 2.4.3). The simulated result of applying the (Figure 2.11a) and actual range bin (Figure 2.11b) shows a slight mismatch in from the experimental estimation of the noise distributions in target absence and high pass 60 dB/decade filter is shown in Figure 2.11a. Analyzing the predicted environment, is shown in Figure 2.12. Figure 2.13a and b show the results of a as explained previously (3 × steady state standard deviation of Figure 2.6b) phase noise, approximately quantified in Section 2.4.2, is taken into account bin with the mean high pass filter bias of Figure 2.6a subtracted. Although the A Gaussian noise distribution with a variance of 26.57 dB is used when there is2 FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 61 — #21 Millimeter Wave RADAR Power-Range Spectra Interpretation 61 0 20 40 60 80 100 120 –10 –5 0 5 10 15 20 25 30 35 40(a) Range (m) R et ur ne d po w er ( dB ) 0 20 40 60 80 100 120 –10 –5 0 5 10 15 20 25 30 35 40(b) Distance (m) R et ur ne d po w er ( dB ) FIGURE 2.11 Predicted and actual RADAR spectra. (a) The effect of the range com- pensation (high pass) filter of 60 dB/decade. (b) Power vs. range of a single range bin obtained from an actual RADAR scan. A reflection is received from a target of RCS 10 m2 at 10.25 m. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 64 — #24 64 Autonomous Mobile Robots becomes acceptable for objects that are small and cylindrical in shape, making their RCS approximately view-point invariant, such as lamp posts, trees, etc., which can be used for outdoor navigation. 2.5 CONSTANT FALSE ALARM RATE PROCESSOR FOR TRUE TARGET RANGE DETECTION To extract the true range values, previous methods have used a power threshold on the range bins (the closest power value to exceed some threshold gives the closest object) [9] or constant false alarm rate (CFAR) techniques [21,24]. The problem with thresholding is, it requires manual adjustment of the threshold as the RCS of objects in an outdoor natural environment will vary. The function of CFAR processors is to maintain a constant and low rate of false alarms in detecting true range values [25]. A cell averaging (CA) detector is useful for maintaining a CFAR where the power noise-plus-clutter observations x = x1, . . . , xi, . . . , xn follow a Weibull random distribution shown in Equation (2.9). The structure of the applied CA- CFAR is shown in Figure 2.14. This figure shows M/2 reference cells (where M = 70) on each side of the cell, Y , under investigation. Guard cells are present to account for the broadened target reflection [26]. A moving window of width M = 70 range points is then used to sum the local noisy power values in the Z t x Z Comparator Output 1 or 0 M/2 M/2 G GY Y Input Xi Reference cells Guard cells Threshold t Σ Σ Σ FIGURE 2.14 The structure of the applied CA-CFAR detector. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 65 — #25 Millimeter Wave RADAR Power-Range Spectra Interpretation 65 range bin as shown in Equation (2.10) [27]. Z = M∑ i=1 xi (2.10) This sum is multiplied by a threshold, τ (in this case τ = 0.033), for later comparison with a test sample power value. The value for τ is chosen for achieving the desired value of Pfa, the design false alarm probability, in the absence of targets [28]. The scalar τ is a function of the number of reference cells M (here M = 70) and Pfa is (1×10−6) for the RADAR used here [10]. The test sample Y is either a noise-plus-clutter observation or a target return. The variable threshold τZ is compared with Y . A target is declared to be present if Y > τZ (2.11) ing a concrete wall at approximately 18 m. The detected features are indicated along with the adaptive threshold. The moving average will set the threshold above which targets are considered detected. Due to the phase noise, the power returned from the target is widened along the range axis, resulting in more feature detections at approximately 18 m. In Figure 2.15a and b, CFAR “picks out” features which lie at closest range. Features at a longer range, however, will not be detected as the noise power variance estimate by the CFAR pro- cessor becomes incorrect due to the range bin distortion caused by the high pass filter. 2.5.1 The Effect of the High Pass Filter on CFAR of the noisy received power values in Equation (2.10) is inaccurate at higher ranges, which ultimately results in the missed detection of targets at these range values. This is evident from Figure 2.15b where CFAR detects a feature (corner reflector) at 10.25 m while it misses a feature (building) at 138 m. The second reflection is due to the beam-width of the RADAR, as part of the transmitted signal passes the corner reflector. It would therefore be useful to reduce the power–range bias before applying the CFAR method. Therefore, to correctly implement the CA-CFAR method here, first, the average of two noise only range bins can be obtained,6 the result of which should be subtracted from the range bin under consideration. This is carried out to obtain a range independent, high pass filter gain for the resultant bin. 6 The noise only range bins are obtained by pointing the RADAR toward open space. © 2006 by Taylor & Francis Group, LLC The range bin in Figure 2.15 was obtained from an environment contain- The CFAR method has been applied to the range bin of Figure 2.11b, the full 200 m bin of which is shown in Figure 2.16a, after subtracting the high In general, since the gain of the high pass filter is not linear (Figure 2.6a) the sum FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 66 — #26 66 Autonomous Mobile Robots 0 20 40 60 80 100 120 140 160 180 200 –20 0 20 40 60 80 100(a) Range (m) P ow er ( dB ) RADAR range bin Features detected Adaptive threshold FIGURE 2.15 CFAR target detection. (a) The detection of a target (concrete wall approx- imately at 18 m) using a CA-CFAR detector. A series of targets around the 18 m mark are obtained due to the phase noise in the returned peak. (b) The missed detection of a feature (a building at 138 m) by a CA-CFAR detector. Due to the gain of the high pass filter, the noise estimation is inaccurate at higher ranges resulting in missed detection of features. containing a corner reflector at 10.25 m and a building at approximately 138 m. By reducing the high pass filter effect (range independent gain for all the ranges), the CFAR detection technique finds features regardless of range as shown in filter characteristics, in the form of power–range bias, before CA-CFAR can be applied correctly. Problems still arise however, as CFAR can misclassify targets as noise (missed-detection) and noise as targets (false-alarm). Both of these are evident and labeled in the CFAR results of Figure 2.16a. 2.5.1.1 Missed detections with CFAR In a typical autonomous vehicle environment the clutter level changes. As the RADAR beam width increases with range, the returned range bin may have multiple peaks from features. © 2006 by Taylor & Francis Group, LLC pass filter bias of Figure 2.6a. This figure shows the result from an environment, Figure 2.16a. It is clearly necessary to compensate for any nonideal high pass FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 69 — #29 Millimeter Wave RADAR Power-Range Spectra Interpretation 69 0 20 40 60 80 100 120 140 160 180 200 –20 –10 0 10 20 30 40 50(b) Range (m) P ow er ( dB ) False alarms Adaptive threshold Missed detection Feature RADAR range bin Features detected FIGURE 2.16 Continued. noise and extracting smaller signal returns along with the higher power returns, a method is now introduced which uses the probability of target presence [30] for feature detection [15]. This method is appealing compared to CFAR and constant threshold methods at ground level, as a threshold can be applied on the target presence probability. By setting a threshold value to be dependent on target presence probability and independent of the returned power in the signal, a higher probability threshold value is more useful for target detection. The proposed method does not require manual assistance. The merits of the proposed problem described here can be stated formally as a binary hypothesis testing problem [31]. Feature detection can be achieved by estimating the noise power contained in the range spectra. The noise is estimated by averaging past spectral power values and using a smoothing parameter. This smoothing parameter is adjusted by the target presence probability in the range bins. The target presence probability is obtained by taking the ratio between the local power of range spectra containing noise and its minimum value. The noise power thus estimated is then subtracted from the range bins to give a reduced noise range spectra. © 2006 by Taylor & Francis Group, LLC algorithm will be demonstrated in the results in Section 2.6.1. The detection FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 70 — #30 70 Autonomous Mobile Robots Let the power of the noisy range spectra be smoothed by a w-point window function b(i) whose length is 2w+ 1 P̆(k, l) = w∑ i=−w b(i)P̆(k − i, l) (2.12) where P̆(k, l) is the kth power value of lth range spectra. Smoothing is then performed by a first order recursive averaging technique: P̆(k, l) = αsP̆(k, l − 1)+ (1− αs)P̆(k, l) (2.13) where αs is a weighting parameter (0 ≤ αs ≤ 1). First the minimum and temporary values of the local power are initialized to Pmin(k, 0) = Ptmp(k, 0) = P̆(k, 0). Then a range bin-wise comparison is performed with the present bin l and the previous bin l − 1. Pmin(k, l) = min{Pmin(k, l − 1), P̆(k, l)} (2.14) Ptmp(k, l) = min{Ptmp(k, l − 1), P̆(k, l)} (2.15) When a predefined number of range bins have been recorded at the same vehicle location, and the same sensor azimuth, the temporary variable, Ptmp is reinitialized as Pmin(k, l) = min{Ptmp(k, l − 1), P̆(k, l)} (2.16) Ptmp(k, l) = P̆(k, l) (2.17) Let the signal-to-noise power (SNP), PSNP(k, l) = P̆(k, l)/Pmin(k, l) be the ratio between the local noisy power value and its derived minimum. In the Neyman–Pearson test [32], the optimal decision (i.e., whether target is present or absent) is made by minimizing the probability of the type II follows. The test, based on the likelihood ratio, is p(PSNP|H1) p(PSNP|H0) H1 ≷ H0 δ (2.18) © 2006 by Taylor & Francis Group, LLC error (see Appendix), subject to a maximum probability of type I error as FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 71 — #31 Millimeter Wave RADAR Power-Range Spectra Interpretation 71 where δ is a threshold,7 H0 and H1 designate hypothetical target absence and presence respectively. p(PSNP|H0) and p(PSNP|H1) are the conditional probability density functions. The decision rule of Equation (2.18) can be expressed as PSNP(k, l) H1 ≷ H0 δ (2.19) An indicator function, I(k, l) is defined where, I(k, l) = 1 for PSNP > δ and I(k, l) = 0 otherwise. The estimate of the conditional target presence probability,8 p̂′(k, l) is p̂′(k, l) = αpp̂′(k, l − 1)+ (1− αp)I(k, l) (2.20) This target presence probability can be used as a target likelihood within mobile robot navigation formulations. αp is a smoothing parameter (0 ≤ αp ≤ 1). The value of αp is chosen in such a way that the probability of target presence in the previous range bin has very small correlation with the next range bin (in this case αp = 0.1). It is of interest to note that, as a consequence of the above analysis, the noise power, λ̂d(k, l) in kth range bin is given by λ̂d(k, l) = α̃d(k, l)λ̂d(k, l − 1)+ [(1− α̃d(k, l))] P̆(k, l) (2.21) where α̃d(k, l) = αd + (1− αd)p′(k, l) (2.22) and αd is a smoothing parameter (0 ≤ αd ≤ 1). This can be used to obtain a noise reduced bin, P̂NR(k, l)using the method of power spectral subtraction [34]. In the basic spectral subtraction algorithm, the average noise power, λ̂d(k, l) is subtracted from the noisy range bin. To overcome the inaccuracies in the noise power estimate, and also the occasional occurrence of negative power estimates, the following method can be used [35] P̂NR(k, l) = { P̆(k, l)− c× λ̂d(k, l) if P̆(k, l) > c× λ̂d(k, l) d × λ̂d(k, l) otherwise 7 This threshold can be chosen based upon the received SNP, at which the signal can be trusted not to be noise. Note that this does not have to be changed for differing environments, or types of targets. 8 Conditioned on the indicator function I(k, l) [33]. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 74 — #34 74 Autonomous Mobile Robots 0 20 40 60 80 100 120 140 160 180 200 –10 0 10 20 30 40 50 60 70(c) Range (m) P ow er ( dB ) FIGURE 2.17 Continued. threshold of 40 dB applied against the raw RADAR data and the target presence probability. Further results conducted show the target presence probability of objects will be the same and is found to be more than 0.8. Feature detection using the target presence probability is then carried out by keeping the threshold at 0.8. probability-based feature detection is easier to interpret and has lower false alarms compared to constant threshold-based feature detection in the typical indoor and outdoor environments tested [36]. 2.6.2 Merits of the Proposed Algorithm over Other Feature Extraction Techniques The constant threshold applied to raw RADAR data requires manual inter- vention for adjusting the threshold depending on the environment. In CA-CFAR, the averaging of power values in the cells provides an automatic, local estimate of the noise level. This locally estimated noise power is used to define the threshold with the power of the signal and classifies the cell content as signal or noise. © 2006 by Taylor & Francis Group, LLC The results shown in Figures 2.18 to 2.20 clearly show that the target presence the adaptive threshold (see e.g., Figure 2.16a). The test window compares FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 75 — #35 Millimeter Wave RADAR Power-Range Spectra Interpretation 75 –30 –20 –10 0 10 20 30 –30 –20 –10 0 10 20 300 50 (a) Di sta nc e (m ) Distance (m) P ow er ( dB ) FIGURE 2.18 Raw RADAR data and corresponding target presence probability plots obtained from an indoor sports hall. (a) Power vs. range of a 2D RADAR scan from an indoor environment. (b) Target presence probability vs. range of a 2D RADAR scan in indoor environment. The probability of the targets detected (i.e., walls) are shown in the figure. When the signal and noise distributions are distinctly separated in range, CFAR works well. But when the signal and noise distributions lie close together, which is often the case at ground level (as shown in Figure 2.21), the method misclassifies noise as signal and vice versa. This is the reason for the poor performance of the CFAR technique with noisy RADAR data. Figure 2.22 shows features obtained by target presence probability and the CA-CFAR technique. The dots are the features obtained by target presence probability while the “+” signs are the features obtained from the CFAR-based target detector. From the figures it can be seen that the target presence-based feature detection has a superior performance to CA-CFAR detector in the environment tested. Figure 2.23 shows the difference between the ground truth and the range observation obtained from the target presence probabil- ity. The ground truth has been obtained by manually measuring the distance of the walls from the RADAR location. The peaks in Figure 2.23 are to some extent due to inaccurate ground truth estimates, but mainly due to multi-path reflections. The proposed algorithm for feature extraction appears to outperform the CFAR method because the CFAR method finds the noise locally, while the target presence probability-based feature detection algorithm estimates © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 76 — #36 76 Autonomous Mobile Robots –30 –20 –10 0 10 20 30 –30 –20 –10 0 10 20 300 0.5 1 (b) Di st an ce (m )Distance (m) P ro ba bi lit y FIGURE 2.18 Continued. the noise power by considering more than one range bin (Equation [2.16]). The target presence probability-based feature extraction, unlike the CFAR detector, is not a binary detection process as is shown in Figure 2.17c. This method of feature detection is useful in data fusion as the feature representation is probabilistic. 2.7 MULTIPLE LINE-OF-SIGHT TARGETS — RADAR PENETRATION At 77 GHz, millimeter waves can penetrate certain nonmetallic objects, which sometimes explains the multiple line-of-sight objects within a range bin.9 This limited penetration property can be exploited in mobile robot navigation in outdoor unstructured environments, and is explored further here. For validating the target penetration capability of the RADAR, tests were carried out with two different objects. In the section of the RADAR scan, shown in Figure 2.24a, a RADAR reflector of RCS 177 m2 and a sheet of 9 Although it should be noted that these can be the results of specular and multiple path reflections also. © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 79 — #39 Millimeter Wave RADAR Power-Range Spectra Interpretation 79 –30 –20 –10 0 10 20 30 –25 –20 –15 –10 –5 0 5 10 15 20 X distance (m) Y d is ta nc e (m ) Indoor stadium(a) Const. threshold on raw data Threshold on probability data FIGURE 2.20 Target presence probability vs. range spectra and the corresponding power vs. range taken from a 2D RADAR scan in an indoor environment. The figures shows a comparison of the proposed feature detection algorithm with the constant threshold method. (a) A constant power threshold of 25 dB is chosen and compared with the threshold (0.8) applied on probability-range spectra. (b) A constant power threshold of 40 dB is chosen and compared with the threshold applied to the probability–range spectra. is a pencil beam device, with a beam width of 1.8◦. This means that multiple returns within the range spectra occur mostly due to penetration. Therefore a model for predicting entire range spectra, based on target penetration is now given. 2.8 RADAR-BASED AUGMENTED STATE VECTOR The state vector consists of the normalized RADAR cross section, ϒR, absorp- tion cross section, ϒa, and the RADAR loss constants, L, along with the vehicle state and feature locations. The variables, ϒR, ϒa, and L are assumed unique to a particular feature/RADAR. Hence, this SLAM formulation makes the (very) simplified assumption that all features are stationary and that the changes in the normalized values of RCS and absorption cross sections of features when sensed from different angles, can be modeled using Gaussian random variables vϒi . © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 80 — #40 80 Autonomous Mobile Robots –80 –60 –40 –20 0 20 40 60 –40 –20 0 20 40 60 X distance (m) Y d is ta nc e (m ) Indoor stadium(b) Const. threshold on raw data Threshold on probability data FIGURE 2.20 Continued. This is a reasonable assumption only for small circular cross sectioned objects such as trees, lamp posts, and pillars, however, as will be shown the method pro- duces good results in semi-structured environments even for the targets which do not conform to these assumptions. The SLAM formulation here can handle multiple line-of-sight targets. 2.8.1 Process Model A simple vehicle predictive state model is assumed with stationary features surrounding it. The vehicle state, xv(k) is given by xv(k) = [x(k), y(k), θR(k)]T where x(k), y(k), and θR(k) are the local position and orientation of the vehicle at time k. The vehicle state, xv(k) is propagated to time (k+1) through a simple steering process model [38]. The model, with control inputs, u(k) predicts the vehicle state at time (k+1) together with the uncertainty in vehicle location represented in the covariance matrix P(k + 1) [39]. xv(k + 1) = f(xv(k), u(k))+ v(k) (2.23) © 2006 by Taylor & Francis Group, LLC FRANKL: “dk6033_c002” — 2006/3/31 — 17:29 — page 81 — #41 Millimeter Wave RADAR Power-Range Spectra Interpretation 81 –15 –10 –5 0 5 10 15 20 25 30 0 200 400 600 800 1000 1200 1400 PDFs for noise and signal + noise Power (dB) Signal + noise distribution Noise distributionN um be r FIGURE 2.21 Experimental estimation of signal and noise distributions. In the CFAR method, the local noise-plus-clutter power (Equation [2.10]) in the window is used to set the detection threshold. The method compares the signal in the test window and the detection threshold. The method fails when there are multiple detections within a range-bin and in cluttered environments. u(k) = [v(k), α(k)]. v(k) is the velocity of the vehicle at time k and α(k) is the steering angle. In full, the predicted state at time, (k + 1) becomes ⎡ ⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣ x̂(k + 1|k) ŷ(k + 1|k) θ̂R(k + 1|k) xp1(k + 1|k) yp1(k + 1|k) ϒR1(k + 1|k) ϒa1(k + 1|k) ... xpN (k + 1|k) ypN (k + 1|k) ϒRN (k + 1|k) ϒaN (k + 1|k) L(k + 1|k) ⎤ ⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ = ⎡ ⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣ x̂(k|k) ŷ(k|k) θ̂R(k|k) xp1(k|k) yp1(k|k) ϒR1(k|k) ϒa1(k|k) ... xpN (k|k) ypN (k|k) ϒRN (k|k) ϒaN (k|k) L(k|k) ⎤ ⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ + ⎡ ⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣ x(k) y(k) α(k) 0p1 0p1 0p1 0p1 ... 0pN 0pN 0pN 0pN 0 ⎤ ⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦ (2.24) © 2006 by Taylor & Francis Group, LLC