Skip to content

PID

commonroad_control.control.pid.pid_control

PIDControl

Bases: ControllerInterface

Source code in commonroad_control/control/pid/pid_control.py 
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
class PIDControl(ControllerInterface):

    def __init__(self, kp: float, ki: float, kd: float, delta_t: float) -> None:
        """
        PID controller
        :param kp: proportional gain
        :param ki: integral gain
        :param kd: derivative gain
        :param delta_t: controller sampling time in seconds
        """
        super().__init__()
        self._kp: float = kp
        self._ki: float = ki
        self._kd: float = kd
        self._delta_t: float = delta_t

        self._integrated_error: float = 0.0
        self._previous_error: float = 0.0

    @property
    def kp(self) -> float:
        """
        :return: proportional gain
        """
        return self._kp

    @property
    def ki(self) -> float:
        """
        :return: integral gain
        """
        return self._ki

    @property
    def kd(self) -> float:
        """
        :return: derivative gain
        """
        return self._kd

    @property
    def delta_t(self) -> float:
        """
        :return: controller sampling time in seconds
        """
        return self._delta_t

    def compute_control_input(
        self,
        measured_output: float,
        reference_output: float,
    ) -> float:
        """
        Computes control output given the measured and the reference output.
        :param measured_output: measured single output
        :param reference_output: reference single output
        :return: control input
        """
        error: float = reference_output - measured_output
        d_error: float = (error - self._previous_error) / self._delta_t
        self._integrated_error += error * self._delta_t
        self._previous_error = error

        return self._kp * error + self._ki * self._integrated_error + self._kd * d_error

    def reset(self) -> None:
        """
        Reset running error values
        """
        self._integrated_error = 0.0
        self._previous_error = 0.0
delta_t property

Returns:

Type Description
float

controller sampling time in seconds
kd property

Returns:

Type Description
float

derivative gain
ki property

Returns:

Type Description
float

integral gain
kp property

Returns:

Type Description
float

proportional gain
__init__(kp, ki, kd, delta_t)

PID controller

Parameters:

Name Type Description Default
kp float

proportional gain

 required
ki float

integral gain

 required
kd float

derivative gain

 required
delta_t float

controller sampling time in seconds

 required
Source code in commonroad_control/control/pid/pid_control.py 
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
def __init__(self, kp: float, ki: float, kd: float, delta_t: float) -> None:
    """
    PID controller
    :param kp: proportional gain
    :param ki: integral gain
    :param kd: derivative gain
    :param delta_t: controller sampling time in seconds
    """
    super().__init__()
    self._kp: float = kp
    self._ki: float = ki
    self._kd: float = kd
    self._delta_t: float = delta_t

    self._integrated_error: float = 0.0
    self._previous_error: float = 0.0
compute_control_input(measured_output, reference_output)

Computes control output given the measured and the reference output.

Parameters:

Name Type Description Default
measured_output float

measured single output

 required
reference_output float

reference single output

 required

Returns:

Type Description
float

control input
Source code in commonroad_control/control/pid/pid_control.py 
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
def compute_control_input(
    self,
    measured_output: float,
    reference_output: float,
) -> float:
    """
    Computes control output given the measured and the reference output.
    :param measured_output: measured single output
    :param reference_output: reference single output
    :return: control input
    """
    error: float = reference_output - measured_output
    d_error: float = (error - self._previous_error) / self._delta_t
    self._integrated_error += error * self._delta_t
    self._previous_error = error

    return self._kp * error + self._ki * self._integrated_error + self._kd * d_error
reset()

Reset running error values

Source code in commonroad_control/control/pid/pid_control.py 
69
70
71
72
73
74
def reset(self) -> None:
    """
    Reset running error values
    """
    self._integrated_error = 0.0
    self._previous_error = 0.0

commonroad_control.control.pid.pid_long_lat

PIDLongLat

Bases: ControllerInterface

PID-based controller that combines two PID controllers for decoupled longitudinal and lateral control. The longitudinal controller tracks a given velocity profile (by adapting the long. acceleration) and the lateral controller reduces the lateral offset from the reference trajectory (by adapting the steering angle velocity).

