基于卡尔曼滤波器的集中式机器人轨迹定位算法

一、系统架构与算法原理

1.1 集中式架构

复制代码

多个机器人 → 无线通信 → 中央处理器（Kalman Filter） → 全局轨迹估计
      ↑____________共享地图/锚点____________↓

关键特点：

每个机器人本地只做传感与通信，不做复杂滤波（或做轻量级预测）
中央节点维护所有机器人的状态向量 和全局一致性（地图、时间基准）
观测可来自：里程计、GPS、UWB测距、激光/视觉相对观测

二、运动与观测模型

2.1 单机器人状态（2D）

xk= $xkykx˙ky˙k$ ⊤ \mathbf{x}_k= \begin{bmatrix}x_k&y_k&\dot x_k&\dot y_k\end{bmatrix}^\top xk= $xkykx˙ky˙k$ ⊤

2.2 运动模型（匀速 + 随机加速度）

xk=F xk−1+B uk+wk,wk∼N(0,Q) \mathbf{x}k=\mathbf F\,\mathbf x{k-1}+ \mathbf B\,\mathbf u_k+\mathbf w_k,\qquad \mathbf w_k\sim\mathcal N(\mathbf 0,\mathbf Q) xk=Fxk−1+Buk+wk,wk∼N(0,Q)

常数速度离散形式（Δt=T\Delta t=TΔt=T）：

F= $10T0010T00100001$ ,B= $T2/200T2/2T00T$ \mathbf F= \begin{bmatrix} 1&0&T&0\\ 0&1&0&T\\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix},\qquad \mathbf B= \begin{bmatrix}T^2/2&0\\0&T^2/2\\T&0\\0&T\end{bmatrix} F= 10000100T0100T01 ,B= T2/20T00T2/20T

（若没有控制输入 u\mathbf uu，可令 Bu=0\mathbf B\mathbf u=0Bu=0；此时过程噪声 Q\mathbf QQ 承担"模型不完美"的角色。）

2.3 观测模型

绝对观测 （GPS/动捕）：zk=Hxk+vk, H= $I2 02\times2$ \mathbf z_k=\mathbf H\mathbf x_k+\mathbf v_k,\;\mathbf H= $\\mathbf I_2\\;\\mathbf 0_{2\\times2}$ zk=Hxk+vk,H= $I202\times2$
距离观测到锚点 （UWB）： di=∥p−pi∥+v\;d_i=\|\mathbf p-\mathbf p_i\|+vdi=∥p−pi∥+v ⇒ 非线性 ⇒ 需用 EKF（一阶线性化）
机器人间相对观测（协作定位）也可纳入同一状态块（扩维或分块Jacobian）

下面的代码先给你一个干净、可跑的集中式 EKF 框架：

每个机器人一个 4维 CV 状态块

中央用 稀疏/分块 方式维护（这里用 cell 维护每个机器人，避免"搜索/全局索引混乱"）

支持 GPS（线性） 与 UWB距离（非线性EKF） 两类观测

三、MATLAB实现代码

3.1 主程序：`centralized_kf_trajectory.m`

matlab 复制代码

%% ========== 集中式 Kalman 机器人轨迹定位 ==========
clear; clc; close all;

%% 1. 参数
N_robots = 2;                 % 机器人数量
T = 0.1;                      % 采样周期 (s)
N_steps = 600;                 % 时间步
gps_period = 5;                % GPS每5步来一次
uwb_period = 3;                % UWB每3步来一次

% 过程噪声 / GPS噪声 / UWB噪声
sigma_Q = diag([0.02, 0.02, 0.1, 0.1].^2);   % 过程
sigma_GPS = diag([0.6, 0.6].^2);             % GPS
sigma_UWB = 0.25^2;                          % 距离噪声方差

% 锚点（UWB，世界坐标）
anchors = [0 0; 5 0; 5 5; 0 5];  % 4个角

rng(42);

%% 2. 模拟真值与"集中式观测生成"
% 真轨迹：绕圆运动（只是demo）
R_circle = 3;
omega = 2*pi/(N_steps*T);

true_traj = cell(N_robots,1);
obs_gps   = cell(N_robots,1);
obs_uwb   = cell(N_robots,1);

