CN115200591B - 一种位姿确定方法、装置、整车控制器及可读存储介质 - Google Patents

Abstract

本发明实施例提供了一种位姿确定方法、装置、整车控制器及可读存储介质，涉及车辆定位技术领域，包括：获取包含多个变量节点的第一因子图；不同变量节点的值用于指示不同时刻的车辆位姿；获取车辆观测位姿，根据车辆观测位姿和第一因子图，确定车辆观测位姿对应的局部因子图；根据局部因子图对关联变量节点进行优化，得到关联变量节点对应时刻的车辆预测位姿。本发明实施例中，可以获取车辆观测位姿，根据因子图快速确定出该车辆观测位姿对应的关联变量节点的局部因子图，并根据局部因子图对关联变量节点进行优化，得到车辆预测位姿，使得车辆的定位结果更加稳定和精确，计算过程更加高效，为车辆实现更加高级的自动驾驶提供了可靠的定位基础。

Description

一种位姿确定方法、装置、整车控制器及可读存储介质

技术领域

本发明涉及车辆定位技术领域，特别涉及一种位姿确定方法、装置、整车控制器及可读存储介质。

背景技术

随着科技的进步，越来越多的车辆开始配备自动驾驶和辅助驾驶功能，在车辆的自动驾驶和辅助驾驶过程中，需要实时确定车辆的位姿信息。

相关技术中，为了使车辆在道路上正确行使，需要实时确定车辆的准确的位姿信息。对于某一时刻的车辆位姿，通常会获取多种不同传感器的位姿数据，在获取到这些位姿数据后，通过预设的算法对这些位姿数据进行融合计算，从而确定出车辆位姿。

但在上述方法中，在获取到所有传感器数据后才能进行某一时刻的位姿运算，然而实际应用中传感器获取数据可能精度较低，也可能存在异常，容易导致确定车辆位姿的稳定性和准确性不佳。

发明内容

有鉴于此，本发明旨在提出一种位姿确定方法、装置、整车控制器及可读存储介质，以解决现有技术中车辆位姿确定过程中稳定性和精度较差的问题。

为达到上述目的，本发明的技术方案是这样实现的：

一种位姿确定方法，所述装置包括：

获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿；

获取车辆观测位姿，根据所述车辆观测位姿和所述第一因子图，确定所述车辆观测位姿对应的局部因子图；其中，所述局部因子图包含所述车辆观测位姿的时间参数对应的关联变量节点，以及由所述车辆观测位姿确定的因子节点；

根据所述局部因子图对所述关联变量节点进行优化，得到所述关联变量节点对应时刻的车辆预测位姿。

一种位姿确定装置，所述装置包括：

获取模块，用于获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿；

确定模块，用于获取车辆观测位姿，根据所述车辆观测位姿和所述第一因子图，确定所述车辆观测位姿对应的局部因子图；其中，所述局部因子图包含所述车辆观测位姿的时间参数对应的关联变量节点，以及由所述车辆观测位姿确定的因子节点；

优化模块，用于根据所述局部因子图对所述关联变量节点进行优化，得到所述关联变量节点对应时刻的车辆预测位姿。

一种整车控制器，所述整车控制器包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述位姿确定方法。

一种可读存储介质，所述可读存储介质上存储程序或指令，所述程序或指令被处理器执行时实现上述位姿确定方法。

一种车辆，包括上述整车控制器。

相对于现有技术，本发明所述的一种位姿确定方法、装置、整车控制器及可读存储介质具有以下优势：

综上，本发明实施例提供了一种位姿确定方法，包括：获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿；获取车辆观测位姿，根据车辆观测位姿和第一因子图，确定车辆观测位姿对应的局部因子图；其中，局部因子图包含车辆观测位姿的时间参数对应的关联变量节点，以及由车辆观测位姿确定的因子节点；根据局部因子图对关联变量节点进行优化，得到关联变量节点对应时刻的车辆预测位姿。本发明实施例中，可以获取车辆观测位姿，根据因子图快速确定出该车辆观测位姿对应的关联变量节点的局部因子图，并根据局部因子图对关联变量节点进行优化，得到车辆预测位姿，使得车辆的定位结果更加稳定和精确，计算过程更加高效，为车辆实现更加高级的自动驾驶提供了可靠的定位基础。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明实施例提供的一种位姿确定方法的步骤流程图；

图2为本发明实施例提供的一种第一因子图示意图；

图3为本发明实施例提供的一种局部因子图示意图；

图4为本发明实施例提供的另一种位姿确定方法的步骤流程图；

图5为本发明实施例提供的一种关联变量节点示意图；

图6为本发明实施例提供的另一种局部因子图示意图；

图7为本发明实施例所述的再一种位姿确定方法的步骤流程图；

图8为本发明实施例提供的另一种第一因子图示意图；

图9为本发明实施例提供的一种团树示意图；

