Código fuente para los ejemplos de control difuso

2. Código fuente para los ejemplos de control difuso#

El ejemplo FIS está compuesto por varios archivos Python que implementan y prueban un sistema de inferencia difusa aplicado al control y simulación de un vehículo sencillo. Los archivos incluidos son los siguientes:

2.1. fuzzy_control.py#

Contiene la definición principal del controlador difuso. En este archivo se especifican las variables lingüísticas de entrada y salida, las funciones de membresía y el conjunto de reglas difusas que conforman el sistema de inferencia.

Código de Sakai#
 1import my_fis as fc
 2import rear_wheel_sim as rw_sim
 3import path
 4import numpy as np
 5
 6def compute_rmse(traces):
 7    errors = np.array([tr.error for tr in traces])
 8    return np.sqrt(np.mean(errors**2))
 9
10
11
12if __name__ == "__main__":
13    print("rear wheel feedback tracking start!!")
14
15    paths = [
16        ([0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0], [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0]),
17        ([0.0, 1.0, 2.5, 5.0, 7.5, 3.0, -1.0], [0.0, -4.0, 6.0, 6.5, 3.0, 5.0, -2.0]),
18        ([0.0, 2.0, 2.5, 5.0, 7.5, -3.0, -1.0], [0.0, 3.0, 6.0, 6.5, 5.0, 5.0, -2.0]),
19    ]
20
21    #controller = None
22    controller = fc.get_controller()
23    rmses = []
24    for ax, ay in paths:
25        goal = [ax[-1], ay[-1]]
26        reference_path = path.CubicSplinePath(ax, ay)
27
28        result = rw_sim.simulacion(
29            reference_path,
30            goal,
31            controller=controller,
32        )
33
34        traces = result["traces"]
35        rmse = compute_rmse(traces)
36        rmses.append(rmse)
37
38        # Visualización (opcional): se ejecuta después de la simulación
39        #rw_sim.animate(result, pause=0.001)
40        rw_sim.plot(result)
41
42    print(float(np.mean(rmses)))
43

2.2. my_fis.py#

Implementa la construcción y configuración del sistema FIS, sirviendo como módulo central que encapsula la lógica del sistema difuso y permite su reutilización desde otros scripts.

