Modelo dinámico del vehículo: enfoque de bicicleta

El sistema de control lateral de Apollo de Baidu utiliza el modelo de bicicleta, una simplificación que preserva las características dinámicas esenciales del vehículo para aplicaciones en tiempo real.

Ecuaciones de estado

En coordenadas Frenet, el vector de estado se define como:

\[x = \begin{bmatrix} \text{error\_lateral} \\ \text{error\_orientacion} \\ \dot{\text{error\_lateral}} \\ \dot{\text{error\_orientacion}} \end{bmatrix}\]

Donde:

• error_lateral: desviación perpendicular entre el vehículo y la trayectoria deseada
• error_orientacion: diferencia angular entre la orientación actual y la deseada
• dot_error_lateral: tasa de cambio del error lateral
• dot_error_orientacion: velocidad angular de giro

La entrada de control es el ángulo de dirección delantero: u = \delta_f.

Formulación linealizada

Bajo suposiciones de ángulos pequeños y modelos de neumáticos lineales, el sistema continuo se expresa como:

\[\dot{x} = A x + B u\]

Las matrices A y B dependen de:

• Velocidad del vehículo v
• Distancias axiales l_f, l_r
• Rigideces laterales C_f, C_r
• Masa m e inercia I_z

Matriz A:

\[A = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2(C_f + C_r)}{mv} & \frac{2(C_f + C_r)}{m} & -\frac{2(C_f l_f - C_r l_r)}{mv} \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2(C_f l_f - C_r l_r)}{I_z v} & \frac{2(C_f l_f - C_r l_r)}{I_z} & -\frac{2(C_f l_f^2 + C_r l_r^2)}{I_z v} \end{bmatrix}\]

Matriz B:

\[B = \begin{bmatrix} 0 \\ \frac{2C_f}{m} \\ 0 \\ \frac{2C_f l_f}{I_z} \end{bmatrix}\]

Discretización

Para implementación en tiempo real, se aplica transformación ZOH:

\[x(k+1) = A_d x(k) + B_d u(k)\]

Donde A_d y B_d se derivan mediante integración exponencial con paso de muestreo T_s.

Controlador LQR en Apollo

Arquitectura mejorada

La implementación de Apollo incluye componentes clave:

• Analizador de trayectoria: identifica puntos relevantes y calcula errores
• Resolución de Riccati: computa ganancia óptima mediante ecuación discreta
• Compensación feedforward: ajusta ángulo según curvatura de ruta
• Adaptación dinámica: modifica pesos Q y R según velocidad
• Límites de actuador: restringe velocidad y rango de dirección

Optimización cuadrática

Minimiza la función de costo:

\[J = \sum_{k=0}^{\infty} [x(k)^T Q x(k) + u(k)^T R u(k)]\]

Donde Q y R son matrices de ponderación para estado y control.

Implementación práctica

La ganancia K se calcula resolviendo:

\[P = A_d^T P A_d - A_d^T P B_d (R + B_d^T P B_d)^{-1} B_d^T P A_d + Q\]

\[K = (R + B_d^T P B_d)^{-1} B_d^T P A_d\]

El control total combina retroalimentación, feedforward y compensación adicional:

\[\delta = \delta_{fb} + \delta_{ff} + \delta_{comp}\]

Controlador MPC en Apollo

Extensión a control conjunto

El MPC gestiona control lateral y longitudinal simultáneo con estado extendido:

\[x = [\text{error\_lateral}, \dot{\text{error\_lateral}}, \text{error\_orientacion}, \dot{\text{error\_orientacion}}, \text{error\_longitudinal}, \dot{\text{error\_longitudinal}}]^T\]

Entrada de control: u = [\delta_f, a]^T.

Optimización en horizonte finito

Resuelve el problema en un horizonte N:

\[\min_{u_0,...,u_{N-1}} \sum_{k=0}^{N-1} [x_k^T Q x_k + u_k^T R u_k] + x_N^T Q_f x_N\]

