用于低速检测平台系统的惯性误差抑制方法与流程

1.本发明涉及惯性轨道检测技术领域，尤其涉及一种用于低速检测平台系统的惯性误差抑制方法。


背景技术：

2.在轨道精调过程中，利用全站仪和cpiii点进行精确定位仍是目前绝对测量的主要手段。利用全站仪可以获得单点亚毫米级精度位置信息，实际测量时一般每60m～120m设一个站点，站点之间的测量精度则通过惯性组合导航系统来实现。差分卫星接收机的精度容易受到多种因素干扰，这些干扰产生的误差量级与低速检测平台工作距离相比比例较大，多数情况下无法忽略，因此低速检测平台一般采用惯性/里程计组合测量全站仪站点间的轨道参数，并利用站点处的高精度定位信息修正惯性/里程计组合的累计误差。但由于站点的稀疏性，无法直接利用站点定位结果作为观测量对惯导系统的航向误差进行更深入的修正，导致修正后的测量轨迹会出现驻波状轨迹误差，站点输出误差为0。


技术实现要素：

3.本发明提供了一种用于低速检测平台系统的惯性误差抑制方法，能够解决现有技术中无法直接利用站点定位结果作为观测量对惯导系统的航向误差进行更深入的修正的技术问题。
4.本发明提供了一种用于低速检测平台系统的惯性误差抑制方法，惯性误差抑制方法包括：获取里程计在载体坐标系下输出的里程；基于里程计在载体坐标系下输出的里程计算获取里程计在载体坐标系下的位置增量；将惯性导航系统在载体坐标系下的位置增量与里程计在载体坐标系下的位置增量的差值作为系统观测量，根据系统观测量获取观测矩阵；获取系统状态转移矩阵；基于系统观测矩阵和系统状态转移矩阵，通过卡尔曼滤波算法，对状态变量进行估计，基于估计后的状态变量对里程计安装误差进行修正；基于修正后的里程计安装误差，获取修正完里程计误差后由于陀螺漂移引起的侧向累计位置误差，基于由于陀螺漂移引起的侧向累计位置误差获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。
5.进一步地，在完成对轨道检测平台系统的惯性误差修正之后，惯性误差抑制方法还包括：将修正后的轨道检测平台系统的惯性误差与设定惯性误差精度阈值范围进行比较，当修正后的轨道检测平台系统的惯性误差超出设定惯性误差精度阈值范围时，重复上述步骤，直至修正后的轨道检测平台系统的惯性误差处于设定惯性误差精度阈值范围内。
6.进一步地，里程计在载体坐标系下输出的里程可根据来获取，其中，为里程计在k时刻在载体坐标系下输出的里程，为里程计与惯性导航系统之间的安装关系矩阵，kd为里程计刻度系数，为里程计在里程计坐标系下的脉冲数矢量形式，
nk为里程计在第k个采样周期内输出的脉冲数。
7.进一步地，里程计在载体坐标系下的位置增量可根据来获取，其中，为里程计在载体坐标系下的位置增量，δα
θ
为俯仰角误差，δα
ψ
为航向角误差，δkd为里程计刻度系数误差，为里程计在k时刻在载体坐标系下沿x轴输出的里程，为里程计在k时刻在载体坐标系下沿y轴输出的里程，为里程计在k时刻在载体坐标系下沿z轴输出的里程，x为状态变量。
8.进一步地，系统观测量可根据获取，其中，为惯性导航系统在载体坐标系下的位置增量，hk为观测矩阵，
9.进一步地，惯性导航系统在载体坐标系下的位置增量可根据来获取，其中，为惯性导航系统在导航坐标系下的位置增量，为k时刻惯性导航系统在导航坐标系下的速度，为k 1时刻惯性导航系统在导航坐标系下的速度，ts为计算周期。
10.进一步地，基于估计后的状态变量对里程计安装误差进行修正具体包括：基于估计后的状态变量对惯性导航系统与里程计之间的安装关系矩阵以及里程计刻度系数进行修正以完成对里程计安装误差进行修正。
11.进一步地，惯性导航系统与里程计之间的安装关系矩阵可根据进行修正，里程计刻度系数可根据k
d,k 1
＝(1 δk
d,k
)k
d,k
进行修正，其中，为k 1时刻里程计与惯性导航系统之间的安装关系矩阵，为k时刻里程计与惯性导航系统之间的安装关系矩阵，k
d,k 1
为k 1时刻里程计刻度系数，k
d,k
为k时刻里程计刻度系数。
12.进一步地，陀螺漂移等效角速度可根据来获取，其中，δx(t)为由于陀螺漂移引起的侧向累计位置误差，ω为陀螺漂移等效角速率，t为从上一个站点出发后经过的时间，v为当前数据段的平均推行速度，τ为任一时刻。
13.应用本发明的技术方案，提供了一种用于低速检测平台系统的惯性误差抑制方法，该惯性误差抑制方法通过获取观测矩阵和系统状态转移矩阵，基于卡尔曼滤波算法，对状态变量进行估计，并根据估计后的状态变量对里程计安装误差进行修正；在对里程计安装误差进行修正后，可通过站点处位置误差反向推导航向误差，即获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。此种方式能够显著降低全站仪定点修正引起的驻波状误差，相比传统惯性/里程计组合导航算法对惯导系统的误差的抑制效果更加显著。
附图说明
14.所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
15.图1示出了根据本发明的具体实施例提供的两条曲线的航向差值变化的示意图。
具体实施方式
16.需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
17.需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本技术的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
18.除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。
19.根据本发明的具体实施例，提供了一种用于低速检测平台系统的惯性误差抑制方
法，该惯性误差抑制方法包括：获取里程计在载体坐标系下输出的里程；基于里程计在载体坐标系下输出的里程计算获取里程计在载体坐标系下的位置增量；将惯性导航系统在载体坐标系下的位置增量与里程计在载体坐标系下的位置增量的差值作为系统观测量，根据系统观测量获取观测矩阵；获取系统状态转移矩阵；基于系统观测矩阵和系统状态转移矩阵，通过卡尔曼滤波算法，对状态变量进行估计，基于估计后的状态变量对里程计安装误差进行修正；基于修正后的里程计安装误差，获取修正完里程计误差后由于陀螺漂移引起的侧向累计位置误差，基于由于陀螺漂移引起的侧向累计位置误差获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。
20.应用此种配置方式，提供了一种用于低速检测平台系统的惯性误差抑制方法，该惯性误差抑制方法通过获取观测矩阵和系统状态转移矩阵，基于卡尔曼滤波算法，对状态变量进行估计，并根据估计后的状态变量对里程计安装误差进行修正；在对里程计安装误差进行修正后，可通过站点处位置误差反向推导航向误差，即获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。此种方式能够显著降低全站仪定点修正引起的驻波状误差，相比传统惯性/里程计组合导航算法对惯导系统的误差的抑制效果更加显著。其中，在本发明中，低速检测平台一般是指速度小于或等于2m/s的检测平台。
21.在本发明中，为了进一步地提高估计精度，在完成对轨道检测平台系统的惯性误差修正之后，惯性误差抑制方法还包括：将修正后的轨道检测平台系统的惯性误差与设定惯性误差精度阈值范围进行比较，当修正后的轨道检测平台系统的惯性误差超出设定惯性误差精度阈值范围时，重复上述步骤，直至修正后的轨道检测平台系统的惯性误差处于设定惯性误差精度阈值范围内。在此种配置方式下，通过对每次修正后的惯性误差进行评估，当不满足所设定的精度要求时，可以进行迭代计算的方式，进一步地提高估计精度。
22.在本发明中，为了实现用于轨道检测平台系统的惯性误差抑制，首先需要获取里程计在载体坐标系下输出的里程。作为本发明的一个具体实施例，低速轨道检测系统包括惯性导航系统和里程计，通过夹紧装置使得低速轨道检测系统与轨道紧密接触，因此侧向速度和垂向速度始终为0，前向速度通过采集里程计输出可以获得。里程计在载体坐标系下输出的里程可根据来获取，其中，为里程计在k时刻在载体坐标系下输出的里程，为里程计与惯性导航系统之间的安装关系矩阵，kd为里程计刻度系数，为里程计在里程计坐标系下的脉冲数矢量形式，nk为里程计在第k个采样周期内输出的脉冲数。
23.进一步地，里程计与惯性导航系统之间的安装关系矩阵可以表示为欧拉角的形式然后基于里程计在载体坐标系下输出的里程计算获取里程计在载体坐标系下的位置增量。令δkd表示里程计刻度系数误差，则对
进行整理，可以表示为误差向量的形式，即里程计在载体坐标系下的位置增量为其中，为里程计在载体坐标系下的位置增量，δα
θ
为俯仰角误差，δα
ψ
为航向角误差，δkd为里程计刻度系数误差，为里程计在k时刻在载体坐标系下沿x轴输出的里程，为里程计在k时刻在载体坐标系下沿y轴输出的里程，为里程计在k时刻在载体坐标系下沿z轴输出的里程，x为状态变量。
24.进一步地，在获取了里程计在载体坐标系下的位置增量之后，系统观测量采用单位时间内的位置增量进行计算，可以充分利用惯导系统短时精度较高的特点，使得对安装误差和刻度系数误差的估计更加准确。在k时刻内，惯性导航系统的位置增量为其中，为惯性导航系统在导航坐标系下的位置增量，为k时刻惯性导航系统在导航坐标系下的速度，为k-1时刻惯性导航系统在导航坐标系下的速度，ts为计算周期。将惯性导航系统在导航坐标系下的位置增量转换到载体坐标系b系下，可得
25.选择惯性导航系统在载体坐标系下的位置增量与所述里程计在载体坐标系下的位置增量的差值作为系统观测量zk：
26.根据系统观测量可得观测矩阵hk，其中，为惯性导航系统在载体坐标系下的位置增量。
27.进一步地，在获取了系统观测矩阵之后，考虑到误差向量短时变化不大，因此系统状态转移矩阵可以近似为单位阵，即fk＝i。
28.基于系统观测矩阵和系统状态转移矩阵，通过卡尔曼滤波算法，对状态变量xk＝
[δk
d,k δα
θ,k δα
ψ,k
]
t
进行估计，基于估计后的状态变量x对里程计安装误差进行修正。
[0029]
具体地，
[0030]
其中，x
k,k-1
为一步预测状态，kk为滤波增益矩阵，xk为k时刻的状态变量，x
k-1
为k-1时刻的状态变量，p
k,k-1
为一步预测均方差误差阵，pk为k时刻的均方差误差阵，p
k-1
为k-1时刻的均方差误差阵，qk为系统噪声矩阵，rk为测量噪声矩阵。
[0031]
在本发明中，基于估计后的状态变量对里程计安装误差进行修正具体包括：基于估计后的状态变量对惯性导航系统与里程计之间的安装关系矩阵以及里程计刻度系数进行修正以完成对里程计安装误差进行修正。其中，惯性导航系统与里程计之间的安装关系矩阵可根据进行修正，里程计刻度系数可根据k
d,k 1
＝(1 δk
d,k
)k
d,k
进行修正，其中，为k 1时刻里程计与惯性导航系统之间的安装关系矩阵，为k时刻里程计与惯性导航系统之间的安装关系矩阵，k
d,k 1
为k 1时刻里程计刻度系数，k
d,k
为k时刻里程计刻度系数。
[0032]
修正里程计安装误差后，每次到达全站仪设站地点时的测量位置误差，则基本上是由测量过程中的航向陀螺漂移导致的，如图1所示，误差增长曲线是一条曲线，不能通过修正航向误差消除。此外由于测量过程中噪声的影响，对比两条曲线(第一曲线为真值曲线，第二曲线为包含误差的测量值曲线)的航向差值变化，会引入更大的航向误差，因此需要利用位置误差对航向陀螺漂移误差进行反向推导。
[0033]
在本发明中，根据误差产生原理，航向陀螺漂移与形式里程之间的关系可以表示为其中，δx为修正完里程计误差后，由于陀螺漂移引起的侧向累计位置误差，ω为陀螺漂移等效角速率，t为从上一个站点出发后经过的时间，v为当前数据段的平均推行速度，δt为系统采样周期，将的离散化形式转换为连续时间函数形式可得其中，δx(t)为由于陀螺漂移引起的侧向累计位置误差，τ为任一时刻。
[0034]
为了对本发明有进一步地了解，下面结合具体实施例对本发明所提供的用于低速检测平台系统的惯性误差抑制方法进行详细说明。
[0035]
根据本发明的具体实施例，提供了一种用于低速检测平台系统的惯性误差抑制方法，该惯性误差抑制方法具体包括如下步骤。
[0036]
获取里程计在载体坐标系下输出的里程；里程计在载体坐标系下输出的里程可根
据来获取，其中，为里程计在k时刻在载体坐标系下输出的里程，为里程计与惯性导航系统之间的安装关系矩阵，kd为里程计刻度系数，为里程计在里程计坐标系下的脉冲数矢量形式，nk为里程计在第k个采样周期内输出的脉冲数。
[0037]
里程计与惯性导航系统之间的安装关系矩阵可以表示为欧拉角的形式
[0038]
基于里程计在载体坐标系下输出的里程计算获取里程计在载体坐标系下的位置增量；令δkd表示里程计刻度系数误差，则对进行整理，可以表示为误差向量的形式，即里程计在载体坐标系下的位置增量为其中，为里程计在载体坐标系下的位置增量，δα
θ
为俯仰角误差，δα
ψ
为航向角误差，δkd为里程计刻度系数误差，为里程计在k时刻在载体坐标系下沿x轴输出的里程，为里程计在k时刻在载体坐标系下沿y轴输出的里程，为里程计在k时刻在载体坐标系下沿z轴输出的里程，x为状态变量。
[0039]
将惯性导航系统在载体坐标系下的位置增量与里程计在载体坐标系下的位置增量的差值作为系统观测量，根据系统观测量获取观测矩阵；获取系统状态转移矩阵；基于系统观测矩阵和系统状态转移矩阵，通过卡尔曼滤波算法，对状态变量进行估计，基于估计后的状态变量对里程计安装误差进行修正。系统观测量可根据获取，其中，为惯性导航系统在载体坐标系下的位置增量，hk为观测矩阵，
惯性导航系统在载体坐标系下的位置增量可根据来获取，其中，为所述惯性导航系统在导航坐标系下的位置增量，为k时刻惯性导航系统在导航坐标系下的速度，为k-1时刻惯性导航系统在导航坐标系下的速度，ts为计算周期。
[0040]
基于修正后的里程计安装误差，获取修正完里程计误差后由于陀螺漂移引起的侧向累计位置误差，基于由于陀螺漂移引起的侧向累计位置误差获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。
[0041]
将修正后的轨道检测平台系统的惯性误差与设定惯性误差精度阈值范围进行比较，当修正后的所述轨道检测平台系统的惯性误差超出设定惯性误差精度阈值范围时，重复上述步骤，直至修正后的所述轨道检测平台系统的惯性误差处于设定惯性误差精度阈值范围内。
[0042]
综上所述，本发明提供了一种用于低速检测平台系统的惯性误差抑制方法，该惯性误差抑制方法通过获取观测矩阵和系统状态转移矩阵，基于卡尔曼滤波算法，对状态变量进行估计，并根据估计后的状态变量对里程计安装误差进行修正；在对里程计安装误差进行修正后，可通过站点处位置误差反向推导航向误差，即获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。此种方式能够显著降低全站仪定点修正引起的驻波状误差，相比传统惯性/里程计组合导航算法对惯导系统的误差的抑制效果更佳显著。
[0043]
为了便于描述，在这里可以使用空间相对术语，如“在
……
之上”、“在
……
上方”、“在
……
上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而，示例性术语“在
……
上方”可以包括“在
……
上方”和“在
……
下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。
[0044]
此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本发明保护范围的限制。
[0045]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。