Method and device of determining kinematics of a target

Abstract

A method for determining kinematics of a target includes generating a measured trace of a target position, speed, and angle of a target relative to an ego vehicle. The radius of the curve that the target is taking is calculated, using the generated trace. A number of paths possible to be followed by the target are projected, expressed as abstract movement functions of a polynomial degree and using as input the calculated radius. At each further cycle, higher probabilities are added to the paths projected in a previous measuring cycle which are equal with the newly projected ones. The projected paths are kept if the radius remains the same as in the previous measuring cycle. The current kinematics values are computed by smoothing filtering as predicted kinematics values, which are compared with the kinematics values resulted from the projected paths, to determine the final kinematics values.

Claims

1. A method of determining kinematics of a target in relation to an ego vehicle, the method comprising: selecting the target detected in a predetermined sensing zone; generating a trace of the target based on position, heading, speed, and acceleration of the target; predicting a curve the target is taking and calculating a radius of the curve using the generated trace; projecting n paths created as abstract movement functions of a polynomial degree p depending on the calculated radius, in response to the trace having at least 2p−1 historical points; at each further cycle, adding higher probabilities to paths projected in a previous measuring cycle which are equal with the newly projected ones and keeping the projected paths, if the radius remains the same as in the previous measuring cycle; computing predicted kinematics values by utilizing a smoothing filter with current kinematics values; comparing the predicted kinematics values with the kinematics values derived from the projected paths; and determining final kinematics values based on the comparison of predicted kinematics values with the kinematics values derived from the projected paths; wherein determining the final kinematics values comprises if the standard deviation of the kinematics values associated to the projected paths is close to the standard deviation of the kinematics values associated to the predicted paths, then the kinematic parameters are those associated to the projected paths, otherwise use the kinematics values associated to the predicted paths.

2. The method according to claim 1, further comprising defining the abstract movement functions as Markov-chain matrices.

3. The method according to claim 1, wherein the radius of the curve taken by target is obtained by polynomial regression using the generated trace.

4. The method according to claim 1, wherein the smoothing filter used to compute the kinematics values associated to predicted paths is a Kalman filter.

5. The method according to claim 1, wherein the smoothing filter used to compute the kinematics values associated to predicted paths is a low-pass filter.

6. A device for determining a kinematic of a target, comprising: an interface adapted to receive sensor data related to the target, as well as ego vehicle kinematics data such as heading, position, speed and acceleration of the target; an evaluation unit adapted to predict a curve the target is taking and calculate a radius of the curve using the generated trace, project n target paths expressed as abstract movement functions of a polynomial degree p depending on the calculated radius, after having a trace of at least 2p−1 historical points; at each further cycle, to add higher probabilities to the paths projected in a previous measuring cycle which are equal with the newly projected ones and to keep the projected paths, if the radius remains the same as in the previous measuring cycle, compute current kinematics values using a smoothing algorithm, as predicted kinematics values, compare the predicted kinematics values with the kinematics values resulted from the projected paths, and determine final kinematics values; wherein determining the final kinematics values comprises if the standard deviation of the kinematics values associated to the projected paths is close to the standard deviation of the kinematics values associated to the predicted paths, then the kinematic parameters are those associated to the projected paths, otherwise use the kinematics values associated to the predicted paths.

7. The device according to claim 6, wherein the evaluation unit is adapted to define the abstract movement functions as Markov-chain matrices.

8. The device according to claim 7, wherein the evaluation unit is adapted to output the determined kinematics data to a driving assistance system, as to assist functions including at least one of blind spot detection, lane change support, front traffic cross alert, and rear traffic cross alert.

