目标运动状态的优化估计方法、存储介质以及智能设备与流程

本技术涉及自动驾驶领域，具体提供一种目标运动状态的优化估计方法、存储介质以及智能设备。

背景技术：

1、自动驾驶环境中，对动态目标准确的运动状态的准确估计是感知周围环境的关键，规划和决策系统做出正确决策的基础，对于实现安全而高效的自主导航至关重要。

2、与自车状态估计相比，对动态目标运动状态的估计由于缺乏车身内传感器，只能通过观测进行估计来获取其状态信息，具有较大难度。

3、目前，为了解决动态目标运动状态难以准确估计的问题，现有的方法往往面临传感器单一模态的限制，导致在某些情况下难以准确估计目标状态。

4、相应地，本领域需要一种新的目标运动状态的优化估计方案来解决上述问题。

技术实现思路

1、为了克服上述缺陷，提出了本技术，提供一种目标运动状态的优化估计方法、存储介质以及智能设备，以解决或至少部分地解决动态目标运动状态难以准确估计的技术问题。

2、在第一方面，本技术提供一种目标运动状态的优化估计方法，包括：

3、通过激光雷达获取目标的第一时序点云观测数据；

4、通过毫米波雷达获取所述目标的第二时序点云观测数据；

5、基于所述第一时序点云观测数据和第二时序点云观测数据，得到所述目标的全时序点云观测数据；以及

6、将所述全时序点云观测数据输入因子图对所述目标的运动状态进行优化，得到所述时序中各观测点的运动状态优化估计结果。

7、在上述目标运动状态的优化估计方法的一个技术方案中，所述基于所述第一时序点云观测数据和第二时序点云观测数据，得到所述目标的全时序点云观测数据，包括：

8、对所述第一时序点云观测数据进行校正以修正各观测点的点云数据在尺寸和位置上的误差，从而得到已校正时序点云数据；

9、基于所述已校正时序点云数据从所述第二时序点云观测数据中去除异常观测点的点云数据，从而得到已过滤时序点云数据。

10、在上述目标运动状态的优化估计方法的一个技术方案中，所述对所述第一时序点云观测数据进行校正以修正各观测点的点云数据在尺寸和位置上的误差，从而得到已校正时序点云数据，包括：

11、固定距离所述激光雷达最近的目标面，统一所述第一时序点云观测数据中各观测点的点云数据对应的第一3d检测框的尺寸，从而得到所述已校正时序点云数据及对应的第三3d检测框。

12、在上述目标运动状态的优化估计方法的一个技术方案中，所述基于所述已校正时序点云数据从所述第二时序点云观测数据中去除异常观测点的点云数据，从而得到已过滤时序点云数据，包括：

13、获取所述第二时序点云观测数据中各观测点的点云数据对应的第二3d检测框以及第一速度观测数据；

14、获取所述已校正时序点云数据中各观测点的点云数据对应的第二速度观测数据；

15、基于所述第二3d检测框、第一速度观测数据、第三3d检测框以及第二速度观测数据进行目标匹配，判断异常观测点。

16、在上述目标运动状态的优化估计方法的一个技术方案中，所述获取所述已校正时序点云数据中各观测点的点云数据对应的第二速度观测数据，包括：

17、基于所述第三3d检测框的中心位置得到的目标轨迹，对所述目标轨迹进行拟合得到所述目标的初始位置观测数据；

18、对所述初始位置观测数据进行微分，得到所述第二速度观测数据。

19、在上述目标运动状态的优化估计方法的一个技术方案中，所述基于所述第二3d检测框、第一速度观测数据、第三3d检测框以及第二速度观测数据进行目标匹配，判断异常观测点，包括：

20、判断所述第二3d检测框与所述第三3d检测框的重合度；

21、若重合度大于第一重合度阈值，则判断为正常观测点；

22、若重合度不大于所述第一重合度阈值但大于第二重合度阈值，基于所述第一速度观测数据和第二速度观测数据进一步判断；

23、若重合度不大于所述第二重合度阈值，则判断为异常观测点。

