CN114964216A - 一种车辆定位方法及*** - Google Patents
一种车辆定位方法及*** Download PDFInfo
- Publication number
- CN114964216A CN114964216A CN202110201534.0A CN202110201534A CN114964216A CN 114964216 A CN114964216 A CN 114964216A CN 202110201534 A CN202110201534 A CN 202110201534A CN 114964216 A CN114964216 A CN 114964216A
- Authority
- CN
- China
- Prior art keywords
- vio
- coordinate system
- data
- uwb
- vector
- 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
- 238000000034 method Methods 0.000 title claims abstract description 42
- 239000013598 vector Substances 0.000 claims abstract description 118
- 239000011159 matrix material Substances 0.000 claims abstract description 80
- 230000009466 transformation Effects 0.000 claims abstract description 52
- 230000000007 visual effect Effects 0.000 claims abstract description 13
- 238000013519 translation Methods 0.000 claims description 43
- 238000006243 chemical reaction Methods 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000012216 screening Methods 0.000 claims description 11
- 238000001914 filtration Methods 0.000 claims description 2
- 230000001131 transforming effect Effects 0.000 claims 1
- 230000007613 environmental effect Effects 0.000 abstract description 5
- 238000005286 illumination Methods 0.000 abstract description 5
- 230000004927 fusion Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000005284 excitation Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0257—Hybrid positioning
- G01S5/0263—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
本发明提供一种车辆定位方法及***，包括，步骤S1，在车辆行驶过程中通过视觉惯性里程计VIO采集VIO数据，通过超宽带UWB模块采集UWB定位数据；步骤S2，根据预先标定的VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据进行坐标变换和外参变换，转换为UWB坐标系下的VIO数据，并与UWB定位数据进行融合；步骤S3，根据融合后的定位数据确定车辆定位信息。本发明成本更低，能更好地避免光照、视野或者环境特征缺失等问题影响实现VIO数据与UWB定位数据进行融合；简单易用且有效的实现车辆定位。
Description
技术领域
本发明涉及室内车辆定位技术领域，特别是涉及一种车辆定位方法及***。
背景技术
室内车辆定位是自动驾驶技术的一个关键所在。UWB(超宽带，Ultra Wide Band)定位是目前广泛使用的室内定位技术，但噪声较大导致其精度有限，而且特征点路标点太少，地图信息不足，无法满足自动驾驶定位的要求。因此UWB往往会配合其他技术手段来提高其定位精度及可靠性，以及丰富其地图特征信息。常见的有UWB结合IMU(InertialMeasurement Unit，惯性测量单元)、激光雷达融合定位，但激光雷达成本高昂，短期内难以在自动驾驶车辆上量产。
现有技术中，典型的解决方案包括采用了激光雷达，但激光雷达成本高昂，难以在自动驾驶车辆中实现量产；或者融合了双目CCD(电荷耦合器件，charge coupled device)与UWB，但光靠双目CCD与UWB定位，鲁棒性不足，容易受到光照、视野或者环境特征缺失等问题影响，使得CCD定位失准，从而进一步影响整体的定位精度。
发明内容
本发明的目的在于，提出一种车辆定位方法及***，解决现有方法成本高、定位精度不足的技术问题。
一方面，提供一种车辆定位方法，包括以下步骤：
步骤S1，在车辆行驶过程中通过视觉惯性里程计VIO采集VIO数据，通过超宽带UWB模块采集UWB定位数据；
步骤S2，根据预先标定的VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据进行坐标变换和外参变换，转换为UWB坐标系下的VIO数据，并与UWB定位数据进行融合；
步骤S3，根据融合后的定位数据确定车辆定位信息。
优选地，在步骤S2中，所述VIO坐标系与UWB坐标系之间的变换矩阵及外参向量的标定方式，具体包括：
步骤S21，将采集的VIO数据和UWB定位数据按照时间戳进行筛选；
步骤S22，判断筛选后的VIO数据量和UWB定位数据量是否满足预设的有效数据量阈值；当满足预设的有效数据量阈值时，获得所述UWB定位数据在世界坐标系下的三维向量；
步骤S23，获取以VIO初始点为原点的坐标系下的初始旋转矩阵与初始平移向量，将获得的三维向量、初始旋转矩阵及初始平移向量输入预设的残差模型，获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量。
优选地，在步骤S2中，所述将采集的VIO数据进行坐标变换和外参变换，转换为UWB坐标系下的VIO数据，具体包括：
步骤S24，根据筛选后的VIO数据确定以VIO运动初始点为原点的坐标系；
步骤S25，根据以VIO运动初始点为原点的坐标系、VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据转化为UWB所在坐标系下的VIO数据。
优选地，在步骤S21中，所述按照时间戳进行筛选，具体包括：
调取预设时间段的VIO数据或UWB定位数据，获得样本数据；
将时间差在预设时间阈值内的样本数据合并为同一时刻的数据，获得筛选后的VIO数据或UWB定位数据。
优选地，在步骤S22中，所述判断筛选后的VIO数据量和UWB定位数据量是否满足预设的有效数据量阈值，具体包括：
当满足预设的有效数据量阈值时，将筛选后的UWB定位数据输入预设的高斯投影模型，获得所述UWB定位数据在世界坐标系下的三维向量；
当不满足预设的有效数据量阈值时，重复执行步骤S21-步骤S22，直到满足有效数据量阈值。
优选地，在步骤S23中，所述获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，具体包括：
将获得的三维向量、初始旋转矩阵及初始平移向量输入以下公式确定：
其中，
为VIO坐标系与UWB坐标系之间的旋转矩阵；
为VIO坐标系与UWB坐标系之间的平移向量；Pex为VIO坐标系与UWB坐标系之间外参向量；
为UWB定位数据在世界坐标系下的三维向量；
为以VIO运动初始点为原点的坐标系下的旋转矩阵；
为以VIO运动初始点为原点的坐标系下的平移向量。
优选地，所述步骤S25中，所述将采集的VIO数据转化为UWB所在坐标系下的VIO数据，具体包括：
将采集的VIO数据输入以下公式，确定UWB所在坐标系下的VIO数据：
其中，
为VIO坐标系与UWB坐标系之间的旋转矩阵；
为VIO坐标系与UWB坐标系之间的平移向量；Pex为VIO坐标系与UWB坐标系之间外参向量；
为UWB定位数据在世界坐标系下的三维向量；
为以VIO运动初始点为原点的坐标系下的旋转矩阵；
为以VIO运动初始点为原点的坐标系下的平移向量；
为UWB所在坐标系下的VIO数据。
另一方面，还提供一种车辆定位***，用以实现所述的车辆定位方法，包括：
视觉惯性里程计VIO，用于在车辆行驶过程中采集VIO数据；
超宽带UWB模块，用于在车辆行驶过程中采集UWB定位数据；
所述变换矩阵及外参向量计算模块，用以根据预设的标定规则获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量；
所述VIO数据转化模块，用以根据VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据进行坐标变换和外参变换，转换为UWB坐标系下的VIO数据，并与UWB定位数据进行融合。
优选地，所述变换矩阵及外参向量计算模块还用于，
将采集的VIO数据和UWB定位数据按照时间戳进行筛选；
判断筛选后的VIO数据量和UWB定位数据量是否满足预设的有效数据量阈值；当满足时，将筛选后的UWB定位数据输入预设的高斯投影模型，获得所述UWB定位数据在世界坐标系下的三维向量；当不满足时，重复执行筛选和判断VIO数据、UWB定位数据过程，直到满足有效数据量阈值；
获取以VIO初始点为原点的坐标系下的初始旋转矩阵与初始平移向量，将获得的三维向量、初始旋转矩阵及初始平移向量输入预设的残差模型，获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量。
优选地，所述VIO数据转化模块还用于根据筛选后的VIO数据确定以VIO运动初始点为原点的坐标系；根据以VIO运动初始点为原点的坐标系、VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据输入以下公式，转化为UWB所在坐标系下的VIO数据：
其中，
为VIO坐标系与UWB坐标系之间的旋转矩阵；
为VIO坐标系与UWB坐标系之间的平移向量；Pex为VIO坐标系与UWB坐标系之间外参向量；
为UWB定位数据在世界坐标系下的三维向量；
为以VIO运动初始点为原点的坐标系下的旋转矩阵；
为以VIO运动初始点为原点的坐标系下的平移向量；
为UWB所在坐标系下的VIO数据。
综上，实施本发明的实施例，具有如下的有益效果：
本发明提供的车辆定位方法及***，利用VIO(visual-inertial odometry，视觉惯性里程计)与UWB进行融合建图定位，不需要引入激光雷达也能获得足够的视觉特征点云信息，成本更低；同时，VIO比CCD有更强的鲁棒性，能更好地避免光照、视野或者环境特征缺失等问题影响；简单易用且有效的实现VIO与UWB外参及坐标变换矩阵标定，进而实现车辆的成本低且精准有效的定位。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，根据这些附图获得其他的附图仍属于本发明的范畴。
图1为本发明实施例中一种车辆定位方法的主流程示意图。
图2为本发明实施例中一种车辆定位方法的逻辑程示意图。
图3为本发明实施例中一种车辆定位***的结构示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述。
如图1和2所示，为本发明提供的一种车辆定位方法的一个实施例的示意图。在该实施例中，所述方法包括以下步骤：
步骤S1，在车辆行驶过程中通过视觉惯性里程计VIO采集VIO数据，通过超宽带UWB模块采集UWB定位数据；可以理解的是，采集VIO数据和UWB定位数据的目的主要是为了采集计算样本，用于确定变换矩阵及外参向量；其次是为了后续VIO数据转化采集样本基础。采集计算样本的时候，需要控制车辆行走一定的路径，具体地，最好在UWB信号较强的区域，尽可能保证场地光照充足，路面平整，减少环境因素的影响；同时，车辆行驶的路径避免单一，需要曲直结合，为不同运动方向提供足够的激励，采集数据的时间以5分钟以上为宜，保证充足的数据样本。
步骤S2，根据预先标定的VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据进行坐标变换和外参变换，转换为UWB坐标系下的VIO数据，并与UWB定位数据进行融合。
具体的实施例中，确定VIO坐标系与UWB坐标系之间的变换矩阵及外参向量的标定方式具体是，将采集的VIO数据和UWB定位数据按照时间戳进行筛选；判断筛选后的VIO数据量和UWB定位数据量是否满足预设的有效数据量阈值；可以理解的是，目的是尽可能保证每组数据的时间戳更接近实际值，具体地如何合并，可通过硬件层面对不同的传感器数据同步进行实现，也可借助额外的手段求出VIO与UWB的实际时间差进行补偿，在此不再赘述。具体的筛选过程，调取预设时间段的VIO数据或UWB定位数据，获得样本数据；将时间差在预设时间阈值内的样本数据合并为同一时刻的数据，获得筛选后的VIO数据或UWB定位数据，例如，将时间差在0.1s以内的数据绑定在一起，看作同一时刻的数据。
具体地，当满足预设的有效数据量阈值时，获得所述UWB定位数据在世界坐标系下的三维向量；当不满足预设的有效数据量阈值时，重复执行按照时间戳进行筛选以及判断筛选后的VIO数据量和UWB定位数据量是否满足预设的有效数据量阈值的过程，直到满足有效数据量阈值；当满足预设的有效数据量阈值时，将筛选后的UWB定位数据输入预设的高斯投影模型，获得所述UWB定位数据在世界坐标系下的三维向量(经过投影变换后，UWB的经纬高度数据可以转换为世界坐标系W下的XYZ坐标，即三维向量PW UWB)。
上述的高斯投影具体为，高斯投影6度带，自0度带子午线起每隔经差6度自西向东分带，一次编号1,2,3，…。我国6度带中央子午线的经度，由69度起每隔6度而至135度，共计12带，带号用n表示，中央子午线的经度用L0表示，它们的关系是：L0＝6n-3。
高斯投影3度带是在6度带的基础上形成的，它的中央子午线一部分同6度带中央子午线重合，一部分同6度带的分界子午线重合，如用n^‘表示3度带的带号，L表示3度带中央子午线经度，它们的关系L＝3n’。
在投影面上，中央子午线和赤道的投影都是直线，并且以中央子午线和赤道的交点O作为坐标原点，以中央子午线的投影为纵坐标轴，以赤道的投影为横坐标轴，这样形成了高斯平面直角坐标系。在我国x坐标都是正的，y坐标的最大值(在赤道上)约为330km。为了避免出现负的横坐标，可在横坐标上加上500000m。此外还应在坐标前面冠以带号。这种坐标称为国家统一坐标。
具体的投影过程也就是将UWB定位坐标数据(世界坐标系下的经纬度B，L)转化为高斯平面直角坐标系下的位置参数(x，y)，其中，高斯投影坐标正算公式(精度为0.1米)，具体如下：
高精度高斯投影坐标正算公式(精度为0.001米)，具体如下：
对上面公式进一步变化，写成：
其中，η＝e′cosBe′为椭圆的第二偏心率，
L0为中央子午线经度；X为自迟到量起的子午线弧长，
M为子午圈曲率半径，
a为椭球长半径；e为椭球第一偏心率；B为大地纬度；
N为卯酉圈曲率半径，
a是椭球长半轴，b是椭球短半轴，
是第一离心率，B是大地纬度；
π＝3.1415926535897932
ρ°＝180÷π＝57.2957795130823210°
ρ′＝180×60÷π＝3437.74677078493917′
ρ″＝180×60×60÷π＝206264.806247096355″
再具体地，获取以VIO初始点为原点的坐标系下的初始旋转矩阵与初始平移向量，将获得的三维向量、初始旋转矩阵及初始平移向量输入预设的残差模型，获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量；在具体的实施例中，还需要判断是计算得到的VIO坐标系与UWB坐标系之间的变换矩阵及外参向量是否有效，也就是是否可以用于计算；若有效则可以存储为预先标定的变换矩阵及外参向量进行计算；若无效，就需要重新计算变换矩阵及外参向量；可以理解的是，设旋转矩阵RV VIO与平移向量PV VIO分别为从视觉惯性里程计VIO获取的在以其初始点为原点的坐标系V下的位姿(旋转+平移)。同时，由于VIO与UWB都是固定安装在车上，两者之间的外参用向量Pex表示，以及根据上述的坐标变换关系以及外参就可以进行转换。
将获得的三维向量、初始旋转矩阵及初始平移向量输入以下公式确定：
其中，
为VIO坐标系与UWB坐标系之间的旋转矩阵；
为VIO坐标系与UWB坐标系之间的平移向量；Pex为VIO坐标系与UWB坐标系之间外参向量；
为UWB定位数据在世界坐标系下的三维向量；
为以VIO运动初始点为原点的坐标系下的旋转矩阵；
为以VIO运动初始点为原点的坐标系下的平移向量。RV W、PV W分别为要求解的坐标变换的旋转矩阵及平移向量，Pex为要求解的外参向量，通过残差方程代入配对好的各组数据进行最小二乘法求解即可得到结果。
具体实施例中，根据筛选后的VIO数据确定以VIO运动初始点为原点的坐标系；根据以VIO运动初始点为原点的坐标系、VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据转化为UWB所在坐标系下的VIO数据。可以理解的是，将VIO数据的平移向量转换为以其运动初始点为原点的坐标xyz，也就是说上述计算得到的是初始点为原点VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，但实际运动中采集的VIO数据并不是以初始点为起点，需要确定以VIO运动初始点为原点的坐标系，然后将VIO数据转化为UWB所在坐标系下的VIO数据。
具体的实施例中，将采集的VIO数据输入以下公式，确定UWB所在坐标系下的VIO数据：
其中，
为VIO坐标系与UWB坐标系之间的旋转矩阵；
为VIO坐标系与UWB坐标系之间的平移向量；Pex为VIO坐标系与UWB坐标系之间外参向量；
为UWB定位数据在世界坐标系下的三维向量；
为以VIO运动初始点为原点的坐标系下的旋转矩阵；
为以VIO运动初始点为原点的坐标系下的平移向量；
为UWB所在坐标系下的VIO数据。
步骤S3，根据融合后的定位数据确定车辆定位信息。可以理解的是，经过对VIO采集到的每帧位姿数据都进行坐标变换和外参变换，则可将VIO数据统一到UWB坐标系下，根据融合后的定位数据对车辆进行定位时，就可以实现信息更全面更可靠的高精度建图定位。
如图3所示，本发明的实施例中，还提供一种车辆定位***，用以实现所述的车辆定位方法，包括：
视觉惯性里程计VIO，用于在车辆行驶过程中采集VIO数据；
超宽带UWB模块，用于在车辆行驶过程中采集UWB定位数据。
变换矩阵及外参向量计算模块，用以根据预设的标定规则获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量；
具体实施例中，还用于将采集的VIO数据和UWB定位数据按照时间戳进行筛选；
判断筛选后的VIO数据量和UWB定位数据量是否满足预设的有效数据量阈值；当满足时，将筛选后的UWB定位数据输入预设的高斯投影模型，获得所述UWB定位数据在世界坐标系下的三维向量；当不满足时，重复执行筛选和判断VIO数据、UWB定位数据过程，直到满足有效数据量阈值；
获取以VIO初始点为原点的坐标系下的初始旋转矩阵与初始平移向量，将获得的三维向量、初始旋转矩阵及初始平移向量输入预设的残差模型，获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量。
VIO数据转化模块，用以根据VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据进行坐标变换和外参变换，转换为UWB坐标系下的VIO数据，并与UWB定位数据进行融合；
具体实施例中，还用于根据筛选后的VIO数据确定以VIO运动初始点为原点的坐标系；根据以VIO运动初始点为原点的坐标系、VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据输入以下公式，转化为UWB所在坐标系下的VIO数据：
其中，
为VIO坐标系与UWB坐标系之间的旋转矩阵；
为VIO坐标系与UWB坐标系之间的平移向量；Pex为VIO坐标系与UWB坐标系之间外参向量；
为UWB定位数据在世界坐标系下的三维向量；
为以VIO运动初始点为原点的坐标系下的旋转矩阵；
为以VIO运动初始点为原点的坐标系下的平移向量；
为UWB所在坐标系下的VIO数据。
更多的细节可参照前述对车辆定位方法的描述，在此不进行赘述。
综上，实施本发明的实施例，具有如下的有益效果：
本发明提供的车辆定位方法及***，利用VIO(visual-inertial odometry，视觉惯性里程计)与UWB进行融合建图定位，不需要引入激光雷达也能获得足够的视觉特征点云信息，成本更低；同时，VIO比CCD有更强的鲁棒性，能更好地避免光照、视野或者环境特征缺失等问题影响；简单易用且有效的实现VIO与UWB外参及坐标变换矩阵标定，进而实现车辆的成本低且精准有效的定位。
以上所揭露的仅为本发明较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。
Claims (10)
1.一种车辆定位方法，其特征在于，包括以下步骤：
步骤S1，在车辆行驶过程中通过视觉惯性里程计VIO采集VIO数据，通过超宽带UWB模块采集UWB定位数据；
步骤S2，根据预先标定的VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据进行坐标变换和外参变换，转换为UWB坐标系下的VIO数据，并与UWB定位数据进行融合；
步骤S3，根据融合后的定位数据确定车辆定位信息。
2.如权利要求1所述的方法，其特征在于，在步骤S2中，所述VIO坐标系与UWB坐标系之间的变换矩阵及外参向量的标定方式，具体包括：
步骤S21，将采集的VIO数据和UWB定位数据按照时间戳进行筛选；
步骤S22，判断筛选后的VIO数据量和UWB定位数据量是否满足预设的有效数据量阈值；当满足预设的有效数据量阈值时，获得所述UWB定位数据在世界坐标系下的三维向量；
步骤S23，获取以VIO初始点为原点的坐标系下的初始旋转矩阵与初始平移向量，将获得的三维向量、初始旋转矩阵及初始平移向量输入预设的残差模型，获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量。
3.如权利要求2所述的方法，其特征在于，在步骤S2中，所述将采集的VIO数据进行坐标变换和外参变换，转换为UWB坐标系下的VIO数据，具体包括：
步骤S24，根据筛选后的VIO数据确定以VIO运动初始点为原点的坐标系；
步骤S25，根据以VIO运动初始点为原点的坐标系、VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据转化为UWB所在坐标系下的VIO数据。
4.如权利要求3所述的方法，其特征在于，在步骤S21中，所述按照时间戳进行筛选，具体包括：
调取预设时间段的VIO数据或UWB定位数据，获得样本数据；
将时间差在预设时间阈值内的样本数据合并为同一时刻的数据，获得筛选后的VIO数据或UWB定位数据。
5.如权利要求4所述的方法，其特征在于，在步骤S22中，所述判断筛选后的VIO数据量和UWB定位数据量是否满足预设的有效数据量阈值，具体包括：
当满足预设的有效数据量阈值时，将筛选后的UWB定位数据输入预设的高斯投影模型，获得所述UWB定位数据在世界坐标系下的三维向量；
当不满足预设的有效数据量阈值时，重复执行步骤S21-步骤S22，直到满足有效数据量阈值。
6.如权利要求5所述的方法，其特征在于，在步骤S23中，所述获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，具体包括：
将获得的三维向量、初始旋转矩阵及初始平移向量输入以下公式确定：
其中，
为VIO坐标系与UWB坐标系之间的旋转矩阵；
为VIO坐标系与UWB坐标系之间的平移向量；Pex为VIO坐标系与UWB坐标系之间外参向量；
为UWB定位数据在世界坐标系下的三维向量；
为以VIO运动初始点为原点的坐标系下的旋转矩阵；
为以VIO运动初始点为原点的坐标系下的平移向量。
7.如权利要求6所述的方法，其特征在于，所述步骤S25中，所述将采集的VIO数据转化为UWB所在坐标系下的VIO数据，具体包括：
将采集的VIO数据输入以下公式，确定UWB所在坐标系下的VIO数据：
其中，
为VIO坐标系与UWB坐标系之间的旋转矩阵；
为VIO坐标系与UWB坐标系之间的平移向量；Pex为VIO坐标系与UWB坐标系之间外参向量；
为UWB定位数据在世界坐标系下的三维向量；
为以VIO运动初始点为原点的坐标系下的旋转矩阵；
为以VIO运动初始点为原点的坐标系下的平移向量；
为UWB所在坐标系下的VIO数据。
8.一种车辆定位***，用以实现如权利要求1-7任一所述的方法，其特征在于，包括：
视觉惯性里程计VIO，用于在车辆行驶过程中采集VIO数据；
超宽带UWB模块，用于在车辆行驶过程中采集UWB定位数据；
变换矩阵及外参向量计算模块，用以根据预设的标定规则获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量；
VIO数据转化模块，用以根据VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据进行坐标变换和外参变换，转换为UWB坐标系下的VIO数据，并与UWB定位数据进行融合。
9.如权利要求8所述的***，其特征在于，所述变换矩阵及外参向量计算模块还用于，
将采集的VIO数据和UWB定位数据按照时间戳进行筛选；
判断筛选后的VIO数据量和UWB定位数据量是否满足预设的有效数据量阈值；当满足时，将筛选后的UWB定位数据输入预设的高斯投影模型，获得所述UWB定位数据在世界坐标系下的三维向量；当不满足时，重复执行筛选和判断VIO数据、UWB定位数据过程，直到满足有效数据量阈值；
获取以VIO初始点为原点的坐标系下的初始旋转矩阵与初始平移向量，将获得的三维向量、初始旋转矩阵及初始平移向量输入预设的残差模型，获得VIO坐标系与UWB坐标系之间的变换矩阵及外参向量。
10.如权利要求9所述的***，其特征在于，所述VIO数据转化模块还用于根据筛选后的VIO数据确定以VIO运动初始点为原点的坐标系；根据以VIO运动初始点为原点的坐标系、VIO坐标系与UWB坐标系之间的变换矩阵及外参向量，将采集的VIO数据输入以下公式，转化为UWB所在坐标系下的VIO数据：
其中，
为VIO坐标系与UWB坐标系之间的旋转矩阵；
为VIO坐标系与UWB坐标系之间的平移向量；Pex为VIO坐标系与UWB坐标系之间外参向量；
为UWB定位数据在世界坐标系下的三维向量；
为以VIO运动初始点为原点的坐标系下的旋转矩阵；
为以VIO运动初始点为原点的坐标系下的平移向量；
为UWB所在坐标系下的VIO数据。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110201534.0A CN114964216A (zh) | 2021-02-23 | 2021-02-23 | 一种车辆定位方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110201534.0A CN114964216A (zh) | 2021-02-23 | 2021-02-23 | 一种车辆定位方法及*** |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114964216A true CN114964216A (zh) | 2022-08-30 |
Family
ID=82970986
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110201534.0A Pending CN114964216A (zh) | 2021-02-23 | 2021-02-23 | 一种车辆定位方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114964216A (zh) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108981692A (zh) * | 2018-06-14 | 2018-12-11 | 兰州晨阳启创信息科技有限公司 | 一种基于惯导/视觉里程计的列车定位方法及*** |
| CN109443348A (zh) * | 2018-09-25 | 2019-03-08 | 同济大学 | 一种基于环视视觉和惯导融合的地下车库库位跟踪方法 |
| CN110542916A (zh) * | 2019-09-18 | 2019-12-06 | 上海交通大学 | 卫星与视觉紧耦合定位方法、***及介质 |
| CN111694010A (zh) * | 2020-05-27 | 2020-09-22 | 东南大学 | 一种基于视觉与激光雷达融合的路侧车辆识别方法 |
| CN111811506A (zh) * | 2020-09-15 | 2020-10-23 | 中国人民解放军国防科技大学 | 视觉/惯性里程计组合导航方法、电子设备及存储介质 |
| CN112256032A (zh) * | 2020-11-02 | 2021-01-22 | 中国计量大学 | Agv定位***、控制方法、设备及存储介质 |
| WO2021017212A1 (zh) * | 2019-07-26 | 2021-02-04 | 魔门塔(苏州)科技有限公司 | 一种多场景高精度车辆定位方法、装置及车载终端 |
| CN112378396A (zh) * | 2020-10-29 | 2021-02-19 | 江苏集萃未来城市应用技术研究所有限公司 | 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法 |
-
2021
- 2021-02-23 CN CN202110201534.0A patent/CN114964216A/zh active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108981692A (zh) * | 2018-06-14 | 2018-12-11 | 兰州晨阳启创信息科技有限公司 | 一种基于惯导/视觉里程计的列车定位方法及*** |
| CN109443348A (zh) * | 2018-09-25 | 2019-03-08 | 同济大学 | 一种基于环视视觉和惯导融合的地下车库库位跟踪方法 |
| WO2021017212A1 (zh) * | 2019-07-26 | 2021-02-04 | 魔门塔(苏州)科技有限公司 | 一种多场景高精度车辆定位方法、装置及车载终端 |
| CN110542916A (zh) * | 2019-09-18 | 2019-12-06 | 上海交通大学 | 卫星与视觉紧耦合定位方法、***及介质 |
| CN111694010A (zh) * | 2020-05-27 | 2020-09-22 | 东南大学 | 一种基于视觉与激光雷达融合的路侧车辆识别方法 |
| CN111811506A (zh) * | 2020-09-15 | 2020-10-23 | 中国人民解放军国防科技大学 | 视觉/惯性里程计组合导航方法、电子设备及存储介质 |
| CN112378396A (zh) * | 2020-10-29 | 2021-02-19 | 江苏集萃未来城市应用技术研究所有限公司 | 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法 |
| CN112256032A (zh) * | 2020-11-02 | 2021-01-22 | 中国计量大学 | Agv定位***、控制方法、设备及存储介质 |
Non-Patent Citations (1)
| Title |
|---|
| 张卡;盛业华;叶春;梁诚;: "车载三维数据采集***的绝对标定及精度分析", 武汉大学学报(信息科学版), no. 01, 5 January 2008 (2008-01-05), pages 55 - 59 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112894832B (zh) | 三维建模方法、装置、电子设备和存储介质 | |
| CN110706279B (zh) | 基于全局地图与多传感器信息融合的全程位姿估计方法 | |
| CN109100730B (zh) | 一种多车协同快速建图方法 | |
| CN104121905B (zh) | 一种基于惯性传感器的航向角获取方法 | |
| CN109710724B (zh) | 一种构建点云地图的方法和设备 | |
| CN111912416B (zh) | 用于设备定位的方法、装置及设备 | |
| CN102353377B (zh) | 一种高空长航时无人机组合导航***及其导航定位方法 | |
| CN111065043B (zh) | 一种基于车路通信的隧道内车辆融合定位***及方法 | |
| CN108225324B (zh) | 一种基于智能终端的地磁匹配与pdr融合的室内定位方法 | |
| CN113358112B (zh) | 一种地图构建方法及一种激光惯性里程计 | |
| CN109446973A (zh) | 一种基于深度神经网络图像识别的车辆定位方法 | |
| CN114111775B (zh) | 一种多传感器融合定位方法、装置、存储介质及电子设备 | |
| CN114485654A (zh) | 一种基于高精地图的多传感器融合定位方法及装置 | |
| CN112762945A (zh) | 高精度地图全要素采集设备的信息同步方法、***及装置 | |
| WO2021254185A1 (zh) | 一种车辆定位方法、装置、设备及存储介质 | |
| CN114964216A (zh) | 一种车辆定位方法及*** | |
| CN108827326A (zh) | 一种基于大数据的导航地图的采集方法及其采集装置 | |
| CN109099923A (zh) | 基于激光、摄像机、gps与惯导融合的道路场景表征***及其方法 | |
| CN117310627A (zh) | 一种应用于车路协同路侧感知***的联合标定方法 | |
| CN113311452A (zh) | 一种基于多传感器的定位方法及其*** | |
| CN110426034B (zh) | 基于地图信息辅助惯导阵列的室内定位方法 | |
| CN111238479A (zh) | 基于地磁指纹匹配的室内定位方法 | |
| CN116698014A (zh) | 一种基于多机器人激光slam和视觉slam地图融合与拼接方法 | |
| CN115900697B (zh) | 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆 | |
| CN115712091A (zh) | 雷达标定及雷达正北标定方法 |
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 |