激光SLAM入门笔记(三):激光雷达运动畸变去除

内容基于深蓝学院课件,如有错误,烦请斧正,不胜感激。

1.激光雷达(激光测距雷达)

1.1 传感器介绍

基于三角测距原理的传感器

主要简单介绍三角测距原理。
原理如下图, A A A是接收器镜头, B B B是激光发射器。
L , α , β L,\alpha,\beta L,α,β均是已知量。
在这里插入图片描述

基于TOF测距原理的传感器

主要简单介绍TOF测距原理。
TOF是飞行时间(Time of Flight)的简写。
在这里插入图片描述
主要简单介绍波形相位差测距原理。
精度较TOF高。
在这里插入图片描述

传感器比较

优点不足适用场景
三角测距特点中近距离精度较高;价格便宜远距离精度较差;易受干扰一般在室内使用(如扫地机器人)
TOF特点测距范围广;测距精度高;抗干扰能力强价格昂贵室内室外皆可

1.2 数学模型介绍

1.2.1 波束模型

介绍

在这里插入图片描述

缺点

在这里插入图片描述

1.2.2 似然域模型

在这里插入图片描述

2.运动畸变

2.1 原因

运动畸变产生的原因

  • 激光点数据不是瞬时获得
  • 激光测量时伴随着机器人的运动
  • 激光帧率较低时,机器人的运动不能忽略
    在这里插入图片描述

知乎上也有相关回答
结合运动畸变示意图来看,以图中左下角仅平移为例,设实心三角形位置为 A A A,空心三角形位置为 B B B
机器人在 A A A处于初始时刻 t 0 t_0 t0发射接受激光束 l 0 l_0 l0(角度为 θ 0 \theta_0 θ0),测得距离 d 0 d_0 d0,得到 p p p点。
在下次发射激光束之前,机器人向前移动了一段距离至 B B B处。
机器人在 B B B处于 t 1 t_1 t1时刻发射接受激光束 l 1 l_1 l1(角度为 θ 1 \theta_1 θ1),测得距离 d 1 d_1 d1,得到 q q q点。
由于机器人向前移动了一段距离,因此 d 1 < d 0 d_1<d_0 d1<d0,但我们还以为机器人在 A A A处,还是以 A A A处坐标系为准,导致了最终画出来的 p p p点贴墙, q q q点不贴墙,以此类推,最终一排点呈现如运动畸变示意图所示。
其实我们假想的是激光束 l 0 , l 1 l_0,l_1 l0,l1为同一个坐标系,然而由于机器人的运动,导致激光束 l 0 , l 1 l_0,l_1 l0,l1实际并不是同一个坐标系,即出现了运动畸变,消除运动畸变的过程实际上就是计算激光束坐标系相对于初始时刻坐标系的转换关系的过程。

2.2 去除

2.2.1 纯估计方法

ICP方法

求解两个点云集合转换关系的最通用的方法。

数学描述

给定两个点云集合 X , P X,P X,P
在这里插入图片描述
求解旋转矩阵 R R R和平移向量 t t t,使得下式最小
在这里插入图片描述

求解方法
已知对应点
  1. 求点云集合 X , P X,P X,P的几何中心 u x , u p u_x,u_p ux,up
    在这里插入图片描述

  2. 点云集合 X , P X,P X,P中的点去中心化
    在这里插入图片描述

  3. SVD分解
    在这里插入图片描述

  4. ICP的解
    在这里插入图片描述

未知对应点(大多数情况)

实际中,一般是不知道对应点匹配的,不能一步到位计算出 R R R t t t,因此需要迭代计算,用到期望最大化(Expectation Maximization,EM)的思想。
流程

  1. 寻找对应点(找最近的点)
  2. 根据对应点,计算 R R R t t t
  3. 对点云进行转换,计算误差
  4. 不断迭代,直至误差小于某一个值
VICP方法

ICP方法没有考虑激光的运动畸变以及出现错误的激光数据。
VICP方法

  • ICP方法的变种
  • 考虑了机器人的运动
  • 匀速运动
  • 进行匹配的同时估计机器人的速度
描述

在这里插入图片描述

流程

在这里插入图片描述

效果

在这里插入图片描述

