CN117346782A - 定位优化方法、装置、电子设备和存储介质 - Google Patents

定位优化方法、装置、电子设备和存储介质 Download PDF

Info

Publication number
CN117346782A
CN117346782A CN202311288615.4A CN202311288615A CN117346782A CN 117346782 A CN117346782 A CN 117346782A CN 202311288615 A CN202311288615 A CN 202311288615A CN 117346782 A CN117346782 A CN 117346782A
Authority
CN
China
Prior art keywords
value
positioning data
current moment
estimated value
positioning
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
Application number
CN202311288615.4A
Other languages
English (en)
Inventor
李聪平
李振
赖志林
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Saite Intelligent Technology Co Ltd
Original Assignee
Guangzhou Saite Intelligent Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guangzhou Saite Intelligent Technology Co Ltd filed Critical Guangzhou Saite Intelligent Technology Co Ltd
Priority to CN202311288615.4A priority Critical patent/CN117346782A/zh
Publication of CN117346782A publication Critical patent/CN117346782A/zh
Pending legal-status Critical Current

Links

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/20—Instruments for performing navigational calculations
    • 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
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G01C21/3811—Point data, e.g. Point of Interest [POI]
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种定位优化方法、装置、电子设备和存储介质，用于对目标对象进行跟踪，首先以预设频率获取当前时刻的定位数据的观测值，预设频率大于定位数据生成的实际频率，则观测值可能为空值，获取上一时刻的定位数据的后验估计值；基于后验估计值计算当前时刻的定位数据的先验估计值；判断观测值是否为空值；若是，将先验估计值作为当前时刻的定位数据的输出值；若否，基于先验估计值、观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值。可以对定位数据进行平滑处理，提高定位的稳定性，同时可以结合卡尔曼滤波算法对定位数据中的异常点进行剔除，增强了滤波系统的稳定性，在不加入其他传感器的情况下，实现了定位数据的高频输出。

Description

