System and method for stochastically predicting the future states of a vehicle

• US 8,489,317 B2
• Filed: 08/29/2008
• Issued: 07/16/2013
• Est. Priority Date: 08/29/2008
• Status: Active Grant

• Current Assignee: Toyota Motor Corporation
• Original Assignee: Toyota Motor Engineering & Manufacturing North America Incorporated (Toyota Motor Corporation)
• Inventors: Caveney, Derek Stanley, Lovell, Jeffrey Clark, Caminiti, Lorenzo, Frantz, Andrew James
• Primary Examiner(s): Fuelling, Michael
• Application Number:
  US12/201,884
• Publication Number:
  US 20100057361A1
• Time in Patent Office: 1,782 Days
• Field of Search: 701/301
• US Class Current: 701/301
• CPC Class Codes: B60W 2050/0031 Mathematical model of the vehicle
• CPC Class Codes: B60W 2520/10 Longitudinal speed
• CPC Class Codes: B60W 2520/14 Yaw
• CPC Class Codes: B60W 2520/28 Wheel speed
• CPC Class Codes: B60W 2540/18 Steering angle
• CPC Class Codes: B60W 2556/50 of positioning data, e.g. GPS [Global Positioning System] data
• CPC Class Codes: B60W 30/08 Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
• CPC Class Codes: B60W 50/0097 Predicting future conditions
• CPC Class Codes: G08G 1/161 Decentralised systems, e.g. inter-vehicle communication

Abstract

A method for predicting future states of a vehicle including the steps of selecting a model having n states reflecting dynamic features of the vehicle; inputting noisy sensor measurements representing a current state of the vehicle to generate (2n+1) sigma points X_i where i=0, . . . . 2n, each of the sigma points having n states; performing (2n+1) integrations, each integration includes propagating the n-states of the respective sigma points X_i through the non-linear function Y_i=f(X_i); and combining the propagated sigma points to generate the predicted future states of the vehicle.

Claims

1. A method for predicting future states of a vehicle by combining an unscented transform (UT) with numerical integration (NI), the method comprising:

selecting a model, by one or more processors, having n states reflecting dynamic features of the vehicle;

inputting, as the UT, noisy sensor measurements representing a current state of the vehicle, by one or more processors, to generate (2n+1) sigma points X_i, where i=0, . . . , 2n, each of the sigma points having n states;

performing, as the NI, (2n+1) numerical integrations by one or more processors, each integration including propagating the n-states of the respective sigma points X_i through the non-linear function Y_i=f(X_i); and

combining, by one or more processors, the propagated sigma points to generate the predicted future states of the vehicle, wherein

the predicted future states of the vehicle are generated only when there is no measurement update or process noise so that an unscented transform is applied only to a stochastic state.

2. The method of claim 1, wherein the input sensor measurements include velocity, yaw-rate, and acceleration.

3. The method of claim 1, further comprising the step of determining a position and orientation of the vehicle using at least one of a Global Positioning System and an inertial positioning system, wherein the determined position and orientation of the vehicle further represent the current state of the vehicle.

4. The method of claim 1, further comprising the step of updating the sensor measurements, wherein the propagating step is repeated each time the sensor measurements are updated.

5. The method of claim 1, further comprising the step of representing the predicted states in a time or space based parameterization.

6. The method of claim 1, further comprising:

determining a value reflecting an uncertainty of the predicted future states.

7. The method of claim 1, wherein the predicted futures states define a predicted patch of the vehicle, the method further comprising:

sharing the predicted path of the vehicle, referred to as a first vehicle, with one or more other vehicles, referred to as a second vehicle or second vehicles;

comparing the predicted path of the first vehicle with a predicted path of one or more of the second vehicles;

determining whether the predicted paths at a same instance in time overlap; and

controlling the first vehicle to take avoidance measures.

8. The method of claim 1, wherein:

X_i is an n-dimensional random variable with a mean X and a covariance P_X;

X₀= X

X_i= X+(√

{square root over ((n+λ

)P_X)})_i, i=1, . . . , n

X_i= X−

(√

{square root over ((n+λ

)P_X)})<sub>i−

n</sub>, i=n+1, . . . , 2n

(√

{square root over ((n+λ

)P_X)})_i is an i-th row or column of a matrix square root; and

λ

is a scaling parameter.

9. The method of claim 8, wherein:

a mean Y and covariance P_Y are approximated by a weighted sample mean and covariance of propagated sigma points Y_i;

