各位同学大家好，非常荣幸来给大家一起分享有关移动机器人定位感知技术的内容。
今天是无人驾驶关键技术与实践的第五节课，在前面的几节课中，张老师主要给大家分享了无人驾驶概况以及整体的架构，从宏观的角度介绍了无人驾驶的现状以及现在的研究现况。刘老师和大家分享了视觉目标感知与多目标感知技术，这些都是非常实用的技术。
后面的几节课，主要是由我来和大家一起分享一些技术。由于我自身是一名工程师，我的目标是想给大家介绍或者展示一些算法的原理以及实现，希望大家在听完这一系列课程之后能够自己动手写出程序，实现一个简单的无人驾驶系统。为了避免在课程的过程中展示大段的代码，我把代码的实现过程用一些图示来展示，这部分我也是花了不少时间准备，希望对大家的理解有些帮助。
不瞒大家说，这个也是我第一次讲课，可能一开始无法做到在像刘老师那样逻辑清晰，深入浅出的讲解，但我还是会努力去做。如果有任何问题大家可以及时在评论区中提出来，同时我也在咱们的课程微信群中，在群里讨论的内容我也能够看到。我能够解答的，我会在直播中尽可能满足大家的需求。
废话不多说，我们来开始今天的内容。
在无人驾驶或者机器人平台中我们想要实现的一个功能就是我们有一张地图，在这张地图上，能显示小车的位置，当我们给出一个目标点之后，小车能自动规划一条路线，并沿着规划的路线行驶到目的地。在行驶的过程中，我们还需要小车实时检测周围的环境，对一些突发情况能作出反应，比如遇到障碍物能自动躲避，对于路况的改变能重新选择一条可行道路。我们细致的取分析这个功能，会发现这个为了完成这个功能，我们需要解决很多问题。比如：
- 这张地图从何而来？是需要自己构建，还是能直接用现有的地图？
- 小车如何能知道它现在所处的地图的位置？
- 如何根据已有的地图构建出一条从当前位置到目标点最优的路线？
- 如何识别到障碍物？
- 如何控制小车沿着给出的道路行进，并且躲避障碍物？
细究下去可能每个问题还能分支出很多子问题。选择不同的解决问提的方法，就能组建自己的一套无人驾驶方案。如果从零开始做，可能很多人和我一样无法下手，所以在这里我给大家分享一个非常经典的，也是现在很多室内服务机器人，agv小车，或者移动机器人平台的导航方案。这套导航方案基本解决了上面提出的问题，在熟悉了解这套方案之后，在此基础上进行改进就能构建自己的一套无人驾驶体系。
这一节课我们就先讨论前两个问题，即如何构建地图以及如何定位。
这节课咱们主要和大家分享四方面的内容。前两节主要介绍卡尔曼滤波的相关内容，后面两节主要介绍粒子滤波的两个实际应用。
相信绝大多数人都听说过卡尔曼滤波，百科中对卡尔曼滤波的介绍是：一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
对于不熟悉它的人来说，看完这个介绍可能还不知道是什么东西。所以我们从最简单的问题下手。
问题1：假如现在有一个小车，在某个时刻我能很明确的知道它的位置，以及速度。现在有个问题是说经过一个很小的时间后，这个小车的状态如何。
为了直观化这个问题，我们使用这个箭头来表示小车的状态，其中箭头所处的位置表示小车的位置，同时箭头的长短用来表示小车的速度
由于我们明确的知道小车的速度以及位置，那么在下方的坐标系中我们就可以使用一个点来表示当前小车的状态
小车经过一小段时间后，移动的另外一个位置，注意这边是一小段时间，所以我们可以认为小车在这段时间里进行匀速直线运动，所以很简单我们认为在deltaT的时间里，小车运行到这个位置，同时速度没有发生改变，这里我们用蓝色的箭头表示，同样我们可以在线面的坐标系中使用一个点来表示小车的状态。
随之而来的就是第二个问题，我们可能只是知道小车可能在某个区域，但并不知道到它的确切位置。就类似图中表示的红色的箭头簇，为了表示这种状态，我们可以使用色带来表示小车的状态，其中这个颜色的深浅表示小车所处的位置的概率。我们仍然假设在短时间里面小车以固定速度做匀速运动，由于速度是恒定的，所以相对来说就是色带的平移，经过daltaT时间后，小车的状态如图所示。
位置是不确定的，同时我们的速度也可能不确定，为了表示这样的状态，我们可以用一个色块来表示状态，其中颜色表示概率。
说明两个点怎么移动，从而说明为啥会有这样的分布的变化
如果这个分布是属于正态分布，那么我们可以使用两组参数来刻画这样的一各色块，其中均值表示。。。。。其中方差表示。。。。我们可以看到方差矩阵包含四个数，其中……位置方差表示小车在x轴方向上的分布范围，速度方差表示。。。。速度与位置的协防差表示。。。
有了刻画方式，我们可以比较严谨的推导这个过程，对于初始状态，我们记当前状态为。。。。这个表示。。。。经过deltaT之后，这个状态为。。。（下面公式），可能学过线性代数的同学可以发现，刚才的两个公式我们可以使用矩阵乘积的写法。从而我们会发现这个变换是一个线性变换，在我们有一定概率基础的情况下，我们知道一个正态分布经过一个现行变换之后人还是一个正态分布，其中变换后的分布的均值为。。。，方差为。。。
在实际中还有另外一个问题就是外部的干扰，可能对于咱们小车表示的不是特别的明显，假如这是一个无人机在空中飞，它很容易收到风的干扰，所以在上面推导过程中我们再加一个小的外部干扰的噪音，比如。。。。这个噪音也服从正态分布，那么将这个过程写成矩阵乘法如下。。。这个时候可能又需要一点概率论的知识，对于两个独立的正态分布随机变量其和也是正态分布，，这个分布的均值为。。。 方差为。。。这样我们就构建了一个比较完整的预测模型。
我们预测是为了什么，当然是为了和测量的数据进行比较，从而去修正测量的数据作为最终的结果，也就是说结果结合测量和预测，所以只要预测合理，那么最终的结果可能也比单独的测量更为的准确。
为了将预测的数据与测量的数据结合，我们就需要把预测数据转换成测量的数据，这里的转换比如单位不同，比如实际我们使用米为单位，而测量使用的是厘米，我们就需要将其转换一下。
假如在这里我们只有一个距离传感器，测量位置，那么这个转换我就将我们预测的结果投射到位置空间。
这张PPT其实非常好理解，就是我们做了一次测量，测量的结果呢是zk，但我们也很清楚这个测量是有误差的，不妨我们再假设这个误差满足正态分布，于是我们就可以画出遮掩各一个分布
将预测和测量我们在测量空间中结合，对于测量这个概率分布函数表示。。。。，对于预测这个概率分布函数表示。。。。。，那么现在我需要问，对于位置x，测量是这个位置，并且预测也是这个位置的概率什么？好了，继续搬出咱们的概率论知识，对于连个独立的随机变量，其联合分布的概率密度函数等于两个随机变量的概率分布函数的乘积，也就是说。。。。。。。
由于正态分布优秀的特性，两个正态分布的乘积还是正态分布，这个结果我就不在这里算了，我直接写出来好了。（这里仔细观察一下公式）对于这个正态分布，我们取均值。。。。就得到了我们想要的结果。
我们再将上述的过程放在二维空间中进行一次，首先我们根据之前的预测模型预测在deltaT之后，小车的状态为。。。这个时候我们可以测量到位置与速度信息，这里假设由于我们实际在使用里作为位置的单位，而测量呢使用的是公里作为单位，这样就会有一个单位转换的过程，这个过程我们可以表示为矩阵乘法。于是在实际空间中的状态转换到测量空间中如图所示。
在测量空间中我们加上测量的状态，类似我们就能够得到他们的联合概率密度分布函数。
然而得到的这个概率分布函数参数世在测量空间中的，所以我们需要将这些参数转化未实际空间中，对于这个例子来说就是将测量空间中的公里单位转换未实际空间中里的单位，这样我们就能够在实际空间中得到滤波后的结果。同时这个结果可以作为下一次滤波的初始状态不断重复上述的过程就可以实现卡尔曼滤波。
我们对上面所讲的过程做一个总结。
首先我们有一个初始状态，还有一个线性的预测模型，根据这些我们可以得到一个预测的状态。
为了和测量状态结合，我们将预测的状态转换到测量空间
转换到测量空间的预测状态和测量状态结合可以得到一个联合的分布，这个分布同时考虑到预测和测量
将这个联合的分布再转换回实际空间就得到了一次最优的估计，将这个估计可以带回下一轮的预测中。
这里我想问一个问题，如果预测那块不是线性模型为啥就不能直接使用卡尔曼滤波呢？还有这里面的分布不是正态分布，会出现什么问题呢？带到上面的讲的过程对这两个问题进行分析。
卡尔曼滤波就讲到这里，可能大家觉得这很简单，但实际使用的过程中还会有很多问题，在这里可能也没有那么多时间和大家分享。现在我大概过一下利用卡尔曼滤波做传感器数据融合这件事情。
说到这里，就不得不说说在无人驾驶中怎么去定位，在无人驾驶这块有两种定位系统分别是。。。。。，其中。。。。。，而这个又包含。。。。有一种融合的方案就是先将。。。。然后再结合。。。。给出。。。。。
为什么要去做数据融合，之前刘老师在课上也提到过，我就不具体说了，这里我拿出了几个常见的传感器在不同场景下其表现做了一个对比，其中星星越多，表示该传感器在这个场景中表现越好，当然这个也就只是一个常规的比较，当然针对不同档次的传感器，不同的算法，这个对比也不一定适用，但这个就表示出在不同场景中，不同的传感器表现不一样。，如果我们能针对这些传感器取长补短，是不是就能够得到一个比较好的结果呢？
对于之前我们讲卡尔曼滤波，我们选择的状态只是位置和速度，然而在实际使用过程中，我们可能会对。。。。。这些状态量进行预测以及测量。这张PPT中就显示出不同传感器可以测量的一些状态量。
下面这个就是数据融合的基本流程，可能大家第一眼看上去这么多框框箭头可能会晕，我在这里就大概说一下，具体实现细节，我后面会给出一篇论文，上面有实际的例子讲解如何做数据融合。
首先我们从。。。来看，这个代表在t-1时刻下我们得到状态分布，这个状态分布中包含。。。。。。。，这个状态分布经过一个线性的转换矩阵得到在T-1时刻下，预测T时刻的状态分布，结合刚才讲的内容，就类似一个色块转变成另外一个色块，只不过这个这里并不是一个色块，而是一个多维的超球体。在这个所谓的超球体中包含多个状态的分布范围，以及不同状态之间的关联关系。
我们将这个预测的状态分布可以投射到各传感器的测量空间，就类似之前我们讨论的一维情况。同时我们结合在t时刻的测量数据我们可以得到预测和测量的差，而这个差可以反过来更新这个传感器的协方差矩阵，同时这个协方差矩阵可以计算出对于这个传感器的增益矩阵，相当于不同传感器的一种信任权重，将这个权重以及测量与预测的差值我们可以得到整体上预测的偏差，将这个偏差加到测量上我们就可以得到在T时刻的最优估计。这个最优估计同时会作为下一次的预测初始值。而在图的左边这部分主要是在迭代计算状态分布的协方差矩阵。这里面具体的计算公式能够在我给的论文中找到。
上面讲了这么多，我觉得大家可能对卡尔曼滤波有了一定的认识。接下来主要给大家介绍粒子滤波的两个应用。这两个应用可以说在我们整个无人驾驶框架体系中占了很大的一部分权重。
第一个讲amcl算法，amcl翻译成中文就是叫做自适应蒙特卡罗算法，这个算法直观的讲是利用单线激光雷达的数据和地图数据进行匹配来确定小车在地图中位置的方法，在这个算法里面起始就是利用粒子滤波来实现的，那这个自适应是啥意思呢？起始就是它有一套机制自动选择合适的粒子数进行定位，再简单点说就是当我很不确定自己的位置时，我会放出更多的粒子来进行采样，而在我比较确定自己的位置时，减少粒子数量来减少计算量。
在讲amcl之前我先给大家过一便常见的运动模型，运动模型起始就是在已知t-1时刻的位置的前提下，和其他的一些参数，来计算在t时刻时，小车在某个位置的概率。同时有了t时刻的不同位置的概率分布，我们也可以通过采样的方法获取t时刻小车可能在的一些位置。
在概率机器人这本书中讲解了两种运动模型，第一种叫速度运动模型，在这个模型中，我们根据小车在t-1时刻的线速度和角速度来判断小车t时刻在不同位置的概率，这个模型起始很简单，由于知道了小车的速度，我们对线速度和角速度做采样，在给定线速度和角速度的情况下，小车是标准圆弧运动，根据时间我们就可以知道在不同位置的概率
第二个运动模型叫做里程计运动模型，在这个模型中，我们并不知道小车的运动速度，我们只是知道在t-1时刻小车里程计的信息，以及t时刻时小车的里程计信息。对于这个模型我们根据前后两帧里程计数据，我们可以求得这些参数。。。。。。，对这些参数进行采样，带入到t-1时刻小车的位置中，我们就可以预测t时刻小车在不同位置的概率
讲解完运动模型，我们开始我们的amcl算法的讲解。首先小车位于一副地图当中，这个时候我们可能会有两种状态，第一种状态是我们完全不知道小车在什么位置，还有一种状态是我们可能大概知道小车在啥位置，但具体不清楚。针对这两种情况，我们可以在地图中撒一些粒子，这个粒子中包含位置以及航向角信息，对于第一种情况。。。。。。。。对于第二种情况。。。。。。
现在在这幅图中灰色箭头表示。。。。。。这个颜色的箭头表示。。。。。
经过deltaT时间后，我们从里程计信息中得到小车在这段时间里面运动的变化，结合里程计运动模型，其实我们就有了里程计运动模型测参数，同时我们可以通过单线激光雷达中获取一圈雷达数据，（介绍单线激光雷达数据）在代码中我们注意一下，对于雷达在某个方向上获取的距离不在其合理范围内，就将这个距离修正为雷达所能测量的最大距离。
有了里程计数据，起始我们就可以进行运动模型采样了，在我现在使用amcl程序中，其运动模型和概率机器人当中的不太一样，这里我比较详细的介绍一下在amcl算法中是怎么做运动预测的。首先通过里程计数据，获三个变量，（对着代码讲 1, 2, 3.。。。。对rot进行采样，对trans进行采样，这个采样包括两个方向，采样的结果我们这里用hat来表示。那么假如有一个粒子的初始状态。。。。）
我们对所有的粒子都进行相同的采样操作，获取之前撒的粒子经过里程计显示的运动后可能在位置
于是乎，所有的采样的粒子经过一次运动采样后达到这样的状态，我们不能只是让这个粒子做运动，我们还需要对每个粒子的状态进行打分，打分这件事情就要利用雷达数据了，这时候我们首先选择一个粒子来演示如何通过雷达数据进行评分，那我们可以把雷达数据带进去，假如真实的位置和朝向是这个粒子所代表的，那么雷达这些点所测量的位置应该都是障碍物，但我们从这个图示中发现并不是这样的，所以我从直观的感觉上觉得这个粒子的评分比较低，但如何建立一个数学模型去计算这个评分呢？
这个模型怎么建立呢？起始就和我们能刚才说的，如果我们所选择粒子距离距离真实越近，那么带入所得的这些点就离障碍物越近，那么我们就选用带入粒子的雷达数据点和障碍物点的距离来构建这个评分，总体上这些距离越近，评分越高。但这个时候又会出现一个问题，如果我在测量的时候有一个障碍物怎么办？这样测量出来的距离可能更短了，这个时候我就给这些距离做一个补偿。同时如果我测量的范围大于雷达实际能测量的范围，我们也给做一个补偿，当然大家都做补偿了，那么我做的所有有效的测量都给一个补偿
经过一系列的评分，我们可以得到下面这张图，其中箭头越粗表示权重越高，箭头越细表示权重越低，在这个时候这个算法还计算了一个平均权重用来表示整体的采样的效果，同时还计算了。。。 和。。。，着两个值的计算如下所示。。。。
下面重复上面的步骤再来一次，获取里程计信息，获取雷达数据
对每个粒子再一次做运动估计
得到更新粒子的位置
对更新粒子的权重
这样的工作我们可以重复若干次，但这样的做法只能让权重低的粒子处于权重低的状态，我们假如很清楚这个粒子不可能在这个位置上，我们应该舍去这些权重低的点，这个时候我们就需要重采样的这个步骤。（分析代码）
介绍w_diff ,重采样（用小筐子举例）
我们一直在说采样之后呢
gmapping
首先可以讲最直观的方法，但是这个方法有问题，累计误差，所以使用粒子滤波多做几条路线，并且根据几条路线的状态去重采样
在这里面就有一个如何取评价这几条路线呢？通过点涉及到线
评判算法
并不是粒子撒出去就不管里，需要帧匹配
如果发现问题路径，怎么处理呢？
网友评论