CN113470121A - 自主移动平台、外参优化方法、装置及存储介质 - Google Patents

自主移动平台、外参优化方法、装置及存储介质 Download PDF

Info

Publication number
CN113470121A
CN113470121A CN202111040371.9A CN202111040371A CN113470121A CN 113470121 A CN113470121 A CN 113470121A CN 202111040371 A CN202111040371 A CN 202111040371A CN 113470121 A CN113470121 A CN 113470121A
Authority
CN
China
Prior art keywords
pose
camera
autonomous mobile
mobile platform
increment
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
CN202111040371.9A
Other languages
English (en)
Other versions
CN113470121B (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.)
Shenzhen Pudu Technology Co Ltd
Original Assignee
Shenzhen Pudu 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 Shenzhen Pudu Technology Co Ltd filed Critical Shenzhen Pudu Technology Co Ltd
Priority to CN202111040371.9A priority Critical patent/CN113470121B/zh
Publication of CN113470121A publication Critical patent/CN113470121A/zh
Application granted granted Critical
Publication of CN113470121B publication Critical patent/CN113470121B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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/20—Instruments for performing navigational calculations
    • 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
    • G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
    • 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/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Studio Devices (AREA)

Abstract

本发明涉及人工智能领域，提供一种自主移动平台、装置、外参优化方法及可读存储介质。本发明中，自主移动平台在预先部署好的标定场景中运行时同步录制相机所拍摄的标定板的图像、第一传感器的传感数据、第二传感器的传感数据，将录制的图像/数据导入相关的算法中进行标定运算，首先会进行初步标定得到第一传感器与相机之间的第一外参以及第二传感器与相机之间的第二外参之后，然后根据录制数据分别构建针对第一外参、第二外参、相机位姿的约束残差以及视觉重投影误差以进行优化标定，这种优化标定方法可以将多传感器的外参一次性同步算出，并非传统的解耦解算，而是耦合到一起优化，在减少标定次数的同时还可以做到毫米级标定精度。

Description

