铰接式车辆的位姿估计方法、装置、车辆及存储介质与流程

1.本发明涉及车辆技术领域，具体涉及铰接式车辆的位姿估计方法、装置、铰接式车辆及存储介质。


背景技术：

2.铰接式车辆通常由两个刚体通过一个节点连接构成。在相关技术中，在对铰接式车辆进行位姿估计时，通常采用适用于单个刚体的车辆位姿估计算法来分别估计铰接式车辆的两个刚体的位姿，以确定铰接式车辆的位姿。但是，这种铰接式车辆的位姿估计方法容易忽略两个刚体的联系，导致铰接式车辆的位姿估计的准确度较差。


技术实现要素：

3.有鉴于此，本发明提供了一种铰接式车辆的位姿估计方法、装置、车辆及存储介质，以解决分别估计铰接式车辆的两个刚体的位姿，造成铰接式车辆的位姿估计的准确度较差的问题。
4.第一方面，本发明提供了一种铰接式车辆的位姿估计方法，所述方法包括：获取目标铰接式车辆的相对运动信息，所述相对运动信息包括第一相机在当前时刻和上一时刻采集到的特征点射线、第二相机在当前时刻和上一时刻采集到的特征点射线、上一时刻所述目标铰接式车辆的第一刚体与第二刚体的刚体坐标系之间的第一刚体旋转矩阵，所述第一相机和所述第二相机分别位于所述第一刚体与所述第二刚体上；基于所述相对运动信息，对所述第一刚体的广义本质矩阵约束和所述目标铰接式车辆的多相机对极几何约束进行处理，得到相应的第一最小化代数误差和第二最小化代数误差；对所述第一刚体的本质矩阵、所述第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻所述第一刚体与所述第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以将所述第一最小化代数误差和所述第二最小化代数误差分别进行转换；对转换后的所述第一最小化代数误差和所述第二最小化代数误差进行求解，得到所述本质矩阵、所述相对旋转矩阵和所述第二刚体旋转矩阵的参数值；基于所述相对运动信息、所述本质矩阵的参数值、所述相对旋转矩阵的参数值和所述第二刚体旋转矩阵的参数值，对所述第一相机和所述第二相机在当前时刻采集的特征点进行处理，得到所述目标铰接式车辆的位姿。
5.在该方式中，通过目标铰接式车辆的相对运动信息，对第一刚体的广义本质矩阵约束和目标铰接式车辆的多相机对极几何约束进行处理，以得到两个约束的第一最小化代数误差和第二最小化代数误差，进而对第一最小化代数误差和第二最小化代数误差进行转换和求解，得到第一刚体的本质矩阵、第一刚体在相邻时刻之间的相对旋转矩阵和当前时刻第一刚体与第二刚体的第二刚体旋转矩阵的参数值，从而能够确定当前时刻第一刚体的运动状态以及第一刚体和第二刚体之间的联系，以对第一相机和第二相机在当前时刻采集
的特征点进行处理，准确确定出目标铰接式车辆两个刚体的相对位姿，以提高铰接式车辆位姿估计的准确性。
6.在一种可选的实施方式中，所述基于所述相对运动信息，对所述第一刚体的广义本质矩阵约束和所述目标铰接式车辆的多相机对极几何约束进行处理，得到相应的第一最小化代数误差和第二最小化代数误差，包括：基于所述相对运动信息，将所述第一刚体的广义本质矩阵约束和所述目标铰接式车辆的多相机对极几何约束分别进行二次型转换；基于转换后的所述广义本质矩阵约束，计算第一最小化代数误差，转换后的所述广义本质矩阵约束以所述本质矩阵和所述相对旋转矩阵为未知变量；基于转换后的所述多相机对极几何约束，计算第二最小化代数误差，转换后的所述多相机对极几何约束以所述本质矩阵、所述相对旋转矩阵和所述第二刚体旋转矩阵为未知变量。
7.在该方式中，基于相对运动信息，将第一刚体的广义本质矩阵约束和目标铰接式车辆的多相机对极几何约束分别进行二次型转换，再基于转换后的广义本质矩阵约束和多相机对极几何约束分别计算第一最小化代数误差和第二最小化代数误差，因此，能够简化基于第一刚体的广义本质矩阵约束和目标铰接式车辆的多相机对极几何约束，确定第一刚体本质矩阵、相对旋转矩阵和第二刚体旋转矩阵的参数值的计算过程，同时能够使所计算的参数值更接近真实值，以提高后续所估计的目标铰接式车辆的位姿的准确性。
8.在一种可选的实施方式中，所述对所述第一刚体的本质矩阵、所述第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻所述第一刚体与所述第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以将所述第一最小化代数误差和所述第二最小化代数误差分别进行转换，包括：对所述第一刚体的本质矩阵、所述第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻所述第一刚体与所述第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，得到相应的第一向量、第二向量和第三向量；基于所述第一向量和所述第二向量，对所述第一最小化代数误差进行转换；基于所述第三向量，对所述第二最小化代数误差进行转换。
9.在该方式中，通过对第一刚体的本质矩阵、第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻第一刚体与第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以基于向量化得到的第一向量、第二向量和第三向量对第一最小化代数误差和第二最小化代数误差进行转换，因此，能够将原有的目标铰接式车辆的相对位姿估计问题转换为二次约束二次规划问题，从而能够在后续过程中求解得到本质矩阵、相对旋转矩阵和第二刚体旋转矩阵的全局最优解，以进一步提高目标铰接式车辆位姿估计的准确性。
10.在一种可选的实施方式中，转换后的所述第一最小化代数误差基于以下公式表示：
；其中，为所述转换后的第一最小化代数误差，e为所述第一向量，r为所述第二向量，为第一半正定对称矩阵，为所述第一向量的转置，为所述第二向量的转置，为所述本质矩阵，为所述第一刚体在当前时刻和上一时刻之间的相对位移，为所述相对旋转矩阵，so(3)为三维旋转群。
11.在一种可选的实施方式中，转换后的所述第二最小化代数误差基于以下公式表示：；其中，为所述转换后的第二最小化代数误差，为所述第二刚体旋转矩阵，为所述第二刚体旋转矩阵的转置，q为所述第三向量，为所述第三向量的转置，为第二半正定对称矩阵，i为单位矩阵，so(3)为三维旋转群。
12.在一种可选的实施方式中，所述对转换后的所述第一最小化代数误差和所述第二最小化代数误差进行求解，得到所述本质矩阵、所述相对旋转矩阵和所述第二刚体旋转矩阵的参数值，包括：对转换后的所述第一最小化代数误差进行求解，得到所述本质矩阵和所述相对旋转矩阵的参数值；基于所述本质矩阵的参数值和所述相对旋转矩阵的参数值，对转换后的所述第二最小化代数误差进行求解，得到所述第二刚体旋转矩阵的参数值。
13.在一种可选的实施方式中，所述基于所述相对运动信息、所述本质矩阵的参数值、所述相对旋转矩阵的参数值和所述第二刚体旋转矩阵的参数值，对所述第一相机的所述第二相机在当前时刻采集的特征点进行处理，得到所述目标铰接式车辆的位姿，包括：基于所述相对运动信息、所述本质矩阵的参数值、所述相对旋转矩阵的参数值和所述第二刚体旋转矩阵的参数值，对所述第一相机和所述第二相机在当前时刻采集的特征点进行处理，以构建多个重投影误差；对所述多个重投影误差进行集束优化，得到所述目标铰接式车辆的位姿。
14.在该方式中，在计算得到本质矩阵的参数值、相对旋转矩阵的参数值和第二刚体旋转矩阵的参数值后，进一步基于计算得到的参数值对第一相机和第二相机在当前时刻采集的特征点进行处理，以构建多个重投影误差，再通过对多个重投影误差进行集束优化的方式，获取目标铰接式车辆的位姿，因此，能够进一步对目标铰接式车辆的位姿进行优化。
15.第二方面，本发明提供了一种铰接式车辆的位姿估计装置，所述装置包括：运动信息获取模块，用于获取目标铰接式车辆的相对运动信息，所述相对运动信
息包括第一相机在当前时刻和上一时刻采集到的特征点射线、第二相机在当前时刻和上一时刻采集到的特征点射线、上一时刻所述目标铰接式车辆的第一刚体与第二刚体的刚体坐标系之间的第一刚体旋转矩阵，所述第一相机和所述第二相机分别位于所述第一刚体与所述第二刚体上；代数误差计算模块，用于基于所述相对运动信息，对所述第一刚体的广义本质矩阵约束和所述目标铰接式车辆的多相机对极几何约束进行处理，得到相应的第一最小化代数误差和第二最小化代数误差；代数误差转换模块，用于对所述第一刚体的本质矩阵、所述第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻所述第一刚体与所述第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以将所述第一最小化代数误差和所述第二最小化代数误差分别进行转换；运动参数求解模块，用于对转换后的所述第一最小化代数误差和所述第二最小化代数误差进行求解，得到所述本质矩阵、所述相对旋转矩阵和所述第二刚体旋转矩阵的参数值；车辆位姿估计模块，用于基于所述相对运动信息、所述本质矩阵的参数值、所述相对旋转矩阵的参数值和所述第二刚体旋转矩阵的参数值，对所述第一相机和所述第二相机在当前时刻采集的特征点进行处理，得到所述目标铰接式车辆的位姿。
16.第三方面，本发明提供了一种铰接式车辆，包括：车辆本体；控制器，设置在所述车辆本体内，所述控制器用于执行上述第一方面或其对应的任一实施方式的铰接式车辆的位姿估计方法。
17.第四方面，本发明提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机指令，计算机指令用于使计算机执行上述第一方面或其对应的任一实施方式的铰接式车辆的位姿估计方法。
附图说明
18.为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
19.图1是根据本发明实施例的第一种铰接式车辆的位姿估计方法的流程示意图；图2是根据本发明实施例的一种铰接式车辆的运动模型的示意图；图3是根据本发明实施例的第二种铰接式车辆的位姿估计方法的流程示意图；图4是根据本发明实施例的第三种铰接式车辆的位姿估计方法的流程示意图；图5是根据本发明实施例的第四种铰接式车辆的位姿估计方法的流程示意图；图6是根据本发明实施例的第五种铰接式车辆的位姿估计方法的流程示意图；图7是根据本发明实施例的一种铰接式车辆的位姿估计装置的结构框图；图8是根据本发明实施例的一种铰接式车辆的控制器的结构示意图。
具体实施方式
20.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
21.自动驾驶技术是指车辆在不需要人为操作的情况下，通过自身配备的传感器感知行驶环境，自主决策行驶路径，并控制车辆按照期望的路径到达期望目的地的技术。与传统驾驶相同，对车辆运行环境实时感知与定位是无人驾驶系统实施决策和控制的基础，相关的感知技术和车辆位姿估计方法也备受关注。为满足实时性与可靠性要求，无人驾驶车辆通常配备有视觉相机、惯性测量单元、激光雷达和轮速计等多种传感器。其中，相机能够感知的外界信息最为丰富，如：车辆周边景物的色彩、结构、纹理以及道路、行人、交通标识等语义信息，因此视觉相机是无人驾驶车辆通常的基本配置。但是，由于单个相机的视场角有限，因此无人驾驶系统越来越多的采用多个相机组成全景成像系统。
22.而铰接式车辆是由两个刚体通过一个节点连接在一起，因此铰接式车辆的姿态需要通过其中一个刚体姿态和两个刚体之间的连接状态来表征。另外，由于铰接式车辆通常车体较长，相机安装位置之间会有较大间隔且相机朝向不同，各相机之间没有或只有很小的视野重叠区，因此若简单地直接采用传统基于单个刚体的车辆位姿估计算法来估计铰接式车辆的两个刚体的位姿，位姿估计的结果准确度较差。因此，在相关技术中，通常采用适用于单个刚体的车辆位姿估计算法来分别估计铰接式车辆的两个刚体的位姿，然后算出两个刚体之间的连接状态，以确定铰接式车辆的位姿。但是，这种铰接式车辆的位姿估计方法容易忽略两个刚体的联系，存在传统算法的固有误差，导致铰接式车辆的位姿估计的准确度较差。
23.有鉴于此，根据本发明实施例，提供了一种铰接式车辆的位姿估计方法实施例，需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
24.在本实施例中提供了一种铰接式车辆的位姿估计方法，可用于上述的铰接式车辆或与铰接式车辆通信连接的云服务器，图1是根据本发明实施例的第一种铰接式车辆的位姿估计方法的流程图，如图1所示，该流程包括如下步骤：步骤s101，获取目标铰接式车辆的相对运动信息，相对运动信息包括第一相机在当前时刻和上一时刻采集到的特征点射线、第二相机在当前时刻和上一时刻采集到的特征点射线、上一时刻目标铰接式车辆的第一刚体与第二刚体的刚体坐标系之间的第一刚体旋转矩阵，第一相机和第二相机分别位于第一刚体与第二刚体上。
25.具体地，可通过计算机视觉领域中常用的图像处理方法，检测第一相机和第二相机采集到的相应拍摄图像中的某一类型2d特征点并对其追踪，包括但不限于sift、surf、orb或深度学习方法提取的superpoint等类型的特征点。并通过计算机视觉中常用的brute force matching、flann或基于深度学习方法的superglue的匹配方法获得相邻图像帧之间匹配的特征点。
26.进一步地，第一相机在当前时刻和上一时刻采集到的特征点射线由第一相机的第
一相机内参分别对第一相机在当前时刻和上一时刻采集到的匹配的特征点进行处理得到，第二相机在当前时刻和上一时刻采集到的特征点射线由第二相机的第二相机内参分别对第二相机在当前时刻和上一时刻采集到的匹配的特征点进行处理得到。
27.步骤s102，基于相对运动信息，对第一刚体的广义本质矩阵约束和目标铰接式车辆的多相机对极几何约束进行处理，得到相应的第一最小化代数误差和第二最小化代数误差。
28.具体地，参见图2所示的目标铰接式车辆的运动模型，图2中的第一刚体a和第二刚体b由可活动节点连接，第一刚体和第二刚体上分别设有多个第一相机和多个第二相机，构成目标铰接式车辆的多相机系统。其中，a、b作为参数下标时分别表示上一时刻第一刚体和第二刚体的状态，和分别表示当前时刻第一刚体a和第二刚体b的状态，则，第一刚体的广义本质矩阵约束根据以下公式表示：；其中，为第一相机在上一时刻采集到的特征点射线，为第一相机在当前时刻采集到的特征点射线，为上一时刻第一相机的相机坐标系到第一刚体的刚体坐标系的坐标位移，为第一刚体在当前时刻和上一时刻之间的相对位移，为第一刚体在当前时刻和上一时刻之间的相对旋转矩阵。
29.同理可知，第二刚体的广义本质矩阵约束根据以下公式表示：；其中，为第二相机在上一时刻采集到的特征点射线，为第二相机在当前时刻采集到的特征点射线，为上一时刻第二相机的相机坐标系到第二刚体的刚体坐标系的坐标位移，为第二刚体在当前时刻和上一时刻之间的相对位移，为第二刚体在当前时刻和上一时刻之间的相对旋转矩阵。
30.需要说明的是，由于第一相机与第一刚体的位置固定，因此，无论是上一时刻还是当前时刻，第一相机的相机坐标系到第一刚体的刚体坐标系的坐标位移均相同，同理，无论是上一时刻还是当前时刻，第二相机的相机坐标系到第二刚体的刚体坐标系的坐标位移也都相同。
31.进一步地，由上述第一刚体和第二刚体的广义本质矩阵约束可推理得到目标铰接式车辆的多相机对极几何约束。具体地，目标铰接式车辆的多相机对极几何约束根据以下公式表示：；其中，为上一时刻第一刚体与第二刚体的刚体坐标系之间的第一刚体旋转矩阵，为当前时刻第一刚体与第二刚体的刚体坐标系之间的第二刚体旋转矩阵。
32.需要说明的是，可由第一相机的第一相机内参得到，可由第二相机的第二相机内参得到，为第一刚体在当前时刻与上一时刻之间的相对运动，为第二刚体在当前时刻与上一时刻之间的相对运动。参见第一刚体的广义本质矩阵约束和多相机对极几何约束的具体公式可见，在已知上一时刻第一刚体与第二刚体的刚体坐标系之间的第一刚体旋转矩阵的情况下，可以利用解多项式来估计第一刚体a在当前时刻与上一时刻之间的相对运动和当前时刻第一刚体a与第二刚体b的刚体坐标系之间的第二刚体旋转矩阵。
33.步骤s103，对第一刚体的本质矩阵、第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻第一刚体与第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以将第一最小化代数误差和第二最小化代数误差分别进行转换。
34.需要说明的是，上述步骤s103的目的是将第一最小化代数误差和第二最小化代数误差转换为二次约束二次规划问题，从而得到一种非最小求解器。
35.步骤s104，对转换后的第一最小化代数误差和第二最小化代数误差进行求解，得到本质矩阵、相对旋转矩阵和第二刚体旋转矩阵的参数值。
36.步骤s105，基于相对运动信息、本质矩阵的参数值、相对旋转矩阵的参数值和第二刚体旋转矩阵的参数值，对第一相机和第二相机在当前时刻采集的特征点进行处理，得到目标铰接式车辆的位姿。
37.本实施例提供的铰接式车辆的位姿估计方法，通过目标铰接式车辆的相对运动信息，对第一刚体的广义本质矩阵约束和目标铰接式车辆的多相机对极几何约束进行处理，以得到两个约束的第一最小化代数误差和第二最小化代数误差，进而对第一最小化代数误差和第二最小化代数误差进行转换和求解，得到第一刚体的本质矩阵、第一刚体在相邻时刻之间的相对旋转矩阵和当前时刻第一刚体与第二刚体的第二刚体旋转矩阵的参数值，从而能够确定当前时刻第一刚体的运动状态以及第一刚体和第二刚体之间的联系，以对第一相机和第二相机在当前时刻采集的特征点进行处理，准确确定出目标铰接式车辆两个刚体的相对位姿，以提高铰接式车辆位姿估计的准确性。
38.图3是根据本发明实施例的第二种铰接式车辆的位姿估计方法的流程图，如图3所示，上述步骤s102包括：步骤s1021，基于相对运动信息，将第一刚体的广义本质矩阵约束和目标铰接式车辆的多相机对极几何约束分别进行二次型转换。
39.具体地，相对运动信息还包括：上一时刻第一相机的相机坐标系到第一刚体的刚体坐标系的坐标位移、上一时刻第一相机的相机坐标系到第一刚体的刚体坐标系的坐标位移，令第一刚体的本质矩阵，则，转换后的第一刚体的广义本质矩阵约束根据以下公式表示：；其中，为的转置，为第一参数，为第二参数，为的转置。
40.转换后的目标铰接式车辆的多相机对极几何约束根据以下公式表示：；其中，为第三参数，为第四参数，为第五参数，为的转置。
41.步骤s1022，基于转换后的广义本质矩阵约束，计算第一最小化代数误差，转换后的广义本质矩阵约束以本质矩阵和相对旋转矩阵为未知变量。
42.具体地，第一最小化代数误差根据以下公式表示：；其中，为第一最小化代数误差，为第i个第一代数误差，为第一相机从采集到的图像中提取的特征点对的数量，即，第一相机在当前时刻和上一时刻采集到的特征点射线的数量。
43.步骤s1023，基于转换后的多相机对极几何约束，计算第二最小化代数误差，转换后的多相机对极几何约束以本质矩阵、相对旋转矩阵和第二刚体旋转矩阵为未知变量。
44.具体地，第二最小化代数误差根据以下公式表示：；其中，为第二最小化代数误差，为第j个第二代数误差，为第二相机采集到的图像中提取的特征点对的数量，即，第二相机在当前时刻和上一时刻采集到的特征点射线的数量。
45.本实施例提供的铰接式车辆的位姿估计方法，基于相对运动信息，将第一刚体的广义本质矩阵约束和目标铰接式车辆的多相机对极几何约束分别进行二次型转换，再基于转换后的广义本质矩阵约束和多相机对极几何约束分别计算第一最小化代数误差和第二最小化代数误差，因此，能够简化基于第一刚体的广义本质矩阵约束和目标铰接式车辆的多相机对极几何约束，确定第一刚体本质矩阵、相对旋转矩阵和第二刚体旋转矩阵的参数值的计算过程，同时能够使所计算的参数值更接近真实值，以提高后续所估计的目标铰接式车辆的位姿的准确性。
46.需要说明的是，由上述可知，第一刚体的广义本质矩阵约束和多相机对极几何约
束的总最小化代数误差可表示为：；其中，为总最小化代数误差。
47.图4是根据本发明实施例的第三种铰接式车辆的位姿估计方法的流程图，如图4所示，上述步骤s103包括：步骤s1031，对第一刚体的本质矩阵、第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻第一刚体与第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，得到相应的第一向量、第二向量和第三向量。
48.具体地，第一向量根据以下公式表示：；其中，e为第一向量，vec表示向量化，为第一刚体的本质矩阵中第1行第1列的元素，为第一刚体的本质矩阵中第2行第1列的元素，为第一刚体的本质矩阵中第3行第3列的元素，t表示转置。
49.需要说明的是，第一刚体的本质矩阵为3
×
3的矩阵，则第一向量e为9维向量。
50.具体地，第二向量根据以下公式表示：；其中，r为第二向量，为相对旋转矩阵中第1行第1列的元素，为相对旋转矩阵中第2行第1列的元素，为相对旋转矩阵中第3行第3列的元素。
51.具体地，第三向量根据以下公式表示：；其中，q为第三向量，为第二刚体旋转矩阵中第1行第1列的元素，为第二刚体旋转矩阵中第2行第1列的元素，为第二刚体旋转矩阵中第3行第3列的元素。
52.步骤s1032，基于第一向量和第二向量，对第一最小化代数误差进行转换。
53.具体地，转换后的第一最小化代数误差基于以下公式表示：；其中，为转换后的第一最小化代数误差，e为第一向量，r为第二向量，为第一半正定对称矩阵，为第一向量的转置，为第二向量的转置，为本质矩阵，为第一刚体在当前时刻和上一时刻之间的相对位移，为相对旋转矩阵，so(3)为三维旋转群。
54.需要说明的是，st.用于表征约束条件。
55.进一步地，第一半正定对称矩阵根据以下公式表示：；其中，表示克罗内克积，为第一半正定对称矩阵的第一元素，为第一半正定对称矩阵的第二元素，为第一半正定对称矩阵的第三元素，为第一半正定对称矩阵的第四元素，为第一半正定对称矩阵的第五元素，为第一半正定对称矩阵的第六元素。
56.步骤s1033，基于第三向量，对第二最小化代数误差进行转换。
57.具体地，转换后的第二最小化代数误差基于以下公式表示：；其中，为转换后的第二最小化代数误差，为第二刚体旋转矩阵，为第二刚体旋转矩阵的转置，q为第三向量，为第三向量的转置，为第二半正定对称矩阵，i为单位矩阵，so(3)为三维旋转群。
58.进一步地，第二半正定对称矩阵根据以下公式表示：；其中，为第二半正定对称矩阵的第一元素，为第二半正定对称矩阵的第二元素，为第二半正定对称矩阵的第三元素。
59.本实施例提供的铰接式车辆的位姿估计方法，通过对第一刚体的本质矩阵、第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻第一刚体与第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以基于向量化得到的第一向量、第二向量
和第三向量对第一最小化代数误差和第二最小化代数误差进行转换，因此，能够将原有的目标铰接式车辆的相对位姿估计问题转换为二次约束二次规划问题，从而能够在后续过程中求解得到本质矩阵、相对旋转矩阵和第二刚体旋转矩阵的全局最优解，以进一步提高目标铰接式车辆位姿估计的准确性。
60.图5是根据本发明实施例的第四种铰接式车辆的位姿估计方法的流程图，如图5所示，上述步骤s104包括：步骤s1041，对转换后的第一最小化代数误差进行求解，得到本质矩阵和相对旋转矩阵的参数值。
61.步骤s1042，基于本质矩阵的参数值和相对旋转矩阵的参数值，对转换后的第二最小化代数误差进行求解，得到第二刚体旋转矩阵的参数值。
62.需要说明的是，转换后的第一最小化代数误差和转换后的第二最小化代数误差为二次约束二次规划问题。因此，可采用拉格朗日乘子法对转换后的第一最小化代数误差和第二最小化代数误差进行求解，得到本质矩阵、相对旋转矩阵和第二刚体旋转矩阵的参数值。
63.图6是根据本发明实施例的第五种铰接式车辆的位姿估计方法的流程图，如图6所示，上述步骤s105包括：步骤s1051，基于相对运动信息、本质矩阵的参数值、相对旋转矩阵的参数值和第二刚体旋转矩阵的参数值，对第一相机和第二相机在当前时刻采集的特征点进行处理，以构建多个重投影误差。
64.具体地，多个重投影误差包括第一重投影误差、第二重投影误差、第三重投影误差和第四重投影误差，具体根据以下公式构建多个重投影误差：；其中，为第一重投影误差，为第二重投影误差，为第三重投影误差，为第四重投影误差，为第一相机在当前时刻采集的特征点在第一刚体的刚体坐标系下的坐标，为第二相机在当前时刻采集的特征点在第二刚体的刚体坐标系下的坐标，为的转置，为q的转置。
65.步骤s1052，对多个重投影误差进行集束优化，得到目标铰接式车辆的位姿。
66.具体地，根据以下公式对多个重投影误差进行集束优化：。
67.本实施例提供的铰接式车辆的位姿估计方法，在计算得到本质矩阵的参数值、相对旋转矩阵的参数值和第二刚体旋转矩阵的参数值后，进一步基于计算得到的参数值对第一相机和第二相机在当前时刻采集的特征点进行处理，以构建多个重投影误差，再通过对多个重投影误差进行集束优化的方式，获取目标铰接式车辆的位姿，因此，能够进一步对目标铰接式车辆的位姿进行优化。
68.值得说明的是，不同于相关技术通过适用于单个刚体的位姿估计算法分别计算目标铰接式车辆的第一刚体和第二刚体的位姿，在本发明中，通过最小化残差的平方和（即上述第一最小化代数误差和第二最小化代数误差），从而提出一种非最小求解器（即上述转换后的第一最小化代数误差和第二最小化代数误差），以将目标铰接式车辆的相对位姿估计问题转换为二次约束二次规划问题，进而通过半正定松弛法得到全局最优解，提高了位姿估计算法的精确度，且本发明提出的求解器考虑到所有相机采集的特征点，因而有更多约束，从而提高算法的鲁棒性。此外，本发明与基于平面运动假设提出的最小求解器而言，不需要限制为平面运动，从而可以应用在更具有一般性的3d空间场景。
69.在本实施例中还提供了一种铰接式车辆的位姿估计装置，该装置用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。
70.本实施例提供一种铰接式车辆的位姿估计装置，如图7所示，包括：运动信息获取模块201，用于获取目标铰接式车辆的相对运动信息，相对运动信息包括第一相机在当前时刻和上一时刻采集到的特征点射线、第二相机在当前时刻和上一时刻采集到的特征点射线、上一时刻目标铰接式车辆的第一刚体与第二刚体的刚体坐标系之间的第一刚体旋转矩阵，第一相机和第二相机分别位于第一刚体与第二刚体上；代数误差计算模块202，用于基于相对运动信息，对第一刚体的广义本质矩阵约束和目标铰接式车辆的多相机对极几何约束进行处理，得到相应的第一最小化代数误差和第二最小化代数误差；代数误差转换模块203，用于对第一刚体的本质矩阵、第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻第一刚体与第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以将第一最小化代数误差和第二最小化代数误差分别进行转换；运动参数求解模块204，用于对转换后的第一最小化代数误差和第二最小化代数误差进行求解，得到本质矩阵、相对旋转矩阵和第二刚体旋转矩阵的参数值；车辆位姿估计模块205，用于基于相对运动信息、本质矩阵的参数值、相对旋转矩阵的参数值和第二刚体旋转矩阵的参数值，对第一相机和第二相机在当前时刻采集的特征点进行处理，得到目标铰接式车辆的位姿。
71.作为其中一个可选的实施方式，代数误差计算模块202包括：二次型转换单元，用于基于相对运动信息，将第一刚体的广义本质矩阵约束和目标铰接式车辆的多相机对极几何约束分别进行二次型转换；第一误差计算单元，用于基于转换后的广义本质矩阵约束，计算第一最小化代数误差，转换后的广义本质矩阵约束以本质矩阵和相对旋转矩阵为未知变量；第二误差计算单元，用于基于转换后的多相机对极几何约束，计算第二最小化代
数误差，转换后的多相机对极几何约束以本质矩阵、相对旋转矩阵和第二刚体旋转矩阵为未知变量。
72.作为其中一个可选的实施方式，代数误差转换模块203包括：矩阵向量化单元，用于对第一刚体的本质矩阵、第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻第一刚体与第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，得到相应的第一向量、第二向量和第三向量；第一误差转换单元，用于基于第一向量和第二向量，对第一最小化代数误差进行转换；第二误差转换单元，用于基于第三向量，对第二最小化代数误差进行转换。
73.具体地，第一误差转换单元中转换后的第一最小化代数误差基于以下公式表示：；其中，为转换后的第一最小化代数误差，e为第一向量，r为第二向量，为第一半正定对称矩阵，为第一向量的转置，为第二向量的转置，为本质矩阵，为第一刚体在当前时刻和上一时刻之间的相对位移，为相对旋转矩阵，so(3)为三维旋转群。
74.具体地，第二误差转换单元中转换后的第二最小化代数误差基于以下公式表示：；其中，为转换后的第二最小化代数误差，为第二刚体旋转矩阵，为第二刚体旋转矩阵的转置，q为第三向量，为第三向量的转置，为第二半正定对称矩阵，i为单位矩阵，so(3)为三维旋转群。
75.作为其中一个可选的实施方式，运动参数求解模块204包括：第一参数求解单元，用于对转换后的第一最小化代数误差进行求解，得到本质矩阵和相对旋转矩阵的参数值；第二参数求解单元，用于基于本质矩阵的参数值和相对旋转矩阵的参数值，对转换后的第二最小化代数误差进行求解，得到第二刚体旋转矩阵的参数值。
76.作为其中一个可选的实施方式，车辆位姿估计模块205包括：误差构建单元，用于基于相对运动信息、本质矩阵的参数值、相对旋转矩阵的参数值和第二刚体旋转矩阵的参数值，对第一相机和第二相机在当前时刻采集的特征点进行处理，以构建多个重投影误差；集束优化单元，用于对多个重投影误差进行集束优化，得到目标铰接式车辆的位姿。
77.上述各个模块和单元的更进一步的功能描述与上述对应实施例相同，在此不再赘
述。
78.本实施例中的铰接式车辆的位姿估计装置是以功能单元的形式来呈现，这里的单元是指asic（application specific integrated circuit，专用集成电路）电路，执行一个或多个软件或固定程序的处理器和存储器，和/或其他可以提供上述功能的器件。
79.本发明实施例还提供一种铰接式车辆，铰接式车辆包括：车辆本体；控制器，设置在车辆本体内，控制器用于执行上述任一实施方式的铰接式车辆的位姿估计方法。
80.具体地，参阅图8，图8是本发明可选实施例提供的一种铰接式车辆的控制器的结构示意图，如图8所示，控制器包括：一个或多个处理器10、存储器20，以及用于连接各部件的接口，包括高速接口和低速接口。各个部件利用不同的总线互相通信连接，并且可以被安装在公共主板上或者根据需要以其它方式安装。处理器可以对在控制器内执行的指令进行处理，包括存储在存储器中或者存储器上以在外部输入/输出装置（诸如，耦合至接口的显示设备）上显示gui的图形信息的指令。在一些可选的实施方式中，若需要，可以将多个处理器和/或多条总线与多个存储器和多个存储器一起使用。同样，可以连接多个铰接式车辆，各个设备提供部分必要的操作（例如，作为服务器阵列、一组刀片式服务器、或者多处理器系统）。图8中以一个处理器10为例。
81.处理器10可以是中央处理器，网络处理器或其组合。其中，处理器10还可以进一步包括硬件芯片。上述硬件芯片可以是专用集成电路，可编程逻辑器件或其组合。上述可编程逻辑器件可以是复杂可编程逻辑器件，现场可编程逻辑门阵列，通用阵列逻辑或其任意组合。
82.其中，存储器20存储有可由至少一个处理器10执行的指令，以使至少一个处理器10执行实现上述实施例示出的方法。
83.存储器20可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据铰接式车辆的使用所创建的数据等。此外，存储器20可以包括高速随机存取存储器，还可以包括非瞬时存储器，例如至少一个磁盘存储器件、闪存器件、或其他非瞬时固态存储器件。在一些可选的实施方式中，存储器20可选包括相对于处理器10远程设置的存储器，这些远程存储器可以通过网络连接至该铰接式车辆。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
84.存储器20可以包括易失性存储器，例如，随机存取存储器；存储器也可以包括非易失性存储器，例如，快闪存储器，硬盘或固态硬盘；存储器20还可以包括上述种类的存储器的组合。
85.该控制器还包括输入装置30和输出装置40。处理器10、存储器20、输入装置30和输出装置40可以通过总线或者其他方式连接，图8中以通过总线连接为例。
86.输入装置30可接收输入的数字或字符信息，以及产生与该铰接式车辆的用户设置以及功能控制有关的键信号输入，例如触摸屏、小键盘、鼠标、轨迹板、触摸板、指示杆、一个或者多个鼠标按钮、轨迹球、操纵杆等。输出装置40可以包括显示设备、辅助照明装置（例如，led）和触觉反馈装置（例如，振动电机）等。上述显示设备包括但不限于液晶显示器，发
光二极管，显示器和等离子体显示器。在一些可选的实施方式中，显示设备可以是触摸屏。
87.本发明实施例还提供了一种计算机可读存储介质，上述根据本发明实施例的方法可在硬件、固件中实现，或者被实现为可记录在存储介质，或者被实现通过网络下载的原始存储在远程存储介质或非暂时机器可读存储介质中并将被存储在本地存储介质中的计算机代码，从而在此描述的方法可被存储在使用通用计算机、专用处理器或者可编程或专用硬件的存储介质上的这样的软件处理。其中，存储介质可为磁碟、光盘、只读存储记忆体、随机存储记忆体、快闪存储器、硬盘或固态硬盘等；进一步地，存储介质还可以包括上述种类的存储器的组合。可以理解，计算机、处理器、微处理器控制器或可编程硬件包括可存储或接收软件或计算机代码的存储组件，当软件或计算机代码被计算机、处理器或硬件访问且执行时，实现上述实施例示出的方法。
88.虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下做出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。

