深入理解如何不费吹灰之力搭建一个无人驾驶车（四）2D-小车自主部分（从无到有自己写一个无人驾驶框架）（CSDN独创）

四、从无到有自己动手写个slam算法（CSDN独创）

注1：必须先看完前三章再看这一章，如果想看得轻松请看《概率机器人》与《机器人学状态估计》完再看
注2：本篇为CSDN独创，转载请备注链接
注3：本篇为自创算法的阉割版，目的是教大家会写
注4：本文力图用白话说明这个原理，希望的是大家学会

下文会提到slam的一些研究方向：1.各传感器误差模型建模问题 2.定位问题 3.建图的占用概率计算问题（数学模型的建立）

4.1、概述
本篇目的就是教大家自己动手写个完整的SLAM系统，首先是从2D开始，等我讲完3D汽车后，我再补充3D的框架如何写。并且下一章也需要用到这一章的知识。
那我们要写哪些东西呢：
1.底盘控制
2.轮式里程计
3.IMU里程计
4.VO及语义slam信息加入
5.GPS里程计
6.定位融合优化算法（如EKF）
7.配置激光雷达
8.栅格建图（若是3D建图，需保存成一个文件，这时候不能用rviz显示，要自己做个软件显示）
9.导航算法。
好，那么开始

4.2、底盘控制
这个我已经在我的第一章写的很详细了，总的来说就是编写一个节点，能实现电脑与编码器通信，包括（读取编码器解析的各轮子速度信息）用于产生轮式里程计及进行PID控制，（能对各轮子速度进行控制）用于进行PID控制及响应其他节点发来的cmd_vel信息并根据此信息对车速进行控制，（不过汽车的运动模型和小车不一样，这个下一章讲）用的是ros定时器：如下：

ros::Timer timer = nh.createTimer(ros::Duration(0.1), timerCallback); 

详见我的第一章，本文主要叙述之前没讲到的东西。
这个节点主要发：轮式里程计信息，收cmd_vel，及用ROS定时器对编码器进行收发

4.3、轮式里程计
同样在我的第一章已经讲了（不过汽车的里程计产生和小车不一样，这个下一章讲），是不是说过假设是个圆？然后里程计的x，y就是t从0到现在位姿增量的累加和，这个幼儿园知识，关键是协方差矩阵，要根据数学建模后算，比较难，我们先用在第三章讲的那种计算方法去算协方差矩阵吧。这个要和底盘控制代码卸载同一个节点里，发布的是wheel_odom信息。
这个节点主要发：轮式里程计信息，收cmd_vel，及用ROS定时器对编码器进行收发

4.4、imu里程计
读取imu数据的包比如AH100b，需要你根据厂家的说明书自己去编程，只要你会点单片机知识，这个小学生都能做。不过imu矫正需要一点技巧：如这篇文章，imu矫正，最后你要发布imu这个话题信息，带协方差矩阵，这个我在第三章给了个tip，大家可以去参考，还有就是如果imu比较好，它会自带卡尔曼滤波输出，如果没有就要自己去写了，见我上一章叙述KF的知识及一阶卡尔曼滤波算法。这里引用上一章的知识：如图，对位姿x方向的估计有这两组，我们更愿意相信数据几的数据？回答是数据2，因为它变化很小，也就是方差较小，这就是卡尔曼滤波的原理，至于在二维是怎样，EKF,UKF具体原理见我上一章。
这个节点主要发：imu里程计信息，及用ROS定时器对imu模块进行通信

4.4、VO及语义slam输入
这里你可以用深度摄像头通过点云估计得到VO信息，也可以直接单目配ORB-SLAM2,或LSD-SLAM,也可以结合imu用VINS算法。不过现在是结合深度学习用这个算法，就是语义slam，你这个可能还会与云服务器通信，这些我们都将在另一篇文章重点介绍，但是协方差矩阵是6*6的，这个必须保证
这个节点主要发：VO里程计信息或点云（如果发点云还需要到base_link的TF）,及用ROS定时器与摄像头进行通信，可能订阅imu信息（如vins）

4.5、GPS里程计输入
这个要根据GPS的厂家说明自己写，无人驾驶对GPS的要求很严格，必须是差分相干GPS，这就像你蒙着眼走路，你总要保证你的手是可以用的吧，（比如用手摸到门，你就知道你在门附近），写完通信后你还要对GPS进行校正，一般可以问问厂家解决。
这个节点主要发：GPS里程计信息，及用ROS定时器与GPS进行通信

