From 9add1f0dc195cd93a2cabe2900ade800f4225778 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 5 Dec 2023 16:49:06 +0300 Subject: [PATCH] feat(pid_longitudinal_controller): update readme Signed-off-by: Berkay Karaman --- control/pid_longitudinal_controller/README.md | 41 +- .../LongitudinalControllerDiagram.drawio.svg | 638 ++++++++++-------- 2 files changed, 368 insertions(+), 311 deletions(-) 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/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