WO2022007437A1

WO2022007437A1 - 传感器安装偏差角的标定方法、组合定位系统和车辆

Info

Publication number: WO2022007437A1
Application number: PCT/CN2021/083032
Authority: WO
Inventors: 王磊杰; 温丰; 刘镇波; 姜锐
Original assignee: 华为技术有限公司
Priority date: 2020-07-04
Filing date: 2021-03-25
Publication date: 2022-01-13
Also published as: CN113884102A; EP4170282A1; EP4170282A4

Abstract

一种传感器安装偏差角的标定方法、组合定位系统和车辆。其中，组合定位系统包括多个传感器，多个传感器在车辆的安装位置不同；该标定方法包括：根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型；构建系统状态方程，系统状态方程的状态变量包括安装偏差角；构建系统观测方程，系统观测方程的观测量包括根据误差模型确定的安装偏差角对应的观测矩阵；对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的安装偏差角的估计结果作为标定结果，该系统能够对传感器之间的安装偏差角进行在线标定，当传感器之间的安装位置关系发生变化时，能够及时更新标定结果，精度高、操作简单。

Description

传感器安装偏差角的标定方法、组合定位系统和车辆

本申请要求于2020年07月04日提交到国家知识产权局、申请号为202010636730.6、发明名称为“传感器安装偏差角的标定方法、组合定位系统和车辆”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。

技术领域

本申请涉及自动驾驶技术领域，尤其涉及一种传感器安装偏差角的标定方法、组合定位系统和车辆。

背景技术

自动驾驶车辆的定位系统用于确定车辆的位置，为车辆的路径规划和导航提供最基础的信息，定位系统的性能将直接决定自动驾驶车辆的稳定性和可靠性。目前定位系统一般为包含多种传感器的组合定位系统，例如：惯性导航系统(inertial navigation system，INS)使用的惯性测量单元(inertial measurement unit，IMU)、全球卫星导航系统(global navigation satellite system，GNSS)接收机、激光雷达(light detection and ranging，LiDAR)、里程计、视觉传感器等。

组合定位系统中的各个传感器通常安装到车辆的不同位置，因此在使用之前需要对各个传感器之间的安装位置关系标定，例如标定各个传感器之间的安装偏差角，以便于准确地对各个传感器的解算结果或定位结果进行滤波计算，提高定位精度。

目前，各个传感器之间的安装偏差角的标定主要采用离线标定方法，依赖专用的标定工具而使用一些光学手段或者间接标定的方式实现，操作较为繁琐，标定时间较长。当车辆由于长时间振动、颠簸等原因导致各个传感器之间的安装位置关系发生变化时，还需要重新标定，否则会影响组合定位系统的定位精度。

发明内容

本申请实施例提供了一种传感器安装偏差角的标定方法、组合定位系统和车辆，能够实现对传感器安装偏差角的在线标定，当各个传感器之间的安装关系发生变化时，标定结果能够在线更新，从而始终保证组合定位系统的定位精度。

第一方面，本申请实施例提供了一种传感器安装偏差角的标定方法，该方法应用于组合定位系统，组合定位系统包括多个传感器，多个传感器的安装位置不同；该方法包括：根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型；构建系统状态方程，系统状态方程的状态变量包括安装偏差角；构建系统观测方程，系统观测方程的观测量包括根据误差模型确定的安装偏差角对应的观测矩阵；对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的安装偏差角的估计结果作为标定结果。

本申请实施例提供的方法，将各个传感器之间的安装偏差角整合到组合定位系统的状态变量中，因此能够实现在车辆行驶过程中对各个传感器之间的安装偏差角进行标定(即在线标定)，标定过程不需要考虑传感器的物理结构，标定精度高。并且，本申请实施例的方法不需要借助用的标定工具，仅需要车辆产生机动即可完成标定，操作简单、快速。另外，当各个传感器之间的安装位置关系发生变化时，本申请实施例的方法能够在线更新标定结果，从而始终保证组合定位系统的定位精度。

在一种可选择的实现方式中，多个传感器包括惯性导航系统INS的惯性测量单元IMU和全球导航卫星系统GNSS接收机，GNSS接收机包括GNSS天线，IMU和GNSS天线的安装位置不同；根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型，包括：构造误差模型的观测量，观测量为IMU和GNSS天线之间的姿态偏差；构建从INS对应的姿态矩阵到GNSS对应的姿态矩阵的转移关系方程；联立误差模型的观测量和转移关系方程构建误差模型，所述误差模型为所述IMU和GNSS天线之间的安装偏差角的误差模型；其中，姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，转移关系方程包含INS的姿态误差和IMU与GNSS天线之间的安装偏差角。

在一种可选择的实现方式中，误差模型的观测量包括：

其中，γ _m、θ _m、

分别为INS输出的滚转角、俯仰角和航向角；γ _g、θ _g、

分别为GNSS输出的滚转角、俯仰角和航向角。

在一种可选择的实现方式中，转移关系方程包括：

其中，

为INS对应的姿态矩阵，

为GNSS对应的姿态矩阵，

为INS的姿态误差，λ ⁰为IMU与GNSS天线之间的安装偏差角，λ ⁰×表示λ ⁰的叉乘。

在一种可选择的实现方式中，误差模型包括：

其中，Z ₁、Z ₂、Z ₃为误差模型的观测量；

分别为INS基于东-北-天坐标系的E轴姿态误差N轴姿态误差和U轴姿态误差；

分别为IMU与GNSS天线之间基于右前上坐标系的X轴安装偏差角、Y轴安装偏差角和Z轴安装偏差角；其他参数为GNSS对应的姿态矩阵

中的参数。

在一种可选择的实现方式中，系统状态方程包括：

X＝[X ₁，X ₂]

其中，X ₁和X ₂为状态变量，X ₁为惯导误差，X ₂为IMU与GNSS天线之间的安装偏差角；

为X ₁的导数；F ₁为组合定位系统的惯导误差模型的系统矩阵，G ₁、W ₁为惯导误差驱动噪声；

为X ₂的导数；F ₂为IMU与GNSS天线之间的安装偏差角的误差模型的系统矩阵，G ₂、W ₂为IMU与GNSS天线之间的安装偏差角的误差驱动噪声。

在一种可选择的实现方式中，系统状态方程包括：

Z＝HX+V＝[H ₁ H ₂ H ₃]

其中，Z为系统观测量，H为系统观测矩阵，X为系统状态方程，V为系统观测噪声矩阵，H ₁为INS的姿态误差对应的观测矩阵，H ₂为惯导误差模型中除了姿态误差以外的其他状态变量对应的观测矩阵，H ₃为IMU与GNSS天线之间的安装偏差角对应的观测矩阵。

在一种可选择的实现方式中，对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的安装偏差角的最优估计结果作为标定结果，包括：对系统状态方程和系统观测方程进行卡尔曼滤波解算，得到IMU与GNSS天线之间的安装偏差角的最优估计结果；根据最优估计结果对GNSS的定位结果进行补偿，以及对INS的惯导解算结果进行修正；当滤波解算结果收敛时，将IMU与GNSS天线之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。

在一种可选择的实现方式中，多个传感器包括IMU和激光雷达LiDAR，IMU和LiDAR的安装位置不同；根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型，包括：构造误差模型的观测量，观测量为IMU和LiDAR之间的姿态偏差；构建从INS对应的姿态矩阵到LiDAR对应的姿态矩阵的转移关系方程；联立误差模型的观测量和转移关系方程构建误差模型，误差模型为IMU和LiDAR之间的安装偏差角的误差模型；其中，姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，转移关系方程包含INS的姿态误差和IMU与LiDAR之间的安装偏差角。

在一种可选择的实现方式中，误差模型的观测量包括：

其中，γ _m、θ _m、

分别为INS输出的滚转角、俯仰角和航向角；γ _l、θ _l、

分别为LiDAR输出的滚转角、俯仰角和航向角。

在一种可选择的实现方式中，转移关系方程包括：

其中，

为INS对应的姿态矩阵，

为LiDAR对应的姿态矩阵，

为INS的姿态误差，λ ¹为IMU与LiDAR天线之间的安装偏差角，λ ¹×表示λ ¹的叉乘。

在一种可选择的实现方式中，误差模型包括：

其中，Z ₁、Z ₂、Z ₃为误差模型的观测量；

分别为IMU与LiDAR天线之间基于右前上坐标系的X轴安装偏差角、Y轴安装偏差角和Z轴安装偏差角；其他参数为LiDAR对应的姿态矩阵

中的参数。

在一种可选择的实现方式中，系统状态方程包括：

X＝[X ₁，X ₃]

其中，X ₁和X ₃为状态变量，X ₁为惯导误差，X ₃为IMU与LiDAR之间的安装偏差角；

为X ₃的导数；F ₃为IMU与LiDAR之间的安装偏差角的误差模型的系统矩阵，G ₃、W ₃为IMU与LiDAR之间的安装偏差角的误差驱动噪声。

