✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、算法改进、程序设计科研仿真。 往期回顾关注个人主页完整代码获取 定制创新 论文复现私信个人信条做科研博学之、审问之、慎思之、明辨之、笃行之是为博学慎思明辨笃行。 内容介绍实现了基于扩展卡尔曼滤波器EKF的同步定位与地图构建SLAM算法结合了里程计观测器并使用 Aruco 标记进行定位和地图构建。以下是对代码的详细分析1. 类定义ekf_slam_simu类实现了 EKF - SLAM 算法的模拟版本。input_velocity根据输入的速度和时间步长预测机器人的状态和协方差。input_measurements根据测量到的地标中心和 ID更新机器人的状态和协方差包括添加新地标。output_robot输出机器人的当前状态和协方差。output_landmarks输出地标的 ID、位置和协方差。state_t存储机器人的估计状态向量初始为[theta; state_t; y]。Cov_t估计状态的协方差矩阵。state_t_pre和Cov_t_pre预测的状态和协方差。sigxy、sigth、siglm分别为线速度、角速度和地标测量的协方差。idx2num从状态向量索引到地标 ID 的映射。属性方法ekf_slam类实现了 EKF - SLAM 算法的另一个版本与ekf_slam_simu类似但在一些计算细节上有所不同例如input_measurements方法中计算Gz和Gx矩阵的部分。odometry_observer类里程计观测器用于估计机器人的位姿和地标位置。odometry_observer构造函数初始化位姿和地标数组。input_measurements根据输入的测量数据、时间步长和速度更新机器人位姿和地标位置。output_state输出机器人的位姿和检测到的地标的位置及 ID。P_hat机器人的位姿矩阵。p_hat存储每个地标位置的单元格数组。gaink、gainl增益参数。iterate迭代次数。c_params映射内部编号到外部 ID 的参数。属性方法2. 主程序部分初始化清除工作区变量并添加必要的路径加载 Aruco 字典和相机参数。加载数据集data_test.mat。初始化最大迭代次数i_max、标记长度marker_length、机器人初始状态state和 EKF - SLAM 对象EKF。初始化速度变量forward_v和angular_v以及存储数据的单元格数组data1。循环部分在第一个子图中使用draw_image函数绘制带有检测到的 Aruco 标记的图像。在第二个子图中使用draw_path函数绘制机器人的路径、估计状态以及地标的位置和协方差。使用Aruco_detector函数检测图像中的 Aruco 标记获取地标中心landmark_centres和 IDmarker_nums。调用EKF.input_measurements方法根据测量数据更新机器人的状态和协方差。从数据集中读取图像img和时间步长dt。EKF 预测调用EKF.input_velocity方法根据当前速度预测机器人的状态和协方差。测量与更新输出状态调用EKF.output_robot和EKF.output_landmarks方法获取机器人的状态、协方差以及地标的信息。积分运动学更新速度forward_v和angular_v并使用integrate_kinematics函数更新机器人的状态。停止检测使用Stop_mark函数检测是否需要停止若需要则增加停止计数num_stop。绘图3. 关键功能说明EKF - SLAM通过预测和更新步骤结合机器人的速度输入和地标的测量数据不断估计机器人的位置和地图中的地标位置。预测步骤基于机器人的运动模型更新步骤则利用地标测量来修正预测结果。Aruco 检测使用Aruco_detector函数检测图像中的 Aruco 标记为 EKF - SLAM 提供地标测量数据。位姿估计与地图构建里程计观测器odometry_observer通过对检测到的地标进行处理估计机器人的位姿和地标位置与 EKF - SLAM 的结果相互补充共同实现定位和地图构建功能。总体而言这段代码通过结合 EKF - SLAM 算法、Aruco 标记检测以及里程计观测实现了一个完整的机器人定位与地图构建系统并通过可视化展示了系统的运行结果。⛳️ 运行结果 部分代码%PiBot MATLAB interface for PenguinPi robot%% A PiBot object provides methods to obtain status from, and control, a wireless% PenguinPi robot.%% Methods::% PiBot constructor% char info in human readable form% display display information about selfect%% Robot control:% setVelocity set motor velocity% stop stop all motors% getCurrent get battery voltage% getVoltage get battery current% resetEncoder zero the hardware encoder counters% resetPose zero the onboard pose estimator%% Camera:% getImage get image from camera%% User interface:% setLED set LEDS 2-4% pulseLED pulse LEDS 2-4% setLEDArray set the blue LED array% getButton get user button status% getDIP get value of DIP switch% printfOLEDn display text to OLED display% setScreen select OLED screen%% Localizer:% getLocalizerPose get robot pose from overhead localizer% getLocalizerImage get IR image from overhead localizer% (c) 2019 Peter Corkeclassdef PiBot handleproperties(Access public)robotlocalizergroupNumendmethodsfunction self PiBot(robotURL, localizerURL, groupNum)%PiBot.PiBot Construct a PiBot selfect%% PB PiBot(IP) creates an object used for communications with the robot% connected via the IP address which is given as a string in dot format.%% eg. pb PiBot(10.0.0.20)%% PB PiBot(IP, IP2, G) creates an object used for communications with the robot% and the localizer. The robots IP address is IP and the localizers IP address% is IP2. Both are given as strings in dot format. G is an integer representing% the teams group number.%% Notes::% - This is a handle class object.%% See also PiBot.setVelocity, PiBot.getImage.assert(ischar(robotURL), Robot robot must be a character array)v regexp(robotURL, ^\d\.\d\.\d\.\d$);assert(~isempty(v) v1, IP address is invalid, can only can contain digits and dots)self.robot http:// string(robotURL) :8080;self.localizer [];if nargin 3assert(ischar(localizerURL), Localizer robot must be a character array)self.localizer http:// string(localizerrobot) :8080;self.groupNum groupNum;elseif nargin ~ 1error(must be 1 or 3 arguments)endendfunction s char(self)% Link.cchar Convert to string%% s PB.char() is a string showing robot parameters in a compact single line format.%% See also PiBot.display.s sprintf(PenguinPi robot at %s, self.robot);if ~isempty(self.localizer)s strcat(s, , with localizer at %s, self.localizer);endendfunction display(self)% PibBot.display Display parameters%% PB.display() displays the robot parameters in compact single line format.%% See also PiBot.char.disp( char(self));end% function delete(self)% endfunction stat setVelocity(self, varargin)%PiBot.setVelocity Set the speeds of the motors%% PB.setVelocity(Vleft, Vright) sets the velocities of the two motors to% the values Vleft and Vright respectively.%% PB.setVelocity(VEL, T) sets the speeds of the two motors to the values in% the 2-vector VEL [Vleft, Vright] and the motion runs for T seconds.% Timing is done locally on the RPi.%% PB.setVelocity(VEL, T, ACC) as above but the speed ramps up and down at% the end of the motion over ACC seconds. The constant velocity part of% the motion lasts for T-ACC seconds, the total motion time is T2*ACC% seconds. This profile moves the same distance as a rectangular speed% profile over T seconds.%% STAT PB.setVelocity(...) as for any of the above call formats, but% returns a structure that contains the encoder count and dead-reckoned% pose at the end of the motion.%% Notes::% - The motor speed is 10V encoders per second.% - If T is given the total distance travelled is 10V*T encoders.% - If ACC is also given the total distance travelled is 10V*(T-ACC)% encoders.%% See also PiBot.stop.tout weboptions(Timeout,20);if length(varargin{1}) 1% then (SA, SB) classic formatvel(1) varargin{1}; vel(2) varargin{2};assert(all(isreal(vel)), arguments must be real);vel round(vel); % convert to intvel min(max(vel, -100), 100); % clipjson webread(self.robot/robot/set/velocity, value, vel);elseif length(varargin{1}) 2% then (SPEED), (SPEED, T) or (SPEED, T, A)vel varargin{1};assert(all(isreal(vel)), arguments must be real);vel round(vel); % convert to intvel min(max(vel, -100), 100); % clipif length(varargin) 1json webread(self.robot/robot/set/velocity, value, vel, tout);elseif length(varargin) 2duration varargin{2};assert(duration 0, duration must be positive);assert(duration 20, duration must be network timeout);if length(varargin) 3accel varargin{3};assert(accel duration/2, accel must be accel/2);json webread(self.robot/robot/set/velocity, value, vel, time, duration, acceleration, accel, tout);elsejson webread(self.robot/robot/set/velocity, value, vel, time, duration, tout);endendendif nargout 0stat jsondecode(json);endendfunction stat stop(self)%PiBot.stop Stop all motors%% PB.stop() stops all motors.%% STAT PB.stop() as above but returns a structure that contains the encoder% count and dead-reckoned pose at the end of the motion.%% See also PiBot.setVelocity.json webread(self.robot/robot/stop);if nargout 0stat jsondecode(json);endendfunction resetEncoder(self)%PiBot.resetEncoder Stop all motors and reset encoders%% PB.resetEncoder() stop all motors and reset encoders.%% See also PiBot.stop, PiBot.setMotorSpeed.webread(self.robot/robot/hw/reset);endfunction resetPose(self)%PiBot.resetPose Reset the onboard pose estimator%% PB.resetPose() will zero the estimated state of the onboard% pose estimator, xytheta0%webread(self.robot/robot/pose/reset);endfunction v getVoltage(self)%PiBot.getVoltage Get battery voltage%% PB.getVoltage() is the battery voltage in volts.%% See also PiBot.getCurrent.v str2num( webread(self.robot/battery/get/voltage) ) /1000.0;endfunction c getCurrent(self)%PiBot.getCurrent Get battery current%% PB.getCurrent() is the battery voltage in amps.%% See also PiBot.getCurrent.c str2num( webread(self.robot/battery/get/current) ) /1000.0;endfunction setLED(self, i, s)%PiBot.setLED Set yellow LED%% PB.setLED(num, state) sets the yellow LED num to the state.%% Notes::% - LED number must in the range 2 to 4.% - state is an integer (0 or 1), or a logical (false or true).%% See also PiBot.pulseLed.assert(i2 i4, invalid LED);assert(s0 || s1, invalid state)webread(self.robot/led/set/state, id, i, value, s);endfunction pulseLED(self, i, duration)%PiBot.pulseLED Pulse yellow LED%% PB.pulseLED(num, time) pulses the yellow LED num to the on state% for time (in seconds).%% Notes::% - LED number must in the range 2 to 4.% - pulse time is in the range 0 to 0.255 seconds.%% See also PiBot.pulseLed.assert(i2 i4, invalid LED);assert(duration0 duration0.255, invalid duration)webread(self.robot/led/set/count, id, i, value, duration*1000);endfunction d getDIP(self)%PiBot.getDIP Get value of DIP switch%% PB.getDIP() is the value of the DIP switch.%% Notes::% - SW4 is the LSB% - SW3 and SW4 are used by the onboard softwared webread(self.robot/hat/dip/get);endfunction b getButton(self)%PiBot.getButton Get user button%% PB.getButton() is the number of times the rightmost button has been% pushed since the last call to this function.b webread(self.robot/hat/button/get);endfunction setLEDArray(self, v)%PiBot.setLEDArray sets the blue LED array%% PB.setLEDArray(v) sets the blue LED array on top of the robot to the% 16-bit integer value v. Each bit represents an LED in the array. The% LSB is the bottom right LED and number increasing upwards and to the% left.webread(self.robot/hat/ledarray/set, value, v);endfunction printfOLED(self, varargin)%PiBot.printfOLED write formatted text to OLED display%% PB.printfOLED(fmt, ...) has printf() like semantics and writes a% string to the user text screen of the OLED display. The display is% only 21x4 characters. New strings are written at the bottom and% scroll up. Long lines are wrapped. Line feed (\n) starts a new% line and form feed (\f) clears the screen. The display is automatically% to the USER TEXT screen.%% eg.% pb.printfOLED(hello world!\n);% pb.printfOLED(the answer is %d\n, 42)webread(self.robot/hat/screen/print, value, sprintf(varargin{:}));endfunction setScreen(self, S)%PiBot.setScreen select screen on OLED%% PB.setScreen(i) sets the OLED display to show information screen S. The OLED% display can show a number of information screens and they can be selected% using the leftmost button or this method where S is:%% 0 IP address% 1 user text, see printfOLED% 2 battery level% 3 encoder values% 4 controller values% 5 system statistics% 6 control loop timing data% 7 error messages% 8 last datagram receivedwebread(self.robot/hat/screen/set, value, screen);endfunction img getImage(self)%PiBot.getImagee Get image from camera%% PB.getImage() is an RGB image captured from the front camera of the% robot.%% See also PiBot.getCurrent.img webread(self.robot/camera/get);endendend% function im getLocalizerImage(self)% %PiBot.getLocalizerImage get overhead image from localizer% %% % IM PiBot.getLocalizerImage() is the infra-red image captured% % by the camera in the overhead localizer system.% %% % Notes::% % - Requires that the object was initialized for the localizer% % - The image is greyscale% %%% if ~isempty(self.localizer)% im webread(self.localizer/camera/get, group, self.groupNum);% else% im [];% end% end%% function p getLocalizerPose(self)% %PiBot.getLocalizerPose get robot pose from localizer% %% % X PiBot.getLocalizerPose() is the pose of the robot as a structure with% % elements x, y and theta.% %% % Notes::% % - Requires that the object was initialized for the localizer% % - Requires that the robot is on the arena under the localizer% %%% if ~isempty(self.localizer)% p jsondecode( webread(self.localizer/pose/get, group, self.groupNum) );% else% p [];% end% end%% function print_json(j1, j2)% state1 jsondecode(j1);% state2 jsondecode(j2);% fprintf(Initial\n);% state1.encoder% state1.pose% fprintf(Final\n);% state2.encoder% state2.pose% end 参考文献更多免费数学建模和仿真教程关注领取