CN111024074B - 一种基于递推最小二乘参数辨识的惯导速度误差确定方法 - Google Patents

Abstract

本发明一种基于递推最小二乘参数辨识的惯导速度误差确定方法，该方法能够改善在动态环境下捷联惯性导航的速度解算精度，提高后续速度更新、位置更新精度；在基于速度匹配量的动基座传递对准过程中，对速度误差的获得有改善作用，从而能有效地改善传递对准过程中对加速度计的零偏BIAS误差估计。本发明在速度更新过程中，建立适当的速度误差补偿模型，以此实现对速度的误差补偿，通过分析一般条件下的捷联惯导速度误差模型，获得相应的速度误差补偿因数，从而建立起捷联惯导速度误差模型，之后将误差模型引入位置更新，能够有效的解决捷联惯性导航解算过程中由于速度发散引起的位置误差，尤其是对于多转弯机动的定位结果有明显的改善作用。

Description

一种基于递推最小二乘参数辨识的惯导速度误差确定方法

技术领域

本发明涉及一种基于递推最小二乘参数辨识的惯导速度误差确定方法，属于捷联惯性导航的速度及位置误差补偿领域。

背景技术

当前，惯性导航***(Inertial NaVigation System，INS)以其自主性、抗干扰能力强、隐蔽性好、数据更新率高的特点，在定位服务中的应用越来越普及，这其中应用微机电***(Micro-Electro-Mechanical-System)的捷联惯性导航***因其体积小、功耗低、重量轻，在个人导航、飞行器、车辆导航上发挥了不可替代的作用，一个典型的捷联惯导***定位过程必须包括三个过程：姿态更新，即通过对陀螺仪数据的处理，实现对***的姿态更新过程；在此基础上，经由基于加速度计数据的速度更新，实现对***速度的更新；最后，基于初始位置，由速度得到实时位置，这就是位置更新。捷联惯导器件的采样频率往往能到200hz-1000hz，在如此高的定位解算频率下，任何不经补偿的误差引入都会经由多次解算的误差累积效应达到难以忽视的地步，想要获得足够高精度的捷联惯导解算结果，就必须做好***的误差补偿。

速度更新环节作为惯导解算中的一个重要环节，其误差直接影响到了后续的位置解算，速度上的常数误差Δe经由时间积分，会表现为位置上的一次发散误差，并且会随着时间的累积，不断累积。现有的一些捷联惯导***速度更新过程，由于自身器件性能的影响，多忽略由加速度计本身性能及姿态耦合引入的误差，这为捷联惯导***的速度和定位引入了不小的误差。

发明内容

本发明的目的在于克服现有技术的不足，提供了一种基于递推最小二乘参数辨识的惯导速度误差确定方法，该方法能够为捷联惯导在速度更新过程中提供对应的补偿量，以补偿由于姿态耦合及加速度计本身性能引入的速度误差，从而改善速度更新结果及姿态更新结果，不但对捷联惯导解算过程中的位置解算由明显的改善，对动基座传递对准时加速度计的速度误差估计及BIAS估计同样能起到明显作用。

本发明的技术方案是：

一种基于递推最小二乘参数辨识的惯导速度误差确定方法，包括以下步骤：

1)判断当前时刻k是否为初始时刻，若k＝0，则进入步骤2)，否则，k≠0，进入步骤3)；

2)读取载体初始速度v₀作为

根据初始姿态俯仰角θ₀、横滚角γ₀和方位角

获得载体初始姿态矩阵

作为***姿态矩阵

读取载体初始纬度L_a0，经度L_o0与高度H₀，分别作为纬度L_a(-)，经度L_o(-)与高度H(-)；设定M₀为惯导速度误差系数递推初始值；转入步骤3)；

3)根据当前时刻k(k＝0，…，t_k)陀螺仪读数

及***姿态矩阵

得到更新后的载体姿态矩阵

4)完成比力坐标转换

根据载体姿态矩阵

更新后的载体姿态矩阵

加速度计读数

获得导航坐标系下的载体的比力

5)获取更新后的载体速度

根据

与

更新递推速度误差补偿前的载体速度

6)完成位置更新

输入L_a(-)、L_o(-)、H(-)及步骤5)所得

