CN116793343A - 车辆姿态初始化方法、装置、自动驾驶车辆及存储介质 - Google Patents

Abstract

本申请涉及一种车辆姿态初始化方法、装置、自动驾驶车辆和存储介质。该方法通过利用前次初始化时的历史IMU零偏和历史GNSS航向安装角，能够提高初始侧倾角和初始俯仰角的初始化精度，进一步地，在初始化完成后进行多传感器融合定位，获得当次融合定位确定的IMU零偏和历史GNSS航向安装角并存储，能够实现历史IMU零偏和历史GNSS航向安装角的闭环处理。并且在确定车辆为静止状态时，直接根据IMU测量数据、GNSS测量数据、历史IMU零偏和历史GNSS航向安装角，计算初始侧倾角、初始俯仰角，以及初始航向角，得到车辆姿态初始化结果，而不需要利用侧倾角和俯仰角再计算方差来判断车辆是否为静止状态，能够通过预先判断车辆运行状态减少车辆姿态初始化过程中计算量。

Description

车辆姿态初始化方法、装置、自动驾驶车辆及存储介质

技术领域

本申请涉及自动驾驶技术领域，特别是涉及一种车辆姿态初始化方法、装置、自动驾驶车辆和存储介质。

背景技术

随着自动驾驶技术的发展，出现了车辆姿态初始化技术，车辆姿态初始化包括对侧倾角、俯仰角以及航向角的初始化。

传统技术中，采用的车辆姿态初始化方法为，在车辆静止时通过惯性测量单元输出的加速度和角速度来初始化侧倾角和俯仰角，在车辆运动时利用惯性测量单元的数据和GNSS(Global Navigation Satellite System，全球导航卫星系统)传感器速度来确定航向角。

然而，传统方法，由于需要在车辆静止状态下初始化侧倾角和俯仰角，在计算得到侧倾角和俯仰角后，还需要通过计算侧倾角和俯仰角在初始化对应的数据采集时间段内的方差来判断车辆是否确实为静止状态，存在计算量大的问题。

发明内容

基于此，有必要针对上述技术问题，提供一种能够减小车辆姿态初始化过程中计算量的车辆姿态初始化方法、装置、自动驾驶车辆、计算机可读存储介质和计算机程序产品。

第一方面，本申请提供了一种车辆姿态初始化方法。所述方法包括：

获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角；

在确定车辆为静止状态时，获取车辆姿态初始化时段内每个采集时刻采集的IMU测量数据以及GNSS测量数据；

根据所述IMU测量数据和所述历史IMU零偏，获取初始侧倾角和初始俯仰角；

根据所述GNSS测量数据和历史GNSS航向安装角，获取初始航向角；

利用所述初始侧倾角、初始俯仰角、初始航向角、实时IMU测量数据和实时GNSS测量数据进行融合定位，得到IMU零偏及其不确定度，以及GNSS航向安装角及其不确定度；

在所述IMU零偏和GNSS航向安装角的不确定度稳定时，存储所述IMU零偏和GNSS航向安装角。

在其中一个实施例中，所述历史IMU零偏包括历史IMU的角速度零偏；

确定所述车辆是否为静止状态的方式，包括：

获取车辆本次上电后IMU测量的上电IMU角速度、GNSS测量的上电GNSS速度以及里程计速度；

根据所述历史IMU的角速度零偏，对所述上电IMU角速度进行修正，得到上电IMU角速度修正值；

根据所述上电IMU角速度修正值、上电GNSS速度和里程计速度确定所述车辆是否为静止状态。

在一个实施例中，所述根据所述上电IMU角速度修正值、上电GNSS速度和里程计速度确定所述车辆是否为静止状态，包括：

根据所述上电IMU角速度修正值，计算第一静止判断速度；

当所述第一静止判断速度小于预设第一阈值、所述上电GNSS速度小于预设第二阈值且所述里程计速度小于预设第三阈值时，确定车辆为静止状态。

在其中一个实施例中，所述IMU测量数据包括初始化IMU角速度和初始化IMU加速度；所述历史IMU零偏包括历史IMU的角速度零偏和历史IMU的加速度零偏；

所述根据所述IMU测量数据和历史所述IMU零偏，获取初始侧倾角和初始俯仰角，包括：

根据所述历史IMU的角速度零偏，对所述初始化IMU角速度进行修正，得到初始化角速度修正值，并根据所述历史IMU的加速度零偏，对所述初始化IMU加速度，得到初始化加速度修正值；

根据所述初始化角速度修正值以及所述初始化加速度修正值，采用双矢量对准算法，计算初始侧倾角和初始俯仰角。

在其中一个实施例中，所述根据所述初始化角速度修正值以及所述初始化加速度修正值，采用双矢量对准算法，计算初始侧倾角和初始俯仰角，包括：

根据所述初始化角速度修正值，确定所述车辆姿态初始化时段内的角速度均值，并根据所述初始化加速度修正值，确定所述车辆姿态初始化时段内的加速度均值；

根据所述角速度均值和所述加速度均值，采用双矢量对准算法，对IMU进行姿态估计，得到姿态估计矩阵；

对所述姿态估计矩阵进行转换，得到初始侧倾角和初始俯仰角。

在其中一个实施例中，所述GNSS测量数据包括初始化GNSS航向安装角，所述根据所述GNSS测量数据和历史GNSS航向安装角，获取初始航向角，包括：

根据所述初始化GNSS航向安装角，确定所述车辆姿态初始化时段内的航向角均值；

计算所述航向角均值与所述历史GNSS航向安装角和的航向角差值；

根据所述航向角差值，得到初始航向角。

在其中一个实施例中，确定所述车辆是否为静止状态的方式，包括：

在车辆姿态初始化时段内每个采集时刻，根据所述历史IMU偏零，对与所述采集时刻对应的初始化角速度进行修正，得到已修正角速度；

根据所述已修正角速度，计算与所述采集时刻对应的第二静止判断速度；

