Patent Application
20120150437
The present disclosure relates generally to vehicle positioning and, more particularly, to systems and methods for determining a vehicles position at sub-lane accuracy by fusing input from GPS, camera, radar, a lane-level digital map, and vehicle-dynamics sensors.
Modern automobiles have on-board computers facilitating vehicle operation and enabling various safety features. Active safety and driver assistance (ASDA) applications require an accurate awareness of vehicle position with respect to a road and other vehicles. Exemplary ASDA applications include navigation applications, lane-keeping and lane centering applications, autonomous driving, and collision-avoidance applications.
For some advanced ASDA operations, accurately positioning the vehicle within a particular lane of a road (lane-level accuracy), not just on-the-road resolution, is needed. Such operations include accurately determining (i) that another vehicle is in the host vehicle path and a separation distance to the other vehicle is decreasing rapidly, (ii) whether the host vehicle is deviating from an intended lateral position (e.g., lane center), and (iii) whether the host vehicle or a nearby vehicle is straying from its present lane, and so possibly changing lanes. Lane-level accuracy generally equates to positioning accuracy within about one meter.
To date, to achieve vehicle positioning of the desired accuracy, correction from a source external to the vehicle has been required. For example, assistance has been provided to the standard global positioning system (GPS) in relative-positioning or differential GPS (DGPS), wide area augmentation system (WAAS) GPS, and combinations of the two.
A primary drawback of such systems is that they inherently rely on the infrastructure additional to the basic GPS. This dependence renders the vehicle less autonomous, and use of the external infrastructure sometimes comes at an added financial cost and/or additional processing cost in terms of time and use of computing resources.
On-board systems have also been used in vehicle positioning. Exemplary systems are inertial-movement units and systems using wheel sensors. These systems, though, cannot be heavily relied upon alone as they inherently accumulate error rather rapidly over time and must themselves be supplemented with accurate positioning updates from the external systems.
The present disclosure relates to a method performed by a vehicle having an on-board computer, vehicle sensors, a satellite-positioning unit, a database storing a lane-level map for determining a new pose of the vehicle using map matching. The method includes the on-board computer of the vehicle receiving new data from at least one of the vehicle sensors and collecting measurements from the vehicle sensors. The method also includes the on-board computer of the vehicle computing propagation of vehicle pose with respect to consecutive time instances and performing a curve-fitting process. The method further includes the on-board computer of the vehicle performing a sub-routine of updating at least one observation model based on results of the curve-fitting process, performing a tracking sub-routine including using a probability distribution to update the vehicle pose in terms of data particles, and performing a particle filtering sub-routine based on the data particles to compute the new vehicle pose.
The present disclosure also relates to a transportation vehicle configured to determine a new pose of the vehicle using map matching. The vehicle includes multiple vehicle sensors, a satellite-positioning unit, and an on-board computer. The on-board computer includes a processor and a memory storing a lane-level map and instructions for execution by the processor. The instructions, when executed by the processor, cause the processor to receive new data from at least one of the vehicle sensors and collect measurements from the vehicle sensors. The instructions also cause the processor to compute propagation of vehicle pose with respect to consecutive time instances and perform a curve-fitting process. The instructions further cause the processor to perform a sub-routine of updating at least one observation model based on results of the curve-fitting process, perform a tracking sub-routine including using a probability distribution to update the vehicle pose in terms of data particles, and perform a particle filtering sub-routine based on the data particles to compute the new vehicle pose.
In another aspect, the disclosure relates to a tangible, non-transitory computer-readable medium for use in a transportation vehicle and storing instructions executable by a processor. The instructions cause, when executed by the processor, cause the processor to receive new data from at least one of the vehicle sensors and collect measurements from the vehicle sensors. The instructions also cause the processor to compute propagation of vehicle pose with respect to consecutive time instances and perform a curve-fitting process. The instructions further cause the processor to perform a sub-routine of updating at least one observation model based on results of the curve-fitting process, perform a tracking sub-routine including using a probability distribution to update the vehicle pose in terms of data particles, and perform a particle filtering sub-routine based on the data particles to compute the new vehicle pose.
Other aspects of the present invention will be in part apparent and in part pointed out hereinafter.
As required, detailed embodiments of the present disclosure are disclosed herein. The disclosed embodiments are merely examples that may be embodied in various and alternative forms, and combinations thereof. As used herein, for example, “exemplary,” and similar terms, refer expansively to embodiments that serve as an illustration, specimen, model or pattern. The figures are not necessarily to scale and some features may be exaggerated or minimized, such as to show details of particular components. In some instances, well-known components, systems, materials or methods have not been described in detail in order to avoid obscuring the present disclosure. Therefore, specific structural and functional details disclosed herein are not to be interpreted as limiting, but merely as a basis for the claims and as a representative basis for teaching one skilled in the art to variously employ the present disclosure.
While the description includes a general context of computer-executable instructions, the present disclosure can also be implemented in combination with other program modules and/or as a combination of hardware and software. The term “application,” or variants thereof, is used expansively herein to include routines, program modules, programs, components, data structures, algorithms, and the like. Applications can be implemented on various system configurations, including single-processor or multiprocessor systems, microprocessor-based electronics, combinations thereof, and the like.
The present disclosure describes systems and methods for precise sub-lane positioning combining satellite positioning systems (SPS; e.g., global positioning system (GPS)), camera, a lane-level digital map, radar, and other vehicle sensors, such as sensors measuring vehicle dynamics.
In some embodiments, positioning is accomplished by receiving data from multiple inputs, matching vehicle position to the digital map having lane-level accuracy, updating various observation models related to the sources of input, updating weights of particles in a probability distribution, performing an importance re-sampling, and computing a mean of the particles.
In various embodiments, road geometry and map matching is used to localize the vehicle on the road, vision-based lane sensing is used to provide lateral correction for map-matched SPS vehicle position, and traffic in nearby lanes and number-of-lanes information from the map database is used to determine the lane in which the host vehicle is positioned.
Turning now to the figures, and more particularly the first figure,
The OBC 100 also includes a sensor sub-system 114 comprising sensors providing information to the OBC 100 regarding vehicle operations, vehicle position, vehicle pose, and/or the environment about the vehicle 102. In some embodiments, the sensor sub-system 114 includes at least one camera 116 and at least one range sensor 118, such as a radar. The camera 116 may include a monocular forward-looking camera, such as those used in lane-departure-warning (LDW) systems. Such sensor sensing external conditions may be oriented in any of a variety of directions without departing from the scope of the present disclosure. For example, cameras 116 and radar 118 may be oriented at each, or select, positions of, for example: (i) facing forward from a front center point of the vehicle 102, (ii) facing rearward from a rear center point of the vehicle 102, and (iii) facing laterally of the vehicle from a side position of the vehicle 102. Accordingly, the descriptions below, made primarily with respect to forward-facing sensors, may be applied with respect to rearward and/or side facing sensors, independently or in combination with forward-facing sensors.
The range sensor 118 may include a short-range radar (SRR), an ultrasonic sensor, a long-range RADAR, such as those used in autonomous-cruise-control (ACC) systems, or a Light Detection And Ranging (LiDAR) sensor, for example.
Other sensor sub-systems include an inertial-momentum unit (IMU) 120, such as one having one or more accelerometers, wheel sensors 122, and other available dynamic vehicle sensors 124, such as a sensor associated with a steering system (e.g., steering wheel) of the vehicle 102.
The OBC 100 also includes a sub-system 126 for communicating with external infrastructure. This sub-system 126 includes a SPS unit 128, having a SPS receiver. In some embodiments, the sub-system 126 includes one or more transceivers 130 facilitating long-range wireless communications, such as by way of satellite and cellular telecommunication networks.
The sub-system 126 also includes one or more transceivers 130 facilitating short-range wireless communications. The OBC 100 uses short-range communications at least for vehicle-to-vehicle (V2V) communications and communicating with transportation system infrastructure (V2I). The short-range communication transceiver 130 may be configured to communicate by way of one or more short-range communication protocols, such as Dedicated Short-Range Communications (DSRC), WI-FI®, BLUETOOTH®, infrared, infrared data association (IRDA), near field communications (NFC), the like, or improvements thereof (WI-FI is a registered trademark of WI-FI Alliance, of Austin, Tex.; BLUETOOTH is a registered trademark of Bluetooth SIG, Inc., of Bellevue, Wash.).
In describing the present method 200, reference is made throughout the disclosure to related figures (e.g.,
The steps of the method 200 are presented in the demonstrated order for ease of description and illustration. Steps can be added, omitted and/or performed simultaneously without departing from the scope of the appended claims. It should also be understood that the illustrated method 200 can be ended at any time. In certain embodiments, some or all steps of this process, and/or substantially equivalent steps are performed by execution of computer-readable instructions stored or included on a computer-readable medium. For instance, the method may be performed by the processor 106 executing instructions, comprising one or more algorithms, stored in the memory 104 of the OBC 100 of the host vehicle 102.
A. General Premises
A digital map of lane-level accuracy has data identifying edges of lanes of roads, not just edges of the roads. Attributes of such digital maps include a number of lanes and global positions of the lane boundaries and the center lane stripe. In the present disclosure, a state of the host vehicle 102 at a time index t is referred to as s(t). North and east displacements n and e are provided with respect to a base reference point. It is contemplated that a southern displacement can be represented by a negative north displacement value and a western displacement can be represented by a negative east displacement value.
The base reference point can be a virtual point for convenience and any nearby point is a valid candidate. The OBC 100 can reset the base reference point as the vehicle 102 moves. A heading □ is an angle of the vehicle 102 with respect to a north velocity scalar v, and a lane in which the vehicle 102 is positioned is represented by L (e.g., L=1, 2, or 3).
B. Map Matchinq
The method 200 begins 201 and flow proceeds to step 202, whereat new data is received, such as from one or more of the various sensors 116-124, 128-134. If new data is received, measurements from the sensors 116-124, 128-134 is collected at step 204.
Upon collecting the new sensor data in step 204, at step 206, propagation of vehicle pose is computed with respect to vehicle poses at consecutive time instances. Vehicle pose refers to an orientation of the vehicle and can be represented in a variety of ways, such as by an angle separating a forward-facing vehicle center line from true North. In some embodiments, pose of the vehicle 102 is evaluated based on a vehicle dynamic model, such as by considering a constant velocity (CV) or a constant turning (CT). The step 206 of computing propagation of vehicle pose is described below in further detail in connection with
At step 208, (i) the current road segment, represented by waypoints identifying aspects of the road (e.g., edges, center line), for example, is matched based on the current vehicle position, (ii) a curvature profile of a cubic curve fitted from the waypoints is computed, and (iii) a closest point on the curve from the vehicle position is identified. This process can be referred to as curve fitting.
The waypoints can be represented by pi=(xi,yi), and the host vehicle's current position denoted as pH=(xH,yH), for example. A standard cubic curve fitting procedure will find a curve profile, e.g., c(s)=(x(s), y(s)), represented by the parameter s (i.e., the arc length). The curvature profile can be computed as:
where the primes refer to derivatives with respect to the parameter s.
The closest point on the curve s* is defined as:
s*=argmin
E
∥c(s)−pH∥,
with argmin being the argument of the minimum.
The curvature of the cloest point on the curve is C(s*). With t(s) corresponding to an equation of the tangent line of the curve at point p(s), the closest point s* can be iteratively computed as:
1. Let s*=0;
2. Compute the tangent equation at s* and find the closest point p* on the tangent to pH;
3. s*=s*(pH−p*)tn, where n is the normalized tangent vector, i.e., ∥n∥=1; and
4. Go to step 2, then 3, for a number (e.g., five) of iterations, and then stop.
C. Update Models
The method 200 further includes a sub-routine 210 in which various observations models are updated. Particularly, the sub-routine 210 includes a step 212 of updating a vision observation model, a step 214 of updating a radar observation model, a step 216 of updating an SPS (e.g., GPS) observation model, and a step 218 of updating an observation model for map attribute-based matching. For updating the observation model of map attribute-based matching, the matching may related to, for example, a curvature or an overhead object or structure (e.g., bridge or tunnel), as described in further detail, below.
1. Update Vision Observation Model
It should be understood that the steps of the method 300 are not necessarily presented in any particular order and that performance of some or all the steps in an alternative order is possible and is contemplated.
The steps of the method 300 are presented in the demonstrated order for ease of description and illustration. Steps can be added, omitted and/or performed simultaneously without departing from the scope of the appended claims. It should also be understood that the illustrated method 300 can be ended at any time. In certain embodiments, some or all steps of this process, and/or substantially equivalent steps are performed by execution of computer-readable instructions stored or included on a computer-readable medium. For instance, the method may be performed by the processor 106 executing instructions, comprising one or more algorithms, stored in the memory 104 of the OBC 100 of the host vehicle 102.
As shown in
After identifying the registered lane stripes, the process 300 proceeds to a step 304 of computing a position of the lane stripe(s) with respect to a reference frame of the host vehicle 102. Given the position and the pose of the vehicle 102, the lane stripes registered in the global coordinate system of the digital map are projected onto the vehicle frame.
In some embodiments, at step 306, the detected and registered lane stripes and boundaries are matched using a Euclidean-type analysis of distance. Therein, only objects within a predefined threshold distance of the host vehicle 102 are matched. With the matched data, at step 308, an observation probability distribution with respect to the camera data is computed. Particularly, at step 308, a vision observation model p(ovis|si) is computed as follows. The vision observation model p(ovis∥si) is updated based on measurements of the vision system (including, e.g., the camera 116). This update process is further described, here, in connection with
Vision system measurements include: left and right lateral measurements L′ and R′, and measurements F of the detected lane strip a′. The left lateral measurement L′=(y′L,φ′L), represents an offset and an orientation, respectively, of the vehicle 102 to a left lane boundary LLB. The right lateral measurement R′=(y′R,φ′R), represents an offset and an orientation, respectively, to a right lane boundary RLB.
Measurements F′ of the detected lane stripe a′ can be represented as: F′=(x′p, y′p, α′), including a longitudinal offset x′p between the vehicle 102 and the detected starting point of the stripe a′, a lateral offset y′p between the vehicle 102 and the detected starting point of the stripe α′, and an orientation a′ of the stripe, respectively.
The detected lane stripe and the lane boundaries are registered in the digital map (e.g., in a map database). Given a particle si, the lane boundaries are matched to the stripe using the position of the vehicle 102, the pose of the vehicle 102, and the present lane index L for the vehicle 102. Then, the matched boundaries and stripe are projected to the vehicle frame as L, R, and F, respectively.
Thus, the vision observation model p(ovis|si) can be computed as:
where c is a normalization constant for a density function, and σL, σR, and σF corresponds to an accuracy specification of the lane sensing data. The process 300 may end 309.
2. Update Radar Observation Model
It should be understood that the steps of the method 500 are not necessarily presented in any particular order and that performance of some or all the steps in an alternative order is possible and is contemplated.
The steps of the method 500 are presented in the demonstrated order for ease of description and illustration. Steps can be added, omitted and/or performed simultaneously without departing from the scope of the appended claims. It should also be understood that the illustrated method 500 can be ended at any time. In certain embodiments, some or all steps of this process, and/or substantially equivalent steps are performed by execution of computer-readable instructions stored or included on a computer-readable medium. For instance, the method may be performed by the processor 106 executing instructions, comprising one or more algorithms, stored in the memory 104 of the OBC 100 of the host vehicle 102.
As shown in
At step 506, it is determined whether there is a present lane-change maneuver (i.e., a lane change has occurred or is apparently occurring) for the host vehicle 102. The vehicle 102 can determine whether a lane change is occurring or apparently imminent in any of a variety of ways, such as by evaluating an angular offset of a vehicle centerline from a lane centerline (or other points or lines, such as a lane edge).
Another way of determining whether a lane change is occurring or apparently imminent, performed independent or along with other ways (e.g., angular offset evaluation), is to evaluate a lateral offset between the vehicle and one or more lane boundaries and/or the lane centerline.
If a lane-change maneuver is not occurring and apparently not imminent in step 506, flow proceeds to step 508, where a lane is assigned to each moving radar object based on a lateral offset from the centerline of the lane of the host vehicle 102. This negative determination in step 506 leading to step 508, can be arrived at by the vehicle 102 not determining that the vehicle 102 has changed/is changing from one lane to another lane, or by the vehicle 102 actively determining that the vehicle 102 is apparently not changing lanes.
If it is determined at step 506 that a lane-change maneuver is apparently present, the step 508 of assigning a lane to the moving radar objects is not performed.
At step 510, an observation probability distribution, or vision observation model p(l|si) is computed with respect to the radar data. In some embodiments, l in this model is a quantifiable value indicating a likelihood that the vehicle 102 is in a lane. For instance, l may be represented as 0.5 or 50% if it is unknown which lane the vehicle is in and that, based on available data, there is an about 50% chance that the vehicle 102 is in a right lane of two adjacent lanes and a 50% chance that the vehicle is in a left lane of the two lanes. As another example, the value l could be 0.9, or 0.1 (depending on perspective of the algorithm), to indicate that there is a 90% chance that the vehicle 102 is in the right lane. The process 500 may end 511.
Data from the radar system (e.g., range sensor 118) identifying nearby traffic can indicate or provide helpful data indicating a lane index L for the host vehicle 102. The radar system, alone, only partially observes the state variable lane index L. Reference is made, here, to
Thus, for the scenarios of
In the scenario of
p(l=1|si=1)=0.9, p(l=2|si=1)=0.1, p(l=1|si=2)=0.1, and
p(l=2|si=2)=0.9 (2)
Similar analysis can apply to roads having more than two lanes (e.g., a three-lane, four-lane, five-lane road, etc.).
3. Update SPS Observation Model
Because the SPS directly observes state variables (except the lane index L), the SPS observation model computation can be represented rather
where c is a normalization constant, νgps corresponds to an accuracy specification of the SPS receiver, and ow, is a SPS measurements of state variables e, n, θ, and ν.
4. Update Observation Model for Map Matching
It should be understood that the steps of the method 700 are not necessarily presented in any particular order and that performance of some or all the steps in an alternative order is possible and is contemplated.
The steps of the method 700 are presented in the demonstrated order for ease of description and illustration. Steps can be added, omitted and/or performed simultaneously without departing from the scope of the appended claims. It should also be understood that the illustrated method 700 can be ended at any time. In certain embodiments, some or all steps of this process, and/or substantially equivalent steps are performed by execution of computer-readable instructions stored or included on a computer-readable medium. For instance, the method may be performed by the processor 106 executing instructions, comprising one or more algorithms, stored in the memory 104 of the OBC 100 of the host vehicle 102.
As shown in
Following determination of yaw rate and speed in step 702, two determinations are performed, substantially simultaneously or in any order. At step 704, it is considered whether an overhead structure or object, such as a bridge or tunnel, is present. At step 706, it is considered whether a lane-change maneuver is apparently present. Though overhead structures are described primarily herein, underlying structures (e.g., a road passing underneath a road of the host vehicle 102) may be considered in similarly fashion.
If at step 704, an overhead object is detected, flow proceeds to step 708, whereat a position of the overhead object is computed. The position of the overhead object is determined based on digital map data identifying the object.
At step 710, detection of the overhead object is performed, such as using SPS data (e.g., data from the SPS unit 128). For instance, many SPS receivers generate information about visible satellites (i.e., satellites in a line-of-sight from the host vehicle's view), such as number and identification of such satellites, and where and when signals from them are expected. If a satellite in line-of-sight configuration is not observed by the receiver, though, then a determination that an overhead object is present can be made.
Following the SPS-based detection of the overhead object in step 710, an observation probability distribution is computed with respect to the overhead object. The computing of step 712 is performed using the object position determined in step 708 and results of the SPS detection of step 710.
If it is determined at step 704 that an overhead object is not present, the steps 708-712 of evaluating the overhead object and computing the observation probability distribution are not performed.
If it is determined at step 706 that a lane-changing maneuver is present, flow proceeds to step 714, whereat a lane curvature is computed. The lane curvature is computed as a function of an arc length of the curve. The arc length can be discerned, for example, using SPS data regarding waypoints on the curve.
At step 716, a vehicle path curvature is computed using, at least in part, the lane curvature determined in step 714.
At step 718, an observation probability distribution for curvature match is computed. This computation is described in further detail in the following paragraphs. The observation probability distribution for curvature match of step 718 is computed based, at least in part, on the vehicle path curvature determined in step 716.
If it is determined at step 706 that a lane-changing maneuver is not apparently present, the steps 714-718 of evaluating lane curvature and computing the observation probability distribution for curvature match are not performed.
At step 720, a combined probably distribution with respect to the map p(omap|si) is computed based on the distributions resulting from steps 712 and/or 718, as the case may be. For example, if it is not determined that an overhead object is overhead or very near in step 704, and it is determined that a lane-change maneuver is apparently present in step 706, the probability distribution computation of step 720 will be based on only the distribution computed in step 718.
If, on the other hand, it is determined at step 704 that that an overhead object is present and at step 706 that the host vehicle has not changed lanes/is not changing lanes, the probability distribution computation of step 720 will be based on only the distribution computed in step 712.
If, though, it is determined that an overhead object and a lane-change maneuver are apparently present, the probability distribution computation in step 720 will be based on both of the distribution results of steps 712 and 718.
Computing the combined probably distribution with respect to the map, or observation model for map-attribute-based matching is described in further detail, here. A map attribute-based matching includes a mechanism for relatively low-cost correction of SPS longitudinal positioning using SPS blockage detection and data from vehicle-dynamics sensors (e.g., the IMU 120, the wheel sensors 122, and/or other available vehicle-dynamics sensors 124).
In some embodiments, when there is no lane-change maneuver, it is assumed that an instantaneous path curvature (yaw rate divided by vehicle speed) of the vehicle 102 is a measurement of lane curvature at the current host vehicle position. By matching the instantaneous vehicle path curvature with the curvature profile from digital map, the vehicle longitudinal offset of the vehicle 102 in the digital map can be inferred.
The graph 800 of
Also, overhead objects can be detected by a SPS blockage algorithm. If a global position of the object is known from the digital map, then the structure can be used to correct SPS measurement (e.g., longitudinal). When the vehicle 102 moves under the object, SPS signals are most often blocked from reaching the vehicle 102, and thereby the vehicle 102 determines that the vehicle 102 is under, or very close to, the object. A schematic illustration of this scenario is illustrated in
An observation model for map-attribute-based matching can be referred to as p(omap|si), and computed as:
where oB is a global location of the bridge in the digital map, κH is a curvature of vehicle path, κ(si) is a road curvature, σB and σκ are heuristic values indicating accuracy of the correction, and cB and cκ are normalization constants for probability density function.
D. Update Weight of Particles
Continuing with the method 200 for determining precise position of the vehicle to
The weight wi of the particles s1 . . . N is updated based on a fusion of the various observation models updated in the steps 212-218 of the sub-routine 210.
A probability distribution of a state vector S(t) is modeled as follows:
S(t)={si(t),i=1, . . . , N}
For each particle si, the weight wi is computed as follows:
where p(ovis|si), p(l|si), p(ogps|si), and p(omap|si) denote the observation models for vision system, radar, SPS, and map-attribute-based matching, respectively, as described above. x symbolizes the combination of each corresponding observation model measurement: ovis, l, oSPS, and omap.
E. Importance Re-Sampling and Computing Vehicle Pose
At step 222, an importance re-sampling is performed. Importance re-sampling, or importance sampling, is a statistical technique for estimating properties of a distribution using only samples of data points (or particles) generated from a different distribution.
In the re-sampling, input includes a list of particles (si, wl), and samples are randomly drawn N times, using the weight w1 as the probability to be chosen. Thus the output of the procedure is a list of new particles {s′i, i=1, . . . , N} with an
At step 224, a correct vehicle pose is computed as a mean of applicable particles. The vehicle position can be represented as follows:
The process of steps 222 and 224 may be referred to as particle filtering by which less-relevant particles, such as more extreme particles of a Gaussian distribution of particles, are less considered, or not at all, considered. The correct vehicle position computed as such represents a final estimate of vehicle pose at time index t.
The steps of the method 900 are not necessarily presented in any particular order and that performance of some or all the steps in an alternative order is possible and is contemplated.
The steps of the method 900 are presented in the demonstrated order for ease of description and illustration. Steps can be added, omitted and/or performed simultaneously without departing from the scope of the appended claims. It should also be understood that the illustrated method 900 can be ended at any time. In certain embodiments, some or all steps of this process, and/or substantially equivalent steps are performed by execution of computer-readable instructions stored or included on a computer-readable medium. For instance, the method may be performed by the processor 106 executing instructions, corresponding to one or more algorithms, stored in the memory 104 of the OBC 100 of the host vehicle 102.
The method 900 begins 901 and at step 902, the following dead-reckoning is used to predict the state variables e, n, □, and v:
s(t+1)=f(s(t),ui)
where f denotes a dead-reckoning function using a previous state vector s(t) and a control input ut (e.g., yaw rate and velocity) from vehicle-dynamics sensors, such as the IMU 120 and wheel sensors 122.
The dead-reckoning function may include constant velocity (CV) and constant turning (CT) vehicle kinematic models. In multi-lane scenarios, a special handling procedure is needed regarding a state variable L. The special handling procedure is outlined below in connection with steps 904-912 of the method 900.
For purpose of illustration with respect to the methods of the present disclosure,
Continuing with
If a lane-change maneuver for the host vehicle 102 is detected at step 904, flow proceeds to step 908, whereat the assigned lane index L is increased by one, in response to a left lane change, or the lane index L is decreased by one in response to a right lane change.
At step 910, it is considered whether there is a lane-split or lanes-merging condition. As provided,
At step 912, the lane index L for the vehicle 102 is reassigned appropriately. For example, for the lane splitting scenario of
If the vehicle 102 stays to the right-most lane in
As a lane index L reassignment regarding a lane merging scenario, the lane index L for the host vehicle 102 would be reassigned to 2 from 1 in the merging scenario of
In a hypothetical scenario, the vehicle 102 is positioned in a lane that is merging (versus being in a lane into which an adjacent lane is merging—see e.g.,
Given the distribution of state s(t) represented by the particle set S(t)={si, i=1, . . . , N} and a number of lane #, the particle representation S(t+1)={s′i, i=1, . . . , N} of the predicted distribution of state vector s(t+1) can be computed. For each particle, siεS(t), the following three steps are executed:
1. s′i=(e′i, n′i, θ′i, ν′i, L′i) and si=(ei, ni, θi, νi, Li);
2. State variables e′i, n′i, θ′i, and ν′i are computed as s′i=f(si,ui)+wi, where wi is a number randomly sampled from the process noise distribution that models the uncertainty of the dead-reckoning; and
3. State variable L′i is computed separately as:
If a lane split or merge occurs, then the value of L′i is assigned according to the rules described above.
In one embodiment, in the above step 2, a random mechanism is added so that the lane indices of a certain percentile (e.g., about 10 percent) of the particles are reassigned a value randomly drawn from a uniform distribution in set {1, . . . , #}. The method 900 may end 913.
As described above, the vision system, including for instance the camera 116, can be used to enhance dead-reckoning performance. This improvement is accomplished by tracking lane-change events as described above. Based on the observation models described in
As described regarding the method 700 of
The vehicle 102 can determine when it is first entering a curve, and then when it is exiting the curve 1202, by, for example, output of a yaw-rate sensor, such as the IMU 120. The yaw information is in some embodiments supplemented with vision data, such as data indicating locations and/or orientations of a road and/or lane edge with respect to position and pose of the vehicle 102. The vehicle 102 can determine that it is under or very close to an overhead structure by determining that SPS, or other, signals are not being received. It is also contemplated that the vehicle 102 can sense aspects of the structure (e.g., by radar and/or camera), such as supporting columns or sidewalls of a bridge or entrance features of a tunnel, for determining that the structure is overhead or very near.
The vehicle 102 corrects or adjusts its SPS position (e.g., longitudinal) and/or pose based on the characteristics of the landmarks sensed by the vehicle 102. The position correction performed with respect to overhead structures, relying heavily on vision capabilities, is especially helpful when SPS signals are blocked.
Various embodiments of the present disclosure are disclosed herein. The disclosed embodiments are merely examples that may be embodied in various and alternative forms, and combinations thereof. As used herein, for example, “exemplary,” and similar terms, refer expansively to embodiments that serve as an illustration, specimen, model or pattern. The figures are not necessarily to scale and some features may be exaggerated or minimized, such as to show details of particular components. In some instances, well-known components, systems, materials or methods have not been described in detail in order to avoid obscuring the present disclosure. Therefore, specific structural and functional details disclosed herein are not to be interpreted as limiting, but merely as a basis for the claims and as a representative basis for teaching one skilled in the art.
The law does not require and it is economically prohibitive to illustrate and teach every possible embodiment of the present claims. Hence, the above-described embodiments are merely exemplary illustrations of implementations set forth for a clear understanding of the principles of the disclosure. Variations, modifications, and combinations may be made to the above-described embodiments without departing from the scope of the claims. All such variations, modifications, and combinations are included herein by the scope of this disclosure and the following claims.