自主移动平台、外参优化方法、装置及存储介质
技术领域
本发明涉及人工智能领域，尤其涉及一种自主移动平台、外参优化方法、装置及存储介质。
背景技术
随着人工智能技术的爆发，各种可自动行驶的自主移动平台的研发与落地也得到了广泛的关注。定位技术作为这类自主移动平台的核心技术，其相关的学术研究与实践落地也得到了广泛的研究与挖掘。
近年来，基于视觉的室内定位技术取得了较大的进展。视觉定位技术由于其采用的相机传感器价格相对低廉，采集的数据信息量大可以较为鲁棒的完成感知、重定位等工作需求的原因，迅速得到学术界和产业界的广泛研究与运用。但是基于纯视觉的定位方案也由于视觉定位天然的会受到光照变化、运动模糊、遮挡等不可抗拒因素的影响，暂时还无法达到激光定位方案的工作鲁棒性，所以基于视觉的传感器融合定位技术就应时而生。
基于视觉的多传感器融合定位算法一般需要提前通过离线标定的方式获得各传感器之间的外参关系，因此多传感器的离线外参标定是一个非常重要的环节，如何以最少的次数并且精准地标定出多个传感器之间的外参是值得研究的课题。
发明内容
本发明实施例所要解决的技术问题为如何以最少的次数并且精准地标定出多个传感器之间的外参。
第一方面，本发明实施例提供了一种自主移动平台，所述自主移动平台包括相机、第一传感器、第二传感器、存储器和处理器；所述相机用于在所述自主移动平台行进过程中实时拍摄预先部署的标定板的多帧图像，所述标定板上具有标定图案；所述存储器存储有可执行程序代码；所述处理器用于调用并运行所述可执行程序代码来实现自主移动平台外参优化方法；所述自主移动平台外参优化方法包括：
通过计算所述相机所拍摄的多帧图像以及所述第一传感器所获取的多个第一传感数据、所述第二传感器的多个第二传感数据，得到所述第一传感器与所述相机之间的第一外参，以及所述第二传感器与所述相机之间的第二外参；
根据所述第一外参、多帧图像、多个第一传感器数据计算所述第一传感器相对于世界坐标系下所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第一位姿增量；
根据所述第二外参、多帧图像、多个第二传感器数据，计算所述第二传感器相对于世界坐标系下所述相机拍摄的所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第二位姿增量；
基于所述多帧图像计算与多帧图像对应的多个位姿信息；
以所述第一位姿增量、第二位姿增量为观测量，以所述第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下所述多帧图像之间的位姿增量和所述第一位姿增量之间的第一约束残差、世界坐标系下所述多帧图像之间位姿增量和所述第二位姿增量之间的第二约束残差，并根据所述视觉重投影误差、所述第一约束残差、所述第二约束残差建立最小二乘法问题以对所述第一外参和所述第二外参进行优化，得到优化后的第一外参和第二外参。
第二方面，本发明实施例还提供了一种自主移动平台外参优化装置，所述自主移动平台外参优化装置应用于自主移动平台中，所述自主移动平台包括相机、第一传感器、第二传感器，所述相机用于在所述自主移动平台行进过程中实时拍摄预先部署的标定板的图像，所述标定板上具有标定图案；所述自主移动平台外参优化装置包括：
外参标定模块，通过计算所述相机所拍摄的多帧图像以及所述第一传感器所获取的多个第一传感数据、所述第二传感器的多个第二传感数据，得到所述第一传感器与所述相机之间的第一外参，以及所述第二传感器与所述相机之间的第二外参；
第一位姿增量计算模块，根据所述第一外参、多帧图像、多个第一传感器数据计算所述第一传感器相对于世界坐标系下所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第一位姿增量；
第二位姿增量计算模块，根据所述第二外参、多帧图像、多个第二传感器数据，计算所述第二传感器相对于世界坐标系下所述相机拍摄的所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第二位姿增量；
位姿信息计算模块，用于基于所述多帧图像计算与多帧图像对应的多个位姿信息；
残差构建及优化模块，用于以所述第一位姿增量、第二位姿增量为观测量，以所述第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下所述多帧图像之间的位姿增量和所述第一位姿增量之间的第一约束残差、世界坐标系下所述多帧图像之间位姿增量和所述第二位姿增量之间的第二约束残差，并根据所述视觉重投影误差、所述第一约束残差、所述第二约束残差建立最小二乘法问题以对所述第一外参和所述第二外参进行优化，得到优化后的第一外参和第二外参。
第三方面，本发明实施例还提供了一种自主移动平台外参优化方法，所述自主移动平台外参优化方法应用于自主移动平台中，所述自主移动平台包括相机、第一传感器、第二传感器，所述相机用于在所述自主移动平台行进过程中实时拍摄预先部署的标定板的图像，所述标定板上具有标定图案；所述自主移动平台外参优化方法包括：
通过计算所述相机所拍摄的多帧图像以及所述第一传感器所获取的多个第一传感数据、所述第二传感器的多个第二传感数据，得到所述第一传感器与所述相机之间的第一外参，以及所述第二传感器与所述相机之间的第二外参；
根据所述第一外参、多帧图像、多个第一传感器数据计算所述第一传感器相对于世界坐标系下所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第一位姿增量；
根据所述第二外参、多帧图像、多个第二传感器数据，计算所述第二传感器相对于世界坐标系下所述相机拍摄的所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第二位姿增量；
基于所述多帧图像计算与多帧图像对应的多个位姿信息；
以所述第一位姿增量、第二位姿增量为观测量，以所述第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下所述多帧图像之间的位姿增量和所述第一位姿增量之间的第一约束残差、世界坐标系下所述多帧图像之间位姿增量和所述第二位姿增量之间的第二约束残差，并根据所述视觉重投影误差、所述第一约束残差、所述第二约束残差建立最小二乘法问题以对所述第一外参和所述第二外参进行优化，得到优化后的第一外参和第二外参。
第四方面，本发明实施例还提供一种可读存储介质，其上存储有计算机程序，所述计算机程序在被处理器执行时实现第三方面所述的约束残差的构建方法。
本发明各实施例中，自主移动平台在预先部署好的标定场景中运行时同步录制相机所拍摄的标定板的图像、第一传感器的传感数据、第二传感器的传感数据，将录制的图像/数据导入相关的算法中进行标定运算，首先会进行初步标定得到第一传感器与相机之间的第一外参以及第二传感器与相机之间的第二外参，然后进一步计算得到第一传感器相对于所有图像帧中相邻图像帧的位姿变化并作为第一位姿增量，并进一步计算得到第二传感器相对于所有图像帧中相邻图像帧的位姿变化并作为第二位姿增量，然后根据上述计算得到的第一外参、第二外参以及多个位姿信息分别构建针对第一外参、第二外参、相机位姿的第一约束残差、第二约束参数和视觉重投影误差，以对第一外参、第二外参、相机位姿进行优化标定，这种优化标定方法可以将多传感器的外参一次性同步算出，并非传统的解耦解算，而是耦合到一起优化，在减少标定次数的同时还可以做到毫米级标定精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明第一实施例提供的自主移动平台的结构示意图；
图2为本发明第二实施例提供的约束残差的构建方法的实现流程图；
图3为本发明第三实施例提供的自主移动平台外参优化装置的模块结构图；
图4为本发明第三实施例提供的残差构建及优化模块的结构图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
参见图1，本发明第一实施例提供的自主移动平台的结构示意图。为了便于说明，仅示出了与本发明实施例相关的部分。该自主移动平台具有自动行进的能力，可以是机器人，例如酒店/餐厅等场所内的迎宾机器人，在酒店、物流园区等固定场所内运行的送货机器人、扫地机器人等，甚至是无人驾驶的汽车等，只要是有自动行进能力且需要对内置的各种传感器进行标定的自主移动平台均可适用。该自主移动平台可包括：
存储器10、处理器20、相机30、第一传感器40、第二传感器50。存储器10中存储有可执行程序代码，可以采用硬盘驱动存储器、非易失性存储器（例如闪存或用于形成固态驱动器的其它电子可编程限制删除的存储器等）、易失性存储器（例如静态或动态随机存取存储器等）等，本发明实施例不作限制。
相机30一般为离线的相机，主要功能为拍摄，用于在自主移动平台行进过程中实时拍摄预先部署的标定板的图像，该标定板上具有标定图案。
第一传感器40和第二传感器50可以是IMU（Inertial Measurement Unit，惯性传感器）、轮式里程计、毫米波雷达、激光雷达等等。
处理器20为自主移动平台的运算和控制核心，是信息处理、程序运行的最终执行单元，例如可以是CPU（Central Processing Unit）、MCU（Microcontroller Unit）等具有数据处理能力的硬件单元。处理器20与存储器10耦合/连接，用于调用存储器10中存储的所述可执行程序代码，执行如下约束残差的构建方法，此方法主要用于一次性构建出相机30与第一传感器40之间的外参的约束残差，以及相机30与第二传感器50之间的外参的约束残差，该约束残差可以对对应的外参进一步优化标定。
参见图2，该自主移动平台的外参优化方法包括如下步骤：
步骤S201，通过计算相机30所拍摄的多帧图像以及第一传感器40所获取的多个第一传感数据、第二传感器50所获取的多个第二传感数据，得到第一传感器40与相机30之间的第一外参，以及第二传感器50与相机30之间的第二外参。
本步骤要求自主移动平台在预先布置的标定场景中按指定方式行进，该标定场景具体可这样布置：搭建一个拱形坡面供自主移动平台行进，该拱形坡面具备一定的长度，例如坡面总长约10米，坡面附近布置有标定板，标定板上具有标定图案。
标定场景中之所以将自主移动平台的行进路线设计为具有一定坡度而不是平面，主要是为了能标定出一些传感器的6自由度外参，例如轮式里程计和相机30之间外参，如果不走拱形路线，只能标定3自由度外参，即x、y方向外参和旋转外参，无法标定平移的z方向外参，因此需要设计一个有坡度的路线来体现出自主移动平台在z坐标方向上的变化。
根据标定图案的不同，标定板一般可选用实心圆阵列图案的标定板、国际象棋盘格图案的标定板、以及二维码图案的AprilTag标定板。
标定板的安装位置和朝向是可以动态调整的，主要是考虑到不同的自主移动平台上的相机的朝向不一样，例如有的相机为朝前设计，有的相机为朝后设计。一旦标定板固定后，需要保证标定板上的标定图案持续在相机30的视野内清晰可识别即可。
另外，标定板的数量也不限，一个标定板或多个标定板均可。当只设置一个标定板时，需要保证该标定板上的标定图案持续被相机30拍摄到，该标定图案在不同时刻的图像帧中表现出的角度不同，该角度可以反映出自主移动平台在每一个时刻所处的位置。而当设置有多个标定板时，由于不同的标定板具有不同的标识，相机30当前时刻所拍摄到的标定板也可以反映出自主移动平台在该时刻所处的位置。
场景布置好以后，自主移动平台按照引导轨迹通过整个标定场景，该轨迹的运动要求是：自主移动平台有充分的旋转运动，例如可以以“8”字形的方案来回穿越整个标定区，并且“8”字形轨迹必须涵盖上坡、下坡。此处，“充分的旋转运动”目的是改变自主移动平台的朝向，并且不能原地旋转，必须同时移动+旋转，旋转运动不充分将会严格干扰标定效果，而“8”字形就是一种典型的非常好的移动中带有旋转的运动方式。
具体需要自主移动平台按照上述方式在拱形坡面的标定区来回行进一遍即可。行进过程中，需要同步录制相机30拍摄的若干图像（图像中包含有标定板上标定图案）、第一传感器40的传感数据、第二传感器50的传感数据。
在标定区行进结束后，将录制的这些信息输入到算法模型里进行初步标定，得到所述第一传感器与所述相机之间的第一外参，以及所述第二传感器与所述相机之间的第二外参，具体可以采用开源的标定算法进行标定，如Kalibr算法可以标定相机30和IMU，从而得到IMU和相机30之间的初步外参，旷视算法可以标定相机30和轮式里程计，从而得到轮式里程计和相机30之间的初步外参。然后再执行下述步骤S202-S206中的构建约束残差、优化标定等过程，最终可以将第一传感器40、第二传感器50的外参一次性同步优化，从而降低系统残差。
步骤S202，根据第一外参、多帧图像、多个第一传感器数据，计算第一传感器40相对于世界坐标系下相机30拍摄的所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第一位姿增量。
本步骤可采用SLAM（simultaneous localization and mapping，同步定位与建图）算法，以vins-mono算法为例，原理是从相机30所拍摄的图像中提取特征点，利用光流追踪的方式建立各帧图像间特征点之间的关联关系，构建完关联关系之后，再利用关联关系构建“重投影残差”，第一传感器40（如IMU）作积分预测自主移动平台的运动，并且构建“运动估计残差”，“运动估计残差”指的是传感器预测的自主移动平台运动位姿与需要优化求解自主移动平台运动位姿之间的残差，具体包括旋转残差和平移残差。这两个残差联合优化得到自主移动平台在各帧图像时刻下的运动位姿Tbi。然后计算在图像紧邻两帧之间自主移动平台运动的相对位姿，作为第一传感器40（如IMU）的第一运动位姿增量∆Tbi。
步骤S203，根据第二外参、多帧图像、多个第二传感器数据，计算第二传感器50相对于世界坐标系下相机30拍摄的所有图像帧相邻图像帧的位姿变化，并将计算结果作为第二位姿增量。
与步骤S202类似，本步骤S203中同样可以采用SLAM算法来计算第二位姿增量。具体地，从相机30所拍摄的图像中提取特征点，利用光流追踪的方式建立各帧图像间特征点之间的关联关系，构建完关联关系之后，再利用关联关系构建“重投影残差”，第二传感器50（如轮式里程计）作积分预测自主移动平台的运动，并且构建“运动估计残差”。这两个残差联合优化得到自主移动平台在各帧图像时刻下的运动位姿Toi。然后计算在图像紧邻两帧之间自主移动平台运动的相对位姿，作为第二传感器50（如轮式里程计）的第二运动位姿增量∆Toi。
步骤S204，基于多帧图像计算与多帧图像对应的多个位姿信息。
可选地，利用多帧图像中标定图案上四个角点数据，可以通过PNP算法求出机器人在各个图像时刻下的位姿信息Tci。
需要说明的是，步骤S202、步骤S203、步骤S204的执行顺序不限于图2中所示的顺序，也可以是其他的执行顺序，例如S203-S202-S204或S202-S204-S203或S204-S202-S203等，还可以是同时执行这三个步骤。
步骤S205，以第一位姿增量、第二位姿增量为观测量，以第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下多帧图像之间的位姿增量和第一位姿增量之间的第一约束残差、世界坐标系下多帧图像之间的位姿增量和第二位姿增量之间的第二约束残差，并根据视觉重投影误差、第一约束残差、第二约束残差建立最小二乘法问题以对第一外参和第二外参进行优化，得到优化后的第一外参和第二外参。
具体地，根据第一外参、第一位姿增量以及自主移动平台在相机30在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化第一外参的第一约束残差。
根据第二外参、第二位姿增量以及自主移动平台在相机30在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差。
根据自主移动平台在相机30拍摄各时刻的初步位姿、相机30拍摄的标定图案的3D像素数据以及标定图案的角点数据，构建用于实时优化相机30位姿的视觉重投影误差。
第一约束残差、第二约束残差和视觉重投影误差这三个残差块构成了总残差，可以表示为：e_all=eb+eo+ec，其中，e_all表示总残差，eb表示第一约束残差，eo表示第二约束残差，ec表示视觉重投影误差。
具体到第一约束残差eb，还需要将第一外参Tcb进行逆变换，得到第一外参逆矩阵(Tcb)-1，然后再根据第一外参Tcb、第一位姿增量∆Tbi、第一外参逆矩阵(Tcb)-1以及自主移动平台在相机30在各时刻拍摄的标定图案之间的位姿变化量∆Tci，构建用于优化第一外参Tcb的第一约束残差eb。具体地，可采用下述公式构建第一约束残差eb：eb=Tcb*∆Tbi*(Tcb)-1-∆Tci。
其中，∆Tci= Tci-1 * (Tci)-1，具体可通过如下方法得到：提取相机30拍摄的标定图案，得到所有标定图案的角点数据，利用图像角点信息通过PNP算法求出自主移动平台在各个图像时刻下的初步位姿Tci，然后计算相邻各帧图像时刻间的位姿∆Tci，这里的i表示第i个时刻。
类似地，具体到第二约束残差eo，将第二外参Tco进行逆变换，得到第二外参逆矩阵(Tco)-1，然后再根据第二外参Tco、第二位姿增量∆Toi、第二外参逆矩阵(Tco)-1以及自主移动平台在相机30在各时刻拍摄的标定图案之间的位姿变化量∆Tci，构建用于优化第二外参Tco的第二约束残差eo。具体地，可采用下述公式构建第二约束残差eo：eo=Tco*∆Toi *(Tco)-1-∆Tci。
具体到视觉重投影误差ec，首先提取相机30拍摄的所有标定图案的3D像素数据Pw，再提取相30机拍摄的所有标定图案的4个角点数据，然后根据下述公式构建用于实时优化相机30位姿的视觉重投影误差：
ec=Tci*Pw-Puv
其中，ec表示视觉重投影误差，Tci表示所述自主移动平台在所述相机拍摄各时刻的初步位姿，Pw表示所述相机拍摄的标定图案的3D像素数据，Puv表示所述相机拍摄的标定图案的4个角点数据。
上述三个残差都得到之后，可以构建非线性最小二乘问题；以图优化求解算法求解非线性最小二乘问题，将非线性最小二乘问题的最优解作为优化后的第一外参、第二外参和相机位姿，具体可以通过图优化理论，用Levenberg-Marquardt算法或高斯牛顿法来进行优化，实现降低系统残差，优化过程不再赘述。
第一实施例中，自主移动平台在预先部署好的标定场景中运行时同步录制相机所拍摄的标定板的图像、第一传感器的传感数据、第二传感器的传感数据，将录制的图像/数据导入相关的算法中进行标定运算，首先会进行初步标定得到第一传感器与相机之间的第一外参以及第二传感器与相机之间的第二外参之后，然后根据录制数据分别构建针对第一外参、第二外参、相机位姿的约束残差/视觉重投影误差，以对第一外参、第二外参、相机位姿进行优化标定，这种优化标定方法可以将多传感器的外参一次性同步算出，并非传统的解耦解算，而是耦合到一起优化，在减少标定次数的同时还可以做到毫米级标定精度。
图3为本发明第二实施例提供的自主移动平台外参优化装置的结构图。为了便于说明，仅示出了与本实施例相关的部分。该视觉定位装置可以以软件模块形式或硬件模块形式或软硬件结合的形式内置于自主移动平台中，该自主移动平台需要包括相机、第一传感器、第二传感器，所述相机用于在所述自主移动平台行进过程中实时拍摄预先部署的标定板的图像，所述标定板上具有标定图案。参见图3，所述自主移动平台外参优化装置包括：
外参标定模块31，通过计算所述相机所拍摄的多帧图像以及所述第一传感器所获取的多个第一传感数据、所述第二传感器的多个第二传感数据，得到所述第一传感器与所述相机之间的第一外参，以及所述第二传感器与所述相机之间的第二外参；
第一位姿增量计算模块32，根据所述第二外参、多帧图像、多个第二传感器数据，计算所述第二传感器相对于世界坐标系下所述相机拍摄的所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第二位姿增量；
第二位姿增量计算模块33，根据所述第二外参、多帧图像、多个第二传感器数据，计算所述第二传感器相对于世界坐标系下所述相机拍摄的所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第二位姿增量；
位姿信息计算模块34，用于基于所述多帧图像计算与多帧图像对应的多个位姿信息；
残差构建及优化模块35，用于以所述第一位姿增量、第二位姿增量为观测量，以所述第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下所述多帧图像之间的位姿增量和所述第一位姿增量之间的第一约束残差、世界坐标系下所述多帧图像之间位姿增量和所述第二位姿增量之间的第二约束残差，并根据所述视觉重投影误差、所述第一约束残差、所述第二约束残差建立最小二乘法问题以对所述第一外参和所述第二外参进行优化，得到优化后的第一外参和第二外参。
其中，自主移动平台所需要的标定场景以及在该标定场景中行进的方式如第一实施例所述。
外参标定模块31可以采用开源的标定算法进行标定，如Kalibr算法可以标定相机30和IMU，从而得到IMU和相机30之间的初步外参，旷视算法可以标定相机30和轮式里程计，从而得到轮式里程计和相机30之间的初步外参。
第一位姿增量计算模块32，可采用SLAM（simultaneous localization andmapping，同步定位与建图）算法，以vins-mono算法为例，原理是从相机30所拍摄的图像中提取特征点，利用光流追踪的方式建立各帧图像间特征点之间的关联关系，构建完关联关系之后，再利用关联关系构建“重投影残差”，第一传感器40（如IMU）作积分预测自主移动平台的运动，并且构建“运动估计残差”，“运动估计残差”指的是传感器预测的自主移动平台运动位姿与需要优化求解自主移动平台运动位姿之间的残差，具体包括旋转残差和平移残差。这两个残差联合优化得到自主移动平台在各帧图像时刻下的运动位姿Tbi。然后计算在图像紧邻两帧之间自主移动平台运动的相对位姿，作为第一传感器40（如IMU）的第一运动位姿增量∆Tbi。
第二位姿增量计算模块33，同样可以采用SLAM算法来计算第二位姿增量。具体地，从相机30所拍摄的图像中提取特征点，利用光流追踪的方式建立各帧图像间特征点之间的关联关系，构建完关联关系之后，再利用关联关系构建“重投影残差”，第二传感器50（如轮式里程计）作积分预测自主移动平台的运动，并且构建“运动估计残差”。这两个残差联合优化得到自主移动平台在各帧图像时刻下的运动位姿Toi。然后计算在图像紧邻两帧之间自主移动平台运动的相对位姿，作为第二传感器50（如轮式里程计）的第二运动位姿增量∆Toi。
本实施例中，如图4所示，残差构建及优化模块35包括第一约束残差构建单元351、第二约束残差构建单元352、视觉重投影误差构建单元353以及优化单元354。
第一约束残差构建单元351用于根据所述第一外参、所述第一位姿增量以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第一外参的第一约束残差。具体地，第一约束残差构建单元351需要将第一外参Tcb进行逆变换，得到第一外参逆矩阵(Tcb)-1，然后再根据第一外参Tcb、第一位姿增量∆Tbi、第一外参逆矩阵(Tcb)-1以及自主移动平台在相机30在各时刻拍摄的标定图案之间的位姿变化量∆Tci，构建用于优化第一外参Tcb的第一约束残差eb。具体地，可采用下述公式构建第一约束残差eb：eb=Tcb*∆Tbi *(Tcb)-1-∆Tci。其中，∆Tci= Tci-1 * (Tci)-1，具体可通过如下方法得到：提取相机30拍摄的标定图案，得到所有标定图案的角点数据，利用图像角点信息通过PNP算法求出自主移动平台在各个图像时刻下的初步位姿Tci，然后计算相邻各帧图像时刻间的位姿∆Tci，这里的i表示第i个时刻。
类似地，第二约束残差构建单元352用于根据所述第二外参、所述第二位姿增量以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差。具体地，第二约束残差构建单元352将第二外参Tco进行逆变换，得到第二外参逆矩阵(Tco)-1，然后再根据第二外参Tco、第二位姿增量∆Toi、第二外参逆矩阵(Tco)-1以及自主移动平台在相机30在各时刻拍摄的标定图案之间的位姿变化量∆Tci，构建用于优化第二外参Tco的第二约束残差eo。具体地，可采用下述公式构建第二约束残差eo：eo=Tco*∆Toi *(Tco)-1-∆Tci。
视觉重投影误差构建单元353用于根据所述自主移动平台在所述相机拍摄各时刻的初步位姿、所述相机拍摄的标定图案的3D像素数据以及标定图案的角点数据，构建用于实时优化所述相机位姿的视觉重投影误差。具体地，视觉重投影误差构建单元353先提取相机30拍摄的所有标定图案的3D像素数据Pw，再提取相30机拍摄的所有标定图案的4个角点数据，然后根据下述公式构建用于实时优化相机30位姿的视觉重投影误差：
ec=Tci*Pw-Puv
其中，ec表示视觉重投影误差，Tci表示所述自主移动平台在所述相机拍摄各时刻的初步位姿，Pw表示所述相机拍摄的标定图案的3D像素数据，Puv表示所述相机拍摄的标定图案的4个角点数据。
优化单元354用于在得到第一约束残差、第二约束残差和视觉重投影误差这三个残差块之后，构建非线性最小二乘问题；以图优化求解算法求解非线性最小二乘问题，将非线性最小二乘问题的最优解作为优化后的第一外参、第二外参和相机位姿，具体可以通过图优化理论，用Levenberg-Marquardt算法或高斯牛顿法来进行优化，实现降低系统残差，优化过程不再赘述。
第二实施例中，自主移动平台在预先部署好的标定场景中运行时同步录制相机所拍摄的标定板的图像、第一传感器的传感数据、第二传感器的传感数据，将录制的图像/数据导入相关的算法中进行标定运算，首先会进行初步标定得到第一传感器与相机之间的第一外参以及第二传感器与相机之间的第二外参之后，然后根据录制数据分别构建针对第一外参、第二外参、相机位姿的约束残差/视觉重投影误差，以对第一外参、第二外参、相机位姿进行优化标定，这种优化标定方法可以将多传感器的外参一次性同步算出，并非传统的解耦解算，而是耦合到一起优化，在减少标定次数的同时还可以做到毫米级标定精度。
本发明第三实施例提供了一种自主移动平台的外参优化方法，具体流程参见图2，该方法可应用于图1所示的自主移动平台，实现本方法所需要执行的程序可以以独立软件程序的形式被上传、下载、安装，也可以烧录到一些存储介质中。如图2所示，该自主移动平台的外参优化方法具体包括：
步骤S201，通过计算相机所拍摄的多帧图像以及第一传感器所获取的多个第一传感数据、第二传感器所获取的多个第二传感数据，得到第一传感器与相机之间的第一外参，以及第二传感器与相机之间的第二外参。
该自主移动平台包括相机、第一传感器、第二传感器，相机用于在自主移动平台行进过程中实时拍摄预先部署的标定板的图像，标定板上具有标定图案。
其中，相机一般为离线的相机，主要功能为拍摄，用于在自主移动平台行进过程中实时拍摄预先部署的标定板的图像，该标定板上具有标定图案。
第一传感器和第二传感器可以是IMU（Inertial Measurement Unit，惯性传感器）、轮式里程计、毫米波雷达、激光雷达等等。
在第一实施例布置的标定区按要求行进结束后，将录制的这些信息输入到算法模型里进行初步标定，得到所述第一传感器与所述相机之间的第一外参，以及所述第二传感器与所述相机之间的第二外参，具体可以采用开源的标定算法进行标定，如Kalibr算法可以标定相机和IMU，从而得到IMU和相机之间的初步外参，旷视算法可以标定相机和轮式里程计，从而得到轮式里程计和相机之间的初步外参。然后再执行下述步骤S202-S206中的构建约束残差、优化标定等过程，最终可以将第一传感器、第二传感器的外参一次性同步优化，从而降低系统残差。
步骤S202，根据第一外参、多帧图像、多个第一传感器数据，计算第一传感器相对于世界坐标系下相机拍摄的所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第一位姿增量。
本步骤可采用SLAM（simultaneous localization and mapping，同步定位与建图）算法，以vins-mono算法为例，原理是从相机所拍摄的图像中提取特征点，利用光流追踪的方式建立各帧图像间特征点之间的关联关系，构建完关联关系之后，再利用关联关系构建“重投影残差”，第一传感器（如IMU）作积分预测自主移动平台的运动，并且构建“运动估计残差”，“运动估计残差”指的是传感器预测的自主移动平台运动位姿与需要优化求解自主移动平台运动位姿之间的残差，具体包括旋转残差和平移残差。这两个残差联合优化得到自主移动平台在各帧图像时刻下的运动位姿Tbi。然后计算在图像紧邻两帧之间自主移动平台运动的相对位姿，作为第一传感器（如IMU）的第一运动位姿增量∆Tbi。
步骤S203，根据第二外参、多帧图像、多个第二传感器数据，计算第二传感器相对于世界坐标系下相机拍摄的所有图像帧相邻图像帧的位姿变化，并将计算结果作为第二位姿增量。
与步骤S202类似，本步骤S203中同样可以采用SLAM算法来计算第二位姿增量。具体地，从相机所拍摄的图像中提取特征点，利用光流追踪的方式建立各帧图像间特征点之间的关联关系，构建完关联关系之后，再利用关联关系构建“重投影残差”，第二传感器（如轮式里程计）作积分预测自主移动平台的运动，并且构建“运动估计残差”。这两个残差联合优化得到自主移动平台在各帧图像时刻下的运动位姿Toi。然后计算在图像紧邻两帧之间自主移动平台运动的相对位姿，作为第二传感器（如轮式里程计）的第二运动位姿增量∆Toi。
步骤S204，基于多帧图像计算与多帧图像对应的多个位姿信息。
步骤S205，以第一位姿增量、第二位姿增量为观测量，以第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下多帧图像之间的位姿增量和第一位姿增量之间的第一约束残差、世界坐标系下多帧图像之间位姿增量和第二位姿增量之间的第二约束残差，并根据视觉重投影误差、第一约束残差、第二约束残差建立最小二乘法问题以对第一外参和第二外参进行优化，得到优化后的第一外参和第二外参。
具体地，根据第一外参、第一位姿增量以及自主移动平台在相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化第一外参的第一约束残差。
根据第二外参、第二位姿增量以及自主移动平台在相机30在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差。
根据自主移动平台在相机30拍摄各时刻的初步位姿、相机30拍摄的标定图案的3D像素数据以及标定图案的角点数据，构建用于实时优化相机30位姿的视觉重投影误差。
第一约束残差、第二约束残差和视觉重投影误差这三个残差块构成了总残差，可以表示为：e_all=eb+eo+ec，其中，e_all表示总残差，eb表示第一约束残差，eo表示第二约束残差，ec表示视觉重投影误差。
具体到第一约束残差eb，还需要将第一外参Tcb进行逆变换，得到第一外参逆矩阵(Tcb)-1，然后再根据第一外参Tcb、第一位姿增量∆Tbi、第一外参逆矩阵(Tcb)-1以及自主移动平台在相机在各时刻拍摄的标定图案之间的位姿变化量∆Tci，构建用于优化第一外参Tcb的第一约束残差eb。具体地，可采用下述公式构建第一约束残差eb：eb=Tcb*∆Tbi *(Tcb)-1-∆Tci。
其中，∆Tci= Tci-1 * (Tci)-1，具体可通过如下方法得到：提取相机拍摄的标定图案，得到所有标定图案的角点数据，利用图像角点信息通过PNP算法求出自主移动平台在各个图像时刻下的初步位姿Tci，然后计算相邻各帧图像时刻间的位姿∆Tci，这里的i表示第i个时刻。
类似地，具体到第二约束残差eo，将第二外参Tco进行逆变换，得到第二外参逆矩阵(Tco)-1，然后再根据第二外参Tco、第二位姿增量∆Toi、第二外参逆矩阵(Tco)-1以及自主移动平台在相机在各时刻拍摄的标定图案之间的位姿变化量∆Tci，构建用于优化第二外参Tco的第二约束残差eo。具体地，可采用下述公式构建第二约束残差eo：eo=Tco*∆Toi*(Tco)-1-∆Tci。
具体到视觉重投影误差ec，首先提取相机拍摄的所有标定图案的3D像素数据Pw，再提取相机拍摄的所有标定图案的4个角点数据，然后根据下述公式构建用于实时优化相机位姿的视觉重投影误差：
ec=Tci*Pw-Puv
其中，ec表示视觉重投影误差，Tci表示所述自主移动平台在所述相机拍摄各时刻的初步位姿，Pw表示所述相机拍摄的标定图案的3D像素数据，Puv表示所述相机拍摄的标定图案的4个角点数据。
上述三个残差块都得到之后，可以构建非线性最小二乘问题；以图优化求解算法求解非线性最小二乘问题，将非线性最小二乘问题的最优解作为优化后的第一外参、第二外参和相机位姿，具体可以通过图优化理论，用Levenberg-Marquardt算法或高斯牛顿法来进行优化，实现降低系统残差，优化过程不再赘述。
本发明第四实施例还提供了一种计算机可读存储介质，该计算机可读存储介质可以是设置于上述各实施例中的自主移动平台中，该计算机可读存储介质可以是自主移动平台中的存储器。该计算机可读存储介质上存储有计算机程序，该程序被处理器执行时实现前述图2所示实施例中描述的自主移动平台外参优化方法。进一步的，该计算机可存储介质还可以是U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、RAM、磁碟或者光盘等各种可以存储程序代码的介质。
需要说明的是，对于前述的各方法实施例，为了简便描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其它顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定都是本发明所必须的。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其它实施例的相关描述。
以上为对本发明各实施例所提供的自主移动平台、自主移动平台外参优化方法、装置及可读存储介质的描述，对于本领域的技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上，本说明书内容不应理解为对本发明的限制。