24、在上述目标运动状态的优化估计方法的一个技术方案中，所述基于所述第一速度观测数据和第二速度观测数据进一步判断，包括：

25、若所述第一速度观测数据和第二速度观测数据的差值的平方小于差值阈值，则判断为正常观测点，否则判断为异常观测点。

26、在上述目标运动状态的优化估计方法的一个技术方案中，所述将所述全时序点云观测数据输入因子图对所述目标的运动状态进行优化，得到所述时序中各观测点的运动状态优化估计结果，包括：

27、基于所述全时序点云观测数据获取激光雷达观测因子以及毫米波雷达观测因子；

28、基于运动模型获取运动转移因子，其中所述运动转移因子约束时序中相邻观测点的运动状态；

29、以所述目标的运动状态作为因子图节点，以所述激光雷达观测因子、毫米波雷达观测因子以及运动转移因子为因子图因子，进行因子图迭代，直至达到收敛条件，得到所述时序中各观测点的运动状态优化估计结果。

30、在上述目标运动状态的优化估计方法的一个技术方案中，所述以所述目标的运动状态作为因子图节点，以所述激光雷达观测因子、毫米波雷达观测因子以及运动转移因子为因子图因子，进行因子图迭代，直至达到收敛条件，得到所述时序中各观测点的运动状态优化估计结果，包括：

31、基于所述激光雷达观测因子、毫米波雷达观测因子以及运动转移因子对因子图迭代；

32、获取每次因子图迭代后的运动状态以及因子图误差；

33、基于相邻两次迭代后的因子图误差获取误差变化；

34、判断所述误差变化是否小于预设阈值，若是迭代结束；

35、以最后一次因子图迭代后的各观测点的运动状态作为优化估计结果。

36、在第二方面，本技术提供一种计算机可读存储介质，其中存储有多条程序代码，其特征在于，所述程序代码适于由处理器加载并运行以执行上述目标运动状态的优化估计方法的技术方案中任一项技术方案所述的目标运动状态的优化估计方法。

37、在第三方面，本技术提供一种智能设备，包括：

38、至少一个处理器；

39、以及，与所述至少一个处理器通信连接的存储器；

40、其中，所述存储器中存储有计算机程序，所述计算机程序被所述至少一个处理器执行时实现上述目标运动状态的优化估计方法的技术方案中任一项技术方案所述的目标运动状态的优化估计方法。

41、方案1.一种目标运动状态的优化估计方法，其特征在于，包括：

42、通过激光雷达获取目标的第一时序点云观测数据；

43、通过毫米波雷达获取所述目标的第二时序点云观测数据；

44、基于所述第一时序点云观测数据和第二时序点云观测数据，得到所述目标的全时序点云观测数据；以及

45、将所述全时序点云观测数据输入因子图对所述目标的运动状态进行优化，得到所述时序中各观测点的运动状态优化估计结果。

46、方案2.根据方案1所述的方法，其特征在于，所述基于所述第一时序点云观测数据和第二时序点云观测数据，得到所述目标的全时序点云观测数据，包括：

47、对所述第一时序点云观测数据进行校正以修正各观测点的点云数据在尺寸和位置上的误差，从而得到已校正时序点云数据；

48、基于所述已校正时序点云数据从所述第二时序点云观测数据中去除异常观测点的点云数据，从而得到已过滤时序点云数据。

49、方案3.根据方案2所述的方法，其特征在于，所述对所述第一时序点云观测数据进行校正以修正各观测点的点云数据在尺寸和位置上的误差，从而得到已校正时序点云数据，包括：

50、固定距离所述激光雷达最近的目标面，统一所述第一时序点云观测数据中各观测点的点云数据对应的第一3d检测框的尺寸，从而得到所述已校正时序点云数据及对应的第三3d检测框。

51、方案4.根据方案3所述的方法，其特征在于，所述基于所述已校正时序点云数据从所述第二时序点云观测数据中去除异常观测点的点云数据，从而得到已过滤时序点云数据，包括：

