显式多边形足迹+SDF实时碰撞检测方法

1. 项目概述这不是“画个圈就完事”的碰撞检测“基于显式多边形足迹的符号距离场实时碰撞评估方法”——光看这个标题很多人第一反应是又一个论文味儿十足的学术黑话。但如果你正在做机器人导航、游戏物理引擎、工业数字孪生仿真或者哪怕只是调试一台扫地机器人在复杂家具间隙中是否真能“不撞墙”那这个标题背后藏着的就是你每天被卡住、被报错、被客户追问“为什么它明明看着有缝却停住了”的那个核心痛点。我干这行十多年从最早用AABB包围盒硬扛简单场景到后来堆GPU算力跑隐式SDF符号距离场再到如今亲手把这套“显式多边形足迹SDF”的方案落地进三个不同行业的实际产线系统里最深的体会是碰撞评估从来不是“有没有撞”而是“在什么精度、什么延迟、什么资源消耗下判断出它即将怎么撞”。这个标题里的每个词都不是装饰而是对现实约束的精准回应。“显式多边形足迹”意味着我们不再依赖模型网格的三角面片去近似脚底接触面而是直接定义一个可编辑、可参数化、带语义的2D多边形——比如AGV小车底盘的真实投影轮廓或是机械臂末端执行器的抓取包络“符号距离场”不是拿来渲染的是作为一张轻量级、可查表、支持梯度计算的“空间势能地图”而“实时”二字是硬指标在嵌入式ARM Cortex-A72上单次全区域评估必须压在8毫秒内否则整个控制环路就断了。它解决的是传统方法在精度、速度、鲁棒性三者间反复撕扯的死结。用包围盒精度太糙窄缝过不去用网格碰撞CPU算不动尤其当场景里有上百个动态物体时用深度学习预测泛化性差换一个货架布局就得重训。而这个方法本质上是在“几何严谨性”和“计算友好性”之间用数学工具打了一个非常结实的补丁。它适合谁不是写综述的研究生而是手上有真实机器人、真实产线、真实交付 deadline 的工程师不是追求理论创新的教授而是需要今天下午就把避障逻辑调通、让设备能稳定跑满8小时的现场调试员。接下来我会像当年带新人一样把这套方法从设计思路、核心细节、实操步骤到踩过的坑掰开揉碎讲清楚。没有废话全是能抄、能改、能立刻上机验证的干货。2. 整体设计思路与方案选型逻辑2.1 为什么放弃“隐式SDF”坚持“显式多边形足迹”这是整个方案的起点也是最容易被误解的地方。很多团队一上来就想用神经辐射场NeRF或深度学习生成SDF觉得“新”就是“好”。我试过也推过结果很明确在工业实时控制场景下这条路是条死胡同。原因很简单——不可控的推理延迟和不可解释的误差边界。一个训练好的网络在标准测试集上IOU交并比99.5%但遇到产线上一块反光的不锈钢板输入图像稍微过曝输出的SDF零等值面就可能整体偏移3厘米。而你的AGV就卡在离货架0.5厘米的地方死活不动。所以我们回归几何本质机器人的“脚”是什么不是它3D模型里某个三角面片而是它与地面接触的那个二维区域。这个区域我们称之为“足迹”Footprint。它必须是“显式”的意味着它是一组明确定义的顶点坐标比如[(0.3, -0.2), (0.3, 0.2), (-0.3, 0.2), (-0.3, -0.2)]代表一个0.6m×0.4m的矩形底盘它可以被程序实时修改比如当AGV抬起前轮进行越障时足迹自动收缩为后轮两点连线它的拓扑结构是已知且稳定的永远是简单多边形无自交、无孔洞这为后续所有计算提供了确定性基础。提示显式足迹的另一个巨大优势是“可审计性”。当客户问“为什么它判定这里不能过”你可以直接把足迹多边形叠在激光雷达点云图上指着两个顶点说“看这里点云密度突降算法据此判断前方是玻璃门足迹在此处的最小安全距离不足故触发停障。” 这种解释能力在B端交付中价值千金。2.2 为什么SDF是“符号距离场”而不是“距离场”这是第二个关键认知分水岭。很多初学者会混淆“Distance Field”和“Signed Distance Field”。前者只告诉你“到最近障碍物有多远”后者则多了一个至关重要的信息“你在障碍物的哪一边”。想象一下扫地机器人。它需要知道前方10cm处有一堵墙 → 距离是10cmDistance Field就能给但它更需要知道自己此刻是在墙的“外侧”还是已经穿模进去了如果是后者说明定位系统已失效必须紧急制动。这个“内外”判断就是符号Sign的价值。SDF的数学定义是SDF(p) d(p, ∂Ω) × sign(p ∈ Ω)其中d是欧氏距离sign是符号函数内部为负外部为正边界为零。这个“符号”不是锦上添花而是安全兜底的基石。我们在产线部署时所有运动规划器的约束条件都直接写成SDF(p) 0.05要求机器人中心点始终在障碍物外5cm以上而不是distance(p, obstacle) 0.05。后者在机器人恰好位于障碍物表面时会因为浮点精度问题产生不确定的比较结果而前者只要SDF值大于零就铁定是安全的。2.3 为什么是“实时评估”而不是“实时生成”这是方案落地成败的临门一脚。很多论文吹嘘“实时SDF生成”指的是每秒能生成几帧SDF纹理。但这对我们毫无意义。我们的需求是给定一个当前机器人位姿x, y, θ和一个预定义的足迹多边形系统必须在10ms内返回该足迹在当前位姿下与整个环境SDF地图的最小距离值及其位置。这是一个“查询”Query问题不是“构建”Construction问题。因此整个架构被清晰地切成两层离线层Offline在产线部署前用高精度激光扫描仪获取静态环境点云通过泊松重建生成高质量、低噪声的网格再用快速多边形栅格化Rasterization算法将这个网格“烘培”Bake成一张固定分辨率如1024×1024、固定范围如50m×50m的SDF纹理贴图。这个过程可以耗时几分钟甚至几小时没关系。在线层Online运行时机器人只做一件事——将自身足迹多边形根据当前位姿平移旋转变换到SDF地图的坐标系中然后在这个变换后的多边形区域内高效地采样并计算其到SDF零等值面的最小有符号距离。这种“离线烘焙在线查询”的范式是我们能在树莓派4B上跑通的核心。它把最耗时的几何计算全部转移到了部署前把最苛刻的实时性要求压缩到了一个高度优化的、内存局部性极佳的查表插值操作上。2.4 方案对比为什么它比传统方法更“稳”我们不是为了创新而创新。下面这张表是我在三个不同客户现场用同一套硬件NVIDIA Jetson Xavier NX实测对比的结果。数据不是实验室理想值而是连续72小时压力测试下的P9595%分位延迟方法平均延迟 (ms)P95延迟 (ms)最小安全距离误差 (cm)静态场景建模时间动态物体支持AABB包围盒0.81.2±8.51s仅支持轴对齐移动网格碰撞 (Bullet)12.428.7±0.35min完整支持但CPU占用率90%深度学习SDF预测6.115.3±2.1分布不均训练需2天仅支持训练集内模式本文方法显式足迹SDF3.25.8±0.1518min通过增量更新SDF纹理支持看到没它的延迟不是最低的但它是唯一一个在“低延迟”、“高精度”、“低资源占用”、“强鲁棒性”四个维度上全部达到工业级交付红线的方法。特别是“最小安全距离误差”这一项±0.15cm意味着当系统报告“前方0.5cm有障碍”那么真实距离就在0.35cm到0.65cm之间这个确定性是其他任何方法都无法提供的。这也是为什么我们最终把它定为公司所有新机型的默认避障方案。3. 核心细节解析与实操要点3.1 显式足迹的定义、参数化与动态更新显式足迹不是画个静态图片就完事了。它必须是一个可编程、可感知、可响应的“活”对象。我们采用了一种分层参数化设计基础层Base Footprint由用户在配置文件中直接定义的顶点序列。例如一个叉车的足迹可能是base_footprint: vertices: - [0.0, -0.4] # 后轴中心点 - [0.0, 0.4] # 后轴中心点 - [1.2, 0.4] # 前轮中心点 - [1.2, -0.4] # 前轮中心点这个四边形精确描述了车辆静止时的投影轮廓。姿态层Pose Layer在运行时根据机器人当前的x, y, θ位姿对基础顶点进行刚体变换。公式很简单x x cos(θ)*vx - sin(θ)*vy y y sin(θ)*vx cos(θ)*vy其中(vx, vy)是基础顶点坐标。这一步必须用SIMD指令如ARM NEON向量化实现4个顶点一次算完耗时0.1ms。状态层State Layer这是体现“智能”的地方。足迹会根据机器人状态动态变形。例如当叉车升起货叉时系统检测到液压缸压力传感器读数阈值自动将足迹的“前部”顶点沿X轴正向收缩0.3m模拟货叉悬空后底盘实际支撑面的缩小当AGV进入“窄巷模式”时通过ROS Topic接收指令将足迹整体按比例缩放至80%以适应更紧凑的通行空间。实操心得足迹顶点数不宜过多。我们做过测试超过8个顶点后后续的SDF采样计算复杂度呈平方级增长而精度收益几乎为零。绝大多数工业车辆4-6个顶点足矣。一个常见的错误是把轮胎的圆弧也用几十个点去拟合——完全没必要。用一个“外接矩形圆角半径”参数来描述轮胎既准确又高效。3.2 SDF地图的生成从点云到可查表纹理SDF地图的质量直接决定了整个系统的上限。我们摒弃了所有“端到端”的黑箱生成器坚持一套可控、可调、可复现的流水线点云去噪与配准使用PCLPoint Cloud Library的StatisticalOutlierRemoval滤波器剔除离群噪点再用IterativeClosestPointICP算法将多站扫描的点云精确配准到统一坐标系。这一步的关键参数是setMaximumIterations(50)和setRANSACOutlierRejectionThreshold(0.02)前者保证收敛后者防止误匹配。泊松曲面重建这是最关键的一步。我们不用默认参数而是根据点云密度动态调整PoissonReconstructionpcl::PointNormal poisson; poisson.setDepth(10); // 深度越大细节越多但内存爆炸 poisson.setSolverDivide(8); // 分治求解平衡速度与内存 // 核心根据平均点间距avg_spacing自适应设置 float avg_spacing computeAverageSpacing(cloud); poisson.setSamplesPerNode(3.0f * avg_spacing); // 经验公式3倍间距最稳SDF纹理烘焙将重建出的网格栅格化到一个二维平面Z0生成SDF。我们不生成3D SDF那会是GB级内存而是生成一个“切片”——即XY平面上的2D SDF因为所有足迹都在这个平面内运动。烘焙算法采用改进的“跳像素”Jump Flood Algorithm, JFA初始化对每个栅格若其在网格投影内则SDF0否则设为一个极大值如1e6多轮传播每轮每个像素向距离为2^k的邻居像素“跳跃”取最小距离值符号标记最后用一个单独的“种子填充”Flood Fill算法标记每个像素属于“内部”还是“外部”从而赋予符号。最终输出的是一张uint16_t类型的灰度图其中像素值v代表距离单位mm最高位bit15代表符号0外部1内部。这样一个像素就同时编码了距离和符号查表时只需一次内存访问。3.3 实时碰撞评估从足迹到最小距离的高效计算这才是真正的“心脏”。给定变换后的足迹多边形P和SDF纹理T如何在毫秒内找到min{SDF(p) | p ∈ P}我们采用了一种混合策略兼顾精度与速度粗筛Coarse Culling首先计算足迹P的轴对齐包围盒AABB。如果这个AABB完全落在SDF纹理之外直接返回一个极大安全距离如10000mm。这一步用整数运算耗时0.01ms。栅格采样Grid Sampling将AABB区域按SDF纹理的分辨率划分为N×N个栅格我们固定N8。对每个栅格中心点双线性插值查询SDF值。记录其中的最小值d_min_grid及其位置p_min_grid。这一步是主干耗时约2.5ms。局部精搜Local Refinement以p_min_grid为中心构造一个3×3的小窗口在这个窗口内用更高密度如16×16的亚像素采样并应用简单的梯度下降法沿着SDF的负梯度方向即朝向障碍物微调寻找真正的局部最小值。梯度通过中心差分法计算∇SDF(x,y) ≈ [ (SDF(xdx,y) - SDF(x-dx,y)) / (2*dx), (SDF(x,ydy) - SDF(x,y-dy)) / (2*dy) ]这一步将精度从栅格级~1cm提升到亚毫米级~0.1mm耗时0.5ms。整个流程下来单次评估稳定在3.2ms左右完全满足实时性要求。而且由于所有操作都是内存局部的集中在一小块纹理缓存上CPU缓存命中率极高性能非常稳定。3.4 安全距离的工程化解读与阈值设定SDF返回的是一个数学上的“有符号距离”但工程师要的是“能不能走”。这就涉及到关键的工程转化名义安全距离Nominal Clearance这是系统对外宣称的最小允许距离比如50mm。它不是一个拍脑袋的数而是由三部分叠加而成机械公差机器人本体制造、装配的累积误差实测为±3mm定位误差激光SLAM在当前环境下的长期漂移实测P95为±8mmSDF建模误差由点云噪声和泊松重建引入实测为±2mm。所以名义安全距离 3 8 2 55mm是留给算法余量的安全裕度 18mm。但我们不会直接用18mm而是将其翻倍设为35mm作为软件中的硬性阈值。为什么翻倍因为这是给“最坏情况”留的缓冲。当SDF值降到35mm以下时系统会进入“预警”状态降低速度降到18mm以下才真正触发急停。动态安全距离Dynamic Clearance在高速运动时仅靠静态距离不够。我们引入了一个速度耦合因子dynamic_clearance nominal_clearance k_v * v^2其中v是当前线速度m/sk_v是经验系数我们取0.5。这意味着当机器人以1m/s行驶时安全距离自动增加0.5m以2m/s时增加2m。这个公式直接来源于经典动力学中的制动距离模型确保了在任何速度下系统都有足够的物理空间来完成安全停车。注意所有这些阈值都必须在客户现场用真实的障碍物如不同材质、不同反光度的墙面、货架、柱子进行“边界测试”。我们有一个标准测试流程让机器人以不同速度从不同角度逼近一个已知尺寸的障碍物记录每次触发预警和急停的实际距离。只有当95%以上的测试点都落在理论安全距离的±10%范围内才算达标。4. 实操过程与核心环节实现4.1 环境建模全流程从激光扫描到SDF纹理生成现在让我们把前面讲的理论变成你电脑上可执行的命令。整个流程我们封装成了一个名为footprint_sdf_builder的Python工具包开源在公司内网GitLab上。以下是完整步骤第一步采集点云使用一台Velodyne VLP-16激光雷达架设在三脚架上围绕目标区域如一个50m×50m的仓库进行360°慢速旋转扫描。关键要点扫描速度必须10rpm否则点云会拉花每站扫描时间≥60秒保证点云密度在区域四角各放置一个已知尺寸如30cm×30cm的黑白棋盘格标定板用于后续配准精度验证。第二步点云预处理# 进入工作目录 cd /path/to/scan_data # 使用PCL工具链进行去噪和配准 pcl_outlier_removal -input scan_001.pcd -output clean_001.pcd -method statistical -mean_k 50 -std_dev_mul 1.0 pcl_icp -target clean_001.pcd -input clean_002.pcd -output aligned_002.pcd -max_iter 50 -max_correspondence_distance 0.02 # 将所有配准后的点云合并 pcl_concatenate_points_pcd -input clean_001.pcd aligned_002.pcd ... -output merged.pcd第三步泊松重建# 使用Open3D Python API import open3d as o3d pcd o3d.io.read_point_cloud(merged.pcd) # 估计法向量为泊松重建做准备 pcd.estimate_normals(search_paramo3d.geometry.KDTreeSearchParamHybrid(radius0.1, max_nn30)) # 泊松重建关键参数已根据点云密度自适应 mesh, densities o3d.geometry.TriangleMesh.create_from_point_cloud_poisson( pcd, depth10, width0, scale1.1, linear_fitFalse ) o3d.io.write_triangle_mesh(reconstructed.ply, mesh)第四步SDF纹理烘焙这是我们自己开发的C核心库lib_sdf_baker。编译后命令行调用# 参数说明 # -i: 输入PLY网格文件 # -o: 输出SDF纹理文件PNG格式16位灰度 # -r: 分辨率1024 # -b: 地图范围50.0, 50.0单位米 # -o: 原点偏移-25.0, -25.0使地图原点在左下角 ./sdf_baker -i reconstructed.ply -o map_sdf.png -r 1024 -b 50.0 50.0 -o -25.0 -25.0执行完成后你会得到一张map_sdf.png。用图像查看器打开它看起来就是一张普通的灰度图。但用代码读取你会发现像素值从0到32767对应距离0mm到32767mm32.767m而最高位32768被用来存储符号。这就是我们实时查询的“地图”。4.2 机器人端集成ROS节点开发与部署我们所有的机器人都基于ROS 2 Foxy。核心节点名为footprint_collision_node。它的架构非常清晰订阅Subscribers/tf: 获取机器人底盘base_link相对于地图map的实时位姿/scan: 可选订阅激光雷达数据用于动态更新SDF见4.3节/footprint_config: 订阅动态足迹配置支持运行时热更新。发布Publishers/collision_state: 发布一个自定义消息CollisionState包含min_distancefloat32当前最小有符号距离mmis_safebool是否处于安全状态warning_leveluint80安全1预警2急停closest_pointgeometry_msgs/Point距离最近点在地图坐标系下的位置。核心循环Main Loopvoid CollisionNode::mainLoop() { while (rclcpp::ok()) { // 1. 获取最新位姿 auto pose getLatestTransform(map, base_link); if (!pose) continue; // 2. 获取当前足迹已根据状态层动态更新 auto footprint getCurrentFootprint(); // 3. 将足迹顶点变换到地图坐标系 std::vectorEigen::Vector2f transformed_vertices; for (const auto v : footprint.vertices) { Eigen::Vector2f p(v.x, v.y); p rotate(p, pose-theta) Eigen::Vector2f(pose-x, pose-y); transformed_vertices.push_back(p); } // 4. 执行实时碰撞评估调用C核心库 CollisionResult result sdf_evaluator_.evaluate(transformed_vertices); // 5. 发布结果 publishCollisionState(result); // 6. 控制频率固定100Hz rate.sleep(); } }这个节点编译后在Jetson Xavier NX上CPU占用率稳定在12%-15%内存占用64MB完全不影响其他导航、视觉等任务。4.3 动态SDF更新应对移动障碍物的增量策略静态SDF地图无法应对人、叉车等移动障碍物。我们的解决方案不是抛弃SDF而是给它加上“动态补丁”。原理很简单我们维护一个独立的、小范围的“动态SDF图层”Dynamic Layer其分辨率更高如2048×2048但覆盖范围极小如5m×5m以机器人当前位置为中心。这个图层只由激光雷达的实时扫描数据驱动。数据源订阅/scan话题将极坐标扫描数据转换为笛卡尔坐标系下的点云片段Point Cloud Patch更新方式对这个点云片段运行一个极简版的、单帧的“泊松重建”实际上我们用更轻量的“体素占据栅格”Voxel Occupancy Grid配合距离变换Distance Transform算法生成一个局部SDF融合策略在实时评估时先查询全局静态SDF再查询局部动态SDF取两者中的较小值即更危险的那个作为最终结果。这个策略的好处是动态更新的计算量被严格限制在一个很小的局部区域不会拖垮整个系统。我们实测在持续有人员走动的仓库环境中动态图层的更新耗时稳定在1.8ms以内而全局SDF查询仍是3.2ms总延迟5ms。4.4 性能调优与资源占用实测在Jetson Xavier NX上我们对整个系统进行了详尽的压力测试。以下是关键指标的实测数据连续运行72小时每秒采样100次模块CPU占用率 (%)GPU占用率 (%)内存占用 (MB)延迟 (ms)延迟抖动 (ms)SDF纹理加载0.20.112.40.050.001位姿变换4顶点0.80.00.10.080.002栅格采样8×83.50.00.52.450.03局部精搜16×161.20.00.30.420.01总计单次评估5.70.113.33.150.04可以看到CPU是主要瓶颈GPU几乎没用上。这是因为我们的算法是典型的“内存带宽受限型”Memory-Bound而非“计算密集型”Compute-Bound。因此所有优化都围绕着“减少内存访问次数”和“提高缓存命中率”展开纹理压缩SDF纹理采用ETC2格式压缩体积从16MB降至4MB加载更快缓存更友好预取Prefetch在查询前主动用__builtin_prefetch()指令将即将访问的纹理内存块预取到L2缓存SIMD向量化所有顶点变换和插值计算都用NEON指令重写4个点并行处理速度提升3.2倍。这些看似琐碎的优化最终把P95延迟从最初的7.8ms压到了现在的5.8ms为整个系统赢得了宝贵的2ms余量。5. 常见问题与排查技巧实录5.1 “明明没障碍系统却频繁报急停”——SDF地图的“幽灵墙”问题这是客户现场反馈最多的问题。现象是机器人在空旷走廊里SDF值突然跌到-50mm表示已穿模触发急停。我们带着设备去现场用激光测距仪一量前方3米内确实空无一物。排查过程首先检查SDF纹理本身。用Python脚本读取map_sdf.png绘制其灰度直方图。发现大量像素值集中在0附近且有明显的“双峰”——一个在0边界一个在-1000深色区域。这说明地图里存在一大片“内部”区域也就是“幽灵墙”。追溯源头发现是建模时激光雷达在扫描一面大面积玻璃幕墙时大部分激光束直接穿透只在边缘反射回来导致点云在玻璃位置出现一个稀疏的、不闭合的轮廓。泊松重建算法面对这种“开口”的轮廓会强行“脑补”一个封闭的表面把玻璃后面的空间错误地标记为“内部”。解决方案建模阶段在扫描前用黑色胶带将所有玻璃门窗的边缘贴出一个清晰、连续的轮廓线。这样激光雷达就能稳定地捕捉到这个“人工边界”泊松重建就有了可靠的输入。运行时在SDF烘焙阶段加入一个“空洞填充”Hole Filling后处理。我们用OpenCV的cv2.floodFill算法以地图四个角为种子点将所有能到达的“外部”区域强制标记为正距离。这一步能消除99%的幽灵墙。实操心得不要迷信“全自动”。在工业现场适当的“人工干预”往往是成本最低、效果最好的方案。贴几卷胶带比重跑一周的建模流程划算多了。5.2 “机器人在斜坡上SDF评估失准”——Z轴忽略带来的俯仰误差这个问题出现在一个有坡道的汽车工厂。机器人在上坡时足迹多边形是平的但实际底盘是倾斜的导致足迹与地面的“真实接触面”发生了偏移。根本原因我们的SDF地图是XY平面的2D切片完全忽略了Z轴变化。当机器人俯仰角Pitch5°时足迹在XY平面上的投影与其实际支撑面的投影会产生显著偏差。解决方案我们没有升级到3D SDF那会带来指数级的计算开销而是采用了一个巧妙的“Z轴补偿”模型机器人IMU提供实时俯仰角θ_p我们预先测量并标定了机器人底盘的几何参数轮距L、质心高度h计算出由于俯仰引起的足迹在X方向上的等效偏移量Δx h * tan(θ_p)在位姿变换时将这个Δx作为一个额外的平移量加到足迹顶点的X坐标上。这个一阶补偿模型在θ_p 15°的范围内将SDF评估误差从±12cm降低到了±0.8cm完全满足要求。它证明了有时候一个简单的物理模型比一个复杂的数学框架更有效。5.3 “多台机器人互相干扰SDF值乱跳”——动态图层的冲突问题在AGV集群调度场景下当多台机器人靠得很近时它们各自的动态SDF图层会相互重叠、覆盖导致评估结果混乱。问题根源所有机器人都在往同一个“以自身为中心”的动态图层里写入数据。A机器人的激光点被B机器人的动态图层当成障碍物反之亦然。终极解法我们放弃了“每台机器人一个动态图层”的思路改为中央动态地图服务Central Dynamic Map Server。所有机器人的/scan数据都发布到一个中央ROS 2 Topic/central_scan一个独立的central_dynamic_map_node订阅这个Topic用一个全局的、高分辨率的动态栅格地图1024×102450m×50m融合所有机器人的扫描数据每台机器人的footprint_collision_node不再自己生成动态图层而是向这个中央服务发起RPC请求获取“以自己为中心”的一个局部动态SDF切片。这个架构将动态更新的计算压力从N台边缘设备集中到了一台服务器上。虽然增加了网络通信但换来的是绝对的一致性和可预测性。在50台AGV的实测中系统稳定性从82%提升到了99.97%。5.4 “SDF值在边界处抖动导致机器人‘抽风’”——浮点精度与插值的陷阱这是一个非常隐蔽但杀伤力巨大的问题。现象是机器人在距离墙壁正好10cm的地方SDF值在10.1,-9.9,10.05,-9.95之间来回跳变导致控制指令在“前进”和“急停”之间疯狂切换。技术分析SDF纹理是uint16_t最大值32767对应32.767m。那么1mm的距离就对应约1个像素值。双线性插值在纹理边缘即SDF零等值面附近时会因为相邻像素的符号不同一个正一个负导致插值结果在零附近剧烈震荡。更糟的是浮点运算的舍入误差会让这个震荡无法消除。根治方案