一种基于序贯滤波的组合导航方法与流程

1.本发明涉及姿态测量领域，尤其涉及一种基于序贯滤波的组合导航方法。


背景技术：

2.目前，gnss/ins组合导航系统被公认为是最佳的组合导航方案，以ins 为核心、gnss为辅助设备。ins能提供姿态、方位、速度和位置，甚至还包括加速度和角速度等导航信息，具有自主性强、动态性能好、导航信息全面且输出频率高等优点，但是它的误差会随时间不断累积。gnss能提供精度高且误差不随时间增大的速度、位置等信息，但是导航信息不够全面、信号易受到干扰、在室内等有遮挡的环境下接收不到信号而无法使用。ins和gnss之间具有互补性。在组合导航领域，通常使用kalman滤波技术对ins和gnss的信息进行融合，实现高精度的导航定位。在短时间内，位置不易受到速度和姿态误差的影响，因此，可直接使用gnss提供的速度信息作为观测量进行姿态解算。然而，仅使用速度作为观测量时，方位失准角的观测性较弱，又由于惯导零偏等噪声的存在，容易导致方位角随时间发散。引入方位角作为观测量可以增强方位失准角的观测性，能有效地抑制方位角随时间的发散，从而提高组合导航姿态测量的精度。
3.如使用方位角和速度作为观测量，前提是方位角和速度一起作为输入量进行观测，即方位角和速度的更新频率一致。在其中一类观测量无效时，有三种方法可以解决此问题：1)手动人为地设置无效观测量的观测误差，存在一定的主观性；2)增加观测方程，只使用有效的观测量进行观测，然而当有n类观测量时，将会需要2
n-1个观测方程，增加了系统的复杂性，从而降低了计算效率； 3)不进行观测，即认为其他的观测量也无效，然而该方法对于数据的利用率不高，会导致系统的计算精度下降。
4.因此需要研发出一种基于序贯滤波的组合导航方法来解决上述问题。


技术实现要素：

