Skip to content

Commit 7a19e8a

Browse files
kosuke55kosuke55
authoredNov 20, 2024··
feat(simple_planning_simulator): add mechanical actuaion sim model (#9300)
* feat(simple_planning_simulator): add mechanical actuaion sim model Signed-off-by: kosuke55 <[email protected]> update docs Signed-off-by: kosuke55 <[email protected]> * update from suggestions Signed-off-by: kosuke55 <[email protected]> * calc internal state using RK4 results Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
·
0.41.20.40.0
1 parent 6717fb2 commit 7a19e8a

15 files changed

+1817
-286
lines changed
 

‎simulator/simple_planning_simulator/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
2323
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp
2424
src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp
2525
src/simple_planning_simulator/utils/csv_loader.cpp
26+
src/simple_planning_simulator/utils/mechanical_controller.cpp
2627
)
2728
target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learning_based_vehicle_model_INCLUDE_DIRS})
2829
# Node executable

‎simulator/simple_planning_simulator/README.md

+29-47
Large diffs are not rendered by default.

‎simulator/simple_planning_simulator/docs/actuation_cmd_sim.md

+105
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
# ACTUATION_CMD model
2+
3+
The simple_planning_simulator usually operates by receiving Control commands, but when the `ACTUATION_CMD*` model is selected, it receives Actuation commands instead of Control commands. This model can simulate the motion using the vehicle command that is actually sent to the real vehicle. Therefore, when this model is selected, the `raw_vehicle_cmd_converter` is also launched.
4+
5+
## ACTUATION_CMD
6+
7+
This model receives the accel/brake commands and converts them using the map to calculate the motion of the model. The steer command is used as it is.
8+
Please make sure that the raw_vehicle_cmd_converter is configured as follows.
9+
10+
```yaml
11+
convert_accel_cmd: true
12+
convert_brake_cmd: true
13+
convert_steer_cmd: false
14+
```
15+
16+
## ACTUATION_CMD_STEER_MAP
17+
18+
This model is inherited from ACTUATION_CMD and receives steering arbitrary value as the actuation command.
19+
The value is converted to the steering tire rate to calculate the motion of the model. An arbitrary value is like EPS (Electric Power Steering) Voltage.
20+
21+
Please make sure that the raw_vehicle_cmd_converter is configured as follows.
22+
23+
```yaml
24+
convert_accel_cmd: true
25+
convert_brake_cmd: true
26+
convert_steer_cmd: true
27+
```
28+
29+
## ACTUATION_CMD_VGR
30+
31+
This model is inherited from ACTUATION_CMD and steering wheel angle is sent as the actuation command.
32+
The value is converted to the steering tire angle to calculate the motion of the model.
33+
34+
Please make sure that the raw_vehicle_cmd_converter is configured as follows.
35+
36+
```yaml
37+
convert_accel_cmd: true
38+
convert_brake_cmd: true
39+
convert_steer_cmd: true
40+
```
41+
42+
![vgr_sim](../media/vgr_sim.drawio.svg)
43+
44+
## ACTUATION_CMD_MECHANICAL
45+
46+
This model is inherited from ACTUATION_CMD_VGR nad has mechanical dynamics and controller for that to simulate the mechanical structure and software of the real vehicle.
47+
48+
Please make sure that the raw_vehicle_cmd_converter is configured as follows.
49+
50+
```yaml
51+
convert_accel_cmd: true
52+
convert_brake_cmd: true
53+
convert_steer_cmd: true
54+
```
55+
56+
The mechanical structure of the vehicle is as follows.
57+
58+
![mechanical_controller](../media/mechanical_controller.drawio.svg)
59+
60+
The vehicle side software assumes that it has limiters, PID controllers, power steering, etc. for the input.
61+
The conversion in the power steering is approximated by a polynomial.
62+
Steering Dynamics is a model that represents the motion of the tire angle when the Steering Torque is input. It is represented by the following formula.
63+
64+
```math
65+
\begin{align}
66+
\dot{\theta} &= \omega \\
67+
\dot{\omega} &= \frac{1}{I} (T_{\text{input}} - D \omega - K \theta - \text{sign}(\omega) F_{\text{friction}} ) \\
68+
\end{align}
69+
```
70+
71+
In this case,
72+
73+
- $\theta$ : Tire angle
74+
- $\omega$ : Tire angular velocity
75+
- $T_{\text{input}}$ : Input torque
76+
- $D$ : Damping coefficient
77+
- $K$ : Spring constant
78+
- $F_{\text{friction}}$ : Friction force
79+
- $I$ : Moment of inertia
80+
81+
Also, this dynamics has a dead zone.
82+
The steering rotation direction is different from the steering torque input direction, and the steering torque input is less than the dead zone threshold, it enters the dead zone. Once it enters the dead zone, it is judged to be in the dead zone until there is a steering input above the dead zone threshold. When in the dead zone, the steering tire angle does not move.
83+
84+
Please refer to the following file for the values of the parameters that have been system-identified using the actual vehicle's driving data.
85+
The blue line is the control input, the green line is the actual vehicle's tire angle output, and the red line is the simulator's tire angle output.
86+
[mechanical_sample_param](../param/simple_planning_simulator_mechanical_sample.param.yaml)
87+
88+
This model has a smaller sum of errors with the observed values of the actual vehicle than when tuned with a normal first-order lag model. For details, please refer to [#9252](https://github.com/autowarefoundation/autoware.universe/pull/9300).
89+
90+
![mechanical_controller_system_identification](../media/mechanical_controller_system_identification.png)
91+
92+
The parameters used in the ACTUATION_CMD are as follows.
93+
94+
| Name | Type | Description | unit |
95+
| :------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--- |
96+
| accel_time_delay | double | dead time for the acceleration input | [s] |
97+
| accel_time_constant | double | time constant of the 1st-order acceleration dynamics | [s] |
98+
| brake_time_delay | double | dead time for the brake input | [s] |
99+
| brake_time_constant | double | time constant of the 1st-order brake dynamics | [s] |
100+
| convert_accel_cmd | bool | If true, it is assumed that the command is received converted to an accel actuation value, and it is converted back to acceleration value inside the simulator. | [-] |
101+
| convert_brake_cmd | bool | If true, it is assumed that the command is received converted to a brake actuation value, and it is converted back to acceleration value inside the simulator. | [-] |
102+
| vgr_coef_a | double | the value of the coefficient a of the variable gear ratio | [-] |
103+
| vgr_coef_b | double | the value of the coefficient b of the variable gear ratio | [-] |
104+
| vgr_coef_c | double | the value of the coefficient c of the variable gear ratio | [-] |
105+
| enable_pub_steer | bool | whether to publish the steering tire angle. if false, it is expected to be converted and published from actuation_status in other nodes (e.g. raw_vehicle_cmd_converter) | [-] |

‎simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -224,7 +224,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
224224
DELAY_STEER_MAP_ACC_GEARED = 6,
225225
LEARNED_STEER_VEL = 7,
226226
DELAY_STEER_ACC_GEARED_WO_FALL_GUARD = 8,
227-
ACTUATION_CMD = 9
227+
ACTUATION_CMD = 9,
228+
ACTUATION_CMD_VGR = 10,
229+
ACTUATION_CMD_MECHANICAL = 11,
230+
ACTUATION_CMD_STEER_MAP = 12,
228231
} vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics
229232
std::shared_ptr<SimModelInterface> vehicle_model_ptr_; //!< @brief vehicle model pointer
230233

