基于城市峡谷稀疏特征地图约束的动态定位方法和装置

技术领域

本发明属于无人车自主定位与导航技术领域，具体涉及一种基于城市峡谷稀疏特征地图约束的动态定位方法和装置。

背景技术

当前，以泛在定位服务为牵引的新兴高新技术产业蓬勃发展，无人车、无人机、无人船等多种移动无人智能设备对导航系统提出了连续、稳定、可靠的定位要求。特别地对于校园无人物流车、智能无人驾驶汽车而言，保证其在城市峡谷等全球导航卫星拒止环境下的准确可靠定位仍是领域中非常具有挑战性的技术难题之一。

由于高层建筑的遮挡，在开放环境下可实现高精度动态定位的卫星无线电通信技术，在城市峡谷中却受到严重的限制。尽管也存在如伪卫星、UWB等可缓解建筑物遮挡问题的定位方法，但是建筑表面反射带来的多径信号干扰问题仍难以解决。随着同步定位与制图技术的蓬勃发展，基于高清地图的定位方法被认为是解决卫星定位“最后一公里”难题的最有潜力技术之一。

目前，相关科研机构正紧锣密鼓地研制智能驾驶汽车高精度地图标准。其最显著特点是厘米级别的高精度三维坐标和非常丰富的地图要素，特别是对城市道路交通标线、交通标识牌、障碍物等的详细语义标注。然而，对于特大城市高精度地图的制作、维护和更新而言，这是一个庞大、复杂且昂贵的综合型工程。在使用高精度地图进行匹配定位方面，有常用的几何匹配、拓扑匹配、智能学习匹配和滤波估计等，其中匹配的精度与初始的概略位姿精度紧密相关，特别是几何、拓扑匹配方法对异常粗差较为敏感。另外，由于高清地图要素的丰富稠密特点，地图检索匹配过程将占用大量的计算资源，因此复杂的匹配定位算法实用性较低，通常被设计在算法的后端通过离线的方式运行。需要特别注意的是，在利用无人车扫描的局部地图与整个高精度地图实时匹配定位过程中，行人、车流动态物体不仅会遮挡激光雷达传感器的扫描视野，还会形成错误的匹配特征，导致匹配估计模型中正确匹配信息过少，错误匹配信息过多，极大地降低了估计参数的准确性和鲁棒性。

总的来说，在城市复杂环境中基于高精度地图的高精度定位技术而言，主要存在三个方面的技术难题：一是由于高精度地图的丰富性和复杂性，稠密的点云地图会导致地图数据在通信传输、匹配检索和维护更新方面增加应用难度；二是计算过于复杂的匹配定位算法通常需要后端离线运行，难以满足实时高效的计算要求；三是初始的概略位置精度和动态物体特征的干扰，会极大地降低匹配定位地稳定性和可靠性。因此，目前的高精度地图匹配定位技术还需要较高的技术应用门槛，难以满足服务于大众的低成本、高精度的城市定位服务要求，急需一种更高效、稳健且可靠的实用型地图匹配定位方法。

发明内容

为解决上述问题，本发明公开了一种基于城市峡谷稀疏特征地图约束的动态定位方法和装置；通过借鉴大地测量领域导线布设测量的思想，设计一种基于道路里程节点为主线的城市峡谷稀疏特征点云地图结构；然后，基于所建的稀疏特征地图，利用改进的激光雷达里程计算法实时估计无人车的位姿参数；最后，结合因子图算法对位姿参数估值进行平滑优化，从而提高了高精度点云地图在分割、拼接、检索及通信传输方面的便捷性和灵活性，实现复杂城市峡谷场景下无人车的动态高精度定位。

为达到上述目的，本发明的技术方案如下：

一种基于城市峡谷稀疏特征地图约束的动态定位方法，包括以下步骤：

S1、根据城市道路形状设计稀疏特征地图的架构，以固定的里程间隔选取里程节点形成地图结构主线，并将提取特征后的先验点云地图以节点为单位进行分割处理；