Código de Sakai#
 1import numpy as np
 2import skfuzzy as fuzz
 3from skfuzzy import control as ctrl
 4
 5
 6def build_fis(params=None):
 7    """
 8    Construye el sistema de inferencia difusa (FIS).
 9
10    params: dict | None
11        Parámetros opcionales para modificar las funciones de membresía.
12        En esta versión base, se ignoran o se usan valores por defecto.
13    """
14
15    # Universos (sin normalizar todavía)
16    e_th = ctrl.Antecedent(np.linspace(-1.5, 1.5, 201), 'e_th')  # rad aprox.
17    e = ctrl.Antecedent(np.linspace(-3.0, 3.0, 201), 'e')  # m
18    omega = ctrl.Consequent(np.linspace(-8.0, 8.0, 201), 'omega')  # rad/s
19
20    # e_th: NS, Z, PS
21    e_th['NS'] = fuzz.trapmf(e_th.universe, [-1.5, -1.5, -0.4, 0.0])
22    e_th['Z'] = fuzz.trimf(e_th.universe, [-0.15, 0.0, 0.15])
23    e_th['PS'] = fuzz.trapmf(e_th.universe, [0.0, 0.4, 1.5, 1.5])
24
25    # e: NS, Z, PS
26    e['NS'] = fuzz.trapmf(e.universe, [-3.0, -3.0, -0.8, 0.0])
27    e['Z'] = fuzz.trimf(e.universe, [-0.30, 0.0, 0.30])
28    e['PS'] = fuzz.trapmf(e.universe, [0.0, 0.8, 3.0, 3.0])
29
30    # omega: NS, Z, PS
31    omega['NS'] = fuzz.trapmf(omega.universe, [-8.0, -8.0, -2.5, 0.0])
32    omega['Z'] = fuzz.trimf(omega.universe, [-0.80, 0.0, 0.80])
33    omega['PS'] = fuzz.trapmf(omega.universe, [0.0, 2.5, 8.0, 8.0])
34
35    # Reglas explícitas (3x3)
36    rules = [
37        ctrl.Rule(e_th['NS'] & e['NS'], omega['PS']),
38        ctrl.Rule(e_th['NS'] & e['Z'], omega['PS']),
39        ctrl.Rule(e_th['NS'] & e['PS'], omega['Z']),
40
41        ctrl.Rule(e_th['Z'] & e['NS'], omega['PS']),
42        ctrl.Rule(e_th['Z'] & e['Z'], omega['Z']),
43        ctrl.Rule(e_th['Z'] & e['PS'], omega['NS']),
44
45        ctrl.Rule(e_th['PS'] & e['NS'], omega['Z']),
46        ctrl.Rule(e_th['PS'] & e['Z'], omega['NS']),
47        ctrl.Rule(e_th['PS'] & e['PS'], omega['NS']),
48    ]
49
50    fis = ctrl.ControlSystem(rules)
51    return fis
52
53
54def get_controller(params=None):
55    """
56   Devuelve un controlador callable: (e_th, e) -> omega.
57   """
58
59    fis = build_fis(params)
60    sim = ctrl.ControlSystemSimulation(fis)
61
62    def controller(e_th, e):
63        # scikit-fuzzy acumula estado interno; para simulación en lazo cerrado
64        # suele ser más robusto reiniciar en cada evaluación.
65        sim.reset()
66        sim.input['e_th'] = float(e_th)
67        sim.input['e'] = float(e)
68        sim.compute()
69        return float(sim.output['omega'])
70
71    return controller
72
73
74def plot_mfs(params=None):
75    """
76   Visualiza las funciones de membresía (modo exploración).
77   """
78    fis = build_fis(params)
79    for variable in fis.fuzzy_variables:
80        variable.view()
81    
82
83if __name__ == "__main__":
84    # Ejecuta: python my_fis.py
85    # para inspeccionar las funciones de membresía.
86    plot_mfs()

2.3. angle.py#

Este script proporciona funciones auxiliares relacionadas con el cálculo y manejo de ángulos, utilizadas durante la simulación para determinar errores de orientación o referencias geométricas.

Código de Sakai#
 1
 2# Upstream file from PythonRobotics (Atsushi Sakai et al.).
 3# Licensed under the MIT License. See LICENSE file for details.
 4# Unmodified copy.
 5# https://github.com/AtsushiSakai/PythonRobotics
 6
 7import numpy as np
 8from scipy.spatial.transform import Rotation as Rot
 9import math
10
11def rot_mat_2d(angle):
12    """
13    Create 2D rotation matrix from an angle
14
15    Parameters
16    ----------
17    angle :
18
19    Returns
20    -------
21    A 2D rotation matrix
22
23    Examples
24    --------
25    >>> angle_mod(-4.0)
26
27
28    """
29    return Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
30
31
32def angle_mod(x, zero_2_2pi=False, degree=False):
33    """
34    Angle modulo operation
35    Default angle modulo range is [-pi, pi)
36
37    Parameters
38    ----------
39    x : float or array_like
40        A angle or an array of angles. This array is flattened for
41        the calculation. When an angle is provided, a float angle is returned.
42    zero_2_2pi : bool, optional
43        Change angle modulo range to [0, 2pi)
44        Default is False.
45    degree : bool, optional
46        If True, then the given angles are assumed to be in degrees.
47        Default is False.
48
49    Returns
50    -------
51    ret : float or ndarray
52        an angle or an array of modulated angle.
53
54    Examples
55    --------
56    >>> angle_mod(-4.0)
57    2.28318531
58
59    >>> angle_mod([-4.0])
60    np.array(2.28318531)
61
62    >>> angle_mod([-150.0, 190.0, 350], degree=True)
63    array([-150., -170.,  -10.])
64
65    >>> angle_mod(-60.0, zero_2_2pi=True, degree=True)
66    array([300.])
67
68    """
69    if isinstance(x, float):
70        is_float = True
71    else:
72        is_float = False
73
74    x = np.asarray(x).flatten()
75    if degree:
76        x = np.deg2rad(x)
77
78    if zero_2_2pi:
79        mod_angle = x % (2 * np.pi)
80    else:
81        mod_angle = (x + np.pi) % (2 * np.pi) - np.pi
82
83    if degree:
84        mod_angle = np.rad2deg(mod_angle)
85
86    if is_float:
87        return mod_angle.item()
88    else:
89        return mod_angle
90
91
92
93def Pi_2_pi(angle):
94    while(angle > math.pi):
95        angle= angle - 2.0 * math.pi
96    while(angle < -math.pi):
97        angle=angle + 2.0 * math.pi
98    return angle

