(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210254330.8 (22)申请日 2022.03.15 (65)同一申请的已公布的文献号 申请公布号 CN 114612665 A (43)申请公布日 2022.06.10 (73)专利权人 北京航空航天大 学 地址 100000 北京市海淀区学院路37号 (72)发明人 陈广志　董傲勇　翁友龙　贾志宇　 (74)专利代理 机构 成都巾帼知识产权代理有限 公司 5126 0 专利代理师 邢伟 (51)Int.Cl. G06V 10/26(2022.01) G06V 10/762(2022.01) G06V 10/50(2022.01)G06K 9/62(2022.01) G06T 7/73(2017.01) (56)对比文件 CN 114037736 A,2022.02.11 CN 111595333 A,2020.08.28 审查员 李富贵 (54)发明名称 基于法向量直方图特征的位姿估计及动态 车辆检测方法 (57)摘要 本发明公开了一种基于法向量直方图特征 的位姿估计及动态车辆检测方法， 包括以下步 骤： S1.通过激光雷达扫描获取检测区域的激光 点云数据， 并对激光点云数据进行预处理， 得到 地上点云簇； S2.根据预处理后得到的地上点云 簇进行动态目标检测； S3.对检测得到的动态目 标进行位姿估计， 并判断检测得到的动态目标是 否为动态车辆。 本发明利用车辆表 面法向量分布 特性提取目标特征， 并利用模型和特征拟合车辆 所处位姿， 以提高位姿估计准确性， 进而提高了 远距离稀疏点云场景 下动态车辆 检测准确性。 权利要求书4页 说明书9页 附图3页 CN 114612665 B 2022.10.11 CN 114612665 B 1.基于法向量直方图特征的位姿估计及动态车辆检测方法， 其特征在于： 包括以下步 骤： S1.通过激光雷达扫描获取检测区域的激光点云数据， 并对激光点云数据进行预处理， 得到地上点云簇； S2.根据预处 理后得到的地上点云簇进行动态目标检测； S3.对检测得到的动态目标进行位姿估计， 并判断检测得到的动态目标是否为动态车 辆： 所述步骤S3包括： S301.对于检测得到的动态目标进行位姿估计， 包括点云法向量求解、 法向量直方图特 征计算和利用法向量 直方图特 征进行位姿估计； 所述点云法向量 求解的过程包括： 针对某动 态目标o， 假设其包含的点云为P， P中任一点pi的法向量计算过程为： 以pi为球 心， r为球半径做虚拟球， 该球包围的所有点表示为Pr＝{pj|pj∈P,||pj‑pi||＜r}， 矩阵Pr中 的每一行表示 一个点， 使用 分别表示Pr中的各列， 表示球中所有点的x,y,z坐标； 使用Pr的协方差矩阵 获取pi的法向量， 其中： 其中τ表示球Pr中点的个数， m,n∈{1,2,3}； 计算 的特征值和特征向量， 最 小特征值对应的特 征向量 为点pi的法向量； 根据球坐标表示， 单位法向量ni通过ni与+z轴间的角 ni在x‑y平面投影与+x轴间的 夹角 来确定， ni坐标与该角关系如下： 其中 为了表示上的统一， 使用ni与+x轴间的夹角 以及与+y 轴间的夹角 来替代 其中 ni与各坐标轴正向的夹角由下式得 出： 其中dc表示各坐标轴 单位向量， ni为单位法向量， < ·>表示内积运算， || ·||表示向量 求模运算； S302.在当前时刻获取某动态目标的位姿后， 沿该目标 所处方向角进行位置关联； S303.通过运动一 致性原理进行 是否属于车辆的判断， 从而实现动态车辆的检测。 2.根据权利要求1所述的基于法向量直方图特征的位姿估计及动态车辆检测方法， 其 特征在于： 所述 步骤S1包括： S101.地面点云分割： 通过激光雷达扫描获取检测区域的激光点云数据后， 通过RANSAC、 EKF方法对地面进行权　利　要　求　书 1/4 页 2 CN 114612665 B 2平面拟合， 并获取点云与拟合平面间的垂距离， 对于任一点云， 若与拟合平面的垂距离小于 预先设定的阈值Tg， 则为地面点云， 反之则为地上点云； 或是根据GPR构建地面高度模型， 将 各点云处的预测高度与真实高度进行比较， 若预测高度大于等于真实高度， 则该点为地面 点云， 反之， 则该点 为地上点云； 对地面点云进行分割滤除， 保留地上点云； S102.地上点云聚类: 使用RBNN聚类方法将地上点云划分为 不同的地上点云簇 。 3.根据权利要求1所述的基于法向量直方图特征的位姿估计及动态车辆检测方法， 其 特征在于： 所述 步骤S2包括： S201.将地上点云簇进行水平面极坐标网格投影， 没有点云投影的网格标记为 “自由” 状态， 点云簇投影中距离激光雷达最近的一层 网格定义为 “占用”状态， 其他包含该点云簇 的网格定义 为“遮挡”状态； S202.将连续两帧点云首先利用GPS/INS数据进行坐标系统一， 随后比较极坐标网格中 的状态变化， 并设定状态变化的网格的数量阈值， 若网格变化数量大于阈值， 则该点云簇被 标记为动态目标， 若网格变化数量不 足阈值， 则认为是测量噪声或环境干扰引起， 将点云簇 标记为非动态目标。 4.根据权利要求1所述的基于法向量直方图特征的位姿估计及动态车辆检测方法， 其 特征在于： 所述法向量 直方图特 征计算的过程包括： 构建车辆测量模型： 将车辆的水平面投影建模为矩形框， 其中设R0为激光雷达视角下 的可见长边区域， R1为可见短边区域， R2为车辆内部， R3为可见角点； 目标局部坐标系原 点O 位于目标中心位置， y轴 与不可见短边法向量方向 同向， x轴为y轴顺时针旋转90度； L为模 型 长度， W为模型宽度， Lv为可 见边区域的宽度； 将区间[0, π]等分为bnum个小区间， 统计落入每个小区间中的角 度的数量， 得到直方 图 表示： 其中b＝1,2,. ..,bnum， card(·)表示集合的基数， c∈{x,y,z}； 根据车辆测量模型， 分别统计落入R0‑R3区域中的点， 计算各点法向量与各坐标轴夹角 的分布情况， 每 个区域中的直方图特 征定义为三个轴的夹角的分布直方图拼接： 其中特征 的长度为3 ×bnum。 5.根据权利要求1所述的基于法向量直方图特征的位姿估计及动态车辆检测方法， 其 特征在于： 所述利用法向量 直方图特 征进行位姿估计的过程包括： 对于某地上动态目标o， 首先获得该点云簇 中各点的法向量Nc， 随后将其三维点云投影 到水平面中， 利用Graham算法获取该点云簇投影的凸包络PK， 随后利用凸包络确定包 围该 点云簇的K种不同位姿下的包围框， 具体过程如下： 首先从PK中选取相邻两点， 计算两点确定的直线line1， 随后从PK中挑选距离line1最远权　利　要　求　书 2/4 页 3 CN 114612665 B 3