CN112445210A - 运动轨迹的确定方法及装置、存储介质、电子装置 - Google Patents

运动轨迹的确定方法及装置、存储介质、电子装置 Download PDF

Info

Publication number
CN112445210A
CN112445210A CN201910755826.1A CN201910755826A CN112445210A CN 112445210 A CN112445210 A CN 112445210A CN 201910755826 A CN201910755826 A CN 201910755826A CN 112445210 A CN112445210 A CN 112445210A
Authority
CN
China
Prior art keywords
determining
track
map
key frame
pose
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.)
Granted
Application number
CN201910755826.1A
Other languages
English (en)
Other versions
CN112445210B (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.)
Ninebot Beijing Technology Co Ltd
Original Assignee
Ninebot Beijing 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 Ninebot Beijing Technology Co Ltd filed Critical Ninebot Beijing Technology Co Ltd
Priority to CN201910755826.1A priority Critical patent/CN112445210B/zh
Publication of CN112445210A publication Critical patent/CN112445210A/zh
Application granted granted Critical
Publication of CN112445210B publication Critical patent/CN112445210B/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/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
    • G05D1/0251—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 extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
    • 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/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • 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/0259—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic means
    • 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/0259—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic means
    • G05D1/0261—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic means using magnetic plots
    • 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/028—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using a RF signal
    • 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

Abstract

本发明提供了一种运动轨迹的确定方法及装置、存储介质、电子装置，其中，上述方法包括：获取通过激光雷达建立的第一地图的多个轨迹节点，以及通过视觉惯性里程计建立的第二地图的多个关键帧；对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿；其中，所述指定关键帧为所述多个关键帧中与所述每一个轨迹节点的时间间隔最小的关键帧；根据所述相对位姿确定机器人的运动轨迹，采用上述技术方案，解决了相关技术中仅根据单个传感器确定机器人的运动轨迹，容易发生估计错误等问题。

Description

