CN107577646A - 一种高精度轨迹运算方法及*** - Google Patents
一种高精度轨迹运算方法及*** Download PDFInfo
- Publication number
- CN107577646A CN107577646A CN201710731072.7A CN201710731072A CN107577646A CN 107577646 A CN107577646 A CN 107577646A CN 201710731072 A CN201710731072 A CN 201710731072A CN 107577646 A CN107577646 A CN 107577646A
- Authority
- CN
- China
- Prior art keywords
- msub
- mrow
- track
- gps
- slam
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种高精度轨迹运算方法及***，运算方法具有以下有益效果：通过点云SLAM技术中的SLAM轨迹对GPS轨迹进行回归分析和误差校正，可以将GPS的测量精度由米级提升至厘米级；由于SLAM技术不存在载波信号的接收问题，几乎可以用于任何场景下的快速高精度GPS测量；通过点云SLAM技术得到的高精度点云地图，和校正得到的该地图中某一点的GPS坐标，可以推算知晓整个高精度点云地图所有点的全球位置坐标，其中包括在实际测量中无法进行GPS测量的坐标点。运算***，包括：获取单元、变换校正单元、坐标计算单元以及逆变换单元，实现了运算方法相同的有益效果。
Description
技术领域
本发明涉及实时定位和导航领域，具体涉及一种高精度轨迹运算方法及***。
背景技术
实时定位和导航是当前智能机器人领域的研究热点，只有实现机器人的精准定位和导航才能保障机器人在未知环境中的安全，快速行驶，进而完成指定任务。现有的实时定位技术主要包括：全球定位***(Global Position System-GPS)，惯性导航以及实时定位和地图构建(Simultaneous Localization And Mapping-SLAM)技术。
全球定位***(Global Position System-GPS)是由美国国防部研制建立的一种具有全方位、全天候、全时段、高精度的卫星导航***，能为全球用户提供低成本、高精度的三维位置、速度和精确定时等导航信息。但是，在现实生活中，实时GPS的定位往往存在较大的测量误差。
这种测量误差是由GPS***中三部分决定的：1)GPS发射部分存在的卫星轨道误差，时钟误差；2)GPS信号传播过程中电离层，对流层，不均匀的气流导致的折射误差，以及现实环境中自然物，建筑物对GPS信号遮挡等问题；3)GPS接收装置接收时存在的钟差，存在的接收噪音等，都会影响GPS的实时定位。在复杂城市环境下，GPS信号会存在几十米的偏差，甚至会出现无GPS信号的情况。
现实精准GPS坐标测量中，为了提高效率，采用了实时动态定位(Real TimeKinematic-RTK)技术，RTK的工作原理是在两台接收机间加上了一套无线电通信***，将相对独立的接收机连接成一个有机的整体，进而实现动态测量站点和基准站点的GPS信号校正测量。但是，在RTK测量中，要求动态测量站点实时与基准站点进行无线电通信，现实环境中存在着自然物和建筑物的遮挡，进而RTK受限于很多应用场景。
实时定位和地图构建(Simultaneous Localization And Mapping-SLAM)技术是实现智能机器人在局部环境中实现实时定位和地图模型生成的主要手段，它主要分为位姿估计与地图表示两部分。其中，位姿估计是指实时计算物体在运动过程中的姿态、方向和轨迹信息，它是解决SLAM技术的关键。
点云匹配算法(Iterative Closet Points-ICP)可以实现两帧点云数据的配准，实现位姿估计。点云自动配准技术是通过一定的算法或者统计学规律，利用计算机计算两片点云之间的错位，从而达到把两片点云自动配准的效果。目前主要有三类方法：第一类是基于标记的配准方法，通过确定两模型中对应的特征点，并由对应特征点计算目标模型和源模型之间的空间变换；第二类是基于表面的配准方法，在模型表面上定义并计算突出的特征，根据模型表面的共同特征在重叠区域内搜索两个表面之间的唯一匹配模式；第三类是基于体素相似性的配准方法，不需要对像素做预处理(去噪或者分割)，直接作用于图像，但该方法计算量太大，很少用于三维模型配准。随着点云精准匹配技术的发展，在一定的运动范围内，位姿估计已经达到了厘米级别的精度。
基于准确的位姿估计，点云数据的SLAM技术可以输出当前点云采集环境的高精度点云地图，该地图是当前环境本身局部坐标系下的，因而难以确定其在全球中的绝对位置，换句话说，只能SLAM技术只能进行局部环境的定位，而不能进行全球定位。
发明内容
本发明的目的是提供一种高精度轨迹运算方法及***，以解决GPS测量误差大，RTK-GPS测量技术在某些应用场景下受限制，以及SLAM技术无法进行全球定位的问题。
为了实现上述目的，本发明提供如下技术方案：
本发明提供一种高精度轨迹运算方法，包括以下步骤：
获得通过时间戳标记的GPS轨迹和点云数据；
根据所述点云数据进行位姿估计，得到SLAM轨迹和点云地图；
对所述GPS轨迹进行坐标系的变换，同时将所述SLAM轨迹与变换后的GPS轨迹对齐后，通过带权重的ICP算法进行匹配并校正；
根据校正结果在所述点云地图中进行轨迹坐标值的计算，并对所述轨迹坐标值进行坐标系的逆变换，得到高精度轨迹。
上述高精度轨迹运算方法，所述SLAM轨迹的获得包括以下步骤：
定义三维空间R3中分别含有M、N个坐标点的两组点集为P1和P2；其中，P1＝{P11，P12，P13……P1M}，P2＝{P21，P22，P23……P2N}；
对点集P1中的坐标点进行旋转后平移，得到P2中相对应的坐标点；则单点变换有P2i＝R P1j+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(A2/2+(5-T+9C+4C2)*A4/24)+(61-58T+T2+600C-330e‘2)*A6/720)，
y＝k0N(A+(1-T+C)*A3/6+(5-18T+T2+72C-58e‘2)*A5/120)，
其中，A表示经度值，B表示纬度值，a表示地球长半轴，b表示地球短半轴，k0＝0.9996，T＝tan2B，A＝(L-L0)cosB，C＝e‘2cos2B，
上述高精度轨迹运算方法，通过带权重的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＝tan2(Bf)，Cf＝e‘2cos2(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、定义三维空间R3中分别含有M、N个坐标点的两组点集为P1和P2；
其中，P1＝{P11，P12，P13……P1M}，P2＝{P21，P22，P23……P2N}；现有的基于点云数据匹配进行的SLAM轨迹计算，时间上利用了点云数据采集的连续性，空间上利用了点云数据分布的相似性，ICP算法是当前应用最广的点云配准算法。
S202、对点集P1中的坐标点进行旋转后平移，得到P2中相对应的坐标点；
三维空间点集P1经过三维空间变换(旋转后平移)，与点集P2对应，单点的变换公式表示为：P2i＝R P1j+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坐标值转换至局部空间坐标系下，优选的，以东北天右手站心直角坐标系(East North Up Coordinates-ENU)为例，地心坐标系至ENU坐标系有多种转换公式，进一步的变换采用通用横轴墨卡托投影(Universal Transverse MercatorProjection-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(A2/2+(5-T+9C+4C2)*A4/24)+(61-58T+T2+600C-330e‘2)*A6/720)，
y＝k0N(A+(1-T+C)*A3/6+(5-18T+T2+72C-58e‘2)*A5/120)，
其中，A表示经度值，B表示纬度值，a表示地球长半轴，b表示地球短半轴，k0＝0.9996，T＝tan2B，A＝(L-L0)cosB，C＝e‘2cos2B，
在步骤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＝tan2(Bf)，Cf＝e‘2cos2(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测量的坐标点。
以上只通过说明的方式描述了本发明的某些示范性实施例，毋庸置疑，对于本领域的普通技术人员，在不偏离本发明的精神和范围的情况下，可以用各种不同的方式对所描述的实施例进行修正。因此，上述附图和描述在本质上是说明性的，不应理解为对本发明权利要求保护范围的限制。
Claims (10)
1.一种高精度轨迹运算方法，其特征在于，包括以下步骤：
获得通过时间戳标记的GPS轨迹和点云数据；
根据所述点云数据进行位姿估计，得到SLAM轨迹和点云地图；
对所述GPS轨迹进行坐标系的变换，同时将所述SLAM轨迹与变换后的GPS轨迹对齐后，通过带权重的ICP算法进行匹配并校正；
根据校正结果在所述点云地图中进行轨迹坐标值的计算，并对所述轨迹坐标值进行坐标系的逆变换，得到高精度轨迹。
2.根据权利要求1所述的高精度轨迹运算方法，其特征在于，所述SLAM轨迹的获得包括以下步骤：
定义三维空间R3中分别含有M、N个坐标点的两组点集为P1和P2；其中，P1＝{P11，P12，P13……P1M}，P2＝{P21，P22，P23……P2N}；
对点集P1中的坐标点进行旋转后平移，得到P2中相对应的坐标点；则单点变换有P2i＝RP1j+T；其中，R是角度旋转矩阵，T是平移矩阵；
求解两组所述点集P1、P2中坐标点的最优化问题，得到SLAM轨迹；
<mrow>
<munder>
<mi>argmin</mi>
<mrow>
<mi>R</mi>
<mo>,</mo>
<mi>T</mi>
</mrow>
</munder>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>N</mi>
</munderover>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>P</mi>
<mrow>
<mn>2</mn>
<mi>i</mi>
</mrow>
</msub>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>RP</mi>
<mrow>
<mn>1</mn>
<mi>j</mi>
</mrow>
</msub>
<mo>+</mo>
<mi>T</mi>
<mo>)</mo>
</mrow>
<mo>|</mo>
<msub>
<mo>|</mo>
<mn>2</mn>
</msub>
</mrow>
3.根据权利要求1所述的高精度轨迹运算方法，其特征在于，所述坐标系的变换包括以下步骤：
根据GPS轨迹坐标值和预先的定义，求得该坐标值处的带数N以及中央经线L0；
N＝[L/3+0.5]，其中，L表示测得的经度值；L0＝3N；
通过UTM投影将处于地心坐标系下的所述GPS轨迹坐标值变换至局部空间坐标系下的局部空间坐标值(x，y)；
x＝k0(M+NtanB(A2/2+(5-T+9C+4C2)*A4/24)+(61-58T+T2+600C-330e‘2)*A6/720)，
y＝k0N(A+(1-T+C)*A3/6+(5-18T+T2+72C-58e‘2)*A5/120)，
其中，A表示经度值，B表示纬度值，a表示地球长半轴，b表示地球短半轴，k0＝0.9996，T＝tan2B，A＝(L-L0)cosB，C＝e‘2cos2B，
4.根据权利要求1所述的高精度轨迹运算方法，其特征在于，通过带权重的ICP算法进行匹配包括以下步骤：
定义变换后的GPS轨迹坐标值在局部空间坐标系-LG中，SLAM轨迹坐标值在局部空间坐标系-LS；
将所述局部空间坐标系-LS、-LG匹配至坐标系LS’中，获得相同时间戳下的匹配关系；
<mrow>
<munder>
<mi>argmin</mi>
<mrow>
<mi>R</mi>
<mo>,</mo>
<mi>T</mi>
</mrow>
</munder>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>N</mi>
</munderover>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>P</mi>
<msub>
<mi>s</mi>
<mi>i</mi>
</msub>
</msub>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>RP</mi>
<msub>
<mi>G</mi>
<mi>i</mi>
</msub>
</msub>
<mo>+</mo>
<mi>T</mi>
<mo>)</mo>
</mrow>
<mo>|</mo>
<msub>
<mo>|</mo>
<mn>2</mn>
</msub>
</mrow>
其中，是SLAM轨迹上的某一点，是变换后的GPS轨迹上的某一点；
则有匹配关系：LS’＝LS*R+T。
5.根据权利要求1所述的高精度轨迹运算方法，其特征在于，对匹配后的轨迹进行校正包括以下步骤：
对转换后的GPS轨迹进行回归分析，得到回归轨迹；
对所述回归轨迹进行误差校正，得到校正结果。
6.根据权利要求5所述的高精度轨迹运算方法，其特征在于，所述回归分析包括以下步骤：
根据匹配结果和所述SLAM轨迹坐标值计算得到回归量
则有其中，为LS’坐标系下某个时间点的坐标，为LS’坐标系下匹配后的单个时间基准点的坐标；
通过所述回归量对所述转换后的GPS轨迹进行多个时间基准点下的多次修正，得到所述回归轨迹；
<mrow>
<msub>
<mi>L</mi>
<msub>
<mi>G</mi>
<mi>j</mi>
</msub>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>N</mi>
</munderover>
<mfrac>
<mrow>
<msub>
<mi>L</mi>
<msub>
<mi>G</mi>
<mi>i</mi>
</msub>
</msub>
<mo>+</mo>
<msub>
<mi>&Delta;</mi>
<msub>
<mi>S</mi>
<mi>i</mi>
</msub>
</msub>
</mrow>
<mi>N</mi>
</mfrac>
<mo>,</mo>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mo>,</mo>
<mo>...</mo>
<mo>...</mo>
<mi>N</mi>
</mrow>
其中，为j个时间基准点各进行N次修正后的结果。
7.根据权利要求5所述的高精度轨迹运算方法，其特征在于，所述误差校正包括以下步骤：
提取所述回归轨迹中的每个时间基准点下的回归坐标值；
通过所述SLAM轨迹坐标值对所述回归坐标值进行二次校正，得到所述校正结果；
<mrow>
<msubsup>
<mi>L</mi>
<msub>
<mi>G</mi>
<mi>i</mi>
</msub>
<mo>&prime;</mo>
</msubsup>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>L</mi>
<msub>
<mi>G</mi>
<mi>i</mi>
</msub>
</msub>
<mo>+</mo>
<msubsup>
<mi>L</mi>
<msub>
<mi>S</mi>
<mi>i</mi>
</msub>
<mo>&prime;</mo>
</msubsup>
</mrow>
<mn>2</mn>
</mfrac>
<mo>,</mo>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mo>,</mo>
<mo>...</mo>
<mo>...</mo>
<mi>N</mi>
</mrow>
8.根据权利要求1所述的高精度轨迹运算方法，其特征在于，所述轨迹坐标值的计算包括以下步骤：
将在坐标系LS’下的所述匹配结果转换回局部空间坐标系-LS中；
根据转换后的匹配结果和所述SLAM轨迹坐标值计算得到偏移量
<mrow>
<msub>
<mi>&Delta;</mi>
<msubsup>
<mi>S</mi>
<mi>i</mi>
<mo>&prime;</mo>
</msubsup>
</msub>
<mo>=</mo>
<msub>
<mi>L</mi>
<msub>
<mi>S</mi>
<mi>i</mi>
</msub>
</msub>
<mo>-</mo>
<msub>
<mi>L</mi>
<msub>
<mi>S</mi>
<mn>0</mn>
</msub>
</msub>
</mrow>
根据所述偏移量和所述校正结果计算得到所述点云地图中的轨迹坐标值；
<mrow>
<msub>
<mi>L</mi>
<mrow>
<msub>
<mi>SG</mi>
<mi>i</mi>
</msub>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>L</mi>
<mrow>
<msub>
<mi>SG</mi>
<mn>0</mn>
</msub>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>&Delta;</mi>
<msubsup>
<mi>S</mi>
<mi>i</mi>
<mo>&prime;</mo>
</msubsup>
</msub>
</mrow>
9.根据权利要求1所述的高精度轨迹运算方法，其特征在于，所述坐标系的逆变换包括以下步骤：
通过UTM反投影将局部坐标系下的所述轨迹坐标值逆变换至地心坐标系下的高精度坐标值；
其中，a表示地球长半轴，b表示地球短半轴，e表示第一偏心率e‘表示第二偏心率k0＝0.9996，Mf＝x/k0， D＝y/k0Nf，Tf＝tan2(Bf)，Cf＝e‘2cos2(Bf)。
根据所述高精度坐标值生成所述高精度轨迹。
10.一种高精度轨迹运算***，其特征在于，包括：
获取单元，用以获得GPS轨迹，并进行位姿估计后得到SLAM轨迹和点云地图；
变换校正单元，用以对所述GPS轨迹进行坐标系的变换，并通过所述SLAM轨迹对转换后的GPS轨迹进行在时间约束下的匹配和轨迹校正；
坐标计算单元，用以根据校正结果在所述点云地图中进行轨迹坐标值的计算；
逆变换单元，用以对所述轨迹坐标值进行坐标系的逆变换，得到高精度轨迹。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710731072.7A CN107577646A (zh) | 2017-08-23 | 2017-08-23 | 一种高精度轨迹运算方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710731072.7A CN107577646A (zh) | 2017-08-23 | 2017-08-23 | 一种高精度轨迹运算方法及*** |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107577646A true CN107577646A (zh) | 2018-01-12 |
Family
ID=61035151
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710731072.7A Pending CN107577646A (zh) | 2017-08-23 | 2017-08-23 | 一种高精度轨迹运算方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107577646A (zh) |
Cited By (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108594282A (zh) * | 2018-05-24 | 2018-09-28 | 武汉大学 | 一种基于高精度gnss实时协同定位的ros机器人导航方法 |
| CN109507677A (zh) * | 2018-11-05 | 2019-03-22 | 浙江工业大学 | 一种结合gps和雷达里程计的slam方法 |
| CN109579844A (zh) * | 2018-12-04 | 2019-04-05 | 电子科技大学 | 定位方法及*** |
| CN109848988A (zh) * | 2019-01-24 | 2019-06-07 | 深圳市普森斯科技有限公司 | 一种基于历史多帧点云信息融合的扫描匹配方法及*** |
| CN109991984A (zh) * | 2019-04-22 | 2019-07-09 | 上海蔚来汽车有限公司 | 用于生成高精细地图的方法、装置和计算机存储介质 |
| JP2019132717A (ja) * | 2018-01-31 | 2019-08-08 | パイオニア株式会社 | 速度算出装置、速度算出方法、及び、プログラム |
| CN110174115A (zh) * | 2019-06-05 | 2019-08-27 | 武汉中海庭数据技术有限公司 | 一种基于感知数据自动生成高精度定位地图的方法及装置 |
| CN110487264A (zh) * | 2019-09-02 | 2019-11-22 | 上海图聚智能科技股份有限公司 | 修正地图的方法、装置、电子设备、及存储介质 |
| CN110595479A (zh) * | 2019-09-23 | 2019-12-20 | 云南电网有限责任公司电力科学研究院 | 一种基于icp算法的slam轨迹评估方法 |
| CN111007530A (zh) * | 2019-12-16 | 2020-04-14 | 武汉汉宁轨道交通技术有限公司 | 激光点云数据处理方法、装置及*** |
| CN111721289A (zh) * | 2020-06-28 | 2020-09-29 | 北京百度网讯科技有限公司 | 车辆定位方法、装置、设备、存储介质及车辆 |
| CN112241016A (zh) * | 2019-07-19 | 2021-01-19 | 北京初速度科技有限公司 | 一种泊车地图地理坐标的确定方法和装置 |
| CN112382116A (zh) * | 2020-11-12 | 2021-02-19 | 浙江吉利控股集团有限公司 | 一种用于获取车辆的点云地图的方法和*** |
| EP3819674A1 (en) * | 2019-11-08 | 2021-05-12 | Outsight | Rolling environment sensing and gps optimization |
| CN112810625A (zh) * | 2021-04-19 | 2021-05-18 | 北京三快在线科技有限公司 | 一种轨迹修正的方法及装置 |
| CN113378694A (zh) * | 2021-06-08 | 2021-09-10 | 北京百度网讯科技有限公司 | 生成目标检测和定位***及目标检测和定位的方法及装置 |
| CN113532472A (zh) * | 2020-04-15 | 2021-10-22 | 北京智行者科技有限公司 | 一种检测激光建图里程计与组合导航定位偏差的方法及*** |
| CN113763573A (zh) * | 2021-09-17 | 2021-12-07 | 北京京航计算通讯研究所 | 一种三维物体数字化标注方法及装置 |
| CN113917547A (zh) * | 2021-10-08 | 2022-01-11 | 深圳安德空间技术有限公司 | 一种基于融合定位的探地雷达地下隐患定位方法及*** |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102928860A (zh) * | 2012-10-18 | 2013-02-13 | 无锡清华信息科学与技术国家实验室物联网技术中心 | 基于局部定位信息提高gps定位精度的方法 |
| US20150092048A1 (en) * | 2013-09-27 | 2015-04-02 | Qualcomm Incorporated | Off-Target Tracking Using Feature Aiding in the Context of Inertial Navigation |
| CN105258702A (zh) * | 2015-10-06 | 2016-01-20 | 深圳力子机器人有限公司 | 一种基于slam导航移动机器人的全局定位方法 |
| CN106030430A (zh) * | 2013-11-27 | 2016-10-12 | 宾夕法尼亚大学理事会 | 用于使用旋翼微型航空载具（mav）在室内和室外环境中的稳健的自主飞行的多传感器融合 |
| CN106840179A (zh) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | 一种基于多传感器信息融合的智能车定位方法 |
-
2017
- 2017-08-23 CN CN201710731072.7A patent/CN107577646A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102928860A (zh) * | 2012-10-18 | 2013-02-13 | 无锡清华信息科学与技术国家实验室物联网技术中心 | 基于局部定位信息提高gps定位精度的方法 |
| US20150092048A1 (en) * | 2013-09-27 | 2015-04-02 | Qualcomm Incorporated | Off-Target Tracking Using Feature Aiding in the Context of Inertial Navigation |
| CN106030430A (zh) * | 2013-11-27 | 2016-10-12 | 宾夕法尼亚大学理事会 | 用于使用旋翼微型航空载具（mav）在室内和室外环境中的稳健的自主飞行的多传感器融合 |
| CN105258702A (zh) * | 2015-10-06 | 2016-01-20 | 深圳力子机器人有限公司 | 一种基于slam导航移动机器人的全局定位方法 |
| CN106840179A (zh) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | 一种基于多传感器信息融合的智能车定位方法 |
Non-Patent Citations (2)
| Title |
|---|
| DMITRY BERSHADSKY ET AL.: "Indoor GPS-denied Context Based SLAM Aided Guidance for Autonomous Unmanned Aerial Systems", 《GUIDANCE, NAVIGATION, AND CONTROL AND CO-LOCATED CONFERENCES》 * |
| 程见童 等: "同时定位与制图辅助的GPS/DR组合导航", 《国防科技大学学报》 * |
Cited By (24)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2019132717A (ja) * | 2018-01-31 | 2019-08-08 | パイオニア株式会社 | 速度算出装置、速度算出方法、及び、プログラム |
| CN108594282A (zh) * | 2018-05-24 | 2018-09-28 | 武汉大学 | 一种基于高精度gnss实时协同定位的ros机器人导航方法 |
| CN109507677B (zh) * | 2018-11-05 | 2020-08-18 | 浙江工业大学 | 一种结合gps和雷达里程计的slam方法 |
| CN109507677A (zh) * | 2018-11-05 | 2019-03-22 | 浙江工业大学 | 一种结合gps和雷达里程计的slam方法 |
| CN109579844A (zh) * | 2018-12-04 | 2019-04-05 | 电子科技大学 | 定位方法及*** |
| CN109579844B (zh) * | 2018-12-04 | 2023-11-21 | 电子科技大学 | 定位方法及*** |
| CN109848988A (zh) * | 2019-01-24 | 2019-06-07 | 深圳市普森斯科技有限公司 | 一种基于历史多帧点云信息融合的扫描匹配方法及*** |
| CN109991984A (zh) * | 2019-04-22 | 2019-07-09 | 上海蔚来汽车有限公司 | 用于生成高精细地图的方法、装置和计算机存储介质 |
| CN109991984B (zh) * | 2019-04-22 | 2024-04-30 | 上海蔚来汽车有限公司 | 用于生成高精细地图的方法、装置和计算机存储介质 |
| CN110174115A (zh) * | 2019-06-05 | 2019-08-27 | 武汉中海庭数据技术有限公司 | 一种基于感知数据自动生成高精度定位地图的方法及装置 |
| CN112241016A (zh) * | 2019-07-19 | 2021-01-19 | 北京初速度科技有限公司 | 一种泊车地图地理坐标的确定方法和装置 |
| CN110487264A (zh) * | 2019-09-02 | 2019-11-22 | 上海图聚智能科技股份有限公司 | 修正地图的方法、装置、电子设备、及存储介质 |
| CN110595479B (zh) * | 2019-09-23 | 2023-11-17 | 云南电网有限责任公司电力科学研究院 | 一种基于icp算法的slam轨迹评估方法 |
| CN110595479A (zh) * | 2019-09-23 | 2019-12-20 | 云南电网有限责任公司电力科学研究院 | 一种基于icp算法的slam轨迹评估方法 |
| EP3819674A1 (en) * | 2019-11-08 | 2021-05-12 | Outsight | Rolling environment sensing and gps optimization |
| WO2021089844A1 (en) * | 2019-11-08 | 2021-05-14 | Outsight | Rolling environment sensing and gps optimization |
| CN111007530A (zh) * | 2019-12-16 | 2020-04-14 | 武汉汉宁轨道交通技术有限公司 | 激光点云数据处理方法、装置及*** |
| CN113532472A (zh) * | 2020-04-15 | 2021-10-22 | 北京智行者科技有限公司 | 一种检测激光建图里程计与组合导航定位偏差的方法及*** |
| CN111721289A (zh) * | 2020-06-28 | 2020-09-29 | 北京百度网讯科技有限公司 | 车辆定位方法、装置、设备、存储介质及车辆 |
| CN112382116A (zh) * | 2020-11-12 | 2021-02-19 | 浙江吉利控股集团有限公司 | 一种用于获取车辆的点云地图的方法和*** |
| CN112810625A (zh) * | 2021-04-19 | 2021-05-18 | 北京三快在线科技有限公司 | 一种轨迹修正的方法及装置 |
| CN113378694A (zh) * | 2021-06-08 | 2021-09-10 | 北京百度网讯科技有限公司 | 生成目标检测和定位***及目标检测和定位的方法及装置 |
| CN113763573A (zh) * | 2021-09-17 | 2021-12-07 | 北京京航计算通讯研究所 | 一种三维物体数字化标注方法及装置 |
| CN113917547A (zh) * | 2021-10-08 | 2022-01-11 | 深圳安德空间技术有限公司 | 一种基于融合定位的探地雷达地下隐患定位方法及*** |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107577646A (zh) | 一种高精度轨迹运算方法及*** | |
| CN106569239B (zh) | 一种广播式网络rtk定位技术 | |
| CN104297773B (zh) | 一种高精度北斗三频sins深组合导航*** | |
| CN103176188B (zh) | 一种区域地基增强ppp-rtk模糊度单历元固定方法 | |
| CN101241011B (zh) | 激光雷达平台上高精度定位、定姿的装置和方法 | |
| Zhang et al. | 3D mapping database aided GNSS based collaborative positioning using factor graph optimization | |
| Petrovski et al. | Digital satellite navigation and geophysics: A practical guide with GNSS signal simulator and receiver laboratory | |
| Chen et al. | Geospatial computing in mobile devices | |
| CN103562741A (zh) | 利用巡回接收器启用非gps、第二pn＆t信号表征本地误差的差分校正***增强 | |
| CN104483688A (zh) | 基于北斗卫星导航***的高精度基线解算方法 | |
| CN108279024A (zh) | 一种基于pos设备与数字航测相机的校验方法及装置 | |
| CN116324511A (zh) | 用于提供gnss校正的***和方法 | |
| CN103697885A (zh) | 自动补偿磁偏角的远程定位方法 | |
| Zhang et al. | Direct georeferencing of airborne LiDAR data in national coordinates | |
| WO2023107742A1 (en) | System and method for correcting satellite observations | |
| CN107121689A (zh) | Glonass频间偏差单历元快速估计方法 | |
| Bhatia et al. | A new approach for Location based Tracking | |
| CN103543454A (zh) | 一种嵌入在移动通讯网中的卫星定轨*** | |
| JP5077054B2 (ja) | 移動体用測位システム | |
| Berber et al. | Online kinematic GNSS data processing for small hydrographic surveys | |
| CN109254270A (zh) | 一种星载x波段合成孔径雷达干涉定标方法 | |
| CN104297761A (zh) | 基于非同时接收的定位方法 | |
| Pereira | On the utilization of Simultaneous Localization and Mapping (SLAM) along with vehicle dynamics in Mobile Road Mapping Systems | |
| CN111103603B (zh) | 基于cors***的云定位方法及装置、定位***及云端服务器 | |
| Yun et al. | On-the-fly ambiguity resolution method for pseudolite/INS integration based on double-difference square observations |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20180112 |
|
| WD01 | Invention patent application deemed withdrawn after publication |