基于SLAM系统建图仿真，完成定位仿真
Posted zylyehuo
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了基于SLAM系统建图仿真，完成定位仿真相关的知识，希望对你有一定的参考价值。
基于SLAM系统完成建图仿真，详见之前的博客
参考链接
Part 1: 准备工作
navigation安装
sudo apt install ros-melodic-navigation
Part 2: 编写 amcl 相关 launch文件
查看并复制 amcl 模版
roscd amcl
gedit examples/amcl_diff.launch
mycar_ws/src/nav_demo/launch/nav04_amcl.launch
<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<!-- set coordinate system: odom、map、base_link -->
<param name="odom_frame_id" value="odom"/>
<param name="base_frame_id" value="base_footprint"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>
编写 amcl 测试 launch文件
mycar_ws/src/nav_demo/launch/test_amcl.launch
<!-- 测试文件 -->
<launch>
<!-- 启动 rviz -->
<node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
<node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
<node pkg="rviz" type="rviz" name="rviz"/>
<!-- 加载地图服务 -->
<include file="$(find nav_demo)/launch/nav03_map_server.launch" />
<!-- 启动 amcl 节点 -->
<include file="$(find nav_demo)/launch/nav04_amcl.launch" />
</launch>
Part 3: 执行 amcl
启动 gazebo 仿真环境
source ./devel/setup.bash
roslaunch mycar environment.launch
启动 amcl 测试 launch文件
source ./devel/setup.bash
roslaunch nav_demo test_amcl.launch
启动键盘控制节点
rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.3 _turn:=0.5
配置 rviz
RobotModel(加载机器人模型)
Map(显示SLAM构建的地图)
PoseArray(显示定位结果)
Part 4: 控制机器人运动，观察定位现象
调整至以下三个窗口均能观察到的布局，便于观看现象
通过键盘即可操控机器人运动
基于Kinect深度图像采集和SLAM室内地图创建算法的matlab仿真
目录
一、理论基础
移动机器人的定位和地图创建是机器人领域的热点研究问题，也是导航中重要环节。 对于已知环境中的机器人自主定位和已知机器人位置的地图创建已经有了一些实用的解决方法。 然而在很多环境中机器人不能利用全局定位系统进行定位,而且事先获取机器人工作环境的地图很困难，甚至是不可能的。这时机器人需要在自身位置不确定的条件下，在完全未知环境中，创建地图同时利用该地图进行自主定位和导航。 这就是移动机器人的同时定位与地图创建(SLAM) 问题，也称为CML (Concurrent Mapping and Localization) 。该研究领域的代表人物有Smith，Self 和Cheeseman。由于SLAM算法具有重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。
SLAM算法有很多重要的属性，影响着地图特征和机器人位置估计中的不确定性。包括：状态估计的收敛性，估计过程的一致性，状态协方差矩阵更新的计算复杂度。
收敛性：SLAM算法具有三个重要的收敛性，这三个关键的收敛结果是：A.地图协方差矩阵的任意子矩阵地行列式随着每一次观测单调下降；B.在极限情况下，随着观测数量的增加，特征估计变得完全相关。C.地图的精度与第一个特征被观测时机器人的位置精度有关。这些结果表明：随着观测数量的增加，地图估计的不确定性将降低到有限的误差范围内，地图特征的关系将是完全确定的。
一致性：为了维护SLAM算法估计的一致性，对状态协方差矩阵进行更新维护是必要的。既然对环境的观测是相对机器人的，所以机器人估计中的任何误差和地图估计中的误差使绝对相关的。在没有其它外部的关于特征和机器人位置信息情况下，为了使系统状态估计的误差在有限的范围内，保持状态估计之间的一致性是很必要的。所以就必须维护机器人状态与环境特征之间的协方差矩阵。
计算复杂度：SLAM算法应用到大规模环境时的一个重要局限性是计算环境特征之间，特征与机器人之间的相关信息时的计算负担。由于特征数量很大，协方差矩阵的更新维护导致了SLAM算法的计算复杂性。对于那些包含上万个特征的环境，计算负担使得系统协方差的更新变的难以执行。所以需要一个有效的方法来提高算法的计算效率。
根据所依据的理论基础的不同，SLAM可以分为以下几种：
（1）基于扩展卡尔曼滤波（EKF）的CML/ SLAM
这是最常用的一种SLAM方法，适合解决非线性系统的估计问题。该方法用平面坐标表示机器人和环境特征的位置，将机器人运动与环境特征的关系描述为两个非线性模型：机器人运动模型和观测模型.通过这两个模型，运用扩展卡尔曼理论的思想来实现。主要包括预测与更新两个阶段。
（2）基于概率的CML/ SLAM
尽管不如EKF 那样流行，但由于用概率表达机器人定位问题的不确定性非常自然合理，基于概率的CML 也吸引了很多人的目光。其中，较流行是最大相似性(Maximum Likelihood Estimation ，MLE)方法。一种非常有效的最大相似性估计算法称为Baum-Welch (或α-β) 算法[30]，基于这种方法的机器人制图与定位问题可看作是机器人位置与环境特征位置的最大相似性估计问题。该算法包括两步：E-Step (expectation) 和M-Step (maximization)。
（3）基于粒子滤波器( particle filter) 的CML/SLAM
粒子滤波器定位也称为Monte Carlo 定位，其基本思想是用一组滤波器来估计机器人的可能位置(处于该位置的概率)，每个滤波器对应一个位置，利用观测对每个滤波器进行加权传播，从而使最有可能的位置的概率越来越高。
（4）基于空间扩展信息滤波器的SLAM
该方法由Sebastian Thrun 等人提出，是对EKF 算法的改进。它不再用协方差矩阵表示空间信息的相关性，取而代之用空间信息矩阵来表示空间信息间的内在固有的关系，并且使用网状数据结构仅维护邻近的环境特征(地图)。
（5）基于集合理论估计的SLAM
基于集合理论的方法不假设噪声服从某种分布，而只假设噪声是有界的。 具体方法是：定义一个可行状态集合(feasible state set) 和一个观测集合(measurement set) ，前者表示机器人和环境特征的状态估计，后者表示符合条件(观测误差小于边界)的状态集合。算法首先根据机器人本身的状态方程计算可行状态集合，然后根据观测值计算观测集合，最后取两个集合的交集作为某时刻的经估计校正后的机器人与环境特征的状态集合(地图)。
此外根据所制地图的不同，SLAM又可以分为：基于栅格的SLAM；基于特征的SLAM以及基于拓扑结构的SLAM。
在基于栅格的SLAM算法中，环境表示为一组具有特定分辨度的栅格的组合。每个栅格中有特定概率值来表示其被占据的可能性。使用基于栅格SLAM算法的时候，假设各个栅格的状态变量是相互独立的，而没有考虑栅格之间的关联性。另外，当机器人采用该种SLAM算法运行较长一段路程后，很难再回到出发点。这是因为该方法中仅在局部坐标中考虑了估计的不确定性，却没考虑局部坐标与全局坐标中的相互联系。总的说来，基于栅格的SLAM方法可以提供较为丰富的环境描述，对于局部环境下的规划与导航有很大帮助，但是通过该方法无法获得一致性较强的全局地图。
基于特征地图的SLAM算法是将环境表示为一组组参数化的特征值，比如说点，线，角等。这里的特征指的是环境中那些突显于背景的、易于传感器分辨检测的且可以通过参数描述的东西。使用这种方法的时候，必须对环境中不同类型的特征分别建立测量模型以便准确提取。基于特征的SLAM算法是最为流行的一种，尽管在环境地图描述中仅仅用到特征的位置值，但是还可以通过大小，颜色等信息进行匹配等。这种SLAM算法常常是基于卡尔曼滤波的。使用这种方法时，状态向量不仅包括机器人的位姿信息而且还有特征的位置信息。随着新特征的不断提取与确认，状态向量将不断的变大。因为用于描述环境的特征值的测量都是相对机器人的，所以对环境特征测量的不确定性是与机器人位姿估计的不确定性息息相关的。可以在理论上证明，随着时间的不断推移、测量的不断进行，地图中的特征将是完全相关的，也就是说此时随意给定一个特征的绝对坐标值，将会得到一个精准的地图。在环境特征容易识别的场合下，该算法运用很好，但是对于特性不是太明显的非结构化环境中，算法的运用遇到难题。
在基于拓扑结构的SLAM算法中使用“图”来描述环境。具体说是通过节点和弧线来描述环境。每个节点表示环境中突显的地方，边用来表示相邻节点间的相对位置。对于纯粹的拓扑地图来说，无须知道每个节点的具体位置，它只用来表示节点间的连通性。但是该方法也有很大的弊端，比如当环境稍微复杂一些时，其对环境的识别能力会明显下降且无法识别相近的环境。
尽管SLAM算法的理论基础已经被很好的研究，但是要将其更好的运用于实际，特别是大型的非结构化环境，仍有大量理论和实际的问题需要解决包括：计算效率、地图管理、局部地图与全局地图之间的协调融合、数据匹配以及传感器管理等]。
基于特征SLAM算法架构
二、核心程序
clc;
clear;
close all;
warning off;
addpath 'func\\'
addpath 'func\\toolbox\\'
IF = 0;
%导入kirect数据，并获得点云图
N = 50;%数据量非常大，只导入100个进行处理
MAP = zeros(600,1800);
IMAGES = cell(1,N);
indx = 0;
if IF == 1
for i = 100:1:100+N
indx = indx + 1;
if i < 10
NAME1 = ['KinectDepthFrames\\','kD[00',num2str(i),'].mat'];
NAME2 = ['KinectColorFrames\\','rgb[00',num2str(i),'].jpg'];
end
if i < 100 & i >= 10
NAME1 = ['KinectDepthFrames\\','kD[0',num2str(i),'].mat'];
NAME2 = ['KinectColorFrames\\','rgb[0',num2str(i),'].jpg'];
end
if i >= 100
NAME1 = ['KinectDepthFrames\\','kD[',num2str(i),'].mat'];
NAME2 = ['KinectColorFrames\\','rgb[',num2str(i),'].jpg'];
end
load(NAME1);
%RGB
I0 = imresize(imread(NAME2),1);
I = I0(4:end-3,4:end-6,:);
%Depth,缩小图像像素，加快仿真速度，
data0 = (imresize(Frame.bin,1));
data = data0(4:end-3,4:end-6);
%数据滤波
Y = func_depath_filter(data);
%首先进行拼接
if indx == 1
mosaic1 = I;
mosaic2 = Y;
end
if indx > 2
[tmps,up,M3,left,N3,Yoffset,Xoffset,M1,N1,tform] = imMosaic(I,mosaic1,1);
mosaic1 = tmps(4:end-3,4:end-6,:);
tmp2 = zeros(size(tmps,1),size(tmps,2));
tmp2(up:up+M3-1,left:left+N3-1) = imtransform(mosaic2,tform);
tmp2(Yoffset+1:Yoffset+M1,Xoffset+1:Xoffset+N1) = Y;
mosaic2 = tmp2(4:end-3,4:end-6,:);
end
figure(1);
subplot(2,2,1);
imshow(I,[]);title('RGB图');
subplot(2,2,2);
imshow(Y,[]);title('深度图');
if i > 1
subplot(2,2,3);
imshow(mosaic1,[])
title('拼接');
subplot(2,2,4);
imshow(mosaic2,[])
title('深度图拼接');
end
drawnow;
end
save R.mat mosaic1 mosaic2
else
load R.mat
figure(1);
subplot(1,3,1);
imshow(mosaic1,[])
title('拼接');
subplot(1,3,2);
imshow(mosaic2,[])
title('深度图拼接');
[X,Y,Z] = func_realword(mosaic2);
figure(1);
subplot(1,3,3);
map2ds = func_map_gen(X,Y,Z);%产生地图
imshow(map2ds,[]);
title('地图');
end
三、仿真测试结果
A23-24
以上是关于基于SLAM系统建图仿真，完成定位仿真的主要内容，如果未能解决你的问题，请参考以下文章
基于Kinect深度图像采集和SLAM室内地图创建算法的matlab仿真