基于三维点云图的路径规划

REF：基于多传感器融合的激光 SLAM 三维场景实时重建与路径规划方法研究

1. SLAM算法建图案例

• Cartographer SLAM 算法建图效果
• LOAM 算法框架和建图
• LeGO-LOAM 算法框架和构建的环境地图
• LOAM-Livox 算法建图
• 多传感器融合的激光 SLAM 算法构建的环境地图

2. LIO前端里程计

• IMU 模型： IMU通常由多个惯性传感器组成，常用的包括加速度计、陀螺仪、磁力计和气压计，测量数据误差包括系统误差和随机误差，前者是由于传感器自身固有的偏差和不准确性引起的，可通过传感器的校准减小，后者零偏不稳定性和随机游走，可以利用误差模型对传感器输出进行修正
• 加速度计测量误差模型：
  • ama_mam：是𝑡时刻加速度的测量值
  • 𝑎t𝑎_tat：是 IMU 加速度真实值
  • batb_{a_t}bat：是加速度计的偏置误差
  • gwg^wgw：是世界坐标系下的重力加速度
  • RwtR^t_wRwt：是𝑡时刻世界坐标系，到当前时刻 IMU 坐标系的旋转矩阵
  • nan_ana：表示加速度计的测量噪声，其服从高斯分布
• 陀螺仪的测量误差模型：
  • wmw_mwm： 是𝑡时刻角速度的测量值
  • 𝜔_t：是 IMU 角速度的真实值
  • bwtb_{w_t}bwt：是陀螺仪的偏置误差
  • nwn_wnw：是陀螺仪的测量噪声，服从高斯分布
• IMU 内参标定： 使用开源工具 imu_utils，确定 IMU 标定的内部误差参数
• 激光雷达与 IMU 外参标定： 由于传感器的采集频率和坐标系均不同，需要对不同传感器进行外参标定（空间中坐标系的相对位姿关系，包括坐标平移和旋转），得到各个传感器坐标系之间的位置变换关系，利用标定确定传感器的外参后，可以实现不同传感器数据在坐标系上的归一化，因此需要标定出激光雷达和 IMU 之间的位置变换关系。
  • 将 VLP-16 和 N100 固定在一起，并运行一段距离
  • 利用 lidar_align 开源工具计算外参矩阵
• 利用IMU去除点云运动畸变： 不同时间的激光点位于不同的坐标系中，导致了一种运动畸变现象，利用 IMU 测量的加速度和角速度等信息，同时结合高精度的运动估计完成畸变去除
• IEKF 实现雷达-IMU 紧耦合： 迭代误差卡尔曼滤波器融合激光雷达和 IMU 数据，以估计载体的运动状态

3. 回环检测及后端全局优化

• 结合高度和强度信息扫描上下文算法（Height and Intensity Scan Context，HISC） 通过将三维点云数据降维到二维图像来降低计算复杂度，选取每个子空间中所有点的最大高度值与强度值之和作为该子空间的值，计算点云帧之间的相似性，以此判断是否构成闭合环路
• 自适应距离阈值： 使用距离阈值来检测和排除相似度高但相距较远的点云对，利用点云关键帧的数量𝑘构造线性函数来定义阈值，自适应距离阈值替代传统的固定阈值，
• GTSAM 优化库优化： 利用 isam2 添加里程计因子、闭环检测因子以及 GPS 因子（绝对位置约束降低轨迹漂移），通过联合优化来消除误差，以求解最优轨迹及各个关键帧位姿，从而得到更准确的地图和定位

全局3D点云地图构建 利用构建的因子图进行全局优化，得到优化后的位姿进而得到全局一致的轨迹，利用得到的位姿将新地图信息融合进先前已有地图中，不断完善环境地图，最终得到完整的3D 点云地图

4. 三维地图路径规划

• 八叉树结构 左侧图像展示了所构建的环境，右侧图像展示了对应的八叉树组织形式：
  • 根节点：对应最大的立方体，八个子节点代表被划分出的八个立方体
  • 左图中白色立方体：对应右图所示树中的黑点，代表不确定
  • 左图灰色立方体：对应于树中的白色方形，表示空闲区域
  • 左图中黑色的立方体：表示该区域已被占用
  • 被再次划分的立方体：对应于树中的灰色圆形，表示还有内部节点，即其子节点的概率并不相等
• 三维点云地图栅格化： 利用八叉树结构将点云分割成均匀大小的单元，并将每个单元内的点密度或属性信息进行统计和压缩
• 障碍物栅格膨化： 不管障碍物是否占满整个栅格，只要栅格中有障碍物存在，就膨化当前栅格将整个看作障碍栅格
• 跳点搜索算法（jump point search，JPS）： 通过跳过某些节点的搜索，减少了搜索空间，从而提高了路径搜索效率
• 优化JSP： 在选择节点的当前跳跃方向扩展时，会一直沿当前方向进行搜索，直至遇到边界或者障碍物