CN111797906B - 基于视觉和惯性里程定位的方法及装置 - Google Patents
基于视觉和惯性里程定位的方法及装置 Download PDFInfo
- Publication number
- CN111797906B CN111797906B CN202010543589.5A CN202010543589A CN111797906B CN 111797906 B CN111797906 B CN 111797906B CN 202010543589 A CN202010543589 A CN 202010543589A CN 111797906 B CN111797906 B CN 111797906B
- Authority
- CN
- China
- Prior art keywords
- vector
- determining
- matrix
- pose
- image
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 87
- 239000013598 vector Substances 0.000 claims abstract description 270
- 239000011159 matrix material Substances 0.000 claims abstract description 153
- 230000008859 change Effects 0.000 claims abstract description 125
- 238000005259 measurement Methods 0.000 claims abstract description 30
- 238000004364 calculation method Methods 0.000 claims abstract description 21
- 230000007613 environmental effect Effects 0.000 claims description 71
- 230000008569 process Effects 0.000 claims description 36
- 230000006870 function Effects 0.000 claims description 31
- 230000000007 visual effect Effects 0.000 claims description 23
- 238000006073 displacement reaction Methods 0.000 claims description 20
- 238000003860 storage Methods 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 15
- 238000011217 control strategy Methods 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims description 11
- 230000010354 integration Effects 0.000 claims description 10
- 230000008030 elimination Effects 0.000 claims description 5
- 238000003379 elimination reaction Methods 0.000 claims description 5
- 238000010586 diagram Methods 0.000 description 14
- 238000005516 engineering process Methods 0.000 description 12
- 230000006872 improvement Effects 0.000 description 8
- 238000012545 processing Methods 0.000 description 7
- 230000003287 optical effect Effects 0.000 description 5
- 230000033001 locomotion Effects 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 238000009826 distribution Methods 0.000 description 3
- 241001265525 Edgeworthia chrysantha Species 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 230000000452 restraining effect Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000005055 memory storage Effects 0.000 description 1
- 229920001296 polysiloxane Polymers 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 239000010979 ruby Substances 0.000 description 1
- 229910001750 ruby Inorganic materials 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
- G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
- G06V10/757—Matching configurations of points or features
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Physics (AREA)
- Databases & Information Systems (AREA)
- Computational Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Software Systems (AREA)
- Computing Systems (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Mathematical Optimization (AREA)
- Mathematical Analysis (AREA)
- Evolutionary Computation (AREA)
- Multimedia (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Medical Informatics (AREA)
- Algebra (AREA)
- General Health & Medical Sciences (AREA)
- General Engineering & Computer Science (AREA)
- Image Analysis (AREA)
Abstract
本说明书公开了一种基于视觉以及惯性里程定位的方法及装置，根据图像特征点匹配结果以及惯性测量数据，确定相邻帧的预估位姿变化，并当该预估位姿变化小于预设值时，确定当前帧待解的状态向量，基于目标函数确定该待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式，通过忽略该矩阵关系式高阶项，求解环境点向量的变化量以及位姿向量的变化量，最后确定当前帧的位姿。在预估位姿变化较小的情况下，确定位姿变化是线性的且可根据上一帧的系数矩阵、状态向量以及残差推导得到，因此构造了系数矩阵和残差的增量表达式，通过求解增量方程来得到全部状态向量。无需解算当前帧的系数矩阵以及残差，减少了解算复杂度，提高定位效率。
Description
技术领域
本申请涉及计算机技术领域，尤其涉及一种基于视觉以及惯性里程定位的方法以及装置。
背景技术
无源定位技术是指终端在不依赖其他定位设备的情况下，根据自身采集的数据确定自身位置的方法。其中，其他定位设备通常是指定位卫星，例如，全球定位系统的卫星。目前，无源定位技术中一种主要的方案是基于视觉以及惯性里程计(Visual-InertialOdometry，VIO)进行定位，通常称为视觉-惯性里程计定位方法。
在现有技术中，通常基于包含前后端设备的系统实现视觉-惯性里程计定位方法。其中，前端是需要定位的设备，前端中可设置有采集图像数据的图像传感器以及采集惯性数据的惯性测量单元(Inertial measurement unit，IMU)，并将采集的数据进行预处理后发送至后端。后端根据图像预处理的结果以及IMU预处理的结果，进行多源信息融合，计算出前端的位姿。并且，由于位姿解算通常较为复杂因此后端通常为服务器。
其中，计算出前端的位姿常用的一种方法是利用非线性优化方法，根据多源信息解算最优解，并根据解算出的解确定前端位姿。通常来说，需要根据历史上解算出的各帧对应的状态向量，确定各相邻帧之间的系数矩阵和残差，再根据目标函数进行计算，确定当前帧的状态向量。
但是，由于状态向量通常包含了图像特征信息、IMU数据信息、环境特征等信息，因此根据系数矩阵和残差进行计算状态向量非常复杂，导致服务器需要较为强大的算力才能满足需求。而若减少参与计算的帧，则会导致定位精度下降。
发明内容
本说明书实施例提供的基于视觉以及惯性里程定位的方法及装置，用于部分解决现有技术中存在的问题。
本说明书实施例采用下述技术方案：
本说明书提供的基于视觉以及惯性里程定位的方法，包括：
获取当前帧的图像以及惯性测量数据；
确定所述当前帧的图像与上一帧的图像的图像特征点的匹配结果；
根据所述匹配结果以及所述惯性测量数据，确定无人车在当前帧与上一帧之间的预估位姿变化；
当所述预估位姿变化小于预设值时，确定当前帧的待解的状态向量，根据基于先验残差项、视觉残差项以及惯性传感器残差项确定的目标函数以及所述待解的状态向量，确定所述待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式，其中，状态向量包含环境点向量以及位姿向量，环境点向量包含各图像特征点对应的环境点的坐标；
根据所述矩阵关系式，确定所述状态向量的变化矩阵中的环境点向量的变化量求解式，对所述求解式进行展开，得到展开后的低阶项和高阶项，并根据将所述求解式展开后的低阶项解算所述环境点向量的变化量；
根据所述矩阵关系式和解算得到的环境点向量的变化量，解算所述位姿向量的变化量；
根据解算得到所述环境点向量的变化量、所述位姿向量的变化量以及上一帧的状态向量，确定当前帧的位姿，以供所述无人车确定控制策略。
可选地，确定当前帧待解的状态向量，具体包括：
根据当前行驶过程中每帧采集的图像，确定各图像的图像特征点对应的环境点的四维齐次坐标；
根据确定出的各环境点的四维齐次坐标，确定待解环境点向量；
确定所述当前帧相对于上一帧的待解位姿向量；
根据所述待解位姿向量以及所述待解环境点向量，确定当前帧待解的状态向量。
可选地，确定所述当前帧相对于上一帧的待解位姿向量，具体包括：
根据所述当前帧相对于上一帧的三维位移矢量、三维速度矢量、四维旋转四元数、惯性测量数据中加速度三维零偏矢量以及惯性测量数据中角速度三维零偏矢量，确定待解位姿向量。
可选地，根据所述匹配结果以及所述惯性测量数据，确定所述前端在当前帧与上一帧之间的预估位姿变化，具体包括：
根据所述匹配结果，确定当前帧的图像与上一帧图像中匹配的各图像特征点；
确定各图像特征点在所述两帧图像中的位置变化；
确定所述惯性测量数据在各坐标上的积分结果；
根据确定出的各图像特征点的位置变化、各图像特征点的匹配结果以及所述积分结果，确定预估位姿变化。
可选地，基于先验残差项、视觉残差项以及服从惯性传感器残差项确定的目标函数，具体包括：
根据当前行驶过程中历史上各帧已经解算出坐标的各环境点，以及所述待解的状态向量中的环境点向量，确定先验残差项；
针对所述当前帧的图像的每个图像特征点，根据采集图像设备的内参矩阵、所述待解的状态向量中的位姿向量的旋转四元数以及待解的状态向量中的该图像特征点对应环境点的坐标，确定该图像特征点在图像中待解的三维齐次坐标，并根据该图像特征点在图像中实际的三维其次坐标与所述待解的三维齐次坐标，确定该图像特征点的视觉残差项；
针对所述待解位姿向量中的三维位移矢量、所述三维速度矢量、所述四维旋转四元数、所述加速度三维零偏矢量以及所述角速度三维零偏矢量，根据上一帧解算得到的结果，确定惯性传感器残差项；
基于先验残差项、视觉残差项以及惯性传感器残差项，确定使各残差项之和最小的待解的状态向量，作为目标函数。
可选地，根据所述矩阵关系式，确定所述状态向量的变化矩阵中的环境点向量的变化量求解式，具体包括：
将所述矩阵关系式中的所述待解系数矩阵展开为上一帧的系数矩阵与系数矩阵变化量，所述待解残差矩阵展开为上一帧的残差矩阵与残差矩阵变化量；
对展开后的矩阵关系式进行舒尔补消元，确定消元后所述状态向量的变化矩阵中的环境点向量的变化量求解式。
可选地，根据将所述求解式展开后的低阶项解算所述环境点向量的变化量，具体包括：
根据将所述求解式展开后的低阶项、上一帧解算出的系数矩阵以及上一帧解算出的残差矩阵，解算所述环境点向量的变化量；
根据解算得到的环境点向量的变化量、所述矩阵关系式，解算所述位姿向量的变化量，具体包括：
根据解算得到的环境点向量的变化量、所述矩阵关系式、上一帧解算出的系数矩阵以及上一帧解算出的残差矩阵，解算所述位姿向量的变化量。
本说明书提供的基于视觉以及惯性里程定位的装置，包括：
获取模块，获取当前帧的图像以及惯性测量数据；
匹配模块，确定所述当前帧的图像与上一帧的图像的图像特征点的匹配结果；
预估模块，根据所述匹配结果以及所述惯性测量数据，确定所述装置在当前帧与上一帧之间的预估位姿变化；
关系式确定模块，当所述预估位姿变化小于预设值时，确定当前帧待解的状态向量，根据基于先验残差项、视觉残差项以及惯性传感器残差项确定的目标函数以及所述待解的状态向量，确定所述待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式，其中，状态向量包含环境点向量以及位姿向量，环境点向量包含各图像特征点对应的环境点的坐标；
第一解算模块，根据所述矩阵关系式，确定所述状态向量的变化矩阵中的环境点向量的变化量求解式，对所述求解式进行展开，得到展开后的低阶项和高阶项，并根据将所述求解式展开后的低阶项解算所述环境点向量的变化量；
第二解算模块，根据所述矩阵关系式和解算得到的环境点向量的变化量，解算所述位姿向量的变化量；
定位模块，根据解算得到所述环境点向量的变化量、所述位姿向量的变化量以及上一帧的状态向量，确定当前帧的位姿，以供所述装置确定控制策略。
本说明书提供的计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述任一所述的方法。
本说明书提供的无人车，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述任一所述的方法。
本说明书实施例采用的上述至少一个技术方案能够达到以下有益效果：
根据相邻帧的图像的特征点匹配结果，以及惯性测量数据，确定当前帧与上一帧的预估位姿变化，并当该预估位姿变化小于预设值时，确定当前帧待解的状态向量，并根据目标函数，确定该待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式，通过该矩阵关系式，确定状态向量的变化矩阵中的环境点向量变化量求解式，通过忽略高阶项，求解环境点向量的变化量，以及根据忽略高阶项的位姿向量的变化量求解式，求解位姿向量的变化量，最后根据解算得到的环境点向量的变化量以及位姿向量的变化量，解算当前帧的位姿。在预估位姿变化较小的情况下，确定位姿变化是线性的并且可根据上一帧的系数矩阵、状态向量以及残差推导得到，因此在相邻帧之间构造了系数矩阵和残差矩阵的增量表达式，从而推导出求解式，通过求解增量方程来得到全部状态向量，得到当前帧的位姿。在位姿解算过程中，无需解算当前帧的系数矩阵以及残差，减少了位姿解算的复杂度，可以有效提高定位效率。
附图说明
此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1为本说明书实施例提供的基于视觉以及惯性里程定位的流程示意图；
图2为本说明书实施例提供的轨迹图；
图3为本说明书实施例提供的另一种定位流程示意图；
图4为本说明书实施例提供的基于视觉以及惯性里程定位的装置的结构示意图；
图5为本说明书实施例提供的实现基于视觉以及惯性里程定位的方法的无人车示意图。
具体实施方式
为使本说明书的目的、技术方案和优点更加清楚，下面将结合本说明书具体实施例及相应的附图对本申请技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本说明书中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
以下结合附图，详细说明本申请各实施例提供的技术方案。
图1为本说明书实施例提供的基于视觉以及惯性里程定位的流程示意图，包括：
S100：获取当前帧的图像以及惯性测量数据。
通常在无人车导航领域中采用基于VIO技术的定位方法，同理在本说明书中该定位流程也可用于无人车导航，具体用于为无人车提供导航或者确定控制策略所需的无人车的位姿。当然，该定位过程也可用于其他场景，例如地图绘制场景等等。为了方便说明，本说明书中对执行主体不再区分前端后端，一并以无人车根据该定位过程确定控制策略的过程为例进行说明。
由于VIO技术所需的基础输入数据包括视觉纬度的图像以及传感器维度的IMU数据，因此在本说明书中，该无人车可确定当前帧采集的图像以及当前帧对应的IMU数据。
具体的，该图像可通过无人车上设置的图像传感器采集，该图像传感器具体可以是采集该无人车车头朝向的图像，或者也可理解为是该无人车行驶方向的图像。该IMU同样设置在无人车上，用于采集IMU数据。并且，由于通常图像传感器采集图像的频率与IMU采集数据的频率不同，因此一般基于图像采集的频率确定位姿，而对于IMU数据则通过插值以及积分的方式，确定在图像传感器采集当前帧图像与采集上一帧图像之间，IMU采集的IMU数据。
另外，由于按照图像传感器采集图像的频率进行定位的，当采集频率较高(例如，60Hz)则单位时间内需要计算的内容更多，但是连续确定出的位姿对应的路径更加平滑，如图2所示。图2为不同采集频率确定出的位姿对应的路径，可见低采集频率由于两帧之间的位姿由于没有解算导致确定出的路径不够平滑，这对于无人车确定控制策略的准确度会产生影响。因此，目前基于VIO技术的定位过程，通常都是兼顾对算力资源要求和定位频率，确定的图像传感器采集频率。
但是对于本说明书提供的方法来说，主要是针对两帧之前位姿变化较小的情况，简化定位需要解算的内容，因此在拥有同样算力资源的情况下，本说明书提供的方法可以并且也应用采用更高的图像采集频率，更高的采集频率使得两帧间的位姿变化可以更小，更有利于通过本说明书提供的定位流程确定位姿。而无人车也可以根据确定出的各帧的位姿确定更平滑的路径，提高无人车确定控制策略的准确性。
S102：确定所述当前帧的图像与上一帧的图像的图像特征点的匹配结果。
在本说明书中，该无人车获取当前帧的图像以及IMU数据后，可先确定当前帧的图像与上一帧的图像的图像特征点的匹配结果，以便后续步骤可以判断该无人车在两帧之间的位姿变化是否小到可以进行简化计算。
具体的，首先该无人车可针对当前帧采集的图像进行特征点提取，确定该图像中的各图像特征点，通常来说确定出的特征点都是图像中的角点。
之后，根据该IMU数据，确定当前帧与上一帧之间该无人车的预估运动方向，以对上一帧的图像中已经提取出的各图像特征点以及当前帧的图像的各图像特征点进行图像特征点匹配，确定相邻两帧的图像特征点的匹配结果。
当然，由于现有基于视觉定位与地图构建(Visual SimultaneousLocalizationAnd Mapping，VSLAM)等技术中，如何确定角点以及如何对两帧之间的图像特征点进行匹配已经是较为成熟的技术，因此对于这一过程的详细过程本说明书不再赘述。
S104：根据所述匹配结果以及所述惯性测量数据，确定无人车在当前帧与上一帧之间的预估位姿变化。
在本说明书中，该无人车在确定图像特征带你的匹配结果后，根据匹配结果以及IMU数据，确定无人车在当前帧与上一帧之间的预估位姿变化，以便后续判断是否可以简化计算位姿的过程。
具体的，在本说明书中，该IMU数据为无人车在当前帧与上一帧之间IMU采集的数据的积分结果，通常积分结果可包括：该IMU在三个维度上的位置的变化，以及该IMU在该三个维度的坐标轴上的滚动角度(或称为旋转角度)。于是，通过确定出的IMU数据，无人车可以预估在当前帧与上一帧之间位姿变化。当然，由于IMU数据的积分结果的准确度较低，通常会出现“漂移”现象，因此通常IMU数据进行作为基础数据用于解算位姿，而不能直接将IMU数据作为输出的位姿结果。
而根据该匹配结果，该无人车可确定当前帧与上一帧的图像特征点中有哪些是匹配的，有哪些是不匹配的，因此可确定匹配的图像特征点占全部图像特征点的比例。由于若匹配的图像特征点占比越多，则可说明书两帧图像之间的差异越小，因此该比例反映了两帧之间无人车位姿变化的程度。
并且，进一步地，无人车可针对确定匹配的每个图像特征点，确定该图像特征点在当前帧的图像中的坐标与在上一帧的图像中的坐标的差值，也就是该图像特征点在两帧图像中的位移，同样各图像特征点在两帧图像中位移也可反映两帧之间无人车位姿变化的程度。以极端情况为例，若当前帧与上一帧之间，无人车没有移动，则两帧图像中的所有图像特征点都应匹配，且每个匹配的图像特征点在两帧图像中的位移都应为0。
于是，该无人车可根据该IMU数据、匹配的图像特征点占比以及各匹配的图像特征点的位移，确定无人车在当前帧与上一帧之间的预估位姿变化。具体可根据图像传感器的内参，确定图像特征点位移对应的实际距离位移，并通过融合IMU数据的位移、实际距离位移等数据，确定预估位姿变化。当然，预估位姿变化在本说明书中仅用于后续步骤判断是否可以简化操作，因此为了节省算力资源，提高定位速度，可以选择误差较大但是速度较快的计算方法。
或者，该无人车也可直接将上述确定出的IMU数据、匹配的图像特征点占比以及各匹配的图像特征点的位移，作为该预估位姿变化，对本说明书最终确定位姿不会产生影响。
S106：当所述预估位姿变化小于预设值时，确定当前帧的待解的状态向量，根据基于先验残差项、视觉残差项以及惯性传感器残差项确定的目标函数以及所述待解的状态向量，确定所述待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式。.
在本说明书中，当确定预估位姿变化后，该无人车可判断该预估位姿变化是否小于预设值，若小于则执行本说明书提供的简化计算过程，即步骤S106～S112的过程，而若不小于，则说明书在两帧之间无人车进行了较大范围的移动，该无人车可以采用现有构建当前帧的系数矩阵以及残差的方法，解算该无人车当前帧的位姿，具体过程在本说明书中就不再赘述。
具体的，以预估位姿变化由IMU数据、匹配的图像特征点占比以及各匹配的图像特征点的位移组成为例，该无人车可分别进行以下判断：IMU数据中的三维坐标的位置移动是否均小于第一阈值，IMU数据中的三维坐标的旋转角度变化是否均小于第二阈值，匹配的图像特征点占比是否大于第三阈值，各匹配的图像特征点的位移是否均小于第四阈值，若判断结果均为是，则确定预估位姿变化小于预设值，若任一判断结果为否，则确定预估位姿变化不小于预设值。
针对本说明书提供的简化计算的过程，也就是当该无人车确定预估位姿变化小于预设值时，该无人车可确定当前帧待解的状态向量。具体的，该状态向量可通过公式X＝(x1x2 x3 … xn P1 P2 P3 … Pm)T表示，其中，xk＝(pk vk qk bak bgk)，1≤k≤n，表示第k帧的位姿向量，当前帧为第n帧，(P1 P2 P3 … Pm)表示前帧对应的环境点向量，可见当前帧对应的环境点一共有m个。需要说明的是，环境点是指图像特征点对应在实际环境中的点，可用Pl＝(Plx Ply Plz 1)，1≤l≤m，表示第l个环境点的四维齐次坐标，并且，m个环境点中可包括当前帧的图像中图像特征点对应的环境点，也可包括当前行驶过程中其他帧的图像的图像特征点对应的环境点。也就是说，在解当前帧的状态向时，会参考历史上其他帧的解算结果，通过目标函数对解进行约束。
进一步地，对于位姿向量xk中的各项，pk＝(pkx pky pkz)为第k帧相对第k-1的三维位移矢量，vk＝(vkx vky vkz)为第k帧相对第k-1帧的三维速度矢量，qk＝(qkw qkx qky qkz)为第k帧相对第k-1帧的四维旋转四元数，bak＝(bakx baky bakz)为第k帧所对应的加速度传感器的三维零偏矢量，bgk＝(bgkx bgky bgkz)为第k帧所对应的角速度传感器的三维零偏矢量。
所述目标函数具体可用公式 表示，也就是要解算得到状态向量的约束条件，即，先验残差项、视觉残差项以及惯性传感器残差项之和最小。
其中，resprior(rprior,X)为服从正态分布的先验残差项，其定义为表示历史上确定出的环境点的坐标与根据解算出的状态向量确定出的环境点的坐标的差值最小。由于历史上其他帧采集的图像的图像特征点，可能不在当前帧的图像中，因此对于这种图像特征点对应的环境点，实际上是无法根据当前帧采集的图像或者IMU数据解算的，但是这种图像特征点对应的环境点仍可作为一种约束条件，约束解算出的状态向量，因此在目标函数中设置该先验残差项。
为服从正态分布的视觉残差项，其定义为其中，/>为图像传感器的内参矩阵，fx和fy表示图像传感器的镜头焦距，并分别表示在图像x轴以及y轴方向上的缩放倍率，(cx，cy)表示图像传感器的光心坐标，或者可以理解为光轴与图像平面交点坐标，通常与图像中心点的坐标重叠。Tk＝(Rk·pk)定义为第k帧相对于第k-1帧的位姿矩阵，pk为第k帧相对第k-1的三维位移矢量，Rk为qk对应的旋转矩阵，ul＝(ulx uly 1)表示第l个环境点对应的图像特征点在图像中的三维齐次坐标。其中，ul分别对应为当前帧的图像与上一帧的图像配的图像特征点，也就是说，未出现在当前帧的图像中，或者不匹配的图像特征点不能作为视觉残差项的内容，用于约束解算出的状态向量。
为服从正态分布的惯性传感器残差项，定义为
可见是针对状态向量中的位姿向量的约束项，其中，Δt表示两帧之间的时间差，g为常数项表示无人车行驶地区的重力加速度，该定义式中/>表示p对bak导数的，以此类推可确定定义式中的定义。惯性传感器残差项中第一行表示在减去重力加速度的影响后，约束IMU传感器采集的IMU数据位移变化量最小，同理第二行为减去重力加速度的影响后，约束IMU传感器采集的IMU数据速度变化量最小，第三行表示约束旋转四元数的变化量最小，第四行以及第五行分别表示约束加速度以及角速度的零偏矢量变化量最小。
更进一步地，为了简化计算对该目标函数进行处理，由于状态向量X最小化为目标，也就是使上一帧的状态向量与当前帧待解的状态向量的ΔX最小，因此可推导目标函数为：
对上式各残差项展开，并令其对ΔX的导数等于零，可以推导出
将上式简化记为AΔX＝b。并且，可将A视为待解系数矩阵(为对称矩阵)，b为待解残差。将公式AΔX＝b进一步矩阵展开，以便于后续根据上一帧的解算出的系数矩阵、状态向量以及残差，解算当前帧的状态向量。得到/>其中，C为与位姿相关的系数矩阵，P为与环境点相关的系数矩阵，B为重叠项，Δx为位姿向量的变化量，ΔP为环境点向量的变化量，bx为与位姿向量的相关的残差，bP为与环境点相关的残差。
最后，该服务器可继续对根据以及bk+1＝bk+δb展开，得到待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式。
S108：根据所述矩阵关系式，确定所述状态向量的变化矩阵中的环境点向量的变化量求解式，对所述求解式进行展开，得到展开后的低阶项和高阶项，并根据将所述求解式展开后的低阶项解算所述环境点向量的变化量。
S110：根据所述矩阵关系式和解算得到的环境点向量的变化量，解算所述位姿向量的变化量。
在本说明书中，在确定出该矩阵关系式之后，可确定所述状态向量的变化矩阵中的环境点向量的变化量求解式，通过删除求解式中的高阶项，根据求解式展开后的低阶项解算当前帧的环境点向量的变化量。再根据解算出的环境点向量的变化量，求解位姿向量的变化量。
具体的，以当前帧为k+1帧为例，则上述矩阵关系式具体为
通过舒尔补进行消元以简化计算得到：
继续推导得到：
提取状态向量的变化矩阵中的环境点向量的变化量求解式，得到：
设/> 因此有：
线性展开后保留低阶项得到：/>
采用同样的方式可以得到 可见此时已经没有当前帧的系数矩阵以及残差，也就是基于上一帧的系数矩阵以及残差，来解算当前帧的位姿向量的变化量和环境点向量的变化量。计算复杂程度相较于现有构建系数矩阵和残差，进行求解的过程更加简单。
S112：根据解算得到所述环境点向量的变化量、所述位姿向量的变化量以及上一帧的状态向量，确定当前帧的位姿，以供所述无人车确定控制策略。
在本说明书中，当解算得到当前帧的位姿向量的变化量和环境点向量的变化量，便可基于上一帧的状态向量，确定当前帧的位姿向量，从而确定但前帧无人车的位姿。该当前帧的位姿，可用于无人车确定控制策略。
基于图1所示的基于视觉以及惯性里程定位的方法，根据相邻帧的图像的特征点匹配结果，以及惯性测量数据，确定当前帧与上一帧的预估位姿变化，并当该预估位姿变化小于预设值时，确定当前帧待解的状态向量，并根据目标函数，确定该待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式，通过该矩阵关系式，确定状态向量的变化矩阵中的环境点向量变化量求解式，通过忽略高阶项，求解环境点向量的变化量，以及根据忽略高阶项的位姿向量的变化量求解式，求解位姿向量的变化量，最后根据解算得到的环境点向量的变化量以及位姿向量的变化量，解算当前帧的位姿。在预估位姿变化较小的情况下，确定位姿变化是线性的并且可根据上一帧的系数矩阵、状态向量以及残差推导得到，因此在相邻帧之间构造了系数矩阵和残差的增量表达式，从而推导出求解式，通过求解增量方程来得到全部状态向量，得到当前帧的位姿。在位姿解算过程中，无需解算当前帧的系数矩阵以及残差，减少了位姿解算的复杂度，可以有效提高定位效率。
通过本说明书提供的定位的方法，基于现有采用非线性优化求解位姿的方案，通过先判断两帧间无人车的位姿变化是否小于预设值。若小于预设值则说明位姿变化较小，并可将两帧间无人车的位姿变化视为是线性的。然后将目标函数作为一种约束条件，也就是位姿线性变化后得到的当前帧位姿应该符合的条件，利用目标函数确定矩阵关系式。该矩阵关系式就基于线性变化的位姿以及约束条件构建的，通过分别求解该矩阵关系式中的环境点向量和位姿向量的变化量，得到当前帧状态向量与上一帧的状态向量的变化量，最后基于上一帧的状态向量，确定当前帧无人车的位姿。
另外，由于本说明书提供的定位方法，可在位姿变化较小的情况下通过简化计算，减少对算力资源的要求，因此可以解算状态向量的过程中使用更多历史观测量对解进行约束，使得解算得到的位姿更加准确，并且鲁棒性更高。
进一步地，在本说明书中，以无人车执行该定位过程为例进行说明，但若以现有VIO系统为例，则步骤S100～S102可以是前端(即无人车)执行的步骤，步骤S104～步骤S112为后端执行的步骤，后端通常为服务器。
另外，本说明书还提供一个完整的定位过程的示意图，如图3所示。该定位过程包括：
S200：获取当前帧的图像以及惯性测量数据；
S202：确定所述当前帧的图像与上一帧的图像的图像特征点的匹配结果；
S204：根据所述匹配结果以及所述惯性测量数据，确定无人车在当前帧与上一帧之间的预估位姿变化；
S206：判断所述预估位姿变化是否小于预设值，若是，则执行步骤S208，若否，则执行步骤S210；
S208：确定当前帧的待解的状态向量，根据基于先验残差项、视觉残差项以及惯性传感器残差项确定的目标函数以及所述待解的状态向量，确定所述待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式，其中，状态向量包含环境点向量以及位姿向量，环境点向量包含各图像特征点对应的环境点的待解坐标；根据所述矩阵关系式，确定所述状态向量的变化矩阵中的环境点向量的变化量求解式，对所述求解式进行展开，得到展开后的低阶项和高阶项，并根据将所述求解式展开后的低阶项解算所述环境点向量的变化量；根据所述矩阵关系式和解算得到的环境点向量的变化量，解算所述位姿向量的变化量；
S210：解算当前帧的系数矩阵以及残差；
S212：根据解算得到的结果，确定当前帧的位姿，以供所述无人车确定控制策略。
其中，步骤S208即本说明书步骤S106～步骤S110的过程，步骤S210即现有技术解算位姿时的过程。
并且，若通过步骤S208得到的解算结果，则还无人车还需要根据解算结果以及上一帧的状态向量，确定当前帧的位姿，若通过步骤S210得到的解算结果，则可确定解算结果中包含的位姿向量，作为当前帧的位姿。
进一步地，在本说明书中无人车是对无人驾驶设备的统称，对无人驾驶的结构或者形式不构成限制。该无人车可为用于无人配送，本说明书提供的上述定位方法具体可应用于使用无人车进行配送的领域中，当无人车进行配送时，通过该定位方法，确定自身位置，以确定控制策略并控制无人车行动，如，使用无人车进行快递、外卖等配送的场景。
基于图1所示的基于视觉以及惯性里程定位的过程，本说明书实施例还对应提供拦基于视觉以及惯性里程定位的装置的结构示意图，如图4所示。
图4为本说明书实施例提供的基于视觉以及惯性里程定位的装置的结构示意图，所述装置包括：
获取模块300，获取当前帧的图像以及惯性测量数据；
匹配模块302，确定所述当前帧的图像与上一帧的图像的图像特征点的匹配结果；
预估模块304，根据所述匹配结果以及所述惯性测量数据，确定所述装置在当前帧与上一帧之间的预估位姿变化；
关系式确定模块306，当所述预估位姿变化小于预设值时，确定当前帧的待解的状态向量，根据基于先验残差项、视觉残差项以及惯性传感器残差项确定的目标函数以及所述待解的状态向量，确定所述待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式，其中，状态向量包含环境点向量以及位姿向量，环境点向量包含各图像特征点对应的环境点的坐标；
第一解算模块308，根据所述矩阵关系式，确定所述状态向量的变化矩阵中的环境点向量的变化量求解式，对所述求解式进行展开，得到展开后的低阶项和高阶项，并根据将所述求解式展开后的低阶项解算所述环境点向量的变化量；
第二解算模块310，根据所述矩阵关系式和解算得到的环境点向量的变化量，解算所述位姿向量的变化量；
定位模块312，根据解算得到所述环境点向量的变化量、所述位姿向量的变化量以及上一帧的状态向量，确定当前帧的位姿，以供所述装置确定控制策略。
可选地，所述关系式确定模块306，根据当前行驶过程中每帧采集的图像，确定各图像的图像特征点对应的环境点的四维齐次坐标，根据确定出的各环境点的四维齐次坐标，确定待解环境点向量，确定所述当前帧相对于上一帧的待解位姿向量，根据所述待解位姿向量以及所述待解环境点向量，确定当前帧待解的状态向量。
可选地，所述关系式确定模块306，根据所述当前帧相对于上一帧的三维位移矢量、三维速度矢量、四维旋转四元数、惯性测量数据中加速度三维零偏矢量以及惯性测量数据中角速度三维零偏矢量，确定待解位姿向量。
可选地，所述预估模块304，根据所述匹配结果，确定当前帧的图像与上一帧图像中匹配的各图像特征点，确定各图像特征点在所述两帧图像中的位置变化，确定所述惯性测量数据在各坐标上的积分结果，根据确定出的各图像特征点的位置变化、各图像特征点的匹配结果以及所述积分结果，确定预估位姿变化。
可选地，所述关系式确定模块306，根据当前行驶过程中历史上各帧已经解算出坐标的各环境点，以及所述待解的状态向量中的环境点向量，确定先验残差项，针对所述当前帧的图像的每个图像特征点，根据采集图像设备的内参矩阵、所述待解的状态向量中的位姿向量的旋转四元数以及待解的状态向量中的该图像特征点对应环境点的坐标，确定该图像特征点在图像中待解的三维齐次坐标，并根据该图像特征点在图像中实际的三维其次坐标与所述待解的三维齐次坐标，确定该图像特征点的视觉残差项，针对所述待解位姿向量中的三维位移矢量、所述三维速度矢量、所述四维旋转四元数、所述加速度三维零偏矢量以及所述角速度三维零偏矢量，根据上一帧解算得到的结果，确定惯性传感器残差项，基于先验残差项、视觉残差项以及惯性传感器残差项，确定使各残差项之和最小的待解的状态向量，作为目标函数。
可选地，所述第一解算模块308，将所述矩阵关系式中的所述待解系数矩阵展开为上一帧的系数矩阵与系数矩阵变化量，所述待解残差矩阵展开为上一帧的残差矩阵与残差矩阵变化量，对展开后的矩阵关系式进行舒尔补消元，确定消元后所述状态向量的变化矩阵中的环境点向量的变化量求解式。
可选地，所述第一解算模块308，根据将所述求解式展开后的低阶项、上一帧解算出的系数矩阵以及上一帧解算出的残差矩阵，解算所述环境点向量的变化量，所述第二解算模块310，根据解算得到的环境点向量的变化量、所述矩阵关系式、上一帧解算出的系数矩阵以及上一帧解算出的残差矩阵，解算所述位姿向量的变化量。
其中，所述装置可以是无人车中用于确定位姿的装置。则确定出的当前帧的位姿，可以用于确定该装置所在的无人车确定控制策略。
本说明书实施例还提供了计算机可读存储介质，该存储介质存储有计算机程序，计算机程序可用于执行上述基于视觉以及惯性里程定位的方法中的任一个。
基于图1提供的基于视觉以及惯性里程定位的过程，本说明书实施例还提出了图5所示的无人车的示意结构图。如图5，在硬件层面，该无人车包括处理器、内部总线、网络接口、内存以及非易失性存储器，处理器从非易失性存储器中读取对应的计算机程序到内存中然后运行，以实现上述基于视觉以及惯性里程定位的方法中的任一个。当然，该无人车中还可包括实现无人车其他功能所需要的硬件。例如，无人车移动所需的移动装置、进行通信所需的通信装置、采集周围环境信息的电子设备等等。
当然，除了软件实现方式之外，本说明书并不排除其他实现方式，比如逻辑器件抑或软硬件结合的方式等等，也就是说以下处理流程的执行主体并不限定于各个逻辑单元，也可以是硬件或逻辑器件。
在20世纪90年代，对于一个技术的改进可以很明显地区分是硬件上的改进(例如，对二极管、晶体管、开关等电路结构的改进)还是软件上的改进(对于方法流程的改进)。然而，随着技术的发展，当今的很多方法流程的改进已经可以视为硬件电路结构的直接改进。设计人员几乎都通过将改进的方法流程编程到硬件电路中来得到相应的硬件电路结构。因此，不能说一个方法流程的改进就不能用硬件实体模块来实现。例如，可编程逻辑器件(Programmable Logic Device,PLD)(例如现场可编程门阵列(Field Programmable GateArray，FPGA))就是这样一种集成电路，其逻辑功能由用户对器件编程来确定。由设计人员自行编程来把一个数字系统“集成”在一片PLD上，而不需要请芯片制造厂商来设计和制作专用的集成电路芯片。而且，如今，取代手工地制作集成电路芯片，这种编程也多半改用“逻辑编译器(logic compiler)”软件来实现，它与程序开发撰写时所用的软件编译器相类似，而要编译之前的原始代码也得用特定的编程语言来撰写，此称之为硬件描述语言(Hardware Description Language，HDL)，而HDL也并非仅有一种，而是有许多种，如ABEL(Advanced Boolean Expression Language)、AHDL(Altera Hardware DescriptionLanguage)、Confluence、CUPL(Cornell University Programming Language)、HDCal、JHDL(Java Hardware Description Language)、Lava、Lola、MyHDL、PALASM、RHDL(RubyHardware Description Language)等，目前最普遍使用的是VHDL(Very-High-SpeedIntegrated Circuit Hardware Description Language)与Verilog。本领域技术人员也应该清楚，只需要将方法流程用上述几种硬件描述语言稍作逻辑编程并编程到集成电路中，就可以很容易得到实现该逻辑方法流程的硬件电路。
控制器可以按任何适当的方式实现，例如，控制器可以采取例如微处理器或处理器以及存储可由该(微)处理器执行的计算机可读程序代码(例如软件或固件)的计算机可读介质、逻辑门、开关、专用集成电路(Application Specific Integrated Circuit，ASIC)、可编程逻辑控制器和嵌入微控制器的形式，控制器的例子包括但不限于以下微控制器：ARC 625D、Atmel AT91SAM、Microchip PIC18F26K20以及Silicone Labs C8051F320，存储器控制器还可以被实现为存储器的控制逻辑的一部分。本领域技术人员也知道，除了以纯计算机可读程序代码方式实现控制器以外，完全可以通过将方法步骤进行逻辑编程来使得控制器以逻辑门、开关、专用集成电路、可编程逻辑控制器和嵌入微控制器等的形式来实现相同功能。因此这种控制器可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构。或者甚至，可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。
为了描述的方便，描述以上装置时以功能分为各种单元分别描述。当然，在实施本说明书时可以把各单元的功能在同一个或多个软件和/或硬件中实现。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。
内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
本领域技术人员应明白，本说明书的实施例可提供为方法、系统或计算机程序产品。因此，本说明书可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本说明书可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本说明书可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构等等。也可以在分布式计算环境中实践本说明书，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
以上所述仅为本说明书的实施例而已，并不用于限制本说明书。对于本领域技术人员来说，本说明书可以有各种更改和变化。凡在本说明书的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本说明书的权利要求范围之内。
Claims (10)
1.一种基于视觉以及惯性里程定位的方法，其特征在于，包括：
获取当前帧的图像以及惯性测量数据；
确定所述当前帧的图像与上一帧的图像的图像特征点的匹配结果；
根据所述匹配结果以及所述惯性测量数据，确定无人车在当前帧与上一帧之间的预估位姿变化；
当所述预估位姿变化小于预设值时，确定当前帧的待解的状态向量，根据基于先验残差项、视觉残差项以及惯性传感器残差项确定的目标函数以及所述待解的状态向量，确定所述待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式，其中，状态向量包含环境点向量以及位姿向量，环境点向量包含各图像特征点对应的环境点的待解坐标；
根据所述矩阵关系式，确定所述状态向量的变化矩阵中的环境点向量的变化量求解式，对所述求解式进行展开，得到展开后的低阶项和高阶项，并根据将所述求解式展开后的低阶项解算所述环境点向量的变化量；
根据所述矩阵关系式和解算得到的环境点向量的变化量，解算所述位姿向量的变化量；
根据解算得到所述环境点向量的变化量、所述位姿向量的变化量以及上一帧的状态向量，确定当前帧的位姿，以供所述无人车确定控制策略。
2.如权利要求1所述的方法，其特征在于，确定当前帧的待解的状态向量，具体包括：
根据当前行驶过程中每帧采集的图像，确定各图像的图像特征点对应的环境点的四维齐次坐标；
根据确定出的各环境点的四维齐次坐标，确定待解环境点向量；
确定所述当前帧相对于上一帧的待解位姿向量；
根据所述待解位姿向量以及所述待解环境点向量，确定当前帧的待解的状态向量。
3.如权利要求2所述的方法，其特征在于，确定所述当前帧相对于上一帧的待解位姿向量，具体包括：
根据所述当前帧相对于上一帧的三维位移矢量、三维速度矢量、四维旋转四元数、惯性测量数据中加速度三维零偏矢量以及惯性测量数据中角速度三维零偏矢量，确定待解位姿向量。
4.如权利要求1所述的方法，其特征在于，根据所述匹配结果以及所述惯性测量数据，确定所述无人车在当前帧与上一帧之间的预估位姿变化，具体包括：
根据所述匹配结果，确定当前帧的图像与上一帧图像中匹配的各图像特征点；
确定各图像特征点在所述两帧图像中的位置变化；
确定所述惯性测量数据在各坐标上的积分结果；
根据确定出的各图像特征点的位置变化、各图像特征点的匹配结果以及所述积分结果，确定预估位姿变化。
5.如权利要求3所述的方法，其特征在于，基于先验残差项、视觉残差项以及服从惯性传感器残差项确定的目标函数，具体包括：
根据当前行驶过程中历史上各帧已经解算出坐标的各环境点，以及所述待解的状态向量中的环境点向量，确定先验残差项；
针对所述当前帧的图像的每个图像特征点，根据采集图像设备的内参矩阵、所述待解的状态向量中的位姿向量的旋转四元数以及待解的状态向量中的该图像特征点对应环境点的坐标，确定该图像特征点在图像中待解的三维齐次坐标，并根据该图像特征点在图像中实际的三维齐次坐标与所述待解的三维齐次坐标，确定该图像特征点的视觉残差项；
针对所述待解位姿向量中的三维位移矢量、所述三维速度矢量、所述四维旋转四元数、所述加速度三维零偏矢量以及所述角速度三维零偏矢量，根据上一帧解算得到的结果，确定惯性传感器残差项；
基于先验残差项、视觉残差项以及惯性传感器残差项，确定使各残差项之和最小的待解的状态向量，作为目标函数。
6.如权利要求1所述的方法，其特征在于，根据所述矩阵关系式，确定所述状态向量的变化矩阵中的环境点向量的变化量求解式，具体包括：
将所述矩阵关系式中的所述待解系数矩阵展开为上一帧的系数矩阵与系数矩阵变化量，所述待解残差矩阵展开为上一帧的残差矩阵与残差矩阵变化量；
对展开后的矩阵关系式进行舒尔补消元，确定消元后所述状态向量的变化矩阵中的环境点向量的变化量求解式。
7.如权利要求6所述的方法，其特征在于，根据将所述求解式展开后的低阶项解算所述环境点向量的变化量，具体包括：
根据将所述求解式展开后的低阶项、上一帧解算出的系数矩阵以及上一帧解算出的残差矩阵，解算所述环境点向量的变化量；
根据解算得到的环境点向量的变化量、所述矩阵关系式，解算所述位姿向量的变化量，具体包括：
根据解算得到的环境点向量的变化量、所述矩阵关系式、上一帧解算出的系数矩阵以及上一帧解算出的残差矩阵，解算所述位姿向量的变化量。
8.一种基于视觉以及惯性里程定位的装置，其特征在于，包括：
获取模块，获取当前帧的图像以及惯性测量数据；
匹配模块，确定所述当前帧的图像与上一帧的图像的图像特征点的匹配结果；
预估模块，根据所述匹配结果以及所述惯性测量数据，确定所述装置在当前帧与上一帧之间的预估位姿变化；
关系式确定模块，当所述预估位姿变化小于预设值时，确定当前帧的待解的状态向量，根据基于先验残差项、视觉残差项以及惯性传感器残差项确定的目标函数以及所述待解的状态向量，确定所述待解的状态向量的变化矩阵、待解系数矩阵以及待解残差矩阵的矩阵关系式，其中，状态向量包含环境点向量以及位姿向量，环境点向量包含各图像特征点对应的环境点的坐标；
第一解算模块，根据所述矩阵关系式，确定所述状态向量的变化矩阵中的环境点向量的变化量求解式，对所述求解式进行展开，得到展开后的低阶项和高阶项，并根据将所述求解式展开后的低阶项解算所述环境点向量的变化量；
第二解算模块，根据所述矩阵关系式和解算得到的环境点向量的变化量，解算所述位姿向量的变化量；
定位模块，根据解算得到所述环境点向量的变化量、所述位姿向量的变化量以及上一帧的状态向量，确定当前帧的位姿，以供所述装置确定控制策略。
9.一种计算机可读存储介质，其特征在于，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述权利要求1-7任一所述的方法。
10.一种无人车，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现上述权利要求1-7任一所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010543589.5A CN111797906B (zh) | 2020-06-15 | 2020-06-15 | 基于视觉和惯性里程定位的方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010543589.5A CN111797906B (zh) | 2020-06-15 | 2020-06-15 | 基于视觉和惯性里程定位的方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111797906A CN111797906A (zh) | 2020-10-20 |
| CN111797906B true CN111797906B (zh) | 2024-03-01 |
Family
ID=72804730
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010543589.5A Active CN111797906B (zh) | 2020-06-15 | 2020-06-15 | 基于视觉和惯性里程定位的方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111797906B (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113324542B (zh) * | 2021-06-07 | 2024-04-12 | 北京京东乾石科技有限公司 | 一种定位方法、装置、设备和存储介质 |
| CN113223007A (zh) * | 2021-06-28 | 2021-08-06 | 浙江华睿科技股份有限公司 | 视觉里程计的实现方法、装置及电子设备 |
| CN115170847B (zh) * | 2022-09-05 | 2022-12-13 | 煤炭科学研究总院有限公司 | 基于先验地图约束的煤矿井下单目视觉定位方法及装置 |
| CN117705107B (zh) * | 2024-02-06 | 2024-04-16 | 电子科技大学 | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109974693A (zh) * | 2019-01-31 | 2019-07-05 | 中国科学院深圳先进技术研究院 | 无人机定位方法、装置、计算机设备及存储介质 |
| CN110246147A (zh) * | 2019-05-14 | 2019-09-17 | 中国科学院深圳先进技术研究院 | 视觉惯性里程计方法、视觉惯性里程计装置及移动设备 |
| CN111260726A (zh) * | 2020-02-07 | 2020-06-09 | 北京三快在线科技有限公司 | 一种视觉定位方法及装置 |
-
2020
- 2020-06-15 CN CN202010543589.5A patent/CN111797906B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109974693A (zh) * | 2019-01-31 | 2019-07-05 | 中国科学院深圳先进技术研究院 | 无人机定位方法、装置、计算机设备及存储介质 |
| CN110246147A (zh) * | 2019-05-14 | 2019-09-17 | 中国科学院深圳先进技术研究院 | 视觉惯性里程计方法、视觉惯性里程计装置及移动设备 |
| CN111260726A (zh) * | 2020-02-07 | 2020-06-09 | 北京三快在线科技有限公司 | 一种视觉定位方法及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111797906A (zh) | 2020-10-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111797906B (zh) | 基于视觉和惯性里程定位的方法及装置 | |
| CN111882611B (zh) | 一种地图构建方法及装置 | |
| US20130162785A1 (en) | Method and system for fusing data arising from image sensors and from motion or position sensors | |
| CN111288971B (zh) | 一种视觉定位方法及装置 | |
| CN111238450B (zh) | 视觉定位方法及装置 | |
| CN111077555A (zh) | 一种定位方法及装置 | |
| CN111127551B (zh) | 一种目标检测的方法及装置 | |
| CN111508258A (zh) | 一种定位方法及装置 | |
| CN111062372B (zh) | 一种预测障碍物轨迹的方法及装置 | |
| US20220314980A1 (en) | Obstacle tracking method, storage medium and unmanned driving device | |
| CN116740361B (zh) | 一种点云分割方法、装置、存储介质及电子设备 | |
| CN116309823A (zh) | 一种位姿的确定方法、装置、设备及存储介质 | |
| CN113674424B (zh) | 一种电子地图绘制的方法及装置 | |
| CN113048989B (zh) | 一种无人驾驶设备的定位方法及定位装置 | |
| CN111798489B (zh) | 一种特征点跟踪方法、设备、介质及无人设备 | |
| CN117333508A (zh) | 一种目标跟踪方法、装置、设备及介质 | |
| CN112461258A (zh) | 一种参数修正的方法及装置 | |
| CN112393723B (zh) | 一种定位方法、设备、介质及无人设备 | |
| CN116929407A (zh) | 一种自适应数据校准方法及装置 | |
| CN114111769B (zh) | 一种视觉惯性定位方法、装置及自动驾驶装置 | |
| CN112734851B (zh) | 一种位姿确定的方法以及装置 | |
| CN112712561A (zh) | 一种建图方法、装置、存储介质及电子设备 | |
| CN116051616B (zh) | 一种深度测量方法、装置、存储介质及电子设备 | |
| CN116558504B (zh) | 一种单目视觉的定位方法及装置 | |
| CN118053153B (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 |