基于 3D 激光雷达的位姿估计

REF：智慧工厂下基于激光 SLAM 的无人车导航技术研究与实现

1. 概念解释

• 阿克曼转向底盘：在车转弯时内外轮胎的转弯角度不同，使得内轮胎的转弯半径小于外轮胎的转弯半径，转向采用相对简单的机械结构，使用传统的转向链接和转向臂，实现了更小的转弯半径，从而提高了车辆在狭窄或弯曲道路上的机动性，使得车辆更易于驾驶和控制。

21c442849adf483629feaa96fdbbc053.png

656335eb919e16f941b1a3fe979b20aa.png

• 两步LM法：用于优化机器人位姿，第一步，通过匹配平面特征和其对应在特征点集合中的位置，计算出t时刻的z轴位置，翻滚角和俯仰角；第二步，通过匹配边缘特征和其对应在特征点集合中的位置，并将第一步中的三个位置信息作为约束，求解出t时刻机器人在x轴和y轴的位置以及航向角，融合计算求得的机器人位姿信息，便可以获得两个连续扫描之间机器人的最优变换位姿。

02383f9d38bbe739bf2e2eb7b2345b41.png

• 八叉树结构：有八个子节点的树，表示实际的数据结构像树根一样不断往下扩展，分成八个小枝，直至叶子为止。

e84714a0d6924304682259a6687417db.png

• 八叉树存储：基本原理是通过不断切割立方体，将大立方体所包含的信息由所分割的小立方体表示，而根据小立方体内是否包含大立方体的信息可在存储数据时将一些未包含信息的小立方体去除，以节省存储空间

b50df898595bf694da45dcd800e8c51e.png

• 点云压缩：将扫描出的3维点云，分离出地面点云，设置地面高度阈值，利用直通滤波将地面点云进行粗处理，实现地面点云初步滤除，再利用半径滤波设定较小的阈值，进一步对地面点云进行噪声处理，只保留具有一定高度的非地面特征点云。
• 马尔可夫定位：一种基于概率的定位方法，基于马尔可夫模型，利用环境感知和控制信息，根据机器人的运动和观测结果来确定机器人的位置概率分布，在无人车行驶过程中，可以通过估计的方法获得车辆在某个时间的精确位置，将车辆的位置假定为是概率分布的，随着车辆的移动，这个概率也会随之分散，利用滤波器对各种变量数据进行处理，可 以缩小这种概率分布，利用贝叶斯滤波器处理各种变量数据，实现无人车的定位
• 蒙特卡洛定位：通过粒子滤波进行定位，以每个粒子代表机器人的一个可能状态，以粒子滤波器表示机器人可能的 状态分布，实现机器人在已知地图中的位姿估计
  1. 初始状态：将大量粒子在配置空间中均匀随机分布以模拟机器人的运动状态
  2. 预测阶段：通过机器人位姿移动生成的运动方程，将原有粒子带入方程，进行重新预测，获得预测粒子
  3. 校正阶段：利用高斯分布钟形曲线，即随着距离增加，权重单调递减的方式给预测粒子添加权重，并进行归一化
  4. 重采样：对粒子进行权重筛选，筛选保留较多的大权重粒子和少量的小权重粒子
  5. 滤波：将重采样后的粒子重新带入步骤（2）中的运动方程获得新的预测粒子，再次进行循环，直至获得机器人的准确位姿，实现机器人的定位。

  

  3e0450877fcc239704e3e88168424678.png

  

  22dd257b5ec6fbfede7f43e44fdad8ff.png