在一种可选择的实现方式中，系统状态方程包括：

Z＝HX+V＝[H ₁ H ₂ H ₃]

其中，Z为系统观测量，H为系统观测矩阵，X为系统状态方程，V为系统观测噪声矩阵，H ₁为INS的姿态误差对应的观测矩阵，H ₂为惯导误差模型中除了姿态误差以外的其他状态变量对应的观测矩阵，H ₃为IMU与LiDAR之间的安装偏差角对应的观测矩阵。

在一种可选择的实现方式中，对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的安装偏差角的最优估计结果作为标定结果，包括：对系统状态方程和系统观测方程进行卡尔曼滤波解算，得到IMU与LiDAR之间的安装偏差角的最优估计结果；根据最优估计结果对LiDAR的定位结果进行补偿，以及对INS的惯导解算结果进行修正；当滤波解算结果收敛时，将IMU与LiDAR之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。

第二方面，本申请实施例提供了一种组合定位系统，该系统包括处理器、存储器和多个传感器，多个传感器的安装位置不同；存储器包括有程序指令，程序指令被处理器运行时，使得组合定位系统用于执行如下步骤：根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型；构建系统状态方程，系统状态方程的状态变量包括安装偏差角；构建系统观测方程，系统观测方程的观测量包括根据误差模型确定的安装偏差角对应的观测矩阵；对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的安装偏差角的估计结果作为标定结果。

本申请实施例提供的组合定位系统，将各个传感器之间的安装偏差角整合到组合定位系统的状态变量中，因此能够实现在车辆行驶过程中对各个传感器之间的安装偏差角进行标定(即在线标定)，标定过程不需要考虑传感器的物理结构，标定精度高。并且，本申请实施例的组合定位系统不需要借助用的标定工具，仅需要车辆产生机动即可完成标定，操作简单、快速。另外，当各个传感器之间的安装位置关系发生变化时，本申请实施例的方法能够在线更新标定结果，从而始终保证组合定位系统的定位精度。

在一种可选择的实现方式中，多个传感器包括惯性导航系统INS的惯性测量单元IMU和全球导航卫星系统GNSS接收机，GNSS接收机包括GNSS天线，IMU和GNSS天线的安装位置不同；程序指令被处理器运行时，使得组合定位系统用于执行如下步骤，以实现根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型：构造误差模型的观测量，观测量为IMU和GNSS天线之间的姿态偏差；构建从INS对应的姿态矩阵到GNSS对应的姿态矩阵的转移关系方程；联立误差模型的观测量和转移关系方程构建误差模型，误差模型为IMU和GNSS天线之间的安装偏差角的误差模型；其中，姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，转移关系方程包含INS的姿态误差和IMU与GNSS天线之间的安装偏差角。

在一种可选择的实现方式中，程序指令被处理器运行时，使得组合定位系统用于执行如下步骤，以实现对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的安装偏差角的最优估计结果作为标定结果：对系统状态方程和系统观测方程进行卡尔曼滤波解算，得到IMU与GNSS天线之间的安装偏差角的最优估计结果；根据最优估计结果对GNSS的定位结果进行补偿，以及对INS的惯导解算结果进行修正；当滤波解算结果收敛时，将IMU与GNSS天线之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。

在一种可选择的实现方式中，多个传感器包括IMU和激光雷达LiDAR，IMU和LiDAR的安装位置不同；程序指令被处理器运行时，使得组合定位系统用于执行如下步骤，以实现根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型：构造误差模型的观测量，观测量为IMU和LiDAR之间的姿态偏差；构建从INS对应的姿态矩阵到LiDAR对应的姿态矩阵的转移关系方程；联立误差模型的观测量和转移关系方程构建误差模型，误差模型为IMU和LiDAR之间的安装偏差角的误差模型；其中，姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，转移关系方程包含INS的姿态误差和IMU与LiDAR之间的安装偏差角。

在一种可选择的实现方式中，程序指令被处理器运行时，使得组合定位系统用于执行如下步骤，以实现对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的安装偏差角的最优估计结果作为标定结果：对系统状态方程和系统观测方程进行卡尔曼滤波解算，得到IMU与LiDAR之间的安装偏差角的最优估计结果；根据最优估计结果对LiDAR的定位结果进行补偿，以及对INS的惯导解算结果进行修正；当滤波解算结果收敛时，将IMU与LiDAR之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。

进一步地，本申请实施例提供的组合定位系统，还可以执行前述第一方面的其他实现方式。

第三方面，本申请实施例提供了一种车辆，该车辆包括组合定位系统，组合定位系统包括处理器、存储器和多个传感器，不同传感器安装在车辆的不同位置；存储器包括有程序指令，程序指令被处理器运行时，使得组合定位系统用于执行前述第一方面及其任意实现方式中的方法。

第四方面，本申请实施例还提供一种计算机可读存储介质，计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述各方面的方法。

第五方面，本申请实施例还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述各方面的方法。

第六方面，本申请实施例还提供了一种芯片系统，该芯片系统包括处理器，用于支持上述装置或系统实现上述方面中所涉及的功能，例如，生成或处理上述方法中所涉及的信息。

附图说明

图1是采用组合定位算法的组合定位系统的数据逻辑框图；

图2是组合定位系统中的各个传感器在车辆上的位置示意图；

图3是本申请实施例示出的安装偏差角的示意图；

图4是本申请实施例提供的组合定位系统的硬件结构图；

图5是本申请实施例提供的扩充后的组合定位系统的数据逻辑框图；

图6是本申请实施例提供的传感器安装偏差角的标定方法应用于IMU与GNSS天线之间的安装偏差角的标定的流程图；

图7是本申请实施例提供的传感器安装偏差角的标定方法应用于IMU与LiDAR安装偏差角的标定的流程图；

图8是本申请实施例提供的组合定位系统的软件模块的示意图；

图9是本申请实施例提供的组合定位系统的软件模块应用于标定IMU和GNSS天线之间的安装偏差角时的数据逻辑框图；

图10是本申请实施例提供的组合定位系统的软件模块应用于标定IMU和LiDAR之间的安装偏差角时的数据逻辑框图。

具体实施方式

自动驾驶车辆(autonomous vehicles或self-driving automobile)，也称无人驾驶车辆，电脑驾驶车辆或者轮式移动机器人。自动驾驶车辆能够以雷达、全球导航卫星系统(global navigation satellite system，GNSS)及机器视觉等技术感测其周围环境，并根据感测数据确定自身位置、规划导航路线、更新地图信息、躲避障碍等，最终实现在没有任何人类主动操作或者少有人类主动操作的情况下自动地驾驶车辆。

一般来说，自动驾驶车辆需要解决驾驶的三个核心问题：在哪里？(车辆定位)；去哪里？(确定目的地)；怎么去？(路径规划)。其中，自动驾驶车辆的定位系统主要用于解决“在哪里？”的问题。因此，作为自动驾驶车辆的关键模块之一，定位系统的性能将直接决定自动驾驶车辆的稳定性和可靠性。

目前，定位系统为了解决“在哪里？”的问题通常会包含有用于车辆定位的传感器，例如：惯性测量单元IMU、GNSS接收机、激光雷达LiDAR、里程计、视觉传感器等。其中，IMU例如可以包括三轴陀螺仪和加速度计，可用于测量自动驾驶车辆的角速度和加速度；GNSS接收机可用于测量GNSS的卫星信号，LiDAR可用于测量车辆与其他物体的距离。由于不同的传感器用于测量不同的数据，各个传感器应用在定位系统中既有优势也有局限性，单独使用任何一种传感器都难以达到定位精度高、抗干扰能力强、数据更新快的要求，因此目前的定位系统通常会将上述多个传感器的数据相结合，根据组合定位算法解算出车辆的位姿，达到取长补短、优势互补的效果，从而提高定位系统的稳定性和可靠性。

目前，采用组合定位算法的组合定位系统的数据逻辑框图如图1所示，其中，组合定位系统以INS 100的解算结果，例如根据IMU 110测量的角速度和加速度输出的位置、速度和姿态信息(以下称惯导位姿)，GNSS 120的定位结果，LiDAR 130的定位结果等作为组合滤波器140的输入，利用滤波结果对INS 100的解算结果进行误差修正，以确定最终的定位结果，例如位姿及其协方差。在图1以及其他常见的定位系统中，组合滤波器140的系统变量一般为惯导相关误差(简称惯导误差)，例如：位置误差、速度误差、姿态误差、加速度计零偏差和陀螺仪零偏差等。

本申请实施例中的组合滤波器140例如可以是卡尔曼滤波器(kalman filter，KF)，卡尔曼滤波器是一种递归滤波器(自回归滤波器)，它能够从一系列的不完全及包含噪声的测量量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变量的估计，因此会比只以单一测量量为基础的估计方式要准确。

