一种基于语义信息的停车场粒子滤波定位方法与流程

本发明属于车辆定位技术领域，具体涉及一种基于语义信息的停车场粒子滤波定位方法。

背景技术

随着自动驾驶技术的快速发展，定位功能几乎成为了自动驾驶的必备功能。当前，自动驾驶通常会使用全球定位系统(Global Positioning System，GPS)对车辆进行定位。然而，在室内或建筑物相对密集的区域，比如停车场这样的场景下，GPS精度很差甚至无法工作，因此，如何在停车场区域内为车辆或其余可移动的设备持续提供稳定且高精度的定位信息就成为急需解决的问题。

现有技术中以激光定位或基于ORB之类的视觉特征识别定位方式为主，但是都各有各的局限性，激光在地上停车场这样的开阔场景下作用有限，而且成本高昂，尚不能普及，而相比之下视觉特征识别的定位方式虽然成本低，但是精度容易受到光线环境变化影响，而且运算强度高，很难保证实时性。因此需要一种新的停车场定位方案。

技术实现要素：

本发明的目的在于提供一种基于语义信息的停车场粒子滤波定位方法，以解决现有的问题。

为实现上述目的，本发明提供如下技术方案：一种基于语义信息的停车场粒子滤波定位方法，包括步骤：

S100判断第一语义目标在语义地图中出现的数量是否小于预设的数量阈值；

S200当所述第一语义目标出现的数量小于预设的所述数量阈值时，在每个所述第一语义目标对应的位置播撒粒子；

S300对所有的所述粒子的位姿求平均，获得车辆在所述语义地图上的定位结果。

作为本发明一种基于语义信息的停车场粒子滤波定位方法优选地，在所述的S200当所述第一语义目标出现的数量小于预设的所述数量阈值时，在每个所述第一语义目标对应的位置播撒粒子与所述的S300对所有的所述粒子的位姿求平均，获得车辆在所述语义地图上的定位结果之间包括步骤：

S210获取车辆CAN数据，根据所述车辆CAN数据计算所述粒子的位姿预测；

S220获取第二语义目标，将所述第二语义目标与语义地图匹配，得到匹配数据，根据所述匹配数据更新粒子权重并重采样，直至粒子收敛。

作为本发明一种基于语义信息的停车场粒子滤波定位方法优选地，在所述的S100判断第一语义目标在语义地图中出现的数量是否小于预设的数量阈值之前包括步骤：

S10预先设定一语义目标出现的数量阈值。

作为本发明一种基于语义信息的停车场粒子滤波定位方法优选地，在所述的S100判断第一语义目标在语义地图中出现的数量是否小于预设的数量阈值之前包括步骤：

S20构建所述语义地图。

作为本发明一种基于语义信息的停车场粒子滤波定位方法优选地，在所述的S100判断第一语义目标在语义地图中出现的数量是否小于预设的数量阈值之前包括步骤：

S30获取所述第一语义目标。

作为本发明一种基于语义信息的停车场粒子滤波定位方法优选地，在所述的S30获取所述第一语义目标之后包括步骤：

S301判断第一语义目标是否包含唯一语义元素。

S302当所述第一语义目标包含所述唯一语义元素时，直接在这个时刻完成粒子初始化以快速收敛。

作为本发明一种基于语义信息的停车场粒子滤波定位方法优选地，所述的S210获取车辆CAN数据，根据所述车辆CAN数据计算所述粒子的位姿预测具体包括步骤：

S2101根据所述车辆CAN数据计算车辆的相对位移量，从而获得每个粒子的位姿预测。

作为本发明一种基于语义信息的停车场粒子滤波定位方法优选地，所述的S220获取第二语义目标，将所述第二语义目标与语义地图匹配，得到匹配数据，根据所述匹配数据更新粒子权重并重采样，直至粒子收敛具体包括步骤：

S2201遍历所有的所述粒子，根据所述粒子的位姿预测，获得位姿预测中语义目标；

S2202换算所述语义目标在语义地图中的位置；

S2203将所述语义目标在语义地图中的位置与车辆当前观察到的第二语义目标的位置进行匹配，得到匹配结果。

作为本发明一种基于语义信息的停车场粒子滤波定位方法优选地，所述的S2203将所述语义目标在语义地图中的位置与车辆当前观察到的第二语义目标的位置进行匹配，得到匹配结果之后包括步骤：