定位优化方法、装置、电子设备和存储介质
技术领域
本发明涉及定位导航技术领域，尤其涉及一种定位优化方法、装置、电子设备和存储介质。
背景技术
在移动机器人领域，作为现有技术，多传感器融合自主导航机器人，自主导航离不开高精度定位功能。定位功能即机器人在环境中移动，通过自身携带的传感器对环境信息进行感知，记录自己在地图中的位置。移动机器人在自主导航过程中通常使用卫星组合惯导进行定位，但容易出现以下情况。
第一种情况，尽管卫星组合惯导传感器加入了差分优化，但不能输出高频率的定位信息，无法实时快速输出定位信息。
第二种情况，前后定位信息平滑性差，呈现齿轮状，甚至出现异常跳动点，导致机器人定位跳动。
发明内容
本发明提供了一种定位优化方法，以解决不能输出高频率的定位信息以及定位信息平滑性差的问题。
第一方面，本发明提供了一种定位优化方法，用于对目标对象进行跟踪，包括：
以预设频率获取当前时刻的定位数据的观测值，所述预设频率大于所述定位数据生成的实际频率；
获取上一时刻的定位数据的后验估计值；
基于所述后验估计值计算当前时刻的定位数据的先验估计值；
判断所述观测值是否为空值；
若是，将所述先验估计值作为当前时刻的定位数据的输出值；
若否，基于所述先验估计值、所述观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值。
第二方面，本发明提供了一种定位优化装置，包括：
观测值获取模块，用于以预设频率获取当前时刻的定位数据的观测值，所述预设频率大于所述定位数据生成的实际频率；
后验估计值计算模块，用于获取上一时刻的定位数据的后验估计值；
先验估计值计算模块，用于基于所述后验估计值计算当前时刻的定位数据的先验估计值；
观测值判断模块，用于判断所述观测值是否为空值；若是执行第一输出模块的内容，若否执行第二输出模块的内容。
第一输出模块，用于将所述先验估计值作为当前时刻的定位数据的输出值；
第二输出模块，用于基于所述先验估计值、所述观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值。
第三方面，本发明提供了一种电子设备，所述电子设备包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的计算机程序，所述计算机程序被所述至少一个处理器执行，以使所述至少一个处理器能够执行本发明第一方面所述的定位优化方法。
第四方面，本发明提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使处理器执行时实现本发明第一方面所述的定位优化方法。
本发明实施例提供的一种定位优化方法，用于对目标对象进行跟踪，首先以预设频率获取当前时刻的定位数据的观测值，预设频率大于定位数据生成的实际频率，获取上一时刻的定位数据的后验估计值；基于后验估计值计算当前时刻的定位数据的先验估计值；判断观测值是否为空值；若是，将先验估计值作为当前时刻的定位数据的输出值；若否，基于先验估计值、观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值。
一方面，获取观测值的预设频率大于定位数据生成的实际频率，则观测值可能为空值，那么在观测值为空值时，将当前时刻的定位数据的先验估计值作为当前时刻的定位数据的输出值，增加了数据的输出密度，可以对定位数据进行平滑处理，提高定位的稳定性，同时可以结合卡尔曼滤波算法对定位数据中的异常点进行剔除，避免异常点对滤波系统造成干扰，增强了滤波系统的稳定性。
另一方面，获取观测值的预设频率大于定位数据生成的实际频率，使得输出的定位数据的频率大于定位数据生成的实际频率，在不加入其他传感器的情况下，实现了定位数据的高频输出，实时快速输出定位信息，还降低了优化成本。
应当理解，本部分所描述的内容并非旨在标识本发明的实施例的关键或重要特征，也不用于限制本发明的范围。本发明的其它特征将通过以下的说明书而变得容易理解。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例一提供的一种定位优化方法的流程图；
图2是本发明实施例二提供的一种定位优化方法的流程图；
图3是本发明实施例三提供的一种定位优化装置的结构示意图；
图4是本发明实施例四提供的电子设备的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
实施例一
图1为本发明实施例一提供的一种定位优化方法的流程图，本实施例可适用于对目标对象进行跟踪，以及对目标对象的定位进行优化的情况，目标对象可以是机器人、自动驾驶车辆等，通过获取目标对象在环境中的定位信息，从而定位导航系统可以规划移动路线并控制目标对象移动。
本发明的定位优化方法应用到卡尔曼滤波(Kalman filtering)算法，卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。其中涉及几个通用公式，具体如下：
1、状态方程
状态估计是卡尔曼滤波的重要组成部分，一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。状态方程用于描述系统状态随时间的变化，通常为一个一阶差分方程，常见形式为：
其中，x(·)为状态向量，为k时刻定位数据的先验估计值，/>为k-1时刻定位数据的后验估计值，A为状态转移矩阵，B为控制矩阵，uk为k时刻的模型修正向量。
状态方程描述了状态的预测过程，它基于上一个时刻的状态和控制量的输入，来计算当前时刻的状态的预测值，状态预测公式是卡尔曼滤波的基础，通过不断迭代，可以实现对未知量状态的准确估计。
2、观测方程
观测方程用于描述系统状态的观测值随时间的变化，通常为一个线性方程，常见形式为：
zk＝Hxk+vk
其中zk为观测值(观测向量)，观测值为卫星组合惯性导航系统(以下简称惯导)输出的原始定位，H为观测矩阵，v为观测噪声。
观测方程描述了通过前一时刻的状态预测值，来计算下一时刻预测的观测值，用于判断当前的状态预测值与真观测值之间的误差，即用于对观测误差进行调整。
该定位优化方法可以由定位优化装置来执行，该定位优化装置可以采用硬件和/或软件的形式实现，该定位优化装置可配置于电子设备中。如图1所示，该定位优化方法包括：
S101、以预设频率获取当前时刻的定位数据的观测值。
其中，预设频率大于定位数据生成的实际频率。
以预设频率获取当前时刻的定位数据的观测值，具体是指，以预设频率获取目标对象在当前时刻的定位数据的观测值，下文中的各种定位数据均为针对目标对象的数据，不再分别指出。
定位数据可以通过定位设备来获取，例如，激光雷达、GPS模块和惯导，定位数据生成的实际频率与定位设备的性能有关，性能较佳则获取数据的频率较高，性能较差则获取数据的频率较低。同时定位设备的性能也与成本成正比，一般的定位设备的性能并非较优的，因而获取定位数据的频率较低。
本实施例可以采用RTK(Real-time kinematic，实时动态)载波相位差分技术来输出定位数据，RTK载波相位差分技术是实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标。目前RTK(设备)输出的频率取决于所使用的设备和系统，市场设备普遍输出频率10-20Hz，最高能达到100Hz，但持续高频率输出需要更加昂贵的硬件设备。
定位数据是定位设备的输出数据，也是作为定位导航系统的输入数据，定位导航系统通过设置程序的执行频率来控制获取输入的定位数据的频率，亦即预设频率，同时相当于控制输出的定位数据的频率。
在一个可选的示例中，预设频率是实际频率的10倍。例如，实际频率为10Hz，预设频率为100Hz。当然，预设频率与实际频率的比值也可以是其他数值，例如，2、5、20等，只要能满足定位导航系统处理定位数据的效率要求，则不加以限制。
预设频率大于定位数据生成的实际频率，那么获取的定位数据的输入值可能是空值，即观测值可能是空值，具体地，以预设频率获取当前时刻的定位数据的观测值，包括：以预设频率获取当前时刻的定位数据的输入值；判断输入值是否为空；若是，将预设空值作为当前时刻的定位数据的观测值；若否，将输入值作为当前时刻的定位数据的观测值。
示例性的，定位数据的实际频率为1Hz时，每秒获取1个定位数据作为观测值，而预设频率为10Hz时，每秒获取10个定位数据获取观测值，而1秒内实际能获得的定位数据仅有1个，那么剩下的9个定位数据均为空值。
S102、获取上一时刻的定位数据的后验估计值。
根据上面提及的状态方程，可以获取上一时刻的定位数据的后验估计值。
S103、基于后验估计值计算当前时刻的定位数据的先验估计值。
同样地，根据上面提及的状态方程，可以计算当前时刻的定位数据的先验估计值，具体地，
其中，为k时刻定位数据的先验估计值，/>为k-1时刻定位数据的后验估计值，A为状态转移矩阵，B为控制矩阵，uk为k时刻的模型修正向量。在本发明中，不增加输入控制，因此B为单位矩阵。当前时刻的先验估计值是根据上一时刻的后验估计值计算得到的，即采用递推形式，来估计当前时刻的定位数据，即先验估计值。x(·)为状态向量，具体可以包括目标对象在X、Y、Z方向的坐标、速度和加速度的信息。
S104、判断观测值是否为空值。
若是，执行S105，若否执行S106。
S105、将先验估计值作为当前时刻的定位数据的输出值。
当观测值为空值时，则说明当前无定位数据，则可以将先验估计值作为当前时刻的定位数据的输出值，先验估计值是一个虚拟的值，但同样也是根据在先的定位数据递推得到的。示例性地，假设当前定位数据(序号100)为空值，而当前定位数据的前一个定位数据(序号99)为非空值，当前定位数据之后的3个定位数据(序号101-103)均为空值，那么，对于100号定位数据来说，根据99号的定位数据的后验估计值来计算得到先验估计值，将先验估计值作为其定位数据，对于101号定位数据来说，根据100号的定位数据的后验估计值来计算得到先验估计值，将先验估计值作为其定位数据，以此类推。
考虑到RTK不稳定、中途输出定位数据超时的情况，则定位数据的密度可能较为稀疏，那么定位导航系统获得的输入数据较少，定位导航的稳定性较差。此时通过在输入端增加高频率的空值，并通过卡尔曼算法预测后续时刻的定位数据，可以增加输出的定位数据的密度，提高定位导航的稳定性。
获取观测值的预设频率大于定位数据生成的实际频率，使得输出的定位数据的频率大于定位数据生成的实际频率，在不加入其他传感器的情况下，实现了定位数据的高频输出，增加了定位数据的密度，实时快速输出定位信息，提高定位导航的稳定性。
S106、基于先验估计值、观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值。
在本实施例中，当观测值不为空值时，可以基于先验估计值、观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值，即以观测值作为参考、采用卡尔曼滤波器算法对先验估计值进行滤波更新，可以剔除异常点，在此基础上进行滤波平滑，可以对定位数据进行平滑处理，提高定位的稳定性。
需要说明的是，在确定当前时刻的定位数据的输出值后，即可返回S101，获取下一个观测值，循环执行S101-S106，直到滤波系统关闭为止。
本发明实施例提供的一种定位优化方法，首先以预设频率获取当前时刻的定位数据的观测值，预设频率大于定位数据生成的实际频率，获取上一时刻的定位数据的后验估计值；基于后验估计值计算当前时刻的定位数据的先验估计值；判断观测值是否为空值；若是，将先验估计值作为当前时刻的定位数据的输出值；若否，基于先验估计值、观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值。一方面，获取观测值的预设频率大于定位数据生成的实际频率，则观测值可能为空值，那么在观测值为空值时，将当前时刻的定位数据的先验估计值作为当前时刻的定位数据的输出值，增加了数据的输出密度，可以对定位数据进行平滑处理，提高定位的稳定性，同时可以结合卡尔曼滤波算法对定位数据中的异常点进行剔除，避免异常点对滤波系统造成干扰，增强了滤波系统的稳定性。另一方面，获取观测值的预设频率大于定位数据生成的实际频率，使得输出的定位数据的频率大于定位数据生成的实际频率，在不加入其他传感器的情况下，实现了定位数据的高频输出，实时快速输出定位信息，还降低了优化成本。
实施例二
图2为本发明实施例二提供的一种定位优化方法的流程图，本发明实施例在上述实施例一的基础上进行优化，如图2所示，该定位优化方法包括：
S201、以预设频率获取当前时刻的定位数据的观测值，预设频率大于定位数据生成的实际频率。
S202、判断滤波初始化是否完成。
若否执行S203，若是执行S204。
S203、将当前时刻的定位数据作为初始时刻的定位数据，以完成滤波初始化。
这一步骤即获取滤波过程的初始输入值，第一个定位数据不需要进行滤波。
S204、获取上一时刻的定位数据的后验估计值。
S205、基于后验估计值计算当前时刻的定位数据的先验估计值。
S206、判断观测值是否为空值。
若是执行S207，若否执行S208。
S207、将先验估计值作为当前时刻的定位数据的输出值。
步骤S201、S204-S207与实施例一的S101-S105相似，具体可以参考S101-S105，在此不加以描述。
S208、计算先验估计值与观测值的误差。
在一个可选实施例中，定位数据包括坐标，计算先验估计值与观测值的误差，包括：在三维空间中，确定先验估计值对应的第一坐标以及观测值对应的第二坐标；将第一坐标与第二坐标之间的空间距离作为误差。
ek为误差，x1、y1、z1分别为观测值的坐标中X、Y、Z方向的坐标值，x2、y2、z2分别为先验估计值的坐标中X、Y、Z方向的坐标值。
S209、判断误差是否超过预设误差阈值。
若否执行S210，若是执行S211。
S210、采用卡尔曼滤波算法对先验估计值进行滤波更新，将滤波后的先验估计值作为当前时刻的定位数据的输出值。
若误差未超过预设误差阈值，说明原始数据(观测值)正常，未发生跳变，则可以采用观测值对先验估计值进行滤波更新。
滤波参数需要更新的原因是：滤波估计是以上一时刻的状态(包括定位信息)估计下一时刻的定位信息，而目标对象的状态是可以一直在改变。从原因方面来分析，以汽车为例，其油门可能加大，则速度可能增加，或者接收到外界指令需要增加速度等，则下一时刻的状态很可能是变化的。而从结果方面来分析，比如目标对象在k时刻的速度是1m/s，则估计k+1时刻是定位增加一米，实际测量增加了1.2m(假设在预设误差范围内)，那么可以观测在k+1时刻，速度已经达到或超过1.2m/s，即使估计误差在范围内，若目标对象的当前状态发生了轻微改变，上一时刻的状态与下一时刻状态就会发生变化。
因此，需要对滤波参数进行更新，更新是一个持续的过程。采用卡尔曼滤波算法对先验估计值进行滤波更新，具体包括以下步骤：
a：根据上一时刻进行滤波更新后的预测方差计算当前时刻的预测方差。
其中m，为k时刻的预测方差，Pk-1为上一时刻进行滤波更新后的预测方差，A为状态转移矩阵，Qk为过程噪声。
b：基于当前时刻的预测方差更新滤波参数。
更新滤波系统系数：其中，/>为k时刻的预测方差，Rk为测量噪声矩阵，H为观测矩阵。
c：基于更新后的滤波参数、先验估计值、误差确定滤波更新后的先验估计值。
更新滤波状态：
其中，为滤波更新后的k时刻定位数据的先验估计值，/>为k时刻定位数据的先验估计值，K为滤波系统系数，ek为误差。
d：根据当前时刻的预测方差计算当前时刻滤波更新后的预测方差。
其中，H为观测矩阵，K为滤波系统系数，为更新前的k时刻的预测方差，Pk为更新后的k时刻的预测方差。此处更新后的预测方差，是用于下一时刻的滤波过程，与当前时刻无关。
S211、判断当前时刻与前一个非空值的观测值对应的时刻的间隔时间是否超出预设时间阈值。
若是执行S210，若否执行S212。
预设时间阈值可以是正常情况下两次非空值的观测值之间的时间间隔。示例性的，假设每10s钟输出一个非空值的定位数据，则预设的时间阈值为10s，当前时刻是第100s，那么前10s内理应存在一个非空值的定位数据，而若前一个非空值的观测值的时刻为第85s，则两者的时刻的间隔时间超过10s，确定为超过预设时间阈值。
考虑到现实中RTK系统不稳定、中途输出定位数据超时的原因，滤波估计的上一时刻状态与下一时刻的状态差别过大，导致估计的结果输出的误差也较大，若不加时间限制，则一直不更新滤波参数，误差也会一直越来越大，估计的状态的可用性将降低，逐渐偏向于无效。因此，增加时间限制，判断当前时刻与前一个非空值的观测值对应的时刻的间隔时间是否超出预设时间阈值，若否执行S212，若是，即当间隔时间超出预设时间阈值，需要对滤波参数以及当前时刻的先验估计值进行更新，即执行S210。
需要说明的是，在这种设置下，可以保障每一次在接收到非空值的观测值时，可以输出有效的定位数据(T)，在此操作之后，如果接下来的观测值一直为空值，则输出的定位数据一直为预测的先验估计值，但是该先验估计值是基于有效的定位数据(T)来递推得到的，因此也可以视为是有效的。
S212、将先验估计值作为当前时刻的定位数据的输出值。
当间隔时间未超出预设时间阈值，则估计的状态的可用性较高，因此，可以将计算得到的先验估计值直接作为当前时刻的定位数据的输出值。
本实施例在，考虑了观测值和先验估计值之间的误差，基于误差来进行判断是否进行滤波操作，在误差较小时可以直接将先验估计值作为当前时刻的定位数据的输出值，快速高效地完成输出，还结合了非空值的观测值的间隔时间来对滤波参数和当前时刻的先验估计值进行更新，可以防止RTK系统不稳定、中途输出定位数据超时的导致滤波估计的上一时刻状态与下一时刻的状态差别过大，保证输出的定位数据的有效性。
实施例三
图3为本发明实施例三提供的一种定位优化装置的结构示意图。如图3所示，该定位优化装置包括：
观测值获取模块301，用于以预设频率获取当前时刻的定位数据的观测值，所述预设频率大于所述定位数据生成的实际频率；
后验估计值计算模块302，用于获取上一时刻的定位数据的后验估计值；
先验估计值计算模块303，用于基于所述后验估计值计算当前时刻的定位数据的先验估计值；
观测值判断模块304，用于判断所述观测值是否为空值；若是执行第一输出模块的内容，若否执行第二输出模块的内容。
第一输出模块305，用于将所述先验估计值作为当前时刻的定位数据的输出值；
第二输出模块306，用于基于所述先验估计值、所述观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值。
在一个可选的实施例中，所述定位优化装置，还包括：
初始化判断模块，用于判断滤波初始化是否完成；
若是执行后验估计值计算模块302的内容，若否执行初始化模块的内容；
初始化模块，用于将当前时刻的定位数据作为初始时刻的定位数据，以完成滤波初始化。
在一个可选的实施例中，所述观测值获取模块301，包括：
输入值获取子模块，用于以预设频率获取当前时刻的定位数据的输入值；
输入值判断子模块，用于判断所述输入值是否为空；
若是执行空值子模块的内容，若否执行输入值子模块的内容；
空值子模块，用于将预设空值作为当前时刻的定位数据的观测值；
输入值子模块，用于将所述输入值作为当前时刻的定位数据的观测值。
在一个可选的实施例中，所述预测频率是所述实际频率的10倍。
在一个可选的实施例中，所述第二输出模块306，包括：
误差计算子模块，用于计算所述先验估计值与所述观测值的误差；
误差判断子模块，用于判断所述误差是否超过预设误差阈值；若否执行滤波子模块的内容，若是执行时间判断子模块的内容；
滤波子模块，用于采用卡尔曼滤波算法对所述先验估计值进行滤波更新，将滤波后的所述先验估计值作为当前时刻的定位数据的输出值；
时差判断子模块，用于判断当前时刻与前一个非空值的所述观测值对应的时刻的间隔时间是否超出预设时间阈值；若是执行滤波子模块的内容，若否执行定位数据输出子模块的内容；
定位数据输出子模块，用于将所述先验估计值作为当前时刻的定位数据的输出值。
在一个可选的实施例中，定位数据包括坐标，所述误差计算子模块，包括：
坐标确定单元，用于在三维空间中，确定所述先验估计值对应的第一坐标以及所述观测值对应的第二坐标；
误差确定单元，用于将所述第一坐标与所述第二坐标之间的空间距离作为误差。
在一个可选的实施例中，所述滤波子模块，包括：
预测方差计算单元，用于根据上一时刻进行滤波更新后的预测方差计算当前时刻的预测方差；
滤波参数更新单元，用于基于当前时刻的预测方差更新滤波参数；
第一更新单元，用于基于更新后的滤波参数、所述先验估计值、所述误差确定滤波更新后的先验估计值；
第二更新单元，用于根据当前时刻的预测方差计算当前时刻滤波更新后的预测方差。
本发明实施例所提供的定位优化装置可执行本发明任意实施例所提供的定位优化方法，具备执行方法相应的功能模块和有益效果。
实施例四
图4示出了可以用来实施本发明的实施例的电子设备40的结构示意图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备(如头盔、眼镜、手表等)和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本发明的实现。
如图4所示，电子设备40包括至少一个处理器41，以及与至少一个处理器41通信连接的存储器，如只读存储器(ROM)42、随机访问存储器(RAM)43等，其中，存储器存储有可被至少一个处理器执行的计算机程序，处理器41可以根据存储在只读存储器(ROM)42中的计算机程序或者从存储单元48加载到随机访问存储器(RAM)43中的计算机程序，来执行各种适当的动作和处理。在RAM 43中，还可存储电子设备40操作所需的各种程序和数据。处理器41、ROM 42以及RAM 43通过总线44彼此相连。输入/输出(I/O)接口45也连接至总线44。
电子设备40中的多个部件连接至I/O接口45，包括：输入单元46，例如键盘、鼠标等；输出单元47，例如各种类型的显示器、扬声器等；存储单元48，例如磁盘、光盘等；以及通信单元49，例如网卡、调制解调器、无线通信收发机等。通信单元49允许电子设备40通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
处理器41可以是各种具有处理和计算能力的通用和/或专用处理组件。处理器41的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机器学习模型算法的处理器、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。处理器41执行上文所描述的各个方法和处理，例如定位优化方法。
在一些实施例中，定位优化方法可被实现为计算机程序，其被有形地包含于计算机可读存储介质，例如存储单元48。在一些实施例中，计算机程序的部分或者全部可以经由ROM 42和/或通信单元49而被载入和/或安装到电子设备40上。当计算机程序加载到RAM 43并由处理器41执行时，可以执行上文描述的定位优化方法的一个或多个步骤。备选地，在其他实施例中，处理器41可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行定位优化方法。
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、现场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、复杂可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本发明的方法的计算机程序可以采用一个或多个编程语言的任何组合来编写。这些计算机程序可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器，使得计算机程序当由处理器执行时使流程图和/或框图中所规定的功能/操作被实施。计算机程序可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本发明的上下文中，计算机可读存储介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的计算机程序。计算机可读存储介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。备选地，计算机可读存储介质可以是机器可读信号介质。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互，可以在电子设备上实施此处描述的系统和技术，该电子设备具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给电子设备。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)、区块链网络和互联网。
计算系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，又称为云计算服务器或云主机，是云计算服务体系中的一项主机产品，以解决了传统物理主机与VPS服务中，存在的管理难度大，业务扩展性弱的缺陷。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本发明中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本发明的技术方案所期望的结果，本文在此不进行限制。
上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。

