激光SLAM从理论到实践学习——第二节（传感器数据处理1:里程计运动模型及标定）

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

里程计模型

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

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

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

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

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

三、航迹推算

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

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

    

  • 递推公式（在代码中是可以经常看到的）
    

    • 实际情况，存在噪声
      

  • 影响里程计的因素

    • 系统误差
      • 是一个确定的误差，比如轮子直径量错一点，可以进行标定，今天也就是为了解决系统误差，进行里程计标定。
    • 随机误差
      • 随机出现的误差，比如轮子打滑，无法标定

• 刚刚讲完了差分、全向三轮的运动模型与航迹推演计算过程，接下来要解决的问题就是去解决系统误差，对里程计进行标定。

里程计标定

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

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

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

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

  • 假设A是一个m * n的矩阵，x为n * 1的向量，x也就是我们要求解的状态量
  • 他的状态也就是三种，当m=n时，是适定方程组，方程组有唯一解
    • m=n其实就是Ax=b中的行表示一个约束，列表示自由度，或者说是未知数变量的个数。也就是约束的数量与未知数的个数是相等的，这时候方程组有唯一解。
  • 当m<n时，欠定方程组，方程组有无限多解
    • m<n也就是说约束的数量小于未知数的个数，约束不足，就叫欠定
  • 当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

    
    

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

    

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

  

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

  

  展开计算有：

  

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

  

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

  

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

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

  

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

  

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

  

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

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

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

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

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

  

然后代入数据得：

求出a=5.063，b=2.009

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

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

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

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

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

• 直线线性方法

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

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

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

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

      

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

      

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

      

    • 那么矩阵就是

      

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

      

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

      

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

      

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

      
      

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

作业实现过程

根据上面说的使用直线线性的方法，用雷达的数据作为观测值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，返回得到的矫正矩阵）
• 然后计算校正之后的位姿，再发布校正之后位姿的路径

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