Claims (16)

1.一种自主移动平台，其特征在于，所述自主移动平台包括相机、第一传感器、第二传感器、存储器和处理器；所述相机用于在所述自主移动平台行进过程中实时拍摄预先部署的标定板的多帧图像，所述标定板上具有标定图案；所述存储器存储有可执行程序代码；所述处理器用于调用并运行所述可执行程序代码来实现自主移动平台外参优化方法；所述自主移动平台外参优化方法包括：
通过计算所述相机所拍摄的多帧图像以及所述第一传感器所获取的多个第一传感数据、所述第二传感器所获取的多个第二传感数据，得到所述第一传感器与所述相机之间的第一外参，以及所述第二传感器与所述相机之间的第二外参；
根据所述第一外参、多帧图像、多个第一传感器数据，计算所述第一传感器相对于世界坐标系下所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第一位姿增量；
根据所述第二外参、多帧图像、多个第二传感器数据，计算所述第二传感器相对于世界坐标系下所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第二位姿增量；
基于所述多帧图像计算与多帧图像对应的多个位姿信息；
以所述第一位姿增量、第二位姿增量为观测量，以所述第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下所述多帧图像之间的位姿增量和所述第一位姿增量之间的第一约束残差、世界坐标系下所述多帧图像之间的位姿增量和所述第二位姿增量之间的第二约束残差，并建立最小二乘法问题以对所述第一外参和所述第二外参进行优化，得到优化后的第一外参和第二外参。
2.如权利要求1所述的自主移动平台，其特征在于，所述以所述第一位姿增量、第二位姿增量为观测量，以所述第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下所述多帧图像之间的位姿增量和所述第一位姿增量之间的第一约束残差、世界坐标系下所述多帧图像之间位姿增量和所述第二位姿增量之间的第二约束残差，包括：
根据所述第一外参、所述第一位姿增量以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第一外参的第一约束残差；
根据所述第二外参、所述第二位姿增量以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差；
根据所述自主移动平台在所述相机拍摄各时刻的初步位姿、所述相机拍摄的标定图案的3D像素数据以及标定图案的角点数据，构建用于实时优化所述相机位姿的视觉重投影误差。
3.如权利要求2所述的自主移动平台，其特征在于，所述根据所述第一外参、所述第一位姿增量以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第一外参的第一约束残差，包括：
将所述第一外参进行逆变换，得到第一外参逆矩阵；
根据所述第一外参、所述第一位姿增量、所述第一外参逆矩阵以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第一外参的第一约束残差。
4.如权利要求3所述的自主移动平台，其特征在于，所述根据所述第一外参、所述第一位姿增量、所述第一外参逆矩阵以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第一外参的第一约束残差，包括：
采用下述公式构建用于优化所述第一外参的第一约束残差：
eb=Tcb*∆Tbi *(Tcb)-1-∆Tci
其中，eb表示第一约束残差，Tcb表示第一外参，∆Tbi表示第一位姿增量，(Tcb)-1表示第一外参逆矩阵，∆Tci表示所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量。
5.如权利要求2所述的自主移动平台，其特征在于，所述根据所述第二外参、所述第二位姿增量以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差，包括：
将所述第二外参进行逆变换，得到第二外参逆矩阵；
根据所述第二外参、所述第二位姿增量、所述第二外参逆矩阵以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差。
6.如权利要求5所述的自主移动平台，其特征在于，所述根据所述第二外参、所述第二位姿增量、所述第二外参逆矩阵以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差，包括：
采用下述公式构建用于优化所述第一外参的第一约束残差：
eo=Tco*∆Toi *(Tco)-1-∆Tci
其中，eo表示第二约束残差，Tco表示第二外参，∆Toi表示第二位姿增量，(Tco)-1表示第二外参逆矩阵，∆Tci表示所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量。
7.如权利要求2所述的自主移动平台，其特征在于，所述根据所述自主移动平台在所述相机拍摄各时刻的初步位姿、所述相机拍摄的标定图案的3D像素数据以及标定图案的角点数据，构建用于实时优化所述相机位姿的视觉重投影误差，包括：
提取所述相机拍摄的所有标定图案的3D像素数据；
提取所述相机拍摄的所有标定图案的4个角点数据；
根据下述公式构建用于实时优化所述相机位姿的视觉重投影误差：
ec=Tci*Pw-Puv
其中，ec表示视觉重投影误差，Tci表示所述自主移动平台在所述相机拍摄各时刻的初步位姿，Pw表示所述相机拍摄的标定图案的3D像素数据，Puv表示所述相机拍摄的标定图案的4个角点数据。
8.一种自主移动平台外参优化装置，其特征在于，所述自主移动平台外参优化装置应用于自主移动平台中，所述自主移动平台包括相机、第一传感器、第二传感器，所述相机用于在所述自主移动平台行进过程中实时拍摄预先部署的标定板的图像，所述标定板上具有标定图案；所述自主移动平台外参优化装置包括：
外参标定模块，通过计算所述相机所拍摄的多帧图像以及所述第一传感器所获取的多个第一传感数据、所述第二传感器的多个第二传感数据，得到所述第一传感器与所述相机之间的第一外参，以及所述第二传感器与所述相机之间的第二外参；
第一位姿增量计算模块，根据所述第一外参、多帧图像、多个第一传感器数据计算所述第一传感器相对于世界坐标系下所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第一位姿增量；
第二位姿增量计算模块，根据所述第二外参、多帧图像、多个第二传感器数据，计算所述第二传感器相对于世界坐标系下所述相机拍摄的所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第二位姿增量；
位姿信息计算模块，用于基于所述多帧图像计算与多帧图像对应的多个位姿信息；
残差构建及优化模块，用于以所述第一位姿增量、第二位姿增量为观测量，以所述第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下所述多帧图像之间的位姿增量和所述第一位姿增量之间的第一约束残差、世界坐标系下所述多帧图像之间位姿增量和所述第二位姿增量之间的第二约束残差，并根据所述视觉重投影误差、所述第一约束残差、所述第二约束残差建立最小二乘法问题以对所述第一外参和所述第二外参进行优化，得到优化后的第一外参和第二外参。
9.一种自主移动平台外参优化方法，其特征在于，所述自主移动平台外参优化方法应用于自主移动平台中，所述自主移动平台包括相机、第一传感器、第二传感器，所述相机用于在所述自主移动平台行进过程中实时拍摄预先部署的标定板的图像，所述标定板上具有标定图案；所述自主移动平台外参优化方法包括：
通过计算所述相机所拍摄的多帧图像以及所述第一传感器所获取的多个第一传感数据、所述第二传感器的多个第二传感数据，得到所述第一传感器与所述相机之间的第一外参，以及所述第二传感器与所述相机之间的第二外参；
根据所述第一外参、多帧图像、多个第一传感器数据计算所述第一传感器相对于世界坐标系下所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第一位姿增量；
根据所述第二外参、多帧图像、多个第二传感器数据，计算所述第二传感器相对于世界坐标系下所述相机拍摄的所有图像帧中相邻图像帧的位姿变化，并将计算结果作为第二位姿增量；
基于所述多帧图像计算与多帧图像对应的多个位姿信息；
以所述第一位姿增量、第二位姿增量为观测量，以所述第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下所述多帧图像之间的位姿增量和所述第一位姿增量之间的第一约束残差、世界坐标系下所述多帧图像之间位姿增量和所述第二位姿增量之间的第二约束残差，并根据所述视觉重投影误差、所述第一约束残差、所述第二约束残差建立最小二乘法问题以对所述第一外参和所述第二外参进行优化，得到优化后的第一外参和第二外参。
10.如权利要求9所述的自主移动平台外参优化方法，其特征在于，所述以所述第一位姿增量、第二位姿增量为观测量，以所述第一外参、第二外参以及多个位姿信息作为优化变量，构建相机的视觉重投影误差、世界坐标系下所述多帧图像之间的位姿增量和所述第一位姿增量之间的第一约束残差、世界坐标系下所述多帧图像之间位姿增量和所述第二位姿增量之间的第二约束残差，包括：
根据所述第一外参、所述第一位姿增量以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第一外参的第一约束残差；
根据所述第二外参、所述第二位姿增量以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差；
根据所述自主移动平台在所述相机拍摄各时刻的初步位姿、所述相机拍摄的标定图案的3D像素数据以及标定图案的角点数据，构建用于实时优化所述相机位姿的视觉重投影误差。
11.如权利要求10所述的自主移动平台外参优化方法，其特征在于，所述根据所述第一外参、所述第一位姿增量以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第一外参的第一约束残差，包括：
将所述第一外参进行逆变换，得到第一外参逆矩阵；
根据所述第一外参、所述第一位姿增量、所述第一外参逆矩阵以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第一外参的第一约束残差。
12.如权利要求11所述的自主移动平台外参优化方法，其特征在于，所述根据所述第一外参、所述第一位姿增量、所述第一外参逆矩阵以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第一外参的第一约束残差，包括：
采用下述公式构建用于优化所述第一外参的第一约束残差：
eb=Tcb*∆Tbi *(Tcb)-1-∆Tci
其中，eb表示第一约束残差，Tcb表示第一外参，∆Tbi表示第一位姿增量，(Tcb)-1表示第一外参逆矩阵，∆Tci表示所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量。
13.如权利要求10所述的自主移动平台外参优化方法，其特征在于，所述根据所述第二外参、所述第二位姿增量以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差，包括：
将所述第二外参进行逆变换，得到第二外参逆矩阵；
根据所述第二外参、所述第二位姿增量、所述第二外参逆矩阵以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差。
14.如权利要求13所述的自主移动平台外参优化方法，其特征在于，所述根据所述第二外参、所述第二位姿增量、所述第二外参逆矩阵以及所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量，构建用于优化所述第二外参的第二约束残差，包括：
采用下述公式构建用于优化所述第一外参的第一约束残差：
eo=Tco*∆Toi *(Tco)-1-∆Tci
其中，eo表示第二约束残差，Tco表示第二外参，∆Toi表示第二位姿增量，(Tco)-1表示第二外参逆矩阵，∆Tci表示所述自主移动平台在所述相机在各时刻拍摄的标定图案之间的位姿变化量。
15.如权利要求10所述的自主移动平台外参优化方法，其特征在于，所述根据所述自主移动平台在所述相机拍摄各时刻的初步位姿、所述相机拍摄的标定图案的3D像素数据以及标定图案的角点数据，构建用于实时优化所述相机位姿的视觉重投影误差，包括：
提取所述相机拍摄的标定图案的3D像素数据；
提取所述相机拍摄的标定图案的4个角点数据；
根据下述公式构建用于实时优化所述相机位姿的视觉重投影误差：
ec=Tci*Pw-Puv
其中，ec表示视觉重投影误差，Tci表示所述自主移动平台在所述相机拍摄各时刻的初步位姿，Pw表示所述相机拍摄的标定图案的3D像素数据，Puv表示所述相机拍摄的标定图案的4个角点数据。
16.一种可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序在被处理器执行时实现权利要求9至15中的任一项所述的自主移动平台外参优化方法。
CN202111040371.9A 2021-09-06 2021-09-06 自主移动平台、外参优化方法、装置及存储介质 Active CN113470121B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111040371.9A CN113470121B (zh) 2021-09-06 2021-09-06 自主移动平台、外参优化方法、装置及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111040371.9A CN113470121B (zh) 2021-09-06 2021-09-06 自主移动平台、外参优化方法、装置及存储介质

