CN117848331A - 基于视觉标签地图的定位方法及装置 - Google Patents
基于视觉标签地图的定位方法及装置 Download PDFInfo
- Publication number
- CN117848331A CN117848331A CN202410251328.4A CN202410251328A CN117848331A CN 117848331 A CN117848331 A CN 117848331A CN 202410251328 A CN202410251328 A CN 202410251328A CN 117848331 A CN117848331 A CN 117848331A
- Authority
- CN
- China
- Prior art keywords
- visual
- tag
- positioning
- positioning result
- sensor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 230000000007 visual effect Effects 0.000 title claims abstract description 348
- 238000000034 method Methods 0.000 title claims abstract description 80
- 230000001133 acceleration Effects 0.000 claims abstract description 41
- 238000001914 filtration Methods 0.000 claims abstract description 28
- 238000012545 processing Methods 0.000 claims abstract description 21
- 239000011159 matrix material Substances 0.000 claims description 52
- 238000001514 detection method Methods 0.000 claims description 35
- 238000013507 mapping Methods 0.000 claims description 21
- 230000008569 process Effects 0.000 claims description 20
- 238000006243 chemical reaction Methods 0.000 claims description 15
- 238000004422 calculation algorithm Methods 0.000 claims description 11
- 238000005259 measurement Methods 0.000 claims description 11
- 230000007704 transition Effects 0.000 claims description 9
- 238000012546 transfer Methods 0.000 claims description 6
- 238000007500 overflow downdraw method Methods 0.000 claims description 4
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000004590 computer program Methods 0.000 description 17
- 238000010586 diagram Methods 0.000 description 11
- 230000006870 function Effects 0.000 description 10
- 238000005516 engineering process Methods 0.000 description 7
- 238000012937 correction Methods 0.000 description 6
- 238000012795 verification Methods 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 125000004122 cyclic group Chemical group 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 206010039203 Road traffic accident Diseases 0.000 description 1
- 230000003321 amplification Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
本申请提供一种基于视觉标签地图的定位方法及装置，涉及陀螺仪定位技术领域，待定位设备包括惯性传感器和视觉传感器；该方法包括：对视觉传感器采集到标签信息，识别处理确定视觉传感器与目标视觉标签的相对位姿关系，并根据视觉标签地图确定目标视觉标签的绝对位姿；根据相对位姿、绝对位姿以及视觉传感器与待定位设备的相对位姿关系，确定待定位设备的绝对位姿以确定视觉定位结果；以视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，获得本历元的组合定位结果；其中，INS定位结果基于上一历元的组合定位结果和惯性传感器采集的加速度和角速度进行状态递推预测得出。本申请实现对INS定位累积误差的限制，提升定位精度。
Description
技术领域
本申请涉及陀螺仪定位技术领域，尤其涉及一种基于视觉标签地图的定位方法及装置。
背景技术
智能汽车技术在近年来取得了迅猛发展，其中，定位技术是其基础组成部分。精准的定位对于智能汽车至关重要，因为它关乎车辆在与外部环境相对位置上的准确性。若定位误差过大，可能导致驾驶安全问题，甚至引发交通安全事故。
当前，全球导航卫星系统（Global Navigation Satellite System，GNSS）定位技术是车辆主要依赖的定位方法。它利用卫星提供的伪距和位置信息计算天线位置，为车辆提供无累计误差的绝对定位信息。然而，GNSS定位技术的抗干扰能力较弱，易受高大建筑、树木、立交桥等遮挡，以及多路径效应和非视距信号影响，导致定位精度下降。在隧道、地下车库等无法接收卫星信号的室内场景中，定位失败，无法满足智能汽车对定位精度的需求。
惯性导航系统（Inertial Navigation System，INS）定位技术通过积分惯性测量单元提供的三轴加速度和三轴角速度，计算载体速度、位置及姿态，相较于GNSS定位技术具有采样频率高、实时性好的特点。在实现本申请实施例过程中，发现相关技术中至少存在如下技术问题：
INS定位技术误差会随时间累积，定位精度有待进一步提升。
发明内容
本申请实施例提供了一种基于视觉标签地图的定位方法及装置，以解决INS定位技术误差会随时间累积，定位精度有待进一步提升的问题。
第一方面，本申请实施例提供了一种基于视觉标签地图的定位方法，待定位设备包括设置在待定位设备本体上的惯性传感器和视觉传感器；该定位方法包括：
获取所述视觉传感器采集到的目标视觉标签的标签信息，对所述标签信息进行识别处理确定视觉传感器与所述目标视觉标签的相对位姿关系，并对所述标签信息进行识别处理确定所述目标视觉标签的标签编码；
根据所述目标视觉标签的标签编码和视觉标签地图确定所述目标视觉标签的绝对位姿；
根据所述视觉传感器与所述目标视觉标签的相对位姿关系、所述目标视觉标签的绝对位姿和所述视觉传感器与所述待定位设备的相对位姿关系进行坐标转换，确定所述待定位设备的绝对位姿，并根据所述待定位设备的绝对位姿确定视觉定位结果；
以所述视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，获得本历元的组合定位结果；
其中，所述INS定位结果基于上一历元的组合定位结果和所述惯性传感器采集的加速度和角速度，使用误差状态卡尔曼滤波进行状态递推预测得出。
在一种可能的实现方式中，所述INS定位结果的递推预测过程，包括：
将所述待定位设备上一历元的组合定位结果对应的真实状态量分为名义状态量/>和误差状态量/>两个状态量；其中，/>；
根据所述惯性传感器采集的加速度和角速度，对名义状态量和误差状态量/>进行递推，得到名义状态量/>的预测值/>，并根据系统噪声/>和状态转移矩阵/>得到所述待定位设备的误差状态量/>的协方差矩阵/>。
在一种可能的实现方式中，所述以所述视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，获得本历元的组合定位结果，包括：
以所述视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，得到误差状态量的后验均值/>及其协方差矩阵/>；
将名义状态量的预测值/>和误差状态量/>的后验均值/>进行合并，得到本历元的组合定位结果/>。
在一种可能的实现方式中，名义状态量的预测值/>和误差状态量/>的协方差矩阵/>的预测公式如下：
其中，为名义状态量/>的预测值；/>(·)为系统名义状态转移函数；/>为误差状态量；/>为惯性传感器观测得到的加速度；/>为惯性传感器观测得到的角速度；/>为两个历元的时间间隔；/>为上一历元的名义状态后验值；/>为误差状态量/>的预测值；/>为误差状态量的状态转移矩阵；/>为上一历元的误差状态后验值；/>为系统的误差状态量协方差矩阵；/>为上一历元的协方差矩阵后验；/>为系统噪声分配矩阵；/>为系统噪声；
其中，系统名义状态转移函数和误差状态转移矩阵/>可由如下名义状态量和误差状态量/>的递推公式确定：
其中，为位置预测量；/>为上一历元的位置后验量；/>为上一历元的速度后验量；/>为两个历元的时间间隔；/>为上一历元得到的由局部坐标系到全局坐标系的旋转矩阵；/>为加速度计测量的加速度；/>为上一历元的加速度计偏差后验量；/>为当地的重力加速度；/>为速度预测量；/>为以四元数表示的姿态预测量；/>为以四元数表示的上一历元姿态后验量；/>{ }为旋转矢量对应的姿态四元数；/>为陀螺仪测量的角速度；/>为上一历元的角速度偏差后验量；/>为加速度偏差预测量；/>为角速度偏差的预测量；/>为位置误差的预测量；/>为上一历元的位置误差后验量；/>为上一历元的速度误差后验量；/>为速度误差预测量；/>为上一历元的姿态误差后验量；/>为上一历元的加速度偏差误差后验量；/>为速度误差预测噪声；/>为姿态误差预测量；/>为上一历元的角速度偏差误差后验量；/>为姿态误差预测噪声；/>为加速度偏差误差预测量；/>为加速度偏差误差预测噪声；/>为角速度偏差误差预测量；/>为角速度偏差误差预测噪声。
在一种可能的实现方式中，当视觉传感器采集到多个目标视觉标签时，所述根据所述待定位设备的绝对位姿确定视觉定位结果，包括：
根据基于检测距离的加权融合方法，对基于各目标视觉标签确定的所述待定位设备的绝对位姿进行融合，得到视觉定位结果，并确定视觉定位结果的标准差。
在一种可能的实现方式中，所述对所述标签信息进行识别处理确定视觉传感器与采集到的视觉标签的相对位姿关系，包括：
对所述标签信息对应的原始视觉标签图像的RGB值进行非线性映射调整，得到修正视觉标签图像；
基于Apriltag检测算法对所述原始视觉标签图像和所述修正视觉标签图像进行二次检测，得到视觉标签角点检测结果和标签编号识别结果；其中，所述二次检测包括：灰度化图像、高斯滤波、二值化、union-find聚类、边界分割、四边形检测、解码；
根据所述视觉标签角点检测结果、标签编号识别结果、预设视觉标签尺寸和所述视觉传感器内参，确定视觉传感器与采集到的视觉标签的相对位姿关系。
在一种可能的实现方式中，所述对所述标签信息对应的原始视觉标签图像的RGB值进行非线性映射调整，得到修正视觉标签图像，包括：
根据如下非线性映射公式对原始视觉标签图像的RGB值进行映射调整：
其中，参数定义为：
；
为原始视觉标签图像的RGB值；/>为修正视觉标签图像的RGB值；/>为输入量下限阈值；/>为输入量上限阈值；/>输出量下限阈值；/>输出量上限阈值。
在一种可能的实现方式中，所述根据所述视觉标签角点检测结果、标签编号识别结果、预设视觉标签尺寸和所述视觉传感器内参，确定视觉传感器与采集到的视觉标签的相对位姿关系，包括：
根据预设视觉标签尺寸和角点在图像坐标系和标签坐标系下的关系，利用所述视觉标签角点检测结果四对角点求解出单应矩阵；其中，角点在图像坐标系和标签坐标系下的关系如下式：
其中，和/>为图像坐标系中坐标；/>和/>为标签坐标系坐标/>为相机内参矩阵；为旋转矩阵第1列；/>为旋转矩阵第2列；/>为两坐标系原点的位移向量；
根据相机内参矩阵及旋转矩阵的正交性，获得图像标签在相机坐标系下的位姿，以确定视觉传感器与采集到的视觉标签的相对位姿关系。
在一种可能的实现方式中，所述根据所述视觉传感器与所述目标视觉标签的相对位姿关系、所述目标视觉标签的绝对位姿和所述视觉传感器与所述待定位设备的相对位姿关系进行坐标转换，确定所述待定位设备的绝对位姿，包括：
基于如下换算公式进行坐标转换：
其中，和/>为所述视觉传感器与所述待定位设备的相对位姿；/>和/>为所述目标视觉标签在相机坐标系下的位姿；/>和/>为所述目标视觉标签的绝对位姿；/>为所述待定位设备的绝对位置；/>和/>为视觉标签在待定位设备坐标系下的位姿。
第二方面，本申请实施例提供了一种基于视觉标签地图的定位装置，待定位设备包括设置在待定位设备本体上的惯性传感器和视觉传感器；该定位装置包括：
第一位姿确定模块，用于获取所述视觉传感器采集到的目标视觉标签的标签信息，对所述标签信息进行识别处理确定视觉传感器与所述目标视觉标签的相对位姿关系，并对所述标签信息进行识别处理确定所述目标视觉标签的标签编码；
第二位姿确定模块，用于根据所述目标视觉标签的标签编码和视觉标签地图确定所述目标视觉标签的绝对位姿；
第三位姿确定模块，用于根据所述视觉传感器与所述目标视觉标签的相对位姿关系、所述目标视觉标签的绝对位姿和所述视觉传感器与所述待定位设备的相对位姿关系进行坐标转换，确定所述待定位设备的绝对位姿，并根据所述待定位设备的绝对位姿确定视觉定位结果；
定位模块，用于以所述视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，获得本历元的组合定位结果；
其中，所述INS定位结果基于上一历元的组合定位结果和所述惯性传感器采集的加速度和角速度，使用误差状态卡尔曼滤波进行状态递推预测得出。
第三方面，本申请实施例提供了一种电子设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上第一方面或第一方面的任一种可能的实现方式所述方法的步骤。
第四方面，本申请实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上第一方面或第一方面的任一种可能的实现方式所述方法的步骤。
本申请实施例提供一种基于视觉标签地图的定位方法及装置，通过对视觉传感器收集的目标视觉标签的标签信息进行识别与处理，确定视觉传感器与目标视觉标签的相对位姿关系，并确定目标视觉标签的标签编码。在此基础上，结合目标视觉标签的标签编码和视觉标签地图，求解出目标视觉标签的绝对位姿。将视觉传感器与目标视觉标签的相对位姿关系、目标视觉标签的绝对位姿和视觉传感器与待定位设备的相对位姿关系进行坐标转换，从而确定待定位设备的绝对位姿。进一步地，根据待定位设备的绝对位姿，获得视觉定位结果。此外，为了提升定位精度，将视觉定位结果与车辆运动约束相结合，构建卡尔曼滤波的观测量，对INS定位结果进行修正，从而获得本历元的组合定位结果。本申请实施例中，通过低成本、易识别、高鲁棒性的视觉标签构建视觉标签地图，通过一次观测即可得到相机与标签间的相对位姿，进而获得绝对定位结果，实现对INS累计误差的修正，适应不同场景下对INS定位累积误差的限制需求，从而在无GNSS环境下获得较高精度的定位结果。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本申请一实施例提供的基于视觉标签地图的定位方法的实现流程图；
图2是本申请一实施例提供的视觉标签的示意图；
图3是本申请一实施例提供的设置视觉标签的场景示意图；
图4是本申请另一实施例提供的设置视觉标签的场景示意图；
图5是本申请一实施例提供的Apriltag检测算法对视觉标签进行二次检测的示意图；
图6是本申请一实施例提供的基于视觉标签地图的定位方法的实现流程图，
图7是本申请一实施例提供定位验证装置；
图8是本申请一实施例提供定位验证装置的定位结果示意图；
图9是本申请一实施例提供的定位验证结果示意图；
图10是本申请一实施例提供的基于视觉标签地图的定位装置的结构示意图；
图11是本申请一实施例提供的电子设备的结构示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本申请。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。
本申请实施例的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请实施例的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。
除非另有说明，术语“多个”表示两个或两个以上。字符“/”表示前后对象是一种“或”的关系。例如，A/B表示：A或B。术语“和/或”是一种描述对象的关联关系，表示可以存在三种关系。例如，A和/或B，表示：A或B，或，A和B这三种关系。
本申请中使用的用词仅用于描述实施例并且不用于限制权利要求。如在实施例以及权利要求的描述中使用的，除非上下文清楚地表明，否则单数形式的“一个”（a）、“一个”（an）和“所述”（the）旨在同样包括复数形式。类似地，如在本申请中所使用的术语“和/或”是指包含一个或一个以上相关联的列出的任何以及所有可能的组合。另外，当用于本申请中时，术语“包括”（comprise）及其变型“包括”（comprises）和/或包括（comprising）等指陈述的特征、整体、步骤、操作、元素，和/或组件的存在，但不排除一个或一个以上其它特征、整体、步骤、操作、元素、组件和/或这些的分组的存在或添加。在没有更多限制的情况下，由语句“包括一个…”限定的要素，并不排除在包括所述要素的过程、方法或者设备中还存在另外的相同要素。
本申请中，每个实施例重点说明的可以是与其他实施例的不同之处，各个实施例之间相同相似部分可以互相参见。对于实施例公开的方法、产品等而言，如果其与实施例公开的方法部分相对应，那么相关之处可以参见方法部分的描述。
本发明公开了一种基于视觉标签地图的车辆INS与视觉组合定位方法，目的在于解决在无GNSS环境下，INS无法独立进行长时间精确定位的问题。通过引入视觉传感器和视觉标签地图，对已知绝对位置的视觉标签进行观测，并将图像对比度调整算法及多标签加权融合定位算法相结合，获得绝对定位结果。同时，借助车辆运动约束辅助的误差状态卡尔曼滤波与INS定位结果融合，实现无GNSS环境下的INS与视觉组合定位。在本申请实施例中，为实现待定位设备的定位，需要待定位设备至少包括设置在其本体上的惯性传感器和视觉传感器。惯性传感器能够检测待定位设备的侧倾角、俯仰角、偏转角和加速度。可选地，惯性传感器包括陀螺仪和加速度计。
本发明所提供的定位方案具有以下优势：视觉标签地图采用低成本、易识别、高鲁棒性的视觉标签构建，使得单次观测即可确定相机与标签间的相对位姿，进而获取待定位设备的绝对定位结果。视觉传感器体积紧凑、成本较低，可提供丰富的环境信息。例如：视觉传感器可以采用单目相机，简化视觉传感器配置。这将有助于修正INS累计误差，实现在无GNSS环境下的高精度定位。
待定位设备可以为自动驾驶车辆、无人配送物流车、无人配送机器人等。当用于自动驾驶车辆的定位时，对于降低交通事故、提高交通效率以及加速自动驾驶技术应用具有重要意义。
为使本申请的目的、技术方案和优点更加清楚，下面将结合附图通过具体实施例来进行说明。
图1是本申请一实施例提供的基于视觉标签地图的定位方法的实现流程图，如图1所示，该方法包括如下步骤：
S101，获取视觉传感器采集到的目标视觉标签的标签信息，对标签信息进行识别处理确定视觉传感器与目标视觉标签的相对位姿关系，并对标签信息进行识别处理确定目标视觉标签的标签编码。
在目标区域内布置视觉标签时，需对视觉标签进行编码，并依据标签编码结果构建标签地图，以便在定位过程中，根据视觉传感器对视觉标签的识别结果，确定待定位设备在标签地图中的大致方位。
标签编码可采用一维条形码或二维码展示，或通过数字及文字形式显现在视觉标签上，以便于识别处理。本申请实施例主要以二维码方式进行说明。
S102，根据目标视觉标签的标签编码和视觉标签地图确定目标视觉标签的绝对位姿。
本申请实施例提供的基于视觉标签地图的定位方法，主要执行主体为待定位设备的控制器或车载导航系统。该执行主体负责整合视觉传感器和惯性传感器的信息以实现定位。
如图2是根据一具体实施例示出的视觉标签示意图。图3和图4是根据不同实施例示出的场景示意图。视觉标签根据具体应用场景设置在待定位设备视觉传感器能够采集到的位置。
在此背景下，待定位设备周围环境中设有多个用于定位的视觉标签，根据待定位设备所处位置不同，部分定位标签被视觉传感器采集到。目标视觉标签可以是当前时刻视觉传感器采集到的所有标签，或当前时刻采集到的完整标签图像的标签，或当前时刻采集到的相对距离视觉传感器较近的标签。
在进行定位前，需对设有定位标签的环境构建标签地图。在构建标签地图前，对各视觉标签进行机械编码，并确定各定位标签在环境中的绝对定位。各视觉标签的图像能反映唯一的编码信息和绝对定位信息，以确保识别后实现准确定位。
实施过程中，如图2所示，需基于待定位设备建立设备坐标系，基于视觉传感器建立传感器坐标系，以及基于视觉标签建立标签坐标系。然后，根据目标视觉标签在传感器坐标系和标签坐标系中的位置和大小关系，确定视觉传感器与目标视觉标签的相对位姿关系。
S103，根据视觉传感器与目标视觉标签的相对位姿关系、目标视觉标签的绝对位姿和视觉传感器与待定位设备的相对位姿关系进行坐标转换，确定待定位设备的绝对位姿，并根据待定位设备的绝对位姿确定视觉定位结果；
如图3所示，视觉传感器往往设置在待定位设备前端位置或后端位置。如图3所示，视觉传感器通常被安装在待定位设备的前端或后端位置。以车辆为例，车辆后轮轴中心更能代表整个车辆的位置，而前视摄像头或后视摄像头仅能反映车辆局部位置信息。当需要确定车辆位置时，通常以车辆后轮轴中心为基准，而非以前视摄像头或后视摄像头为准。因此，在确定目标视觉标签与视觉传感器的相对位姿和目标视觉标签的绝对位姿后，还要进一步根据视觉传感器与待定位设备的相对位姿关系，确定待定位设备的绝对位姿。如图3所示，以车辆后轮轴中心为基准建立坐标，坐标轴为x b、y b和z b；以摄像头为基准建立坐标，坐标轴为x c、y c和z c；以视觉标签为基准建立坐标，坐标轴为x t、y t和z t。
在具体实施过程中，需要在待定位设备的系统中预置其重心（或其他表征待定位设备位置的点）与视觉传感器的相对位姿关系，这样，在获取到目标视觉标签的绝对位姿后，就可以根据预先设定的相对位姿关系进行坐标转换，从而得到待定位设备的绝对位姿。该视觉定位方式具有更高的精确度和可靠性。
S104，以视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，获得本历元的组合定位结果；其中，INS定位结果基于上一历元的组合定位结果和惯性传感器采集的加速度和角速度，使用误差状态卡尔曼滤波进行状态递推预测得出。
INS定位结果依赖于上一历元的组合定位成果及惯性传感器（如陀螺仪和加速度计）获取的加速度和角速度数据。通过误差状态卡尔曼滤波进行状态递推预测，得出INS定位结果。这一过程在每次历元中不断执行，以实现车辆运动状态的实时更新。
本申请实施例提出的组合定位方法只需一次观测便可获得视觉传感器与视觉标签间的相对位姿关系。在此基础上，根据视觉标签地图，可获取车辆的绝对定位结果。与传统基于循环神经网络进行定位的方法相比，该方法无需累积多帧图像进行计算，从而提升定位效率。同时，该方法避免了基于图像识别结果的定位误差在累积过程中逐步放大，进一步提高定位精度。
在自动驾驶车辆的定位与导航应用中，通过实时修正INS定位结果，使车辆在复杂环境中更能精确感知自身位置，从而提高行驶安全性和稳定性。
在本实施例中，通过对视觉传感器收集的目标视觉标签的标签信息进行识别与处理，确定视觉传感器与目标视觉标签的相对位姿关系，并确定目标视觉标签的标签编码。在此基础上，结合目标视觉标签的标签编码和视觉标签地图，求解出目标视觉标签的绝对位姿。将视觉传感器与目标视觉标签的相对位姿关系、目标视觉标签的绝对位姿和视觉传感器与待定位设备的相对位姿关系进行坐标转换，从而确定待定位设备的绝对位姿。进一步地，根据待定位设备的绝对位姿，获得视觉定位结果。此外，为了提升定位精度，将视觉定位结果与车辆运动约束相结合，构建卡尔曼滤波的观测量，对INS定位结果进行修正，从而获得本历元的组合定位结果。本申请实施例中，通过低成本、易识别、高鲁棒性的视觉标签构建视觉标签地图，通过一次观测即可得到相机与标签间的相对位姿，进而获得绝对定位结果，实现对INS累计误差的修正，适应不同场景下对INS定位累积误差的限制需求，从而在无GNSS环境下获得较高精度的定位结果。
总之，本申请实施例所提出的基于卡尔曼滤波的组合定位方法，在提升定位精度与实时性的同时，减少了误差累积的可能性，这对于自动驾驶技术的发展具有重大意义。此外，该方法同样适用于其他自动配送领域或自动导航领域。
在一种可能的实现方式中，步骤S104中，INS定位结果的递推预测过程，包括：
将待定位设备上一历元的组合定位结果对应的真实状态量分为名义状态量/>和误差状态量/>两个状态量；其中，/>；
根据惯性传感器采集的加速度和角速度，对名义状态量和误差状态量/>进行递推，得到名义状态量/>的预测值/>，并根据系统噪声/>和状态转移矩阵/>得到待定位设备的误差状态量/>的协方差矩阵/>。
其中，将待定位设备上一历元的组合定位结果所对应的真实状态量进行划分，将其分为名义状态量和误差状态量两个部分。真实状态量，是指在实际应用环境中，待定位设备所处的真实状态。而名义状态量/>，则代表了待定位设备在理想状态下的定位结果。误差状态量/>则表示在实际定位过程中，由于各种因素所导致的定位误差。
在具体实施例中，名义状态向量共16维，分别为东北天向的位置、速度、方位四元数以及加速度计和陀螺仪的三轴偏差；误差状态向量/>共15维，分别为东北天向位置误差、速度误差、旋转向量误差以及加速度计和陀螺仪的三轴测量偏差的误差，分别表示如下：
在本实施例中，INS定位系统对待定位设备的定位结果进行递推预测，从而在不断更新的状态下，实现更加精确的定位，为待定位设备提供了可靠的定位信息。
在一种可能的实现方式中，步骤S104中，以视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，获得本历元的组合定位结果，包括：
以视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，得到误差状态量的后验均值/>及其协方差矩阵/>；
将名义状态量的预测值/>和误差状态量/>的后验均值/>进行合并，得到本历元的组合定位结果/>。
其中，利用视觉定位结果和车辆运动约束来构造卡尔曼滤波的观测量，旨在对INS定位结果进行修正，从而获得本历元的组合定位结果。在修正过程中，得到误差状态量的后验均值/>及其协方差矩阵/>，旨在减小定位结果与真实定位之间的误差。
其中，对于地面载具，不考虑轮胎侧滑等特殊工况，其在正常的行驶状态下满足在自身载体坐标系下的侧向速度和纵向速度均为0，在具体实施过程中，利用此性质构造车辆运动约束来对组合定位系统进行误差修正。
在一种可能的实现方式中，卡尔曼滤波的观测量为/>。
在本实施例中，融合视觉定位和车辆运动约束的信息得到组合定位结果，相较于单一的INS定位结果更精确。另外，通过循环定位提高定位精度的同时减小误差，持续对INS的累计误差进行修正，使得导航系统在复杂环境下依然能够保持较高的准确性。
在一种可能的实现方式中，名义状态量的预测值/>和误差状态量/>的协方差矩阵/>的预测公式如下：
。
其中，为名义状态量/>的预测值；/>(·)为系统名义状态转移函数；/>为误差状态量；/>为惯性传感器观测得到的加速度；/>为惯性传感器观测得到的角速度；/>为两个历元的时间间隔；/>为上一历元的名义状态后验值；/>为误差状态量/>的预测值；/>为误差状态量的状态转移矩阵；/>为上一历元的误差状态后验值；/>为系统的误差状态量协方差矩阵；/>为上一历元的协方差矩阵后验；/>为系统噪声分配矩阵；/>为系统噪声；
其中，系统名义状态转移函数和误差状态转移矩阵/>可由如下名义状态量和误差状态量/>的递推公式确定：
其中，为位置预测量；/>为上一历元的位置后验量；/>为上一历元的速度后验量；/>为两个历元的时间间隔；/>为上一历元得到的由局部坐标系到全局坐标系的旋转矩阵；/>为加速度计测量的加速度；/>为上一历元的加速度计偏差后验量；/>为当地的重力加速度；/>为速度预测量；/>为以四元数表示的姿态预测量；/>为以四元数表示的上一历元姿态后验量；/>{ }为旋转矢量对应的姿态四元数；/>为陀螺仪测量的角速度；/>为上一历元的角速度偏差后验量；/>为加速度偏差预测量；/>为角速度偏差的预测量；/>为位置误差的预测量；/>为上一历元的位置误差后验量；/>为上一历元的速度误差后验量；/>为速度误差预测量；/>为上一历元的姿态误差后验量；/>为上一历元的加速度偏差误差后验量；/>为速度误差预测噪声；/>为姿态误差预测量；/>为上一历元的角速度偏差误差后验量；/>为姿态误差预测噪声；/>为加速度偏差误差预测量；/>为加速度偏差误差预测噪声；/>为角速度偏差误差预测量；/>为角速度偏差误差预测噪声。
相应的，步骤S104中，以视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正过程如下：
其中，为增益矩阵，/>为协方差矩阵预测值，/>为观测矩阵，/>为观测噪声矩阵，/>为本历元误差状态后验值，/>为观测值，/>为观测值的预测。
经过以上修正处理得到修正后的系统误差状态量及其协方差矩阵/>，之后，将名义状态量/>和误差状态量/>进行合并，并将误差状态参数重置：
其中，方差重置矩阵：
。
其中，将名义状态量和误差状态量/>进行合并后，将误差状态参数重置，以完成下一历元的修正。/>
在一种可能的实现方式中，当视觉传感器采集到多个目标视觉标签时，步骤S103中，根据待定位设备的绝对位姿确定视觉定位结果，包括：
根据基于检测距离的加权融合方法，对基于各目标视觉标签确定的待定位设备的绝对位姿进行融合，得到视觉定位结果，并确定视觉定位结果的标准差。
其中，考虑到实际应用时会使用不同大小的Apriltag标签的情况，设计距离因子：
其中，、/>分别为基准标签尺寸和第i个标签的尺寸；/>为第i个标签与待定位设备间的相对距离；
基于尺寸因子，融合定位结果并构造对应的标准差：
其中，为通过第i个Apriltag标签得到的待定位设备的绝对定位结果/>：/>为第i个Apriltag标签的距离因子；
构造量测矩阵用于后续的卡尔曼滤波更新，并通过链式法则求解量测矩阵/>：
。
在具体实施例中，基于上述关系式进行视觉定位更新。
卡尔曼滤波的观测量为：
量测方程表示为：
对应的量测矩阵表示为：
/>
其中，各分量分别为：
在具体实施例中，基于上述关系式完成车辆运动约束的更新。
在一种可能的实现方式中，步骤S101中，对标签信息进行识别处理确定视觉传感器与采集到的视觉标签的相对位姿关系，包括：
对标签信息对应的原始视觉标签图像的RGB值进行非线性映射调整，得到修正视觉标签图像；
基于Apriltag检测算法对原始视觉标签图像和修正视觉标签图像进行二次检测，得到视觉标签角点检测结果和标签编号识别结果；其中，二次检测包括：灰度化图像、高斯滤波、二值化、union-find聚类、边界分割、四边形检测、解码；
根据视觉标签角点检测结果、标签编号识别结果、预设视觉标签尺寸和视觉传感器内参，确定视觉传感器与采集到的视觉标签的相对位姿关系。
在具体实施过程中，视觉标签的颜色通常选取具有较高对比度的颜色，以便在白色背景下更容易被识别。而白色背景在光照条件不佳的情况下可能会变得较暗，影响标签的识别效果。因此，在本申请实施例中，在进行标签识别前，采用非线性映射技术对图像的RGB值进行调整，以增强视觉标签与白色背景之间的对比度，可以有效避免较暗的白色背景对标签边界的影响，提高检测算法的准确性。如图5所示，在完成RGB值调整后，使用第二代Apriltag检测算法对视觉标签的原始图像和修正图像进行二次检测，能够提升视觉标签图像的对比度，从而提高图像处理的准确性和可靠性。图5中视觉标签中心白色区域为二维码区域。
在一种可能的实现方式中，对标签信息对应的原始视觉标签图像的RGB值进行非线性映射调整，得到修正视觉标签图像，包括：
根据如下非线性映射公式对原始视觉标签图像的RGB值进行映射调整：
其中，参数定义为：
；
为原始视觉标签图像的RGB值；/>为修正视觉标签图像的RGB值；/>为输入量下限阈值；/>为输入量上限阈值；/>输出量下限阈值；/>输出量上限阈值。
其中，输入量即原始视觉标签图像的RGB值；输出量即修正视觉标签图像的RGB值。
在不同实施例中，对标签信息对应的原始视觉标签图像的RGB值进行非线性映射调整的方式不同。
在一种可能的实现方式中，对于彩色图像的每个通道，使用以上的映射公式对每个像素的取值进行调整。
在本实施例中，鉴于每个通道中像素信息存在差异，因此，对于彩色图像的每个通道，使用以上的映射公式对每个像素的取值进行调整，能够提升映射精确度。
在一种可能的实现方式中，对于彩色图像的每个通道，使用以上的映射公式对标签目标范围内的每个像素的取值进行调整，其中，目标范围对应部分标签内容，目标范围至少包括标签的角点。可选的，目标范围为标签四条边上距离中心区域设定距离以外的所有点，以及这些点所围成的区域。设定距离为标签边长的。
在本实施例中，鉴于每个通道中像素信息存在差异，因此，对于彩色图像的每个通道，使用以上的映射公式对标签目标范围内的每个像素的取值进行调整，能够提升映射精确度，同时，提升映射效率。
在一种可能的实现方式中，根据视觉标签角点检测结果、标签编号识别结果、预设视觉标签尺寸和视觉传感器内参，确定视觉传感器与采集到的视觉标签的相对位姿关系，包括：
根据预设视觉标签尺寸和角点在图像坐标系和标签坐标系下的关系，利用视觉标签角点检测结果四对角点求解出单应矩阵；其中，角点在图像坐标系和标签坐标系下的关系如下式：
其中，和/>为图像坐标系中坐标；/>和/>为标签坐标系坐标/>为相机内参矩阵；为旋转矩阵第1列；/>为旋转矩阵第2列；/>为两坐标系原点的位移向量；
根据相机内参矩阵及旋转矩阵的正交性，获得图像标签在相机坐标系下的位姿，以确定视觉传感器与采集到的视觉标签的相对位姿关系。
在一种可能的实现方式中，步骤S103中，根据视觉传感器与目标视觉标签的相对位姿关系、目标视觉标签的绝对位姿和视觉传感器与待定位设备的相对位姿关系进行坐标转换，确定待定位设备的绝对位姿，包括：
基于如下换算公式进行坐标转换：
/>
其中，和/>为视觉传感器与待定位设备的相对位姿；/>和/>为目标视觉标签在相机坐标系下的位姿；/>和/>为目标视觉标签的绝对位姿；/>为待定位设备的绝对位置；/>和/>为视觉标签在待定位设备坐标系下的位姿。
图6是本申请一实施例提供的基于视觉标签地图的定位方法的实现流程图，如图6所示，提前构建视觉标签地图。基于改进的AprilTag检测算法对视觉传感器获取到的视觉标签图像进行检测，确定视觉标签角点检测结果和标签编号识别结果。然后，根据视觉标签地图和标签编号识别结果确定标签的绝对位姿（和/>），并根据视觉标签角点检测结果确定视觉传感器与待定位设备的相对位姿（/>和/>）。根据所述目标视觉标签的绝对位姿（/>和/>）和所述视觉传感器与所述待定位设备的相对位姿关系（/>和/>）进行坐标转换确定定位设备的绝对位置（/>）。在视觉传感器获取到的目标视觉标签数量大于1时，根据基于检测距离的加权融合方法对基于各目标视觉标签确定的待定位设备的绝对位姿进行融合，得到视觉定位结果，并确定视觉定位结果的标准差（/>和/>）。同时以车辆运动约束构造误差状态卡尔曼滤波器(Error State Kalman Filter, ESKF)的观测量（/>），利用视觉定位结果和车辆运动约束进行ESKF量测更新，得到误差状态的后验量（/>），进一步地与基于上一历元的组合定位结果/>和惯性传感器（Inertial Measurement Unit，IMU）采集的加速度和角速度（/>和/>）进行ESKF状态递推预测得到的ESKF状态预测（/>）进行ESKF状态合并，得到最终的后验状态量（/>）。其中，IMU由加速度传感器和陀螺仪两个单元组成，加速度传感器采集加速度，陀螺仪采集角速度。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。
为了验证本申请实施例提供的方法的有效性，基于图7所示的实验载具，在图4所示的场景中进行实验。该实验载具设有激光雷达、相机、控制单元（内置有惯性测量单元）和驱动电机。通过本申请实施例提供的方案基于相机采集的视觉标签信息进行定位实验，并使用激光雷达SLAM结果进行验证。
图8为基于本申请实施例提供的方案的定位结果，其中，多个圆圈为根据Apriltag检测算法确定的视觉标签的位置分布，针对多个圆圈覆盖的范围构建图8所示的横纵坐标系。实验过程中，图7所示的实验载具在图4所示场景下，逆时针行驶一周，基于本申请实施例所提供方法得到的定位轨迹结果如图8中ESKF曲线，基于激光SLAM方法得到的参考定位轨迹结果如图8中True曲线。
图9为定位误差示意图。其中，图9使用具有cm级精度的激光雷达SLAM结果作为真值。在第i个历元下（共n个历元），将所提方法得到的定位结果和真值结果分别记为、，定位误差/>表示为：/>
整个实验过程中的定位误差Error-T曲线如图9所示，其中，纵坐标对应定位误差，横坐标为时间轴。
利用误差均方根RMSE作为定位精度的统计指标，使用下式进行计算：
利用上式得到的本次实验定位误差均方根为0.1363m。由此，证明本申请实施例提供的方案可以在无GNSS环境中得到很高的定位精度。
以下为本申请的装置实施例，对于其中未详尽描述的细节，可以参考上述对应的方法实施例。
图10是本申请一实施例提供的基于视觉标签地图的定位装置的结构示意图，如图10所示，为了便于说明，仅示出了与本申请实施例相关的部分，如图10所示，该装置包括：
第一位姿确定模块1001，用于获取所述视觉传感器采集到的目标视觉标签的标签信息，对所述标签信息进行识别处理确定视觉传感器与所述目标视觉标签的相对位姿关系，并对标签信息进行识别处理确定所述目标视觉标签的标签编码；
第二位姿确定模块1002，用于根据所述目标视觉标签的标签编码和视觉标签地图确定所述目标视觉标签的绝对位姿；
第三位姿确定模块1003，用于根据视觉传感器与目标视觉标签的相对位姿关系、目标视觉标签的绝对位姿和视觉传感器与待定位设备的相对位姿关系进行坐标转换，确定所述待定位设备的绝对位姿，并根据所述待定位设备的绝对位姿确定视觉定位结果；
定位模块1004，用于以所述视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，获得本历元的组合定位结果；
其中，所述INS定位结果基于上一历元的组合定位结果和所述惯性传感器采集的加速度和角速度，使用误差状态卡尔曼滤波进行状态递推预测得出。
本申请实施例通过对视觉传感器收集的目标视觉标签的标签信息进行识别与处理，确定视觉传感器与目标视觉标签的相对位姿关系，并确定目标视觉标签的标签编码。在此基础上，结合目标视觉标签的标签编码和视觉标签地图，求解出目标视觉标签的绝对位姿。将视觉传感器与目标视觉标签的相对位姿关系、目标视觉标签的绝对位姿和视觉传感器与待定位设备的相对位姿关系进行坐标转换，从而确定待定位设备的绝对位姿。进一步地，根据待定位设备的绝对位姿，获得视觉定位结果。此外，为了提升定位精度，将视觉定位结果与车辆运动约束相结合，构建卡尔曼滤波的观测量，对INS定位结果进行修正，从而获得本历元的组合定位结果。本申请实施例中，通过低成本、易识别、高鲁棒性的视觉标签构建视觉标签地图，通过一次观测即可得到相机与标签间的相对位姿，进而获得绝对定位结果，实现对INS累计误差的修正，适应不同场景下对INS定位累积误差的限制需求，从而在无GNSS环境下获得较高精度的定位结果。
图11是本申请一实施例提供的电子设备的结构示意图。如图11所示，该实施例的电子设备11包括：处理器110、存储器111以及存储在所述存储器111中并可在所述处理器110上运行的计算机程序112。所述处理器110执行所述计算机程序112时实现上述各个基于视觉标签地图的定位方法实施例中的步骤，例如图1所示的各步骤。或者，所述处理器110执行所述计算机程序112时实现上述各装置实施例中各模块/单元的功能，例如图10所示各模块的功能。
示例性的，所述计算机程序112可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器111中，并由所述处理器110执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序112在所述电子设备11中的执行过程。例如，所述计算机程序112可以被分割成图10所示各模块。
所述电子设备11可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述电子设备11可包括，但不仅限于，处理器110、存储器111。本领域技术人员可以理解，图11仅仅是电子设备11的示例，并不构成对电子设备11的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述电子设备还可以包括输入输出设备、网络接入设备、总线等。
所称处理器110可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器 (Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列 (Field-Programmable Gate Array，FPGA) 或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器111可以是所述电子设备11的内部存储单元，例如电子设备11的硬盘或内存。所述存储器111也可以是所述电子设备11的外部存储设备，例如所述电子设备11上配备的插接式硬盘，智能存储卡（Smart Media Card，SMC），安全数字（Secure Digital，SD）卡，闪存卡（Flash Card）等。进一步地，所述存储器111还可以既包括所述电子设备11的内部存储单元也包括外部存储设备。所述存储器111用于存储所述计算机程序以及所述电子设备所需的其他程序和数据。所述存储器111还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本申请中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置/电子设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/电子设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个基于视觉标签地图的定位方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器（Read-Only Memory，ROM）、随机存取存储器（Random Access Memory，RAM）、电载波信号、电信信号以及软件分发介质等。
以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。
Claims (10)
1.一种基于视觉标签地图的定位方法，其特征在于，待定位设备包括设置在待定位设备本体上的惯性传感器和视觉传感器；该定位方法包括：
获取所述视觉传感器采集到的目标视觉标签的标签信息，对所述标签信息进行识别处理确定视觉传感器与所述目标视觉标签的相对位姿关系，并对所述标签信息进行识别处理确定所述目标视觉标签的标签编码；
根据所述目标视觉标签的标签编码和视觉标签地图确定所述目标视觉标签的绝对位姿；
根据所述视觉传感器与所述目标视觉标签的相对位姿关系、所述目标视觉标签的绝对位姿和所述视觉传感器与所述待定位设备的相对位姿关系进行坐标转换，确定所述待定位设备的绝对位姿，并根据所述待定位设备的绝对位姿确定视觉定位结果；
以所述视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对惯性导航系统INS定位结果进行修正，获得本历元的组合定位结果；
其中，所述INS定位结果基于上一历元的组合定位结果和所述惯性传感器采集的加速度和角速度，使用误差状态卡尔曼滤波进行状态递推预测得出。
2.根据权利要求1所述的基于视觉标签地图的定位方法，其特征在于，所述INS定位结果的递推预测过程，包括：
将所述待定位设备上一历元的组合定位结果对应的真实状态量分为名义状态量/>和误差状态量/>两个状态量；其中，/>；
根据所述惯性传感器采集的加速度和角速度，对名义状态量和误差状态量/>进行递推，得到名义状态量/>的预测值/>，并根据系统噪声/>和状态转移矩阵/>得到所述待定位设备的误差状态量/>的协方差矩阵/>。
3.根据权利要求2所述的基于视觉标签地图的定位方法，其特征在于，所述以所述视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，获得本历元的组合定位结果，包括：
以所述视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，得到误差状态量的后验均值/>及其协方差矩阵/>；
将名义状态量的预测值/>和误差状态量/>的后验均值/>进行合并，得到本历元的组合定位结果/>。
4.根据权利要求2所述的基于视觉标签地图的定位方法，其特征在于，名义状态量的预测值/>和误差状态量/>的协方差矩阵/>的预测公式如下：
其中，为名义状态量/>的预测值；/>(·)为系统名义状态转移函数；/>为误差状态量；/>为惯性传感器观测得到的加速度；/>为惯性传感器观测得到的角速度；/>为两个历元的时间间隔；/>为上一历元的名义状态后验值；/>为误差状态量/>的预测值；为误差状态量的状态转移矩阵；/>为上一历元的误差状态后验值；/>为系统的误差状态量协方差矩阵；/>为上一历元的协方差矩阵后验；/>为系统噪声分配矩阵；/>为系统噪声；
其中，系统名义状态转移函数和误差状态转移矩阵/>可由如下名义状态量/>和误差状态量/>的递推公式确定：
其中，为位置预测量；/>为上一历元的位置后验量；/>为上一历元的速度后验量；为两个历元的时间间隔；/>为上一历元得到的由局部坐标系到全局坐标系的旋转矩阵；/>为加速度计测量的加速度；/>为上一历元的加速度计偏差后验量；/>为当地的重力加速度；/>为速度预测量；/>为以四元数表示的姿态预测量；/>为以四元数表示的上一历元姿态后验量；/>{ }为旋转矢量对应的姿态四元数；/>为陀螺仪测量的角速度；为上一历元的角速度偏差后验量；/>为加速度偏差预测量；/>为角速度偏差的预测量；/>为位置误差的预测量；/>为上一历元的位置误差后验量；/>为上一历元的速度误差后验量；/>为速度误差预测量；/>为上一历元的姿态误差后验量；/>为上一历元的加速度偏差误差后验量；/>为速度误差预测噪声；/>为姿态误差预测量；/>为上一历元的角速度偏差误差后验量；/>为姿态误差预测噪声；/>为加速度偏差误差预测量；/>为加速度偏差误差预测噪声；/>为角速度偏差误差预测量；/>为角速度偏差误差预测噪声。
5.根据权利要求1所述的基于视觉标签地图的定位方法，其特征在于，当视觉传感器采集到多个目标视觉标签时，所述根据所述待定位设备的绝对位姿确定视觉定位结果，包括：
根据基于检测距离的加权融合方法，对基于各目标视觉标签确定的所述待定位设备的绝对位姿进行融合，得到视觉定位结果，并确定视觉定位结果的标准差。
6.根据权利要求5所述的基于视觉标签地图的定位方法，其特征在于，所述对所述标签信息进行识别处理确定视觉传感器与采集到的视觉标签的相对位姿关系，包括：
对所述标签信息对应的原始视觉标签图像的RGB值进行非线性映射调整，得到修正视觉标签图像；
基于Apriltag检测算法对所述原始视觉标签图像和所述修正视觉标签图像进行二次检测，得到视觉标签角点检测结果和标签编号识别结果；其中，所述二次检测包括：灰度化图像、高斯滤波、二值化、union-find聚类、边界分割、四边形检测、解码；
根据所述视觉标签角点检测结果、标签编号识别结果、预设视觉标签尺寸和所述视觉传感器内参，确定视觉传感器与采集到的视觉标签的相对位姿关系。
7.根据权利要求6所述的基于视觉标签地图的定位方法，其特征在于，所述对所述标签信息对应的原始视觉标签图像的RGB值进行非线性映射调整，得到修正视觉标签图像，包括：
根据如下非线性映射公式对原始视觉标签图像的RGB值进行映射调整：
其中，参数定义为：
；
为原始视觉标签图像的RGB值；/>为修正视觉标签图像的RGB值；/>为输入量下限阈值；/>为输入量上限阈值；/>输出量下限阈值；/>输出量上限阈值。
8.根据权利要求6所述的基于视觉标签地图的定位方法，其特征在于，所述根据所述视觉标签角点检测结果、标签编号识别结果、预设视觉标签尺寸和所述视觉传感器内参，确定视觉传感器与采集到的视觉标签的相对位姿关系，包括：
根据预设视觉标签尺寸和角点在图像坐标系和标签坐标系下的关系，利用所述视觉标签角点检测结果四对角点求解出单应矩阵；其中，角点在图像坐标系和标签坐标系下的关系如下式：
其中，和/>为图像坐标系中坐标；/>和/>为标签坐标系坐标 />为相机内参矩阵；/>为旋转矩阵第1列；/>为旋转矩阵第2列；/>为两坐标系原点的位移向量；
根据相机内参矩阵及旋转矩阵的正交性，获得图像标签在相机坐标系下的位姿，以确定视觉传感器与采集到的视觉标签的相对位姿关系。
9.根据权利要求1所述的基于视觉标签地图的定位方法，其特征在于，所述根据所述视觉传感器与所述目标视觉标签的相对位姿关系、所述目标视觉标签的绝对位姿和所述视觉传感器与所述待定位设备的相对位姿关系进行坐标转换，确定所述待定位设备的绝对位姿，包括：
基于如下换算公式进行坐标转换：
其中，和/>为所述视觉传感器与所述待定位设备的相对位姿；/>和/>为所述目标视觉标签在相机坐标系下的位姿；/>和/>为所述目标视觉标签的绝对位姿；/>为所述待定位设备的绝对位置；/>和/>为视觉标签在待定位设备坐标系下的位姿。
10.一种基于视觉标签地图的定位装置，其特征在于，待定位设备包括设置在待定位设备本体上的惯性传感器和视觉传感器；该定位装置包括：
第一位姿确定模块，用于获取所述视觉传感器采集到的目标视觉标签的标签信息，对所述标签信息进行识别处理确定视觉传感器与所述目标视觉标签的相对位姿关系，并对所述标签信息进行识别处理确定所述目标视觉标签的标签编码；
第二位姿确定模块，用于根据所述目标视觉标签的标签编码和视觉标签地图确定所述目标视觉标签的绝对位姿；
第三位姿确定模块，用于根据所述视觉传感器与所述目标视觉标签的相对位姿关系、所述目标视觉标签的绝对位姿和所述视觉传感器与所述待定位设备的相对位姿关系进行坐标转换，确定所述待定位设备的绝对位姿，并根据所述待定位设备的绝对位姿确定视觉定位结果；
定位模块，用于以所述视觉定位结果和车辆运动约束构造卡尔曼滤波的观测量，对INS定位结果进行修正，获得本历元的组合定位结果；
其中，所述INS定位结果基于上一历元的组合定位结果和所述惯性传感器采集的加速度和角速度，使用误差状态卡尔曼滤波进行状态递推预测得出。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410251328.4A CN117848331A (zh) | 2024-03-06 | 2024-03-06 | 基于视觉标签地图的定位方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410251328.4A CN117848331A (zh) | 2024-03-06 | 2024-03-06 | 基于视觉标签地图的定位方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117848331A true CN117848331A (zh) | 2024-04-09 |
Family
ID=90529506
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410251328.4A Pending CN117848331A (zh) | 2024-03-06 | 2024-03-06 | 基于视觉标签地图的定位方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117848331A (zh) |
Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109163721A (zh) * | 2018-09-18 | 2019-01-08 | 河北美泰电子科技有限公司 | 姿态测量方法及终端设备 |
| WO2019157925A1 (zh) * | 2018-02-13 | 2019-08-22 | 视辰信息科技(上海)有限公司 | 视觉惯性里程计的实现方法及系统 |
| CN110879066A (zh) * | 2019-12-26 | 2020-03-13 | 河北美泰电子科技有限公司 | 一种姿态解算算法、装置及车载惯性导航系统 |
| CN111197984A (zh) * | 2020-01-15 | 2020-05-26 | 重庆邮电大学 | 一种基于环境约束下的视觉-惯性运动估计方法 |
| WO2020155616A1 (zh) * | 2019-01-29 | 2020-08-06 | 浙江省北大信息技术高等研究院 | 一种基于数字视网膜的拍摄装置的定位方法 |
| CN113804184A (zh) * | 2020-06-15 | 2021-12-17 | 上海知步邦智能科技有限公司 | 基于多传感器的地面机器人定位方法 |
| WO2022000713A1 (zh) * | 2020-06-28 | 2022-01-06 | 南京翱翔信息物理融合创新研究院有限公司 | 一种基于航空装配的增强现实自定位方法 |
| WO2022105024A1 (zh) * | 2020-11-17 | 2022-05-27 | 深圳市优必选科技股份有限公司 | 机器人位姿的确定方法、装置、机器人及存储介质 |
| CN115235478A (zh) * | 2022-09-23 | 2022-10-25 | 武汉理工大学 | 基于视觉标签和激光slam的智能汽车定位方法及系统 |
| CN116182867A (zh) * | 2023-02-22 | 2023-05-30 | 东南大学 | 复杂室内环境下基于紧组合的ins/uwb无人车定位方法 |
| US20230236280A1 (en) * | 2022-01-25 | 2023-07-27 | Taizhou University | Method and system for positioning indoor autonomous mobile robot |
| CN116678406A (zh) * | 2023-07-31 | 2023-09-01 | 河北美泰电子科技有限公司 | 组合导航姿态信息确定方法、装置、终端设备及存储介质 |
| CN116753953A (zh) * | 2023-05-25 | 2023-09-15 | 扎鲁特旗扎哈淖尔煤业有限公司 | 一种基于视觉的无人机组合导航方法 |
| CN117330058A (zh) * | 2023-09-08 | 2024-01-02 | 同济大学 | 一种车路联合精准定位方法、设备及存储介质 |
| CN117516515A (zh) * | 2023-09-22 | 2024-02-06 | 北京理工大学 | 一种足履惯导/头戴视觉协同的人员定位装置和方法 |
-
2024
- 2024-03-06 CN CN202410251328.4A patent/CN117848331A/zh active Pending
Patent Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019157925A1 (zh) * | 2018-02-13 | 2019-08-22 | 视辰信息科技(上海)有限公司 | 视觉惯性里程计的实现方法及系统 |
| CN109163721A (zh) * | 2018-09-18 | 2019-01-08 | 河北美泰电子科技有限公司 | 姿态测量方法及终端设备 |
| WO2020155616A1 (zh) * | 2019-01-29 | 2020-08-06 | 浙江省北大信息技术高等研究院 | 一种基于数字视网膜的拍摄装置的定位方法 |
| CN110879066A (zh) * | 2019-12-26 | 2020-03-13 | 河北美泰电子科技有限公司 | 一种姿态解算算法、装置及车载惯性导航系统 |
| CN111197984A (zh) * | 2020-01-15 | 2020-05-26 | 重庆邮电大学 | 一种基于环境约束下的视觉-惯性运动估计方法 |
| CN113804184A (zh) * | 2020-06-15 | 2021-12-17 | 上海知步邦智能科技有限公司 | 基于多传感器的地面机器人定位方法 |
| WO2022000713A1 (zh) * | 2020-06-28 | 2022-01-06 | 南京翱翔信息物理融合创新研究院有限公司 | 一种基于航空装配的增强现实自定位方法 |
| WO2022105024A1 (zh) * | 2020-11-17 | 2022-05-27 | 深圳市优必选科技股份有限公司 | 机器人位姿的确定方法、装置、机器人及存储介质 |
| US20230236280A1 (en) * | 2022-01-25 | 2023-07-27 | Taizhou University | Method and system for positioning indoor autonomous mobile robot |
| CN115235478A (zh) * | 2022-09-23 | 2022-10-25 | 武汉理工大学 | 基于视觉标签和激光slam的智能汽车定位方法及系统 |
| CN116182867A (zh) * | 2023-02-22 | 2023-05-30 | 东南大学 | 复杂室内环境下基于紧组合的ins/uwb无人车定位方法 |
| CN116753953A (zh) * | 2023-05-25 | 2023-09-15 | 扎鲁特旗扎哈淖尔煤业有限公司 | 一种基于视觉的无人机组合导航方法 |
| CN116678406A (zh) * | 2023-07-31 | 2023-09-01 | 河北美泰电子科技有限公司 | 组合导航姿态信息确定方法、装置、终端设备及存储介质 |
| CN117330058A (zh) * | 2023-09-08 | 2024-01-02 | 同济大学 | 一种车路联合精准定位方法、设备及存储介质 |
| CN117516515A (zh) * | 2023-09-22 | 2024-02-06 | 北京理工大学 | 一种足履惯导/头戴视觉协同的人员定位装置和方法 |
Non-Patent Citations (3)
| Title |
|---|
| 付强文等: "车辆运动学约束辅助的惯性导航算法", 中国惯性技术学报, vol. 20, no. 6, 31 December 2012 (2012-12-31), pages 640 - 643 * |
| 吴建文;周志飞;詹军;张声勇;: "铁路物流园内集装箱AGV的导航与精度研究", 起重运输机械, no. 08, 25 April 2020 (2020-04-25) * |
| 尚明超;杨斌;张翠芳;: "二维码修正EKF-SLAM定位的室内无人驾驶小车", 单片机与嵌入式系统应用, no. 07, 1 July 2017 (2017-07-01) * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11131999B2 (en) | Method and apparatus for identifying laser point cloud data of autonomous vehicle | |
| CN111272165B (zh) | 一种基于特征点标定的智能车定位方法 | |
| US11852751B2 (en) | Method, apparatus, computing device and computer-readable storage medium for positioning | |
| US11525682B2 (en) | Host vehicle position estimation device | |
| US11144770B2 (en) | Method and device for positioning vehicle, device, and computer readable storage medium | |
| CN112904395B (zh) | 一种矿用车辆定位系统及方法 | |
| US11158065B2 (en) | Localization of a mobile unit by means of a multi hypothesis kalman filter method | |
| CN113580134B (zh) | 视觉定位方法、设备、机器人、存储介质及程序产品 | |
| EP3882885A1 (en) | Vehicle control device | |
| US10796569B2 (en) | Vehicle determination apparatus, vehicle determination method, and computer readable medium | |
| CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
| CN114088114B (zh) | 车辆位姿校准方法、装置和电子设备 | |
| CN111091037A (zh) | 用于确定驾驶信息的方法和设备 | |
| CN113920198B (zh) | 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法 | |
| CN114730472A (zh) | 车载相机的外部参数的标定方法及相关装置 | |
| CN116997771A (zh) | 车辆及其定位方法、装置、设备、计算机可读存储介质 | |
| CN114248778A (zh) | 移动设备的定位方法和定位装置 | |
| CN114694111A (zh) | 车辆定位 | |
| CN111605481A (zh) | 基于环视的拥堵跟车系统和终端 | |
| CN113469045B (zh) | 无人集卡的视觉定位方法、系统、电子设备和存储介质 | |
| CN117848331A (zh) | 基于视觉标签地图的定位方法及装置 | |
| CN115456898A (zh) | 停车场的建图方法、装置、车辆及存储介质 | |
| CN113034538B (zh) | 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 | |
| Lee et al. | Map Matching-Based Driving Lane Recognition for Low-Cost Precise Vehicle Positioning on Highways | |
| CN114440864A (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 |