S2204根据所述匹配结果更新所述粒子的权重。

作为本发明一种基于语义信息的停车场粒子滤波定位方法优选地，所述的S2204根据所述匹配结果更新所述粒子的权重之后包括步骤：

S2205根据粒子的权重进行序贯重要性重采样；

S2206附加高斯噪声保持粒子多样性。

本发明与现有技术相比，具有以下有益效果：本发明在车辆行进至看到某些地图中数量小的语义目标时再选择播撒粒子，根据观察到的语义与地图中的语义匹配结果，在所有可能的位置上播撒一定数量比例的粒子，或者在观察到地图中某些唯一的语义元素，比如通道入口、闸机等，可以直接在这个时刻完成粒子初始化以快速收敛，减小粒子数量并加快收敛速度，实现停车场区域内的高精度定位；利用车辆自带的摄像头传感器实现低成本的实时高精度定位；适用于自动驾驶场景。

附图说明

图1为本发明的流程图之一；

图2为本发明的流程图之二；

图3为本发明的流程图之三；

图4为本发明的流程图之四；

图5为本发明的流程图之五；

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1所示，本发明提供如下技术方案：一种基于语义信息的停车场粒子滤波定位方法，包括步骤：

S100判断第一语义目标在语义地图中出现的数量是否小于预设的数量阈值；

S200当所述第一语义目标出现的数量小于预设的所述数量阈值时，在每个所述第一语义目标对应的位置播撒粒子；

S300对所有的所述粒子的位姿求平均，获得车辆在所述语义地图上的定位结果。

本实施例中，数量阈值可以根据使用需求进行设定，例如数量阈值设置为5，则当所获取的第一语义目标在语义地图中出现的数量小于5时，例如其出现的数量为3时，则在该三个第一语义目标对应的位置播撒粒子，然后再对三个位置上播撒的所有粒子的位姿求平均，最终获得车辆在语义地图上的定位结果，如此，能够大大减小粒子的播撒量，从而在停车场区域内实现快速稳定的高精度实时定位。

值得说明的是，语义地图上的语义类别越丰富，目标检测率与精度越高，定位越可靠。

请参阅图2所示，具体地，在所述的S200当所述第一语义目标出现的数量小于预设的所述数量阈值时，在每个所述第一语义目标对应的位置播撒粒子与所述的S300对所有的所述粒子的位姿求平均，获得车辆在所述语义地图上的定位结果之间包括步骤：

S210获取车辆CAN数据，根据所述车辆CAN数据计算所述粒子的位姿预测；

S220获取第二语义目标，将所述第二语义目标与语义地图匹配，得到匹配数据，根据所述匹配数据更新粒子权重并重采样，直至粒子收敛。

本实施例中，随着车辆的运动，将车辆上的传感器返回的数据与语义地图做匹配，对粒子进行重新计算，按照粒子滤波算法中采样与重采样的计算方法，对粒子簇进行实时更新；经过短暂的运动，粒子簇会呈现收敛的趋势，最终收敛成一个较小的粒子簇；对所有粒子进行加权平均计算，得到一个估计位姿，该估计位姿就是该时刻车辆的定位结果，由此完成了车辆初始化的定位工作。

请参阅图3所示，具体地，在所述的S100判断第一语义目标在语义地图中出现的数量是否小于预设的数量阈值之前包括步骤：

S10预先设定一语义目标出现的数量阈值。

具体地，在所述的S100判断第一语义目标在语义地图中出现的数量是否小于预设的数量阈值之前包括步骤：

S20构建所述语义地图。

本实施例中，构建所述语义地图具体包括步骤：

构建栅格地图，停车场中要有明显的路障特征用于障碍物信息的识别；

通过卷积神经网络的目标检测方法识别物体，并得到该物体的语义信息；

利用视觉传感器模拟激光雷达，得到语义激光数据；

根据车辆与视觉传感器位置关系和全局坐标系与车辆的位置关系，以及物体的语义激光数据，构建语义地图。

具体地，在所述的S100判断第一语义目标在语义地图中出现的数量是否小于预设的数量阈值之前包括步骤：

S30获取所述第一语义目标。

请参阅图4所示，具体地，在所述的S30获取所述第一语义目标之后包括步骤：

