[发明专利]惯性测量单元和全球导航系统之间杆臂的标定方法和装置

权利要求书

1.一种惯性测量单元和全球导航系统之间杆臂的标定方法，其特征在于，包括：

在GNSS的采样时刻，将IMU的运动状态信息作为预设滑动窗口中待优化的关键帧中的数据，所述运动状态信息包括位置、姿态和速度；

利用所述关键帧中的数据，以及在相邻关键帧之间所获取的IMU的多帧采样数据的增量，构建相邻关键帧之间包含有IMU运动状态增量残差的量测方程，所述量测方程中还包括待优化的IMU和GNSS之间的杆臂值和IMU在车体坐标系下的安装角；

基于采样得到的GNSS数据，并根据基于预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对所述量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值和安装角；其中，所述约束关系用于在新的一帧GNSS数据到来时，确定除第一个关键帧之外的其他关键帧，以形成预设滑动窗口中新的关键帧。

2.根据权利要求1所述的方法，其特征在于，所述相邻关键帧之间的约束关系通过如下方式建立：

将所述量测方程对所述预设滑动窗口中第一个关键帧中IMU的运动状态信息，以及除第一个关键帧之外的其他关键帧中IMU的运动状态信息进行分别求导，分别将得到的雅克比矩阵作为所述第一个关键帧中IMU的运动状态信息和所述其他关键帧中IMU的运动状态信息对应的系数矩阵；

根据所述系数矩阵及其对应关键帧中IMU的运动状态信息，以及所述IMU运动状态增量残差，建立观测方程；

在所述预设滑动窗口进行滑动的过程中，当接收到新的GNSS采样数据时，在所述观测方程中，利用消元法消除所述预设滑动窗口中的第一个关键帧中IMU的运动状态信息，得到相邻关键帧之间的约束关系。

3.根据权利要求2所述的方法，其特征在于，根据所述系数矩阵及其对应关键帧中IMU的运动状态信息，以及所述IMU运动状态增量残差，建立观测方程，包括：

按照如下公式建立观测方程：

其中，x₁为预设滑动窗口中第一个关键帧中IMU的运动状态信息；x₂为预设滑动窗口中所述其他关键帧中IMU的运动状态信息；A为所述量测方程对x₁求导得到的雅克比矩阵；B为所述量测方程对x₂求导得到的雅克比矩阵；C为所述量测方程对x₁求导得到的雅克比矩阵；和D为所述量测方程对x₂求导得到的雅克比矩阵；a为第一个关键帧中IMU的运动状态增量残差；b为其他关键帧中IMU的运动状态信息增量残差。

4.根据权利要求2或3所述的方法，其特征在于，基于采样得到的GNSS数据，并根据基于预设滑动窗口的优化算法，以及相邻关键帧之间的约束关系，对所述量测方程中待优化的数据进行优化处理，包括：

所述预设滑动窗口中的各个关键帧采用先进先出的方式，当每采样得到新的一帧GNSS数据时，根据所述相邻关键帧之间的约束关系，得到包含有IMU运动状态信息的其他关键帧，所述其他关键帧为所述预设滑动窗口中除第一个关键帧之外的其他关键帧；

将所述其他关键帧，以及新的一帧GNSS数据到来时所确定的关键帧作为预设滑动窗口中新的关键帧；

利用所述新的关键帧更新所述量测方程，并采用非线性优化算法对更新后的量测方程中的杆臂值和安装角进行优化。

5.根据权利要求1所述的方法，其特征在于，对所述量测方程中待优化的数据进行优化处理，得到满足预设收敛条件的目标杆臂值，包括：

采用迭代方式进行，对所述量测方程中待优化的数据进行优化，将每次迭代得到的杆臂值作为下次迭代的杆臂初值，直到相邻两次迭代所得到的杆臂值之间的误差小于预设长度阈值时，将相邻两次迭代过程中最后一次迭代所得到的杆臂值作为目标杆臂值。