当每个采集时刻对应的第二静止判断速度均小于预设第一阈值、每个采集时刻对应的初始化GNSS速度均小于预设第二阈值且每个采集时刻对应的里程计速度小于预设第三阈值时，确定车辆为静止状态。

第二方面，本申请还提供了一种车辆姿态初始化装置。所述装置包括：

第一采集模块，用于获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角；

第二采集模块，用于在确定车辆为静止状态时，获取车辆姿态初始化时段内每个采集时刻采集的IMU测量数据以及GNSS测量数据；

计算模块，用于根据所述IMU测量数据和所述历史IMU零偏，获取初始侧倾角和初始俯仰角，并根据所述GNSS测量数据和历史GNSS航向安装角，获取初始航向角；

定位模块，用于利用所述初始侧倾角、初始俯仰角、初始航向角、实时IMU测量数据和实时GNSS测量数据进行融合定位，得到IMU零偏及其不确定度，以及GNSS航向安装角及其不确定度；

处理模块，用于在所述IMU零偏和GNSS航向安装角的不确定度稳定时，存储所述IMU零偏和GNSS航向安装角。

第三方面，本申请还提供了一种自动驾驶车辆，包括车辆本体，以及安装在车辆本体上的IMU、GNSS以及车辆控制器，所述车辆控制器包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角；

在确定车辆为静止状态时，获取车辆姿态初始化时段内每个采集时刻采集的IMU测量数据以及GNSS测量数据；

根据所述IMU测量数据和所述历史IMU零偏，获取初始侧倾角和初始俯仰角；

根据所述GNSS测量数据和历史GNSS航向安装角，获取初始航向角；

利用所述初始侧倾角、初始俯仰角、初始航向角、实时IMU测量数据和实时GNSS测量数据进行融合定位，得到IMU零偏及其不确定度，以及GNSS航向安装角及其不确定度；

在所述IMU零偏和GNSS航向安装角的不确定度稳定时，存储所述IMU零偏和GNSS航向安装角。

第四方面，本申请还提供了一种计算机可读存储介质。所述计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：

获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角；

在确定车辆为静止状态时，获取车辆姿态初始化时段内每个采集时刻采集的IMU测量数据以及GNSS测量数据；

根据所述IMU测量数据和所述历史IMU零偏，获取初始侧倾角和初始俯仰角；

根据所述GNSS测量数据和历史GNSS航向安装角，获取初始航向角；

利用所述初始侧倾角、初始俯仰角、初始航向角、实时IMU测量数据和实时GNSS测量数据进行融合定位，得到IMU零偏及其不确定度，以及GNSS航向安装角及其不确定度；

在所述IMU零偏和GNSS航向安装角的不确定度稳定时，存储所述IMU零偏和GNSS航向安装角。

第五方面，本申请还提供了一种计算机程序产品。所述计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现以下步骤：

获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角；

在确定车辆为静止状态时，获取车辆姿态初始化时段内每个采集时刻采集的IMU测量数据以及GNSS测量数据；

根据所述IMU测量数据和所述历史IMU零偏，获取初始侧倾角和初始俯仰角；

根据所述GNSS测量数据和历史GNSS航向安装角，获取初始航向角；

利用所述初始侧倾角、初始俯仰角、初始航向角、实时IMU测量数据和实时GNSS测量数据进行融合定位，得到IMU零偏及其不确定度，以及GNSS航向安装角及其不确定度；

在所述IMU零偏和GNSS航向安装角的不确定度稳定时，存储所述IMU零偏和GNSS航向安装角。

上述车辆姿态初始化方法、装置、自动驾驶车辆、存储介质和计算机程序产品，获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角，并在车辆为静止状态时，根据IMU测量数据和历史IMU零偏，获取初始侧倾角和初始俯仰角，以及根据GNSS测量数据和历史GNSS航向安装角，获取初始航向角。通过利用前次初始化时的历史IMU零偏和历史GNSS航向安装角，能够提高初始侧倾角和初始俯仰角的初始化精度，进一步地，在初始化完成后进行多传感器融合定位，获得当次融合定位确定的IMU零偏和历史GNSS航向安装角并存储，能够实现历史IMU零偏和历史GNSS航向安装角的闭环处理。并且在确定车辆为静止状态时，直接根据IMU测量数据、GNSS测量数据、历史IMU零偏和历史GNSS航向安装角，计算初始侧倾角、初始俯仰角，以及初始航向角，得到车辆姿态初始化结果，而不需要利用侧倾角和俯仰角再计算方差来判断车辆是否为静止状态，能够通过预先判断车辆运行状态减少车辆姿态初始化过程中计算量。

附图说明

图1为一个实施例中车辆姿态初始化方法的应用环境示意图；

图2为一个实施例中车辆姿态初始化方法的流程示意图；

图3为一个实施例中确定所述车辆是否为静止状态的方式的步骤的流程示意图；

图4为一个实施例中判断车辆是否静止的示意图；

图5为另一个实施例中车辆姿态初始化方法的流程示意图；

图6为一个实施例中车辆姿态初始化装置的结构框图；

图7为一个实施例中计算机设备的内部结构图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

在一个实施例中，如图1所示，提供了一种自动驾驶车辆，所述自动驾驶车辆包括车辆本体10，以及车装在车辆本体10上的惯性测量单元101(Inertial measurement unit，简称IMU)、GNSS102(Global Navigation Satellite System,全球导航卫星系统)以及车辆控制器103。如图1所示，惯性测量单元101、GNSS102均与车辆控制器103连接，车辆控制器103与服务器20通信连接。一种车辆姿态初始化方法，可以应用于服务器20，也可以应用于车辆控制器103，服务器可以用独立的服务器或者是多个服务器组成的服务器集群来实现。

一个实施例中，车辆姿态初始化方法包括以下步骤：

步骤202，获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角。

其中，自动驾驶车辆的IMU，是测量物体三轴姿态角(或角速率)以及加速度的装置。自动驾驶车辆的GNSS,是全球导航卫星系统。自动驾驶车辆上电后，需要对车辆姿态进行初始化，以为车辆提供准备的角度和航向信息。