Claims (10)

1.一种定位优化方法，其特征在于，用于对目标对象进行跟踪，包括：
以预设频率获取当前时刻的定位数据的观测值，所述预设频率大于所述定位数据生成的实际频率；
获取上一时刻的定位数据的后验估计值；
基于所述后验估计值计算当前时刻的定位数据的先验估计值；
判断所述观测值是否为空值；
若是，将所述先验估计值作为当前时刻的定位数据的输出值；
若否，基于所述先验估计值、所述观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值。
2.如权利要求1所述的方法，其特征在于，在所述以预设频率获取当前时刻的定位数据的观测值之后，还包括：
判断滤波初始化是否完成；
若是，执行获取上一时刻的定位数据的后验估计值的步骤；
若否，将当前时刻的定位数据作为初始时刻的定位数据，以完成滤波初始化。
3.如权利要求1所述的方法，其特征在于，所述以预设频率获取当前时刻的定位数据的观测值，包括：
以预设频率获取当前时刻的定位数据的输入值；
判断所述输入值是否为空；
若是，将预设空值作为当前时刻的定位数据的观测值；
若否，将所述输入值作为当前时刻的定位数据的观测值。
4.如权利要求1所述的方法，其特征在于，所述预测频率是所述实际频率的10倍。
5.如权利要求1所述的方法，其特征在于，所述基于所述先验估计值、所述观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值，包括：
计算所述先验估计值与所述观测值的误差；
判断所述误差是否超过预设误差阈值；
若否，采用卡尔曼滤波算法对所述先验估计值进行滤波更新，将滤波后的所述先验估计值作为当前时刻的定位数据的输出值；
若是，判断当前时刻与前一个非空值的所述观测值对应的时刻的间隔时间是否超出预设时间阈值；
若超出时间阈值，执行采用卡尔曼滤波算法对所述先验估计值进行滤波更新，将滤波后的所述先验估计值作为当前时刻的定位数据的输出值的步骤；
若未超出阈值，将所述先验估计值作为当前时刻的定位数据的输出值。
6.如权利要求5所述的方法，其特征在于，定位数据包括坐标，所述计算所述先验估计值与所述观测值的误差，包括：
在三维空间中，确定所述先验估计值对应的第一坐标以及所述观测值对应的第二坐标；
将所述第一坐标与所述第二坐标之间的空间距离作为误差。
7.如权利要求5所述的方法，其特征在于，所述采用卡尔曼滤波算法对所述先验估计值进行滤波更新，包括：
根据上一时刻进行滤波更新后的预测方差计算当前时刻的预测方差；
基于当前时刻的预测方差更新滤波参数；
基于更新后的滤波参数、所述先验估计值、所述误差确定滤波更新后的先验估计值；
根据当前时刻的预测方差计算当前时刻滤波更新后的预测方差。
8.一种定位优化装置，其特征在于，包括：
观测值获取模块，用于以预设频率获取当前时刻的定位数据的观测值，所述预设频率大于所述定位数据生成的实际频率；
后验估计值计算模块，用于获取上一时刻的定位数据的后验估计值；
先验估计值计算模块，用于基于所述后验估计值计算当前时刻的定位数据的先验估计值；
观测值判断模块，用于判断所述观测值是否为空值；若是执行第一输出模块的内容，若否执行第二输出模块的内容；
第一输出模块，用于将所述先验估计值作为当前时刻的定位数据的输出值；
第二输出模块，用于基于所述先验估计值、所述观测值以及卡尔曼滤波器算法确定当前时刻的定位数据的输出值。
9.一种电子设备，其特征在于，所述电子设备包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的计算机程序，所述计算机程序被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-7中任一项所述的定位优化方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使处理器执行时实现权利要求1-7中任一项所述的定位优化方法。
CN202311288615.4A 2023-10-07 2023-10-07 定位优化方法、装置、电子设备和存储介质 Pending CN117346782A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311288615.4A CN117346782A (zh) 2023-10-07 2023-10-07 定位优化方法、装置、电子设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311288615.4A CN117346782A (zh) 2023-10-07 2023-10-07 定位优化方法、装置、电子设备和存储介质