Publications (2)

Publication Number Publication Date
CN113470121A true CN113470121A (zh) 2021-10-01
CN113470121B CN113470121B (zh) 2021-12-28

Family

ID=77864636

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111040371.9A Active CN113470121B (zh) 2021-09-06 2021-09-06 自主移动平台、外参优化方法、装置及存储介质

Country Status (1)

Country Link
CN (1) CN113470121B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114463429A (zh) * 2022-04-12 2022-05-10 深圳市普渡科技有限公司 机器人、地图创建方法、定位方法及介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110514225A (zh) * 2019-08-29 2019-11-29 中国矿业大学 一种矿井下多传感器融合的外部参数标定及精准定位方法
CN111739063A (zh) * 2020-06-23 2020-10-02 郑州大学 一种基于多传感器融合的电力巡检机器人定位方法
CN112184824A (zh) * 2019-07-05 2021-01-05 杭州海康机器人技术有限公司 一种相机外参标定方法、装置
US20210003684A1 (en) * 2019-07-05 2021-01-07 DeepMap Inc. Online sensor calibration for autonomous vehicles
CN112734765A (zh) * 2020-12-03 2021-04-30 华南理工大学 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质
CN112880687A (zh) * 2021-01-21 2021-06-01 深圳市普渡科技有限公司 一种室内定位方法、装置、设备和计算机可读存储介质

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN112184824A (zh) * 2019-07-05 2021-01-05 杭州海康机器人技术有限公司 一种相机外参标定方法、装置
US20210003684A1 (en) * 2019-07-05 2021-01-07 DeepMap Inc. Online sensor calibration for autonomous vehicles
CN110514225A (zh) * 2019-08-29 2019-11-29 中国矿业大学 一种矿井下多传感器融合的外部参数标定及精准定位方法
CN111739063A (zh) * 2020-06-23 2020-10-02 郑州大学 一种基于多传感器融合的电力巡检机器人定位方法
CN112734765A (zh) * 2020-12-03 2021-04-30 华南理工大学 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质
CN112880687A (zh) * 2021-01-21 2021-06-01 深圳市普渡科技有限公司 一种室内定位方法、装置、设备和计算机可读存储介质

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
JOSÉ L. LERMA等: "Camera Calibration with Baseline Distance Constraints", 《THE PHOTOGRAMMETRIC RECORD》 *
TIANBI LIU等: "A Novel Multi-Sensor Fusion Based Object Detection and Recognition Algorithm for Intelligent Assisted Driving", 《 IEEE ACCESS》 *
张雪丽: "基于多传感器融合的机器人定位研究与应用", 《中国优秀硕士论文全文数据库信息科技辑》 *
赵天阳: "融合惯性与视觉的多传感器空间位姿计算方法的研究", 《中国优秀硕士论文全文数据库信息科技辑》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114463429A (zh) * 2022-04-12 2022-05-10 深圳市普渡科技有限公司 机器人、地图创建方法、定位方法及介质