其中，IMU零偏是指IMU(惯性测量单元)的零点偏置，历史IMU零偏即车辆在前次状态初始化时的IMU的零点偏置，包括加速度零偏和角速度零偏，其中加速度零偏是指加速度计零偏，角速度零偏是指陀螺仪零偏。

GNSS航向安装角是指全球导航卫星系统和车辆纵轴之间的角度误差，及历史GNSS航向安装角即车辆在前次状态初始化时的GNSS航向安装角，车辆纵轴是指车辆坐标系的纵轴方向。

步骤204，在确定车辆为静止状态时，获取车辆姿态初始化时段内每个采集时刻采集的IMU测量数据以及GNSS测量数据。

其中，车辆姿态初始化时段是指对车辆姿态进行初始化的时段。比如，车辆姿态初始化时段具体可以是指采集姿态初始化所需要的数据的一段时间，举例说明，车辆姿态初始化时段具体可以为两分钟。采集时刻是指在车辆姿态初始化时段内用于采集数据的时刻，可按照需要自行设置。比如，可以通过预先设置的采集间隔和车辆姿态初始化时段来确定采集时刻。

IMU测量数据指在车辆姿态初始化时段内每个采集时刻惯性测量单元的采集数据，可以包括车辆姿态初始化时段内每个采集时刻惯性测量单元的陀螺仪所输出的角速度信息，包括三个轴向的角速度。还可以包括加速度是指在车辆姿态初始化时段内每个采集时刻惯性测量单元的加速度计所输出的加速度信息，包括三个轴向的加速度。

GNSS测量数据是指在车辆姿态初始化时段内每个采集时刻车辆上的GNSS传感器输出数据，可以包括GNSS速度，GNSS速度是指在车辆姿态初始化时段内每个采集时刻车辆上的GNSS传感器输出的GNSS天线的速度，具体包括东向速度以及北向速度。

其中，全球导航卫星系统包括主天线和从天线，主天线用于位置测量、速度测量和航迹角测量，从天线用于航向角测量，双天线安装位置的连线(即从天线到主天线的矢量)与正东方向的夹角即为GNSS双天线航向角，GNSS双天线航向角可通过全球导航卫星系统测量获得，其原理可以理解为主天线和从天线分别测量得到一个经纬度位置，两个位置的维度差值除经度差值即为GNSS双天线航向角的正切值。

其中，车辆姿态初始化需要在车辆静止时才可以进行。

步骤206，根据IMU测量数据和历史IMU零偏，获取初始侧倾角和初始俯仰角。

其中，初始侧倾角表示车辆载体绕坐标轴X轴旋转的角度。初始俯仰角表示车辆载体绕坐标轴Y轴旋转的角度。初始航向角表示车辆载体绕坐标轴Z轴旋转的角度。这里的坐标轴具体可以为世界坐标系对应的坐标轴。

具体的，由于需要保证在进行姿态初始化时，车辆一直处于静止状态，服务器会根据在车辆姿态初始化时段内每个采集时刻采集IMU测量数据以及历史IMU零偏，去计算初始侧倾角和初始俯仰角。

由于IMU存在加速度零偏和角速度零偏，相较于利用IMU测量数据以及本次初始化得到的IMU零偏而言，由于历史IMU零偏为车辆在前次融合定位确定的历史IMU零偏，而车辆前后两次上电的IMU偏应当是接近的，故而利用车辆前次初始化时确定的历史IMU零偏和测量数量，能够提高初始侧倾角和初始俯仰角的初始化精度。

步骤208，根据GNSS测量数据和历史GNSS航向安装角，获取初始航向角。

由于航向安装角表示的是全球导航卫星系统和车辆纵轴之间的角度误差，可以通过GNSS测量数据和历史GNSS航向安装角，得到初始航向角。

为在静止期间初始化航向，本实施例预先记录上次定位算法估计出的双天线航向与IMU之间的航向安装角，并利用车辆静止期间的双天线航向观测数据和记录的航向安装角，来初始化车辆航向。

其中，在得到初始侧倾角、初始俯仰角和初始航向角后完成了车辆姿态初始化。

步骤210，利用初始侧倾角、初始俯仰角、初始航向角、实时IMU测量数据和实时GNSS测量数据进行融合定位，得到IMU零偏及其不确定度，以及GNSS航向安装角及其不确定度。

在完成车辆姿态初始化后，基于初始化得到的初始侧倾角、初始俯仰角、初始航向角，以及实时IMU测量数据和实时GNSS测量数据进行融合定位。其中，融合定位算法可以采用卡尔曼滤波算法，融合IMU和GNSS的测量数据，可以得到位置、以及IMU零偏及其不确定度，以及GNSS航向安装角及其不确定度。

其中，IMU零偏的不确定度可以表示IMU零偏的确定性，不确定度越大，IMU零偏的确定性就越小。类似的，GNSS航向安装角的不确定度可以表示GNSS航向安装角的确定性，不确定度越大，GNSS航向安装角的确定性就越小。

步骤212，在IMU零偏和GNSS航向安装角的不确定度稳定时，存储IMU零偏和GNSS航向安装角。

具体地，在IMU零偏和双天线航向安装角的不确定度稳定时，可以表示IMU零偏和双天线航向安装角的预测趋向稳定，确定度增加，此时，将不确定度稳定时的IMU零偏和GNSS航向安装角，用于下次车辆姿态初始化。

上述车辆姿态初始化方法，获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角，并在车辆为静止状态时，根据IMU测量数据和历史IMU零偏，获取初始侧倾角和初始俯仰角，以及根据GNSS测量数据和历史GNSS航向安装角，获取初始航向角。通过利用前次初始化时的历史IMU零偏和历史GNSS航向安装角，能够提高初始侧倾角和初始俯仰角的初始化精度，进一步地，在初始化完成后进行多传感器融合定位，获得当次融合定位确定的IMU零偏和历史GNSS航向安装角并存储，能够实现历史IMU零偏和历史GNSS航向安装角的闭环处理。并且在确定车辆为静止状态时，直接根据IMU测量数据、GNSS测量数据、历史IMU零偏和历史GNSS航向安装角，计算初始侧倾角、初始俯仰角，以及初始航向角，得到车辆姿态初始化结果，而不需要利用侧倾角和俯仰角再计算方差来判断车辆是否为静止状态，能够通过预先判断车辆运行状态减少车辆姿态初始化过程中计算量。

