自动驾驶定位算法：基于多传感器融合的状态估计(muti-Sensors Fusion)

2024-04-08 04:44
文章标签 算法 自动 定位 状态 传感器 驾驶 估计 融合 fusion sensors muti

本文主要是介绍自动驾驶定位算法：基于多传感器融合的状态估计(muti-Sensors Fusion)，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

自动驾驶定位算法：基于多传感器融合的状态估计(muti-Sensors Fusion)

image

附赠自动驾驶学习资料和量产经验：链接

1、传感器(Sensor)选取

自动驾驶系统中用于状态估计(State Estimation)的常用传感器包括GPS/GNSS、IMU、激光雷达(Lidar)。

image

状态估计(State Estimation)选用传感器需要考虑哪些因素：

1）误差不相关性。也就是说，用于Sensor Fusion的传感器其中单个传感器(Sensor Measurement)测量失败，不会导致其它传感器(Sensor)由于相同的原因而同时失败。

2）传感器的相互补充性。 比如IMU可以填充GPS两次定位间隔期间的定位输出，用于平滑GPS/GNSS的定位结果；GPS为IMU提供初值，消除IMU单独使用出现的偏移(Drift)的问题；Lidar可以弥补定位精度的问题，而GNSS可以为Lidar定位地图匹配提供地图范围数据。

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

2、传感器的标定(Sensor Calibration)

如果想要各个传感器能够相互协同，无间配合，传感器的标定是必不可少的。传感器的标定通常分为三种: 内参标定(Intrinsic Calibration)、外参标定(Extrinsic Calibration)和时间校准(Temporal Calibration)。

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

2.1 内参标定(Intrinsic Calibration)

传感器或者车辆的内参在传感器制造的时候就已经固定下来，传感器模型中的固定参数都是内参，都需要通过Intrinsic Calibration事先确定。

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

比如估计车辆运动距离的轮速计模型 �=�� 中，r就一个内参。另外激光雷达(Lidar)中扫描线的角度，在激光雷达计算模型中需要事先知道这个参数，以实现激光雷达扫描线(Scan Line)的拼接。

如何获取传感器的内参呢？实践中有几种方法:

1）从传感器制造商的使用说明书中获取。这种方法往往只能获取大概的参数，每个设备的内参都是不同，所以并不能获取比较精确的参数。

2）手工测量内参。比如车轮的半径，可以通过手工测量的方法获取。但是类似于激光雷达的内参无法通过手工测量获取。

3）Estimate as part of State。这种方式不仅可以获取精确的传感器内参，而且可以解决内参随时间变化的情况。比如汽车的轮胎半径漏气导致半径变小等。

2.2 外参标定(Extrinsic Calibration)

传感器的外参主要表达各个传感器之间的位置相对姿态，它是把各个传感器的数据坐标统一起来的必不可少的参数。

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

如何获取传感器的外参呢？实践中有几种方法:

1、CAD图纸。如果你能获取传感器安装的CAD图纸，那你就可以获得比较准确的传感器外参。

2、手动测量。当然手动测量的难度也非常高，因为传感器的中心往往在传感器内部，难以精确测量。

3、Estimate as part of State。这也是一个研究的方向。可以比较好的应对外参标定问题，但难度也比较高。

2.3 时间校准(Temporal Calibration)

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

时间校准对于各个传感器的数据融合至关重要。比如IMU的输出频率是200HZ，Lidar的输出频率是20HZ，只有按照最相近的时间进行对齐，才能将IMU和Lidar数据准确融合起来。

在实际应用中，各个传感器的相对时间误差是未知的，这些误差可能是由于各个传感器的预处理耗时不同导致的，也可能是由于各个传感器的计时器精度不同造成的。

如何校准传感器的时间呢？实践中有几种方法:

1）假设这些传感器的时间相对误差为0。当然忽略这些误差，会导致最终的融合结果比预期要差。

2）硬件同步。在硬件设计上保证各个传感器的时间戳对齐。

3、EKF-多传感器融合(Multi-Sensors Fusion)

image

自动驾驶汽车一般包含多个Camera、3D 激光雷达(Lidar)、惯性测量单元(IMU)、多个Radar、GPS/GNSS Reciver、轮速计(Wheel Odmetry)，这些传感器在运行过程中时刻都在以不同的频率发送不同类型的数据，多传感器融合模块需要将这些信息融合起来，不断更新自动驾驶车辆的状态(Vehicle State)。多传感器融合进行状态估计(State Estimation)的流程如下：

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

车辆运动模型(Motion Model Input)如下，它的信息一般来自于IMU，包含x、y、z三个方向上的加速度和角速度，是一个6维向量。

image

车辆运动模型的计算过程如下:

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

为了应用EKF，我们定义Error State如下，其中 image 是3x1的矩阵。

image

EKF的Motion Model如下:

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