S301判断第一语义目标是否包含唯一语义元素。

S302当所述第一语义目标包含所述唯一语义元素时，直接在这个时刻完成粒子初始化以快速收敛。

本实施例中，为了减小粒子数量并加快收敛速度，可以在车辆行进至看到某些地图中数量小的语义目标时再选择播撒粒子，根据观察到的语义与地图中的语义匹配结果，在所有可能的位置上播撒一定数量比例的粒子。甚至如果能够观察到地图中某些唯一的语义元素(比如通道入口、闸机等)，可以直接在这个时刻完成粒子初始化以快速收敛。

请参阅图5所示，具体地，所述的S210获取车辆CAN数据，根据所述车辆CAN数据计算所述粒子的位姿预测具体包括步骤：

S2101根据所述车辆CAN数据计算车辆的相对位移量，从而获得每个粒子的位姿预测。

本实施例中，基于车辆CAN线实时提供的轮速、车轮脉冲、方向盘转角等数据，通过航位推算算法计算相对位移量，从而获得每个粒子的位姿预测。

具体地，所述的S220获取第二语义目标，将所述第二语义目标与语义地图匹配，得到匹配数据，根据所述匹配数据更新粒子权重并重采样，直至粒子收敛具体包括步骤：

S2201遍历所有的所述粒子，根据所述粒子的位姿预测，获得位姿预测中语义目标；

S2202换算所述语义目标在语义地图中的位置；

S2203将所述语义目标在语义地图中的位置与车辆当前观察到的第二语义目标的位置进行匹配，得到匹配结果。

本实施例中，遍历所有粒子，根据其位姿预测可以观测到的语义目标并换算其在粒子位姿坐标系中的位置，然后根据当前车辆观察到的车位、减速带、立柱等各种语义目标位置进行匹配，匹配方式可以是ICP等，根据匹配结果来更新粒子的权重。

具体地，所述的S2203将所述语义目标在语义地图中的位置与车辆当前观察到的第二语义目标的位置进行匹配，得到匹配结果之后包括步骤：

S2204根据所述匹配结果更新所述粒子的权重。

具体地，所述的S2204根据所述匹配结果更新所述粒子的权重之后包括步骤：

S2205根据粒子的权重进行序贯重要性重采样；

S2206附加高斯噪声保持粒子多样性。

本发明还提供一种实施例，包括步骤：

S1语义初始定位播撒粒子：

为了减小粒子数量并加快收敛速度，可以在车辆行进至看到某些地图中数量小的语义目标时再选择播撒粒子，根据观察到的语义与地图中的语义匹配结果，在所有可能的位置上播撒一定数量比例的粒子。甚至如果能够观察到地图中某些唯一的语义元素(比如通道入口、闸机等)，可以直接在这个时刻完成粒子初始化以快速收敛。

S2航位推算预测粒子位置：

基于车辆CAN线实时提供的轮速、车轮脉冲、方向盘转角等数据，通过航位推算算法计算相对位移量，从而获得每个粒子的位姿预测。

S3语义匹配更新粒子权重：

遍历所有粒子，根据其位姿预测可以观测到的语义目标并换算其在粒子位姿坐标系中的位置，然后根据当前车辆观察到的车位、减速带、立柱等各种语义目标位置进行匹配，匹配方式可以是ICP等，根据匹配结果来更新粒子的权重。

S4粒子重采样：

根据粒子的权重进行序贯重要性重采样，并附加高斯噪声保持粒子多样性。

S5综合粒子更新定位结果：

根据所有粒子的方差决定当前粒子是否收敛，若收敛则使用所有原生粒子的均值来更新最终定位结果。

通过以上步骤实现的语义粒子滤波，可以在停车场区域内实现快速稳定的高精度实时定位。语义类别越丰富，目标检测率与精度越高，定位越可靠。

本发明还提供一种实施例，包括：

S1000车辆行驶过程中，前视或环视摄像头观测到高精地图中的稀有语义类别的目标，匹配计算结果并播撒粒子完成初始化。

S2000根据车辆CAN数据预测粒子位置，根据车辆实时观测到的各种语义目标匹配地图数据，更新粒子权重并重采样，直至粒子收敛。

综合所有粒子的位姿求平均，获得车辆在该道路上的高精度定位。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。