基于图优化的集群场景智能认知方法与流程

本申请涉及定位技术的，尤其是涉及基于图优化的集群场景智能认知方法。

背景技术：

1、同时定位与建图(simultaneous localization and mapping，slam)是当今自动控制领域重要的研究方向。智能体仅通过在特定的路径上行走并通过多种传感器获得自身及环境的信息，便可实时地获得自身的位置和运动路径，并且建立周围场景的地图。场景的认知则再次基础上添加了周围场景的语义分割信息，使得地图所包含信息量更大。

2、传统的slam技术采用例如卡尔曼滤波的优化方法，算法复杂，计算量大，准确率相对较低。在实际的运用中，智能体往往不能搭载太多的传感器，或者需要配置积算能力较高的芯片，否则会因为数据过多而导致积算缓慢不能实时建图，因此在实际操作中效果欠佳。

3、同时，当前的slam技术主要基于单智能体的巡航和数据采集。由于整个定位与建图过程中只有一个智能体参与，偶然误差在整个定位过程中会起到较大的影响，因此单智能体定位的鲁棒性和精确性都较低。同时，单智能体还会导致定位整个定位过程耗时长的问题。

技术实现思路

1、为了提升定位效率，并降低场景认知误差及耗时，本申请提供了基于图优化的集群场景智能认知方法。

2、一种基于图优化的集群场景智能认知方法，其特征在于，所述基于图优化的集群场景智能认知方法包括：

3、基于rgb-d相机获取带有语义标签的点云数据；

4、基于gnss-rtk获取与多个智能体相互对应的多个位置信息；

5、基于多个所述位置信息获取任意两个所述智能体之间的无线测距信息；

6、基于所述无线测距信息构建多个所述智能体之间的多智能体因子图，并基于时间信息和所述多智能体因子图获取多个所述智能体的姿位信息，所述姿位信息包括位置信息和姿态信息；

7、基于所述点云数据和所述姿态信息构建三维地图。

8、可选地，所述基于rgb-d相机获取带有语义标签的点云数据包括：

9、基于所述rgb-d相机获取rgb图片和深度图片；

10、基于所述深度图片获取以所述智能体为坐标系的所述点云数据；

11、将所述rgb图片输入经过预训练的卷积神经网络进行语义分割，使所述rgb图片中的每个像素获得一个所述语义标签；

12、将所述语义标签添加至所述点云数据中。

13、可选地，所述基于gnss-rtk获取与多个智能体相互对应的多个位置信息包括：

14、基于所述位置信息获取gnss-rtk数据并构建gnss-rtk因子：

15、

16、其中，为gnss-rtk因子；为第k个所述智能体在时刻t的实际位置信息；exp为自然指数函数；第k个所述智能体在时刻t的观测值；‖‖∑gnss为马氏范数。

17、可选地，所述基于多个所述位置信息获取任意两个所述智能体之间的无线测距信息包括：

18、基于所述无线测距信息获取无线测距数据并构建无线测距因子：

19、

20、其中，为无线测距因子；exp为自然指数函数；hk()为无线测距数据；为第j个所述智能体与第k个所述智能体在时刻t的实际距离信息；为第j个所述智能体与第k个所述智能体在时刻t的观测距离信息；为马氏范数。

21、可选地，所述基于所述时间信息和所述多智能体因子图获取多个所述智能体的姿位信息包括：

22、多个所述智能体在每个时刻的所述位置信息和所述姿态信息通过如下公式获得：

23、

24、其中，χ*为预测得到的最优的各所述智能体在每个时刻的实际位姿信息；为使得括号中式子的值最大时对应χ值；fprior(x)为先验因子；为gnss-rtk因子；为第k个所述智能体在时刻t的实际位置信息；为无线测距因子；为第j个所述智能体与第k个所述智能体在时刻t的实际距离信息。

25、可选地，所述基于所述点云数据和所述姿态信息构建三维地图包括：

26、按所述点云数据中每个点的第二位置信息将所述点云数据聚类为多个体素v；

27、对于体素v中的点集pv＝{p1,p2,…,pm}，体素v的坐标xv为这些点的坐标平均值。

28、可选地，基于图优化的集群场景智能认知方法，其特征在于，还包括：

29、通过权重投票获得体素的第二语义标签；

30、具体地，对于属于体素v的点集pv＝{p1,p2,…,pm}中的每一个点pi，都有一个对应的语义标签li，i为点集pv中点的编号，m为点集pv中点的数量；对于每一个点pi，定义其权重wi为：

31、

32、其中di为点pi到体素中心坐标xv的欧氏距离，其计算公式为：

33、

34、其中为点xv在传感器自身坐标系下的x，y，z轴坐标，为点pi在传感器自身坐标系下的x，y，z轴坐标；

35、将每个语义标签li转化为独热码ci＝(0,0,…,1,…,0)，则每个体素的标签加权值为：

36、

37、选取标签加权值cv中最大的一项，其对应的标签lv则为这个三维体素所对应的所述第二语义标签，并将所述第二语义标签添加到所述三维地图中。

38、综上所述，本申请至少包括以下有益技术效果：

39、1.通过采用多个智能体进行定位，具有高鲁棒性和精确性。相较于对偶然误差敏感性大的单智能体系统，多智能体系统能够在多个智能体之间实现协同工作，并且在面对各种复杂环境和干扰时保持稳定性和准确性。

40、2.通过采用图优化的方法，使得整个系统具有快速的处理速度。相较于传统的滤波优化定位方法，图优化是一种高效的定位优化方法，能够在处理大规模数据时保持较低的计算复杂度。

41、3.在建图的基础上添加了更细化的场景分割信息。这种详细的场景分割信息能够提供对地图环境的更准确和全面的理解。这对于后续的应用，比如自动驾驶系统的感知和理解道路、障碍物和其他交通参与者等功能有益。

1.一种基于图优化的集群场景智能认知方法，其特征在于，所述基于图优化的集群场景智能认知方法包括：

2.如权利要求1所述的基于图优化的集群场景智能认知方法，其特征在于，所述基于rgb-d相机获取带有语义标签的点云数据包括：

3.如权利要求1所述的基于图优化的集群场景智能认知方法，其特征在于，所述基于gnss-rtk获取与多个智能体相互对应的多个位置信息包括：

4.如权利要求1所述的基于图优化的集群场景智能认知方法，其特征在于，所述基于多个所述位置信息获取任意两个所述智能体之间的无线测距信息包括：

5.如权利要求1所述的基于图优化的集群场景智能认知方法，其特征在于，所述基于所述时间信息和所述多智能体因子图获取多个所述智能体的姿位信息包括：

6.如权利要求1所述的基于图优化的集群场景智能认知方法，其特征在于，所述基于所述点云数据和所述姿态信息构建三维地图包括：

7.如权利要求6所述的基于图优化的集群场景智能认知方法，其特征在于，还包括：