‎simulator/simple_planning_simulator/include/simple_planning_simulator/utils/mechanical_controller.hpp

+208
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,208 @@
1+
// Copyright 2024 The Autoware Foundation.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_
16+
#define SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_
17+
18+
#include <deque>
19+
#include <map>
20+
#include <optional>
21+
#include <string>
22+
#include <utility>
23+
24+
namespace autoware::simple_planning_simulator::utils::mechanical_controller
25+
{
26+
27+
using DelayBuffer = std::deque<std::pair<double, double>>;
28+
using DelayOutput = std::pair<std::optional<double>, DelayBuffer>;
29+
30+
DelayOutput delay(
31+
const double signal, const double delay_time, const DelayBuffer & buffer,
32+
const double elapsed_time);
33+
34+
double sign(const double x);
35+
36+
double apply_limits(
37+
const double current_angle, const double previous_angle, const double angle_limit,
38+
const double rate_limit, const double dt);
39+
40+
double feedforward(const double input_angle, const double ff_gain);
41+
42+
double polynomial_transform(
43+
const double torque, const double speed, const double a, const double b, const double c,
44+
const double d, const double e, const double f, const double g, const double h);
45+
46+
struct PIDControllerParams
47+
{
48+
double kp{0.0};
49+
double ki{0.0};
50+
double kd{0.0};
51+
};
52+
53+
struct PIDControllerState
54+
{
55+
double integral{0.0};
56+
double error{0.0};
57+
};
58+
59+
class PIDController
60+
{
61+
public:
62+
PIDController(const double kp, const double ki, const double kd);
63+
64+
[[nodiscard]] double compute(
65+
const double error, const double integral, const double prev_error, const double dt) const;
66+
67+
void update_state(const double error, const double dt);
68+
69+
void update_state(
70+
const double k1_error, const double k2_error, const double k3_error, const double k4_error,
71+
const double dt);
72+
73+
[[nodiscard]] PIDControllerState get_state() const;
74+
75+
void clear_state();
76+
77+
private:
78+
double kp_{0.0}, ki_{0.0}, kd_{0.0};
79+
PIDControllerState state_;
80+
};
81+
82+
struct SteeringDynamicsParams
83+
{
84+
double angular_position{0.0};
85+
double angular_velocity{0.0};
86+
double inertia{0.0};
87+
double damping{0.0};
88+
double stiffness{0.0};
89+
double friction{0.0};
90+
double dead_zone_threshold{0.0};
91+
};
92+
93+
struct SteeringDynamicsState
94+
{
95+
double angular_position{0.0};
96+
double angular_velocity{0.0};
97+
bool is_in_dead_zone{false};
98+
};
99+
100+
struct SteeringDynamicsDeltaState
101+
{
102+
double d_angular_position{0.0};
103+
double d_angular_velocity{0.0};
104+
};
105+
106+
/**
107+
* @brief SteeringDynamics class
108+
* @details This class calculates the dynamics which receives the steering torque and outputs the
109+
* steering tire angle. The steering system is modeled as a spring-damper system with friction and
110+
* dead zone.
111+
*/
112+
class SteeringDynamics
113+
{
114+
public:
115+
SteeringDynamics(
116+
const double angular_position, const double angular_velocity, const double inertia,
117+
const double damping, const double stiffness, const double friction,
118+
const double dead_zone_threshold);
119+
120+
[[nodiscard]] bool is_in_dead_zone(
121+
const SteeringDynamicsState & state, const double input_torque) const;
122+
123+
[[nodiscard]] SteeringDynamicsDeltaState calc_model(
124+
const SteeringDynamicsState & state, const double input_torque) const;
125+
126+
void set_state(const SteeringDynamicsState & state);
127+
128+
[[nodiscard]] SteeringDynamicsState get_state() const;
129+
130+
void clear_state();
131+
132+
void set_steer(const double steer);
133+
134+
private:
135+
SteeringDynamicsState state_;
136+
const double inertia_;
137+
const double damping_;
138+
const double stiffness_;
139+
const double friction_;
140+
const double dead_zone_threshold_;
141+
};
142+
143+
struct StepResult
144+
{
145+
DelayBuffer delay_buffer{};
146+
double pid_error{0.0};
147+
SteeringDynamicsDeltaState dynamics_d_state{};
148+
bool is_in_dead_zone{false};
149+
};
150+
151+
struct MechanicalParams
152+
{
153+
double kp{0.0};
154+
double ki{0.0};
155+
double kd{0.0};
156+
double ff_gain{0.0};
157+
double dead_zone_threshold{0.0};
158+
double poly_a{0.0};
159+
double poly_b{0.0};
160+
double poly_c{0.0};
161+
double poly_d{0.0};
162+
double poly_e{0.0};
163+
double poly_f{0.0};
164+
double poly_g{0.0};
165+
double poly_h{0.0};
166+
double inertia{0.0};
167+
double damping{0.0};
168+
double stiffness{0.0};
169+
double friction{0.0};
170+
double delay_time{0.0};
171+
172+
// limit
173+
double angle_limit{0.0};
174+
double rate_limit{0.0};
175+
double steering_torque_limit{0.0};
176+
double torque_delay_time{0.0};
177+
};
178+
179+
class MechanicalController
180+
{
181+
public:
182+
explicit MechanicalController(const MechanicalParams & mechanical_params);
183+
184+
[[maybe_unused]] double update_euler(
185+
const double input_angle, const double speed, const double prev_input_angle, const double dt);
186+
187+
double update_runge_kutta(
188+
const double input_angle, const double speed, const double prev_input_angle, const double dt);
189+
190+
void clear_state();
191+
192+
void set_steer(const double steer);
193+
194+
private:
195+
DelayBuffer delay_buffer_;
196+
PIDController pid_;
197+
SteeringDynamics steering_dynamics_;
198+
const MechanicalParams params_;
199+
200+
[[nodiscard]] StepResult run_one_step(
201+
const double input_angle, const double speed, const double prev_input_angle, const double dt,
202+
const DelayBuffer & delay_buffer, const PIDController & pid,
203+
const SteeringDynamics & dynamics) const;
204+
};
205+
206+
} // namespace autoware::simple_planning_simulator::utils::mechanical_controller
207+
208+
#endif // SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_

0 commit comments

Comments
 (0)
Please sign in to comment.