在另一个实施例中，IMU零偏包括IMU的角速度零偏。

其中，确定车辆是否为静止状态的方式，如图3所示，包括：

步骤302，获取车辆本次上电后IMU测量的上电IMU角速度、GNSS测量的上电GNSS速度以及里程计速度。

其中，上电IMU角速度是把车辆上电后IMU测量的角速度。上电GNSS速度是指车辆上电后GNSS测量的速度。里程计速度是指车辆上电后里程计测量的速度。

步骤304，根据历史IMU的角速度零偏，对上电IMU角速度进行修正，得到上电IMU角速度修正值。

由于IMU存在加速度零偏和角速度零偏，直接利用IMU测量数据，如角速度判断车辆是否为静止状态会存在误差。为提高静止状态的计算精度，本实施例中，利用IMU的角速度零偏，对上电IMU角速度进行修正，得到上电IMU角速度修正值。由于IMU的角速度零偏为车辆在前次融合定位确定的历史IMU的角速度零偏，而车辆前后两次上电的IMU的角速度零偏应当是接近的，故而利用车辆前次初始化时确定的历史IMU的角速度零偏对上次上电的IMU角速度进行修正，能够提高IMU角速度的精确度。

步骤306，根据上电IMU角速度修正值、上电GNSS速度和里程计速度确定车辆是否为静止状态。

本实施例中，从上电IMU角速度、GNSS速度和里程计速度这三个维度判断车辆是否为静止状态，相较于单一维度的判断原则，能够提高判断的准确度。

本实施例中，通过IMU角速度的精确度的提高，以及多维度的判断原则，能够提高静止状态判断的准确度。

在一个实施例中，根据上电IMU角速度修正值、上电GNSS速度和里程计速度确定车辆是否为静止状态，包括：根据上电IMU角速度修正值，计算第一静止判断速度；当第一静止判断速度小于预设第一阈值、上电GNSS速度小于预设第二阈值且里程计速度小于预设第三阈值时，确定车辆为静止状态具体的，在车辆状态判断时段内包括多个数据采集时刻，服务器会根据上电IMU角速度修正值和预设静止判断速度计算公式，计算第一静止判断速度，比对第一静止判断速度与预设第一阈值，比对上电GNSS传感器速度和预设第二阈值，比对里程计速度和预设第三阈值，当第一静止判断速度小于预设第一阈值、上电GNSS传感器速度小于预设第二阈值且里程计于预设第三阈值时，确定车辆为静止状态。其中，第一阈值、第二阈值和第三阈值可按照需要自行设置。其中，静止判断速度计算公式可按照需要自行设置。

举例说明，判断车辆是否为静止状态的示意图可以如图4所示，可以利用历史IMU(Inertial Measurement Unit，惯性测量单元)的角速度零偏对角速度进行零偏修正，在零偏修正后利用修正后角速度、GNSS速度和时程计速度，判断车辆是否静止。

本实施例中，通过根据历史角速度零偏，对上电角速度进行修正，能够上电角速度的误差，得到准确的上电角速度修正值，通过根据上电角速度修正值，计算第一静止判断速度，能够利用第一静止判断速度、上电GNSS传感器速度以及里程计速度实现对车辆运行状态的判断，从而可以在车辆静止时开始车辆姿态初始化。

在一个实施例中，根据上电IMU角速度修正值，计算第一静止判断速度包括：根据x轴方向上的上电IMU角速度修正值、y轴方向上的上电IMU角速度修正值以及z轴方向上的上电IMU角速度修正值来计算第一静止判断速度。

具体的，服务器会根据每个轴向上的上电IMU角速度修正值以及预设静止判断速度计算公式，计算第一静止判断速度，其中，预设静止判断速度计算公式为：

其中，N为车辆状态判断时段内的数据采集总次数，k为与数据采集时刻对应的第k次数据采集，w_i，k为数据采集时刻对应的上电IMU角速度修正值，x表示为x轴方向的角速度，y表示为y轴方向的角速度，z表示为z轴方向的角速度。

本实施例中，通过利用上电IMU角速度修正值以及预设静止判断速度计算公式，计算上电IMU角速度修正值，能够实现对上电IMU角速度修正值的准确计算，进而可以利用上电IMU角速度修正值实现对车辆状态的准确判断。

在一个实施例中，IMU测量数据包括初始化IMU角速度和初始化IMU加速度；历史IMU零偏包括历史IMU的角速度零偏和历史IMU的加速度零偏。

其中，根据IMU测量数据和历史IMU零偏，获取初始侧倾角和初始俯仰角，包括：根据历史IMU的角速度零偏，对始化IMU角速度进行修正，得到初始化角速度修正值，并根据历史IMU的加速度零偏，对初始化IMU加速度，得到初始化加速度修正值；根据初始化角速度修正值以及初始化加速度修正值，采用双矢量对准算法，计算初始侧倾角和初始俯仰角。

其中，双矢量对准算法是指通过两对不共线的矢量来求解出两坐标系之间姿态转换矩阵，也叫双矢量定姿算法，主要用于初始姿态确定。

具体的，服务器会根据历史IMU的角速度零偏，对每个采集时刻的初始化IMU角速度进行修正，得到初始化角速度修正值，并根据历史IMU加速度零偏，对每个采集时刻的初始化IMU加速度进行修正，得到初始化加速度修正值。在得到初始化角速度修正值以及初始化加速度修正值后可以根据初始化角速度修正值，计算车辆姿态初始化时间段内的角速度均值，并根据初始化加速度修正值，计算车辆姿态初始化时间段内的加速度均值，以根据角速度均值和加速度均值，采用双矢量对准算法，对惯性测量单元进行姿态估计，得到姿态估计矩阵，对姿态估计矩阵进行转换，得到初始侧倾角和初始俯仰角。