图10为本发明实施例提供的一种贝叶斯网络示意图；

图11为本发明实施例提供的一种位姿确定装置的结构框图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面将参考附图并结合实施例来详细说明本发明。

参照图1，图1为本发明实施例提供的一种位姿确定方法的步骤流程图。

步骤101，获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿。

因子图（Factor Graph）是概率图的一种，是一种对函数因子分解的表示图，在因子图中，一般内含两种节点：变量节点和因子节点，以上两种节点之间通过边线连接，表示他们之间的函数关系。

车辆位姿可以包括车辆运动状态、运行姿态和所处位置的信息，例如，车辆在的定位坐标，车辆的角速度、行驶速度等速度信息，以及车辆的倾角、行驶方向等姿态信息。

第一因子图中的每个变量节点分别对应于一个时刻的车辆预测位姿，需要说明的是，该车辆预测位姿是根据各个传感器直接获取的车辆观测位姿经过整合、计算后得到的融合位姿。当一个变量节点新加入因子图中时，可以赋予其一个初始车辆预测位姿，并在后续获取到传感器数据后对该变量节点进行持续更新和优化。

参照图2，图2为本发明实施例提供的一种第一因子图示意图，如图2所示，X1、X2和X3是因子图的三个变量节点，其中，变量节点X1对应于时刻T1，变量节点X2对应于时刻T2，变量节点X3对应于时刻T3。D1是变量节点X1的因子节点，D2是变量节点X2的因子节点，D3是变量节点X3的因子节点。D1、D2和D3可以由与之连接的变量节点对应时刻的车辆位置信息生成，例如D1、D2和D3可以是车辆直接从卫星定位模块获取到的定位信息。

具体的，在车辆启动或每次行程开始时，可以开始构建第一因子图，并以预设时间间隔向第一因子图中增加变量节点，例如，预设时间间隔为1S，则每秒在第一因子图中增加一个与上一变量节点具有连接关系的新变量节点，这样，随着车辆的不断行驶与时间的推移，第一因子图中的变量节点数量会越来越多。

由于车辆的传感器信息获取通常存在延迟，因此，在生成某一时刻对应的变量节点时，在第一因子图中并不会同步生成该变量节点对应的因子节点，而是在一段时间后，才能获取到该时刻的传感器信息，并根据该传感器信息将因子节点加入第一因子图中。

举例来说，如果需要确定车辆在T2时刻的车辆定位坐标，则在第一因子图中增加对应于T2时刻的变量节点X2，同时通过卫星定位模块获取车辆在T2时刻的卫星定位信息，获取到车辆在T2时刻的卫星定位信息后，根据该卫星定位信息在第一因子图中增加一个与该变量节点X2相连接的因子节点。这样，便可以在后续过程中根据该第一因子图对变量节点X2进行优化。

需要说明的是，由于车辆位姿包含多种不同的信息类型，对于每一种信息类型，可以建立对应的因子图。例如，车辆位姿可以包含车辆定位坐标，则可以建立用于预测车辆定位坐标的因子图，其中的变量节点表征车辆的预测定位坐标，其中的因子节点表征车辆的观测坐标，其中观测坐标是通过传感器数据直接获取到的车辆定位坐标，或通过传感器数据与历史车辆位置经过函数计算得到的车辆定位坐标。为方便讨论，本发明实施例中主要以预测车辆定位坐标的因子图作为示例进行说明。

步骤102，获取车辆观测位姿，根据所述车辆观测位姿和所述第一因子图，确定所述车辆观测位姿对应的局部因子图；其中，所述局部因子图包含所述车辆观测位姿的时间参数对应的关联变量节点，以及由所述车辆观测位姿确定的因子节点。

在本发明实施例中，获取到车辆观测位姿后，可以根据该车辆观测位姿作为因子节点加入第一因子图中，并连接对应的变量节点，通过这些第一因子图中的因子节点，便可以优化变量节点的数值，进而得到更为准确的车辆预测位姿。

具体的，在车辆进行融合定位的过程中，每个时刻的位置都会通过多种信息进行综合计算确定，例如，对于T2时刻车辆的位置，可以根据T2时刻的卫星定位信息、环境影像定位信息、T1时刻的车辆预测位姿、T1时刻与T2时刻之间的车辆行驶路程等多帧定位信息进行综合计算得到。因此，当第一因子图中加入新的因子节点后，该新加入的因子节点可能会对多个变量节点的准确性提供贡献，需要根据该新加入的因子节点以及，受到该新加入的因子节点影响的变量节点相连的因子节点，对受到该新加入的因子节点影响的变量节点进行优化，在这个过程中，需要对第一因子图中变量节点和因子节点的连接关系进行分析，从而确定出需要优化的变量节点以及需要参与优化过程的因子节点。