图2是组合定位系统中的各个传感器在车辆上的位置示意图。如图2所示，该组合定位系统的传感器包括IMU 110、LiDAR 130和GNSS天线，GNSS天线可以包括一个GNSS主天线121和一个GNSS从天线122，简称GNSS双天线。组合定位系统中的各个传感器通常会被安装到车辆的不同位置，例如，IMU 110被安装到车辆的中部，GNSS双天线被安装到车辆的顶部，并且GNSS主天线121和GNSS从天线122前后分布设置，以便于接收卫星的GNSS信号，LiDAR 130被安装到车辆的头部，以便于测量车辆行驶前方的物体。

一般来说，各个传感器会基于自身的坐标系描述其解算结果或定位结果，由于各个传感器的安装位置不同，因此各个传感器的坐标系也不同。示例地，如图2所示，组合定位系统中包括：环境坐标系(E，N，U)，LiDAR坐标系(X _l，Y _l，Z _l)，IMU坐标系(X _b，Y _b，Z _b)，GNSS坐标系(X _g，Y _g，Z _g)和后轮轴地面投影坐标系(X _m，Y _m，Z _m)。可以理解的是，由于各个传感器的坐标系不同，因此将各个传感器的解算结果或定位结果进行滤波计算时，需要依赖各个传感器之间的安装位置关系以归化坐标系。各个传感器之间的安装位置关系可以通过标定获得，因此，标定的准确性就成为了组合定位系统定位精度的重要因素。

在一个实施例中，环境坐标系(E，N，U)例如可以是站心坐标系，也称东-北-天坐标系ENU。站心坐标系以目标(例如GNSS天线)所在位置为坐标原点，包括指向东方向的E轴、指向北方向的N轴和指向天空的U轴。

在一个实施例中，LiDAR坐标系(X _l，Y _l，Z _l)，IMU坐标系(X _b，Y _b，Z _b)，GNSS坐标系(X _g，Y _g，Z _g)和后轮轴地面投影坐标系(X _m，Y _m，Z _m)均可以使用右前上坐标系。在组合定位系统中，右前上坐标系以载具(例如车辆)的前进方向为Y轴方向，以载具的右侧为X轴方向，以垂直于X轴和Y轴的朝上方向为Z轴方向，各个坐标系的原点可以是其对应的传感器的中心。

本申请实施例中，各个传感器之间的安装位置关系可以包括臂参数以及安装偏差角。其中，杆臂参数主要描述各个传感器之间的安装位置距离，一般可以通过测量设备直接测量得到；安装偏差角主要描述各个传感器之间的安装角度偏差，考虑到车辆上传感器安装位置关系以及传感器的自身结构特性，各个传感器之间的安装偏差角一般很难通过测量设备直接精确测量。一般来说，安装偏差角可以包括IMU和GNSS天线之间的安装偏差角，以及IMU与LiDAR之间的安装偏差角。

图3是本申请实施例示出的安装偏差角的示意图，如图3所示，IMU 110和GNSS天线之间的航向安装偏差角是指GNSS双天线之间的连线(从GNSS主天线121到GNSS从天线122)与IMU坐标系前向轴(yb)的夹角λgb；IMU 110与LiDAR 130之间的航向安装偏差角指LiDAR坐标系前向轴(yl)与IMU坐标系前向轴(yb)的夹角λlb。

目前，IMU和GNSS天线之间的安装偏差角的标定主要采用离线标定方法，并且大多采用光学手段实现。以航向安装偏差角为例，可以通过光学手段分别对IMU和GNSS天线的航向进行标定，然后计算二者航向的偏差值即为IMU和GNSS天线之间的航向安装偏差角。然而，由于GNSS天线的相位中心位于天线内部，致使难以准确测量双天线的轴线，因此该方法的标定精度较低；并且，该方法需要依赖专用的标定工具，例如激光扫描仪、经纬仪等，操作较为繁琐，标定时间较长；另外，当车辆由于长时间振动、颠簸等原因导致IMU与GNSS安装位置关系发生变化时，需要重新标定，否则会影响组合定位系统的定位精度。

目前，IMU与LiDAR之间的安装偏差角的标定方法同样主要为离线标定方法，并且大多采用间接标定法，以航向安装偏差角为例，可以借助测量公共点或者辅助测量坐标系，建立IMU与LiDAR之间的联系，从而进一步确定IMU与LiDAR之间的航向安装偏差角。该方法同样需要依赖专用的标定工具，操作较为繁琐，标定时间较长，并且，当IMU与LiDAR安装位置关系发生变化时，需要重新标定，否则会影响组合定位系统的定位精度。

为了解决现有技术中存在的技术问题，本申请实施例提供了一种传感器安装偏差角的标定方法。

本申请实施例中的组合定位系统，可以是包括两个或者两个以上的定位传感器的定位系统，并且该定位系统使用上述两个或者两个以上的传感器的解算结果或者定位结果综合确定最终的定位结果。对于组合定位系统具体包含哪些定位传感器，本申请实施例不做具体限定，例如：在一些实施例中，组合定位系统可以包括IMU和GNSS；在另一些实施例中，组合定位系统包括IMU和LiDAR；在另一些实施例中，组合定位系统可以包括IMU、LiDAR和GNSS；在另一些实施例中，组合定位系统除包含上述定位传感器之外，还可以包括其他传感器。

图4是本申请实施例提供的组合定位系统的硬件结构图。在一个实施例中，该组合定位系统可以包括：IMU 110、GNSS 120、LiDAR 130、里程计140、视觉传感器150等传感器，处理器160、存储器170和通信模块180等。其中，IMU 110、GNSS 120、LiDAR 130等传感器用于测量和接收各类测量数据，处理器160用于对根据相应的组合定位算法对测量数据进行解算，例如：惯性导航解算、卡尔曼滤波计算等，以得到定位结果。

处理器160例如可以包括一个或者多个处理单元，例如中央处理器(central processing unit，CPU)、微控制器(microcontroller，MCU)、图像信号处理器(image signal processor，ISP)、神经网络处理器(neural-network processing unit，NPU)等。其中，不同的处理单元可以是独立的器件，也可以集成在一个或多个处理器中，例如集成在系统芯片(system on a chip，SoC)中。处理器160中还可以设置存储器，用于存储指令和数据。在一些实施例中，处理器中的存储器为高速缓冲存储器。该存储器可以保存处理器刚用过或循环使用的指令或数据。

存储器170可以包括一个或者多个存储单元，例如可以包括易失性存储器(volatile memory)，如：动态随机存取存储器(dynamic random access memory，DRAM)、静态随机存取存储器(static random access memory，SRAM)等；还可以包括非易失性存储器(non-volatile memory，NVM)，如：只读存储器(read-only memory，ROM)、闪存(flash memory)等。存储器170可以用于存储处理器可执行的程序代码和指令，例如：用于处理传感器数据的程序代码和指令、用于惯性导航解算的程序和指令、用于实现卡尔曼滤波算法的程序和指令等。存储器170还可以存储有自动驾驶计算平台的车机操作系统，例如：鸿蒙操作系统Harmony OS、UQX操作系统等。

通信模块180用于使车辆实现与外界的2G、3G、4G和5G多种网络模式的通信，例如实现车辆的车联万物(vehicle-to-everything，V2X)等，通信模块例如可以包括基带芯片、功率放大器和天线等。在一些实施例中，通信模块180的部分或者全部器件可以与一个或者多个处理器集成在一起，例如基带芯片可以与CPU、NPU、ISP等集成在SoC中。

另外，组合定位系统还可以包括通信接口，例如控制器局域网(controller area network，CAN)接口，使得处理器可以与车辆的其他芯片和设备，例如电子控制单元(electrical control unit，ECU)进行通信。

可以理解的是，本申请实施例示意的组合定位系统的硬件结构并不构成对组合定位系统的具体限定。在本申请另一些实施例中，组合定位系统的硬件结构可以包括比图示更多或更少的部件，或者组合某些部件，或者拆分某些部件，或者不同的部件布置。图示的部件可以以硬件，软件或软件和硬件的组合实现。

本申请实施例中的组合定位系统可以安装到各类应用自动驾驶技术或导航定位的载具之上，包括但不限于各种交通工具：例如车辆(汽车)、轮船、火车、地铁、飞机等，以及各种机器人，例如：服务机器人、运输机器人、自主导引机器人(automated guided vehicle，AGV)、无人地面车(unmanned ground vehicle，UGV)等，以及各种工程机械，例如：隧道掘进机等。

下面以组合定位系统包括IMU、LiDAR和GNSS等传感器为例，对本申请实施例提供的传感器安装偏差角的标定方法的技术方案进行示例地阐述说明。

