(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210897015.7 (22)申请日 2022.07.28 (71)申请人 苏州轻棹科技有限公司 地址 215100 江苏省苏州市相城区高铁新 城青龙港路66号领寓商务广场1幢21 层2101-2108室 (72)发明人 李经纬　赵政　柯岩　张雨　 (74)专利代理 机构 北京慧诚智道知识产权代理 事务所 (特殊普通合伙) 11539 专利代理师 殷炳蕾 (51)Int.Cl. G06K 9/62(2022.01) G06V 10/80(2022.01) G06V 20/56(2022.01)G01S 7/41(2006.01) G01S 7/48(2006.01) (54)发明名称 一种多传感器融合的目标轨 迹处理方法 (57)摘要 本发明实施例涉及一种多传感器融合的目 标轨迹处理方法， 所述方法包括： 在时刻t获取与 指定目标对应的第一观测数据、 第一目标中心 点， 并获取自车的第一自车中心点； 进行标定参 考点确定生成第一标定参考点； 若观测时刻i＝t 则进行队列记录添加并设置第一轨迹处理模式 为第一模式； 若观测时刻i<t， 则在i满足最近时 段范围时进行队列记录插入并设置第一轨迹处 理模式为第二模式,在i不满足最近时段范围时 将第一标定参考点丢弃并设置第一轨迹处理模 式为忽略模式； 第一轨迹处理模式为忽略模式时 不做任何处理； 为第一模式时进行最新轨迹预测 处理； 为第二模式时进行历史轨迹修正处理。 通 过本发明可以保证目标轨 迹的平滑性与鲁棒 性。 权利要求书5页 说明书16页 附图2页 CN 115169493 A 2022.10.11 CN 115169493 A 1.一种多传感器融合的目标轨 迹处理方法， 其特 征在于， 所述方法包括： 在时刻t获取与指定目标对应的传感器观测数据作为对应的第一观测数据Mc,i， 并获取 所述指定目标最近时刻的中心点位置记为对应的第一目标中心 点， 并获取自车最近时刻的 中心点位置作为对应的第一自车中心点； 传感器类型c包括激光雷达、 摄像头和毫米波雷 达； 观测时刻i≤时刻t； 根据所述传感器类型c、 所述第一观测数据Mc,i、 所述观测时刻i、 所述时刻t和所述第一 自车中心点进行 标定参考点确定生成对应的第一标定参 考点Pi； 若所述观测时刻i等于所述时刻t， 则根据所述第一标定参考点Pi在预设的反映目标运 动状态与参考点坐标对应关系的第一关系队列中进行队列记录添加处理得到对应的第一 添加队列记录， 并设置第一轨 迹处理模式为第一模式； 若所述观测时刻i小于所述时刻t， 则对所述观测时刻i是否满足预设的最近时段范围 进行识别； 若满足， 则根据所述第一标定参考点Pi在所述第一关系队列中进行队列记录插 入处理得到对应的第一插入队列记录， 并设置所述第一轨迹处理模式为第二模式； 若不满 足， 则将所述第一标定参 考点Pi丢弃并设置所述第一轨 迹处理模式为忽略模式； 当所述第一轨迹处理模式为忽略模式时， 不做任何轨迹处理； 当所述第一轨迹处理模 式为第一模式时， 根据所述第一添加队列记录、 所述第一关系队列和所述第一目标中心点 进行最新轨迹预测处理并输出最新的第一目标轨迹； 当所述第一轨迹处理模式为第二模式 时， 根据所述第一插入队列记录、 所述第一关系队列和所述第一 目标中心点进行历史轨迹 修正处理并输出修 正后的第二目标轨 迹。 2.根据权利要求1所述的多传感器融合的目标轨 迹处理方法， 其特 征在于， 当所述传感器类型c为激光雷达时， 所述第一观测数据Mc,i包括第一多边形凸包和/或 第一目标检测框， 所述第一目标检测框包括四个顶点； 当所述传感器类型c为摄像头时， 所 述第一观测数据Mc,i包括第二目标检测框， 所述第二目标检测框包括四个顶 点； 当所述传感 器类型c为毫米波雷达时， 所述第一观测数据Mc,i包括第一雷达观测点； 所述第一目标中心点、 所述第 一自车中心点、 所述第 一多边形凸包、 所述第 一目标检测 框、 所述第二目标检测框和所述第一雷达观测点的坐标系都为同一个鸟瞰视角坐标系； 所述第一关系队列包括多个第一队列记录； 所述第一队列记录包括第一时刻字段、 第 一运动状态字段、 第一传感器类型字段和第一参考点坐标字段； 所述第一时刻字段对应的 时间应满足预设的最近时段范围； 所述第一传感器类型字段的传感器类型包括激光雷达、 摄像头和毫米波雷达 。 3.根据权利要求2所述的多传感器融合的目标轨迹处理方法， 其特征在于， 所述根据 所 述传感器类型c、 所述第一观测数据Mc,i、 所述观测时刻i、 所述时刻t和所述第一自车中心点 进行标定参考点确定生成对应的第一标定参 考点Pi， 具体包括： 当所述传感器类型c为激光雷达时， 对所述第一观测数据Mc,i是否包括所述第一目标检 测框进行确认； 若确认不包括， 则对所述第一多边形凸包进行多边形凸包重心估计并将估 计出的多边形凸包重心作为对应 的所述第一标定参考点Pi； 若确认包括， 则对所述观测时 刻i是否小于所述时刻t进 行识别， 若小于则将所述第一目标检测框的四个顶 点中距离所述 第一自车中心点最远的那个顶点作为对应的所述第一标定参考点Pi， 若不小于则将所述第 一目标检测框的四个顶点中距离所述第一自车中心点最近的那个顶点作为对应的所述第权　利　要　求　书 1/5 页 2 CN 115169493 A 2一标定参 考点Pi； 当所述传感器类型c为摄像头时， 对所述观测时刻i是否小于所述时刻t进行识别； 若小 于， 则将所述第二目标检测框的四个顶点中距离所述第一自车中心 点最远的那个顶点作为 对应的所述第一标定参考点Pi； 若不小于， 则将所述第二目标检测框的四个顶点中距离所 述第一自车中心点 最近的那个顶点作为对应的所述第一标定参 考点Pi； 当所述传感器类型c为毫米波雷达时， 将所述第一雷达观测点作为对应的所述第一标 定参考点Pi。 4.根据权利要求2所述的多传感器融合的目标轨迹处理方法， 其特征在于， 所述根据 所 述第一标定参考点Pi在预设的反映目标运动状态与参考点坐标对应关系的第一关系队列 中进行队列记录添加处 理得到对应的第一添加队列记录， 具体包括： 在第一关系队列的队列尾新增一个所述第一队列记录作为对应的所述第一添加队列 记录； 并在所述第一添加队列记录中， 将所述第一时刻字段设为第一标定参考点Pi的所述 观测时刻i， 并将所述第一运动状态字段设为空， 并将所述第一传感器类型字段设为所述第 一标定参考点Pi对应的所述传感器类型c， 并将所述第一参考点坐标字段设为所述第一标 定参考点Pi在所述鸟瞰视角坐标系下的参 考点坐标。 5.根据权利要求2所述的多传感器融合的目标轨迹处理方法， 其特征在于， 所述根据 所 述第一标定参考点Pi在所述第一关系队列中进行队列记录插入处理得到对应的第一插入 队列记录， 具体包括： 将所述第一关系队列中第一个大于所述第一标定参考点Pi的所述观测时刻i的所述第 一时刻字段对应的所述第一队列 记录记为当前队列 记录； 并在所述当前队列 记录的前一个 队列记录位置插入一个新的所述第一队列记录作为对应的所述第一插入队列 记录； 并在所 述第一插入队列记录中， 将所述第一时刻字段设为所述第一标定参考点Pi的所述观测时刻 i， 并将所述第一运动状态字段设为空， 并将所述第一传感器类型字段设为所述第一标定参 考点Pi对应的所述传感器类型c， 并将所述第一参考点坐 标字段设为所述第一标定 参考点Pi 在所述鸟瞰视角坐标系下的参 考点坐标。 6.根据权利要求2所述的多传感器融合的目标轨迹处理方法， 其特征在于， 所述根据 所 述第一添加队列 记录、 所述第一关系队列和所述第一目标中心 点进行最新轨迹预测处理并 输出最新的第一目标轨 迹， 具体包括： 将所述第一关系队列中所述第一添加队列记录的上一个所述第一队列记录作为对应 的前一队列 记录； 并将所述前一队列 记录的所述第一传感器类型字段作为对应的第一传感 器类型； 当所述第一添加队列记录与 所述前一队列记录的所述第 一传感器类型字段不一致 时， 查询预设的传感器偏移向量列表， 将源传感器字段与所述前一队列 记录的所述第一传感器 类型字段匹配且目标传感器字段与所述第一添加队列记录的所述第一传感器类型字段匹 配的传感器偏移向量记录的偏移向量字段提取出来作为对应的当前偏移向量； 并根据所述 当前偏移向量对所述第一添加队列记录之前的各个所述第一队列记录的所述第一参考点 坐标字段进行坐标偏移 转换； 并将所述第一添加队列 记录之前的各个所述第一队列 记录的 所述第一传感器类型字段均设为与所述第一添加队列记录的所述第一传感器类型字段一 致的传感器类型； 所述传感器偏移向量列表包括多个所述传感器偏移向量记录； 所述传感权　利　要　求　书 2/5 页 3 CN 115169493 A 3