本实施例中，通过根据历史IMU的角速度零偏，对初始化IMU角速度进行修正，能够减小角速度的误差，得到准确的初始化角速度修正值，通过根据历史IMU的加速度零偏，对初始化IMU加速度进行修正，能够减小加速度的误差，得到初始化加速度修正值，从而可以利用初始化角速度修正值以及初始化加速度修正值，采用双矢量对准算法，计算初始侧倾角和初始俯仰角，提高初始侧倾角和初始俯仰角的精度。

在一个实施例中，根据初始化角速度修正值以及初始化加速度修正值，采用双矢量对准算法，计算初始侧倾角和初始俯仰角，包括：根据初始化角速度修正值，确定车辆姿态初始化时段内的角速度均值，并根据初始化加速度修正值，确定车辆姿态初始化时段内的加速度均值；根据角速度均值和加速度均值，采用双矢量对准算法，对IMU进行姿态估计，得到姿态估计矩阵；对姿态估计矩阵进行转换，得到初始侧倾角和初始俯仰角。

具体的，可以根据初始化角速度修正值，计算车辆姿态初始化时段内的角速度均值，并根据初始化加速度修正值，计算车辆姿态初始化时段内的加速度均值，同时，可以获取本地重力加速度矢量以及地球角速度在东北天方向的矢量，通过角速度均值、加速度均值、本地重力加速度矢量以及地球角速度在东北天方向的矢量，采用双矢量对准算法，对惯性测量单元进行姿态估计，得到姿态估计矩阵，根据侧倾角以及俯仰角与姿态估计矩阵之间可以相互转换的关系，利用转换公式对姿态估计矩阵进行转换，得到初始侧倾角和初始俯仰角。其中，地球角速度在东北天方向的矢量可根据地球角速度以及本地纬度得到，具体的计算公式可以为：地球角速度在东北天方向的矢量＝(0，w*cos(L)，w*sin(L))，其中w为地球角速度，L为本地维度。

其中，双矢量对准算法所利用的公式可以为：

其中，为姿态估计矩阵，gⁿ为本地重力加速度矢量，/>为地球角速度在东北天方向的矢量，/>为加速度均值，/>为角速度均值。

其中，转换公式具体可以为：

其中，pitch为俯仰角，roll为侧倾角。需要说明的是，利用转换公式对姿态估计矩阵进行转换，也可以得到一个转换后的航向角，但是由于此转换后的航向角误差较大，本申请中，采用另外的方式去计算初始航向角。

本实施例中，通过根据初始化角速度修正值，确定角速度均值，并根据初始化加速度修正值，确定加速度均值，能够根据角速度均值和加速度均值，采用双矢量对准算法，实现对惯性测量单元的准确姿态估计，得到姿态估计矩阵，进而可以通过对姿态估计矩阵进行转换，实现对初始侧倾角和初始俯仰角的计算。

在一个实施例中，GNSS测量数据包括初始化GNSS航向安装角。

GNSS测量数据包括初始化GNSS航向安装角，根据GNSS测量数据和历史GNSS航向安装角，获取初始航向角，包括：根据初始化GNSS航向安装角，确定车辆姿态初始化时段内的航向角均值；计算航向角均值与历史GNSS航向安装角和的航向角差值；根据航向角差值，得到初始航向角。

具体的，服务器会先根据每个采集时刻的GNSS双天线航向角，去计算车辆姿态初始化时段内的航向角均值，再计算航向角均值与历史航向安装角的航向角差值，将航向角差值作为初始航向角，以实现在静止状态下对初始航向角的估计。

本实施例中，通过根据GNSS双天线航向角，确定航向角均值，计算航向角均值与历史航向安装角的航向角差值，将航向角差值作为初始航向角，能够在静止状态下实现对初始航向角的估计。

在一个实施例中，确定车辆是否为静止状态的方式，还可以包括：在车辆姿态初始化时段内每个采集时刻，根据历史IMU偏零，对与采集时刻对应的初始化角速度进行修正，得到已修正角速度；根据已修正角速度，计算与采集时刻对应的第二静止判断速度；当每个采集时刻对应的第二静止判断速度均小于预设第一阈值、每个采集时刻对应的初始化GNSS速度均小于预设第二阈值且每个采集时刻对应的里程计速度小于预设第三阈值时，确定车辆为静止状态具体的，在车辆姿态初始化时段内每个采集时刻，服务器都需要对车辆是否为静止状态进行判断，此时，服务器会在每个采集时刻，根据历史IMU偏零，对与采集时刻对应的初始化角速度进行修正，得到已修正角速度，根据已修正角速度，和与在采集时刻之前的已采集时刻对应的已修正角速度，计算与采集时刻对应的第二静止判断速度，比对第二静止判断速度与预设第一阈值，并比对初始化GNSS速度和预设第二阈值，以及每个采集时刻对应的里程计速度和预设第三阈值，当每个采集时刻对应的第二静止判断速度均小于预设第一阈值、每个采集时刻对应的初始化GNSS速度均小于预设第二阈值，且每个采集时刻对应的里程计速度小于预设第三阈值时，表示在车辆姿态初始化时段内，车辆均为静止状态。

其中，第二静止判断速度的计算公式可按照需要自行设置，举例说明，第二静止判断速度的计算公式具体可以为：

其中，N表示与采集时刻对应的已采集次数，k为与采集时刻或在采集时刻之前的已采集时刻对应的第k次采集，w_i，k为采集时刻或在采集时刻之前的已采集时刻对应的已修正角速度，x表示为x轴方向的角速度，y表示为y轴方向的角速度，z表示为z轴方向的角速度。

本实施例中，通过在每个采集时刻，根据角速度零偏，对与采集时刻对应的初始化角速度进行修正，能够减小初始化角速度的误差，得到准确的已修正角速度，从而可以根据已修正角速度，计算与采集时刻对应的第二静止判断速度，利用每个采集时刻对应的第二静止判断速度、初始化GNSS传感器速度以及里程计速度实现对车辆姿态初始化时段内车辆运行状态的判断。