EKF中的GNSS测量模型:

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

EKF中的Lidar测量模型:

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

这里假设激光雷达(Lidar)的测量结果和GNSS的测量结果都在同一个坐标系下(注意，实际情况下，需要经过坐标变换才能达到这种效果)

EKF的IMU+GNSS+Lidar多传感器融合流程如下:

1）Update State With IMU Inputs

image

2、Propagate Uncertainty

image

3、当有GNSS或者LIDAR测量结果到达时，进入步骤4），否则进入步骤1）。

4、计算GNSS/Lidar的卡尔曼增益(Kalman Gain)。

image

4、计算Error State。

image

5、Correct Predicted State。

image

6、Compute Corrected Covariance。

image

4.状态估计(State Estimation)的精度需求(Accuracy Requirements)

不同的应用场景对State Estimation的精度的要求不同，比如高速场景下的Lane Keeping一般要求亚米级级精度。如下图所示的场景，车辆宽度为1.8m，机动车道宽度为3m，所以车辆两侧有约60cm的冗余空间，在这种场景下，如果要实现Lane Keeping的功能，只要状态估计的精度小于60cm就可以满足实际应用的需求。

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

但在拥挤的城市道路交通场景下，对State Estimation的精度要求是越高越好，状态估计的精度越高，自动驾驶就越安全。

5、状态估计(State Estimation)的更新频率要求

以人类驾驶汽车为例，一个人开车过程中闭着眼睛，但为了保证行车安全，她每间隔1s睁开一次眼睛，以确定自己所在的位置。在空旷的道路场景下，1HZ的位置确认频率就可以保证，但是在繁忙的交通的道路上，1s确认一次位置的做法就非常不靠谱了。

但是，越高的定位频率带来的越高的计算资源消耗，而车载计算资源是有限的，并且还是感知、控制、决策、路径规划等所有功能共享的，所以在更新频率和计算资源之间需要有一个trade-off。

根据经验，15HZ-30HZ的状态更新的频率就能够满足自动驾驶的应用需求，当然在计算资源允许的情况下，状态更新(State Estimation)频率越高越好。

6、Sensor Failures

自动驾驶使用的传感器系统可能由于外部环境因素而失效，比如恶劣天气状况、硬件故障、系统连接线松了等等；也可能由于传感器自身的短板导致，比如GNSS在隧道场景下无法定位、在城市环境下定位的误差达到数十米，IMU容易收到温度变换的影响等。

即使在没有传感器异常的情况下，我们依然能够从多传感器的使用中收益。如下图所示，各个传感器的功能相互补充，构建安全的自动驾驶系统。

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

各个传感器各有所长，比如短距测量传感器可以在停车场景下，检测附近的障碍物，避免发生碰撞；中距测量传感器在车道保持场景下，检测周围的行人、机动/非机动车辆；长距测量传感器帮助我们检测和预测远距离障碍物的运动等等。在实际应用要充分考虑到这些传感器的长处和短板，并增加一定的冗余系统，保证在部分系统无法工作的情况下，仍然可以保证车辆的正常运行。

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

7、多传感器融合的代码实战

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

Couresas上的Multi-Sensors Fusion Project效果如下:

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

image

Coursera Lecture-State Estimation and Localization for Self-Driving Cars

参考链接:

https://medium.com/@wilburdes/sensor-fusion-algorithms-for-autonomous-driving-part-1-the-kalman-filter-and-extended-kalman-a4eab8a833dd​medium.com/@wilburdes/sensor-fusion-algorithms-for-autonomous-driving-part-1-the-kalman-filter-and-extended-kalman-a4eab8a833dd

https://www.coursera.org/learn/state-estimation-localization-self-driving-cars​www.coursera.org/learn/state-estimation-localization-self-driving-cars

这篇关于自动驾驶定位算法：基于多传感器融合的状态估计(muti-Sensors Fusion)的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/884593。

相关文章

不懂推荐算法也能设计推荐系统

不懂推荐算法也能设计推荐系统

本文以商业化应用推荐为例，告诉我们不懂推荐算法的产品，也能从产品侧出发， 设计出一款不错的推荐系统。 相信很多新手产品，看到算法二字，多是懵圈的。 什么排序算法、最短路径等都是相对传统的算法（注：传统是指科班出身的产品都会接触过）。但对于推荐算法，多数产品对着网上搜到的资源，都会无从下手。特别当某些推荐算法 和 “AI”扯上关系后，更是加大了理解的难度。 但，不了解推荐算法，就无法做推荐系
阅读更多...
无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

