用于GNSS天线杆臂和里程计杆臂的在线估计方法、设备及介质

用于gnss天线杆臂和里程计杆臂的在线估计方法、设备及介质
技术领域
1.本发明属于组合导航领域，具体涉及一种用于gnss天线杆臂和里程计杆臂的在线估计方法、设备及介质。


背景技术：

2.gnss/ins组合导航系统是经典的组合模式，其中gnss连续定位精度高，但是容易受到环境因素的影响，ins通过递推方式导航定位，但是存在累计误差。因此，利用gnss和ins进行优势互补，可实现高精度定位。但由于低成本的mems imu较大的噪声，在隧道、林荫道等卫星信号遮蔽的路段，ins导航独立工作，使得误差快速发散。车载导航系统中，通过使用里程计(odometer，odo)辅助ins导航，能抑制ins误差的发散，从而实现在复杂场景下连续的高精度定位。
3.然而，在gnss/ins/odo组合导航系统中，由于gnss解算出来的位置坐标和惯导机械编排计算出位置坐标不重合，因此在对两者进行数据融合解算时需做杆臂效应补偿，提高组合导航定位精度。在里程计辅助惯导组合导航的过程中，由于与imu的位置不重合，在转弯时将导致载车测量的速度与真实速度不一致。尤其当两者距离越远时，速度误差越大。组合导航系统中的杆臂通常是利用直尺测量出x，y，z三个方向的大小，然后填入代码中进行杆臂误差补偿。这种做法既无法大规模商业化推广，同时在测量的过程中容易产生杆臂测量误差。针对这一问题，本发明提出了一种在线实时估计组合导航系统gnss天线杆臂和里程计杆臂的方法。


技术实现要素：