9. A vehicle comprising: a sensor system adapted to provide sensor data of surroundings of the vehicle; a device for determining a kinematic of a target including an interface adapted to receive sensor data related to the target, as well as ego vehicle kinematics data such as heading, position, speed, and acceleration of the target; an evaluation unit adapted to predict a curve the target is taking and calculate a radius of the curve using the generated trace, project n target paths expressed as abstract movement functions of a polynomial degree p depending on the calculated radius, after having a trace of at least 2p−1 historical points; at each further cycle, to add higher probabilities to the paths projected in a previous measuring cycle which are equal with the newly projected ones and to keep the projected paths, if the radius remains the same as in the previous measuring cycle, compute current kinematics values using a smoothing algorithm, as predicted kinematics values, compare the predicted kinematics values with the kinematics values resulted from the projected paths, and determine final kinematics values; wherein determining the final kinematics values comprises if the standard deviation of the kinematics values associated to the projected paths is close to the standard deviation of the kinematics values associated to the predicted paths, then the kinematic parameters are those associated to the projected paths, otherwise use the kinematics values associated to the predicted paths; and a driving assistance system configured to receive signals from the device.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) Other advantages of the disclosed subject matter will be readily appreciated, as the same becomes better understood by reference to the following detailed description when considered in connection with the accompanying drawings wherein:

(2) FIG. 1 is a schematic block diagram of a device of determining a kinematic of a target vehicle, according to an exemplary embodiment;

(3) FIG. 2 is a schematic block diagram of a vehicle equipped with a device according to an exemplary embodiment;

(4) FIGS. 3a-3d are an illustrative example of a first road scenario with a target vehicle going straight with a constant speed at a certain distance behind an ego vehicle, and how measurement errors could influence the detection the target vehicle in the blind spot detection zone against the actual position;

(5) FIG. 4 is an illustration of possible maneuvers to be followed by a vehicle while running on a highway;

(6) FIG. 5 is an example of actual position of straight-going target vehicle, calculated according to one exemplary embodiment of the method;

(7) FIGS. 6a-6d are examples of actual heading and position of a target vehicle changing lanes, calculated according to the method, in comparison with the heading and positions computed by using a Kalman-filter; and

(8) FIG. 7 is a flowchart of a method of determining a kinematic of a target, according to one exemplary embodiment.

DETAILED DESCRIPTION

(9) FIG. 1 shows a schematic block diagram of a device 1 for determining a kinematic of a target T. Target T may be, a motorized traffic participant (described as a rectangle), such as a motor vehicle, a motorcycle/bicycle, or the like.

(10) A schematic view of an ego vehicle E equipped with such a device 1 and with a sensing system 2, as well as a driving assistance system 5 is illustrated in FIG. 2.

(11) The device 1 comprises an interface 4 which is adapted to receive input data from any active form of remote sensing, such as radar, lidar, optic, ultrasonic or infrared sensors, for example, as well as GPS sensors. The received input data are related to a target external to said ego vehicle E, the target T being present in a predetermined scanning zone of the sensing system 2. In the illustrated examples, the traffic participant designated as target T in this description is a vehicle whose direction (angle) of movement over ground has been detected and history of measurement positions has been generated as a trace.

(12) The device 1 includes an evaluation unit 3 adapted to use input data extracted from the trace of path and positions, and to further determine a kinematic of target T according to the method which is described in detail in the following section.

(13) In order to illustrate the way target vehicle T is perceived by sensing systems 2, FIG. 3a shows an actual position of target vehicle T at the limit of the blind spot detection zone of ego vehicle E, for example, while FIG. 3b-3d show how the measurement errors could influence the detection of target vehicle T in or out the blind detection zone. In this first road scenario, target vehicle T is going straight with constant speed at a certain distance behind ego vehicle E, so there are absolute no fluctuations of kinematic parameters from one cycle to another, even the position of target vehicle T relative to ego vehicle E remains the same. However, due to the measurement noise, a current algorithm (Kalman filter, for example) will compute slight deviations in heading, position, speed, acceleration and so on. Even small fluctuations are enough to wrongly put target vehicle T in the blind spot detection zone, for example (see FIG. 3b, 3d), and give an alarm.

(14) It is of interest, for the purpose of this disclosure, to discuss the cases requiring to map the kinematic parameters to a set of functions which reflect the reality. In reality there is noise in the motion of a vehicle; while driving, in 99% of the cases, the usual situation is that only simple movements are made, having a constant acceleration and a constant direction change. If these movements are expressed by mathematical functions, the exact movement of a target vehicle can be predicted and analyzed.

