Grade angle estimation system based on three-dimensional map data

A method for estimating a grade angle of a roadway that a vehicle is driving along, the method comprising:

• receiving, by one or more controllers, three-dimensional map data of the roadway and measurements indicating an inertial measurement unit (IMU)-based grade angle of the roadway collected by an IMU, wherein the three-dimensional map data is capped and omits the grade angle of the roadway once the grade angle exceeds a predefined capped value;
• determining, by the one or more controllers, the IMU-based grade angle based on the measurements received by the IMU, wherein the IMU-based grade angle is measured at a current position of the vehicle along the roadway;
• comparing, by the one or more controllers, an absolute value of the IMU-based grade angle with a threshold grade value, wherein the threshold grade value is equal to the predefined capped value of the three-dimensional map data;
• in response to determining the absolute value of the IMU-based grade angle is less than or equal to the threshold grade value, setting the grade angle equal to a look-ahead angle, wherein the look-ahead angle is calculated based on the three-dimensional map data and represents the grade angle of the roadway at a look-ahead distance;
• in response to determining the absolute value of the IMU-based grade angle is greater than the threshold grade value, setting the grade angle equal to the IMU-based grade angle;
• transmitting the grade angle to one or more vehicular systems that are part of the vehicle, wherein the one or more vehicular systems determine one or more control actions of the vehicle based on the grade angle;
• and
• commanding the one or more vehicular systems to execute the one or more control actions based on the grade angle.

A vehicle, comprising:

• a plurality of perception sensors;
• a prime mover;
• a braking system;
• and
• a grade angle estimation system for estimating a grade angle of a roadway that the vehicle is driving along, the grade angle estimation system comprising:
• one or more controllers in electronic communication with the plurality of perception sensors, the prime mover, and the braking system, wherein the one or more controllers include one or more processors that execute instructions to:
• receive three-dimensional map data of the roadway and measurements indicating an inertial measurement unit (IMU)-based grade angle of the roadway collected by an IMU, wherein the three-dimensional map data is capped and omits the grade angle of the roadway once the grade angle exceeds a predefined capped value;
• determine the IMU-based grade angle based on the measurements received by the IMU, wherein the IMU-based grade angle is measured at a current position of the vehicle along the roadway;
• compare an absolute value of the IMU-based grade angle with a threshold grade value, wherein the threshold grade value is equal to the predefined capped value of the three-dimensional map data;
• in response to determining the absolute value of the IMU-based grade angle is less than or equal to the threshold grade value, set the grade angle equal to a look-ahead angle, wherein the look-ahead angle is calculated based on the three-dimensional map data and represents the grade angle of the roadway at a look-ahead distance;
• in response to determining the absolute value of the IMU-based grade angle is greater than the threshold grade value, set the grade angle equal to the IMU-based grade angle;
• transmit the grade angle to an adaptive cruise control system that is part of the vehicle, wherein the adaptive cruise control system determines one or more control actions of the vehicle based on the grade angle;
• commanding the prime mover and the braking system to execute the one or more control actions based on the grade angle;
• in response to determining the grade angle is equal to the look-ahead angle, extract grade angle data from the three-dimensional map data;
• transmit the grade angle data to a filter;
• determine, by the filter, filtered grade data based on the grade angle data, where a time constant is introduced to the filtered grade data by the filter and the time constant includes a time lag;
• determine the time lag introduced to the filtered grade data;
• and
• perform a time shift correction that shifts a time of the filtered grade data by the time lag introduced by the time constant, wherein the filtered grade data determined by the filter compensates for the time lag.