Con restricciones explícitas en estado y control.

Implementación con OSQP

Transforma el problema a forma cuadrática estándar:

\[\min_{U} \frac{1}{2} U^T H U + f^T U\]

\[\text{sujeto a } G U \leq h\]

Usa solver OSQP para optimización en tiempo real:

apollo::common::math::MpcOsqp mpc_osqp(
    matrix_ad_, matrix_bd_, matrix_q_updated_, matrix_r_updated_,
    matrix_state_, lower_bound, upper_bound, lower_state_bound,
    upper_state_bound, reference_state, mpc_max_iteration_, ...
);

Ejemplos de implementación en MATLAB

Controlador LQR optimizado

function [steering_command, gain_matrix] = lateral_lqr_controller(
    vehicle_velocity, 
    lateral_deviation, 
    heading_deviation, 
    lateral_velocity, 
    angular_velocity, 
    vehicle_specs
)
    % Extraer parámetros del vehículo
    mass = vehicle_specs.vehicle_mass;
    inertia_z = vehicle_specs.inertia_z;
    front_axle = vehicle_specs.front_axle_length;
    rear_axle = vehicle_specs.rear_axle_length;
    front_stiffness = vehicle_specs.front_stiffness;
    rear_stiffness = vehicle_specs.rear_stiffness;
    sampling_interval = vehicle_specs.sampling_time;
    
    % Construir matriz A continuo
    A = zeros(4, 4);
    A(1, 2) = 1;
    A(2, 2) = -2 * (front_stiffness + rear_stiffness) / (mass * vehicle_velocity);
    A(2, 3) = 2 * (front_stiffness + rear_stiffness) / mass;
    A(2, 4) = -2 * (front_stiffness * front_axle - rear_stiffness * rear_axle) / (mass * vehicle_velocity);
    A(3, 4) = 1;
    A(4, 2) = -2 * (front_stiffness * front_axle - rear_stiffness * rear_axle) / (inertia_z * vehicle_velocity);
    A(4, 3) = 2 * (front_stiffness * front_axle - rear_stiffness * rear_axle) / inertia_z;
    A(4, 4) = -2 * (front_stiffness * front_axle^2 + rear_stiffness * rear_axle^2) / (inertia_z * vehicle_velocity);
    
    % Construir matriz B continuo
    B = [0; 2 * front_stiffness / mass; 0; 2 * front_stiffness * front_axle / inertia_z];
    
    % Discretización ZOH
    continuous_model = ss(A, B, eye(4), 0);
    discrete_model = c2d(continuous_model, sampling_interval, 'zoh');
    A_d = discrete_model.A;
    B_d = discrete_model.B;
    
    % Pesos LQR
    Q = diag([12, 0.15, 6, 0.12]);
    R = 0.08;
    
    % Resolver ecuación de Riccati
    [gain_matrix, ~, ~] = dlqr(A_d, B_d, Q, R);
    
    % Vector de estado
    state_vector = [lateral_deviation; lateral_velocity; heading_deviation; angular_velocity];
    feedback_control = -gain_matrix * state_vector;
    
    % Compensación feedforward
    curvature = vehicle_specs.curvature;
    feedforward_control = (front_axle + rear_axle) * curvature + ...
                          (mass * vehicle_velocity^2 / (2 * rear_stiffness * (front_axle + rear_axle))) * ...
                          (rear_axle / front_stiffness - front_axle / rear_stiffness) * curvature;
    
    % Comando final con límites
    steering_command = feedback_control + feedforward_control;
    max_angle = deg2rad(30);
    steering_command = max(min(steering_command, max_angle), -max_angle);
end

Controlador MPC con OSQP