在一个实施例中，当存在任一采集时刻对应的第二静止判断速度大于或者等于预设第一阈值和/或初始化GNSS传感器速度大于或者等于预设第二阈值，表示车辆不为静止状态，服务器会确定车辆为非静止状态，在确定车辆为非静止状态时，表示在车辆姿态初始化时段内所获取到的数据不为车辆静止时所获取到的，不可以用于车辆姿态初始化，服务器会清除掉在车辆姿态初始化时段内所获取到的数据，跳转至获取车辆本次上电后IMU测量的上电IMU角速度、GNSS测量的上电GNSS速度以及里程计速度，当确定车辆为静止状态时为止，再次开始车辆姿态初始化。

本实施例中，通过在确定车辆为非静止状态时，跳转至车辆状态判断时段内每个采集时刻采集角速度以及GNSS传感器速度，重新开始进行对车辆运行状态进行判断，在车辆为静止状态时才重新开始车辆姿态初始化，能够确保车辆姿态初始化在车辆为静止状态时进行，而不需要利用侧倾角和俯仰角再计算方差来判断车辆是否为静止状态，能够通过预先判断车辆运行状态减少车辆姿态初始化过程中计算量。

应该理解的是，虽然如上的各实施例所涉及的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，如上的各实施例所涉及的流程图中的至少一部分步骤可以包括多个步骤或者多个阶段，这些步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。

在一个实施例中，通过一个具体实施例来说明本申请的车辆姿态初始化方法，该车辆姿态初始化方法具体包括以下步骤：

第一步：首先判断车辆是否静止。

具体的，服务器会获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角，并采集车辆本次上电后IMU测量的上电IMU角速度、GNSS测量的上电GNSS速度以及里程计速度，利用历史IMU的角速度零偏，对上电IMU角速度进行修正，得到上电IMU角速度修正值，根据上电IMU角速度修正值，计算第一静止判断速度；当第一静止判断速度小于预设第一阈值、上电GNSS速度小于预设第二阈值且里程计速度小于预设第三阈值时，确定车辆为静止状态。

第二步：当车辆静止时，获取车辆姿态初始化时段内每个采集时刻采集的IMU测量数据，使用双矢量对准算法计算初始侧倾角和初始俯仰角。

具体的，服务器会在车辆姿态初始化时段内(即一段时间内)每个采集时刻采集初始化IMU角速度初始化GNSS传感器速度以及加速度，在车辆姿态初始化时段内每个采集时刻，根据历史IMU偏零，对与采集时刻对应的角速度进行修正，得到已修正角速度，根据已修正角速度，计算与采集时刻对应的第二静止判断速度，当每个采集时刻对应的第二静止判断速度均小于预设第一阈值、每个采集时刻对应的初始化GNSS传感器速度均小于预设第二阈值且每个采集时刻对应的里程计速度小于预设第三阈值时，确定车辆为静止状态，根据历史IMU的角速度零偏，对初始化IMU角速度进行修正，得到初始化角速度修正值，并根据历史IMU的加速度零偏，对初始化IMU加速度，得到初始化加速度修正值；根据初始化角速度修正值以及初始化加速度修正值，采用双矢量对准算法，计算初始侧倾角和初始俯仰角。

第三步：通过GNSS双天线航向角和存储的航向安装角来初始化航向。

具体的，服务器会在车辆姿态初始化时段内(即一段时间内)每个采集时刻采集GNSS双天线航向角，根据GNSS双天线航向角，确定车辆姿态初始化时段内的航向角均值，计算航向角均值与航向安装角的航向角差值，根据航向角差值，得到初始航向角。

需要说明的是，在车辆姿态初始化时段内在进行数据采集时，需要保证车辆是否为静止状态，若判断出车辆为非静止状态，服务器会清除在车辆姿态初始化时段内采集到的数据，返回第一步，重新判断车辆是否静止，直到判断车辆静止后重新开始数据采集，即重新开始车辆姿态初始化。

如图5所示，一种车辆姿态初始化方法，

第一步：首先判断车辆是否静止。

其中，判断车辆是否静止的步骤如图4所示，IMU输出加速度和角速度数据，利用记录的IMU零偏数值分别对其进行修正；使用GNSS速度、里程计速度和修正后IMU数据判断车辆是否静止，判断公式为：

其中，w_i，k为k时刻修正后的IMU的i轴角速度。

当GNSS速度、里程计速度与T同时小于对应阈值时，认为车辆静止，开始计算车辆初始姿态。

第二步：当车辆静止后，采集一段时间(如2秒)内的IMU数据，使用双矢量对准算法计算侧倾角和俯仰角。双矢量对准算法如下：

其中，为估计的IMU姿态，gⁿ为当地的重力加速度矢量，/>为地球角速度在东北天方向的矢量，/>为修正后的IMU加速度在采集时间内的均值，/>为修正后的IMU角速度在采集时间内的均值。

第三步：航向通过GNSS双天线航向和存储的航向安装角来进行初始化。采集车辆静止的一段时间(如2秒)内的GNSS双天线航向，计算均值，并减去航向安装角，获得初始航向。

第四步：在采集时间内，需保证车辆始终为静止状态；若判断为车辆运动，则清除前面两步内采集的IMU和GNSS双天线航向数据，直到判断车辆静止后重新开始采集。

第五步：在初始化完成后，多传感器融合定位算法会在车辆运动过程中估计IMU的加速度零偏、角速度零偏和双天线航向安装角。若这三类状态的估计稳定收敛，则将估计的数值进行存储，用于下一次的车辆姿态初始化。其中，稳定收敛有两个条件：状态的不确定度小于阈值且状态在一定时间内的变化小于阈值。

该车辆姿态初始化方法，提高姿态的初始化精度，减小计算量，保证在静止时完成姿态初始化。