• 自适应蒙特卡洛定位：是基于 MCL 算法提出的一种增强算法，解决了 MCL 算法可能出现的绑架问题和粒子数固定的问题
  • 机器人绑架问题：指类似机器人突然被移动到另一个地方的问题，自适应蒙特卡洛定位算法通过计算预测粒子的平均权重，与上次迭代过程中的平均权重作对比，发现平均权重突然降低，则通过在全局中重新布置一些新粒子，使机器人重新估计在地图中的位姿，提高粒子的权重。
  • 粒子数固定问题：指算法迭代至定位快完成时，剩余粒子重叠，继续以这些粒子进行迭代会造成计算资源的浪费问题；算法通过采样对粒子占据比例进行计算，并设定阈值，在每次迭代重采样过程中，对于超过阈值的分散粒子，设置较高的粒子数量上限；低于阈值的集中粒子，设置较低的粒子数量上限，减少下次迭代粒子数目。
• NDT定位：将多维参照数据正态分布化，通过优化方法求出最优变换参数，使参考系中变换点的概率密度最大，实现激光点云的最优匹配，求解出机器人的准确位姿

5bb21e01161482758691220d87b694b9.png

• 广度优先搜索算法：从指定的起始节点开始，逐层地向外扩展，先访问当前节点的所有相邻节点，然后依次访问相邻节点的相邻节点，直到遍历到目标节点或搜索完整个图。
• 机器人距离：一般数学计算采用的是欧几里得距离，但通常沿途障碍物导致机器人运动受限，不被允许按这种方式直接抵达，则采用曼哈顿距离能更好的描述运动代价，对于运动更加灵活的机器人运动，采用切比雪夫距离能更准确的描述机器人的运动代价： dis=max(∣x1−x2∣,∣y1−y2∣)dis = max(|x_1-x_2|, |y_1-y_2|)dis=max(∣x1−x2∣,∣y1−y2∣)

0f05a6e8fc82f14267f3ed6432038a58.png

• 贝塞尔曲线：是二维图形应用的数学曲线，以二阶贝塞尔曲线为例，在两条线段上找两个能等比例平分两条线段的点，将其连线中点改变比例连接，即为二阶贝塞尔曲线

7a5c86cdfe2f17fd9877fae5dd1e6004.png

• 时间弹性带算法：在所生成的全局路径初始轨迹上进行修正，调整机器人实时的运动轨迹，只考虑机器人几个连续状态影响运动时的动态约束，通过增加时间间隔产生n-1个时差T，通过机器人在某段时间内的位姿变化，显示机器人的轨迹运动学信息

a378a7babc4ad6f03baf3493fdbee2cb.png

2. 位姿估计

基于 3D 激光雷达扫描匹配的 NDT 定位算法，优化无人车定位精度。

a1ad0081c3750329f10ee7254ffe6613.png

3. 路径规划

全局路径规划算法改进：传统 A*算法核心公式：f(n)=g(n)+h(n)f(n)=g(n)+h(n)f(n)=g(n)+h(n)，其中中的g(n)是 Dijkstra 算法中实际代价的体现，表示从起点到当前节点 n 的实际代价，是已走过的路径的总代价，而h(n)是 BFS 算法中预估代价的体现，表示从当前节点 n 到目标节点的预估代价，通常通过启发式函数来计算，启发式函数可以依据地图的不同类型来选择；当 g(n)>h(n)时，路径更为准确，但搜索效率较低；而当 g(n)<h(n)时，搜索效率更高，但可能降低路径的准确性

• 传统 A-star算法采用四方向机器人运动进行节点的搜索，现代机器人普遍具有高自由度，将四方向机器人运动改进为八方向机器人运动，使机器人在栅格地图中可以沿对角线进行移动，可有效缩短路径距离
• 运动搜索使用切比雪夫距离
• 为代价函数增加动态权重：代价函数的值作为权重系数的判断依据，设定阈值，当代价函数的值大于阈值时，增大权重系数，以偏向快速搜索（在某个实验场景下：经验阈值为 18，权重系数 为 3） f(n)=g(n)+w∗h(n)f(n)=g(n)+w*h(n)f(n)=g(n)+w∗h(n)

8c35649e40567dc679c36f42b5bc9b20.png

局部路径规划算法改进：融合贝塞尔曲线和 TEB算法，对所得路径进行平滑处理

493197a50f44418562af1df3541993a6.png