✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。完整代码获取 定制创新 论文复现点击Matlab科研工作室 关注我领取海量matlab电子书和数学建模资料个人信条做科研博学之、审问之、慎思之、明辨之、笃行之是为博学慎思明辨笃行。 内容介绍一、引言同步定位与建图SLAM是移动机器人领域的核心问题之一旨在让机器人在未知环境中实时构建地图并确定自身位置。扩展卡尔曼滤波EKF作为一种经典的状态估计方法在 SLAM 中得到了广泛应用。它能够有效处理机器人运动和传感器测量中的不确定性通过对机器人状态和地图特征的联合估计实现精确的定位与建图。二、SLAM 问题概述一SLAM 的基本概念SLAM 问题可描述为机器人在一个未知环境中移动通过自身携带的传感器如激光雷达、相机等获取环境信息。在移动过程中机器人需要同时估计自身的位姿位置和姿态以及构建环境地图。地图可以是基于特征点的地图、栅格地图或拓扑地图等不同形式而机器人位姿的准确估计是构建精确地图的基础地图又为机器人的定位提供重要参考两者相互依赖、相互促进。二SLAM 面临的挑战不确定性机器人的运动存在误差例如车轮打滑、电机控制精度有限等导致机器人实际运动与预期运动不一致。同时传感器测量也会引入噪声如激光雷达测量距离存在误差相机图像识别可能出现错误。这些不确定性使得准确估计机器人位姿和地图特征变得困难。计算复杂度随着机器人在环境中移动地图中的特征数量不断增加需要处理的数据量迅速增长。如何在保证估计精度的同时高效地处理大量数据是 SLAM 面临的重要挑战之一。三、扩展卡尔曼滤波EKF原理一卡尔曼滤波基础卡尔曼滤波是一种最优线性递推估计方法用于处理线性系统中的状态估计问题。它基于系统的状态方程和观测方程通过预测和更新两个步骤不断地对系统状态进行估计。在预测步骤中根据上一时刻的状态估计和系统的动态模型预测当前时刻的状态在更新步骤中利用当前时刻的观测数据对预测状态进行修正得到更准确的状态估计。二扩展卡尔曼滤波⛳️ 运行结果 部分代码classdef Robot handleproperties% Robot propertieswheelbase % Width of wheel base (dist. between opposite wheels)length % Length of robotR % True pose [x y a]r % Estimated pose [x y a]q % True system noiseu % Control vector [dx da]maxspeed % Maximum speed (m/s)min_turn_rad % Turning radius (m)curr_wpt % Current waypoint reachedendmethodsfunction rob Robot % Constructor method% Default valuesrob.wheelbase 1;rob.length 1.5;rob.R [0, 0, 0];rob.r rob.R;rob.q [0.02;pi/180]; % 2cm, 1 degrob.u [0.1; 0.02];rob.maxspeed 1.4; % 1.4m/s ~ 5km/hrob.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. ufunction [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);elseerror(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 orientationa_new a da;% Ensure that angle is between -pi and pia_new getPiToPi(a_new);if nargout 1% Translate robot to new position in global framep_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];endr_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 trianglefunction p computeTriangle(rob, strRobType, r_pos)if nargin 2pose rob.r;elseif nargin 3 strcmp(strRobType, true)pose rob.R;elseif nargin 4pose r_pos;elseerror(strRobType can only take the value True);endp 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:3p(:, 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);endfunction steer(rob, Wpts)% Determine if current waypoint reachedwpt Wpts(:, rob.curr_wpt);% Distance from current waypointdist sqrt((wpt(1)-rob.R(1))^2 (wpt(2)-rob.R(2))^2);if dist rob.min_turn_radif rob.curr_wpt size(Wpts, 2)rob.curr_wpt rob.curr_wpt 1;elserob.curr_wpt 1; % Go back to the startendwpt Wpts(:, rob.curr_wpt);end% Change in robot orientation to face current waypointda 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 piu(2) getPiToPi(u(2));if abs(u(1)) rob.maxspeed% Adjust speed so that max speed isnt exceededu(1) sign(u(1)) * rob.maxspeed;endturning_radius u(1)/u(2); % Using formula v omega * rif abs(turning_radius) rob.min_turn_rad% Adjust angular velocity so that min turning radius isnt% exceededu(2) sign(u(2)) * abs(u(1)) / rob.min_turn_rad;endrob.u u;endendend 参考文献[1]李永强.基于信息滤波器的同步定位与地图创建技术的研究[D].哈尔滨工业大学[2026-05-10].DOI:CNKI:CDMD:2.2008.195662.更多免费数学建模和仿真教程关注领取