基于语义地图的路径规划

REF：基于动-静态语义等级的农业机器人环境建图与高效路径规划方法的研究

1. 系统框架

15702faeea1ba0e12d557a31f63a73ea.png

2. 环境建图

• 语义高斯体表征：结合语义信息和高斯体表征的环境建模方法，通过高斯分布描述环境中的几何信息，融入语义信息（物体类别、功能属性），为空间中的高斯体赋予不同的语义类别，为机器人提供更高层次的环境理解能力
• 语义高斯体模型：
  • 峰值信噪比：评价图像质量，表示信号的最大可能功率和影响其精度的破坏性噪声功率的比值，用分贝单位来表示，当 PSNR 高于40dB 时，图像质量高；在低于30dB 时，图像质量较差

  

  125059238a09148addb5e226a8640783.png

  其中：

  • 结构相似性：表示两幅图像之间相似度的指标，包含亮度、对比度和结构信息

  

  a9592cf9ae5d39a67e9c8b9089329b90.png

  • 感知相似度：表示两张图像之间的差别，越低表示两张图像越相似

  

  f0fbcabe9ea7cf50480958c1a2f0108e.png

  • 深度 L1 误差：衡量预测深度图和真实深度图之间的误差，即每个像素点的绝对误差的平均值

  

  8e04c5d7b9051958d0f1053016baa607.png

  • 绝对轨迹误差：表示相机位姿的真实值与 SLAM 系统估计值之间的差值，表示位姿估计的精度

  

  20f7fc79e610c26cefd9ab106b16deb0.png

  

  802743000a67c027bb1a0e675fb0cda7.png

  • I：大小为 m*n 的原始图像
  • K：添加噪声后的噪声图像
  • MSE：均方误差
  • li：表示第 i 个高斯点的语义等级信息
  • di：表示第 i 个高斯点的深度，即相机坐标系中纵坐标的值
  • ci：表示第 i 个高斯点的颜色，即相机坐标系中的颜色值
  • fi,p2D：表示第 i 个像素点对该像素的影响权重，用来衡量该高斯点投影到该像素的贡献程度
  • ∏j=1i−1(1−fj,p2D)：表示前面所有地高斯点未完全覆盖该像素的概率，确保前面的高斯点影响后，当前高斯点仍然能够贡献
  • 3D投影为2D图像：通过在像素坐标中近似地影响函数沿着深度的积分投影，从而将 3D 高斯函数泼溅到平面上，把高斯分布用可微的方式渲染为二维图像，扩展颜色在渲染深度的功能

  

  033b43f00f68cfbfa4a5881a68f47915.png

  • 像素点颜色值：所有高斯分布对像素点的影响，会根据深度顺序进行排序，然后执行前后体积渲染，

  

  f58c7326ae98717a988d704d39868b73.png

  其中：

  • 像素点深度值：渲染方法跟颜色类似，从前到后累计语义信息，然后再对语义信息进行加权平均，通过层叠式计算深度，进行深度加权平均，确保了深度值平滑变化

  

  12ae3548fcb1915f5f29d8bc61d74bdc.png

  其中：

  • 像素点语义等级值：每个高斯点表示场景，附加一个语义等级值L来标识物体的动态属性，动-静态语义等级划分为三类：动态物体（级别为 0）、半静态物体（1）和静态物体（2），语义等级信息不仅有助于更清晰地区分不同类型物体的运动特性。

  

  abfa20c6818c2f197ec259ea1a00f3a6.png

  其中：

  • 多通道信息融合：在原有的 RGB 信息通道、深度信息通道和语义等级信息通道中加入了语义等级信息约束，从而提高重建图像的质量

  

  82adb71a6410f17d1adfcbb922c66a47.png

  • 更新损失函数：不同语义类别的物体对损失函数的影响不同，引入wLpw_{L_p}wLp （依赖于像素对应物体的语义等级）来调整各个像素点的误差贡献，最终损失函数如下：

  

  041033a47df161c277964acebea833a1.png

  • 选择关键帧：通过固定时间间隔来选择帧，通过几何约束和语义信息约束进行关键帧筛选。具体是从当前帧中随机选择一部分像素，从中提取对应的高斯分布，将采样的高斯分布投影到当前帧的相机视角，形成投影的高斯分布，最后通过计算高斯分布在关键帧上的几何重叠率判断当前帧是否选定为关键帧

  

  43610acc5be0c7ccc03bf5642fe24cd8.png

  • 评价指标：常用指标包含峰值信噪比（PSNR）、结构相似性（SSIM）、感知相似度（LPIPS）、深度 L1 误差、绝对轨迹误差（ATE）等，分别从图像质量、深度误差估计和位姿估计精度等方面对算法进行评估。

3. 路径规划

考虑到农业作业环境普遍存在半结构化布局、动态变化频繁、路径实时性与鲁棒性要求较高等特点，综合对比当前应用广泛的改进型 Astar、RRT-star、蚁群优化与粒子群优化等方法，选定双向RRT算法作为改进基础。

• 传统RRT算法：

2b1bb5b7181d63a87b6801f5bb46ef38.png

3e6785269528ec60624f69fb8b6ed621.png

• 双向RRT算法：通过双向扩展两棵随机树，分别向最近邻节点扩展生成新节点，并不断尝试连接两棵树，最终形成一条可行路径。

3f0fc4fae4047fb36166dc55af0693f5.png

0ca6525ef4ddca35ff2e0853269d98b4.png

• 动态步长优化：引入基于贪心策略的动态步长调整机制，根据当前节点与目标节点之间的环境距离动态调整扩展步长，以提升双向RRT算法路径搜索效率和适应性，随着距离变化参数范围[0.2-1.0]，并根据障碍物的出现和消失动态调整距离。

46ed74989f9cb0de514f065e9a4fe010.png

• 采样点方向优化：在每次迭代采样过程中，生成两个候选采样点，比较它们到目标点的距离，选取距离更近的那个 作为新节点。

db6aca897f13e30f4444b641f5c5fc44.png

• 动-静态语义等级信息优化：根据实时感知的障碍物状态动态评估环境变化，对静态障碍物保持避让规划，对半静态障碍物则根据其运动状态动态调整路径
• 删除路径冗余点优化：基于连通性检测，直接连接路径中相邻且可通行的节点，从而跳过中间冗余点，形成更简洁、顺畅的路径
• B 样条平滑路径优化： 从路径的起点开始，以设定的固定间隔依次选取路径中的节点作为控制点，直至遍历整个路径，并在最后补充终点节点作为控制点，构造一条平滑连续的 B 样条曲线，使得路径的一阶、二阶导数均连续

06718a7eb3739d395cb6a06bacabaa24.png