一种高精度轨迹运算方法及系统与流程

本发明涉及实时定位和导航领域，具体涉及一种高精度轨迹运算方法及系统。

背景技术：

实时定位和导航是当前智能机器人领域的研究热点，只有实现机器人的精准定位和导航才能保障机器人在未知环境中的安全，快速行驶，进而完成指定任务。现有的实时定位技术主要包括：全球定位系统(globalpositionsystem-gps)，惯性导航以及实时定位和地图构建(simultaneouslocalizationandmapping-slam)技术。

全球定位系统(globalpositionsystem-gps)是由美国国防部研制建立的一种具有全方位、全天候、全时段、高精度的卫星导航系统，能为全球用户提供低成本、高精度的三维位置、速度和精确定时等导航信息。但是，在现实生活中，实时gps的定位往往存在较大的测量误差。

这种测量误差是由gps系统中三部分决定的：1)gps发射部分存在的卫星轨道误差，时钟误差；2)gps信号传播过程中电离层，对流层，不均匀的气流导致的折射误差，以及现实环境中自然物，建筑物对gps信号遮挡等问题；3)gps接收装置接收时存在的钟差，存在的接收噪音等，都会影响gps的实时定位。在复杂城市环境下，gps信号会存在几十米的偏差，甚至会出现无gps信号的情况。

现实精准gps坐标测量中，为了提高效率，采用了实时动态定位(realtimekinematic-rtk)技术，rtk的工作原理是在两台接收机间加上了一套无线电通信系统，将相对独立的接收机连接成一个有机的整体，进而实现动态测量站点和基准站点的gps信号校正测量。但是，在rtk测量中，要求动态测量站点实时与基准站点进行无线电通信，现实环境中存在着自然物和建筑物的遮挡，进而rtk受限于很多应用场景。

实时定位和地图构建(simultaneouslocalizationandmapping-slam)技术是实现智能机器人在局部环境中实现实时定位和地图模型生成的主要手段，它主要分为位姿估计与地图表示两部分。其中，位姿估计是指实时计算物体在运动过程中的姿态、方向和轨迹信息，它是解决slam技术的关键。

点云匹配算法(iterativeclosetpoints-icp)可以实现两帧点云数据的配准，实现位姿估计。点云自动配准技术是通过一定的算法或者统计学规律，利用计算机计算两片点云之间的错位，从而达到把两片点云自动配准的效果。目前主要有三类方法：第一类是基于标记的配准方法，通过确定两模型中对应的特征点，并由对应特征点计算目标模型和源模型之间的空间变换；第二类是基于表面的配准方法，在模型表面上定义并计算突出的特征，根据模型表面的共同特征在重叠区域内搜索两个表面之间的唯一匹配模式；第三类是基于体素相似性的配准方法，不需要对像素做预处理(去噪或者分割)，直接作用于图像，但该方法计算量太大，很少用于三维模型配准。随着点云精准匹配技术的发展，在一定的运动范围内，位姿估计已经达到了厘米级别的精度。

基于准确的位姿估计，点云数据的slam技术可以输出当前点云采集环境的高精度点云地图，该地图是当前环境本身局部坐标系下的，因而难以确定其在全球中的绝对位置，换句话说，只能slam技术只能进行局部环境的定位，而不能进行全球定位。

技术实现要素：

本发明的目的是提供一种高精度轨迹运算方法及系统，以解决gps测量误差大，rtk-gps测量技术在某些应用场景下受限制，以及slam技术无法进行全球定位的问题。

为了实现上述目的，本发明提供如下技术方案：

本发明提供一种高精度轨迹运算方法，包括以下步骤：

获得通过时间戳标记的gps轨迹和点云数据；

根据所述点云数据进行位姿估计，得到slam轨迹和点云地图；

对所述gps轨迹进行坐标系的变换，同时将所述slam轨迹与变换后的gps轨迹对齐后，通过带权重的icp算法进行匹配并校正；

根据校正结果在所述点云地图中进行轨迹坐标值的计算，并对所述轨迹坐标值进行坐标系的逆变换，得到高精度轨迹。

上述高精度轨迹运算方法，所述slam轨迹的获得包括以下步骤：

定义三维空间r³中分别含有m、n个坐标点的两组点集为p1和p2；其中，p1＝{p11，p12，p13……p1m}，p2＝{p21，p22，p23……p2n}；

对点集p1中的坐标点进行旋转后平移，得到p2中相对应的坐标点；则单点变换有p2i＝rp1j+t；其中，r是角度旋转矩阵，t是平移矩阵；

