Heading Angle Estimation for Object Tracking

Abstract

An illustrative example method of tracking an object includes: detecting points on the object over time to obtain a plurality of detections; determining a position of each of the detections; determining a relationship between the determined positions; and determining an estimated heading angle of the object based on the relationship.

Description

[0001] CROSS-REFERENCE TO RELATED APPLICATIONS

[0002] This application claims the benefit of U.S. Patent Application No. 16 / 398,745, filed on April 30, 2019, the entire disclosure of which is hereby incorporated by reference in its entirety. Background Art

[0003] Various sensor types have proven useful for detecting objects near a vehicle or in the vehicle's path. Example sensor types include ultrasound, radio detection and ranging (RADAR), and light detection and ranging (LIDAR). Such sensors are being used in increasing numbers on passenger vehicles.

[0004] One challenge associated with tracking objects using such sensors is that the objects may have varying shapes and sizes, which impairs the sensor's ability to determine certain characteristics of the object, such as the direction the object is heading. Known Kalman filters are designed to quickly provide heading angle or direction information about the tracked object. Although Kalman filter estimates are often very useful, the estimates can be inaccurate.

[0005] Kalman filters are designed to operate based on tracking the movement of a single point. Since three-dimensional objects have multiple points that can be detected by sensors, the Kalman filter can interpret detector information about multiple points as if it indicates the movement of a single point. In other words, the Kalman filter cannot distinguish between the detection of multiple different points on an object and the movement of a single point on the object. Given the possibility of interpreting different points on an object as if they were the same point, sensor information about these points can be mistaken for the movement of a single point, resulting in inaccurate tracking information. Summary of the invention

[0006] An illustrative example method of tracking an object includes: detecting one or more points on the object over time to obtain a plurality of detections; determining a position of each of the detections; determining a relationship between the determined positions; and determining an estimated heading angle of the object based on the relationship.

[0007] In an example embodiment having one or more features of the method of the preceding paragraphs, determining the spatial relationship comprises: defining a shape that encloses the determined position; and determining the estimated heading angle of the object comprises determining an orientation of the defined shape.

[0008] In an example embodiment having one or more features of the method of the preceding paragraphs, the defined shape is at least one of an arc, a line, and a rectangle.

[0009] In an example embodiment having one or more features of the method of the preceding paragraphs, determining the estimated heading angle is based on the determined orientation and range rate information associated with at least some of the detections.

[0010] In an example embodiment having one or more features of the method of the preceding paragraphs, includes detecting the one or more points at least until a distance spanned by the determined positions exceeds a preselected threshold distance.

[0011] In an example embodiment having one or more features of the method of the preceding paragraphs, includes correcting a previously estimated heading angle of the object using the determined estimated heading angle.

[0012] In an example embodiment having one or more features of the method of the preceding paragraphs, the previously estimated heading angle of the object is determined by a Kalman filter.

[0013] In an example embodiment having one or more features of the method of any of the preceding paragraphs, determining the estimated heading angle comprises using: likelihood estimation, least squares estimation, principal component analysis, or a Hough transform.

[0014] An illustrative example device for tracking an object includes: a detector that detects one or more points on the object over time; and a processor that is configured to: determine a position of each of the one or more detected points, determine a spatial relationship between the determined positions, and determine an estimated heading angle of the object based on the relationship.

[0015] In an example embodiment having one or more features of the device of the preceding paragraph, the processor is configured to: determine the spatial relationship by defining a shape that encloses the determined position; and determine an estimated heading angle of the object by determining an orientation of the defined shape.

[0016] In an example embodiment having one or more features of the apparatus of the preceding paragraph, the defined shape is at least one of an arc, a line, and a rectangle.

[0017] In an example embodiment having one or more features of the device of the preceding paragraphs, the processor is configured to: determine an estimated heading angle based on the determined orientation and range rate information associated with at least some of the detected one or more points.

[0018] In an example embodiment having one or more features of the device of the preceding paragraphs, the distance spanned by the determined positions exceeds a preselected threshold distance.

