- 博客(73)
- 收藏
- 关注
RSS订阅原创 为什么边缘化先验的Hessian矩阵可分解得到Jacobian矩阵？
边缘化后得到的先验信息矩阵仍保持对称半正定特性，确保优化问题的数学合理性。该方法有效解决了滑动窗口优化中旧帧信息保留的问题。
2026-04-16 15:31:29
437
原创 为什么优化问题求解最终是HΔx=b 而非 Hx=b？
首先，只有线性最小二乘才能直接解，因为线性模型的海森矩阵是常数矩阵，能一步求全局最优。但 SLAM 里的观测模型都是非线性的，比如激光配准、IMU 预积分的残差都是位姿的非线性函数，没法直接构造。我们只能在当前状态估计值附近做一阶泰勒展开，把非线性问题转化为关于增量的线性最小二乘问题，然后迭代更新状态直到收敛。
2026-04-16 12:37:59
517
原创 为什么优化问题求解用最小二乘法？
最小二乘问题有一整套完善的数值解法，从线性系统的 QR 分解、SVD，到非线性系统的高斯 - 牛顿法、LM 算法（SLAM 中最常用），都经过了几十年的工程验证，数值稳定性和收敛性都有保障。：我们常用的 Ceres、g2o、GTSAM 等 SLAM 优化库，本质上都是专门为最小二乘问题设计的，提供了高度封装的接口，能快速构建和求解大规模图优化问题。：我们常用的点到线、点到面 ICP 残差，IMU 预积分的残差，边缘化的先验残差，本质上都是平方损失的形式，推导和实现都非常直观。
2026-04-15 16:21:26
343
原创 LIO-SAM Scan-to-Map退化检测
在 SLAM 中，“退化”意味着传感器在当前环境中收集不到足够的信息来约束全部的 6 个自由度（x、y、z轴方向上的平移和绕三轴的旋转）。按照右手系的定义，以x轴作为机器人前进方向。场景A（长直走廊）：雷达在一个超长的长廊里前进无法，激光束无法打到长廊的尽头，两边和上下全是平坦的墙。雷达可以完美知道自己有没有左右动、上下动、有没有旋转，但无法判断自己往前走了多远（前后方向退化）。场景B（开阔广场+单面墙）：雷达面前只有一堵巨大的墙。
2026-04-13 22:47:44
394
原创 LIO-SAM系统架构
LIO-SAM是一个基于因子图优化的激光惯性里程计系统，主要由四个核心节点组成：点云预处理节点：负责点云去畸变和格式统一 特征提取节点：提取边缘和平面特征。前后端优化节点：执行Scan-to-Map匹配和因子图优化。IMU预积分节点：实现高频状态预测。关键技术点： 采用双因子图架构：前端IMU预积分图和后端激光雷达图；点云处理流程：缓存→运动补偿→深度图投影→特征提取；特征提取策略：基于曲率分析，分区均匀采样；位姿优化机制：结合IMU预积分和激光雷达观测；高频里程计输出：通过TransformFusion类
2026-04-13 21:51:11
809
原创 IMU预积分-旋转残差求雅可比
本文基于IMU预积分(VINS)推导了旋转残差项的雅可比矩阵。采用右乘扰动模型，利用李群BCH公式和伴随性质，详细推导了残差对各时刻旋转误差向量和陀螺仪bias的雅可比。重点实现了右乘BCH近似雅可比的逆的计算，通过判断旋转角大小选择闭式公式或小角度近似，确保数值稳定性。代码使用Eigen库实现了该计算过程，处理了旋转角趋近0时的数值稳定性问题。
2026-04-11 22:47:21
569
原创 滑动窗口模型与边缘化
本文探讨了SLAM系统中基于滑动窗口的边缘化方法。通过因子图模型分析，指出随着时间推移，图模型增大导致优化耗时递增的问题。采用滑动窗口机制，通过舒尔补理论将旧帧约束转化为边缘化先验因子，保留历史信息的同时控制计算复杂度。详细推导了Hessian矩阵构建、边缘化操作及先验因子生成过程，并给出代码实现框架。
2026-04-11 17:01:04
432
原创 贝叶斯滤波与卡尔曼滤波
贝叶斯滤波作为通用理论框架，在实际应用中面临无穷积分计算难题。卡尔曼滤波通过引入线性系统和高斯分布两个关键假设，使后验分布获得闭式解。在满足线性变换和高斯噪声条件下，利用联合高斯分布的条件分布特性，通过舒尔补直接推导出后验分布的均值和协方差，完美对应卡尔曼滤波的更新公式。这证明卡尔曼滤波并非独立于贝叶斯框架，而是其在线性高斯场景下的最优实现方式，二者本质上是同一理论在不同约束条件下的等价表达。
2026-04-08 21:00:18
277
原创 IMU预积分（VINS）
预积分技术通过计算IMU在相邻关键帧间的相对运动量（位置、速度、姿态），避免优化迭代时重复积分高频数据。其核心是将连续IMU测量离散化为与初始状态无关的预积分量，并假设短时间内零偏恒定。为处理零偏优化后的更新，预积分结果通过一阶泰勒展开和雅可比矩阵（利用误差状态传递递推）实现快速修正。残差设计采用载体坐标系投影以简化雅可比推导，协方差传递则通过离散化误差微分方程量化累积噪声。代码实现中，预积分量、方差及雅可比矩阵通过中值积分逐步递推，最终作为二元约束边参与图优化。
2026-04-08 20:28:34
599
原创 误差状态卡尔曼滤波（ESKF）推导
本文系统介绍了概率基础知识和滤波器原理。首先阐述了概率论核心概念，重点分析了高斯分布及其线性变换特性。随后详细推导了卡尔曼滤波（KF）和扩展卡尔曼滤波（EKF）的理论框架，包括状态估计模型、贝叶斯滤波和线性化处理方法。针对EKF的局限性，进一步介绍了误差状态卡尔曼滤波（ESKF），通过分离名义状态和误差状态，有效解决了非线性系统特别是3D姿态估计中的约束问题。文章完整呈现了ESKF的运动方程、观测方程及其离散化形式，并给出了实际应用中的初始化、预测更新和量测更新等关键步骤。
2026-04-05 17:52:36
567
原创 惯性导航解算及误差分析
本篇笔记记录了惯性导航系统的微分性质与解算方法。首先推导了旋转矩阵、四元数和旋转向量的连续时间微分方程，建立了不同姿态表示形式间的数学关系。随后详细阐述了惯性导航解算过程，包括基于中值法的姿态更新、速度更新和位置更新的离散化实现。针对IMU误差问题，重点分析了姿态、速度和位置的误差微分方程，推导了名义值与真实值间的误差传播机制，为误差状态卡尔曼滤波提供了理论基础。
2026-04-02 10:50:37
439
2
原创 IMU误差分析与内参标定
本文系统介绍了IMU内参误差建模与标定方法。首先将IMU误差分为信号误差（随机噪声）和内参误差（确定性误差），重点分析内参误差模型，包括零偏、刻度系数误差和安装误差的数学表达。然后详细阐述两种标定方法：基于转台的正向建模（利用精确输入构建超定方程组求解）和基于优化的反向建模（通过残差函数优化估计参数）。特别推导了加速度计误差模型对参数的雅可比矩阵，为非线性优化提供理论基础。
2026-04-01 07:15:10
475
原创 点到线/面ICP-残差及雅可比推导（A-LOAM）
点到线/面ICP配准通过引入曲率信息将点云分为边缘点和平面点，分别处理以提高匹配精度。对于边缘点采用点到线距离残差，平面点采用点到面距离残差。本文详细推导了两种情况的残差计算公式和雅可比矩阵。通过代码实现展示了点线残差因子和点面残差因子的具体计算过程。
2026-03-30 19:44:43
466
1
原创 点到点ICP-基于SVD分解的帧间点云匹配
本文介绍了两帧点云配准的基于直接法的点点ICP的基本流程。通过寻找相邻时刻点云间的对应点，构建残差函数最小化问题，采用SVD分解方法求解最优旋转矩阵R和平移向量t。具体步骤包括：1)利用kd-tree加速对应点搜索；2)计算去中心化协方差矩阵H；3)对H进行SVD分解得到R；4)根据质心计算t；5)迭代优化直至收敛
2026-03-28 17:08:51
362
原创 LVI-SAM中激光点云辅助视觉特征点获取深度
本文介绍了了LVI-SAM中的一种基于激光点云辅助的视觉特征点深度估计方法。首先将视觉特征点投影到单位球面，同时对激光点云进行角度下采样并归一化到单位球面。通过构建KDTree快速搜索与视觉特征点方向最近的三个激光点，利用三点拟合平面并计算视觉射线与平面的交点，从而估计特征点深度。该方法通过球面投影消除深度影响，仅基于方向差异进行匹配，实现了视觉特征与激光点云的高效关联。
2025-12-10 18:03:20
472
原创 A-LOAM
本文介绍了激光SLAM系统的三个核心节点：scanRegistration节点负责点云预处理，包括滤波、特征提取（边缘点和平面点）；alaserOdometry节点通过点线/面残差优化实现帧间位姿估计；alaserMapping节点进行全局优化消除漂移。系统采用分层处理架构，先进行局部帧间匹配（odometry），再通过地图匹配（mapping）优化全局位姿。特征点提取采用曲率计算和分段采样，位姿优化使用Ceres求解器最小化残差。整个流程实现了从原始点云到位姿估计的完整处理链。
2025-12-06 17:24:47
932
原创 LVI-SAM/LIO-SAM/LEGO-LOAM中点到线/面残差计算及其雅可比矩阵推导
摘要：本文详细解析了LOAM系列SLAM算法中激光点云特征匹配的关键计算过程。重点阐述了点到线/面残差的计算方法及其雅可比矩阵推导：1）通过PCA主成分分析进行直线拟合，使用向量叉积计算点到直线距离；2）采用最小二乘法进行平面拟合，求解点到平面距离；3）推导残差函数对位姿的偏导数构造雅可比矩阵。最后介绍了基于高斯牛顿法的线性系统求解过程，实现位姿优化。文章以LVI-SAM源码为例，系统讲解了激光SLAM中特征匹配的核心数学原理。
2025-11-20 18:44:57
860
原创 Cartographer源码-扫描匹配
本文摘要： Cartographer的扫描匹配过程分为两个主要步骤：相关性扫描匹配和基于Ceres的优化扫描匹配。 在相关性扫描匹配阶段，系统通过暴力搜索方法在指定窗口内（线性0.1m，角度20度）评估候选位姿。该过程先对点云进行多角度旋转预处理，然后计算每个候选位姿下点云与栅格地图的匹配得分，选取最优解作为初始位姿。 基于优化的扫描匹配则构建了包含三个残差项的最小二乘问题：1)地图残差，通过插值计算点云与地图的匹配程度；2)平移残差，限制位姿变化幅度；3)旋转残差，保持角度稳定性。Ceres求解器通过自动
2025-10-19 15:16:15
774
原创 Cartographer源码-激光数据走向
本文分析了Cartographer中激光雷达数据的处理流程：1. 数据订阅与回调注册：通过ROS节点订阅激光话题，使用模板函数和lambda表达式实现多参数回调；2. 数据转换：将LaserScan消息转换为带时间戳的三维点云，处理多回波数据并计算点云坐标；3. 数据分发：通过Collator和OrderedMultiQueue实现多传感器数据的时间同步；4. 扫描匹配：最终将处理后的点云数据送入LocalTrajectoryBuilder进行扫描匹配和位姿优化。系统采用分层处理架构，确保数据有序性和实时性
2025-09-28 16:13:05
1105
原创 Cartographer源码-概率栅格地图
本文主要介绍了Cartographer中2D概率栅格子图的构建过程，涉及LocalTrajectoryBuilder2D、ActiveSubmaps2D、Submap2D、ProbabilityGrid等核心类。通过扫描匹配后的点云数据被插入到两个活跃子图中，每个子图使用ProbabilityGrid存储栅格地图，并基于射线投射模型更新命中点和自由空间的概率值。文章详细分析了栅格地图的数据结构、坐标转换、概率更新机制以及地图扩容裁剪等关键实现细节。整个系统采用分层设计，通过智能指针管理地图对象。
2025-09-26 22:29:51
909
原创 Cartographer源码-位姿推测器pose_extrapolator
本文分析了Cartographer中位姿推测器（PoseExtrapolator）的实现细节。PoseExtrapolator通过融合IMU、里程计和扫描匹配数据来预测机器人位姿，主要包括： 核心组件： IMUTracker：基于IMU数据跟踪姿态和重力方向 数据队列：维护位姿、IMU和里程计的历史数据 速度估计：通过位姿变化和里程计计算线/角速度 关键流程： 数据添加：处理IMU、里程计和扫描匹配位姿 位姿预测：结合速度估计和IMU数据外推未来位姿 数据修剪：维护有效时间窗口内的数据 实现特点： 多传感器
2025-09-09 21:38:17
842
原创 Cartographer源码-扫描匹配前的点云预处理
本文分析了Cartographer中激光点云预处理的关键步骤，主要包括时间同步、去畸变和滤波处理。首先通过RangeDataCollator实现多雷达数据的时间同步；然后利用位姿推测器获取每个点采集时刻的位姿，将点云从tracking_frame转换到local_frame坐标系完成去畸变；接着通过体素滤波和自适应体素滤波减少数据量，其中自适应体素滤波会根据点云密度动态调整体素大小；最后将处理后的点云用于扫描匹配。这些预处理步骤确保了点云数据的准确性和高效性，为后续的定位和建图提供了可靠输入。
2025-09-02 21:58:35
1263
原创 Cartographer源码-从ordered_multi_queue的数据分发到GlobalTrajectoryBuilder::AddSensorData()
本文分析了Cartographer中从ordered_multi_queue的数据分发到GlobalTrajectoryBuilder::AddSensorData()流程：1) OrderedMultiQueue按时间顺序分发数据；2) 数据通过CollatedTrajectoryBuilder传递给GlobalTrajectoryBuilder；3) GlobalTrajectoryBuilder作为SLAM核心，连接前端(LocalTrajectoryBuilder)和后端(PoseGraph)
2025-09-02 13:37:28
908
原创 Cartographer中的gflag与lua文件
本文介绍了Google开源的命令行参数处理库gflags及其在Cartographer项目中的应用。gflags支持多种参数类型（bool、int、string等），通过DEFINE宏定义参数，FLAGS_name访问参数值。Cartographer通过gflags解析launch文件传入的Lua配置文件路径和名称，使用LuaParameterDictionary类读取配置内容并转换为NodeOptions和TrajectoryOptions结构体。Lua配置采用模块化设计，支持include机制和参数重写
2025-08-31 14:42:59
1057
原创 catkin使用
本文介绍了ROS中创建自定义话题消息的完整流程。首先创建工作空间catkin_ws和功能包test_pkg，然后在msg目录下创建Person.msg定义消息类型。需要修改package.xml添加依赖，并在CMakeLists.txt中配置编译选项。最后给出了C++和Python两种语言的消息发布实现示例，包括初始化节点、创建发布者、填充消息内容和发布消息的过程。
2025-08-07 09:24:52
430
原创 蓝桥杯嵌入式 特殊操作
1.LCD反向显示 2.LCD一行中指定字符使用其他颜色显示 3.修改PWM的频率和占空比 4.单timer双通道测量PWM频率 5.单adc双通道测量
2025-06-11 21:55:30
420
原创 CT117E-M4 串口通信
本文介绍了基于STM32G431RBT6与DAP Link调试器的串口通信实现过程，主要用于接收串口数据并在LCD界面上显示。首先，通过CubeMX将PA10配置为USART1_RX、PA9配置为USART1_TX，并设置串口为异步通信、波特率为9600bps，同时启用中断。随后，在中断回调函数 HAL_UART_RxCpltCallback 中实现逐字节接收，并在 UART_proc() 函数中检测数据接收是否完成。若数据长度为22字节，则用 sscanf 解析成车类型、车牌号和时间信息；若长度错误，则通
2025-03-25 21:29:59
917
原创 CT117E-M4 IIC通信-EEPROM读写
1.原理图 2.CubeMX配置 3.代码编写 3.1 向EEPROM写入数据 3.2 从EEPROM读取数据 4.测试
2025-03-25 17:24:35
712
原创 CT117E-M4 模数转换ADC测量
上篇内容为，本篇笔记记录模数转换ADC。整个系列每篇笔记中的CubeMX配置、MDK-ARM配置以及代码编写，均在前一篇笔记的基础上进行，功能有关联代码会进行微调。
2025-03-25 14:44:16
423
原创 CT117E-M4 PWM信号输出
1.PWM信号输出 1.1 CubeMX配置 1.2 代码编写 2.PWM频率测量 2.1 CubeMX配置 2.2 代码编写 3.占空比测量
2025-03-25 12:48:35
1123
原创 CT117E-M4 按键短按长按
上篇内容为，本篇内容实现按键短按长按。整个系列每篇笔记中的CubeMX配置、MDK-ARM配置以及代码编写，均在前一篇笔记的基础上进行，功能有关联代码会进行微调。
2025-03-15 16:09:34
821
原创 CT117E-M4 LCD屏显示
上篇内容为LED显示，本篇内容实现LCD屏显示。整个系列每篇笔记中的CubeMX配置、MDK-ARM配置以及代码编写，均在前一篇笔记的基础上进行，功能有关联代码会进行微调。
2025-03-14 20:59:33
419
原创 CT117E-M4 CubeMX与Keil5 MDK-ARM基础配置
1.CubeMX构建项目 2.芯片时钟配置2.1 芯片引脚配置 2.2 时钟树配置 3.调试接口配置 4.项目管理器Project Manager配置4.1 project4.2 Code Generator 5. 生成代码 6.MDK_ARM配置 6.1 编译器选择 6.2 调试器配置 7. 创建本地代码文件夹 8. #include 路径设置
2025-03-10 21:13:01
1210
原创 面向对象考试
一、面向对象二、三大特性三、类间关系1.继承（泛化）2.实现3.依赖4.关联5.聚合6.组合四、设计原则（1）单一职责原则（SRP）（2）开放-封闭原则（OCP）3. 里氏替换原则（LS）五、面向对象分析设计
2024-11-21 11:07:10
1205
原创 TCP/IP网络体系结构总结整理
物理层、数据链路层、将IP数据报封装成帧或拆帧、IP地址解析为MAC地址、完成相邻机器间的寻址与数据传输、差错控制、介质访问控制、网络层、对传输层传递的数据报进行分组的组装与拆分、路由选择、路由转发、拥塞控制、传输层、解决进程在主机中通信地址标识的问题、UDP、TCP、连接实质、面向字节流的通信、差错控制、流量控制、连接管理
2023-05-19 19:40:50
2433
原创 C/C++函数调用时参数传递过程、调用约定与可变参函数的实现
参数传递过程、参数压栈顺序从右至左的影响、调用约定、__cdecl C/C++ 缺省调用方式、 __stdcall 、 __fastcall (快速调用约定，通过寄存器来传送参数)、C++的函数名修饰方式、thiscall 本身调用、C语言可变参函数的实现、可变参函数需要使用__cdecl调用约定的原因
2023-05-14 15:10:50
2297
【概率论与数理统计】概率公式详解及其在SLAM中的应用：条件贝叶斯公式优化机器人状态估计概率
2025-07-21
空空如也
TA创建的收藏夹 TA关注的收藏夹
TA关注的人