求解两组所述点集p1、p2中坐标点的最优化问题，得到slam轨迹；

上述高精度轨迹运算方法，所述坐标系的变换包括以下步骤：

根据gps轨迹坐标值和预先的定义，求得该坐标值处的带数n以及中央经线l0；

n＝[l/3+0.5]，其中，l表示测得的经度值；l0＝3n；

通过utm投影将处于地心坐标系下的所述gps轨迹坐标值变换至局部空间坐标系下的局部空间坐标值(x，y)；

x＝k0(m+ntanb(a²/2+(5-t+9c+4c²)*a⁴/24)+(61-58t+t²+600c-330e^‘2)*a⁶/720)，

y＝k0n(a+(1-t+c)*a³/6+(5-18t+t²+72c-58e^‘2)*a⁵/120)，

其中，a表示经度值，b表示纬度值，a表示地球长半轴，b表示地球短半轴，k0＝0.9996，t＝tan²b，a＝(l-l0)cosb，c＝e^‘2cos²b，

上述高精度轨迹运算方法，通过带权重的icp算法进行匹配包括以下步骤：

定义变换后的gps轨迹坐标值在局部空间坐标系-lg中，slam轨迹坐标值在局部空间坐标系-ls；

将所述局部空间坐标系-ls和所述局部空间坐标系-lg匹配至坐标系ls’中，获得相同时间戳下的匹配关系；

其中，是slam轨迹上的某一点，是变换后的gps轨迹上的某一点；

则有匹配关系：ls’＝ls*r+t。

上述高精度轨迹运算方法，对匹配后的轨迹进行校正包括以下步骤：

对转换后的gps轨迹进行回归分析，得到回归轨迹；

对所述修正轨迹进行误差校正，得到校正结果。

上述高精度轨迹运算方法，所述回归分析包括以下步骤：

根据匹配结果和所述slam轨迹坐标值计算得到回归量

则有其中，为ls’坐标系下某个时间点的坐标，为ls’坐标系下匹配后的单个时间基准点的坐标；

通过所述回归量对所述转换后的gps轨迹进行多个时间基准点下的多次修正，得到所述回归轨迹；

其中，为j个时间基准点各进行n次修正后的结果。

上述高精度轨迹运算方法，所述误差校正包括以下步骤：

提取所述回归轨迹中的每个时间基准点下的回归坐标值；

通过所述slam轨迹坐标值对所述回归坐标值进行二次校正，得到所述校正结果；

上述高精度轨迹运算方法，所述轨迹坐标值的计算包括以下步骤：

将在坐标系ls^’下的所述匹配结果转换回局部空间坐标系-ls中；

根据转换后的匹配结果和所述slam轨迹坐标值计算得到偏移量

根据所述偏移量和所述校正结果计算得到所述点云地图中的轨迹坐标值；

上述高精度轨迹运算方法，所述坐标系的逆变换包括以下步骤：

通过utm反投影将局部坐标系下的所述轨迹坐标值逆变换至地心坐标系下的高精度坐标值；

其中，a表示地球长半轴，b表示地球短半轴，e表示第一偏心率e^‘表示第二偏心率-k0＝0.9996，mf＝x/k0，d＝y/k0nf，tf＝tan²(bf)，cf＝e^‘2cos²(bf)。

根据所述高精度坐标值生成所述高精度轨迹。

上述技术方案中，本发明提供的一种高精度轨迹运算方法，具有以下有益效果：

1)针对于现实环境中gps测量存在的误差，通过点云slam技术中的slam轨迹对gps轨迹进行回归分析和误差校正，可以将gps的测量精度由米级提升至厘米级；

2)针对于rtk-gps测量技术在某些应用场景下的限制，slam技术不存在载波信号的接收问题，几乎可以用于任何场景下的快速高精度gps轨迹测量；

3)针对于当前点云slam技术仅仅能得到局部环境的坐标，通过点云slam技术得到的高精度点云地图，和校正得到的该地图中某一点的gps坐标，可以推算知晓整个高精度点云地图所有点的全球位置坐标，其中包括在实际测量中无法进行gps测量的坐标点。

本发明还提供一种高精度轨迹运算系统，包括：

获取单元，用以获得gps轨迹，并进行位姿估计后得到slam轨迹和点云地图；

变换校正单元，用以对所述gps轨迹进行坐标系的变换，并通过所述slam轨迹对转换后的gps轨迹进行在时间约束下的匹配和轨迹校正；

