无人系统基础教学课件第5章-无人驾驶定位系统

无人系统基础第5章无人驾驶定位系统载波相位差分（RTK）技术基于GPS/IMU的组合导航GPS/IMU传感器信息融合定位基于GPS/IMU的组合导航组合导航系统中基于卡尔曼滤波的估计方法依据滤波状态选择方式可分为直接法与间接法。

直接法

间接法绪论1坐标变换基础知识2前端里程计3后端图优化5回环检测4基于激光雷达的SLAM算法SLAM(SimultaneousLocalizationAndMapping)问题描述机器人在运动过程中根据地图进行位姿估计，同时增量构建地图，以实现机器人自主定位和导航分类根据传感器类型可将SLAM划分为：视觉SLAM激光雷达SLAM应用领域服务机器人无人驾驶无人机等热门领域激光雷达SLAM流程图前端里程计快速估计相邻两帧点云间相对位姿变换回环检测根据环境信息判断机器人是否曾经到达过当前位姿附近并添加回环约束后端优化使用非线性优化方法，消除前端里程计的累计误差地图创建更新和重建全局地图坐标变换基础知识-旋转矩阵内积：外积：：三维向量：

的反对称矩阵坐标变换基础知识-旋转矩阵三维坐标系中位姿由旋转和平移组成初始坐标系下的单位正交基旋转后坐标系下的单位正交基向量

t

在初始坐标系下的坐标表示向量

t

在旋转后坐标系下的坐标表示R

为旋转矩阵坐标变换基础知识-旋转矩阵旋转矩阵是行列式为1的正交矩阵，定义如下：因为旋转矩阵是正交矩阵，那么反向旋转可以由逆运算表示成：将旋转和平移组合：R

表示旋转矩阵，

表示平移坐标变换基础知识-变换矩阵在三维向量的基础之上增加一维度变换成齐次坐标上式将旋转和平移合并，称为变换矩阵用T表示变换矩阵定义如下：反向变换可用T矩阵的逆表示：坐标变换基础知识-旋转向量旋转向量由旋转轴和旋转角组成通过罗德里格斯可以实现到旋转矩阵的转换

表示旋转轴，为单位向量

表示旋转角，标量

表示旋转向量

旋转轴上的向量在旋转后不会发生改变

坐标变换基础知识-四元数四元数

q

由一个实部和三个虚部组成i

j

k

为四元数虚部

四元数可以用4x1列向量表示旋转向量转四元数：

根据罗德里格斯公式

坐标变换基础知识-四元数四元数转旋转向量：四元数转旋转向量：旋转矩阵转四元数：坐标变换基础知识-李群

对任意旋转矩阵R：将旋转矩阵表示为时间的函数并求导是一个反对称矩阵李群既有群特性，又具有光滑特性旋转SE(3)和SO(3)在空间上连续，二者均为李群坐标变换基础知识-李群

对旋转矩阵求导只需左乘

它描述旋转矩阵R的局部倒数关系反对称矩阵A到向量a的转换：向量a的反对称矩阵：坐标变换基础知识-李代数

每个李群都有与之对应的李代数，李代数由一个集合A、一个数域和一个二元运算[,]组成，且需要满足以下条件：

坐标变换基础知识-李代数

SO(3)对应的李代数为so(3),二者为指数映射关系李代数se(3)推导：其中

T

随时间变化，表示为T(t)SO(3)对应的李代数为so(3),二者为指数映射关系坐标变换基础知识-李代数

由so(3)知：由上式推导可得：

坐标变换基础知识-李代数

李群、李代数相互转换：

坐标变换基础知识-李代数

最终可得：se(3)的指数映射推导：n=0n=1n=2以此类推：令：坐标变换基础知识-李代数

最终可得：

令：坐标变换基础知识-李代数

李群与李代数间互相转化图坐标变换基础知识-李代数求导

李代数向李群的转换为指数映射常数情况下：指数为矩阵时：令F=C+D：令so(3)上两个向量相加不等于SO(3)上两个矩阵相乘坐标变换基础知识-李代数求导

引入Baker-Campbell-Hausdorff公式考虑SO(3)的李代数：

坐标变换基础知识-李代数求导

SLAM中经常需要构建与位姿有关的函数，讨论位姿导数的函数，有两种位姿求导方法：(1)直接对T求导(2)通过左扰动或右扰动求导先介绍直接对T求导考虑SO(3)的情况对一个空间点p进行旋转得到Rp记旋转之后点的坐标相对于旋转的导数：

坐标变换基础知识-李代数求导

通过左扰动或右扰动求导

一阶泰勒展开得

T

左乘扰动

设扰动的李代数为

坐标变换基础知识-李代数求导

通过

SLAM-前端里程计

前端里程计点云预处理特征点提取特征点匹配通过特征点构建约束方程迭代求解约束方程得到位姿前端里程计主要内容SLAM-前端里程计

