-
2022-03-20 17:13:22
多传感器融合定位-章节索引
前言:
本博客为深蓝学院多传感器融合定位的课程作业笔记,为了方便个人检索,故将笔记记录到网上,同时也希望能给大家一些启发。在讲师任乾老师和其他学员的帮助下,总算完成十章的作业。
回顾整个历程,先后修了三次的课程,也十分感慨。刚录取研究生(2020年)的时候,导师给了我一个课题,做3D巡检小车的定位建图,在本科一直参与robotics的比赛,接触了不少的2D agv项目,因此也欣然接受。开始了资料的检索,开始了解到点云,先修了深蓝学院黎嘉信老师的三维点云处理,对点云有了初级的了解。
第一阶段 2020.10:后来接触到了任乾老师的第一期多传感器融合定位课程(2020.10),开始发现自己的知识十分薄弱,对非线性优化、惯导等概念一无所知,甚至不知道什么是外参、内参,多线激光雷达也从没接触过,c++也十分的差。第一期课程的难度对于我这样的菜鸟来说确实很难,我一边上课一遍不断补习知识,去翻阅高博的slam14讲、机器人状态估计。最后因为临近期末考试和个人基础知识太薄弱,上到第四章(惯性导航分析)就没跟上。之后我重新把高翔博士的14讲重看了一遍,也在实验室自己搭建造了一辆3D的slam移动小平台,通过自己的选型,设计框架,开始慢慢了解在自动驾驶领域传感器的种类和属性,也开始对传感器有了更深的理解。
第二阶段 2021.8 : 在外出实习期间,一直没有放弃对slam的学习,进行了第四期课程的学习,这一次的学习更加细致,进行了1-8章较为详细的作业,对比之前,这一次开始看懂代码,理解其中算法的含义,理解ceres g2o 等非线性优化的使用,很好地了解了kalman的融合,在此之前kalman一直停留在本科期间机器人比赛中自瞄跟踪算法,但也只是调库,这一次有了更深的理解,也不会畏惧公式,开始耐心的推导。因为临近开题原因,故这一次学习到第八章kalman容和定位。
第三阶段 2022.3:在第二阶段的课程学习之后,期间认真阅读了课程的框架代码,也开始自己搭建平台,制作数据集,将课程的框架部署到自己的搭建的平台上。如:3D-SLAM自搭平台 主动阿克曼 + RS16 + LPMS_IMU LEGO_LOAM 建图 B站视频。开始了第三次的多传感器容和定位的学习,这一次,补充了第九和第十章,预积分和图优化的部分,优化和预积分的方法是未来的一个趋势,可以说是课程的核心内容。通过不断的提问,反复看视频,交叉观看VIO的课程,开始有了自己的理解。
一路以来的学习,有赖于任乾老师和各位助教学员的解答,非常感谢各位。
列宁曾说过“人的认识不是直线,而是无限地近似于一串圆圈、近似于螺旋式的曲线”,事物的自身发展,经过肯定、否定和新的肯定。同样人的认知也需要经过“否定之否定”的过程,在不断地否定中自我成长。
课程笔记检索
1-10章的课程检索 github 链接 (如果对大家有帮助,烦请大家star我一下~)
多传感器融合定位 第五章 惯性导航原理及误差分析-不需要转台的IMU内参标定
自搭实验平台
jackal + 镭神32
搭建实验室3d slam 移动小车 2.1-镭神32线激光雷达使用调试
搭建实验室3d slam 移动小车 2.2镭神32线激光雷达修改主从机IP
搭建实验室3d slam 移动小车 2.3镭神32线激光雷达ROS-RVIZ上方向确定
搭建实验室3d slam 移动小车 3.1jackal移动小车平台调试
搭建实验室3d slam 移动小车 3.2jackal移动平台axis-ptz魚眼摄像头调试
搭建实验室3d slam 移动小车 3.3jackal移动平台 组合导航POMS-GI201C、镭神32线激光雷达 卫星授时
搭建实验室3d slam 移动小车 4.1jackal小车+镭神32线激光雷达lego-loam建图
使用Mapviz、中科图新 进行机器人GPS轨迹卫星地图绘制
使用Mapviz,进行机器人GPS轨迹卫星地图绘制(2)-调用天地图API,快速加载刷新地图
主动阿克曼 + RS16 + LPMS_IMU
3D-SLAM自搭平台 主动阿克曼 + RS16 + LPMS_IMU LEGO_LOAM 建图
第一阶段深蓝多传感课程检索
第一期课程学习,也进行了一些笔记记录,但不全,望见谅
多传感器融合定位(1-3D激光里程计)3-前端里程计LOAM系列
多传感器融合定位(1-3D激光里程计)4-实现调用pcl-icp-1
多传感器融合定位(1-3D激光里程计)5-实现调用pcl-icp-2 通过 config.yaml 实现接口的多态性
多传感器融合定位(1-3D激光里程计)6-实现调用pcl-icp-3 evo里程计精度评价
多传感器融合定位(2-点云地图构建及基于地图定位)1-回环检测及代码实现
多传感器融合定位(2-点云地图构建及基于地图定位)2-后端优化与点云地图构建
多传感器融合定位(3-点云地图构建及基于地图定位)3-实现ScanContext 回环检测
多传感器融合定位(4-点云地图构建及基于地图定位)4-通过GNSS 实现地图定位
多传感器融合定位(3-惯性导航原理及误差分析)2-IMU误差分析及处理:随机误差理论分析&allan方差分析及实现
多传感器融合定位(3-惯性导航原理及误差分析)3-内参模型与分立级、系统级、迭代优化等标定方法:器件内参标定
多传感器融合定位(3-惯性导航原理及误差分析)4-IMU温补:常见温补模型与基于多项式你和的温补方法:惯性器件误差分析
多传感器融合定位(3-惯性导航原理及误差分析)7-实现 分立级标定 accel加速度计内参公式验证
多传感器融合定位(3-惯性导航原理及误差分析)6-Allan方差 进行随机误差分析
多传感器融合定位(4-基于滤波的融合方法)kitti数据集 IMU频率改为100HZ
多传感器融合定位(3-惯性导航原理及误差分析)8-惯性导航解算验证
多传感器融合定位(4-基于滤波的2融合方法)2-使用仿真数据进行imu-gnss eskf和时变系统下的可观测性分析
ROS自定义消息类型,编译无法生成 msg/srv 文件产生的头文件
edited by kaho 2022.3.20 祝愿疫情早日消退,花开满地
更多相关内容 -
Multi-Sensor Fusion (GNSS, IMU, Camera) 多源多传感器融合定位 GPS/INS组合导航 PPP/INS紧组合-C/C++开发
2021-05-26 23:26:15Multi-Sensor Fusion (GNSS, IMU, Camera) 多源多传感器融合定位 GPS/INS组合导航 PPP/INS紧组合 Multi-Sensor Fusion 版本号:release/3.0.0 初衷 学习组合导航和VIO相关内容. 希望和有兴趣的小伙伴一起交流... -
多传感器融合定位技术
2021-10-24 23:32:08多传感器融合定位技术1. 多传感器融合系统简介1.1 多传感器融合系统体系结构1.2 多传感器融合系统分层2. 多传感器融合定位系统原理3. 多传感器融合误差分析4. 多传感器融合算法4.1 数据融合算法概述4.2 卡尔曼滤波...多传感器融合定位技术
由于 GNSS 定位信息更新频率低,不能满足自动驾驶中实时性的要求,且定位信号会因隧道、建筑群等障碍物的遮挡面而中断。而 INS(Inertial Navigation System) 中配备高频传感器,一定时间内可以提供连续的较高精度的汽车位置、速度和航向信息,但其定位误差会随着系统运行时间积累而剧增。将 GNSS 与 INS 相结合,可以利用 GNSS 提供的不随时间增加的高精度定位来纠正 INS 的累积定位误差。同时,INS 可以解决 GNSS 特定场景易受影响的问题。通过结合这两种系统的优点,就能得到实时和精准的定位。如果再与地图匹配技术相结合,利用高精度地图提供的信息,可进一步提高定位精度。
要实现多个定位系统融合,提高定位精度,设计一种融合多个传感器数据的系统尤为重要。这里将从多传感器融合系统简介、系统原理、误差分析以及融合算法等方面对多传感器融合定位系统进行介绍。
1. 多传感器融合系统简介
多传感器数据融合是 20 世纪 80 年代出现的一门新兴学科,它是将不同传感器对某一目标或环境特征描述的信息融合成统一的特征表达信息及其处理的过程。在多传感器系统中,各种传感器提供的信息可能具有不同的特征,如模糊的与确定的、时变的与非时变的、实时的与非实时的等。多传感器数据融合实际上是模拟人脑综合处理复杂问题的过程,通过对各种传感器及其观测信息的合理支配与使用,将各种传感器在空间和时间上的互补与冗余信息,依据某种优化准则加以组合,产生对观测环境或对象的一致性解释和描述,实现多个传感器共同或联合操作,提高整个传感器系统的有效性。数据融合的目标是利用各种传感器的独立观测信息,对数据进行多级别、多方位和多层次的处理,产生新的有意义的信息,这种信息是最佳协同作用的结果,是任何单一传感器无法获得的。
自动驾驶汽车定位的主要模式有 DR、GNSS、GNSS/DR 组合定位模式。在系统精度要求不高的前提条件下可以单独使用这 3 种定位模式。为了进一步提高定位系统的精度,保障自动驾驶的安全,在上述 3 种定位模式中引入了地图匹配,可组合产生出新的 3 种定位模式:DR/MM、GNSS/MM、GNSS/DR/MM。多传感器融合定位系统可在 6 种模式中自动切换以提高整个系统的定位精度和可靠性。接下来对多传感器融合系统体系结构以及系统分层进行介绍。
1.1 多传感器融合系统体系结构
多传感器融合系统体系结构主要包括松耦合(Loosely Coupled)、紧耦合(Tightly Coupled)以及深耦合(Deep Coupled)等组合结构。
-
松耦合:在松耦合系统里,GNSS 给 INS 提供位置信息,二者硬件上相互独立且随时断开连接,分别输出定位信息与速度信息到融合滤波器,融合滤波器进行优化处理后将结果反馈给惯性导航系统对其修正后进行输出。GNSS/INS 松耦合系统原理图如下所示:
-
紧耦合:紧耦合系统是将由 GNSS 环码与载波跟踪环解算得到的伪距、伪距率与由惯性导航系统结合自身信息和卫星星历进行计算得到的伪距、伪距率做差,得到伪距与伪距率的测量残差,将其作为融合滤波器的输入观测量,得到惯性导航系统计算误差以及传感器偏差以完成对惯性导航系统的矫正并获得位置与速度的最优估计值。GNSS/INS 紧耦合系统原理图如下所示:
-
深耦合:深耦合系统相对于紧耦合系统,增加了 INS 单元对 GNSS 接收机的辅助。利用 INS 单元结合星历信息可以对伪距和载波的多普勒频移进行估计,利用估计结果辅助接收机的捕获与跟踪环路,可以有效地提高 GNSS 接收机跟踪环路的动态性与灵敏度。
1.2 多传感器融合系统分层
如下图所示,按照信息处理的流程,可将多传感器融合系统划分为数据层融合、特征层融合和决策层融合。
- 数据层融合:数据层融合也称像素级融合,首先将传感器的观测数据融合,然后从融合的数据中提取特征向量,并进行判断识别。数据层融合需要传感器是同质的(传感器观测的是同一物理量),如果多个传感器是异质的(传感器观测的不是同一个物理量),那么数据只能在特征层或决策层进行融合。数据层融合不存在数据丢失的问题,得到的结果也是最准确的,但计算量最大,且对系统通信带宽的要求很高。
- 特征层融合:特征层融合属于中间层次,先从每种传感器提供的观测数据中提取有代表性的特征,这些特征融合成单一的特征向量,然后运用模式识别的方法进行处理。这种方法的计算量及对通信带宽的要求相对较低,但部分数据的舍弃使其准确性有所下降。
- 决策层融合:决策层融合指在每个传感器对目标做出识别后,再将多个传感器的识别结果进行融合,属于高层次的融合。决策层融合由于对可能包含误差的传感器数据进行再处理,产生的结果相对而言最不准确,但其计算量及对通信带宽的要求最低。
2. 多传感器融合定位系统原理
多传感器数据融合定位系统的输入主要来自 GNSS-RTK、惯性导航系统和地图匹配定位系统。融合定位系统对其数据进行预处理、数据配准和数据融合等处理后,可输出汽车自身的速度、位置和姿态信息。下图为多传感器数据融合定位流程示意图:
数据预处理可以考虑为传感器初始化及校准,传感器初始化相对于系统坐标独立地校准每一个传感器。一旦完成了传感器初始化,就可以利用各传感器对共同目标采集得到的数据进行数据配准。所谓数据配准,就是把来自一个或多个传感器的观测或点迹数据与已知或已经确认的事件归并到一起,保证每个事件集合所包含的观测与点迹数据来自同一个实体的概率较大(直白点说就是匹配嘛)。具体的说,就是要把每批目标的观测或点迹数据与事件集合中各自的数据配对。在传感器配准过程中,收集足够的数据点来计算系统偏差,计算得到的系统偏差用来调整随后得到的传感器数据。其次,传感器的配准主要包括时间配准和空间配准两个方面。-
时间配准:时间配准,就是将关于同一目标的各传感器不同步的量测信息同步到同一时刻。由于各传感器对目标的量测是相互独立进行的,且采样周期(如观星测量单元和激光雷达大采样周期)往往不同,所以它们向数据处理中心报告的时刻往往也是不同的。另外,由于通信网络的不同延迟,各传感器和融合处理中心之间传送信息所需的时间也各不相同,因此,各传感器上的数据的发送时间有可能存在时间差,所以融合处理前需将不同步的信息配准到相同的时刻。
时间配准的一般做法是将各传感器数据统一到扫描周期较长的一个传感器数据上,目前,常用的方法包括最小二乘法(Least Squares, LS)和内插外推法。这两种方法都对目标的运动模型做了匀速运动的假设,对于做变加速运动的目标,配准效果往往很差。下面仅对基于最小二乘法的时间配准法做简单介绍:
假设有两类传感器,分别表示为传感器1和传感器2,其采样周期分别为 τ \tau τ 和 T,且两者之比为 τ \tau τ: -
空间配准:空间配准,就是借助多传感器对空间共同目标的量测结果对传感器的偏差进行估计和补偿。对于同一系统内采用不同坐标系的各传感器的量测,定位时必须将它们转换成同一坐标系中的数据,对于多个不同子系统,各子系统采用的坐标系是不同的,所以在融合处理各子系统间信息前,也需要将它们转换到同一量测坐标系中,而处理后还需将结果转换成各子系统坐标系的数据,再传送给各子系统。
如下图左所示,由于传感器1(传感器2)存在斜距和方位角偏差 Δ r 1 \Delta{r_1} Δr1、 Δ θ 1 \Delta{\theta_1} Δθ1( Δ r 2 \Delta{r_2} Δr2、 Δ θ 2 \Delta{\theta_2} Δθ2),导致在系统平面上出现两个目标,而实际上只有一个真实目标,所以需要进行空间配准,配准过程如下右图所示:
上图中, r 1 r_1 r1、 θ 1 \theta_1 θ1 分别表示传感器 1 的斜距和方位角量测值; r 2 r_2 r2、 θ 2 \theta_2 θ2 分别表示传感器 2 的斜距和方位角量测值; ( x s 1 , y s 1 ) (x_{s1},y_{s1}) (xs1,ys1) 表示传感器 1 在导航坐标平面上的位置; ( x s 2 , y s 2 ) (x_{s2},y_{s2}) (xs2,ys2) 表示传感器 2 在导航坐标平面上的位置; ( x 1 , y 1 ) (x_1,y_1) (x1,y1) 表示传感器 1 在导航坐标系上的测量值; ( x 2 , y 2 ) (x_2,y_2) (x2,y2) 表示传感器 2 在导航坐标系上的测量值。从上图右可以推导出如下的基本方程:
{ x 1 = x s 1 + r 1 sin θ 1 y 1 = y s 1 + r 1 cos θ 1 x 2 = x s 2 + r 2 sin θ 2 y 2 = y s 2 + r 2 cos θ 2 \left\{\begin{array}{l} x_{1}=x_{\mathrm{s} 1}+r_{1} \sin \theta_{1} \\ y_{1}=y_{\mathrm{s} 1}+r_{1} \cos \theta_{1} \\ x_{2}=x_{\mathrm{s} 2}+r_{2} \sin \theta_{2} \\ y_{2}=y_{\mathrm{s} 2}+r_{2} \cos \theta_{2} \end{array}\right. ⎩⎪⎪⎨⎪⎪⎧x1=xs1+r1sinθ1y1=ys1+r1cosθ1x2=xs2+r2sinθ2y2=ys2+r2cosθ2如果忽略噪声,则有:
{ r 1 = r 1 ′ + Δ r 1 θ 1 = θ 1 ′ + Δ θ 1 r 2 = r 2 ′ + Δ r 2 θ 2 = θ 2 ′ + Δ θ 2 \left\{\begin{array}{l} r_{1}=r_{1}^{\prime}+\Delta r_{1} \\ \theta_{1}=\theta_{1}^{\prime}+\Delta \theta_{1} \\ r_{2}=r_{2}^{\prime}+\Delta r_{2} \\ \theta_{2}=\theta_{2}^{\prime}+\Delta \theta_{2} \end{array}\right. ⎩⎪⎪⎨⎪⎪⎧r1=r1′+Δr1θ1=θ1′+Δθ1r2=r2′+Δr2θ2=θ2′+Δθ2其中, r 1 ′ r'_1 r1′、 θ 1 ′ \theta'_1 θ1′ 分别表示目标相对于传感器 1 的真实斜距和方位角; r 2 ′ r'_2 r2′、 θ 2 ′ \theta'_2 θ2′ 分别表示目标相对于传感器 2 的真实斜距和方位角; Δ r 1 \Delta{r_1} Δr1、 Δ θ 1 \Delta{\theta_1} Δθ1 表示传感器 1 的斜距和方位角偏差; Δ r 2 \Delta{r_2} Δr2、 Δ θ 2 \Delta{\theta_2} Δθ2 表示传感器 2 的斜距和方位角偏差。将上面两式合并,并且将所得到的方程相对于 Δ r 1 \Delta{r_1} Δr1、 Δ θ 1 \Delta{\theta_1} Δθ1 和 Δ r 2 \Delta{r_2} Δr2、 Δ θ 2 \Delta{\theta_2} Δθ2 进行一阶泰勒级数展开,可得:
{ x 1 − x 2 ≈ sin θ 1 Δ r 1 − sin θ 2 Δ θ 2 + r 1 cos θ 1 Δ θ 1 − r 2 cos θ 2 Δ θ 2 y 1 − y 2 ≈ cos θ 1 Δ r 1 − cos θ 2 Δ r 2 − r 1 sin θ 1 Δ θ 1 + r 2 sin θ 2 Δ θ 2 \left\{\begin{array}{l} x_{1}-x_{2} \approx \sin \theta_{1} \Delta r_{1}-\sin \theta_{2} \Delta \theta_{2}+r_{1} \cos \theta_{1} \Delta \theta_{1}-r_{2} \cos \theta_{2} \Delta \theta_{2} \\ y_{1}-y_{2} \approx \cos \theta_{1} \Delta r_{1}-\cos \theta_{2} \Delta r_{2}-r_{1} \sin \theta_{1} \Delta \theta_{1}+r_{2} \sin \theta_{2} \Delta \theta_{2} \end{array}\right. {x1−x2≈sinθ1Δr1−sinθ2Δθ2+r1cosθ1Δθ1−r2cosθ2Δθ2y1−y2≈cosθ1Δr1−cosθ2Δr2−r1sinθ1Δθ1+r2sinθ2Δθ2该式对与目标运动航迹无关的偏差估计方法提供了基础。(这里是为啥)常用的与目标运动航迹无关的偏差估计方法主要有实时质量控制法(Real Time Quality Control,RTQC)、最小二乘法、极大似然法(Maximum Likelihood,ML)和基于卡尔曼滤波器的空间配准算法等。在给出的几种算法中,实时质量控制法和最小二乘法完全忽略了传感器量测噪声的影响,认为公共坐标系中的误差来源于传感器配准误差(传感器偏差,在下一节会讲)。广义最小二乘法(Generalized Least Square,GLS)和基于卡尔曼滤波器的方法虽然考虑了传感器量测噪声的影响,但只有在量测噪声相对小时,才会产生好的性能。为了克服前两种局限性,提出了精确极大似然(Exact Maximum Likelihood,EML)空间配准算法。
尽管前面已经介绍了多种不同的配准算法,但它们都是基于立体投影在一个二维区域平面上实现的。更准确地说,首先通过立体投影技术把传感器量测投影到与地球正切的局部传感器坐标上,然后变换到区域平面,并利用不同传感器量测之间的差异来估计传感器偏差。虽然立体投影能够减轻单个配准算法的计算复杂度,但这一方法还有一些缺点。首先,立体投影给局部传感器和区域平面的量测都引入了误差。尽管更高阶的近似可以将变换的精度保证到几米,但由于地球本身是一个椭圆形球而不是一个圆柱,因此地球非正圆球体造成的误差仍然存在。其次,立体投影扭曲了数据,值得注意的是立体投影的保角性只能保留方位角,而不能保留斜距。由此可以断定系统偏差将会依赖于量测,而不再是时不变的。这样,在区域平面上的二维配准模型就不能正确地表示实际的传感器模型。这时,一种直接在三维空间中对传感器偏差进行估计的基于地心坐标系的空间配准(Earth Centered Earth Fixed,ECEF)算法被提出以解决上述问题。
3. 多传感器融合误差分析
在多传感器融合系统中,来自多个传感器的数据通常要变换到相同的时空参照系中。但由于存在量测误差直接进行变换很难保证精度来发挥多传感器的优越性,因此在对多传感器数据进行处理时需要寻求一些传感器的配准算法,但配准误差也随之而来。
多传感器配准的主要误差来源有:
- 传感器的误差,也就是传感器本身因制造误差带来的偏差;
- 各传感器参考系中量测的方位角、高低角和斜距偏差。通常是因量测系统解算传感器数据时造成的误差;
- 相对于公共坐标系的传感器的位置误差和计时误差。位置误差通常由传感器导航系统的偏差引起,而计时误差由传感器的时钟偏差所致;
- 各传感器采用的定位算法不同,从而引起单系统内局部定位误差;
- 各传感器本身的位置不确定为融合处理而进行坐标转换时产生偏差;
- 坐标转换的精度不够,为了减少系统的计算负担而在投影变换时采用了一些近似方法(如将地球视为标准的球体等)所导致的误差。
由于以上原因,同一个目标由不同传感器定位产生的航迹基友一定的偏差。这种偏差不同于单传感器定位时对目标的随机量测误差,它是一种固定的偏差(至少在较长时间段内不会改变)。对于单传感器来说,目标航迹的固定偏差对各个目标来说都是一样的,只是产生一个固定的偏移,并不会影响整个系统的定位性能。而对于多传感器系统来说,本来是同一个目标的航迹,却由于互相偏差较大而被认为是不同的目标,从而给航迹关联和融合带来了模糊和困难,使融合处理得到的系统航迹的定位精度下降,丧失了多传感器处理本身应有的优点。
4. 多传感器融合算法
实现多传感器融合定位的算法有很多种,下面首先简要介绍一下各种数据融合算法及其优缺点。其中,卡尔曼滤波算法作为一种经典算法,由于其实时性强、融合精度高等优点,在自动驾驶领域中被广泛使用,下面将重点介绍卡尔曼滤波技术。
4.1 数据融合算法概述
目前,融合算法可概括为随机类和人工智能类。随机类多传感器数据融合算法主要有综合估计法、贝叶斯估计法、D-S证据推理、最大似然估计、贝叶斯估计、最优估计、卡尔曼滤波算法及鲁棒估计等。人工智能类多传感器数据融合算法主要有模糊逻辑法、神经网络算法及鲁棒估计等。人工智能类多传感器数据融合算法主要有模糊逻辑法、神经网络算法以及专家系统等。下面简介上述算法:
用某种适当的模型来描述一个实际的物理系统,对分析、研究该物理系统是非常重要的。在导航、信号处理、通信、雷达、声呐等许多实际工程应用中,经常采用动态空间模型来描述其中的许多问题。动态空间模型是一个很重要的统计分析工具,如卡尔曼滤波器采用的高斯-马尔可夫线性模型就是一个很好的例子,它用状态方程(动力学方程)来描述状态随时间演变的过程,而用观测方程来描述与状态有关的噪声变量。同样的,只要将高斯-马尔可夫线性模型写成一般的数学映射,就可以用这两个方程来描述更一般的动态系统了:
状 态 方 程 : X k = f ( X k − 1 , W k ) 观 测 方 程 : L k = h ( X k , V k ) 状态方程: X_{k}=f\left(X_{k-1}, W_{k}\right) \\ 观测方程: L_{k}=h\left(X_{k}, V_{k}\right) 状态方程:Xk=f(Xk−1,Wk)观测方程:Lk=h(Xk,Vk) 上式被称为动态空间模型。其中, X k ∈ R k x X_{k} \in \mathbf{R}^{k_{x}} Xk∈Rkx 为系统在 k k k 时刻的状态, L k ∈ R k x L_{k} \in \mathbf{R}^{k_{x}} Lk∈Rkx 为系统状态 X k X_{k} Xk 的观测值; W k W_{k} Wk 、 V k V_{k} Vk 分别为过程和观测噪声。
- 综合平均法:该算法是把来自多个传感器的众多数据进行综合平均。它适用于用同样的传感器检测同一个目标的情况。如果对一个检测目标进行了 k k k 次检测,其平均值 S ˉ = ∑ i = 1 k W i S i / ∑ i = 1 k W i , W i \bar{S}=\sum_{i=1}^{k} W_{i} S_{i} / \sum_{i=1}^{k} W_{i}, W_{i} Sˉ=∑i=1kWiSi/∑i=1kWi,Wi 为分配给第 i i i 次检测的权值。
- 贝叶斯估计法:贝叶斯估计理论是较经典的统计估计理论,具有更大的优势,逐渐 成为科学界推理的一个重要工具,提供了一种与传统算法不同的概率分布形式的估计。贝 叶斯推理技术主要用来进行决策层融合。贝叶斯估计法通过先验信息和样本信息合成为后 验分布,对检测目标做出推断。因此贝叶斯估计是一个不断预测和更新的过程。这样就包 括了观测值和先验知识在内的所有可以利用的信息,得到的估计误差自然较小。
- D-S 证据推理:D-S 证据推理是目前数据融合技术中比较常用的一种算法,该算法通常用来对检测目标的大小、位置以及存在与否进行推断,采用概率区间和不确定区间决定多证据下假设的似然函数来进行推理。提取的特征参数构成了该理论中的证据,利用这些证据构造相应的基本概率分布函数,对于所有的命题赋予一个信任度。基本概率分布函数及其相应的分辨框合称为一个证据体。因此,每个传感器就相当于一个证据体。而多个 传感器数据融合,实质上就是在同一分辨框下,利用 Dempster 合并规则将各个证据体合并成一个新的证据体,产生新证据体的过程就是 D-S 证据推理数据融合。
- 卡尔曼滤波算法:卡尔曼滤波在控制领域得到广泛应用以后,也逐渐成为多传感器数据融合系统的主要技术手段之一。联合卡尔曼滤波器的设计思想是先分散处理、再全局融合,即在诸多非相似子系统中选择一个信息全面、输出速率高、可靠性绝对保证的子系统作为公共参考系统,与其他子系统两两结合,形成若干子滤波器。各子滤波器并行运行,获得建立在子滤波器局部观测基础上的局部最优估计,这些局部最优估计在主滤波器内按融合算法合成,从而获得建立在所有观测基础上的全局估计。
- 模糊逻辑法:针对数据融合中所检测的目标特征具有某种模糊性的现象,利用模 糊逻辑算法来对检测目标进行识别和分类。建立标准检测目标和待识别检测目标的模糊子 集是此算法的研究基础。
- 神经网络算法:神经网络是一种试图仿效生物神经系统处理信息方式的新型计算模型。一个神经网络由多层处理单元或节点组成,可以用各种方法互联。在指挥和控制多传感器数据融合的系统中,神经网络的输入可能是与一个目标有关的测量参数集,输出可能 是目标身份,也可能是推荐的响应或行动。基于神经网络的融合优于传统的聚类算法,尤其 是当输人数据中带有噪声和数据不完整时。然而,要使神经网络算法在实际的融合系统中得到应用,无论在网络结构设计或是算法规则方面,还有许多基砩工作要做,如网络模型、网络的层次和每层的节点数、网络学习策略、神经网络算法与传统分类算法的关系和综合应用等。
- 专家系统:专家系统是一组计算机程序,它获取专家们在某个特定领域内的知识,然后根据专家的知识或经验导出一组规则,由计算机做出本应由专家做出的结论。目前,专家系统已在军用和民用领域得到了广泛应用。
此外,其他数据融合算法还有品质因数、模板算法、聚合分析、统计决策理论等。各种融合算法的特点比较如下表所示:
融合算法 运行环境 信息类型 信息表示 不确定性 融合技术 适用范围 综合平均法 动态 冗余 原始读数值 —— 加权平均 低层融合 贝叶斯估计法 静态 冗余 概率分布 高斯噪声 贝叶斯估计 高层融合 D-S 证据推理 静态 冗余互补 命题 —— 逻辑推理 高层融合 卡尔曼滤波 动态 冗余 概率分布 高斯噪声 系统模型滤波 低层融合 模糊逻辑法 静态 冗余互补 命题 隶属度 逻辑推理 高层融合 神经网络算法 动、静态 冗余互补 神经元输入 学习误差 神经元网络 低/高层融合 专家系统 静态 冗余互补 命题 置信因子 逻辑推理 高层融合 4.2 卡尔曼滤波算法
4.2.1 卡尔曼滤波
4.2.2 扩展卡尔曼滤波
4.2.3 无迹卡尔曼滤波
4.2.4 联邦卡尔曼滤波
-
-
SensorFusionWork:多传感器融合课程作业代码
2021-03-03 11:00:22基于激光的多传感器融合定位与建图 深蓝学院学习环境。 概述 本Repo为基于ROS melodic @ Ubuntu 18.04的学习环境。 为了节约安装配置的时间: 部分依赖项由于网络原因难以取得 新依赖的约会可能会导致与本地依赖的... -
学习笔记20--定位系统之多传感器融合定位技术
2021-11-21 18:22:21本系列博客包括6个专栏,分别为:《自动驾驶技术概览》、《自动驾驶汽车平台技术基础》、《自动驾驶汽车定位技术》、《自动驾驶汽车环境感知...3.定位系统之多传感器融合定位技术 3.1 多传感器融合介绍 GNSS:GNSS系统本系列博客包括6个专栏,分别为:《自动驾驶技术概览》、《自动驾驶汽车平台技术基础》、《自动驾驶汽车定位技术》、《自动驾驶汽车环境感知》、《自动驾驶汽车决策与控制》、《自动驾驶系统设计及应用》,笔者不是自动驾驶领域的专家,只是一个在探索自动驾驶路上的小白,此系列丛书尚未阅读完,也是边阅读边总结边思考,欢迎各位小伙伴,各位大牛们在评论区给出建议,帮笔者这个小白挑出错误,谢谢!
此专栏是关于《自动驾驶技术概览》书籍的笔记。
3.定位系统之多传感器融合定位技术
3.1 多传感器融合介绍
- GNSS:GNSS系统应用于无人车辆时,更新频率过低的缺点突出,10Hz左右的更新频率不能满足无人驾驶系统对实时定位的要求;
- 由于误差积累,INS惯性导航系统的准确度随时间和行驶距离的增加而降低,故也无法完整保证定位的准确性,但INS惯性导航系统更新频率高,达200Hz以上,满足自动驾驶系统需求;
- 通过适当的方法将GNSS和INS的数据进行融合,可以为车辆定位提供准确、实时的位置信息;
- 多传感器信息融合是将不同传感器对某一目标或环境特征描述的信息,综合成统一的特征表达信息及其处理的过程;
- 多传感器信息融合,协调利用多个传感器资源实现多传感器信息融合,通过对各种传感器及其观测信息的合理支配及使用,将各种传感器在空间和时间上的互补与冗余信息依据某种优化准则加以组合,产生对观测环境或对象的一致性解释和描述;
- 多传感器信息融合的目标:利用各种传感器分离观测信息,对数据进行多级别、多方位、多层次的处理,产生新的有意义的信息;
3.2 多传感器融合原理
多传感器融合的数据主要包括:GNSS-RTK、惯性导航系统和特征匹配自定位系统的输入数据;对这些数据进行预处理、数据配准、数据融合等处理后,输出车辆自身的速度、位置、姿态信息;
- 数据预处理。数据预处理可以考虑为传感器初始化及校准,传感器初始化即相对于系统坐标独立地校准每一个传感器;一旦完成传感器初始化,就可以利用共同的目标开始相对的数据配准过程;
- 数据配准。把来自一个或多个传感器的观测或点迹与已知或已经确认的事件归并到一起,保证每个事件集合所包含的观测来自同一个实体的概率较大;
- 传感器的配准。指多传感器数据"无误差"转换时所需要的处理过程,一般包括:时间配准和空间配准。
- 时间配准。
将关于同一目标的各传感器不同步的测量信息同步到同一时刻; - 空间配准。
借助于多传感器对空间共同目标的测量,对传感器的偏差进行估计和补偿;对于同一系统内采用不同坐标系的各传感器的测量,定位时必须将它们转换成同一坐标系中的数据;对于多个不同子系统,各子系统采用的坐标系是不同的,在融合处理各子系统的信息前,需要将它们转换到同一测量坐标系中,处理后需要将结果转换成各子系统坐标系的数据后,再传送到各个子系统。
- 时间配准。
- 数据融合。
融合算法分为:随机类和人工智能类;
随机类多传感器数据融合方法主要有:贝叶斯推理、D-S证据理论,及包括最大似然估计、综合平均法、贝叶斯估计、D-S法、最优估计、卡尔曼滤波、鲁棒估计等在内的估计理论;
人工智能类多传感器数据融合方法主要有:基于神经网络的多传感器数据融合、基于模糊聚类的数据融合及专家系统等;
经典融合算法:
- 综合平均法。
把来自多个传感器的众多数据进行综合平均;适用于用同样的传感器检测同一个目标的情况;如果对一个检测目标进行了 k k k次检测,平均值为: S ‾ = ∑ i = 1 k W i S i / ∑ i = 1 k W i \overline{S}=\sum_{i=1}^{k}W_iS_i/\sum_{i=1}^{k}W_i S=i=1∑kWiSi/i=1∑kWi
其 中 W i 为 分 配 给 第 i 次 检 测 的 权 值 其中W_i为分配给第i次检测的权值 其中Wi为分配给第i次检测的权值; - 贝叶斯估计法。
主要用来进行决策层融合,通过先验信息和样本信息合成为后验分布,对检测目标做出推断; - D-S法。
D-S法通常用来对检测目标的大小、位置及存在与否进行推断;采用概率区间和不确定区间决定多证据下假设的似然函数来进行推理;提取的特征参数构成了该理论中的证据,利用这些证据构成相应的基本概率分布函数,对于所有的命题赋予一个信任度;基本概率分布函数及相应的分辨框合称为一个证据体;故每个传感器相当于一个证据体,多个传感器信息融合,实质是在同一分辨框下,利用Dempster合并规则将各个证据体合并成一个新的证据体;产生新证据体的过程就是D-S法信息融合; - 卡尔曼滤波方法。
卡尔曼滤波器可以从一组有限的、包含噪声的、通过对物体位置的观察序列预测出物体的位置坐标及速度;卡尔曼滤波器运行时的两个阶段:预测阶段基于上一个时间点的位置信息去预测当前的位置信息;更新阶段通过对当前物体位置的观测去纠正位置预测,从而更新物体的位置信息;
通过卡尔曼滤波器,将惯性传感器与GNSS数据进行融合;首先,依据上一次的位置估算使用惯性传感器对当前位置进行实时预测;在得到新的GNSS数据前,只能通过积分惯性传感器的数据预测当前位置;惯性传感器的定位误差随着时间增加,所以当接收到新的GNSS信号时,由于GNSS数据比较精确,可以使用GNSS数据对当前的位置预测进行更新;通过不断地执行这两个步骤,系统可以取两者所长,对无人车进行准确的实时定位;
假设惯性传感器的频率是1kHz,GNSS频率是10Hz,那么每两次GNSS更新之间会使用100个惯性传感器数据点进行位置预测;
-
多传感器融合定位 第十章 基于优化的定位方法
2022-03-20 13:32:04本章是基于先验地图的图优化方法,先验地图的构建可参考多传感器融合定位 第九章 基于优化的建图方法 代码下载: 1.环境配置: 出现以下问题,是由于 make_unique 是c++ 14的新特性,需要在CMakelists.txt 中添加c++...第十章 基于优化的定位方法
本章是基于先验地图的图优化方法,先验地图的构建可参考多传感器融合定位 第九章 基于优化的建图方法
代码下载:
1.环境配置:
出现以下问题,是由于 make_unique 是c++ 14的新特性,需要在CMakelists.txt 中添加c++14 的编译指向。参考链接
lidar_localization/src/models/sliding_window/ceres_sliding_window.cpp:25:38: error: ‘make_unique’ is not a member of ‘std’ config_.loss_function_ptr = std::make_unique<ceres::CauchyLoss>(new ceres::CauchyLoss(1.0));
解决, 在CMakelists.txt 添加
set(CMAKE_CXX_STANDARD 14)
2.因子图优化代码补充
参考博客:
本章因子图优化,涉及的内容较多,建议先修参考一下内容:
VINS系列相关代码:滑动窗口部分
邱笑晨博士的《预积分总结与公式推导》(发表于泡泡机器人公众号)
机器人状态估计:理论推导部分
深蓝学院《从零手写VIO》课程:滑窗和边缘话理论讲解 (重点参考)
多传感器融合定位理论基础(十二):滑动窗口原理及应用 (重点参考)
以下代码补充的公式摘自GeYao助教的手推公式,图表摘自任乾老师课上的笔记,需要补全的代码主要有两大类:
代码参考:1. GEYAO助教的github 2.张嘉皓助教的github
1.实现预积分、地图匹配、边缘化、帧间匹配四种优化因子。
2.将上述四种约束因子,加入滑窗,进行优化
2.1 实现预积分、地图匹配、边缘化、帧间匹配四种优化因子
2.1.1 激光里程计因子
a.示意图
b.激光里程计因子 残差函数和雅克比矩阵 推导
注意的是,GeYao助教的公式推导和代码少了,位置残差对姿态的雅克比,以下为补充的推导。
c.代码补全
FILE: factor_prvag_relative_pose.hpp
// // TODO: get square root of information matrix: // Eigen::Matrix<double, 6, 6> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 6 ,6>>( I_ ).matrixL().transpose() ;
// // TODO: compute residual: // Eigen::Map<Eigen::Matrix<double, 6 ,1>> residual(residuals); // 残差 r_P r_R residual.block(INDEX_P, 0 , 3 , 1) = ori_i.inverse() * (pos_j - pos_i) - pos_ij ; residual.block(INDEX_R, 0 , 3 , 1) = (ori_i.inverse()*ori_j*ori_ij.inverse()).log( );
// // TODO: compute jacobians: 因为是二元边,所以需要求解两个jacobian // if ( jacobians ) { // compute shared intermediate results: const Eigen::Matrix3d R_i_inv = ori_i.inverse().matrix(); const Eigen::Matrix3d J_r_inv = JacobianRInv(residual.block(INDEX_R, 0 ,3 , 1)); // 右雅克比 const Eigen::Vector3d pos_ij = ori_i.inverse() * (pos_j - pos_i) ; if ( jacobians[0] ) { // 残差rL0(rp rq ) 对 T0(p q) M0(v ba bg) 的雅克比 // implement computing: Eigen::Map<Eigen::Matrix<double, 6 , 15, Eigen::RowMajor>> jacobian_i (jacobians[0] ); // col : rp_i[3] rq_i[3] row : p[3] q[3] v[3] ba[3] bg[3] jacobian_i.setZero(); jacobian_i.block<3, 3>(INDEX_P, INDEX_P) = -R_i_inv; jacobian_i.block<3, 3>(INDEX_R,INDEX_R) = -J_r_inv*(ori_ij*ori_j.inverse()*ori_i).matrix(); jacobian_i.block<3, 3>(INDEX_P,INDEX_R) = Sophus::SO3d::hat(pos_ij).matrix(); jacobian_i = sqrt_info * jacobian_i ; // 注意 sqrt_i 为对角的协方差矩阵对角线为观测的方差,可理解为传感器的测量误差,用于调整权重用 } if ( jacobians[1] ) { // 残差rL0(rp rq ) 对 T0(p q) M0(v ba bg) 的雅克比 // implement computing: Eigen::Map<Eigen::Matrix<double, 6 ,15, Eigen::RowMajor>> jacobian_j (jacobians[1]); jacobian_j.setZero(); jacobian_j.block<3, 3>(INDEX_P, INDEX_P) = R_i_inv; jacobian_j.block<3, 3>(INDEX_R,INDEX_R) = J_r_inv*ori_ij.matrix(); jacobian_j = sqrt_info * jacobian_j ; } }
// // TODO: correct residual by square root of information matrix: // residual = sqrt_info * residual;
2.1.2 地图匹配因子
a.示意图
b.地图匹配因子 残差函数和雅克比矩阵 推导
c.代码补全
// // TODO: get square root of information matrix: // // Cholesky 分解 : http://eigen.tuxfamily.org/dox/classEigen_1_1LLT.html Eigen::Matrix<double, 6, 6> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 6, 6>>( I_ ).matrixL().transpose();
// // TODO: compute residual: // Eigen::Map<Eigen::Matrix<double, 6 ,1>> residual(residuals); residual.block(INDEX_P, 0 , 3 , 1 ) = pos - pos_prior ; residual.block(INDEX_R,0 , 3 , 1 ) = (ori*ori_prior.inverse()).log();
// // TODO: compute jacobians: 一元边 // if ( jacobians ) { if ( jacobians[0] ) { // implement jacobian computing: Eigen::Map<Eigen::Matrix<double, 6, 15, Eigen::RowMajor>> jacobian_prior(jacobians[0] ); jacobian_prior.setZero(); jacobian_prior.block<3, 3>(INDEX_P, INDEX_P) = Eigen::Matrix3d::Identity(); jacobian_prior.block<3, 3>(INDEX_R, INDEX_R) = JacobianRInv( residual.block(INDEX_R, 0, 3, 1)) * ori_prior.matrix(); jacobian_prior = sqrt_info * jacobian_prior ; } }
// // TODO: correct residual by square root of information matrix: // residual = sqrt_info * residual;
2.1.3 IMU预积分因子
a.示意图
b.IMU预积分因子 残差函数和雅克比矩阵 推导
GeYao助教的推导为基于SO3形式的推导,代码也是SO3的形式。四元数的推导部分上一章已推导完成,可参考多传感器融合定位 第九章 基于优化的建图方法
c.代码补全
// // TODO: get square root of information matrix: // Cholesky 分解 : http://eigen.tuxfamily.org/dox/classEigen_1_1LLT.html Eigen::LLT<Eigen::Matrix<double,15,15>> LowerI(I_); // sqrt_info 为上三角阵 Eigen::Matrix<double,15,15> sqrt_info = LowerI.matrixL().transpose();
// // TODO: compute residual: // Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals); residual.block<3, 1>(INDEX_P, 0) = ori_i.inverse().matrix() * (pos_j - pos_i - (vel_i - 0.5 * g_ * T_) * T_) - alpha_ij ; residual.block<3, 1>(INDEX_R,0) = (Sophus::SO3d::exp(theta_ij).inverse()*ori_i.inverse()*ori_j).log(); residual.block<3, 1>(INDEX_V,0) = ori_i.inverse()* (vel_j - vel_i + g_ * T_) - beta_ij ; residual.block<3, 1>(INDEX_A,0) = b_a_j - b_a_i ; residual.block<3, 1>(INDEX_G,0) = b_g_j - b_g_i;
// // TODO: compute jacobians: imu预积分的残差 对状态量的雅克比,第九章已推导 // if ( jacobians ) { // compute shared intermediate results: const Eigen::Matrix3d R_i_inv = ori_i.inverse().matrix(); const Eigen::Matrix3d J_r_inv = JacobianRInv(residual.block(INDEX_R, 0 ,3 , 1)); // 右雅克比 if ( jacobians[0] ) { Eigen::Map<Eigen::Matrix<double, 15 , 15, Eigen::RowMajor>> jacobian_i (jacobians[0] ); jacobian_i.setZero(); // a. residual, position: jacobian_i.block<3, 3>(INDEX_P, INDEX_P) = -R_i_inv; jacobian_i.block<3, 3>(INDEX_P, INDEX_R) = Sophus::SO3d::hat( ori_i.inverse() * (pos_j - pos_i - (vel_i - 0.50 * g_ * T_) * T_) ); jacobian_i.block<3, 3>(INDEX_P, INDEX_V) = -T_*R_i_inv; jacobian_i.block<3, 3>(INDEX_P, INDEX_A) = -J_.block<3,3>(INDEX_P, INDEX_A); jacobian_i.block<3, 3>(INDEX_P, INDEX_G) = -J_.block<3,3>(INDEX_P, INDEX_G); // b. residual, orientation: jacobian_i.block<3, 3>(INDEX_R, INDEX_R) = -J_r_inv*(ori_j.inverse()*ori_i).matrix(); jacobian_i.block<3, 3>(INDEX_R, INDEX_G) = -J_r_inv*( Sophus::SO3d::exp(residual.block<3, 1>(INDEX_R, 0)) ).matrix().inverse()*J_.block<3,3>(INDEX_R, INDEX_G); // c. residual, velocity: jacobian_i.block<3, 3>(INDEX_V, INDEX_R) = Sophus::SO3d::hat( ori_i.inverse() * (vel_j - vel_i + g_ * T_) ); jacobian_i.block<3, 3>(INDEX_V, INDEX_V) = -R_i_inv; jacobian_i.block<3, 3>(INDEX_V, INDEX_A) = -J_.block<3,3>(INDEX_V, INDEX_A); jacobian_i.block<3, 3>(INDEX_V, INDEX_G) = -J_.block<3,3>(INDEX_V, INDEX_G); // d. residual, bias accel: jacobian_i.block<3, 3>(INDEX_A, INDEX_A) = -Eigen::Matrix3d::Identity(); // d. residual, bias accel: jacobian_i.block<3, 3>(INDEX_G, INDEX_G) = -Eigen::Matrix3d::Identity(); jacobian_i = sqrt_info * jacobian_i; } if ( jacobians[1] ) { Eigen::Map<Eigen::Matrix<double, 15, 15, Eigen::RowMajor>> jacobian_j(jacobians[1]); jacobian_j.setZero(); // a. residual, position: jacobian_j.block<3, 3>(INDEX_P, INDEX_P) = R_i_inv; // b. residual, orientation: jacobian_j.block<3, 3>(INDEX_R, INDEX_R) = J_r_inv; // c. residual, velocity: jacobian_j.block<3, 3>(INDEX_V, INDEX_V) = R_i_inv; // d. residual, bias accel: jacobian_j.block<3, 3>(INDEX_A, INDEX_A) = Eigen::Matrix3d::Identity(); // d. residual, bias accel: jacobian_j.block<3, 3>(INDEX_G, INDEX_G) = Eigen::Matrix3d::Identity(); jacobian_j = sqrt_info * jacobian_j; } }
// // TODO: correct residual by square root of information matrix: // residual = sqrt_info * residual;
2.1.4 边缘化先验因子
边缘化先验因子部分,算是滑窗算法的精华部分,可参考vins的做法
a.示意图
b.边缘先验因子 残差函数和雅克比矩阵 推导
c.代码补全
地图匹配H B阵
// // TODO: Update H: // // a. H_mm: H_.block<15, 15>(INDEX_M, INDEX_M) += J_m.transpose() * J_m ;
// // TODO: Update b: // // a. b_m: b_.block<15, 1>(INDEX_M , 0) += J_m.transpose() * residuals ; // 因子图叠加
点云匹配H B阵
// // TODO: Update H: // // a. H_mm: H_.block<15, 15>(INDEX_M, INDEX_M) += J_m.transpose() * J_m; // b. H_mr: H_.block<15, 15>(INDEX_M, INDEX_R) += J_m.transpose()* J_r; // c. H_rm: H_.block<15, 15>(INDEX_R, INDEX_M) += J_r.transpose() * J_m; // d. H_rr: H_.block<15, 15>(INDEX_R, INDEX_R) += J_r.transpose() * J_r;
// // TODO: Update b: // // a. b_m: b_.block<15, 1>(INDEX_M, 0) += J_m.transpose() * residuals ; // a. b_r: b_.block<15, 1>(INDEX_R, 0) += J_r.transpose() * residuals ;
IMU预积分H阵 B阵
// // TODO: Update H: // // a. H_mm: H_.block<15, 15>(INDEX_M, INDEX_M) += J_m.transpose() * J_m; // b. H_mr: H_.block<15, 15>(INDEX_M, INDEX_R) += J_m.transpose()* J_r; // c. H_rm: H_.block<15, 15>(INDEX_R, INDEX_M) += J_r.transpose() * J_m; // d. H_rr: H_.block<15, 15>(INDEX_R, INDEX_R) += J_r.transpose() * J_r;
// // TODO: Update b: // // a. b_m: b_.block<15, 1>(INDEX_M, 0) += J_m.transpose() * residuals ; // a. b_r: b_.block<15, 1>(INDEX_R, 0) += J_r.transpose() * residuals ;
边缘化操作 (核心代码)
此部分代码主要参考自vins部分
void Marginalize( const double *raw_param_r_0 ) { // TODO: implement marginalization logic // save x_m_0 Eigen::Map<const Eigen::Matrix<double, 15 , 1>> x_0(raw_param_r_0); x_0_ = x_0 ; // marginalize const Eigen::MatrixXd &H_mm = H_.block<15, 15>(INDEX_M, INDEX_M); const Eigen::MatrixXd &H_mr = H_.block<15, 15>(INDEX_M,INDEX_R); const Eigen::MatrixXd &H_rm = H_.block<15, 15>(INDEX_R,INDEX_M); const Eigen::MatrixXd &H_rr = H_.block<15, 15>(INDEX_R,INDEX_R); const Eigen::VectorXd &b_m = b_.block<15, 1>(INDEX_M, 0); const Eigen::VectorXd &b_r = b_.block<15, 1>(INDEX_R, 0); Eigen::MatrixXd H_mm_inv = H_mm.inverse(); Eigen::MatrixXd H_marginalized = H_rr - H_rm * H_mm_inv * H_mr ; Eigen::MatrixXd b_marginalized = b_r - H_rm * H_mm_inv * b_m ; // 线性化残差 和 雅克比 Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(H_marginalized); Eigen::VectorXd S = Eigen::VectorXd( (saes.eigenvalues().array() > 1.0e-5).select(saes.eigenvalues().array(), 0) ); Eigen::VectorXd S_inv = Eigen::VectorXd( (saes.eigenvalues().array() > 1.0e-5).select(saes.eigenvalues().array().inverse(), 0) ); Eigen::VectorXd S_sqrt = S.cwiseSqrt(); Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt(); // finally: J_ = S_sqrt.asDiagonal() * saes.eigenvectors().transpose(); // b0 e_ = S_inv_sqrt.asDiagonal() * saes.eigenvectors().transpose() * b_marginalized; // eo }
关于函数SelfAdjointEigenSolver的解析(1.摘自 张嘉皓助教)(2.摘自游博的知乎)
1.由于每次迭代需要使用奇异值分解,从边缘化后的信息矩阵中恢复出来雅克比矩阵 linearized_jacobians 和残差 linearized_residuals ,这两者会作为先验残差带入到下一轮的先验残差的雅克比和残差的计算当中去。这一部分和vins是差不多的,如果看不懂的可以去 vins 那边参考参考。其中推导部分相对比较简单,如图片所示
计算边缘因子传递的残差
// // TODO: compute residual: // Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals); residual = e_ + J_ * dx ; // e_prior
计算边缘因子传递的雅克比
// // TODO: compute jacobian: // if ( jacobians ) { if ( jacobians[0] ) { // implement computing: Eigen::Map<Eigen::Matrix<double, 15, 15, Eigen::RowMajor>> jacobian_marginalization(jacobians[0]); jacobian_marginalization.setZero(); jacobian_marginalization = J_ ; } }
2.2 将四种约束因子,加入滑窗,进行优化
2.2.1 更新状态量
FILE : param_prvag.hpp
// // TODO: evaluate performance penalty of applying exp-exp-log transform for each update // ori_plus = (Sophus::SO3d::exp(ori) * Sophus::SO3d::exp(d_ori)).log(); vel_plus = vel + d_vel; b_a_plus = b_a + d_b_a; b_g_plus = b_g + b_g_plus;
2.2.2 将因子添加到优化框架中
FILE : sliding_window.cpp
// // add node for new key frame pose: // // fix the pose of the first key frame for lidar only mapping: if ( sliding_window_ptr_->GetNumParamBlocks() == 0 ) { // TODO: add init key frame sliding_window_ptr_->AddPRVAGParam(current_key_frame_, true); } else { // TODO: add current key frame sliding_window_ptr_->AddPRVAGParam(current_key_frame_, false); }
// TODO: add constraint, GNSS position: sliding_window_ptr_->AddPRVAGMapMatchingPoseFactor( param_index_j, prior_pose, measurement_config_.noise.map_matching );
// a. lidar frontend: // // get relative pose measurement: Eigen::Matrix4d relative_pose = (last_key_frame_.pose.inverse() * current_key_frame_.pose).cast<double>(); // TODO: add constraint, lidar frontend / loop closure detection: sliding_window_ptr_->AddPRVAGRelativePoseFactor( param_index_i, param_index_j, relative_pose, measurement_config_.noise.lidar_odometry );
// // b. IMU pre-integration: // if ( measurement_config_.source.imu_pre_integration ) { // TODO: add constraint, IMU pre-integraion: sliding_window_ptr_->AddPRVAGIMUPreIntegrationFactor( param_index_i, param_index_j, imu_pre_integration_ ); }
2.2.3 ceres 中添加残差块
FILE : ceres_sliding_window.cpp
创建问题
// TODO: create new sliding window optimization problem: ceres::Problem problem;
添加待优化参数快
// TODO: add parameter block: 添加待优化的参数快 problem.AddParameterBlock(target_key_frame.prvag, 15, local_parameterization); if( target_key_frame.fixed ) { problem.SetParameterBlockConstant(target_key_frame.prvag); }
添加边缘先验因子残差块
// add marginalization factor into sliding window problem.AddResidualBlock( factor_marginalization, NULL, key_frame_r.prvag // 一元边 );
添加地图匹配因子残差块
// TODO: add map matching factor into sliding window problem.AddResidualBlock( factor_map_matching_pose, NULL, // loss_function key_frame.prvag // 一元边 );
添加帧间点云匹配匹配因子残差块
// TODO: add relative pose factor into sliding window problem.AddResidualBlock( factor_relative_pose, NULL, // loss_function key_frame_i.prvag, key_frame_j.prvag // 二元边 );
添加IMU预积分因子残差块
// TODO: add IMU factor into sliding window problem.AddResidualBlock( factor_imu_pre_integration, NULL, // loss_function key_frame_i.prvag, key_frame_j.prvag // 二元边 );
3. evo 评估
evo_ape kitti ground_truth.txt optimized.txt -r full --plot --plot_mode xyz
3.1 EKF 与 因子图优化方法比较
Factor Graph EKF IMU-Lidar Fusion max 5.546784
mean 1.907476
median 1.682371
min 0.000001
rmse 2.167268
sse 21268.254731
std 1.028876max 1.166536
mean 0.244853
median 0.187717
min 0.010400
rmse 0.298805
sse 391.959520
std 0.171264结果:EKF的效果暂时来看要比图优化的效果要好,但是这不一定说明图优化的效果不好,原因是KITTI数据集自身也存在一定的问题,并且在这次对比实验中,EKF 和 factor graph 所使用的先验地图构建方法不一样,EKF 使用第四章的激光里程计mapping,而factor graph 使用的是第九章imu预积分图优化的方法构建的地图。
3.2不同滑窗长度比较
FILE : lidar_localization/config/matching/sliding_window.yaml
修改不同的滑窗长度
# sliding window size: # sliding_window_size: 20 #滑窗长度为20帧
sliding windows length 2 10 15 20 30 地图 max 5.590900 5.590900 5.546784 5.510841 8.340555 2.325375 mean 2.345147 2.26939 1.907476 1.886341 2.906110 0.801601 median 2.178942 2.111248 1.682371 1.718590 2.988234 0.701903 min 0.000001 0.000001 0.000001 0.000001 0.000001 0.023720 rmse 2.648810 2.531902 2.167268 2.143079 3.207210 0.928723 sse 31769.338233 29026.859453 21268.254731 20796.134607 46452.465579 1650.013734 std 1.231456 1.122670 1.028876 1.017106 1.356733 0.469001 总结:
1.滑动窗口的长度也是一个关键的因素,过高或者过低的窗口长度会造成精度的降低。
2.按照乾哥的说法EKF的滤波方法就相当于滑动窗口为1的情况,可以看出,选取适当滑窗大小是有必要的,能够直接影响最后的性能。
4. 注意
sqrt_info 的理解
在求解图优化因子雅克比、残差的过程中,都会乘以一个sqrt_info 矩阵,代码的写法是参考vins,以下参考博客均有对sqrt_info 矩阵的理解
Eigen::LLT<Eigen::Matrix<double,15,15>> LowerI(I_); // sqrt_info 为上三角阵 Eigen::Matrix<double,15,15> sqrt_info = LowerI.matrixL().transpose();
2.后端优化 | VINS-Mono 论文公式推导与代码解析分讲
综上个人理解:在高斯牛顿法优化中, 非线性优化的式子为 J^T (W^-1) J = -J^T (W^-1) r,其中W为观测的协方差矩阵,为对角矩阵,对角线的每个元素为该残差的方差,即可理解为观测误差(传感器的噪声)。将W求逆即为残差的信息矩阵,可理解为权重矩阵,用于调整不同的残差项的权重(具有归一化的意义) 。因为ceres 只接受 min(e^T e ) 形式的最小二乘优化形式,所以可以通过cholesy分解的方式将残差的信息矩阵(W^-1)分解为上下三角形式,然后分别乘到残差和雅克比上,其形式与原高斯牛顿法的等式等价,相当于在把残差的信息矩阵开根号,然后分别乘到雅克比和残差上。
// 雅克比 乘 上 sqrt(残差信息矩阵) jacobian_i = sqrt_info * jacobian_i; // 残差 乘 上 sqrt(残差信息矩阵) residual = sqrt_info * residual;
右乘BCH及其求逆公式
此处部分为课程中较多争议的一个部分,为扰动模型中,右雅克比在代码上的实现,摘抄自陈皓嘉助教的“第十章思路讲解”。
FILE : factor_prvag_imu_pre_integration factor_prvag_map_matching_pose factor_prvag_marginalization
// 右雅克比的逆 static Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d &w) { Eigen::Matrix3d J_r_inv = Eigen::Matrix3d::Identity(); double theta = w.norm(); if ( theta > 1e-5 ) { Eigen::Vector3d a = w.normalized(); Eigen::Matrix3d a_hat = Sophus::SO3d::hat(a); double theta_half = 0.5 * theta ; double cot_theta = 1.0 / tan(theta_half); J_r_inv = theta_half * cot_theta * J_r_inv + (1.0 - theta_half * cot_theta) * a * a.transpose() + theta_half * a_hat ; } return J_r_inv; }
关于是否需要添加帧间里程计因子的讨论
摘自学员解答:
由于我们播的是相同的数据集,点云地图匹配和激光里程计的结果差不多,如果地图变化比较大,激光里程计的存在会使系统更加鲁棒。
5. 课程答疑总结
5.1 进行舒尔补构建b矩阵是,正负号问题
答:公式上是没问题的,区别在于构建残差是是 (预测-观测) or (观测-预测)
5.2 因子图优化中Ti Tj 问题
问:我有个疑问,这里添加的因子是,激光里程计因子,是点云的scan2scan, 作残差时,是这两帧通过点云匹配相对位姿 Tij 和 Ti Tj计算的相对位姿比较,这里没有矛盾么,因为这里的因子是点云匹配出来的,而Ti Tj 一开始也是点云匹配给的,在做残差时,不相当于自己减自己?
Ti Tj 一般是预测值,一般是由scan2scan 上一时刻的后验POSE给的后验,作为预测。残差模型一般都是遵循 (观测 - 预测)模式,scan2map 先验地图匹配一般不作为先验因子,一般作为观测量。
5.3 EKF 与 factor graph 对比
问:因子图优化对比EKF在工程上的优势,因为我在第十章EKF 和 因子图对比中,发现EKF的效果要好点。
答:在课程的效果中,EKF 对比 因子图优化的效果要好,鉴于数据集本身存在分米级的误差,所以不用太在意。实际上,因子图优化开始取代EKF。在建图领域,大部分都使用优化的方法,在定位,目前大多数还是kalman的方法,包括apollo也是沿用kalman的定位方案,但是优化方法也在逐步取代。优化方法体现的优势在于:EKF的方法基于一阶马尔科夫性,只是局部最优,而图优化的方法是基于全局最优,即使是滑窗方法上的优化方法,也是选取多帧最优。
5.4 scan2map中的map
问:因子图优化中的激光里程计约束指的scan2scan 还是 scan2map, 如果是scan2map 的话,那它和“先验地图匹配因子”的区别是在于 “激光里程计因子”的map是局部分割的小地图,“先验地图匹配因子”的map是大地图是吗? 使用全局的大地图匹配,对资源消耗不是很大吗?
答:激光里程计:前端激光里程计主要还是loam的方案,使用的是scan2map 方式构建,这里的map并不是只小的先验地图,只是指loam里的submap小特征地图。“先验地图匹配因子”里所说的先验地图,使用滑窗分割出的小地图,为了节省运算资源。
5.5 如何去评估地图的质量精度
a.使用更高精度的惯性传感器,进行建图时的轨迹评估;
b.标志物评判,实地选取一个标志物,使用RTK去测量当地标志物的经纬高,对应地图上同一个标志物,计算误差,多取几个,可以构建出误差系数。
5.6 slam中GPS的主要作用
gps的主要作用个人认为主要有两个:1.消除行驶过程中累积误差; 2.室外重定位过程中的初始化有很大作用。
5.7 点云匹配过程中,特征退化的现象(长廊、隧道),有什么方法可以判断出来?(待补充)
1.特征退化的场景(如长廊、隧道),因为只有两侧的观测,可以对匹配的协方差矩阵进行特征值分解,通过判断特征值的分布状况判断。
2.也可已增加imu uwb等辅助传感器。
5.8 IMU上电的bias需要怎么估计?(待补充)
kitti数据集中貌似已估
5.9 图优化 因子图 位姿图 概念(待补充)
5.10 SLAM 算法岗位应届生招聘要求
a.slam 的基础知识,前端匹配:icp ndt, 后端优化: 高斯牛顿法、LM优化
b.代码能力(leetcode top 100 、 剑指offer)
c.常见的开源框架, 优点(为什么选择他) ,缺点(如何改进)
d.项目经验(重点): 为什么这样搞,这样搞的好处、优势。
edited by kaho 2022.3.19
-
深蓝学院-多传感器融合定位-第4章作业
2021-02-20 02:40:51深蓝学院-多传感器融合定位-第4章作业一. 及格部分1. 代码跑通二. 良好部分三. 优秀部分Scan Context版本GNSS版本 一. 及格部分 1. 代码跑通 直接编译,跑通: catkin config --install && catkin build ... -
深蓝学院-多传感器融合定位-第6章作业
2021-03-09 06:25:07深蓝学院-多传感器融合定位-第6章作业1. 及格题目2. 良好题目3. 优秀题目 1. 及格题目 2. 良好题目 3. 优秀题目 -
深蓝学院-多传感器融合定位-第3章作业
2021-02-10 06:00:54深蓝学院-多传感器融合定位-第三章作业 -
深蓝学院 《多传感器融合定位》 第2章作业
2020-11-17 22:16:44位姿初始化 整体框架参考 从零开始做自动驾驶定位(九): 建图系统结构优化 我的电脑上 rosbag play kitti_2011_10_03_drive_0027_synced.bag -r 1.3 基本前端里程计能跟上gnss轨迹 先跑通建图 roslaunch mapping.... -
多传感器融合定位 第八章 基于滤波的融合方法进阶
2021-10-22 18:40:54多传感器融合定位 第八章 基于滤波的融合方法进阶 参考博客:深蓝学院-多传感器融合定位-第7章作业 代码下载:... -
多传感器融合定位 第四章 点云地图构建及基于点云地图定位
2021-09-05 16:24:53多传感器融合定位 第四章 点云地图构建及基于点云地图定位 代码下载 ... -
多传感器融合定位 课程概述
2020-10-10 21:04:27多传感器融合定位(1)1-课程概述 -
多传感器融合定位在高速铁路的应用
2021-01-19 16:47:202009年在武广、郑西等客运专线中,C3列控系统的应用,对高速铁路列车定位技术提出了更高的要求。C3列控系统的主要技术原则中明确提出,列车的运营速度达到350 km/h,追踪间隔为3 min,并且300 km/h及以上动车组不... -
传感技术中的多传感器融合定位在高速铁路的应用
2020-11-09 03:58:522009年在武广、郑西等客运专线中,C3列控系统的应用,对高速铁路列车定位技术提出了更高的要求。C3列控系统的主要技术原则中明确提出,列车的运营速度达到350 km/h,最小追踪间隔为3 min,并且300 km/h及以上动车... -
多传感器融合定位 第一章 概述
2021-08-18 12:56:05多传感器融合定位 第一章 概述 本记录深蓝学院多传感器定位融合第四期学习笔记,官方推荐使用docker进行开发,为了方便之后移植部署,故本次在次在本地环境进行开发。 代码下载 : ... -
多传感器融合定位 第七章 基于滤波的融合方法
2021-10-22 13:48:35多传感器融合定位 第七章 基于滤波的融合方法 参考博客:深蓝学院-多传感器融合定位-第7章作业 本次作业:主要参考张松鹏大佬的代码,因为大佬的解析太好了,为了保留记录,以下大部分文字,均摘自大佬的原话~ 代码... -
深蓝学院-多传感器融合定位-第7章作业
2021-03-21 17:57:30深蓝学院-多传感器融合定位-第7章作业 作业解析及代码 -
车载多传感器融合定位方案:GPS +IMU+MM
2019-08-21 10:49:59导读 高德定位业务包括云上定位和端上定位两大模块。...特别是车机端定位,由于定位设备安装在车上,一方面,它可以搭载更丰富的定位传感器来解决特殊场景的问题,另一方面,各个传感器之间相互固连,有利于... -
多传感器融合系统特点及体系结构的介绍
2020-07-25 15:38:59所谓多传感器信息融合,就是利用计算机技术将来自多传感器或多源的信息和数据,在一定的准则下加以自动分析和综合,以完成所需要的决策和估计而进行的信息处理过程。 -
深蓝学院 《多传感器融合定位》 第1章作业
2020-10-30 18:11:54} ReadData():获取来自 Subscriber 的原始数据存储在std::deque中,同步各传感器数据 InitCalibration():获取 lidar_to_imu_ 坐标系变换矩阵 InitGNSS():初始化GNSS数据 UpdateGNSSOdometry():更新GNSS位置,并... -
多传感器融合定位开源工程与论文
2020-12-14 16:38:14Fusion:基于激光雷达、惯性导航和相机结合的里程计(2)使用点线特征配合激光雷达辅助的单目视觉里程计(3)间歇的GPS辅助VIO:在线初始化和标定(4)强大的高精度视觉惯性激光SLAM系统(5)地面机器人的视觉辅助定位**(6)... -
多传感器融合定位 第六章 惯性导航结算及误差模型
2021-10-02 13:26:36多传感器融合定位 第六章 惯性导航结算及误差模型 参考博客:深蓝学院-多传感器融合定位-第6章作业 代码下载:... -
深蓝学院多传感器融合课(第二章相关代码)
2022-04-13 10:14:05添加了用于计算每帧点云处理时间的相关代码 -
在本地运行多传感器融合定位模块
2019-09-10 14:26:44如何在本地运行多传感器融合定位模块 定位技术横跨好几个专业,包括测绘、导航、计算机视觉知识、以及点云处理的知识。业界所说的“多传感器融合”,都是指对摄像头、激光雷达、毫米波雷达、超声波雷达等多种... -
多传感器融合定位在高速铁路的应用[图]
2020-10-22 13:37:182009年在武广、郑西等客运专线中,C3列控系统的应用,对高速铁路列车定位技术提出了更高的要求。C3列控系统的主要技术原则中明确...目前,陀螺、加速度计、里程仪、GPS接收机等传感器已经普遍应用于列车测速定位系统。 -
[自动驾驶]车载多传感器融合定位方案:GPS +IMU+MM
2019-11-01 19:18:45导读 高德定位业务包括云上定位和端上定位两大模块。...特别是车机端定位,由于定位设备安装在车上,一方面,它可以搭载更丰富的定位传感器来解决特殊场景的问题,另一方面,各个传感器之间相互固连,有利于... -
多传感器融合定位(GPS+Wheel+camera)(1)-读取传感器数据
2021-02-09 21:37:43多传感器融合定位(GPS+Wheel+camera)(1)-读取传感器数据 文章目录1、读取Kaist数据集到融合系统中 1、读取Kaist数据集到融合系统中 int main(int argc, char** argv) { if (argc != 3) { LOG(ERROR) << ... -
【多传感器融合定位】
2022-03-26 11:48:28多传感器融合定位 本文记录多传感器融合定位系列的环境配置相关问题 原文: 多传感器融合定位 第一章 概述. 环境安装 ubuntu18.04 + ROS Melodic 1.g2o // 从github上下载源码 $ ...