CN119359796B - 一种位姿确定方法及电子设备 - Google Patents

一种位姿确定方法及电子设备 Download PDF

Info

Publication number
CN119359796B
CN119359796B CN202411922986.8A CN202411922986A CN119359796B CN 119359796 B CN119359796 B CN 119359796B CN 202411922986 A CN202411922986 A CN 202411922986A CN 119359796 B CN119359796 B CN 119359796B
Authority
CN
China
Prior art keywords
information
imu
observation
pose
determining
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202411922986.8A
Other languages
English (en)
Other versions
CN119359796A (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.)
Hangzhou Hikvision Digital Technology Co Ltd
Original Assignee
Hangzhou Hikvision Digital 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 Hangzhou Hikvision Digital Technology Co Ltd filed Critical Hangzhou Hikvision Digital Technology Co Ltd
Priority to CN202411922986.8A priority Critical patent/CN119359796B/zh
Publication of CN119359796A publication Critical patent/CN119359796A/zh
Application granted granted Critical
Publication of CN119359796B publication Critical patent/CN119359796B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/60—Rotation of whole images or parts thereof
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/50—Image enhancement or restoration using two or more images, e.g. averaging or subtraction
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20212—Image combination
    • G06T2207/20221—Image fusion; Image merging

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Navigation (AREA)

Abstract

本申请实施例适用于同步定位与建图技术领域，提供了一种位姿确定方法及电子设备，方法包括：获取测量数据；测量数据包括定位信息、IMU信息、图像信息以及点云信息；基于定位信息和IMU信息，生成初始数据；获取定位观测信息、运动观测信息；构建第一误差函数；采用定位观测信息、运动观测信息和第一误差函数对初始数据进行优化，生成第一位姿数据；基于定位信息、IMU信息、图像信息以及第一位姿数据，生成第二位姿数据；基于点云信息、IMU信息、以及第二位姿数据，生成相对位姿；依据定位信息、IMU信息、图像信息、第二位姿数据和相对位姿，确定目标位姿。本申请实施例可以实现由粗到精引导位姿的优化收敛方向，提升位姿估计精度。

Description

