1.本发明涉及自动驾驶控制领域，尤其涉及一种车辆定位方法、装置、设备及存储介质。
背景技术：2.车辆实时定位是自动驾驶系统中不可或缺的关键模块，为下游的规划、感知模块提供车辆实时位姿信息，方便车辆更好地决策和控制。因为传统的滤波方案具备实时性好、计算资源少以及可靠性高等优点，传统的车辆实时定位系统多采用滤波方案。但是滤波方案由于本身的一些限制，对于车辆驾驶过程中容易出现的不定延时的传感数据、非线性较强的观测状态数据等，在对车辆进行实时定位时，其定位精度容易受到影响，难以取得理想结果，同时在对观测状态数据的优化过程中，传统非线性优化方案无法兼顾效率和精确度，难以实时运行。总而言之，即现有的车辆的实时定位方法存在定位精度较低的问题。
技术实现要素：3.本发明的主要目的在于解决现有的车辆的实时定位方法存在定位精度较低的技术问题。
4.本发明第一方面提供了一种车辆定位方法，包括：获取车辆实时的传感数据和因子图，其中，所述因子图包含驾驶过程中按观测时序排列的多个第一状态节点；根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图；利用预设的状态变化条件，对所述新的因子图进行线性化处理和状态增量更新，得到所述车辆对应的各个观测状态信息；根据各所述观测状态信息，依次对所述车辆的位姿进行传播更新，得到车辆位姿信息，并基于所述车辆位姿信息对所述车辆进行定位。
5.可选的，在本发明第一方面的第一种实现方式中，所述根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图包括：确定所述因子图中各个第一状态节点的第二观测时间，并分别计算各所述第二观测时间与所述第一观测时间之间的时间差值；按照时间差值从小到大，从各所述第一状态节点中选取至少一个第一状态节点；根据选取的第一状态节点对应的时间差值和第二观测时间，利用预置imu积分算法，将所述传感数据与选取的第一状态节点进行关联，得到新的因子图。
6.可选的，在本发明第一方面的第二种实现方式中，所述新的因子图包括第一因子图和第二因子图，所述根据选取的第一状态节点对应的时间差值和第二观测时间，利用预置imu积分算法，将所述传感数据与选取的第一状态节点进行关联，得到新的因子图包括：判断选取的第一状态节点对应的时间差值是否小于预置时间差阈值；若小于预置时间差阈值，则利用预置imu积分算法，将所述传感数据同步至时间差值最小的第一状态节点中，并将所述传感数据对应的观测因子关联至时间差值最小的第一状态节点中，得到第一因子图；若不小于预置时间差阈值，则利用预置imu积分算法，采用所述传感数据生成第二状态
节点；根据选取的第一状态节点对应的第二观测时间，将所述第二状态节点与选取的第一状态节点进行关联，并将所述传感数据对应的观测因子关联至所述第二状态节点中，得到第二因子图。
7.可选的，在本发明第一方面的第三种实现方式中，所述根据选取的第一状态节点对应的第二观测时间，将所述第二状态节点与选取的第一状态节点进行关联包括：判断选取的第一状态节点对应的第二观测时间是否小于预置时间阈值；若小于预置时间阈值，则将所述第二状态节点与时间差值最小的在后第一状态节点进行关联；若不小于预置时间阈值，则将所述第二状态节点分别与时间差值最小的在前第一状态节点和在后第一状态节点进行关联。
8.可选的，在本发明第一方面的第四种实现方式中，所述利用预设的状态变化条件，对所述新的因子图进行线性化处理和状态增量更新，得到所述车辆对应的各个观测状态信息包括：计算所述新的因子图中每两个相邻状态节点之间的状态差值，并从所述新的因子图中选取状态差值大于预设的状态变化阈值的状态节点；利用所述新的因子图中选取的状态节点进行线性化处理；按照观测时序排列，利用反向链式传播方式，在线性化处理后的因子图中采用在前的状态节点对在后的状态节点进行状态更新；当状态更新的更新量小于预设的状态更新阈值时停止，得到所述车辆对应的各个观测状态信息。
9.可选的，在本发明第一方面的第五种实现方式中，所述根据各所述观测状态信息，依次对所述车辆的位姿进行传播更新，得到车辆位姿信息包括：确定第一个观测状态信息于所在观测时间对应的位姿信息，并利用在后的观测状态信息和在前的位姿信息，计算在后的观测状态信息于所在观测时间对应的位姿信息，直到计算得到最新观测时间对应的位姿信息时停止，得到车辆位姿信息。
10.可选的，在本发明第一方面的第六种实现方式中，在所述根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图之后，还包括：利用预置的时间窗口，从所述新的因子图中筛选边缘状态节点，并放置于预置消元序列；执行所述消元序列的消元操作，以将所述新的因子图中的边缘状态节点剔除。
11.本发明第二方面提供了一种车辆定位装置，包括：获取模块，用于获取车辆实时的传感数据和因子图，其中，所述因子图包含驾驶过程中按观测时序排列的多个第一状态节点；关联模块，用于根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图；优化模块，用于利用预设的状态变化条件，对所述新的因子图进行线性化处理和状态增量更新，得到所述车辆对应的各个观测状态信息；定位模块，用于根据各所述观测状态信息，依次对所述车辆的位姿进行传播更新，得到车辆位姿信息，并基于所述车辆位姿信息对所述车辆进行定位。
12.可选的，在本发明第二方面的第一种实现方式中，所述关联模块包括：差值计算单元，用于确定所述因子图中各个第一状态节点的第二观测时间，并分别计算各所述第二观测时间与所述第一观测时间之间的时间差值；时间差选取单元，用于按照时间差值从小到大，从各所述第一状态节点中选取至少一个第一状态节点；关联单元，用于根据选取的第一状态节点对应的时间差值和第二观测时间，利用预置imu积分算法，将所述传感数据与选取的第一状态节点进行关联，得到新的因子图。
13.可选的，在本发明第二方面的第二种实现方式中，所述新的因子图包括第一因子图和第二因子图，所述关联单元还用于：判断选取的第一状态节点对应的时间差值是否小于预置时间差阈值；若小于预置时间差阈值，则利用预置imu积分算法，将所述传感数据同步至时间差值最小的第一状态节点中，并将所述传感数据对应的观测因子关联至时间差值最小的第一状态节点中，得到第一因子图；若不小于预置时间差阈值，则利用预置imu积分算法，采用所述传感数据生成第二状态节点；根据选取的第一状态节点对应的第二观测时间，将所述第二状态节点与选取的第一状态节点进行关联，并将所述传感数据对应的观测因子关联至所述第二状态节点中，得到第二因子图。
14.可选的，在本发明第二方面的第三种实现方式中，所述关联单元还用于：判断选取的第一状态节点对应的第二观测时间是否小于预置时间阈值；若小于预置时间阈值，则将所述第二状态节点与时间差值最小的在后第一状态节点进行关联；若不小于预置时间阈值，则将所述第二状态节点分别与时间差值最小的在前第一状态节点和在后第一状态节点进行关联。
15.可选的，在本发明第二方面的第四种实现方式中，所述优化模块包括：状态差选取单元，用于计算所述新的因子图中每两个相邻状态节点之间的状态差值，并从所述新的因子图中选取状态差值大于预设的状态变化阈值的状态节点；优化单元，用于利用所述新的因子图中选取的状态节点进行线性化处理；按照观测时序排列，利用反向链式传播方式，在线性化处理后的因子图中采用在前的状态节点对在后的状态节点进行状态更新；循环单元，用于当状态更新的更新量小于预设的状态更新阈值时停止，得到所述车辆对应的各个观测状态信息。
16.可选的，在本发明第二方面的第五种实现方式中，所述定位模块还用于：确定第一个观测状态信息于所在观测时间对应的位姿信息，并利用在后的观测状态信息和在前的位姿信息，计算在后的观测状态信息于所在观测时间对应的位姿信息，直到计算得到最新观测时间对应的位姿信息时停止，得到车辆位姿信息。
17.可选的，在本发明第二方面的第六种实现方式中，所述车辆定位装置还包括消元模块，用于：利用预置的时间窗口，从所述新的因子图中筛选边缘状态节点，并放置于预置消元序列；执行所述消元序列的消元操作，以将所述新的因子图中的边缘状态节点剔除。
18.本发明第三方面提供了一种车辆定位设备，包括：存储器和至少一个处理器，所述存储器中存储有指令；所述至少一个处理器调用所述存储器中的所述指令，以使得所述车辆定位设备执行上述的车辆定位方法。
19.本发明的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述的车辆定位方法。
20.本发明提供的技术方案中，在每次有新的传感数据，会在因子图中的合适位置新增状态节点或者直接同步到原有的状态节点中，其中，每个状态节点之间可以依靠惯性传感器的预积分建立连接边，执行关联操作；由于因子图可以保存车辆的任意个观测状态，对于不同延时的传感数据，只要在其观测时间建立状态节点或者并入最接近的状态节点，即可保证时间同步性和观测状态对齐。利用状态变化条件，来对新的因子图进行线性化处理和状态增量更新，以兼顾观测状态的精确度和计算效率，同时采用改进的迭代优化求解方法，相比于滤波方案的一次性求解，可以更加精确地求解车辆在当前的车辆位置信息，以提
升对车辆的定位精度。
附图说明
21.图1为本发明实施例中车辆定位方法的第一个实施例示意图；
22.图2为本发明实施例中因子图的第一个实施例示意图；
23.图3为本发明实施例中车辆定位方法的第二个实施例示意图；
24.图4为本发明实施例中因子图的第二个实施例示意图；
25.图5为本发明实施例中因子图的第三个实施例示意图；
26.图6为本发明实施例中因子图的第四个实施例示意图；
27.图7为本发明实施例中车辆定位装置的第一个实施例示意图；
28.图8为本发明实施例中车辆定位装置的第二个实施例示意图；
29.图9为本发明实施例中车辆定位设备的一个实施例示意图。
具体实施方式
30.本发明实施例提供了一种车辆定位方法、装置、设备及存储介质，获取车辆实时的传感数据和因子图，其中，因子图包含驾驶过程中按观测时序排列的多个第一状态节点；根据传感数据的第一观测时间，对传感数据和因子图的第一状态节点执行关联操作，得到新的因子图；利用预设的状态变化条件，对新的因子图进行线性化处理和状态增量更新，得到车辆对应的各个观测状态信息；根据各观测状态信息，依次对车辆的位姿进行传播更新，得到车辆位姿信息，并基于车辆位姿信息对车辆进行定位。本发明实现了对不同类型传感数据的时间对其，提升了车辆定位的精准程度。
31.本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”或“具有”及其任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
32.为便于理解，下面对本发明实施例的具体流程进行描述，请参阅图1，本发明实施例中车辆定位方法的第一个实施例包括：
33.101、获取车辆实时的传感数据和因子图，其中，所述因子图包含驾驶过程中按观测时序排列的多个第一状态节点；
34.可以理解的是，本发明的执行主体可以为车辆定位装置，还可以是终端或者服务器，具体此处不做限定。本发明实施例以服务器为执行主体为例进行说明。
35.本实施例中，在车辆驾驶过程中，实时获取车辆上安装的各个传感器采集到的驾驶场景的传感数据，其中，车辆上安装的传感器包括imu(inertial measurement unit，即惯性测量单元)传感器、gps(global positioning system，全球定位系统)传感器、轮速计和雷达传感器等，则对应获取得到imu传感数据、gps传感数据、轮速传感数据和雷达点云数据等。
36.本实施例中，为了对不同传感器因延时采集到的传感器数据进行时间对齐，配置一张因子图，来记录采集到的传感器数据，并关联到带有观测时序排列的状态节点中，此处第一状态节点指的是最新记录传感器数据后维护得到的因子图，该最新的因子图中所包含的状态节点。
37.本实施例中，因子图中除了保留当前最新观测对应的状态节点，也会保留过去一系列时刻观测对应的状态节点，相邻时刻的状态节点通过惯性传感器(imu)的预积分来建立联系，将这样一个整体的因子图作为最后的优化对象，而不是像传统滤波方案只优化当前状态，可有效地解决不同传感数据的时间同步问题。
38.在一种实施方式中，如图2所述为因子图的一个实时示意图，观测因子{
……
，i，i+1，i+2，i+3，
……
}根据观测时间进行排序，每个观测时间通过因子e(imu因子)的imu预积分来对相邻观测时间的观测因子进行关联连接，每个状态节点同时关联有对应观测时间下采集到的传感数据，并以观测因子的形式，同时将状态节点与对应观测因子进行关联连接。每个不同类型传感数据对应的观测因子，比如因子a(gps pos因子)、因子b(gps vel因子)、因子c(lidar因子)、因子d(轮速因子)，所关联的状态节点之间的观测时序排列与观测因子类型无关，根据观测时间前后进行排列。其中，由于因子a和因子b由gps传感器同时产生的两类传感数据得到，故因子a和因子b会同时关联在同一个观测时间对应的状态节点中。
39.102、根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图；
40.本实施例中，每当采集到一个传感器数据，则需要在因子图中与过往采集到的传感器数据进行时间对齐，然后再进行后续的车辆位姿估计。因子图通过状态节点，维护了在此之前采集到的传感器数据按观测时序排列的每个时间节点，此处对传感数据的时间对齐具体在于将其关联到对应时间节点的第一状态节点中。
41.具体的，传感数据与第一状态节点的关联方式包括：将传感数据匹配并同步到因子图已存在的第一状态节点；为传感器数据创建一个新的状态节点，再将新的状态节点与第一状态节点进行匹配并插入。使得最新采集到的传感器数据按照观测时间排序加入过往采集的传感器数据中，使得传感器数据按采集时间实现对齐。
42.其中，若将传感数据匹配并同步到因子图已存在的第一状态节点，则新的因子图中的第一状态节点保持不变，仅新增传感器数据(增加观测因子)；若为传感器数据创建一个新的状态节点，则新的因子图中的第一状态节点的数量和排序发生变化。
43.在一种实施方式中，比如若因子图中包括第一状态节点分别为{s1、s2、
……
、sc}，对应观测时序排列的观测时间为{t1、t2、
……
、tc}，此时接收到某一传感器的传感数据m，该传感数据对应的测量时间是tm，由于这一传感器的处理延时，使得tm《tc，此时两者之间的时间差值为负数，则如果直接将状态节点sc和传感数据m融合，由于时间差造成的状态不同步则变相将观测误差融合到后续定位中，对定位精度造成影响。故可以在tm时刻建立一个状态节点sm，sm通过惯性传感器(imu)与匹配的状态节点si关联起来(建立连接边)，此处关联既包含了状态和对应的协方差信息。关联之后，再将传感数据m与状态节点sm融合，得到新的因子图。
44.103、利用预设的状态变化条件，对所述新的因子图进行线性化处理和状态增量更新，得到所述车辆对应的各个观测状态信息；
45.本实施例中，通过迭代的“线性化-状态增量更新”来计算车辆在各个观测时序的观测状态信息，可以克服非线性的各个传感数据带来的误差，同时加入状态变化条件，针对每两个相邻状态节点的状态变化幅度，来执行新的因子图的线性化和状态增量更新，有效提高观测状态信息的生成效率，同时保证生成精度。
46.具体的，新的因子图中的状态变化为非线性化，故需要将其转化为线性化的表示方式进行状态增量更新。预设的状态变化条件中包括以下两部分：关于线性化处理时设置的状态变化阈值、以及关于状态增量更新时设置的状态更新阈值。在处理新的因子图的线性化处理时，通过状态变化阈值来筛选用于线性化处理的观测因子，再通过状态更新阈值，来将线性化表达的观测因子进行状态传播和更新，以得到车辆在不同观测时间的各个观测状态信息。
47.具体的，新的因子图中除了新增加的传感数据所在的状态节点之外，其他状态节点在前一次位姿估计过程中，已计算得到其观测状态信息(基于每个状态节点包含的传感数据进行imu预积分得到)，故此处对新的因子图进行线性化处理和状态增量更新，实则是对每个状态节点已存在的观测状态信息，再结合新加入的传感数据进行线性化处理和状态增量更新，得到处理后保留下来的状态节点的新观测状态信息。
48.104、根据各所述观测状态信息，依次对所述车辆的位姿进行传播更新，得到车辆位姿信息，并基于所述车辆位姿信息对所述车辆进行定位。
49.本实施例中，在得到实时的观测状态信息后，即可执行对车辆的位姿估计，以对该车辆进行定位，车辆位姿的传播更新以新的因子图优化得到的观测状态信息作为积分起点，通过imu积分来计算车辆实时的位姿信息。具体如下所示：
50.获取因子图优化后的位姿pose_s0；获取s1时刻的imu，并在pose_s0基础上，计算得到s1时刻的位姿pose_s1；获取s2时刻的imu，并在pose_s1基础上，计算得到s2时刻的位姿pose_s2；
…
获取sn时刻的imu，并在pose_s
n-1
基础上，计算得到sn时刻的位姿pose_sn；最后将最新时刻的pose_sn作为车辆实时的车辆位姿信息，包括车辆的姿态、速度、位置等，以对车辆进行定位。
51.本发明实施例中，在每次有新的传感数据，会在因子图中的合适位置新增状态节点或者直接同步到原有的状态节点中，其中，每个状态节点之间可以依靠惯性传感器的预积分建立连接边，执行关联操作；由于因子图可以保存车辆的任意个观测状态，对于不同延时的传感数据，只要在其观测时间建立状态节点或者并入最接近的状态节点，即可保证时间同步性和观测状态对齐。利用状态变化条件，来对新的因子图进行线性化处理和状态增量更新，以兼顾观测状态的精确度和计算效率，同时采用改进的迭代优化求解方法，相比于滤波方案的一次性求解，可以更加精确地求解车辆在当前的车辆位置信息，以提升对车辆的定位精度。
52.请参阅图3，本发明实施例中车辆定位方法的第二个实施例包括：
53.301、获取车辆实时的传感数据和因子图，其中，所述因子图包含驾驶过程中按观测时序排列的多个第一状态节点；
54.本实施例中，在对车辆定位过程中，可以分为前端模块和后端模块，前端模块对所有的传感数据进行预处理并输入到后端模块中。后端模块接收到传感数据后，则根据不同的传感数据类型并结合其第一观测时间，在因子图中加入相应的因子并关联相应的状态节
点，计算最新的观测状态信息，并同步发送给前端模块。前端模块使用imu积分计算得到最新的车辆位姿信息。imu积分的起点是由后端模块给出的，这样的设计既保证了实时性，又确保了精度不会有损失。
55.本实施例中，在获取得到车辆实时的传感数据后，需要在前端模块中对传感数据进行数据状态检查、不同类型传感数据的坐标转换等。前者在于保证数据的准确度，后续进行车辆位姿估计和定位时，计算效率和准确度更高；后者在于将不同类型的传感数据统一转换到同一个坐标系下，以为了方便后续对不同类型传感数据的融合以及进行统一处理。
56.具体的，在对传感数据进行数据状态检查时，排除发生以下异常情况的数据：imu类型传感数据中出现连续相同的数据、imu类型传感数据中出现时间戳回跳的数据、gps类型传感数据中的采集信号星数过低和精度较差的数据、轮速类型传感数据中出现车辆左右端采集到的不一致的数据、雷达类型传感数据中匹配失败的数据。
57.具体的，在对不同类型传感数据进行坐标转换，主要包含gps类型传感数据和雷达类型传感数据分别到imu坐标系下的转换。执行过程如下所示：
58.在将gps类型传感数据转换到imu坐标系下时，若天线位置到imu传感器位置的偏移在imu坐标系下为t
ant
，gps类型传感数据表示的位置为p
ant
，则p
ant
转换到imu坐标系得到p
1imu
＝p
ant-r*t
ant
，其中，r1为imu坐标系到enu坐标系的旋转值。
59.在将雷达类型传感数据转换到imu坐标系下时，若雷达安装位置到imu位置的偏移在imu坐标系下为t
lidar
，雷达类型传感数据表示的位置为p
lidar
，则将p
lidar
转换到imu坐标系下得到p
2imu
＝p
lidar-r2*t
lidar
，其中，r2为imu坐标系到enu坐标系下的旋转值。若imu坐标系到雷达坐标系的旋转值为r
i-l
，雷达坐标系到enu坐标系的旋转值为r
l-e
，则imu坐标系到enu坐标系的旋转值r2＝r
l-e
*r
i-l
。
60.302、确定所述因子图中各个第一状态节点的第二观测时间，并分别计算各所述第二观测时间与所述第一观测时间之间的时间差值；
61.303、按照时间差值从小到大，从各所述第一状态节点中选取至少一个第一状态节点；
62.304、根据选取的第一状态节点对应的时间差值和第二观测时间，利用预置imu积分算法，将所述传感数据与选取的第一状态节点进行关联，得到新的因子图；
63.本实施例中，基于实时获取的传感数据的观测时间和因子图中的观测时序排列，确定传感数据所在因子图的状态节点位置，以将传感数据关联到因子图中时，使得新的因子图在加入传感数据后，各个状态节点的排序以及包括的传感数据仍然按观测时间从最早到最晚进行排序。
64.本实施例中，根据第二观测时间与各个第一状态节点的第二观测时间之间的差值大小，以及第一观测时间的早晚，来采用不同的方式执行传感数据与第一状态节点之间的关联，以刷新因子图。关于时间差值大小和第一观测时间的早晚，与对应执行的关联策略，具体如下所示：
65.10)判断选取的第一状态节点对应的时间差值是否小于预置时间差阈值；
66.20)若小于预置时间差阈值，则利用预置imu积分算法，将所述传感数据同步至时间差值最小的第一状态节点中，并将所述传感数据对应的观测因子关联至时间差值最小的第一状态节点中，得到第一因子图；
67.30)若不小于预置时间差阈值，则利用预置imu积分算法，采用所述传感数据生成第二状态节点；
68.40)根据选取的第一状态节点对应的第二观测时间，将所述第二状态节点与选取的第一状态节点进行关联，并将所述传感数据对应的观测因子关联至所述第二状态节点中，得到第二因子图。
69.本实施例中，根据传感数据的第一观测时间和每个状态节点的第二观测时间之间的时间差值，可以执行的关联策略分为两种，一种是不构建新的状态节点，一种是构建新的状态节点。前者直接将传感数据同步时间差值最小的第一状态节点中，并构建与该第一状态节点的关联；后者则在时间差值最小的一个或两个状态节点之间、之前或之后构建一个新的状态节点，并将传感数据同步到该状态节点中。
70.具体的，如图4所示，在因子图中找到与观测时间差值小于时间差阈值(比如5ms)的第一状态节点(比如为i+1的状态节点)，则基本可以等同于在该状态节点的第二观测时间(即将第一观测时间等同于第二观测时间)同步采集到了传感数据，这种情况则直接通过imu积分，将传感数据同步到该第一观测节点中，无须额外添加状态节点和关联的连接边，然后将对应的观测因子(因子d)关联到该状态节点。此时虚线部分的因子d则为新的因子图新增的内容。
71.具体的，在因子图中无法找到观测时间差值小于时间差阈值的第一状态节点时，则需要加入一个新的状态节点，以代表其第一观测时间，可以根据第二观测时间的早晚，分为以下两种情况：
72.41)判断选取的第一状态节点对应的第二观测时间是否小于预置时间阈值；
73.42)若小于预置时间阈值，则将所述第二状态节点与时间差值最小的在后第一状态节点进行关联；
74.43)若不小于预置时间阈值，则将所述第二状态节点分别与时间差值最小的在前第一状态节点和在后第一状态节点进行关联。
75.如图5所示，在选取的第一状态节点(i+1的状态节点)对应的第二观测时间小于时间阈值(比如在当前时间的150ms之内)时，则表示传感数据的观测时间较新，第二状态节点(i+2的状态节点)通过因子e的imu积分将其与在后的第一状态节点(i+1的状态节点)关联起来，然后将对应的观测因子(因子c)关联到该第二状态节点(i+2的状态节点)上。其中，虚线框中的一个因子c、一个i+2的状态节点和一个因子e，则为新的因子图中新增的内容。
76.如图6所示，在选取的第一状态节点(i和i+1的状态节点)对应的第二观测时间均大于时间阈值(比如在当前时间的150ms之前)时，则表示传感数据的观测时间较老，则通过两个因子e的imu积分，将该第二状态节点(i+2的状态节点)与观测时间最近的一个在前状态节点(i的状态节点)以及一个在后的状态节点(i+1的状态节点)关联起来，同时还需要将之前连接上下节点的关联边删掉，然后将对应的观测因子(因子c)关联到该第二状态节点(i+2的状态节点)上。其中，虚线框中的一个因子c、一个i+2的状态节点和两个因子e，则为新的因子图中新增的内容。
77.需注意的是，一个观测时间(包括第一观测时间和第二观测时间)节点对应一个状态节点，通过imu积分来进行相互关联连接，一个状态节点与一个或多个观测因子关联，绑定对应的传感数据。
78.305、利用预置的时间窗口，从所述新的因子图中筛选边缘状态节点，并放置于预置消元序列；
79.306、执行所述消元序列的消元操作，以将所述新的因子图中的边缘状态节点剔除；
80.本实施例中，由于观测时间较早的状态节点在执行后续的车辆位姿估计时，基本没有贡献，故预先设置好的时间窗口长度，以确定每次进行车辆位姿估计的观测时间跨度，比如300ms，对于处在时间窗口以外的节点，可以将其通过边缘化的方式在图中去掉，使得因子图的规模始终保持在一定大小，而不是无限增长，在保证定位精度的前提下，达到降低计算资源的目的。为了保证边缘状态节点的边缘化效率，将待边缘化的节点放在消元序列的开头，在执行边缘化操作时，只需要将消元序列中的边缘状态节点删除掉即可，无须其他额外的操作。
81.307、计算所述新的因子图中每两个相邻状态节点之间的状态差值，并从所述新的因子图中选取状态差值大于预设的状态变化阈值的状态节点；
82.308、利用所述新的因子图中选取的状态节点进行线性化处理；
83.309、按照观测时序排列，利用反向链式传播方式，在线性化处理后的因子图中采用在前的状态节点对在后的状态节点进行状态更新；
84.310、当状态更新的更新量小于预设的状态更新阈值时停止，得到所述车辆对应的各个观测状态信息；
85.本实施例中，针对新的因子图的线性化处理，不会对整个因子图的所有状态进行重新线性化，而是会选取状态变化(两个相邻状态节点之间的状态差值)超过一定阈值(预先设置好状态变化阈值)的状态节点，再以选取的状态节点进行重新的线性化，这是因为在对因子图的非线性化的各个状态节点进行处理时，状态变化相差不大的两个状态节点，得到该局部的线性化结果与之前的相差不大，对最终结果的精度影响甚微，为减少计算资源故可不参与线性化处理。
86.在一个实施方式中，在筛选状态差值大于预设的状态变化阈值的状态节点时，从观测时间最晚的状态节点开始，将其作为参照节点，计算参照节点与在前相邻的一个状态节点之间的状态差值，若差值不大于状态变化阈值，则剔除该在前相邻的状态节点，然后计算与下一个在前相邻的状态节点之间的状态差值，以此类推直到状态差值大于状态变化阈值，对最后一个在前相邻的状态节点进行选取以供线性化处理；然后将该选取的状态节点作为新的参照节点，执行下一个用于线性化处理的状态节点，直到筛选完因子图中所有的状态节点。
87.具体的，每两个相邻状态节点的状态差值也可以为在前状态节点的线性化值和在后状态节点的原始值之间的差值，以此实时前述实施方式。
88.本实施例中，在对线性化的因子图执行状态增量更新过程中，可以等效为一个反向链式传播的过程，从最新的状态节点逆时序逐步更新至最老的状态节点，也就是先更新最新的状态节点，得到该最新的状态节点在对应观测时间的观测状态信息；然后更新与该最新的状态节点关联的上一个状态节点，以此类推，直到更新完整个因子图，得到每个状态节点对应的观测状态信息，为了提高效率，在反向链式传播过程中，一旦发现某一状态节点的更新量小于某一阈值(预先设置状态更新阈值)，则立即停止继续更新，因为状态节点的
更新量小于设置的阈值时，意味着后续的各个状态节点的状态变化都不大，停止更新以减少更新时间。
89.311、确定第一个观测状态信息于所在观测时间对应的位姿信息，并利用在后的观测状态信息和在前的位姿信息，计算在后的观测状态信息于所在观测时间对应的位姿信息，直到计算得到最新观测时间对应的位姿信息时停止，得到车辆位姿信息，并基于所述车辆位姿信息对所述车辆进行定位。
90.本实施例中，获取因子图优化后第一个观测状态信息对应的位姿信息pose_s0后；根据在后s1时刻的观测状态信息和在前的位姿信息pose_s0，计算在后s1时刻的位姿信息pose_s1；然后根据在后s2时刻的观测状态信息和在前的位姿信息pose_s1，计算在后s2时刻的位姿pose_s2，以此类推，直到计算得到最新观测时间sn的位姿信息pose_sn，并将pose_sn作为车辆在第一观测时间的车辆位姿信息，具体可以包括车辆的姿态、速度、位置等。
91.具体的，根据在后的观测状态信息和在前的姿态，来计算在后观测时间对应的姿态时，若在前的姿态为则在后观测时间对应的姿态为：
[0092][0093]
其中，为常数，是ecef坐标系相对于eci坐标系的旋转，是enu坐标系相对于ecef坐标系的旋转，(+)和(-)分别代表正向和逆向旋转方向，是在前时刻到在后时刻的旋转，可以通过在后的观测状态信息中的角速度计算得到：
[0094][0095]
其中，其中，为在后的观测状态信息中的角速度。
[0096]
具体的，根据在后的观测状态信息和在前的速度，来计算在后观测时间对应的速度时，若在前的速度为则在后观测时间对应的速度为：
[0097][0098]
其中，为重力加速度，为重力加速度，为在后的观测状态信息中的加速度。
[0099]
具体的，根据在后的观测状态信息和在前的位置，来计算在后观测时间对应的位置时，若在前的位置包括高度hb(-)、纬度lb(-)和经度λb(-)，则在后观测时间对应的高度为：
[0100][0101]
在后观测时间对应的纬度为：
[0102][0103]
在后观测时间对应的经度为：
[0104][0105]
上面对本发明实施例中车辆定位方法进行了描述，下面对本发明实施例中车辆定位装置进行描述，请参阅图7，本发明实施例中车辆定位装置一个实施例包括：
[0106]
获取模块701，用于获取车辆实时的传感数据和因子图，其中，所述因子图包含驾驶过程中按观测时序排列的多个第一状态节点；
[0107]
关联模块702，用于根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图；
[0108]
优化模块703，用于利用预设的状态变化条件，对所述新的因子图进行线性化处理和状态增量更新，得到所述车辆对应的各个观测状态信息；
[0109]
定位模块704，用于根据各所述观测状态信息，依次对所述车辆的位姿进行传播更新，得到车辆位姿信息，并基于所述车辆位姿信息对所述车辆进行定位。
[0110]
本发明实施例中，在每次有新的传感数据，会在因子图中的合适位置新增状态节点或者直接同步到原有的状态节点中，其中，每个状态节点之间可以依靠惯性传感器的预积分建立连接边，执行关联操作；由于因子图可以保存车辆的任意个观测状态，对于不同延时的传感数据，只要在其观测时间建立状态节点或者并入最接近的状态节点，即可保证时间同步性和观测状态对齐。利用状态变化条件，来对新的因子图进行线性化处理和状态增量更新，以兼顾观测状态的精确度和计算效率，同时采用改进的迭代优化求解方法，相比于滤波方案的一次性求解，可以更加精确地求解车辆在当前的车辆位置信息，以提升对车辆的定位精度。
[0111]
请参阅图8，本发明实施例中车辆定位装置的另一个实施例包括：
[0112]
获取模块701，用于获取车辆实时的传感数据和因子图，其中，所述因子图包含驾驶过程中按观测时序排列的多个第一状态节点；
[0113]
关联模块702，用于根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图；
[0114]
优化模块703，用于利用预设的状态变化条件，对所述新的因子图进行线性化处理和状态增量更新，得到所述车辆对应的各个观测状态信息；
[0115]
定位模块704，用于根据各所述观测状态信息，依次对所述车辆的位姿进行传播更新，得到车辆位姿信息，并基于所述车辆位姿信息对所述车辆进行定位。
[0116]
具体的，所述关联模块702包括：
[0117]
差值计算单元7021，用于确定所述因子图中各个第一状态节点的第二观测时间，并分别计算各所述第二观测时间与所述第一观测时间之间的时间差值；
[0118]
时间差选取单元7022，用于按照时间差值从小到大，从各所述第一状态节点中选取至少一个第一状态节点；
[0119]
关联单元7023，用于根据选取的第一状态节点对应的时间差值和第二观测时间，利用预置imu积分算法，将所述传感数据与选取的第一状态节点进行关联，得到新的因子图。
[0120]
具体的，所述新的因子图包括第一因子图和第二因子图，所述关联单元7023还用
于：
[0121]
判断选取的第一状态节点对应的时间差值是否小于预置时间差阈值；
[0122]
若小于预置时间差阈值，则利用预置imu积分算法，将所述传感数据同步至时间差值最小的第一状态节点中，并将所述传感数据对应的观测因子关联至时间差值最小的第一状态节点中，得到第一因子图；
[0123]
若不小于预置时间差阈值，则利用预置imu积分算法，采用所述传感数据生成第二状态节点；
[0124]
根据选取的第一状态节点对应的第二观测时间，将所述第二状态节点与选取的第一状态节点进行关联，并将所述传感数据对应的观测因子关联至所述第二状态节点中，得到第二因子图。
[0125]
具体的，所述关联单元7023还用于：
[0126]
判断选取的第一状态节点对应的第二观测时间是否小于预置时间阈值；
[0127]
若小于预置时间阈值，则将所述第二状态节点与时间差值最小的在后第一状态节点进行关联；
[0128]
若不小于预置时间阈值，则将所述第二状态节点分别与时间差值最小的在前第一状态节点和在后第一状态节点进行关联。
[0129]
具体的，所述优化模块703包括：
[0130]
状态差选取单元7031，用于计算所述新的因子图中每两个相邻状态节点之间的状态差值，并从所述新的因子图中选取状态差值大于预设的状态变化阈值的状态节点；
[0131]
优化单元7032，用于利用所述新的因子图中选取的状态节点进行线性化处理；按照观测时序排列，利用反向链式传播方式，在线性化处理后的因子图中采用在前的状态节点对在后的状态节点进行状态更新；
[0132]
循环单元7033，用于当状态更新的更新量小于预设的状态更新阈值时停止，得到所述车辆对应的各个观测状态信息。
[0133]
具体的，所述定位模块704还用于：
[0134]
确定第一个观测状态信息于所在观测时间对应的位姿信息，并利用在后的观测状态信息和在前的位姿信息，计算在后的观测状态信息于所在观测时间对应的位姿信息，直到计算得到最新观测时间对应的位姿信息时停止，得到车辆位姿信息。
[0135]
具体的，所述车辆定位装置还包括消元模块705，用于：
[0136]
利用预置的时间窗口，从所述新的因子图中筛选边缘状态节点，并放置于预置消元序列；
[0137]
执行所述消元序列的消元操作，以将所述新的因子图中的边缘状态节点剔除。
[0138]
上面图7和图8从模块化功能实体的角度对本发明实施例中的车辆定位装置进行详细描述，下面从硬件处理的角度对本发明实施例中车辆定位设备进行详细描述。
[0139]
图9是本发明实施例提供的一种车辆定位设备的结构示意图，该车辆定位设备900可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理器(central processing units，cpu)910(例如，一个或一个以上处理器)和存储器920，一个或一个以上存储应用程序933或数据932的存储介质930(例如一个或一个以上海量存储设备)。其中，存储器920和存储介质930可以是短暂存储或持久存储。存储在存储介质930的程序可以包括
一个或一个以上模块(图示没标出)，每个模块可以包括对车辆定位设备900中的一系列指令操作。更进一步地，处理器910可以设置为与存储介质930通信，在车辆定位设备900上执行存储介质930中的一系列指令操作。
[0140]
车辆定位设备900还可以包括一个或一个以上电源940，一个或一个以上有线或无线网络接口950，一个或一个以上输入输出接口960，和/或，一个或一个以上操作系统931，例如windows serve，mac os x，unix，linux，freebsd等等。本领域技术人员可以理解，图9示出的车辆定位设备结构并不构成对车辆定位设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
[0141]
本发明还提供一种车辆定位设备，所述计算机设备包括存储器和处理器，存储器中存储有计算机可读指令，计算机可读指令被处理器执行时，使得处理器执行上述各实施例中的所述车辆定位方法的步骤。
[0142]
本发明还提供一种计算机可读存储介质，该计算机可读存储介质可以为非易失性计算机可读存储介质，该计算机可读存储介质也可以为易失性计算机可读存储介质，所述计算机可读存储介质中存储有指令，当所述指令在计算机上运行时，使得计算机执行所述车辆定位方法的步骤。
[0143]
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
[0144]
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(read-only memory，rom)、随机存取存储器(random access memory，ram)、磁碟或者光盘等各种可以存储程序代码的介质。
[0145]
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
技术特征：1.一种车辆定位方法，其特征在于，所述车辆定位方法包括：获取车辆实时的传感数据和因子图，其中，所述因子图包含驾驶过程中按观测时序排列的多个第一状态节点；根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图；利用预设的状态变化条件，对所述新的因子图进行线性化处理和状态增量更新，得到所述车辆对应的各个观测状态信息；根据各所述观测状态信息，依次对所述车辆的位姿进行传播更新，得到车辆位姿信息，并基于所述车辆位姿信息对所述车辆进行定位。2.根据权利要求1所述的车辆定位方法，其特征在于，所述根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图包括：确定所述因子图中各个第一状态节点的第二观测时间，并分别计算各所述第二观测时间与所述第一观测时间之间的时间差值；按照时间差值从小到大，从各所述第一状态节点中选取至少一个第一状态节点，并根据选取的第一状态节点对应的时间差值和第二观测时间，利用预置imu积分算法，将所述传感数据与选取的第一状态节点进行关联，得到新的因子图。3.根据权利要求2所述的车辆定位方法，其特征在于，所述新的因子图包括第一因子图和第二因子图，所述根据选取的第一状态节点对应的时间差值和第二观测时间，利用预置imu积分算法，将所述传感数据与选取的第一状态节点进行关联，得到新的因子图包括：判断选取的第一状态节点对应的时间差值是否小于预置时间差阈值；若小于预置时间差阈值，则利用预置imu积分算法，将所述传感数据同步至时间差值最小的第一状态节点中，并将所述传感数据对应的观测因子关联至时间差值最小的第一状态节点中，得到第一因子图；若不小于预置时间差阈值，则利用预置imu积分算法，采用所述传感数据生成第二状态节点；根据选取的第一状态节点对应的第二观测时间，将所述第二状态节点与选取的第一状态节点进行关联，并将所述传感数据对应的观测因子关联至所述第二状态节点中，得到第二因子图。4.根据权利要求3所述的车辆定位方法，其特征在于，所述根据选取的第一状态节点对应的第二观测时间，将所述第二状态节点与选取的第一状态节点进行关联包括：判断选取的第一状态节点对应的第二观测时间是否小于预置时间阈值；若小于预置时间阈值，则将所述第二状态节点与时间差值最小的在后第一状态节点进行关联；若不小于预置时间阈值，则将所述第二状态节点分别与时间差值最小的在前第一状态节点和在后第一状态节点进行关联。5.根据权利要求1所述的车辆定位方法，其特征在于，所述利用预设的状态变化条件，对所述新的因子图进行线性化处理和状态增量更新，得到所述车辆对应的各个观测状态信息包括：
计算所述新的因子图中每两个相邻状态节点之间的状态差值，并从所述新的因子图中选取状态差值大于预设的状态变化阈值的状态节点；利用所述新的因子图中选取的状态节点进行线性化处理；按照观测时序排列，利用反向链式传播方式，在线性化处理后的因子图中采用在前的状态节点对在后的状态节点进行状态更新；当状态更新的更新量小于预设的状态更新阈值时停止，得到所述车辆对应的各个观测状态信息。6.根据权利要求1所述的车辆定位方法，其特征在于，所述根据各所述观测状态信息，依次对所述车辆的位姿进行传播更新，得到车辆位姿信息包括：确定第一个观测状态信息于所在观测时间对应的位姿信息，并利用在后的观测状态信息和在前的位姿信息，计算在后的观测状态信息于所在观测时间对应的位姿信息，直到计算得到最新观测时间对应的位姿信息时停止，得到车辆位姿信息。7.根据权利要求1-6中任一项所述的车辆定位方法，其特征在于，在所述根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图之后，还包括：利用预置的时间窗口，从所述新的因子图中筛选边缘状态节点，并放置于预置消元序列；执行所述消元序列的消元操作，以将所述新的因子图中的边缘状态节点剔除。8.一种车辆定位装置，其特征在于，所述车辆定位装置包括：获取模块，用于获取车辆实时的传感数据和因子图，其中，所述因子图包含驾驶过程中按观测时序排列的多个第一状态节点；关联模块，用于根据所述传感数据的第一观测时间，对所述传感数据和所述因子图的第一状态节点执行关联操作，得到新的因子图；优化模块，用于利用预设的状态变化条件，对所述新的因子图进行线性化处理和状态增量更新，得到所述车辆对应的各个观测状态信息；定位模块，用于根据各所述观测状态信息，依次对所述车辆的位姿进行传播更新，得到车辆位姿信息，并基于所述车辆位姿信息对所述车辆进行定位。9.一种车辆定位设备，其特征在于，所述车辆定位设备包括：存储器和至少一个处理器，所述存储器中存储有指令；所述至少一个处理器调用所述存储器中的所述指令，以使得所述车辆定位设备执行如权利要求1-7中任意一项所述的车辆定位方法的步骤。10.一种计算机可读存储介质，所述计算机可读存储介质上存储有指令，其特征在于，所述指令被处理器执行时实现如权利要求1-7中任意一项所述车辆定位方法的步骤。
技术总结本发明涉及自动驾驶控制领域，公开了一种车辆定位方法、装置、设备及存储介质。获取车辆实时的传感数据和因子图，其中，因子图包含驾驶过程中按观测时序排列的多个第一状态节点；根据传感数据的第一观测时间，对传感数据和因子图的第一状态节点执行关联操作，得到新的因子图；利用预设的状态变化条件，对新的因子图进行线性化处理和状态增量更新，得到车辆对应的各个观测状态信息；根据各观测状态信息，依次对车辆的位姿进行传播更新，得到车辆位姿信息，并基于车辆位姿信息对车辆进行定位。本发明实现了对不同类型传感数据的时间对其，提升了车辆定位的精准程度。了车辆定位的精准程度。了车辆定位的精准程度。
技术研发人员：柯超凡 韩旭
受保护的技术使用者：广州文远知行科技有限公司
技术研发日：2022.07.20
技术公布日：2022/11/1