2.4. path.py#

Define la trayectoria o camino que debe seguir el sistema simulado. Este módulo se utiliza para generar puntos de referencia y evaluar el desempeño del controlador difuso respecto a la ruta deseada.

Código de Sakai#
 1# Upstream file from PythonRobotics (Atsushi Sakai et al.).
 2# Licensed under the MIT License. See LICENSE file for details.
 3# Unmodified copy.
 4
 5import numpy as np
 6from scipy import interpolate
 7from scipy import optimize
 8import math
 9from angle import Pi_2_pi
10
11
12class CubicSplinePath:
13    def __init__(self, x, y):
14        x, y = map(np.asarray, (x, y))
15        s = np.append([0], (np.cumsum(np.diff(x) ** 2) + np.cumsum(np.diff(y) ** 2)) ** 0.5)
16
17        self.X = interpolate.CubicSpline(s, x)
18        self.Y = interpolate.CubicSpline(s, y)
19
20        self.dX = self.X.derivative(1)
21        self.ddX = self.X.derivative(2)
22
23        self.dY = self.Y.derivative(1)
24        self.ddY = self.Y.derivative(2)
25
26        self.length = s[-1]
27
28    def calc_yaw(self, s):
29        # Ref Theta
30        dx, dy = self.dX(s), self.dY(s)
31        return np.arctan2(dy, dx)
32
33    def calc_curvature(self, s):
34        # K(s) La curvatura de S en s
35        dx, dy = self.dX(s), self.dY(s)
36        ddx, ddy = self.ddX(s), self.ddY(s)
37        return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2) ** (3 / 2))
38
39    def __find_nearest_point(self, s0, x, y):
40
41        def calc_distance(_s, *args):
42            # Se busca un parámetro gamma para S(t) el cual
43            # minimize la distancia
44            # s(t) = argmin/g  (xr(t), yr(t)) - (xref(g),yref (g))
45            # s0 es el punto de inicio de la búsqueda
46            # _s son las opciones que se prueban po fmin_cg
47            _x, _y = self.X(_s), self.Y(_s)
48            return (_x - args[0]) ** 2 + (_y - args[1]) ** 2
49
50
51        def calc_distance_jacobian(_s, *args):
52            # gradiente del error para encontrar gamma
53            _x, _y = self.X(_s), self.Y(_s)
54            _dx, _dy = self.dX(_s), self.dY(_s)
55            return 2 * _dx * (_x - args[0]) + 2 * _dy * (_y - args[1])
56
57        minimum = optimize.fmin_cg(calc_distance, s0, calc_distance_jacobian, args=(x, y), full_output=True, disp=False)
58        return minimum
59
60    def calc_track_error(self, x, y, s0):
61        ret = self.__find_nearest_point(s0, x, y)
62
63        s = ret[0][0] # punto gamma en la ruta con distancia más corta
64        e = ret[1]    # distancia minima
65
66        k = self.calc_curvature(s)
67        yaw = self.calc_yaw(s) # ref
68
69        dxl = self.X(s) - x
70        dyl = self.Y(s) - y
71        angle = Pi_2_pi(yaw - math.atan2(dyl, dxl))
72        if angle < 0:
73            e *= -1
74        # No se usan, solo e, Theta, k, s
75        # Algunas veces es un np array
76        if isinstance(e, np.ndarray):
77            e = e[0]
78        return e, k, yaw, s
79
80def calc_target_speed(yaw, yaw_ref, direction):
81    target_speed = 10 / 3
82
83    dyaw = yaw_ref - yaw
84    switch = math.pi / 4.0 <= dyaw < math.pi / 2.0
85
86    if switch:
87        direction *= -1
88        return 0.0, direction
89
90    if direction != 1:
91        return -target_speed, direction
92
93    return target_speed, direction