Also Published As

Publication number Publication date
CN113470121B (zh) 2021-12-28

Similar Documents

Publication Publication Date Title
US10953545B2 (en) System and method for autonomous navigation using visual sparse map
US10796151B2 (en) Mapping a space using a multi-directional camera
CN107907131B (zh) 定位系统、方法及所适用的机器人
CN107680135B (zh) 定位方法、系统及所适用的机器人
CN107610084A (zh) 一种对深度图像和激光点云图进行信息融合的方法与设备
US11846949B2 (en) Systems and methods for calibration of a pose of a sensor relative to a materials handling vehicle
CN205121338U (zh) 基于图像识别与无线网络的agv导航系统
JP2020530598A (ja) 地図構築方法、画像収集処理システムと測位方法
CN111665826A (zh) 基于激光雷达与单目相机的深度图获取方法及扫地机器人
CN113470121B (zh) 自主移动平台、外参优化方法、装置及存储介质
KR101341204B1 (ko) 레이저 스캐너 및 구조물을 이용한 모바일 로봇의 위치추정장치 및 방법
CN103092205A (zh) 一种基于预设运动路径的移动机器人及其控制方法
Liu A robust and efficient lidar-inertial-visual fused simultaneous localization and mapping system with loop closure
Nüchter et al. Irma3D—An intelligent robot for mapping applications
CN113867333A (zh) 基于视觉感知的四足机器人楼梯攀爬规划方法及其应用
CN111656138A (zh) 构建地图及定位方法、客户端、移动机器人及存储介质
Bobkov et al. Vision-based navigation method for a local maneuvering of the autonomous underwater vehicle
CN111780744A (zh) 移动机器人混合导航方法、设备及存储装置
Roggeman et al. Embedded vision-based localization and model predictive control for autonomous exploration
CN113379850B (zh) 移动机器人控制方法、装置、移动机器人及存储介质
CN115597600A (zh) 基于视觉识别的机器人导航方法、导航机器人及介质
CN112489131B (zh) 基于路面检测构建代价地图的方法、装置、介质和机器人
CN114577216A (zh) 导航地图构建方法、装置、机器人及存储介质
CN114066963A (zh) 一种建图方法、装置及机器人
CN115019167B (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