坐标计算单元，用以根据校正结果在所述点云地图中进行轨迹坐标值的计算；

逆变换单元，用以对所述轨迹坐标值进行坐标系的逆变换，得到高精度轨迹。

本发明提供的一种高精度轨迹运算系统，具有以下有益效果：

1)针对于现实环境中gps测量存在的误差，通过变换校正单元中的slam轨迹对gps轨迹进行回归分析和误差校正，可以将gps的测量精度由米级提升至厘米级；

2)针对于rtk-gps测量技术在某些应用场景下的限制，slam技术不存在载波信号的接收问题，几乎可以用于任何场景下的快速高精度gps测量；

3)针对于当前点云slam技术仅仅能得到局部环境的坐标，通过各个单元的配合得到的高精度点云地图，和校正得到的该地图中某一点的gps坐标，可以推算知晓整个高精度点云地图所有点的全球位置坐标，其中包括在实际测量中无法进行gps测量的坐标点。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明中记载的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的高精度轨迹运算方法的结构示意图；

图2为本发明一优选实施例提供的高精度轨迹运算方法的流程示意图；

图3为本发明一优选实施例提供的高精度轨迹运算方法的流程示意图；

图4为本发明一优选实施例提供的高精度轨迹运算方法的流程示意图；

图5为本发明一优选实施例提供的高精度轨迹运算方法的流程示意图；

图6为本发明一优选实施例提供的高精度轨迹运算方法的流程示意图；

图7为本发明一优选实施例提供的高精度轨迹运算方法的流程示意图；

图8为本发明一优选实施例提供的高精度轨迹运算方法的流程示意图；

图9为本发明一优选实施例提供的高精度轨迹运算方法的流程示意图；

图10为本发明实施例提供的高精度轨迹运算系统的结构示意图。

具体实施方式

为了使本领域的技术人员更好地理解本发明的技术方案，下面将结合附图对本发明作进一步的详细介绍。

如图1-9所示，为本发明实施例提供的一种高精度轨迹运算方法，包括以下步骤：

s101、获得通过时间戳标记的gps轨迹和点云数据；

gps轨迹可以通过gps定位系统、rtk-gps定位系统或者现有技术中的其他定位技术测量获得。优选的，使用车载激光雷达采集高精度点云数据，同时使用车载gps接收机采集gps信号；两个信号源均与计算机相连接，计算机发出时间序列，给点云数据和gps数据标记上时间戳。通过这个过程，可以得到时间戳对齐的gps轨迹和点云信号。测得的gps轨迹存在较大的误差，因此，需要再通过slam技术进行位姿估计后测得slam轨迹，基于准确的位姿估计，点云数据的slam技术可以输出当前点云采集环境的高精度点云地图。slam轨迹由激光雷达生成，具有广泛的可用性，在各种不良的环境下仍可稳定提供高精度轨迹，这避免了rtk方案对于环境要求的局限性。

s102、根据所述点云数据进行位姿估计，得到slam轨迹和点云地图；

在步骤s102中，所述slam轨迹的获得包括以下步骤：

s201、定义三维空间r³中分别含有m、n个坐标点的两组点集为p1和p2；

其中，p1＝{p11，p12，p13……p1m}，p2＝{p21，p22，p23……p2n}；现有的基于点云数据匹配进行的slam轨迹计算，时间上利用了点云数据采集的连续性，空间上利用了点云数据分布的相似性，icp算法是当前应用最广的点云配准算法。

s202、对点集p1中的坐标点进行旋转后平移，得到p2中相对应的坐标点；

三维空间点集p1经过三维空间变换(旋转后平移)，与点集p2对应，单点的变换公式表示为：p2i＝rp1j+t；其中，r是角度旋转矩阵，t是平移矩阵；

s203、求解两组所述点集p1、p2中坐标点的最优化问题，得到slam轨迹；

考虑整个点集p1的匹配情况，可以将icp问题转化为最优化问题：

s103、对所述gps轨迹进行坐标系的变换，同时将所述slam轨迹与变换后的gps轨迹对齐后，通过带权重的icp算法进行匹配并校正；

采用带权重的icp算法，将分段的slam轨迹与对应gps轨迹进行匹配，其中点对点的匹配按照已经加上的时间戳对齐。每个点的权重由两部分组成：第一，即时速率，速度较慢的轨迹得到的权重较低，这样可以消除由于停车带来的重采样问题；第二，匹配结果，初次匹配结果较差的点得到的权重较低，经过多次迭代后这些点的权重会更低。权重分配代表每个点gps轨迹的置信度，最后匹配的结果主要由高置信度的gps轨迹点决定。在gps信号出现短时间偏差时，slam轨迹通过匹配算法，有效的消除这种偏差；在gps信号缺失的情况下，使用slam轨迹分段贴合，代替gps信号，生成高精度轨迹，直到gps信号再次可用。

