做激光感知的同学，应该都遇到过这样一幕：聚类出一堆点云，下一步却犯了难——目标到底该用什么几何形状传给下游？是直接甩五千个点，还是用个矩形框？我自己实践下来，最顺手的组合是：先用凸包把目标的最小凸多边形算出来，再基于凸包解航向角。这套方案基本不依赖点云的内部分布，轮廓一出来，后续面积、中心、朝向就都有了。今天这篇文章就是围绕“激光感知（八）：计算目标最小凸多边形及航向角”这个主题，把我实际调试过的思路、代码和坑一次性讲透。适合正在做激光雷达感知、目标跟踪或者对点云几何建模感兴趣的工程师参考，也适合刚入门的同学当实操笔记看。
1. 为什么凸多边形和航向角是感知闭环里的硬需求
1.1 从聚类到目标描述：凸包的作用
激光雷达一帧扫下来，经过地面分割、聚类之后，我们手里拿到的通常是若干个点簇。每个点簇代表一个可能的目标，但点簇本身只是一堆无规则的三维坐标，没法直接跟“目标”两个字挂钩。下游的跟踪模块想知道这个目标有多大、中心在哪、对着哪个方向走，而轨迹预测模块更在意目标的轮廓是否贴合真实形状，以便判断它是不是占用了一个更复杂的空间。
这时候如果只用轴对齐包围盒（Axis-Aligned Bounding Box, AABB），会出不少问题。AABB没有旋转自由度，车辆稍微斜着停，框出来就带着一大片空角，面积虚高，中心点也会偏移。反过来，如果用一个固定的旋转矩形框，又很难贴合行人之类的非规则外形，行人张开手臂和放下手臂，同一个矩形框可能完全看不出变化。凸包则不同：凸包是包含点集所有点的最小凸多边形，它天然贴合目标的最外侧轮廓，不限定形状，也不会有内部空隙。对一辆斜置的轿车，凸包能包出棱线分明的前后杠轮廓；对行人，凸包也能把手臂舒展开的部分包进来，比矩形框真实得多。
我经常把凸包理解成一个“橡皮筋模型”——把所有点钉在平面上，绷一根橡皮筋从外面箍住它们，松手之后橡皮筋收缩到极点时形成的多边形，就是凸包。这个比喻很形象，也直接点出了凸包的本质：它是和点集形状最贴合的凸外边界。在感知管线上，凸包最直接的用处是给聚类结果做一个轻量、稳定、信息密度高的几何摘要。计算完凸包之后，点云的数量从几百上千骤降到十几个顶点，存储和传输都轻松了，后续的面积、质心、朝向、最小外接矩形等计算也能基于顶点快速完成。
1.2 航向角的用途与定义
航向角（Yaw）是描述目标朝向的关键状态量。在跟踪模块里，卡尔曼滤波的状态向量通常会包含目标位置、速度和航向角，航向角用来预测下一帧的位置；在匹配阶段，航向角也用来计算两个目标之间的朝向距离，作为关联代价的一部分。如果航向角估计得不准，跟踪容易把横向经过的车和同向行驶的车混为一谈，轨迹预测更是会偏到离谱。
航向角一般定义为目标的主轴方向与参考坐标系x轴的夹角，逆时针为正，范围通常在[-π, π]。这里“参考坐标系”可以是车身坐标系，也可以是全局坐标系，具体看下游约定。激光感知模块内部一般先算目标在雷达坐标系里的几何朝向，再通过外参转换到车体或全局坐标。注意，航向角只描述旋转，不包含平移，所以它的计算完全依赖于目标点云的形状分布。
需要特别强调的是，航向角并不等于运动方向。静止车辆可能斜着停，但运动方向是零；运动目标在转弯时，车身朝向和瞬间速度方向也会存在夹角。很多刚入门的同事会习惯性把航向角用相邻帧位移方向代替，这在直线行驶时误差不大，但一旦目标原地画圈或者被遮挡，运动方向会严重失真。更好的做法是：用几何形状给出一个可靠的朝向初值，再用跟踪层的历史信息消除前后二义性，必要时再和运动方向做融合。
2. 最小凸多边形计算：算法选型与实现
2.1 数据预处理：去噪、投影、去重
激光雷达点云不是干净的，尤其是在室外场景，目标附近经常混着地面残留点、树叶边缘点、车牌反光产生的杂点。凸包对最外圈的点非常敏感，一个离群点就能让凸包尖出去一块，导致面积被撑大很多。所以在算凸包之前，我至少会做四步预处理。
第一步是剔除无效点和离群点。有效点需要有有限坐标和合理的强度，NaN、Inf这种必须第一时间清掉。离群点可以用统计滤波：对每个点找它的K近邻，计算邻域平均距离，如果超过了全局均值加若干倍标准差，就判定为离群点。K一般取8～20，倍数取1.0～2.0，具体要看激光线束密度。第二步是地面分离，目标点集不应该包含地面点，否则凸包会被地面平板撑出一条长尾巴。常规做法是高度阈值或者平面拟合，把高程接近局地平面的点全滤掉。第三步是坐标投影。我们计算几何形状时通常会忽略高度信息，直接把点云压到x-y平面，所以物体的垂直倾斜对凸包影响不大。如果雷达安装存在俯仰角，得先把点云转到水平坐标系，再做投影，否则投影出来的轮廓是拉伸的，面积和朝向都会错。第四步是去重和下采样。同一物体被多线雷达反复扫描，会产生大量非常接近的重复点，这些点不会影响凸包顶点位置，但会拖慢排序。体素滤波（Voxel Grid Filter）用一个小立方体把空间划分成格子，每个格子保留重心或中心点坐标，既降密度又保轮廓，对凸包计算非常友好。
预处理做完后，输入到凸包算法里的点集应该只剩Nx2的二维坐标数组。初创项目有时候为了省事直接跳过预处理，结果凸包算出来坑坑洼洼，航向角也随后乱跳。后来我把预处理环节固定成为所有几何计算的前置函数，后面再没出过那种大面积虚包的问题。
2.2 Andrew单调链算法原理与代码
二维凸包的算法不少：Graham Scan、QuickHull、Jarvis March、Andrew单调链。工业上我推荐Andrew单调链，理由很朴素：它实现简单、不容易出边界bug、时间复杂度和Graham一样是O(n log n)，而且对岩石般的密集点云照样跑得动。Andrew算法的核心是两次扫描，先从左到右构造下凸包，再从右到左构造上凸包，最后拼起来得到逆时针排列的完整凸包。
关键的判断逻辑是叉积。对于三个点A、B、C，叉积cross(A, B, C)可以判断C相对于有向线段AB的位置：如果大于0，C在AB左侧；如果小于0，C在AB右侧；等于0则三点共线。构造凸包时，我们维护一个栈，遇到右转的情况（叉积小于等于0）就把栈顶弹出，直到满足左转为止。这里“小于等于0”意味着剔除共线的中间点，只保留凸包顶点。但如果你的下游需要保留轮廓中间点，也可以改成“小于0”，让共线点留在边上，只是顶点数量会变多。
下面是我实际用的一段Python实现，依赖只有NumPy，移植起来很方便：
import numpy as np def cross_2d(o, a, b): return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0]) def convex_hull(points): """ 输入: points, shape (N, 2) 的二维点集 输出: hull, shape (M, 2) 的凸包顶点，逆时针顺序，不含重复首尾点 """ pts = np.unique(points, axis=0) # 去重并排序 if len(pts) < 3: return pts def build(seq): stack = [] for p in seq: while len(stack) >= 2 and cross_2d(stack[-2], stack[-1], p) <= 0: stack.pop() stack.append(tuple(p)) return stack[:-1] # 去掉最后一个重复点 sorted_pts = sorted(pts, key=lambda p: (p[0], p[1])) lower = build(sorted_pts) upper = build(sorted_pts[::-1]) return np.array(lower + upper)这段代码里build函数构造一条链，sorted_pts[::-1]完成了从右到左的扫描。返回值lower[:-1]和upper[:-1]联合起来刚好是一圈凸包顶点，最后一个点和第一个点不重复。调用时传入聚类后投影到二维的目标点集，返回的凸包大概率是十几个顶点，有时候行人只有五六条边。算完之后我习惯把顶点画出来做一遍可视化，用折线把顶点按顺序连起来，颜色和原始点云做对比，很容易看出离群点是否混进了轮廓。
如果项目用的是C++和PCL，可以直接用pcl::ConvexHull类，它内部做了很多边界处理，只要把输入点云指定为一个PointCloud 或者PointCloud ，然后调用reconstruct把凸包顶点写出来。PCL版本之间API稍有差异，编译前建议先查清楚所在版本的函数签名。不过算法原理都是一样的，自己实现一遍Andrew单调链，再去看PCL源码会非常轻松。
2.3 面积和几何中心：后续匹配的几何基元
有了凸包顶点，几何基元就能继续往上层抽象了。凸包面积是衡量目标运动占用空间的重要指标，也是匹配时的尺寸特征之一。计算凸多边形面积最常用的是鞋带公式（Shoelace Formula）：把多边形的顶点按顺序排列，相邻顶点坐标交叉相乘累加，最后取一半。公式简单，但在工程里要小心顶点的顺序和正负号。如果顶点是逆时针排列，面积正；顺时针排列，面积负。所以我统一约定凸包输出逆时针，面积最后取绝对值。
比面积更常用的是几何中心。这里有个容易踩的坑：很多人直接对凸包顶点坐标取平均当作中心点，这在顶点分布不均匀时误差很大。比如一辆车的前保险杠附近点云密集、后保险杠附近稀疏，顶点取平均的中心会往密集侧偏。正确的做法是按多边形重心公式计算：对每个边，用两个端点和原点构成的三角形面积做加权，把所有加权坐标累加后除以总面积。这样一个中心才真正对应目标占用的空间中心，做跟踪的基础位置也好，做传感器融合的空间对齐也好，都更稳。
我参考的参考实现如下：
def polygon_area_and_centroid(vertices): """ 输入: vertices, (M, 2) 的凸包顶点，逆时针顺序 输出: (面积, 几何中心坐标) """ verts = np.asarray(vertices, dtype=np.float64) n = len(verts) if n == 0: return 0.0, (0.0, 0.0) if n == 1: return 0.0, (verts[0, 0], verts[0, 1]) signed_area = 0.0 cx = 0.0 cy = 0.0 for i in range(n): j = (i + 1) % n cross_val = verts[i, 0] * verts[j, 1] - verts[j, 0] * verts[i, 1] signed_area += cross_val cx += (verts[i, 0] + verts[j, 0]) * cross_val cy += (verts[i, 1] + verts[j, 1]) * cross_val signed_area *= 0.5 if abs(signed_area) < 1e-12: # 近似退化多边形，退化为顶点均值 return 0.0, (verts[:, 0].mean(), verts[:, 1].mean()) cx /= 6.0 * signed_area cy /= 6.0 * signed_area return abs(signed_area), (cx, cy)这个函数我在多个项目里直接复用，面积单位是平方米（假设输入点是米），几何中心单位和输入点一致。用它算行人、车辆、甚至路障的几何特征都很稳定。有一点要提醒：若是用PCL算凸包后拿到的顶点顺序可能不保证是逆时针，需要先检查多边形面积的方向，如果面积为负，就逆序一下顶点再计算，否则后面所有几何量都会出错。
3. 航向角估计：最小外接矩形与主方向
3.1 PCA法为何不够稳
航向角最直觉的做法是主成分分析（PCA）：把目标点云所有点放进去，算协方差矩阵，最大特征值对应的特征向量就是主方向，角度直接用atan2解出来。这个方法在点云均匀分布时看起来还行，但真实场景里很不听话。原因在于PCA是对所有采样点做整体统计，它会被内部点的分布密度带着跑。举个典型例子：一辆货车，侧面被近处的线束扫得很密，顶面或者车尾点少，PCA会把主方向往点云密集的扫描线方向偏，而不是车身真正的长轴方向。还有更夸张的，当目标被部分遮挡，只剩下一块矩形的一角时，PCA主方向可能变成对角线方向，偏差非常离谱。
另外一个问题是对称性。细长物体本身有两个特征值接近的轴，比如一个近似正方形的箱子，两个特征值几乎相等，最大特征值方向会非常不稳定，帧与帧之间可能跳变到垂直方向。虽然算法没有错，但下游跟踪会很痛。
我后来在项目里放弃直接对原始点云做PCA，改成先计算凸包，再在凸包上做最小外接矩形（Minimum Area Rectangle）。严格说，这仍然是一种PCA式的投影，但它只针对凸包边界，不会被内部点分布干扰。最小外接矩形的长边方向作为航向角，在轮廓轮廓明显时非常稳定，而且天然贴合车辆、行人的外形。
3.2 最小外接矩形算法推导与代码
最小外接矩形，也叫最小面积矩形或OBB（Oriented Bounding Box），它的定义是：在所有能够包围凸包的矩形中，选取面积最小的那个。计算这个矩形的理论依据是旋转卡壳（Rotating Calipers）：最小外接矩形必然至少有一条边与凸包的一条边重合。所以算法可以简化成枚举凸包每条边，以该边方向为矩形的一条边方向，把凸包顶点投影到这条边和它的法线方向上，计算出宽度和高度，乘积就是面积，最后取面积最小的组合。
对凸包顶点数量很少的场景，枚举法的效率完全够用。凸包边长M通常在5~20之间，枚举所有边，每边对全部凸包顶点投影排序求极值，计算量是O(M^2)，在几十个顶点的情况下几乎可以忽略。如果凸包特别大，想优化成真正的旋转卡壳双指针法，可以后续再升级，但多数感知场景用不着。
下面是实现示例：
def min_area_rect(hull): """ 输入: hull, (M, 2) 凸包顶点，逆时针 输出: (中心, (宽, 高), 航向角) 航向角是长边方向与x轴夹角，范围 [-pi/2, pi/2) """ hull = np.asarray(hull, dtype=np.float64) n = len(hull) if n < 3: return None best_area = float('inf') best_center = None best_length = None # 长边长度 best_width = None # 短边长度 best_yaw = 0.0 for i in range(n): p1 = hull[i] p2 = hull[(i + 1) % n] dx = p2[0] - p1[0] dy = p2[1] - p1[1] edge_len = np.hypot(dx, dy) if edge_len < 1e-12: continue # 边的单位向量u，和法向量v ux, uy = dx / edge_len, dy / edge_len vx, vy = -uy, ux # 投影坐标 proj_u = hull[:, 0] * ux + hull[:, 1] * uy proj_v = hull[:, 0] * vx + hull[:, 1] * vy u_min, u_max = proj_u.min(), proj_u.max() v_min, v_max = proj_v.min(), proj_v.max() w = u_max - u_min h = v_max - v_min area = w * h if area < best_area: best_area = area # 矩形中心在u-v坐标系下的坐标 u_center = (u_min + u_max) / 2.0 v_center = (v_min + v_max) / 2.0 # 转回原始xy坐标 center_x = u_center * ux + v_center * vx center_y = u_center * uy + v_center * vy best_center = (center_x, center_y) best_length = max(w, h) best_width = min(w, h) # 航向角取长边方向 if w >= h: best_yaw = np.arctan2(uy, ux) else: best_yaw = np.arctan2(vy, vx) return best_center, (best_length, best_width), best_yaw注意这段代码返回的航向角范围是[-π/2, π/2)，因为长边方向与短边方向垂直，如果不作处理，角度会跳变。实际工程里我更习惯让航向角保持连续，同时再结合跟踪速度方向来消除二义性，也就是把角度放宽到[-π, π]，并决定车头朝前还是朝后。做法是：先算出目标运动速度方向vel_yaw = atan2(vy, vx)，然后比较best_yaw和vel_yaw的夹角，如果夹角大于90度，就把best_yaw加π并归一化到[-π, π]。这样输出给跟踪模块的航向角就带方向信息了。
3.3 航向角后处理：二义性去除与平滑
很多情况下航向角的“二义性”才是真正的问题，而不是算法本身。一辆车在雷达点云里长边方向可能有180度翻转，反映在矩形上就是车头朝左还是朝右并不容易区分。几何上最小外接矩形长边和短边都通过旋转能对齐，但只能给出一个范围，不能给出绝对朝向。解决思路通常有两个来源：上一帧的历史航向，以及当前帧的速度方向。如果上一帧估计的航向是120度，这一帧候选角是-60度和120度，那么直接选与120度更接近的那个；如果目标静止，历史航向可能没有参考价值，那就结合速度方向或者曼哈顿匹配中的预定义类别（比如行人前后差别小，可以接受任意朝向）。
角度平滑也很必要。直接输出原始计算值，帧与帧之间可能因为边界点扰动产生几度的抖动，尤其目标距离远、点云稀疏时，凸包顶点会在几条边之间切换，航向角会来回跳。我一般会在跟踪模块里加一个低通滤波：当前帧航向角先和上一帧做差值，把差值归一化到[-π, π]，然后用0.3 * diff进行平滑更新。这样既不会割裂角度连续性，也不至于反应太过迟钝。在目标高速变道的时候，0.3的系数依然能跟上真实朝向变化，我调到这个参数后跟踪匹配的成功率明显提升。
还有一点，静止目标的航向角天然不可观。如果一个目标在原地停了好几秒，它的点云形状不变，航向角单独靠几何算出来可能会因为噪声左右漂移。我的经验是：静止目标的航向角直接用滤波器冻结住，不更新，或者锁到与该目标关联轨迹的运动方向一致。不然跟踪输出里会出现一辆静止车原地转头的幻觉，下游决策会很困惑。
4. 工程落地中的经典坑与排查清单
4.1 凸包退化和极端情况
凸包算法在数据干净时很省心，但真实数据总有边界情况。最常见的退化是目标点云只有一两个点，比如车辆跑到80米外，雷达只有寥寥几根线扫中了后尾灯附近。此时凸包函数应返回空或者原始点本身，而不是去强算多边形。我在convex_hull里专门加了点数判断，小于3直接返回，由上层决定是否放弃这次几何估计。
另一个坑是大量共线点。直墙、道路护栏这类目标，点云几乎排成一条直线，凸包会退化为二边形或接近零面积的多边形，计算几何中心也失去意义。处理办法是增加最小面积和最小点数校验，面积小于某个阈值（比如0.3平方米）就不生成航向角，只保留位置。护栏这种目标真正有意义的朝向是墙面的法向，但感知系统通常不会给它做完整跟踪，所以用阈值把它滤掉也不影响整体效果。
去重也是容易被忽略的点。如果不做np.unique，同一个点被重复计算，排序后可能出现栈里连续压入两个几乎相同坐标的点，叉积判断时由于浮点误差产生错误符号，导致凸包顶点突然多出一个靠近拐角的毛刺。我在多个项目里遇到过类似情况，加了去重和浮点容差检查后，凸包边界干净多了。具体容差可以按激光雷达的分辨率定，比如1厘米以内视为重复点。
4.2 航向角跳变与静止目标
航向角跳变是排查频率最高的问题。我整理过一个简易排查表格，遇到异常直接从表里找原因：
| 现象 | 可能原因 | 处理办法 |
|---|---|---|
| 航向角在±90度来回跳 | 矩形长宽接近，矩形方向不稳定 | 结合运动方向消歧义；设置航向置信度 |
| 航向角连续几个帧突然翻转180度 | 目标被部分遮挡，凸包长边切换 | 用历史航向做平滑；增加遮挡检测 |
| 单帧内多个目标航向角互相矛盾 | 坐标系没有统一，外参有误 | 检查雷达安装角度和坐标转换 |
| 静止目标航向角缓慢漂移 | 点云噪声导致凸包顶点抖动 | 冻结静止目标航向角 |
| 目标航向角与速度方向相差很大 | 车辆侧滑或目标明显转弯 | 跟踪模型里增加转角约束 |
我印象最深的是一次停车场场景测试：一辆车在直角弯里慢速拐弯，点云轮廓在车头和车身之间切换，凸包长边方向每帧跳变90度，航向角滤波器没扛住，跟踪轨迹直接乱了。后来把最小外接矩形算出的航向角作为候选，再用上一帧航向差做约束，只有当角度跳变超过阈值（比如60度）才允许从候选里选另一个方向，否则沿用平滑结果。这套规则虽然简单，但实战里非常管用。
4.3 坐标系与标定误差
航向角是角度量，坐标系偏差会直接导致角度误差。雷达安装时如果存在俯仰角没校正，点云投影到地面后目标轮廓会被压缩，计算出的凸包更扁，航向角的偏差可到几度甚至十几度。所以项目上线前一定要做雷达外参标定，至少保证安装角度误差在0.1度级别。如果使用多雷达拼接，还需要考虑各雷达之间的时间同步和坐标对齐，否则同一目标的部分点云出现重影，凸包会被撑大。
另一个容易忽视的是目标点是否经过运动补偿。雷达自身在运动，直接对原始点云做几何计算，静止物体的点云也会被拉伸。对于航向角来说，未补偿的点云会让目标看起来像有一个虚拟的拖尾角度，尤其当车速较快时，这个偏差会随着车速上升。正确的做法是在聚类之前做ego-motion补偿，把点云转换到统一时刻的坐标系下，再算凸包和航向角。
4.4 实时性优化建议
凸包计算本身开销不大，但感知系统往往要同时处理上百个目标，如果每个目标都做完整的点云预处理+凸包+旋转卡壳，还是会占掉不少CPU资源。我在实际系统里做过三步优化。一是只对进入感兴趣区域（ROI）的聚类点做几何计算，超过半径30米的目标直接用粗框代替，不生成精细凸包；二是对凸包顶点数量做限制，如果目标太碎，下采样到每0.1米保留一个点，把凸包顶点数量控制在20以内；三是对已经处于稳定跟踪状态的目标，几何计算可以隔帧执行，跟踪模块用上一帧结果做预测，然后下一帧再更新一次。
代码层面还有一个小技巧：把凸包面积和中心计算从航向角计算中拆开，因为有些下游模块只需要面积不需要朝向，没必要每帧都跑一遍最小外接矩形。按照模块解耦的原则，我把点云几何处理拆成三个独立函数：build_convex_hull、compute_polygon_metrics、estimate_orientation，分别缓存结果，这样在调试的时候可以单独调用，也方便做单元测试。
最后再多说一句真实体会：这套凸包+最小外接矩形的方案，我在不少项目里跟PCA方案做过对比，在车辆、行人、骑车人三类主要目标上，最小外接矩形的航向角鲁棒性明显更好，尤其是在近距离、点云密集的场景。代价是要多花一点点算力，但从跟踪、预测到决策的整条链路上来说，比那个几微秒的CPU占用值太多了。如果你也在为点云目标的形状描述和朝向估计头疼，不妨先把这套流程跑通，再根据你的雷达线束和场景微调阈值，应该能少走不少弯路。