Publications (1)

Publication Number Publication Date
CN117346782A true CN117346782A (zh) 2024-01-05

Family

ID=89355251

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311288615.4A Pending CN117346782A (zh) 2023-10-07 2023-10-07 定位优化方法、装置、电子设备和存储介质

Country Status (1)

Country Link
CN (1) CN117346782A (zh)

Similar Documents

Publication Publication Date Title
CN115457152A (zh) 外参标定方法、装置、电子设备及存储介质
CN117392241B (zh) 自动驾驶中的传感器标定方法、装置及电子设备
CN114312843A (zh) 用于确定信息的方法和装置
CN114018269A (zh) 定位方法、装置、电子设备、存储介质以及自动驾驶车辆
CN117261880A (zh) 一种车辆控制方法、装置、设备以及存储介质
CN116499487B (zh) 一种车辆的路径规划方法、装置、设备及介质
CN116883460A (zh) 一种视觉感知定位方法、装置、电子设备及存储介质
CN115235527B (zh) 传感器外参标定方法、装置以及电子设备
CN114919661B (zh) 一种泊车控制方法、装置、设备和存储介质
CN117346782A (zh) 定位优化方法、装置、电子设备和存储介质
CN115979288A (zh) 一种航向角确定方法、电子设备和存储介质
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN114987497A (zh) 一种后向车道线拟合方法、装置、电子设备和存储介质
CN114170300A (zh) 高精地图点云位姿优化方法、装置、设备及介质
CN114199236A (zh) 定位数据处理的方法、装置、电子设备和自动驾驶车辆
CN116380056B (zh) 惯性定位方法、装置、电子设备及存储介质
CN117864172B (zh) 一种自动驾驶控制方法、装置以及设备
CN117589188B (zh) 行车路径规划方法、装置、电子设备和存储介质
CN117289686B (zh) 一种参数标定方法、装置、电子设备及存储介质
CN117367431B (zh) 带未知量测偏置的mems与uwb紧组合定位方法及系统
CN115096328B (zh) 车辆的定位方法、装置、电子设备以及存储介质
CN116448105B (zh) 位姿更新方法、装置、电子设备和存储介质
CN117725834A (zh) 坡度估计参数优化方法、装置、电子设备及存储介质
CN117516544A (zh) 一种车辆的融合定位方法、装置、电子设备及存储介质
CN118010049A (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