4.发明目的：为了克服gnss/ins/odo组合导航系统中杆臂获取效率低且容易引入量测误差的问题，本发明提供一种用于gnss天线杆臂和里程计杆臂的在线估计方法。该方法即可同时估计gnss天线杆臂和里程计杆臂，也可单独估计gnss天线杆臂和里程计杆臂向量。
5.本发明内容如下：
6.一种用于gnss天线杆臂和里程计杆臂的在线估计方法，
7.构建三个卡尔曼滤波模型，第一个模型包含gnss杆臂误差和里程计杆臂误差，第二个模型仅包含gnss杆臂误差，第三个模型仅包含里程计杆臂误差；
8.获取gnss、ins和odo的数据；
9.根据不同的需求选择对应的系统状态转移矩阵和观测矩阵；其中滤波模型的选择通过一个全局变量控制；
10.利用卡尔曼滤波的预测和更新过程在线估计gnss杆臂和里程计杆臂。
11.作为优选，卡尔曼滤波模型具体构建过程包括
12.构建系统状态方程和观测方程
[0013][0014]
式中，xk、x
k-1
分别为k时刻、k-1时刻系统的状态向量，φ
k,k-1
表示k-1到k时刻的状态一步转移矩阵，γ
k-1
为噪声相关的矩阵，zk为k时刻的观测值，hk为k时刻的观测矩阵，w
k-1
为系统噪声，vk为观测噪声；其中w
k-1
和vk是白噪声，且互不相关；
[0015]
构建状态向量
[0016]
xk＝[x
0 lever
gnsst lever
odot
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0017]
其中，x0表示组合导航的位置误差、速度误差、姿态误差陀螺仪和加速度计的零偏误差，lever
gnsst
、lever
odot
分别表示gnss天线杆臂误差和里程计杆臂误差。
[0018]
作为优选，获取gnss、ins和odo的数据时，微处理器通过串口、串行外设接口、定时器分别连接gnss接收机惯性导航单元和里程计，以获取gnss、ins和odo数据；gnss数据包括位置、卫星信号强度和时间戳信息；ins数据包括角速度、加速度信息；odo数据包括车辆的前向位置增量和速度信息。
[0019]
作为优选，根据不同的需求选择对应的系统状态转移矩阵和观测矩阵。
[0020]
作为优选，在线估计过程包括：预测过程和更新过程；
[0021]
在预测过程中，卡尔曼滤波算法使用车辆的动态模型来预测车辆的导航信息、gnss杆臂向量和里程计杆臂向量等信息，并计算预测值的方差；
[0022]
在更新过程中，卡尔曼滤波算法使用gnss、ins和odo的数据来计算卡尔曼增益，利用卡尔曼增益修正预测值；卡尔曼增益的计算基于观测值的方差和预测值的方差之比，越准确的观测值对预测值的修正权重越大；更新过程后得到包含gnss杆臂误差和里程计杆臂误差的状态向量，结合初始设定值或者前一历元的杆臂估计值计算得到当前历元的杆臂估计值。
[0023]
一种用于gnss天线杆臂和里程计杆臂的在线估计系统，包括
[0024]
第一模块：被配置为用于构建三个卡尔曼滤波模型，第一个模型包含gnss杆臂误差和里程计杆臂误差，第二个模型仅包含gnss杆臂误差，第三个模型仅包含里程计杆臂误差；
[0025]
第二模块：被配置为用于获取gnss、ins和odo的数据；
[0026]
第三模块：被配置为根据不同的需求选择对应的系统状态转移矩阵和观测矩阵；其中滤波模型的选择通过一个全局变量控制；
[0027]
第四模块：被配置为利用卡尔曼滤波的预测和更新过程在线估计gnss杆臂和里程计杆臂。
[0028]
作为优选，卡尔曼滤波模型具体构建过程包括
[0029]
构建系统状态方程和观测方程
[0030][0031]
式中，xk、x
k-1
分别为k时刻、k-1时刻系统的状态向量，φ
k,k-1
表示k-1到k时刻的状态一步转移矩阵，γ
k-1
为噪声相关的矩阵，zk为k时刻的观测值，hk为k时刻的观测矩阵，w
k-1
为系统噪声，vk为观测噪声；其中w
k-1
和vk是白噪声，且互不相关；
[0032]
构建状态向量
[0033]
xk＝[x
0 lever
gnsst lever
odot
]
t
ꢀꢀꢀꢀ
(2)
[0034]
其中，x0表示组合导航的位置误差、速度误差、姿态误差陀螺仪和加速度计的零偏误差，lever
gnsst
、lever
odot
分别表示gnss天线杆臂误差和里程计杆臂误差。
[0035]
作为优选，获取gnss、ins和odo的数据时，微处理器通过串口、串行外设接口、定时器分别连接gnss接收机惯性导航单元和里程计，以获取gnss、ins和odo数据；gnss数据包括位置、卫星信号强度和时间戳信息；ins数据包括角速度、加速度信息；odo数据包括车辆的前向位置增量和速度信息；
[0036]
根据不同的需求选择对应的系统状态转移矩阵和观测矩阵；
[0037]
在线估计过程包括：预测过程和更新过程；
[0038]
在预测过程中，卡尔曼滤波算法使用车辆的动态模型来预测车辆的导航信息、gnss杆臂向量和里程计杆臂向量等信息，并计算预测值的方差；
[0039]
在更新过程中，卡尔曼滤波算法使用gnss、ins和odo的数据来计算卡尔曼增益，利用卡尔曼增益修正预测值；卡尔曼增益的计算基于观测值的方差和预测值的方差之比，越准确的观测值对预测值的修正权重越大；更新过程后得到包含gnss杆臂误差和里程计杆臂误差的状态向量，结合初始设定值或者前一历元的杆臂估计值计算得到当前历元的杆臂估计值。
[0040]
一种电子设备，存储计算机可执行指令的计算机可读存储介质；和一个或多个处理器，所述一个或多个处理器耦接到所述计算机可读存储介质并被配置为执行所述计算机可执行指令，以使得所述设备执行根据权利要求1-5中任一项所述的方法。
[0041]
一种可读存储介质，存储有计算机可执行指令，所述计算机可执行指令当由处理器执行时，将处理器配置为执行根据权利要求1-5中任一项所述的方法。
[0042]
本发明提供的一种在线实时估计组合导航系统gnss天线杆臂和里程计杆臂的方法，该方法通过将gnss天线杆臂误差和里程计杆臂误差拓展到kalman滤波状态向量中，在导航结算时，实时估计杆臂向量，无需人工操作测量杆臂向量，避免非专业人员操作困难，同时减少了量测误差的产生，该方法可以结合gnss、ins和odo数据进行处理，具有较高的定位精度和实时性，另外该方法即可同时估计gnss天线杆臂和里程计杆臂，也可单独估计gnss天线杆臂和里程计杆臂向量，实用性较好。
附图说明
[0043]
图1是gnss天线杆臂和里程计杆臂估计流程图。
[0044]
图2是gnss天线相位中与imu测量中心的空间位置坐标关系图。
[0045]
图3是车轮触地点与imu测量中心的空间位置坐标关系图。
具体实施方式：
[0046]
下面结合附图和具体实施，对本发明做进一步说明。
[0047]
实施例1
[0048]
如附图1所示为gnss天线杆臂和里程计杆臂估计流程图，本发明的具体步骤如下：
[0049]
步骤1：建立三个卡尔曼滤波模型，第一个模型包含gnss杆臂误差和里程计杆臂误
差，第二个模型仅包含gnss杆臂误差，第三个模型仅包含里程计杆臂误差。其详细内容如下：
[0050]
1.1选取合适的系统状态向量。经典gnss/ins组合导航滤波模型选取向量为21维，包含组合导航的位置误差、速度误差、姿态误差、陀螺仪和加速度计的零偏误差和比例因子误差。但是对于低成本mems imu，由于随机误差较大，比例因子误差建模不但对滤波性能的提升有限，反而增加了计算量。因此本发明选取位置误差、速度误差、姿态误差、陀螺仪和加速度计的零偏误差，以及gnss天线杆臂误差和里程计杆臂误差共计21维矢量作为系统状态向量，具体为：
[0051]
xk＝[(δrn)
t
(δvn)
t
φ
t b
gt b
at lever
gnsst lever
odot
]
t
ꢀꢀꢀꢀ
(3)
[0052]
其中，(δrn)
t
、(δvn)
t
、φ
t
分别表示组合导航的位置误差、速度误差和姿态误差，b
gt
、b
at
分别表示陀螺仪和加速度计的零偏误差，lever
gnsst
、lever
odot
分别表示gnss天线杆臂误差和里程计杆臂误差。
[0053]
1.2构建系统状态方程：
[0054]
xk＝φ
k,k-1
x
k-1
+γ
k-1wk-1
ꢀꢀꢀꢀꢀꢀ
(4)
[0055]
式中，xk、x
k-1
分别为k时刻、k-1时刻系统的状态向量，φ
k,k-1
表示k-1到k时刻的状态一步转移矩阵，γ
k-1
为噪声相关的矩阵，w
k-1
为系统噪声。其中三个卡尔曼滤波模型的转移矩阵分别表示为：
[0056]
同时估计时转移矩阵φ
k,k-1
(1)构造为
[0057][0058]
仅估计gnss杆臂时转移矩阵φ
k,k-1
(2)构造为
[0059][0060]
仅估计里程计杆臂时转移矩阵φ
k,k-1
(3)构造为
[0061]
[0062]
其中，i3是三维单位矩阵，fn是n系下的比力，t
gb
和t
ab
分别是陀螺和加速度计零偏相关时间。
[0063]
1.3构建系统观测方程：
[0064]
zk＝hkxk+vkꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0065]
式中，zk为k时刻的观测值，hk为k时刻的观测矩阵，xk为状态向量预测值，vk为观测噪声。其中三个卡尔曼滤波模型的观测矩阵分别表示为：
[0066]
同时估计时观测矩阵h1构造为
[0067][0068]
仅估计gnss杆臂时观测矩阵h2构造为
[0069][0070]
仅估计里程计杆臂时观测矩阵h3构造为
[0071][0072]
其中，i3×3是维度为3的单位矩阵、03×3是维度为3的全零矩阵、是载体系到导航系的转移矩阵、载体系相对于导航系的角速度。
[0073]
步骤2：获取gnss、ins和odo的数据。gnss/ins/odo导航系统包括一个微处理器、一个gnss接收机模块、一个惯性导航单元和一个里程计。微处理器通过串口、串行外设接口、定时器分别连接gnss接收机惯性导航单元和里程计，以获取gnss、ins和odo数据。gnss数据包括位置、卫星信号强度和时间戳等信息；ins数据包括角速度、加速度等信息；odo数据包括车辆的前向位置增量和速度信息。
[0074]
步骤3：根据需求选择对应的系统状态转移矩阵和观测矩阵。默认使用同时估计gnss杆臂和里程计杆臂模型。如有需要，变量宏控制选择其他的系统状态转移矩阵和观测矩阵。
[0075]
步骤4：利用卡尔曼滤波的预测和更新过程在线估计gnss杆臂和里程计杆臂。
[0076]
一步预测过程：
[0077][0078][0079]
式中，为一步预测的状态值，为k-1时刻的状态最优估计值，p
k,k-1
为一步预测的状态协方差矩阵，p
k-1
为k-1时刻的状态协方差最优估计值，q
k-1
为k-1时刻的系统噪声协方差矩阵。
[0080]
[0081][0082][0083]
式中，kk为增益矩阵，rk为量测噪声协方差矩阵，为k时刻的状态最优估计值，pk为k时刻的状态协方差最优估计值。
[0084]
更新后得到包含gnss杆臂误差和里程计杆臂误差的状态向量，结合初始设定值或者前一历元的杆臂估计值计算得到当前历元的杆臂估计值。
[0085]
实施例2
[0086]
基于同样的发明构思，本实施例还提供一种用于gnss天线杆臂和里程计杆臂的在线估计系统，包括
[0087]
第一模块：被配置为用于构建三个卡尔曼滤波模型，第一个模型包含gnss杆臂误差和里程计杆臂误差，第二个模型仅包含gnss杆臂误差，第三个模型仅包含里程计杆臂误差；
[0088]
第二模块：被配置为用于获取gnss、ins和odo的数据；
[0089]
第三模块：被配置为根据不同的需求选择对应的系统状态转移矩阵和观测矩阵；其中滤波模型的选择通过一个全局变量控制；
[0090]
第四模块：被配置为利用卡尔曼滤波的预测和更新过程在线估计gnss杆臂和里程计杆臂。
[0091]
实施例3
[0092]
基于同样的发明构思，本实施例还提供一种电子设备，存储计算机可执行指令的计算机可读存储介质；和一个或多个处理器，所述一个或多个处理器耦接到所述计算机可读存储介质并被配置为执行所述计算机可执行指令，以使得所述设备执行实施例1所述的方法。
[0093]
实施例4
[0094]
基于同样的发明构思，本实施例还提供一种可读存储介质，存储有计算机可执行指令，所述计算机可执行指令当由处理器执行时，将处理器配置为执行实施例1所述的方法。
[0095]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。本技术实施例中的方案可以采用各种计算机语言实现，例如，面向对象的程序设计语言java和直译式脚本语言javascript等。
[0096]
本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0097]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0098]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0099]
尽管已描述了本技术的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本技术范围的所有变更和修改。
[0100]
显然，本领域的技术人员可以对本技术进行各种改动和变型而不脱离本技术的精神和范围。这样，倘若本技术的这些修改和变型属于本技术权利要求及其等同技术的范围之内，则本技术也意图包含这些改动和变型在内。