S2、利用所述里程节点高程信息剔除各节点上的先验点云地图近地面点云，以获得由各节点组成的仅包含角点特征、面特征(非地面)的先验高精度稀疏特征点云地图；

S3、基于所述的稀疏特征点云地图，利用2D激光雷达匹配算法估计无人车的实时位姿参数估值信息；

S4、结合因子图算法，构造约束因子节点对所述的实时位姿估值进行平滑优化，从而获得无人车在城市峡谷道路行驶过程中的动态导航定位信息；

作为一种优选方案，所述的稀疏特征地图架构设计，依据下述过程获得：

首先，根据依据城市道路中线进行插值，以获得各个里程节点的三维坐标。所述三维坐标按下式获得：

式中，e

然后，将提取特征后的先验点云地图以里程节点为单位进行分割处理，依据下述过程获得：

式中，e

作为一种优选方案，优先的，所述固定的里程间隔预设值为10m。

作为一种优选方案，所述利用里程节点高程信息剔除各节点上的先验点云地图近地面点云，依据下述过程获得：

式中，u

作为一种优选方案，所述利用2D激光雷达里程计算法匹配估计无人车的实时位姿参数估值信息，依据下述过程获得：

首先，借助常规的激光雷达里程计前端LOAM算法，对无人车上安装的激光雷达传感器当前帧扫描的点云筛选提取得到面、角点特征，并估算出初步的概略位姿信息，从而获得经过此概略位姿初步纠正后的当前帧面、角点特征点云。

然后，利用所述的初步概略位姿中的三维坐标信息，计算寻找与该道路里程节点中最近的节点，计算公式为：

式中，x

最后，依据所述的当前帧面、角点特征点云分别与所述的最近里程节点地图中的立面、角点特征分别进行匹配，利用2D激光雷达匹配算法估计无人车的实时位姿参数估值信息。

作为一种优选方案，所述2D激光雷达匹配算法，依据下述过程获得：

首先，建立当前帧角点特征的点到地图中的角点特征所在直线的距离方程：

式中，p

式中，p

式中，J

利用牛顿迭代算法估计位移参数：

式中，

作为一种优选方案，所述构造约束因子节点对所述的实时位姿估值进行平滑优化，可依据下述过程获得：

首先，利用常规LOAM算法估计关键帧间的相对位姿参数构造3D里程计因子，建立因子图模型；

然后，利用所述的2D激光雷达匹配算法估算的位移参数，并结合里程节点的高程，构造成3D位置约束因子添加到所建的因子图模型当中；

最后，对整个因子图模型进行平滑更新，输出优化后的位姿参数作为无人车在城市峡谷道路行驶过程中的动态导航定位信息。

由此构建了一种基于城市峡谷稀疏特征地图约束定位装置，其包括处理器和存储器，所述存储器存储有稀疏特征地图约束定位程序，所述程序在被处理器运行时用于实现方案一所述的基于城市峡谷稀疏特征地图约束定位方法。

本发明的有益效果是：

(1)通过设计一种基于道路里程节点的稀疏特征地图架构，使得在地图匹配定位应用过程中，可直接通过寻找最近的里程节点来检索局部的高精度地图，避免了对整个地图所有点云数据进行轮询式检索判断，从而大大减少地图检索时间。通过里程节点来局部化分割地图的方式，可以有效避免匹配到错误的特征(如某些法向量一致或相似但位置相差较远的面特征)，同时也可避免将整个地图分发传输给终端用户，而只需将与终端距离较近的里程节点上局部地图打包传输即可，大大降低实时通信带宽的要求。整个地图可以通过以里程节点为单位进行局部的更新和拼接，当某个地段场景发生改变时(如新建楼宇、路灯等)，可通过寻找其所属里程节点上的地图进行局部化更新，然后便可与周围节点上的地图进行拼接组装，构成更新后的完整高精度地图。

