(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210254272.9 (22)申请日 2022.03.15 (71)申请人 浙江大学 地址 310058 浙江省杭州市西湖区余杭塘 路866号 申请人 杭州视光半导体科技有限公司 (72)发明人 钟锴　周武平　郭冕　金彬龙　 李佳杰　汪小知　 (74)专利代理 机构 杭州求是专利事务所有限公 司 33200 专利代理师 林超 (51)Int.Cl. G01S 7/48(2006.01) G01S 7/4912(2020.01) G01S 17/89(2020.01)G01S 17/931(2020.01) G06T 7/277(2017.01) G06V 10/762(2022.01) G06K 9/62(2022.01) (54)发明名称 FMCW激光雷达随机采样提取静止点云的速 度估计方法 (57)摘要 本发明公开了一种FMCW激光雷达随机采样 提取静止点云的速度估计方法。 方法步骤包括： 通过FMCW激光雷达获取物 体的当前帧的点云集； 通过扫描点及所有周围点的位置与径向速度计 算扫描点的归一化速度； 通过 RANSAC算法随机多 次采样提取点云集中的静止点， 由所有静止点构 成静止点云集； 估计FMCW激光雷达自身速度； 聚 类运动物体， 并估计运动物体的速度。 本发明大 大简化了运动物 体检测复杂 度， 提高了运动物体 检测精度； 本发明根据单帧数据， 通过粗略估计 和精确估计的方法估计雷达自身速度和运动物 体速度， 符合 不同场景的需求。 权利要求书4页 说明书9页 附图3页 CN 114609609 A 2022.06.10 CN 114609609 A 1.一种FM CW激光雷达随机采样提取静止点云的速度估计方法， 其特 征在于： 方法包括如下步骤： 步骤一： 通过FMCW激光雷达获取物体的当前帧的点云集， 点云集包括点云中的所有点 以及各个点的位置和径向速度； 步骤二： 遍历点云集中的每个点， 以当前点作为扫描点， 在扫描点周围的点中选取若干 点作为周围点， 所述的周围点的径向速度与扫描点的径向速度之间的差小于预设速度阈 值， 通过扫描点及所有周围点的位置与径向速度计算扫描点的归一 化速度； 步骤三： 根据点云集中各点的归一化速度， 通过RANSAC算法随机多次采样提取点云集 中的静止点， 由所有静止点构成静止点云集； 步骤四： 根据步骤三得到的静止点云集估计FM CW激光雷达自身速度； 步骤五： 根据步骤二得到的点云集中各点的归一化速度聚类运动物体， 并通过步骤四 估计的FM CW激光雷达自身速度， 估计运动物体的速度。 2.根据权利要求1所述的一种FMCW激光雷达随机采样提取静止点云的速度估计方法， 其特征在于： 所述的步骤二中， 具体为： 遍历点云集中的每个点， 以当前点作为扫描点， 针对每一个扫描点， 通过设立速度阈值 Tth1对扫描点周围的点进行以下判断： 当扫描点周围的点和扫描点之间的径向速度差Δv满足Δv≥Tth1时， 则认为扫描点周 围的该点和扫描点 不属于同一类， 将扫描点的归一 化速度设置为空值； 当扫描点周围的点和扫 描点之间的径向速度差Δv满足Δv<Tth1时， 则将扫 描点周围所 有的该点均作为周围点， 认为扫描点和所有周围点同属于同一类， 将和扫描点属于同一类 的所有周围点 一起组成同类点 集， 并进行以下判断： 当同类点 集中点的总数量小于 3， 则将扫描点的归一 化速度设置为空值； 当同类点集中点的总数量不小于3， 则通过扫描点和自身对应的所有周围点的位置与 径向速度计算扫描点的归一 化速度， 具体为： 建立扫描点的归一 化速度的目标函数 其中， 表示对目标函数进行求解获得的扫描点的归一化速度向量； 表示二范 数； V表示扫描点和所有周围点的径向速度的集合； A表示扫描点和所有周围点的单位方向 向量的集 合； 通过最小二乘法计算出扫描点的归一化速度的目标函数 进而计算出扫描点 的归一化速度。 3.根据权利要求1所述的一种FMCW激光雷达随机采样提取静止点云的速度估计方法， 其特征在于：权　利　要　求　书 1/4 页 2 CN 114609609 A 2所述的步骤三中， RANSAC算法如下： 1)随机选取出点云集 中的n个点， 并将n个点全部作为静止点， 组成点云子集， 其中， n 为预设值； 2)根据点云子集的归一 化速度， 使用概 率统计方法设定点云子集的静止点 参考速度； 3)计算点 云集 中除点云子集外的各个点的归一化速度与步骤2)得到的静止点参考速 度之间的静止点速度差Δvs， 针对点云集 中除点云子集外的每一个点， 通过设立静止点 阈值Tth2进行以下判断： 当静止点速度差 Δvs满足Δvs<Tth2时， 则将该点加入点云子集中； 当静止点速度差 Δvs满足Δvs≥Tth2时， 则不将该点加入点云子集中； 4)统计步骤3)所 得当前点云子集中的点数量： 若点数量大于预设数量阈值 N时， 则保留当前点云子集， 继续进行 下一步操作； 若点数量小于等于数量阈值 N时， 则舍弃当前点云子集， 返回步骤1)继续进行操作； 5)重复执行步骤1)～4)， 在迭代过程中， 随机组成若干不同的点云子集， 针对当前点云 子集， 计算当前点云子集的误差度量， 并进行以下判断： 若当前点云子集是第 一次迭代的点云子集， 则将当前点云子集的误差度量作为最优误 差度量， 将当前点云子集作为静止点云集 进行保留； 若当前点云子集 不是第一次迭代的点云子集， 则进行以下判断： 若当前点云子集的误差度量不小于上一 次迭代获得的最优误差度量， 则 舍弃当前点云 子集， 保留上一次迭代获得的静止点云集 和最优误差度量； 若当前点云子集的误差度量小于上一 次迭代获得的最优误差度量， 则保留当前点云子 集， 舍弃上一次迭代 获得的静止点云集 和最优误差度 量， 并将当前点云子集的误差度 量 作为最优误差度量， 将当前点云子集作为静止点云集 进行保留； 最终的静止点云集 中的每个点均为静止点。 4.根据权利要求3所述的一种FMCW激光雷达随机采样提取静止点云的速度估计方法， 其特征在于： 所述的步骤三中的步骤2)中， 概率统计方法包括但不限于： a)对点云子集的归一化速 度做高斯分布概率拟合， 将拟合的高斯分布的期望作为静止点参考速度； b)对点云子集的 归一化速度排序， 将排序的归一化速度中的中位数作为静止点参考速度； c)对点云子集的 归一化速度做算 术平均， 将算 术平均值作为静止点 参考速度。 5.根据权利要求3所述的一种FMCW激光雷达随机采样提取静止点云的速度估计方法， 其特征在于： 所述的步骤三中的步骤5)中， 计算当前点云子集中每个点的归一化速度与静止点参考 速度之差的平方值， 将所有点所 得的平方值取平均值作为 误差度量。 6.根据权利要求1所述的一种FMCW激光雷达随机采样提取静止点云的速度估计方法， 其特征在于： 所述的步骤四中： a)使用静止点云集 的归一化速度粗略估计FM CW激光雷达自身速度， 公式如下：权　利　要　求　书 2/4 页 3 CN 114609609 A 3