for r = 1:N_robots
    offset = (r-1)*pi;  % 相位错开
    Xgt = zeros(4, N_steps);
    for k = 1:N_steps
        th = omega*(k-1)*T + offset;
        Xgt(:,k) = [R_circle*cos(th); R_circle*sin(th);  % x y
                   -R_circle*omega*sin(th); R_circle*omega*cos(th)]; % vx vy
    end
    true_traj{r} = Xgt;

    % GPS观测
    Zgps = zeros(2, N_steps);
    for k = 1:N_steps
        if mod(k-1, gps_period)==0
            Zgps(:,k) = Xgt(1:2,k) + mvnrnd([0 0], sigma_GPS)';
        else
            Zgps(:,k) = nan(2,1);
        end
    end
    obs_gps{r} = Zgps;

    % UWB距离观测（到每个锚）
    Zuwb = cell(1, size(anchors,1));
    for a = 1:size(anchors,1)
        za = nan(1, N_steps);
        for k = 1:N_steps
            if mod(k-1, uwb_period)==0
                d_true = norm(Xgt(1:2,k)-anchors(a,:)');
                za(k) = d_true + sqrt(sigma_UWB)*randn;
            end
        end
        Zuwb{a} = za;
    end
    obs_uwb{r} = Zuwb;
end

%% 3. 中央EKF初始化
F = [1 0 T 0;
     0 1 0 T;
     0 0 1 0;
     0 0 0 1];

H_gps = [1 0 0 0;
         0 1 0 0];

% 每个机器人的滤波器状态
ekf = struct();
for r = 1:N_robots
    ekf(r).x = true_traj{r}(:,1) + [1.5;1.5;0.3;0.3].*randn(4,1); % 故意偏初始
    ekf(r).P = diag([2,2,0.5,0.5].^2);
    ekf(r).traj = nan(4, N_steps);
    ekf(r).traj(:,1) = ekf(r).x;
end

%% 4. 集中式时间更新 + 观测更新（主循环）
R_gps = sigma_GPS;
R_uwb = sigma_UWB;

for k = 2:N_steps
    %% ---- 4.1 时间更新（每个机器人都要做） ----
    for r = 1:N_robots
        ekf(r).x = F * ekf(r).x;                     % 预测均值
        ekf(r).P = F * ekf(r).P * F' + sigma_Q;       % 预测协方差
    end

    %% ---- 4.2 观测更新（谁发来数据，就更新谁） ----
    for r = 1:N_robots
        % (a) GPS线性更新
        if ~isnan(obs_gps{r}(1,k))
            z = obs_gps{r}(:,k);
            y = z - H_gps*ekf(r).x;
            S = H_gps*ekf(r).P*H_gps' + R_gps;
            K = ekf(r).P*H_gps'/S;
            ekf(r).x = ekf(r).x + K*y;
            ekf(r).P = (eye(4)-K*H_gps)*ekf(r).P;
        end

        % (b) UWB非线性更新（EKF一阶线性化）
        if ~isnan(obs_uwb{r}{1}(k))  % 只要有一个锚这步有数据就更新
            for a = 1:size(anchors,1)
                if ~isnan(obs_uwb{r}{a}(k))
                    z_d = obs_uwb{r}{a}(k);
                    p_est = ekf(r).x(1:2);
                    d_est = norm(p_est - anchors(a,:)');
                    % 避免除0
                    if d_est < 1e-6, continue; end
                    % Jacobians: h(x)=||p-p_a||
                    dh_dx = [(p_est(1)-anchors(a,1))/d_est, ...
                              (p_est(2)-anchors(a,2))/d_est, ...
                              0, 0];
                    innov = z_d - d_est;
                    % 角度缠绕保护（距离无缠绕，可省）
                    S = dh_dx*ekf(r).P*dh_dx' + R_uwb;
                    K = ekf(r).P*dh_dx'/S;
                    ekf(r).x = ekf(r).x + K*innov;
                    ekf(r).P = (eye(4)-K*dh_dx)*ekf(r).P;
                end
            end
        end
    end

    % 保存轨迹
    for r=1:N_robots, ekf(r).traj(:,k)=ekf(r).x; end
end

%% 5. 误差评估 + 图
plot_centralized_results(true_traj, ekf, anchors, obs_gps, obs_uwb, N_robots, N_steps, T);

参考代码基于卡尔曼滤波器的集中式机器人轨迹定位算法 www.youwenfan.com/contentcsv/79402.html

四、`plot_centralized_results.m`