因此，在接收到车辆观测位姿的情况下，需要确定该车辆观测位姿作为因子节点在第一因子图中连接的变量节点，并将受到该因子节点影响的关联变量节点以及这些关联变量节点连接的因子节点从第一因子图中提取出来，得到局部因子图，从而在后续过程中根据局部因子图对这些关联变量节点进行优化。

参照图3，图3示出了本发明实施例提供的一种局部因子图示意图，如图3所示，如果在图2示出了第一因子图中加入连接在变量节点X2和变量节点X3之间的因子节点C2（可以理解为变量节点X2与变量节点X3对应时刻之间车辆运动的路程），变量节点X3受到变量节点X2与因子节点C2的和值的影响，则可以将变量节点X2和变量节点X3，以及与变量节点X2和变量节点X3相连的所有因子节点提取出来，生成如图3所示的局部因子图，在图3中，变量节点X2和变量节点X3构成因子节点C2连接的所有变量节点，因子节点D2、因子节点D3和因子节点C2构成连接变量节点X3和变量节点X3的所有因子节点。

步骤103，根据所述局部因子图对所述关联变量节点进行优化，得到所述关联变量节点对应时刻的车辆预测位姿。

通常来说，如果车辆仅仅依靠单一的传感器数据确定车辆的位姿，会导致车辆定位精度较低且定位可靠性较差。因此，在本发明实施例中，车辆可以通过多种传感器数据共同确定车辆的位姿，即通过多种传感器数据对车辆在某一时刻的位姿进行优化。

在本发明实施例中，局部因子图中的关联变量节点是待优化的车辆预测位姿，即车辆真实的位姿，可以理解为未知量，而各个关联变量节点连接的因子节点是通过传感器获取的车辆观测位姿，可以理解为已知量。当获取到车辆观测位姿后，相当于获取到了已知量，可以根据未知量与已知量的偏移程度对未知量进行修正。

在得到局部因子图后，可以根据每个因子节点计算出与该因子节点相连的关联变量节点的观测值，再通过这些观测值和与之相连的关联变量节点之间的偏差，对关联变量节点的值进行优化，具体可以采用L-M算法或梯度下降法，使关联变量节点的值与观测值之间的偏差区域最小，从而得到优化后的车辆预测位姿。

综上，本发明实施例提供了一种位姿确定方法，包括：获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿；获取车辆观测位姿，根据车辆观测位姿和第一因子图，确定车辆观测位姿对应的局部因子图；其中，局部因子图包含车辆观测位姿的时间参数对应的关联变量节点，以及由车辆观测位姿确定的因子节点；根据局部因子图对关联变量节点进行优化，得到关联变量节点对应时刻的车辆预测位姿。本发明实施例中，可以获取车辆观测位姿，根据因子图快速确定出该车辆观测位姿对应的关联变量节点的局部因子图，并根据局部因子图对关联变量节点进行优化，得到车辆预测位姿，使得车辆的定位结果更加稳定和精确，计算过程更加高效，为车辆实现更加高级的自动驾驶提供了可靠的定位基础。

参照图4，为本发明实施例提供的另一种位姿确定方法的步骤流程图。

步骤201，获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿。

此步骤可参见步骤101，本发明实施例不再赘述。

步骤202，根据所述车辆观测位姿的时间参数，将所述车辆观测位姿作为所述变量节点的因子节点添加至所述第一因子图中，得到第二因子图。

在获取到车辆观测位姿时，可以同时获取该车辆观测位姿对应的时间参数，并根据车辆观测位姿的时间参数，将车辆观测位姿作为因子节点添加至第一因子图中，并建立与该时间参数匹配的变量节点的连接关系，这样便得到了第二因子图。其中，车辆观测位姿的时间参数可以是某个时刻，也可以是某个时间段。

相应的，在本发明实施例中，车辆观测位姿可以包括表征车辆在某一时刻具体位置的一元信息，例如车辆的直接定位信息，可以包括卫星定位信息、激光定位信息、环境影像定位信息等。

车辆观测位姿也可以包括表征车辆在两个时刻之间运动路程的二元信息，例如车辆的间接定位信息，可以包括根据车辆历史位姿和运动路程计算的位置信息等，其中运动路程可以通过激光雷达里程计、轮速里程计等传感器获得。

可选的，步骤202还可以包括：

子步骤2021，在所述车辆观测位姿指示车辆在两个时刻之间的运动路程的情况下，将所述车辆观测位姿确定为所述两个时刻对应的变量节点之间的二元因子节点。

在本发明实施例中，指示车辆在两个时刻之间的运动路程的车辆观测位姿可以称作二元信息，对应的时间信息为时间段，其中，时间段包含起点时刻和终点时刻，用于指示在起点时刻与终点时刻之间，车辆的运动路程。在将二元信息加入第一因子图时，可以将二元信息作为一个二元因子节点分别与该二元信息的起点时刻对应的变量节点和终点时刻对应的变量节点相连。

