公开/公告号CN112378400A
专利类型发明专利
公开/公告日2021-02-19
原文格式PDF
申请/专利权人 湖南航天机电设备与特种材料研究所;
申请/专利号CN202011189897.9
申请日2020-10-30
分类号G01C21/16(20060101);G01C25/00(20060101);G01S19/39(20100101);G01S19/40(20100101);
代理机构43113 长沙正奇专利事务所有限责任公司;
代理人郭立中;曾利平
地址 410205 湖南省长沙市枫林三路217号
入库时间 2023-06-19 09:55:50
技术领域
本发明属于组合导航技术领域,尤其涉及一种双天线GNSS辅助的捷联惯导组合导航方法。
背景技术
惯性导航作为一种全自主的导航方式,能够在各种工况条件下工作,不受运动环境的限制,但长期工作导致误差累积发散。GNSS(Global Navigation Satellite System,全球导航卫星系统)作为一种全天候、高精度的导航定位系统,能够提供较为精确的导航定位信息,但在城市峡谷环境中,常常受到高层建筑、树木、隧道等遮挡或者受到多径散射,导致信号丢失或没有足够的卫星获取高精度的定位信息,可靠性有所下降,面临着难以定位的问题。
发明内容
本发明的目的在于提供一种双天线GNSS辅助的捷联惯导组合导航方法,以解决惯性导航因长期工作导致误差累积发散以及GNSS难以定位的问题,将捷联惯导与双天线GNSS结合进行组合导航,双天线GNSS除了常规的速度、位置信息,还能够提供较为精确的方位,在与捷联惯导组合时可进一步提升方位精度,同时在动态机动条件下也可完成对捷联惯导的初始化,采用双天线GNSS辅助的捷联惯导组合导航可以在定向、导航定位方面改善系统的性能,提升系统精度。
本发明独立权利要求的技术方案解决了上述发明目的中的一个或多个。
本发明是通过如下的技术方案来解决上述技术问题的:一种双天线GNSS辅助的捷联惯导组合导航方法,包括以下步骤:
步骤1:分别在双天线GNSS辅助的捷联惯导组合导航系统静止、动态时对捷联惯导进行初始对准,以获得初始姿态、初始速度和初始位置;并在初始对准后捷联惯导进入组合导航状态;
步骤2:捷联惯导进行导航解算,获得实时姿态、实时速度和实时位置;
步骤3:建立双天线GNSS辅助的捷联惯导组合导航系统的卡尔曼滤波模型;
步骤4:利用所述卡尔曼滤波模型对状态量进行估计,得到捷联惯导的速度误差、姿态误差、位置误差、陀螺零偏误差和加速度计零偏误差;
步骤5:利用所述步骤4的速度误差、姿态误差、位置误差、陀螺零偏误差和加速度计零偏误差对捷联惯导解算出的实时速度、姿态矩阵、实时位置、陀螺输出和加速度计输出进行反馈校正,得到校正后的实时速度、姿态矩阵、实时位置、陀螺输出和加速度计输出。
本发明中,通过GNSS提供位置、速度和方位等信息,使系统在静态或动态运动过程均可完成初始对准并进入组合导航,使系统具有更为普遍的适用性;在组合过程中对导航误差、陀螺零偏误差及加速度计零偏误差进行在线校正,提高了系统的导航精度。
进一步地,所述步骤1中,在双天线GNSS辅助的捷联惯导组合导航系统静止时对捷联惯导进行初始对准的实现过程为:
若GNSS的方位输出无效,则采集捷联惯导陀螺的静态输出增量和加速度计的静态输出增量,得到初始姿态为:
α
其中,α
若GNSS的方位输出有效,则采用GNSS测得的方位角
所述GNSS测得的速度和位置为初始速度和初始位置。
优选地,所述步骤1中,在双天线GNSS辅助的捷联惯导组合导航系统动态时对捷联惯导进行初始对准的实现过程为:
若GNSS的方位输出无效,则判断运动速度是否大于速度阈值,当运动速度小于或等于所述速度阈值时,加速运动,直到运动速度大于所述速度阈值;当运动速度大于所述速度阈值时,初始姿态为:
其中,α
若GNSS的方位输出有效,则采用GNSS测得的方位角
所述GNSS测得的速度和位置为初始速度和初始位置。
设置速度阈值避免了GNSS速度在低速时计算误差相对较大的问题。
优选地,所述速度阈值为5m/s。
进一步地,所述步骤2中,姿态矩阵的更新方程为:
其中,下标k为当前时刻,下标k-1为上一时刻,
实时速度的更新方程为:
其中,
实时位置的更新方程为:
其中,
进一步地,所述步骤3中,卡尔曼滤波模型的建模过程为:
步骤3.1:选取状态量,状态量X为:
其中,δv
步骤3.2:建立卡尔曼滤波模型的状态转移方程,所述状态转移方程为:
X
其中,X
步骤3.3:建立卡尔曼滤波观测方程,所述卡尔曼滤波观测方程为:
其中,Z
进一步地,所述步骤5中,校正后的实时速度为:
v
v
v
其中,v
校正后的姿态矩阵为:
其中,
校正后的实时位置为:
L'=L-X(7)
λ'=λ-X(8)
h'=h-X(9)
其中,L、λ和h分别为校正前的纬度、经度和高度,L'、λ'和h'分别为校正后的纬度、经度和高度,X(7)、X(8)和X(9)分别为状态量估计值的第7个、第8个和第9个;
校正后的陀螺输出为:
校正后的加速度计输出为:
其中,
进一步地,在所述步骤1之前还包括捷联惯导与双天线GNSS安装的步骤,具体安装步骤为:
根据捷联惯导定义的方位输出轴方向,确定GNSS双天线连线的方向,使捷联惯导方位输出轴方向与GNSS方位输出轴方向平行,且GNSS双天线之间的距离尽可能长;将GNSS主天线安装在捷联惯导的正上方,且GNSS主天线与捷联惯导的安装位置尽可能靠近。
GNSS双天线之间的距离越长,其方位精度越高,GNSS主天线安装于捷联惯导的正上方,可以减小较大方位机动下的杆臂误差对系统精度的影响。
进一步地,在所述步骤3与步骤4之间还包括对卡尔曼滤波模型中的状态量以及协方差矩阵进行更新的步骤,具体更新公式为:
P
其中,下标k为当前时刻,下标k-1为上一时刻,m=1,2,…,M,M为卡尔曼滤波观测量Z
在卡尔曼滤波模型更新时,对于卡尔曼增益的求解包含了矩阵求逆运算,为了避免复杂的矩阵求逆运算,采用标量测量依次处理法来优化卡尔曼滤波模型的更新,提升了组合导航卡尔曼滤波计算过程的效率。
进一步地,在所述对卡尔曼滤波模型中的状态量以及协方差矩阵更新之后,在所述步骤4之前还包括系统测量值检测隔离的步骤,具体步骤为:
构造RAIM(RAIM,Receiver Autonomous Integrity Monitoring)故障检测隔离观测信息计算值y
y
其中,Z
分别设定速度故障检测门限值、位置故障检测门限值以及方位故障检测门限值,如果超过对应的故障检测门限值,则仅进行捷联惯导的纯惯性导航解算,否则利用卡尔曼滤波模型进行状态量的估计。
有益效果
与现有技术相比,本发明所提供的一种双天线GNSS辅助的捷联惯导组合导航方法,通过GNSS提供位置、速度和方位等信息,使系统在静态或动态运动过程均可完成初始对准并进入组合导航,使系统具有更为普遍的适用性;在组合过程中对导航误差、陀螺零偏误差及加速度计零偏误差进行在线校正,提高了系统的导航精度。
附图说明
为了更清楚地说明本发明的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一个实施例,对于本领域普通技术人员来说,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本发明实施例中双天线GNSS辅助的捷联惯导组合导航原理图。
具体实施方式
下面结合本发明实施例中的附图,对本发明中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例,都属于本发明保护的范围。
如图1所示,本实施例所提供的一种双天线GNSS辅助的捷联惯导组合导航方法,包括以下步骤:
1、捷联惯导与双天线GNSS的安装
根据捷联惯导定义的方位输出轴方向,确定GNSS双天线连线的方向,使捷联惯导方位输出轴方向与GNSS方位输出轴方向平行,且GNSS双天线之间的距离尽可能长;将GNSS主天线安装在捷联惯导的正上方,且GNSS主天线与捷联惯导的安装位置尽可能靠近。
GNSS双天线之间的距离越长,其方位精度越高,GNSS主天线安装于捷联惯导的正上方,可以减小较大方位机动下的杆臂误差对系统精度的影响。
2、捷联惯导的初始对准
分别在双天线GNSS辅助的捷联惯导组合导航系统静止、动态时对捷联惯导进行初始对准,以获得初始姿态、初始速度和初始位置;并在初始对准后捷联惯导进入组合导航状态。
定义捷联惯导所在的载体坐标系为b,导航坐标系n为东北天(ENU)坐标系,i为惯性坐标系,e为地球坐标系。
2.1静态时的初始对准过程
若GNSS的方位输出无效,即GNSS此时方位信息无效,则采集5min内捷联惯导陀螺的静态输出增量和加速度计的静态输出增量,得到初始姿态为:
α
其中,α
若GNSS的方位输出有效,则采用GNSS测得的方位角
GNSS测得的速度和位置为初始速度和初始位置。
2.2动态时的初始对准过程
若GNSS的方位输出无效,则判断运动速度是否大于速度阈值,当运动速度小于或等于速度阈值时,加速运动,直到运动速度大于速度阈值;当运动速度大于速度阈值时,初始姿态为:
其中,α
若GNSS的方位输出有效,则采用GNSS测得的方位角
GNSS测得的速度和位置为初始速度和初始位置。
3、捷联惯导进行导航解算,获得实时姿态、实时速度和实时位置。
利用以下姿态矩阵、速度以及位置更新方程进行捷联惯导导航解算。
姿态矩阵的更新方程为:
其中,下标k为当前时刻,下标k-1为上一时刻,
实时速度的更新方程为:
其中,
实时位置的更新方程为:
其中,
4、建立双天线GNSS辅助的捷联惯导组合导航系统的卡尔曼滤波模型。
卡尔曼滤波模型的建模过程为:
4.1:选取卡尔曼滤波模型的状态量,状态量X为:
其中,δv
4.2:建立卡尔曼滤波模型的状态转移方程,状态转移方程为:
X
其中,X
4.3:建立卡尔曼滤波观测方程,卡尔曼滤波观测方程为:
其中,Z
5、对卡尔曼滤波模型中的状态量以及协方差矩阵进行更新
在完成捷联惯导导航解算和卡尔曼滤波模型的构建后,利用卡尔曼滤波对系统状态量进行估计。在卡尔曼滤波模型更新时,对于卡尔曼增益的求解包含了矩阵求逆运算,为了避免复杂的矩阵求逆运算,采用标量测量依次处理法来优化卡尔曼滤波模型的更新,提升了组合导航卡尔曼滤波计算过程的效率,标量测量依次处理法的处理结果与正常向量法的处理结果一致,且与各个标量测量值的先后顺序无关。
状态量以及协方差矩阵的更新公式为:
P
其中,下标k为当前时刻,下标k-1为上一时刻,m=1,2,…,M,M为卡尔曼滤波观测量Z
6、系统测量值检测隔离
当系统某些测量值出现错误或严重偏差时,容易污染其它通道,甚至会造成所有通道误差跟踪计算的失锁。比如卫星信号突然失效时,此时测量值会变得不准确,所以对于系统测量值的故障检测和隔离,变得很有必要,本发明采用一种测量残余检测RAIM方法实现GNSS卫星信号丢失或数据跳变等异常观测信息的检测及隔离,提高系统计算结果的可靠性,避免了利用错误测量数据进行计算,确保系统的组合导航精度。
构造RAIM(RAIM,Receiver Autonomous Integrity Monitoring)故障检测隔离观测信息计算值y
y
其中,Z
分别设定速度故障检测门限值、位置故障检测门限值以及方位故障检测门限值,如果超过对应的故障检测门限值,则仅进行捷联惯导的纯惯性导航解算,不进行当前观测信息的组合导航,提高了系统的可靠性,否则利用卡尔曼滤波模型进行状态量的估计。
7、利用卡尔曼滤波模型对状态量进行估计,得到捷联惯导的速度误差、姿态误差、位置误差、陀螺零偏误差和加速度计零偏误差。
8、利用步骤7的速度误差、姿态误差、位置误差、陀螺零偏误差和加速度计零偏误差对捷联惯导解算出的实时速度、姿态矩阵、实时位置、陀螺输出和加速度计输出进行反馈校正,得到校正后的实时速度、姿态矩阵、实时位置、陀螺输出和加速度计输出。
校正后的实时速度为:
v
v
v
其中,v
校正后的姿态矩阵为:
其中,
校正后的实时位置为:
L'=L-X(7)
λ'=λ-X(8)
h'=h-X(9)
其中,L、λ和h分别为校正前的纬度、经度和高度,L'、λ'和h'分别为校正后的纬度、经度和高度,X(7)、X(8)和X(9)分别为状态量估计值的第7个、第8个和第9个;
校正后的陀螺输出为:
校正后的加速度计输出为:
其中,
本发明所涉及的一种双天线GNSS辅助的捷联惯导组合导航方法,其中GNSS(含双天线)采集的方位、三维速度、三维位置信息,与捷联惯导导航解算的方位、三维速度、三维位置信息进行组合,建立组合导航状态及观测模型,采用卡尔曼滤波快速计算方法对捷联惯导的误差进行估计,并通过反馈校正对导航误差及陀螺和加速度计的零偏误差进行实时在线补偿,提高了组合导航的精度;采用测量残余检测RAIM方法,对GNSS卫星信号丢失或数据跳变等异常观测信息进行检测及隔离,确保了系统的组合导航精度。该组合导航方法可适用于系统初始时刻处于静态或动态运动的场合,具有普遍适用性。
以上所揭露的仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到变化或变型,都应涵盖在本发明的保护范围之内。
机译: 地下采煤机捷联惯导系统组合初始对准系统和对准方法
机译: 地下采煤机捷联惯导系统组合初始对准系统和对准方法
机译: 一种辅助血管导航的方法,包括现场采集图像,准备口罩,用仪器采集直接图像以及确定组合导航图像