网站手机客户端开发教程,网页开发者模式快捷键,做淘宝客网站哪个好用,建筑业大数据服务平台官网1.算法流程 4D雷达点云跟踪处理沿用3D毫米波雷达的处理流程#xff0c;如下图#xff1a; 从接收到点云开始#xff0c;先对点云做标定、坐标转换、噪点剔除、动静分离#xff0c;再分别对动态目标和静态目标做聚类#xff0c;然后根据聚类结果做目标的特征分析和检测等如下图 从接收到点云开始先对点云做标定、坐标转换、噪点剔除、动静分离再分别对动态目标和静态目标做聚类然后根据聚类结果做目标的特征分析和检测等之后对符合条件的聚类结果做目标起始、关联和跟踪等处理输出目标结果。
2.预处理 预处理主要包括标定、坐标转换、噪点剔除和动静分离。
2.1标定 将安装雷达的车辆停在水平地面上。根据车身确定车辆行进方向在车辆正前方5~50m确定测试区域直线与行进方向平行或垂直误差小于2cm将角反射器调整到与雷达相同高度误差小于1cm选择多个点不少于10个放置角反射器并通过雷达读取角度值根据多个点的数据拟合的直线斜率和夹角最终确定偏航角θ。俯仰角φ的标定同偏航角方法。
2.2坐标转换 雷达点云通常是极坐标下的表示为径向距离r方位角azi俯仰角ele需要将坐标转到自车后轴中心在地面的投影为原点的坐标系如下图。 x,y,z方向上的偏移分别为OFFSET_X,OFFSET_Y,OFFSET_Z,则最终的坐标为 x r*cos(eleφ)*cos(aziθ)OFFSET_X y r*cos(eleφ)*sin(aziθ)OFFSET_Y z r*sin(eleφ)OFFSET_Z 2.3噪点剔除 部分雷达点云是不感兴趣的可以通过设置ROI区域剔除。 ROI区域通常设置成矩形图中是XOY平面投影实际处理需考虑Z轴方向的范围应为矩形盒。 由于多径或环境噪声影响雷达会检测到各种虚假目标通常这些目标的RCS很小可以通过RCS门限剔除。 而RCS随距离增加而增加因此设置RCS门限时也应把距离作为参数。
2.4动静分离 动静分离主要根据径向速度在车辆前进方向的投影为0进行判断。如下图所示Vego表示本车车速VR表示目标的径向速度α表示目标的方位角β表示雷达相对于车头正前方的横向偏角由方向盘转角计算得到将Vego向径向速度方向投影对于每帧中的每个目标均可建立各参数间的关系方程如下 Vego * cos(α- β) VR 0 实际处理时可通过设定门限判断目标是否静止。由于自车车速存在一定误差Vego越大误差越大设置门限时应考虑这个因素。
3.目标检测
3.1目标聚类 DBSCAN是一种基于密度的聚类算法这类密度聚类算法一般假定类别可以通过样本分布的紧密程度决定。同一类别的样本他们之间的紧密相连的也就是说在该类别任意样本周围不远处一定有同类别的样本存在。通过将紧密相连的样本划为一类这样就得到了一个聚类类别。通过将所有各组紧密相连的样本划为各个不同的类别就能得到最终的所有聚类类别结果。 DBSCAN是基于一组邻域来描述样本集的紧密程度的参数(ϵ, MinPts)用来描述邻域的样本分布紧密程度。其中ϵ描述了某一样本的邻域距离阈值MinPts描述了某一样本的距离为ϵ的邻域中样本个数的阈值。 假设样本集是D(x1,x2,...,xm),则DBSCAN具体的密度描述定义如下 1 ϵ-邻域对于xj∈D 其ϵ-邻域包含样本集D中与xj的距离不大于ϵ的子样本集即Nϵ(xj){xi∈D|distance(xi,xj)≤ϵ}、 这个子样本集的个数记为|Nϵ(xj)| 2) 核心对象对于任一样本xj∈D如果其ϵ-邻域对应的Nϵ(xj)至少包含MinPts个样本即如果Nϵ(xj)|≥MinPts则xj是核心对象。 3密度直达如果xi位于xj的ϵ-邻域中且xj是核心对象则称xi由xj密度直达。注意反之不一定成立即此时不能说xj由xi密度直达, 除非且xi也是核心对象。 4密度可达对于xi和xj,如果存在样本序列p1,p2,...,pt满足p1xi,ptxj, 且pt1由pt密度直达则称xj由xi密度可达。也就是说密度可达满足传递性。此时序列中的传递样本p1,p2,...,pT−1均为核心对象因为只有核心对象才能使其他样本密度直达。注意密度可达也不满足对称性这个可以由密度直达的不对称性得出。 5密度相连对于xi和xj,如果存在核心对象样本xk使xi和xj均由xk密度可达则称xi和xj密度相连。注意密度相连关系是满足对称性的。 从下图可以很容易看出理解上述定义图中MinPts5红色的点都是核心对象因为其ϵ-邻域至少有5个样本。黑色的样本是非核心对象。所有核心对象密度直达的样本在以红色核心对象为中心的超球体内如果不在超球体内则不能密度直达。图中用绿色箭头连起来的核心对象组成了密度可达的样本序列。在这些密度可达的样本序列的ϵ-邻域内所有的样本相互都是密度相连的。 DBSCAN聚类算法的流程如下。 输入样本集D(x1,x2,...,xm)邻域参数(ϵ,MinPts)。 输出类划分C。 1初始化核心对象集合Ω∅初始化聚类数k0初始化未访问样本集合Γ D类划分C ∅。 2) 对于j1,2,...m, 按下面的步骤找出所有的核心对象 a) 通过距离度量方式找到样本xj的ϵ-邻域子样本集Nϵ(xj)。 b) 如果子样本集样本个数满足|Nϵ(xj)|≥MinPts 将样本xj加入核心对象样本集合ΩΩ∪{xj}。 3如果核心对象集合Ω∅则算法结束否则转入步骤4。 4在核心对象集合Ω中随机选择一个核心对象o初始化当前类核心对象队列Ωcur{o}, 初始化类别序号kk1初始化当前类样本集合Ck{o}, 更新未访问样本集合ΓΓ−{o}。 5如果当前类核心对象队列Ωcur∅则当前类Ck生成完毕, 更新类划分{C1,C2,...,Ck}, 更新核心对象集合ΩΩ−Ck 转入步骤3。 6在当前类核心对象队列Ωcur中取出一个核心对象o′,通过邻域距离阈值ϵ找出所有的ϵ-邻域子样本集Nϵ(o′)令ΔNϵ(o′)∩Γ, 更新当前类样本集合CkCk∪Δ 更新未访问样本集合ΓΓ−Δ 更新ΩcurΩcur∪(Δ∩Ω)−o′转入步骤5。 输出结果为 类划分C{C1,C2,...,Ck}。
3.2特征提取 聚类后可以得到基本信息比如目标位置x,y,z)、点云数量、长度、宽度、高度、体积、投影面积、RCS、SNR以及各种统计分布如位置方差等。根据这些信息可以对目标进行分类得到目标的分类结果。
3.3目标分类 毫米波分类可以考虑使用机器学习的方法进行分类常见的有决策树模型、最近邻模型和支持向量机等。 使用激光雷达对毫米波点云进行标注前提是激光雷达本身精度和准确度要足够高如果存在问题可考虑人工进行修改。 如图红色和黄色圆圈代表毫米波点云黑色方框是未经过时间补偿的lidar目标而蓝色方框是经过时间补偿的lidar目标。通常情况下补偿后的lidar目标框可以包围实际目标的毫米波点云这样就能同时得到训练样本点云类和标注结果补偿后的lidar目标。 Python中导入sklearn工具包可以方便地调用不同模型进行训练并且可以使用不同参数进行对比实验输出分类准确率、错误率或混淆矩阵等。 另外Python的m2cgen工具包支持输出sklearn模型的C代码方便部署到各类平台运行。
3.4路沿检测 对静止点聚类如果某类中点数明显较多且位置跨度较大则认为是路沿需要对路沿做拟合生成。 由于路沿存在一定宽度如果直接拟合则会生成如下图的黑色路沿和实际结果存在较大误差。因此这里需要每隔一段距离提取聚类的内侧点再对内侧点进行拟合得到下图中红色的路沿结果与实际值误差较小。 4.目标跟踪 目标跟踪分为目标起始、目标关联、跟踪滤波和生命周期管理等。
4.1目标起始 点云检测结果符合一定条件就可以起始目标一般需要满足以下条件
未被成熟航迹或临时航迹跟踪检测结果至少有三个毫米波点云检测结果高度大于0.5m检测结果的最大RCS大于-2.5 目标起始并没有统一的标准需要根据实际情况调整参数上述几个条件是较为常见的根据实际场景可以微调或增加其他条件。
4.2目标关联 目标关联分为临时航迹的关联和稳定航迹的关联。 临时航迹使用检测结果中心点和径向速度关联也即: |center_xi - center_xj| x_threshold |center_yi - center_yj| y_threshold |vri - vrj| vr_threshold 稳定航迹使用毫米波点云和目标包围框进行关联判断点云位置和径向速度是否在关联门限内类似于激光目标给毫米波点云做标注可参考3.3节。
4.3目标属性更新 航迹成功和点云关联则更新目标属性。如果累计帧数未达到生成航迹要求则只保存历史数据只有达到帧数要求或者稳定航迹成功关联点云才做属性更新。 目标属性包括位置、速度、类别、朝向、长宽等。
4.3.1朝向估计 首先将历史点云做坐标转换统一转换到当前坐标系下可以通过最小面积、最小距离等遍历方法拟合朝向具体方法可参考论文《Efficient L-Shape Fitting for Vehicle Detection Using Laser Scanners》。 实际数据仿真验证表明速度较快时用最小二乘就能得到很好的结果。速度较慢时使用更多历史点云可以得到更准确的朝向估计。 上图左是朝向估计的结果绿色是激光雷达估计的目标框蓝色和灰色是使用距离方差准则和最近距离准则估计的朝向。上图右是目标不同姿态下的统计结果可以看到随着点数的增加朝向估计误差在减小。 有了朝向之后可以计算聚类点云在朝向方向上的投影从而计算长宽这样聚类点云就多了朝向、长宽等特征可以通过3.3节的机器学习方法做分类再根据类别得到目标长宽的推理结果。
4.3.2速度估计 得到准确的目标朝向之后可以通过径向速度、目标朝向、点云角度计算得到目标的速度。 如下图Vt是目标速度α是朝向角Vego是自车速度θ是点云方位角β是自车速度和车体坐标系偏角。 首先计算补偿车速后的绝对径向速度comp_Vr Vr Vego*cos(θβ) 由于Vt_abs*cos(θ-α comp_Vr 根据绝对径向速度和角度关系计算目标绝对速度 Vt_abs comp_Vr /cos(θ-α 然后根据朝向分解得到绝对横纵向速度 Vt_abs_x Vt_abs*cos(α) Vt_abs_y Vt_abs*sin(α) 最后减去自车的横纵向速度得到相对横纵向速度 Vt_x Vt_abs_x - Vego*cos(θβ) Vt_y Vt_abs_y - Vego*sin(θβ)
4.3.3跟踪滤波 使用卡尔曼滤波对检测得到的位置和速度等做跟踪滤波处理一般由下面5个公式组成。 和分别表示k时刻和k-1时刻的后验状态估计值是滤波的结果这里使用匀加速直线CA运动模型[x,y,vx,vy,ax,ay]。 是k时刻的先验状态估计值是滤波的中间计算结果即根据k-1时刻的最优估计预测的k时刻的结果是预测方程的结果。 F是状态转移矩阵实际上是对目标状态转换的一种猜想模型。状态转移矩阵通常用来对目标的运动建模其模型可能为匀速直线运动或者匀加速运动。 Pk和Pk-1分别表示k时刻和k-1时刻的后验估计协方差。 Ppre是k时刻的先验估计协方差的协方差。 H是状态变量到测量观测的转换矩阵表示将状态和观测连接起来的关系卡尔曼滤波里为线性关系它负责将m维的测量值转换到n维使之符合状态变量的数学形式是滤波的前提条件之一。 是测量值观测值是滤波的输入。 Kk是滤波增益矩阵是滤波的中间计算结果。 Q代表过程噪声协方差该参数用来表示状态转换矩阵与实际过程之间的误差。因为无法直接观测到过程信号所以Q的取值是很难确定的可以参考何友《雷达数据处理及应用》给出初始值。 R是测量噪声协方差。滤波器实际实现时测量噪声协方差R一般可以观测或统计分析得到。 将点云检测结果放到滤波器通过调整参数Q和R可以得到不同场景下的状态最优估计结果。
4.4生命周期管理 生命周期管理主要对目标生成和消亡做处理。航迹生成时满足关联条件且n个观测周期存在至少m次则符合航迹生成要求生成新航迹赋予新ID。 如果n个观测周期存在次数小于m次则不符合航迹生成要求航迹不会生成直接删除。 如果航迹稳定跟踪单帧未关联点云则用外推值更新如果关联点云则用卡尔曼滤波得到的新状态更新。 如果航迹稳定一段时间连续t个观测周期中k次无法成功关联则删除这条航迹。
5.特殊处理
5.1直线拟合估计目标速度和朝向 点云聚类后通过径向速度和方位角也可以估计目标朝向。 假设目标朝向角为α目标点云在速度方向的投影即是点云的径向速度Vr则有 V(i)*cos(θ(i)-α) Vr(i) V(i) Vr(i) / cos(θ(i)-α) 由于车辆是刚体车身每个点速度相等则有V(1) V(2) … V(n) V(i) V(j) V Vr(i) / cos(θ(i)-α) Vr(j) / cos(θ(j)-α) 可以计算得到点云的朝向角α进而得到目标的真实速度和横纵向速度。 由于雷达对速度的测量可能存在模糊或出现错误因此任意两个点的结果有较高错误概率但多个点拟合得到的结果有较高的可行度可以准确估计目标朝向和横纵向速度。 上文已经推导Vr(i) V* cos(θ(i)-α) V*(cos(θ(i))cos(α) sin(θ(i))sin(α)) 两边同时除以V*Vr(i)* cos(α)则有 cos(θ(i))/Vr(i) 1 / V cos(α) - tan(α) * sin(θ(i))* /Vr(i) 令y(i) cos(θ(i))/Vr(i), x(i) sin(θ(i))* /Vr(i)b 1 / V cos(α), k - tan(α)则有 y(i) k * x(i) b 根据最小二乘拟合公式可以得到V和α的估计值。 对于行人由于不是刚体运动并不适用上述方法。
5.2二分迭代估计目标速度和朝向 由于单个目标聚类后速度和角度离散性不足使用最小二乘拟合往往不能得到最优解此时可以考虑使用二分迭代法。 仍旧是角度和速度的关系表达式 V(i)*cos(θ(i)-α) Vr(i) 通过预设的角度α计算目标速度V(i) Vr(i) / cos(θ(i)-α) 再计算不同角度α的V(i)离散程度离散程度越小则α是实际角度的概率越高。V(i)的离散程度可以用方差表示。
5.3自车轨迹路径静止点处理 通常目标起始跟踪不对静态点做处理但考虑到碰撞风险在自车运动轨迹上的静态点需要跟踪处理做碰撞预警。 自车运动可以用圆周运动建模通常直线行驶时认为圆周半径非常大比如10000m。如果转弯时则可以用下图示意的方法计算。 下图中自车后轴中心为O,在以C为圆心的原点做圆周运动半径为R红色曲线代表自车运动方向。 则有Vego*t R*α R*yawrate*t -- R V/yawrate 这里Vego是自车速度yawrate是横摆角速度。 然后可以计算自车轨迹的方程 在自车轨迹附近的静态点加入到目标起始、跟踪流程中生成一般障碍物或过滤虚假目标起到预警作用。