2.5. rear_wheel_sim.py#

Implementa la simulación del modelo cinemático del vehículo con rueda trasera, integrando el controlador difuso para calcular las acciones de control y evaluar su comportamiento dinámico.

Código de Sakai#
  1
  2# Based on PythonRobotics (Atsushi Sakai et al.).
  3# Licensed under the MIT License. See LICENSE file for details.
  4# Modified substantially for this book: ODE simulation + fuzzy control, refactoring of animation, plots.
  5
  6from scipy.integrate import odeint
  7import matplotlib.pyplot as plt
  8import math
  9import numpy as np
 10from angle import Pi_2_pi
 11import warnings
 12import path
 13from dataclasses import dataclass
 14
 15# Parámetros iniciales
 16L= 2.9      # longitud del vehiculo en mts
 17ERROR_MAX = 10 # En metros
 18
 19
 20@dataclass
 21class SimulationTrace:
 22    """
 23    Registro temporal de la simulación.
 24    """
 25    t: int          # tiempo (s)
 26    x: float        # posición x (m)
 27    y: float        # posición y (m)
 28    yaw: float      # orientación (rad)
 29    v: float        # velocidad lineal (m/s)
 30    error_theta: float  # error de orientación (rad)
 31    error: float    # error lateral (m)
 32    path_s: float   # parámetro de progreso sobre la ruta
 33
 34
 35
 36
 37# Modelo del robot estilo bicicleta
 38#
 39# Paden, Brian, et al.
 40# "A survey of motion planning and control techniques for self-driving urban vehicles."
 41# IEEE Transactions on intelligent vehicles 1.1(2016): 33 - 55.
 42# https://arxiv.org/abs/1604.07446
 43
 44def modelo(z, t, delta, aceleracion):
 45    x, y, teta, v = z
 46    dx_dt    = v * np.cos(teta)
 47    dy_dt    = v * np.sin(teta)
 48    dteta_dt = v / L * np.tan(delta)
 49    dv_dt    = aceleracion
 50
 51    return [dx_dt, dy_dt,dteta_dt,dv_dt]
 52
 53def paden_control(error, error_theta, v, k):
 54    """
 55    Controlador de referencia basado en Paden et al.
 56    """
 57
 58    # Constantes del controlador (baseline)
 59    KTH = 1.0
 60    KE = 0.5
 61
 62    omega = (
 63        v * k * math.cos(error_theta) / (1.0 - k * error)
 64        - KTH * abs(v) * error_theta
 65        - KE * v * math.sin(error_theta) / error_theta * error
 66    )
 67
 68    return omega
 69
 70# Control rueda trasera
 71def control_rueda_trasera(v, yaw0, e, k, yaw_ref, controller, params):
 72    # calcular el error
 73    error_theta = Pi_2_pi(yaw0 - yaw_ref)
 74    omega = 0.0
 75    if not controller:
 76        omega = paden_control(e, error_theta, v, k)
 77    else:
 78        omega = controller(error_theta, e)
 79    if error_theta == 0.0 or omega == 0.0 or v == 0.0:
 80        return 0.0
 81
 82    delta = math.atan2(L * omega / v, 1.0)
 83    return delta
 84
 85def pid_control(velocidad_objetivo, v):
 86    Kp = 1.0
 87    a = Kp * (velocidad_objetivo - v)
 88    return a
 89
 90def simulacion(ruta, meta_objetivo, controller=None, params=None):
 91    # posiciones iniciales
 92    x0 = 0.0
 93    y0 = 0.0
 94    yaw0 = 0.0
 95    v0 = 0.0
 96    s0 = 0
 97    direction = 1
 98    z0 = x0, y0, yaw0, v0
 99