基于同样的发明构思，本申请实施例还提供了一种用于实现上述所涉及的车辆姿态初始化方法的车辆姿态初始化装置。该装置所提供的解决问题的实现方案与上述方法中所记载的实现方案相似，故下面所提供的一个或多个车辆姿态初始化装置实施例中的具体限定可以参见上文中对于车辆姿态初始化方法的限定，在此不再赘述。

在一个实施例中，如图6所示，提供了一种车辆姿态初始化装置，包括：

第一采集模块602，用于获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角；

第二采集模块604，用于在确定车辆为静止状态时，获取车辆姿态初始化时段内每个采集时刻采集的IMU测量数据以及GNSS测量数据；

计算模块606，用于根据所述IMU测量数据和所述历史IMU零偏，获取初始侧倾角和初始俯仰角，并根据所述GNSS测量数据和历史GNSS航向安装角，获取初始航向角；

定位模块608，用于利用所述初始侧倾角、初始俯仰角、初始航向角、实时IMU测量数据和实时GNSS测量数据进行融合定位，得到IMU零偏及其不确定度，以及GNSS航向安装角及其不确定度；

处理模块610，用于在所述IMU零偏和GNSS航向安装角的不确定度稳定时，存储所述IMU零偏和GNSS航向安装角。

上述车辆姿态初始化装置，获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角，并在车辆为静止状态时，根据IMU测量数据和历史IMU零偏，获取初始侧倾角和初始俯仰角，以及根据GNSS测量数据和历史GNSS航向安装角，获取初始航向角。通过利用前次初始化时的历史IMU零偏和历史GNSS航向安装角，能够提高初始侧倾角和初始俯仰角的初始化精度，进一步地，在初始化完成后进行多传感器融合定位，获得当次融合定位确定的IMU零偏和历史GNSS航向安装角并存储，能够实现历史IMU零偏和历史GNSS航向安装角的闭环处理。并且在确定车辆为静止状态时，直接根据IMU测量数据、GNSS测量数据、历史IMU零偏和历史GNSS航向安装角，计算初始侧倾角、初始俯仰角，以及初始航向角，得到车辆姿态初始化结果，而不需要利用侧倾角和俯仰角再计算方差来判断车辆是否为静止状态，能够通过预先判断车辆运行状态减少车辆姿态初始化过程中计算量。

在一个实施例中，所述历史IMU零偏包括历史IMU的角速度零偏；

第二采集模块还用于获取车辆本次上电后IMU测量的上电IMU角速度、GNSS测量的上电GNSS速度以及里程计速度；根据所述历史IMU的角速度零偏，对所述上电IMU角速度进行修正，得到上电IMU角速度修正值；根据所述上电IMU角速度修正值、上电GNSS速度和里程计速度确定所述车辆是否为静止状态。

在一个实施例中，第二采集模块还用于根据所述上电IMU角速度修正值，计算第一静止判断速度；当所述第一静止判断速度小于预设第一阈值、所述上电GNSS速度小于预设第二阈值且所述里程计速度小于预设第三阈值时，确定车辆为静止状态。

在一个实施例中，所述IMU测量数据包括初始化IMU角速度和初始化IMU加速度；所述历史IMU零偏包括历史IMU的角速度零偏和历史IMU的加速度零偏；计算模块还用于根据所述历史IMU的角速度零偏，对所述初始化IMU角速度进行修正，得到初始化角速度修正值，并根据所述历史IMU的加速度零偏，对所述初始化IMU加速度，得到初始化加速度修正值；根据所述初始化角速度修正值以及所述初始化加速度修正值，采用双矢量对准算法，计算初始侧倾角和初始俯仰角。

在一个实施例中，计算模块还用于根据所述初始化角速度修正值，确定所述车辆姿态初始化时段内的角速度均值，并根据所述初始化加速度修正值，确定所述车辆姿态初始化时段内的加速度均值；根据所述角速度均值和所述加速度均值，采用双矢量对准算法，对IMU进行姿态估计，得到姿态估计矩阵；对所述姿态估计矩阵进行转换，得到初始侧倾角和初始俯仰角。

在一个实施例中，所述GNSS测量数据包括初始化GNSS航向安装角，计算模块还用于根据所述初始化GNSS航向安装角，确定所述车辆姿态初始化时段内的航向角均值；计算所述航向角均值与所述历史GNSS航向安装角和的航向角差值；根据所述航向角差值，得到初始航向角。

在一个实施例中，第二采集模块还用于在车辆姿态初始化时段内每个采集时刻，根据所述历史IMU偏零，对与所述采集时刻对应的初始化角速度进行修正，得到已修正角速度；根据所述已修正角速度，计算与所述采集时刻对应的第二静止判断速度；当每个采集时刻对应的第二静止判断速度均小于预设第一阈值、每个采集时刻对应的初始化GNSS速度均小于预设第二阈值且每个采集时刻对应的里程计速度小于预设第三阈值时，确定车辆为静止状态。

上述车辆姿态初始化装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图7所示。该计算机设备包括通过系统总线连接的处理器、存储器和网络接口。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质和内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于存储惯性测量单元零偏、航向安装角等数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种车辆姿态初始化方法。

本领域技术人员可以理解，图7中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现上述各实施例的车辆姿态初始化方法的步骤。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述各实施例的车辆姿态初始化方法的步骤。

在一个实施例中，提供了一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现上述各实施例的车辆姿态初始化方法的步骤。