点云预处理-去地面激光雷达线束纵向排列，其发射的64条激光光束全部投影到xoy平面上

，如下图：

SLAM-前端里程计

地面点第一种情况SLAM-前端里程计

地面点第二种情况SLAM-前端里程计

地面点第三种情况SLAM-前端里程计

原始点云图去地面效果图SLAM-前端里程计

特征点提取

需要从激光雷达生成的稠密点云中提取特征点用于后续计算，这里我们将特征点按照曲率大小分为平面点和边角点，如下图所示：

曲率计算公式：L：

表示雷达坐标系

为k条线束的i个点

S：当前点的周围点集

提取原理：平面点的曲率小

边角点的曲率大SLAM-前端里程计

曲率很大的点云

曲率大的点云

曲率很小的点云

曲率小的点云SLAM-前端里程计

特征点匹配-点云去畸变

如下图所示，雷达从A点运动B点，激光雷达旋转一圈。但雷达认为得到的点云是在A点扫描到的，这就产生了运动畸变，

实线：障碍物原型

虚线：A到B采集的点云SLAM-前端里程计

通过线性插值的方式去除运动畸变

前三个为平移向量

t

后三个为旋转向量q插值公式为：

：在

i

点的位姿

去畸变原理图SLAM-前端里程计

特征点匹配经典ICP(IterativeClosestPoints)

在特征点提取时，我们得到四种点云：曲率很大、曲率很小、曲率和曲率小

由于曲率大的和曲率小的含有更多的信息，我们接下来在上一帧的曲率大的边角点中寻找当前帧曲率很大的边角点的最近邻点。最后优化求解，更新车辆位姿

以边角点为例，在上一帧中寻找当前帧边角点A的最近邻点a、b，最小化A到a、b组成的边角线（即点到线）的距离，求解并优化车辆位姿SLAM-前端里程计

(1)构建约束方程

:上一时刻点云转换到结束位置的点:是最近邻点:当前时刻点云转换到起始位置的点:当前帧待匹配的点边角点距离原理图i:当前帧点i:a和b为最近邻点

SLAM-前端里程计

(2)构建Jaccobian矩阵平移向量旋转向量当前帧点投影到上一帧：

目标函数为：通过对q、t

分别求偏导，优化目标函数R中自变量为：中自变量为：SLAM-前端里程计

(3)优化求解优化方法：牛顿法、最速下降法、L-M优化最速下降法公式：沿着负梯度方向寻找最优解，在靠近最优解时速度会变慢牛顿法公式：速度比上面更快，但对初值要求很高L-M：综合了前面两种的优势，远端用最速下降法,最优解附近用牛顿法

SLAM-前端里程计

假设目标函数为：残差模块损失函数代价函数(1)LossFunction：损失函数用来减小输入为异常值的影响。代码中使用鲁棒核函数HuberLoss

s:常量基本概念：(2)LocalParameterization：维数重构将四元数归一化，使四个自由度变为三个。如果直接传递四元数进行优化，冗余的维数会带来计算资源的浪费SLAM-前端里程计

(3)AddResidualBlock()：残差模块传递的参数主要包括代价函数模块、损失函数模块和参数模块。代价函数：包含参数模块的维度信息。损失函数：避免错误量对估计的影响。参数模块：待优化的参数，程序中待优化参数为平移向量t

和旋转向量

q。

(4)AddParameterBlock()：传递参数模块(5)AutoDiffCostFunction：自动求导SLAM-地图创建

由于外界噪声的原因，前端里程计得到的车辆位姿存在偏差。

因为子地图相比较于单帧点云拥有更丰富的特征信息，使用当前帧点云与子地图进行匹配能够得到更精确的车辆位姿。

基于前端里程计输出的特征点云建图：

将当前帧特征点云与对应子地图进行匹配(即边角点与边角点地图匹配，平面点与平面点地图进行匹配)，更新位姿。如右图所示:

SLAM-地图创建

LOAM-东北大学刘长春体育馆地图创建SLAM-回环检测

回环检测激光雷达SLAM相对独立的模块，并不是所有SLAM算法都有回环检测。

得到的位姿都是通过点云和点云匹配得到的，该位姿会产生漂移误差。且这种漂移误差会随着距离的增加而逐渐增加。

回环检测通过判断小车是否回到之前到达过的某个地方，修正回环内所有的位姿，进而优化车辆位姿，减小漂移误差回环检测原理图

SLAM-后端图优化

因子图优化

贝叶斯网络概念：贝叶斯网络又称之为有向无环图，是一种概率图模型。

a为b父节点表示成概率为P(b|a),同理c可以表示为P(c|a,b)。联合概率表示两个事件同时发生概率P(a,b,c)=P(a)P(b|a)P(c|a,b)

先验概率表示根据以往分析得到的概率，后验概率为事情发生后计算的概率,利用贝叶斯概率进行计算。

贝叶斯网络

SLAM-后端图优化

链状贝叶斯网络