diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 57eab2d87f18e..c9a60edbd703b 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -55,18 +55,27 @@ For reliable stopping, the target acceleration calculated by the FeedForward sys Based on the slope information, a compensation term is added to the target acceleration. -There are two sources of the slope information, which can be switched by a parameter. +There are two sources of the slope information, both sources are used in controller with respect to state of the node. -- Pitch of the estimated ego-pose (default) +- Pitch of the estimated ego-pose + - Only used when `slope_source` is "raw_pitch" - Calculates the current slope from the pitch angle of the estimated ego-pose - Pros: Easily available - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. + - Cons: Can not be used in combination with delay compensation. - Z coordinate on the trajectory - - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory - - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained - - Pros: Can be used in combination with delay compensation (not yet implemented) + - Only used when `slope_source` is "trajectory_pitch" + - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory. + - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained. + - Pros: Can be used in combination with delay compensation. - Cons: z-coordinates of high-precision map is needed. - - Cons: Does not support free space planning (for now) + - Cons: Does not support free space planning (for now). +- Adaptive pitch information + - Only used when `slope_source` is "trajectory_adaptive" + - It switches the slope sources with respect to velocity of the ego vehicle + - The trajectory_pitch is used when the vehicle's velocity is higher than `adaptive_trajectory_velocity_th` otherwise pitch of the estimated ego-pose is used for slope value. + - Pros: Can be used in combination with delay compensation for high speeds + - Cons: There might be a loss of comfort when slope source is switched **Notation:** This function works correctly only in a vehicle system that does not have acceleration feedback in the low-level control system. @@ -108,23 +117,6 @@ Depending on the actuating principle of the vehicle, the mechanism that physical In this controller, the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem. -### Slope compensation - -Based on the slope information, a compensation term is added to the target acceleration. - -There are two sources of the slope information, which can be switched by a parameter. - -- Pitch of the estimated ego-pose (default) - - Calculates the current slope from the pitch angle of the estimated ego-pose - - Pros: Easily available - - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. -- Z coordinate on the trajectory - - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory - - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained - - Pros: Can be used in combination with delay compensation (not yet implemented) - - Cons: z-coordinates of high-precision map is needed. - - Cons: Does not support free space planning (for now) - ## Assumptions / Known limits 1. Smoothed target velocity and its acceleration shall be set in the trajectory @@ -178,7 +170,8 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | min_acc | double | min value of output acceleration [m/s^2] | -5.0 | | max_jerk | double | max value of jerk of output acceleration [m/s^3] | 2.0 | | min_jerk | double | min value of jerk of output acceleration [m/s^3] | -5.0 | -| use_trajectory_for_pitch_calculation | bool | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used. | false | +| slope_source | string | It defines the slope source for the vehicle. Available options: "raw_pitch", "trajectory_pitch" or "trajectory_adaptive" | "raw_pitch" | +| adaptive_trajectory_velocity_th | double | Threshold velocity value for state transition of slope sources. Only usable when `slope_source` is "trajectory_adaptive". [m/s] | 1.0 | | lpf_pitch_gain | double | gain of low-pass filter for pitch estimation | 0.95 | | max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | | min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp index 8b981c0485ed9..2e989321e0c70 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp @@ -29,6 +29,7 @@ class PIDController /** * @brief calculate the output of this PID * @param [in] error previous error + * @param [in] error_filtered filtered error * @param [in] dt time step [s] * @param [in] is_integrated if true, will use the integral component for calculation * @param [out] pid_contributions values of the proportional, integral, and derivative components @@ -36,7 +37,7 @@ class PIDController * @throw std::runtime_error if gains or limits have not been set */ double calculate( - const double error, const double dt, const bool is_integrated, + const double error, const double error_filtered, const double dt, const bool is_integrated, std::vector & pid_contributions); /** * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms diff --git a/control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg b/control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg index c9b0883534c86..8bedafef1bf85 100644 --- a/control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg +++ b/control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg @@ -1,38 +1,38 @@ - + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + +
-
+
Predicted
@@ -43,19 +43,21 @@
- Predict... + Predic... - - + + + + - +
-
+
Predicted
@@ -66,23 +68,23 @@
- Predict... + Predic... - - - - - - + + + + + + - +
-
+
Slope Force Conversion
@@ -91,397 +93,411 @@
- Slope Force Conversion... + Slope Force Conversion... - - - + - -
-
+ +
+
- Slope + Predicted Slope
Angle
- Slope... + Predicted Slope... - - - + + + - +
-
+
P
- P + P - - - + + + - +
-
+
I
- I + I - - - + + + - +
-
+
D
- D + D - - - + + + - +
-
+
∫
- ∫ + ∫ - - - + + + - +
-
+
d/dt
- d/dt + d/dt - - - + + + - +
-
+
LPF
- LPF - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
+ LPF + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
- + + + + - -
-
-
+ +
+
+
+
- + + + + - -
-
-
+ +
+
+
+
- + + + + - +
-
-
+
+
+
- + + + + - +
-
-
+
+
+
- + + + + - +
-
-
+
+
+
- + + + + - +
-
-
+
+
-
- - + - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + - +
-
-
+
+
Slope Compensation Limit
- Slope Compensation Limit + Slope Compensation Limit + - +
-
-
+
+
FeedForward Acceleration Limit
- FeedForward Acceleration Limit + FeedForward Acceleration Limit - - + + + - +
-
-
+
+
FeedBack Control
- FeedBack Control + FeedBack Control + - -
-
-
+ +
+
+
+
- + + + + - -
-
-
+ +
+
+
-
- - + - + - -
-
-
+ +
+
+
Reference
Trajectory @@ -489,53 +505,58 @@
- Referen... + Refere... + + + - +
-
-
+
+
Delay Compensation
- Delay Compensation + Delay Compensation - - + + + - -
-
-
+ +
+
+
Ego Pose
- Ego Pose + Ego Po... - - + + + - +
-
-
+
+
Predicted
Ego Velocity @@ -543,56 +564,57 @@
- Predict... + Predic... - - - + + + - +
-
-
+
+
Prediction Model
- Prediction Model + Prediction Model - + - +
-
-
+
+
getReferencePoint
- getReferencePoint + getReferencePoint + - +
-
-
+
+
Predicted
Ego Pose @@ -600,108 +622,150 @@
- Predict... + Predic... + - +
-
-
+
+
Ego Velocity
- Ego Vel... + Ego Ve... + - +
-
-
+
+
FeedForward Control
- FeedForward Control + FeedForward Control + - +
-
-
+
+
Acceleration
- Acceler... + Accele... - + - +
-
-
+
+
Vehicle
- Vehicle + Vehicle - - - - - - - - + + + + + + + + + - +
-
-
+
+
Brake keeping Limit
- Brake keeping Limit + Brake keeping Limit + + + + + + + + +
+
+
+ + storeAccelCmd +
+ +
+
+
+ + storeAccelCmd + + + + + + +
+
+
+
Last Send Acc Commands
+
+
+
+ + Last Send Acc Commands - - Viewer does not support full SVG 1.1 + + Text is not SVG - cannot display diff --git a/control/pid_longitudinal_controller/src/pid.cpp b/control/pid_longitudinal_controller/src/pid.cpp index 530b5cd15e754..4ca8ebbd35ef2 100644 --- a/control/pid_longitudinal_controller/src/pid.cpp +++ b/control/pid_longitudinal_controller/src/pid.cpp @@ -32,7 +32,7 @@ PIDController::PIDController() } double PIDController::calculate( - const double error, const double dt, const bool enable_integration, + const double error, const double filtered_error, const double dt, const bool enable_integration, std::vector & pid_contributions) { if (!m_is_gains_set || !m_is_limits_set) { @@ -55,12 +55,12 @@ double PIDController::calculate( error_differential = 0; m_is_first_time = false; } else { - error_differential = (error - m_prev_error) / dt; + error_differential = (filtered_error - m_prev_error) / dt; } double ret_d = p.kd * error_differential; ret_d = std::min(std::max(ret_d, p.min_ret_d), p.max_ret_d); - m_prev_error = error; + m_prev_error = filtered_error; pid_contributions.resize(3); pid_contributions.at(0) = ret_p; diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index aab2ecf8f081e..edad56b460093 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -1053,6 +1053,8 @@ PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedS double PidLongitudinalController::applyVelocityFeedback(const ControlData & control_data) { // NOTE: Acceleration command is always positive even if the ego drives backward. + const double nearest_target_vel = + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps; const double vel_sign = (control_data.shift == Shift::Forward) ? 1.0 : (control_data.shift == Shift::Reverse ? -1.0 : 0.0); @@ -1076,8 +1078,8 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel); std::vector pid_contributions(3); - const double pid_acc = - m_pid_vel.calculate(error_vel_filtered, control_data.dt, enable_integration, pid_contributions); + const double pid_acc = m_pid_vel.calculate( + diff_vel, error_vel_filtered, control_data.dt, enable_integration, pid_contributions); // Feedforward scaling: // This is for the coordinate conversion where feedforward is applied, from Time to Arclength. @@ -1087,7 +1089,9 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont constexpr double ff_scale_max = 2.0; // for safety constexpr double ff_scale_min = 0.5; // for safety const double ff_scale = std::clamp( - std::abs(current_vel) / std::max(std::abs(target_motion.vel), 0.1), ff_scale_min, ff_scale_max); + 1.0 + (abs(nearest_target_vel - current_vel) / std::max(abs(current_vel), 0.1)), ff_scale_min, + ff_scale_max); + const double ff_acc = control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 * ff_scale; diff --git a/control/pid_longitudinal_controller/test/test_pid.cpp b/control/pid_longitudinal_controller/test/test_pid.cpp index 82d01e0028a9c..f0ceccd8499a3 100644 --- a/control/pid_longitudinal_controller/test/test_pid.cpp +++ b/control/pid_longitudinal_controller/test/test_pid.cpp @@ -29,14 +29,14 @@ TEST(TestPID, calculate_pid_output) PIDController pid; // Cannot calculate before initializing gains and limits - EXPECT_THROW(pid.calculate(0.0, dt, enable_integration, contributions), std::runtime_error); + EXPECT_THROW(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), std::runtime_error); pid.setGains(1.0, 1.0, 1.0); pid.setLimits(10.0, 0.0, 10.0, 0.0, 10.0, 0.0, 10.0, 0.0); double error = target - current; double prev_error = error; while (current != target) { - current = pid.calculate(error, dt, enable_integration, contributions); + current = pid.calculate(error, error, dt, enable_integration, contributions); EXPECT_EQ(contributions[0], error); EXPECT_EQ(contributions[1], 0.0); // integration is deactivated EXPECT_EQ(contributions[2], error - prev_error); @@ -50,18 +50,18 @@ TEST(TestPID, calculate_pid_output) enable_integration = true; // High errors to force each component to its upper limit - EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0); + EXPECT_EQ(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), 0.0); for (double error = 100.0; error < 1000.0; error += 100.0) { - EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), 10.0); + EXPECT_EQ(pid.calculate(error, error, dt, enable_integration, contributions), 10.0); EXPECT_EQ(contributions[0], 10.0); EXPECT_EQ(contributions[1], 10.0); // integration is activated EXPECT_EQ(contributions[2], 10.0); } // Low errors to force each component to its lower limit - EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0); + EXPECT_EQ(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), 0.0); for (double error = -100.0; error > -1000.0; error -= 100.0) { - EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), -10.0); + EXPECT_EQ(pid.calculate(error, error, dt, enable_integration, contributions), -10.0); EXPECT_EQ(contributions[0], -10.0); EXPECT_EQ(contributions[1], -10.0); // integration is activated EXPECT_EQ(contributions[2], -10.0);