CN102538788B - 一种基于状态估计和预测的低成本阻尼导航方法 - Google Patents
一种基于状态估计和预测的低成本阻尼导航方法 Download PDFInfo
- Publication number
- CN102538788B CN102538788B CN201110273080.4A CN201110273080A CN102538788B CN 102538788 B CN102538788 B CN 102538788B CN 201110273080 A CN201110273080 A CN 201110273080A CN 102538788 B CN102538788 B CN 102538788B
- Authority
- CN
- China
- Prior art keywords
- error
- epsiv
- navigation
- phi
- inertial
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Landscapes
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明属于导航领域,具体涉及一种基于状态估计和预测的低成本阻尼导航方法。本方法利用卫星信息和惯导信息之差作为观测量,通过卡尔曼滤波对各项惯性误差进行估计,以速度误差作为控制量修正惯性导航的速度误差和失调角;并根据惯性误差估计结果对惯性输出信息进行修正。本发明提供了一种简单、廉价、有效的阻尼导航方法,提高低成本惯性/卫星组合导航系统精度,特别是姿态精度,以适应低成本高精度导航系统的要求。
Description
技术领域
本发明属于导航领域,具体涉及一种惯性/卫星组合导航系统基于状态估计和预测的阻尼方法,特别是一种低成本MEMS/GPS组合导航的误差阻尼方法。
背景技术
在惯性技术高速发展的基础上,低成本微机械惯性测量单元逐渐被广泛应用于惯性测量领域。但其精度较差,零偏稳定性和随机漂移比较大,无法进行惯性自主导航,即使使用GPS等辅助导航手段进行组合导航也难于克服非线性系统误差估计精度差的缺点。
发明内容
本发明的目的为提高低成本惯性/卫星组合导航系统精度,提供一种基于状态估计和预测的低成本阻尼导航方法。
本发明所采用的技术方案是:
一种基于状态估计和预测的低成本阻尼导航方法,利用卫星信息和惯导信息之差作为观测量,通过卡尔曼滤波对各项惯性误差进行估计,以速度误差作为控制量修正惯性导航的速度误差和失调角;并根据惯性误差估计结果对惯性输出信息进行修正。
如上所述的一种基于状态估计和预测的低成本阻尼导航方法,其中:当卫星导航系统正常定位时,惯性导航系统以卫星导航信息与惯导导航信息作为量测输入,使用卡尔曼滤波方法估计惯性导航系统各项惯性误差;当卫星导航出现异常时,不再进行量测计算,使用卡尔曼滤波方法预测惯性导航系统各项惯性误差。
如上所述的一种基于状态估计和预测的低成本阻尼导航方法,其中:在指北导航坐标系下,选取位置误差Δλ,速度误差ΔVN、ΔVE,失调角ΦN、ΦE、ΦU,陀螺漂移εz、εy、εz,加速度计零偏作为系统状态变量。
如上所述的一种基于状态估计和预测的低成本阻尼导航方法,其中:根据系统状态变量建立的阻尼导航惯导基本误差方程为:
式中:
VE为导航坐标系北向速度值,单位:米/秒;
为导航得到的当地纬度值,单位:弧度;
h为海拔高度,单位:米;
RM、RN分别为地球子午圈和卯酉圈地球半径,单位:米;
τx、τy、τz为陀螺一阶马尔可夫过程的时间常数;
ωxr、ωyr、ωzr为陀螺零均值高斯白噪声;
为加速度计零均值白噪声;
Tij(i=1~3,j=1~3)为捷联姿态矩阵相应各元素,
分别为惯性导航的北向和东向速度误差修正量;
分别为惯性导航的北向和东向失调角修正量;
为的导数。
如上所述的一种基于状态估计和预测的低成本阻尼导航方法,其中:所述修正量计算如下:
其中,C1、C2为误差修正阻尼系数,取值范围为0~1;
分别为惯性导航的北向和东向速度误差估计结果。
如上所述的一种基于状态估计和预测的低成本阻尼导航方法,其中:所述卡尔曼滤波过程如下:
取系统噪声向量:
其中,εxr,εyr,εzr分别为εx,εy,εz的噪声;
取惯导系统和卫星定位系统的位置、速度差为观测向量:
其中,λ、VE、VN表示由惯导得到的纬度、经度、东向速度、北向速度,λGPS、VE-GPS、VN-GPS表示由GPS得到的纬度、经度、东向速度、北向速度;
惯导系统误差状态方程表示为:
其中,A(t)为系统参数矩阵,D为系统噪声矩阵,L为控制量系数矩阵,U(t)为修正量,
量测方程为:
Y=HX(t)+η(t)
其中,H为量测参数阵,η(t)为量测噪声向量;
将状态方程和量测方程离散化为:
Xk=Φk,k-1Xk-1+LUk-1+DWk-1
Yk=HXk+ηk
其中,k表示卡尔曼滤波拍数,Φk,k-1为状态转移矩阵,T为滤波周期,I为单位矩阵,Ak为系统参数矩阵,Uk-1为修正量,Wk-1为系统噪声,Yk为观测量。
如上所述的一种基于状态估计和预测的低成本阻尼导航方法,其中:当卫星导航系统正常定位时,卡尔曼滤波器滤波模型为:
当卫星导航出现异常时,卡尔曼滤波器预测模型为:
Pk=Pk-1
其中,为状态一步预测值,Φk,k-1为状态转移矩阵,Kk为滤波器增益阵,L为控制量系数矩阵,Uk-1为修正量,D为系统噪声矩阵,Pk,k-1为滤波器预测误差方差阵,Pk为滤波器的协方差阵,Rk为滤波器量测噪声方差阵,H为量测参数阵,Qk为滤波器系统噪声方差阵,为状态估计量,上标T表示转置。
本发明的有益效果是:
1.本发明提供了一种简单、廉价、有效的阻尼导航方法,提高低成本惯性/卫星组合导航系统精度,特别是姿态精度,以适应低成本高精度导航系统的要求。解决了低成本、低精度惯性测量单元导航误差大引起的非线性系统误差估计精度差的问题。
2.本发明通过卡尔曼滤波对各项惯性误差进行估计,以速度误差作为控制量通过校正网络阻尼惯性导航的舒拉回路,实时修正惯性导航的速度误差和失调角,将其控制在较小的范围内。实现低成本惯性/卫星组合导航系统的高精度,提高动态性能和长时间工作稳定性。
具体实施方式
下面结合实施例对本发明提供的一种基于状态估计和预测的低成本阻尼导航方法进行介绍:
一种基于状态估计和预测的低成本阻尼导航方法,利用卫星信息和惯导信息之差作为观测量,通过卡尔曼滤波对各项惯性误差进行估计,以速度误差作为控制量通过校正网络阻尼惯性导航的舒拉回路,将惯性导航的速度误差和失调角控制在较小的范围内;并根据惯性误差估计结果对惯性输出信息进行修正,以获得高精度的位置、速度和姿态信息。具体包括如下步骤:
(1)选取多项惯性误差作为系统状态变量建立卡尔曼滤波模型。
在指北导航坐标系下,选取位置误差Δλ,速度误差ΔVN、ΔVE,失调角ΦN、ΦE、ΦU,陀螺漂移εz、εy、εz,加速度计零偏作为系统状态变量,即:
其中:
为纬度误差,单位:弧度;
Δλ为经度误差,单位:弧度;
ΔVN为北向速度误差,单位:米/秒;
ΔVE为东向速度误差,单位:米/秒;
ΦN为北向失调角,单位:弧度;
ΦE为东向失调角,单位:弧度;
ΦU为天向失调角,单位:弧度;
εx为处理成一阶马尔可夫过程的载体系X向陀螺漂移误差,单位:弧度/秒;
εy为处理成一阶马尔可夫过程的载体系Y向陀螺漂移误差,单位:弧度/秒;
εz为处理成一阶马尔可夫过程的载体系Z向陀螺漂移误差,单位:弧度/秒;
为载体系X向加速度计零偏误差,单位:米/秒2;
为载体系Z向加速度计零偏误差,单位:米/秒2。
阻尼导航惯导基本误差方程:
式中:
VE为导航坐标系北向速度值,单位:米/秒;
为导航得到的当地纬度值,单位:弧度;
h为海拔高度,单位:米;
RM、RN分别为地球子午圈和卯酉圈地球半径,单位:米;
τx、τy、τz为陀螺一阶马尔可夫过程的时间常数;
ωxr、ωyr、ωzr为陀螺零均值高斯白噪声;
为加速度计零均值白噪声;
Tij(i=1~3,j=1~3)为捷联姿态矩阵相应各元素,
分别为惯性导航的北向和东向速度误差修正量;
分别为惯性导航的北向和东向失调角修正量;
为的导数。
(2)利用惯性导航的北向和东向速度误差估计修正卡尔曼滤波模型中速度误差与失调角。
式(2)中修正量计算如下:
其中,C1、C2为误差修正阻尼系数,取值范围为0~1;
分别为惯性导航的北向和东向速度误差估计结果。
(3)求解不同情况下的卡尔曼滤波模型。
取系统噪声向量:
其中,εxr,εyr,εzr分别为εx,εy,εz的噪声。
取惯导系统和卫星定位系统的位置、速度差为观测向量:
其中,λ、VE、VN表示由惯导得到的纬度、经度、东向速度、北向速度,λGPS、VE-GPS、VN-GPS表示由GPS得到的纬度、经度、东向速度、北向速度。
惯导系统误差状态方程可表示为:
式中:A(t)为12×12维系统参数矩阵,根据式(2)计算;D为12×12维系统噪声矩阵;L为12×2维控制量系数矩阵;U(t)为修正量,
量测方程为:
Y=HX(t)+η(t) (7)
式中H为量测参数阵,η(t)为量测噪声向量。
将状态方程和量测方程离散化为:
Xk=Φk,k-1Xk-1+LUk-1+DWk-1
Yk=HXk+ηk (8)
式中k表示卡尔曼滤波拍数,Φk,k-1为状态转移矩阵,T为滤波周期,I为单位矩阵,Ak为系统参数矩阵,Uk-1为修正量,Wk-1为系统噪声,Yk为观测量。
当基准信息有效时,卡尔曼滤波器滤波模型为:
Kk=Pk,k-1HT(HPk,k-1HT+Rk)-1
(9)
当基准信息无效时,卡尔曼滤波器预测模型为:
Pk=Pk-1
式(9)(10)中:为状态一步预测值,Kk为滤波器增益阵,Pk,k-1为滤波器预测误差方差阵,Pk为滤波器的协方差阵,Rk为滤波器量测噪声方差阵,Qk为滤波器系统噪声方差阵,为状态估计量,上标T表示转置。
当判断卫星导航系统正常定位时,采用惯性导航系统和卫星定位位置、速度信息,使用式(5)计算观测量,使用式(9)所述的卡尔曼滤波算法估计惯性导航系统位置、速度、失调角等误差。当卫星导航出现失锁或异常等无效时,使用式(10)卡尔曼滤波器预测模型预测惯性导航系统位置、速度、失调角等误差。根据估计或预测速度误差,通过式(3)实时反馈修正惯性导航速度和失调角误差,保证长时间工作各项误差仍保持小量,满足系统线性化条件。根据估计或预测的位置误差、速度误差和失调角误差修正惯性导航的位置、速度和姿态角,误差修正后便得到了阻尼导航系统高精度的组合位置、速度和姿态信息。
试验时,采用MEMS/GPS组合导航系统,陀螺漂移零偏重复性和稳定性均优于30°/h,选择阻尼系数C1=0.1414、C2=0.001,建立惯导误差模型,进行阻尼式导航。根据卫星状态判断卫星是否正常,当卫星导航系统正常定位时,惯性导航系统以卫星导航信息与惯导导航信息作为量测输入,使用卡尔曼滤波算法估计惯性导航系统位置、速度、水平失调角等误差;当卫星导航出现异常时,不再进行量测计算,使用卡尔曼滤波算法预测惯性导航系统位置、速度、水平失调角等误差。根据估计或预测的惯导系统速度误差信息,通过阻尼系数对惯导速度、失调角误差进行补偿。
MEMS/GPS组合导航系统在静态条件下进入阻尼式导航状态后,采用卡尔曼滤波算法估计惯导的误差信息,通过阻尼系数进行速度误差和失调角的修正,1min后阻尼式导航算法收敛,实时修正的惯性导航纬度误差为46.53m,经度误差为-75.68m,北速误差为0.101m/s,东速误差为-0.105m/s。卡尔曼滤波估计结果为:
根据估计结果,组合纬度误差为-0.73m,组合经度误差为-2.16m,组合北速误差为0.003m/s,组合东速误差为-0.004m/s,有效提高了组合导航精度。
导航至1953s卫星开始失锁,失锁97s卫星重新定位,在卫星失锁过程中,采用速度误差的预测值修正惯导系统的速度和姿态角,失锁97s内组合纬度误差为-58.77m,组合经度误差为-33.73m,组合北速误差为-1.10m/s,组合东速误差为-0.58m/s,有效提高了卫星失锁过程中的组合导航精度。
Claims (5)
1.一种基于状态估计和预测的低成本阻尼导航方法,利用卫星信息和惯导信息之差作为观测量,通过卡尔曼滤波对各项惯性误差进行估计,以速度误差作为控制量修正惯性导航的速度误差和失调角;并根据惯性误差估计结果对惯性输出信息进行修正;
在指北导航坐标系下,选取位置误差Δλ,速度误差ΔVN、ΔVE,失调角ΦN、ΦE、ΦU,陀螺漂移εx、εy、εz,加速度计零偏作为系统状态变量;
根据系统状态变量建立的阻尼导航惯导基本误差方程为:
式中:
VE为导航坐标系北向速度值,单位:米/秒;
为导航得到的当地纬度值,单位:弧度;
h为海拔高度,单位:米;
RM、RN分别为地球子午圈和卯酉圈地球半径,单位:米;
τx、τy、τz为陀螺一阶马尔可夫过程的时间常数;
ωxr、ωyr、ωzr为陀螺零均值高斯白噪声;
为加速度计零均值白噪声;
Tij(i=1~3,j=1~3)为捷联姿态矩阵相应各元素,
分别为惯性导航的北向和东向速度误差修正量;
分别为惯性导航的北向和东向失调角修正量;
为的导数。
2.根据权利要求1所述的一种基于状态估计和预测的低成本阻尼导航方法,其特征在于:当卫星导航系统正常定位时,惯性导航系统以卫星导航信息与惯导导航信息作为量测输入,使用卡尔曼滤波方法估计惯性导航系统各项惯性误差;当卫星导航出现异常时,不再进行量测计算,使用卡尔曼滤波方法预测惯性导航系统各项惯性误差。
3.根据权利要求1或2所述的一种基于状态估计和预测的低成本阻尼导航方法,其特征在于:所述修正量计算如下:
其中,C1、C2为误差修正阻尼系数,取值范围为0~1;
分别为惯性导航的北向和东向速度误差估计结果。
4.根据权利要求3所述的一种基于状态估计和预测的低成本阻尼导航方法,其特征在于:所述卡尔曼滤波过程如下:
取系统噪声向量:
其中,εxr,εyr,εzr分别为εx,εy,εz的噪声;
取惯导系统和卫星定位系统的位置、速度差为观测向量:
其中,λ、VE、VN表示由惯导得到的纬度、经度、东向速度、北向速度,λGPS、VE-GPS、VN-GPS表示由GPS得到的纬度、经度、东向速度、北向速度;
惯导系统误差状态方程表示为:
其中,A(t)为12×12维系统参数矩阵,D为12×12维系统噪声矩阵,L为12×2维控制量系数矩阵,U(t)为修正量,
量测方程为:
Y=HX(t)+η(t)
其中,H为量测参数阵,η(t)为量测噪声向量;
将状态方程和量测方程离散化为:
Xk=Φk,k-1Xk-1+LUk-1+DWk-1
Yk=HXk+ηk
其中,k表示卡尔曼滤波拍数,Φk,k-1为状态转移矩阵,T为滤波周期,I为单位矩阵,Ak为系统参数矩阵,Uk-1为修正量,Wk-1为系统噪声,Yk为观测量。
5.根据权利要求4所述的一种基于状态估计和预测的低成本阻尼导航方法,其特征在于:当卫星导航系统正常定位时,卡尔曼滤波器滤波模型为:
Kk=Pk,k-1HT(HPk,k-1HT+Rk)-1
当卫星导航出现异常时,卡尔曼滤波器预测模型为:
Pk=Pk-1
其中,为状态一步预测值,Φk,k-1为状态转移矩阵,Kk为滤波器增益阵,L为控制量系数矩阵,Uk-1为修正量,D为系统噪声矩阵,Pk,k-1为滤波器预测误差方差阵,Pk为滤波器的协方差阵,Rk为滤波器量测噪声方差阵,H为量测参数阵,Qk为滤波器系统噪声方差阵,为状态估计量,上标T表示转置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201110273080.4A CN102538788B (zh) | 2011-09-15 | 2011-09-15 | 一种基于状态估计和预测的低成本阻尼导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201110273080.4A CN102538788B (zh) | 2011-09-15 | 2011-09-15 | 一种基于状态估计和预测的低成本阻尼导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN102538788A CN102538788A (zh) | 2012-07-04 |
CN102538788B true CN102538788B (zh) | 2015-02-11 |
Family
ID=46346279
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201110273080.4A Active CN102538788B (zh) | 2011-09-15 | 2011-09-15 | 一种基于状态估计和预测的低成本阻尼导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN102538788B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102853848B (zh) * | 2012-08-03 | 2015-03-25 | 南京航空航天大学 | 基于捷联惯导系统定位精度的惯性器件误差仿真方法 |
CN103389097B (zh) * | 2013-08-02 | 2016-04-20 | 哈尔滨工程大学 | 一种基于阻尼捷联惯性导航重力异常滤波匹配的方法 |
CN103674059A (zh) * | 2013-11-11 | 2014-03-26 | 北京航天控制仪器研究所 | 一种基于外测速度信息的sins水平姿态误差修正方法 |
CN103900568B (zh) * | 2014-03-08 | 2016-06-29 | 哈尔滨工程大学 | 一种改进的捷联惯性导航系统快速阻尼方法 |
CN106989761B (zh) * | 2017-05-25 | 2019-12-03 | 北京航天自动控制研究所 | 一种基于自适应滤波的空间飞行器制导工具在轨标定方法 |
CN114353786B (zh) * | 2021-11-30 | 2023-07-07 | 安徽海博智能科技有限责任公司 | 一种基于改进卡尔曼滤波器的无人矿卡融合定位方法 |
CN117214933B (zh) * | 2023-11-07 | 2024-02-06 | 中国船舶集团有限公司第七〇七研究所 | 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101769743A (zh) * | 2010-01-04 | 2010-07-07 | 北京航空航天大学 | 一种适用于微惯性与全球定位组合导航系统的分布式滤波装置 |
CN101819041A (zh) * | 2010-04-16 | 2010-09-01 | 北京航空航天大学 | 自进化anfis与ukf结合的gps/mems-ins组合定位误差动态预测方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7702459B2 (en) * | 2006-10-17 | 2010-04-20 | Alpine Electronics, Inc. | GPS accuracy adjustment to mitigate multipath problems for MEMS based integrated INS/GPS navigation systems |
-
2011
- 2011-09-15 CN CN201110273080.4A patent/CN102538788B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101769743A (zh) * | 2010-01-04 | 2010-07-07 | 北京航空航天大学 | 一种适用于微惯性与全球定位组合导航系统的分布式滤波装置 |
CN101819041A (zh) * | 2010-04-16 | 2010-09-01 | 北京航空航天大学 | 自进化anfis与ukf结合的gps/mems-ins组合定位误差动态预测方法 |
Non-Patent Citations (3)
Title |
---|
GPS/SINS组合导航系统与实现;林雪原等;《计算机测量与控制》;20071231;第15卷(第12期);1798-1800 * |
低成本MIMU/GPS组合导航研究;许国珍等;《计算机仿真》;20081031;第25卷(第10期);69-72 * |
低成本MIMU/GPS组合导航系统设计与实验;周姜滨等;《西北工业大学学报》;20070430;第25卷(第2期);215-219 * |
Also Published As
Publication number | Publication date |
---|---|
CN102538788A (zh) | 2012-07-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102538788B (zh) | 一种基于状态估计和预测的低成本阻尼导航方法 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN101514900B (zh) | 一种单轴旋转的捷联惯导系统初始对准方法 | |
CN102519470B (zh) | 多级嵌入式组合导航系统及导航方法 | |
CN101963513B (zh) | 消除水下运载体捷联惯导系统杆臂效应误差的对准方法 | |
CN103792561B (zh) | 一种基于gnss通道差分的紧组合降维滤波方法 | |
CN103217174B (zh) | 一种基于低精度微机电系统的捷联惯导系统初始对准方法 | |
CN101571394A (zh) | 基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法 | |
CN101713666B (zh) | 一种基于单轴转停方案的系泊估漂方法 | |
CN102853837B (zh) | 一种mimu和gnss信息融合的方法 | |
CN102998685A (zh) | 一种惯性/卫星导航系统伪距/伪距率紧组合方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN104655131A (zh) | 基于istssrckf的惯性导航初始对准方法 | |
CN103389506A (zh) | 一种用于捷联惯性/北斗卫星组合导航系统的自适应滤波方法 | |
CN103076026B (zh) | 一种捷联惯导系统中确定多普勒计程仪测速误差的方法 | |
CN103344259A (zh) | 一种基于杆臂估计的ins/gps组合导航系统反馈校正方法 | |
CN102628691A (zh) | 一种完全自主的相对惯性导航方法 | |
CN104483973A (zh) | 基于滑模观测器的低轨挠性卫星姿态跟踪控制方法 | |
CN102997915A (zh) | 一种闭环正向滤波结合反向平滑的pos后处理方法 | |
CN105891863A (zh) | 一种基于高度约束的扩展卡尔曼滤波定位方法 | |
CN105091907A (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
CN103727938A (zh) | 一种管道测绘用惯导里程计组合导航方法 | |
CN103557864A (zh) | Mems捷联惯导自适应sckf滤波的初始对准方法 | |
CN102168978B (zh) | 一种船用惯性导航系统摇摆基座开环对准方法 | |
CN104374405A (zh) | 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant |