Systems and methods for control state estimation of vehicles based on force disturbance signals
For one embodiment of the present invention, a computer implemented method for determining force disturbance signals for a vehicle is described. The computer implemented method includes obtaining sensor signals from a sensor system of the vehicle to monitor driving operations and to determine localization of the vehicle. The computer implemented method further includes determining a set of parameters including vehicle pose estimation, velocity, acceleration, roll, pitch, yaw rate based on the localization and the sensor signals and determining force disturbance signals including lateral force disturbances for front and rear axle lateral accelerations and a total longitudinal force disturbance based on the localization and the sensor signals.
1 . A computer implemented method for a control model of a vehicle comprising:
obtaining sensor signals from a sensor system of the vehicle to monitor driving operations and to determine localization of the vehicle, including location of tires of the vehicle;
determining a set of parameters, including vehicle pose estimation, velocity, acceleration, roll, pitch, and yaw rate based on the localization and the sensor signals;
interpolating a previous plan of a path planning system to a current time and identifying a set of expected values, including vehicle pose estimation, velocity, acceleration, roll, pitch, and yaw rate, based on the interpolated previous plan and determining an expected initial state that includes vehicle pose estimation, velocity, acceleration, vehicle commands, and force disturbance signals for the vehicle, wherein the measurement update module generates an initial state by applying a filter to the comparison signals and the expected initial state;
comparing the vehicle pose estimation, the velocity, and the acceleration to the expected values from the previous plan of the path planning system to generate comparison signals;
determining force disturbance signals, including lateral force disturbances for front and rear axle lateral accelerations and a total longitudinal force disturbance based on the localization and the sensor signals;
adjusting the control model of the vehicle based on the determined force disturbance signals based on the localization and the sensors signals; and
controlling the vehicle using the control model.
2 . The computer implemented method of claim 1 , further comprising:
using the comparison signals and the expected initial state to generate an initial state of the vehicle, including pose estimation, velocity, acceleration, vehicle command, and force disturbance signals for the vehicle.
3 . The computer implemented method of claim 1 , wherein the control model comprises a switch-free control model to handle all speeds of the vehicle including zero speed with no divergence.
4 . The computer implemented method of claim 1 , wherein the control model is able to independently determine pitch, roll, yaw rate, road grade angle, and road bank angle.
5 . The computer implemented method of claim 1 , wherein the sensor system comprises a camera sensor system and a Light Detection and Ranging (LIDAR) sensor system to perform ranging measurements for localization of the vehicle, chassis of the vehicle, and nearby objects within a certain distance of the vehicle and the sensor system.
6 . The computer-implemented method of claim 1 , wherein the filter is one of a linear quadratic estimator and a Kalman filter.
7 . A computing system, comprising:
a memory storing instructions; and
a processor coupled to the memory, the processor is configured to execute instructions of a software program to:
initialize driving operations of an autonomous vehicle;
receive sensor signals from a sensor system of the autonomous vehicle;
determine localization of the autonomous vehicle based on the sensor signals, including location of tires of the vehicle;
determine a set of parameters including vehicle pose estimation, velocity, and acceleration based on the localization and the sensor signals;
interpolating a previous plan of a path planning system to a current time and identifying a set of expected values, including vehicle pose estimation, velocity, acceleration, roll, pitch, and yaw rate, based on the interpolated previous plan, wherein the measurement update module generates an initial state by applying a filter to the comparison signals and the expected initial state;
comparing the vehicle pose estimation, the velocity, and the acceleration to the expected values from the previous plan of the path planning system to generate comparison signals; and
determine force disturbance signals including lateral force disturbances for front and rear axle lateral accelerations and a total longitudinal force disturbance based on the localization of the autonomous vehicle and the sensor signals, and adjusting a control model of the vehicle based on the determined force disturbance signals; and
controlling the vehicle using the control model.
8 . The computing system of claim 7 , wherein the processor is configured to execute instructions to:
determine an expected initial state that includes vehicle pose estimation, velocity, acceleration, commands, and force disturbance signals for the vehicle.
9 . The computing system of claim 8 , wherein the processor is configured to execute instructions to:
utilize the comparison signals and the expected initial state to generate an initial state including pose estimation, velocity, acceleration, commands, and force disturbance signals for the vehicle.
10 . The computing system of claim 7 , wherein the processor is configured to execute instructions to:
provide a switch-free control model to handle all speeds of the autonomous vehicle including zero speed with no divergence.
11 . The computing system of claim 7 , wherein the processor is configured to execute instructions to:
determine the set of parameters including roll, pitch, and yaw rate based on the localization and the sensor signals.
12 . The computing system of claim 11 , wherein the processor is configured to execute instructions to:
independently determine pitch, roll, yaw rate, road grade angle, and road bank angle.
13 . The computing system of claim 7 , wherein the processor is configured to execute instructions to:
provide a three dimensional (3D) vehicle model with roll and pitch.
14 . A non-transitory computer readable storage medium having embodied thereon a program, wherein the program is executable by a processor to perform a method comprising:
initializing driving operations for a vehicle;
obtaining sensor signals from a sensor system of the vehicle and determining localization of the vehicle based on the sensor signals, including location of tires of the vehicle;
determining vehicle pose estimation, velocity, acceleration, road bank angle, and road grade angle based on the localization of the vehicle and the sensor signals, including location of tires of the vehicle
interpolating a previous plan of a path planning system to a current time and identifying a set of expected values, including vehicle pose estimation, velocity, acceleration, roll, pitch, and yaw rate, based on the interpolated previous plan, wherein the measurement update module generates an initial state by applying a filter to the comparison signals and the expected initial state;
comparing the vehicle pose estimation, the velocity, and the acceleration to the expected values from the previous plan of the path planning system to generate comparison signals;
determining lateral acceleration limits and force disturbance signals including front and rear axle lateral accelerations (ayf, ayr)) and a total longitudinal force disturbance based on the localization, road bank angle, and road grade angle;
adjusting a control model of the vehicle based on the determined force disturbance signals; and
controlling the vehicle using the control model.
15 . The non-transitory computer readable storage medium of claim 14 , wherein the force disturbance signals comprise front and rear axle lateral accelerations (ayf, ayr)) and a total longitudinal force disturbance.
16 . The non-transitory computer readable storage medium of claim 15 , wherein the method further comprises:
maintaining a control model that causes no shift in steering angle limits that correspond to lateral acceleration limits in presence of one or more disturbances.
17 . The non-transitory computer readable storage medium of claim 16 , wherein the control model is able to independently determine pitch, roll, yaw rate, road grade angle, and road bank angle.
18 . The non-transitory computer readable storage medium of claim 15 , wherein the lateral acceleration limits can be set a priori or adjusted in real-time due to measurements of the sensor signals.