[0019] An example embodiment having one or more features of the device of the previous paragraph includes a Kalman filter that determines an initial estimated heading angle of the object, and the processor is configured to provide the determined estimated heading angle to the Kalman filter to correct the initial estimated heading angle of the object.

[0020] In an example embodiment having one or more features of the device of any of the preceding paragraphs, the processor is configured to: determine the estimated heading angle using: likelihood estimation, least squares estimation, principal component analysis, or Hough transform.

[0021] An illustrative example apparatus for tracking an object includes: a detection device for detecting one or more points on the object over time; and a determination device for determining the following: determining a position of each of the one or more detected points, determining a spatial relationship between the determined positions; and determining an estimated heading angle of the object based on the relationship.

[0022] In an example embodiment having one or more features of the apparatus of the preceding paragraphs, the spatial relationship defines a shape enclosing the determined position; and the determining means determines the estimated heading angle of the object by determining an orientation of the defined shape.

[0023] In an example embodiment having one or more features of the apparatus of the preceding paragraph, the defined shape is at least one of an arc, a line, and a rectangle.

[0024] In an example embodiment having one or more features of the method of the apparatus of the preceding paragraphs, the determining means determines the estimated heading angle based on the determined orientation and range rate information relating to at least some of the detected one or more points.

[0025] In an example embodiment having one or more features of the device of the preceding paragraphs, the distance spanned by the determined positions exceeds a preselected threshold distance.

[0026] The numerous features and advantages of at least one disclosed example embodiment will become apparent to those skilled in the art from the following detailed description.The drawings that accompany the detailed description can be briefly described as follows. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 An example use of a device for tracking an object is schematically illustrated.

[0028] Figure 2 Selected portions of an example object tracking device are schematically illustrated.

[0029] Figure 3 is a flow chart outlining an example method of tracking an object.

[0030] Figure 4 The log-likelihood function used in the example embodiment is shown graphically.

[0031] Figure 5 An angle grid used to determine an object's heading angle in an example embodiment is graphically illustrated.

[0032] Figure 6 Shown with Figure 5 The grid shown is compared to another example angle grid with a finer resolution. DETAILED DESCRIPTION

[0033] Figure 1 Diagrammatically shown is a device 20 for tracking a moving object 22. In this example, the device 20 is located on a vehicle 24. For purposes of discussion, the moving object 22 is another vehicle located in front of the vehicle 24 and at least partially in the path of the vehicle 24. The device 20 tracks the vehicle 22 using RADAR signaling as schematically shown at 26.

[0034] like Figure 1 As schematically shown in FIG. 1 , the vehicle 22 includes a center of mass 32. The vehicle 22 is traveling along a curved trajectory, such as a turn (to the right according to the figure). In such a case, Figure 1 In the illustrated case, the velocity vector of the center of mass 32 of the vehicle 22 is located at a heading angle 34. The coordinate system in which the heading angle 34 is determined may be based on a world coordinate system. Alternatively, the coordinate system may be fixed relative to the vehicle 24 or the device 20.

[0035] In this document, heading angle refers to the body orientation angle of a moving object, such as vehicle 22, that device 20 is tracking. Body orientation angle or heading angle is the azimuth direction that the centerline or longitudinal axis of the moving object points.

[0036] In this document, heading angle is the direction of motion of a particular reference point on a moving object, such as vehicle 22. It is worth noting that in some contexts, such as aviation, the term "heading angle" is used to refer to the angle referred to as "pointing angle" in this document. Similarly, in aviation contexts, the term "tracking" is used to refer to tracking, which is referred to as "heading angle" in this document.

[0037] Figure 2 Selected portions of device 20 are schematically shown including an emitter 42 and a detector 44. The emitter 42 emits radiation in an outward direction, and when such radiation reflects from an object, the reflected radiation is received and detected by the detector 44. In some example embodiments, the emitter 42 and the detector 44 operate according to known RADAR principles and techniques. Other embodiments include emitter and detector configurations that can be used for LIDAR or ultrasonic detection techniques.

[0038] The device 20 includes a filter 46 configured to estimate dynamic quantities of the tracked object, such as the position, velocity, acceleration, and trajectory curvature of the object. In some example embodiments, the filter 46 operates according to the known principles of a Kalman filter. In this example, the filter 46 provides information indicating the heading angle of at least one reference point on the moving object 22. For example, the filter 46 provides information indicating the heading angle 34 of the center of mass 32 of the vehicle 22.

