一种基于加计内阻尼的组合导航系统及其导航方法与流程

本发明属于卫星导航领域，具体涉及一种基于加计内阻尼的组合导航系统及其导航方法。

背景技术：

惯性导航系统以其自主性高、数据连续性好等优点应用到很多领域，但高精度惯性器件价格高昂，低精度惯性器件导航误差难以满足导航精度要求。随着惯性/卫星组合导航系统技术的发展，低精度惯性导航系统在无人系统领域得到广泛应用。传统惯性/卫星组合导航方法，卫星定位断开的精度、保持能力较差，通过加计内阻尼方法，能够提高惯性/卫星组合精度以及卫星定位断开的保持能力。

现有中国专利文件200610040662.7公布了一种基于捷联惯性导航系统的姿态估计和融合的方法，其主要技术方案为：一种基于捷联惯性导航系统的姿态估计和融合的方法，包括：(1)利用六自由度惯性测量组件(简称imu)中的传感器感应载体运动特性；(2)进行捷联惯性解算；(3)在进行步骤(2)同时利用加速度估计系统的姿态信号；(4)判断姿态估计值的可信度；(5)进行姿态信息融合；(6)输出导航参数；本发明的方法具有以下优点：(1)不增加任何硬件成本并且不改变硬件安装结构；(2)具有完全的自主性，不受外界环境的影响，能够全天候工作；(3)能够有效提高系统的导航精度，特别是姿态精度；(4)系统能够为控制显示装置提供每秒钟50次以上的导航信号。其是基于捷联惯性导航系统，虽都解决提高导航精度的问题，但整体方案与本发明方案不同。

技术实现要素：

本发明的目的是解决上述问题，提供一种基于加计内阻尼的组合导航系统及其导航方法，能够提高惯性/卫星组合导航精度以及卫星定位断开的保持能力。

为实现上述目的，本发明提供如下技术方案：

一种基于加计内阻尼的组合导航系统，包括卫星接收机设备、惯导系统、组合导航计算机及数据采集模块，所述卫星接收机设备输出卫星定位数据以及时间同步信息，所述惯导系统输出惯性导航数据，所述组合导航计算机进行基于加计内阻尼的组合导航计算，所述数据采集模块负责接收导航指令、惯性数据以及卫星定位数据，并发送组合导航结果。

一种采用上述基于加计内阻尼的组合导航系统的组合导航方法，包括以下步骤：

a)数据采集模块通过总线接收导航控制指令、惯性数据及携带同步时间戳的卫星定位数据，并输送至组合导航计算机；

b)组合导航计算机通过惯性数据判断当前的运动状态，结合外部的导航控制指令及卫星数据有效性，进行导航模式的选择；

c)组合导航计算机根据选择的导航模式进行组合量测更新计算，得到系统状态误差；

d)数据采集模块输出组合导航计算结果。

进一步的，所述步骤b)中导航模式的选择方法为：导航控制指令分为自动选择、人工选择两种方式，在自动选择模式下，导航模式的降级顺序为惯性/卫星组合导航、加计内阻尼航姿导航、纯惯性导航；人工选择模式下，控制指令为卫星组合导航和自主导航，控制指令为卫星组合导航时，若卫星定位有效，则进入惯性/卫星组合导航模式；否则，进入纯惯性导航模式；控制指令为自主导航时，若满足运动平稳无大机动条件，则进入加计内阻尼航姿导航模式，否则进入纯惯性导航模式。

进一步的，所述运动平稳无大机动条件为：

其中，

为载体线加速度绝对值(1秒滑动平均值)；

为载体角速率绝对值(1秒滑动平均值)。

5、根据权利要求2所述的一种基于加计内阻尼的组合导航方法，其特征在于，所述步骤c)中组合量测更新计算方法为：

pk＝apk-1a^t+q

kk＝pkh^t(hpkh^t+r)^-1

pk+＝(i-kkh)pk

其中，

为惯导系统误差状态，包括：

[δφδλδhδveδvnδvuδplat_eδplat_nδplat_uδaccl_xδaccl_yδaccl_z]

其依次为纬度误差、经度误差、高度误差、东向速度误差、北向速度误差、天向速度误差、等效东向姿态误差、等效北向姿态误差、等效天向姿态误差、x加计误差、y加计误差、z加计误差；

a为状态转移矩阵；

b为驱动矩阵，u为输入矩阵；

pk为状态协方差矩阵；

q为系统状态噪声矩阵；

h为观测矩阵；

r为量测噪声矩阵；

kk为增益矩阵；

zk为量测矩阵。

进一步的，所述量测矩阵zk计算方法为：

其中：

inslat_syn为时间同步后的惯导纬度，gpslat_syn为时间同步后的gps纬度；

inslon_syn为时间同步后的惯导经度，gpslon_syn为时间同步后的gps经度；

insalt_syn为时间同步后的惯导高度，gpsalt_syn为时间同步后的gps高度；

insve_syn为时间同步后的惯导东向速度，gpsve_syn为时间同步后的gps动向速度；

insvn_syn为时间同步后的惯导北向速度，gpsvn_syn为时间同步后的gps北向速度；

insvu_syn为时间同步后的惯导天向速度，gpsvu_syn为时间同步后的gps天向速度；

为等效东向平均比力(1秒滑动平均)；

为等效北向平均比力(1秒滑动平均)。

进一步的，所述量测矩阵h计算方法为：

若导航模式为惯性/卫星组合，则：