运动轨迹的确定方法及装置、存储介质、电子装置
技术领域
本发明涉及通信领域，具体而言，涉及一种运动轨迹的确定方法及装置、存储介质、电子装置。
背景技术
现实中的激光与视觉SLAM系统几乎都会配备惯性元件、轮机里程计、卫星定位系统、室内基站定位系统等辅助定位工具，常见的机器人的运动轨迹估计方案有视觉惯性里程计方案和依靠激光雷达方案，视觉-惯性里程计通过相机采集的图片信息、以及惯性测量单元采集的信息实现对机器人自身运动轨迹的估计、以及建立由空间中三维点云组成的地图；激光雷达方案采用激光点云与局部地图(submap)匹配的方法来生成机器人运动轨迹，以及建立栅格化的二维平面地图。
但是机器人利用依靠不同传感器生成的自身运动轨迹往往难以保持一致，而且上述两种都有难以适用的场景。比如，基于视觉-惯性的方案往往在视觉纹理特征丰富的场景表现优秀，但是在光照少、弱纹理的环境下表现欠佳，而激光雷达方案则不会受到光照变化、纹理变化的影响，但是激光雷达方案在特定环境下，如一些玻璃材质墙壁的室内，则易发生位姿估计错误。
针对相关技术中，仅根据单个传感器确定机器人的运动轨迹，容易发生估计错误等问题，尚未提出有效的技术方案。
发明内容
本发明实施例提供了一种运动轨迹的确定方法及装置、存储介质、电子装置，以至少解决相关技术中仅根据单个传感器确定机器人的运动轨迹，容易发生估计错误等问题。
根据本发明的一个实施例，提供了一种运动轨迹的确定方法，包括：获取通过激光雷达建立的第一地图的多个轨迹节点，以及通过视觉惯性里程计建立的第二地图的多个关键帧；对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿；其中，所述指定关键帧为所述多个关键帧中与所述每一个轨迹节点的时间间隔最小的关键帧；根据所述相对位姿确定机器人的运动轨迹。
在本发明实施例中，对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿，包括：获取每一个轨迹节点和所述指定关键帧的时间间隔；确定所述时间间隔内所述每一个轨迹节点和所述指定关键帧的相对位姿。
在本发明实施例中，确定所述时间间隔内所述每一个轨迹节点和所述指定关键帧的相对位姿，包括：通过所述视觉惯性里程计确定所述时间间隔内所述轨迹节点和所述指定关键帧的相对位姿。
在本发明实施例中，根据所述相对位姿确定机器人的运动轨迹，包括：根据获取的所述指定关键帧和所述相对位姿确定出所述指定关键帧所对应的虚拟轨迹节点；根据所述虚拟轨迹节点的位姿确定所述运动轨迹。
在本发明实施例中，根据所述虚拟轨迹节点的位姿确定所述运动轨迹，包括：将所述虚拟轨迹节点的位姿替代所述第一地图的轨迹节点的位姿，得到处理后的第一地图，以使所述处理后的第一地图和所述第二地图的运动轨迹一致；根据所述处理后第一地图确定所述机器人的运动轨迹。
根据本发明的另一个实施例，还提供了一种运动轨迹的确定装置，包括：获取模块，用于获取通过激光雷达建立的第一地图的多个轨迹节点，以及通过视觉惯性里程计建立的第二地图的多个关键帧；第一确定模块，用于对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿；其中，所述指定关键帧为所述多个关键帧中与所述每一个轨迹节点的时间间隔最小的关键帧；第二确定模块，用于根据所述相对位姿确定机器人的运动轨迹。
在本发明实施例中，所述第一确定模块，用于获取每一个轨迹节点与所述指定关键帧的时间间隔；确定所述时间间隔内所述每一个轨迹节点和所述指定关键帧的相对位姿。
在本发明实施例中，所述第一确定模块，还用于通过所述视觉惯性里程计确定所述时间间隔内所述每一个轨迹节点和所述指定关键帧的相对位姿。
在本发明实施例中，所述第二确定模块，还用于根据获取的所述指定关键帧和所述相对位姿确定出所述指定关键帧所对应的虚拟轨迹节点；根据所述虚拟轨迹节点的位姿确定所述运动轨迹。
在本发明实施例中，所述第二确定模块，还用于将所述虚拟轨迹节点的位姿替代所述第一地图的轨迹节点的位姿，得到处理后的第一地图，以使所述处理后的第一地图和所述第二地图的运动轨迹一致；根据所述处理后第一地图确定所述机器人的运动轨迹。
根据本发明的另一个实施例，还提供了一种存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。
根据本发明的又一个实施例，还提供了一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行上述任一项方法实施例中的步骤。
通过本发明，对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿；根据所述相对位姿确定机器人的运动轨迹，其中，所述指定关键帧为与所述每一个轨迹节点间隔最小时间的关键帧，采用上述技术方案，解决了相关技术中仅根据单个传感器确定机器人的运动轨迹，容易发生估计错误等问题，进而将来自激光雷达建立的第一地图中的轨迹节点和视觉惯性里程计的第二地图的关键帧的关系进行融合，进而确定机器人的运动轨迹，提高了机器人的运动轨迹的准确性。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是本发明实施例的一种运动轨迹的确定方法的机器人的硬件结构框图；
图2是根据本发明实施例的运动轨迹的确定方法的流程图；
图3是根据本发明实施例的运动轨迹的确定装置的结构框图；
图4是根据本发明实施例的视觉-惯性地图的结构描述的示意图；
图5是根据本发明实施例的激光雷达地图的结构描述的示意图；
图6是根据本发明优选实施例的运动轨迹的确定方法的流程示意图。
具体实施方式
下文中将参考附图并结合实施例来详细说明本发明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
为了更好的理解以下本发明实施例以及优选实施例的技术方案，以下对本发明实施例以及优选实施例涉及的名词进行解释：
视觉惯性里程计(visual-inertial odometry)：简称VIO，一般是通过一个或多个摄像机和惯性测量器件估计移动装置运动轨迹和位置的方法。.
紧耦合(tightly-couple):一种多传感器融合的方案，对传感器的信息的利用更加充分。
闭环检测(Loop Closure Detection)：一种用于判断机器人当前位置是否为之前已经经过的位置区域的方法。
轨迹节点(trajectory node)：激光雷达机器人运动轨迹中的某一特定时刻机器人的位姿。
局部地图(submap)：多帧激光点云建立的局部的地图，完整的地图由若干个局部地图组成。
偏置(bias)：IMU传感器信号中的一种噪声的参数。
重投影误差(re-project error)：将3D点投影到相机帧上的像素误差。
航迹推算(dead reckoning)：一种根据轮式里程计或陀螺仪等传感器来计算某一时期内机器人的运动轨迹的方法。
本申请实施例所提供的方法实施例可以在机器人或者类似的运算装置中执行。以运行在机器人上为例，图1是本发明实施例的一种运动轨迹的确定方法的机器人的硬件结构框图。如图1所示，机器人10可以包括一个或多个(图1中仅示出一个)处理器102(处理器102可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置)和用于存储数据的存储器104，可选地，上述机器人还可以包括用于通信功能的传输设备106以及输入输出设备108。本领域普通技术人员可以理解，图1所示的结构仅为示意，其并不对上述机器人的结构造成限定。例如，机器人10还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。
存储器104可用于存储计算机程序，例如，应用软件的软件程序以及模块，如本发明实施例中的运动轨迹的确定方法对应的计算机程序，处理器102通过运行存储在存储器104内的计算机程序，从而执行各种功能应用以及数据处理，即实现上述的方法。存储器104可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器104可进一步包括相对于处理器102远程设置的存储器，这些远程存储器可以通过网络连接至机器人10。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
传输装置106用于经由一个网络接收或者发送数据。上述的网络具体实例可包括机器人10的通信供应商提供的无线网络。在一个实例中，传输装置106包括一个网络适配器(Network Interface Controller，简称为NIC)，其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输装置106可以为射频(Radio Frequency，简称为RF)模块，其用于通过无线方式与互联网进行通讯。
在本实施例中提供了一种运行于上述机器人的运动轨迹的确定方法，图2是根据本发明实施例的运动轨迹的确定方法的流程图，如图2所示，该流程包括如下步骤：
步骤S202，获取通过激光雷达建立的第一地图的多个轨迹节点，以及通过视觉惯性里程计建立的第二地图的多个关键帧；
步骤S204，对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿；其中，所述指定关键帧为所述多个关键帧中与所述每一个轨迹节点的时间间隔最小的关键帧；
步骤S206，根据所述相对位姿确定机器人的运动轨迹。
通过本发明，按照预定规则确定获取的每一个轨迹节点和关键帧的相对位姿；根据所述相对位姿确定机器人的运动轨迹，采用上述技术方案，解决了相关技术中仅根据单个传感器确定机器人的运动轨迹，容易发生估计错误等问题，进而将来自激光雷达建立的第一地图中的轨迹节点和视觉惯性里程计的第二地图的关键帧的关系进行融合，进而确定机器人的运动轨迹，提高了机器人的运动轨迹的准确性。
需要说明的是，每一个轨迹节点都对应一个指定关键帧，最终确定的相对位姿为多个。
上述步骤S204可以有多种实现方式，在一个可选实施例中，可以通过以下方式实现：获取每一个轨迹节点与指定关键帧的时间间隔；确定所述时间间隔内所述每一个轨迹节点和所述指定关键帧的相对位姿，进而通过所述视觉惯性里程计确定所述时间间隔内所述每一个轨迹节点和所述指定关键帧的相对位姿，基于该方法确定的相对位姿，根据获取的所述指定关键帧和所述相对位姿确定出所述指定关键帧所对应的虚拟轨迹节点；根据所述虚拟轨迹节点的位姿确定所述运动轨迹，其中，根据所述虚拟轨迹节点的位姿确定所述运动轨迹，可以通过以下技术方案实现：将所述虚拟轨迹节点的位姿替代所述第一地图的轨迹节点的位姿，得到处理后的第一地图，以使所述处理后的第一地图和所述第二地图的运动轨迹一致；根据所述处理后第一地图确定所述机器人的运动轨迹。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网络设备等)执行本发明各个实施例的方法。
在本实施例中还提供了一种运动轨迹的确定装置，该装置用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。
图3是根据本发明实施例的运动轨迹的确定装置的结构框图，如图3所示，该装置包括：
获取模块30，用于获取通过激光雷达建立的第一地图的多个轨迹节点，以及通过视觉惯性里程计建立的第二地图的多个关键帧；
第一确定模块32，用于对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿；其中，所述指定关键帧为所述多个关键帧中与所述每一个轨迹节点的时间间隔最小的关键帧；
第二确定模块34，用于根据所述相对位姿确定机器人的运动轨迹。
通过本发明，按照预定规则确定获取的每一个轨迹节点和指定关键帧的相对位姿；根据所述相对位姿确定机器人的运动轨迹，采用上述技术方案，解决了相关技术中仅根据单个传感器确定机器人的运动轨迹，容易发生估计错误等问题，进而将来自激光雷达建立的第一地图中的轨迹节点和视觉惯性里程计的第二地图的关键帧的关系进行融合，进而确定机器人的运动轨迹，提高了机器人的运动轨迹的准确性。
可选地，所述第一确定模块32，用于获取每一个轨迹节点与每一个轨迹节点对应的关键帧的时间间隔；确定所述时间间隔内所述轨迹节点和指定关键帧的相对位姿，其中，所述第一确定模块32，还用于通过所述视觉惯性里程计确定所述时间间隔内所述每一个轨迹节点和指定关键帧的相对位姿。
在本发明实施例中，所述第二确定模块34，还用于根据获取的所述指定关键帧和所述相对位姿确定出所述指定关键帧所对应的虚拟轨迹节点；根据所述虚拟轨迹节点的位姿确定所述运动轨迹，进一步地，第二确定模块34，还用于将所述虚拟轨迹节点的位姿替代所述第一地图的轨迹节点的位姿，得到处理后的第一地图，以使所述处理后的第一地图和所述第二地图的运动轨迹一致；根据所述处理后第一地图确定所述机器人的运动轨迹。
需要说明的是，上述各个模块是可以通过软件或硬件来实现的，对于后者，可以通过以下方式实现，但不限于此：上述模块均位于同一处理器中；或者，上述各个模块以任意组合的形式分别位于不同的处理器中。
为了更好的理解上述运动轨迹的确定流程，以下结合优选实施例进行说明，但不用于限定本发明实施例的技术方案，具体可包括以下步骤：
在本发明优选实施例中，视觉-惯性地图的结构描述如图4所示，包含：
1)相机关键帧；其中，视觉关键帧可以理解为是视觉里程计或视觉-惯性里程计在估计机器人运动轨迹时选取的特定时刻的一帧图片，通常用关键帧的位姿来记录机器人的位姿；
2)三维空间中的三维点云；三维点云可以理解为是二维图像中的特征点投影到三维空间中的具有6自由度信息的点的集合，能够记录机器人所处环境的特征点的位置；
3)关键帧之间的约束，包括有相邻两关键帧之间的惯性测量单元(IMU)的信息，可选地，该信息至少包括以下之一：陀螺仪测量值经过一次积分得到的角度差，以及加速度计测量值分别经过一次积分和两次积分得到的速度与位移，以及轮式里程计测量的信息，可选地，该信息至少包括：通过航迹推算得到的位移，以及闭环检测的约束(LoopClosureDetection)，其中，闭环检测的主要过程如下：机器人判断自己当前位置是否位于已经访问过的环境区域，并以此作为地图是否需要更改的依据)。
此外，激光雷达地图的结构由位姿轨迹(Trajectory)和栅格地图两部分组成，如图5所示，其中：
1)运动轨迹由大量轨迹节点(Trajectory node)组成，每个节点记录该时刻机器人位姿；
2)栅格地图由许多局部地图(submap)组成。
除上述结构外，如图5所示，地图中的轨迹节点与局部地图之间存在相对位姿约束，节点与节点之间存在轮式里程计的相对位姿约束(图5中未示出)。
进一步地，基于上述技术方案，轨迹节点(trajectory node)与关键帧(keyframe)的关联方案如下：
为了解决视觉-惯性地图和激光雷达建立的地图在建图过程中，难以保证关键帧的采样时刻和轨迹节点的采样时刻保持一致，无法直接进行关联的问题，本发明优选实施例中，首先根据激光雷达地图中的每一个轨迹节点，找到轨迹节点时间上最邻近的视觉-惯性地图中的关键帧，如图6所示，求出轨迹节点与关键帧间隔时间delta_t。然后使用轮式里程计测量的信息，计算出delta_t时间内的轨迹节点与其联系的关键帧之间的相对位姿，从而构成轨迹节点(trajectory node)与关键帧(keyframe)的关联。
进一步地，最后的紧耦合优化中，使用最小二乘法(数学优化技术，使得求得的数据和实际数据之间的误差的平方和为最小)来对关联在一起的两个地图进行优化，轨迹节点与关键帧之间的相对位姿设为常量，不参与优化，将关键帧优化后的位姿作为机器人的最终估计位姿。以保证最终轨迹的一致性。
本发明实施例中，上述构建的最小二乘法进行优化的过程中采用的变量包括：1)视觉-惯性地图的相机关键帧的位姿；2)IMU测得的偏置(bias)；3)特征点的位置，位置可以通过坐标信息来体现，具体地，偏置的大小和特征点的坐标信息可以通过上述第二地图获取，进而将上述三个变量作为最小二乘法的输入，得到优化后的相机关键帧的位姿、偏置、特征点的位置。
优化问题的残差约束为：1)视觉里程计的重投影误差约束，2)IMU的积分误差约束，3)轮式里程计的航迹推算得到的位姿误差约束，4)回环检测得到的两关键帧之间的约束，5)以关键帧位姿推算出的虚拟轨迹节点在激光雷达地图中的约束。
其中，以关键帧位姿推算出的虚拟轨迹节点在激光雷达地图中的约束的具体确定步骤为：通过相机关键帧的位姿，以及上述相对位姿确定出该关键帧对应的虚拟轨迹节点的位姿(可选地，可以是关键帧的位姿与相对位姿相乘，得到虚拟轨迹节点的位姿)，由于视觉-惯性地图和激光雷达地图的轨迹会存在不一致的问题，进而导致由相机关键帧推算的虚拟轨迹节点位姿与激光地图中的轨迹节点的位姿是不一致的，本发明优选实施例中，使用计算出的虚拟轨迹节点的位姿来替代原始的激光雷达地图中的轨迹节点来计算约束，保证了两种地图轨迹的一致性，也将两类地图各自独立的优化问题整合为一个总的优化问题。
紧耦合优化过程中通过梯度下降等方法，能求解出相机关键帧位姿、特征点、IMU传感器的偏置，最后根据关键帧的位姿，还原出激光地图中的机器人的各个局部地图(submap)的位置，重构出新的与视觉-惯性地图相一致的激光地图。
本发明的实施例还提供了一种存储介质，该存储介质包括存储的程序，其中，上述程序运行时执行上述任一项的方法。
可选地，在本实施例中，上述存储介质可以被设置为存储用于执行以下步骤的程序代码：
S1，获取通过激光雷达建立的第一地图的多个轨迹节点，以及通过视觉惯性里程计建立的第二地图的多个关键帧；
S2，对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿；其中，所述指定关键帧为所述多个关键帧中与所述每一个轨迹节点的时间间隔最小的关键帧；
S3，根据所述相对位姿确定机器人的运动轨迹。
本发明的实施例还提供了一种存储介质，该存储介质包括存储的程序，其中，上述程序运行时执行上述任一项的方法。
本发明的实施例还提供了一种电子装置，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。
可选地，上述电子装置还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。
可选地，在本实施例中，上述处理器可以被设置为通过计算机程序执行以下步骤：
S1，获取通过激光雷达建立的第一地图的多个轨迹节点，以及通过视觉惯性里程计建立的第二地图的多个关键帧；
S2，对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿；其中，所述指定关键帧为所述多个关键帧中与所述每一个轨迹节点的时间间隔最小的关键帧；
S3，根据所述相对位姿确定机器人的运动轨迹。
可选地，在本实施例中，上述存储介质可以包括但不限于：U盘、只读存储器(Read-Only Memory，简称为ROM)、随机存取存储器(Random Access Memory，简称为RAM)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
可选地，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。
显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种运动轨迹的确定方法，其特征在于，包括：
获取通过激光雷达建立的第一地图的多个轨迹节点，以及通过视觉惯性里程计建立的第二地图的多个关键帧；
对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿；其中，所述指定关键帧为所述多个关键帧中与所述每一个轨迹节点的时间间隔最小的关键帧；
根据所述相对位姿确定机器人的运动轨迹。
2.根据权利要求1所述的方法，其特征在于，对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿，包括：
获取每一个轨迹节点和所述指定关键帧的时间间隔；
确定所述时间间隔内所述每一个轨迹节点和所述指定关键帧的相对位姿。
3.根据权利要求1所述的方法，其特征在于，根据所述相对位姿确定机器人的运动轨迹，包括：
根据获取的所述指定关键帧和所述相对位姿确定出所述指定关键帧所对应的虚拟轨迹节点；
根据所述虚拟轨迹节点的位姿确定所述运动轨迹。
4.根据权利要求3所述的方法，其特征在于，根据所述虚拟轨迹节点的位姿确定所述运动轨迹，包括：
将所述虚拟轨迹节点的位姿替代所述第一地图的轨迹节点的位姿，得到处理后的第一地图，以使所述处理后的第一地图和所述第二地图的运动轨迹一致；
根据所述处理后第一地图确定所述机器人的运动轨迹。
5.一种运动轨迹的确定装置，其特征在于，包括：
获取模块，用于获取通过激光雷达建立的第一地图的多个轨迹节点，以及通过视觉惯性里程计建立的第二地图的多个关键帧；
第一确定模块，用于对于所述多个轨迹节点中的每个轨迹节点，按照预定规则确定所述每一个轨迹节点和指定关键帧的相对位姿；其中，所述指定关键帧为所述多个关键帧中与所述每一个轨迹节点的时间间隔最小的关键帧；
第二确定模块，用于根据所述相对位姿确定机器人的运动轨迹。
6.根据权利要求5所述的装置，其特征在于，所述第一确定模块，用于获取每一个轨迹节点与所述指定关键帧的时间间隔；确定所述时间间隔内所述每一个轨迹节点和所述指定关键帧的相对位姿。
7.根据权利要求5所述的装置，其特征在于，所述第二确定模块，还用于根据获取的所述指定关键帧和所述相对位姿确定出所述指定关键帧所对应的虚拟轨迹节点；根据所述虚拟轨迹节点的位姿确定所述运动轨迹。
8.根据权利要求7所述的装置，其特征在于，所述第二确定模块，还用于将所述虚拟轨迹节点的位姿替代所述第一地图的轨迹节点的位姿，得到处理后的第一地图，以使所述处理后的第一地图和所述第二地图的运动轨迹一致；根据所述处理后第一地图确定所述机器人的运动轨迹。
9.一种存储介质，其特征在于，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行所述权利要求1至4任一项中所述的方法。
10.一种电子装置，包括存储器和处理器，其特征在于，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行所述权利要求1至4任一项中所述的方法。
CN201910755826.1A 2019-08-15 2019-08-15 运动轨迹的确定方法及装置、存储介质、电子装置 Active CN112445210B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910755826.1A CN112445210B (zh) 2019-08-15 2019-08-15 运动轨迹的确定方法及装置、存储介质、电子装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910755826.1A CN112445210B (zh) 2019-08-15 2019-08-15 运动轨迹的确定方法及装置、存储介质、电子装置

