基于卡尔曼滤波器的集中式机器人轨迹定位算法
一、系统架构与算法原理
1.1 集中式架构
多个机器人 → 无线通信 → 中央处理器（Kalman Filter） → 全局轨迹估计
↑____________共享地图/锚点____________↓
关键特点：
- 每个机器人本地只做传感与通信，不做复杂滤波（或做轻量级预测）
- 中央节点维护所有机器人的状态向量和全局一致性（地图、时间基准）
- 观测可来自：里程计、GPS、UWB测距、激光/视觉相对观测
二、运动与观测模型
2.1 单机器人状态（2D）
2.2 运动模型（匀速 + 随机加速度）
常数速度离散形式（
（若没有控制输入
2.3 观测模型
- 绝对观测（GPS/动捕）：
- 距离观测到锚点（UWB）：
⇒ 非线性 ⇒ 需用 EKF（一阶线性化） - 机器人间相对观测（协作定位）也可纳入同一状态块（扩维或分块Jacobian）
下面的代码先给你一个干净、可跑的集中式 EKF 框架：
- 每个机器人一个 4维 CV 状态块
- 中央用 稀疏/分块 方式维护（这里用 cell 维护每个机器人，避免“搜索/全局索引混乱”）
- 支持 GPS（线性） 与 UWB距离（非线性EKF） 两类观测
三、MATLAB实现代码
3.1 主程序：centralized_kf_trajectory.m
%% ========== 集中式 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
function plot_centralized_results(true_traj, ekf, anchors, obs_gps, ~, N_robots, N_steps, T)
figure('Position',[100 100 1300 620]);
tvec = (0:N_steps-1)*T;
% 1) 轨迹对比
subplot(2,3,[1 2]);
hold on;
plot(anchors(:,1),anchors(:,2),'ks','MarkerFaceColor','k'); % 锚点
for r=1:N_robots
plot(true_traj{r}(1,:), true_traj{r}(2,:),'-','LineWidth',2,'DisplayName',sprintf('真实 R%d',r));
plot(ekf(r).traj(1,:), ekf(r).traj(2,:),'--','LineWidth',1.5,'DisplayName',sprintf('EKF R%d',r));
% GPS采样点
gps_valid = find(~isnan(obs_gps{r}(1,:)));
plot(true_traj{r}(1,gps_valid), true_traj{r}(2,gps_valid),'ro','MarkerSize',4,'HandleVisibility','off');
end
xlabel('x (m)'); ylabel('y (m)'); title('集中式EKF轨迹定位'); grid on; axis equal; legend();
% 2) per-robot 位置误差
subplot(2,3,3);
for r=1:N_robots
err = sqrt((ekf(r).traj(1,:)-true_traj{r}(1,:)).^2 + ...
(ekf(r).traj(2,:)-true_traj{r}(2,:)).^2);
plot(tvec, err,'LineWidth',1.5,'DisplayName',sprintf('R%d RMSE',r)); hold on;
end
xlabel('t (s)'); ylabel('pos error (m)'); title('位置误差'); grid on; legend();
% 3) NEES (归一化估计误差平方和) — 一致性检验
subplot(2,3,4);
for r=1:N_robots
nees = zeros(1,N_steps);
for k=1:N_steps
e = true_traj{r}(:,k)-ekf(r).traj(:,k);
nees(k) = e'/ekf(r).P*e;
end
plot(tvec, nees,'LineWidth',1.2,'DisplayName',sprintf('NEES R%d',r)); hold on;
end
% 95%区间（4-dof）
chi2_95 = chi2inv(0.95,4);
plot(tvec, chi2_95*ones(size(tvec)),'r--','DisplayName','\chi^2_{0.95}(4)');
xlabel('t (s)'); ylabel('NEES'); title('一致性 NEES (应大部分低于红线)'); grid on; legend();
% 4) 速度对比（robot1）
subplot(2,3,5);
plot(tvec, true_traj{1}(3,:),'b','LineWidth',2); hold on;
plot(tvec, ekf(1).traj(3,:),'r--','LineWidth',1.5);
xlabel('t (s)'); ylabel('vx (m/s)'); title('Robot1 vx 对比'); grid on;
legend('true','EKF');
% 5) 协方差trace（不确定性半径代理）
subplot(2,3,6);
plot(tvec, trace(ekf(1).P)*ones(size(tvec)),'m','LineWidth',1.5);
xlabel('t (s)'); ylabel('Tr(P)'); title('Robot1 协方差迹（不确定度）'); grid on;
end