[0039] The filter 46 can provide information about the heading angle of the center of mass of the moving object, however, the filter 46 cannot distinguish between multiple detection points on the object or vehicle 22. For example, in Figure 1 , detector 44 may receive reflected radiation from multiple points A, B, C, D at various locations on vehicle 22. Filter 46 cannot distinguish between information about those detected points AD and information about a single point that is detected multiple times. In other words, filter 46 will interpret information about points AD (such as the rate of change of range) as if the information indicated the movement of a single point rather than indicating the movement of multiple different points. When filter 46 operates as a Kalman filter, it treats all detected point information as if it were related to a single point.

[0040] Device 20 includes processor 50, which may be a dedicated microprocessor or part of another computing device supported on vehicle 24. In this embodiment, processor 50 is configured to determine an estimated heading angle based on position information about one or more detection points AD on object 22 over time.

[0041] Memory 52 is associated with processor 50. In some example embodiments, memory 52 includes computer executable instructions that cause processor 50 to operate for the purpose of tracking a moving object and determining a pointing angle or body orientation angle of the object. In some example embodiments, memory 52 at least temporarily contains information about various features or characteristics of detection points on tracked object 22 to assist processor 50 in making desired determinations about the position or movement of such an object.

[0042] Example embodiments of the present invention allow for more accurate determination of the heading angle of a moving object such as vehicle 22. This feature is particularly useful at the beginning of a tracking session when filter 46 has relatively limited information for determining the heading angle. Providing a heading angle estimate to filter 46 by processor 50 improves object tracking. Thus, embodiments of the present invention provide improvements in tracking techniques and improvements in vehicle control based on information regarding moving objects near or in the path of the vehicle.

[0043] In some embodiments, filter 46 operates to provide an indication of the presence of an object even if the heading angle has not yet been defined. This approach allows for early object detection and subsequent determination or refinement of the heading angle.

[0044] Figure 3 60, which summarizes an example method of tracking an object 22, including using position information about one or more points AD on the tracked object to estimate a heading angle. At 62, the detector 44 detects radiation reflected from one or more points on the object 22 over time. Over time, multiple detections provide information about the movement of the object 22 as the object 22 moves. At 64, the processor 50 determines the position of each of the detections, which corresponds to the corresponding position of the points AD detected over time. In this example, the position information is determined in a world coordinate system.

[0045] At 66, processor 50 determines a relationship between the positions determined at 64. The relationship in this example includes the detected relative positions over an area defining a shape. For example, the determined relationship defines an arc, line, or rectangle that surrounds or includes the determined positions. The processor determines the orientation of the shape within the world coordinate system and uses the orientation to determine an estimated heading angle of the object at 68. Thus, the determination of the heading angle is based on the relationship determined at 66.

[0046] In some cases, the orientation of the shape defined by the relationship determined at 66 will indicate a path that the tracked object has moved, but not the direction of movement along that path. In this example, processor 50 uses range rate information about a plurality of detections to determine the direction of movement.

[0047] At 70, processor 50 provides the determined estimated heading angle to filter 46. Filter 46 will update the previous heading angle determined by filter 46 based on the estimated heading angle from processor 50. In some examples, filter 46 will replace the previously determined heading angle with the estimated heading angle from processor 50.

[0048] Having enough detections over time allows processor 50 to make a more accurate estimate of the heading angle. In an example embodiment, the detected positions span a distance that exceeds a preselected threshold. Having a sufficiently large distance between the farthest detected positions increases the likelihood that object 22 has moved sufficiently for the position information determined by processor 50 to accurately estimate the heading angle. Given this description, those skilled in the art will be able to select an appropriate threshold distance to meet the needs of their particular situation.

[0049] The processor determines the estimated heading angle using one of several techniques. In one embodiment, the likelihood estimate provides the most likely heading angle based on the detected position.