function [control_sequence, total_cost] = mpc_controller(
    initial_state, 
    reference_trajectory, 
    controller_config
)
    % Configuración del MPC
    prediction_steps = controller_config.prediction_horizon;
    state_weights = controller_config.state_weights;
    control_weights = controller_config.control_weights;
    sample_time = controller_config.sample_time;
    
    % Modelo dinámico discreto
    [A_d, B_d] = compute_vehicle_model(controller_config);
    
    % Variables de optimización
    state_trajectory = sdpvar(4, prediction_steps + 1);
    control_trajectory = sdpvar(1, prediction_steps);
    
    % Restricción inicial
    constraints = [state_trajectory(:, 1) == initial_state];
    
    % Dinámica del sistema
    for k = 1:prediction_steps
        constraints = [constraints, 
                      state_trajectory(:, k+1) == A_d * state_trajectory(:, k) + B_d * control_trajectory(:, k)];
    end
    
    % Límites de control
    max_steering = deg2rad(30);
    constraints = [constraints, -max_steering <= control_trajectory <= max_steering];
    
    % Restricción de variación de control
    if isfield(controller_config, 'max_control_rate')
        for k = 2:prediction_steps
            delta_control = control_trajectory(k) - control_trajectory(k-1);
            constraints = [constraints, 
                          -controller_config.max_control_rate <= delta_control <= controller_config.max_control_rate];
        end
    end
    
    % Función de costo
    total_cost = 0;
    for k = 1:prediction_steps
        state_error = state_trajectory(:, k) - reference_trajectory(k, :)';
        total_cost = total_cost + state_error' * state_weights * state_error + ...
                     control_trajectory(k)' * control_weights * control_trajectory(k);
    end
    
    % Costo terminal
    terminal_error = state_trajectory(:, prediction_steps + 1) - reference_trajectory(prediction_steps + 1, :)';
    total_cost = total_cost + terminal_error' * controller_config.terminal_weights * terminal_error;
    
    % Resolver optimización
    options = sdpsettings('solver', 'osqp', 'verbose', 0);
    solution = optimize(constraints, total_cost, options);
    
    if solution.problem == 0
        control_sequence = value(control_trajectory);
        total_cost = value(total_cost);
    else
        control_sequence = zeros(1, prediction_steps);
        total_cost = inf;
    end
end

function [A_d, B_d] = compute_vehicle_model(config)
    velocity = config.velocity;
    mass = config.mass;
    inertia_z = config.inertia_z;
    front_axle = config.front_axle_length;
    rear_axle = config.rear_axle_length;
    front_stiffness = config.front_stiffness;
    rear_stiffness = config.rear_stiffness;
    Ts = config.sample_time;
    
    A = [0, 1, 0, 0;
         0, -2*(front_stiffness+rear_stiffness)/(mass*velocity), 2*(front_stiffness+rear_stiffness)/mass, -2*(front_stiffness*front_axle-rear_stiffness*rear_axle)/(mass*velocity);
         0, 0, 0, 1;
         0, -2*(front_stiffness*front_axle-rear_stiffness*rear_axle)/(inertia_z*velocity), 2*(front_stiffness*front_axle-rear_stiffness*rear_axle)/inertia_z, -2*(front_stiffness*front_axle^2+rear_stiffness*rear_axle^2)/(inertia_z*velocity)];
    
    B = [0; 2*front_stiffness/mass; 0; 2*front_stiffness*front_axle/inertia_z];
    
    continuous_sys = ss(A, B, eye(4), 0);
    discrete_sys = c2d(continuous_sys, Ts, 'zoh');
    A_d = discrete_sys.A;
    B_d = discrete_sys.B;
end

Recomendaciones prácticas

Ajuste de parámetros

• LQR: Aumentar pesos de estado para baja velocidad, reducir para alta velocidad
• MPC: Horizonte de predicción 10-20 pasos; pesos de estado 10-100 veces mayores que pesos de control

Optimización de rendimiento

• Usar solvers especializados como OSQP para problemas cuadráticos
• Implementar inicialización térmica basada en soluciones anteriores
• Separar procesamianto en hilos independientes para controlador y estimador

Robustez avanzada

• Adaptar modelos dinámicos en tiempo real según condiciones de conducción
• Incorporar observadores de perturbaciones para compensar efectos externos