子步骤2022，在所述车辆观测位姿指示车辆在一个时刻的位置信息的情况下，将所述车辆观测位姿确定为所述一个时刻对应的变量节点的一元因子节点。

在本发明实施例中，指示车辆在在一个时刻的位置信息的车辆观测位姿可以称作一元信息，对应的时间信息为一个时刻。在将一元信息加入第一因子图时，可以将一元信息作为因子节点，与该一元信息的时间信息匹配的变量节点相连。

步骤203，从所述第二因子图中确定由所述因子节点相关联的关联变量节点，以及由所述关联变量节点相关联的因子节点构成的局部因子图。

第二因子图中的新增的因子节点如果是为一个一元因子节点，则该新增的因子节点会影响到与其连接点的变量节点，以及该变量节点之后的所有具有二元因子节点变量节点，因此，在新增的因子节点为一元因子节点的情况下，可以将与该新增的因子节点直接连接的变量节点确定为关联变量节点，并将其后的所有变量节点中，存在连续二元因子节点连接关系的变量节点确定为关联变量节点。

如图5所示，图5示出了本发明实施例提供的一种关联变量节点示意图，如图5所示，节点C1为新加入的因子节点，则变量节点X1至X3之间存在连续二元因子节点（因子节点C1和因子节点C2）连接关系，则可以将变量节点X1、X2和X3确定为关联变量节点。从而将关联变量节点X1、X2和X3，以及与关联变量节点X1、X2和X3连接的所有因子节点（因子节点D1、D2、D3、C1和C2）提取出来并保留关联关系，得到局部因子图。需要说明的是，二元因子节点还可以是一种函数关系，例如根据变量节点X2推测变量节点X3的函数，也可以作为二元因子节点。

步骤204，根据所述局部因子图中关联变量节点的因子节点，确定所述关联变量节点的车辆观测位姿；其中，所述车辆观测位姿与所述因子节点具有一一对应关系。

由于关联变量节点的因子节点是通过实际的传感器数据产生的，因此通过关联变量节点相连的因子节点可以计算出该关联变量节点对应的车辆观测位姿，例如，在因子节点是根据卫星定位信息确定的一元因子节点的情况下，可以将该因子节点的位置信息直接确定为与其项链的关联变量节点的车辆观测位姿；在因子节点是根据激光里程计数据确定的二元因子节点的情况下，可以将该因子节点的里程数据与该因子节点连接的一个关联变量节点求进行求和，确定出与该因子节点相连的另一个关联变量节点的车辆观测位姿。

在本发明实施例中，对于每一个关联变量节点，可以根据与其相连的每一个因子节点计算出一个对应的车辆观测位姿，例如，一个关联变量节点连接有4个因子节点，则相应的，该关联变量节点对应有4个车辆观测位姿。

步骤205，根据所述关联变量节点的车辆观测位姿对所述关联变量节点进行优化，得到所述关联变量节点对应时刻的车辆预测位姿。

可选的，步骤205还可以包括：

子步骤2051，针对每个关联变量节点，确定所述关联变量节点的初始车辆预测位姿与所述关联变量节点的车辆观测位姿之间的差值。

关联变量节点的初始车辆预测位姿是指本次优化之前，该关联变量节点所表示的车辆预测位姿，在本发明实施例中，每个关联变量节点可能被优化过不止一次，因此，需要说明的是，在第一次优化时，初始车辆预测位姿可以被赋予一个固定值或任意值，关联变量节点的初始车辆预测位姿并不一定是该关联变量节点在生成时所表示的车辆预测位姿。

对于局部因子图中的每个关联变量节点，将该关联变量节点的初始车辆预测位姿与该关联变量节点对应的每个车辆观测位姿分别做差，得到与该关联变量节点对应的车辆观测位姿的数量个差值。

参照图6，图6示出了本发明实施例提供的另一种局部因子图示意图，如图6所示，该局部因子图中包含X1、X2这两个关联变量节点，关联变量节点X1与一元因子节点L1和D1相连，且与二元因子节点C1相连，关联变量节点X2与一元因子节点L2相连，且与二元因子节点C1相连。则在本发明实施例中，关联变量节点X1的观测值可以包含一元因子节点L1的值和一元因子节点D1的值，关联变量节点X2的一个观测值可以是一元因子节点L2的值，关联变量节点X2另一个观测值可以是二元因子节点C1与关联变量节点X1进行累加计算得到。可以进一步计算X1所表示的初始车辆预测位姿与上述X1的各个观测值之间的差值（数量为2个），计算X2所表示的初始车辆预测位姿与上述X2的各个观测值之间的差值（数量为2个）。

子步骤2052，根据各个关联变量节点对应的差值构建损失函数。