技术特征：
1.一种铰接式车辆的位姿估计方法，其特征在于，所述方法包括：获取目标铰接式车辆的相对运动信息，所述相对运动信息包括第一相机在当前时刻和上一时刻采集到的特征点射线、第二相机在当前时刻和上一时刻采集到的特征点射线、上一时刻所述目标铰接式车辆的第一刚体与第二刚体的刚体坐标系之间的第一刚体旋转矩阵，所述第一相机和所述第二相机分别位于所述第一刚体与所述第二刚体上；基于所述相对运动信息，对所述第一刚体的广义本质矩阵约束和所述目标铰接式车辆的多相机对极几何约束进行处理，得到相应的第一最小化代数误差和第二最小化代数误差；对所述第一刚体的本质矩阵、所述第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻所述第一刚体与所述第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以将所述第一最小化代数误差和所述第二最小化代数误差分别进行转换；对转换后的所述第一最小化代数误差和所述第二最小化代数误差进行求解，得到所述本质矩阵、所述相对旋转矩阵和所述第二刚体旋转矩阵的参数值；基于所述相对运动信息、所述本质矩阵的参数值、所述相对旋转矩阵的参数值和所述第二刚体旋转矩阵的参数值，对所述第一相机和所述第二相机在当前时刻采集的特征点进行处理，得到所述目标铰接式车辆的位姿。2.根据权利要求1所述的方法，其特征在于，所述基于所述相对运动信息，对所述第一刚体的广义本质矩阵约束和所述目标铰接式车辆的多相机对极几何约束进行处理，得到相应的第一最小化代数误差和第二最小化代数误差，包括：基于所述相对运动信息，将所述第一刚体的广义本质矩阵约束和所述目标铰接式车辆的多相机对极几何约束分别进行二次型转换；基于转换后的所述广义本质矩阵约束，计算第一最小化代数误差，转换后的所述广义本质矩阵约束以所述本质矩阵和所述相对旋转矩阵为未知变量；基于转换后的所述多相机对极几何约束，计算第二最小化代数误差，转换后的所述多相机对极几何约束以所述本质矩阵、所述相对旋转矩阵和所述第二刚体旋转矩阵为未知变量。3.根据权利要求1所述的方法，其特征在于，所述对所述第一刚体的本质矩阵、所述第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻所述第一刚体与所述第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以将所述第一最小化代数误差和所述第二最小化代数误差分别进行转换，包括：对所述第一刚体的本质矩阵、所述第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻所述第一刚体与所述第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，得到相应的第一向量、第二向量和第三向量；基于所述第一向量和所述第二向量，对所述第一最小化代数误差进行转换；基于所述第三向量，对所述第二最小化代数误差进行转换。4.根据权利要求3所述的方法，其特征在于，转换后的所述第一最小化代数误差基于以下公式表示：
；其中，为所述转换后的第一最小化代数误差，e为所述第一向量，r为所述第二向量，为第一半正定对称矩阵，为所述第一向量的转置，为所述第二向量的转置，为所述本质矩阵，为所述第一刚体在当前时刻和上一时刻之间的相对位移，为所述相对旋转矩阵，so(3)为三维旋转群。5.根据权利要求3所述的方法，其特征在于，转换后的所述第二最小化代数误差基于以下公式表示：；其中，为所述转换后的第二最小化代数误差，为所述第二刚体旋转矩阵，为所述第二刚体旋转矩阵的转置，q为所述第三向量，为所述第三向量的转置，为第二半正定对称矩阵，i为单位矩阵，so(3)为三维旋转群。6.根据权利要求3所述的方法，其特征在于，所述对转换后的所述第一最小化代数误差和所述第二最小化代数误差进行求解，得到所述本质矩阵、所述相对旋转矩阵和所述第二刚体旋转矩阵的参数值，包括：对转换后的所述第一最小化代数误差进行求解，得到所述本质矩阵和所述相对旋转矩阵的参数值；基于所述本质矩阵的参数值和所述相对旋转矩阵的参数值，对转换后的所述第二最小化代数误差进行求解，得到所述第二刚体旋转矩阵的参数值。7.根据权利要求1所述的方法，其特征在于，所述基于所述相对运动信息、所述本质矩阵的参数值、所述相对旋转矩阵的参数值和所述第二刚体旋转矩阵的参数值，对所述第一相机的所述第二相机在当前时刻采集的特征点进行处理，得到所述目标铰接式车辆的位姿，包括：基于所述相对运动信息、所述本质矩阵的参数值、所述相对旋转矩阵的参数值和所述第二刚体旋转矩阵的参数值，对所述第一相机和所述第二相机在当前时刻采集的特征点进行处理，以构建多个重投影误差；对所述多个重投影误差进行集束优化，得到所述目标铰接式车辆的位姿。8.一种铰接式车辆的位姿估计装置，其特征在于，所述装置包括：运动信息获取模块，用于获取目标铰接式车辆的相对运动信息，所述相对运动信息包括第一相机在当前时刻和上一时刻采集到的特征点射线、第二相机在当前时刻和上一时刻采集到的特征点射线、上一时刻所述目标铰接式车辆的第一刚体与第二刚体的刚体坐标系之间的第一刚体旋转矩阵，所述第一相机和所述第二相机分别位于所述第一刚体与所述第
二刚体上；代数误差计算模块，用于基于所述相对运动信息，对所述第一刚体的广义本质矩阵约束和所述目标铰接式车辆的多相机对极几何约束进行处理，得到相应的第一最小化代数误差和第二最小化代数误差；代数误差转换模块，用于对所述第一刚体的本质矩阵、所述第一刚体在当前时刻和上一时刻之间的相对旋转矩阵、以及当前时刻所述第一刚体与所述第二刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以将所述第一最小化代数误差和所述第二最小化代数误差分别进行转换；运动参数求解模块，用于对转换后的所述第一最小化代数误差和所述第二最小化代数误差进行求解，得到所述本质矩阵、所述相对旋转矩阵和所述第二刚体旋转矩阵的参数值；车辆位姿估计模块，用于基于所述相对运动信息、所述本质矩阵的参数值、所述相对旋转矩阵的参数值和所述第二刚体旋转矩阵的参数值，对所述第一相机和所述第二相机在当前时刻采集的特征点进行处理，得到所述目标铰接式车辆的位姿。9.一种铰接式车辆，其特征在于，包括：车辆本体；控制器，设置在所述车辆本体内，所述控制器用于执行权利要求1至7中任一项所述的铰接式车辆的位姿估计方法。10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机指令，所述计算机指令用于使计算机执行权利要求1至7中任一项所述的铰接式车辆的位姿估计方法。

技术总结
本发明公开了一种铰接式车辆的位姿估计方法、装置、车辆及存储介质，包括：获取目标铰接式车辆的相对运动信息；基于相对运动信息对广义本质矩阵约束和多相机对极几何约束进行处理，得到第一、第二最小化代数误差；对第一刚体的本质矩阵、第一刚体在相邻时刻之间的相对旋转矩阵和当前时刻两个刚体的刚体坐标系之间的第二刚体旋转矩阵进行向量化，以将第一、第二最小化代数误差分别进行转换，从而求解本质矩阵、相对旋转矩阵和第二刚体旋转矩阵的参数值；基于相对运动信息、本质矩阵、相对旋转矩阵和第二刚体旋转矩阵的参数值，对相机在当前时刻采集的特征点进行处理，得到车辆位姿。本发明能够解决铰接式车辆的位姿估计的准确度较差的问题。较差的问题。较差的问题。


技术研发人员：彭欣 张如高 虞正华
受保护的技术使用者：深圳魔视智能科技有限公司
技术研发日：2023.08.29
技术公布日：2023/10/5