本申请实施例对目前组合定位系统的状态变量进行了扩充，扩充后的组合定位系统的数据逻辑框图如图5所示，其中，该组合定位系统以根据IMU 110测量的角速度和加速度确定的惯导位姿、GNSS 120的定位结果、LiDAR 130的定位结果等作为组合定位滤波器的输入，并且在原有的惯导相关的误差，例如：位置误差、速度误差、姿态误差、加速度计零偏差和陀螺仪零偏差等基础之上，将IMU和GNSS天线之间的安装偏差角，以及IMU与LiDAR之间的安装偏差角也扩充到组合定位系统的系统变量中，从而通过组合滤波器对IMU和GNSS天线之间的安装偏差角，以及IMU与LiDAR之间的安装偏差角进行在线标定；当组合定位滤波器的状态变量完成收敛时，根据在线标定结果中的IMU和GNSS天线之间的安装偏差角，以及IMU与LiDAR之间的安装偏差角对GNSS和LiDAR的定位结果进行补偿，以提高定位精度。

在一些实施例中，本申请提供的传感器安装偏差角的标定方法具体可以由两部分组成，第一部分包括IMU与GNSS双天线安装偏差角的标定，第二部分包括IMU与LiDAR之间的安装偏差角的标定方。下面分别结合更多附图分别对本方法的上述两部分进行具体地解释说明。

图6是本申请实施例提供的传感器安装偏差角的标定方法应用于IMU与GNSS天线之间的安装偏差角的标定的流程图。如图6所示，该方法可以包括以下步骤：

步骤S101，获取IMU的测量结果。

其中，IMU的测量结果例如可以包括IMU测量得到的角速度和加速度信息。

具体实现中，在IMU和GNSS等传感器固定安装在载具上之后，可以使载具产生机动，例如加速、减速、匀速行驶、左转和右转等，在载具机动期间，组合定位系统可以实时并且连续地获取IMU测量得到的角速度和加速度信息。

步骤S102，对IMU的测量结果进行解算，以得到INS的姿态信息。

INS的姿态信息可以通过姿态矩阵来表达。另外，步骤S102对测量结果进行解算之后还可以得到INS的位置和速度等信息。上述位置、速度和姿态信息也可以概括称为惯导位姿。

具体实现中，在载具产生机动的过程中，组合定位系统可以实时并且连续地对IMU的测量结果数据进行解算，从而连续地获取车辆当前的位置、速度和姿态信息等。

在一个实施例中，在解算的初始阶段，可以根据GNSS定位结果确定车辆的初始位置和初始速度，作为IMU的初始数据；然后，组合定位系统可以根据初始数据和当前IMU的测量结果数据解算得到车辆最新的位置、速度和姿态信息。在解算的非初始阶段，组合定位系统可以根据上一次解算得到的车辆的位置、速度和姿态信息和当前IMU的测量结果数据得到车辆最新的位置、速度和姿态信息。

步骤S103，根据INS的姿态信息和GNSS姿态信息构建INS与GNSS天线之间的安装偏差角的误差模型。

其中，该误差模型以通过组合定位滤波器的状态变量进行描述。

在一个实施例中，为了建立IMU与GNSS之间的安装偏差角的误差模型，本申请实施例可以构建INS对应的姿态矩阵

以及，GNSS对应的姿态矩阵

姿态矩阵

例如可以是一个3×3大小的矩阵，其表示INS输出的位姿从后轮轴地面投影坐标系(Xm，Ym，Zm)经过平移和旋转等映射到环境坐标系(E，N，U)。一般来说，由于IMU的安装位置可能存在误差，因此，将此IMU输出的位姿从后轮轴地面投影坐标系(Xm，Ym，Zm)进行映射之后，可能会映射到有误差的环境坐标系(Xn′，Yn′，Zn′)，因此，本申请实施例将上述IMU对应的姿态矩阵记作

姿态矩阵

例如可以是一个3×3大小的矩阵，其表示GNSS输出的位姿从GNSS坐标系(Xg，Yg，Zg)经过平移和旋转等映射到环境坐标系(E，N，U)。

示例地，IMU对应的姿态矩阵

和GNSS对应的姿态矩阵

可以表示为以下公式(1)：

其中，姿态矩阵

中的参数

等用来表示INS输出的位姿从后轮轴地面投影坐标系(Xm，Ym，Zm)映射到环境坐标系(E，N，U)需要的平移和旋转等参数；姿态矩阵

中的参数

等用来表示GNSS输出的位姿从GNSS坐标系(Xg，Yg，Zg)映射到环境坐标系(E，N，U)需要的平移和旋转等参数。

另外，为便于描述，本申请实施例这里将INS输出的滚转角表示为γ _m、俯仰角表示为θ _m、航向角表示为

以及，将GNSS定位输出的滚转角表示为γ _g、俯仰角表示为θ _g、航向角表示为

那么，该误差模型的观测量可以表示为以下公式(2)

其中，δγ ₀表示IMU与GNSS输出的滚转角的偏差、δθ ₀表示IMU与GNSS输出的俯仰角的偏差、

表示IMU与GNSS输出的航向角的偏差，为便于概括描述，本申请实施例将上述偏差称作IMU和GNSS天线之间的姿态偏差。

需要补充说明的是，在车辆自动驾驶的定位和导航过程中，航向角对定位结果的准确性影响比较明显，因此，本申请实施例在后续输出标定结果时，也可以仅考虑输出IMU与GNSS双天线之间的航向安装偏差角。

进一步地，本申请实施例可以对IMU对应的姿态矩阵

进行以下变换，构造从INS对应的姿态矩阵到GNSS对应的姿态矩阵的转移关系方程，以建立其与GNSS对应的姿态矩阵

之间的联系。

其中，姿态矩阵

可以分解成两个变换矩阵，即

和

并且

其中，

例如可以是一个3×3大小的矩阵，其表示将位姿从环境坐标系(E，N，U)经过平移和旋转等映射到有误差的环境坐标系(Xn′，Yn′，Zn′)；

例如可以是一个3×3大小的矩阵，其表示将位姿从后轮轴地面投影坐标系(Xm，Ym，Zm)经过平移和旋转等映射到环境坐标系(E，N，U)。

可以进一步分解成两个变换矩阵，即

和

并且

其中，

例如可以是一个3×3大小的矩阵，其表示将位姿从GNSS坐标系(Xg，Yg，Zg)经过平移和旋转等映射到环境坐标系(E，N，U)；

例如可以是一个3×3大小的矩阵，其表示将位姿从后轮轴地面投影坐标系(Xm，Ym，Zm)经过平移和旋转等映射到GNSS坐标系(Xg，Yg，Zg)。

进而得到以下公式(3)：

进一步地，本申请实施例可以基于公式(3)向组合定位滤波器的状态变量中引入INS的姿态误差和IMU与GNSS天线之间的安装偏差角。

示例地，以东-北-天坐标系描述的INS的姿态误差

可以为：

其中，

为INS的E轴姿态误差、

为INS的N轴姿态误差、

为INS的U轴姿态误差。则有：

其中，

表示

的叉乘，I为单位矩阵：

示例地，IMU与GNSS天线之间的安装偏差角λ可以为：

其中，

为X轴方向的安装偏差角、

为Y轴方向的安装偏差角、

为Z轴方向的安装偏差角。则有：

其中，λ ⁰×表示λ ⁰的叉乘。

由此，根据公式(3)可进一步得到：

上述公式(4)即可以作为从INS对应的姿态矩阵到GNSS对应的姿态矩阵的转移关系方程。

接下来，将公式(1)、公式(2)与公式(4)联立，可推导出IMU对应的姿态矩阵

和GNSS对应的姿态矩阵

中的各个元素间的关系如公式(5)所示：

将公式(5)的左端按照泰勒级数展开，并且忽略公式中的二阶以上的高阶项，可以得到公式(6)：

该公式(6)可以称作IMU与GNSS双天线之间的安装偏差角的误差模型。其中，Z ₁,Z ₂,Z ₃为该误差模型的观测量；

为前述提到的INS的姿态误差，例如平台误差角；

为前述提到的IMU与GNSS天线之间的安装偏差角。

步骤S104，构建系统状态方程，系统状态方程的状态变量包括INS与GNSS天线之间的安装偏差角。

一般来说，组合定位系统中原有的状态方程是基于惯导误差模型构建的方程，状态方程中的状态变量为惯导误差，例如：位置误差、速度误差、姿态误差、加速度计零偏差和陀螺仪零偏差等，而本申请实施例可以将IMU与GNSS天线之间的安装偏差角的误差模型引入到原有的状态方程中，使新的状态方程的状态变量既包含惯导误差相关的变量，也包含IMU与GNSS天线之间的安装偏差角的误差。

在一个实施例中，本申请实施例定义该组合定位系统新的系统状态方程包括：

X＝[X ₁，X ₂] (7)

其中，X ₁和X ₂为状态变量，X ₁为惯导误差，X ₂为IMU与GNSS天线之间的安装偏差角。

则有，X ₁的系统状态方程为：

其中，

