2026年了，FPGA工程师做激光雷达点云处理，点云配准ICP算法在Zynq上跑实时，有什么加速思路？

最近在做一个基于Zynq的激光雷达点云配准项目，老板要求实时性，ICP算法迭代次数一多就卡顿。目前只用了PS端的ARM核做浮点运算，PL端基本闲置。想问问各位大佬，ICP里最耗时的最近邻搜索能不能用PL端的并行查找表或者硬件加速来实现？另外点云降采样用体素滤波在FPGA上怎么高效实现？有没有现成的开源IP核可以用？求实战经验，最好能给出具体的模块划分和资源占用情况。