100    traces = [SimulationTrace(t=0, x=x0, y=y0, yaw=yaw0, v=v0, error_theta=0, error=0, path_s=s0)]
101
102    # defines un arreglo de los tiempos que vas a medir de 0-10 seg, y los partes en 100 pedazos
103    # lo pones en 101 para que haga 100 pedazos
104    t = np.linspace(1, 50, 501)
105
106    for i in range(len(t) - 1):
107
108        goal_flag = False
109        error_flag = False
110
111        # di = metodo de control
112        # aceleracion
113        # control_rueda_trasera = feedback por la retroaliemntacion que da
114        e, k, yaw_ref, s0 = ruta.calc_track_error(x0, y0, s0)
115        # estaba en 100, vamos a ver que pasa si lo reducimos a 10
116        if abs(e) > ERROR_MAX:
117            # pass
118            error_flag = True
119            break
120
121        error_t = Pi_2_pi(yaw0 - yaw_ref)
122
123        try:
124            di = control_rueda_trasera(v0, yaw0, e, k, yaw_ref, controller, params)
125        except Exception as ex:
126            error_flag = True
127            break
128
129        speed_ref, direction = path.calc_target_speed(yaw0, yaw_ref, direction)
130        aceleracion = pid_control(speed_ref, v0)
131
132        inputs = (di, aceleracion)
133
134        z = None
135        # integración
136        with warnings.catch_warnings():
137            warnings.simplefilter("ignore")
138            z = odeint(modelo, z0, [0, 0.1], args=inputs)
139
140        z0 = z[-1]
141        x0, y0, yaw0, v0 = z0  # le asignas el ultimo valor de z que es donde estan los valores
142
143        traces.append(SimulationTrace(t=i, x=x0, y=y0, yaw=yaw0, v=v0, error_theta=error_t, error=e, path_s=s0))
144
145        dx = x0 - meta_objetivo[0]
146        dy = y0 - meta_objetivo[1]
147
148        if math.hypot(dx, dy) <= 0.3:
149            # print("META")
150            goal_flag = True
151            break
152
153    result = {
154        'traces':traces,
155        'error_flag': error_flag,
156        'goal_flag': goal_flag,
157        'ruta': ruta,
158    }
159
160    return result
161
162
163def animate(sim_trace, pause=0.0001):
164    ruta = sim_trace['ruta']
165    traces= sim_trace['traces']
166    for trace in traces:
167        #trace= traces[i]
168        plt.cla()
169        # for stopping simulation with the esc key.
170        plt.gcf().canvas.mpl_connect('key_release_event',
171                                     lambda event: [exit(0) if event.key == 'escape' else None])
172        spline = np.arange(0, ruta.length + 0.09, 0.1)
173        plt.plot(ruta.X(spline), ruta.Y(spline), "-r", label="course")
174        plt.plot(trace.x, trace.y, "ob", label="trajectory")
175        plt.plot(ruta.X(trace.path_s), ruta.Y(trace.path_s), "xg", label="target")
176        plt.axis("equal")
177        plt.grid(True)
178        plt.title(f"speed[km/h]:{round(trace.v * 3.6, 2):.2f}, target s-param:{trace.path_s:.2f}")
179        plt.pause(pause)
180
181def plot(sim_trace):
182    plt.close()
183    plt.subplots(1)
184    ruta = sim_trace['ruta']
185    traces= sim_trace['traces']
186    #plt.plot(ax, ay, "xb", label="input")
187    spline = np.arange(0, ruta.length + 0.09, 0.1)
188    plt.plot(ruta.X(spline), ruta.Y(spline), "-r", label="spline")
189    plt.plot(np.array([t.x for t in traces]), np.array([t.y for t in traces]), "-g", label="tracking")
190    plt.grid(True)
191    plt.axis("equal")
192    plt.xlabel("x[m]")
193    plt.ylabel("y[m]")
194    plt.legend()
195
196    plt.subplots(1)
197    plt.plot(spline, np.rad2deg(ruta.calc_yaw(spline)), "-r", label="yaw")
198    plt.grid(True)
199    plt.legend()
200    plt.xlabel("line length[m]")
201    plt.ylabel("yaw angle[deg]")
202
203    plt.subplots(1)
204    plt.plot(spline, ruta.calc_curvature(spline), "-r", label="curvature")
205    plt.grid(True)
206    plt.legend()
207    plt.xlabel("line length[m]")
208    plt.ylabel("curvature [1/m]")
209
210    plt.show()
211
Contenido