为惯导误差X ₁的导数；F ₁为惯导误差模型中可以推导的系统矩阵，例如可以是惯导误差模型的非线性转移矩阵；G ₁,W ₁为惯导误差驱动噪声，其中，G ₁可以是惯导误差的噪声驱动矩阵，W ₁可以是惯导误差的状态噪声矩阵，可选的，G ₁和W ₁均为高斯白噪声。

示例地，

其中，

分别为东、北、天方向平台失准角，δv _E,δv _N,δv _U分别为INS在东、北、天方向的速度误差，δλ,δL,δH分别为INS的经度、纬度和高度误差，ε _x,ε _y,ε _z为陀螺仪随机常值漂移在X，Y，Z轴的分量，

为INS的加速度计零偏在X，Y，Z轴的分量。可以理解的是，上述X ₁包含的参数项仅仅作为一个示例，不构成对X ₁的限定，在一些其他的实现方式中，X ₁可以包含更多的参数项或者更少的参数项，本申请实施例此处不做具体展开说明。

以及，X ₂的系统状态方程为：

其中，

为IMU与GNSS天线之间的安装偏差角X ₂的导数；F ₂为IMU与GNSS天线之间的安装偏差角的误差模型中可以推导的系统矩阵，例如可以是该误差模型的非线性转移矩阵；G ₂,W ₂为IMU与GNSS天线之间的安装偏差角的误差驱动噪声，其中，G ₂可以是IMU与GNSS天线之间的安装偏差角的误差的噪声驱动矩阵，W ₂可以是IMU与GNSS天线之间的安装偏差角的误差的状态噪声矩阵，可选的，G ₂和W ₂均为高斯白噪声。

示例地，

其中，

为X轴方向的安装偏差角、

为Y轴方向的安装偏差角、

为Z轴方向的安装偏差角。可以理解的是，上述X ₂包含的参数项仅仅作为一个示例，不构成对X ₂的限定，在一些其他的实现方式中，X ₂可以包含更多的参数项或者更少的参数项，本申请实施例此处不做具体展开说明。

另设，IMU与GNSS天线之间的安装偏差角的误差为

则

对应的系统状态方程为：

其中，

为

的导数。

以上，公式(7)-(9)即构成了本申请实施例的组合定位系统的系统状态方程，可以看出，该系统状态方程的状态变量不仅包含了惯导误差，还包含了IMU与GNSS天线之间的安装偏差角，由此，在后续使用组合定位滤波器对系统状态方程的状态变量进行在线估计时，就能够同时对IMU与GNSS天线之间的安装偏差角进行在线标定。

步骤S105，构建系统观测方程，系统观测方程的观测量包括根据误差模型确定的INS与GNSS天线之间的安装偏差角对应的观测矩阵。

具体来说，该系观测方程的观测量为惯导误差对应的观测矩阵和IMU与GNSS天线之间的安装偏差角的误差对应的观测矩阵。

在一个实施例中，该系统观测方程可以为：

Z＝HX+V＝[H ₁ H ₂ H ₃]

其中，Z为系统观测量，H为系统观测矩阵，X为本申请实施例在步骤S105中建立的新的系统状态方程，V为系统观测噪声矩阵。进一步地，H ₁为姿态误差对应的观测矩阵，H ₂为误差模型中除了姿态误差以外的其他状态变量对应的观测矩阵，H ₃为IMU与GNSS天线之间的安装偏差角对应的观测矩阵。H ₂可根据惯导误差模型得到，H ₁和H ₃如下所示：

步骤S106，使用组合定位滤波器对系统状态方程和系统观测方程进行滤波解算，以得到IMU与GNSS天线之间的安装偏差角的最优估计结果。

具体实现中，组合定位滤波器可以是卡尔曼滤波器，即对系统状态方程和系统观测方程进行卡尔曼滤波，以得到IMU与GNSS之间的航向安装偏差角的最优估计结果。

步骤S107，根据IMU与GNSS天线之间的安装偏差角的最优估计结果对GNSS定位结果进行补偿。

这样，GNSS与IMU之间消除了由于安装偏差角而产生的实际安装位置关系与其坐标系位置关系的不一致性，从而消除了由安装偏差角造成的组合定位误差，提高组合定位系统的定位精度。

另外，本申请实施例还可以根据卡尔曼滤波得到的状态变量最优估计结果对惯导解算结果，例如位置、速度和姿态等进行校正，以修正惯导解算结果的累积误差，提高组合定位系统的定位精度。

步骤S108，当组合定位滤波器对的滤波解算结果收敛时，将IMU与GNSS天线之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。

在此之后，如果IMU与GNSS之间的安装位置关系没有发生变化，那么IMU与GNSS天线之间的安装偏差角就不会改变，那么卡尔曼滤波器对的滤波解算结果将一直处于收敛状态；如果车辆在行驶过程中发生振动或者车辆维修等导致IMU与GNSS之间的安装位置关系，那么当卡尔曼滤波器对的滤波解算结果经过短暂的不收敛状态之后也会再次进入到收敛状态，这时，IMU与GNSS天线之间的安装偏差角的最优估计结果会发生变化，组合定位系统可以在卡尔曼滤波器重新收敛之后，将最新的标定结果保存至配置文件中。

由此可见，本申请实施例的方法将IMU与GNSS天线之间的安装偏差角整合到组合定位系统的状态变量中，实现了在车辆行驶过程中对组合定位系统的IMU与GNSS天线之间的安装偏差角进行标定(即在线标定)，标定过程不需要考虑IMU和GNSS的物理结构，标定精度高。并且，本申请实施例的方法不需要借助用的标定工具，仅需要车辆产生机动即可完成标定，操作简单、快速。另外，当IMU与GNSS之间的安装位置关系发生变化时，本申请实施例的方法能够在线更新标定结果，从而始终保证组合定位系统的准确性。

图7是本申请实施例提供的传感器安装偏差角的标定方法应用于IMU与LiDAR安装偏差角的标定的流程图。如图7所示，该方法可以包括以下步骤：

步骤S201，获取IMU的测量结果。

步骤S202，对IMU的测量结果进行解算，以得到INS的姿态信息。

步骤S201和步骤S202的具体实现方式可以参照步骤S101和步骤S102，此处不再赘述。

步骤S203，根据INS的姿态信息和LiDAR姿态信息构建INS与LiDAR之间的安装偏差角的误差模型。

其中，该误差模型的参数可以通过组合定位滤波器的状态变量进行表述。

在一个实施例中，为了建立IMU与LiDAR之间的安装偏差角的误差模型，本申请实施例可以构建INS对应的姿态矩阵

以及LiDAR对应的姿态矩阵

姿态矩阵

的具体实现方式可以参考本申请实施例对步骤S103和公式(1)描述的内容，此处不再赘述。

姿态矩阵

例如可以是一个3×3大小的矩阵，其表示LiDAR输出的位姿从LiDAR坐标系(Xl，Yl，Zl)经过平移和旋转等映射到环境坐标系(E，N，U)。

示例地，IMU对应的姿态矩阵

和LiDAR对应的姿态矩阵

可以表示为以下公式(10)：

其中，姿态矩阵

中的参数

等用来表示LiDAR输出的位姿从LiDAR坐标系(Xl，Yl，Zl)映射到环境坐标系(E，N，U)需要的平移和旋转等参数。

另外，为便于描述，本申请实施例这里将IMU解算输出的滚转角表示为γ _m、俯仰角表示为θ _m、航向角表示为

以及，将LiDAR定位输出的滚转角表示为γ _l、俯仰角表示为θ _l、航向角表示为

那么，该组合定位系统的姿态观测量可以表示为以下公式(11)

其中，δγ ₁表示IMU与LiDAR输出的滚转角的偏差、δθ ₁表示IMU与LiDAR输出的俯仰角的偏差、

表示IMU与LiDAR输出的航向角的偏差，为便于概括描述，本申请实施例将上述偏差称作IMU和LiDAR之间的姿态偏差。

需要补充说明的是，在车辆自动驾驶的定位和导航过程中，航向角对定位结果的准确性影响比较明显，因此，本申请实施例在后续输出标定结果时，可以仅考虑输出IMU与LiDAR 之间的航向安装偏差角。

进一步地，本申请实施例可以对INS对应的姿态矩阵

进行以下变换，构造从INS对应的姿态矩阵到LiDAR对应的姿态矩阵的转移关系方程，以建立其与GNSS对应的姿态矩阵

之间的联系。

其中，姿态矩阵

可以分解成两个变换矩阵，即

和

并且

其中，

可以进一步分解成两个变换矩阵，即

和

并且

其中，

例如可以是一个3×3大小的矩阵，其表示将位姿从LiDAR坐标系(X _l，Y _l，Z _l)经过平移和旋转等映射到环境坐标系(E，N，U)；

例如可以是一个3×3大小的矩阵，其表示将位姿从后轮轴地面投影坐标系(Xm，Ym，Zm)经过平移和旋转等映射到LiDAR坐标系(X _l，Y _l，Z _l)。

进而得到以下公式(12)：