一种位姿确定方法及电子设备
技术领域
本申请实施例属于同步定位与建图技术领域，特别是涉及一种位姿确定方法及电子设备。
背景技术
同步定位与建图（Simultaneous Localization and Mapping，简称SLAM）技术是一种用于机器人或自动驾驶车辆等智能设备在未知环境中进行自主导航的技术。SLAM技术通常需要通过激光和视觉传感器获取环境中同一静态物体在不同次观测中相对位置变化来进行位姿估计，但在隧道场景中经常出现观测退化（例如：激光观测和视觉观测），导致位姿估计出现较大误差。激光观测退化指的是静态物体在某一方向的观测不足，导致位姿估计缺乏约束。视觉观测退化指的是对静态物体特征点的提取与跟踪准确率低，导致位姿估计缺乏约束。
针对观测退化场景的建图问题，相关技术采用LIO（Lidar Inertial Odometry,激光惯性里程计）/LVIO（Lidar Visual Inertial Odometry，激光视觉惯性里程计）算法递推位姿，并通过位姿图优化的方式融合LIO/LVIO位姿与GPS观测，减少位姿的累积漂移。但是相关技术中，在隧道场景中GPS有效性不足，LIO/LVIO缺乏有效观测信息，导致计算的位姿存在较大误差。
发明内容
有鉴于此，本申请实施例提供了一种位姿确定方法及电子设备，用以提高在激光观测退化和视觉观测退化的情况下，保证位姿计算的准确性。
本申请实施例的第一方面提供了一种位姿确定方法，包括：
获取测量数据；所述测量数据包括定位信息、IMU信息、图像信息以及点云信息；
基于所述定位信息和IMU信息，生成初始数据；
获取定位观测信息、运动观测信息；
构建第一误差函数；
采用所述定位观测信息、所述运动观测信息和所述第一误差函数对所述初始数据进行优化，生成第一位姿数据
基于所述定位信息、所述IMU信息、所述图像信息以及所述第一位姿数据，生成第二位姿数据；
基于所述点云信息、IMU信息、以及所述第二位姿数据，生成相对位姿；
依据所述定位信息、所述IMU信息、所述图像信息、所述第二位姿数据和所述相对位姿，确定目标位姿。
本申请实施例的第二方面提供了一种位姿确定装置，包括：
数据获取模块，用于获取测量数据；所述测量数据包括定位信息、IMU信息、图像信息以及点云信息；
初始数据生成模块，用于基于所述定位信息和IMU信息，生成初始数据；
观测信息第一获取模块，用于获取定位观测信息、运动观测信息；
第一误差函数构建模块，用于构建第一误差函数；
第一位姿数据生成模块，用于采用所述定位观测信息、所述运动观测信息和所述第一误差函数对所述初始数据进行优化，生成第一位姿数据；
第二位姿数据生成模块，用于基于所述定位信息、所述IMU信息、所述图像信息以及所述第一位姿数据，生成第二位姿数据；
相对位姿生成模块，用于基于所述点云信息、IMU信息、以及所述第二位姿数据，生成相对位姿；
目标位姿生成模块，用于依据所述定位信息、所述IMU信息、所述图像信息、所述第二位姿数据和所述相对位姿，确定目标位姿。
本申请实施例的第三方面提供了一种电子设备，包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时，使得所述电子设备实现如上述第一方面所述的位姿确定方法。
本申请实施例的第四方面提供了一种计算机程序产品，包括计算机程序，所述计算机程序被运行时，使得如上述第一方面所述的位姿确定方法被执行。
本申请实施例的第五方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述第一方面所述的位姿确定方法。
与现有技术相比，本申请实施例具有以下有益效果：
本申请实施例，通过获取测量数据；所述测量数据包括定位信息、IMU信息、图像信息以及点云信息；基于定位信息和IMU信息，生成初始数据；获取定位观测信息、运动观测信息；构建第一误差函数；采用定位观测信息、运动观测信息和第一误差函数对初始数据进行优化，生成第一位姿数据；基于所述定位信息、所述IMU信息、所述图像信息以及所述第一位姿数据，生成第二位姿数据；基于所述点云信息、IMU信息、以及所述第二位姿数据，生成相对位姿；依据所述定位信息、所述IMU信息、所述图像信息、所述第二位姿数据和所述相对位姿，确定目标位姿；从而实现通过多阶段的数据融合，由粗到精的引导位姿的优化收敛方向，提升位姿估计精度。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的一种位姿确定方法的示意图；
图2是本申请实施例提供的一种GI位姿优化因子示意图；
图3是本申请实施例提供的一种GVI位姿优化因子示意图；
图4是本申请实施例提供的一种GLVI位姿优化因子示意图；
图5是本申请实施例提供的一种位姿确定装置的示意图；
图6是本申请实施例提供的一种电子设备的示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本申请。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。
应当理解，当在本申请说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
如在本申请说明书和所附权利要求书中所使用的那样，术语“如果”可以依据上下文被解释为“当...时”或“一旦”或“响应于确定”或“响应于检测到”。类似地，短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述条件或事件]”或“响应于检测到[所描述条件或事件]”。
另外，在本申请说明书和所附权利要求书的描述中，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
在本申请说明书中描述的参考“一个实施例”或“一些实施例”等意味着在本申请的一个或多个实施例中包括结合该实施例描述的特定特征、结构或特点。由此，在本说明书中的不同之处出现的语句“在一个实施例中”、“在一些实施例中”、“在其他一些实施例中”、“在另外一些实施例中”等不是必然都参考相同的实施例，而是意味着“一个或多个但不是所有的实施例”，除非是以其他方式另外特别强调。术语“包括”、“包含”、“具有”及它们的变形都意味着“包括但不限于”，除非是以其他方式另外特别强调。
在同步定位与建图技术中，全局地图的构建需要利用激光传感器实时扫描周围环境，获取周围环境的几何信息，并确定激光触感器的位姿，利用估计的位姿将激光点云数据拼接，构建全局一致性环境地图，因此激光传感器的位姿准确性，对全局地图的一致性具有关键意义。本身实施例公开了一种位姿确定方法，该方法包含有多阶段的数据融合过程，以使载体的位姿逐步收敛，并且上述多个阶段中得到相关信息可以用于降低LIO/LVIO在上述针对在发生激光观测退化或者视觉观测退化的场景下的产生的测量误差，进而提高最终输出的目标位姿的准确性。下面通过具体实施例来说明本申请的技术方案。
参照图1，示出了本申请实施例提供的一种位姿方法的示意图，具体可以包括如下步骤：
步骤101，获取测量数据；所述测量数据包括定位信息、IMU信息、图像信息以及点云信息；
可以通过在同一载体中设置GNSS（Global Navigation Satellite System，全球卫星导航系统）信号处理模组、IMU（Inertial Measurement Unit，惯性测量单元）模组、视觉模组、激光模组，以获取测量数据。
GNSS信号处理模组可以接收并处理卫星信号（例如：GPS信号、北斗卫星信号）；IMU模组设置有一个或多个IMU，用于获取IMU信息（例如：角速度、加速度）；视觉模组设置有一个或多个视觉传感器（例如：摄像头），视觉传感器用于对当前所述环境进行视觉信息采集并输出图像信息；激光模组用于发送激光信号，并根据接收到的反射激光信号生成点云信息。
由于载体的不同模组之间存在一定距离，因此可以针对各个模组构建多个坐标系，例如：激光模组的激光坐标系，视觉模组的视觉坐标系， IMU模组的IMU坐标系， GNSS信号处理模组的GNSSS坐标系。
可以针对实际环境构建一个或多个地理坐标系，例如：地心坐标系，站心坐标系（例如：东-北-天坐标系），以及针对LIO/LVIO构建的世界坐标系。
可以理解的是，由于上述坐标系是预见构建确定，因此可以在本申请实施例的任意步骤或过程中，采用上述任一坐标系中的坐标位置，转换至另外任一坐标系中的坐标位置，下文对具体的转换过程和原理不再赘述。
位姿包括位置和姿态，在本申请实施例中分别通过B样条曲线分别表示位置和姿态。
对于d阶均匀B样条曲线（第一B样条曲线），平移，，由控制点决定，上述控制点分别对应时刻， 与各控制点之间的关系可以写成如下表达形式，即B样条曲线的表达形式为：
（1）
式中，，，表示的第j列，且只与均匀B样条曲线的阶数有关，且为可预先确定的值，即具有相同阶数的B样条曲线的矩阵相同。p(t)为任一时刻t的位置，该位置通过若干个控制点表征。
公式（1）为通过控制点pi+j计算曲线上点p(t)的位置，为包含u的不同次幂的向量，用于表示当前时间t在当前控制点时间区间[ti,ti+1），ti表示当前控制点的起始时间，ti+1表示为下一个控制点的起始时间。通过公式，对时间t进行归一化处理。
本申请实施例以一条3阶B样条曲线表示位姿中的位置对于3阶（d=3 ）B样条曲线，具体为：
（2）
对公式（1）进行转换得到公式（3），公式（3）通过积累相邻控制点之间的差来计算曲线上点的位置，即可以通过公式（3）表示与各控制点之间的关系可以按照如下的累积：
（3）
与此对应的3阶B样条矩阵为：
（4）
速度为单位时间内平移距离大小，因此可以根据公式（3）确定速度的表达形式为：
（5）
式（5）中，表示控制点之间的时间间隔，加速度为单位时间内的速度变化，基于加速度定义以及速度表达式即公式（5），加速度的表示形式为：
（6）
对于旋转，，可用如下的均匀B样条曲线（第二B样条曲线）表示：
（7）
对于任一时刻t的姿态为R（t），姿态通过若干个控制点表征。式（7）中，R(t)为时间t时的旋转矩阵，Ri为初始旋转矩阵，和分别表示的指数映射和对数映射，用于旋转矩阵和旋转向量之间的转换。令，，则角速度为旋转方向上单位时间转动距离，在数学形式上，角速度为公式（7）的导数，公式（7）的导数可以通过链式法则和指数映射的性质来计算，并得到角速度的表示形式为：
(8)
其中，局部旋转矩阵的表达式为：
(9)
局部旋转矩阵的时间导数为：
(10)
基于上述B样条曲线的特征，可以得到任一时刻的位置由所述任一时刻之前的一个第一控制点以及所述任一时刻后连续第一目标数量的第一控制点确定；任一时刻的姿态由所述任一时刻之前的一个第二控制点以及所述任一时刻后连续第二目标数量的第二控制点确定；所述第一目标数量基于所述第一控制点所在第一B样条曲线的阶数确定，所述第二目标数量基于所述第二控制点所在的第二B样条曲线的阶数确定。
作为一种示例，第一目标数量为第一B样条曲线阶数，第二目标数量为第二B样条曲线阶数。以第一B样条曲线为3阶为例，任一时刻的位置由所述任一时刻之前的一个第一控制点以及所述任一时刻后连续3个第一控制点确定。
作为一种示例，分别采用两条3阶均有理B样条曲线表示IMU坐标系相对于站心坐标系的平移和旋转。对于时刻，其对应的平移和旋转控制点分别为和，该时刻对应的平移、速度、加速度、旋转和角速度分别由上述的式（3）、（5）、（6）、（7）和（8）插值得到。
由于IMU信息只与载体运动相关，不受环境影响，不存在观测退化。因此可以作为位姿估计的关键信息，并基于IMU信息计算各个融合阶段的位姿。
由于遮挡，隧道内部无法获取GPS定位信息，因此对于隧道环境的观测退化场景，可以基于隧道两端的定位信息和IMU信息融合得到第一姿态数据，第一姿态数据可以包括初始融合位姿。
第一位姿数据可以通过步骤102-105确定，具体如下：
步骤102，基于所述定位信息和IMU信息，生成初始数据；
可以依据定位信息和IMU信息，生成初始数据，初始数据可以包含初始位姿，初始位姿由位置曲线和姿态曲线确定。
步骤103，获取定位观测信息、运动观测信息；
通过引入IMU因子和GNSS因子，对初始位姿进行优化。
IMU因子包括：加速度观测模型、角速度观测模型、加速度零偏观测模型和角速度零偏观测模型。
针对IMU信息中的加速度构建加速度观测模型：
（11）
针对IMU信息中的角速度构建角速度观测模型：
（12）
其中，和由样条曲线插值得到，表示当地重力加速度，和分别表示时刻IMU信息中的加速度和角速度，和表示时刻IMU加速度零偏和角速度零偏，对应的协方差为，由加速度的测量噪声决定，对应的协方差为，由角速度的测量噪声决定。
由于载体的运动符合随机游走特征，因此本申请实施例还可以构建约束条件（13）和（14）：
（13）
（14）
加速度零偏观测模型为：
（15）
角速度零偏观测模型为：
（16）
其中，对应的协方差为，由加速度的零偏稳定性确定；对应的协方差为，由角速度的零偏稳定性确定。
GNSS因子包括GNSS位置观测模型。
GNSS位置观测模型为：
（17）
式（17）中，表示位置信息相对于IMU的时间延迟，表示GNSS的杆臂，表示GNSS时间戳，和由样条曲线插值得到。
的协方差矩阵为，由通过GNSS信号进行解算结果确定。
通过上述IMU因子确定运动观测信息，包括：加速度观测A，角速度观测W，加速度零偏观测BA，角速度零偏观测BW，以及位置观测信息，包括：GNSS位置观测P。
以对初始位姿进行优化的阶段为GI位姿优化阶段，G表示GNSS，I表示IMU。由于初始位姿可以用样条曲线表示，因此可以确定在GI位姿优化阶段中，待优化变量为：
（18）
式（18）中，表示样条曲线的旋转，表示样条曲线的平移控制点。表示加速度零偏，表示加角速度零偏。
步骤104，构建第一误差函数；
步骤105，采用所述定位观测信息、所述运动观测信息和所述第一误差函数对所述初始数据进行优化，生成第一位姿数据；
基于上述观测信息可以构建第一误差函数：
（19）
参照图2，示出了本申请实施例提供的一种GI位姿优化因子示意图，第一误差函数对应的因子图如图2所示，采用预设算法对第一误差函数进行优化，得到第一位姿数据。第一位姿数据包括：初始融合位姿。
步骤105，基于所述定位信息、所述IMU信息、所述图像信息以及所述第一位姿数据，生成第二位姿数据；
在隧道环境中，视觉退化并非必然会一定且持续存在，若根据图像信息中未发生观测退化的信息，对前述得到的初始融合位姿进行优化，以对位姿进行约束优化，并将此过程确定为视觉融合阶段。
在视觉融合阶段，可以结合定位信息，IMU信息和图像信息对初始融合位姿进行优化，得到包含有视觉融合位姿的第二位姿数据。
步骤106，基于所述点云信息、IMU信息、以及所述第二位姿数据，生成相对位姿；
隧道大部分区域存在观测退化导致缺少纵向平移约束，但其他方向不存在观测退化，仍可提供较高精度的横向、天向以及旋转方向的位姿约束。仅依靠点云数据无法得到高精度位姿，因此通过由激光模组采集的点云信息与IMU信息进行融合，使用LIO算法获取高精度相对位姿。
步骤107，依据所述定位信息、所述IMU信息、所述图像信息、所述第二位姿数据和所述相对位姿，确定目标位姿。
通过上述步骤得到的包括视觉优化位姿的第二位姿数据和相对位姿，基于定位信息、IMU信息、图像信息和点云信息，构建位姿输出模型，以此对位姿进一步约束，得到目标位姿。
在具体实现中，可以将各个目标位姿确定为可用于进行全局地图的全局位姿，基于全局位姿以及对应的点云信息，构建全局地图，提高了全局地图的精确度。
在本申请实施例中，通过获取测量数据；所述测量数据包括定位信息、IMU信息、图像信息以及点云信息；基于所述定位信息和所述IMU信息，生成第一位姿数据；基于所述定位信息、所述IMU信息、所述图像信息以及所述第一位姿数据，生成第二位姿数据；基于所述点云信息、IMU信息、以及所述第二位姿数据，生成相对位姿；依据所述定位信息、所述IMU信息、所述图像信息、所述第二位姿数据和所述相对位姿，确定目标位姿；从而实现通过多阶段的数据融合，由粗到精的引导位姿的优化收敛方向，提升位姿估计精度。
在本申请的一种实现方式中，所述基于定位信息和IMU信息，生成初始数据，包括：
针对第一区域，基于所述定位信息确定第一控制点、第二控制点；
针对第二区域，基于所述定位信息和所述IMU信息确定第一初始点和第二初始点；
基于所述第一控制点和所述第一初始点确定位置曲线；
基于所述第二控制点和所述第二初始点确定姿态曲线；
基于所述位置曲线和所述姿态曲线，生成初始数据；
其中，所述第二区域位于两个所述第一区域之间，所述第一区域为GNSS信号满足预设信号状态条件的区域，所述第二区域为GNSS信号不满足预设信号状态条件的区域，所述定位信息基于所述GNSS信号生成。
在真实环境中，隧道区域中接收到的GNSS信号容易丢失，造成解算结果与实际结果存在较大差异，因此可以设置预设信号状态条件以评估GNSS信号的信号质量。对于满足在预设信号状态条件的信号，确定为质量良好的GNSS信号，可以基于此计算载体的位置和姿态；对于不满足在预设信号状态条件的信号，确定为质量不佳的GNSS信号，无法直接基于此计算载体的位置和姿态。
因此可以将实际环境划分为第一区域和第二区域，第一区域位于隧道之外，第二区域为隧道所在区域。
基于定位信息确定位于第一区域中的第一控制点和第二控制点，基于定位信息和IMU信息确定位于第二区域中的第一初始点和第二初始点。
以第一控制点，第一初始点和第一差值控制点作为样条曲线控制点，生成位置曲线，以第二控制点和第二初始点，生成位姿曲线。由该位置曲线和姿态曲线确定初始数据，初始数据包括由位置曲线和姿态曲线确定的初始位姿。
作为一种示例，可以通过如下方式确定位置曲线和位姿曲线的初始位姿，包括：
1.将GPS的经纬高测量值变换到局部ENU坐标系，得到ENU测量值；
2.在GPS信号良好的区域，使用GPS的ENU测量值初始化位置样条曲线的控制点，使用GPS的航向角测量值初始化姿态样条曲线控制点；
3.在GPS信号不佳的区域，对于位置样条曲线，取前后两个GPS信号良好的ENU测量值进行线性插值获得该区域控制点的初值（即上述公式（3）中的pi）；对于姿态样条曲线，在前后两个GPS信号良好的航向角测量值的基础上使用IMU的角速度递推，取两次递推结果的均值得到该区域控制点的初值（即上述公式（7）中的Ri）。
在本申请的一种实现方式中，所述定位信息包括空间三维信息以及航向角信息；
所述基于所述定位信息确定第一控制点、第二控制点，包括：基于所述空间三维信息以及预设的站心坐标系，确定站心测量值；基于所述站心测量值确定第一控制点，并基于所述航向角信息确定第二控制点；
所述基于所述定位信息和所述IMU信息确定第一初始点和第二初始点，包括：基于目标站心测量值确定第一初始点；基于目标航向角信息以及所述IMU信息确定第二初始点；所述目标站心测量值由位于所述第二区域两端的第一区域的站心测量值确定，所述目标航向角信息由位于所述第二区域两端的第一区域站的航向角信息确定。
可以将定位信息中的空间三维信息（经度、纬度、高度）进行变换，得到站心测量值，以站心测量值确定第一控制点，依据定位信息中的航向角信息确定第二控制点。
可以对第一初始点和第一控制点之间的区域进行线性差值，得到第一差值控制点。可以依据IMU信息中的角速度对目标航向角信息进行递推，基于递推结果的均值确定第二初始点。
在本申请的一种实现方式中，所述基于所述定位信息、所述IMU信息、所述图像信息以及所述第一位姿数据，生成第二位姿数据；获取定位观测信息、运动观测信息、视觉观测信息；构建第二误差函数；采用所述定位观测信息、所述运动观测信息、所述视觉观测信息和所述第二误差函数对所述第一位姿数据进行优化，生成第二位姿数据。
可以通过引入视觉因子，对初始融合位姿进行优化，视觉因子可以根据视觉观测信息确定得到。
所述视觉观测信息包括针对视觉特征点的视觉残差；所述视觉特征点由所述图像信息进行提取得到；所述视觉残差通过如下步骤确定：
确定所述视觉特征点位于所述图像信息中的位置观测值；
确定所述视觉特征点位于预设站心坐标系下的特征点站心位置；
依据所述特征点站心位置和预设的相机投影信息，确定所述视觉特征点的位置预测值；
依据所述位置观测值和所述位置预测值确定所述视觉残差；
其中，所述依据所述特征点站心位置和预设的相机投影信息，确定所述视觉特征点的位置预测值，包括：
依据所述特征点站心位置，确定所述视觉特征点至相机原点的特征点距离；
依据所述特征点距离确定特征点夹角；所述特征点夹角为所述视觉特征点在所述站心坐标系下的位置与相机指定方向轴的夹角；
依据所述特征点夹角确定深度畸变信息；
依据所述深度畸变信息、所述特征点站心位置以及所述相机投影信息，确定所述位置预测值。
作为一种示例，上述的相机指定方向轴为相机水平方向轴。
视觉因子包括：视觉观测模型。
本申请实施例对当前图像信息提取到的K个视觉特征点进行跟踪，若跟踪误差大于某一阈值，则表示跟踪失败，如果有特征点跟踪失败，则再提取特征点，保持每帧图像的特征点总数为K。对于站心坐标系下的特征点坐标和它的一系列视觉观测，可以得到如下方程：
（20）
（21）
（22）
上述式（20），矩阵由上述的初始融合位姿插值得到， 表示该矩阵的第行。对矩阵A进行SVD分解，取最小奇异值对应的奇异向量,则。
视觉观测模型为：
（23）
（24）
其中，表示第帧图像的第个特征点的像素坐标（位置观测值），为位置预测值。和由B样条曲线插值得到，表示相机投影方程（相机投影信息）。
本申请实施例以四参数鱼眼相机为例，对相机投影方程作进一步说明。对于四参数鱼眼相机有：
（25）
（26）
（27）
（28）
（29）
（30）
其中，表示图像信息中特征点对应的时间戳，，，为视觉特征点在站心坐标系的坐标，和，分别表示为对，，进行归一化后的坐标。
对于全局曝光模型有：
（31）
其中，表示第i帧图像的起始曝光时间，表示图像曝光时长，对于卷帘曝光模型有：
（32）
其中，表示该特征点的行号，表示卷帘相机的行延时，的协方差矩阵为，由角点的提取以及跟踪误差决定。
以对初始融合位姿进行优化的阶段为GVI位姿优化阶段，G表示GNSS，V表示visual（视觉），I表示IMU。由于初始融合位姿可以用样条曲线表示，因此可以确定在GVI位姿优化阶段中，待优化变量为：
（33）
其中表示视觉特征点在站心坐标系下的坐标。
通过上述IMU因子确定运动观测信息，包括：加速度观测A，角速度观测W，加速度零偏观测BA，角速度零偏观测BW。通过上述GNSS因子确定位置观测信息，包括：GNSS位置观测P。通过上述视觉因子确定视觉观测信息，包括：视觉特征观测C。
基于上述的观测信息可以构建第二误差函数：
（34）
参照图3，示出了本申请实施例提供的一种GVI位姿优化因子示意图，第二误差函数对应的因子图如图3所示，采用预设算法对第二误差函数进行优化，得到第二位姿数据。第二位姿数据包括：视觉融合位姿、速度信息、零偏观测信息。
在本申请的一种实现方式中，所述依据所述定位信息、所述IMU信息、所述图像信息、所述第二位姿数据和所述相对位姿，确定目标位姿，包括：
获取定位观测信息、运动观测信息、视觉观测信息；
基于所述相对位姿确定激光观测信息；
构建第三误差函数；
采用所述定位观测信息、所述运动观测信息、所述视觉观测信息、所述激光观测信息和所述第三误差函数对所述第二位姿数据进行优化，确定目标位姿。
可以通过引入激光因子，对视觉融合位姿进行优化，激光因子可以根据激光观测信息确定得到。
激光因子包括：相对位姿观测模型。
（35）
式（35）中，和表示相对位姿观测信息，其协方差矩阵为。
以对初始融合位姿进行优化的阶段为GLVI位姿优化阶段，G表示GNSS，L表示lidar（激光），V表示visual（视觉），I表示IMU。由于视觉融合位姿可以用样条曲线表示，因此可以确定在GLVI位姿优化阶段中，待优化变量为：
（36）
通过上述IMU因子确定运动观测信息，包括：加速度观测A，角速度观测W，加速度零偏观测BA，角速度零偏观测BW。通过上述GNSS因子确定位置观测信息，包括：GNSS位置观测P。通过上述视觉因子确定视觉观测信息，包括：视觉特征观测C，通过激光因子确定激光观测信息，包括：相对位姿观测LP。
基于上述的观测信息可以构建第三误差函数：
（37）
参照图4，示出了本申请实施例提供的一种GLVI位姿优化因子示意图，第二误差函数对应的因子图如图4所示，采用预设算法对第三误差函数进行优化，得到目标位姿。
在本申请的一种实现方式中，所述基于所述点云信息、IMU信息、以及所述第二位姿数据，生成相对位姿，包括：
基于所述IMU信息构建状态预测初始模型；
基于所述点云信息、所述IMU信息和所述第二位姿数据，生成激光观测模型；
确定当前所在位置的激光观测退化程度；
基于所述第二位姿数据以及所述激光观测退化程度确定速度观测模型；
基于所述状态预测初始模型、所述激光观测模型、所述速度观测模型，生成激光惯性里程计更新模型；
采用所述激光惯性里程计更新模型输出相对位姿。
本申请实施例提供了一种改进LIO算法模型，由于在上述GVI阶段，能够获取视觉融合位姿、速度信息、零偏观测信息，因此本申请实施例提供的改进LIO算法能够进行动态初始化，并更根据场景退化程度自适应调整GVI阶段生成的第二位姿数据中各个信息观测权重，减小观测退化场景的位姿估计误差。
所述状态预测初始模型基于所述IMU信息以及基于迭代卡尔曼滤波的激光惯性里程计模型得到；所述基于迭代卡尔曼滤波的激光惯性里程计模型设置有状态向量，所述状态向量包括： IMU坐标与预设的世界坐标系的旋转、所述IMU坐标与所述世界坐标系的平移、IMU坐标系在世界坐标系的速度、IMU的角速度零偏、IMU的加速度零偏；其中，所述IMU坐标系基于IMU模组构建得到，所述IMU坐标为IMU模组位于所述IMU坐标系的位置，所述IMU模组为用于测量IMU信息的器件。具体如下：本申请实施例提供的改进LIO算法模型为基于迭代卡尔曼滤波的LIO算法模型，其状态向量为：
（38）
式（38）中，表示IMU相对于LIO世界坐标系的旋转，表示IMU相对于LIO世界坐标系的平移，表示IMU坐标系在LIO世界坐标系下的速度，表示IMU的角速度零偏，表示IMU的加速度零偏。
基于IMU信息构建状态预测初始模型：
（39）
其中，（40）
式（40）中，表示IMU的角速度测量值，表示IMU的加速度测量值，表示IMU角度测量噪声，表示IMU加速度测量噪声，表示IMU角速度零偏的高斯白噪声，表示IMU加速度零偏的高斯白噪声，表示单次递推时间，表示LIO世界坐标系下的重力加速度。
根据点云信息和激光观测模型，对点到面误差进行状态更新。激光观测模型为：
（41）
式（41）中，为当前激光点云中的源点，为地图中的匹配点，为匹配点所在平面的法向量，表示当前位姿，表示激光相对于IMU的外参。
在本申请实施例中，基于所述第二位姿数据以及所述激光观测退化程度确定速度观测模型，包括：确定所述第二位姿数据中的IMU坐标系相对于站心坐标系的旋转、所述IMU坐标系相对于站心坐标系的速度；采用所述IMU坐标系相对于站心坐标系的旋转、所述IMU坐标系相对于站心坐标系的速度构建站心速度向量；确定所述IMU坐标系相对于预设世界坐标系的旋转、所述IMU坐标系相对于世界坐标系的速度；采用所述IMU坐标系相对于预设世界坐标系的旋转、所述IMU坐标系相对于世界坐标系的速度构建世界速度向量；依据所述站心速度向量和所述世界速度向量确定速度观测向量；确定与所述激光观测退化程度对应的退化加权值；依据所述退化加权值与所述速度观测向量的乘积，构建速度观测模型。
基于GVI阶段输出的速度信息（包括上述的IMU坐标系相对于站心坐标系的旋转、IMU坐标系相对于站心坐标系的速度），构建速度观测模型：
（42）
其中，
式（42）中，和分别表示k时刻IMU坐标系相对于站心坐标系的旋转和速度，通过上述GVI阶段得到。为k时刻IMU坐标系相对于世界坐标系的旋转，为k时刻IMU坐标系相对于世界坐标系的速度。
为站心速度向量，为世界速度向量。
表示当前环境的激光退化程度，为矩阵的最小特征值。通过激光退化函数1（），确定不同退化程度对应的退化加权值，退化加权值为1或者0。其中矩阵H为：
（43）
式（43）中，表示匹配点总数。
在本申请实施例的一种实现方式中，所述基于所述状态预测初始模型、所述激光观测模型、所述速度观测模型，生成激光惯性里程计更新模型，包括：
所述状态预测初始模型用于输出IMU的预测状态向量与当前状态向量的状态差异矩阵，以及状态权重值；
所述激光观测模型用于输出当前点云信息中源端至预设地图中的距离误差矩阵；
所述速度观测模型用于输出当前站心坐标系下的速度向量与当前世界坐标系下的速度向量之间的速度差异矩阵；
所述激光惯性里程计更新模型用于确定第一计算值、第二计算值和第三计算值之和为最小值时的目标状态向量；
其中，所述第一计算值为所述状态差异矩阵及其转置的自乘与所述状态权重值的乘积、所述距离误差矩阵及其转置的自乘与预设距离观测权重的乘积、所述速度差异矩阵及其转置的自乘与预设速度观测权重的乘积。
结合上述初始模型、激光观测模型、速度观测模型，生成激光惯性里程计更新模型（即上述的改进LIO算法模型），激光惯性里程计更新模型为：
（44）
式（44）中，表示先验状态（预测状态向量），表示为协方差矩阵（状态权重值），表示激光观测误差的协方差矩阵（距离观测权重值），表示速度观测的协方差矩阵（速度观测权重值），通过对激光惯性里程计更新模型迭代求解，获得目标状态向量，并结合公式（31）获得相对位姿。
参照图5，示出了本申请实施例提供的一种位姿确定装置的示意图，具体可以包括数据获取模块501、初始数据生成模块502、观测信息第一获取模块503、第一误差函数构建模块504、第一位姿数据生成模块505、第二位姿数据生成模块506、相对位姿生成模块507、目标位姿生成模块508，其中：
数据获取模块501，用于获取测量数据；所述测量数据包括定位信息、IMU信息、图像信息以及点云信息；
初始数据生成模块502，用于基于所述定位信息和IMU信息，生成初始数据；
观测信息第一获取模块503，用于获取定位观测信息、运动观测信息；
第一误差函数构建模块504，用于构建第一误差函数；
第一位姿数据生成模块505，用于采用所述定位观测信息、所述运动观测信息和所述第一误差函数对所述初始数据进行优化，生成第一位姿数据；
第二位姿数据生成模块506，用于基于所述定位信息、所述IMU信息、所述图像信息以及所述第一位姿数据，生成第二位姿数据；
相对位姿生成模块507，用于基于所述点云信息、IMU信息、以及所述第二位姿数据，生成相对位姿；
目标位姿生成模块508，用于依据所述定位信息、所述IMU信息、所述图像信息、所述第二位姿数据和所述相对位姿，确定目标位姿。
在本申请实施例的一种实现方式中，位姿包括位置和姿态；
任一时刻的位置由所述任一时刻之前的一个第一控制点以及所述任一时刻后连续第一目标数量的第一控制点确定；任一时刻的姿态由所述任一时刻之前的一个第二控制点以及所述任一时刻后连续第二目标数量的第二控制点确定；所述第一目标数量基于所述第一控制点所在第一B样条曲线的阶数确定，所述第二目标数量基于所述第二控制点所在的第二B样条曲线的阶数确定。
在本申请实施例的一种实现方式中，所述初始数据生成子模块包括：
控制点确定单元，用于针对第一区域，基于所述定位信息确定第一控制点、第二控制点；
初始点确定单元，用于针对第二区域，基于所述定位信息和所述IMU信息确定第一初始点和第二初始点；
位置曲线确定单元，用于基于所述第一控制点和所述第一初始点确定位置曲线；
姿态曲线确定单元，用于基于所述第二控制点和所述第二初始点确定姿态曲线；
初始数据生成单元，用于基于所述位置曲线和所述姿态曲线，生成初始数据；
其中，所述第二区域位于两个所述第一区域之间，所述第一区域为GNSS信号满足预设信号状态条件的区域，所述第二区域为GNSS信号不满足预设信号状态条件的区域，所述定位信息基于所述GNSS信号生成。
在本申请实施例的一种实现方式中，所述定位信息包括空间三维信息以及航向角信息；
所述控制点确定单元包括：
站心测量值确定子单元，用于基于所述空间三维信息以及预设的站心坐标系，确定站心测量值；
控制点确定子单元，用于基于所述站心测量值确定第一控制点，并基于所述航向角信息确定第二控制点；
所述初始点确定单元包括：
第一初始点确定单元，用于基于目标站心测量值确定第一初始点；
第二初始点确定单元，用于基于目标航向角信息以及所述IMU信息确定第二初始点；所述目标站心测量值由位于所述第二区域两端的第一区域的站心测量值确定，所述目标航向角信息由位于所述第二区域两端的第一区域站的航向角信息确定。
在本申请实施例的一种实现方式中，所述第二位姿数据生成模块506，包括：
观测信息第二获取子模块，用于获取定位观测信息、运动观测信息、视觉观测信息；
第二误差函数构建子模块，用于构建第二误差函数；
第二位姿数据生成子模块，用于采用所述定位观测信息、所述运动观测信息、所述视觉观测信息和所述第二误差函数对所述第一位姿数据进行优化，生成第二位姿数据。
在本申请实施例的一种实现方式中，所述目标位姿生成模块508包括：
观测信息第三获取子模块，用于获取定位观测信息、运动观测信息、视觉观测信息；
激光观测信息确定子模块，用于基于所述相对位姿确定激光观测信息；
第三误差函数构建子模块，用于构建第三误差函数；
目标位姿确定子模块，用于采用所述定位观测信息、所述运动观测信息、所述视觉观测信息、所述激光观测信息和所述第三误差函数对所述第二位姿数据进行优化，确定目标位姿。
在本申请实施例的一些实现方式中，所述视觉观测信息包括针对视觉特征点的视觉残差；所述视觉特征点由所述图像信息进行提取得到；所述视觉残差通过如下子模块确定：
位置观测值确定子模块，用于确定所述视觉特征点位于所述图像信息中的位置观测值；
特征点站心位置确定子模块，用于确定所述视觉特征点位于预设站心坐标系下的特征点站心位置；
位置预测值确定子模块，用于依据所述特征点站心位置和预设的相机投影信息，确定所述视觉特征点的位置预测值；
视觉残差确定子模块，用于依据所述位置观测值和所述位置预测值确定所述视觉残差；
其中，所述位置观测值确定子模块包括：
特征点距离确定单元，用于依据所述特征点站心位置，确定所述视觉特征点至相机原点的特征点距离；
特征点夹角确定单元，用于依据所述特征点距离确定特征点夹角；所述特征点夹角为所述视觉特征点在所述站心坐标系下的位置与相机指定方向轴的夹角；
深度畸变信息确定单元，用于依据所述特征点夹角确定深度畸变信息；
位置预测值确定单元，用于依据所述深度畸变信息、所述特征点站心位置以及所述相机投影信息，确定所述位置预测值。
在本申请实施例的一种实现方式中，所述相对位姿生成模块507包括：
初始模型构建子模块，用于基于所述IMU信息构建状态预测初始模型；
激光观测模型生成子模块，用于基于所述点云信息、所述IMU信息和所述第二位姿数据，生成激光观测模型；
激光观测退化程度确定子模块，用于确定当前所在位置的激光观测退化程度；
速度观测模型确定子模块，用于基于所述第二位姿数据以及所述激光观测退化程度确定速度观测模型；
激光惯性里程计更新模型生成子模块，用于基于所述状态预测初始模型、所述激光观测模型、所述速度观测模型，生成激光惯性里程计更新模型；
相对位姿输出子模块，用于采用所述激光惯性里程计更新模型输出相对位姿。
在本申请实施例的一些实现方式中，所述速度观测模型确定子模块包括：
第二位姿数据提取单元，用于确定所述第二位姿数据中的IMU坐标系相对于站心坐标系的旋转、所述IMU坐标系相对于站心坐标系的速度；
站心速度向量确定单元，用于采用所述IMU坐标系相对于站心坐标系的旋转、所述IMU坐标系相对于站心坐标系的速度构建站心速度向量；
世界坐标系相关信息确定单元，用于确定所述IMU坐标系相对于预设世界坐标系的旋转、所述IMU坐标系相对于世界坐标系的速度；
世界速度向量确定单元，用于采用所述IMU坐标系相对于预设世界坐标系的旋转、所述IMU坐标系相对于世界坐标系的速度构建世界速度向量；
速度观测向量确定单元，用于依据所述站心速度向量和所述世界速度向量确定速度观测向量；
退化加权值确定单元，用于确定与所述激光观测退化程度对应的退化加权值；
速度观测模型构建单元，用于依据所述退化加权值与所述速度观测向量的乘积，构建速度观测模型。
在本申请实施例的一种实现方式中，所述状态预测初始模型用于输出IMU的预测状态向量与当前状态向量的状态差异矩阵，以及状态权重值；
所述激光观测模型用于输出当前点云信息中源端至预设地图中的距离误差矩阵；
所述速度观测模型用于输出当前站心坐标系下的速度向量与当前世界坐标系下的速度向量之间的速度差异矩阵；
所述激光惯性里程计更新模型用于确定第一计算值、第二计算值和第三计算值之和为最小值时的目标状态向量；
其中，所述第一计算值为所述状态差异矩阵及其转置的自乘与所述状态权重值的乘积、所述距离误差矩阵及其转置的自乘与预设距离观测权重的乘积、所述速度差异矩阵及其转置的自乘与预设速度观测权重的乘积。
在本申请实施例的一种实现方式中，所述状态预测初始模型基于所述IMU信息以及基于迭代卡尔曼滤波的激光惯性里程计模型得到；所述基于迭代卡尔曼滤波的激光惯性里程计模型设置有状态向量，所述状态向量包括：
IMU坐标与预设的世界坐标系的旋转、所述IMU坐标与所述世界坐标系的平移、IMU坐标系在世界坐标系的速度、IMU的角速度零偏、IMU的加速度零偏；
其中，所述IMU坐标系基于IMU模组构建得到，所述IMU坐标为IMU模组位于所述IMU坐标系的位置，所述IMU模组为用于测量IMU信息的器件。
本申请实施例提供的一种位姿确定装置，应用该装置，可以实现前述各个方法实施例中的各个步骤。
对于装置实施例而言，由于其与方法实施例基本相似，所以描述得比较简单，相关之处参见方法实施例部分的说明即可。
参照图6，示出了本申请实施例提供的一种电子设备的示意图。如图6所示，本申请实施例中的电子设备600包括：处理器610、存储器620以及存储在所述存储器620中并可在所述处理器610上运行的计算机程序621。所述处理器610执行所述计算机程序621时实现上述位姿方法各个实施例中的步骤，例如图1所示的步骤101至105。或者，所述处理器610执行所述计算机程序621时实现上述各装置实施例中各模块/单元的功能，例如图5所示模块501至505的功能。
示例性的，所述计算机程序621可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器620中，并由所述处理器610执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段可以用于描述所述计算机程序621在所述电子设备600中的执行过程。例如，所述计算机程序621可以被分割成数据获取模块、第一位姿数据生成模块、第二位姿数据生成模块、相对位姿生成模块、目标位姿生成模块，各模块具体功能如下：
数据获取模块，用于获取测量数据；所述测量数据包括定位信息、IMU信息、图像信息以及点云信息；
第一位姿数据生成模块，用于基于所述定位信息和所述IMU信息，生成第一位姿数据；
第二位姿数据生成模块，用于基于所述定位信息、所述IMU信息、所述图像信息以及所述第一位姿数据，生成第二位姿数据；
相对位姿生成模块，用于基于所述点云信息、IMU信息、以及所述第二位姿数据，生成相对位姿；
目标位姿生成模块，用于依据所述定位信息、所述IMU信息、所述图像信息、所述第二位姿数据和所述相对位姿，确定目标位姿。
所述电子设备600可以是桌上型计算机、云端服务器等计算设备。所述电子设备600可包括，但不仅限于，处理器610、存储器620。本领域技术人员可以理解，图6仅仅是电子设备600的一种示例，并不构成对电子设备600的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述电子设备600还可以包括输入输出设备、网络接入设备、总线等。
所述处理器610可以是中央处理单元（Central Processing Unit，CPU），还可以是其他通用处理器、数字信号处理器（Digital Signal Processor，DSP）、专用集成电路（Application Specific Integrated Circuit，ASIC）、现成可编程门阵列（Field-Programmable Gate Array，FPGA）或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器620可以是所述电子设备600的内部存储单元，例如电子设备600的硬盘或内存。所述存储器620也可以是所述电子设备600的外部存储设备，例如所述电子设备600上配备的插接式硬盘，智能存储卡（Smart Media Card，SMC），安全数字（SecureDigital，SD）卡，闪存卡（Flash Card）等等。进一步地，所述存储器620还可以既包括所述电子设备600的内部存储单元也包括外部存储设备。所述存储器620用于存储所述计算机程序621以及所述电子设备600所需的其他程序和数据。所述存储器620还可以用于暂时地存储已经输出或者将要输出的数据。
本申请实施例还公开了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如前述各个实施例所述的位姿确定方法。
本申请实施例还公开了一种计算机程序产品，包括计算机程序，所述计算机程序被运行时，使得如前述各个实施例所述的位姿确定方法被执行。
以上所述实施例仅用以说明本申请的技术方案，而非对其限制。尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (11)

1.一种位姿确定方法，其特征在于，包括：
获取测量数据；所述测量数据包括定位信息、IMU信息、图像信息以及点云信息；
基于所述定位信息和IMU信息，生成初始数据；
获取定位观测信息、运动观测信息；
构建第一误差函数；
采用所述定位观测信息、所述运动观测信息和所述第一误差函数对所述初始数据进行优化，生成第一位姿数据
基于所述定位信息、所述IMU信息、所述图像信息以及所述第一位姿数据，生成第二位姿数据；
基于所述点云信息、IMU信息、以及所述第二位姿数据，生成相对位姿；
依据所述定位信息、所述IMU信息、所述图像信息、所述第二位姿数据和所述相对位姿，确定目标位姿。
2.根据权利要求1所述的方法，其特征在于，位姿包括位置和姿态；
任一时刻的位置由所述任一时刻之前的一个第一控制点以及所述任一时刻后连续第一目标数量的第一控制点确定；
任一时刻的姿态由所述任一时刻之前的一个第二控制点以及所述任一时刻后连续第二目标数量的第二控制点确定；
所述第一目标数量基于所述第一控制点所在第一B样条曲线的阶数确定，所述第二目标数量基于所述第二控制点所在的第二B样条曲线的阶数确定。
3.根据权利要求1所述的方法，其特征在于，所述基于所述定位信息和IMU信息，生成初始数据，包括：
针对第一区域，基于所述定位信息确定第一控制点、第二控制点；
针对第二区域，基于所述定位信息和所述IMU信息确定第一初始点和第二初始点；
基于所述第一控制点和所述第一初始点确定位置曲线；
基于所述第二控制点和所述第二初始点确定姿态曲线；
基于所述位置曲线和所述姿态曲线，生成初始数据；
其中，所述第二区域位于两个所述第一区域之间，所述第一区域为GNSS信号满足预设信号状态条件的区域，所述第二区域为GNSS信号不满足预设信号状态条件的区域，所述定位信息基于所述第一区域的GNSS信号生成。
4.根据权利要求3所述的方法，其特征在于，所述定位信息包括空间三维信息以及航向角信息；
所述基于所述定位信息确定第一控制点、第二控制点，包括：基于所述空间三维信息以及预设的站心坐标系，确定站心测量值；基于所述站心测量值确定第一控制点，并基于所述航向角信息确定第二控制点；
所述基于所述定位信息和所述IMU信息确定第一初始点和第二初始点，包括：基于目标站心测量值确定第一初始点；基于目标航向角信息以及所述IMU信息确定第二初始点；所述目标站心测量值由位于所述第二区域两端的第一区域的站心测量值确定，所述目标航向角信息由位于所述第二区域两端的第一区域的航向角信息确定。
5.根据权利要求1所述的方法，其特征在于，所述基于所述定位信息、所述IMU信息、所述图像信息以及所述第一位姿数据，生成第二位姿数据，包括：
获取定位观测信息、运动观测信息、视觉观测信息；
构建第二误差函数；
采用所述定位观测信息、所述运动观测信息、所述视觉观测信息和所述第二误差函数对所述第一位姿数据进行优化，生成第二位姿数据；
所述依据所述定位信息、所述IMU信息、所述图像信息、所述第二位姿数据和所述相对位姿，确定目标位姿，包括：
获取定位观测信息、运动观测信息、视觉观测信息；
基于所述相对位姿确定激光观测信息；
构建第三误差函数；
采用所述定位观测信息、所述运动观测信息、所述视觉观测信息、所述激光观测信息和所述第三误差函数对所述第二位姿数据进行优化，确定目标位姿。
6.根据权利要求5所述的方法，其特征在于，所述视觉观测信息包括针对视觉特征点的视觉残差；所述视觉特征点由所述图像信息进行提取得到；所述视觉残差通过如下步骤确定：
确定所述视觉特征点位于所述图像信息中的位置观测值；
确定所述视觉特征点位于预设站心坐标系下的特征点站心位置；
依据所述特征点站心位置和预设的相机投影信息，确定所述视觉特征点的位置预测值；
依据所述位置观测值和所述位置预测值确定所述视觉残差；
其中，所述依据所述特征点站心位置和预设的相机投影信息，确定所述视觉特征点的位置预测值，包括：
依据所述特征点站心位置，确定所述视觉特征点至相机原点的特征点距离；
依据所述特征点距离确定特征点夹角；所述特征点夹角为所述视觉特征点在所述站心坐标系下的位置与相机指定方向轴的夹角；
依据所述特征点夹角确定深度畸变信息；
依据所述深度畸变信息、所述特征点站心位置以及所述相机投影信息，确定所述位置预测值。
7.根据权利要求1所述的方法，其特征在于，所述基于所述点云信息、IMU信息、以及所述第二位姿数据，生成相对位姿，包括：
基于所述IMU信息构建状态预测初始模型；
基于所述点云信息、所述IMU信息和所述第二位姿数据，生成激光观测模型；
确定当前所在位置的激光观测退化程度；
基于所述第二位姿数据以及所述激光观测退化程度确定速度观测模型；
基于所述状态预测初始模型、所述激光观测模型、所述速度观测模型，生成激光惯性里程计更新模型；
采用所述激光惯性里程计更新模型输出相对位姿。
8.根据权利要求7所述的方法，其特征在于，所述基于所述激光观测退化程度确定速度观测模型，包括：
确定所述第二位姿数据中的IMU坐标系相对于站心坐标系的旋转、所述IMU坐标系相对于站心坐标系的速度；
采用所述IMU坐标系相对于站心坐标系的旋转、所述IMU坐标系相对于站心坐标系的速度构建站心速度向量；
确定所述IMU坐标系相对于预设世界坐标系的旋转、所述IMU坐标系相对于世界坐标系的速度；
采用所述IMU坐标系相对于预设世界坐标系的旋转、所述IMU坐标系相对于世界坐标系的速度构建世界速度向量；
依据所述站心速度向量和所述世界速度向量确定速度观测向量；
确定与所述激光观测退化程度对应的退化加权值；
依据所述退化加权值与所述速度观测向量的乘积，构建速度观测模型。
9.根据权利要求7所述的方法，其特征在于，
所述状态预测初始模型用于输出IMU的预测状态向量与当前状态向量的状态差异矩阵，以及状态权重值；
所述激光观测模型用于输出当前点云信息中源端至预设地图中的距离误差矩阵；
所述速度观测模型用于输出当前站心坐标系下的速度向量与当前世界坐标系下的速度向量之间的速度差异矩阵；
所述激光惯性里程计更新模型用于确定第一计算值、第二计算值和第三计算值之和为最小值时的目标状态向量；
其中，所述第一计算值为所述状态差异矩阵及其转置的自乘与所述状态权重值的乘积、所述距离误差矩阵及其转置的自乘与预设距离观测权重的乘积、所述速度差异矩阵及其转置的自乘与预设速度观测权重的乘积。
10.根据权利要求7所述的方法，其特征在于，所述状态预测初始模型基于所述IMU信息以及基于迭代卡尔曼滤波的激光惯性里程计模型得到；所述基于迭代卡尔曼滤波的激光惯性里程计模型设置有状态向量，所述状态向量包括：
IMU坐标与预设的世界坐标系的旋转、所述IMU坐标与所述世界坐标系的平移、IMU坐标系在世界坐标系的速度、IMU的角速度零偏、IMU的加速度零偏；
其中，所述IMU坐标系基于IMU模组构建得到，所述IMU坐标为IMU模组位于所述IMU坐标系的位置，所述IMU模组为用于测量IMU信息的器件。
11.一种电子设备，其特征在于，包括处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时，使得所述电子设备实现如权利要求1-10中任一项所述的方法。
CN202411922986.8A 2024-12-25 2024-12-25 一种位姿确定方法及电子设备 Active CN119359796B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202411922986.8A CN119359796B (zh) 2024-12-25 2024-12-25 一种位姿确定方法及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202411922986.8A CN119359796B (zh) 2024-12-25 2024-12-25 一种位姿确定方法及电子设备

Publications (2)

Publication Number Publication Date
CN119359796A CN119359796A (zh) 2025-01-24
CN119359796B true CN119359796B (zh) 2025-05-02

Family

ID=94301414

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202411922986.8A Active CN119359796B (zh) 2024-12-25 2024-12-25 一种位姿确定方法及电子设备

Country Status (1)

Country Link
CN (1) CN119359796B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN119958547A (zh) * 2025-02-20 2025-05-09 招商局重庆交通科研设计院有限公司 基于事件辅助的视觉惯性里程计方法及相关装置

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118258378A (zh) * 2024-03-28 2024-06-28 华中科技大学 一种融合gnss信号的多模态高精度鲁棒位姿估计方法和系统
CN118670382A (zh) * 2023-03-20 2024-09-20 广州行深智能科技有限公司 视觉惯性定位方法及装置

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109887057B (zh) * 2019-01-30 2023-03-24 杭州飞步科技有限公司 生成高精度地图的方法和装置
CN111561923B (zh) * 2020-05-19 2022-04-15 北京数字绿土科技股份有限公司 基于多传感器融合的slam制图方法、系统
CN115690338A (zh) * 2021-07-30 2023-02-03 北京图森智途科技有限公司 地图构建方法、装置、设备及存储介质
CN114061596B (zh) * 2021-11-19 2024-03-22 北京国家新能源汽车技术创新中心有限公司 自动驾驶定位方法、系统、测试方法、设备及存储介质
CN117268373B (zh) * 2023-11-21 2024-02-13 武汉大学 一种多传感器信息融合的自主导航方法及系统
CN118274815A (zh) * 2024-03-25 2024-07-02 浙江大学 一种长隧道环境下的实时定位和建图方法
CN118424254A (zh) * 2024-03-29 2024-08-02 人工智能与数字经济广东省实验室(深圳) 一种基于无人机的未知封闭空间的自主化建图检测方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118670382A (zh) * 2023-03-20 2024-09-20 广州行深智能科技有限公司 视觉惯性定位方法及装置
CN118258378A (zh) * 2024-03-28 2024-06-28 华中科技大学 一种融合gnss信号的多模态高精度鲁棒位姿估计方法和系统

Also Published As

Publication number Publication date
CN119359796A (zh) 2025-01-24

Similar Documents

Publication Publication Date Title
CN114013449B (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
CN109887057B (zh) 生成高精度地图的方法和装置
CN114217665B (zh) 一种相机和激光雷达时间同步方法、装置及存储介质
US10247556B2 (en) Method for processing feature measurements in vision-aided inertial navigation
US9020204B2 (en) Method and an apparatus for image-based navigation
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
US20190301871A1 (en) Direct Sparse Visual-Inertial Odometry Using Dynamic Marginalization
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
Bucci et al. Comparison of feature detection and outlier removal strategies in a mono visual odometry algorithm for underwater navigation
US20060256200A1 (en) Method and system for improving video metadata through the use of frame-to-frame correspondences
CN111829532A (zh) 一种飞行器重定位系统和重定位方法
CN116027351B (zh) 一种手持/背包式slam装置及定位方法
US20140286537A1 (en) Measurement device, measurement method, and computer program product
CN115930948A (zh) 一种果园机器人融合定位方法
CN115829879A (zh) 面向敏捷卫星的姿态四元数处理方法、装置、设备
CN117292118B (zh) 雷达引导光电跟踪坐标补偿方法、装置、电子设备及介质
CN118936478A (zh) 多源数据融合的定位定姿方法、装置和巡检系统
CN119359796A (zh) 一种位姿确定方法及电子设备
CN115601431B (zh) 针对视觉惯性里程计的稳定性的控制方法及相关设备
CN112097758A (zh) 定位方法、装置、机器人定位方法和机器人
CN119984244A (zh) 一种基于多传感器融合的联合建图方法、装置、设备及介质
CN116912310B (zh) 相机位姿估计方法、装置、计算机设备及介质
CN118115597A (zh) 双目相机、imu和激光雷达联合标定方法及系统
CN118169725A (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