光传感器数据：传感器数据处理1：里程计运动模型及标定 第1张" title="激光传感器数据：传感器数据处理1：里程计运动模型及标定 第1张-传感器知识网"/>

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

激光SLAM——第二节（传感器数据处理1:里程计运动模型及标定）
技术标签： SLAM
传感器数据处理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

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：
原作者删帖
不实内容删帖
广告或垃圾文章投诉
智能推荐
毕业论文设计：第二部分—激光雷达里程计研究
激光雷达里程计 1. LOAM 1.1 符号定义 1.2 系统结构 1.3 激光里程计 1.3.1 特征提取 1.3.2 寻找特征点对应关系 1.3.3 运动估计 1.3.4 激光里程计算法 1.4 激光建图 1.5 实验结果 1. LOAM 传感器采用：2轴激光雷达（6个自由度运动） 使用激光雷达的一个主要优势是它对环境照明和场景中的光学纹理不敏感。激光雷达的最新发展缩小了它们的尺寸和重量。激光...
二轮差分模型分析及里程计计算
小型移动机器人的底盘主要有两轮差速底盘和四轮差速底盘两种，因为两轮差速底盘的控制原理以及运动模型分析比较简单，所以运用的比较多，下面主要介绍两轮差速底盘的工作原理。 如图1所示，两轮差速底盘由两个驱动轮和一个万向轮组成，两个驱动轮分别位于底盘的左右两侧，并且独立控制速度，通过不同的速度来实现底盘的转向，万向轮的作用是辅助支撑。 如图5.1所示，v所指方向为移动机器人的前进方向，vl、vr分别为左轮...
视觉里程计1
特征点的选择 图像特征点选取经历了长久的发展，在选取特征的构成中形成了若干种发方式，比较著名的有SIFT,SURF,ORB等。以上所说的三种方式各有各的特点，在处理不同的slam问题时，可以面向自己的问题选择一种较好的方式。 本文是以单目视觉传感器为基础将的，所以GPU加速等设备就不考虑了，因为成本问题，所以选择了性能折中的ORB特征。 ORB特征也是由关键点和描述子两部分组成。它的关键点是Ori...
SLAM入门之视觉里程计(6)：相机标定 张正友经典标定法详解
想要从二维图像中获取到场景的三维信息，相机的内参数是必须的，在SLAM中，相机通常是提前标定好的。张正友于1998年在论文："A Flexible New Technique fro Camera Calibration"提出了基于单平面棋盘格的相机标定方法。该方法介于...
物理机与虚拟机redhat 6.4 如何实现telnet远程控制
虚拟机中安装telnet 并实现物理机通过telnet远程控制虚拟机步骤 1安装telnet 2安装虚拟机 3在虚拟机中安装linux 4查看虚拟机中是否安装telnet: Root权限下收入telnet命令,具体如下图： 出现”command not found”表示telnet没有安装。 5安装telnet:（虚拟机简体中文版） 5.1加载linux虚拟光盘 ? 在&l...
猜你喜欢
python字符串类型本质上是一种字符序列_Python 文本序列类型（字符串类型 str）[学习 Python 必备基础知识][看此一篇就够了][长字符串][原始字符串 r][字符串常用方法]...
您的“关注”和“点赞”，是信任，是认可，是支持，是动力...... 如意见相佐，可留言。 本人必将竭尽全力试图做到准确和全面，终其一生进行修改补充更新。 1 字符串类型概述 在 Python 中处理文本数据是使用 str 对象，也称为字符串类型。 字符串是由 Unicode 码位构成的不可变序列。 简单理解，若干个字符(可以包含字母、标点、特殊符号、...
Android关于APP has stopped和R文件出错的原因
众所周知，R文件是项目开发中自动生成、自动编译的，不可手动编译。那为什么有的时候R文件那行代码还会报错呢？ 就像下面的图片，R.layout，这是系统自动生成的文件，检查了一下代码，代码也没有问题。 我看了一下网上的几个方法，基本上都是说clean一下工程或者Rebulid一下工程，甚至重启一下AS，虽然有的人反映可以了，但是我这边还是一样的报错。 这时候我就感觉应该版本的问题了，我的AS版本是比...
java 编程个人突破3
...
湖仓一体解决方案
数据湖产生的背景 由于云技术的推动，企业对于跨公司、跨行业、跨领域的综合型数据的需求日趋明显，不同类型、格式数据之间的关联性碰撞越来越激烈，刺激着数据技术的创新发展，逐渐形成了大数据生态结构。当前面临的问题的复杂性、综合性、交叉性，导致数据的使用成本越来越高，企业迫切需求能够有效打破数据孤岛、解决数据主权、统一数据汇聚和共享的混合式数据平台，数据湖应运而生。 数据湖的概念 早在2011年，福布斯的...
torch.utils.data.sampler的使用讲解
今天上午过来继续看代码昨天走的有点早啊 ，这部分没看完。这里的sampler的作用是，我们的batdch的文件中的图片是以什么样的形式采样得到的（这里就是根据概率值的大小来采样然后放到batch中去）。这里的关键字：weigths和num_samples ，replacement的含义分别是：weigths权重是为了给每一类的样本赋一个权重值，真正的概率值大小和权重数值大小没关，和权重之间的比值是...

激光传感器数据：激光SLAM传感器数据处理

激光传感器是利用激光技术进行测量的传感器。它由激光器、激光检测器和测量电路组成。激光传感器是新型测量仪表，它的优点是能实现无接触远距离测量，速度快，精度高，量程大，抗光、电干扰能力强等。
激光和激光器——激光是20世纪60年代出现的最重大的科学技术成就之一。它发展迅速，已广泛应用于国防、生产、医学和非电测量等各方面。激光与普通光不同，需要用激光器产生。激光器的工作物质，在正常状态下，多数原子处于稳定的低能级E1,在适当频率的外界光线的作用下，处于低能级的原子吸收光子能量受激发而跃迁到高能级E2。光子能量E=E2-E1=hv,式中h为普朗克常数，v为光子频率。反之，在频率为v的光的诱发下，处于能级 E2的原子会跃迁到低能级释放能量而发光，称为受激辐射。激光器首先使工作物质的原子反常地多数处于高能级（即粒子数反转分布），就能使受激辐射过程占优势，从而使频率为v的诱发光得到增强,并可通过平行的反射镜形成雪崩式的放大作用而产生强大的受激辐射光，简称激光。激光具有3个重要特性：（1）高方向性（即高定向性，光速发散角小），激光束在几公里外的扩展范围不过几厘米；（2）高单色性，激光的频率宽度比普通

激光传感器数据：激光SLAM理论与实践 笔记-传感器数据处理（II） 第四期

第三章 传感器数据处理II：激光雷达
3.2 激光雷达畸变的矫正
3.2.1 产生的原因
激光雷达由于小车在旋转，激光雷达会有一个角度的运动畸变，需要矫正，否则和实际偏差很大；

3.2.2 激光雷达的分类
① 三角测距原理（A1雷达）

② TOF测距（飞行时间）
精度是固定的（由于光速不变，时间计数器精度不变 ）

3.2.3 激光雷达的数学模型
激光雷达光束有四种数学模型：

a、光束混合模型，计算期望值、得分。缺陷明显，一般不用

b、似然场计算模型，计算量低，适合结构化环境和非结构化环境，最常用；

b、似然场模型

3.2.4 运动畸变及其去除
产生原因：运动导致激光雷达未采集完一帧时，机器人已经运动出一段距离（雷达坐标系原点相对世界坐标系不断移动）。（硬件帧率较低）可以使用算法（修正雷达坐标系原点）修正运动畸变问题。

去除激光雷达运动畸变：
 1、纯激光估计方法，VICP（迭代最近点）方法，EM算法（固定A点，计算B点；固定B点，计算A点~~不断迭代）的特例。【缺点，没有考虑激光的运动畸变，当前的激光数据源是错误的、认为小车匀速运动、慢速运动才好（激光帧率5Hz）、数据预处理和状态估计过程耦合】，获取每一个点的真实坐标，通过运动速度、位姿变换等逆算出激光点原点去统一原点的数据。
– 3.2.4.1 纯估计方法去除畸变（仅激光）：
ICP方法求解流程：
 1、求解两帧点云的几何中心ux、up；2、在点云矩阵中减去几何中心ux、up（为了提高数据的稳定性）；3、得到一个W矩阵4、代入对应点，得到ICP的解：

注意：实际中，并不知道对应点，不能一步求解出R、t，所以需要迭代求解对应点；求解对应点算法流程（一种EM算法）：
 1、寻找对应点（需要初始输入的R、t），由R、t求‘对应点’；2、根据对应点，再求解进一步的R、t，由‘对应点’求解R、t；3、对点云进行转换，计算误差；4、不断重复123点，迭代求解，直到误差小于某阈值；
缺点：没有考虑小车的运动速度，求解的点云偏差较大，没法修正；

VICP方法求解流程：
解决了ICP的缺点。

简介：

1、ICP算法的变种；

2、考虑了机器人的运动（假设是匀速运动）；

3、进行点云匹配的同时，估计机器人的速度；

数据变量的设置：

1、Xi、Xi-1表示第i帧和第i-1帧的数据；

2、Ti，Ti-1表示其对应的位姿矩阵；

3、机器人的速度Vi；

4、设第Ti帧的时刻表（全部以Ti时刻加或减去一个固定的时间间隔Ts，因为假设匀速运动）；

5、则第i帧第i个节点的位姿矩阵可求得：李群矩阵

VICP方法的缺点：

1、低帧率激光，（肯定有加减速的过程）匀速运动假设不成立；

2、数据预处理和状态估计过程耦合；

算法流程:

– 3.2.4.2 传感器辅助去除畸变（IMU、odom）
解决了VICP的问题，可以尽可能的准确反映运动情况；实现预处理和状态估计的解耦合；

里程计/传感器辅助使用Odom/IMU，200Hz位姿更新频率
 a、IMU精度效果很差，一般不用（只用角速度数据）；b、轮式里程计精度很高；

具体方法：

线性插值，假设在200hz，5ms的时间内是匀速的。求速度

1、激光数据和里程计数据的时间已经同步，用下图的第1点；

2、激光数据和里程计数据的时间没有同步，则线性插值处理，用下图的第2点；

求ts、te时刻里程计位姿：

1、二次插值：
得到激光数据的起点的位姿后，认为机器人做匀加速运动，求终点位姿（这段路程的终点）。

2、二次曲线的近似
Psi：获取这段曲线中的每一个点的位姿（间隔5ms，近似）

3、坐标系统以及矫正的激光数据发布

– 3.2.4.3 融合方法去除畸变
配合5.4.1和5.4.2的方法，按照下面的方法融合两者：

一般5.4.2的轮式里程计畸变去除已足够，这是拓展内容

激光传感器数据：用STM32F103去读取激光传感器的数据

1 准备工作
1) 硬件设备：激光测距VL53L1模块、六合一、USB-TTL、杜邦线、STM32F103C8T6单片机。给大家看下图片。
VL53L1
STM32F103C8T6
2）软件准备 ： KEIL 5 (5.17版本的） 链接： 提取码：eq50.
2 接线方式
1、使用数据线将STM32F103C8T6与电脑连接通信
2、VL53L1与STM32F103C8T6使用杜邦线连接。具体接线如下图所示：
VL53L1---------------STM32F103C8T6
VCC--------------------VCC
? ? ?  TX-----------------PA3
? ? ?  RX-----------------PA2
GND--------------------GND
3 生成下载文件
1、在OUTUT勾选Creat HEX File
2、找到生成的HEX文件
## 4 下载程序
1、用数据线连接好STM32F103C8T6单片机和PC端。
2、按照“软件配置.png”图示设置好软件。
3、找到所需的hex文件。
4、按STM32F103C8T6单片机的红色的Bootloader 键
5、点击开始编程按钮，下载程序
## 5 输出结果显示
在电脑上正确连接好板子，首先打开串口调试助手，找到相应的端口，然后打开串口，注意这里波特率设置为，然后就可以观察到左边的窗口有数据输出了。如图所示：