进一步地，本申请实施例可以基于公式(12)向组合定位滤波器的状态变量中引入INS的姿态误差，以及IMU与LiDAR之间的安装偏差角。

示例地，以东-北-天坐标系描述的INS的姿态误差

可以为：

其中，

为INS的E轴姿态误差、

为INS的N轴姿态误差、

为INS的U轴姿态误差。则有：

其中，

表示

的叉乘，I为单位矩阵：

示例地，IMU与LiDAR之间的安装偏差角λ ¹可以为：

其中，

为X轴方向的安装偏差角、

为Y轴方向的安装偏差角、

为Z轴方向的安装偏差角。则有：

其中，λ ¹×表示λ ¹的叉乘。

由此，根据公式(12)可进一步得到：

上述公式(13)即可以作为从INS对应的姿态矩阵到LiDAR对应的姿态矩阵的转移关系方程。

接下来，将公式(10)、公式(11)与公式(13)联立，可推导出INS对应的姿态矩阵

和LiDAR对应的姿态矩阵

中的各个元素间的关系如公式(14)所示：

将公式(14)的左端按照泰勒级数展开，并且忽略公式中的二阶以上的高阶项，可以得到公式(15)：

该公式(15)可以称作IMU与LiDAR之间的安装偏差角的误差模型。其中，Z ₁,Z ₂,Z ₃为该误差模型的观测量；

为前述提到的INS的姿态误差，例如平台误差角；

为前述提到的IMU与LiDAR之间的安装偏差角。

步骤S204，构建系统状态方程，系统状态方程的状态变量包括INS与LiDAR之间的安装偏差角。

一般来说，组合定位系统中原有的状态方程是基于惯导误差模型构建的方程，状态方程中的状态变量为惯导误差，例如：位置误差、速度误差、姿态误差、加速度计零偏差和陀螺仪零偏差等，而本申请实施例可以将IMU与LiDAR之间的安装偏差角的误差模型引入到原有的状态方程中，使新的状态方程的状态变量既包含惯导误差相关的变量，也包含IMU与LiDAR之间的安装偏差角的误差相关的变量。

X＝[X ₁，X ₃] (16)

其中，X ₁为状态变量中的惯导误差，X ₃为IMU与LiDAR之间的安装偏差角。

则有，X ₁的系统状态方程为：

其中，

示例地，

其中，

以及，X ₂的系统状态方程为：

其中，

为IMU与LiDAR之间的安装偏差角X ₃的导数；F ₃为IMU与LiDAR之间的安装偏差角的误差模型中可以推导的系统矩阵，例如可以是该误差模型的非线性转移矩阵； G ₃,W ₃为IMU与LiDAR之间的安装偏差角的误差驱动噪声，其中，G ₃可以是IMU与LiDAR之间的安装偏差角的误差的噪声驱动矩阵，W ₃可以是IMU与LiDAR之间的安装偏差角的误差的状态噪声矩阵，可选的，G ₃和W ₃均为高斯白噪声。

示例地，

其中，

为IMU与LiDAR之间的X轴方向的安装偏差角、

为IMU与LiDAR之间的Y轴方向的安装偏差角、

为IMU与LiDAR之间的Z轴方向的安装偏差角。可以理解的是，上述X ₃包含的参数项仅仅作为一个示例，不构成对X ₃的限定，在一些其他的实现方式中，X ₃可以包含更多的参数项或者更少的参数项，本申请实施例此处不做具体展开说明。

另设，IMU与LiDAR之间的安装偏差角的误差为

则

对应的系统状态方程为：

其中，

为

的导数。

以上，公式(16)-(18)即构成了本申请实施例的组合定位系统的系统状态方程，可以看出，该系统状态方程的状态变量不仅包含了惯导误差，还包含了IMU与LiDAR之间的安装偏差角，由此，在后续使用组合定位滤波器对系统状态方程的状态变量进行在线估计时，就能够同时对IMU与LiDAR之间的安装偏差角进行在线标定。

步骤S205，构建系统观测方程，系统观测方程的观测量包括根据误差模型确定的INS与LiDAR之间的安装偏差角对应的观测矩阵。

具体来说，该系观测方程的观测量为惯导误差对应的观测矩阵和IMU与LiDAR之间的安装偏差角的误差对应的观测矩阵。

在一个实施例中，该系统观测方程可以为：

Z＝HX+V＝[H ₁ H ₂ H ₃]

其中，Z为系统观测量，H为系统观测矩阵，X为本申请实施例在步骤S205中建立的新的系统状态方程，V为系统观测噪声矩阵。进一步地，H ₁为姿态误差对应的观测矩阵，H ₂为误差模型中除了姿态误差以外的其他状态变量对应的观测矩阵，H ₃为IMU与LiDAR之间的安装偏差角对应的观测矩阵。H ₂可根据惯导误差模型得到，H ₁和H ₃如下所示：

步骤S206，使用组合定位滤波器对系统状态方程和系统观测方程进行滤波解算，以得到IMU与LiDAR之间的安装偏差角的最优估计结果。

具体实现中，组合定位滤波器可以是卡尔曼滤波器，即对系统状态方程和系统观测方程进行卡尔曼滤波，以得到IMU与LiDAR之间的安装偏差角的最优估计结果。

步骤S207，根据IMU与LiDAR之间的安装偏差角的最优估计结果对LiDAR定位结果进行补偿。

这样，LiDAR与IMU之间消除了由于安装偏差角而产生的实际安装位置关系与其坐标系位置关系的不一致性，从而消除了由安装偏差角造成的组合定位误差，提高组合定位系统的定位精度。

步骤S208，当组合定位滤波器对的滤波解算结果收敛时，将IMU与LiDAR之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。

在此之后，如果IMU与LiDAR之间的安装位置关系没有发生变化，那么IMU与LiDAR之间的安装偏差角就不会改变，那么卡尔曼滤波器对的滤波解算结果将一直处于收敛状态；如果车辆在行驶过程中发生振动或者车辆维修等导致IMU与LiDAR之间的安装位置关系，那么当卡尔曼滤波器对的滤波解算结果经过短暂的不收敛状态之后也会再次进入到收敛状态，这时，IMU与LiDAR之间的安装偏差角的最优估计结果会发生变化，组合定位系统可以在卡尔曼滤波器重新收敛之后，将最新的标定结果保存至配置文件中。

由此可见，本申请实施例的方法将IMU与LiDAR之间的安装偏差角整合到组合定位系统的状态变量中，实现了在车辆行驶过程中对组合定位系统的IMU与LiDAR之间的安装偏差角进行标定(即在线标定)，标定过程不需要考虑IMU和LiDAR的物理结构，标定精度高。并且，本申请实施例的方法不需要借助用的标定工具，仅需要车辆产生机动即可完成标定，操作简单、快速。另外，当IMU与LiDAR之间的安装位置关系发生变化时，本申请实施例的方法能够在线更新标定结果，从而始终保证组合定位系统的定位精度。

需要补充说明的是，本申请实施例的方法可以应用到包含IMU和GNSS的组合定位系统中，以标定IMU和GNSS双天线之间的安装偏差角；本申请实施例的方法可以应用到包含IMU和LiDAR的组合定位系统中，以标定IMU和LiDAR之间的安装偏差角；本申请实施例的方法可以应用到包含IMU、GNSS和LiDAR的组合定位系统中，以标定IMU和GNSS双天线之间的安装偏差角和IMU和LiDAR之间的安装偏差角，其中X＝[X ₁，X ₂，X ₃]。

上述本申请提供的实施例对组合定位系统实现传感器安装偏角标定方法的各方案进行了介绍。可以理解的是，组合定位系统为了实现上述功能，可以包含执行各个功能相应的硬件结构和/或软件模块。本领域技术人员应该很容易意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，本申请能够以硬件或硬件和计算机软件的结合形式来实现。某个功能究竟以硬件还是计算机软件驱动硬件的方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

在一个实施例中，组合定位系统可以通过如4所示的硬件结构实现相应的功能。其中，各个传感器可以安装到车辆的不同位置，存储器和处理器例如可以是车辆的行车电脑，例如电子控制单元(electronic control unit，ECU)，或者自动驾驶计算平台，例如移动数据中心(mobile data center，MDC)中的存储器和处理器。存储器包括有用于实现相应的功能的程序指令，程序指令被处理器运行时，使得组合定位系统用于执行如下步骤：根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型；构建系统状态方程，系统状态方程的状态变量包括安装偏差角；构建系统观测方程，系统观测方程的观测量包括根据误差模型确定的安装偏差角对应的观测矩阵；对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的安装偏差角的估计结果作为标定结果。

