制图轨迹优化方法、设备及存储介质与流程

1.本技术实施例涉及导航和无人驾驶技术领域，尤其涉及一种制图轨迹优化方法、设备及存储介质。


背景技术：

2.高精度地图是推动无人驾驶技术落地的重要一环，根据行业标准，要求高精度地图的相对精度达到厘米级、绝对精度达到亚米级(小于1米)，才能够满足车辆导航中高精定位算法对于高精度地图的需求。
3.在大规模的高精度地图的制作过程中，通常会用到高精度测绘设备以及用于推算高精度测绘设备的制图轨迹的轨迹推算技术。其中，轨迹推算技术是影响制作得到的高精度地图的相对精度与绝对精度的主要因素。而且，在采用轨迹推算技术对制图轨迹进行推算的过程中，获取的定位信息，例如全球定位系统(global positioning system，gps)信号对最终的绝对精度起着至关重要的作用。
4.然而，在高楼林立、高架纵横交错的城市场景中，gps信号的精度极易受到遮挡以及多路径的影响而降低，即便采用差分基站信息，也可能得不到高精度的定位信息，该种情况下会导致最终制作得到的高精度地图可能出现较大的绝对偏差(例如，约1-8米)，无法满足行业标准和用户需求。


技术实现要素：

5.本技术实施例提供一种制图轨迹优化方法、设备及存储介质，以提高高精度地图的绝对精度，满足行业标准和用户需求。
6.第一方面，本技术实施例提供一种制图轨迹优化方法，包括：
7.获取多个控制点的位姿信息和关联点云地图；所述控制点位于待优化制图轨迹的轨迹节点范围内；所述关联点云地图为与所述待优化制图轨迹关联采集的三维点云地图；
8.根据各个控制点的所述位姿信息，从所述关联点云地图中搜索与各个所述控制点分别对应的同名点；
9.基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹。
10.在一种可能的设计中，所述根据各个控制点的所述位姿信息，从所述关联点云地图中搜索与各个所述控制点分别对应的同名点，包括：
11.针对每个控制点，根据所述控制点的位姿信息中的位置坐标，确定所述关联点云地图中的搜索范围；
12.获取各所述控制点的图像信息；
13.针对每个所述控制点的图像信息，在所述关联点云地图中的搜索范围内定位与所述控制点位置对应的点作为所述控制点的同名点。
14.在一种可能的设计中，所述基于各个所述控制点与其对应的同名点之间的距离
差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹，包括：
15.针对每个控制点，根据所述控制点的位姿信息中的位置坐标和所述控制点对应的同名点的位置坐标，计算所述控制点与对应的同名点之间的第一距离差；
16.在所述待优化制图轨迹的各轨迹节点中搜索与所述控制点距离最近的第一目标轨迹节点；
17.根据所述第一距离差对所述第一目标轨迹节点的位置坐标进行修正，得到修正后的所述第一目标轨迹节点的位置坐标；
18.针对每个修正后的第一目标轨迹节点，计算所述修正后的第一目标轨迹节点与所述第一目标轨迹节点之间的误差；
19.基于各所述误差确定第一纠偏位姿约束项；
20.根据所述第一纠偏位姿约束项，构建目标函数，并计算得到使所述目标函数值最小化的各轨迹节点的位姿信息，获得所述优化后的制图轨迹。
21.在一种可能的设计中，所述基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹，包括：
22.将多个所述控制点划分为多个控制点组，并针对每个控制点组，将所述控制点组中各控制点对应的各同名点构建为所述控制点组对应的同名点组；
23.针对每个控制点组和所述控制点组对应的同名点组，根据所述控制点组中各控制点的位置坐标确定所述控制点组的第一几何重心的位置坐标；根据所述同名点组中各同名点的位置坐标确定所述同名点组的第二几何重心的位置坐标；根据所述第一几何重心的位置坐标和所述第二几何重心的位置坐标，计算所述控制点组与对应的同名点组之间的第二距离差；
24.在所述待优化制图轨迹的各轨迹节点中搜索与所述第一几何重心距离最近的第二目标轨迹节点；
25.根据所述第二距离差对所述第二目标轨迹节点的位置坐标进行修正，得到修正后的所述第二目标轨迹节点的位置坐标；
26.针对每个修正后的第二目标轨迹节点，计算所述修正后的第二目标轨迹节点与所述第二目标轨迹节点之间的误差；
27.基于各所述误差确定第二纠偏位姿约束项；
28.根据所述第二纠偏位姿约束项，构建目标函数，并计算得到使所述目标函数值最小化的各轨迹节点的位姿信息，获得所述优化后的制图轨迹。
29.在一种可能的设计中，所述目标函数还包括待优化制图轨迹中相邻轨迹节点之间的位姿误差约束项和所述待优化制图轨迹中各轨迹节点的位姿的先验误差约束项。
30.在一种可能的设计中，所述获取所述关联点云地图，包括：
31.根据待优化制图轨迹，将与所述待优化制图轨迹关联采集的多个三维点云地图进行拼接，获得所述关联点云地图。
32.在一种可能的设计中，所述根据待优化制图轨迹，将与所述待优化制图轨迹关联采集的多个三维点云地图进行拼接，获得所述关联点云地图，包括：
33.针对每个三维点云地图中每个目标点，根据所述目标点的时间戳以及所述待优化制图轨迹中各轨迹节点的时间戳，从所述待优化制图轨迹中确定与所述目标点时间最近的
第一轨迹节点和第二轨迹节点；所述第一轨迹节点的时间戳小于所述目标点的时间戳，所述第二轨迹节点的时间戳大于所述目标点的时间戳；
34.根据所述目标点的时间戳和位姿信息、所述第一轨迹节点的时间戳和位姿信息以及所述第二轨迹节点的时间戳和位姿信息，通过线性插值算法确定所述目标点对应的虚拟轨迹节点的时间戳和位姿信息；
35.根据刚体变换矩阵，将所述虚拟轨迹节点的位姿信息进行转换，得到所述目标点在所述关联点云地图中的对应点。
36.第二方面，本技术实施例提供一种制图轨迹优化设备，包括：
37.获取模块，用于获取多个控制点的位姿信息和关联点云地图；所述控制点位于待优化制图轨迹的轨迹节点范围内；所述关联点云地图为与所述待优化制图轨迹关联采集的三维点云地图；
38.搜索模块，用于根据各个控制点的所述位姿信息，从所述关联点云地图中搜索与各个所述控制点分别对应的同名点；
39.优化模块，用于基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹。
40.第三方面，本技术实施例提供一种制图轨迹优化设备，包括：至少一个处理器和存储器；
41.所述存储器存储计算机执行指令；
42.所述至少一个处理器执行所述存储器存储的计算机执行指令，使得所述至少一个处理器执行如上第一方面以及第一方面各种可能的设计所述的方法。
43.第四方面，本技术实施例提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行指令，当处理器执行所述计算机执行指令时，实现如上第一方面以及第一方面各种可能的设计所述的方法。
44.第五方面，本技术实施例提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时，实现如上第一方面以及第一方面各种可能的设计所述的方法。
45.本实施例提供的制图轨迹优化方法、设备及存储介质，该方法通过获取多个控制点的位姿信息和关联点云地图，所述控制点位于待优化制图轨迹的轨迹节点范围内，所述关联点云地图为与所述待优化制图轨迹关联采集的三维点云地图，并根据各个控制点的所述位姿信息，从所述关联点云地图中搜索与各个所述控制点分别对应的同名点，并基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹，能够在多个控制点的同名点的约束下，减小待优化制图轨迹的偏差，实现对待优化制图轨迹的优化，从而提高基于优化后的待优化制图轨迹生成的高精度地图的绝对精度，满足行业标准和用户需求。
附图说明
46.为了更清楚地说明本技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
47.图1为本技术实施例提供的制图轨迹优化前后的对比图；
48.图2为本技术实施例提供的制图轨迹优化方法的流程示意图一；
49.图3为本技术实施例提供的设置的多个控制点的示意图；
50.图4为本技术实施例提供的设置的多个控制点的示意图；
51.图5为本技术实施例提供的制图轨迹优化方法的流程示意图二；
52.图6为本技术实施例提供的制图轨迹优化设备的结构示意图；
53.图7为本技术实施例提供的一种制图轨迹优化设备的框图。
具体实施方式
54.为使本技术实施例的目的、技术方案和优点更加清楚，下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
55.高精度地图是推动无人驾驶技术落地的重要一环，根据行业标准，要求高精度地图的相对精度达到厘米级、绝对精度达到亚米级(小于1米)，才能够满足车辆导航中高精定位算法对于高精度地图的需求。
56.在大规模的高精度地图的制作过程中，通常会用到高精度测绘设备以及用于推算高精度测绘设备的制图轨迹的轨迹推算技术。其中，轨迹推算技术是影响制作得到的高精度地图的相对精度与绝对精度的主要因素。而且，在采用轨迹推算技术对制图轨迹进行推算的过程中，获取的定位信息，例如全球定位系统(global positioning system，gps)信号对最终的绝对精度起着至关重要的作用。
57.然而，在高楼林立、高架纵横交错的城市场景中，gps信号的精度极易受到遮挡以及多路径的影响而降低，即便采用差分基站信息，也可能得不到高精度的定位信息，该种情况下会导致最终制作得到的高精度地图可能出现较大的绝对偏差(例如，约1-8米)，无法满足行业标准和用户需求。
58.针对上述技术问题，发明人研究发现可以通过在制图轨迹的轨迹节点范围内设置多个控制点，在根据制图轨迹生成的关联点云地图中控制点的同名点的约束下，通过调整制图轨迹中轨迹点的位姿，使得从根据制图轨迹生成的关联点云地图中搜索到的控制点的同名点与对应的控制点距离尽可能小，实现对制图轨迹的纠偏优化，从而能够提高根据制图轨迹生成的高精度地图的绝对精度。基于此，本技术实施例提供一种制图轨迹优化方法，能够实现对制图轨迹的纠偏优化，从而能够提高根据制图轨迹生成的高精度地图的绝对精度。
59.图1为本技术实施例提供的制图轨迹优化前后的对比图。如图1所示，图1中包括左中右三幅图，左图为在制图轨迹所在真实物理世界设置的控制点的图像信息，如左图所示，控制点选在了人行横道线的顶点。中图为根据纠偏优化前的制图轨迹确定的关联点云地图中同名点与控制点之间的位置关系示意图，如中图所示，对制图轨迹纠偏优化前，同名点与控制点之间的距离较大，也即制图轨迹的绝对精度较低，从而根据该绝对精度较低的制图轨迹确定的关联点云地图的绝对精度也较低。右图为根据纠偏优化后的制图轨迹确定的关联点云地图中同名点与控制点之间的位置关系示意图，如右图所示，对制图轨迹纠偏优化
后，同名点与控制点几乎重合，也即制图轨迹的绝对精度明显提高，从而根据该绝对精度较低的制图轨迹确定的关联点云地图的绝对精度也明显提高。
60.下面以具体地实施例对本技术的技术方案进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例不再赘述。
61.图2为本技术实施例提供的制图轨迹优化方法的流程示意图一。如图2所示，该方法包括：
62.201、获取多个控制点的位姿信息和关联点云地图；所述控制点位于待优化制图轨迹的轨迹节点范围内；所述关联点云地图为与所述待优化制图轨迹关联采集的三维点云地图。
63.本实施例的执行主体可以是计算机、服务器、车载设备等能够进行数据处理的设备。
64.本实施例中，关联点云地图可以为待优化制图轨迹关联采集的多个三维点云地图拼接得到的地图。具体拼接过程可以参照图5所示实施例的说明，此处不再赘述。
65.本实施例中，多个控制点的设置可以有多种方式，在一种可实现方式中，如图3所示，可以沿待优化制图轨迹，分散设置多个控制点，例如，假设待优化制图轨迹为一条直线，可以在待优化制图轨迹的起始点(0米)附近设置第一控制点，在10米附近设置第二控制点，在20米附近设置第三控制点。当然，控制点间隔可以根据实际需要进行调整，本实施例对此不做限定。在另一种可实现方式中，如图4所示，可以将控制点进行分组设置。例如，假设待优化制图轨迹为一条直线，可以在待优化制图轨迹的起始点(0米)附近设置第一组控制点，在10米附近设置第二组控制点，在20米附近设置第三组控制点。相邻组之间的间隔以及每组控制点的控制点数量均可以根据实际需要进行调整，本实施例对此不做限定。
66.本实施例中，控制点是作为对待优化制图轨迹进行纠偏优化的参考点，为保证控制点的位置信息的精确度，控制点的位置坐标可以通过高精度测绘设备进行测量获得。
67.202、根据各个控制点的所述位姿信息，从所述关联点云地图中搜索与各个所述控制点分别对应的同名点。
68.本实施例中，为了能够快速查找到同名点，可以将控制点设置在具有明显特征的位置。例如，如图1中的左图所示，将控制点选择人行横道线的某一顶点。并且，可以在设置好控制点后进行拍照记录，得到每个控制点对应的图像信息。
69.在一些实施例中，所述位置信息还包括所述控制点所处位置的图像信息，所述根据各个控制点的所述位姿信息，从所述关联点云地图中搜索与各个所述控制点分别对应的同名点，包括：针对每个控制点，根据所述控制点的位置坐标，确定所述关联点云地图中的搜索范围；根据所述图像信息，在所述搜索范围内定位所述控制点的同名点。
70.具体的，可以根据控制点的位置坐标，在关联点云地图中对控制点的同名点进行粗搜索，例如，在搜索第一控制点对应的第一同名点时，可以将第一控制点的坐标作为圆心，以预设长度为半径划定关联点云地图中的搜索范围，在确定搜索范围后，在搜索范围内，查找关联点云地图中与第一控制点的图像信息一致的节点作为第一同名点。通过位置坐标和图像信息的配合，可以更加快速的在关联点云地图中找到控制点对应的同名点。
71.示例性的，得到拼接好的关联点云地图后，根据控制点的图像信息及其位置坐标，在关联点云地图中搜索与之对应的同名点，并获取同名点的局部站心坐标。
72.具体的，如图1中的中图所示。假设在待优化制图轨迹所在的真实物理世界中通过高精度测绘设备测算了m个控制点的精确坐标，并对其进行了拍照存档，记控制点的集合为c＝{(loni，lati，heighti)|i＝1，...，m}，为了后续计算方便，利用图5所示实施例中的表达式(1)将所有控制点转换到局部站心坐标系下，得到c
′
＝{(xi，yi，zi)|i＝1，...，m}。
73.根据控制点的位置坐标和图像信息，在关联点云地图中找到每个控制点的同名点，同名点的集合为s＝{(x
′i，y
′i，z
′i)|i＝1，...，m}，其中(xi，yi，zi)的同名点是(x
′i，y
′i，z
′i)。
74.可以理解，若待优化制图轨迹无偏移，则根据待优化制图轨迹确定的关联点云地图中的同名点的坐标将与预先在待优化轨迹轨迹节点范围内设置的控制点的坐标是重合的。那么，如果控制点的坐标与对应的同名点的坐标之间的距离越接近于0，说明该控制点对应的关联点云地图的绝对精度越高，反之，则代表绝对精度越低。
75.203、基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹。
76.本实施例中，基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹的方式有多种。
77.在一种可实现方式中，所述基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹，包括：将多个所述控制点划分为多个控制点组，并针对每个控制点组，将所述控制点组中各控制点对应的各同名点构建为所述控制点组对应的同名点组；针对每个控制点组和所述控制点组对应的同名点组，根据所述控制点组中各控制点的位置坐标确定所述控制点组的第一几何重心的位置坐标；根据所述同名点组中各同名点的位置坐标确定所述同名点组的第二几何重心的位置坐标；根据所述第一几何重心的位置坐标和所述第二几何重心的位置坐标，计算所述控制点组与对应的同名点组之间的第二距离差；在所述待优化制图轨迹的各轨迹节点中搜索与所述第一几何重心距离最近的第二目标轨迹节点；根据所述第二距离差对所述第二目标轨迹节点的位置坐标进行修正，得到修正后的所述第二目标轨迹节点的位置坐标；针对每个修正后的第二目标轨迹节点，计算所述修正后的第二目标轨迹节点与所述第二目标轨迹节点之间的误差；基于各所述误差确定第二纠偏位姿约束项；根据所述第二纠偏位姿约束项，构建目标函数，并计算得到使所述目标函数值最小化的各轨迹节点的位姿信息，获得所述优化后的制图轨迹。
78.示例性的，为了提升待优化制图轨迹的绝对精度，需要对其进行纠偏优化，即通过调整轨迹中节点的位姿，使得关联点云地图中搜索到的同名点与对应的控制点距离尽可能小。
79.在实际采集中，控制点可以是分组采集的，即沿着待优化制图轨迹的方向，每隔一定的距离采集几个控制点(每组控制点的个数大于等于2)，如图4所示。假设m个控制点总共分为g组。
80.针对每组控制点，首先，可以计算该组控制点的几何重心以及该组控制点对应的同名点组的几何重心。
81.举例来说，对于包含3个控制点的控制点组{c1，c2，c3}，ci∈c
′
，该控制点组的几何
重心可以为：
82.c
avg
＝(x
avg
，y
avg
，z
avg
)，x
avg
＝(xc1 x
c2
x
c3
)/3，
83.y
avg
＝(y
c1
y
c2
y
c3
)/3，z
avg
＝(z
c1
z
c2
z
c3
)/3。
84.同理，对应的同名点组{s1，s2，s3}，si∈s的几何重心可以为：
85.c
′
avg
＝(x
′
avg
，y
′
avg
，z
′
avg
)，x
′
avg
＝(x
′
s1
x
′
s2
x
′
s3
)/3，
86.y
′
avg
＝(y
′
s1
y
′
s2
y
′
s3
)/3，z
′
avg
＝(z
′
s1
z
′
s2
z
′
s3
)/3。
87.其次，可以根据控制点组和对应同名点组的几何重心，确定对应的纠偏位姿约束。
88.具体的，在待优化制图轨迹trj＝{(tsi，ti)}中搜索与控制点组的几何重心c
avg
距离最近的节点即满足对于所有的轨迹点，ti与c
avg
的距离最近。那么对于节点其纠偏后的位姿为：
[0089][0090]
以此类推，针对g组控制点均执行上述操作，即可得到g个纠偏位姿约束。
[0091]
在得到g个纠偏位姿约束之后，可以利用位姿图优化技术，将所有的轨迹点姿态作为待优化变量，即位姿图的顶点，然后构建目标函数。
[0092]
在一些实施例中，所述目标函数还可以包括待优化制图轨迹中相邻轨迹节点之间的位姿误差约束项和所述待优化制图轨迹中各轨迹节点的位姿的先验误差约束项。
[0093]
示例性的，目标函数可以构建如下：
[0094][0095]
其中，为相邻顶点位姿约束误差约束项，表示轨迹中相邻顶点之间的位姿约束误差，目的是保证原始制图轨迹的相对精度；为先验误差约束项，表示每个位姿的先验误差，确保待优化轨迹不会进行大幅度移动；为纠偏位姿约束项，表示控制点与同名点间的位姿误差，基于此对轨迹进行纠偏；∑1，∑2，∑3分别是上述三类误差的信息矩阵，均可以采用6
×
6的对角矩阵；ξi是第i个待优化位姿顶点对应的李代数，是一个6维向量。
[0096]
基于上述目标函数的位姿图优化问题属于非线性最小二乘问题，可以采用开源的优化库求解，如ceres，g2o，gtsam等。
[0097]
在优化完成后，将得到的优化后的待优化制图轨迹作为最终的制图轨迹输出，基于输出的最终的制图轨迹重新生产高精度地图，即可得到绝对精度更高的地图数据，如图1中的右图所示，根据经过优化后的制图轨迹重新拼接的关联点云地图中同名点与控制点的位置几乎重合，说明地图的绝对精度得到了明显提升。
[0098]
在另一种可实现方式中，所述基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹，包括：针对
每个控制点，根据所述控制点的位姿信息中的位置坐标和所述控制点对应的同名点的位置坐标，计算所述控制点与对应的同名点之间的第一距离差；在所述待优化制图轨迹的各轨迹节点中搜索与所述控制点距离最近的第一目标轨迹节点；根据所述第一距离差对所述第一目标轨迹节点的位置坐标进行修正，得到修正后的所述第一目标轨迹节点的位置坐标；针对每个修正后的第一目标轨迹节点，计算所述修正后的第一目标轨迹节点与所述第一目标轨迹节点之间的误差；基于各所述误差确定第一纠偏位姿约束项；根据所述第一纠偏位姿约束项，构建目标函数，并计算得到使所述目标函数值最小化的各轨迹节点的位姿信息，获得所述优化后的制图轨迹。
[0099]
本实施例提供的制图轨迹优化方法，通过获取多个控制点的位姿信息和关联点云地图，所述控制点位于待优化制图轨迹的轨迹节点范围内，所述关联点云地图为与所述待优化制图轨迹关联采集的三维点云地图，并根据各个控制点的所述位姿信息，从所述关联点云地图中搜索与各个所述控制点分别对应的同名点，并基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹，能够在多个控制点的同名点的约束下，减小待优化制图轨迹的偏差，实现对待优化制图轨迹的优化，从而提高基于优化后的待优化制图轨迹生成的高精度地图的绝对精度，满足行业标准和用户需求。
[0100]
图5为本技术实施例提供的制图轨迹优化方法的流程示意图二。如图5所示，在上述实施例的基础上，例如在图2所示实施例的基础上，本实施例中增加了由多个点云地图拼接得到关联点云地图的具体实现过程的说明，该方法包括：
[0101]
501、根据待优化制图轨迹，将与所述待优化制图轨迹关联采集的多个三维点云地图进行拼接，获得所述关联点云地图。
[0102]
本实施例中，所述待优化制图轨迹由多个轨迹点组成，示例性的，可以采用以下表达式来表示：
[0103]
trj＝{(tsi，ri，lo
ni
，lati，heighti)|i＝1...n}
ꢀꢀꢀ
(1)
[0104]
其中，tsi是轨迹点i的时间戳、ri是轨迹点i的三维旋转矩阵(包括姿态信息)，(loni，lati，heighti)分别是轨迹点i的经度、纬度以及高度，代表tsi时刻的绝对位置信息。
[0105]
示例性的，假设当前获取的待优化制图轨迹的各轨迹点采用的是地心坐标系(world geodetic system 1984，wgs84)，为了便于将根据待优化制图轨迹采集的三维点云进行拼接，可以将待优化制图轨迹各轨迹点的坐标系由wgs84坐标转换为局部站心坐标。具体的，可以从待优化制图轨迹的多个轨迹点中选取一个参考点，例如，可以选取轨迹的第一个轨迹点(lon1，lat1，height1)，作为参考点。在参考点选定后，根据wgs84坐标与局部站心坐标的转换关系，将待优化制图轨迹的所有轨迹点的坐标转换到以参考点为原点的三维直角坐标系下，转换关系可以采用现有转换公式，例如可以从大地测量学书籍中获得，还可以调用开源的大地测量学库，例如geographic lib等。以下将坐标转换简记为表达式(1)：
[0106]
ti＝(xi，yi，zi)＝f(lomi，lati，heighti)
ꢀꢀꢀ
(2)
[0107]
其中，(xi，yi，zi)为轨迹点i的局部站心坐标，(loni，lati，heighti)为轨迹点i的wgs84坐标，参考点(lon1，lat1，height1)经过转换后的局部坐标为(0，0，0)。
[0108]
根据以上表达式(1)和表达式(2)，可以将制图轨迹转换为：
[0109]
trj＝{(tsi，ti)}
ꢀꢀꢀ
(3)
[0110]
其中，
[0111]
在将待优化制图轨迹的坐标转换到局部站心坐标后，可以将待优化制图轨迹关联采集的各点云地图进行由激光雷达坐标系到局部站心坐标系的转换。每个点云地图包括多个目标点。
[0112]
示例性的，所述根据待优化制图轨迹，将与所述待优化制图轨迹关联采集的多个三维点云地图进行拼接，获得所述关联点云地图，可以包括以下步骤：
[0113]
针对每个三维点云地图中每个目标点，根据所述目标点的时间戳以及所述待优化制图轨迹中各轨迹节点的时间戳，从所述待优化制图轨迹中确定与所述目标点时间最近的第一轨迹节点和第二轨迹节点；所述第一轨迹节点的时间戳小于所述目标点的时间戳，所述第二轨迹节点的时间戳大于所述目标点的时间戳。
[0114]
根据所述目标点的时间戳和位姿信息、所述第一轨迹节点的时间戳和位姿信息以及所述第二轨迹节点的时间戳和位姿信息，通过线性插值算法确定所述目标点对应的虚拟轨迹节点的时间戳和位姿信息。
[0115]
根据刚体变换矩阵，将所述虚拟轨迹节点的位姿信息进行转换，得到所述目标点在所述关联点云地图中的对应点。
[0116]
具体的，对于与待优化制图轨迹关联采集的每个点云地图p，点云地图的拼接可以理解为是在待优化制图轨迹的约束下，将点云地图中各点转换到同一个参考系下，从而得到与真实物理世界一致的点云结构。
[0117]
假设点云地图p包含n个节点，每个节点包含一个时间戳，点云地图p在激光雷达坐标系下的三维坐标以及强度反射率，可以表示为：
[0118][0119]
其中，tsi是节点i的时间戳、是节点i的激光雷达坐标，ii是节点i的强度反射率。
[0120]
点云地图p中的各轨迹节点p的采集频率要大于待优化制图轨迹中各轨迹点的采集频率。因此，待优化制图轨迹中相邻两个轨迹点的采集时刻之间对应点云地图p中的多个节点p。对于p中的每个节点p，可以从待优化制图轨迹trj＝{(tsi，ti)}中找到与节点p的时间戳ts
p
最近的两个节点trj(i)，trj(i 1)，即trj(i).ts≤ts
p
≤trj(i 1).ts。
[0121]
可以采用线性插值算法得到节点p对应的轨迹位姿具体地：
[0122]
第一步，计算插值比例系数：
[0123]
第二步，计算节点p的时间戳：
[0124]
第三步，计算节点p的四元数：
[0125]
其中，是旋转矩阵对应的四元数，qi，q
i 1
分别是旋转矩阵ri，r
i 1
对应的
四元数。
[0126]
第四步，通过刚体变换矩阵对节点p进行坐标变换，得到变换后的节点p
′
即为拼接后的关联点云地图中的一个节点。
[0127]
类似的，对点云地图中的每个节点均进行如上操作，即可得到拼接后的三维点云地图。
[0128]
502、获取多个控制点的位姿信息和关联点云地图；所述控制点位于待优化制图轨迹的轨迹节点范围内；所述关联点云地图为与所述待优化制图轨迹关联采集的三维点云地图。
[0129]
503、根据各个控制点的所述位姿信息，从所述关联点云地图中搜索与各个所述控制点分别对应的同名点。
[0130]
504、基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹。
[0131]
本实施例中，步骤502至步骤504与上述实施例中步骤201至步骤203相类似，此处不再赘述。
[0132]
本实施例提供的制图轨迹优化方法，通过在待优化制图轨迹的约束下，将根据待优化制图轨迹关联采集的多个点云地图进行拼接，即可得到关联点云地图，进而根据关联点云地图中查找与多个控制点对应的同名点，在控制点的同名点的约束下调整关联点云地图，进而实现对待优化制图轨迹的纠偏优化，能够提高基于优化后的待优化制图轨迹生成的高精度地图的绝对精度，满足行业标准和用户需求。
[0133]
图6为本技术实施例提供的制图轨迹优化设备的结构示意图。如图6所示，该制图轨迹优化设备60包括：获取模块601、搜索模块602以及优化模块603。
[0134]
获取模块601，用于获取多个控制点的位姿信息和关联点云地图；所述关联点云地图是根据待优化制图轨迹，将所述待优化制图轨迹关联采集的多个点云地图进行拼接得到的。
[0135]
搜索模块602，用于根据各个控制点的所述位姿信息，从所述关联点云地图中搜索与各个所述控制点分别对应的同名点。
[0136]
优化模块603，用于基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹。
[0137]
本技术实施例提供的制图轨迹优化设备，通过获取模块601获取多个控制点的位姿信息和关联点云地图，所述控制点位于待优化制图轨迹的轨迹节点范围内，所述关联点云地图为与所述待优化制图轨迹关联采集的三维点云地图，搜索模块602根据各个控制点的所述位姿信息，从所述关联点云地图中搜索与各个所述控制点分别对应的同名点，搜索模块602基于各个所述控制点与其对应的同名点之间的距离差，对所述待优化制图轨迹的轨迹节点的位姿进行优化得到优化后的制图轨迹，能够在多个控制点的同名点的约束下，减小待优化制图轨迹的偏差，实现对待优化制图轨迹的优化，从而提高基于优化后的待优化制图轨迹生成的高精度地图的绝对精度，满足行业标准和用户需求。
[0138]
本技术实施例提供的制图轨迹优化设备，可用于执行上述的方法实施例，其实现原理和技术效果类似，本实施例此处不再赘述。
[0139]
图7为本技术实施例提供的一种制图轨迹优化设备的框图，该设备可以是计算机，消息收发设备，平板设备，医疗设备，服务器等。
[0140]
装置70可以包括以下一个或多个组件：处理组件701，存储器702，电源组件703，多媒体组件704，音频组件705，输入/输出(i/o)接口706，传感器组件707，以及通信组件708。
[0141]
处理组件701通常控制装置70的整体操作，诸如与显示，电话呼叫，数据通信，相机操作和记录操作相关联的操作。处理组件701可以包括一个或多个处理器709来执行指令，以完成上述的方法的全部或部分步骤。此外，处理组件701可以包括一个或多个模块，便于处理组件701和其他组件之间的交互。例如，处理组件701可以包括多媒体模块，以方便多媒体组件704和处理组件701之间的交互。
[0142]
存储器702被配置为存储各种类型的数据以支持在装置70的操作。这些数据的示例包括用于在装置70上操作的任何应用程序或方法的指令，联系人数据，电话簿数据，消息，图片，视频等。存储器702可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(sram)，电可擦除可编程只读存储器(eeprom)，可擦除可编程只读存储器(eprom)，可编程只读存储器(prom)，只读存储器(rom)，磁存储器，快闪存储器，磁盘或光盘。
[0143]
电源组件703为装置70的各种组件提供电力。电源组件703可以包括电源管理系统，一个或多个电源，及其他与为装置70生成、管理和分配电力相关联的组件。
[0144]
多媒体组件704包括在所述装置70和用户之间的提供一个输出接口的屏幕。在一些实施例中，屏幕可以包括液晶显示器(lcd)和触摸面板(tp)。如果屏幕包括触摸面板，屏幕可以被实现为触摸屏，以接收来自用户的输入信号。触摸面板包括一个或多个触摸传感器以感测触摸、滑动和触摸面板上的手势。所述触摸传感器可以不仅感测触摸或滑动动作的边界，而且还检测与所述触摸或滑动操作相关的持续时间和压力。在一些实施例中，多媒体组件704包括一个前置摄像头和/或后置摄像头。当装置70处于操作模式，如拍摄模式或视频模式时，前置摄像头和/或后置摄像头可以接收外部的多媒体数据。每个前置摄像头和后置摄像头可以是一个固定的光学透镜系统或具有焦距和光学变焦能力。
[0145]
音频组件705被配置为输出和/或输入音频信号。例如，音频组件705包括一个麦克风(mic)，当装置70处于操作模式，如呼叫模式、记录模式和语音识别模式时，麦克风被配置为接收外部音频信号。所接收的音频信号可以被进一步存储在存储器702或经由通信组件708发送。在一些实施例中，音频组件705还包括一个扬声器，用于输出音频信号。
[0146]
i/o接口706为处理组件701和外围接口模块之间提供接口，上述外围接口模块可以是键盘，点击轮，按钮等。这些按钮可包括但不限于：主页按钮、音量按钮、启动按钮和锁定按钮。
[0147]
传感器组件707包括一个或多个传感器，用于为装置70提供各个方面的状态评估。例如，传感器组件707可以检测到装置70的打开/关闭状态，组件的相对定位，例如所述组件为装置70的显示器和小键盘，传感器组件707还可以检测装置70或装置70一个组件的位置改变，用户与装置70接触的存在或不存在，装置70方位或加速/减速和装置70的温度变化。传感器组件707可以包括接近传感器，被配置用来在没有任何的物理接触时检测附近物体
的存在。传感器组件707还可以包括光传感器，如cmos或ccd图像传感器，用于在成像应用中使用。在一些实施例中，该传感器组件707还可以包括加速度传感器，陀螺仪传感器，磁传感器，压力传感器或温度传感器。
[0148]
通信组件708被配置为便于装置70和其他设备之间有线或无线方式的通信。装置70可以接入基于通信标准的无线网络，如wifi，2g或3g，或它们的组合。在一个示例性实施例中，通信组件708经由广播信道接收来自外部广播管理系统的广播信号或广播相关信息。在一个示例性实施例中，所述通信组件708还包括近场通信(nfc)模块，以促进短程通信。例如，在nfc模块可基于射频识别(rfid)技术，红外数据协会(irda)技术，超宽带(uwb)技术，蓝牙(bt)技术和其他技术来实现。
[0149]
在示例性实施例中，装置70可以被一个或多个应用专用集成电路(asic)、数字信号处理器(dsp)、数字信号处理设备(dspd)、可编程逻辑器件(pld)、现场可编程门阵列(fpga)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述方法。
[0150]
在示例性实施例中，还提供了一种包括指令的非临时性计算机可读存储介质，例如包括指令的存储器702，上述指令可由装置70的处理器709执行以完成上述方法。例如，所述非临时性计算机可读存储介质可以是rom、随机存取存储器(ram)、cd-rom、磁带、软盘和光数据存储设备等。
[0151]
上述的计算机可读存储介质，上述可读存储介质可以是由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(sram)，电可擦除可编程只读存储器(eeprom)，可擦除可编程只读存储器(eprom)，可编程只读存储器(prom)，只读存储器(rom)，磁存储器，快闪存储器，磁盘或光盘。可读存储介质可以是通用或专用计算机能够存取的任何可用介质。
[0152]
一种示例性的可读存储介质耦合至处理器，从而使处理器能够从该可读存储介质读取信息，且可向该可读存储介质写入信息。当然，可读存储介质也可以是处理器的组成部分。处理器和可读存储介质可以位于专用集成电路(application specific integrated circuits，简称：asic)中。当然，处理器和可读存储介质也可以作为分立组件存在于设备中。
[0153]
本领域普通技术人员可以理解：实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。前述的程序可以存储于一计算机可读取存储介质中。该程序在执行时，执行包括上述各方法实施例的步骤；而前述的存储介质包括：rom、ram、磁碟或者光盘等各种可以存储程序代码的介质。
[0154]
本技术实施例还提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时，实现如上制图轨迹优化设备执行的制图轨迹优化方法。
[0155]
最后应说明的是：以上各实施例仅用以说明本技术的技术方案，而非对其限制；尽管参照前述各实施例对本技术进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本技术各实施例技术方案的范围。