Position estimation for ground vehicle navigation based on landmark identification/yaw rate and perception of landmarks

TECHNICAL FIELD OF THE INVENTION

This invention deals with automated navigation in ground vehicles as well as non-autonomous vehicles.

BACKGROUND OF THE INVENTION

GPS (global positioning systems) have become ubiquitous in today's passenger vehicles. They are used for helping the driver navigate to a destination and do not have very high accuracy. Satellite reception in various urban settings can be sporadic and can result in loss of signal periodically. High accurate GPS is a very critical feedback in autonomous ground vehicles.

Unmanned urban ground vehicles (UGV's) are robotic platforms that are either remotely operated or autonomous. They are generally capable of operating outdoors and over a wide variety of terrain.

An autonomous UGV is essentially an autonomous robot with mechanisms that allow it to navigate on the surface of the ground. Among other tasks, a fully autonomous robot in the real world has the ability to travel without human navigation assistance.

One approach to the autonomous navigation problem is referred to as the Simultaneous Localization and Mapping (SLAM) approach. In the SLAM approach, the true world location of landmarks or of the vehicle itself is not needed. The basic operating methods of SLAM are the ability to place an autonomous vehicle at an unknown location in an unknown environment, have it build a map, using only relative observations of the environment, and then use this map to navigate. The main advantage of SLAM is that it eliminates the need for a priori topological knowledge of the environment. A solution to the SLAM problem is valuable in applications where absolute position or information is unobtainable.

Another approach to autonomous navigation uses GPS data. If GPS data is available to the vehicle, the vehicle can acquire GPS data live to determine its location. Alternatively, for environments where GPS data is not available, with the advent of advanced mapping information from companies such as Google, Yahoo and Microsoft, a priori knowledge of GPS coordinates of landmarks can be stored before driving a route. Based on perception technologies such as machine vision or LIDAR, and intelligent processing, landmarks can be identified en route. The vehicle determines its position based on GPS coordinates for the landmarks. Generally, once each current location is known, the vehicle can then navigate to a next location.

BRIEF DESCRIPTION OF THE DRAWINGS

A more complete understanding of the present embodiments and advantages thereof may be acquired by referring to the following description taken in conjunction with the accompanying drawings, in which reference numbers indicate like features, and wherein:

<figref idrefs="DRAWINGS">FIG. 1</figref> illustrates an example of an autonomous AGV equipped with position-finding equipment in accordance with the invention.

<figref idrefs="DRAWINGS">FIG. 2</figref> illustrates the landmark database and processing unit of the AGV of <figref idrefs="DRAWINGS">FIG. 1</figref>.

<figref idrefs="DRAWINGS">FIG. 3</figref> illustrates an example of two landmarks encountered by the AGV en route.

<figref idrefs="DRAWINGS">FIG. 4A</figref> illustrates the LIDAR identification of the landmarks and the estimated GPS coordinates for the landmarks.

<figref idrefs="DRAWINGS">FIG. 4B</figref> illustrates the deviation (or offsets) between the stored global coordinates and estimated GPS coordinates.

<figref idrefs="DRAWINGS">FIG. 4C</figref> illustrates the transformation (reset) necessary to adjust the estimated GPS values discussed in this embodiment.

<figref idrefs="DRAWINGS">FIG. 5</figref> illustrates a test route undertaken by the AGV.

<figref idrefs="DRAWINGS">FIG. 6</figref> illustrates the test route of <figref idrefs="DRAWINGS">FIG. 5</figref> with its landmarks.

<figref idrefs="DRAWINGS">FIG. 7</figref> illustrates the error between actual and estimated GPS coordinates for a portion of the test route.

<figref idrefs="DRAWINGS">FIG. 8</figref> illustrates the use of optical camera-based equipment to characterize lane markings as landmarks.

DETAILED DESCRIPTION OF THE INVENTION

The following invention describes at least two aspects of navigation in ground vehicles. One aspect is estimation of vehicle position during GPS outages based on urban landmarks. A second aspect is perception of such landmarks for supporting position estimation and for sensing lane deviations.

The methods described herein can be applied to autonomous ground vehicles, where GPS feedback is critical to staying on desired path and vehicle position estimation becomes essential during GPS outages. The methods can also be used for conventional vehicle location estimation for use in GPS-based driver navigation. The perception aspect of the invention can also be used for detecting deviation from lane in passenger cars and especially in trailer trucks. For example, in cab-trailer trucks, the weaving of the trailer can be dangerous and warning the driver at the inception of such oscillation can prevent a roll over.

The term “autonomous” is used broadly herein to refer to any vehicle that uses position data for navigation, whether as its sole, alternative, or back-up means of navigation. Thus, as indicated below, an AGV (autonomous ground vehicle) could be a driver-operated vehicle that uses GPS navigation. A “vehicle” can be any object capable of navigation. An example of a suitable setting for the AGV is an urban setting, where landmarks are abundant and their locations are known through GPS detection.

As explained below, an AGV looks for and recognizes landmarks during travel. The AGV begins from a landmark whose location coordinates are known. It then navigates according to position estimations that are based on measured yaw rate and speed. Once a next landmark is recognized, its exact (also referred to herein as “true”) GPS coordinates are looked up from a database. These true coordinates are then used to enhance position estimation accuracy between landmarks. This method can be referred to as a “pseudo feedback” method. It should be understood that “GPS coordinates” is a readily understood term for geographical location coordinates, but coordinates could be obtained from sources other than GPS.

A typical application of the position estimation method is for AGVs that navigate in territory in which GPS locations can be obtained and stored, but in which no live GPS signal is available to the AGV. The method can also be used as a backup for AGVs that normally navigate by live GPS but for situations when the GPS signal is not available.

Fundamental in landmark-based AGV navigation is the identification of landmarks. The most reliable methodology is to utilize as much a priori information as possible from readily available mapping sources for the desired route. Examples of a priori information are locations of major road features such as intersections, lane markings, cross roads, bridges, business buildings, and overpasses.

Once this high-level a priori world information is identified along the desired route, it is necessary to identify subfeatures that characterize these higher level landmarks. For example, an intersection can be characterized and identified by the presence of a stop sign, traffic lights, road markings, and even the shape of its curbs.

For purposes of this description, it is assumed that the AGV has onboard visual perception equipment and related processing equipment for identifying landmarks. An example of such equipment is LIDAR (Light Detection and Ranging) equipment, which applies an optical remote sensing technology to obtain data about a target object or surface. Similar to radar technology, which uses radio waves instead of light, LIDAR determines the range to an object by measuring the time delay between transmission of an optical (typically laser) pulse and detection of the reflected signal. LIDAR data processing techniques can be extended to provide imaging applications, such as landmark identification.

<figref idrefs="DRAWINGS">FIGS. 1 and 2</figref> illustrate an AGV <b>100</b> equipped for navigation in accordance with the invention. A visual perception unit <b>103</b> provides data for landmark recognition. In the example of this description, the visual perception is based on LIDAR technology; however, this methodology of perception is equally valid for devices such as optical cameras or sonars.

A landmark database <b>104</b> stores data representing various landmarks expected along the route of the AGV. For each landmark, database <b>104</b> also stores its GPS coordinates.

A processing unit <b>105</b> is programmed in accordance with the algorithm described below, to estimate the vehicle's position when landmarks are not available, and to correct the estimations when landmarks are identified. An example of a suitable implementation of landmark database <b>104</b> and processing unit <b>105</b> is a dSPACE AutoBox, equipped with appropriate memory and processing devices. This equipment is typically onboard the vehicle for prototyping demonstration. With the advent of powerful processors, integration of real time databases into automotive networks such as CAN is rapidly progressing from prototype to production-ready solutions. These technologies will see easy deployment of the navigation databases into production cars in the near future.

Suitable yaw rate sensors and speedometers are commercially available. A yaw rate sensor measures a vehicle's angular velocity around its vertical axis and its output is usually in degrees (or radians) per second. An inertial navigation system, such as that manufactured by Oxford Technical Solutions, has suitable yaw rate and speed rate sensors. AGV has whatever additional equipment is required to provide locomotion and steering, based on position data provided by processing unit <b>105</b>.

<figref idrefs="DRAWINGS">FIG. 3</figref> illustrates a road intersection being used as a landmark as AGV <b>100</b> approaches it. Each feature of a landmark is characterized by an individual GPS coordinate that may or may not lie on the landmark and has a fixed geometric relationship to the landmark. For example, an intersection can be characterized by a minimum of four GPS coordinates that lie at the geometric intersection of virtual lines (generated from curbs) that make up the two roads as shown in <figref idrefs="DRAWINGS">FIG. 3</figref>. These GPS coordinates are then available to the processing unit <b>105</b> from landmark database <b>104</b>.

Examples of other suitable landmarks are road features such as lane markings, cross roads, bridges, business buildings or other architectural landmarks, and overpasses. Each landmark can then be characterized by its subfeatures. For example, an intersection can be characterized by signage, traffic lights, road markings, or curb shapes. Continued awareness of its position in real time enables efficient search of the database to match LIDAR data for identification.

On any given route, there may be limitless landmarks available as a priori data for storage in database <b>104</b>. During travel, many landmarks may be difficult to identify and match against the a priori data. Although the more landmarks that can be identified and located, the more accurate the navigation, it may not be necessary to identify each and every landmark.

For navigation, processing unit <b>105</b> receives data representing vehicle speed, inertial yaw rate and the true location of landmark coordinates. It is assumed that there is always a landmark at the start of the route. For this initial landmark, coordinates are provided to processing unit <b>105</b> from landmark database <b>104</b>.

As AGV navigates, LIDAR unit <b>103</b> continually detects potential landmarks. Also, processing unit <b>105</b> continually determines vehicle position.

When no landmark is currently identified, the position of the vehicle is given by some form of vehicle state estimation such as: <br /><i>y</i>_est<i>=∫v </i>cos(<i>H</i>_est)<i>dt </i><br /><i>x</i>_est<i>=∫v </i>sin(<i>H</i>_est)<i>dt </i><br />H_est=∫{dot over (ψ)}dt<br /> , where x_est, y_estare the estimated location (longitude and latitude with respect to a local origin), v is vehicle speed, H_est, is the estimated heading of the vehicle, and {dot over (ψ)} the measured yaw rate of the vehicle.

When a vehicle passes a landmark that can be detected, identified, and located, the landmark's location values are read from database <b>104</b>. <br /><i>y</i>_est<i>=y</i>_landmark<i>+y</i>_offset<br /><i>x</i>_est<i>=x</i>_landmark<i>+x</i>_offset<br /><i>H</i>_est=θ_landmark+θ_offset<br /> , where x_landmark, y_landmark, θ_landmarkare the true location (longitude, latitude and heading with respect to a local origin) of the landmark. x_offset, y_offset, θ_offsetare the offsets of vehicle from the landmark in space and time. Typically, the offsets can be easily computed using basic geometry and image comparisons. For example, as vehicle <b>100</b> approaches an intersection, the actual image from LIDAR or camera can be matched against the intersection image in the database and the amount of mismatch gives the offsets (rotation, x and y).

Referring again to <figref idrefs="DRAWINGS">FIG. 3</figref>, and to <figref idrefs="DRAWINGS">FIGS. 4A-4C</figref>, for each landmark, its true location from apriori (stored) coordinates can be compared with its estimated location. <figref idrefs="DRAWINGS">FIGS. 4A-4C</figref> illustrate LIDAR sensor data obtained by LIDAR unit <b>103</b> for the intersection of <figref idrefs="DRAWINGS">FIG. 3</figref>. As illustrated, each landmark has been recognized based on features, history, and knowledge of driving route.

As illustrated, the LIDAR sensor data can be used to provide estimated GPS coordinates, which are corrected by the stored GPS data to determine the vehicle's true position. The vehicle's x and y locations can be corrected, as well as its heading, on the basis of the landmark's x and y coordinates and angular change. It must be noted that the vehicle does not have to be precisely at the landmark when the correction is applied. It may also be noted that the transformation does not need to occur while the vehicle <b>100</b> is in the intersection. It can be carried out prior to, in the intersection, or at some distance past the intersection since processing unit <b>105</b> maintains historical information of vehicle travel variables. Based on LIDAR data on relative position and heading of the landmark from the vehicle, landmark coordinates can be transformed to vehicle location precisely when the correction is applied.

<figref idrefs="DRAWINGS">FIG. 5</figref> illustrates a test route for an AGV programmed to navigate with a landmark-based position estimation method in accordance with the invention described herein. Landmark coordinates were stored in database <b>104</b> for markers on the test route. All cross streets were used as landmarks. For test purposes, the vehicle was navigated along this route, and actual and estimated position data were acquired and compared.

<figref idrefs="DRAWINGS">FIGS. 6 and 7</figref> illustrate, for the test route of <figref idrefs="DRAWINGS">FIG. 6</figref>, the results of comparing estimated locations determined by the AGV and actual locations determined by GPS. As illustrated, error in estimation is maximum (up to two meters) after traveling low curvature paths. The error is small when there is increased turning activity. This error can be reduced by increasing the number of landmarks on low curvature paths or improved state estimation algorithms.

As indicated above, the methods described herein can also be applied to GPS-based navigation of any commercial vehicle when satellite connectivity to GPS is lost. In this case, a yaw rate sensor and LIDAR are added along with an additional processing unit on the vehicle communication bus.

In the absence of landmarks, lane markings and onboard optical cameras can assist a vehicle to remain within a desired path. Detected deviation from lane direction can be used to determine the vehicle position with respect to the lane and issue lane departure warnings. It can provide alerts to swaying of trailers with respect to truck cabs and warn drivers of impending roll over.

<figref idrefs="DRAWINGS">FIG. 8</figref> illustrates the use of optical camera-based equipment to characterize lane markings as landmarks. In <figref idrefs="DRAWINGS">FIG. 8</figref>, an autonomous or conventional vehicle <b>100</b> is traveling from left to right within its lane. The method of <figref idrefs="DRAWINGS">FIG. 8</figref> uses side-mounted cameras <b>801</b> for lane detection and defines a geometric relationship between the left and right lane markings (or lane width) as a feature of the landmark. The lane width is computed from the known width of the vehicle and the estimated distances from the left and right side of the vehicle (dL and dR) extracted from the images. These and other features of the lane markings can be incorporated into the database including lane color and shape (including breaks in certain sections of the road). These features are dependent on specific GPS coordinates. Thus, the matching of these features by the processing unit <b>105</b> allows the vehicle <b>100</b> to be accurately positioned during GPS outages. This technology can also enhance lane departure warning systems and alert the driver in heavy duty trailer trucks at the onset of trailer instability with reference to the cab.