2.2.2 里程计辅助方法(本次重点

VICP方法缺点

  • 低帧率激光(5HZ),匀速运动假设不成立
  • 数据预处理和状态估计过程耦合

解决方法

  • 尽可能准确地反映运动情况
  • 实现预处理和状态估计的解耦

引出传感器辅助方法(里程计/IMU)

  • 极高的位姿更新频率(200HZ),可以比较准确地反应运动情况
  • 较高精度的局部位姿估计
  • 跟状态估计完全解耦
对比

IMU与轮式里程计对比

IMU×轮式里程计√
直接测量角速度和线加速度直接测量机器人的位移和角度
具有较高的角速度测量精度具有较高的局部角度测量精度
测量频率极高(1kHz~8kHz)具有较高的局部位置测量精度
线加速度精度太差,二次积分在局部的精度依然很差×更新速度较高(100Hz~200Hz)√

在单片机和处理器上处理对比

单片机×处理器√
用单片机读取激光雷达数据,每次读取激光点数据时都可以获取当时机器人的位姿,根据机器人位姿消除运动畸变。得到一帧完整的数据后,上传至处理器用CPU读取激光雷达数据,同时单片机上传里程计积分数据,两者进行时间同步。 在CPU上统一进行运动畸变去除
在单片机层消除运动畸变体系清晰,不会产生延时
无需考虑时间同步的问题需要进行时间同步
需要对数据进行压缩,否则会产生较大延时×需要进行位姿插值√
目的

通过已知量求未知量。

已知量
  • 当前帧激光起始时间结束时间: t s , t e t_s,t_e ts,te
  • 两个激光束之间的时间间隔: Δ t \Delta t Δt

且满足以下条件

  • 里程计数据按照时间顺序存储在一个队列中
    FIFO,队首数据时间最早,队尾数据时间最晚。
  • 最早的里程计数据的时间戳< t s t_s ts
  • 最晚的里程计数据的时间戳> t e t_e te
未知量(求解目标
  • 当前帧激光数据中每一个激光点对应的机器人位姿: p t s , p t s + Δ t , . . . , p t e p_{t_s},p_{t_{s+\Delta t}},...,p_{t_e} pts,pts+Δt,...,pte

接下来

  1. 根据求解的位姿把所有激光点转换到同一坐标系下
  2. 重新封装成一帧激光数据,发布出去
过程
求解 t s , t e t_s,t_e ts,te时刻的位姿 p s , p e p_s,p_e ps,pe

利用 p l , p k p_l,p_k pl,pk可以线性插值得到一条直线 L 1 L_1 L1,而 l < s < k l<s<k l<s<k,可得出 p s p_s ps p e p_e pe求解过程类似。
在这里插入图片描述

二次插值

使用线性插值得到机器人在 t s , t e t_s,t_e ts,te中间时刻 t m t_m tm的位姿 p m p_m pm
在这里插入图片描述
假设在一帧激光数据之间,认为机器人做匀加速运动,因此机器人的位姿是关于时间 t t t的二次函数。
已知 p s , p m , p e p_s,p_m,p_e ps,pm,pe,二次插值近似该二次函数得到二次曲线 L 2 L_2 L2
在这里插入图片描述
二次曲线 L 2 L_2 L2如下图
在这里插入图片描述

二次曲线的近似

目标
使用分段线性函数对二次曲线 L 2 L_2 L2进行近似
在这里插入图片描述
⚠️分段数大于3时,近似误差可以忽略不计,例如
在这里插入图片描述
过程

  1. t s t_s ts t e t_e te时间段内,一共取 m m m个位姿{ p s , p s + 1 , . . . , p s + m − 2 , p e p_s,p_{s+1},...,p_{s+m-2},p_e ps,ps+1,...,ps+m2,pe}。
    位姿通过线性插值获取,在这 m m m个位姿之间,又进行线性插值
    在这里插入图片描述
  2. 得到这些数据之后即可分段线性插值得到二次曲线 L 2 L_2 L2的近似
激光点坐标转换及激光数据发布

在这里插入图片描述

2.2.3 融合方法

将纯估计方法与里程计辅助方法结合,过程如下

  1. 用里程计辅助方法进行矫正,去除绝大部分的运动畸变
  2. 认为里程计存在误差,但是误差值是线性分布的
  3. 用ICP的方法进行匹配,匹配的结果作为正确值,得到里程计的误差值
  4. 把误差值均摊到每一个点上,重新进行激光点位置修正
  5. 再一次进行ICP迭代,直到收敛为止

⚠️位置误差的线性假设比位置线性假设更合理

3.附录

线性插值

几何意义

用过插值节点的直线近似原函数。
如图,利用过点( x 0 , y 0 x_0,y_0 x0,y0)和( x 1 , y 1 x_1,y_1 x1,y1)的直线 L ( x ) L(x) L(x)来近似原函数 f ( x ) f(x) f(x)
在这里插入图片描述

过程

线性插值百度百科

二次插值(抛物线法)

几何意义

过程

抛物线法百度百科

评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值