在本发明实施例中，可以采用非线性最小二乘算法（例如Levenberg-Marquardt）对局部因子图进行优化计算，以获得局部因子图中各关联变量节点的最大后验概率对应的数值。具体为，首先构建损失函数（也可称为代价函数），损失函数可以设计为局部因子图中，每个因子节点对应的观测值（即关联变量节点的与每个因子节点对应的观测值），与相应关联变量节点的初始车辆预测位姿之间差值的平方的累加和。

以图6示出的局部因子图举例来说，若关联变量节点X1与因子节点L1对应的观测值为m1，关联变量节点X1与因子节点D1对应的观测值为n1，关联变量节点X2与因子节点L2对应的观测值为m2，关联变量节点X2与因子节点C1对应的观测值为r2，且关联变量节点X1的初始车辆预测位姿用X1表示，关联变量节点X2的初始车辆预测位姿用X2表示，则可以构建出如下损失函数：

其中，L表示损失函数。

需要说明的是，在图6示出了局部因子图中，虽然关联变量节点X1与二元因子节点C1相连，但C1并不会影响关联变量节点X1，而是C1与关联变量节点X1共同作用从而影响其后的关联变量节点X2，因此，在上述示例中，二元因子节点C1只与关联变量节点X2存在对应的观测值。

子步骤2053，调整所述关联变量节点的初始车辆预测位姿，在所述损失函数满足预设条件的情况下，将调整后的初始车辆预测位姿确定为所述关联变量节点对应时刻的车辆预测位姿。

根据局部因子图确定出损失函数之后，再调整损失函数中各个初始车辆预测位姿（使其增大或减小），使得损失函数趋近最小值，当损失函数达到最小值（通过调整初始车辆预测位姿无法进一步减小损失函数的取值时），将损失函数中调整后的初始车辆位置作为对应关联变量节点的优化结果。这样便可以将局部因子图的优化计算问题就转化为了最小二乘问题，有效降低了计算难度，并提升了计算效率。需要说明的是，预设条件可以是损失函数达到0或达到一个预设的最小值，也可以是其他条件，技术人员可以根据实际需要灵活设置，本发明实施例对此并不进行具体限定。

综上，本发明实施例提供了一种位姿确定方法，包括：获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿；获取车辆观测位姿，根据车辆观测位姿和第一因子图，确定车辆观测位姿对应的局部因子图；其中，局部因子图包含车辆观测位姿的时间参数对应的关联变量节点，以及由车辆观测位姿确定的因子节点；根据局部因子图对关联变量节点进行优化，得到关联变量节点对应时刻的车辆预测位姿。本发明实施例中，可以获取车辆观测位姿，根据因子图快速确定出该车辆观测位姿对应的关联变量节点的局部因子图，并根据局部因子图对关联变量节点进行优化，得到车辆预测位姿，使得车辆的定位结果更加稳定和精确，计算过程更加高效，为车辆实现更加高级的自动驾驶提供了可靠的定位基础。

参照图7，为本发明实施例所述的再一种位姿确定方法的步骤流程图。

步骤301，获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿。

此步骤可参见步骤101，本发明实施例不再赘述。

步骤302，将所述第一因子图转换为团树；其中，所述团树包含用于指示所述第一因子图中各个节点间连接关系的团。

随着车辆行驶时长的增加，第一因子图中的变量节点会越来越多，导致第一因子图越来越大，举例来说，如果车辆以0.1秒为间隔确定位姿，则车辆行驶1小时后，第一因子图中会包含36000个变量节点以及数量更多的因子节点，如果直接对第一因子图进行操作以获取局部因子图，需要解析大量的节点关系从而确定与一个新加入的因子节点相关联的关联变量节点，这样会消耗大量的算力。

在本发明实施例中，还可以采用将第一因子图转换为团树(clique tree)的方式，记录第一因子图中各个节点的影响关系，并采用团树传播算法根据团树确定出该因子节点的关联变量节点，这样，便可以在确定出一个因子节点的情况下，快速根据团树确定出受到该因子节点影响的关联变量节点，从而实现高效构建局部因子图的效果。

参照图8和图9，图8示出了本发明实施例提供的另一种第一因子图示意图，图9示出了本发明实施例提供的一种团树示意图，图9所示出的团树为图8所示出的第一因子图经过转化后得到的，如图8所示，X1、X2和X3为变量节点，P1和P2为一种一元因子节点（如用于进行环境影像定位的车辆周围的环境影像数据），L1、L2和L3可以为根据环境影像数据结合高精度地图匹配到的视觉定位结果，D1、D2和D3可以是根据卫星定位信息确定的一元因子节点，J1和J2可以是基于激光里程计确定的二元因子节点。对图8的第一因子图进行转化后，得到如图9所示的团树，其中团树中的团用于描述图8所示的第一因子图中节点之间的连接关系。需要说明的是，团树中的团用于表示变量节点的全连接关系，团中的“：”的表示这个团通过“：”后面的节点与上一级团相连。在上述过程中，在得到团树之前，还可以对第一因子图进行简化，消除第一因子图中的部分因子节点以简化计算过程，具体可以参照子步骤3021。

