1.本发明属于机器人、自动驾驶领域，尤其涉及一种基于语义点云的对象级快速场景识别方法。
背景技术：2.slam同步定位与地图构建技术在自动驾驶、无人机、机器人、vr和ar等领域有着广泛应用。slam具体指的是：设备在未知环境下从一个未知位置开始移动，在移动过程中根据传感器收集到的数据和构建出的环境地图进行自身定位，并在自身定位的基础上构建增量式地图的过程。
3.在实际slam过程中，实现回环检测和重定位都需要用到slam系统中的场景识别技术。该场景识别技术需要前后两帧传感器数据，通过分析两帧传感器数据来识别出前后是否为同一场景。若为同一场景则需要计算出对应的前后两帧车辆之间的相对位姿，该相对位姿用以对建图过程中每个地图点的空间物理位置进行优化，此为完整的场景识别过程。
4.现有的slam系统中的场景识别技术会出现许多问题：该场景识别算法是以当前车辆的传感器为中心建立描述子scan context来进行的，当车辆经过同一场景两次时，由于两次的传感器位置有差异，导致描述子scan context不一样，使得系统无法识别为同一场景；除此之外，现有的场景识别算法无法计算出两次经过同一场景的相对位姿。
5.由于以上场景识别算法的缺陷，在同一个场景下，当前车辆与历史时刻的位置不重合时，系统无法识别为同一场景且无法计算相对位姿。
技术实现要素：6.本发明针对现有技术的不足，提出一种基于语义点云的对象级快速场景识别方法，目的在于解决现有回环检测算法和重定位算法对于在同一个场景下，车辆当前与历史时刻的两帧位置不重合时，系统无法检测到回环和无法重定位的问题。
7.本发明为解决其技术问题，提出以下技术方案：
8.一种基于语义点云的对象级快速场景识别方法，其特点是,包括以下步骤：
9.步骤一、建图前确定主对象：通过语义分割网络对传感器采集的三维点云进行处理，根据语义点云的分布确定主对象；
10.步骤二、建图同时构建k维树结构：以主对象为极点，传感器与主对象的连线方向为极轴建立极坐标系，并构建描述子object scan context，生成按行求均值得到的列向量ringkey、以及按列求均值得到的行向量sectorkey，构建以按行求均值得到的列向量ringkey为节点构建的k维树结构；
11.步骤三、场景识别时，计算历史帧和当前帧的相似度：在以按行求均值得到的列向量ringkey为节点构建的k维树结构中搜索与当前按行求均值得到的列向量相似的节点对应的描述子object scan context作为候选，旋转匹配描述子object scan context，计算相似度；
12.步骤四、场景识别时，计算当前帧的准确位姿：根据历史帧描述子和当前帧描述子相似度大小确定场景是否识别成功，基于主对象在历史帧和当前帧的观测位置计算出当前帧与历史帧的相对位姿。
13.所述步骤一的建图以前，确定主对象，具体过程如下：
14.1)传感器收集环境空间信息输出3d点云，所述传感器包括所有能提供环境空间3d点云信息的传感器：激光雷达、结构光相机、毫米波雷达；
15.2)使用训练完成的rangenet++网络处理3d点云，得到每一个扫描点的语义标签；使用广度优先搜索算法聚类出每个物体的具体点云范围，若某个物体的范围内带有其他标签的点的数量较多，则忽略该物体对应的语义，反之该物体对应的语义进入下一轮筛选；
16.3)动态物体和可移动的静态物体都会造成场景误识别和位姿计算错误，因此过滤掉动态物体、再过滤掉可移动的静态物体；
17.4)通过语义分布和语义方差确定主对象，具体为：
18.i、语义分布：以设备行驶的里程为横坐标、语义在不同里程段出现的次数为纵坐标，绘制一维多语义分布直方图，过滤掉不同里程段分布不均匀的语义、以及不同里程段出现次数差别较大的语义。
19.ii、语义方差：设当前语义在当前轨迹上对应n个描述子，将n个描述子对应的n个按列求均值得到的行向量分别求和得到一个集合{s
i
|i＝1,2...n}，计算该集合的方差，方差表示数据的离散状态，方差越大，代表n个主对象周围环境差异越大；每个语义在当前轨迹上都对应一个方差，挑选出方差最大的三个语义作为最终的主对象，最终选择的语义可以是一类或者多类。
20.所述步骤二的建图过程中，构建k维树结构，该k维树结构就是以每个主对象描述子object scan context的n
r
×
1的列向量为节点构建的树结构：
21.具体过程如下：
22.1)将点云旋转，所述将点云旋转用于消除因车体姿态的原因使得点云和真实世界不对应造成的影响；旋转后的点云的姿态与真实世界的姿态一致，通过此步骤消除设备的姿态对描述子object scan context的影响：假设在某一关键帧中，设备的3d位姿表示为(r,t)，r为设备坐标系相对于世界坐标系的旋转变换矩阵，t为设备坐标系相对于世界坐标系的平移变换矩阵，点云用p
t
表示，点云中的点用p
i
表示，首先将点云按以下公式旋转：
23.p
′
t
：{p
′
i
|p
′
i
＝r
·
p
i
，p
i
∈p
t
}
ꢀꢀ
(1)
24.2)确定主对象的x
‑
y平面坐标：该主对象的x
‑
y平面坐标用于过滤掉距离主对象超过设定极坐标半径范围的扫描点，并且确定有效扫描点对应的所述栅格位置；若在此关键帧中检测到主对象，通过广度优先搜索算法聚类出该主对象包含的所有点，用高斯分布拟合，计算主对象分布的均值和方差，若方差大于预设值，则忽略该主对象；否则将该主对象附近的点投影到x
‑
y平面，以主对象点云包含的扫描点的均值对应的xy坐标为极点，以传感器中心到极点的方向为极轴，在x
‑
y平面建立极坐标系；
25.补充说明：
26.3)构建描述子object scan context：以极坐标系为基础，考虑极径小于等于设定极坐标半径的范围，将设定极坐标半径划分为n
r
等分，每等分称为ring，将360度极角划分为n
s
等分，每等分称为一个扇区，得到n
r
*n
s
个环扇形栅格；以极角的大小为横轴、以极径的
大小为纵轴建立直角坐标系，对应的产生一个n
r
×
n
s
个矩阵，矩阵中每个位置保存对应栅格内所有点的特征，该点的特征包括该点的z坐标即扫描点的高度，或者是扫描点的反射强度以及语义标签等；
27.4)将描述子object scan context按行计算均值得到一个n
r
×
1的列向量，称为按行求均值得到的列向量ringkey；按列计算均值得到一个1
×
n
s
的行向量，称为按列求均值得到的行向量sectorkey；构建以按行求均值得到的列向量ringkey为节点构建的k维树结构，以便场景识别时快速搜索候选描述子object scan context；
28.所述步骤三的场景识别时计算相似度，具体过程如下：
29.1)搜索候选对象：当设备丢失定位需要重新确定自身位置或者设备回到历史经过的位置需要进行场景识别的时候，在建图过程中已完成构建的k维树结构中，搜索与当前按行求均值得到的列向量ringkey相似的节点对应的描述子object scan context作为候选对象；
30.2)以行向量计算粗偏移量：遍历候选描述子object scan context，并计算当前按列求均值得到的行向量sectorkey，用该行向量与当前候选描述子object scan context对应的行向量对齐所需要的粗偏移量，所述对齐即为采用平移当前行向量的方法，计算公式如下：
[0031][0032]
上式表示使得最小对应的shift的取值，式中s
q
表示候选描述子object scan context的按列求均值得到的行向量sectorkey，表示当前描述子object scan context的按列求均值得到的行向量sectorkey向左偏移shift个单位，||s||表示向量s的二范数；
[0033]
3)以偏移窗口计算精偏移量：在以shift为中心的一个偏移窗口内构建如下优化方程，计算出精偏移量和相似度。
[0034][0035][0036]
(3)式表示使得最小对应的n的取值，n表示偏移单位；
[0037]
(4)式表示n在[shift
‑
k,shift+k]范围内变化时的最小值，式中shift为粗偏移量，[shift
‑
k,shift+k]表示以shift为中心、以k为半径的偏移量窗口，设k＝3，i
q
表示候选描述子object scan context，表示当前描述子object scan context向左偏移n个单位，d表示两个描述子object scan context的距离，计算公式如下：
[0038][0039]
式中，代表候选描述子object scan context从左往右第j个列向量，代表当前描述子object scan context向左平移n个单位与候选描述子对齐之后从左往右第j个列向量，||c||表示向量c的二范数；相似度计算公式如下，相似度取值范围为0到1，越接近1说明越相似，计算公式如下：
[0040][0041]
所述步骤四的场景识别时计算当前帧的准确位姿，具体过程如下：
[0042]
1)若相似度小于阈值，则认为两个描述子object scan context不同，将当前帧对应的按行求均值得到的列向量ringkey作为节点添加到以按行求均值得到的列向量ringkey为节点构建的k维树结构中；
[0043]
2)若相似度大于阈值，则认为两个描述子object scan context是同一个，结合主对象在当前帧和匹配帧中的观测位置推算出当前帧的位姿。
[0044]
所述2)具体如下：假设主对象在历史帧的观测位置为(x1,y1)，历史帧的位姿为(x
o1
,y
o1
,θ
o1
)，主对象在当前帧的观测位置为(x2,y2)，推导出当前帧的位姿(x
o2
,y
o2
,θ
o2
)的表达式如下：
[0045]
x
o2
＝x
o1
+x1cosθ
o1
+y1sinθ
o1
‑
x2cosθ
o2
+y2sinθ
o2
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0046]
y
o2
＝y
o1
+x1s
i
nθ
o1
+y1cosθ
o1
‑
x2sinθ
o2
‑
y2cosθ
o2
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0047][0048]
式中γ为描述子object scan context向左偏移n个单位对应的角度，n为精偏移量。
[0049][0050]
进一步可以推导出当前帧相对于历史帧的相对位姿为：
[0051]
δx＝x1‑
x2cosδθ
o
+y2sinδθ
o
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0052]
δy＝y1‑
x2sinδθ
o
‑
y2cosδθ
o
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0053][0054]
至此，场景识别结束。
[0055]
本发明的优点效果
[0056]
1、不同于其他以设备为中心建立描述子scan context的技术，本发明本以主对象为中心建立描述子object scan context，这种不依赖观测位置的描述子object scan context能解决现有技术两帧位置不重合时无法进行场景识别的问题。
[0057]
2、本技术能在描述子object scan context匹配完成后，快速准确地计算出当前帧的位姿作为重定位的结果或者提供给前端作为点云匹配的初值，或者计算出当前帧与历史帧的相对位姿作为后端的一个约束，提高整个slam系统的鲁棒性和定位建图的精度。
[0058]
3、本技术提出的主对象是场景识别的基础，本技术使用的多种语义筛选方法能够快速有效地确定主对象，为场景识别提供保障。
附图说明
[0059]
图1为本发明对象级快速场景识别方法主流程图；
[0060]
图2为图1的s110处理步骤流程图；
[0061]
图3为本发明语义点云分布直方图；
[0062]
图4为图1的s120处理步骤流程图；
[0063]
图5为本发明环扇形栅格矩阵示意图；
[0064]
图6为图1的s130处理步骤流程图；
[0065]
图7为图1的s140处理步骤流程图；
[0066]
图8为本发明场景识别示意图；
具体实施方式
[0067]
本发明设计原理
[0068]
1、本发明场景识别的特点。本发明场景识别是基于当前车辆传感器以外的“主对象”实现的，该“主对象”就是当前车辆传感器以外的环境中的物体，如垃圾桶、路灯或树木，如对当前车辆传感器以外的路灯周围的场景进行识别、对当前车辆传感器以外的树木周围的场景进行识别。当在一帧3d点云中检测到主对象，场景识别方法就是对该3d点云中所包含的所有扫描点进行处理，具体处理方法是在主对象的固定范围内建立许多环扇形栅格，该矩阵也称作描述子object scan context，每个主对象描述子object scan context的栅格里面保存该栅格中所有扫描点的z坐标的均值。每个主对象都有这样一个唯一对应的环扇形栅格矩阵，每个环扇形栅格矩阵中保存的z值都唯一描述该主对象周围的场景。
[0069]
2、本发明场景识别的原理。本发明场景识别是识别场景中各个扫描点的空间高度z、而不是识别场景中各个扫描点的平面x、y位置，当前场景中各个扫描点的空间高度z与观测位置无关、或者与当前车辆距离扫描点的远近无关。无论当前车辆传感器距离主对象是远、是近、是偏左、还是偏右，无论当前车辆传感器“站在”相对主对象远一点位置、或“站在”相对主对象近一点位置、或者“站在”相对主对象偏左一点的位置、或者“站在”相对主对象偏右一点的位置，其计算出来的主对象场景中各个扫描点的高度都是一致的，利用这个原理，假设回环检测或重定位时，车辆经过同一场景两次时的传感器位置有差异、并且车辆的方向也会有差异，但车辆位置的差异只是造成激光雷达在水平面位置的移动、而非上下位置的移动，因此激光雷达当在水平面位置移动时计算场景中各个扫描点的z值是相同的；车辆方向的差异只是造成激光雷达在水平面内从一个角度旋转到另一个角度、而非激光雷达在垂直平面上下角度的旋转，因此激光雷达在水平面内从一个角度旋转到另一个角度时计算场景中各个扫描点的z值也是相同的。所以两次产生的描述子是相同的，不会误判为两个主对象而是同一个主对象。
[0070]
3、本发明场景识别与回环检测和重定位的关系。所述回环检测就是车辆前后两次经过地图中同一场景时，分别以主对象为中心建立描述子object scan context，使用本发明的场景识别技术确定两个描述子为同一个描述子并计算出两帧的相对位姿作为建图的一个约束，这种方法可以限制由于建图中匹配算法缺陷以及传感器不准确带来的累计误差。所述重定位就是车辆在定位信息如gps信号突然消失时，车辆无法确定自身在地图中的位置，也就无法进行后续的规划、控制等自动驾驶行为，这时使用本发明的场景识别技术找到当前帧中描述子object scan context在地图中的位置，从而确定车辆自身的位置。
[0071]
4、本发明分为三个阶段四个步骤来实现基于语义点云的对象级快速场景识别方法。第一个阶段是建图以前：确定地图中的所有主对象，该主对象需要满足一定条件才能成为主对象：例如在不同的里程里面出现的次数均匀、在不同的里程里分布均匀。在城市道路环境通常主对象选择交通标识、红绿灯等，在园区通常主对象选择垃圾桶、路灯、树干等。第二个阶段是建图过程中：建图过程中要建立每个主对象的描述子object scan context、也
就是建立每个主对象的环扇形栅格矩阵，同时，为了快速检索每个主对象的环扇形栅格矩阵，对每个主对象的环扇形栅格矩阵进行简化、建立k维树结构，所述k维树结构就是把一个nr*ns的矩阵用n
r
×
1的列向量表示，当需要识别某个场景时，通过识别该描述子object scan context对应的n
r
×
1的列向量加快搜索速度，因为n
r
×
1就是对栅格中每一行的值求平均、最终得到该矩阵对应的一个列向量，当描述子object scan context旋转时，该n
r
×
1的列向量是不变的。在数据库中给每个主对象保存对应三个数据：该主对象的描述子object scan context、该描述子object scan context的n
r
×
1的列向量、该描述子object scan context的1
×
n
s
的行向量。第三个阶段是场景识别过程中：在这个阶段分为二步，第一步是计算当前帧和历史帧的相似度，第二步是计算当前帧和历史帧的相对位姿。本发明最终目的是通过计算相对位姿对当前帧的建图效果进行优化、或者丢失定位信息时快速找回车辆在地图中的位置。因此第三个阶段的第二步最终落实在计算当前帧和历史帧的相对位姿。在这之前首先判定是否在同一个场景中，只有在同一个场景中的相对位姿才是有效的，因此第三阶段的第一步是通过计算相似度判定是否在同一个场景中。所述当前帧可能会包含多个主对象，假如一个当前帧会包含垃圾桶、树木、路灯三个主对象，当主对象数量超过1个时，则回环检测每次扫描n个主对象，总的次数还是2次。
[0072]
5、本发明的创造性：创造性在于采用了一种本领域技术人员不容易想到的“以主对象为中心替代以传感器为中心建立场景识别描述子”的方法。现有技术的场景识别方法虽然能快速检索出回环候选帧并计算两帧之间的相似度，但要求两帧坐标基本重合，无法识别位置相差一定距离的两帧的场景；其次，此类方法只能计算出当前帧相对匹配帧的旋转角度，无法计算两帧之间的x和y坐标的偏移，在实际使用时有一定局限性。
[0073]
本发明和现有技术对比，具有以下创新点：1)区别于以传感器为中心的建立描述子scan context的方法，本技术以主对象为中心建立描述子object scan context，无需设备当前帧与历史帧位置重合，只需要两者位置接近且能观测到场景中同样的一个或多个主对象即可，扩大了场景识别的适用范围，提供了方法的鲁棒性。2)本技术中当前帧与匹配帧通过旋转匹配输出旋转角度，结合匹配帧的位置以及主对象在两帧中的观测位置即可计算出当前帧的准确坐标，进一步推算出两帧之间的相对位姿变换(包含x和y方向的偏移)。3)本技术提出了主对象的概念，选择局部与其它物体区分度较高、在整个里程范围内均覆盖较好的物体作为主对象，并且排除周围环境差异小的物体，避免发生误匹配。
[0074]
6、相关关键词解释：
[0075]
frame：帧，传感器的数据以帧的形式读取，每一帧为某一时刻下的一包完整的传感器数据，帧发送接收频率是由设备所使用的系统决定的；
[0076]
main object：主对象，是本技术实现场景识别的基础，对场景中含有的众多语义按一定条件筛选的结果。
[0077]
object scan context：对象扫描内容，以main object为中心，对周围点云类别和分布进行表示的一种形式。
[0078]
ringkey：将object scan context按行求均值得到的列向量。
[0079]
sectorkey：将object scan context按列求均值得到的行向量。
[0080]
kd
‑
tree：以按行求均值得到的列向量为节点构建的k维树结构，可以加速候选object scan context的搜索。
[0081]
坐标系：本技术使用了直角坐标系和极坐标系，直角坐标系使用机器人操作系统中通用的坐标系定义，为笛卡尔直角坐标系右手系(向前为x轴，向上为z轴，向左为y轴)，极坐标系定义从极轴开始逆时针旋转为极角的正方向。
[0082]
位姿：本技术所述位姿为物体在2d空间的位置和姿态，位置为x方向和y方向的坐标，姿态为以x轴为正方向的方位角θ。
[0083]
观测位置：以传感器中心为坐标原点、设备正方向为x轴、向左为y轴建立直角坐标系，main object在坐标系中的位置称为观测位置。
[0084]
关键帧：在slam算法建图阶段产生的，带有位姿的数据，可作为地图的数据存储方式。
[0085]
基于以上原理，本发明设计了一种基于语义点云的对象级快速场景识别方法。一种基于语义点云的对象级快速场景识别方法，如图1所示，其特点是,包括以下步骤：
[0086]
步骤一、建图前确定主对象main object：通过语义分割网络对传感器采集的三维点云进行处理，根据语义点云的分布确定主对象main object；
[0087]
确定主对象main object的过程如图2所示，具体过程如下：
[0088]
1)传感器收集环境空间信息输出3d点云，所述传感器包括所有能提供环境空间3d点云信息的传感器：激光雷达、结构光相机、毫米波雷达；
[0089]
2)使用训练完成的rangenet++网络处理3d点云，得到每一个扫描点的语义标签；使用广度优先搜索算法聚类出每个物体的具体点云范围，若某个物体的范围内带有其他标签的点的数量较多，则忽略该物体对应的语义，反之该物体对应的语义进入下一轮筛选；
[0090]
3)动态物体和可移动的静态物体都会造成场景误识别和位姿计算错误，因此过滤掉动态物体、再过滤掉可移动的静态物体；
[0091]
补充说明：动态物体例如机动车、非机动车、行人等；可移动的静态物体，例如锥桶、路障、水马等。
[0092]
4)通过语义分布和语义方差确定主对象，具体为：
[0093]
i、语义分布：以设备行驶的里程为横坐标、语义在不同里程段出现的次数为纵坐标，绘制一维多语义分布直方图，过滤掉不同里程段分布不均匀的语义、以及不同里程段出现次数差别较大的语义。
[0094]
补充说明：
[0095]
确定主对象的第一个条件是语义分布均匀，语义分布均匀的目的是在设备行驶的每个里程都布设主对象，在设备行驶的每个里程都布设主对象才能在设备行驶的每个里程都能够进行场景识别。
[0096]
确定最终主对象main object的过程如图3所示，只有语义2适合作为main object；语义1在不同里程段出现次数差别较大；语义3整体出现次数少；语义4在200m到250m之间没有出现。经过以上两步筛选，若剩下的可选择语义仍较多，则计算同一语义对应的多个object scan context差异性，根据差异性选择main object，筛选方式如下。假设某一语义对应有n个object scan context，将所有对应的sectorkey分别求和得到一个集合{s
i
|i＝1,2...n}，计算该集合的方差作为该语义的差异性。每个语义都对应一个方差，挑选出方差最大的三个语义作为最终的main object，最终选择的语义可以是一类或者多类，比如在城市道路环境通常选择交通标识、红绿灯等，在园区通常选择垃圾桶、路灯、树干等。
[0097]
ii、语义方差：设当前语义在当前轨迹上对应n个描述子，将n个描述子对应的n个按列求均值得到的行向量分别求和得到一个集合{s
i
|i＝1，2...n}，计算该集合的方差，方差表示数据的离散状态，方差越大，代表n个主对象周围环境差异越大；每个语义在当前轨迹上都对应一个方差，挑选出方差最大的三个语义作为最终的主对象，最终选择的语义可以是一类或者多类。
[0098]
补充说明：
[0099]
确定主对象的第二个条件是语义方差。语义方差越大，代表n个主对象周围环境差异越大，所述n个主对象周围环境差异大，不是将n个主对象之间相比，而是将n个主对象周围环境相比，也就是n个主对象各自周围场景相比，假设当前轨迹上有3个垃圾桶，如果3个垃圾桶各自场景中比较相似，也就是语义方差不大，则“垃圾桶”不适合当做主对象，如果3个垃圾桶各自场景中扫描点的高度相差很大，也就是语义方差大，则“垃圾桶”适合当做主对象。
[0100]
步骤二、建图时构建k维树结构：以主对象main object为极点，传感器与主对象main object的连线方向为极轴建立极坐标系，并构建描述子object scan context，生成按行求均值得到的列向量ringkey以及按列求均值得到的行向量sectorkey，构建以按行求均值得到的列向量ringkey为节点的k维树结构kd
‑
tree；
[0101]
所述步骤二的建图过程中，构建k维树结构，该k维树结构就是以每个主对象main object描述子object scan context的n
r
×
1的列向量为节点的树结构：
[0102]
构建k维树结构如图4所示，具体过程如下：
[0103]
1)将点云旋转，所述将点云旋转用于消除因车体姿态的原因使得点云和真实世界不对应造成的影响；旋转后的点云的姿态与真实世界的姿态一致，通过此步骤消除设备的姿态对描述子object scan context的影响：假设在某一关键帧中，设备的3d位姿表示为(r,t)，r为设备坐标系相对于世界坐标系的旋转变换矩阵，t为设备坐标系相对于世界坐标系的平移变换矩阵，点云用p
t
表示，点云中的点用p
i
表示，首先将点云按以下公式旋转：
[0104]
p
′
t
：{p
′
i
|p
′
i
＝r
·
p
i
，p
i
∈p
t
}
ꢀꢀ
(1)
[0105]
补充说明：
[0106]
此处的点云旋转，是指传感器设备在上下坡或者车辆倾斜、颠簸等过程中，设备的姿态发生变化，通过点云旋转消除设备的姿态对描述子的影响。
[0107]
2)确定主对象main object的x
‑
y平面坐标：该主对象main object的x
‑
y平面坐标用于过滤掉距离主对象main object超过设定极坐标半径范围的扫描点，并且确定有效扫描点对应的所述栅格位置；若在此关键帧中检测到主对象main object，通过广度优先搜索算法聚类出该主对象main object包含的所有点，用高斯分布拟合，计算主对象main object分布的均值和方差，若方差大于预设值，则忽略该主对象main object；否则将该主对象main object附近的点投影到x
‑
y平面，以主对象main object点云包含的扫描点的均值对应的xy坐标为极点，以传感器中心到极点的方向为极轴，在x
‑
y平面建立极坐标系；
[0108]
补充说明：
[0109]
所述设定极坐标半径范围，例如极径小于等于80米的范围，将80米极径划分为n
r
等分，本实施例中取n
r
＝20，每等分称为ring，将360度极角划分为n
s
等分，本实施例中取n
s
＝60，每等分称为sector，得到1200个环扇形栅格。
[0110]
补充说明：
[0111]
所述方差预设值应使主对象对应扫描点的xy坐标处于一固定小范围内，目的是避免观测位置不同造成主对象xy坐标计算不准确。预设值需要根据具体情况具体设定，确定预设值之后，系统会忽略xy坐标分布分散的主对象，仅考虑xy坐标分布集中的对象。
[0112]
3)构建描述子object scan context：以极坐标系为基础，考虑极径小于等于设定极坐标半径的范围，将设定极坐标半径划分为n
r
等分，每等分称为ring，将360度极角划分为n
s
等分；设定极坐标角度范围为n
s
，每等分称为一个扇区，得到n
r
×
n
s
个环扇形栅格；以极角的大小为横轴、以极径的大小为纵轴建立直角坐标系，对应的产生一个n
r
×
n
s
个矩阵，矩阵中每个环扇形栅格保存对应平面栅格内所有点的特征，该点的特征包括该点的z坐标，或者是对应平面栅格内所有点的z坐标的均值；
[0113]
补充说明：
[0114]
如图5所示，右图的横坐标代表角度0
‑
2π，如果取n
s
＝60，则横坐标每个格子代表6
°
；纵坐标代表极径，设极径＝80米的范围，当n
r
＝20的时候，则纵坐标每个格子代表4米。当n
s
＝60时，图5左图按照每6
°
一份共分为60份，当n
r
＝20的时候，图5左图每个环的宽度为4米，共有20个环。
[0115]
4)将描述子object scan context，按行计算均值得到一个n
r
×
1的列向量，称为按行求均值得到的列向量ringkey；按列计算均值得到一个1
×
n
s
的行向量，称为按列求均值得到的行向量sectorkey；构建以按行求均值得到的列向量ringkey为节点构建的k维树结构kd
‑
tree，以便场景识别时快速搜索候选描述子object scan context；
[0116]
步骤三、场景识别时，计算历史帧和当前帧的相似度：在以按列求均值得到的行向量sectorkey为节点构建的k维树结构中搜索与当前按行求均值得到的列向量ringkey相似的节点对应的描述子object scan context作为候选，旋转匹配描述子object scan context，计算相似度：
[0117][0118]
补充说明：
[0119]
步骤三是在完成步骤一和步骤二的基础上进行场景识别，场景识别是用当前描述子和历史描述子进行径向对比：具体为检测到当前激光雷达扫描得到的3d点云中存在主对象时，按照上述方法生成描述子object scan context，计算当前描述子按行求均值得到的列向量和按列求均值得到的行向量，在k维树结构中搜索出与当前按行求均值得到的列向量相似的列向量对应的描述子object scan context，并用当前描述子和历史描述子作相似度的对比，对比的方法是先通过按列求均值得到的行向量计算粗偏移量、再根据整个描述子在粗偏移量的一定范围内计算精偏移量，然后基于精偏移量计算历史描述子和当前描述子的距离距离越小，相似度越大。
[0120]
步骤三计算相似度的过程如图6所示，具体过程如下：
[0121]
1)搜索候选对象：当设备丢失定位需要重新确定自身位置或者设备回到历史经过的位置需要进行场景识别的时候，在建图过程中已完成构建的k维树结构中，搜索与当前按行求均值得到的列向量ringkey相似的节点对应的描述子object scan context作为候选对象；
[0122]
2)以行向量计算粗偏移量：遍历候选描述子object scan context，并计算当前按
列求均值得到的行向量sectorkey，用该行向量与当前候选描述子object scan context对应的行向量对齐所需要的粗偏移量，所述对齐即为采用平移当前行向量的方法，计算公式如下：
[0123][0124]
上式表示使得最小对应的shift的取值，式中s
q
表示候选描述子object scan context的按列求均值得到的行向量sectorkey，表示当前描述子object scan context的按列求均值得到的行向量sectorkey向左偏移shift个单位，||s||表示向量s的二范数；
[0125]
3)以偏移窗口计算精偏移量：在以shift为中心的一个偏移窗口内构建如下优化方程，计算出精偏移量和相似度。
[0126][0127][0128]
(3)式表示使得最小对应的n的取值，n表示偏移单位；
[0129]
(4)式表示n在[shift
‑
k,shift+k]范围内变化时的最小值，式中shift为粗偏移量，[shift
‑
k,shift+k]表示以shift为中心、以k为半径的偏移量窗口，设k＝3，i
q
表示候选描述子object scan context，表示当前描述子object scan context向左偏移n个单位，d表示两个描述子object scan context的距离，计算公式如下：
[0130][0131]
式中，代表候选描述子object scan context从左往右第j个列向量，代表当前描述子object scan context向左平移n个单位与候选描述子对齐之后从左往右第j个列向量，||c||表示向量c的二范数；相似度计算公式如下，相似度取值范围为0到1，越接近1说明越相似，计算公式如下：
[0132][0133]
步骤四、场景识别时，计算当前帧的准确位姿：根据历史帧和当前帧相似度大小确定场景是否识别成功，基于主对象main object在当前帧和匹配帧的观测位置计算出当前帧的准确位姿。
[0134]
所述计算当前帧的准确位姿如图7所示，具体过程如下：
[0135]
1)若相似度小于阈值，则认为两个描述子object scan context不同，将当前帧对应的按行求均值得到的列向量ringkey作为节点添加到以按行求均值得到的列向量ringkey为节点构建的k维树结构kd
‑
tree中；
[0136]
补充说明：
[0137]
本实施例设置阈值为0.8，如果小于这个阈值，则认为两个描述子object scan context不同，将当前帧对应的按行求均值得到的列向量ringkey作为新的节点添加到kd
‑
tree中。
[0138]
2)若相似度大于阈值，则认为两个描述子object scan context是同一个，结合主
对象main object在当前帧和匹配帧中的观测位置推算出当前帧的位姿。
[0139]
所述推算出当前帧的位姿如图8所示，具体过程如下：
[0140]
假设主对象main object在历史帧的观测位置为(χ1,y1)，历史帧的位姿为(x
o1
,y
o1
,θ
o1
)，主对象main object在当前帧的观测位置为(x2,y2)，推导出当前帧的位姿(x
o2
,y
o2
,θ
o2
)的表达式如下：
[0141]
x
o2
＝x
o1
+x1cosθ
o1
‑
y1sinθ
o1
‑
x2cosθ
o2
+y2sinθ
o2
ꢀꢀ
(7)
[0142]
y
o2
＝y
o1
+x
1 sin θ
o1
+y1cos θ
o1
‑
x
2 sin θ
o2
‑
y2cos θ
o2
ꢀꢀ
(8)
[0143][0144]
式中γ为描述子object scan context向左偏移n个单位对应的角度，n为精偏移量。
[0145][0146]
进一步可以推导出当前帧相对于历史帧的相对位姿为：
[0147]
δx＝x1‑
x2cosδθ
o
+y2sinδθ
o
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0148]
δy＝y1‑
x2sinδθ
o
‑
y2cosδθ
o
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0149][0150]
至此，场景识别结束。
[0151]
补充说明：
[0152]
如图8所示，在俯视场景中有两个车道(一个车道是竖直的，另一个是倾斜的)和一个主对象(用星号表示)。
[0153]
第一次车辆是在竖直车道上行驶，对主对象有一个观测，这时以ρ1为极轴方向建立极坐标系和环扇形栅格描述子，每个栅格中保存了所有扫描点的z坐标，所以车辆距离主对象远近不会影响描述子的建立。
[0154]
第二次车辆在倾斜车道上行驶，对主对象有一个新的观测，这时以ρ2为极轴方向建立极坐标系和和环扇形栅格描述子。每个栅格中同样保存了所有扫描点的z坐标，所以车辆距离主对象远近不会影响描述子的建立。
[0155]
可以看出，这两个描述子唯一的不同就是极轴的方向不同，也就是矩阵需要通过向左偏移一定单位才能够匹配上，相当于描述子旋转匹配。
[0156]
匹配对应的偏移的单位数能算出ρ1和ρ2之间的夹角，再结合主对象在两帧中的位置(车辆距离主对象远近在这一步用上了)，就能算出两帧车辆的相对位姿。
[0157]
以上所述并非是对本发明的限制，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明实质范围的前提下，还可以做出若干变化、改型、添加或替换，这些改进和润饰也应视为本发明的保护范围。