在步骤s103中，所述坐标系的变换包括以下步骤：

s301、根据gps轨迹坐标值和预先的定义，求得该坐标值处的带数n以及中央经线l0；

现实环境中测得的gps坐标值，是在地心坐标系下的。为了进行gps轨迹的回归分析，需要将地心坐标系下的gps坐标值转换至局部空间坐标系下，优选的，以东北天右手站心直角坐标系(eastnorthupcoordinates-enu)为例，地心坐标系至enu坐标系有多种转换公式，进一步的变换采用通用横轴墨卡托投影(universaltransversemercatorprojection-utm)。

作为本实施例中优选的，utm投影以中央经线为x轴，赤道为y轴建立坐标系，其中正北方为x轴正方向，正东方为y轴正方向。本发明中预先的定义为以3度划分带为例，从1.5度经线开始划分，每隔3度划分为一带，将地球总共划分为120带。

则n＝[l/3+0.5]，其中，l表示测得的经度值；l0＝3n；

s302、通过utm投影将处于地心坐标系下的所述gps轨迹坐标值变换至局部空间坐标系下的局部空间坐标值(x，y)；

x＝k0(m+ntanb(a²/2+(5-t+9c+4c²)*a⁴/24)+(61-58t+t²+600c-330e^‘2)*a⁶/720)，

y＝k0n(a+(1-t+c)*a³/6+(5-18t+t²+72c-58e^‘2)*a⁵/120)，

其中，a表示经度值，b表示纬度值，a表示地球长半轴，b表示地球短半轴，k0＝0.9996，t＝tan²b，a＝(l-l0)cosb，c＝e^‘2cos²b，

在步骤s103中，通过带权重的icp算法进行匹配包括以下步骤：

s401、定义变换后的gps轨迹坐标值在局部空间坐标系-lg中，slam轨迹坐标值在局部空间坐标系-ls；

s402、将所述局部空间坐标系-ls、-lg匹配至坐标系ls’中，获得相同时间戳下的匹配关系；

其中，是slam轨迹上的某一点，是变换后的gps轨迹上的某一点；

则有匹配关系：ls’＝ls*r+t。

gps轨迹坐标值由地心坐标系投影至局部空间坐标系-lg，slam轨迹坐标值在局部空间坐标系-ls，将坐标系ls统一匹配至坐标系ls’，即将slam轨迹进行南北方向的校正和平移。对于同一个时间而言，运动物体计算得到的gps轨迹坐标值和slam轨迹坐标值理论上是一致的，所以，本发明采用了相同时间戳约束下的icp方法，进而进行gps轨迹回归。通过上述运算实现了时间戳约束下的gps轨迹和slam轨迹的匹配，得到两者间的匹配关系。

在步骤s103中，对匹配后的轨迹进行校正包括以下步骤：

s501、对转换后的gps轨迹进行回归分析，得到回归轨迹；

gps轨迹和slam轨迹进行匹配后，在同一个坐标系下，slam轨迹上不同时间点间的相对距离和gps轨迹相对距离一致，可以利用精准的slam轨迹进行gps轨迹的回归校正，得到坐标精度较高的回归轨迹。

在步骤s501中，所述回归分析包括以下步骤：

s601、根据匹配结果和所述slam轨迹坐标值计算得到回归量

则有其中，为ls’坐标系下某个时间点的坐标，为ls’坐标系下匹配后的单个时间基准点的坐标；

s602、通过所述回归量对所述转换后的gps轨迹进行多个时间基准点下的多次修正，得到所述回归轨迹；

其中，为j个时间基准点各进行n次修正后的结果。

单个时间基准点下的单次修正：

其中，为lg坐标系下的某一点，为lg坐标系下单个基准点单次回归的结果；

单个时间基准点下的多次修正，后取平均值：

根据上述运算，多个时间基准点下的多次修正的平均结果为：

s502、对所述回归轨迹进行误差校正，得到校正结果。

在步骤s502中，所述误差校正包括以下步骤：

s701、提取所述回归轨迹中的每个时间基准点下的回归坐标值；

s702、通过所述slam轨迹坐标值对所述回归坐标值进行二次校正，得到所述校正结果；