52、获取所述第二时序点云观测数据中各观测点的点云数据对应的第二3d检测框以及第一速度观测数据；

53、获取所述已校正时序点云数据中各观测点的点云数据对应的第二速度观测数据；

54、基于所述第二3d检测框、第一速度观测数据、第三3d检测框以及第二速度观测数据进行目标匹配，判断异常观测点。

55、方案5.根据方案4所述的方法，其特征在于，所述获取所述已校正时序点云数据中各观测点的点云数据对应的第二速度观测数据，包括：

56、基于所述第三3d检测框的中心位置得到的目标轨迹，对所述目标轨迹进行拟合得到所述目标的初始位置观测数据；

57、对所述初始位置观测数据进行微分，得到所述第二速度观测数据。

58、方案6.根据方案4所述的方法，其特征在于，所述基于所述第二3d检测框、第一速度观测数据、第三3d检测框以及第二速度观测数据进行目标匹配，判断异常观测点，包括：

59、判断所述第二3d检测框与所述第三3d检测框的重合度；

60、若重合度大于第一重合度阈值，则判断为正常观测点；

61、若重合度不大于所述第一重合度阈值但大于第二重合度阈值，基于所述第一速度观测数据和第二速度观测数据进一步判断；

62、若重合度不大于所述第二重合度阈值，则判断为异常观测点。

63、方案7.根据方案6所述的方法，其特征在于，所述基于所述第一速度观测数据和第二速度观测数据进一步判断，包括：

64、若所述第一速度观测数据和第二速度观测数据的差值的平方小于差值阈值，则判断为正常观测点，否则判断为异常观测点。

65、方案8.根据方案1-7中任一项所述的方法，其特征在于，所述将所述全时序点云观测数据输入因子图对所述目标的运动状态进行优化，得到所述时序中各观测点的运动状态优化估计结果，包括：

66、基于所述全时序点云观测数据获取激光雷达观测因子以及毫米波雷达观测因子；

67、基于运动模型获取运动转移因子，其中所述运动转移因子约束时序中相邻观测点的运动状态；

68、以所述目标的运动状态作为因子图节点，以所述激光雷达观测因子、毫米波雷达观测因子以及运动转移因子为因子图因子，进行因子图迭代，直至达到收敛条件，得到所述时序中各观测点的运动状态优化估计结果。

69、方案9.根据方案8所述的方法，其特征在于，所述以所述目标的运动状态作为因子图节点，以所述激光雷达观测因子、毫米波雷达观测因子以及运动转移因子为因子图因子，进行因子图迭代，直至达到收敛条件，得到所述时序中各观测点的运动状态优化估计结果，包括：

70、基于所述激光雷达观测因子、毫米波雷达观测因子以及运动转移因子对因子图迭代；

71、获取每次因子图迭代后的运动状态以及因子图误差；

72、基于相邻两次迭代后的因子图误差获取误差变化；

73、判断所述误差变化是否小于预设阈值，若是迭代结束；

74、以最后一次因子图迭代后的各观测点的运动状态作为优化估计结果。

75、方案10.一种计算机可读存储介质，其中存储有多条程序代码，其特征在于，所述程序代码适于由处理器加载并运行以执行方案1至9中任一项所述的目标运动状态的优化估计方法。

76、方案11.一种智能设备，其特征在于，包括：

77、至少一个处理器；

78、以及，与所述至少一个处理器通信连接的存储器；

79、其中，所述存储器中存储有计算机程序，所述计算机程序被所述至少一个处理器执行时实现方案1至9中任一项所述的目标运动状态的优化估计方法。

80、本技术上述一个或多个技术方案，至少具有如下一种或多种

81、有益效果：

82、在实施本技术的技术方案中，融合了激光雷达和毫米波雷达两种模态的数据，对动态目标的进行因子图优化，以准确估计动态目标的运动状态，精度高、鲁棒性强；既能利用激光雷达的高空间分辨率，又能借助毫米波雷达的高速度分辨率，为动态目标的运动状态的优化估计提供了更为全面和精准的信息。