基于搜索的 L-Shape 拟合算法原理详解（第一次写，大家多多包涵#

传统算法的局限性#

如果直接使用 PCA（主成分分析） 来计算方向，由于 PCA 寻找的是点云分布方差最大的方向，对于一个标准的 “L” 形点云，PCA 计算出的主轴往往是 45° 对角线方向。

这会导致机器人误判物体的朝向（Yaw），从而在避障或抓取时产生严重误差。L-Shape 算法的核心目的，就是强制假设物体是矩形的，并找到最贴合这个“L”形边缘的矩形框。

算法步骤#

1. 旋转遍历： 将点云中的所有点 $(x, y)$ 绕原点逆时针旋转一个角度 $\theta$。

   $$\begin{bmatrix} x' \\ y' \end{bmatrix} = \begin{bmatrix} \cos\theta & \sin\theta \\ -\sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix}$$

   我们让 $\theta$ 从 $0^\circ$ 逐步增加到 $90^\circ$（例如步长为 $1^\circ$ 或 $2^\circ$）。

2. 计算包围盒 (AABB)： 在每一个 $\theta$ 下，计算旋转后点云的边界：

   • $x'_{min}, x'_{max}$
   • $y'_{min}, y'_{max}$

3. 计算面积： 计算当前角度下的矩形面积： $Area(\theta) = (x'_{max} - x'_{min}) \times (y'_{max} - y'_{min})$

4. 寻找最优解： 记录下让 $Area(\theta)$ 最小的那个角度 $\theta_{best}$。这个角度即为物体边缘最“正”的朝向。

为什么面积最小是对的？#

当矩形框的边没有与物体边缘平行时，框的四个角会有大量“空隙”，导致包围盒面积较大。只有当矩形框完美贴合点云边缘（即 $x_{min}$ 等边界紧贴点云）时，围出来的面积才是最小的。

补偿步骤#

1. 确定锚点： 在拟合出的矩形框的 4 个顶点中，计算它们到雷达原点 $(0,0)$ 的距离。距离最近的那个顶点，即为锚点。物理上，这通常对应物体“L”形的拐角点。

2. 推算中心： 基于锚点坐标 $(x_{anchor}, y_{anchor})$，向矩形内部延伸。

   • 如果已知物体尺寸（如已知标准集装箱尺寸）：从锚点沿长宽方向分别移动 $Length/2$ 和 $Width/2$。
   • 如果尺寸未知：通常假设当前扫描到的 $x'_{max} - x'_{min}$ 和 $y'_{max} - y'_{min}$ 就是物体的近似长宽，直接取当前包围盒的几何中心作为物体中心（虽然有误差，但保证了包围盒覆盖了可见部分）。