(15) The concept consists into projecting at each cycle a set of paths expressed by movement functions of a polynomial degree n (which describe every kinematic parameter and make a Markov-chain model) from the trace and current parameters of target T and choosing the movement function which defines most accurately the actual path of the target, by comparing the projected paths with a path predicted by using existing smoothing algorithms.

(16) In case none of the projected paths match with the predicted ones, the updated kinematics values from the smoothing algorithms will represent an unusual situation and thus the output data will be the one resulting from the smoothing algorithm. In the end, the goal is to reach a simplified solution where, using a minimal set of parameters, all the other variables/kinematics values to be calculated.

(17) The set of functions which define the Markov-chain models associated to the kinematic of a vehicle (heading, acceleration, speed and position) comes as follow for a certain maneuver, and period of time t:

(18) $\begin{array}{cc}\{\begin{array}{c}\Phi \left(t\right)\\ a_{abs}\left(t\right)\\ a_x\left(t\right)=a_{abs}\left(t\right)*\cos \left(\Phi \left(t\right)\right)\\ a_y\left(t\right)=a_{abs}\left(t\right)*\cos \left(\Phi \left(t\right)\right)\\ v_{abs}\left(t\right)=v_{abs}\left(t_0\right)+v_{abs}\left(t-t_0\right)\\ v_x\left(t\right)=v_{abs}\left(t\right)*\sin \left(\Phi \left(t\right)\right)=v_x\left(t_0\right)+a_x\left(t-t_0\right)\\ v_y\left(t\right)=v_{abs}\left(t\right)*\cos \left(\Phi \left(t\right)\right)=v_y\left(t_0\right)+a_y\left(t-t_0\right)\\ x\left(t\right)=x\left(t_0\right)+v_x\left(t_0\right)*\left(t-t_0\right)+a_x\left(t\right)*\frac{{\left(t-t_0\right)}^2}{2}\\ y\left(t\right)=y\left(t_0\right)+v_y\left(t_0\right)*\left(t-t_0\right)+a_y\left(t\right)*\frac{{\left(t-t_0\right)}^2}{2}\end{array},& \left(1\right)\end{array}$
where Φ(t), a.sub.abs(t) are independent functions describing heading and acceleration in absolute coordinates, and

(19) $\begin{array}{cc}\{\begin{array}{c}\Phi \left(t\right)={\Phi }_0,\mathrm{constant}\mathrm{heading}\mathrm{before}\mathrm{maneuver},t<t_1\\ \Phi \left(t\right)={\Phi }_0+\alpha *t,\mathrm{first}\mathrm{part}\mathrm{of}\mathrm{maneuver},t<t_2\\ \Phi \left(t\right)=\Phi \left(t_2\right)+\beta *t,\mathrm{first}\mathrm{part}\mathrm{of}\mathrm{maneuver},t<t_3\\ \Phi \left(t\right)={\Phi }_f,\mathrm{constant}\mathrm{heading}\mathrm{after}\mathrm{maneuver},t>t_3\end{array}.& \left(2\right)\end{array}$

(20) Next comes the simplified model for acceleration, describing a normal behavior, considering how an acceleration pedal is pressed over time, and how the pedal movement modifies the speed of the vehicle—almost linear changes.

(21) $\begin{array}{cc}\{\begin{array}{c}{a}_{abs}\left(t\right)=0,\mathrm{no}\mathrm{acceleration},t<t_1\\ a_{abs}\left(t\right)=\alpha *t,\mathrm{constantly}\mathrm{accelerating}/\mathrm{decelerating},t<t_2\\ a_{abs}\left(t\right)=a_{absFinal}\left(t\right),\mathrm{constant}\mathrm{acceleration},t<t_3\\ a_{abs}\left(t\right)=\alpha *t,\mathrm{constantly}\mathrm{acccelerating}/\mathrm{decelerating},t<t_4\\ a_{abs}\left(t\right)=0,\mathrm{no}\mathrm{acceleration},t>t_4\end{array}.& \left(3\right)\end{array}$

(22) The set of functions serialized, to be used in case of pass-by-pass/frame-to-frame computation looks like that:

(23) $\begin{array}{cc}\{\begin{array}{c}\Phi \left(t_n\right)\\ a_{abs}\left(t_n\right)\\ a_x\left(t_n\right)=a_{abs}\left(t_n\right)*\cos \left(\Phi \left(t_n\right)\right)\\ a_y\left(t_n\right)=a_{abs}\left(t_n\right)*\cos \left(\Phi \left(t_n\right)\right)\\ v_{abs}\left(t_n\right)=v_{abs}\left(t_{n-1}\right)+\left(a_{abs}\left(t_n\right)-a_{abs}\left(t_{n-1}\right)\right)*\left(t_n-t_{n-1}\right)\\ v_x\left(t_n\right)=v_{abs}\left(t_n\right)*\sin \left(\Phi \left(t_n\right)\right)\\ v_y\left(t_n\right)=v_{abs}\left(t_n\right)*\cos \left(\Phi \left(t_n\right)\right)\\ x\left(t_n\right)=x\left(t_{n-1}\right)+\left(a_{abs}\left(t_n\right)-a_{abs}\left(t_{n-1}\right)\right)*\frac{{\left(t_n-t_{n-1}\right)}^2}{2}\\ y\left(t_n\right)=y\left(t_{n-1}\right)+\left(a_{abs}\left(t_n\right)-a_{abs}\left(t_{n-1}\right)\right)*\frac{{\left(t_n-t_{n-1}\right)}^2}{2}\end{array}.& \left(4\right)\end{array}$

(24) For every type of maneuver only the definition of the absolute heading functions differs. This heading function is only a simple first degree polynomial. It is based on observation and mapping of how a normal maneuver is done by a vehicle. For a better understanding of how this normal maneuvers are mathematically described, FIG. 4 shows five possible paths, expressed as functions F1, . . . , F5, respectively.

(25) Functions F1 and F5 describing constant turning to left/right are expressed by the following Markov-chain:

(26) $\begin{array}{cc}\{\begin{array}{c}\Phi \left(t\right)=0,\mathrm{before}\mathrm{maneuver},t<t_1\\ \Phi \left(t\right)=\alpha *t=\frac{v_{abs}\left(t\right)}{R}*t,\mathrm{where}R\mathrm{is}\mathrm{the}\mathrm{curve}\mathrm{radius},t<t_2\\ \Phi \left(t\right)={\Phi }_f,\mathrm{constant}\mathrm{heading}\mathrm{after}\mathrm{maneuver},t>t_3\end{array}.& \left(5\right)\end{array}$

(27) Functions F2 and F4 describing lane changing are expressed by the following Markov-chain:

(28) $\begin{array}{cc}\{\begin{array}{c}\Phi \left(t\right)={\Phi }_0,\mathrm{before}\mathrm{maneuver},t<t_1\\ \Phi \left(t\right)={\Phi }_0+\alpha *t={\Phi }_0+\frac{v_{absM}\left(t\right)}{R}*t,R\mathrm{is}\mathrm{the}\mathrm{curve}\mathrm{radius},\\ t<t_2,t<R*\arccos \frac{\left(1\frac{d_{\mathrm{lane}}}{2*R}\right)}{v_{absM}\left(t\right)}\\ \Phi \left(t\right)=2*{\Phi }_{\max }-\alpha *t=2*{\Phi }_{\max }-\frac{v_{absM}\left(t\right)}{R}*t,t<t_3,\\ {\Phi }_{\max }=\arccos \left(1-\frac{d_{\mathrm{lane}}}{2*R}\right)\\ \Phi \left(t\right)={\Phi }_0,\mathrm{constant}\mathrm{heading}\mathrm{after}\mathrm{maneuver},\\ t>t_3=2*R*\arccos \frac{\left(1\frac{d_{\mathrm{lane}}}{2*R}\right)}{v_{absM}\left(t\right)}\end{array}.& \left(6\right)\end{array}$

(29) Function 3 describing straight heading is described by the following equation:
Φ(t)=0  (7).

(30) An exemplary embodiment of the method of determining a kinematic of a target, applied for highway driving scenarios, is represented in FIG. 7. The method follows includes the steps: S1: Select a target T when it is detected in the predetermined sensing zone; S2: Generate a trace of the target based on position, heading, speed and acceleration; S3: Predict a curve the target is taking and calculating a radius of the curve using the generated trace; S4: Project n possible paths of target T as calculated functions (n=5 in case of highway—see FIG. 4) depending on the calculated radius and compute the associated kinematics values; S4.0: once the target is selected and a trace of at least five historical points is detected, initialize the n projected paths as calculated functions; S4.1: at each further cycle keep projecting the n paths as functions depending on the calculated radius; look at the previously projected paths and add higher probabilities to those which are equal with the newly projected ones; S4.2: If the radius is the same as in the previous cycle, then target T is on one of the projected paths; S5: Compute current kinematics values using an existing, usual algorithm (Kalman filter, low pass filter etc.); S6: Compare the kinematics values calculated by the existing, usual algorithm with the kinematics values resulted from the projected paths from step S4; S7: Determine the final kinematics values: S7.1. If the standard deviation of the kinematics values associated to projected paths is close to the standard deviation of the kinematics values associated to predicted paths, then the kinematic parameters will be equal to the output of the projected path (as function)—eliminating all the noise; S7.2. If the deviation of any of the paths is high, then use the kinematics values from the usual algorithm.

(31) FIG. 5 shows an exemplary embodiment on how the method of determining a kinematic of a target computes the actual position of a straight-going target vehicle T on a two lane highway, a right lane and a left lane, respectively, in comparison with the position predicted by using Kalman-filter in global coordinates. Target vehicle T is running on the right lane, while ego vehicle E is running on the left lane. There are illustrated (see FIG. 3b-3d) three possible positions of target T behind ego vehicle E at the limit of the blind spot detection zone of ego vehicle E (meaning 12.1 m in axial direction and 3 m in transversal direction), as perceived according to the sensor data after Kalman filtering. Target vehicle T position as predicted by Kalman filter is illustrated by a rectangle Tk comprising the afferent cluster of points joined by lines (FIG. 5), while the actual position of target vehicle T, computed correctly after matching with afferent movement functions, is shown by a dotted rectangle Tf situated in the middle of the right lane. F1 is the function expressing the straight heading path of target vehicle T, F2 is the function expressing the lane changing path of target vehicle T, by turning left.

(32) Target vehicle T is considered as having constant speed behind the ego vehicle E, the same speed as ego vehicle E. In this case, there are absolutely no fluctuations of kinematic parameters from one cycle to another, even the position of target vehicle T relative to ego vehicle E remains the same. The radius as extracted from the trace is calculated as being almost zero. By using polynomial regression, speed and acceleration are determined as constant as well, so there is no acceleration, only constant speed. A number of n paths which could be followed by target vehicle T as normal maneuvers are created as abstract movement functions (in this case two, namely F1—heading straight ahead and F2—lane changing), and the associated kinematics values are computed. Also, current values of kinematics are computed using the existing, normal algorithms (Kalman filter, low pass filter etc.) and compared with the kinematics values associated to the respective functions (projected paths). In the end, the chosen path will be the one of constant absolute heading, since it have the highest probability to be followed by target vehicle T. In this manner, speed, position, heading, acceleration is computed 100% correctly and accurately, eliminating all the noise and, most important, no warning is triggered.

(33) Another exemplary embodiment of how the method of determining the kinematic of a target vehicle T, according to the invention, applies for a specific road scenario is shown in FIG. 6a-6d. The road scenario concerned in this case is that of lane changing from a current position of target vehicle T running behind ego vehicle E on the same lane (left lane, to be specific).

(34) In this case, in the first cycle, as the Kalman filter predicts target vehicle T is going straight, the straight path is chosen as projected path; however, starting with the next cycle the lane changing maneuver is detected, and from the existing data the radius of the followed curve is determined. For the next few cycles target vehicle T is exactly on the predicted path which is the lane changing path (expressed as F2, as it is more important than the combination of left/right curve path expressing the overtaking maneuver), and as soon as target vehicle T finishes the maneuver, the path is computed correctly on the next lane, according to the method.

(35) While certain embodiments of the present invention have been described in detail, those familiar with the art to which this invention relates will recognize various alternative designs and embodiments for practicing the invention as defined by the following claims.