本申请公开了一种基于统计数据的4D毫米波雷达点云空间位置校正方法，通过地面行驶目标的位置拟合地面高度随纵向距离的变化，校正雷达的随机安装误差，并识别镜像反射目标，将其校正至真实目标位置，从而实现点云增强和质量优化。
1.一种基于统计数据的4D毫米波雷达点云空间位置校正方法，其特征在于，包括以下步骤：步骤一、获取当前场景的原始点云数据；
步骤二、从原始点云数据中提取本车车速、并查找点云中的带速目标点；
步骤三、按照雷达测距范围，以 为步长划定距离区间，根据带速目标点的纵向距离将其划入相应的区间；
步骤四、计算每个距离区间内带速目标的数量，将其与数量阈值 进行比较，带速目标数量大于 ，则认为当前区间内存在带速目标，反之则不存在；
步骤五、若存在距离区间内有带速目标，则通过带速目标的高度信息估算每个距离区间内的地面高度；
步骤六、得到不同距离区间内的高度后，构造纵向距离和地面高度之间的映射关系，以纵向距离x为自变量，相应距离段内的地面高度 为因变量，通过曲线拟合方法，估计雷达偏角；
所述步骤六中估计雷达偏角具体步骤如下：S6‑1取近距 的纵向距离x与地面高度 集合，认为在近距地面高度 与纵向距离x为一元线性关系：；
S6‑2基于统计得到的纵向距离集合x和地面高度集合 ，对k进行估计：其中，N为存在带速目标的距离区间数量，为纵向距离x集合的平均值，为纵向距离集合x的第i个纵向距离；为地面高度集合 的平均值，为地面高度集合 的第i个地面高度；
步骤八、根据安装偏角对所有点云的空间位置进行校正。
2.如权利要求1所述的一种基于统计数据的4D毫米波雷达点云空间位置校正方法，其特征在于，所述步骤二中提取本车车速的步骤包括：S2‑1‑1计算所有点速度中的最大值 和最小值 ；
S2‑1‑2根据雷达的速度分辨力设置步长，对速度区间 分段；
S2‑1‑3统计每段内的点云数量，选择点云数量最多的子区间，取子区间的中值作为本车车速 。
3.如权利要求2所述的一种基于统计数据的4D毫米波雷达点云空间位置校正方法，其特征在于，所述步骤二中查找点云中的带速目标点的步骤包括：S2‑2‑1计算每个点与车速的差：
S2‑2‑2将 与预设阈值 进行比较，若大于 则判定为带速目标点，反之判定为静止目标。
4.如权利要求1所述的一种基于统计数据的4D毫米波雷达点云空间位置校正方法，其特征在于，所述步骤四中带速目标为紧贴地面、在路面行驶或行走的车辆或行人。
5.如权利要求1所述的一种基于统计数据的4D毫米波雷达点云空间位置校正方法，其特征在于，所述步骤五中估算地面高度具体步骤如下：S5‑1计算当前纵向距离区间内目标的高度最大值 和最小值 ；
S5‑3统计每段高度区间内的点云数量，选择点云数量最多的区间，取区间中值作为当前纵向距离区间的地面高度 。
6.如权利要求5所述的一种基于统计数据的4D毫米波雷达点云空间位置校正方法，其特征在于，所述步骤八根据安装偏角对所有点云的空间位置进行校正具体步骤如下：S8‑1更新所有点的俯仰角：
S8‑2按照更新后的点云数据重新计算点云的空间位置。
7.如权利要求6所述的一种基于统计数据的4D毫米波雷达点云空间位置校正方法，其特征在于，还包括：步骤九、校正点云空间位置后，根据全距离段的纵向距离集合x与地面高度集合 ，拟合路面起伏情况，对各距离段的地面高度进行估计；
步骤十、计算得到地面高度关于纵向距离的函数关系后，以地面为反射面，判断目标是否为镜像反射目标，具体步骤为：为雷达距离地面的高度， 为雷达对于真实目标的高度测量值：为镜像目标距离地面的高度，存在如下几何关系：此处 、 及 均大于等于零，表示绝对高度值；
S10‑1首先判断当前目标点是否处于地面以下的位置；
S10‑2若目标点位于地面以下，则以当前目标点的纵向距离 及横向距离 为中心，计算以 为半径的范围内，是否存在关于地面对称的目标，若不存在，则判定为非镜像目标，不做进一步处理；
S10‑3若关于地面对称的位置存在目标，则以地面为反射面，将其高度z翻转：最后即可输出镜像反射校正后的点云信息。
一种基于统计数据的4D毫米波雷达点云空间位置校正方法
技术领域
[0001] 本申请涉及一种基于统计数据的4D毫米波雷达点云空间位置校正方法，属于毫米波雷达技术领域。
背景技术
[0002] 随着雷达天线阵列及信号处理技术的不断发展，已可对目标实现高分辨成像，生成描绘目标空间轮廓的点云，为智能驾驶提供稳定可靠的路况信息。然而城市中环境复杂，路旁建筑物、交通标识以及绿化等静止目标较多，电磁波的传输特性将导致目标回波在静止物体表面多次反射，干扰雷达定位目标真实位置。
[0003] 对于镜面反射导致的假目标，现有方法一般为划定目标候选区，将候选区以外的目标判定为假目标直接滤除。然而雷达安装存在随机偏角，导致目标空间位置测量值出现误差，此时真实目标可能超出预设的候选区外，固定候选区无法根据雷达安装情况及当前道路情况自适应更新，最终将造成真实目标被误判为假目标被滤除，灵活性较差。此外，镜面反射假目标的回波中包含真实目标信息，其空间位置相对真实目标存在一定的变换关系，直接滤除镜像目标将导致目标信息损失。因此本发明提出一种基于统计数据的4D毫米波雷达点云空间位置校正方法，通过统计原始点云的空间信息和速度信息进行误差校正，同时将保留并充分利用镜像目标，丰富点云轮廓。
发明内容
[0004] 本申请要解决的技术问题是现有雷达对镜面反射导致的假目标的滤除方法是划定目标候选区，将候选区以外的目标判定为假目标直接滤除，但雷达安装存在随机偏角，导致目标空间位置测量值出现误差，真实目标可能超出预设的候选区外，且固定候选区无法根据雷达安装情况及当前道路情况自适应更新，最终将造成真实目标被误判为假目标被滤除，灵活性较差；此外，镜面反射假目标的回波中包含真实目标信息，其空间位置相对真实目标存在一定的变换关系，直接滤除镜像目标将导致目标信息损失。
[0005] 为了解决上述技术问题，本申请的技术方案是提供了一种基于统计数据的4D毫米波雷达点云空间位置校正方法，包括以下步骤：
[0006] 步骤一、获取当前场景的原始点云数据；
[0007] 步骤二、从原始点云数据中提取本车车速、并查找点云中的带速目标点；
[0008] 步骤三、按照雷达测距范围，以 为步长划定距离区间，根据带速目标点的纵向距离 将其划入相应的区间；
[0009] 步骤四、计算每个距离区间内带速目标的数量，将其与数量阈值 进行比较，带速目标数量大于 ，则认为当前区间内存在带速目标，反之则不存在；
[0010] 步骤五、若存在距离区间内有带速目标，则通过带速目标的高度信息估算每个距离区间内的地面高度；
[0011] 步骤六、得到不同距离区间内的高度后，构造纵向距离和地面高度之间的映射关系，以纵向距离x为自变量，相应距离段内的地面高度 为因变量，通过曲线拟合方法，估计雷达偏角；
[0012] 步骤七、计算雷达的俯仰向安装偏角；
[0013] 步骤八、根据安装偏角对所有点云的空间位置进行校正。
[0014] 作为一种计算方式，所述步骤二中提取本车车速的步骤包括：
[0015] S2‑1‑1计算所有点速度中的最大值 和最小值 ；
[0016] S2‑1‑2根据雷达的速度分辨力设置步长，对速度区间 分段；
[0017] S2‑1‑3统计每段内的点云数量，选择点云数量最多的子区间，取子区间的中值作为本车车速 。
[0018] 进一步的，所述步骤二中查找点云中的带速目标点的步骤包括：
[0019] S2‑2‑1计算每个点与车速的差：
[0020]
[0021] 其中 为点云中第n个点的速度， 为取绝对值运算；
[0022] S2‑2‑2将 与预设阈值 进行比较，若大于 则判定为带速目标点，反之判定为静止目标。
[0023] 其中，所述步骤四中带速目标是指紧贴地面、在路面行驶或行走的车辆或行人。
[0024] 作为一种优选计算方式，步骤五中估算地面高度具体步骤如下：
[0025] S5‑1计算当前纵向距离区间内目标的高度最大值 和最小值 ；
[0026] S5‑2以 为步长，对高度区间 分段；
[0027] S5‑3统计每段高度区间内的点云数量，选择点云数量最多的区间，取区间中值作为当前纵向距离区间的地面高度 。
[0028] 作为一种优选估算方式，所述步骤六中估计雷达偏角具体步骤如下：
[0029] S6‑1取近距 的纵向距离x与地面高度 集合，认为在近距地面高度 与纵向距离x为一元线性关系：
[0030]
[0031] 其中k为函数斜率，b为函数偏置，均为未知量；
[0032] S6‑2基于统计得到的纵向距离集合x和地面高度集合 ，对k进行估计：
[0033]
[0034] 其中，N为存在带速目标的距离区间数量，为纵向距离x集合的平均值，为纵向距离集合x的第i个纵向距离；为地面高度集合 的平均值，为地面高度集合 的第i个地面高度；
[0035] S6‑3对b进行估计：
[0036]
[0037] 进一步的，所述步骤七中俯仰向安装偏角 。
[0038] 具体的，所述步骤八根据安装偏角对所有点云的空间位置进行校正具体步骤如下：
[0039] S8‑1更新所有点的俯仰角：
[0040]
[0041] 其中 为第 个点的俯仰角；
[0042] S8‑2按照更新后的点云数据重新计算点云的空间位置，重新计算后的点云数据是雷达安装偏角校准后的点云数据。
[0043] 前述步骤实现了雷达安装偏角的校准，对镜像反射的校准依赖于下述步骤：
[0044] 步骤九，校正点云空间位置后，根据全距离段的纵向距离集合x与地面高度集合 ，拟合路面起伏情况，对各距离段的地面高度进行估计；
[0045] 步骤十、计算得到地面高度关于纵向距离的函数关系后，以地面为反射面，判断目标是否为镜像反射目标，具体步骤为：
[0046] 为雷达距离地面的高度， 为雷达对于真实目标的高度测量值：
[0047]
[0048] 为镜像目标距离地面的高度，存在如下几何关系：
[0049]
[0050] 此处 、 及 均大于等于零，表示绝对高度值；
[0051] 雷达对于镜像目标的测量值为：
[0052]
[0053] S10‑1首先判断当前目标点是否处于地面以下的位置；
[0054] S10‑2若目标点位于地面以下，则以当前目标点的纵向距离 及横向距离 为中心，计算以 为半径的范围内，是否存在关于地面对称的目标，若不存在，则判定为非镜像目标，不做进一步处理；
[0055] S10‑3若关于地面对称的位置存在目标，则以地面为反射面，将其高度z翻转：
[0056]
[0057] 最后即可输出镜像反射校正后的点云信息。
[0058] 本申请通过点云数据信息统计，既校正了雷达的俯仰角偏差，且俯仰角偏差的校正不受路况和环境的限制；也校正了点云信息，校准了镜像反射目标，可有效丰富点云信息。
附图说明
[0059] 图1为本申请提供的校正方法流程图；
[0060] 图2为在空间坐标下镜像目标原理示意图；
[0061] 图3‑1、图3‑2、图3‑3分别为原始电云y‑x、y‑z、x‑z视图；
[0062] 图4‑1、图4‑2、图4‑3分别为处理后点云y‑x、y‑z、x‑z视图成像效果。
具体实施方式
[0063] 为使本申请更明显易懂，兹以优选实施例，并配合附图作详细说明如下。
[0064] 本实施例提供的是基于统计数据的4D毫米波雷达点云空间位置校正方法，参见图1，通过地面行驶目标的位置拟合地面高度随纵向距离的变化，校正雷达的随机安装误差，并识别镜像反射目标，将其校正至真实目标位置，从而实现点云增强和质量优化。
[0065] 具体包括以下步骤：
[0066] 本实施例的具体实施是选择调频连续波（Frequency Module Continue Waveform，FMCW）体制的多发多收（Multi‑Input Multi‑Output，MIMO）雷达进行数据采集，测试场景为假设有交通路牌的高架桥场景，对回波处理后得到原始点云数据，该雷达以及测试场景仅是为了展示本申请技术方案的实施过程，并不构成对本申请的限制。
[0067] 步骤一、首先获取当前场景的原始点云数据，其中点云数据包括每个点的径向距离 、方位角 、俯仰角 以及速度信息 。本申请采取的空间坐标建模如图2所示，x表示纵向距离，y表示横向距离，z表示高度。将位置信息变换至笛卡尔坐标系：
[0068]
[0069]
[0070]
[0071] 步骤二、从原始点云数据中提取本车车速，一般雷达在提供原始点云数据的同时，也会提供本车车速的计算方法，本实施例采用的本车车速的计算方法步骤包括：
[0072] S2‑1‑1计算所有点速度中的最大值 和最小值 ；
[0073] S2‑1‑2根据雷达的速度分辨力设置步长，对速度区间 分段；
[0074] S2‑1‑3统计每段内的点云数量，选择点云数量最多的子区间，取子区间的中值作为本车车速 。
[0075] 获取本车车速信息后，查找点云中的带速目标点，具体步骤包括：
[0076] S2‑2‑1计算每个点与车速的差：
[0077]
[0078] 其中 为点云中第n个点的速度， 为取绝对值运算；
[0079] S2‑2‑2将 与预设阈值 进行比较，若大于 则判定为带速目标点，反之判定为静止目标，其中 根据雷达的速度分辨力进行设计，本实施例中选择设置为7m/s。
[0080] 步骤三、按照雷达测距范围，以 为步长划定距离区间，根据带速目标点的纵向距离 将其划入相应的区间，本实施例中的测距范围设置为120m， 设置为5m。
[0081] 步骤四、计算每个距离区间内带速目标的数量，将其与数量阈值 进行比较，带速目标数量大于 ，则认为当前区间内存在带速目标，反之则不存在，本实施例中 设置为10。
[0082] 在本申请的应用范围内，认为带速目标为路面行驶的车辆或行人，紧贴地面行走。若所有距离区间内都不存在带速目标，则不在当前信号处理周期内估计地面高度，采用上一信号处理周期的估计结果。
[0083] 步骤五、若存在距离区间内有带速目标，则通过带速目标的高度信息估算每个距离区间内的地面高度。具体步骤包括：
[0084] S5‑1计算当前纵向距离区间内目标的高度最大值 和最小值 ；
[0085] S5‑2以 为步长，对高度区间 分段，实施例中将 设置为2m；
[0086] S5‑3统计每段高度区间内的点云数量，选择点云数量最多的区间，取区间中值作为当前纵向距离区间的地面高度 。
[0087] 步骤六、得到不同距离区间内的高度后，构造纵向距离和地面高度之间的映射关系，以纵向距离x为自变量，相应距离段内的地面高度 为因变量，通过曲线拟合方法，首先估计雷达偏角，具体步骤为：
[0088] S6‑1取近距 的纵向距离x与地面高度 集合，认为在近距地面高度 与纵向距离x为一元线性关系：
[0089]
[0090] 其中k为函数斜率，b为函数偏置，均为未知量；
[0091] S6‑2基于统计得到的纵向距离集合x和地面高度集合 ，对k进行估计：
[0092]
[0093] 其中，N为存在带速目标的距离区间数量，为纵向距离集合x的平均值，为纵向距离集合x的第i个纵向距离：
[0094]
[0095] 为地面高度集合 的平均值：
[0096]
[0097] S6‑3对b进行估计：
[0098]
[0099] 步骤七、根据k计算雷达的俯仰向安装偏角：
[0100]
[0101] 步骤八、根据安装偏角对所有点云的空间位置进行校正，具体步骤为：
[0102] S8‑1更新所有点的俯仰角：
[0103]
[0104] 其中 为第 个点的俯仰角。
[0105] S8‑2按照公式（1） —（3）重新计算点云空间位置。
[0106] 前述步骤实现了雷达安装偏角的校准，重新计算后的点云数据是校准后的点云数据，对镜像反射的校准依赖于下述步骤：
[0107] 步骤九、校正点云空间位置后，根据全距离段的纵向距离x与地面高度 集合，拟合路面起伏情况，对各距离段的地面高度进行估计，包括但不限于最小二乘法、三次样条曲线拟合等。
[0108] 步骤十、计算得到地面高度关于纵向距离的函数关系后，以地面为反射面，判断目标是否为镜像反射目标，具体步骤为：
[0109] 由于雷达具有一定的安装高度，如图2所示， 为雷达距离地面的高度， 为雷达对于真实目标的高度测量值：
[0110]
[0111] 为镜像目标距离地面的高度，存在如下几何关系：
[0112]
[0113] 此处 、 及 均大于等于零，表示绝对高度值。
[0114] 雷达对于镜像目标的测量值为：
[0115]
[0116] S10‑1首先判断当前目标点是否处于地面以下的位置；
[0117] S10‑2若目标点位于地面以下，则以当前目标点的纵向距离 及横向距离 为中心，计算以 为半径的范围内，是否存在关于地面对称的目标，若不存在，则判定为非镜像目标，不作进一步处理，其中 根据雷达的距离分辨力设计；
[0118] S10‑3若关于地面对称的位置存在目标，则以地面为反射面，将其高度z翻转：
[0119]
[0120] 最后即可输出镜像反射校正后的点云信息。
[0121] 实施例中处理前后的点云对比如附图所示，图3‑1、图3‑2、图3‑3为处理前的点云，点云的透明度表示高度，高度值越大透明度越低，从不同的视角中可以看到约在45m处，高架桥上设置有交通路牌，从y‑z视图中可以清晰地看出，原始点云中存在地面下的镜像目标，从x‑z视图可以看出，由于雷达的安装偏角，点云整体存在向上偏移的趋势；图4‑1、图4‑2、图4‑3为经本发明方法处理后的点云，校正了雷达的安装偏角，镜像目标校正至真实目标位置。
[0122] 综上，针对点云空间位置校正的问题，本申请通过点云数据信息统计，既校正了雷达的俯仰角偏差，且俯仰角偏差的校正不受路况和环境的限制；也校正了点云信息，校准了镜像反射目标，可有效丰富点云信息。