[0050] Assume that in a world coordinate system there exists a N ] and y=[y1,...,y N ]. The log-likelihood l(x,y|θ) of a detection with heading angle θ roughly describes how well the detection trace can fit the trajectory with heading angle θ. This embodiment includes the assumption that each detection has a uniform likelihood of being inside object 22. Due to angular errors in the detections, the likelihood of the detection position being outside object 22 decreases to zero as the detection position moves further away from object 22.

[0051] This example includes defining the body coordinate system of the object so that its longitudinal axis is parallel to the object heading angle 34 and its origin is the same as the origin of the world coordinates. t ,y t ), then the position of its center of mass in the object's body coordinate system is:

[0052]

[0053] Detect in the object's body coordinate system (x i ,y i ) is located at:

[0054]

[0055] The log-likelihood of a single test l(x t ,y t |θ) can be adequately represented by detecting the orthogonal position relative to the object's center of mass, which can be expressed as:

[0056]

[0057] Figure 4 Shown as O' i An example of the log-likelihood of detection as a function of . The width W of the cones on the left and right sides of graph 74 may depend on the object pose, object range, and the nominal standard deviation of the angular error in the detection. It follows

[0058] W=W min +|cos β|(max(r,r min )-r min )

[0059] Where β is the object heading angle relative to the host vehicle 24, r is the object range, and e is the nominal standard deviation of the angular error in the detection. Parameter W min and r min It is scheduled.

[0060] Assuming independence between multiple tests, the overall log likelihood can be written as:

[0061]

[0062] In this example, the total likelihood is determined by processor 50 as the sum of the individual likelihoods normalized by the number of detections. In an example embodiment, if the longitudinal span of the shape determined according to the relationship determined at 66 is less than a threshold distance, the total likelihood will take a low value. Detections close to the current object position are preferred over detections further away to account for any manipulation of object 22 at longer distances. In addition, processor 50 assigns a low value to the total likelihood when too few detections over time are available.

[0063] The maximum likelihood estimate of the heading angle can be obtained by solving the following equation:

[0064]

[0065] This optimization equation cannot be solved analytically. To solve this problem numerically, an example embodiment includes a two-step angle search to find the optimal heading angle π. Note that since the detection likelihood at heading angles θ and θ+π is the same, it is sufficient to search inside θ∈[0,π].

[0066] Figure 5 and Figure 6 A two-step angle search is presented. First, the corresponding detection likelihood is evaluated on a coarse angle grid, as Figure 5 If none of the likelihoods exceed the preselected threshold at 80, the processor 50 will stop the heading angle determination due to insufficient information in the detection. When at least one of the detections meets the threshold 80, the processor selects the angle points with a likelihood above the threshold 80 and builds a fine angle grid covering the range of these angle points, as shown in FIG. Figure 6 As shown in Figure 5 The heading angles 0, 0.8π, and 0.9π in each exceed the threshold 80, so the processor uses these angles as Figure 6 Angles of interest in the fine grid. Figure 5 The resolution in is π / 10, and Figure 6 The resolution in is π / 60.

[0067] As can be understood from this illustration, the processor 50 evaluates a plurality of candidate angles around angles 0, 0.8π, and 0.9π. In this example, the best heading angle that maximizes the likelihood is selected among those candidates as the estimated heading angle π.

[0068] The foregoing description is exemplary rather than limiting in nature. Variations and modifications to the disclosed examples may become apparent to those skilled in the art that do not necessarily depart from the essence of the invention. The scope of legal protection given to this invention can only be determined by studying the following claims.

Claims

1. A method for tracking an object, the method comprising: detecting one or more points on the object over time to obtain a plurality of detections; determining a position of each of said detections relative to a two-dimensional world coordinate system; determining a spatial relationship between the determined locations to define a shape surrounding the determined locations; as well as An estimated heading angle of the object is determined based on the spatial relationship, the estimated heading angle defining a direction of motion of a center of mass of the moving object relative to the host vehicle, the estimated heading angle of the object being determined by at least: For each of the multiple candidate heading angles: defining a body coordinate system of the object for the detection, the body coordinate system of the object having the same origin as the two-dimensional world coordinate system and a longitudinal axis parallel to the corresponding candidate heading angle of the object; for each of the detections, determining a log-likelihood of the corresponding detection for the corresponding candidate heading angle based on an orthogonal position of the corresponding detection in a body coordinate system of the object relative to the center of mass; determining an overall log-likelihood for the corresponding candidate heading angle based on the log-likelihood of the detection determined for the corresponding candidate heading angle; as well as The estimated heading angle is selected from the candidate heading angles based on their respective overall log-likelihoods.