墙体画线地图切分方案 针对问题：墙体两侧特征混淆误匹配，导致建图和定位偏差，表现为过门跳变、外月台走歪等 ·解决思路：预期的根治方案IGICP需要较长时间完成上线，先使用切分地图的工程化方案，即墙体两侧切分为不同地图，在某一侧只使用该侧地图进行定位 方案思路 切分原理：切分地图基于关键帧位置，而非点云。 理论基础：光照是直线的，一帧点云必定只能照射到墙的一侧，无法同时照到两侧实践考虑：关
阅读更多...
hdu1043（八数码问题，广搜 + hash（实现状态压缩） ）

hdu1043（八数码问题，广搜 + hash（实现状态压缩） ）

利用康拓展开将一个排列映射成一个自然数，然后就变成了普通的广搜题。 #include<iostream>#include<algorithm>#include<string>#include<stack>#include<queue>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#inclu
阅读更多...
康拓展开（hash算法中会用到）

康拓展开（hash算法中会用到）

康拓展开是一个全排列到一个自然数的双射（也就是某个全排列与某个自然数一一对应） 公式： X=a[n]*(n-1)!+a[n-1]*(n-2)!+...+a[i]*(i-1)!+...+a[1]*0! 其中,a[i]为整数,并且0<=a[i]<i,1<=i<=n。（a[i]在不同应用中的含义不同）； 典型应用： 计算当前排列在所有由小到大全排列中的顺序，也就是说求当前排列是第
阅读更多...
hdu1565（状态压缩）

hdu1565（状态压缩）

本人第一道ac的状态压缩dp，这题的数据非常水，很容易过 题意：在n*n的矩阵中选数字使得不存在任意两个数字相邻，求最大值 解题思路： 一、因为在1<<20中有很多状态是无效的，所以第一步是选择有效状态，存到cnt[]数组中 二、dp[i][j]表示到第i行的状态cnt[j]所能得到的最大值，状态转移方程dp[i][j] = max(dp[i][j],dp[i-1][k]) ,其中k满足c
阅读更多...
csu 1446 Problem J Modified LCS （扩展欧几里得算法的简单应用）

csu 1446 Problem J Modified LCS （扩展欧几里得算法的简单应用）

这是一道扩展欧几里得算法的简单应用题，这题是在湖南多校训练赛中队友ac的一道题，在比赛之后请教了队友，然后自己把它a掉 这也是自己独自做扩展欧几里得算法的题目 题意：把题意转变下就变成了：求d1*x - d2*y = f2 - f1的解,很明显用exgcd来解 下面介绍一下exgcd的一些知识点：求ax + by = c的解 一、首先求ax + by = gcd(a,b)的解 这个
阅读更多...
综合安防管理平台LntonAIServer视频监控汇聚抖动检测算法优势

综合安防管理平台LntonAIServer视频监控汇聚抖动检测算法优势

LntonAIServer视频质量诊断功能中的抖动检测是一个专门针对视频稳定性进行分析的功能。抖动通常是指视频帧之间的不必要运动，这种运动可能是由于摄像机的移动、传输中的错误或编解码问题导致的。抖动检测对于确保视频内容的平滑性和观看体验至关重要。 优势 1. 提高图像质量 - 清晰度提升：减少抖动，提高图像的清晰度和细节表现力，使得监控画面更加真实可信。 - 细节增强：在低光条件下，抖
阅读更多...
【数据结构】——原来排序算法搞懂这些就行，轻松拿捏

【数据结构】——原来排序算法搞懂这些就行，轻松拿捏

前言：快速排序的实现最重要的是找基准值，下面让我们来了解如何实现找基准值 基准值的注释：在快排的过程中，每一次我们要取一个元素作为枢纽值，以这个数字来将序列划分为两部分。 在此我们采用三数取中法，也就是取左端、中间、右端三个数，然后进行排序，将中间数作为枢纽值。 快速排序实现主框架： //快速排序 void QuickSort(int* arr, int left, int rig
阅读更多...
poj 3974 and hdu 3068 最长回文串的O（n）解法(Manacher算法)

poj 3974 and hdu 3068 最长回文串的O（n）解法(Manacher算法)

求一段字符串中的最长回文串。 因为数据量比较大，用原来的O（n^2）会爆。 小白上的O(n^2)解法代码：TLE啦~ #include<stdio.h>#include<string.h>const int Maxn = 1000000;char s[Maxn];int main(){char e[] = {"END"};while(scanf("%s", s) != EO
阅读更多...
秋招最新大模型算法面试，熬夜都要肝完它

秋招最新大模型算法面试，熬夜都要肝完它

💥大家在面试大模型LLM这个板块的时候，不知道面试完会不会复盘、总结，做笔记的习惯，这份大模型算法岗面试八股笔记也帮助不少人拿到过offer ✨对于面试大模型算法工程师会有一定的帮助，都附有完整答案，熬夜也要看完，祝大家一臂之力 这份《大模型算法工程师面试题》已经上传CSDN，还有完整版的大模型 AI 学习资料，朋友们如果需要可以微信扫描下方CSDN官方认证二维码免费领取【保证100%免费
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2