更新得到L_a(+)、L_o(+)、H(+)；

7)根据外部输入的载体速度真值V_k及步骤5)所得

获得速度误差数据

8)根据速度误差

及步骤4)中所得导航坐标系下的载体的加速度

完成一次基于递推的最小二乘参数辨识的惯导速度误差模型递推，得到递推结果M_N，N＝kΔt；

9)历元间信息传递：将步骤3)步骤5)解算得到的

分别作为下一历元解算的

将步骤6)所得L_a(+)、L_o(+)、H(+)作为下一历元解算的L_a(-)、L_o(-)、H(-)；

10)在下一时刻k+1历元开始时，判断步骤7)中外部输入的载体速度真值V_k是否停止输入：若外部未停止输入载体速度真值，则进入步骤11)，若外部停止输入载体速度真值，则进入步骤12)；

11)重复步骤1)至步骤10)直至外部停止输入载体速度真值后进入步骤12)；

12)根据当前k历元得到的步骤8)递推结果M_N，确定捷联惯导速度误差。

本发明与现有技术相比有益效果为：

1)本发明在建立捷联惯导误差模型的过程中，充分考虑到捷联惯导***姿态与加速度的耦合项，从而能够获得更加精确的惯导速度，并由此显著的改善了捷联惯导***的速度更新值及定位误差。

2)本发明算法计算负荷小，能满足实时定位要求，对于复杂运动、转弯运动有着很好的补偿精度，亦可用于指导在捷联惯导***传递对准过程中的速度误差修正和加速度计BIAS估计过程。

附图说明

图1为本发明捷联惯导***速度误差补偿过程图。

具体实施方式

下面对本发明做进一步的详细描述：

本发明提供的基于参数拟合的捷联惯导速度误差补偿方法，可以用于惯导***定位、组合***中的惯导位置解算，亦可改善动基座传递对准过程中的速度误差量修正及加速度计估计偏差BIAS。

当基于本发明进行对捷联惯导速度误差进行误差建模矫正时，需满足以下条件以保证模型精度：一、捷联***的姿态解算过程必须保证足够的精度，以消除由姿态误差而引入的速度误差，一般而言，由姿态解算引起的误差在10^-3°/h以内；二、捷联惯导***需保证有足够长时间的运动状态，以满足参数拟合的样本要求，其速度、姿态在行进过程中变化量要足够明显。对此，本发明采用了如下措施以达到如上要求：首先以旋转矢量法进行姿态更新，其姿态精度达到10^-7°/h，其次，在建模数据采集过程中，保证捷联惯导***在各个方向上有充分的位移运动及速度变化，从而为参数辨识提供充分的数据模型。此外在进行捷联惯导的速度补偿算法时，如要对动基座传递对准过程中的速度误差量及加速度计BIAS进行有效补偿，则需额外满足如下要求：一、传递对准滤波器的状态量中必须含有速度误差及加速度静态零偏；二、需保证足够的对准时间并确保经由传递对准后的滤波器本身收敛。

如图1所示，本发明一种基于递推最小二乘参数辨识的惯导速度误差确定方法，包括以下步骤：

1)判断当前时刻k是否为初始时刻，若k＝0，则进入步骤2)，否则，k≠0，进入步骤3)；

2)读取载体初始速度v₀作为

根据初始姿态俯仰角θ₀、横滚角γ₀和方位角

获得载体初始姿态矩阵

作为***姿态矩阵

读取载体初始纬度L_a0，经度L_o0与高度H₀，分别作为纬度L_a(-)，经度L_o(-)与高度H(-)；设定M₀为惯导速度误差系数递推初始值；转入步骤3)；

3)根据当前时刻k(k＝0，…，t_k)陀螺仪读数

及***姿态矩阵

得到更新后的载体姿态矩阵

4)完成比力坐标转换

根据载体姿态矩阵

更新后的载体姿态矩阵

加速度计读数

获得导航坐标系下的载体的比力

5)获取更新后的载体速度

根据

与

更新递推速度误差补偿前的载体速度

6)完成位置更新

输入L_a(-)、L_o(-)、H(-)及步骤5)所得

更新得到L_a(+)、L_o(+)、H(+)；

7)根据外部输入的载体速度真值V_k及步骤5)所得