2. The method according to claim 1, characterized in that The defined shape is at least one of an arc, a line, and a rectangle.

3. The method according to claim 1, characterized in that include: The one or more points are detected at least until a distance spanned by the determined positions exceeds a preselected threshold distance.

4. The method according to claim 1, characterized in that: include: The determined estimated heading angle is used to correct a previously estimated heading angle of the object.

5. The method according to claim 4, characterized in that The previously estimated heading angle of the object is determined by a Kalman filter.

6. The method according to claim 1, characterized in that Determining the estimated heading angle includes using: Likelihood estimation, Least squares estimation, Principal component analysis, or Hough transform.

7. A device for tracking an object, the device comprising: a detector that detects one or more points on the object over time; as well as A processor configured to: determining a position of each of the detected one or more points relative to a two-dimensional world coordinate system; determining a spatial relationship between the determined locations to define a shape surrounding the determined locations; as well as An estimated heading angle of the object is determined based on the spatial relationship, the estimated heading angle defining a direction of motion of a center of mass of the moving object relative to the host vehicle, the estimated heading angle of the object being determined based on at least: For each of the multiple candidate heading angles: a body coordinate system of the object defined by the detection, the body coordinate system of the object having the same origin as the two-dimensional world coordinate system and a longitudinal axis parallel to the corresponding candidate heading angle of the object; determining, for each of the detections, a log-likelihood of the corresponding detection for the corresponding candidate heading angle based on an orthogonal position of the corresponding detection in a body coordinate system of the object relative to the center of mass; determining an overall log-likelihood of the respective candidate heading angles based on the log-likelihoods of the detections determined for the respective candidate heading angles; as well as The estimated heading angle is selected from among the candidate heading angles based on their respective overall log-likelihoods.

8. The device according to claim 7, characterized in that The defined shape is at least one of an arc, a line, and a rectangle.

9. The device according to claim 7, characterized in that The distance spanned by the determined positions exceeds a preselected threshold distance.

10. The device according to claim 7, characterized in that A Kalman filter is included that determines an initial estimated heading angle of the object, and wherein the processor is configured to provide the determined estimated heading angle to the Kalman filter to correct the initial estimated heading angle of the object.

11. The device according to claim 7, characterized in that The processor is configured to determine the estimated heading angle including using: Likelihood estimation, Least squares estimation, Principal component analysis, or Hough transform.

12. A device for tracking an object, the device comprising: detection means for detecting one or more points on the object over time; as well as Determining means for determining: determining a position of each of the detected one or more points relative to a two-dimensional world coordinate system; determining a spatial relationship between the determined locations to define a shape surrounding the determined locations; as well as An estimated heading angle of the object is determined based on the spatial relationship, the estimated heading angle defining a direction of motion of a center of mass of the moving object relative to the host vehicle, the estimated heading angle of the object being determined based on at least: For each of the multiple candidate heading angles: a body coordinate system of the object defined by the detection, the body coordinate system of the object having the same origin as the two-dimensional world coordinate system and a longitudinal axis parallel to the corresponding candidate heading angle of the object; determining, for each of the detections, a log-likelihood of the corresponding detection for the corresponding candidate heading angle based on an orthogonal position of the corresponding detection in a body coordinate system of the object relative to the center of mass; determining an overall log-likelihood of the respective candidate heading angles based on the log-likelihoods of the detections determined for the respective candidate heading angles; as well as The estimated heading angle is selected from among the candidate heading angles based on their respective overall log-likelihoods.

13. The device according to claim 12, characterized in that The defined shape is at least one of an arc, a line, and a rectangle.

14. The device according to claim 12, characterized in that The distance spanned by the determined positions exceeds a preselected threshold distance.

Images