需要说明的是，上述实施例中所涉及的计算机设备、计算机可读存储介质以及计算机程序产品，均可以部署在智能驾驶车辆计算平台中，以执行本申请中的车辆姿态初始化方法。需要说明的是，本申请所涉及的数据(包括但不限于用于分析的数据、存储的数据、展示的数据等)，均为经用户授权或者经过各方充分授权的信息和数据。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、数据库或其它介质的任何引用，均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-OnlyMemory，ROM)、磁带、软盘、闪存、光存储器、高密度嵌入式非易失性存储器、阻变存储器(ReRAM)、磁变存储器(Magnetoresistive Random Access Memory，MRAM)、铁电存储器(Ferroelectric Random Access Memory，FRAM)、相变存储器(Phase Change Memory，PCM)、石墨烯存储器等。易失性存储器可包括随机存取存储器(Random Access Memory，RAM)或外部高速缓冲存储器等。作为说明而非局限，RAM可以是多种形式，比如静态随机存取存储器(Static Random Access Memory，SRAM)或动态随机存取存储器(Dynamic RandomAccess Memory，DRAM)等。本申请所提供的各实施例中所涉及的数据库可包括关系型数据库和非关系型数据库中至少一种。非关系型数据库可包括基于区块链的分布式数据库等，不限于此。本申请所提供的各实施例中所涉及的处理器可为通用处理器、中央处理器、图形处理器、数字信号处理器、可编程逻辑器、基于量子计算的数据处理逻辑器等，不限于此。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请的保护范围应以所附权利要求为准。

Claims (10)

1.一种车辆姿态初始化方法，其特征在于，所述方法包括：

获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角；

在确定车辆为静止状态时，获取车辆姿态初始化时段内每个采集时刻采集的IMU测量数据以及GNSS测量数据；

根据所述IMU测量数据和所述历史IMU零偏，获取初始侧倾角和初始俯仰角；

根据所述GNSS测量数据和历史GNSS航向安装角，获取初始航向角；

利用所述初始侧倾角、初始俯仰角、初始航向角、实时IMU测量数据和实时GNSS测量数据进行融合定位，得到IMU零偏及其不确定度，以及GNSS航向安装角及其不确定度；

在所述IMU零偏和GNSS航向安装角的不确定度稳定时，存储所述IMU零偏和GNSS航向安装角。

2.根据权利要求1所述的方法，其特征在于，所述历史IMU零偏包括历史IMU的角速度零偏；

确定所述车辆是否为静止状态的方式，包括：

获取车辆本次上电后IMU测量的上电IMU角速度、GNSS测量的上电GNSS速度以及里程计速度；

根据所述历史IMU的角速度零偏，对所述上电IMU角速度进行修正，得到上电IMU角速度修正值；

根据所述上电IMU角速度修正值、上电GNSS速度和里程计速度确定所述车辆是否为静止状态。

3.根据权利要求2所述的方法，其特征在于，所述根据所述上电IMU角速度修正值、上电GNSS速度和里程计速度确定所述车辆是否为静止状态，包括：

根据所述上电IMU角速度修正值，计算第一静止判断速度；

当所述第一静止判断速度小于预设第一阈值、所述上电GNSS速度小于预设第二阈值且所述里程计速度小于预设第三阈值时，确定车辆为静止状态。

4.根据权利要求1所述的方法，其特征在于，所述IMU测量数据包括初始化IMU角速度和初始化IMU加速度；所述历史IMU零偏包括历史IMU的角速度零偏和历史IMU的加速度零偏；

所述根据所述IMU测量数据和历史所述IMU零偏，获取初始侧倾角和初始俯仰角，包括：

根据所述历史IMU的角速度零偏，对所述初始化IMU角速度进行修正，得到初始化角速度修正值，并根据所述历史IMU的加速度零偏，对所述初始化IMU加速度，得到初始化加速度修正值；

根据所述初始化角速度修正值以及所述初始化加速度修正值，采用双矢量对准算法，计算初始侧倾角和初始俯仰角。

5.根据权利要求4所述的方法，其特征在于，所述根据所述初始化角速度修正值以及所述初始化加速度修正值，采用双矢量对准算法，计算初始侧倾角和初始俯仰角，包括：

根据所述初始化角速度修正值，确定所述车辆姿态初始化时段内的角速度均值，并根据所述初始化加速度修正值，确定所述车辆姿态初始化时段内的加速度均值；

根据所述角速度均值和所述加速度均值，采用双矢量对准算法，对IMU进行姿态估计，得到姿态估计矩阵；

对所述姿态估计矩阵进行转换，得到初始侧倾角和初始俯仰角。

6.根据权利要求1所述的方法，其特征在于，所述GNSS测量数据包括初始化GNSS航向安装角，所述根据所述GNSS测量数据和历史GNSS航向安装角，获取初始航向角，包括：

根据所述初始化GNSS航向安装角，确定所述车辆姿态初始化时段内的航向角均值；

计算所述航向角均值与所述历史GNSS航向安装角和的航向角差值；

根据所述航向角差值，得到初始航向角。

7.根据权利要求1所述的方法，其特征在于，确定所述车辆是否为静止状态的方式，包括：

在车辆姿态初始化时段内每个采集时刻，根据所述历史IMU偏零，对与所述采集时刻对应的初始化角速度进行修正，得到已修正角速度；

根据所述已修正角速度，计算与所述采集时刻对应的第二静止判断速度；

当每个采集时刻对应的第二静止判断速度均小于预设第一阈值、每个采集时刻对应的初始化GNSS速度均小于预设第二阈值且每个采集时刻对应的里程计速度小于预设第三阈值时，确定车辆为静止状态。

8.一种车辆姿态初始化装置，其特征在于，所述装置包括：

第一采集模块，用于获取记录的车辆在前次融合定位确定的历史IMU零偏以及历史GNSS航向安装角；

第二采集模块，用于在确定车辆为静止状态时，获取车辆姿态初始化时段内每个采集时刻采集的IMU测量数据以及GNSS测量数据；

计算模块，用于根据所述IMU测量数据和所述历史IMU零偏，获取初始侧倾角和初始俯仰角，并根据所述GNSS测量数据和历史GNSS航向安装角，获取初始航向角；

定位模块，用于利用所述初始侧倾角、初始俯仰角、初始航向角、实时IMU测量数据和实时GNSS测量数据进行融合定位，得到IMU零偏及其不确定度，以及GNSS航向安装角及其不确定度；

处理模块，用于在所述IMU零偏和GNSS航向安装角的不确定度稳定时，存储所述IMU零偏和GNSS航向安装角。

9.一种自动驾驶车辆，其特征在于，包括车辆本体，以及安装在车辆本体上的IMU、GNSS以及车辆控制器，所述车辆控制器包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述的方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。