System and method for controlling a vehicle under sensor uncertainty

Patent metadata

Field	Value
Publication number	US-10408638-B2
Application number	US-201815862568-A
Country	US
Kind code	B2
Filing date	Jan 4, 2018
Priority date	Jan 4, 2018
Publication date	Sep 10, 2019
Grant date	Sep 10, 2019

Abstract

A system for controlling a vehicle a sensor to sense measurements indicative of a state of the vehicle and a memory to store a motion model of the vehicle, a measurement model of the vehicle, and a mean and a variance of a probabilistic distribution of a state of calibration of the sensor. The motion model of the vehicle defines the motion of the vehicle from a previous state to a current state subject to disturbance caused by an uncertainty of the state of calibration of the sensor in the motion of the vehicle. The measurement model relates the measurements of the sensor to the state of the vehicle using the state of calibration of the sensor. The system includes a processor to update the probabilistic distribution of the state of calibration based on a function of the sampled states of calibration weighted with weights determined based on a difference between the state of calibration sampled on a feasible space defined by the probabilistic distribution and the corresponding state of calibration estimated based on the measurements using the motion and the measurements models. The system includes a controller to control the vehicle using the measurements of the sensor adapted using the updated probabilistic distribution of the state of calibration of the sensor.

First claim

We claim:

1. A system for controlling a vehicle, comprising:
at least one sensor to sense measurements indicative of a state of the vehicle;
a memory to store a motion model of the vehicle, a measurement model of the vehicle, and a mean and a variance of a probabilistic distribution of a state of calibration of the sensor, wherein the motion model of the vehicle defines the motion of the vehicle from a previous state of the vehicle to a current state of the vehicle subject to disturbance caused by an uncertainty of the state of calibration of the sensor in the motion of the vehicle, such that the motion model includes a state of calibration sampled on the probabilistic distribution of the state of calibration of the sensor, and wherein the measurement model relates the measurements of the sensor to the state of the vehicle using the state of calibration of the sensor;
a processor configured to
sample a feasible space of the state of calibration of the sensor defined by the probabilistic distribution to produce a set of sampled states of calibration of the sensor;
estimate, for each sampled state of calibration using the motion model, an estimation of the current state of the vehicle to produce a set of estimated states of the vehicle;
estimate, for each estimated state of the vehicle, an estimated state of calibration of the sensor by inserting the measurements and the estimated state of the vehicle into the measurement model; and
update the mean and the variance of the probabilistic distribution of the state of calibration of the sensor stored in the memory based on a function of the sampled states of calibration weighted with weights determined based on a difference between the sampled state of calibration and the corresponding estimated state of calibration; and

a controller to control the vehicle using the measurements of the sensor adapted using the updated probabilistic distribution of the state of calibration of the sensor.

2. The system of claim 1 , wherein the set of sampled states of calibration of the sensor represents the state of calibration of the sensor as a set of particles, each particle includes a mean and a variance of the state of calibration of the sensor defining the feasible space of the parameters of the state of calibration of the sensor, and wherein the processor
updates iteratively, until a termination condition is met, the mean and the variance of at least one particle using a difference between the estimated state of calibration of the sensor estimated for the particle and the measured state of calibration of the sensor determined for the particle;
updates the mean and the variance of the probabilistic distribution of the state of calibration of the sensor as a function of the updated mean and the updated variance of the particle.

3. The system of claim 2 , wherein, for the iteration updating the particle, the processor is configured to
determine the mean of the estimated state of calibration of the sensor that results in the state of the vehicle estimated for the particle according to the measurement model;
determine the variance of the estimated state of calibration of the sensor as a combination of an uncertainty of the measurements and the variance of the particle;
update the mean of the sampled state of calibration of the sensor of the particle using the mean of the estimated state of calibration of the sensor; and
update the variance of the sampled state of calibration of the sensor of the particle using the variance of the estimated state of calibration of the sensor.

4. The system of claim 3 , wherein the processor determines the variance of the estimated state of calibration of the sensor as the combination of the uncertainty of the measurements and a set of variances of the set of particles.

5. The system of claim 4 , wherein the number of particles in the set of particle are varying over time.

6. The system of claim 1 , wherein the function uses a weighted combination of the sampled states of calibration of the sensor.

7. The system of claim 1 , wherein the sensor is calibrated using the updated probabilistic distribution of the state of calibration of the sensor.

8. The system of claim 1 , wherein the at least one sensor includes a first sensor to measure an angle indicative of the steering angle of the steering wheel of the vehicle and a second sensor to measure at least one of a lateral acceleration and a heading rate, wherein the motion model includes the state of calibration of the first sensor, but does not include the state of calibration of the second sensor, and wherein the measurement model includes both the state of calibration of the first sensor and the state of calibration of the second sensor.

9. The system of claim 8 ,
wherein the processor updates the mean and the variance of the probabilistic distribution of the state of calibration of the first sensor based on the function of a difference of weighted sampled states of calibration of the first sensor and weighted estimated states of calibration of the first sensor, and
wherein the processor updates the mean and the variance of a probabilistic distribution of the state of calibration of the second sensor based on the function of a difference of weighted estimated states of calibration of the second sensor and the sensor measurement.

10. The system of claim 1 , wherein the state of the vehicle includes a velocity and a heading rate of the vehicle,
wherein the motion model of the vehicle includes a combination of a deterministic component of the motion and a probabilistic component of the motion, wherein the deterministic component of the motion is independent from the state of calibration of the sensor and defines the motion of the vehicle as a function of time, wherein the probabilistic component of the motion includes the state of calibration of the sensor having an uncertainty and defines disturbance on the motion of the vehicle,

wherein the measurement model of the vehicle includes a combination of a deterministic component of the measurement model independent from the state of calibration of the sensor and a probabilistic component of the measurement model that includes the state of calibration of the sensor.

11. A method for controlling a vehicle, wherein the method uses a processor coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor carry out at least some steps of the method, comprising:
sensing, using at least one sensor, measurements indicative of a state of the vehicle;
retrieving, from a memory operatively connected to the processor, a motion model of the vehicle, a measurement model of the vehicle, and a mean and a variance of a probabilistic distribution of a state of calibration of the sensor, wherein the motion model of the vehicle defines the motion of the vehicle from a previous state of the vehicle to a current state of the vehicle subject to disturbance caused by an uncertainty of the state of calibration of the sensor in the motion of the vehicle, such that the motion model includes a state of calibration sampled on the probabilistic distribution of the state of calibration of the sensor, and wherein the measurement model relates the measurements of the sensor to the state of the vehicle using the state of calibration of the sensor;
sampling a feasible space of the state of calibration of the sensor defined by the probabilistic distribution to produce a set of sampled states of calibration of the sensor;
estimating, for each sampled state of calibration using the motion model, an estimation of the current state of the vehicle to produce a set of estimated states of the vehicle;
estimati

Assignees

• Mitsubishi Electric Res Laboratories Inc

Inventors

• Berntorp Karl
• Di Cairano Stefano

Classifications

• B60W2050/0088

  Adaptive recalibration · CPC title

• B60W2050/0034

  Multiple-track, 2D vehicle model, e.g. four-wheel model · CPC title

• B60W2050/0033

  Single-track, 2D vehicle model, i.e. two-wheel bicycle model · CPC title

• B60W2050/0025

  Transfer function weighting factor · CPC title

• B60W2050/0016

  State machine analysis · CPC title