其中，zero_i为元素为0的矩阵；

若导航模式为加计内阻尼航姿导航，则：

其中，insg_syn为当前位置的地球重力；

若导航模式为纯惯性导航，则：

进一步的，所述量测噪声矩阵r采用自适应方法进行计算，其计算方式为：

其中，

r0为量测噪声值，为统计经验值；

zki为系统状态对应的可观量测；

k1为状态方差加权系数；

k2为量测加权系数。

本发明的有益效果在于：

通过本方法能够很清晰地描述了组合导航的整体方案，数据模块清晰，并对影响惯性/卫星组合导航精度以及卫星定位断开精度保持能力的关键技术点进行了准确描述，能够很容易地设计惯性/卫星组合导航系统，得到较高精度的结果，能够提高惯性/卫星组合精度以及卫星定位断开的保持能力。

附图说明

为了更清楚地说明本发明实施例技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明为了更清楚地说明本发明实施例或现有技术中的技术方案，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明整体工作流程图；

图2为本发明导航模式选择流程图。

具体实施方式

如图1—2所示的一种基于加计内阻尼的组合导航系统及其导航方法，所述组合导航系统包括卫星接收机设备、惯导系统、组合导航计算机及数据采集模块，所述卫星接收机设备输出卫星定位数据以及时间同步信息，所述惯导系统输出惯性导航数据，所述组合导航计算机进行基于加计内阻尼的组合导航计算，所述数据采集模块负责接收导航指令、惯性数据以及卫星定位数据，并发送组合导航结果。

所述组合导航方法，包括以下步骤：

a)数据采集模块通过总线接收导航控制指令、惯性数据及携带同步时间戳的卫星定位数据，并输送至组合导航计算机；

b)组合导航计算机通过惯性数据判断当前的运动状态，结合外部的导航控制指令及卫星数据有效性，进行导航模式的选择；

c)组合导航计算机根据选择的导航模式进行组合量测更新计算，得到系统状态误差；

d)数据采集模块输出组合导航计算结果。

进一步的，所述步骤b)中导航模式的选择方法为：导航控制指令分为自动选择、人工选择两种方式，在自动选择模式下，导航模式的降级顺序为惯性/卫星组合导航、加计内阻尼航姿导航、纯惯性导航；人工选择模式下，控制指令为卫星组合导航和自主导航，控制指令为卫星组合导航时，若卫星定位有效，则进入惯性/卫星组合导航模式；否则，进入纯惯性导航模式；控制指令为自主导航时，若满足运动平稳无大机动条件，则进入加计内阻尼航姿导航模式，否则进入纯惯性导航模式。

进一步的，所述运动平稳无大机动条件为：

其中，

为载体线加速度绝对值(1秒滑动平均值)；

为载体角速率绝对值(1秒滑动平均值)。

5、根据权利要求2所述的一种基于加计内阻尼的组合导航方法，其特征在于，所述步骤c)中组合量测更新计算方法为：

pk＝apk-1a^t+q

kk＝pkh^t(hpkh^t+r)^-1

pk+＝(i-kkh)pk

其中，

为惯导系统误差状态，包括：

[δφδλδhδveδvnδvuδplat_eδplat_nδplat_uδaccl_xδaccl_yδaccl_z]

其依次为纬度误差、经度误差、高度误差、东向速度误差、北向速度误差、天向速度误差、等效东向姿态误差、等效北向姿态误差、等效天向姿态误差、x加计误差、y加计误差、z加计误差；

a为状态转移矩阵；

b为驱动矩阵，u为输入矩阵；

pk为状态协方差矩阵；

q为系统状态噪声矩阵；

h为观测矩阵；

r为量测噪声矩阵；

kk为增益矩阵；

zk为量测矩阵。

进一步的，所述量测矩阵zk计算方法为：

其中：

inslat_syn为时间同步后的惯导纬度，gpslat_syn为时间同步后的gps纬度；

inslon_syn为时间同步后的惯导经度，gpslon_syn为时间同步后的gps经度；

insalt_syn为时间同步后的惯导高度，gpsalt_syn为时间同步后的gps高度；

insve_syn为时间同步后的惯导东向速度，gpsve_syn为时间同步后的gps动向速度；

insvn_syn为时间同步后的惯导北向速度，gpsvn_syn为时间同步后的gps北向速度；

insvu_syn为时间同步后的惯导天向速度，gpsvu_syn为时间同步后的gps天向速度；

为等效东向平均比力(1秒滑动平均)；

为等效北向平均比力(1秒滑动平均)。

进一步的，所述量测矩阵h计算方法为：

若导航模式为惯性/卫星组合，则：

其中，zero_i为元素为0的矩阵；

若导航模式为加计内阻尼航姿导航，则：

其中，insg_syn为当前位置的地球重力；

若导航模式为纯惯性导航，则：

进一步的，所述量测噪声矩阵r采用自适应方法进行计算，其计算方式为：

其中，

r0为量测噪声值，为统计经验值；

zki为系统状态对应的可观量测；

k1为状态方差加权系数；

k2为量测加权系数。

通过本方法能够很清晰地描述了组合导航的整体方案，数据模块清晰，并对影响惯性/卫星组合导航精度以及卫星定位断开精度保持能力的关键技术点进行了准确描述，能够很容易地设计惯性/卫星组合导航系统，得到较高精度的结果，能够提高惯性/卫星组合精度以及卫星定位断开的保持能力。

本发明中未做详细描述的内容均为现有技术。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。