CN109542093B - 一种处理数据的方法和装置 - Google Patents

一种处理数据的方法和装置 Download PDF

Info

Publication number
CN109542093B
CN109542093B CN201710868757.6A CN201710868757A CN109542093B CN 109542093 B CN109542093 B CN 109542093B CN 201710868757 A CN201710868757 A CN 201710868757A CN 109542093 B CN109542093 B CN 109542093B
Authority
CN
China
Prior art keywords
vector
state vector
constraint equation
state
linearized
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
Application number
CN201710868757.6A
Other languages
English (en)
Other versions
CN109542093A (zh
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.)
Huawei Technologies Co Ltd
Original Assignee
Huawei Technologies 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 Huawei Technologies Co Ltd filed Critical Huawei Technologies Co Ltd
Priority to CN201710868757.6A priority Critical patent/CN109542093B/zh
Priority to PCT/CN2018/106910 priority patent/WO2019057146A1/zh
Priority to EP18857990.8A priority patent/EP3677978B1/en
Publication of CN109542093A publication Critical patent/CN109542093A/zh
Priority to US16/825,694 priority patent/US20200219000A1/en
Application granted granted Critical
Publication of CN109542093B publication Critical patent/CN109542093B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N7/00—Computing arrangements based on specific mathematical models
    • G06N7/01—Probabilistic graphical models, e.g. probabilistic networks
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0285—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using signals transmitted via a public communication network, e.g. GSM network
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N20/00—Machine learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Software Systems (AREA)
  • Multimedia (AREA)
  • Evolutionary Computation (AREA)
  • Artificial Intelligence (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Electromagnetism (AREA)
  • Medical Informatics (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Algebra (AREA)
  • Pure & Applied Mathematics (AREA)
  • Probability & Statistics with Applications (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Business, Economics & Management (AREA)
  • Health & Medical Sciences (AREA)
  • Game Theory and Decision Science (AREA)

Abstract

本申请提供了一种处理数据的方法和装置，涉及人工智能领域，具体涉及机器人、自动驾驶和无人机等领域。该方法包括：获取状态向量集合，该状态向量集合包括智能设备的位姿向量和/或该智能设备所处环境的路标特征向量；确定第一线性化约束方程；确定第一状态向量，该第一状态向量为该状态向量集合中待边缘化的状态向量；对该第一线性化约束方程中的该第一状态向量进行边缘化，得到第二线性化约束方程；根据该第二线性化约束方程，确定目标函数；根据该目标函数，对该至少一个位姿向量和/或该至少一个路标特征向量进行优化，输出该智能设备的轨迹和该智能设备所处环境的地图。本申请有助于降低计算资源和计算时间开销，提升整个SLAM过程的运行效率。

Description

一种处理数据的方法和装置
技术领域
本申请涉及人工智能领域，并且更具体地，涉及一种处理数据的方法和装置。
背景技术
同时定位与地图构建(Simultaneous Localization And Mapping，SLAM)技术是机器人能够探索未知环境所依赖的关键能力。机器人在没有环境先验信息的情况下，利用自身搭载的传感器(摄像头、激光等)对环境中的特征进行观测，在移动过程中对环境进行持续建模，进而对系统状态向量——环境特征位置与机器人位姿进行估计，同时完成地图构建与自定位。
随着机器人在场景中不断移动，以及运动场景的不断扩大，环境特征位置和机器人位姿的数量都会持续增加，导致在通过滤波器或者非线性方法求解时的计算量以及内存开销持续增大，大大降低SLAM过程的实时性，甚至最终造成系统不可正常运行。
目前比较常用的是一种基于舒尔补的边缘化方法，在经典SLAM框架中，将后端优化之后的状态向量—机器人位姿和路标特征点，通过舒尔补持续的进行边缘化处理，并将通过边缘化后的状态向量构建新的目标函数作为下一次优化的输入，但是基于舒尔补的边缘化方法在后端优化之后，后端需要对所有的状态向量全部进行优化，并且在边缘化状态向量时，需要对信息矩阵求逆，计算资源和计算时间的开销相对较高。
因此，如何降低SLAM状态向量估计过程中的计算资源和计算时间开销，提升整个SLAM过程的运行效率成为了一个亟待解决的问题。
发明内容
本申请提供一种处理数据的方法，在优化之前先对状态向量进行边缘化处理，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
第一方面，提供了一种处理数据的方法，包括：获取智能设备的状态向量集合，所述状态向量集合包括所述智能设备的至少一个位姿向量和/或所述智能设备所处环境的至少一个路标特征向量，所述至少一个位姿向量和/或所述至少一个路标特征向量用于所述智能设备的定位和所述智能设备所处环境的地图构建；确定第一线性化约束方程，该第一线性化约束方程表示该至少一个位姿向量和/或该至少一个路标特征向量的线性化约束关系；确定第一状态向量，所述第一状态向量为所述状态向量集合中待边缘化的状态向量；对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程，该第二线性化约束方程表示第二状态向量的线性化约束关系，所述第二状态向量为所述状态向量集合中除所述第一状态向量以外的状态向量；根据所述第二线性化约束方程，确定该状态向量的最大似然估计的目标函数；根据所述目标函数，对所述至少一个位姿向量和/或所述至少一个路标特征向量进行优化，输出所述智能设备的轨迹和所述智能设备所处环境的地图。
因此，本申请实施例的处理数据的方法，在优化之前先对状态向量进行边缘化处理，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
在本申请实施例中，可以选择至少一个位姿向量中的至少部分作为待边缘化的状态向量，也可以选择至少一个路标特征向量中的至少部分作为待边缘化的状态向量，还可以至少一个位姿向量和至少一个路标特征向量中的至少部分作为待边缘化的状态向量。
结合第一方面，在第一方面的某些实现方式中，所述对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程，包括：根据所述第一状态向量的雅克比矩阵的零空间，对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到所述第二线性化约束方程。
本申请实施例的处理数据的方法，通过零空间对状态向量进行边缘化处理，在不丢失信息的前提下，去掉状态向量中的非关键状态向量，避免所有的状态向量全部进行优化，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
结合第一方面，在第一方面的某些实现方式中，所述根据所述第一状态向量的雅克比矩阵的零空间，对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到所述第二线性化约束方程，包括：确定所述第一状态向量的雅克比矩阵；根据所述第一状态向量的雅克比矩阵，确定所述第一状态向量的雅克比矩阵的左零空间；将所述第一状体向量的雅克比矩阵的左零空间左乘所述第一线性化约束方程，得到所述第二线性化约束方程。
在一些可能的实现方式中，通过公式
Hm TUm＝0
确定所述第一状态向量的雅克比矩阵的左零空间，其中，Hm为所述第一状态向量的雅克比矩阵，Um为所述第一状态向量的雅克比矩阵的左零空间。
在一些可能的实现方式中，所述第一状态向量的雅克比矩阵的左零空间为：
Figure BDA0001416697790000021
其中，I为单位矩阵。
结合第一方面，在第一方面的某些实现方式中，所述第二线性化约束方程为：
Figure BDA0001416697790000022
其中，
Figure BDA0001416697790000023
为所述第二状态向量的误差向量，Hu为所述第二状态向量的雅克比矩阵，Um为所述第一状态向量的雅克比矩阵的零空间，
Figure BDA0001416697790000024
为被观测向量的残差向量，n为高斯白噪声向量。
结合第一方面，在第一方面的某些实现方式中，所述对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程之前，所述方法还包括：根据所述第一状态向量，对所述第一线性化约束方程进行重排列，重排列后的所述第一线性化约束方程中的所述第一状态向量和所述第二状态向量分离。
结合第一方面，在第一方面的某些实现方式中，所述重排列后的所述第一线性化约束方程为：
Figure BDA0001416697790000025
其中，
Figure BDA0001416697790000026
为第二状态向量的误差向量，
Figure BDA0001416697790000027
为所述第一状态向量的误差向量，Hm为所述第一状态向量的雅克比矩阵，Hu为所述第二状态向量的雅克比矩阵，z～为被观测向量的残差向量，n为高斯白噪声向量。
结合第一方面，在第一方面的某些实现方式中，所述方法还包括：根据所述目标函数，确定信息矩阵，所述信息矩阵为下一次处理数据的约束条件。
在一些可能的实现方式中，所述目标函数为：
Figure BDA0001416697790000031
其中，
Figure BDA0001416697790000032
H'u＝Um ΤHu，R为Um Τn的协方差。
结合第一方面，在第一方面的某些实现方式中，所述确定所述至少一个位姿向量和/或至少一个路标特征向量的第一线性化约束方程之前，所述方法还包括：确定所述至少一个位姿向量和/或至少一个路标特征向量的非线性化约束方程；根据所述至少一个位姿向量和/或至少一个路标特征向量的雅克比矩阵，对所述非线性化约束方程进行线性化，得到所述第一线性化约束方程。
在一些可能的实现方式中，通过高斯-牛顿法对所述非线性化约束方程进行线性优化，得到第一线性化约束方程。
在一些可能的实现方式中，所述方法还包括：根据该目标函数，确定信息矩阵，该信息矩阵用于优化并更新该第二状态向量，并基于优化更新后的第二状态向量，构建新的约束项，该约束项用于对下一次处理数据时建立的线性化约束方程进行约束。
在一些可能的实现方式中，所述信息矩阵为：
Figure BDA0001416697790000033
其中，∑为所述信息矩阵。
在一些可能的实现方式中，对第二状态向量进行图优化，对第二状态向量进行更新
Figure BDA0001416697790000034
并根据得到信息矩阵构建下一次图优化的先验约束：
Figure BDA0001416697790000035
该先验约束作为下一次数据处理时新的约束加入整个位姿图中，从而完善整个图优化。
其中，
Figure BDA0001416697790000036
为要保留的状态向量的误差向量，
Figure BDA0001416697790000037
为要保留的状态向量上一次的估计值，
Figure BDA0001416697790000038
为要保留的状态向量更新后的估计值。本申请实施例的处理数据的方法，在对目标函数优化之前先通过零空间对状态向量进行边缘化处理，在不丢失信息的前提下，去掉状态向量中的非关键状态向量，避免所有的状态向量全部进行优化，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
第二方面，提供了一种处理数据的装置，用于执行第一方面或第一方面任一种可能的实现方式。具体地，该处理数据的装置可以包括用于执行第一方面或第一方面任一种可能的实现方式中的方法的单元。
第三方面，提供了一种处理数据装置，包括：存储器、处理器和通信接口，所述存储器用于存储计算机程序，所述处理器用于从所述存储器中调用并运行所述计算机程序，使得所述处理数据的装置执行上述第一方面或第一方面任一种可能实现方式中的方法。
具体地，所述处理器用于：
获取智能设备的状态向量集合，所述状态向量集合包括所述智能设备的至少一个位姿向量和/或所述智能设备所处环境的至少一个路标特征向量，所述至少一个位姿向量和/或所述至少一个路标特征向量用于所述智能设备的定位和所述智能设备所处环境的地图构建；
确定第一线性化约束方程，该第一线性化约束方程表示该至少一个位姿向量和/或该至少一个路标特征向量的线性化约束关系；
确定第一状态向量，所述第一状态向量为所述状态向量集合中待边缘化的状态向量；对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程，该第二线性化约束方程表示第二状态向量的线性化约束关系，所述第二状态向量为所述状态向量集合中除所述第一状态向量以外的状态向量；
根据所述第二线性化约束方程，确定该状态向量的最大似然估计的目标函数；
根据所述目标函数，对所述至少一个位姿向量和/或所述至少一个路标特征向量进行优化，输出所述智能设备的轨迹和所述智能设备所处环境的地图。
第四方面，提供了一种计算机程序产品，所述计算机程序产品包括：计算机程序(也可以称为代码，或指令)，当所述计算机程序在计算机上运行时，使得所述计算机执行上述第一方面或第一方面中任一种可能实现方式中的方法。
第五方面，提供了一种计算机可读存储介质，用于存储计算机程序，该计算机程序包括用于执行第一方面或第一方面任一种可能实现方式的方法的指令。
本申请实施例在对目标函数优化之前先通过零空间对状态向量进行边缘化处理，在不丢失信息的前提下，去掉状态向量中的非关键状态向量，避免所有的状态向量全部进行优化，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
附图说明
图1是本申请实施例的技术方案的一种应用场景的示意图。
图2是本申请实施例的处理数据的方法的示意性流程图。
图3是本申请实施例的零空间边缘化方法的示意性流程图。
图4是本申请实施例的处理数据的方法的另一示意性流程图。
图5是本申请实施例的零空间边缘化方法的另一示意性流程图。
图6是本申请实施例的处理数据的方法的再一示意性流程图。
图7是本申请实施例的基于零空间的边缘化方法和基于舒尔补的边缘化方法的计算过程比较。
图8是本申请实施例的处理数据的装置的示意性框图。
图9是本申请实施例的处理数据的装置的另一示意性框图。
具体实施方式
下面将结合附图，对本申请中的技术方案进行描述。
在对本申请实施例的技术方案介绍之前，首先介绍几个关键术语的定义。
零空间：已知A为一个m×n的矩阵。A的零空间(nullspace)，又称核(kernel)，是一组由下列公式定义的n维向量：
Figure BDA0001416697790000051
即线性方程组Ax＝0的所有解x的集合。
雅克比矩阵：雅克比矩阵是一阶偏导数以一定的方式排列成的矩阵，设有m个n元函数组成的函数组：yi(x1,x2,...xn)(i＝1，2，…m)，称之为函数组。对这个函数组取一阶导数，获得下面的雅克比矩阵：
Figure BDA0001416697790000052
本申请实施例适用于同时定位与地图构建(Simultaneous Localization AndMapping，SLAM)领域以及任何用于非线性估计问题中的状态边缘化处理的智能设备(Intelligent Equipment)中。
应理解，智能设备是指任何一种具有计算处理能力的设备、器械或者机器，本申请实施例中的智能设备可以是机器人(Robot)、自动驾驶汽车(Autonomous Vehicles)、无人机(Unmanned Aerial Vehicle)、智能家居(Smart Home)和手机终端等，本申请对该智能设备并不做任何限定。
图1示出了本申请实施例的技术方案的一种应用场景的示意图，如图1所示，本申请实施例的技术方案适用于机器人的SLAM系统100中，该SLAM系统包括前端(Front-end)110和后端(Back-end)120，其中，前端110主要完成传感器的数据采集和数据关联，而后端120主要功能是对前端产生的模型进行参数估计以及优化。下面以机器人为例对本申请实施例的技术方案进行描述。
图2示出了根据本申请实施例的处理数据的方法200的示意性流程图，如图2所示，该方法200可以由图1中的SLAM系统100执行，该方法200包括：
S210，获取智能设备的状态向量集合，该状态向量集合包括该智能设备的至少一个位姿向量和/或该智能设备所处环境的至少一个路标特征向量，该至少一个位姿向量和/或该至少一个路标特征向量用于该智能设备的定位和该智能设备所处环境的地图构建。
具体而言，该机器人的SLAM系统100中前端110获取视觉传感器、惯性传感器IMU的数据，提取机器人的至少一个位姿向量，并对获取的图像进行和IMU数据进行时间同步，提取视觉图像信息中的该机器人所处环境的至少一个路标特征向量，并进行特征匹配。
应理解，特征匹配就是确定两次获取的图像数据哪些对应的是同一个路标特征。
例如，机器人在运动过程中会产生一条轨迹(x1,x2,...,xk)，由于机器人的运动符合一定的运行学模型规律，基于该IMU数据和运动模型，建立机器人的运动方程：
xk＝fk(xk-1,uk)+wk (1)
其中，uk表示来自于里程计、惯性测量单元(Inertial Measurement Unit，IMU)等运动传感器的观测值，wk是协方差Qk为的高斯白噪声。
应理解，视觉传感器提取的是路标特征向量，但是视觉传感器可以通过上下两帧的数据推算出机器人的位姿向量，惯性传感器提取的是该机器人的位姿向量。
又例如，机器人在运动场景中存在许多路标，而机器人在运动过程的每个位置，都会通过传感器测量到一定的路标点，得到相应的观测值。进而机器人依靠观测模型来检测和跟踪特征，基于上述提取的该路标特征向量、观测模型以及上一帧优化之后的位姿作为先验信息，建立机器人的观测方程：
zkj＝hkj(xk,xfj)+vkj (2)
其中，Zkj表示机器人在位姿xk时对于特征xfj的观测值，vkj是协方差Rk为的高斯白噪声。
应理解，S210获取状态向量集合可以由图1中SLAM系统100的前端110执行。
还应理解，噪声是满足N(0，σ2)的高斯分布，这里的0表示均值，σ2表示方差，对应到具体的运动方程中噪声的方差就是Qk，对应到具体的观测方程中噪声的方差就是Rk。
S220，确定第一线性化约束方程，该第一线性化约束方程表示该至少一个位姿向量和/或该至少一个路标特征向量的线性化约束关系。
具体而言，在S210获取该机器人的至少一个位姿向量和/或该机器人所处环境的至少一个路标特征向量，所建立的运动方程(1)和观测方程(2)为该至少一个位姿向量和/或该至少一个路标特征向量的非线性化方程，通过该至少一个位姿向量和/或该至少一个路标特征向量的雅克比矩阵，将该非线性方程线性化。
应理解，构建线性约束方程主要是将状态向量的非线性化的约束表示转换成线性化的数学表达以简化求解过程。
还应理解，S220确定该至少一个位姿向量和/或该至少一个路标特征向量的第一线性化约束方程可以由SLAM系统100的后端120执行。
分别计算约束方程中运动方程(1)和观测方程(2)的雅克比矩阵Fk和Hkj、Hfj：
Figure BDA0001416697790000061
这里Fk为机器人位姿在运动模型中状态k的雅克比矩阵，Hkj和Hfj分别为机器人位姿向量和路标特征向量在观测方程中状态k的雅克比矩阵。
利用雅克比矩阵线性化约束方程后得到第一线性化约束方程：
线性化的运动方程：
Figure BDA0001416697790000062
线性化的观测方程：
Figure BDA0001416697790000063
S230，确定第一状态向量，该第一状态向量为该状态向量集合中待边缘化的状态向量。
具体而言，根据当前SLAM应用场景，可以从机器人的位姿向量和路标特征向量两者中选择一个作为具体的要进行边缘化处理状态向量，也可以将两者同时作为待边缘化的状态向量。
应理解，利用确定的待优化的状态向量对线性化的约束方程进行重排，为后续的边缘化操作做好准备。
还应理解，S230确定第一状态向量可以由SLAM系统100的后端120执行。
例如，可以通过滑动窗口的方法，将窗口中距离机器人当前状态点时间比较久的点作为实际要边缘化掉的点，首先设置滑动窗口的大小为D(仅在系统初始化时执行)，将当前窗口内观测到的(k-D)时刻的位姿向量和所有的路标特征向量作为要边缘化的状态向量。
又例如，可以通过视差法，将与机器人当前状态点观测视差较小的点作为实际的要边缘化掉的点。
应理解，S220和S230并没有实际的先后顺序，可以先确定该第一线性化约束方程，后确定待边缘化的状态向量，也可以先确定待边缘化的状态向量，后确定该第一线性化约束方程，本申请并不限于此。
S240，对该第一线性化约束方程中的该第一状态向量进行边缘化，得到第二线性化约束方程，该第二线性化约束方程表示第二状态向量的线性化约束关系，该第二状态向量为该状态向量集合中除该第一状态向量以外的状态向量。
可选地，在对该第一线性化约束方程中的该第一状态向量进行边缘化，得到第二状态向量的第二线性化约束方程之前，该方法还包括：
根据该第一状态向量，对该第一线性化约束方程进行重排列，重排列后的该第一线性化约束方程中的该第一状态向量和该第二状态向量分离。
例如，在S230中确定将当前窗口内观测到的(k-D)时刻的位姿向量和所有的路标特征向量作为待边缘化的状态向量，重排列步骤S220建立的线性化约束方程，将待边缘化的状态向量分离出来，得到排列后的第一线性化约束方程(6)：
Figure BDA0001416697790000071
将重排列后的第一线性化约束方程记为：
Figure BDA0001416697790000072
其中，
Figure BDA0001416697790000073
表示要保留的状态向量的误差向量，Hu为要保留的状态向量的雅克比矩阵，
Figure BDA0001416697790000074
为待边缘化的状态向量的误差向量，Hm为待边缘化的状态向量的雅克比矩阵，
Figure BDA0001416697790000075
为被观测向量的残差向量，n为高斯白噪声向量。
应理解，该第一状态向量和该第一状态向量的误差向量存在一个差值，该第二状体向量和该第二状态向量的误差向量也存在一个差值，上述重排列后的第一线性化约束方程中
Figure BDA0001416697790000076
和
Figure BDA0001416697790000077
分离也可以理解为该第一状态向量和该第二状态向量分离。
还应理解，S240确定该第二线性化约束方程可以由SLAM系统100的后端120执行。
本申请实施例中，为了保持SLAM状态向量有界，需要限制状态向量的数据，以降低计算开销和计算时间，所以在优化之前对状态向量进行边缘化处理，确定状态向量集合中非关键的状态向量，避免所有的状态向量全部进行优化，通过对第一线性化约束方程中待边缘化的状态向量进行边缘化，得到该第二状态向量的第二线性化约束方程，使得第二线性化约束方程中只保留需要的状态向量。
可选地，对该第一线性化约束方程中的该第一状态向量进行边缘化，得到第二状态向量的第二线性化约束方程，包括：
根据该第一状态向量的雅克比矩阵的零空间，对该第一线性化约束方程中的该第一状态向量进行边缘化，得到该第二线性化约束方程。
图3示出了根据本申请实施例的零空间边缘化方法300的示意性流程图，如图3所示，该零空间边缘化方法300可以由图1中SLAM系统的后端120执行，该方法300包括：
S310，确定该第一状态向量的雅克比矩阵。
具体而言，公式(7)中的Hm即为待边缘化的状态向量的雅克比矩阵。
S320，根据该第一状态向量的雅克比矩阵，确定该第一状态向量的雅克比矩阵的左零空间。
具体而言，计算待边缘化状态向量的雅克比矩阵的左零空间，根据线性化之后的约束方程(7)，求解Hm对应的左零空间Um，求解方程为：
Hm TUm＝0 (8)
给出一种解析解的方式对左零空间进行求解，Um的解析解表示为：
Figure BDA0001416697790000081
S330，将该第一状体向量的雅克比矩阵的左零空间左乘该第一线性化约束方程，得到该第二线性化约束方程。
具体而言，将计算得到的左零空间Um左乘重排列后的第一线性化约束方程，方程如下所示：
Figure BDA0001416697790000082
由于Hm TUm＝0，所以最终得到的第二线性化约束方程为：
Figure BDA0001416697790000083
本申请实施例的零空间边缘化方法，通过待边缘化状态向量的零空间，去掉状态向量中非关键的状态向量，有助于降低计算时间。
S250，根据该第二线性化约束方程，确定该状态向量的最大似然估计的目标函数。
应理解，最大似然估计(Maximum Likelihood，ML)也称为最大概率估计，是一种具有理论性的点估计法，该方法的基本思想是：当从模型总体随机抽取n组样本观测值后，最合理的参数估计量应该使得从模型中抽取该n组样本观测值的概率最大。
具体而言，根据新构建的第二线性化约束方程，可以构建新的优化目标函数用于SLAM系统100的后端120进行优化，具体的，根据新构建的约简后的第二线性化约束方程(11)，构建优化目标函数，得到：
Figure BDA0001416697790000084
其中，
Figure BDA0001416697790000085
H'u＝Um ΤHu，R为Um Τn的协方差。
可选地，该处理数据的方法100还包括：
可选地，所述方法还包括：
根据该目标函数，确定信息矩阵，该信息矩阵用于优化并更新该第二状态向量，并基于优化更新后的第二状态向量，构建新的约束项，该约束项用于对下一次处理数据时建立的线性化约束方程进行约束。
具体而言，在得到式(12)后，根据式(12)计算边缘化后的信息矩阵如下：
Figure BDA0001416697790000086
具体而言，根据该目标函数，通过图优化方法估计该第二状态向量的最优值，并确定信息矩阵，共同作为下一次数据处理的约束条件。
S260，根据该目标函数，对该至少一个位姿向量和/或该至少一个路标特征向量进行优化，输出该智能设备的轨迹和该智能设备所处环境的地图。
具体而言，基于约简后的目标函数，利用扩展卡尔曼滤波器(Extended KalmanFilter，EKF)方法或者非线性优化方法，对所有的状态向量(机器人的位姿向量和当前机器人所处环境的路标特征向量)进行最优估计和优化，得到该机器人的轨迹和该机器人所处环境的地图。对第二状态向量进行图优化，并对第二状态向量进行更新
Figure BDA0001416697790000091
根据得到的信息矩阵构建下一次图优化的先验约束：
Figure BDA0001416697790000092
作为下次优化时新的约束加入整个位姿图中，完善整个图优化。
其中，
Figure BDA0001416697790000093
为要保留的状态向量的误差向量，
Figure BDA0001416697790000094
为要保留的状态向量上一次的估计值，
Figure BDA0001416697790000095
为要保留的状态向量更新后的估计值。本申请实施例的处理数据的方法，在对目标函数优化之前先通过零空间对状态向量进行边缘化处理，在不丢失信息的前提下，去掉状态向量中的非关键状态向量，避免所有的状态向量全部进行优化，有助于降低SLAM状态向量估计过程中的计算资源和计算时间开销，从而提升整个SLAM过程的运行效率，避免状态向量持续增大造成的系统崩溃问题。
作为一个具体的实施例，图4示出了根据本申请实施例的处理数据的方法400的示意性流程图，如图4所示，该方法400包括：
S410，获取传感器的测量值。
采集相机、IMU等传感器信息，对相机的图像信息进行预测，对相机、IMU等传感器数据进行同步，获取智能设备当前位姿的观测状态量。
S420，建立状态向量的非线性化约束关系。
通过运动方程(1)和观测方程(2)，建立相邻帧之间传感器数据的约束关系，进而通过约束关系的传导建立所有状态向量之间的非线性化约束关系。
S430，基于零空间的边缘化处理。
对S420建立的所有状态向量之间的非线性化约束关系进行线性化处理，利用左零空间去除状态向量中的非关键的状态向量，并根据要保留的状态向量的约束构建优化目标函数。
S440，对S430处理得到的目标函数进行优化。
基于约简后的目标函数，利用EKF方法或者非线性优化方法，对所有的状态向量进行最优估计和优化，得到全局一致的轨迹和地图。
应理解，S410和S420可以由SLAM系统100的前端110处理，S430和S440可以由SLAM系统100的后端120处理。
作为另一个具体的实施例，图5示出了根据本申请实施例的基于零空间边缘化处理方法500的示意性流程图，如图5所示，该方法500包括：
S510，构建线性化约束方程。
构建线性约束方程主要是将状态向量的非线性化的约束表示转换成线性化的数学表达以简化求解过程。同时，利用确定的待优化的状态向量对线性化的约束方程进行重排，为后续的边缘化操作做好准备。其主要涉及以下2个步骤：
S511，确定待边缘化的状态向量。
根据当前SLAM过程的应用场景，确定是对智能设备的位姿向量或者该智能设备所处环境的路标特征向量进行边缘化，同时确定对哪些位姿向量或者路标特征向量进行边缘化处理。
S512，线性化约束方程。
计算状态向量的雅克比矩阵，并利用雅克比矩阵构建状态向量的线性化约束方程，根据确定的待边缘化的状态向量，对线性化方程进行重排。
S520，计算左零空间。
计算左零空间主要是完成对待边缘化状态向量的雅克比矩阵的左零空间的计算操作。
S530，约简目标函数。
约简目标函数主要是对线性化方程进行左乘零空间的运算，去除线性化约束方程中非关键的状态向量，得到边缘化之后的线性化约束方程，并根据边缘化之后的线性方程构建新的优化目标函数。其主要涉及以下2个步骤：
S531，零空间左乘线性化约束方程。
将待边缘化状态向量的雅克比矩阵的零空间左乘线性化约束方程，去除线性化方程中非关键的状态向量。
S532，构建约简后的目标函数。
在去除非关键状态向量后，得到新的状态向量的约束表示，依据新的约束表示构建新的优化目标函数。
作为一个具体的实施例，图6示出了根据本申请实施例的处理数据方法600的示意性流程图，如图6所示，该方法600包括：
S601，获取视觉传感器、惯性传感器IMU的数据，并对获取的图像和IMU数据进行时间同步；
S602，提取视觉图像信息中的环境路标特征向量，并进行特征匹配；
S603，建立该至少一个位姿向量和/或至少一个路标特征向量的非线性化约束方程，包括运动方程和观测方程，基于S601的IMU数据和运动模型，建立智能设备的运动方程；基于S602提取的路标特征向量、观测模型和上一帧优化之后的位姿作为先验信息，建立观测观测方程；
S604，确定待边缘化的状态向量，通过滑动窗口的方法或者视差法，确定位姿向量和路标特征向量中待边缘化的状态向量；
S605，线性化S603建立的非线性化约束方程，分别计算S603中运动方程和观测方程的雅克比矩阵，根据该雅克比矩阵，线性化S603中的非线性化约束方程，得到第一线性化约束方程；
S606，重排列S605建立的线性化约束方程，将待边缘化的状态向量分离出来，根据S604中确定的待边缘化的状态向量，将S605建立的第一线性化约束方程重排列，将该第一线性化约束方程中要保留的状态向量和待边缘化的状态向量分离；
S607，计算待边缘化向量的雅克比矩阵的左零空间，根据S606中重排列后的第一线性化约束方程，求解该待边缘化的状态向量的左零空间；
S608，边缘化状态向量，确定约简后的线性化约束方程，利用S607计算得到的该待边缘化的状态向量的左零空间左乘该S606中重排列后的该第一线性化约束方程，得到约简后的第二线性化约束方程；
S609，基于S608中确定的约简后的第二线性化约束方程，构建目标函数，计算边缘化后的信息矩阵；
S610，对目标函数进行图优化，并根据S609得到的信息矩阵构建下一次优化的先验约束；
S611，如果SLAM过程结束，则输出该智能设备的全部位姿和该智能设备所处环境的地图；否则，循环执行步骤S601到S610。
表1示出了根据本申请实施例的基于舒尔补边缘化方法与基于零空间边缘化方法的计算时间比较，由表1可以看出，相比于舒尔补边缘化方法，基于零空间边缘化方法的运行效率是其4.6倍。
表1
边缘化算法 平均运行时间(s)
基于舒尔补的边缘化算法 0.0440574
基于零空间的边缘化算法 0.00953969
图7示出了根据本申请实施例的零空间方法和舒尔补方法的计算步骤比较，如图7所示，对比分析两者的计算复杂度，假设m个机器人位姿跟踪n个路标特征，黑框表示两种边缘化方法中最耗时的步骤，通常情况下n是远大于m的，由此可见，零空间方法计算复杂度要小于舒尔补方法。
以上结合如2至图7，对本申请实施的数据处理的方法进行了详细描述，下面结合图8至图9，对本申请实施例的处理数据的装置进行详细描述。
图8示出了根据本申请实施例的处理数据的装置700的示意性框图，如图8所示，该装置700包括：
获取单元710，用于获取智能设备的状态向量集合，该状态向量集合包括智能设备的至少一个位姿向量和/或该智能设备所处环境的至少一个路标特征向量，该至少一个位姿向量和/或该至少一个路标特征向量用于该智能设备的定位和该智能设备所处环境的地图构建；
确定单元720，用于确定第一线性化约束方程，该第一线性化约束方程表示该至少一个位姿向量和/或该至少一个路标特征向量的线性化约束关系；
该确定单元720还用于确定第一状态向量，该第一状态向量为该状态向量集合中待边缘化的状态向量；
边缘化单元730，用于对该第一线性化约束方程中的该第一状态向量进行边缘化，得到第二线性化约束方程，该第二线性化约束方程表示第二状态向量的线性化约束关系，该第二状态向量为该状态向量集合中除该第一状态向量以外的状态向量；
该确定单元720还用于根据该第二线性化约束方程，确定该状态向量的最大似然估计的目标函数；
优化单元740，用于根据该目标函数，对该至少一个位姿向量和/或该至少一个路标特征向量进行优化，输出该智能设备的轨迹和该智能设备所处环境的地图。
可选地，该边缘化单元730具体用于：
根据该第一状态向量的雅克比矩阵的零空间，对该第一线性化约束方程中的该第一状态向量进行边缘化，得到该第二线性化约束方程。
可选地，该边缘化单元具体用于：
确定该第一状态向量的雅克比矩阵；
根据该第一状态向量的雅克比矩阵，确定该第一状态向量的雅克比矩阵的左零空间；
将该第一状体向量的雅克比矩阵的左零空间左乘该第一线性化约束方程，得到该第二线性化约束方程。
可选地，该第二线性化约束方程为：
Figure BDA0001416697790000121
其中，
Figure BDA0001416697790000122
为该第二状态向量的误差向量，Hu为该第二状态向量的雅克比矩阵，Um为该第一状态向量的雅克比矩阵的零空间，
Figure BDA0001416697790000123
为被观测向量的残差向量，n为高斯白噪声向量。
可选地，该装置700还包括：
重排列单元，用于根据该第一状态向量，对该第一线性化约束方程进行重排列，重排列后的该第一线性化约束方程中的该第一状态向量和该第二状态向量分离。
可选地，该重排列后的该第一线性化约束方程为：
Figure BDA0001416697790000124
其中，
Figure BDA0001416697790000125
为第二状态向量的误差向量，
Figure BDA0001416697790000126
为该第一状态向量的误差向量，Hm为该第一状态向量的雅克比矩阵，Hu为该第二状态向量的雅克比矩阵，
Figure BDA0001416697790000127
为被观测向量的残差向量，n为高斯白噪声向量。
可选地，该确定单元720还用于：
根据该目标函数，确定信息矩阵，该信息矩阵为下一次处理数据的约束条件。
可选地，该确定单元还用于：
确定该至少一个位姿向量和/或至少一个路标特征向量的非线性化约束方程；
根据该至少一个位姿向量和/或至少一个路标特征向量的雅克比矩阵，对该非线性化约束方程进行线性化，得到该第一线性化约束方程。
可选地，该确定单元720还用于：
根据该目标函数，确定信息矩阵，该信息矩阵用于优化并更新该第二状态向量，并基于优化更新后的第二状态向量，构建新的约束项，该约束项用于对下一次处理数据时建立的线性化约束方程进行约束。
应理解，处理数据的装置700可以对应于根据本申请实施例的处理数据的方法200中的SLAM系统，该处理数据的装置700可以包括用于执行图2中处理数据的方法200的SLAM系统执行的方法的模块。并且，该处理数据的装置700中的各模块和上述其他操作和/或功能分别为了实现图2中处理数据的方法200的相应流程，具体地，获取单元710用于执行方法200中的步骤210，确定单元720用于执行方法200中的步骤220、步骤230和步骤250，边缘化单元730用于执行方法200中的步骤240，优化单元740用于执行方法200中的步骤260，各单元执行上述相应步骤的具体过程在方法200中已经详细说明，为了简洁，在此不再赘述。
图9是本申请实施例提供的处理数据的装置800的结构性示意图。如图9所示，该处理数据的装置800包括：处理器810、存储器820和通信接口830。其中，存储器820中存储有指令，处理器810用于执行存储器820中的指令，当该指令被执行时，该处理器510用于执行上述方法实施例提供的方法，处理器810还用于控制通信接口830与外界进行通信。
具体地，处理数据的装置800可以对应于根据本申请实施例的SLAM系统100，该处理数据的装置800可以包括用于执行图2中处理数据的方法200的SLAM系统100执行的方法的模块。并且，该处理数据的装置800中的各模块和上述其他操作和/或功能分别为了实现图2中处理数据的方法200的相应流程。各模块执行上述相应步骤的具体过程在方法200中已经详细说明，为了简洁，在此不再赘述。
本申请实施例还提供一种计算机可读存储介质，该计算机可读存储介质包括计算机程序，当其在计算机上运行时，使得该计算机执行上述方法实施例提供的方法。
本申请实施例还提供一种包含指令的计算机程序产品，当该计算机程序产品在计算机上运行时，使得该计算机执行上述方法实施例提供的方法。
应理解，本申请实施例中，该处理器可以为中央处理单元(central processingunit，CPU)，该处理器还可以是其他通用处理器、数字信号处理器(digital signalprocessor，DSP)、专用集成电路(application specific integrated circuit，ASIC)、现成可编程门阵列(field programmable gate array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
还应理解，本申请实施例中的存储器可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(read-only memory，ROM)、可编程只读存储器(programmable ROM，PROM)、可擦除可编程只读存储器(erasable PROM，EPROM)、电可擦除可编程只读存储器(electrically EPROM，EEPROM)或闪存。易失性存储器可以是随机存取存储器(random access memory，RAM)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的随机存取存储器(random accessmemory，RAM)可用，例如静态随机存取存储器(static RAM，SRAM)、动态随机存取存储器(DRAM)、同步动态随机存取存储器(synchronous DRAM，SDRAM)、双倍数据速率同步动态随机存取存储器(double data rate SDRAM，DDR SDRAM)、增强型同步动态随机存取存储器(enhanced SDRAM，ESDRAM)、同步连接动态随机存取存储器(synchlink DRAM，SLDRAM)和直接内存总线随机存取存储器(direct rambus RAM，DR RAM)。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。

Claims (11)

1.一种处理数据的方法，其特征在于，包括：
获取智能设备的状态向量集合，所述状态向量集合包括所述智能设备的至少一个位姿向量和/或所述智能设备所处环境的至少一个路标特征向量，所述至少一个位姿向量和/或所述至少一个路标特征向量用于所述智能设备的定位和所述智能设备所处环境的地图构建；
确定第一线性化约束方程，所述第一线性化约束方程表示所述至少一个位姿向量和/或所述至少一个路标特征向量的线性化约束关系；
确定第一状态向量，所述第一状态向量为所述状态向量集合中待边缘化的状态向量；
根据公式(1)和(2)，确定所述第一状态向量的雅克比矩阵的左零空间：
Hm TUm＝0 (1)
Figure FDF0000015720020000011
其中，Hm为所述第一状态向量的雅克比矩阵，Um为所述左零空间；
将所述左零空间左乘所述第一线性化约束方程，得到第二线性化约束方程，所述第二线性化约束方程表示第二状态向量的线性化约束关系，所述第二状态向量为所述状态向量集合中除所述第一状态向量以外的状态向量；
根据所述第二线性化约束方程，确定所述状态向量的最大似然估计的目标函数；
根据所述目标函数，对所述至少一个位姿向量和/或所述至少一个路标特征向量进行优化，输出所述智能设备的轨迹和所述智能设备所处环境的地图。
2.根据权利要求1所述的方法，其特征在于，所述第二线性化约束方程为：
Figure FDF0000015720020000012
其中，
Figure FDF0000015720020000013
为所述第二状态向量的误差向量，Hu为所述第二状态向量的雅克比矩阵，
Figure FDF0000015720020000014
为被观测向量的残差向量，n为高斯白噪声向量。
3.根据权利要求1或2所述的方法，其特征在于，所述对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到第二线性化约束方程之前，所述方法还包括：
根据所述第一状态向量，对所述第一线性化约束方程进行重排列，重排列后的所述第一线性化约束方程中的所述第一状态向量和所述第二状态向量分离。
4.根据权利要求3所述的方法，其特征在于，所述重排列后的所述第一线性化约束方程为：
Figure FDF0000015720020000015
其中，
Figure FDF0000015720020000016
为第二状态向量的误差向量，
Figure FDF0000015720020000017
为所述第一状态向量的误差向量，Hu为所述第二状态向量的雅克比矩阵，
Figure FDF0000015720020000018
为被观测向量的残差向量，n为高斯白噪声向量。
5.根据权利要求1、2和4中任一项所述的方法，其特征在于，所述确定所述至少一个位姿向量和/或至少一个路标特征向量的第一线性化约束方程之前，所述方法还包括：
确定所述至少一个位姿向量和/或至少一个路标特征向量的非线性化约束方程；
根据所述至少一个位姿向量和/或至少一个路标特征向量的雅克比矩阵，对所述非线性化约束方程进行线性化，得到所述第一线性化约束方程。
6.一种处理数据的装置，其特征在于，包括：
获取单元，用于获取智能设备的状态向量集合，所述状态向量集合包括所述智能设备的至少一个位姿向量和/或所述智能设备所处环境的至少一个路标特征向量，所述至少一个位姿向量和/或所述至少一个路标特征向量用于所述智能设备的定位和所述智能设备所处环境的地图构建；
确定单元，用于确定第一线性化约束方程，所述第一线性化约束方程表示所述至少一个位姿向量和/或所述至少一个路标特征向量的线性化约束关系；
所述确定单元还用于确定第一状态向量，所述第一状态向量为所述状态向量集合中待边缘化的状态向量；
边缘化单元，用于根据公式(1)和(2)，确定所述第一状态向量的雅克比矩阵的左零空间：
Hm TUm＝0 (1)
Figure FDF0000015720020000021
其中，所述Hm为所述第一状态向量的雅克比矩阵，所述Um为所述左零空间；
将所述左零空间左乘所述第一线性化约束方程，得到第二线性化约束方程，所述第二线性化约束方程表示第二状态向量的线性化约束关系，所述第二状态向量为所述状态向量集合中除所述第一状态向量以外的状态向量；
所述确定单元还用于根据所述第二线性化约束方程，确定所述状态向量的最大似然估计的目标函数；
优化单元，用于根据所述目标函数，对所述至少一个位姿向量和/或所述至少一个路标特征向量进行优化，输出所述智能设备的轨迹和所述智能设备所处环境的地图；
所述边缘化单元具体用于：根据所述第一状态向量的雅克比矩阵的零空间，对所述第一线性化约束方程中的所述第一状态向量进行边缘化，得到所述第二线性化约束方程。
7.根据权利要求6所述的装置，其特征在于，所述第二线性化约束方程为：
Figure FDF0000015720020000022
其中，
Figure FDF0000015720020000023
为所述第二状态向量的误差向量，Hu为所述第二状态向量的雅克比矩阵，
Figure FDF0000015720020000024
为被观测向量的残差向量，n为高斯白噪声向量。
8.根据权利要求6或7所述的装置，其特征在于，所述装置还包括：
重排列单元，用于根据所述第一状态向量，对所述第一线性化约束方程进行重排列，重排列后的所述第一线性化约束方程中的所述第一状态向量和所述第二状态向量分离。
9.根据权利要求8所述的装置，其特征在于，所述重排列后的所述第一线性化约束方程为：
Figure FDF0000015720020000025
其中，
Figure FDF0000015720020000026
为第二状态向量的误差向量，
Figure FDF0000015720020000027
为所述第一状态向量的误差向量，Hu为所述第二状态向量的雅克比矩阵，
Figure FDF0000015720020000028
为被观测向量的残差向量，n为高斯白噪声向量。
10.根据权利要求6、7和9中任一项所述的装置，其特征在于，所述确定单元还用于：
确定所述至少一个位姿向量和/或至少一个路标特征向量的非线性化约束方程；
根据所述至少一个位姿向量和/或至少一个路标特征向量的雅克比矩阵，对所述非线性化约束方程进行线性化，得到所述第一线性化约束方程。
11.一种计算机可读存储介质，其特征在于，包括计算机程序，当所述计算机程序在计算机上运行时，使得所述计算机执行权利要求1至5中任一项所述的处理数据方法。
CN201710868757.6A 2017-09-22 2017-09-22 一种处理数据的方法和装置 Active CN109542093B (zh)

Priority Applications (4)

Application Number Priority Date Filing Date Title
CN201710868757.6A CN109542093B (zh) 2017-09-22 2017-09-22 一种处理数据的方法和装置
PCT/CN2018/106910 WO2019057146A1 (zh) 2017-09-22 2018-09-21 一种处理数据的方法和装置
EP18857990.8A EP3677978B1 (en) 2017-09-22 2018-09-21 Data processing method and device
US16/825,694 US20200219000A1 (en) 2017-09-22 2020-03-20 Data Processing Method And Apparatus

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710868757.6A CN109542093B (zh) 2017-09-22 2017-09-22 一种处理数据的方法和装置

Publications (2)

Publication Number Publication Date
CN109542093A CN109542093A (zh) 2019-03-29
CN109542093B true CN109542093B (zh) 2022-06-07

Family

ID=65811099

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710868757.6A Active CN109542093B (zh) 2017-09-22 2017-09-22 一种处理数据的方法和装置

Country Status (4)

Country Link
US (1) US20200219000A1 (zh)
EP (1) EP3677978B1 (zh)
CN (1) CN109542093B (zh)
WO (1) WO2019057146A1 (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108876854B (zh) * 2018-04-27 2022-03-08 腾讯科技（深圳）有限公司 相机姿态追踪过程的重定位方法、装置、设备及存储介质
CN111238497B (zh) 2018-11-29 2022-05-06 华为技术有限公司 一种高精度地图的构建方法及装置
CN111256689B (zh) * 2020-01-15 2022-01-21 北京智华机器人科技有限公司 一种机器人定位方法、机器人和存储介质

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100601960B1 (ko) * 2004-08-05 2006-07-14 삼성전자주식회사 로봇의 위치 추적 및 지도 작성 방법
CN102402225B (zh) * 2011-11-23 2013-09-04 中国科学院自动化研究所 一种实现移动机器人同时定位与地图构建的方法
CN102706342A (zh) * 2012-05-31 2012-10-03 重庆邮电大学 一种智能移动机器人的定位与环境建模方法
WO2014130854A1 (en) * 2013-02-21 2014-08-28 Regents Of The Univesity Of Minnesota Extrinsic parameter calibration of a vision-aided inertial navigation system
US9607401B2 (en) * 2013-05-08 2017-03-28 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
US20140341465A1 (en) * 2013-05-16 2014-11-20 The Regents Of The University Of California Real-time pose estimation system using inertial and feature measurements
US10306206B2 (en) * 2013-07-23 2019-05-28 The Regents Of The University Of California 3-D motion estimation and online temporal calibration for camera-IMU systems
CN103644903B (zh) * 2013-09-17 2016-06-08 北京工业大学 基于分布式边缘无味粒子滤波的同步定位与地图构建方法
CN103631264A (zh) * 2013-12-04 2014-03-12 苏州大学张家港工业技术研究院 一种同时定位与地图创建方法及装置
US9658070B2 (en) * 2014-07-11 2017-05-23 Regents Of The University Of Minnesota Inverse sliding-window filters for vision-aided inertial navigation systems
CN104574387B (zh) * 2014-12-29 2017-11-21 张家港江苏科技大学产业技术研究院 水下视觉slam系统中的图像处理方法
CN106289181B (zh) * 2015-05-22 2018-12-18 北京雷动云合智能技术有限公司 一种基于视觉测量的实时slam方法
CN106197428B (zh) * 2016-07-10 2019-03-22 北京工业大学 一种利用测量信息优化分布式ekf估计过程的slam方法

Also Published As

Publication number Publication date
WO2019057146A1 (zh) 2019-03-28
US20200219000A1 (en) 2020-07-09
CN109542093A (zh) 2019-03-29
EP3677978A1 (en) 2020-07-08
EP3677978A4 (en) 2020-08-05
EP3677978B1 (en) 2022-04-06

Similar Documents

Publication Publication Date Title
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
US9709404B2 (en) Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation
CN107369166B (zh) 一种基于多分辨率神经网络的目标跟踪方法及系统
Agamennoni et al. An outlier-robust Kalman filter
Indelman et al. Information fusion in navigation systems via factor graph based incremental smoothing
CN109670474B (zh) 一种基于视频的人体姿态估计方法、装置及设备
CN111814719B (zh) 一种基于3d时空图卷积的骨架行为识别方法
CN107735797B (zh) 用于确定第一坐标系与第二坐标系之间的运动的方法
CN109542093B (zh) 一种处理数据的方法和装置
WO2020173044A1 (zh) 无人机路径规划方法、装置、存储介质及计算机设备
WO2020221307A1 (zh) 一种运动物体的追踪方法和装置
CN110874636B (zh) 一种神经网络模型压缩方法、装置和计算机设备
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN109323709B (zh) 视觉里程计方法、装置和计算机可读存储介质
US10115208B2 (en) Image characteristic estimation method and device
CN113066127B (zh) 一种在线标定设备参数的视觉惯性里程计方法和系统
WO2021162018A1 (en) System and method for tracking expanded state of moving object with model geometry learning
CN117367412B (zh) 一种融合捆集调整的紧耦合激光惯导里程计与建图方法
Han et al. Precise positioning with machine learning based Kalman filter using GNSS/IMU measurements from android smartphone
CN113902862A (zh) 一种基于一致性集群的视觉slam回环验证系统
CN114415698B (zh) 机器人、机器人的定位方法、装置和计算机设备
CN115170826A (zh) 基于局部搜索的运动小目标快速光流估计方法及存储介质
CN111898620A (zh) 识别模型的训练方法、字符识别方法、装置、设备和介质
CN111141279A (zh) 行车轨迹的处理方法及装置
Gamage et al. Reduced Dimensionality Extended Kalman Filter for SLAM.

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