Source code in commonroad_control/control/pid/pid_long_lat.py 
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
class PIDLongLat(ControllerInterface):
    """
    PID-based controller that combines two PID controllers for decoupled longitudinal and lateral control. The longitudinal controller
    tracks a given velocity profile (by adapting the long. acceleration) and the lateral controller reduces the
    lateral offset from the reference trajectory (by adapting the steering angle velocity).
    """

    def __init__(
        self,
        kp_long: float,
        ki_long: float,
        kd_long: float,
        kp_lat: float,
        ki_lat: float,
        kd_lat: float,
        delta_t: float,
    ) -> None:
        """
        Initialize controller.
        :param kp_long: proportional gain longitudinal velocity
        :param ki_long: integral gain longitudinal velocity
        :param kd_long: derivative gain longitudinal velocity
        :param kp_lat: proportional gain lateral offset
        :param ki_lat: integral gain lateral offset
        :param kd_lat: derivative gain lateral offset
        :param delta_t: controller sampling time in seconds
        """
        super().__init__()
        self._v_long_pid: PIDControl = PIDControl(kp=kp_long, ki=ki_long, kd=kd_long, delta_t=delta_t)

        self._steer_pid_offset: PIDControl = PIDControl(kp=kp_lat, ki=ki_lat, kd=kd_lat, delta_t=delta_t)
        self._delta_t: float = delta_t

    @property
    def longitudinal_pid(self) -> PIDControl:
        """
        :return: longitudinal PID controller
        """
        return self._v_long_pid

    @property
    def lateral_pid(self) -> PIDControl:
        """
        :return: lateral PID controller
        """
        return self._steer_pid_offset

    @property
    def delta_t(self) -> float:
        """
        :return: controller sampling time in seconds used for both PID controllers
        """
        return self._delta_t

    def compute_control_input(
        self,
        measured_v_long: float,
        reference_v_long: float,
        measured_lat_offset: float,
        reference_lat_offset: float = 0,
    ) -> Tuple[float, float]:
        """
        Computes input from controller given the measured and the reference outputs
        :param measured_v_long: measured longitudinal velocity
        :param reference_v_long: reference longitudinal velocity
        :param measured_lat_offset: measured lateral offset
        :param reference_lat_offset: reference lateral offset, default 0
        :return: control input for acceleration, control input for steering angle velocity
        """
        u_acc: float = self._v_long_pid.compute_control_input(
            measured_output=measured_v_long, reference_output=reference_v_long
        )
        u_steer_lat_offset: float = self._steer_pid_offset.compute_control_input(
            measured_output=measured_lat_offset, reference_output=reference_lat_offset
        )

        return u_acc, u_steer_lat_offset

    def reset(self) -> None:
        """
        Resets internal running states
        """
        self._v_long_pid.reset()
        self._steer_pid_offset.reset()
delta_t property

Returns:

Type Description
float

controller sampling time in seconds used for both PID controllers
lateral_pid property

Returns:

Type Description
PIDControl

lateral PID controller
longitudinal_pid property

Returns:

Type Description
PIDControl

longitudinal PID controller
__init__(kp_long, ki_long, kd_long, kp_lat, ki_lat, kd_lat, delta_t)

Initialize controller.

Parameters:

Name Type Description Default
kp_long float

proportional gain longitudinal velocity

 required
ki_long float

integral gain longitudinal velocity

 required
kd_long float

derivative gain longitudinal velocity

 required
kp_lat float

proportional gain lateral offset

 required
ki_lat float

integral gain lateral offset

 required
kd_lat float

derivative gain lateral offset

 required
delta_t float

controller sampling time in seconds

 required
Source code in commonroad_control/control/pid/pid_long_lat.py 
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
def __init__(
    self,
    kp_long: float,
    ki_long: float,
    kd_long: float,
    kp_lat: float,
    ki_lat: float,
    kd_lat: float,
    delta_t: float,
) -> None:
    """
    Initialize controller.
    :param kp_long: proportional gain longitudinal velocity
    :param ki_long: integral gain longitudinal velocity
    :param kd_long: derivative gain longitudinal velocity
    :param kp_lat: proportional gain lateral offset
    :param ki_lat: integral gain lateral offset
    :param kd_lat: derivative gain lateral offset
    :param delta_t: controller sampling time in seconds
    """
    super().__init__()
    self._v_long_pid: PIDControl = PIDControl(kp=kp_long, ki=ki_long, kd=kd_long, delta_t=delta_t)

    self._steer_pid_offset: PIDControl = PIDControl(kp=kp_lat, ki=ki_lat, kd=kd_lat, delta_t=delta_t)
    self._delta_t: float = delta_t
compute_control_input(measured_v_long, reference_v_long, measured_lat_offset, reference_lat_offset=0)

Computes input from controller given the measured and the reference outputs

Parameters:

Name Type Description Default
measured_v_long float

measured longitudinal velocity

 required
reference_v_long float

reference longitudinal velocity

 required
measured_lat_offset float

measured lateral offset

 required
reference_lat_offset float

reference lateral offset, default 0

 0

Returns:

Type Description
Tuple[float, float]

control input for acceleration, control input for steering angle velocity
Source code in commonroad_control/control/pid/pid_long_lat.py 
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
def compute_control_input(
    self,
    measured_v_long: float,
    reference_v_long: float,
    measured_lat_offset: float,
    reference_lat_offset: float = 0,
) -> Tuple[float, float]:
    """
    Computes input from controller given the measured and the reference outputs
    :param measured_v_long: measured longitudinal velocity
    :param reference_v_long: reference longitudinal velocity
    :param measured_lat_offset: measured lateral offset
    :param reference_lat_offset: reference lateral offset, default 0
    :return: control input for acceleration, control input for steering angle velocity
    """
    u_acc: float = self._v_long_pid.compute_control_input(
        measured_output=measured_v_long, reference_output=reference_v_long
    )
    u_steer_lat_offset: float = self._steer_pid_offset.compute_control_input(
        measured_output=measured_lat_offset, reference_output=reference_lat_offset
    )

    return u_acc, u_steer_lat_offset
reset()

Resets internal running states

Source code in commonroad_control/control/pid/pid_long_lat.py 
85
86
87
88
89
90
def reset(self) -> None:
    """
    Resets internal running states
    """
    self._v_long_pid.reset()
    self._steer_pid_offset.reset()