在一个实施例中，组合定位系统用于标定IMU和GNSS天线之间的安装偏差角，因此多个传感器可以至少包括IMU和GNSS接收机，GNSS接收机包括GNSS天线，例如主天线和从天线，简称GNSS双天线，IMU和GNSS的每根天线安装在车辆的不同位置；程序指令被处理器运行时，使得组合定位系统用于执行如下步骤，以实现根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型：根据INS和GNSS输出的姿态信息，构造误差模型的观测量，观测量为IMU和GNSS天线之间的姿态偏差；构建从INS对应的姿态矩阵到GNSS对应的姿态矩阵的转移关系方程；联立误差模型的观测量和转移关系方程构建误差模型，误差模型为IMU和GNSS天线之间的安装偏差角的误差模型；其中，姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，转移关系方程包含INS的姿态误差和IMU与GNSS天线之间的安装偏差角。

在一个实施例中，程序指令被处理器运行时，使得组合定位系统用于执行如下步骤，以实现对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的安装偏差角的最优估计结果作为标定结果：对系统状态方程和系统观测方程进行卡尔曼滤波解算，得到IMU与GNSS天线之间的安装偏差角的最优估计结果；根据最优估计结果对GNSS的定位结果进行补偿，以及对INS的惯导解算结果进行修正；当滤波解算结果收敛时，将IMU与GNSS天线之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。

在一个实施例中，组合定位系统用于标定IMU和LiDAR之间的安装偏差角，因此多个传感器至少包括IMU和LiDAR，IMU和LiDAR安装在车辆的不同位置；程序指令被处理器运行时，使得组合定位系统用于执行如下步骤，以实现根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型：根据INS和LiDAR输出的姿态信息，构造误差模型的观测量，观测量为IMU和LiDAR之间的姿态偏差；构建从INS对应的姿态矩阵到LiDAR对应的姿态矩阵的转移关系方程；联立误差模型的观测量和转移关系方程构建误差模型，误差模型为IMU和LiDAR之间的安装偏差角的误差模型；其中，姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，转移关系方程包含INS的姿态误差和IMU与LiDAR之间的安装偏差角。

在一个实施例中，程序指令被处理器运行时，使得组合定位系统用于执行如下步骤，以实现对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的安装偏差角的最优估计结果作为标定结果：对系统状态方程和系统观测方程进行卡尔曼滤波解算，得到IMU与LiDAR之间的安装偏差角的最优估计结果；根据最优估计结果对LiDAR的定位结果进行补偿，以及对INS的惯导解算结果进行修正；当滤波解算结果收敛时，将IMU与LiDAR之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。

在另一个实施例中，组合定位系统可以通过图8所示的软件模块实现相应的功能。如图8所示，组合定位系统可以包括惯导解算模块310、误差模型构建模块320、系统状态方程构建模块330、系统状态方程构建模块340和滤波解算模块350。下面对上述模块的功能进行具体说明：

如图9所示，当组合定位系统用于标定IMU和GNSS天线之间的安装偏差角时：

惯导解算模块310，用于对IMU的测量结果进行惯导解算，以得到INS的姿态信息。

误差模型构建模块320，用于根据INS的姿态信息和GNSS姿态信息构建INS与GNSS天线之间的安装偏差角的误差模型；

系统状态方程构建模块330，用于构建系统状态方程，系统状态方程的状态变量包括INS与GNSS天线之间的安装偏差角；

系统观测方程构建模块340，用于构建系统观测方程，系统观测方程的观测量包括根据误差模型确定的INS与GNSS天线之间的安装偏差角对应的观测矩阵。

滤波解算模块350，用于对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的INS与GNSS天线之间的安装偏差角的估计结果作为标定结果。

在一个实施例中，误差模型构建模块320，具体可以用于：根据INS和GNSS输出的姿态信息，构造误差模型的观测量；构建从INS对应的姿态矩阵到GNSS对应的姿态矩阵的转移关系方程；联立误差模型的观测量和转移关系方程构建误差模型；其中，姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，转移关系方程包含INS的姿态误差和IMU与GNSS天线之间的安装偏差角。

在一个实施例中，滤波解算模块350，具体可以用于：对系统状态方程和系统观测方程进行卡尔曼滤波解算，得到IMU与GNSS天线之间的安装偏差角的最优估计结果；根据最优估计结果对GNSS的定位结果进行补偿，以及对INS的惯导解算结果进行修正；当滤波解算结果收敛时，将IMU与GNSS天线之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。

进一步如图10所示，当组合定位系统用于标定IMU和LiDAR之间的安装偏差角时：

误差模型构建模块320，用于根据INS的姿态信息和LiDAR姿态信息构建INS与LiDAR之间的安装偏差角的误差模型；

系统状态方程构建模块330，用于构建系统状态方程，系统状态方程的状态变量包括INS与LiDAR之间的安装偏差角；

系统观测方程构建模块340，用于构建系统观测方程，系统观测方程的观测量包括根据误差模型确定的INS与LiDAR之间的安装偏差角对应的观测矩阵。

滤波解算模块350，用于对系统状态方程和系统观测方程进行滤波解算，将滤波解算结果收敛时得到的INS与LiDAR之间的安装偏差角的估计结果作为标定结果。

在一个实施例中，误差模型构建模块320，具体可以用于：根据INS和LiDAR输出的姿态信息，构造误差模型的观测量；构建从INS对应的姿态矩阵到LiDAR对应的姿态矩阵的转移关系方程；联立误差模型的观测量和转移关系方程构建误差模型；其中，姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，转移关系方程包含INS的姿态误差和IMU与LiDAR之间的安装偏差角。

在一个实施例中，滤波解算模块350，具体可以用于：对系统状态方程和系统观测方程进行卡尔曼滤波解算，得到IMU与LiDAR之间的安装偏差角的最优估计结果；根据最优估计结果对LiDAR的定位结果进行补偿，以及对INS的惯导解算结果进行修正；当滤波解算结果收敛时，将IMU与LiDAR之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。

本申请实施例还提供了一种车辆，该车辆可以包含前述各实施例提供的组合定位系统，并且用户执行前述各个实施例提供的传感器安装偏差角的标定方法。

本申请实施例还提供一种计算机可读存储介质，该计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述各方面的方法。

本申请实施例还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述各方面的方法。

以上的具体实施方式，对本申请实施例的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本申请实施例的具体实施方式而已，并不用于限定本申请实施例的保护范围，凡在本申请实施例的技术方案的基础之上，所做的任何修改、等同替换、改进等，均应包括在本申请实施例的保护范围之内。

Claims

一种传感器安装偏差角的标定方法，其特征在于，应用于组合定位系统，所述组合定位系统包括多个传感器，所述多个传感器的安装位置不同；

所述方法包括：

根据各个传感器输出的姿态信息，构建所述各个传感器之间的安装偏差角的误差模型；

构建系统状态方程，所述系统状态方程的状态变量包括所述安装偏差角；

构建系统观测方程，所述系统观测方程的观测量包括根据所述误差模型确定的所述安装偏差角对应的观测矩阵；

对所述系统状态方程和所述系统观测方程进行滤波解算，将滤波解算结果收敛时得到的所述安装偏差角的估计结果作为标定结果。
根据权利要求1所述的方法，其特征在于，所述多个传感器包括惯性导航系统INS的惯性测量单元IMU和全球导航卫星系统GNSS接收机，所述GNSS接收机包括GNSS天线，所述IMU和所述GNSS天线的安装位置不同，

所述根据各个传感器输出的姿态信息，构建所述各个传感器之间的安装偏差角的误差模型，包括：

构造所述误差模型的观测量，所述观测量为所述IMU和所述GNSS天线之间的姿态偏差；

构建从所述INS对应的姿态矩阵到所述GNSS对应的姿态矩阵的转移关系方程；

联立所述误差模型的观测量和所述转移关系方程构建所述误差模型，所述误差模型为所述IMU和所述GNSS天线之间的安装偏差角的误差模型；

其中，所述姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，所述转移关系方程包含所述INS的姿态误差和所述IMU与所述GNSS天线之间的安装偏差角。
根据权利要求2所述的方法，其特征在于，所述误差模型的观测量包括：

其中，γ _m、θ _m、
分别为INS输出的滚转角、俯仰角和航向角；γ _g、θ _g、
分别为GNSS输出的滚转角、俯仰角和航向角。
根据权利要求2或3所述的方法，其特征在于，所述转移关系方程包括：

其中，
为所述INS对应的姿态矩阵，
为所述GNSS对应的姿态矩阵，
为所述INS的姿态误差，λ ⁰为所述IMU与所述GNSS天线之间的安装偏差角，λ ⁰×表示λ ⁰的叉乘。
根据权利要求2-4任一项所述的标定方法，其特征在于，所述系统状态方程包括：

X＝[X ₁，X ₂]

其中，X ₁和X ₂为状态变量，X ₁为惯导误差，X ₂为所述IMU与所述GNSS天线之间的安装偏差角；
为X ₁的导数；F ₁为所述组合定位系统的惯导误差模型的系统矩阵，G ₁、W ₁为惯导误差驱动噪声；
为X ₂的导数；F ₂为所述IMU与所述GNSS天线之间的安装偏差角的误差模型的系统矩阵，G ₂、W ₂为所述IMU与所述GNSS天线之间的安装偏差角的误差驱动噪声。
根据权利要求5所述的标定方法，其特征在于，所述系统状态方程包括：

