激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)

传感器数据处理1:里程计运动模型及标定

里程计模型

一、两轮差分底盘的运动学模型

  • 差分模型与运动解算(线速度和角速度的计算过程分析
    • 差分运动底盘它其实是一个欠驱动的模型,他的自由度是三个,x,y,角度C塔。但是他的驱动数和输入数是两个,一个是左轮的速度VL和右轮速度VR,它的运动是耦合的,它只能做圆弧运动。
    • d为左轮或者右轮到车底盘中心的距离,假设左轮与右轮d相等,最左边的点为圆心,现在要让机器人围绕这个圆心做圆弧运动,r为圆心到机器人底盘中心的距离,也就是机器人做圆弧运动的半径。他会一直满足运动解算的两个公式。下面是对两个公式的推导
      -激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第1张图片
      激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第2张图片
    • 推导角速度w
      • 首先,如果机器人围绕圆心做圆弧运动,那么左轮和右轮肯定也是做圆弧运动,做圆弧运动的前提就是他们的角速度是相等的,中心角速度=左轮角速度=右轮角速度:W=Wr = Wl,VL表示左轮线速度,VR表示右轮线速度。我们知道线速度V等于角速度乘以半径,V=wr,w=v/r,而右轮与左轮角速度又相等,所以根据角速度公式就可列出方程,然后计算得到圆弧运动的半径r,
      • 激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第3张图片
      • 然后r得到了公式之后,就要回到w=v/r上,因为半径r已经得到了公式,那么代入公式之后,就可以得到角速度公式了,就可以算出角速度,这就是角速度的推导过程。
      • 激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第4张图片
    • 推导线速度v
      • 线速度和角速度也就是这样的一个过程,按照公式进行计算即可。
        -激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第5张图片

二、三轮全向底盘的运动学模型

​ 三个自由度,同时也有三个输入量。

  • 三轮全向底盘平移X运动分解:
    • 激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第6张图片
    • 运动分解如图所示
    • 车体X轴方向上的速度在三个轮子上V1,V2,V3的的分速度。也就是说假设想要车以Vx的速度前进的话,那么就可以按照如上三个公式对轮子的速度进行赋值。车就会按照正前方前进
  • 三轮全向底盘平移Y运动分解:
    • Y方向和V1是平行的,所以V1就等于Vy,而cos60 = V2/Vy,所以V2=-COS60Vy,v3也同理
    • 激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第7张图片
    • 所以如果要使用三轮全向底盘进行垂直运动,可以按照如上三个公式对其进行赋值。
  • 三轮全向底盘旋转运动分解:
    • 旋转使每个轮子和车体中心旋转的角速度都是相等的,所以要让车体旋转起来,那么可以直接使用公式V=Wd,w是角速度。
    • 激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第8张图片
    • 所以旋转直接用角速度乘以轮子到底盘中心的距离。按照如上三个公式即可。
  • 将其进行合成
    • 激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第9张图片
    • 根据所给条件(蓝点标注)列出整体公式,所以想要其运动就可以按照这个公式去计算
      -激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第10张图片
    • 但在实际中,通常是反过来的,我们编码器只能测量出每个轮子的速度V1、V2、V3而不能直接测量某个方向上的速度或角速度。所以需要列出矩阵随后进行变换
      • 激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第11张图片
      • 矩阵可写为:V = A*v,两边同时乘以A的逆矩阵就得到了v = A的逆矩阵 *V
      • 激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第12张图片
    • 这样就可以通过编码器测得V1、V2、V3,计算得到Vx(x方向的线速度)、Vy(y方向的线速度)、Vc塔(角速度)。当然也可以通过转换前的公式,在ROS下发线速度和角速度时,STM32接收并计算转速随后控制机器人运动。
    • 疑问:求出x、y方向得线速度和旋转的角速度有什么用?
      • 因为这不仅仅是关于速度的,还有关与距离和角度,试想矩阵两边同时乘以△t,也就是积分就得到了dx、dy、dc塔,也就是位移和转过的角度了。这也就是里程计的模型了
      • 激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第13张图片
    • 总结一下里程计的模型
      • 不管是差分还是三轮全向,他们的运动学模型,就是根据他们自身的特性和编码器测量来的数据(差分是VL和VR,全向是V1、V2、V3)怎么去得到一个dx、dy、和dc塔的一个过程
      • 底盘的任务就是计算编码器的tick值,然后计算线速度,通过IMU计算角速度上发到给ROS
      • ROS端控制时就直接发送的线速度和角速度信息给STM32,随后STM32再计算成转速然后再通过PID控制输出PWM驱动电机

三、航迹推算

激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第14张图片

  • 运动示意图如上,横坐标就是dx,纵坐标就是dy,上方角度就是dc塔。现在我们假设(x,y,c塔)是底盘的当前位姿,(dx、dy、dc塔)为运动学解算的增量,这个运动学可能是差分的,也可能是全向的,但不重要,最终都会出来这三个。

    • 现在的任务就是要先将机器人都当前坐标点转换到世界坐标系中,可以通过如下矩阵进行计算。这个矩阵就是平面上的变换矩阵

      激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第15张图片

    • 递推公式(在代码中是可以经常看到的)
      激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第16张图片

      • 实际情况,存在噪声
        激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第17张图片
    • 影响里程计的因素

      • 系统误差
        • 是一个确定的误差,比如轮子直径量错一点,可以进行标定,今天也就是为了解决系统误差,进行里程计标定。
      • 随机误差
        • 随机出现的误差,比如轮子打滑,无法标定
  • 刚刚讲完了差分、全向三轮的运动模型与航迹推演计算过程,接下来要解决的问题就是去解决系统误差,对里程计进行标定。

里程计标定

一、线性最小二乘的基本原理

  • 线性最小二乘法通用性强,但效果绝对不是最好的。

  • 最小二乘是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据,并使得这些求得的数据与实际数据之间误差的平方和为最小。

  • 线性最小二乘是用来干嘛的?是用来求解线性方程组Ax=b的。所有的工程问题,基本都是Ax=b方程组的问题。

    • 假设A是一个m * n的矩阵,x为n * 1的向量,x也就是我们要求解的状态量
    • 他的状态也就是三种,当m=n时,是适定方程组方程组有唯一解
      • m=n其实就是Ax=b中的行表示一个约束,列表示自由度,或者说是未知数变量的个数。也就是约束的数量与未知数的个数是相等的,这时候方程组有唯一解。
    • m时,欠定方程组,方程组有无限多解
      • m约束不足,就叫欠定
    • 当m>n时,超定方程组,方程组有通常无解
      • 约束的数量大于未知数的个数,超定。实际情况中碰到最多的方程组。比如说我们在做曲线拟合的时候,会采很多个点,比如拟合直线,y=ax+b,如果采点10000,那么矩阵A=10000*2维的,这个方程组是无解的。但可以找到最靠近真实解的解。无解但是有最小二乘解。都可以通过化成Ax=b的形式,套用公式通解如下图。但是通常A的转置矩阵 *A的条件数都会增大,所以实际中都会对ATA进行QR分解,可以降低它的条件数,解会更稳定。
    • 接下来解释这个通解是如何得到的
  • 最小二乘的求解——从线性空间的角度

    • 从线性空间的角度看,一个系统可以有Ax=b,当x这个向量趋近与每一个值的时候,那么Ax实际上表示的A的列空间(A列向量的线性组合)。假设A=[a1,a2,a3]

      x=[x0,

      ​ x1,

      ​ x2]

      那么Ax=[x0a1+x1a2+x2a3],他其实表示的是A的列向量的线性组合(已经证明,一个32的矩阵与一个2*1的矩阵相乘,得到的是一个3 *1的矩阵,表示的其实就是前面矩阵的列空间)。那么当x取完所有值之后,表示的就是矩阵A的列空间。那么上小节所说:Ax=b无解,那么就是说向量b不在矩阵A的列空间内,比如说矩阵A的列空间是一个平面,而向量b是一个三维向量,那么向量b肯定是不在A矩阵的列向空间内。虽然说在A矩阵的列空间S内找不到b向量,但我们可以在A的列向量空间内找到一个解,或者说一个向量,他到三维向量b的距离是最短的。那么这个解对应的就是向量b的A矩阵的列空间S内的投影

    • 假设我们知道这个向量(就是在A的列向量空间到三维向量b的最短距离的向量),设Ax* 为三维向量b在空间S中的投影。在向量空间中,就构成了一个加单的向量图如下,想象Ax* 在平面中b是一个三维向量,这个投影向量到三维向量b相连接就构成了一个向量图,所以直角边向量(b-Ax*)是垂直于平面的,也就是垂直于列向量空间S。实际(b-Ax * )跟矩阵A的每一个列向量都是垂直的,他整个就竖在平面中的,而根据我们之前所学的向量知识,平面向量与垂直向量点乘为0

      激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第18张图片
      激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第19张图片

      那么也就是说所有这样的列向量(A原始元素是一个行向量,所以要进行转置,就是AT)点乘(b-Ax *)都是为0的,所以有

      在这里插入图片描述

      那么用矩阵形式写的话就是这样

    激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第20张图片

    左边a1T,a2T…就是原来向量A=[a1,a2,a3…an]的转置矩阵,所以记为AT,所以得到

    在这里插入图片描述

    展开计算有:

    在这里插入图片描述

    两边同时乘以A的转置矩阵的逆矩阵,就得到了通解x*

    在这里插入图片描述

    就验证了上一小点最小二乘法的基本原理。

二、最小二乘的直线拟合(使用这个例子来简单验证一下)

  • 拟合一条直线,方程为y=5x+2。理想情况就是一条直线,但实际情况下采用总会产生噪声,

    激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第21张图片

  • 假设采样数据有十组,混入噪声之后的y值采样如下,按照:方程y=5x+2,当x=1时,y应该等于7,混入噪声后y=6.9918…

    激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第22张图片

    然后按照我们前面的说法,我们要把一个非线性最小二乘问题列出一个方程组:Ax=b,x就是我们要求的向量,有无穷多个,在直线拟合中,方程为y=ax+b,我们要求的就是a和b,所以Ax=b中的x矩阵如下:

    激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第23张图片

    x和y可构成一个方程如下,yi = axi+b或者说yj=axj+b

激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第24张图片

那么挑出一个y1 = ax1+b这个方程,可列成矩阵为

激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第25张图片

列出来就是Ax=b的问题:

激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第26张图片

而通解已经得出来了,那么直接套x的公式就可以了

在这里插入图片描述

激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第27张图片

  • 还有一种算误差的方法:ei=axi+b-yi,然后e就等于∑ei=∑axi+b-yi,然后对这个误差进行求导然后令其等于0,然后计算得出的解跟上面是一模一样的,只是从不同角度对线性最小二乘做一个分析。

    激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第28张图片

然后代入数据得:

激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第29张图片

求出a=5.063,b=2.009

​ 除了这个其实还有一个迭代最小二乘,我们标定的时候不止是出厂标定,我们可以进行在线标定,上面的过程我们是已知10组数据来进行标定,实际中需要进行在线标定,这就是迭代最小二乘,他跟卡尔曼滤波是非常相近的,如果没有噪声存在,那么卡尔曼滤波就是迭代最小二乘,他也就是一个滤除噪声的过程。

混入噪声与拟合之后的对比:

激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第30张图片

三、最小二乘在里程计标定中的应用

  • 里程计标定的方法有两种,一种是直线线性方法,一种是基于模型的方法。

    • 直线线性方法
      • 优点
        • 通用性强
        • 实现简单
      • 缺点
        • 精度不高
    • 基于模型的方法
      • 优点
        • 精度高
      • 缺点
        • 实现复杂
        • 特异性高
  • 直线线性方法

    • 用雷达的数据作为观测值ui*,ui表示的是第i帧和第j帧的相对位置关系。pi的逆矩阵 * pj。

    • 用里程计数据为ui,他也是表示的第i帧oi和第j帧oj的相对位置关系。oi的逆矩阵 * oj。

    • 假设成线性关系ui* = X * ui,也就是假设里程计的观测值激光的值是成线性关系的。那我们可以通过一个3 * 3的矩阵X,将里程计观测值与激光雷达观测值做一个映射。然后求出通解,再往方程中代入,就使里程计与激光雷达数据匹配了,这就是标定。

      • ui又可视为x,y,c塔组成的一列,ui* 也同样

        激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第31张图片

      • 相乘之后就得到一个方程组

        激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第32张图片

      • 那我们要往Ax=b方程上靠,那么X就是

        激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第33张图片

      • 那么矩阵就是

        激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第34张图片

      • 那么每组观测数据就可以用这个公式来表示

        在这里插入图片描述

      • 当我们有很多帧,n帧,那么就可以写成这样的形式

        激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第35张图片

      • 写成这样的形式之后,就可以套用通解的公式了

        在这里插入图片描述

      • 直接就可以得到一个解,这个解是一个九维向量(九行),然后转换成一个三行三列的向量,激光数据和里程计观测值的每个元素的数据都用x进行校正之后,就得到了一个校正后的数据

        激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第36张图片

  • 作业:实现一个直接线性方法的里程计标定模块

激光SLAM——第二节(传感器数据处理1:里程计运动模型及标定)_第37张图片

作业实现过程

根据上面说的使用直线线性的方法,用雷达的数据作为观测值ui* ,ui表示的是第i帧和第j帧的相对位置关系。用里程计数据为ui,他也是表示的第i帧oi和第j帧oj的相对位置关系。假设成线性关系ui* = X * ui,也就是假设里程计的观测值与激光的值是成线性关系的,列出方程组Ax=b。那我们可以通过一个3 * 3的矩阵X(因为观测值是存在三个变量的x,y,c塔),将里程计观测值与激光雷达观测值做一个映射。然后求出通解,再往方程中代入,就使里程计与激光雷达数据匹配了,这就是标定。

  • 那我们首先要做的就是先给定两个里程计位姿,计算这两个位姿之间的位姿差,求解得到两帧数据之间的位姿差,即求解当前位姿在上一时刻坐标系中的坐标,也就是求出ui这个向量(因为是三个方向,而且源码有定义为Eigen::Vector3d向量类型)。
  • 然后我们通过输入里程计ui向量和激光数据ui*向量,构建超定方程组Ax=b
  • 最后我们就要对构建的超定方程组进行求解(求解线性最小二乘Ax=b,返回得到的矫正矩阵)
  • 然后计算校正之后的位姿,再发布校正之后位姿的路径

对第二节最小二乘法的小总结

你可能感兴趣的:(SLAM)