Publications (2)

Publication Number Publication Date
CN112445210A true CN112445210A (zh) 2021-03-05
CN112445210B CN112445210B (zh) 2023-10-27

Family

ID=74741422

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910755826.1A Active CN112445210B (zh) 2019-08-15 2019-08-15 运动轨迹的确定方法及装置、存储介质、电子装置

Country Status (1)

Country Link
CN (1) CN112445210B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140333741A1 (en) * 2013-05-08 2014-11-13 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
CN108319976A (zh) * 2018-01-25 2018-07-24 北京三快在线科技有限公司 建图方法及装置
CN109084732A (zh) * 2018-06-29 2018-12-25 北京旷视科技有限公司 定位与导航方法、装置及处理设备
CN109166140A (zh) * 2018-07-27 2019-01-08 长安大学 一种基于多线激光雷达的车辆运动轨迹估计方法及系统
CN109816696A (zh) * 2019-02-01 2019-05-28 西安全志科技有限公司 一种机器人定位与建图方法、计算机装置及计算机可读存储介质
CN109887057A (zh) * 2019-01-30 2019-06-14 杭州飞步科技有限公司 生成高精度地图的方法和装置

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140333741A1 (en) * 2013-05-08 2014-11-13 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
CN108319976A (zh) * 2018-01-25 2018-07-24 北京三快在线科技有限公司 建图方法及装置
CN109084732A (zh) * 2018-06-29 2018-12-25 北京旷视科技有限公司 定位与导航方法、装置及处理设备
CN109166140A (zh) * 2018-07-27 2019-01-08 长安大学 一种基于多线激光雷达的车辆运动轨迹估计方法及系统
CN109887057A (zh) * 2019-01-30 2019-06-14 杭州飞步科技有限公司 生成高精度地图的方法和装置
CN109816696A (zh) * 2019-02-01 2019-05-28 西安全志科技有限公司 一种机器人定位与建图方法、计算机装置及计算机可读存储介质

