基于粒子滤波的GNSS实时高精度单差测姿方法

基于粒子滤波的gnss实时高精度单差测姿方法
技术领域
1.本发明涉及gnss导航定位技术领域，具体而言，涉及基于粒子滤波的gnss实时高精度单差测姿方法。


背景技术：

2.gnss(global navigation satellite system，全球导航卫星系统)测姿技术可为陆、海、空、天的移动载体提供实时的三维姿态（航向角、俯仰角以及横滚角）信息，且相较于传统的惯性测量系统，具有长期稳定性好、误差不随时间累积、成本低、体积小、重量轻以及维护简单等优点，在智能交通、精准农业、高精度测绘、航海航空、港口机械等民用领域有着广阔的应用前景。
3.在gnss测姿中，利用短基线相对定位技术求解出天线间精确的基线向量是获取载体精确姿态的前提，其观测模型广泛采用双差模型。然而受几何观测条件的限制，基于双差模型获取的垂直方向基线分量精度比平面方向差2~3倍，这就使得与垂直方向基线分量紧密相关的俯仰角和横滚角的精度明显低于航向角，进而严重制约了gnss测姿技术在实时高精度姿态测量领域的应用能力和水平。
4.随着共时钟gnss接收机产品的出现和日益成熟，利用其两端接收机钟差相同的特性，可以采用单差观测模型提升基线向量和姿态角测量的精度。目前已有的方法或是将相位线缆偏差参数与模糊度参数进行参数重组而只能固定双差模糊度，或是事后处理模式而无法应用于实时测姿应用中，尚未有有效的实时快速准确估计方法。


技术实现要素：