Z＝HX+V＝[H ₁ H ₂ H ₃]

其中，Z为系统观测量，H为系统观测矩阵，X为所述系统状态方程，V为系统观测噪声矩阵，H ₁为INS的姿态误差对应的观测矩阵，H ₂为所述惯导误差模型中除了所述姿态误差以外的其他状态变量对应的观测矩阵，H ₃为所述IMU与所述GNSS天线之间的安装偏差角对应的观测矩阵。
根据权利要求2-6任一项所述的标定方法，其特征在于，所述对所述系统状态方程和所述系统观测方程进行滤波解算，将滤波解算结果收敛时得到的所述安装偏差角的最优估计结果作为标定结果，包括：

对所述系统状态方程和所述系统观测方程进行卡尔曼滤波解算，得到所述IMU与所述GNSS天线之间的安装偏差角的最优估计结果；

根据所述最优估计结果对所述GNSS的定位结果进行补偿，以及对所述INS的惯导解算结果进行修正；

当所述滤波解算结果收敛时，将所述IMU与所述GNSS天线之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。
根据权利要求1所述的标定方法，其特征在于，所述多个传感器包括IMU和激光雷达LiDAR，所述IMU和所述LiDAR的安装位置不同；

所述根据各个传感器输出的姿态信息，构建所述各个传感器之间的安装偏差角的误差模型，包括：

构造所述误差模型的观测量，所述观测量为所述IMU和所述LiDAR之间的姿态偏差；

构建从所述INS对应的姿态矩阵到所述LiDAR对应的姿态矩阵的转移关系方程；

联立所述误差模型的观测量和所述转移关系方程构建所述误差模型，所述误差模型为所述IMU和所述LiDAR之间的安装偏差角的误差模型；

其中，所述姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，所述转移关系方程包含所述INS的姿态误差和所述IMU与所述LiDAR之间的安装偏差角。
根据权利要求8所述的方法，其特征在于，所述误差模型的观测量包括：

其中，γ _m、θ _m、
分别为INS输出的滚转角、俯仰角和航向角；γ _l、θ _l、
分别为LiDAR输出的滚转角、俯仰角和航向角。
根据权利要求8或9所述的方法，其特征在于，所述转移关系方程包括：

其中，
为所述INS对应的姿态矩阵，
为所述LiDAR对应的姿态矩阵，
为所述INS的姿态误差，λ ¹为所述IMU与所述LiDAR天线之间的安装偏差角，λ ¹×表示λ ¹的叉乘。
根据权利要求8-10任一项所述的标定方法，其特征在于，所述系统状态方程包括：

X＝[X ₁，X ₃]

其中，X ₁和X ₃为状态变量，X ₁为惯导误差，X ₃为所述IMU与所述LiDAR之间的安装偏差角；
为X ₁的导数；F ₁为所述组合定位系统的惯导误差模型的系统矩阵，G ₁、W ₁为惯导误差驱动噪声；
为X ₃的导数；F ₃为所述IMU与所述LiDAR之间的安装偏差角的误差模型的系统矩阵，G ₃、W ₃为所述IMU与所述LiDAR之间的安装偏差角的误差驱动噪声。
根据权利要求11所述的标定方法，其特征在于，所述系统状态方程包括：

Z＝HX+V＝[H ₁ H ₂ H ₃]

其中，Z为系统观测量，H为系统观测矩阵，X为所述系统状态方程，V为系统观测噪声矩阵，H ₁为INS的姿态误差对应的观测矩阵，H ₂为所述惯导误差模型中除了所述姿态误差以外的其他状态变量对应的观测矩阵，H ₃为所述IMU与所述LiDAR之间的安装偏差角对应的观测矩阵。
根据权利要求8-12任一项所述的标定方法，其特征在于，所述对所述系统状态方程和所述系统观测方程进行滤波解算，将滤波解算结果收敛时得到的所述安装偏差角的最优估计结果作为标定结果，包括：

对所述系统状态方程和所述系统观测方程进行卡尔曼滤波解算，得到所述IMU与所述LiDAR之间的安装偏差角的最优估计结果；

根据所述最优估计结果对所述LiDAR的定位结果进行补偿，以及对所述INS的惯导解算结果进行修正；

当所述滤波解算结果收敛时，将所述IMU与所述LiDAR之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。
一种组合定位系统，其特征在于，包括：处理器、存储器和多个传感器，所述多个传感器的安装位置不同；所述存储器包括有程序指令，所述程序指令被所述处理器运行时，使得所述组合定位系统用于执行如下步骤：

根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型；

构建系统状态方程，所述系统状态方程的状态变量包括所述安装偏差角；

构建系统观测方程，所述系统观测方程的观测量包括根据所述误差模型确定的所述安装偏差角对应的观测矩阵；

对所述系统状态方程和所述系统观测方程进行滤波解算，将滤波解算结果收敛时得到的所述安装偏差角的估计结果作为标定结果。
根据权利要求14所述的组合定位系统，其特征在于，所述多个传感器包括惯性导航系统INS的惯性测量单元IMU和全球导航卫星系统GNSS接收机，所述GNSS接收机包括GNSS天线，所述IMU和所述GNSS天线的安装位置不同；

所述程序指令被所述处理器运行时，使得所述组合定位系统用于执行如下步骤，以实现根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型：

构造所述误差模型的观测量，所述观测量为所述IMU和所述GNSS天线之间的姿态偏差；

构建从所述INS对应的姿态矩阵到所述GNSS对应的姿态矩阵的转移关系方程；

联立所述误差模型的观测量和所述转移关系方程构建所述误差模型，所述误差模型为所述IMU和所述GNSS天线之间的安装偏差角的误差模型；

其中，所述姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，所述转移关系方程包含所述INS的姿态误差和所述IMU与所述GNSS天线之间的安装偏差角。
根据权利要求15所述的组合定位系统，其特征在于，所述程序指令被所述处理器运行时，使得所述组合定位系统用于执行如下步骤，以实现对所述系统状态方程和所述系统观测方程进行滤波解算，将滤波解算结果收敛时得到的所述安装偏差角的最优估计结果作为标定结果：

对所述系统状态方程和所述系统观测方程进行卡尔曼滤波解算，得到所述IMU与所述GNSS天线之间的安装偏差角的最优估计结果；

根据所述最优估计结果对所述GNSS的定位结果进行补偿，以及对所述INS的惯导解算结果进行修正；

当所述滤波解算结果收敛时，将所述IMU与所述GNSS天线之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。
根据权利要求14所述的组合定位系统，其特征在于，所述多个传感器包括IMU和激光雷达LiDAR，所述IMU和所述LiDAR的安装位置不同；

所述程序指令被所述处理器运行时，使得所述组合定位系统用于执行如下步骤，以实现根据各个传感器输出的姿态信息，构建各个传感器之间的安装偏差角的误差模型：

构造所述误差模型的观测量，所述观测量为所述IMU和所述LiDAR之间的姿态偏差；

构建从所述INS对应的姿态矩阵到所述LiDAR对应的姿态矩阵的转移关系方程；

联立所述误差模型的观测量和所述转移关系方程构建所述误差模型，所述误差模型为所述IMU和所述LiDAR之间的安装偏差角的误差模型；

其中，所述姿态信息包括滚转角、俯仰角和航向角中的一个或者多个，所述转移关系方程包含所述INS的姿态误差和所述IMU与所述LiDAR之间的安装偏差角。
根据权利要求17所述的组合定位系统，其特征在于，所述程序指令被所述处理器运行时，使得所述组合定位系统用于执行如下步骤，以实现对所述系统状态方程和所述系统观测方程进行滤波解算，将滤波解算结果收敛时得到的所述安装偏差角的最优估计结果作为标定结果：

对所述系统状态方程和所述系统观测方程进行卡尔曼滤波解算，得到所述IMU与所述LiDAR之间的安装偏差角的最优估计结果；

根据所述最优估计结果对所述LiDAR的定位结果进行补偿，以及对所述INS的惯导解算结果进行修正；

当所述滤波解算结果收敛时，将所述IMU与所述LiDAR之间的安装偏差角的最优估计结果作为标定结果保存至配置文件中。
一种车辆，其特征在于，包括：组合定位系统，所述组合定位系统包括处理器、存储器和多个传感器，不同传感器安装在所述车辆的不同位置；所述存储器包括有程序指令，所述程序指令被所述处理器运行时，使得所述组合定位系统用于执行如权利要求1-13任一项所述的方法。
一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有指令，当所述指令在计算机上运行时，使得所述计算机执行如权利要求1-13任一项所述的方法。
一种计算机程序产品，其特征在于，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如权利要求1-13任一项所述的方法。