获得速度误差数据

8)根据速度误差

及步骤4)中所得导航坐标系下的载体的加速度

完成一次基于递推的最小二乘参数辨识的惯导速度误差模型递推；

9)历元间信息传递：将步骤3)步骤5)解算得到的

分别作为下一历元解算的

将步骤6)所得L_a(+)、L_o(+)、H(+)作为下一历元解算的L_a(-)、L_o(-)、H(-)；

10)在下一时刻k+1历元开始时，判断步骤7)中外部输入的载体速度真值V_k是否停止输入：若外部未停止输入载体速度真值，则进入步骤11)，若外部停止输入载体速度真值，则进入步骤12)；

11)重复步骤1)至步骤10)直至外部停止输入载体速度真值后进入步骤12)；

12)以当前k历元迭代得到的***参数M_N(N＝kΔt)为基础，确定捷联惯导速度误差。然后进入步骤13)；

13)根据步骤12)所得惯导速度误差，可得到任意历元速度误差补偿量ΔV_k，重复步骤1)至步骤5)获得速度误差补偿前的载体速度

以补偿后的载体速度

代替补偿前的速度

并继续执行步骤6)及步骤9)，从而获得速度补偿后的载体姿态、位置与速度，进而完成基于递推最小二乘参数辨识的惯导速度误差补偿过程。

步骤2)由俯仰角θ、横滚角γ和方位角

获得载体初始姿态矩阵

根据下列等式确定：

式中，O-X_bY_bZ_b载体坐标系与O-X_nY_nZ_n惯导***固连，以载体运动方向的正前方作为坐标轴Y_b的正向，载体运动方向的正上方作为坐标轴Z_b的正向，载体运动方向的右侧作为坐标轴X_b的正向；导航坐标系为地理坐标系，以地理东向作为坐标轴X_n的正向，以地理北向作为坐标轴Y_n的正向，以竖直面向上作为坐标轴Z_n的正向系表示；

意为从b系至n系的姿态旋转矩阵，俯仰角θ、横滚角γ和方位角

定义如下：O-X_bY_bZ_b坐标系绕自身O-Y_b轴旋转γ角，得到中间坐标系O-X₁Y₁Z₁系，O-X₁Y₁Z₁系绕自身O-X₁轴旋转θ角，得到中间坐标系O-X₂Y₂Z₂系，O-X₂Y₂Z₂系绕自身O-Z₂轴旋转

角，从而得到O-X_nY_nZ_n系。其中，角度正方向均遵循右手定则。

3、根据权利要求2所述的一种基于递推最小二乘参数辨识的惯导速度误差确定方法，其特征在于，步骤3)姿态更新的过程遵循如下方程：

其中，定义运算符：(-)表示更新前的量，(+)表示更新后的量，后文其他量同样遵循此定义；I₃为3维单位向量，

表示基于东北天三个方向上陀螺仪读数

生成的列向量：

表示地球自转角速度在当地导航系的表示，其公式如下：

ω_ie表示地球自转角速度在惯性系下的表示；

为转移角速率，其定义如下：

其中，

R_E为载体所在位置的地球卯酉圈半径，R_N为载体所在位置的地球子午圈半径；Δt为此次解算时间间隔。

步骤4)比力坐标变换按照如下公式：

步骤5)载体速度

更新须遵循如下公式：

其中，g为当前重力加速度。

步骤6)，完成位置更新的公式如下：

步骤7)，获得待补偿速度误差数据的方法，具体为：

步骤8)：完成一次最小二乘参数辨识的惯导速度误差模型递推过程遵循如下公式：

L(N)＝P_NΨ_NW_N

其中，N＝kΔt，M_N为当前历元惯导速度误差系数，Y_N为δv_k在此时刻形成的列向量，Ψ_N为

形成的列向量，二者的定义满足如下方程：

W_N为加速度计噪声矩阵，R_a，x、R_a，y、R_a，z分别为对应方向上加速度计噪声误差方差值，其定义如下：

步骤12)捷联惯导速度误差的确定遵循如下公式：

上式中，

表示任意时间t(0≤t≤k)时刻的加速度计读数，ΔV_t为对应时刻的基于递推最小二乘参数辨识确定的捷联惯导速度误差模型。根据此模型可确定此刻导航坐标系下速度误差ΔV_E，t、ΔV_N，t、ΔV_U，t。