5.本发明解决的问题是如何获取高精度的基线向量和姿态测量结果。
6.为解决上述问题，本发明提供一种基于粒子滤波的gnss实时高精度单差测姿方法，包括如下步骤：初始化若干粒子集；通过共时钟接收机获取相位和伪距观测值，根据所述相位和所述伪距观测值构建gnss单差相对定位模型；求解所述gnss单差相对定位模型的待估参数，得到单差模糊度浮点解和方差协方差矩阵；将所述粒子集内的各粒子值逐个改正到单差模糊度参数的单差模糊度浮点解中，使用最小二乘模糊度降相关平差法固定单差模糊度参数，并计算各个粒子对应的ratio值；根据所述ratio值更新粒子权值并归一化，得到所有粒子的样本均值和样本方差；根据所述样本方差计算有效样本容量，判断是否需要对粒子进行重采样，并对下个历元粒子进行预测；将所述样本均值作为相位的线缆偏差值改正到所述单差模糊度浮点解中，并使用最小二乘模糊度降相关平差法解算单差模糊度参数固定解；根据所述单差模糊度参数固定解计算基线向量固定解，根据所述基线向量固定解解算得到姿态角。
7.可选地，所述根据所述相位和所述伪距观测值构建gnss单差相对定位模型，具体包括：基于共时钟接收机输出的gnss单频载波相位与伪距观测值建立接收机和卫星的非差伪距和载波相位观测方程；根据所述非差伪距和载波相位观测方程建立基于共时钟接收机
的单差伪距和载波相位观测方程；根据所述共时钟接收机的单差伪距和载波相位观测方程构建线性化后的基于共时钟接收机的gnss单差相对定位模型。
8.可选地，所述求解所述gnss单差相对定位模型的待估参数，得到单差模糊度浮点解和方差协方差矩阵，具体包括：建立gnss单差相对定位模型对应的方差协方差矩阵；gnss单差相对定位模型中的随机模型使用高度角模型，对于每个卫星到接收机的非差观测值，根据第一公式计算所述非差观测值的方差，其中所述第一公式包括：其中、分别为流动站非差观测值的方差和基准站非差观测值的方差，表示卫星高度角，表示经验参数；求解所述gnss单差相对定位模型，得到待估参数单差模糊度浮点解。
9.可选地，所述将所述粒子集内的各粒子值逐个改正到单差模糊度参数的单差模糊度浮点解中，具体包括：对每一个粒子根据第二公式进行改正，其中所述第二公式包括：其中，表示单个粒子值，表示单差模糊度浮点解，表示改正后的单差模糊度浮点解，n表示卫星个数。
10.可选地，所述根据所述ratio值更新粒子权值并归一化，得到所有粒子的样本均值和样本方差，具体包括：将ratio值与相位线缆偏差的关系曲线视为观测值关于相位线缆偏差的似然函数，对粒子权值进行更新；对更新后的权值进行归一化，得到归一化后的粒子权值；根据所述归一化后的粒子权值计算样本粒子的样本均值和样本方差。
11.可选地，所述将所述样本均值作为相位的线缆偏差值改正到所述单差模糊度浮点解中，具体包括根据第三公式进行改正，其中所述第三公式包括：其中，表示样本均值，表示单差模糊度浮点解，表示改正后的单差模糊度浮点解，n表示卫星个数。
12.可选地，所述根据所述单差模糊度参数固定解计算基线向量固定解，具体包括：根据所述单差模糊度参数固定解使用第四公式计算基线向量固定解及其方差协方差矩阵，其中所述第四公式包括：
其中，表示基线向量固定解，、、表示三维基线向量固定解，表示伪距线缆偏差固定解，表示的方差矩阵，表示基线向量浮点解，表示的方差矩阵，表示单差模糊度固定解，表示单差模糊度浮点解中非单差模糊度参数与单差模糊度参数的方差协方差矩阵，表示单差模糊度浮点解单差模糊度参数的方差协方差矩阵。
13.可选地，所述根据所述基线向量固定解解算得到姿态角，具体包括：根据第五公式将地心地固坐标系的基线向量固定解转换到站心地平坐标系，所述第五公式包括：其中，b、l表示主天线所在位置的大地纬度与大地经度，表示站心地平坐标系下的基线向量，表示地心地固坐标系基线向量，表示站心地平坐标系基线向量，、、分别表示站心地平坐标系下北方向、东方向、天方向的基线分量；根据站心地平坐标系下的基线向量，根据第六公式计算载体姿态角，其中所述第六公式包括：其中，yaw为航向角，pitch为俯仰角。
14.可选地，所述根据所述样本方差计算有效样本容量，判断是否需要对粒子进行重采样，具体包括：根据第七公式计算有效样本容量，其中所述第七公式包括：其中，表示归一化后的粒子权值，n表示粒子数，，表示粒子集
中粒子的编号；并根据判断是否进行重采样。
15.可选地，所述对下个历元粒子进行预测，具体包括：根据第八公式对下个历元的粒子进行预测得到下个历元的预测的粒子，其中所述第八公式包括：其中，表示过程噪声，表示历元的粒子值，表示历元的粒子值，，表示粒子集中粒子的编号，其中n表示粒子数。
16.与现有技术相比，本发明具有以下技术效果：本发明通过使用粒子的样本均值作为gnss单差测姿中相位线缆偏差的估计值，实现了gnss单差测姿中相位线缆偏差实时快速准确的估计。可以在没有相位线缆偏差先验信息、不增加额外待估参数的情况下，实现相位线缆偏差的实时快速准确估计和单差模糊度的固定。进而实现了获取高精度的基线向量和姿态测量结果，提升了姿态测量的精度，尤其是俯仰角和航向角的精度。
附图说明
17.图1为本发明实施例所述的基于粒子滤波的gnss实时高精度单差测姿方法的流程图。
具体实施方式
18.随着共时钟gnss接收机产品的出现和应用，利用其两端接收机钟差相同的特性，可以采用gnss单差相对定位模型提升基线向量和姿态角测量的精度。尚未有有效的实时快速准确估计gnss单差相位线缆偏差值的方法。为此，本发明提供一种基于粒子滤波的gnss实时高精度单差测姿方法，通过使用粒子的样本均值估计相位线缆偏差值，从而提升基线向量和姿态角测量的精度。
19.为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
20.结合图1所示，本发明实施例提供一种基于粒子滤波的gnss实时高精度单差测姿方法，包括如下步骤：通过共时钟接收机获取相位和伪距观测值，根据所述相位和所述伪距观测值构建gnss单差相对定位模型；求解所述gnss单差相对定位模型的待估参数，得到单差模糊度浮点解和方差协方差矩阵；将粒子集初始化并将所述粒子集内的各粒子值逐个改正到单差模糊度参数的单差模糊度浮点解中，使用最小二乘模糊度降相关平差法固定单差模糊度参数，并计算各个粒子对应的ratio（比例）值；根据所述ratio值更新粒子权值并归一化，得到所有粒子的样本均值和样本方差；根据所述样本方差计算有效样本容量，判断是否需要对粒子进行重采样，并对下个历元粒子进行预测；将所述样本均值作为相位的线缆偏差值改正到所述单差模糊度浮点解中，并使用最小二乘模糊度降相关平差法解算单差模糊度参数固定解；根据所述单差模糊度参数固定解计算基线向量固定解，根据所述基线向
量固定解解算得到姿态角。
21.具体地，在本实施例中，“初始化若干粒子集”指的是，如果当前历元是首历元，那么将粒子集初始化，使用初始化后的粒子集。
22.其中，在首历元中使用如下方式得到粒子集：在[
‑
0.5,0.5]之间采样n个粒子，粒子权值为，得到粒子集，分别表示粒子值及其权值。其中闭区间[
‑
0.5,0.5]的单位为周，所述采样可以使用随机采样或者均匀采样n个粒子，n按经验取值，例如n=200。
[0023]
需要说明的是，如果当前历元不是首历元，那么需要使用预测后的粒子集。
[0024]
基于共时钟接收机采集的相位和伪距观测值，建立单差相位与伪距观测方程，构建基于共时钟接收机的gnss单差相对定位模型，所述gnss单差相对定位模型包括函数模型与随机模型。
[0025]
求解定位模型，得到基线向量、单差模糊度参数以及伪距的线缆偏差参数的单差模糊度浮点解及其方差协方差矩阵。
[0026]
将所述粒子集内的各粒子值逐个改正到单差模糊度参数的单差模糊度浮点解中，使用最小二乘模糊度降相关平差法固定单差模糊度参数，并计算各个粒子对应的ratio值。
[0027]
根据粒子对应的ratio值，更新粒子权值，记为，并归一化得到归一化后的粒子权值，记为。然后计算所有粒子的样本均值，记为；样本方差，记为。
[0028]
根据样本方差计算有效样本容量，判断是否需要对粒子进行重采样，并对下个历元粒子进行预测。
[0029]
根据粒子的样本均值，将其作为相位线缆偏差值，改正到单差模糊度浮点解上，并使用最小二乘模糊度降相关平差法固定单差模糊度参数。
[0030]
根据固定的单差模糊度参数，计算基线向量固定解。将地心地固坐标系基线向量转换到站心地平坐标系后计算载体姿态角，包括航向角和俯仰角。
[0031]
在本实施例中，通过使用粒子的样本均值作为gnss单差测姿中相位线缆偏差的估计值，实现了gnss单差测姿中相位线缆偏差实时快速准确的估计。可以在没有相位线缆偏差先验信息、不增加额外待估参数的情况下，实现相位线缆偏差的实时快速准确估计和单差模糊度的固定。进而实现了获取高精度的基线向量和姿态测量结果，提升了姿态测量的精度，尤其是俯仰角和航向角的精度。
[0032]
可选地，所述根据所述相位和所述伪距观测值构建gnss单差相对定位模型，具体包括：基于共时钟接收机输出的gnss单频载波相位与伪距观测值建立接收机和卫星的非差伪距和载波相位观测方程；根据所述非差伪距和载波相位观测方程建立基于共时钟接收机的单差伪距和载波相位观测方程；根据所述共时钟接收机的单差伪距和载波相位观测方程构建线性化后的基于共时钟接收机的gnss单差相对定位模型。
[0033]
具体地，在本实施例中，构建基于共时钟接收机的gnss单差相对定位模型的方法如下：步骤s1：基于共时钟接收机输出的gnss单频载波相位与伪距观测值，建立接收机
和卫星的非差伪距和载波相位观测方程如下：其中，、分别表示伪距与载波相位观测值，表示信号发射时刻卫星与接收机间的几何距离；、分别表示接收机钟差与卫星钟差，、分别表示接收机端与卫星端的伪距硬件延迟，、分别表示接收机端与卫星端的初始相位；、分别表示接收机端与卫星端的相位硬件延迟；表示模糊度参数；、分别表示电离层延迟与对流层延迟误差；、分别表示伪距和载波相位噪声，表示光速；表示波长。
[0034]
步骤s2：接收机的站间单差可消除卫星端的卫星钟差、初始相位以及伪距和相位硬件延迟等公共误差。同时，忽略电离层延迟和对流层延迟误差的影响，并顾及到共时钟接收机两端的接收机钟差相同，根据步骤s1中的非差伪距和载波相位观测方程，可建立基于共时钟接收机的单差伪距和载波相位观测方程如下：其中分别为相位和伪距的线缆偏差。
[0035]
步骤s3：根据步骤s2中的基于共时钟接收机的单差伪距和载波相位观测方程，将相位线缆偏差参数赋值0，即不考虑相位线缆偏差参数，构建线性化后的基于共时钟接收机的gnss单差相对定位模型中的函数模型如下：
其中，为设计矩阵；为三维基线向量。
[0036]
在本实施例中，通过基于共时钟接收机采集的相位和伪距观测值，建立单差相位与伪距观测方程，构建了基于共时钟接收机的gnss单差相对定位模型。进而可以采用gnss单差相对定位模型提升基线向量和姿态角测量的精度。
[0037]
可选地，所述求解所述gnss单差相对定位模型的待估参数，得到单差模糊度浮点解和方差协方差矩阵，具体包括：建立gnss单差相对定位模型对应的方差协方差矩阵；gnss单差相对定位模型中的随机模型使用高度角模型，对于每个卫星到接收机的非差观测值，根据第一公式计算所述非差观测值的方差，其中所述第一公式包括：其中、分别为流动站非差观测值的方差和基准站非差观测值的方差，表示卫星高度角，表示经验参数；求解所述gnss单差相对定位模型，得到待估参数单差模糊度浮点解。
[0038]
具体地，在本实施例中，建立gnss单差相对定位模型对应的方差协方差矩阵：其中，表示伪距的方差矩阵、表示相位观测值的方差矩阵、表示对角矩阵。
[0039]
其中，gnss单差相对定位模型中的随机模型采用高度角模型，对于每个卫星到接收机的非差观测值，根据第一公式计算所述非差观测值的方差，其中所述第一公式包括：其中、分别为流动站非差观测值的方差和基准站非差观测值的方差，表示卫星高度角，表示经验参数。优选地，载波相位观测值的取0.003m，伪距的值取0.3m。
[0040]
求解所述gnss单差相对定位模型，得到待估参数单差模糊度浮点解：其中，表示三维基线向量估计值，表示线缆偏差估计值，表示单差模糊度参数估计值。
[0041]
在本实施例中，通过求解所述gnss单差相对定位模型的待估参数，得到单差模糊
度浮点解和方差协方差矩阵，构建了基于共时钟接收机的gnss单差相对定位模型。进而可以采用gnss单差相对定位模型提升基线向量和姿态角测量的精度。
[0042]
可选地，所述将所述粒子集内的各粒子值逐个改正到单差模糊度参数的单差模糊度浮点解中，具体包括：对每一个粒子根据第二公式进行改正，其中所述第二公式包括：其中，表示单个粒子值，表示单差模糊度浮点解，表示改正后的单差模糊度浮点解，n表示卫星个数。
[0043]
具体地，在本实施例中需要注意的是，对每一个粒子根据第二公式进行改正的步骤中，表示改正后的单差模糊度浮点解，此处与第三公式中的使用相同的字母表示，但是表达的含义不同，此处是将粒子值改正到单差模糊度参数的单差模糊度浮点解中。
[0044]
在本实施例中，通过将粒子集初始化并将所述粒子集内的各粒子值逐个改正到单差模糊度参数的单差模糊度浮点解中，构建了基于共时钟接收机的gnss单差相对定位模型。进而可以采用gnss单差相对定位模型提升基线向量和姿态角测量的精度。
[0045]
可选地，所述根据所述ratio值更新粒子权值并归一化，得到所有粒子的样本均值和样本方差,具体包括：将ratio值与相位线缆偏差的关系曲线视为观测值关于相位线缆偏差的似然函数，对粒子权值进行更新；对更新后的权值进行归一化，得到归一化后的粒子权值；根据所述归一化后的粒子权值计算样本粒子的样本均值和样本方差。
[0046]
具体地，在本实施例中，将ratio值与相位线缆偏差的关系曲线视为观测值关于相位线缆偏差的似然函数，得到更新后的粒子权值：然后对更新后的权值进行归一化，得到归一化后的粒子权值：最后计算所有粒子的样本均值和样本方差：
其中所述样本方差要求满足一定精度。
[0047]
在本实施例中，通过ratio值更新粒子权值并归一化，得到所有粒子的样本均值和方差，得到了满足条件的样本均值，构建了基于共时钟接收机的gnss单差相对定位模型。进而可以采用gnss单差相对定位模型提升基线向量和姿态角测量的精度。
[0048]
可选地，所述将所述样本均值作为相位的线缆偏差值改正到所述单差模糊度浮点解中,具体包括根据第三公式进行改正，其中所述第三公式包括：其中，表示样本均值，表示单差模糊度浮点解，表示改正后的单差模糊度浮点解，n表示卫星个数。
[0049]
具体地，在本实施例中，需要注意的是，根据第三公式将所述样本均值作为相位的线缆偏差值改正到所述单差模糊度浮点解中，表示改正后的单差模糊度浮点解，此处与第二公式中的使用相同的字母表示，但是表达的含义不同，此处是将样本均值改正到单差模糊度浮点解中。
[0050]
在本实施例中，通过将样本均值作为相位的线缆偏差值改正到所述单差模糊度浮点解中，得到了改正后的单差模糊度浮点解，构建了基于共时钟接收机的gnss单差相对定位模型。进而可以采用gnss单差相对定位模型提升基线向量和姿态角测量的精度。
[0051]
可选地，所述根据所述单差模糊度参数固定解计算基线向量固定解，具体包括：根据所述单差模糊度参数固定解使用第四公式计算基线向量固定解及其方差协方差矩阵，其中所述第四公式包括：其中，表示基线向量固定解，、、表示三维基线向量固定解，表示伪距线缆偏差固定解，表示的方差矩阵，表示基线向量浮点解，表示的方差矩阵，表示单差模糊度固定解，表示单差模糊度浮点解中非单差模糊度参数与单差模糊度参数的方差协方差矩
阵，表示单差模糊度浮点解单差模糊度参数的方差协方差矩阵。
[0052]
具体地，在本实施例中，使用最小二乘模糊度降相关平差法固定单差模糊度参数，得到单差模糊度固定解。根据所述单差模糊度参数固定解使用第四公式计算基线向量固定解及其方差协方差矩阵。
[0053]
在本实施例中，根据单差模糊度参数固定解，计算得到了基线向量固定解，构建了基于共时钟接收机的gnss单差相对定位模型，采用gnss单差相对定位模型提升了基线向量测量的精度。
[0054]
可选地，所述根据所述基线向量固定解解算得到姿态角，具体包括：根据第五公式将地心地固坐标系的基线向量固定解转换到站心地平坐标系，所述第五公式包括：其中，b、l表示主天线所在位置的大地纬度与大地经度，表示站心地平坐标系下的基线向量，表示地心地固坐标系基线向量，表示站心地平坐标系基线向量，、、分别表示站心地平坐标系下北方向、东方向、天方向的基线分量；根据站心地平坐标系下的基线向量，根据第六公式计算载体姿态角，其中所述第六公式包括：其中，yaw为航向角，pitch为俯仰角。
[0055]
具体地，在本实施例中，使用坐标变换公式将将地心地固坐标系的基线向量固定解转换到站心地平坐标系，根据站心地平坐标系下的基线向量计算载体姿态角。
[0056]
在本实施例中，通过使用坐标变换公式将将地心地固坐标系的基线向量固定解转换到站心地平坐标系，可以更方便的计算载体姿态角。构建了基于共时钟接收机的gnss单差相对定位模型，采用gnss单差相对定位模型提升了载体姿态角测量的精度。
[0057]
可选地，所述根据所述样本方差计算有效样本容量，判断是否需要对粒子进行重采样，具体包括：根据第七公式计算有效样本容量，其中所述第七公式包括：
其中，表示归一化后的粒子权值，n表示粒子数，，表示粒子集中粒子的编号；并根据判断是否进行重采样。
[0058]
具体地，在本实施例中，根据归一化后的粒子权值，计算滤波有效样本容量：如果，则使用多项式重采样或分层重采样或系统重采样或拒绝重采样或均匀重采样方法进行重采样。
[0059]
在本实施例中，通过归一化后的粒子权值计算滤波有效样本容量，可以提高采样的质量，从而使构建的gnss单差相对定位模型更精确，进而可以采用gnss单差相对定位模型提升基线向量和姿态角测量的精度。
[0060]
可选地，所述对下个历元粒子进行预测，具体包括：根据第八公式对下个历元的粒子进行预测得到下个历元的预测的粒子，其中所述第八公式包括：其中，表示过程噪声，表示历元的粒子值，表示历元的粒子值，，表示粒子集中粒子的编号，其中n表示粒子数。
[0061]
具体地，在本实施例中提供了一种预测得到下个历元的预测的粒子的方法，其中表示过程噪声，所述过程噪声为一个经验参数。
[0062]
在本实施例中，通过使用第八公式对下个历元的粒子进行预测，可以实时修正gnss单差相对定位模型，使模型更准确，进而可以采用gnss单差相对定位模型提升基线向量和姿态角测量的精度。
[0063]
虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。