(2)由于激光雷达通常安装在无人车顶，因此近地面上的点云通常占据整个点云集的一半以上。然而，地面点云对于定位而言，一般仅在高程方面具有强约束力，对于其他位姿参数的约束力则较弱。因此，通过利用里程节点上的高程信息去代替近地面点云对定位的约束效力，可以有效降低点云地图的数据量，使得特征地图具有较好的稀疏特性。通过观测分析可知，在城市道路中的动态物体(移动汽车、行人等)，通常分布于离地面数米的范围之内。因此，通过里程节点高程剔除掉近地面点云可以有效排除动态物体对地图匹配定位的干扰。

(3)通过利用2D激光雷达里匹配定位算法，可以有效避免估计模型的过度参数化，提高参数估值的稳定性，同时结合因子图算法可实现进一步的位姿参数的平滑优化。

附图说明

图1是实施例1所述基于城市峡谷稀疏特征地图约束的动态定位方法实施流程图；

图2是实施例1所述基于城市峡谷稀疏特征地图约束的动态定位方法的稀疏特征地图架构设计示意图；

图3是实施例1所述基于城市峡谷稀疏特征地图约束的动态定位方法的立面特征点云地图；

图4是实施例1所述基于城市峡谷稀疏特征地图约束的动态定位方法的角点特征点云地图；

图5是实施例1所述基于城市峡谷稀疏特征地图约束的动态定位方法的地图信息统计表；

图6是实施例1所述基于城市峡谷稀疏特征地图约束的动态定位方法的动态定位和测姿的误差累积分布图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

结合图1和图2所示，实施例1公开一种基于城市峡谷稀疏特征地图约束的动态定位方法，具体步骤如下：

S1、根据城市道路形状设计稀疏特征地图的架构，以固定的里程间隔选取里程节点形成地图结构主线，并将提取特征后的先验点云地图以节点为单位进行分割处理，依据下述过程获得：

首先，根据依据城市道路中线进行插值，以获得各个里程节点的三维坐标。所述三维坐标按下式获得：

式中，e

然后，将提取特征后的先验点云地图以里程节点为单位进行分割处理，依据下述过程获得：

式中，e

S2、利用所述里程节点高程信息剔除各节点上的先验点云地图近地面点云，以获得由各节点组成的仅包含角点特征、面特征(非地面)的先验高精度稀疏特征点云地图，依据下述过程获得：

式中，u

S3、基于所述的稀疏特征点云地图，利用2D激光雷达匹配算法估计无人车的实时位姿参数估值信息，依据下述过程获得：

首先，借助常规的激光雷达里程计前端LOAM算法，对无人车上安装的激光雷达传感器当前帧扫描的点云筛选提取得到面、角点特征，并估算出初步的概略位姿信息，从而获得经过此概略位姿初步纠正后的当前帧面、角点特征点云。

然后，利用所述的初步概略位姿中的三维坐标信息，计算寻找与该道路里程节点中最近的节点，计算公式为：

式中，x

最后，依据所述的当前帧面、角点特征点云分别与所述的最近里程节点地图中的立面、角点特征分别进行匹配，利用2D激光雷达匹配算法估计无人车的实时位姿参数估值信息。

所述的2D激光雷达匹配算法，依据下述过程获得：

首先，建立当前帧角点特征的点到地图中的角点特征所在直线的距离方程：

式中，p

式中，p

式中，J

利用牛顿迭代算法估计位移参数：

式中，

S4、结合因子图算法，构造约束因子节点对所述的实时位姿估值进行平滑优化，从而获得无人车在城市峡谷道路行驶过程中的动态导航定位信息，依据下述过程获得：

首先，利用常规LOAM算法估计关键帧间的相对位姿参数构造3D里程计因子，建立因子图模型；

然后，利用所述的2D激光雷达匹配算法估算的位移参数，并结合里程节点的高程，构造成3D位置约束因子添加到所建的因子图模型当中；

最后，对整个因子图模型进行平滑更新，输出优化后的位姿参数作为无人车在城市峡谷道路行驶过程中的动态导航定位信息

相应的，实施例2公开一种基于城市峡谷稀疏特征地图约束定位装置，该装置包括处理器和存储器，所述存储器存储有稀疏特征地图约束定位程序，所述程序在被处理器运行时，执行实施例1所述的步骤S1～S4对应的指令。

需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。