✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。
🍎完整代码获取 定制创新 论文复现点击：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：做科研，博学之、审问之、慎思之、明辨之、笃行之，是为：博学慎思，明辨笃行。
🔥 内容介绍
一、引言
同步定位与建图（SLAM）是移动机器人领域的核心问题之一，旨在让机器人在未知环境中实时构建地图并确定自身位置。扩展卡尔曼滤波（EKF）作为一种经典的状态估计方法，在 SLAM 中得到了广泛应用。它能够有效处理机器人运动和传感器测量中的不确定性，通过对机器人状态和地图特征的联合估计，实现精确的定位与建图。
二、SLAM 问题概述
（一）SLAM 的基本概念
SLAM 问题可描述为：机器人在一个未知环境中移动，通过自身携带的传感器（如激光雷达、相机等）获取环境信息。在移动过程中，机器人需要同时估计自身的位姿（位置和姿态）以及构建环境地图。地图可以是基于特征点的地图、栅格地图或拓扑地图等不同形式，而机器人位姿的准确估计是构建精确地图的基础，地图又为机器人的定位提供重要参考，两者相互依赖、相互促进。
（二）SLAM 面临的挑战
- 不确定性
：机器人的运动存在误差，例如车轮打滑、电机控制精度有限等，导致机器人实际运动与预期运动不一致。同时，传感器测量也会引入噪声，如激光雷达测量距离存在误差，相机图像识别可能出现错误。这些不确定性使得准确估计机器人位姿和地图特征变得困难。
- 计算复杂度
：随着机器人在环境中移动，地图中的特征数量不断增加，需要处理的数据量迅速增长。如何在保证估计精度的同时，高效地处理大量数据，是 SLAM 面临的重要挑战之一。
三、扩展卡尔曼滤波（EKF）原理
（一）卡尔曼滤波基础
卡尔曼滤波是一种最优线性递推估计方法，用于处理线性系统中的状态估计问题。它基于系统的状态方程和观测方程，通过预测和更新两个步骤，不断地对系统状态进行估计。在预测步骤中，根据上一时刻的状态估计和系统的动态模型，预测当前时刻的状态；在更新步骤中，利用当前时刻的观测数据对预测状态进行修正，得到更准确的状态估计。
（二）扩展卡尔曼滤波
⛳️ 运行结果
📣 部分代码
classdef Robot < handle
properties
% Robot properties
wheelbase % Width of wheel base (dist. between opposite wheels)
length % Length of robot
R % True pose [x y a]
r % Estimated pose [x y a]
q % True system noise
u % Control vector [dx da]
maxspeed % Maximum speed (m/s)
min_turn_rad % Turning radius (m)
curr_wpt % Current waypoint reached
end
methods
function rob = Robot % Constructor method
% Default values
rob.wheelbase = 1;
rob.length = 1.5;
rob.R = [0, 0, 0]';
rob.r = rob.R;
rob.q = [0.02;pi/180]; % 2cm, 1 deg
rob.u = [0.1; 0.02];
rob.maxspeed = 1.4; % 1.4m/s ~ 5km/h
rob.min_turn_rad= 0.5;
rob.curr_wpt = 1;
end
% Move the robot and obtain new pose and Jacobians.
%
% Inputs:
% rob : Robot object
% n : Noise (n = [nx na]')
% Optional:
% strRobType : Robot pose to move (true or estimated)
%
% Outputs:
% r_new : New position
% RNEW_r : Jacobian of r_new wrt. r
% RNEW_u : Jacobian of r_new wrt. u
function [r_new, RNEW_r, RNEW_u] = move(rob, n, strRobType)
if nargin < 3
% Robot position (p = [x y]') and orientation (a):
pose = rob.r; a = rob.r(3);
elseif strcmp(strRobType, 'true')
pose = rob.R; a = rob.R(3);
else
error('strRobType can only take the value "true"');
end
% Control input:
dx = rob.u(1) + n(1);
da = rob.u(2) + n(2);
dp = [dx; 0];
% Determine new orientation
a_new = a + da;
% Ensure that angle is between -pi and pi
a_new = getPiToPi(a_new);
if nargout == 1
% Translate robot to new position in global frame
p_new = transToGlobal(pose, dp);
else % Get Jacobians
% Translate robot to new position in global frame
[p_new, PNEW_r, PNEW_dp] = transToGlobal(pose, dp);
% Jacobians of r_a_new:
ANEW_a = 1;
ANEW_da = 1;
% Hence, Jacobians of r_new:
RNEW_r = [PNEW_r; 0 0 ANEW_a];
% We define the perturbation such that it is in the same space
% as control u. This means that there is uncertainty in how much
% the robot advances and how much it turns. This means that the
% Jacobians of RNEW with respect to u are the same as the Jacobian
% of RNEW with respect to n. Less coding necessary, so a good
% option.
RNEW_u = [PNEW_dp(:, 1) zeros(2, 1); 0 ANEW_da];
end
r_new = [p_new; a_new];
end
% Return the vertices of a triangle of dimensions base*length,
% centred at (x, y) and with orientation a.
%
% Inputs:
% r = [x y a]' : local co-ordinate frame of robot
% base : size of triangle base
% length : length of triangle
%
% Outputs:
% p = [x1 x2 x3 x4;
% y1 y2 y3 y4] : vertices of resulting triangle
function p = computeTriangle(rob, strRobType, r_pos)
if nargin < 2
pose = rob.r;
elseif nargin < 3 && strcmp(strRobType, 'true')
pose = rob.R;
elseif nargin < 4
pose = r_pos;
else
error('strRobType can only take the value "True"');
end
p = zeros(2, 4);
p(:, 1) = [-rob.length/3; -rob.wheelbase/2];
p(:, 2) = [2*rob.length/3; 0];
p(:, 3) = [-rob.length/3; rob.wheelbase/2;];
for i = 1:3
p(:, i) = transToGlobal(pose, p(:, i));
end
% We include a fourth point that is equal to the first point in the
% triangle. This allows us to 'close' the triangle when plotting it
% using the line function.
p(:, 4) = p(:, 1);
end
function steer(rob, Wpts)
% Determine if current waypoint reached
wpt = Wpts(:, rob.curr_wpt);
% Distance from current waypoint
dist = sqrt((wpt(1)-rob.R(1))^2 + (wpt(2)-rob.R(2))^2);
if dist < rob.min_turn_rad
if rob.curr_wpt < size(Wpts, 2)
rob.curr_wpt = rob.curr_wpt + 1;
else
rob.curr_wpt = 1; % Go back to the start
end
wpt = Wpts(:, rob.curr_wpt);
end
% Change in robot orientation to face current waypoint
da = getPiToPi(atan2(wpt(2)-rob.R(2), wpt(1)-rob.R(1))-rob.R(3));
% Amount by which to perturb current robot control:
dda = da - rob.u(2);
du = [0; dda];
rob.setControl(du);
end
% Perturb control vector by an amount du = [accel_x; accel_a].
function u = setControl(rob, du)
u = rob.u + du;
% Make sure that angular control is between -pi and pi
u(2) = getPiToPi(u(2));
if abs(u(1)) > rob.maxspeed
% Adjust speed so that max speed isn't exceeded
u(1) = sign(u(1)) * rob.maxspeed;
end
turning_radius = u(1)/u(2); % Using formula v = omega * r
if abs(turning_radius) < rob.min_turn_rad
% Adjust angular velocity so that min turning radius isn't
% exceeded
u(2) = sign(u(2)) * abs(u(1)) / rob.min_turn_rad;
end
rob.u = u;
end
end
end
🔗 参考文献
[1]李永强.基于信息滤波器的同步定位与地图创建技术的研究[D].哈尔滨工业大学[2026-05-10].DOI:CNKI:CDMD:2.2008.195662.
