一种基于高精度地图的高速场景多雷达航迹融合方法.pdf

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN115542307A(43)申请公布日2022.12.30(21)申请号202211121356.1G01S13/88(2006.01)(22)申请日2022.09.15G01S13/89(2006.01)G01S13/931(2020.01)(71)申请人河北省交通规划设计研究院有限公司地址050011河北省石家庄市建设南大街36号申请人北京理工睿行电子科技有限公司(72)发明人雷伟金烨李春杰焦彦利赵凯张龙潘秀庞宏杰吕璇杨宇鹏(74)专利代理机构北京理工大学专利中心11120专利代理师李微微(51)Int.Cl.G01S13/06(2006.01)权利要求书2页说明书4页附图1页(54)发明名称一种基于高精度地图的高速场景多雷达航迹融合方法(57)摘要本发明提供了一种基于高精度地图的高速场景多雷达航迹融合方法，目标行驶在盲区或量测漏检时，可通过高精度地图进行航迹预测，而通过引入高精度地图这一先验信息，微分拟合出车道线信息，得到在不同位置时车辆的行驶方向信息，就能很大程度上保证航迹不会发散，进而保证车辆的位置、速度信息准确，航迹ID唯一性较高。CN115542307ACN115542307A权利要求书1/2页1.一种基于高精度地图的高速场景多雷达航迹融合方法，其特征在于，包括：S1：将高精度地图中的上、下行车道的4条边线按照雷达的威力覆盖区大小进行切分，切分出来的车道的地图区域要保证大于雷达的威力覆盖区；S2：已知雷达自身经纬度坐标和波束指向的偏北角信息；将地图区域的4条边线在地图中的坐标信息转换到该雷达坐标系下；S3：利用雷达坐标系下的2条上行车道边线和2条下行车道边线数据，分别对上行和下行车道中心线按照设定的步进值进行微分拟合，分别得到上行车道和下行车道的中心线；S4：通过中心线上相邻两个微分点的位置坐标，求取中心线上每个微分点在雷达坐标系下的偏北角，作为车辆在该微分点处的速度方向；S5：遍历上一帧融合后的东北天坐标系下的暂态航迹列表，对每条航迹当前帧的位置、速度信息进行预测；其中，对正常航迹按照车辆运动模型进行预测；对外推航迹进行预测时，进入步骤S6；S6：先将航迹由东北天坐标系转到雷达坐标系，根据航迹中各点的Y轴坐标值，在车道中心线找到具有相同Y轴坐标值的微分点，然后确定该微分点的偏北角，即为车辆的速度方向；然后，将航迹的坐标信息和速度信息由东北天坐标系转到原点不变，y轴方向为车辆所在位置车道方向的直角坐标系下；此时车辆的速度信息中的x轴速度分量置零，y轴速度分量按实际大小进行坐标转换，依据转换坐标系后的坐标信息和速度信息按照车辆运动模型进行外推航迹的预测，最后将预测后的外推航迹转换回东北天坐标系；S7：根据寿命参数tick值对暂态航迹列表中各航迹设置预测模型噪声Q，并以此更新航迹的协方差矩阵P；S8：将S5和S6中预测的航迹与量测航迹进行第一次关联，得到所有关联列表，关联列表包括预测航迹与量测航迹的索引和关联权值；S9：提高S8中关联所使用的关联权值，将剩余未关联的预测航迹与量测航迹进行第二次关联，将关联结果合并到S8得到的关联列表；S10：基于关联权值，从关联列表获取最优关联，得到最终的关联列表；S11：对关联列表中关联的航迹进行更新，包括：根据被关联的量测航迹的tick值，自适应设置观测噪声R；根据航迹协方差矩阵P、观测噪声R和观测矩阵H计算得到卡尔曼增益K；根据量测航迹、观测矩阵H和航迹预测值，计算得到残差；由卡尔曼增益K、残差和航迹预测值，更新航迹预测结果；由卡尔曼增益K、观测矩阵H和航迹协方差矩阵P，更新航迹协方差矩阵P结果，且航迹质量参数中的寿命参数tick加1；S12：对未取得关联的航迹中满足删除判定条件的航迹在暂态航迹列表中删除，不满足删除判定条件的航迹，且航迹预测是采用S5进行正常航迹预测得到的，重新采用S6的方法对航迹进行重新预测；S13：对未取得关联的量测航迹进行初始航迹生成；S14：对更新后的暂态航迹列表中，满足上报条件的航迹进行上报；返回S5，进行下一帧航迹的预测。2CN115542307A权利要求书2/2页2.如权利要求1所述的一种基于高精度地图的高速场景多雷达航迹融合方法，其特征在于，所述S9中，对于关联权值全部增加设定的固定值，使得第二次关联的关联权值大于第一次的关联权值。3.如权利要求1所述的一种基于高精度地图的高速场景多雷达航迹融合方法，其特征在于，所述步进值为10米。4.如权利要求1所述的一种基于高精度地图的高速场景多雷达航迹融合方法，其特征在于，所述S10中，利用贪心算法依次从关联列表获取最优关联。5.如权利要求1所述的一种基于高精度地图的高速场景多雷达航迹融合方法，其特征在于，所述S12中，将航迹质量参数中的外推