From cf77546c7f7ce8f0a668da546e1b795b91a54408 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 16 Aug 2022 00:25:51 +0900 Subject: [PATCH] docs(trajectory_follower, trajectory_follower_nodes): update docs Signed-off-by: kosuke55 --- .../design/media/BrakeKeeping.drawio.svg | 0 .../LongitudinalControllerDiagram.drawio.svg | 0 ...udinalControllerStateTransition.drawio.svg | 0 .../design/mpc_lateral_controller-design.md} | 63 ++++++++-- .../pid_longitudinal_controller-design.md} | 117 +++++++++--------- .../design/trajectory_follower-design.md | 34 +++++ .../design/latlon_muxer-design.md | 40 ------ .../design/media/Controller.drawio.svg | 4 + .../design/trajectory_follower-design.md | 47 +++++-- 9 files changed, 191 insertions(+), 114 deletions(-) rename control/{trajectory_follower_nodes => trajectory_follower}/design/media/BrakeKeeping.drawio.svg (100%) rename control/{trajectory_follower_nodes => trajectory_follower}/design/media/LongitudinalControllerDiagram.drawio.svg (100%) rename control/{trajectory_follower_nodes => trajectory_follower}/design/media/LongitudinalControllerStateTransition.drawio.svg (100%) rename control/{trajectory_follower_nodes/design/lateral_controller-design.md => trajectory_follower/design/mpc_lateral_controller-design.md} (81%) rename control/{trajectory_follower_nodes/design/longitudinal_controller-design.md => trajectory_follower/design/pid_longitudinal_controller-design.md} (88%) delete mode 100644 control/trajectory_follower_nodes/design/latlon_muxer-design.md create mode 100644 control/trajectory_follower_nodes/design/media/Controller.drawio.svg diff --git a/control/trajectory_follower_nodes/design/media/BrakeKeeping.drawio.svg b/control/trajectory_follower/design/media/BrakeKeeping.drawio.svg similarity index 100% rename from control/trajectory_follower_nodes/design/media/BrakeKeeping.drawio.svg rename to control/trajectory_follower/design/media/BrakeKeeping.drawio.svg diff --git a/control/trajectory_follower_nodes/design/media/LongitudinalControllerDiagram.drawio.svg b/control/trajectory_follower/design/media/LongitudinalControllerDiagram.drawio.svg similarity index 100% rename from control/trajectory_follower_nodes/design/media/LongitudinalControllerDiagram.drawio.svg rename to control/trajectory_follower/design/media/LongitudinalControllerDiagram.drawio.svg diff --git a/control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg b/control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg similarity index 100% rename from control/trajectory_follower_nodes/design/media/LongitudinalControllerStateTransition.drawio.svg rename to control/trajectory_follower/design/media/LongitudinalControllerStateTransition.drawio.svg diff --git a/control/trajectory_follower_nodes/design/lateral_controller-design.md b/control/trajectory_follower/design/mpc_lateral_controller-design.md similarity index 81% rename from control/trajectory_follower_nodes/design/lateral_controller-design.md rename to control/trajectory_follower/design/mpc_lateral_controller-design.md index 450e7d04707d3..0ee13ebfe891e 100644 --- a/control/trajectory_follower_nodes/design/lateral_controller-design.md +++ b/control/trajectory_follower/design/mpc_lateral_controller-design.md @@ -1,4 +1,4 @@ -# Lateral Controller +# MPC Lateral Controller This is the design document for the lateral controller node in the `trajectory_follower_nodes` package. @@ -22,10 +22,29 @@ The node uses an implementation of linear model predictive control (MPC) for acc The MPC uses a model of the vehicle to simulate the trajectory resulting from the control command. The optimization of the control command is formulated as a Quadratic Program (QP). -These functionalities are implemented in the `trajectory_follower` package -(see [trajectory_follower-mpc-design](../../trajectory_follower/design/trajectory_follower-mpc-design.md#mpc-trajectory-follower)) +Different vehicle models are implemented: -### Assumptions / Known limits +- kinematics : bicycle kinematics model with steering 1st-order delay. +- kinematics_no_delay : bicycle kinematics model without steering delay. +- dynamics : bicycle dynamics model considering slip angle. + The kinematics model is being used by default. Please see the reference [1] for more details. + +For the optimization, a Quadratic Programming (QP) solver is used with two options are currently implemented: + +- unconstraint_fast : use least square method to solve unconstraint QP with eigen. +- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) algorithm (for more details see the related papers at the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section): + +### Filtering + +Filtering is required for good noise reduction. +A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as input of the MPC as well as for +the output steering angle. +Other filtering methods can be considered as long as the noise reduction performances are good +enough. +The moving average filter for example is not suited and can yield worse results than without any +filtering. + +## Assumptions / Known limits @@ -33,18 +52,34 @@ The tracking is not accurate if the first point of the reference trajectory is a - Issue to add points behind ego: -### Inputs / Outputs / API +## Inputs / Outputs / API -Inputs +### Inputs + +Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) + +- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry +- `autoware_auto_vehicle_msgs/SteeringReport` current steering -- `input/reference_trajectory` : reference trajectory to follow. -- `input/current_kinematic_state`: current state of the vehicle (position, velocity, etc). - Output -- `output/lateral_control_cmd`: generated lateral control command. +### Outputs + +Return LateralOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) + +- `autoware_auto_control_msgs/AckermannLateralCommand` +- LateralSyncData + - steer angle convergence + +### MPC class + +The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm. +Once a vehicle model, a QP solver, and the reference trajectory to follow have been set +(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command +can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`. ### Parameter description @@ -132,8 +167,16 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. - `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability. - `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability. +## References / External links + + + +- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", + Robotics Institute, Carnegie Mellon University, February 2009. + ## Related issues - +- diff --git a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md b/control/trajectory_follower/design/pid_longitudinal_controller-design.md similarity index 88% rename from control/trajectory_follower_nodes/design/longitudinal_controller-design.md rename to control/trajectory_follower/design/pid_longitudinal_controller-design.md index 207a9755d7117..eb373d2ba3c13 100644 --- a/control/trajectory_follower_nodes/design/longitudinal_controller-design.md +++ b/control/trajectory_follower/design/pid_longitudinal_controller-design.md @@ -1,4 +1,4 @@ -# Longitudinal Controller +# PID Longitudinal Controller ## Purpose / Use cases @@ -9,56 +9,7 @@ It is assumed that the target acceleration calculated here will be properly real Note that the use of this module is not mandatory for Autoware if the vehicle supports the "target speed" interface. -## Separated lateral (steering) and longitudinal (velocity) controls - -This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows. - -- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking. -- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking. - -Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below. - -### Complex requirements for longitudinal motion - -The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement. - -In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important. - -There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability. - -### Nonlinear coupling of lateral and longitudinal motion - -The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development. - -Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed. - -## Design - -## Assumptions / Known limits - -1. Smoothed target velocity and its acceleration shall be set in the trajectory - 1. The velocity command is not smoothed inside the controller (only noise may be removed). - 2. For step-like target signal, tracking is performed as fast as possible. -2. The vehicle velocity must be an appropriate value - 1. The ego-velocity must be a signed-value corresponding to the forward/backward direction - 2. The ego-velocity should be given with appropriate noise processing. - 3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced. -3. The output of this controller must be achieved by later modules (e.g. vehicle interface). - 1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller. - -## Inputs / Outputs / API - -### Output - -- control_cmd [`autoware_auto_msgs/LongitudinalCommand`] : command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. -- debug_values [`autoware_auto_msgs/Float32MultiArrayDiagnostic`] : debug values used for the control command generation (e.g. the contributions of each P-I-D terms). - -### Input - -- current_state [`autoware_auto_msgs/VehicleKinematicState`] : Current ego state including the current pose and velocity. -- current_trajectory [`autoware_auto_msgs/Trajectory`] : Current target trajectory for the desired velocity on the each trajectory points. - -## Inner-workings / Algorithms +## Design / Inner-workings / Algorithms ### States @@ -141,7 +92,59 @@ 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. -### Parameter description +### 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 + 1. The velocity command is not smoothed inside the controller (only noise may be removed). + 2. For step-like target signal, tracking is performed as fast as possible. +2. The vehicle velocity must be an appropriate value + 1. The ego-velocity must be a signed-value corresponding to the forward/backward direction + 2. The ego-velocity should be given with appropriate noise processing. + 3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced. +3. The output of this controller must be achieved by later modules (e.g. vehicle interface). + 1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller. + +## Inputs / Outputs / API + +### Input + +Set the following from the [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) + +- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry + +### Output + +Return LongitudinalOutput which contains the following to [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md) + +- `autoware_auto_control_msgs/LongitudinalCommand`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. +- LongitudinalSyncData + - velocity convergence(currently not used) + +### PIDController class + +The `PIDController` class is straightforward to use. +First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components. +Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function. + +## Parameter description The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the AutonomouStuff Lexus RX 450h for under 40 km/h driving. @@ -164,7 +167,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | | min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | -#### State transition +### State transition | Name | Type | Description | Default value | | :---------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | @@ -177,7 +180,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | emergency_state_traj_trans_dev | double | If the ego's position is `emergency_state_traj_tran_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m] | 3.0 | | emergency_state_traj_rot_dev | double | If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad] | 0.784 | -#### DRIVE Parameter +### DRIVE Parameter | Name | Type | Description | Default value | | :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | @@ -196,7 +199,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | 0.5 | | brake_keeping_acc | double | If `enable_brake_keeping_before_stop` is true, a certain acceleration is kept during DRIVE state before the ego stops [m/s^2] See [Brake keeping](#brake-keeping). | 0.2 | -#### STOPPING Parameter (smooth stop) +### STOPPING Parameter (smooth stop) Smooth stop is enabled if `enable_smooth_stop` is true. In smooth stop, strong acceleration (`strong_acc`) will be output first to decrease the ego velocity. @@ -218,7 +221,7 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig | smooth_stop_weak_stop_dist | double | Weak acceleration will be output when the ego is `smooth_stop_weak_stop_dist`-meter before the stop point. [m] | -0.3 | | smooth_stop_strong_stop_dist | double | Strong acceleration will be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m] | -0.5 | -#### STOPPED Parameter +### STOPPED Parameter | Name | Type | Description | Default value | | :----------- | :----- | :------------------------------------------- | :------------ | @@ -226,7 +229,7 @@ If the ego is still running, strong acceleration (`strong_stop_acc`) to stop rig | stopped_acc | double | target acceleration in STOPPED state [m/s^2] | -3.4 | | stopped_jerk | double | target jerk in STOPPED state [m/s^3] | -5.0 | -#### EMERGENCY Parameter +### EMERGENCY Parameter | Name | Type | Description | Default value | | :------------- | :----- | :------------------------------------------------ | :------------ | diff --git a/control/trajectory_follower/design/trajectory_follower-design.md b/control/trajectory_follower/design/trajectory_follower-design.md index 2ade485f1a641..4b696c8e00737 100644 --- a/control/trajectory_follower/design/trajectory_follower-design.md +++ b/control/trajectory_follower/design/trajectory_follower-design.md @@ -16,6 +16,40 @@ Mainly, it implements two algorithms: - PID control for the computation of velocity and acceleration commands. - [trajectory_follower-pid-design](trajectory_follower-pid-design.md) +## Design + +![Controller](../../trajectory_follower_nodes/design/media/Controller.drawio.svg) + +There are lateral and longitudinal base interface classes and each algorithm inherits from this class to implement. +The interface class has the following base functions. + +- `setInputData()`: Input the data subscribed in [controller_node](../../trajectory_follower_nodes/design/trajectory_follower-design.md). This must be implemented with the inherited algorithm and the used data must be selected. +- `run()`: Compute control commands and return to [controller_node](../../trajectory_follower_nodes/design/). This must be implemented by inherited algorithms. +- `syncData()`: Input the result of running the other controller. + +## Separated lateral (steering) and longitudinal (velocity) controls + +This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows. + +- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking. +- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking. + +Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below. + +### Complex requirements for longitudinal motion + +The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement. + +In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important. + +There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability. + +### Nonlinear coupling of lateral and longitudinal motion + +The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development. + +Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed. + ## Related issues diff --git a/control/trajectory_follower_nodes/design/latlon_muxer-design.md b/control/trajectory_follower_nodes/design/latlon_muxer-design.md deleted file mode 100644 index aa5ea71f2bfcb..0000000000000 --- a/control/trajectory_follower_nodes/design/latlon_muxer-design.md +++ /dev/null @@ -1,40 +0,0 @@ -# Lateral/Longitudinal Control Muxer - -## Purpose - -When using controllers that independently compute lateral and longitudinal commands, -this node combines the resulting messages into a single control command message. - -## Design - -Inputs. - -- `AckermannLateralCommand`: lateral command. -- `LongitudinalCommand`: longitudinal command. - -Output. - -- `AckermannControlCommand`: message containing both lateral and longitudinal commands. - -Parameter. - -- `timeout_thr_sec`: duration in second after which input messages are discarded. - -Each time the node receives an input message it publishes an `AckermannControlCommand` -if the following two conditions are met. - -1. Both inputs have been received. -2. The last received input messages are not older than defined by `timeout_thr_sec`. - -## Implementation Details - -Callbacks `latCtrlCmdCallback` and `lonCtrlCmdCallback` are defined for each input message. -Upon reception, the message is stored and function `publishCmd` is called. - -Function `publishCmd` first checks that both messages have been received -and that the stored messages are not older than the timeout. -If both conditions are true, the combined control message is built and published. - -`checkTimeout` is used to check that the stored messages are not too old -by comparing the timeout parameter `timeout_thr_sec` -with the difference between `rclcpp::Time stamp` and the current time `now()`. diff --git a/control/trajectory_follower_nodes/design/media/Controller.drawio.svg b/control/trajectory_follower_nodes/design/media/Controller.drawio.svg new file mode 100644 index 0000000000000..8bb8709e481f3 --- /dev/null +++ b/control/trajectory_follower_nodes/design/media/Controller.drawio.svg @@ -0,0 +1,4 @@ + + + +trajectory_follower
longitudinal_controller_base
longitudinal_controller_ba...
lateral_controller_base
lateral_controller_ba...mpc_lateral_controller
pid_longitudinal_controller
pid_longitudinal_controll...pure_pursuit
pure_pursuit_lateral_controller
pure_pursuit_lateral_co...trajectory_follower_node
controller_node
controller_nodeText is not SVG - cannot display \ No newline at end of file diff --git a/control/trajectory_follower_nodes/design/trajectory_follower-design.md b/control/trajectory_follower_nodes/design/trajectory_follower-design.md index 3a1c8ab2457a5..a7b6361cff856 100644 --- a/control/trajectory_follower_nodes/design/trajectory_follower-design.md +++ b/control/trajectory_follower_nodes/design/trajectory_follower-design.md @@ -6,16 +6,49 @@ Generate control commands to follow a given Trajectory. ## Design -This functionality is decomposed into three nodes. +This is a node of the functionalities implemented in [trajectory_follower](../../trajectory_follower/design/trajectory_follower-design.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands. -- [lateral-controller-design](lateral_controller-design.md) : generates lateral control messages. -- [longitudinal-controller-design](longitudinal_controller-design.md) : generates longitudinal control messages. -- [latlon-muxer-design](latlon_muxer-design.md) : combines the lateral and longitudinal control commands - into a single control command. +![Controller](media/Controller.drawio.svg) -Core functionalities are implemented in the [trajectory_follower](../../trajectory_follower/design/trajectory_follower-design.md#trajectory-follower) package. +The process flow is as follows. -@image html images/trajectory_follower-diagram.png "Overview of the Trajectory Follower package" +1. Set input data to the lateral controller +2. Compute lateral commands. +3. Sync the result of the lateral control to the longitudinal controller. + - steer angle convergence +4. Set input data to the longitudinal controller +5. Compute longitudinal commands. +6. Sync the result of the longitudinal control to the longitudinal controller. + - velocity convergence(currently not used) + +Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` + +- lateral controller + - `keep_steer_control_until_converged` +- longitudinal controller + - `enable_keep_stopped_until_steer_convergence` + +### Inputs / Outputs / API + +#### Inputs + +- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry +- `autoware_auto_vehicle_msgs/SteeringReport` current steering + +#### Outputs + +- `autoware_auto_control_msgs/AckermannControlCommand`: message containing both lateral and longitudinal commands. + +#### Parameter + +- `ctrl_period`: control commands publishing period +- `timeout_thr_sec`: duration in second after which input messages are discarded. + - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `AckermannControlCommand` if the following two conditions are met. + 1. Both commands have been received. + 2. The last received commands are not older than defined by `timeout_thr_sec`. +- `lateral_controller_mode`: `mpc_follower` or `pure_pursuit` + - (currently there is only `PID` for longitudinal controller) ## Debugging