【车辆控制】基于卡尔曼滤波和PID实现自主车辆系统分布式控制附Matlab源代码

matlab科研社

于 2025-06-08 16:45:25 发布

阅读量957

点赞数 30

CC 4.0 BY-SA版权

文章标签：分布式 matlab 开发语言

本文链接：https://blog.youkuaiyun.com/Matlab_dashi/article/details/148515006

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码及仿真咨询内容私信。

🔥 内容介绍

一、研究背景

随着汽车工业的智能化发展，自主车辆系统成为交通领域的研究热点。在复杂的交通环境中，准确的状态估计和高效的控制策略是实现自主车辆安全、稳定运行的关键。卡尔曼滤波（Kalman Filter，KF）作为一种高效的状态估计方法，能够在存在噪声的情况下，对系统状态进行最优估计；比例 - 积分 - 微分（PID）控制则凭借结构简单、鲁棒性强等优点，在车辆控制领域得到广泛应用。同时，分布式控制架构可以提升系统的灵活性和可靠性，降低集中控制的压力。将卡尔曼滤波和 PID 控制应用于自主车辆系统的分布式控制中，能够充分发挥各技术优势，提高车辆对复杂环境的适应能力，对推动自主驾驶技术的发展具有重要意义。

二、核心技术原理

2.1 卡尔曼滤波原理

卡尔曼滤波是一种基于线性系统和高斯噪声假设的最优状态估计算法，适用于处理动态系统的状态估计问题。对于线性离散系统，其状态方程和观测方程可表示为：

⛳️ 运行结果

📣 部分代码

% One Vehicle estimation (showing effects of measurements and EKF estimation)

clear; clc; close all;

%% ---- PARAMETERS ----

dt = 0.1; % Time step
N = 200; % Number of iterations
L = 2.5; % Vehicle length

% Initial States [x, y, theta, v]
X1 = [0; 0; 0; 2]; % Main Robot
X2 = [10; 5; pi/6; 2]; % Radar Reference

% Initial Control Inputs (acceleration, steering angle)
u1 = [0.1; 0.05];
u2 = [0.05; 0.1];

% Coefficients for Radar Variance modeling
alpha = 0.05;
beta = 1.5;

% Initial System Covariance
P = diag([1, 1, 0.1, 0.5]);

% Initial Covariances
Q = diag([0.05, 0.05, 0.01, 0.02]);
var_GPS = 0.5;
var_R = 0.2;
Pd = 0.1;
R_k = diag([var_GPS, var_GPS, var_R + Pd, var_R + Pd]);

%% Trajectories and Plot Initialization

actual_trajectory1 = zeros(2, N);
actual_trajectory2 = zeros(2, N);
estimated_trajectory1 = zeros(2, N);
estimated_trajectory2 = zeros(2, N);
gps_measures = zeros(2, N);
radar_measures = zeros(2, N);

% Initial States for simulation
X1_real = X1;
X2_real = X2;
X1_est = X1;
X2_est = X2;

figure;
hold on; grid on; axis equal;
xlabel('X Position (m)'); ylabel('Y Position (m)');
title('EKF Vehicle Estimation - Two Robots');
xlim([-2 50]); ylim([-2 50]);
% Trajectory Plot
actual_plot1 = plot(NaN, NaN, 'k--', 'LineWidth', 2, 'DisplayName', 'Actual Trajectory 1');
actual_plot2 = plot(NaN, NaN, 'm--', 'LineWidth', 2, 'DisplayName', 'Actual Trajectory 2');
estimated_plot1 = plot(NaN, NaN, 'b-', 'LineWidth', 2, 'DisplayName', 'EKF Estimation 1');
gps_plot = plot(NaN, NaN, 'rx', 'MarkerSize', 8, 'DisplayName', 'GPS Measures');
radar_plot = plot(NaN, NaN, 'go', 'MarkerSize', 8, 'DisplayName', 'Radar Measures');

% Robot Shape
[X_tri1, Y_tri1] = RobotFigure(X1_real,2);
robot_patch1 = patch(X_tri1, Y_tri1, 'r');
[X_tri2, Y_tri2] = RobotFigure(X2_real,2);
robot_patch2 = patch(X_tri2, Y_tri2, 'g');

%% ---- EXTENDED KALMAN FILTER LOOP ----

for k = 2:N

% Real Vehicle evolution
X1_real = Car_Like_Model(X1_real, u1, dt, L);
X2_real = Car_Like_Model(X2_real, u2, dt, L);

actual_trajectory1(:, k) = X1_real(1:2);
actual_trajectory2(:, k) = X2_real(1:2);

% Generazione misure GPS e Radar con rumore
Z = Measurement_Model(X1_real) + normrnd(0, sqrt(diag(R_k)));