4.6、定位融合优化算法
我们之后就可以用EKF融合wheel，imu，vo,gps的信息了，但是都说了是自己写一个框架了。那我们如何写融合？不直接用ROS官方的EKF包（以UKF为例）其他融合算法见我上一章，我对原理做了很详细的介绍。
步骤如下：
1.创建一个节点。（见ROS官网教程）
2.创建一个cpp文件
3.订阅imu到base_link的tf，imu/wheel/vo-gps的信息，必须用ros param（参数服务器）封装，因为这四个是可选项，不是都必要的。
4.根据imu到base_link的tf算出imu在base_link下的增量（有1个，就是转角），然后根据轮式里程计的增量（有3个就是x,y,θ）。及他们的协方差矩阵，通过UKF计算式计算里程计增量，最后里程计就是从t=0到现在的里程计增量累加。协方差矩阵就是按UKF公式算出来的结果见UKF_slam。最后要发布这个odom到base_link的tf，在这里，博主用了自己的方法完成这个过程，所以也希望大家知道，如果大家想研究slam，就是从这里入手。
这个节点主要发：融合后的里程计信息odom_combined，及其到base_link的TF，订阅imu/wheel/gps/vo及监听imu到base_link的tf

4.7、配置激光雷达
一般厂家会写好，思岚科技的居然把从点云到laserscan的包也写了，velodyne居然都可以直接3D建图了（loam），不同厂家不一样，但大多数是只给你一个手册或者只能发pointcloud2信息。如下定义：