可选的，步骤302还可以包括：

子步骤3021，将所述第一因子图转换为贝叶斯网络。

为了进一步简化运算，先将第一因子图转化为贝叶斯网络，再由贝叶斯网络生成团树。在将第一因子图转化为贝叶斯网络的过程中，还可以对第一因子图进行消元，以减少第一因子图中的因子节点，举例来说，当两个变量节点之间连接有多个二元因子节点的情况下，可以将多个二元因子节点合并成一个二元因子节点，还可以将一元因子节点与自身的先验信息进行合并，其中，先验信息是用于确定另一个一元因子节点的一元因子节点，例如，在图8中，P1为L2的先验信息，P1也为L1的先验信息。需要说明的是，在图9中，部分因子节点在转化过程中被消去了，转化后的团树中并不一定包含第一因子图中的所有因子节点。技术人员可以根据需要选择合适的消元方法，例如可以采用QR分解法对因子图进行消元，本发明实施例在此并不进行具体限定。

参照图10，图10示出了本发明实施例提供的一种贝叶斯网络示意图，经过对图8中的第一因子图进行消元后并进行转化后，可以得到如图10所示的贝叶斯网络。其中，图10所示的贝叶斯网络中节点间的箭头用于指示节点之间的依赖关系，例如，变量节点X3受到因子节点P2的影响，则变量节点X3与因子节点P2之间具有由变量节点X3指向因子节点P2的箭头，变量节点X2受到因子节点P1的影响，则变量节点X2与因子节点P1之间具有由变量节点X2指向因子节点P1的箭头，变量节点X3可以受到变量节点X2的影响，即变量节点X3依赖于变量节点X2，则变量节点X3与变量节点X2之间具有由变量节点X3指向变量节点X2的箭头。

步骤303，根据所述车辆观测位姿的时间参数，匹配所述车辆观测位姿在所述第一因子图中对应的关联变量节点。

在本发明实施例中，车辆观测位姿对应的关联变量节点可以包括第一关联变量节点和第二关联变量节点，其中，第一关联变量节点是与该车辆观测位姿直接相关的变量节点，第二关联变量节点是与该车辆观测位姿间接相关的变量节点。

在获取到车辆观测位姿时，可以同时获取该车辆观测位姿对应的时间参数，并根据车辆观测位姿的时间参数在第一因子图中匹配各个变量节点对应的时刻，以确定与该车辆观测位姿直接相关的第一关联变量节点。

在确定出车辆观测位姿直接相关的第一关联变量节点，可以在团树中查找该第一关联变量节点，并根据团树中团与团之间的连接关系，将受到该第一关联变量节点影响的变量节点确定为第二关联变量节点。这样便确定出了车辆观测位姿对应的所有关联变量节点。

步骤304，根据所述关联变量节点和所述团树，从所述第一因子图中提取中间因子图。

将车辆观测位姿对应的所有关联变量节点与第一因子图进行匹配，对第一因子图中匹配的变量节点及其因子节点进行提取，得到中间因子图。

步骤305，将所述车辆观测位姿作为所述关联变量节点的因子节点添加至所述中间因子图中，得到局部因子图。

步骤306，根据所述局部因子图中关联变量节点的因子节点，确定所述关联变量节点的车辆观测位姿；其中，所述车辆观测位姿与所述因子节点具有一一对应关系。

此步骤可参见步骤204，本发明实施例不再赘述。

步骤307，根据所述关联变量节点的车辆观测位姿对所述关联变量节点进行优化，得到所述关联变量节点对应时刻的车辆预测位姿。

此步骤可参见步骤205，本发明实施例不再赘述。

综上，本发明实施例提供了一种位姿确定方法，包括：获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿；获取车辆观测位姿，根据车辆观测位姿和第一因子图，确定车辆观测位姿对应的局部因子图；其中，局部因子图包含车辆观测位姿的时间参数对应的关联变量节点，以及由车辆观测位姿确定的因子节点；根据局部因子图对关联变量节点进行优化，得到关联变量节点对应时刻的车辆预测位姿。本发明实施例中，可以获取车辆观测位姿，根据因子图快速确定出该车辆观测位姿对应的关联变量节点的局部因子图，并根据局部因子图对关联变量节点进行优化，得到车辆预测位姿，使得车辆的定位结果更加稳定和精确，计算过程更加高效，为车辆实现更加高级的自动驾驶提供了可靠的定位基础。

在上述实施例的基础上，本发明实施例还提供了一种位姿确定装置。

参考图11，图11示出了本发明实施例提供的一种位姿确定装置的结构框图：

获取模块401，用于获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿。

