松组合 紧组合
传感器介绍
- GPS/INSS和INS/DVL来分别解释组合导航的松组合和紧组合算法,给出公式进行解释
- GPS(Global Positioning System,全球定位系统):一种卫星导航系统,通过接收卫星发射的无线电信号来确定地球上任意位置的经纬度和高程信息。
- INS(Inertial Navigation System,惯性导航系统):一种利用惯性传感器(陀螺仪和加速度计)测量载体的加速度和角速度信息,通过积分计算出位置、速度和姿态的自主导航系统。
- DVL(Doppler Velocity Log,多普勒速度计):一种基于声纳技术的水下速度计,通过测量水声信号多普勒效应来确定水下载体的速度。
解释GPS/INS和INS/DVL组合导航的松组合和紧组合算法
- GPS(Global Positioning System,全球定位系统):一种卫星导航系统,通过接收卫星发射的无线电信号来确定地球上任意位置的经纬度和高程信息。
- INS(Inertial Navigation System,惯性导航系统):一种利用惯性传感器(陀螺仪和加速度计)测量载体的加速度和角速度信息,通过积分计算出位置、速度和姿态的自主导航系统。
- DVL(Doppler Velocity Log,多普勒速度计):一种基于声纳技术的水下速度计,通过测量水声信号多普勒效应来确定水下载体的速度。
GPS/INS和INS/DVL组合导航的松组合和紧组合算法。 - GPS/INS组合导航:
(1)松组合:
在GPS/INS松组合中,GPS和INS分别输出位置、速度和姿态信息。这些信息在组合导航计算机中通过卡尔曼滤波器进行融合,得到最终的导航解。卡尔曼滤波器的状态向量通常包括位置、速度和姿态误差。
假设我们已经获得了GPS和INS的位置和速度信息 p G P S \boldsymbol{p}{\mathrm{GPS}} pGPS, v G P S \boldsymbol{v}{\mathrm{GPS}} vGPS, p I N S \boldsymbol{p}{\mathrm{INS}} pINS, v I N S \boldsymbol{v}{\mathrm{INS}} vINS,那么位置和速度误差可以表示为:
e
p
=
p
I
N
S
−
p
G
P
S
e
v
=
v
I
N
S
−
v
G
P
S
\begin{aligned} \boldsymbol{e}_{\boldsymbol{p}} & =\boldsymbol{p}_{\mathrm{INS}}-\boldsymbol{p}_{\mathrm{GPS}} \\ \boldsymbol{e}_{\boldsymbol{v}} & =\boldsymbol{v}_{\mathrm{INS}}-\boldsymbol{v}_{\mathrm{GPS}} \end{aligned}
epev=pINS−pGPS=vINS−vGPS
通过卡尔曼滤波器进行估计和更新后,我们可以得到修正后的INS位置、速度和姿态信息。
(2) 紧组合:
在GPS/INS紧组合中,GPS和INS传感器之间的数据融合发生在较低层次,例如原始观测值层面。这意味着我们使用GPS卫星的伪距和伪距率信息(而非位置和速度信息)与INS的导航信息进行融合。卡尔曼滤波器的状态向量通常包括位置、速度、姿态误差以及接收机钟差等。
假设我们已经获得了GPS卫星的伪距信息
ρ
i
\rho_i
ρi,伪距率信息
ρ
˙
i
\dot{\rho}i
ρ˙i,以及INS的位置、速度和姿态信息
p
I
N
S
\boldsymbol{p}{\mathrm{INS}}
pINS,
v
I
N
S
\boldsymbol{v}{\mathrm{INS}}
vINS,
θ
I
N
S
\boldsymbol{\theta}{\mathrm{INS}}
θINS。首先,我们需要将伪距和伪距率信息转换为位置和速度信息。可以通过以下公式进行转换:
p
G
P
S
=
∑
i
=
1
N
ρ
i
(
p
s
a
t
,
i
−
p
I
N
S
)
∥
p
s
a
t
,
i
−
p
I
N
S
∥
v
G
P
S
=
∑
i
=
1
N
ρ
˙
i
(
p
s
a
t
,
i
−
p
I
N
S
)
∥
p
s
a
t
,
i
−
p
I
N
S
∥
\begin{aligned} & \boldsymbol{p}_{\mathrm{GPS}}=\sum_{i=1}^N \frac{\rho_i\left(\boldsymbol{p}_{\mathrm{sat}, i}-\boldsymbol{p}_{\mathrm{INS}}\right)}{\left\|\boldsymbol{p}_{\mathrm{sat}, i}-\boldsymbol{p}_{\mathrm{INS}}\right\|} \\ & \boldsymbol{v}_{\mathrm{GPS}}=\sum_{i=1}^N \frac{\dot{\rho}_i\left(\boldsymbol{p}_{\mathrm{sat}, i}-\boldsymbol{p}_{\mathrm{INS}}\right)}{\left\|\boldsymbol{p}_{\mathrm{sat}, i}-\boldsymbol{p}_{\mathrm{INS}}\right\|} \end{aligned}
pGPS=i=1∑N
psat,i−pINS
ρi(psat,i−pINS)vGPS=i=1∑N
psat,i−pINS
ρ˙i(psat,i−pINS)其中,
N
N
N表示GPS卫星数量,
p
s
a
t
,
i
\boldsymbol{p}_{\mathrm{sat}, i}
psat,i表示第
i
i
i颗卫星的位置。
然后,我们可以通过类似于松组合的方法计算位置、速度误差并使用卡尔曼滤波器进行融合。紧组合方法能更好地处理恶劣环境和信号遮挡等问题。
两种组合的时间不同步影响
1松组合
在松组合中,GPS和INS输出位置、速度和姿态信息。假设GPS和INS的速度分别为
v
G
P
S
n
\boldsymbol{v}{\mathrm{GPS}}^n
vGPSn和
v
I
N
S
n
\boldsymbol{v}{\mathrm{INS}}^n
vINSn,由于采样时间不同步,我们可以得到:
v G P S n ( t ) = v I N S n ( t − δ t ) + a n δ t \boldsymbol{v}_{\mathrm{GPS}}^n(t)=\boldsymbol{v}_{\mathrm{INS}}^n(t-\delta t)+\boldsymbol{a}^n \delta t vGPSn(t)=vINSn(t−δt)+anδt
其中,
a
n
\boldsymbol{a}^n
an表示载体在不同步时间附近的平均线加速度。类似地,对于位置信息,我们可以得到:
p
G
P
S
n
(
t
)
=
p
I
N
S
n
(
t
−
δ
t
)
+
v
I
N
S
n
δ
t
+
1
2
a
n
δ
t
2
\boldsymbol{p}_{\mathrm{GPS}}^n(t)=\boldsymbol{p}_{\mathrm{INS}}^n(t-\delta t)+\boldsymbol{v}_{\mathrm{INS}}^n \delta t+\frac{1}{2} \boldsymbol{a}^n \delta t^2
pGPSn(t)=pINSn(t−δt)+vINSnδt+21anδt2
在紧组合中,我们可以考虑GPS观测的伪距和伪距率信息。设GPS的伪距为
ρ
\rho
ρ,伪距率为
ρ
˙
\dot{\rho}
ρ˙,在时间不同步的情况下,
我们有:时间不同步对转换后的
p
G
P
S
\boldsymbol{p}{\mathrm{GPS}}
pGPS和
v
G
P
S
\boldsymbol{v}{\mathrm{GPS}}
vGPS的影响。
考虑时间不同步的情况下,伪距和伪距率观测值的变化将导致位置和速度的计算中引入误差。将受影响的 ρ ( t ) \rho(t) ρ(t)和 ρ ˙ ( t ) \dot{\rho}(t) ρ˙(t)代入转换公式,我们可以得到受影响的位置和速度信息: p G P S ′ = ∑ i = 1 N ( ρ ( t − δ t ) + v I N S n δ t + 1 2 a n δ t 2 ) ( p s a t , i − p I N S ) ∥ p s a t , i − p I N S ∥ v G P S ′ = ∑ i = 1 N ( ρ ˙ ( t − δ t ) + a n δ t ) ( p s a t , i − p I N S ) ∥ p s a t , i − p I N S ∥ \begin{aligned} & \boldsymbol{p}_{\mathrm{GPS}}^{\prime}=\sum_{i=1}^N \frac{\left(\rho(t-\delta t)+\boldsymbol{v}_{\mathrm{INS}}^n \delta t+\frac{1}{2} \boldsymbol{a}^n \delta t^2\right)\left(\boldsymbol{p}_{\mathrm{sat}, i}-\boldsymbol{p}_{\mathrm{INS}}\right)}{\left\|\boldsymbol{p}_{\mathrm{sat}, i}-\boldsymbol{p}_{\mathrm{INS}}\right\|} \\ & \boldsymbol{v}_{\mathrm{GPS}}^{\prime}=\sum_{i=1}^N \frac{\left(\dot{\rho}(t-\delta t)+\boldsymbol{a}^n \delta t\right)\left(\boldsymbol{p}_{\mathrm{sat}, i}-\boldsymbol{p}_{\mathrm{INS}}\right)}{\left\|\boldsymbol{p}_{\mathrm{sat}, i}-\boldsymbol{p}_{\mathrm{INS}}\right\|} \\ & \end{aligned} pGPS′=i=1∑N psat,i−pINS (ρ(t−δt)+vINSnδt+21anδt2)(psat,i−pINS)vGPS′=i=1∑N psat,i−pINS (ρ˙(t−δt)+anδt)(psat,i−pINS)
由于 p ′ G P S \boldsymbol{p'}{\mathrm{GPS}} p′GPS和 v ′ G P S \boldsymbol{v'}{\mathrm{GPS}} v′GPS受到时间不同步的影响,这将导致紧组合中位置和速度误差的计算也受到影响。在紧组合的卡尔曼滤波器中,可以通过添加一个状态变量来估计和补偿时间不同步误差。这种方法能够提高紧组合的性能,更好地处理恶劣环境和信号遮挡等问题。
的 p G P S \boldsymbol{p}{\mathrm{GPS}} pGPS和 v G P S \boldsymbol{v}{\mathrm{GPS}} vGPS
INS/DVL松组合/紧组合
- DVL(Doppler Velocity Log,多普勒速度计):一种基于声纳技术的水下速度计,通过测量水声信号多普勒效应来确定水下载体的速度。
INS/DVL组合导航的松组合和紧组合算法。
(1) 松组合
在INS/DVL松组合中,INS和DVL分别输出位置、速度和姿态信息。这些信息在组合导航计算机中通过卡尔曼滤波器进行融合,得到最终的导航解。卡尔曼滤波器的状态向量通常包括位置、速度和姿态误差。
假设我们已经获得了DVL和INS的速度信息
v
D
V
L
\boldsymbol{v}_{\mathrm{DVL}}
vDVL,
v
I
N
S
\boldsymbol{v}_{\mathrm{INS}}
vINS,那么速度误差可以表示为:
e
v
=
v
I
N
S
−
v
D
V
L
e_v=v_{\mathrm{INS}}-v_{\mathrm{DVL}}
ev=vINS−vDVL
通过卡尔曼滤波器进行估计和更新后,我们可以得到修正后的INS位置、速度和姿态信息。
(2) 紧组合
在INS/DVL紧组合中,INS和DVL传感器之间的数据融合发生在较低层次。在这种情况下,我们可以将DVL的速度信息与INS的速度和姿态信息进行直接融合。卡尔曼滤波器的状态向量通常包括位置、速度、姿态误差以及DVL的漂移等。
通过紧密融合INS和DVL的信息,我们可以在更低的层次上利用DVL的速度信息来修正INS的误差。与松组合方法相比,紧组合可以更有效地减小导航误差的累积,从而提高整个系统的精度和稳定性。
为了实现INS/DVL紧组合,我们首先需要将DVL的速度信息转换到惯性坐标系。这可以通过以下公式实现:
v
D
V
L
n
=
R
(
θ
I
N
S
)
v
D
V
L
b
\boldsymbol{v}_{\mathrm{DVL}}^n=\boldsymbol{R}\left(\boldsymbol{\theta}_{\mathrm{INS}}\right) \boldsymbol{v}_{\mathrm{DVL}}^b
vDVLn=R(θINS)vDVLb
其中,
v
D
V
L
b
\boldsymbol{v}_{\mathrm{DVL}}^b
vDVLb表示DVL测量的速度在载体坐标系下的表示,
R
(
θ
I
N
S
)
\boldsymbol{R}(\boldsymbol{\theta}_{\mathrm{INS}})
R(θINS)表示从载体坐标系到惯性坐标系的旋转矩阵。
然后,我们可以计算速度误差:
e
v
=
v
I
N
S
n
−
v
D
V
L
n
\boldsymbol{e}_{\boldsymbol{v}}=\boldsymbol{v}_{\mathrm{INS}}^n-v_{\mathrm{DVL}}^n
ev=vINSn−vDVLn
接下来,通过卡尔曼滤波器对位置、速度、姿态误差以及DVL的漂移进行估计和更新。这使得我们能够在紧组合情况下更有效地利用DVL的速度信息来修正INS的误差。
总之,无论是GPS/INS还是INS/DVL组合导航系统,松组合和紧组合算法都可以有效地整合各自传感器的优势,提高导航系统的精度和稳定性。在实际应用中,选择合适的组合算法应根据系统需求、传感器性能和环境条件等因素进行权衡。
采样时间差的影响
(1) 松组合:
在INS/DVL松组合中,INS和DVL分别输出位置、速度和姿态信息。当采样时间不同步时,可能导致融合过程中的数据时滞,从而影响导航解的准确性。为了解决这个问题,可以采用数据插值方法对DVL或INS数据进行插值,以使它们在相同的时间点上具有一致的数据。
(2) 紧组合:
在INS/DVL紧组合中,数据融合发生在较低层次。采样时间不同步可能导致DVL速度信息与INS速度和姿态信息之间的误差增大,从而影响紧组合的性能。
为了解决这个问题,可以采用类似于松组合中的数据插值方法,对DVL速度信息进行插值,以匹配INS的采样时间。此外,在卡尔曼滤波器中可以增加一个状态变量来估计和补偿时间不同步误差,从而提高紧组合的性能。
总之,当传感器之间存在频率差距,导致采样时间不一致时,GPS/INS和INS/DVL组合导航系统的性能可能会受到影响。为了缓解这种影响,可以采用数据插值方法对不同步的数据进行处理,使得它们在相同的时间点上具有一致的数据。此外,在卡尔曼滤波器中增加一个状态变量来估计和补偿时间不同步误差,可以进一步提高松组合和紧组合的性能。在实际应用中,根据系统需求和传感器性能,可以选择合适的组合算法以及相应的处理方法,以实现高精度和稳定的组合导航解。
- 计算过程
(1) 松组合:
在松组合中,INS和DVL输出位置、速度和姿态信息。假设DVL和INS的速度分别为 v D V L n \boldsymbol{v}_{\mathrm{DVL}}^n vDVLn和 v I N S n \boldsymbol{v}_{\mathrm{INS}}^n vINSn,由于采样时间不同步,我们可以得到: v D V L n ( t ) = v I N S n ( t − δ t ) + a n δ t \boldsymbol{v}_{\mathrm{DVL}}^n(t)=\boldsymbol{v}_{\mathrm{INS}}^n(t-\delta t)+\boldsymbol{a}^n \delta t vDVLn(t)=vINSn(t−δt)+anδt
类似地,对于位置信息,我们可以得到:
p D V L n ( t ) = p I N S n ( t − δ t ) + v I N S n δ t + 1 2 a n δ t 2 p_{\mathrm{DVL}}^n(t)=\boldsymbol{p}_{\mathrm{INS}}^n(t-\delta t)+\boldsymbol{v}_{\mathrm{INS}}^n \delta t+\frac{1}{2} \boldsymbol{a}^n \delta t^2 pDVLn(t)=pINSn(t−δt)+vINSnδt+21anδt2
(2) 紧组合:
在INS/DVL紧组合中,我们需要考虑INS的位置、速度和姿态信息 p I N S \boldsymbol{p}_{\mathrm{INS}} pINS, v I N S \boldsymbol{v}_{\mathrm{INS}} vINS, θ I N S \boldsymbol{\theta}_{\mathrm{INS}} θINS,以及DVL的速度观测值 v D V L \boldsymbol{v}_{\mathrm{DVL}} vDVL。这里,我们关注时间不同步值为 δ t \delta t δt时对紧组合算法的影响。
首先,在时间不同步的情况下,我们需要考虑受影响的DVL速度观测值。假设载体在不同步时间附近的平均线加速度为
a
n
\boldsymbol{a}^n
an,我们可以得到受影响的DVL速度观测值为:
v
D
V
L
′
=
v
D
V
L
(
t
−
δ
t
)
+
a
n
δ
t
\boldsymbol{v}_{\mathrm{DVL}}^{\prime}=\boldsymbol{v}_{\mathrm{DVL}}(t-\delta t)+\boldsymbol{a}^n \delta t
vDVL′=vDVL(t−δt)+anδt
在紧组合中,我们需要将DVL速度观测值转换为导航坐标系下的速度。这可以通过将DVL速度观测值乘以旋转矩阵
C
b
n
C_{b}^{n}
Cbn(由INS姿态信息得到)来实现:
v
D
V
L
′
n
=
C
b
n
v
D
V
L
′
\boldsymbol{v}_{\mathrm{DVL}}^{\prime n}=C_b^n \boldsymbol{v}_{\mathrm{DVL}}^{\prime}
vDVL′n=CbnvDVL′然后,我们需要计算速度误差:
δ
v
=
v
I
N
S
n
−
v
D
V
L
′
n
\boldsymbol{\delta} \boldsymbol{v}=\boldsymbol{v}_{\mathrm{INS}}^n-\boldsymbol{v}_{\mathrm{DVL}}^{\prime n}
δv=vINSn−vDVL′n在紧组合中,我们可以使用卡尔曼滤波器来估计和补偿误差。扩展状态向量以包括时间不同步误差
δ
t
\delta t
δt:
x
=
[
δ
p
δ
v
θ
δ
t
]
\boldsymbol{x}=\left[\begin{array}{l} \boldsymbol{\delta} \boldsymbol{p} \\ \boldsymbol{\delta} \boldsymbol{v} \\ \boldsymbol{\theta} \\ \delta t \end{array}\right]
x=
δpδvθδt
然后,我们可以使用扩展的卡尔曼滤波器来估计和更新状态向量
x
\boldsymbol{x}
x。在此过程中,受影响的DVL速度观测值将作为输入。更新后的状态向量将被用于修正INS输出,从而提高组合导航系统的性能。
总之,在INS/DVL紧组合导航系统中,时间不同步值为 δ t \delta t δt会影响DVL速度观测值,从而影响紧组合算法的性能。为了解决这个问题,可以在紧组合的卡尔曼滤波器中添加一个状态变量来估计和补偿时间不同步误差。这将有助于提高紧组合的性能,使其在各种环境条件下表现更加稳定。
根据之前的推导,我们知道了GNSS和INS传感器之间的时间不同步误差会导致速度不同步误差
δ
v
δ
t
n
\delta \boldsymbol{v}_{\delta t}^n
δvδtn和位置不同步误差
δ
p
δ
t
\delta \boldsymbol{p}_{\delta t}
δpδt。现在我们结合松组合和紧组合来说明这些误差对组合导航系统的影响。
{}()【】{}()【】{}()【】{}()【】{}()【】
我们得到了速度不同步误差
δ
v
δ
t
n
\delta \boldsymbol{v}{\delta t}^n
δvδtn和位置不同步误差
δ
p
δ
t
\delta \boldsymbol{p}{\delta t}
δpδt的详细表达式。在实际应用中,可以通过测量和计算这些误差,对组合导航系统进行修正和补偿,以提高整个系统的精度和稳定性。
(2)计算位置不同步误差
δ
p
δ
t
\delta \boldsymbol{p}_{\delta t}
δpδt
计算不同步时间段内的速度积分矩阵
M
p
v
\boldsymbol{M}_{p v}
Mpv:
位置不同步误差
δ
p
δ
t
\delta \boldsymbol{p}_{\delta t}
δpδt。
不同步误差
δ
v
δ
t
n
\delta \boldsymbol{v}_{\delta t}^n
δvδtn。
(1)计算载体的平均线加速度
a
n
\boldsymbol{a}^n
an:
速度不同步误差
δ
v
δ
t
n
\delta \boldsymbol{v}_{\delta t}^n
δvδtn: