CN112068174B - 定位方法、定位装置和计算机可读存储介质 - Google Patents
定位方法、定位装置和计算机可读存储介质 Download PDFInfo
- Publication number
- CN112068174B CN112068174B CN202010829797.1A CN202010829797A CN112068174B CN 112068174 B CN112068174 B CN 112068174B CN 202010829797 A CN202010829797 A CN 202010829797A CN 112068174 B CN112068174 B CN 112068174B
- Authority
- CN
- China
- Prior art keywords
- positioning
- moving object
- particle
- navigation system
- particle filter
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 239000002245 particle Substances 0.000 claims abstract description 118
- 238000005259 measurement Methods 0.000 claims abstract description 51
- 230000008859 change Effects 0.000 claims abstract description 42
- 238000013507 mapping Methods 0.000 claims abstract description 14
- 230000001360 synchronised effect Effects 0.000 claims abstract description 11
- 238000004590 computer program Methods 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims description 11
- 230000007613 environmental effect Effects 0.000 abstract description 8
- 230000000694 effects Effects 0.000 abstract description 5
- 238000005516 engineering process Methods 0.000 description 16
- 238000012216 screening Methods 0.000 description 11
- 238000012937 correction Methods 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 230000007774 longterm Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000012163 sequencing technique Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
Images
Classifications
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
本发明提供了一种定位方法、定位装置和计算机可读存储介质，定位方法，包括采用导航系统，获取移动物体的状态信息；初始化粒子滤波器；将状态信息，输入至粒子滤波器的预测方程；采用同步定位与建图，获取移动物体的点云数据，通过相邻的两个点云数据，获取移动物体的环境变化信息；将环境变化信息，输入至粒子滤波器的测量方程；粒子滤波器通过预测方程和测量方程，获取移动物体的定位结果。本发明在导航系统信号丢失的情况下，通过点云匹配能得到很好的定位效果，环境适应性强。同时本发明不需要提前建图或者安装配置基站，操作简单，成本低。
Description
技术领域
本发明涉及定位的技术领域，具体而言，涉及一种定位方法、定位装置和计算机可读存储介质。
背景技术
目前，定位方法主要采用高精度组合导航方案，但其存在对卫星信号依赖高，长期静止不动的情况位置会缓慢漂移的问题。
发明内容
本发明旨在解决上述技术问题的至少之一。
为此，本发明的第一目的在于提供一种定位方法。
本发明的第二目的在于提供一种定位装置。
本发明的第三目的在于提供一种计算机可读存储介质。
为实现本发明的第一目的，本发明的实施例提供了一种定位方法，用于移动物体，包括：采用导航系统，获取移动物体的状态信息；初始化粒子滤波器；将状态信息，输入至粒子滤波器的预测方程；采用同步定位与建图，获取移动物体的点云数据，通过相邻的两个点云数据，获取移动物体的环境变化信息；将环境变化信息，输入至粒子滤波器的测量方程；粒子滤波器通过预测方程和测量方程，获取移动物体的定位结果。
本实施例采用实时同步定位与建图技术进行短期定位，不需要提前建图，操作简单。同步定位与建图可以采用实时slam，本实施例采用导航系统的获取的移动物体状态信息作为预测方程，采用实时slam技术获取的移动物体环境变化信息作为测量方程，结合粒子滤波器融合实时激光slam技术，得到定位结果。本实施例在导航系统信号良好的情况下，可以采用导航系统进行定位，即使在导航系统信号丢失的情况，通过点云匹配也能得到很好的定位效果，所以本实施例的方法环境适应性强，不依赖于RTK。同时因为采用的是实时slam技术，不需要提前建图或者安装配置基站，操作简单，成本低。
另外，本发明上述实施例提供的技术方案还可以具有如下附加技术特征：
上述技术方案中，导航系统包括全球卫星导航系统和惯性测量单元组合导航系统。
采用全球卫星导航系统和惯性测量单元组合导航系统，在卫星信号好的时，通过全球卫星导航系统进行定位，通过惯性测量单元进行顺时定位，提高定位精度高。通过导航系统获取移动物体的状态信息，操作简单，准确率高。
上述任一技术方案中，状态信息包括导航系统获取的移动物体的第一定位、航向、线速度、角速度和线加速度。
通过导航系统，获取移动物体的第一定位、航向、线速度、角速度和线加速度，将其输入至粒子滤波器的预测方程。
上述任一技术方案中，环境变化信息包括相邻两个点云数据的点云间隔时间内，移动物体的位置变化。
通过点云数据获取物体的位置变化，不需要构建提前建图或者安装配置基站，操作简单，成本低。
上述任一技术方案中，粒子滤波器通过预测方程和测量方程，获取移动物体的定位结果，包括：粒子滤波器通过预测方程，获取每个粒子的预测状态；粒子滤波器通过测量方程，获取每个粒子的权重；选取由高至低排列的多个权重中的首个粒子的状态，设置为移动物体的定位结果。
粒子滤波器设有预测方程和测量方程，通过预测方程获取每个粒子的预测状态，在通过测量方程，获取每个粒子的权重，将每个粒子的权重由大到小排列，选取权重最大的粒子对应的状态，设为定位结果，进行定位。通过采用粒子滤波器，对粒子的权重进行筛选，根据权重选取定位结果，提高了定位结果的准确度。
上述任一技术方案中，粒子滤波器通过测量方程，获取每个粒子的权重，包括：将每个粒子的预测状态与移动物体的位置变化进行比较；根据比较结果，设置每个粒子的权重。
采用点云间隔时间内车辆的位置变化，对每个粒子进行评估，将每个粒子的预测状态与移动物体的位置变化进行比较，粒子预测状态与点云的位置变化越接近，则粒子权重越大。通过比较粒子的预测状态与移动物体的位置变化，获取粒子的权重，可行性高，通过权重筛选获取的定位结果，准确率高。
上述任一技术方案中，执行选取由高至低排列的至少两个权重中的首个粒子状态，设置为移动物体的定位结果之前，还包括：更新每个粒子的权重，并且对权重进行归一化。
基于每个粒子获取了新的权重，对每个粒子的权重进行更新，然后对更新后的权重进行归一化，即经过变换，将权重化为无量纲的表达式，成为标量，便于后续对权重进行排序以及筛选。
上述任一技术方案中，定位方法还包括：将定位结果反馈至全球卫星导航系统和惯性测量单元组合导航系统；全球卫星导航系统根据定位结果，设置卫星信号的权重；惯性测量单元根据定位结果，校正累积误差。
本实施例将定位结果反馈至全球卫星导航系统和惯性测量单元组合导航系统，从而对全球卫星导航系统和惯性测量单元组合导航系统的位置进行校准，也可以对全球卫星导航系统进行卫星筛选，去掉误差大的卫星数据，同时可以对惯性测量单元的累计误差进行较正，解决零飘问题，即使长时间无RTK信号，也不会出现位置漂移现象。本实施例通过定位结果，进行自反馈，还可以进一步提高定位精度及稳定性。
为实现本发明的第二目的，本发明的实施例提供了一种定位装置，包括存储器，存储有计算机程序；处理器，执行计算机程序；其中，处理器在执行计算机程序时，实现如本发明任一实施例的定位方法的步骤。
本发明实施例提供的定位装置实现如本发明任一实施例的定位方法的步骤，因而其具有如本发明任一实施例的定位方法的全部有益效果，在此不再赘述。
为实现本发明的第三目的，本发明的实施例提供了一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被执行时，实现上述任一实施例的定位方法的步骤。
本发明实施例提供的计算机可读存储介质实现如本发明任一实施例的定位方法的步骤，因而其具有如本发明任一实施例的方法的全部有益效果，在此不再赘述。
本发明的附加方面和优点将在下面的描述部分中变得明显，或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
图1为本发明一个实施例的定位方法流程示意图一；
图2为本发明一个实施例的定位方法流程示意图二；
图3为本发明一个实施例的定位方法流程示意图三；
图4为本发明一个实施例的定位方法流程示意图四；
图5为本发明一个实施例的定位方法流程示意图五；
图6为本发明一个实施例的定位装置的结构示意图；
图7为本发明一个实施例的定位方法流程示意图六；
图8为本发明一个实施例的定位方法流程示意图七。
其中，图6和图7中附图标记与部件名称之间的对应关系为：
100：组合导航模块，110：粒子滤波器，120：激光点云数据，130：定位结果，200：定位装置，210：存储器，220：处理器。
具体实施方式
为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。
下面参照图1至图8描述本发明一些实施例的定位方法、定位装置200和计算机可读存储介质。
目前，高精度组合导航方案主要包括2种，RTK和IMU组合导航、UWB和北斗和惯导组合导航。其中，RTK表示载波相位差分技术(Real-time kinematic)，IMU表示惯性测量单元，UWB表示超宽带(Ultra Wide Band)。
RTK和IMU组合导航的定位方式原理为：实时接收基站传送的GNSS偏差数据，对比消除自身误差，得出厘米级的定位结果，同时利用IMU做瞬时定位，提高更新频率。优点：成本低，操作简单；缺点：(1)无法消除零偏；(2)对GPS(Global Positioning System，全球定位系统)及RTK信号要求高，信号干扰或遮挡时无法使用；(3)无自反馈。
UWB和北斗和惯导组合导航的定位原理为：GNSS(Global Navigation SatelliteSystem，全球卫星导航系统)信号好时采用RTK+IMU组合导航，信号差时采用UWB+IMU组合导航，优点：可在无GPS条件下使用，环境适应性好；缺点：(1)UWB基站需要提前安装，标定，需要安装大量基站，操作复杂，成本非常高；(2)无自反馈。
现有方案的主要缺点为：
(1)对RTK依赖高，环境适应性差；
(2)有零飘(长期静止不动的情况位置会缓慢漂移)；
(3)无自反馈信息；
(4)需要提前布置基站(融合UWB定位)，操作复杂，成本高。
综上所述，本实施例的目的在于解决以上问题的至少之一。
实施例1：
如图1所示，本实施例提供了一种定位方法，用于移动物体，包括以下步骤：
步骤S102，采用导航系统，获取移动物体的状态信息；
步骤S104，初始化粒子滤波器；
步骤S106，将状态信息，输入至粒子滤波器的预测方程；
步骤S108，采用同步定位与建图，获取移动物体的点云数据，通过相邻的两个点云数据，获取移动物体的环境变化信息；
步骤S110，将环境变化信息，输入至粒子滤波器的测量方程；
步骤S112，粒子滤波器通过预测方程和测量方程，获取移动物体的定位结果。
举例而言，同步定位与建图可以采用实时激光slam或实时视觉slam，或者也可以同时使用实时视觉slam和实时激光slam，slam(simultaneous localization andmapping)表示同步定位与建图。
本实施例采用实时同步定位与建图技术进行短期定位，不需要提前建图，操作简单。
同步定位与建图可以采用实时slam，本实施例采用导航系统的获取的移动物体状态信息作为预测方程，采用实时slam技术获取的移动物体环境变化信息作为测量方程，结合粒子滤波器融合实时激光slam技术，得到定位结果。
本实施例在导航系统信号良好的情况下，可以采用导航系统进行定位，即使在导航系统信号丢失的情况，通过点云匹配也能得到很好的定位效果，所以本实施例的方法环境适应性强，不依赖于RTK。同时因为采用的是实时slam技术，不需要提前建图或者安装配置基站，操作简单，成本低。
实施例2：
除上述实施例的技术特征以外，本实施例进一步地包括了以下技术特征：
导航系统包括全球卫星导航系统和惯性测量单元组合导航系统。
采用全球卫星导航系统和惯性测量单元组合导航系统，在卫星信号好的时，通过全球卫星导航系统进行定位，通过惯性测量单元进行顺时定位，提高定位精度高。
实施例3：
除上述实施例的技术特征以外，本实施例进一步地包括了以下技术特征：
状态信息包括导航系统获取的移动物体的第一定位、航向、线速度、角速度和线加速度。
通过导航系统，获取移动物体的第一定位、航向、线速度、角速度和线加速度，将其输入至粒子滤波器的预测方程。
第一定位为移动物体在大地坐标系下的精确坐标(x：经度，y：纬度，z：高度)。
通过导航系统获取移动物体的状态信息，操作简单，准确率高。
实施例4：
除上述实施例的技术特征以外，本实施例进一步地包括了以下技术特征：
环境变化信息包括相邻两个点云数据的点云间隔时间内，移动物体的位置变化。
举例而言，采用视觉slam或激光slam，可以获取移动物体的点云数据，通过两个相邻的点云数据，可以得到移动物体的环境变化信息，进而得到移动物体在点云间隔时间的位置变化，位置变化包括移动物体的坐标(x，y，z)和方向信息，还可以获取移动物体的航向、线速度、线加速度等信息。
通过实时slam技术的点云数据获取物体的位置变化，不需要构建提前建图或者安装配置基站，操作简单，成本低。
实施例5：
如图2所示，除上述实施例的技术特征以外，本实施例进一步地包括了以下技术特征：
粒子滤波器通过预测方程和测量方程，获取移动物体的定位结果，包括以下步骤：
步骤S202，粒子滤波器通过预测方程，获取每个粒子的预测状态；
步骤S204，粒子滤波器通过测量方程，获取每个粒子的权重；
步骤S206，选取由高至低排列的多个权重中的首个粒子的状态，设置为移动物体的定位结果。
粒子滤波器设有预测方程和测量方程，通过预测方程获取每个粒子的预测状态，在通过测量方程，获取每个粒子的权重，将每个粒子的权重由大到小排列，选取权重最大的粒子对应的状态，设为定位结果，进行定位。
通过采用粒子滤波器，对粒子的权重进行筛选，根据权重选取定位结果，提高了定位结果的准确度。
实施例6：
如图3所示，除上述实施例的技术特征以外，本实施例进一步地包括了以下技术特征：
粒子滤波器通过测量方程，获取每个粒子的权重，包括以下步骤：
步骤S302，将每个粒子的预测状态与移动物体的位置变化进行比较；
步骤S304，根据比较结果，设置每个粒子的权重。
采用点云间隔时间内车辆的位置变化，对每个粒子进行评估，将每个粒子的预测状态与移动物体的位置变化进行比较，粒子预测状态与点云的位置变化越接近，则粒子权重越大。
通过比较粒子的预测状态与移动物体的位置变化，获取粒子的权重，可行性高，通过权重筛选获取的定位结果，准确率高。
实施例7：
如图4所示，除上述实施例的技术特征以外，本实施例进一步地包括了以下技术特征：
执行选取由高至低排列的至少两个权重中的首个粒子状态，设置为移动物体的定位结果之前，还包括以下步骤：
步骤S402，更新每个粒子的权重，并且对权重进行归一化。
基于每个粒子获取了新的权重，对每个粒子的权重进行更新，然后对更新后的权重进行归一化，即经过变换，将权重化为无量纲的表达式，成为标量，便于后续对权重进行排序以及筛选。
实施例8：
如图5所示，除上述实施例的技术特征以外，本实施例进一步地包括了以下技术特征：
定位方法还包括以下步骤：
步骤S502，将定位结果反馈至全球卫星导航系统和惯性测量单元组合导航系统；
步骤S504，全球卫星导航系统根据定位结果，设置卫星信号的权重；
步骤S506，惯性测量单元根据定位结果，校正累积误差。
本实施例将定位结果反馈至全球卫星导航系统和惯性测量单元组合导航系统，从而对全球卫星导航系统和惯性测量单元组合导航系统的位置进行校准。
本实施例中，根据定位结果，也可以对全球卫星导航系统进行卫星筛选，去掉误差大的卫星数据，保留误差小的卫星数据，进而提高定位的准确性。
同时，本实施例根据定位结果，还可以对惯性测量单元的累计误差进行较正，解决零飘问题，即使长时间无RTK信号，也不会出现位置漂移现象。
本实施例通过定位结果，进行自反馈，还可以进一步提高定位精度及稳定性。
举例而言，定位结果包括位置、航向、线速度、角速度和线加速度。
通过自反馈，全球卫星导航系统和惯性测量单元组合导航系统的精度提高，粒子滤波器的预测方程进行更新，精度也随之提高，获取更佳精确的预测方程。
实施例9：
如图6所示，本实施例提供了一种定位装置200，包括存储器210，存储有计算机程序；处理器220，执行计算机程序；其中，处理器220在执行计算机程序时，实现如本发明任一实施例的定位方法的步骤。
实施例10：
本实施例提供了一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被执行时，实现上述任一实施例的定位方法的步骤。
具体实施例：
如图7和图8所示，本实施例提供了一种定位方法，设有组合导航模块100，组合导航模块100采用GPS和IMU组合导航，本实施例采用组合导航作为预测方程，结合粒子滤波算法融合实时激光slam技术得到定位结果130，并将该定位结果130反馈给组合导航模块100，从而对组合导航模块100位置进行校准，也可以进行卫星筛选，去掉误差大的卫星数据，同时可以对imu累计误差进行较正，解决零飘问题。这样即使在gnss信号丢失的情况，通过点云匹配也能得到很好的定位效果，环境适应性强。同时因为采用的是实时slam技术，不需要提前建图或者安装配置基站，操作简单，成本低。
总体方案如图7所示，采用组合导航模块100，获取移动物体的状态信息，将状态信息，输入至粒子滤波器110的预测方程；采用激光同步定位与建图，获取移动物体的激光点云数据120，通过相邻的两个点云数据，获取移动物体的环境变化信息，将环境变化信息，输入至粒子滤波器110的测量方程；粒子滤波器110通过预测方程和测量方程，获取移动物体的定位结果130；将自反馈数据反馈至组合导航模块100，进行位置校准，卫星筛选，IMU误差校正。其中，自反馈数据包括位置、航向、线速度、角速度和线加速度。
定位方法具体步骤如图8所示，包括：
步骤S602，采用组合导航模块定位；
步骤S604，判定粒子滤波器是否初始化；
如果否，进入步骤S606，如果是，进入步骤S608；
步骤S606，对粒子滤波器进行初始化；
步骤S602至步骤S606为初始状态：开始时，使用GPS+IMU组合导航模块100得到初始位置并完成粒子滤波器110的初始化；
步骤S608，预测状态；
利用组合导航模块100的初始位置，航向及运动状态信息，作为粒子滤波器110的预测方程，得到每个粒子的预测状态；
步骤S608为预测阶段：初始化完成后，利用组合导航模块100的初始位置，航向及运动状态信息，作为粒子滤波器110的预测方程，得到每个粒子的预测状态；
步骤S610，输入激光点云数据；
步骤S612，测量状态；
通过输入的激光点云数据120，对前后收到的点云进行帧匹配，得到点云间隔时间内车辆的位置变化，用得到的位置变化对每个粒子进行评估，计算权重，粒子预测状态与点云的位置变化越接近，权重越大。
步骤S610至步骤S612为校正阶段；
步骤S614，更新每个粒子权重并归一化；
步骤S614为更新阶段；
步骤S616，获取定位结果；
步骤S616为输出，输出权重最大的粒子状态，即为定位结果；
还包括自反馈和校准：
自反馈：将得到的定位结果(包括位置、航向及线速度，角速度，线加速度)反馈给组合导航模块100；
校准：组合导航模块100利用反馈的数据(定位结果)，对接收的卫星信号进行筛选，误差小的卫星数据的权重加大，反之则权重减小，同时矫正imu累积误差，完成位置校准，卫星筛选，IMU误差校正；
通过校准后得到更为精确的预测方程，输入到粒子滤波器110；
不断重复步骤步骤S602至步骤步骤S616，并输出定位结果。
本实施例完成初始化后，导航过程中对RTK依赖度低，通过点云匹配数据得到定位结果的自反馈，即使在没有RTK信号的地下车库或信号短期遮挡的市区等环境也能实现高精度定位，环境适应性更强。
本实施例因为增加了定位结果的自反馈后，可有效矫正组合导航中的IMU误差，解决零飘问题，禁止时，即使长时间无RTK信号，也不会出现位置漂移现象。
本实施例使用实时激光slam技术进行短期定位，不需要提前建图，减少了很多复杂操作。
本实施例通过定位结果的自反馈可以进一步提高定位精度及稳定性。
本实施例可在室内，室外多环境下联合使用。
综上，本发明实施例的有益效果为：
1.本实施例采用导航系统的获取的移动物体状态信息作为预测方程，采用实时slam技术获取的移动物体环境变化信息作为测量方程，结合粒子滤波器融合实时slam技术，得到定位结果。本实施例即使在导航系统信号丢失的情况，通过点云匹配也能得到很好的定位效果，环境适应性强。同时因为采用的是实时slam技术，不需要提前建图或者安装配置基站，操作简单，成本低。
2.通过预测方程获取每个粒子的预测状态，在通过测量方程，获取每个粒子的权重，将每个粒子的权重由大到小排列，选取权重最大的粒子对应的状态，设为定位结果，进行定位。通过采用粒子滤波器，对粒子的权重进行筛选，根据权重选取定位结果，提高了定位结果的准确度。
3.采用点云间隔时间内车辆的位置变化，对每个粒子进行评估，将每个粒子的预测状态与移动物体的位置变化进行比较，粒子预测状态与点云的位置变化越接近，则粒子权重越大。通过比较粒子的预测状态与移动物体的位置变化，获取粒子的权重，可行性高，通过权重筛选获取的定位结果，准确率高。
4.本实施例将定位结果反馈至全球卫星导航系统和惯性测量单元组合导航系统，从而对全球卫星导航系统和惯性测量单元组合导航系统的位置进行校准，也可以对全球卫星导航系统进行卫星筛选，去掉误差大的卫星数据，同时可以对惯性测量单元的累计误差进行较正，解决零飘问题，即使长时间无RTK信号，也不会出现位置漂移现象。本实施例通过定位结果，进行自反馈，还可以进一步提高定位精度及稳定性。
在本发明中，术语“第一”、“第二”、“第三”仅用于描述的目的，而不能理解为指示或暗示相对重要性；术语“多个”则指两个或两个以上，除非另有明确的限定。术语“安装”、“相连”、“连接”、“固定”等术语均应做广义理解，例如，“连接”可以是固定连接，也可以是可拆卸连接，或一体地连接；“相连”可以是直接相连，也可以通过中间媒介间接相连。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
本发明的描述中，需要理解的是，术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或单元必须具有特定的方向、以特定的方位构造和操作，因此，不能理解为对本发明的限制。
在本说明书的描述中，术语“一个实施例”、“一些实施例”、“具体实施例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或实例。而且，描述的具体特征、结构、材料或特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (8)
1.一种定位方法，用于移动物体，其特征在于，包括：
采用导航系统，获取所述移动物体的状态信息；
初始化粒子滤波器；
将所述状态信息，输入至所述粒子滤波器的预测方程；
采用同步定位与建图，获取所述移动物体的点云数据，通过相邻的两个所述点云数据，获取所述移动物体的环境变化信息；
将所述环境变化信息，输入至所述粒子滤波器的测量方程；
所述粒子滤波器通过所述预测方程和所述测量方程，获取所述移动物体的定位结果；
所述导航系统包括全球卫星导航系统和惯性测量单元组合导航系统；
所述定位方法，还包括：
将所述定位结果反馈至所述全球卫星导航系统和惯性测量单元组合导航系统；
所述全球卫星导航系统根据所述定位结果，设置卫星信号的权重；
惯性测量单元根据所述定位结果，校正累积误差。
2.根据权利要求1所述的定位方法，其特征在于，所述状态信息包括所述导航系统获取的所述移动物体的第一定位、航向、线速度、角速度和线加速度。
3.根据权利要求2所述的定位方法，其特征在于，所述环境变化信息包括相邻两个所述点云数据的点云间隔时间内，所述移动物体的位置变化。
4.根据权利要求3所述的定位方法，其特征在于，所述粒子滤波器通过所述预测方程和所述测量方程，获取所述移动物体的定位结果，包括：
所述粒子滤波器通过所述预测方程，获取每个粒子的预测状态；
所述粒子滤波器通过所述测量方程，获取每个所述粒子的权重；
选取由高至低排列的多个所述权重中的首个所述粒子的状态，设置为所述移动物体的定位结果。
5.根据权利要求4所述的定位方法，其特征在于，所述粒子滤波器通过测量方程，获取每个所述粒子的权重，包括：
将每个所述粒子的预测状态与所述移动物体的位置变化进行比较；
根据比较结果，设置每个所述粒子的权重。
6.根据权利要求5所述的定位方法，其特征在于，执行所述选取由高至低排列的至少两个所述权重中的首个所述粒子状态，设置为所述移动物体的定位结果之前，还包括：
更新每个所述粒子的权重，并且对所述权重进行归一化。
7.一种定位装置（200），其特征在于，包括：
存储器（210），存储有计算机程序；
处理器（220），执行所述计算机程序；
其中，所述处理器（220）在执行所述计算机程序时，实现如权利要求1至6中任一项所述的定位方法的步骤。
8.一种计算机可读存储介质，其特征在于，包括：
所述计算机可读存储介质存储有计算机程序，所述计算机程序被执行时，实现如权利要求1至6中任一项所述的定位方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010829797.1A CN112068174B (zh) | 2020-08-18 | 2020-08-18 | 定位方法、定位装置和计算机可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010829797.1A CN112068174B (zh) | 2020-08-18 | 2020-08-18 | 定位方法、定位装置和计算机可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112068174A CN112068174A (zh) | 2020-12-11 |
| CN112068174B true CN112068174B (zh) | 2021-11-23 |
Family
ID=73661960
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010829797.1A Active CN112068174B (zh) | 2020-08-18 | 2020-08-18 | 定位方法、定位装置和计算机可读存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112068174B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101625572A (zh) * | 2009-08-10 | 2010-01-13 | 浙江大学 | 基于改进重采样方法和粒子选取的FastSLAM算法 |
| CN106840179A (zh) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | 一种基于多传感器信息融合的智能车定位方法 |
| CN108594282A (zh) * | 2018-05-24 | 2018-09-28 | 武汉大学 | 一种基于高精度gnss实时协同定位的ros机器人导航方法 |
| CN109633666A (zh) * | 2019-01-18 | 2019-04-16 | 广州高新兴机器人有限公司 | 室内动态环境下基于激光雷达的定位方法及计算机存储介质 |
| CN109934868A (zh) * | 2019-03-18 | 2019-06-25 | 北京理工大学 | 一种基于三维点云与卫星图匹配的车辆定位方法 |
| CN111352066A (zh) * | 2020-03-27 | 2020-06-30 | 西安震有信通科技有限公司 | 基于粒子滤波的定位方法、装置、计算机设备和存储介质 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR100809352B1 (ko) * | 2006-11-16 | 2008-03-05 | 삼성전자주식회사 | 파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치 |
| KR101796322B1 (ko) * | 2011-01-07 | 2017-12-01 | 삼성전자주식회사 | 항법 알고리즘을 이용한 위치 정보 검출 장치 및 방법 |
| CN109991984B (zh) * | 2019-04-22 | 2024-04-30 | 上海蔚来汽车有限公司 | 用于生成高精细地图的方法、装置和计算机存储介质 |
-
2020
- 2020-08-18 CN CN202010829797.1A patent/CN112068174B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101625572A (zh) * | 2009-08-10 | 2010-01-13 | 浙江大学 | 基于改进重采样方法和粒子选取的FastSLAM算法 |
| CN106840179A (zh) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | 一种基于多传感器信息融合的智能车定位方法 |
| CN108594282A (zh) * | 2018-05-24 | 2018-09-28 | 武汉大学 | 一种基于高精度gnss实时协同定位的ros机器人导航方法 |
| CN109633666A (zh) * | 2019-01-18 | 2019-04-16 | 广州高新兴机器人有限公司 | 室内动态环境下基于激光雷达的定位方法及计算机存储介质 |
| CN109934868A (zh) * | 2019-03-18 | 2019-06-25 | 北京理工大学 | 一种基于三维点云与卫星图匹配的车辆定位方法 |
| CN111352066A (zh) * | 2020-03-27 | 2020-06-30 | 西安震有信通科技有限公司 | 基于粒子滤波的定位方法、装置、计算机设备和存储介质 |
Non-Patent Citations (3)
| Title |
|---|
| A Two-Step Particle Filter for SLAM of Corridor Environment;Yutong Zang et al.;《Proceedings of the 2006 IEEE International Conference on Information Acquisition》;20060823;第370-375页 * |
| 一种新的粒子滤波SLAM算法;郭剑辉 赵春霞;《计算机研究与发展》;20081231;第853-860页 * |
| 室内UWB/LiDAR组合定位算法;陈志键 等;《导航定位学报》;20190331;第38-42页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112068174A (zh) | 2020-12-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| He et al. | An integrated GNSS/LiDAR-SLAM pose estimation framework for large-scale map building in partially GNSS-denied environments | |
| CN109556569B (zh) | 地形图测绘方法及装置 | |
| US11428532B2 (en) | Generating a geomagnetic map | |
| CN110160545B (zh) | 一种激光雷达与gps的增强定位系统及方法 | |
| CN113295174B (zh) | 一种车道级定位的方法、相关装置、设备以及存储介质 | |
| US20230349698A1 (en) | Correlating Overlapping Magnetic Measurement Data from Multiple Magnetic Navigation Devices and Updating a Geomagnetic Map with that Data | |
| US20020188386A1 (en) | GPS based terrain referenced navigation system | |
| Yu et al. | Automatic extrinsic self-calibration of mobile LiDAR systems based on planar and spherical features | |
| CN112068174B (zh) | 定位方法、定位装置和计算机可读存储介质 | |
| CN105466423A (zh) | 一种无人机导航系统及其运行方法 | |
| Ellum et al. | A mobile mapping system for the survey community | |
| WO2007042843A1 (en) | Terrain mapping | |
| JP4884109B2 (ja) | 移動軌跡算出方法、移動軌跡算出装置及び地図データ生成方法 | |
| CN116380119A (zh) | 组合导航的校准方法、装置和系统 | |
| US7107146B2 (en) | Methods and systems for generating a terrain elevation map in a cartesian format | |
| CN112824936B (zh) | 一种地物高度确定方法、装置、电子设备和介质 | |
| CN114199220A (zh) | 一种无人机空中在线磁罗盘校准方法和装置 | |
| KR101565259B1 (ko) | 비정지 위성용 안테나의 구동 제어 방법 및 이를 위한 프로그램 기록매체 | |
| CN111340952A (zh) | 一种移动测量失锁区域制图方法和装置 | |
| EP3255465B1 (en) | Buried asset locate device motion sensing for quality control | |
| CN118244785B (zh) | 空地双模的两栖无人机及其定位方法、装置、存储介质 | |
| Lu et al. | Research on UAV Large Scale Mapping and its elevation fitting | |
| CN113985458A (zh) | 一种用于室外高精度定位与显示的系统和方法 | |
| CN118573815A (zh) | 基于位姿信息的田间植物动态表型采集方法及系统 | |
| Ellum et al. | Portable mobile mapping |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20201211 Assignee: Sany Automobile Manufacturing Co.,Ltd. Assignor: SANY SPECIAL PURPOSE VEHICLE Co.,Ltd. Contract record no.: X2024980010904 Denomination of invention: Positioning method, positioning device, and computer-readable storage medium Granted publication date: 20211123 License type: Common License Record date: 20240801 |