确定模块402，用于获取车辆观测位姿，根据车辆观测位姿和第一因子图，确定车辆观测位姿对应的局部因子图；其中，局部因子图包含车辆观测位姿的时间参数对应的关联变量节点，以及由车辆观测位姿确定的因子节点。

优化模块403，用于根据局部因子图对关联变量节点进行优化，得到关联变量节点对应时刻的车辆预测位姿。

可选的，确定模块，包括：

第二因子图子模块，用于根据车辆观测位姿的时间参数，将车辆观测位姿作为变量节点的因子节点添加至第一因子图中，得到第二因子图。

第一局部因子图子模块，用于从第二因子图中确定由因子节点相关联的关联变量节点，以及由关联变量节点相关联的因子节点构成的局部因子图。

可选的，确定模块，包括：

团树子模块，用于将第一因子图转换为团树；其中，团树包含用于指示第一因子图中各个节点间连接关系的团。

关联变量节点子模块，用于根据车辆观测位姿的时间参数，匹配车辆观测位姿在第一因子图中对应的关联变量节点。

中间因子图子模块，用于根据关联变量节点和团树，从第一因子图中提取中间因子图。

第二局部因子图子模块，用于将车辆观测位姿作为关联变量节点的因子节点添加至中间因子图中，得到局部因子图。

可选的，团树子模块，包括：

贝叶斯网络子模块，用于将第一因子图转换为贝叶斯网络。

团树转换子模块，用于将贝叶斯网络转换为团树；其中，团树包含用于指示第一因子图中各个变量节点间连接关系的团。

可选的，因子节点包括一元因子节点和二元因子节点，装置还包括：

二元因子子模块，用于在车辆观测位姿指示车辆在两个时刻之间的运动路程的情况下，将车辆观测位姿确定为两个时刻对应的变量节点之间的二元因子节点。

一元因子子模块，用于在车辆观测位姿指示车辆在一个时刻的位置信息的情况下，将车辆观测位姿确定为一个时刻对应的变量节点的一元因子节点。

可选的，优化模块，包括：

观测位姿子模块，用于根据局部因子图中关联变量节点的因子节点，确定关联变量节点的车辆观测位姿；其中，车辆观测位姿与因子节点具有一一对应关系。

预测位姿子模块，用于根据关联变量节点的车辆观测位姿对关联变量节点进行优化，得到关联变量节点对应时刻的车辆预测位姿。

可选的，预测位姿子模块，包括：

差值子模块，用于针对每个关联变量节点，确定关联变量节点的初始车辆预测位姿与关联变量节点的车辆观测位姿之间的差值。

损失函数子模块，用于根据各个关联变量节点对应的差值构建损失函数。

调整子模块，用于调整关联变量节点的初始车辆预测位姿，在损失函数满足预设条件的情况下，将调整后的初始车辆预测位姿确定为关联变量节点对应时刻的车辆预测位姿。

综上，本发明实施例提供了一种位姿确定装置，包括：获取模块，用于获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿；确定模块，用于获取车辆观测位姿，根据车辆观测位姿和第一因子图，确定车辆观测位姿对应的局部因子图；其中，局部因子图包含车辆观测位姿的时间参数对应的关联变量节点，以及由车辆观测位姿确定的因子节点；优化模块，用于根据局部因子图对关联变量节点进行优化，得到关联变量节点对应时刻的车辆预测位姿。本发明实施例中，可以获取车辆观测位姿，根据因子图快速确定出该车辆观测位姿对应的关联变量节点的局部因子图，并根据局部因子图对关联变量节点进行优化，得到车辆预测位姿，使得车辆的定位结果更加稳定和精确，计算过程更加高效，为车辆实现更加高级的自动驾驶提供了可靠的定位基础。

本发明实施例还提供一种整车控制器，整车控制器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行上述位姿确定方法。

本发明实施例还提供一种可读存储介质，可读存储介质上存储程序或指令，程序或指令被处理器执行时实现上述位姿确定方法。

本发明实施例还提供一种车辆，包括上述整车控制器。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述装置实施例中的对应过程，在此不再赘述。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (8)

1.一种位姿确定方法，其特征在于，所述方法包括：

获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿；

获取车辆观测位姿，根据所述车辆观测位姿和所述第一因子图，确定所述车辆观测位姿对应的局部因子图；其中，所述局部因子图包含所述车辆观测位姿的时间参数对应的关联变量节点，以及由所述车辆观测位姿确定的因子节点；

根据所述局部因子图对所述关联变量节点进行优化，得到所述关联变量节点对应时刻的车辆预测位姿；

所述根据所述车辆观测位姿和所述第一因子图，确定所述车辆观测位姿对应的局部因子图，包括：

根据所述车辆观测位姿的时间参数，将所述车辆观测位姿作为所述变量节点的因子节点添加至所述第一因子图中，得到第二因子图；