本发明的基于递推最小二乘参数辨识的捷联惯导速度误差补偿方法，其原理推导过程如下：

为姿态误差角，

和δA_i(i＝x，y，z)分别为加速度计的刻度系数误差和安装角误差，则当取地理坐标系为导航坐标系时，可得捷联惯导***的速度误差方程为：

记：

将各分量分别带入上式中，可得到分量表示的速度误差方程为：

在速度更新过程中，由于地球自转角速度和纬度变化而共同合成的牵连加速度经过补偿后，上式中，影响捷联惯性速度更新的误差方程可以写成如下形式：

在某一历元时刻t_k，捷联惯导的姿态误差足够小，捷联惯导单纯由姿态误差角

引起的误差可近似为零，这样速度误差可以以如下函数代表：

其中ε为该方法未能完全补偿的速度误差的高阶小量，上式可整理为如下形式：

一般性地，上式中的δV_E、δV_N、δV_U可用如下线性函数代表：

y＝Ax+ε

其***输出为

数据矩阵

参数向量

噪声矩阵为

将上述***离散化后得到离散化***：

Y_N＝Ψ_NX_N+ε(N，X_N)

递推的基本思路为：此次(新)的估计值M_N+1＝前次(旧)的数值M_N+修正值。

t_k时刻(t_k＝NT)得到的参数估计用M_N表示，已知

M_N＝[Ψ_NW_NΨ_N]^-1[Ψ_NW_NΨ_N]

W_N为噪声方差矩阵，其定义如下

R_a，x、R_a，y、R_a，z分别为对应方向上加速度计噪声误差方差值，此处赋为R_a，x＝R_a，y＝R_a，z＝0.01。

当新增一组输入输出信息x_N+1、y_N+1后，可获得如下矩阵方程：

Y_N+1＝Ψ_N+1M_N+ε(N+1，M_N)

令

L(N+1)＝P_N+1Ψ_N+1W_N+1

则基于递推最小二乘参数辨识的捷联惯导速度误差补偿模型建立过程可依照下列递推公式实现：

1)

2)L(N)＝P_NΨ_NW_N

3)

通过上式，可最终获得用于实现捷联惯导速度误差补偿的参数M_N。

以上所述，仅为本发明一个具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

Claims (9)

1.一种基于递推最小二乘参数辨识的惯导速度误差确定方法，其特征在于，包括以下步骤：

1)判断当前时刻k是否为初始时刻，若k＝0，则进入步骤2)，否则，k≠0，进入步骤3)；

2)读取载体初始速度v₀作为

根据初始姿态俯仰角θ₀、横滚角γ₀和方位角

获得载体初始姿态矩阵

作为***姿态矩阵

读取载体初始纬度L_a0，经度L_o0与高度H₀，分别作为纬度L_a(-)，经度L_o(-)与高度H(-)；设定M₀为惯导速度误差系数递推初始值；转入步骤3)；

3)根据当前时刻k陀螺仪读数

及***姿态矩阵

得到更新后的载体姿态矩阵

k＝0，…，t_k；

4)完成比力坐标转换

根据载体姿态矩阵

更新后的载体姿态矩阵

加速度计读数

获得导航坐标系下的载体的比力

5)获取更新后的载体速度

根据

与

更新递推速度误差补偿前的载体速度

6)完成位置更新

输入L_a(-)、L_o(-)、H(-)及步骤5)所得

更新得到L_a(+)、L_o(+)、H(+)；

7)根据外部输入的载体速度真值V_k及步骤5)所得

获得速度误差数据δv_k；

8)根据速度误差δv_k及步骤4)中所得导航坐标系下的载体的加速度

完成一次基于递推的最小二乘参数辨识的惯导速度误差模型递推，得到递推结果M_N，N＝kΔt；

9)历元间信息传递：将步骤3)步骤5)解算得到的

分别作为下一历元解算的

将步骤6)所得L_a(+)、L_o(+)、H(+)作为下一历元解算的L_a(-)、L_o(-)、H(-)；

10)在下一时刻k+1历元开始时，判断步骤7)中外部输入的载体速度真值V_k是否停止输入：若外部未停止输入载体速度真值，则进入步骤11)，若外部停止输入载体速度真值，则进入步骤12)；