经过一次回归分析后的gps轨迹和精确的slam轨迹间存在较小误差，本发明进行二次校正，进一步提高gps轨迹的精度。

s104、根据校正结果在所述点云地图中进行轨迹坐标值的计算；并对所述轨迹坐标值进行坐标系的逆变换，得到高精度轨迹。

经过以上步骤的匹配、回归分析以及误差校正后，根据校正结果在点云地图中的坐标计算后得到的轨迹坐标值，是在局部空间坐标系下的。为了得到地心坐标系下的gps坐标值，同样需要进行坐标值的转换工作。如上文所述的utm投影，本发明采用utm反投影进行逆变换即可，计算出经度值a和纬度值b。在步骤s104中，所述轨迹坐标值的计算包括以下步骤：

s801、将在坐标系ls’下的所述匹配结果转换回局部空间坐标系-ls中；

s802、根据转换后的匹配结果和所述slam轨迹坐标值计算得到偏移量

其中，为ls坐标系下的某个时间点坐标，为ls坐标系下回归后的单个时间基准点的slam轨迹坐标值；

s803、根据所述偏移量和所述校正结果计算得到所述点云地图中的轨迹坐标值；

其中，为ls坐标系下回归后的单个时间基准点的gps轨迹坐标值。

在步骤s104中，所述坐标系的逆变换包括以下步骤：

通过utm反投影将局部坐标系下的所述轨迹坐标值逆变换至地心坐标系下的高精度坐标值；

其中，a表示地球长半轴，b表示地球短半轴，e表示第一偏心率-e‘表示第二偏心率-k0＝0.9996，mf＝x/k0，d＝y/k0nf，tf＝tan²(bf)，cf＝e^‘2cos²(bf)。

根据所述高精度坐标值生成所述高精度轨迹。

通过逆变换得到的高精度坐标值进行轨迹的绘制，可以实时了解到机器人的工作位置，从而可以保障机器人在未知环境中的安全，快速行驶，进而完成指定任务；将两个独立来源的信号(gps接收机和激光雷达)结合，匹配得到全局坐标系下的高精度轨迹。

综上所述，本发明提供的一种高精度轨迹运算方法，具有以下有益效果：

1)针对于现实环境中gps测量存在的误差，通过点云slam技术中的slam轨迹对gps轨迹进行回归分析和误差校正，可以将gps的测量精度由米级提升至厘米级；

2)针对于rtk-gps测量技术在某些应用场景下的限制，slam技术不存在载波信号的接收问题，几乎可以用于任何场景下的快速高精度gps测量；

3)针对于当前点云slam技术仅仅能得到局部环境的坐标，通过点云slam技术得到的高精度点云地图，和校正得到的该地图中某一点的gps坐标，可以推算知晓整个高精度点云地图所有点的全球位置坐标，其中包括在实际测量中无法进行gps测量的坐标点。

如图10所示，为本发明实施例还提供的一种高精度轨迹运算系统，包括：

获取单元10，用以获得gps轨迹，并进行位姿估计后得到slam轨迹和点云地图；

变换校正单元20，用以对所述gps轨迹进行坐标系的变换，并通过所述slam轨迹对转换后的gps轨迹进行在时间约束下的匹配和轨迹校正；

坐标计算单元30，用以根据校正结果在所述点云地图中进行轨迹坐标值的计算；

逆变换单元40，用以对所述轨迹坐标值进行坐标系的逆变换，得到高精度轨迹。

本发明提供的一种高精度轨迹运算系统，具有以下有益效果：

1)针对于现实环境中gps测量存在的误差，通过变换校正单元20中的slam轨迹对gps轨迹进行回归分析和误差校正，可以将gps的测量精度由米级提升至厘米级；

2)针对于rtk-gps测量技术在某些应用场景下的限制，slam技术不存在载波信号的接收问题，几乎可以用于任何场景下的快速高精度gps测量；

3)针对于当前点云slam技术仅仅能得到局部环境的坐标，通过各个单元的配合得到的高精度点云地图，和校正得到的该地图中某一点的gps坐标，可以推算知晓整个高精度点云地图所有点的全球位置坐标，其中包括在实际测量中无法进行gps测量的坐标点。

以上只通过说明的方式描述了本发明的某些示范性实施例，毋庸置疑，对于本领域的普通技术人员，在不偏离本发明的精神和范围的情况下，可以用各种不同的方式对所描述的实施例进行修正。因此，上述附图和描述在本质上是说明性的，不应理解为对本发明权利要求保护范围的限制。