设为首页收藏本站
楼主: 科蚪之家
收起左侧

【科技教学】SLAM的前世今生,未完待续。。。已完结

  [复制链接]
回帖奖励 60 金币 回复本帖可获得 5 金币奖励! 每人限 2 次
  • TA的每日心情
    开心
    2019-5-13 21:51
  • 签到天数: 1070 天

    [LV.10]以坛为家III

    发表于 2017-9-23 23:27:32 | 显示全部楼层
    科技越来越厉害
  • TA的每日心情
    开心
    2019-5-13 21:51
  • 签到天数: 1070 天

    [LV.10]以坛为家III

    发表于 2017-9-23 23:27:37 | 显示全部楼层
    科技越来越厉害
  • TA的每日心情
    开心
    2019-5-13 21:51
  • 签到天数: 1070 天

    [LV.10]以坛为家III

    发表于 2017-9-23 23:27:45 | 显示全部楼层
    哇,科技越来越厉害
  • TA的每日心情
    开心
    2019-5-13 21:51
  • 签到天数: 1070 天

    [LV.10]以坛为家III

    发表于 2017-9-23 23:28:10 | 显示全部楼层
    科技越来越厉害
  • TA的每日心情
    开心
    2019-5-13 21:51
  • 签到天数: 1070 天

    [LV.10]以坛为家III

    发表于 2017-9-23 23:28:35 | 显示全部楼层
    期待下一期
  • TA的每日心情
    开心
    2017-11-22 10:43
  • 签到天数: 30 天

    [LV.5]常住居民I

     楼主| 发表于 2017-9-25 10:08:19 | 显示全部楼层
    SLAM端优化理论
          最麻烦的问题,就是“噪声”。这种渐近式的匹配方式,和那些惯性测量设备一样,存在着累积噪声。因为我们在不断地更新关键帧,把新图像与最近的关键帧比较,从而获得机器人的位移信息。但是你要想到,如果有一个关键帧出现了偏移,那么剩下的位移估计都会多出一个误差。这个误差还会累积,因为后面的估计都基于前面的机器人位置……哇!这后果简直不堪设想啊(例如,你的机器人往右转了30度,再往左转了30度回到原来的位置。然而由于误差,你算成了向右转29度,再向左转31度,这样你构建的地图中,会出现初始位置的两个“重影”)。我们能不能想办法消除这个该死的误差呢?
      朋友们,这才是SLAM的研究,前面的可以说是“图像前端”的处理方法。我们的解决思路是:如果你和最近的关键帧相比,会导致累计误差。那么,我们最好是和更前面的关键帧相比,而且多比较几个帧,不要只比较一次。
      我们用数学来描述这个问题。设:

    281107284396698.png

      不要怕,只有借助数学才能把这个问题讲清楚。上面的公式中,xp是机器人小萝卜的位置,我们假定由n个帧组成。xL则是路标,在我们的图像处理过程中就是指SIFT提出来的关键点。如果你做2D SLAM,那么机器人位置就是x, y加一个转角theta。如果是3D SLAM,就是x,y,z加一个四元数姿态(或者rpy姿态)。这个过程叫做参数化(Parameterization)。
      不管你用哪种参数,后面两个方程你都需要知道。前一个叫运动方程,描述机器人怎样运动。u是机器人的输入,w是噪声。这个方程最简单的形式,就是你能通过什么方式(码盘等)获得两帧间的位移差,那么这个方程就直接是上一帧与u相加即得。另外,你也可以完全不用惯性测量设备,这样我们就只依靠图像设备来估计,这也是可以的。
      后一个方程叫观测方程,描述那些路标是怎么来的。你在第i帧看到了第j个路标,产生了一个测量值,就是图像中的横纵坐标。最后一项是噪声。偷偷告诉你,这个方程形式上和上一页的那个方程是一模一样的。
      在求解SLAM问题前,我们要看到,我们拥有的数据是什么?在上面的模型里,我们知道的是运动信息u以及观测z。用示意图表示出来是这样的:

    281107477838540.jpg

      我们要求解的,就是根据这些u和z,确定所有的xp和xL。这就是SLAM问题的理论。从SLAM诞生开始科学家们就一直在解决这个问题。最初,我们用Kalman滤波器,所以上面的模型(运动方程和观测方程)被建成这个样子。直到21世纪初,卡尔曼滤波器仍在SLAM系统占据最主要的地位,Davison经典的单目SLAM就是用EKF做的。但是后来,出现了基于图优化的SLAM方法,渐渐有取而代之的地位[1]。我们在这里不介绍卡尔曼滤波器,有兴趣的同学可以在wiki上找卡尔曼滤波器,另有一篇中文的《卡尔曼滤波器介绍》也很棒。由于滤波器方法存储n个路标要消耗n平方的空间,在计算量上有点对不住大家。尽管08年有人提出分治法的滤波器能把复杂度弄到O(n) [2],但实现手段比较复杂。我们要介绍那种新兴的方法: Graph-based SLAM。
      图优化方法把SLAM问题做成了一个优化问题。学过运筹学的同学应该明白,优化问题对我们有多么重要。我们不是要求解机器人的位置和路标位置吗?我们可以先做一个猜测,猜想它们大概在什么地方。这其实是不难的。然后呢,将猜测值与运动模型/观测模型给出的值相比较,可以算出误差:

    281108447836354.png

      通俗一点地讲,例如,我猜机器人第一帧在(0,0,0),第二帧在(0,0,1)。但是u1告诉我机器人往z方向(前方)走了0.9米,那么运动方程就出现了0.1m的误差。同时,第一帧中机器人发现了路标1,它在该机器人图像的正中间;第二帧却发现它在中间偏右的位置。这时我们猜测机器人只是往前走,也是存在误差的。至于这个误差是多少,可以根据观测方程算出来。
      我们得到了一堆误差,把这些误差平方后加起来(因为单纯的误差有正有负,然而平方误差可以改成其他的范数,只是平方更常用),就得到了平方误差和。我们把这个和记作phi,就是我们优化问题的目标函数。而优化变量就是那些个xp, xL。

    281108133148174.png

      改变优化变量,误差平方和(目标函数)就会相应地变大或变小,我们可以用数值方法求它们的梯度和二阶梯度矩阵,然后用梯度下降法求最优值。这些东西学过优化的同学都懂的。

    281108587201279.png

      注意到,一次机器人SLAM过程中,往往会有成千上万帧。而每一帧我们都有几百个关键点,一乘就是几百万个优化变量。这个规模的优化问题放到小萝卜的机载小破本上可解吗?是的,过去的同学都以为,Graph-based SLAM是无法计算的。但就在21世纪06,07年后,有些同学发现了,这个问题规模没有想象的那么大。上面的J和H两个矩阵是“稀疏矩阵”,于是呢,我们可以用稀疏代数的方法来解这个问题。“稀疏”的原因,在于每一个路标,往往不可能出现在所有运动过程中,通常只出现在一小部分图像里。正是这个稀疏性,使得优化思路成为了现实。
      优化方法利用了所有可以用到的信息(称为full-SLAM, global SLAM),其精确度要比我们一开始讲的帧间匹配高很多。当然计算量也要高一些。
      由于优化的稀疏性,人们喜欢用“图”来表达这个问题。所谓图,就是由节点和边组成的东西。我写成G={V,E},大家就明白了。V是优化变量节点,E表示运动/观测方程的约束。什么,更糊涂了吗?那我就上一张图。

    281109171273435.png

      图有点模糊,而且数学符号和我用的不太一样,我用它来给大家一个图优化的直观形象。上图中,p是机器人位置,l是路标,z是观测,t是位移。其中呢,p, l是优化变量,而z,t是优化的约束。看起来是不是像一些弹簧连接了一些质点呢?因为每个路标不可能出现在每一帧中,所以这个图是蛮稀疏的。不过,“图”优化只是优化问题的一个表达形式,并不影响优化的含义。实际解起来时还是要用数值法找梯度的。这种思路在计算机视觉里,也叫做Bundle Adjustment。它的具体方法请参见一篇经典文章[4]。
      不过,BA的实现方法太复杂,不太建议同学们拿C来写。好在2010年的ICRA上,其他的同学们提供了一个通用的开发包:g2o [5]。它是有图优化通用求解器,很好用,我改天再详细介绍这个软件包。总之,我们只要把观测和运动信息丢到求解器里就行。这个优化器会为我们求出机器人的轨迹和路标位置。如下图,红点是路标,蓝色箭头是机器人的位置和转角(2D SLAM)。细心的同学会发现它往右偏转了一些。:

    281110316584506.jpg





  • TA的每日心情
    开心
    2018-6-26 19:27
  • 签到天数: 422 天

    [LV.9]以坛为家II

    发表于 2017-9-25 19:20:49 | 显示全部楼层

    回帖奖励 +5 金币

    这技术真牛啊

    本版积分规则

    快速回复 返回顶部 返回列表