5.本发明的目的就在于为了解决上述问题设计了一种基于序贯滤波的组合导航方法。
6.本发明通过以下技术方案来实现上述目的：
7.一种基于序贯滤波的组合导航方法，包括以下步骤：
8.s1、获取载体的位置信息p(tk)、陀螺仪输出的角速度信息加速度计输出的比力fb(tk)；
9.s2、根据载体的位置信息p(tk)以及陀螺仪输出的角速度信息更新姿态，设和表示t
k-1
和tk时刻从b系旋转至n系的姿态矩阵，为i系到 t
k-1
时刻n系的旋转矩阵，为t
k-1
时刻b系到i系的旋转矩阵，表示以i系作为参考基准，b系从tk时刻到t
k-1
时刻的旋转矩阵，表示以i系作为参考基准，n系从t
k-1
时刻到tk时刻的旋转
矩阵：
[0010][0011]
其中，旋转矩阵由陀螺仪的输出角速度信息确定，由n系相对于i系的角速度确定；
[0012]
若陀螺仪在时间段[t
k-1
,tk]内，t＝t
k-t
k-1
；假设为常值，记为并且认为由速度和位置引起的变化很小，可将视为常值，那么：
[0013][0014][0015]
式中，m
rv
(
·
)表示将等效旋转矢量转换成对应的旋转矩阵，i表示3
×
3的单位矩阵，(
·
)
t
表示矩阵的转置，为b系从tk时刻到t
k-1
时刻的等效旋转矢量，为n系从tk时刻到t
k-1
时刻的等效旋转矢量，为等效旋转矢量的转角大小，为等效旋转矢量的转角大小；
[0016]
s3、根据加速度计输出的比力fb(tk)、位置信息p(tk)和上一步获得姿态矩阵更新速度g为当地的重力加速度，gn(＝[0 0
ꢀ‑
g]
t
)为n系下的重力加速度，为地球自转引起的导航系旋转，为惯性系统在地球表面附近移动因地球表面弯曲而引起的n系旋转，将更新后的旋转矩阵简化为fb(tk)简化为fb，速度vn的微分方程为
[0017][0018]
式中，为求一阶导数的符号，为n系下的运动加速度，为由载体运动和地球自转引起的哥氏加速度，为由载体运动引起的对地向心加速度，为有害加速度；
[0019]
s4、根据姿态和速度更新方程，确定姿态误差和速度误差δvn的微分方程：
[0020][0021][0022]
式中，φ为计算导航系到理想导航系的等效旋转矢量，为n系相对于i系的旋转，包含地球自转引起的导航系旋转和惯性系统在地球表面附近移动因地球表面弯曲而引起的n系旋转两部分，为导航系计算误差，εb为b系下的陀螺仪零偏，
为n系下的加速度，为导航系旋转计算误差，
▽b为b系下的加速度计零偏；
[0023]
s5、设状态变量为xk＝[φ δv
n εbꢀ▽b]
t
，将观测量zk分为gps与imu之间的方位角误差和速度误差两部分，即根据姿态、速度误差的微分方程确定状态方程和量测方程：
[0024]
xk＝φ
k/k-1
x
k-1
+w
k-1
[0025][0026]
式中，φ
k/k-1
为12*12维的状态系数矩阵，为1*12和3*12系数矩阵，w
k-1
、和为零均值的高斯白噪声噪声序列，并且并且它们之间互不相关，即满足：
[0027][0028][0029][0030]
其中，e[
·
]表示求期望，q
k-1
、rk为噪声均方差阵；设om×n为m*n维的零矩阵，im×m为m阶的单位阵，p为俯仰角，h为方位角，和可描述为
[0031][0032][0033]
s6、确定初始状态变量x0、噪声的方差矩阵q0、r0以及误差均方阵p0的初始值，其中
[0034]
s7、进行kalman滤波时间更新
[0035][0036][0037]
式中，x
k/k-1
为状态一步预测误差，p
k/k-1
为状态一步预测均方误差阵；
[0038]
s8、判断方位角量测量是否有效，若无效时，则令并执行步骤s9；否则，使用方位角观测量进行量测更新
[0039][0040]
[0041][0042]
式中，为kalman滤波增益。
[0043]
s9、判断速度量测量是否有效，若无效时，则令并执行步骤s10；否则，使用速度量测量进行量测更新，并令
[0044][0045][0046][0047]
式中，为kalman滤波增益。
[0048]
s10、反馈状态变量xk，补偿tk时刻的姿态、速度误差，对姿态、速度进行修正，令k＝k+1；
[0049]
上述中，i系为地心惯性坐标系(oxiyizi)：原点位于地球中心，zi轴沿地轴指向北，xi、yi轴在地球上分别指向空间的两颗恒星；
[0050]
n系为导航坐标系(ox
nynzn
)：原点位于载体重心，xn轴指向东，yn轴指向北，zn轴指向天，为东北天坐标系；
[0051]
b系为载体坐标系(oxbybzb)：原点位于载体重心，xb轴沿载体横轴指向右， yb轴沿载体纵轴指向前，zb轴指向上。
[0052]
本发明的有益效果在于：
[0053]
通过用基于序贯滤波的kalman滤波技术，可以将更新频率不同的观测量和量测方程分组，只使用有效的观测量进行量测更新，在不增加计算量的同时(即，当有n类量测量时，量测方程最多可分为n组)，能有效解决方位角量测和速度量测更新频率不一致的问题，提高了观测量使用的灵活性；同时由于序贯滤波技术将量测方程分为多个低维量测方程的原因，降低了求kalman滤波增益kk时矩阵的求逆计算量，从而在一定程度上提高了系统的计算效率。
具体实施方式
[0054]
下面对本发明的具体实施方式进行详细说明。
[0055]
一种基于序贯滤波的组合导航方法，包括以下步骤：
[0056]
s1、获取载体的位置信息p(tk)、陀螺仪输出的角速度信息加速度计输出的比力fb(tk)；
[0057]
s2、根据载体的位置信息p(tk)以及陀螺仪输出的角速度信息更新姿态，设和表示t
k-1
和tk时刻从b系旋转至n系的姿态矩阵，为i系到t
k-1
时刻n系的旋转矩阵，为t
k-1
时刻b系到i系的旋转矩阵，表示以i系作为参考基准，b系从tk时
刻到t
k-1
时刻的旋转矩阵，表示以i系作为参考基准，n系从t
k-1
时刻到tk时刻的旋转矩阵：
[0058][0059]
其中，旋转矩阵由陀螺仪的输出角速度信息确定，由n系相对于i系的角速度确定；
[0060]
若陀螺仪在时间段[t
k-1
,tk]内，t＝t
k-t
k-1
；假设为常值，记为并且认为由速度和位置引起的变化很小，可将视为常值，那么：
[0061][0062][0063]
式中，m
rv
(
·
)表示将等效旋转矢量转换成对应的旋转矩阵，i表示3
×
3的单位矩阵，(
·
)
t
表示矩阵的转置，为b系从tk时刻到t
k-1
时刻的等效旋转矢量，为n系从tk时刻到t
k-1
时刻的等效旋转矢量，为等效旋转矢量的转角大小，为等效旋转矢量的转角大小；
[0064]
s3、根据加速度计输出的比力fb(tk)、位置信息p(tk)和上一步获得姿态矩阵更新速度g为当地的重力加速度，gn(＝[00-g]
t
)为n系下的重力加速度，为地球自转引起的导航系旋转，为惯性系统在地球表面附近移动因地球表面弯曲而引起的n系旋转，将更新后的旋转矩阵简化为fb(tk)简化为fb，速度vn的微分方程为
[0065][0066]
式中，为求一阶导数的符号，为n系下的运动加速度，为由载体运动和地球自转引起的哥氏加速度，为由载体运动引起的对地向心加速度，为有害加速度；
[0067]
s4、根据姿态和速度更新方程，确定姿态误差和速度误差δvn的微分方程：
[0068][0069][0070]
式中，φ为计算导航系到理想导航系的等效旋转矢量，为n系相对于i系的旋转，包含地球自转引起的导航系旋转和惯性系统在地球表面附近移动因地球
表面弯曲而引起的n系旋转两部分，为导航系计算误差，εb为b系下的陀螺仪零偏，为n系下的加速度，为导航系旋转计算误差，
▽b为b系下的加速度计零偏；
[0071]
s5、设状态变量为xk＝[φ δv
n εbꢀ▽b]
t
，将观测量zk分为gps与imu之间的方位角误差和速度误差两部分，即根据姿态、速度误差的微分方程确定状态方程和量测方程：
[0072]
xk＝φ
k/k-1
x
k-1
+w
k-1
[0073][0074]
式中，φ
k/k-1
为12*12维的状态系数矩阵，为1*12和3*12系数矩阵，w
k-1
、和为零均值的高斯白噪声噪声序列，并且并且它们之间互不相关，即满足：
[0075][0076][0077][0078]
其中，e[
·
]表示求期望，q
k-1
、rk为噪声均方差阵；设om×n为m*n维的零矩阵，im×m为m阶的单位阵，p为俯仰角，h为方位角，和可描述为
[0079][0080][0081]
s6、确定初始状态变量x0、噪声的方差矩阵q0、r0以及误差均方阵p0的初始值，其中
[0082]
s7、进行kalman滤波时间更新
[0083][0084][0085]
式中，x
k/k-1
为状态一步预测误差，p
k/k-1
为状态一步预测均方误差阵；
[0086]
s8、判断方位角量测量是否有效，若无效时，则令并执行步骤s9；否则，使用方位角观测量进行量测更新
[0087]
[0088][0089][0090]
式中，为kalman滤波增益。
[0091]
s9、判断速度量测量是否有效，若无效时，则令并执行步骤s10；否则，使用速度量测量进行量测更新，并令
[0092][0093][0094][0095]
式中，为kalman滤波增益。
[0096]
s10、反馈状态变量xk，补偿tk时刻的姿态、速度误差，对姿态、速度进行修正，令k＝k+1；
[0097]
上述中，i系为地心惯性坐标系(oxiyizi)：原点位于地球中心，zi轴沿地轴指向北，xi、yi轴在地球上分别指向空间的两颗恒星；
[0098]
n系为导航坐标系(ox
nynzn
)：原点位于载体重心，xn轴指向东，yn轴指向北，zn轴指向天，为东北天坐标系；
[0099]
b系为载体坐标系(oxbybzb)：原点位于载体重心，xb轴沿载体横轴指向右， yb轴沿载体纵轴指向前，zb轴指向上。
[0100]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。