基于高翔博士自动驾驶与机器人中的SLAM技术理论实现的_多传感器融合_三维点云建图_激光雷达视觉惯性里程计_深度学习辅助定位_实...

# 3DSlam # 问题记录 1. ICP的优化解法和解析解法是什么概念？ ## 雅克比矩阵 什么是雅可比矩阵？ 答：一阶偏导数以一定方式排列乘的矩阵 为什么是一阶的偏导数呢？在非线性的EKF或者其他优化问题的时候，非线性的解是比较难求的 所以我们通常将非线性的函数->线性函数。于是高等数学中用泰勒展开，将非线性的转换为线性的并忽略高阶的部分。 函数 $y_1(x_1,x_2,....x_n), y_2(x_1,x_2,....x_n)$ 如果偏导数存在 如果函数在$x_0$处可微分，根据泰勒公式就有 $$ F(x)\approx F(x_0) + J(x) *(x-x_0) $$ ## 多线雷达的扫描匹配 高博: 多线雷达的扫描匹配比单线的更好，而且不使用子图的管理，而是直接管理点云 利用scan to scan 或者 scan to map。 ### 点到点的icp 1. 描述: 存在两个点云 $ S1 = \{p_1, p_2, ......, p_m\}, S2 = \{q_1, q_2,......,q_n\}$。这两个点云需要相同的点数么？显然不需要，当scan to map的时候，明显map的点云会多很多。可以是任意无关的点云，顺序也是任意的。 如果点云是匹配的那么 $p_i = R_0 * q_i + t_0$.对S1的集合，存在$R_0, t_0$的SE3可以将S2的点转到S1下。 2. 算法步骤: * 假设初始位姿$R_0,t_0$. * 从初始化姿态开始估计，设K次迭代的结果为$R_k, t_k$ * 在 $R_k, t_k$ 估计下，最邻近方式查找匹配，匹配后的点$(p_i,q_i)$ * 计算本次的迭代的结果: $$R_{k+1},t_{k+1} = argmin \sum ||p_i - (Rq_i+t)||_2^2$$ * 判断上一步迭代的结果是否收敛，不收敛返回第三步 3. 总结 所以ICP 就是不断的计算位姿和匹配的交替过程 4. 误差的定义 $$ e = p_i - R * q_i - t$$ 5. 对R,t的雅可比矩阵 $$ \frac{\partial e_i}{\partial R} = R * skew(q_i) \frac{\partial e_i}{\partial t} = -I $$ ## 点到线/面的icp 区别于点到点的ICP，查找最邻近不是一个点了，而是一系列的点，拟合成直线或者平面 ## ndt 匹配 ## 雷达里程计 ### 直接法雷达里程计 1. NDT构建雷达里程计 2. 增量NDT雷达里程计 ### 特征法里程计 同一根线束的曲率，如果曲率较大，就是角点，曲率较小就是平面点 #### 算法步骤 1. 计算每个点的线束并归类 2. 计算每个线束中每个点的曲率 3. 把点云按一周分成若干个区间，选择其中曲率最大的点作为角点，剩余点作为平面点 注： 特征提取没有明确的理论依据，更多是工程实践 4. 为角点和平面分别构建两个局部地图，两者的ICP会放到同一个优化问题 # 松耦合的lio系统 使用imu来指导激光雷达的匹配的预测方向 LIO系统/LINS系统 ## 紧耦合 区别于松耦合，将点云的残差方程直接作为观测方程，写入到观测模型中。相当于状态估计系统或者优化算法中内置了一个ICP/NDT ## 松耦合 系统维护了一个状态估计器，imu和轮速计为这个估计系统提供了惯性和速度方面的观测源，点云的匹配为系统提供了位姿的观测源，松耦合不是将点->线/面，或者NDT的残差放入状态估计器，而是点云的匹配为状态估计器提供观测值，状态估计器为点云匹配提供状态。 1. 特点：卡尔曼滤波器和点云匹配是分开的，相对解耦的。 2. 缺点: 当然我们可以使用卡尔曼的预测输出作为点云匹配的初始值进行匹配。但是如果激光雷达发生退化或者受到干扰，那么也会影响状态估计其 ## 松耦合的LIO系统 ### 坐标系说明 1. 世界坐标系，imu坐标系， laser坐标系 2. 记 TIL为lidar到imu的变换 3. 因为IMU的运动模型是在imu坐标系下推导的，所以我们通过外参将lidar的点云转到imu坐标系下。整个系统以imu为中心来实现定位和建图的效果 4. 设$P_l$为lidar坐标系下的点，那么IMU坐标系下的点为$P_i = T_L^I * P_l = R_{IL} * P_L + t_{IL} $ ### 运动和观测方程 5. 运动方程： 同imu的运动方程 6. 观测方程: 同GNSS观测是一样的，相对于状态量R,p的预测 ### 数据 1. 10hz雷达 2. 100hz imu 3. 雷达的运动补偿 4. 将ESKF的预测输出作为初始值,交给里程计算法，里程计算法匹配完毕后，再更新状态量。