Also Published As

Publication number Publication date
CN112445210B (zh) 2023-10-27

Similar Documents

Publication Publication Date Title
CN109084732B (zh) 定位与导航方法、装置及处理设备
CN108717710B (zh) 室内环境下的定位方法、装置及系统
US11313684B2 (en) Collaborative navigation and mapping
US8259994B1 (en) Using image and laser constraints to obtain consistent and improved pose estimates in vehicle pose databases
WO2020253260A1 (zh) 时间同步处理方法、电子设备及存储介质
US20210183100A1 (en) Data processing method and apparatus
CN103874193A (zh) 一种移动终端定位的方法及系统
CN102607532B (zh) 一种利用飞控数据的低空影像快速匹配方法
CN111380515B (zh) 定位方法及装置、存储介质、电子装置
CN109767470B (zh) 一种跟踪系统初始化方法及终端设备
KR101985344B1 (ko) 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
CN113870379A (zh) 地图生成方法、装置、电子设备及计算机可读存储介质
CN111220155A (zh) 基于双目视觉惯性里程计估计位姿的方法、装置与处理器
CN109040525B (zh) 图像处理方法、装置、计算机可读介质及电子设备
CN112950710A (zh) 位姿确定方法、装置、电子设备和计算机可读存储介质
CN113587934A (zh) 一种机器人、室内定位方法、装置和可读存储介质
CN112991440B (zh) 车辆的定位方法和装置、存储介质和电子装置
CN110749308B (zh) 使用消费级gps和2.5d建筑物模型的面向slam的室外定位方法
CN115355901A (zh) 一种融合动态目标感知的多机联合建图方法
CN115371673A (zh) 一种未知环境中基于Bundle Adjustment的双目相机目标定位方法
CN113610702B (zh) 一种建图方法、装置、电子设备及存储介质
CN112233149A (zh) 场景流的确定方法及装置、存储介质、电子装置
CN116958452A (zh) 三维重建方法和系统
CN113129422A (zh) 一种三维模型构建方法、装置、存储介质和计算机设备
CN112445210B (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
GR01 Patent grant
GR01 Patent grant