10. The method of claim 1, wherein the NI is performed with the Runge-Kutta-Fehlberg (RFK) equations.

11. The method of claim 1, wherein the propagating the n-states of the respective sigma points X_i through the non-linear function Y_i=f(X_i) includes propagating, in parallel, individual sigma points by parallel-processing at a same time.

12. A system for predicting future states of a vehicle by combining an unscented transform (UT) with numerical integration (NI), the system comprising:

a first plurality of sensors, each sensor configured to output a performance measurement of the vehicle representative of a current state of the vehicle; and

a processor configured to predict future states of the vehicle based on the sensed performance measurements, the prediction calculated by the processor by generating, as the UT, (2n+1) sigma points X_i, where i=0, . . . , 2n, each of the sigma points having n states, performing, as the NI, (2n+1) numerical integrations, each integration including propagating the n-states of the respective sigma points X_i through the non-linear function Y_i=f(X_i), and combining the propagated sigma points to generate the predicted future states of the vehicle, wherein

the predicted future states of the vehicle are generated only when there is no measurement update or process noise so that an unscented transformation transform is applied only to a stochastic state.

13. The system of claim 12, wherein the plurality of sensors provided on the vehicle sense the velocity, yaw-rate, and acceleration of the vehicle.

14. The system of claim 12, further comprising means for determining a position and orientation of the vehicle, wherein the determined position and orientation of the vehicle further represent the current state of the vehicle.

15. The system of claim 12, further comprising:

a second plurality of sensors configured to sense a current state of at least one other vehicle;

the processor configured to predict whether a collision involving the vehicle and a sensed vehicle will occur in the future, and to control the vehicle to avoid the collision.

16. The system of claim 15, wherein the second plurality of sensors are selected from the group consisting of radars, cameras, and ultra-sonic sensors.

17. The system of claim 12, further comprising:

a wireless communication device for transmitting the predicted future states of the vehicle with at least one other vehicle, and for receiving the predicted future states of the at least one other vehicle;

the processor configured to predict whether a collision involving the vehicle and the at least one other vehicle will occur in the future based on the predicted future states of the vehicle and the received predicted future states of the at least one other vehicle, and to control the vehicle to avoid the collision.

18. The system of claim 12, further comprising:

a wireless communication device for transmitting the predicted future states of the vehicle to a roadside device;

the roadside device configured to receive the predicted future states of the vehicle and at least one other vehicle within transmission range of the roadside device, to predict whether a collision involving the vehicle and the at least one other vehicle will occur in the future based on the received predicted future states of the vehicle and the received predicted future states of the at least one other vehicle, and to transmit instructions to the vehicle and the at least one other vehicle to avoid the collision.

19. A roadside system for predicting future states of vehicles by combining an unscented transform (UT) with numerical integration (NI), the system comprising:

a plurality of sensors configured to sense the state of at least one vehicle; and

a processor configured to predict future states of each sensed vehicle based on sensed performance measurements, the prediction calculated by the processor by generating, as the UT, (2n+1) sigma points X_i, where i=0, . . . , 2n, each of the sigma points having n states, performing, as the NI, (2n+1) numerical integrations, each integration including propagating the n-states of the respective sigma points X_i through the non-linear function Y_i=f(X_i), and combining the propagated sigma points to generate the predicted future states of each sensed vehicle, wherein

the processor is configured to predict whether a collision involving the sensed vehicles will occur in the future based on the predicted future states of the sensed vehicles, and to transmit instructions to the sensed vehicles to avoid the collision, and

the predicted future states of the vehicle are generated only when there is no measurement update or process noise so that an unscented transform is applied only to a stochastic state.

20. A non-transitory computer readable medium having stored thereon instructions for causing the computer to implement a method for predicting future states of a vehicle by combining an unscented transform (UT) with numerical integration (NI), the method comprising:

inputting, as the UT, noisy sensor measurements representing a current state of the vehicle to generate (2n+1) sigma points X_i, where i=0, . . . , 2n, each of the sigma points having n states;

performing, as the NI, (2n+1) numerical integrations, each integration including propagating the n-states of the respective sigma points X_i through the non-linear function Y_i=f(X_i); and

combining the propagated sigma points to generate the predicted future states of the vehicle, wherein

the predicted future states of the vehicle are generated only when there is no measurement update or process noise so that an unscented transform is applied only to a stochastic state.