从所述第二因子图中确定由所述因子节点相关联的关联变量节点，以及由所述关联变量节点相关联的因子节点构成的局部因子图；

其中，所述关联变量节点包括所述第二因子图中，与所述因子节点直接相连的变量节点，以及受所述因子节点影响的变量节点；

或者，

所述根据所述车辆观测位姿和所述第一因子图，确定所述车辆观测位姿对应的局部因子图，包括：

将所述第一因子图转换为团树；其中，所述团树包含用于指示所述第一因子图中各个节点间连接关系的团；

根据所述车辆观测位姿的时间参数，匹配所述车辆观测位姿在所述第一因子图中对应的关联变量节点；

根据所述关联变量节点和所述团树，从所述第一因子图中提取中间因子图；

将所述车辆观测位姿作为所述关联变量节点的因子节点添加至所述中间因子图中，得到局部因子图。

2.根据权利要求1所述的位姿确定方法，其特征在于，所述将所述第一因子图转换为团树，包括：

将所述第一因子图转换为贝叶斯网络；

将所述贝叶斯网络转换为团树；其中，所述团树包含用于指示所述第一因子图中各个变量节点间连接关系的团。

3.根据权利要求1所述的位姿确定方法，其特征在于，所述因子节点包括一元因子节点和二元因子节点，所述方法还包括：

在所述车辆观测位姿指示车辆在两个时刻之间的运动路程的情况下，将所述车辆观测位姿确定为所述两个时刻对应的变量节点之间的二元因子节点；

在所述车辆观测位姿指示车辆在一个时刻的位置信息的情况下，将所述车辆观测位姿确定为所述一个时刻对应的变量节点的一元因子节点。

4.根据权利要求1所述的位姿确定方法，其特征在于，所述根据所述局部因子图对所述关联变量节点进行优化，得到所述关联变量节点对应时刻的车辆预测位姿，包括：

根据所述局部因子图中关联变量节点的因子节点，确定所述关联变量节点的车辆观测位姿；其中，所述车辆观测位姿与所述因子节点具有一一对应关系；

根据所述关联变量节点的车辆观测位姿对所述关联变量节点进行优化，得到所述关联变量节点对应时刻的车辆预测位姿。

5.根据权利要求4所述的位姿确定方法，其特征在于，所述根据所述关联变量节点的车辆观测位姿对所述关联变量节点进行优化，得到所述关联变量节点对应时刻的车辆预测位姿，包括：

针对每个关联变量节点，确定所述关联变量节点的初始车辆预测位姿与所述关联变量节点的车辆观测位姿之间的差值；

根据各个关联变量节点对应的差值构建损失函数；

调整所述关联变量节点的初始车辆预测位姿，在所述损失函数满足预设条件的情况下，将调整后的初始车辆预测位姿确定为所述关联变量节点对应时刻的车辆预测位姿。

6.一种位姿确定装置，其特征在于，所述装置包括：

获取模块，用于获取包含多个变量节点的第一因子图；其中，不同变量节点的值用于指示不同时刻的车辆位姿；

确定模块，用于获取车辆观测位姿，根据所述车辆观测位姿和所述第一因子图，确定所述车辆观测位姿对应的局部因子图；其中，所述局部因子图包含所述车辆观测位姿的时间参数对应的关联变量节点，以及由所述车辆观测位姿确定的因子节点；

优化模块，用于根据所述局部因子图对所述关联变量节点进行优化，得到所述关联变量节点对应时刻的车辆预测位姿；

所述确定模块，包括：

第二因子图子模块，用于根据车辆观测位姿的时间参数，将车辆观测位姿作为变量节点的因子节点添加至第一因子图中，得到第二因子图；

第一局部因子图子模块，用于从第二因子图中确定由因子节点相关联的关联变量节点，以及由关联变量节点相关联的因子节点构成的局部因子图；其中，所述关联变量节点包括所述第二因子图中，与所述因子节点直接相连的变量节点，以及受所述因子节点影响的变量节点；

或者，

所述确定模块，包括：

团树子模块，用于将第一因子图转换为团树；其中，团树包含用于指示第一因子图中各个节点间连接关系的团；

关联变量节点子模块，用于根据车辆观测位姿的时间参数，匹配车辆观测位姿在第一因子图中对应的关联变量节点；

中间因子图子模块，用于根据关联变量节点和团树，从第一因子图中提取中间因子图；

第二局部因子图子模块，用于将车辆观测位姿作为关联变量节点的因子节点添加至中间因子图中，得到局部因子图。

7.一种整车控制器，其特征在于，所述整车控制器包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至5中任一项所述的位姿确定方法。

8.一种可读存储介质，其特征在于，所述可读存储介质上存储程序或指令，所述程序或指令被处理器执行时实现如权利要求1至5中任一项所述的位姿确定方法。

Images