本发明属于无人驾驶汽车研究领域，特别涉及一种无人车在地下车库中的精确定位方法。
背景技术
近年来，随着新一代人工智能技术的兴起，无人驾驶汽车作为人工智能技术的重要算法验证平台应运而生，并呈现出井喷式的发展，尤其是在国内涌现出了一大批创业公司，直接推动了这一研究领域的快速发展。无人驾驶汽车是一种特殊的轮式移动机器人，其是依靠自身所携带的各类传感器，融合感知道路环境，自动获得车辆定位信息，规划行车路线，控制车辆运动，从而实现车辆的自主行驶功能。
定位技术在无人车研究领域内具有很重要的作用，涉及车辆感知、规划、控制等功能的准确实现。鉴于rtk-gps具有高精度、高可靠性的特点，绝大部分无人驾驶车辆选择用gps来进行定位，然而在当前的城市道路中存在着很多隧道、桥梁、较高的树木等遮挡物，gps信号在这种环境下很容易受到干扰和阻挡，车辆定位精度严重不足，为了解决这一问题，人们提出利用gps+imu的组合定位方法，但其缺点是在长时间的隧道等gps信号受到阻挡的环境中，很容易失效，或者车辆在gps信号受到遮挡的环境中启动，无法确定车辆位姿。
为此，研究者提出要利用车辆自身携带的各类传感器进行感知定位，该方法首先要建立先验地图，然后利用车辆运动模型和惯性导航器件进行定位，这种方法在无gps信号的环境中效果明显，但唯一不足的是这种方法容易受到感知环境数据中噪声或者动态障碍物的影响，从而影响定位精度。
技术实现要素：
本发明的目的在于提供一种无人车在地下车库中的精确定位方法，以解决上述问题。
为实现上述目的，本发明采用以下技术方案：
一种无人车在地下车库中的精确定位方法，该方法包括无人车在地下车库中的精确定位系统，所述无人车在地下车库中的精确定位系统包括地面分割模块m1、特征提取与特征匹配，以及非地面有效点选择模块m2、有效点合并与降采样模块m3、栅格地图产生与三维地图匹配模块m4和初始位姿模块m5；地面分割模块m1连接特征提取与特征匹配，以及非地面有效点选择模块m2和有效点合并与降采样模块m3，特征提取与特征匹配，以及非地面有效点选择模块m2连接有效点合并与降采样模块m3，有效点合并与降采样模块m3连接栅格地图产生与三维地图匹配模块m4，初始位姿模块m5连接栅格地图产生与三维地图匹配模块m4；
地面分割模块m1包括地面分割模块m11、地面点云m13和非地面点云m12；
特征提取与特征匹配，以及非地面有效点选择模块m2包括特征提取m21、特征匹配m22和特征地图m23，
有效点合并与降采样模块m3包括点云合并m31、降采样m33和最终参与匹配点云m34；
栅格地图产生与三维地图匹配模块m4包括格栅地图生成模块m42和地图匹配模块m41，用于确定车辆在地下车库中的精确位姿；
初始位姿模块m5，用于为栅格地图产生与三维地图匹配模块m4提供车辆的初始位姿；
具体包括以下步骤：
步骤1，地面分割模块m11采用高斯过程回归算法实现对完整的三维激光扫描场景的分割，得到地面点云m13和非地面点云m12，地面点云m13和非地面点云m12分别表示为pgrd和pnogrd；地面点云是场景结构中的点，属于有效点云；
步骤2，提取非地面点云的特征，并和事先建立的特征地图进行匹配，选择出场景中的有效特征，进而反推获得非地面有效点云，记为pnogrdvalid；
步骤3，模块m2产生的非地面有效点云pnogrdvalid与模块m1产生的地面点云pgrd参与的合并模块m31，得到全部的有效点云m32，表示为pvalid，再经过降采样处理模块m33，得到最终参与地图匹配的部分有效点云m34，表示为plastvalid。该模块可表示为：
pvalid＝pgrd∪pnogrdvalid，其中
plastvalid＝downsample(pvalid)；
步骤4，基于graph-slam技术实现栅格地图生成，通过点到面测度的配准算法实现地图匹配；假设两个点集为
其中，{pi,qi}∈cpq为两个点集之间的一对对应点；
步骤5，初始位姿模块m5为地图匹配模块m41提供初始位姿信息，匹配得到精确位姿。
进一步的，地面分割模块m11利用高斯回归过程对采集到的激光点云进行分割处理，得到地面点云和包含有墙面、柱子、车辆、行人障碍物的非地面点云，其中地面点云能够在定位过程中提供丰富的俯仰角和横滚角信息，属于有效点云。
进一步的，特征提取m21功能是提取非地面点云的圆柱特征；特征地图m23是在地图创建过程中所标注的车库内部柱子、墙面、以及部分车辆的圆柱特征，为特征匹配m22提供模版；特征匹配m22是选择距离测度，度量特征之间的距离，采用余弦相似性测度：设两个圆柱特征f1和f2的距离为d(f1,f2)，则有
以此来区分点云是否属于车库本身几何结构信息，如果属于则保留，否则删除，得到场景特征m24，为立柱和墙面点云的圆柱特征，最后根据场景特征，反推得到非地面有效点云m25。
进一步的，所述初始位姿模块m5用于为地图匹配模块m41提供足够精确的初始位姿；分为两种情况：第一，当车辆运行正常时，用前一帧定位的结果作为当前车辆定位的初始位姿，第二，当车辆在地下车库中启动时，或者车辆定位失败时，需要重新确定车辆的初始位姿，此时，启动全局重定位机制，确保车辆定位的鲁棒性。
进一步的，提取点云特征时，设计为圆柱体空间搜索，以便尽可能将整个立柱包含进来；搜索空间利用最近邻搜索算法快速实现，圆柱体采用二维欧式距离与高度阈值结合实现。
进一步的，圆柱特征提取流程，针对非地面点云进行处理，最后的输出为圆柱特征；首先寻找关键点，根据相关的一些显著性特点选择关键点，采用随机选取的方法；找到关键点之后，利用最近邻搜索和高度阈值限制的方法找出以关键点为中心的圆柱体内所有点，以这些点为基础，利用主成份分析技术建立局部坐标系；随后进行坐标变换，将圆柱内部所有的点通过坐标变换，转换到激光坐标下，以此来实现圆柱特征的旋转不变性；后续通过每次沿圆柱体中轴旋转固定角度，分别计算圆柱内所有点投影到x-y、x-z、y-z三个平面的统计量，该统计量为图像的高阶统计矩和香农熵，最后通过先后顺序组合成圆柱特征。
进一步的，圆柱特征的计算过程如下：首先分别计算圆柱内所有点sc在x-y、x-z、y-z平面上的投影，投影图像分别表示为ixy(i,j)、ixz(i,j)、iyz(i,j)，然后计算投影图像的中心矩和香农熵，如下公式所示：
其中：
接下来，将圆柱体内部所有点sc绕z轴以固定角度θ旋转，计算投影图像，以及图像的中心矩和香农熵，得到本次旋转后的圆柱特征fk＝{fxy,fxz,fyz}，以此类推，旋转k次之后结束；最后将每次旋转之后计算得到的投影图像统计信息按照一定的排列规律合并起来，构成完整的圆柱特征向量f＝f0∪…∪fk。
在特征计算完毕之后，判断此特征是否属于车库本身结构，以此来保留或是删除该圆柱体内部的点云；
其特征选择策略如下：假设创建的特征地图为fmap＝{fv,fc,fw}，其中fv表示车辆的圆柱特征，fc表示车库中立柱的特征，fw表示墙面的特征，在特征相似性计算过程中，得到当前特征fx与特征地图之间的相似性关系d＝{d(fx,fv),d(fx,fc),d(fx,fw)}，由此，根据如下公式获得圆柱体内的点属性：
<l,dmax>＝argmaxd
其中，l表示特征的标签，1表示立柱，2表示墙面，3表示车辆，dmax表示fx与特征地图的最大相似距离，则最终当前的圆柱特征fx的属性为：
如果条件不满足则为未知特征属性，在后续处理中直接剔除；在地下车库定位中，只有属于车库本身结构的点才会准确定位，车辆、行人等不属于车库本身结构的动态障碍物会造成定位偏差，为此，我们将保留墙面和立柱上的点云，删除车辆等其他动态障碍物上的点；非地面有效点云pnogrdvalid为：
pnogrdvalid＝(pc∪pw)-(pv∪pothers)
其中，pc表示所有立柱上的点，pw表示所有墙面上的点，pv表示所有车辆上的点，pothers表示其他未知物体上的点。
进一步的，坐标变换示意，fuvw表示圆柱内所有点构建的局部坐标，fxyz表示经过坐标转换后的激光坐标系，
其中，ns为关键点ps的法向量。
进一步的，在重定位机制中选择fpfh特征作为全局匹配，启用ransac算法，找出对应关系确定粗定位变换，再根据场景几何一致性检测确定是否有效，以此类推，对每一关键帧做同样的处理，得到一组初始变换结果，最后根据每个初始变换的评估结果，选择最佳的变换作为初始位姿；其中评估函数为：
其中，
与现有技术相比，本发明有以下技术效果：
本发明与常用的差分gps定位系统相比，在城区环境中不容易受到高楼、隧道等高大物体的遮挡而被干扰。该方法能够在完全丧失gps信号的环境中实现鲁棒的定位，唯一的要求是提前生成三维激光占据栅格地图。
本发明与其他地图匹配方法相比较，该方法实时性较好，能够快速的得到精确定位结果。
本发明与其他地图匹配方法相比较，该方法对初始位姿的敏感度较低，能够在复杂的无gps信号的环境中，鲁棒的实现精确定位。
附图说明
图1为本发明的系统整体框架示意图。
图2为本发明的典型搜索空间示意图。
图3为本发明的典型三维激光扫描图。
图4为本发明的圆柱特征提取流程图。
图5为本发明的圆柱特征细节示意图。
图6为本发明的坐标变换示意图。
图7为本发明的三种形状模型示意图。
图8为本发明的三种形状模型的圆柱特征示意图。
图9为本发明的获取初始定位结果示意图。
图10为本发明的地下车库三维占据栅格地图。
图11为本发明的剔除障碍物点云结果图一。
图12为本发明的剔除障碍物点云结果图二。
图13为本发明的车辆定位结果图。
图14为本发明的x、y、z三个方向定位结果变化情况。
具体实施方式
以下结合附图对本发明作详细的说明：
参见图1，为本发明的系统整体框架示意图，包括五个功能模块，分别是：
地面分割模块m1，其目的是通过地面分割模块m11，得到地面点云m13和非地面点云m12，其中地面分割模块采用高斯过程回归算法实现。地面点云和非地面点云分别表示为pgrd和pnogrd；
特征提取与特征匹配，以及非地面有效点选择模块m2，其包括特征提取模块m21，特征匹配模块m22，得到场景特征m24，再反推得到非地面有效点云m25，另外还包括提前制作的特征地图m23，其中特征提取模块m21是在非地面有效点云中提取圆柱特征，该圆柱特征对检测地下车库中的立柱、墙面具有很高的效率，特征匹配模块m22是根据所提取的圆柱特征的特点，找到合适的距离测度对各个特征进行距离度量，在本发明中采用余弦相似性测度：设两个圆柱特征f1和f2的距离为d(f1,f2)，则有
有效点合并与降采样模块m3，包括模块m2产生的非地面有效点云pnogrdvalid与模块m1产生的地面点云pgrd参与的合并模块m31，得到全部的有效点云m32，表示为pvalid，再经过降采样处理模块m33，得到最终参与地图匹配的部分有效点云m34，表示为plastvalid。该模块可表示为：
pvalid＝pgrd∪pnogrdvalid，其中
plastvalid＝downsample(pvalid)
栅格地图产生与三维地图匹配模块m4，包括地图匹配模块m41和栅格地图产生模块m42，其中栅格地图产生模块m42采用graph-slam技术实现，地图匹配模块m41是基于点到面测度的配准算法实现。假设两个点集为
其中，{pi,qi}∈cpq为两个点集之间的一对对应点。
初始位姿模块m5为地图匹配模块m41提供初始位姿信息，以加快算法的收敛性，提高定位效率，重定位机制见图9的说明。
参见图2，为本发明的典型搜索空间示意图。在计算提取点云特征时，常用的搜索空间为立方体和球形，立方体是划分三维空间的基本单元，较为直观，方便理解，球形设计将距离为半径以内都视为内点，本发明为了检测地下车库中的立柱和墙面，设计为圆柱体空间搜索，以便尽可能将整个立柱包含进来。以上三种搜索空间均利用最近邻搜索算法快速实现，其中立方体采用曼哈顿距离，球形采用欧式距离，圆柱体采用二维欧式距离与高度阈值结合实现。
参见图3，为本发明的典型三维激光扫描图，该图为velodyne公司hdl-64e-s3激光传感器所采集的激光点云，场景为典型城区十字路口。此传感器有效探测距离可达100m，能够获取车辆周围360度范围内的场景三维信息，具有内密外疏的扫描特点，距离较远时误差较大。
参见图4，为本发明的圆柱特征提取流程，针对非地面点云进行处理，最后的输出为圆柱特征。首先寻找关键点，通常的做法是根据相关的一些显著性特点选择关键点，本发明采用随机选取的方法，简单快捷，不足的是某些点会被计算两次或者多次，经过实验对比发现，这种选取方法对时间消耗影响不大。找到关键点之后，利用最近邻搜索和高度阈值限制的方法找出以关键点为中心的圆柱体内所有点，以这些点为基础，利用主成份分析技术(pca)建立局部坐标系。随后进行坐标变换，将圆柱内部所有的点通过坐标变换，转换到激光坐标下，以此来实现圆柱特征的旋转不变性。后续的步骤用来计算旋转统计不变特征，通过每次沿圆柱体中轴旋转固定角度，分别计算圆柱内所有点投影到x-y、x-z、y-z三个平面的统计量，该统计量为图像的高阶统计矩和香农熵，最后通过先后顺序组合成圆柱特征。
参见图5，为本发明的圆柱特征细节示意，图中符号说明：
ps为关键点；
po为圆柱体底部中心点；
pi为圆柱体内部任意一点；
r为圆柱体半径；
z为圆柱体高度；
w为x-z、y-z平面投影图像的宽度，也是x-y平面投影图像的高度；
h为x-z、y-z平面投影图像的高度；
dz为pi点到圆柱底平面的距离，满足0＜dz≤z；
dxy为pi点在圆柱底部平面的投影到底部中心点的距离，满足0＜dxy≤r；
假设sc为圆柱体内所有点构成的集合，则有pi∈sc。
圆柱特征的计算过程如下：首先分别计算圆柱内所有点sc在x-y、x-z、y-z平面上的投影，投影图像分别表示为ixy(i,j)、ixz(i,j)、iyz(i,j)，然后计算投影图像的中心矩和香农熵，如下公式所示：
其中：
接下来，将圆柱体内部所有点sc绕z轴以固定角度θ旋转，计算投影图像，以及图像的中心矩和香农熵，得到本次旋转后的圆柱特征fk＝{fxy,fxz,fyz}，以此类推，旋转k次之后结束。最后将每次旋转之后计算得到的投影图像统计信息按照一定的排列规律合并起来，构成完整的圆柱特征向量f＝f0∪…∪fk。本发明公开的方法中，sc绕局部坐标的z轴共旋转三次，每次的角度为30度，为了更加丰富圆柱特征，将旋转的角度适当调节。
在特征计算完毕之后，需要及时的判断此特征是否属于车库本身结构，以此来保留或是删除该圆柱体内部的点云。其特征选择策略如下：假设创建的特征地图为fmap＝{fv,fc,fw}，其中fv表示车辆的圆柱特征，fc表示车库中立柱的特征，fw表示墙面的特征，在特征相似性计算过程中，得到当前特征fx与特征地图之间的相似性关系d＝{d(fx,fv),d(fx,fc),d(fx,fw)}，由此，根据如下公式获得圆柱体内的点属性：
<l,dmax>＝argmaxd
其中，l表示特征的标签，1表示立柱，2表示墙面，3表示车辆，dmax表示fx与特征地图的最大相似距离，则最终当前的圆柱特征fx的属性为：
如果条件不满足则为未知特征属性，在后续处理中直接剔除即可。在地下车库定位中，只有属于车库本身结构的点才会准确定位，车辆、行人等不属于车库本身结构的动态障碍物会造成定位偏差，为此，我们将保留墙面和立柱上的点云，删除车辆等其他动态障碍物上的点。非地面有效点云pnogrdvalid为：
pnogrdvalid＝(pc∪pw)-(pv∪pothers)
其中，pc表示所有立柱上的点，pw表示所有墙面上的点，pv表示所有车辆上的点，pothers表示其他未知物体上的点。
参见图6，为本发明的坐标变换示意，fuvw表示圆柱内所有点构建的局部坐标，fxyz表示经过坐标转换后的激光坐标系，
其中，ns为关键点ps的法向量。
参见图7，为本发明的三种典型形状模型以及图8为三种典型形状模型的圆柱特征。此模型只针对常见地下车库中的结构，通常认为包含有墙面、立柱、车辆，从图7中看到三种典型形状模型的圆柱特征都是在全局坐标系下计算，图8中是图7中对应形状的圆柱特征，从图中看到，圆柱特征具有很强的鉴别能力，将立柱和墙面从复杂的场景中分离出来。
参见图9，为本发明的获取初始定位结果示意，此图描述了当无人车在地下车库中启动时，或者在定位识别时，启动重定位机制。根据图示，图中关键帧为创建地图时的点云帧，它包含有场景点云和其对应的位姿信息。输入信息为实时点云和地图关键帧，在本发明中该关键帧选为7帧，其根据场景大小，以及每帧点云所覆盖的范围来确定，选择的越多，得到的初始化位姿会越准确，相应的计算时间会变长。在重定位机制中选择fpfh特征作为全局匹配，启用ransac算法，找出对应关系确定粗定位变换，再根据场景几何一致性检测确定是否有效。以此类推，对每一关键帧做同样的处理，得到一组初始变换结果，最后根据每个初始变换的评估结果，选择最佳的变换作为初始位姿。其中评估函数为：
其中，
参见图10，为本发明的地下车库三维占据栅格地图，该地图通过graph-slam技术实现。
参见图11和图12，为本发明的剔除障碍物点云的结果图，从图中看到本发明通过提取圆柱特征的方法剔除动态障碍物，保留能够提高定位精度的场景本身结果的点。
参见图13和图14，分别为本发明的车辆定位结果图和x、y、z三个方向定位结果变化情况，从图13中看到车辆定位轨迹光滑准确，结合图14，x和y方向是的位姿变换非常光滑，只有z方向上存在一些波动，但波动范围在0.1m以内，满足定位要求。
以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施方式仅限于此，对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还做出若干简单的推演或替换，都应当视为属于本发明由所提交的权利要求书确定专利保护范围。