技术特征：
1.一种用于gnss天线杆臂和里程计杆臂的在线估计方法，其特征在于，包括构建三个卡尔曼滤波模型，第一个模型包含gnss杆臂误差和里程计杆臂误差，第二个模型仅包含gnss杆臂误差，第三个模型仅包含里程计杆臂误差；获取gnss、ins和odo的数据；根据不同的需求选择对应的系统状态转移矩阵和观测矩阵；其中滤波模型的选择通过一个全局变量控制；利用卡尔曼滤波的预测和更新过程在线估计gnss杆臂和里程计杆臂。2.根据权利要求1所述的一种用于gnss天线杆臂和里程计杆臂的在线估计方法，其特征在于，卡尔曼滤波模型具体构建过程包括构建系统状态方程和观测方程式中，x
k
、x
k-1
分别为k时刻、k-1时刻系统的状态向量，φ
k,k-1
表示k-1到k时刻的状态一步转移矩阵，γ
k-1
为噪声相关的矩阵，z
k
为k时刻的观测值，h
k
为k时刻的观测矩阵，w
k-1
为系统噪声，v
k
为观测噪声；其中w
k-1
和v
k
是白噪声，且互不相关；构建状态向量x
k
＝[x
0 lever
gnsst lever
odot
]
t
ꢀꢀꢀꢀꢀ
(2)其中，x0表示组合导航的位置误差、速度误差、姿态误差陀螺仪和加速度计的零偏误差，lever
gnsst
、lever
odot
分别表示gnss天线杆臂误差和里程计杆臂误差。3.根据权利要求1所述的一种用于gnss天线杆臂和里程计杆臂的在线估计方法，其特征在于，获取gnss、ins和odo的数据时，微处理器通过串口、串行外设接口、定时器分别连接gnss接收机惯性导航单元和里程计，以获取gnss、ins和odo数据；gnss数据包括位置、卫星信号强度和时间戳信息；ins数据包括角速度、加速度信息；odo数据包括车辆的前向位置增量和速度信息。4.根据权利要求1所述的一种用于gnss天线杆臂和里程计杆臂的在线估计方法，其特征在于，根据不同的需求选择对应的系统状态转移矩阵和观测矩阵。5.根据权利要求1所述的一种用于gnss天线杆臂和里程计杆臂的在线估计方法，其特征在于，在线估计过程包括：预测过程和更新过程；在预测过程中，卡尔曼滤波算法使用车辆的动态模型来预测车辆的导航信息、gnss杆臂向量和里程计杆臂向量等信息，并计算预测值的方差；在更新过程中，卡尔曼滤波算法使用gnss、ins和odo的数据来计算卡尔曼增益，利用卡尔曼增益修正预测值；卡尔曼增益的计算基于观测值的方差和预测值的方差之比，越准确的观测值对预测值的修正权重越大；更新过程后得到包含gnss杆臂误差和里程计杆臂误差的状态向量，结合初始设定值或者前一历元的杆臂估计值计算得到当前历元的杆臂估计值。6.一种用于gnss天线杆臂和里程计杆臂的在线估计系统，其特征在于，包括第一模块：被配置为用于构建三个卡尔曼滤波模型，第一个模型包含gnss杆臂误差和里程计杆臂误差，第二个模型仅包含gnss杆臂误差，第三个模型仅包含里程计杆臂误差；
第二模块：被配置为用于获取gnss、ins和odo的数据；第三模块：被配置为根据不同的需求选择对应的系统状态转移矩阵和观测矩阵；其中滤波模型的选择通过一个全局变量控制；第四模块：被配置为利用卡尔曼滤波的预测和更新过程在线估计gnss杆臂和里程计杆臂。7.根据权利要求6所述的系统，其特征在于，卡尔曼滤波模型具体构建过程包括构建系统状态方程和观测方程式中，x
k
、x
k-1
分别为k时刻、k-1时刻系统的状态向量，φ
k,k-1
表示k-1到k时刻的状态一步转移矩阵，γ
k-1
为噪声相关的矩阵，z
k
为k时刻的观测值，h
k
为k时刻的观测矩阵，w
k-1
为系统噪声，v
k
为观测噪声；其中w
k-1
和v
k
是白噪声，且互不相关；构建状态向量x
k
＝[x
0 lever
gnsst lever
odot
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀ
(2)其中，x0表示组合导航的位置误差、速度误差、姿态误差陀螺仪和加速度计的零偏误差，lever
gnsst
、lever
odot
分别表示gnss天线杆臂误差和里程计杆臂误差。8.根据权利要求7所述的系统，其特征在于，获取gnss、ins和odo的数据时，微处理器通过串口、串行外设接口、定时器分别连接gnss接收机惯性导航单元和里程计，以获取gnss、ins和odo数据；gnss数据包括位置、卫星信号强度和时间戳信息；ins数据包括角速度、加速度信息；odo数据包括车辆的前向位置增量和速度信息；根据不同的需求选择对应的系统状态转移矩阵和观测矩阵；在线估计过程包括：预测过程和更新过程；在预测过程中，卡尔曼滤波算法使用车辆的动态模型来预测车辆的导航信息、gnss杆臂向量和里程计杆臂向量等信息，并计算预测值的方差；在更新过程中，卡尔曼滤波算法使用gnss、ins和odo的数据来计算卡尔曼增益，利用卡尔曼增益修正预测值；卡尔曼增益的计算基于观测值的方差和预测值的方差之比，越准确的观测值对预测值的修正权重越大；更新过程后得到包含gnss杆臂误差和里程计杆臂误差的状态向量，结合初始设定值或者前一历元的杆臂估计值计算得到当前历元的杆臂估计值。9.一种电子设备，其特征在于，存储计算机可执行指令的计算机可读存储介质；和一个或多个处理器，所述一个或多个处理器耦接到所述计算机可读存储介质并被配置为执行所述计算机可执行指令，以使得所述设备执行根据权利要求1-5中任一项所述的方法。10.一种可读存储介质，其特征在于，存储有计算机可执行指令，所述计算机可执行指令当由处理器执行时，将处理器配置为执行根据权利要求1-5中任一项所述的方法。

技术总结
本发明涉及一种用于GNSS天线杆臂和里程计杆臂的在线估计方法、设备及介质，该方法通过将GNSS天线杆臂误差和里程计杆臂误差拓展到Kalman滤波状态向量中，在导航结算时，实时估计杆臂向量，无需人工操作测量杆臂向量，避免非专业人员操作困难，同时减少了量测误差的产生，该方法即可同时估计GNSS天线杆臂和里程计杆臂，也可单独估计GNSS天线杆臂和里程计杆臂向量，实用性较好。实用性较好。实用性较好。


技术研发人员：江金光 严培辉
受保护的技术使用者：武汉大学
技术研发日：2023.04.28
技术公布日：2023/8/9