pointcloud2类型定义
std_msgs/Header header（ID）
uint32 height(长度，unordered是1）
uint32 width(宽度）            
sensor_msgs/PointField[] fields （这是个结构体数组，里面每个元素有下列）
bool is_bigendian（大端存储？可以去看计算机组成原理）
uint32 point_step （点云字节长度）
uint32 row_step    （点云行长）
uint8[] data            （有用的数据）
bool is_dense        （没有无效点就是真）

或者pointcloud这种信息

std_msgs/Header header
geometry_msgs/Point32[] points（这里有XYZ，你可以直接访问）
sensor_msgs/ChannelFloat32[] channels

这样发送：

ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);
或pointcloud2情况
ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud2>("cloud", 50);

这个节点主要发：pointcloud2/pointcloud点云以及激光雷达坐标系laser到base_link的TF

4.8、栅格建图与定位修正
这个是本章的重点，值得一提的是，你如果是建3D的图，请自定义地图存储数据格式，且前面几个部分都要做相应修改，这时你就完全不需要ros的什么包了，这个后面讲，我们今天先讲2D下建图：
1.订阅激光雷达的点云（pointcloud2）及摄像头估计的点云（摄像头的点云是可选项）
2.订阅定位融合后的里程计odom_combined，监听里程计到base_link的TF
3.监听激光雷达到base_link的TF，如果用VO还需要camera到base_link的TF。
4.发布一个叫/map的里程计，这个里程计XYθ都是0，（也可以直接用occupancy grid发布的坐标系，我这是为了其他用途）
5.将所有的pointcloud2转换为pointcloud信息，这样好处理，这个后面讲。
6.建立从base_link到/map坐标系的TF，这个TF必须实时变化，故不能为静态的TF，之后讲.
7.将在/laser坐标系的点云通过到base_link的TF及base_link到/map的TF转换为在/map下的点云，并并进行栅格建图。
（如果是单独一个/map请发布occupancy grid到map的TF）。
8.结合t=0到现在的地图信息对位姿进行修正（gmapping为什么可以修改建议分布就是因为这个）

下面一个个来讲
第一步：订阅激光雷达的点云（pointcloud2）及摄像头估计的点云（摄像头的点云是可选项）

ros::Subscriber sub = node.subscribe("xxx/point",1,&cut_pointcloud::call_back,this); //订阅pointcloud2信息，名字由激光雷达那边发布节点给出

第二步：订阅定位融合后的里程计odom_combined，监听里程计到base_link的TF
订阅odom：

ros::Subscriber vel_sub = node.subscribe<nav_msgs::Odometry>(wheel_odom_, 10, &Createpointnode::vel_read_callback, this);

监听TF

try {
    listener.waitForTransform(destination_frame, original_frame, ros::Time(0), ros::Duration(10.0) );
    listener.lookupTransform(destination_frame, original_frame, ros::Time(0), transform);
} catch (tf::TransformException ex) {
    ROS_ERROR("%s",ex.what());
}

第三步：监听激光雷达到base_link的TF，如果用VO还需要camera到base_link的TF
监听同上

第四步：发布一个叫/map的里程计，这个里程计XYθ都是0（也可以直接用occupancy grid发布的坐标系，我这是为了其他用途）

odom_.header.stamp = now_;
odom_.pose.pose.position.x = 0;
odom_.pose.pose.position.y = 0;
odom_.pose.pose.position.z = 0;
odom_.twist.twist.linear.x = 0;
odom_.twist.twist.linear.y = 0;
odom_.twist.twist.angular.z = 0;
在class的某个函数中：
odom_pub_ = node.advertise<nav_msgs::Odometry>("/map", map_f);

第五步：将所有的pointcloud2转换为pointcloud信息（也可以不转换，直接用pointcloud2建图也可以）

调用PCL的函数（先把全部写了，后面都会用到，ros自己安装了PCL）
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree.h>
#include <pcl/kdtree/kdtree_flann.h>
在回调函数里，回调函数入口指针是input
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud {new pcl::PointCloud<pcl::PointXYZ>};
pcl::fromROSMsg(*input,*cloud);//把ROS格式转为PointXYZ

第六步：建立从base_link到/map坐标系的TF，这个TF必须实时变化，故不能为静态的TF（如果是occupancygrid发布的全局坐标系就转换到occupancygrid坐标系）

先做定义
tf2_ros::TransformBroadcaster mapbase_;
之后转换
transformStamped_.header.stamp = ros::Time::now();
transformStamped_.header.frame_id = map_frame_;
transformStamped_.child_frame_id = base_frame_;
transformStamped_.transform.translation.x = baseX;
transformStamped_.transform.translation.y = baseY;
transformStamped_.transform.translation.z = 0.0;
transformStamped_.transform.rotation.x = q.x();
transformStamped_.transform.rotation.y = q.y();
transformStamped_.transform.rotation.z = q.z();
transformStamped_.transform.rotation.w = q.w();
mapbase_.sendTransform(transformStamped_);

第七步：.将在/laser坐标系的点云通过到base_link的TF及base_link到/map的TF转换为在/map下的点云，并进行栅格建图。
（如果是单独一个/map请发布occupancy grid到map的TF）。

监听TF，然后把点云的坐标加上map到laser坐标系的增量，（这个小学生都会，仔细去想想），如要进行粒子滤波，请根据我在第三章讲的生成粒子，（gmapping还是一个粒子维持一副地图呢），然后进行栅格建图。
主要是发布nav_msgs/OccupancyGrid

nav_msgs/OccupancyGrid定义：
# This represents a 2-D grid map, in which each cell represents the probability of
# occupancy.

Header header 

#MetaData for the map
MapMetaData info

# The map data, in row-major order, starting with (0,0).  Occupancy
# probabilities are in the range [0,100].  Unknown is -1.
int8[] data
######################################
其中nav_msgs/MapMetaData定义：
# This hold basic information about the characterists of the OccupancyGrid

# The time at which the map was loaded
time map_load_time
# The map resolution [m/cell]
float32 resolution
# Map width [cells]
uint32 width
# Map height [cells]
uint32 height
# The origin of the map [m, m, rad].  This is the real-world pose of the
# cell (0,0) in the map.
geometry_msgs/Pose origin(地图起点坐标)

引用map格式的图：更加直观

对nav_msgs/OccupancyGrid举个例子

先在class函数里申明
ros::Publisher pub = nh.advertise<nav_msgs::OccupancyGrid>("/youMap", 1);
之后用ROS定时器写：
map.header.frame_id="Umap";
map.header.stamp = ros::Time::now(); 
map.info.resolution = resolution_;         // 分辨率,建议写成param
map.info.width      =  map_width_;           // 地图长，建议写成param
map.info.height     = map_height_;         // 地图宽，建议写成param
for(int i = 0;i<=99;i++)
{
	//注意如果不为10*10没探测的值要记做-1
	map.data[i] = 100; //10*10的地图（从0，0）开始，行优先序（自下而上，自左向右）。 值从0到100，-1表示未探测。
}
pub.publish(map);

data存储格式如下图

分辨率是干什么用的呢，我们可以通过设定
设置地图起点坐标比如是(orgin_x,orgin_y),分辨率为resolution_;，宽度为 map_width_，对于一个在地图坐标系的坐标为(px,py)的点云，其索引index = (px-orgin_x)/resolution_ + (py-orgin_y)*map_width_/resolution_,即data[index]，这个学过数据结构都知道为什么，就不说了。
关键是地图怎么更新：
一个简单的方法，没有点落入栅格data的值就减少（最小是0），越多点落如栅格，栅格值越大，若栅格值超过某个限度Mx，那么data始终不减少。这样就完成了栅格建图。还比如静态二值贝叶斯滤波：不过是把我说的更加定量化了。

这是啥意思？l为对数概率，避免了0,1附近的不稳定性，我们用t时观测结果更新某栅格的概率，这样只要概率大于50，占用概率增加，其可以通过波束模型表述。
比方说你在t-1时刻，因为计算你的占用概率是80%了，那在t时刻，有粒子落入栅格，且占用概率为90%，那你现在的占用概率就更新为82%（举个例子），如果t时刻落入粒子占用概率是10%，那么占用概率就可能更新为60%这样。具体我已经在我的第七章写好了，见我的第七章建图算法。
但是我希望通过更好的办法处理动态场景建图问题，所以需要语义信息进行修正，或者给地图估计加一个检测动态物体的算法，如果栅格概率跳变大的，我们需要用不同的处理办法，例如概率机器人中所说的接受陡然增大的激光数据，滤除陡然减小的激光数据。

第八步：结合t=0到现在的地图信息对位姿进行修正

说白了就是有了P(Xt|mt-1,zt)这一项。可以用hectorslam的方法对齐点云以矫正位姿，见hectorslam原理解析。
这样就能得到一个位姿修正值，你可以用这个值在每次odom_combined过来的时候修正位姿。如果是粒子滤波那么粒子越符合这个观测，权重提升就越大。而如果太不匹配导致权重过于平均，gmapping就会重采样：见伪代码，这个我们下下章讲：（会贴上红字的注释，现在只是了解）

而graph-slam就是把观测点通过变换加入H矩阵，然后优化得一个位姿偏差用于修正。修正值记得用一个全局变量存储哦！
这样，栅格定位与修正这步就完成了吗？没有，请进行同步分析及对延时进行时序协调控制吧，这一步是最麻烦的。

4.9 导航算法
先见ros导航框架

说白了就是rviz给地图发个goal给全局规划器，（也可以用节点发），然后先进行全局规划，这个结合从地图来的全局代价地图，会用dijkstra算法产生全局路径，然后发给局部规划器，局部规划器根据激光雷达信息（laserscan）产生的局部代价地图及全局路线及小车里程计信息用于dwa（动态窗口法规划），这些我在第一章就讲了，全局规划算法也可以自己去看数据结构。
那关键是我们如何进行导航？换句话说，你们看到无人车寻迹人家不是最短路径啊：
好，如何写自己的导航路线，忘记ros自带的path吧，（因为要产生costmap）用cmd_vel自己写一个，用一个数组存储路径不就好了，这是个结构体数组，数组元素必须包含需要向x移动的值，向y移动的值，最终转角，这样就可以根据里程计运动模型去运动了。所以步骤是
1.订阅odom_combined, map信息，TF信息及poseStamped信息（rviz发的坐标）
2.根据你的算法生成全局路径存入一个数组（或线性表）
3.订阅激光雷达数据或点云数据并根据你的位姿及全局路径路线，用dwa算法及额外约束产生局部路径（或线性表）（如车道线检测约束，红绿灯约束等），也 是个数组。这个长度不要太大，不然反应太慢了容易撞上
4.根据局部路径的数组进行导航，若局部路径突然被遮挡或里程计丢失则暂停小车，重新生成全局路径数组，否则把全局路径删除已经导航的一段并继续进行局部路径生成，循环往复。
1.那么posestamped是如何定义的呢？

std_msgs/Header header
geometry_msgs/Pose pose （这个pose包含xyz及四元数，如下定义）
	geometry_msgs/Point position
		float64 x
  		float64 y
		float64 z
	geometry_msgs/Quaternion orientation
		float64 x
		float64 y
		float64 z
		float64 w

这样你就能解析了，其他我都说过
（这就是简单的流程了，那对于无人驾驶汽车呢）
2.根据你的算法生成全局路径存入一个数组（或线性表）
我们无人驾驶不能走小道，要尽量走公道，这需要高精地图，还要尽量沿着车道线走并且遵守交通规则，这就是约束。并且可以不用dijkstra了，因为能走的地方少了，可以用一些快速的算法产生路径或者直接调用百度地图API
3.生成局部规划路线并发cmd_vel
在选择地图前，需知道地图栅格的类别，如果你要转弯，那么如果算法（dwa搜索到你旁边是直行道）那么即使走直行道是最短路径车都不会走，又比如车道线检测后，车会倾向于绕着车道线走，因为当车检测到这是车道线后，移位代价会很大，这是基于人类先验知识的约束，目前无人驾驶就是这样，但可以用深度学习探知一些约束。比如探知红绿灯，直接用个while死循环等待就好。所以在真实无人驾驶中必须给地图加上相应的属性，并给无人车一些约束。而且如果是无人车，局部路径规划也不用dwa了，或者探测角度范围降低也是可以的。
（做过这个的我表示，这个没有定位有意思，多是逻辑问题，但难的在于如果应用深度学习信息，这个比较麻烦）
当然你也可以产生特定路径，比如绕着一个柱子选择，你只要计算下方程把点带入方程把里程计存入数组就好。

好了，我们现在已经从无到有搭建了一个2D无人驾驶小车了，只用了ros节点及rosmsg，下下下章，我们将讲述如何亲手写一个3D的无人驾驶框架。