11)重复步骤1)至步骤10)直至外部停止输入载体速度真值后进入步骤12)；

12)根据当前k历元得到的步骤8)递推结果M_N，确定捷联惯导速度误差。

2.根据权利要求1所述的一种基于递推最小二乘参数辨识的惯导速度误差确定方法，其特征在于，步骤2)由俯仰角θ、横滚角γ和方位角

获得载体初始姿态矩阵

根据下列等式确定：

式中，O-X_bY_bZ_b载体坐标系与O-X_nY_nZ_n惯导***固连，以载体运动方向的正前方作为坐标轴Y_b的正向，载体运动方向的正上方作为坐标轴Z_b的正向，载体运动方向的右侧作为坐标轴X_b的正向；导航坐标系为地理坐标系，以地理东向作为坐标轴X_n的正向，以地理北向作为坐标轴Y_n的正向，以竖直面向上作为坐标轴Z_n的正向系表示；

意为从b系至n系的姿态旋转矩阵，俯仰角θ、横滚角γ和方位角

定义如下：O-X_bY_bZ_b坐标系绕自身O-Y_b轴旋转γ角，得到中间坐标系O-X₁Y₁Z₁系，O-X₁Y₁Z₁系绕自身O-X₁轴旋转θ角，得到中间坐标系O-X₂Y₂Z₂系，O-X₂Y₂Z₂系绕自身O-Z₂轴旋转

角，从而得到O-X_nY_nZ_n系；其中，角度正方向均遵循右手定则。

3.根据权利要求2所述的一种基于递推最小二乘参数辨识的惯导速度误差确定方法，其特征在于，步骤3)姿态更新的过程遵循如下方程：

其中，定义运算符：(-)表示更新前的量，(+)表示更新后的量，I₃为3维单位向量，

表示基于东北天三个方向上陀螺仪读数

生成的列向量：

表示地球自转角速度在当地导航系的表示，其公式如下：

ω_ie表示地球自转角速度在惯性系下的表示；

为转移角速率，其定义如下：

其中，

R_E为载体所在位置的地球卯酉圈半径，R_N为载体所在位置的地球子午圈半径；Δt为此次解算时间间隔。

4.根据权利要求3所述的一种基于递推最小二乘参数辨识的惯导速度误差确定方法，其特征在于，步骤4)导航坐标系下的载体的比力的确定方法，具体为：

5.根据权利要求4所述的一种基于递推最小二乘参数辨识的惯导速度误差确定方法，其特征在于，步骤5)载体速度

更新须遵循如下公式：

其中，g为当前重力加速度。

6.根据权利要求5所述的一种基于递推最小二乘参数辨识的惯导速度误差确定方法，其特征在于，步骤6)，完成位置更新的公式如下：

7.根据权利要求6所述的一种基于递推最小二乘参数辨识的惯导速度误差确定方法，其特征在于，步骤7)，获得待补偿速度误差数据的方法，具体为：

8.根据权利要求7所述的一种基于递推最小二乘参数辨识的惯导速度误差确定方法，其特征在于，步骤8)：完成一次最小二乘参数辨识的惯导速度误差模型递推过程遵循如下公式：

L(N)＝P_NΨ_NW_N

时，P_Δt＝0；N＝kΔt；

其中，N＝kΔt，M_N为当前历元惯导速度误差系数，Y_N为δv_k在此时刻形成的列向量，Ψ_N为

形成的列向量，二者的定义满足如下方程：

W_N为加速度计噪声矩阵，R_a，x、R_a，y、R_a，z分别为对应方向上加速度计噪声误差方差值，其定义如下：

9.根据权利要求8所述的一种基于递推最小二乘参数辨识的惯导速度误差确定方法，其特征在于，步骤12)捷联惯导速度误差ΔV_t的确定遵循如下公式：

上式中，

表示任意时间t时刻的加速度计读数，0≤t≤k，ΔV_t为对应时刻的基于递推最小二乘参数辨识确定的捷联惯导速度误差模型；根据此模型可确定此刻导航坐标系下速度误差ΔV_E，t、ΔV_N，t、ΔV_U，t。

Images