CN110779514B - 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置 - Google Patents
面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置 Download PDFInfo
- Publication number
- CN110779514B CN110779514B CN201911030649.7A CN201911030649A CN110779514B CN 110779514 B CN110779514 B CN 110779514B CN 201911030649 A CN201911030649 A CN 201911030649A CN 110779514 B CN110779514 B CN 110779514B
- Authority
- CN
- China
- Prior art keywords
- measurement
- subsystem
- hierarchical
- coordinate system
- navigation
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/02—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means
- G01C21/025—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means with the use of startrackers
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
本发明公开了一种面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法及装置,其中,方法包括:初始化数据,建立坐标系;建立以四元数微分方程为模型的系统状态模型;以IMU中加速计的测量数据、重力矢量作为输入建立分级扩展卡尔曼融合系统的一级子系统测量模型;以仿生偏振导航传感器的输出、导航坐标系下的太阳矢量为输入建立分级扩展卡尔曼融合系统的二级子系统测量模型;将一级子系统测量模型和二级子系统测量模型进行组合,得到完整的面向仿生偏振导航传感器辅助定姿的分级扩展卡尔曼融合系统;解算出三轴的姿态角度,得到三轴姿态测量结果。该方法计算量低,易于工程实现,强鲁棒性、抗扰能力强,简单易实现。
Description
技术领域
本发明涉及航姿测量技术领域,特别涉及一种面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法及装置。
背景技术
目前,最常见的航姿测量组合方案有IMU(Inertial measurement unit,惯性测量单元)/地磁组合,地磁/GPS(Global Positioning System,全球定位系统)/SINS(Strapdown inertial navigation system,捷联惯导系统)组合等。现有的多数组合导航系统,都用到了卫星导航系统或者地磁导航,如一种双模式仿生偏振/地磁辅助组合导航系统、仿生偏振光/GPS/地磁组合导航方法设计及实现、偏振光/地磁/GPS/SINS组合导航算法研究、利用偏振光、地磁、GPS进行多信息源融合导航方法的研究与实现、基于多方向偏振光导航传感器的定位系统及其定位方法等。近年来、很多学者将仿生偏振传感器引入了导航领域,但目前应用于移动载体的仿生导航传感器还仅仅限于点源式的仿生导航传感器。
然而,地磁场相对较弱且容易受到铁磁性物质的干扰,导致航姿测量过程容易受到干扰,这使得这种组合方式在使用过程中不稳定、鲁棒性降低。卫星导航易受自然环境及人为干扰,在拒止、干扰、对抗等环境下无法提供准确可靠的导航信息。且现有技术中,比如,基于多方向偏振光导航传感器的定位系统及其定位方法,在该技术中没用到地磁传感器,但只能输出经纬度信息,无法输出三轴旋转角度信息。而且点源式仿生导航传感器分辨率比较低,在天顶点有遮挡等情况下,精度会有所下降。
综上,目前使用的仿生偏振传感器和IMU的组合仅仅是通过仿生导航传感器输出的偏航角进行了简单的组合,并没有考虑到深度的信息融合,这种组合的方式虽然简单且略有效果,但是可靠性有待提高,偏振信息和IMU数据的深度信息融合问题,亟待解决。
发明内容
本申请是基于发明人对以下问题的认识和发现做出的:
导航是引导载体从某一出发地沿设定的路径有效到达目的地的一种技术手段,而导航系统中最重要的一个环节就是航姿测量,航姿测量广泛应用于无人机、智能机器人、舰船、无人驾驶等重要的领域。而航姿测量精度的下降,会进一步导致导航系统的不稳定。因此迫切需要找到一种更稳定且鲁棒性强的组合方式进行航姿测量。
太阳光经过瑞丽散射的作用,会形成稳定的天空偏振分布模式,通过仿生偏振导航传感器采集大气的偏振模式可以得到绝对的航向信息,由于大气分布模式稳定且覆盖面广,不容易受到影响,具有抗电磁干扰和使用范围广、无累积误差的特性。仿生偏振导航自主性强及不受电磁干扰影响,能够为载体提供可靠的姿态信息,可用于惯性导航系统进行优势互补,为提高组合导航系统的自主性、可靠性提供了新的解决途径。现有的基于仿生偏振导航传感器的组合导航系统,大多基于点源式仿生偏振导航传感器,这种偏振导航传感器分辨率较低,在天顶点受到遮挡的情况下精度容易受到影响
因此,引入一种更稳定、精度更高的仿生偏振导航传感器,找到一个容易实现、且鲁棒性强精度高的紧组合的测姿方法且显得十分得迫切和重要。
本发明旨在至少在一定程度上解决相关技术中的技术问题之一。
为此,本发明的一个目的在于提出一种面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法,该方法在强磁场的干扰环境中,在天顶点处于遮挡状态下,导航性能不会受到影响,保持高精度;且计算量低,易于工程实现,强鲁棒性、抗扰能力强,简单易实现。
本发明的另一个目的在于提出一种面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置。
为达到上述目的,本发明一方面实施例提出了一种面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法,包括以下步骤:将IMU、成像式仿生偏振光传感器固定安装在载体上,建立遵循右手定则的载体坐标系;获取IMU数据、方位角信息和所述载体坐标系下的太阳矢量信息;计算以当地时间为标准的导航坐标系下的太阳矢量;以IMU中陀螺仪的测量数据、四元数为系统状态量,并基于四元数微分方程建立系统的状态模型;以所述IMU中加速计的测量数据、重力矢量作为输入建立分级扩展卡尔曼融合系统的一级子系统测量模型;以所述仿生偏振导航传感器的输出、所述导航坐标系下的太阳矢量为输入建立分级扩展卡尔曼融合系统的二级子系统测量模型;将一级子系统测量模型和二级子系统测量模型进行组合,得到完整的基于仿生偏振导航传感器辅助定姿的分级扩展卡尔曼融合系统;解算出三轴的姿态角度,得到三轴姿态测量结果。
本发明实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法,通过引入一种更稳定、精度更高的成像式仿生偏振导航传感器,实现强鲁棒性、高精度的测姿,从而在强磁场的干扰环境中,不会受到影响保持高精度,在天顶点处于遮挡状态下,导航性能不会受到影响,保持高精度;且计算量低,易于工程实现,强鲁棒性、抗扰能力强,简单易实现。
另外,根据本发明上述实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法还可以具有以下附加的技术特征:
进一步地,在本发明的一个实施例中,所述导航坐标系下的太阳矢量的表示公式为:
其中,sx=cos(hs)sin(as),sy=cos(hs)cos(as),sz=sin(hs),导航坐标系中的X轴指向西、Y轴指向南、Z轴指向天顶,其中,
其中,δ表示太阳赤纬,β表示观测点的纬度,t表示太阳时角。
进一步地,在本发明的一个实施例中,所述系统状态模型的建立公式为:
其中,Xk为分级扩展卡尔曼融合系统的待估计的参数,A为系统的状态转移矩阵,Wk-1是系统噪声。
进一步地,在本发明的一个实施例中,所述一级子系统测量模型和所述二级子系统测量模型为:
其中,V1k是量测噪声,噪声为高斯白噪声,V1k的协方差矩阵为R1k,V2k是二级子系统量测噪声,噪声为高斯白噪声,V2k的协方差阵为R2k。
进一步地,在本发明的一个实施例中,所述三轴的姿态角度的计算公式为:
γ=-arcsin(2(q1q3-q0q2)),
其中,横滚角为γ、俯仰角为θ、偏航角为Ψ,q0、q1、q2和q3为四元数。
为达到上述目的,本发明另一方面实施例提出了一种面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置,包括:固定模块,用于将IMU、成像式仿生偏振光传感器固定安装在载体上,建立遵循右手定则的载体坐标系;获取模块,用于获取IMU数据、方位角信息和所述载体坐标系下的太阳矢量信息;计算模块,用于计算以当地时间为标准的导航坐标系下的太阳矢量;第一建立模块,用于根据以IMU中陀螺仪的测量数据、四元数为系统状态量,以四元数的微分方程为基础,建立系统状态模型;第二建立模块,用于以所述IMU中加速计的测量数据、重力矢量作为输入建立分级扩展卡尔曼融合系统的一级子系统测量模型;第三建立模块,用于以所述仿生偏振导航传感器的输出、所述导航坐标系下的太阳矢量为输入建立分级扩展卡尔曼融合系统的二级子系统测量模型;组合模块,用于将一级子系统测量模型和二级子系统测量模型进行组合,得到完整的基于仿生偏振导航传感器辅助定姿的分级扩展卡尔曼融合系统;解算模块,用于解算出三轴的姿态角度,得到三轴姿态测量结果。
本发明实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置,通过引入一种更稳定、精度更高的成像式仿生偏振导航传感器,实现强鲁棒性、高精度的测姿,从而在强磁场的干扰环境中,不会受到影响保持高精度,在天顶点处于遮挡状态下,导航性能不会受到影响,保持高精度;且计算量低,易于工程实现,强鲁棒性、抗扰能力强,简单易实现。
另外,根据本发明上述实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置还可以具有以下附加的技术特征:
进一步地,在本发明的一个实施例中,所述导航坐标系下的太阳矢量的表示公式为:
其中,sx=cos(hs)sin(as),sy=cos(hs)cos(as),sz=sin(hs),导航坐标系中的X轴指向西、Y轴指向南、Z轴指向天顶,其中,
其中,δ表示太阳赤纬,β表示观测点的纬度,t表示太阳时角。
进一步地,在本发明的一个实施例中,所述系统状态模型的建立公式为:
其中,Xk为分级扩展卡尔曼融合系统的待估计的参数,A为系统的状态转移矩阵,Wk-1是系统噪声。
进一步地,在本发明的一个实施例中,所述一级子系统测量模型和所述二级子系统测量模型为:
其中,V1k是量测噪声,噪声为高斯白噪声,V1k的协方差矩阵为R1k,V2k是二级子系统量测噪声,噪声为高斯白噪声,V2k的协方差阵为R2k。
进一步地,在本发明的一个实施例中,所述三轴的姿态角度的计算公式为:
γ=-arcsin(2(q1q3-q0q2)),
其中,横滚角为γ、俯仰角为θ、偏航角为Ψ,q0、q1、q2和q3为四元数。
本发明附加的方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本发明的实践了解到。
附图说明
本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解,其中:
图1为根据本发明实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法的流程图;
图2为根据本发明一个实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法的流程图;
图3为根据本发明实施例的一个实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法的算法框架示意图;
图4为根据本发明实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置的结构示意图。
具体实施方式
下面详细描述本发明的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,旨在用于解释本发明,而不能理解为对本发明的限制。
下面参照附图描述根据本发明实施例提出的仿生偏振导航传感器辅助定姿分级卡尔曼融合方法及装置,首先将参照附图描述根据本发明实施例提出的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法。
图1是本发明一个实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法的流程图。
如图1所示,该面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法包括以下步骤:
在步骤S101中,将IMU、成像式仿生偏振光传感器固定安装在载体上,建立遵循右手定则的载体坐标系。
可以理解的是,如图2所示,本发明实施例首先初始化数据,建立坐标系,具体地:将IMU、成像式仿生偏振光传感器固定安装在载体上,建立遵循右手定则的载体坐标系,定义为b系。
其中,IMU是惯性测量组合单元(Inertial Measuring Unit),包括了一个三轴的陀螺仪和一个三轴的加速度计,如图3所示,其中b系为载体坐标系,遵循右手定则,Y轴指向载体前方,X轴指向载体右方,Z轴指向载体上方。
在步骤S102中,获取IMU数据、方位角信息和载体坐标系下的太阳矢量信息。
可以理解的是,本发明实施例获取IMU数据;获取成像式偏振导航传感器输出的方位角信息;载体坐标系下的太阳矢量。
其中,IMU的数据包括在载体坐标系下加速度计采集的信息、陀螺仪采集的信息;成像式偏振导航传感器输出的方位角信息为太阳子午线与载体体轴(载体的Y轴)的夹角信息。
在步骤S103中,计算以当地时间为标准的导航坐标系下的太阳矢量。
可以理解的是,本发明实施例可以根据天文年历,得到当地时间为标准的导航坐标系下的太阳矢量,导航坐标系定义为n系。
其中,导航坐标系中的X轴指向西、Y轴指向南、Z轴指向天顶,导航坐标系下的太阳矢量是根据天文年历、当地时间以及经纬度信息获得,本发明实施例用 表示导航坐标系下的太阳矢量,其中sx=cos(hs)sin(as),sy=cos(hs)cos(as),sz=sin(hs)。
在步骤S104中,以IMU中陀螺仪的测量数据、四元数为系统状态量,以四元数的微分方程为基础建立系统状态模型。
可以理解的是,如图2所示,本发明实施例基于IMU中陀螺仪三轴角速度输入、以及相四元数的微分方程为系统状态量建立系统状态模型。
具体而言,基于IMU中陀螺仪三轴角速度输入ω=[ωxωyωz]T、IMU的数据在载体坐标系中测量得来,其中ωx为X轴的角速率,ωy为Y轴的角速率,ωz为Z轴的角速率,再根据四元数的微分方程:为系统状态量建立系统状态模型,其中Q表示刚体旋转的规范化四元数,具体实现步骤如下:
X=[q0 q1 q2 q3]T,
其中,X为四元数,建立分级扩展卡尔曼融合系统的状态方程为:
其中,Xk为分级扩展卡尔曼融合系统的待估计的参数,A为系统的状态转移矩阵:
Wk-1是系统噪声,为高斯白噪声,Wk-1的协方差矩阵为Qk-1。
在步骤S105中,以IMU中加速计的测量数据、重力矢量作为输入建立分级扩展卡尔曼融合系统的一级子系统测量模型。
可以理解的是,如图2所示,本发明实施例在步骤S104的基础上以IMU中加速计的测量数据,重力矢量作为输入建立分级扩展卡尔曼融合系统的一级子系统测量模型。
其中,以加速度计的三轴测量值为输出建立分级扩展卡尔曼融合系统的一级量测模型为:
其中,V1k是量测噪声,噪声为高斯白噪声,V1k的协方差矩阵为R1k。
在步骤S106中,以仿生偏振导航传感器的输出、导航坐标系下的太阳矢量为输入建立分级扩展卡尔曼融合系统的二级子系统测量模型。
可以理解的是,如图2所示,本发明实施例在步骤S104和S105的基础上以仿生偏振导航传感器的输出,导航坐标系下的太阳矢量为输入建立分级扩展卡尔曼融合系统的二级子系统测量模型。
其中,以仿生偏振光导航传感器计算出的方位角为输出,建立分级扩展卡尔曼融合系统的二级量测模型为:
其中,V2k是二级子系统量测噪声,噪声为高斯白噪声,V2k的协方差阵为R2k。
在步骤S107中,将一级子系统测量模型和二级子系统测量模型进行组合,得到完整的基于仿生偏振导航传感器辅助定姿的分级扩展卡尔曼融合系统。
可以理解的是,如图2所示,本发明实施例根据步骤S101至步骤S106将两级系统的测量模型进行组合,设计一个完整的面向仿生偏振导航传感器辅助定姿的分级扩展卡尔曼融合系统。其中,分级扩展卡尔曼融合系统也可以称为分级扩展卡尔曼滤波器。
具体而言,设计的基于仿生偏振导航传感器辅助定姿的分级扩展卡尔曼滤波器实现如下:
(1)首先进行时间的更新;
(4)计算一级子系统预测协方差矩阵P1k|k-1,其中,
P1k|k-1=AP1k-1|k-1+Qk-1,
其中,P1k-1|k-1是第k-1次的估计状态的协方差矩阵;
(5)接着进行量测状态更新部分,计算一级系统量测预测的误差M1k|k-1,M1k|k-1=Z1k-h1k|k-1,其中Z1k是第k次的测量值,h1k|k-1为系统量测在第k次的预测值;具体地:Z1k取IMU中加速度计在机体坐标系下的测量值Z1k=[ax ay az]T,其中ax是载体X轴方向加速度,ay是载体Y轴方向加速度,az是载体Z轴方向加速度,h1k|k-1为导航坐标系中地球重力矢量G=[gx gy gz]T,经过坐标转换得来,其中为坐标转换矩阵,表示从导航坐标系转换到载体坐标系,gx是载体X轴方向重力的分量,gy是载体Y轴方向重力的分量,gz是载体Z轴方向重力的分量;
(7)更新一级子系统中的卡尔曼增益:
K1k=P1k|k-1H1k T(H1kPk|k-1H1k T+V1kR1kV1k T)-1;
(9)更新分级扩展卡尔曼融合系统的一级子系统协方差矩阵:
P1k=(I-KkH1k)P1k|k-1,
其中,I为单位矩阵;
(10)根据此时刻更新的X=[q0 q1 q2 q3]T,解算出k时刻的载体坐标系相对于导航坐标系的横滚角γ和俯仰角θ:
γ=-arcsin(2(q1q3-q0q2)),
(11)计算二级子系统预测协方差矩阵P2k|k-1,
P2k|k-1=AP2k-1|k-1AT+Qk-1,
其中,P2k-1|k-1是第k-1次的估计状态的协方差矩阵;
(12)接着进行量测状态更新部分,计算二级系统量测预测的误差M2k|k-1,
M2k|k-1=Z2k-h2k|k-1,
其中,Z2k是第k次的测量值,h2k|k-1为系统量测在第k次的预测值;
具体地:Z2k是根据仿生偏振导航传感器输出的载体坐标系下的太阳方位角,重新进行构造的载体坐标系下的三轴太阳分量,Z2k=[polx poly 0],h2k|k-1是根据太阳高度角hs和方位角as以及天文年历计算出来的导航坐标系下的太阳矢量经过坐标转换得来,
hs=arcsin(sinδsinβ+cosδcosβcost),
其中:δ表示太阳赤纬,β表示观测点的纬度,t表示太阳时角;
(14)更新二级系统中的卡尔曼增益:
K2k=P2k|k-1H2k T(H2kPk|k-1H2k T+V2kR2kV2k T)-1;
(16)更新分级扩展卡尔曼融合系统的二级子系统协方差矩阵:
P2k=(I-K2kH2k)P2k|k-1,
其中,I为单位矩阵;
在步骤S108中,解算出三轴的姿态角度,得到三轴姿态测量结果。
可以理解的是,将数据输入分级扩展卡尔曼融合系统后可以解算出三轴的姿态角度,完成三轴姿态测量。
需要说明的是,如图3所示,本发明实施例首次引入了分焦平面成像的仿生偏振导航传感器,并在载体坐标系上对测量的太阳矢量进行了矢量分解,该系统一共分为两个子系统,为了算法的高精度和高鲁棒性,在整套算法里面,共启用了两套参数,第一级采用的是P10、P1、R1,第二级的系统采用的是P20、P2、R2,同时选用了两套不同的量测方程来进行最优估计。输出载体坐标系(b系)相对于导航坐标系(n系)的三轴姿态角,横滚角为γ、俯仰角为θ、偏航角为Ψ,其中:
γ=-arcsin(2(q1q3-q0q2)),
综上,本发明实施例提出的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法,通过引入一种更稳定、精度更高的成像式仿生偏振导航传感器,实现强鲁棒性、高精度的测姿,从而在强磁场的干扰环境中,不会受到影响,保持高精度,在天顶点处于遮挡状态下,导航性能不会受到影响,保持高精度;且计算量低,易于工程实现,强鲁棒性、抗扰能力强,简单易实现。
其次参照附图描述根据本发明实施例提出的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置。
图4是本发明一个实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置的结构示意图。
如图4所示,该面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置10包括:固定模块100、获取模块200、计算模块300、第一建立模块400、第二建立模块500、第三建立模块600、组合模块700和解算模块800。
其中,固定模块100用于将IMU、成像式仿生偏振光传感器固定安装在载体上,建立遵循右手定则的载体坐标系;获取模块200用于获取IMU数据、方位角信息和载体坐标系下的太阳矢量信息;计算模块,用于计算以当地时间为标准的导航坐标系下的太阳矢量;第一建立模块400用于以IMU中陀螺仪的测量数据、四元数为系统状态量,基于四元数微分方程建立系统的状态模型;第二建立模块500用于以IMU中加速计的测量数据、重力矢量作为输入建立分级扩展卡尔曼融合系统的一级子系统测量模型;第三建立模块600用于以仿生偏振导航传感器的输出、导航坐标系下的太阳矢量为输入建立分级扩展卡尔曼融合系统的二级子系统测量模型;组合模块700用于将一级子系统测量模型和二级子系统测量模型进行组合,得到完整的基于仿生偏振导航传感器辅助定姿的分级扩展卡尔曼融合系统;解算模块800用于解算出三轴的姿态角度,得到三轴姿态测量结果。本发明实施例的装置10可以在强磁场的干扰环境中,不会受到影响保持高精度,在天顶点处于遮挡状态下,导航性能不会受到影响,保持高精度;且计算量低,易于工程实现,强鲁棒性、抗扰能力强,简单易实现。
进一步地,在本发明的一个实施例中,导航坐标系下的太阳矢量的表示公式为:
其中,sx=cos(hs)sin(as),sy=cos(hs)cos(as),sz=sin(hs),导航坐标系中的X轴指向西、Y轴指向南、Z轴指向天顶,其中,
其中,δ表示太阳赤纬,β表示观测点的纬度,t表示太阳时角。
进一步地,在本发明的一个实施例中,系统状态模型的建立公式为:
其中,Xk为分级扩展卡尔曼融合系统的待估计的参数,A为系统的状态转移矩阵,Wk-1是系统噪声。
进一步地,在本发明的一个实施例中,一级子系统测量模型和二级子系统测量模型为:
其中,V1k是量测噪声,噪声为高斯白噪声,V1k的协方差矩阵为R1k,V2k是二级子系统量测噪声,噪声为高斯白噪声,V2k的协方差阵为R2k。
进一步地,在本发明的一个实施例中,三轴的姿态角度的计算公式为:
γ=-arcsin(2(q1q3-q0q2)),
其中,横滚角为γ、俯仰角为θ、偏航角为Ψ,q0、q1、q2和q3为四元数。
需要说明的是,前述对面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法实施例的解释说明也适用于该实施例的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置,此处不再赘述。
根据本发明实施例提出的面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置,通过引入一种更稳定、精度更高的仿生偏振导航传感器,实现强鲁棒性、高的测姿,从而在强磁场的干扰环境中,不会受到影响保持高精度,在天顶点处于遮挡状态下,导航性能不会受到影响,保持高精度;且计算量低,易于工程实现,强鲁棒性、抗扰能力强,简单易实现。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任一个或N个实施例或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中,“N个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为,表示包括一个或更N个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分,并且本发明的优选实施方式的范围包括另外的实现,其中可以不按所示出或讨论的顺序,包括根据所涉及的功能按基本同时的方式或按相反的顺序,来执行功能,这应被本发明的实施例所属技术领域的技术人员所理解。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤,例如,可以被认为是用于实现逻辑功能的可执行指令的定序列表,可以具体实现在任何计算机可读介质中,以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用,或结合这些指令执行系统、装置或设备而使用。就本说明书而言,"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下:具有一个或N个布线的电连接部(电子装置),便携式计算机盘盒(磁装置),随机存取存储器(RAM),只读存储器(ROM),可擦除可编辑只读存储器(EPROM或闪速存储器),光纤装置,以及便携式光盘只读存储器(CDROM)。另外,计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质,因为可以例如通过对纸或其他介质进行光学扫描,接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序,然后将其存储在计算机存储器中。
应当理解,本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,N个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如,如果用硬件来实现和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成,所述的程序可以存储于一种计算机可读存储介质中,该程序在执行时,包括方法实施例的步骤之一或其组合。
此外,在本发明各个实施例中的各功能单元可以集成在一个处理模块中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现,也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时,也可以存储在一个计算机可读取存储介质中。
上述提到的存储介质可以是只读存储器,磁盘或光盘等。尽管上面已经示出和描述了本发明的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本发明的限制,本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (10)
1.一种面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合方法,其特征在于,包括以下步骤:
将IMU、成像式仿生偏振光传感器固定安装在载体上,建立遵循右手定则的载体坐标系;
获取IMU数据、方位角信息和所述载体坐标系下的太阳矢量信息;
计算以当地时间为标准的导航坐标系下的太阳矢量;
以IMU中陀螺仪的测量数据、四元数为系统状态量,并基于四元数微分方程建立系统的状态模型;
以所述IMU中加速计的测量数据、重力矢量作为输入建立分级扩展卡尔曼融合系统的一级子系统测量模型;
以所述仿生偏振导航传感器的输出、所述导航坐标系下的太阳矢量为输入建立分级扩展卡尔曼融合系统的二级子系统测量模型;
将一级子系统测量模型和二级子系统测量模型进行组合,得到完整的基于仿生偏振导航传感器辅助定姿的分级扩展卡尔曼融合系统,包括:更新时间,并根据载体的初始时刻姿态确定四元数初值,设定初始化状态变量和状态变量的协方差矩阵P10;预测 是依据第k-1次测量数据预测第k次测量的状态量,为第k-1次测量估计的状态量,A为分级扩展卡尔曼融合系统的状态转移矩阵;计算一级子系统的预测协方差矩阵P1k|k-1、量测预测的误差M1k|k-1和量测转移矩阵H1k;根据所述预测协方差矩阵P1k|k-1、所述量测预测的误差M1k|k-1和所述量测转移矩阵H1k更新所述一级子系统中的卡尔曼增益,估计所述一级子系统状态值,并更新所述一级子系统协方差矩阵;根据更新的X=[q0 q1 q2 q3]T解算出k时刻的载体坐标系相对于导航坐标系的横滚角γ和俯仰角θ,q0、q1、q2和q3为四元数;计算二级子系统的预测协方差矩阵P2k|k-1、量测预测的误差M2k|k-1和量测转移矩阵H2k;根据所述预测协方差矩阵P2k|k-1、所述量测预测的误差M2k|k-1和所述量测转移矩阵H2k更新二级系统中的卡尔曼增益,估计所述二级子系统状态值更新所述二级子系统协方差矩阵;根据更新的X=[q0 q1 q2 q3]T解算出k时刻载体系相对于导航系的偏航角;
解算出三轴的姿态角度,得到三轴姿态测量结果。
6.一种面向仿生偏振导航传感器辅助定姿的分级卡尔曼融合装置,其特征在于,包括:
固定模块,用于将IMU、成像式仿生偏振光传感器固定安装在载体上,建立遵循右手定则的载体坐标系;
获取模块,用于获取IMU数据、方位角信息和所述载体坐标系下的太阳矢量信息;
计算模块,用于计算以当地时间为标准的导航坐标系下的太阳矢量;
第一建立模块,用于以所述IMU中陀螺仪的测量数据、四元数为系统状态量,基于四元数微分方程建立系统的状态模型;
第二建立模块,用于以所述IMU中加速计的测量数据、重力矢量作为输入建立分级扩展卡尔曼融合系统的一级子系统测量模型;
第三建立模块,用于以所述仿生偏振导航传感器的输出、所述导航坐标系下的太阳矢量为输入建立分级扩展卡尔曼融合系统的二级子系统测量模型;
组合模块,用于将一级子系统测量模型和二级子系统测量模型进行组合,得到完整的基于仿生偏振导航传感器辅助定姿的分级扩展卡尔曼融合系统,包括:更新时间,并根据载体的初始时刻姿态确定四元数初值,设定初始化状态变量和状态变量的协方差矩阵P10;预测 是依据第k-1次测量数据预测第k次测量的状态量,为第k-1次测量估计的状态量,A为分级扩展卡尔曼融合系统的状态转移矩阵;计算一级子系统的预测协方差矩阵P1k|k-1、量测预测的误差M1k|k-1和量测转移矩阵H1k;根据所述预测协方差矩阵P1k|k-1、所述量测预测的误差M1k|k-1和所述量测转移矩阵H1k更新所述一级子系统中的卡尔曼增益,估计所述一级子系统状态值,并更新所述一级子系统协方差矩阵;根据更新的X=[q0 q1 q2 q3]T解算出k时刻的载体坐标系相对于导航坐标系的横滚角γ和俯仰角θ,q0、q1、q2和q3为四元数;计算二级子系统的预测协方差矩阵P2k|k-1、量测预测的误差M2k|k-1和量测转移矩阵H2k;根据所述预测协方差矩阵P2k|k-1、所述量测预测的误差M2k|k-1和所述量测转移矩阵H2k更新二级系统中的卡尔曼增益,估计所述二级子系统状态值更新所述二级子系统协方差矩阵;根据更新的X=[q0 q1 q2 q3]T解算出k时刻载体系相对于导航系的偏航角;
解算模块,用于解算出三轴的姿态角度,得到三轴姿态测量结果。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911030649.7A CN110779514B (zh) | 2019-10-28 | 2019-10-28 | 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911030649.7A CN110779514B (zh) | 2019-10-28 | 2019-10-28 | 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110779514A CN110779514A (zh) | 2020-02-11 |
CN110779514B true CN110779514B (zh) | 2021-04-06 |
Family
ID=69386969
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911030649.7A Active CN110779514B (zh) | 2019-10-28 | 2019-10-28 | 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110779514B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113418674B (zh) * | 2021-06-25 | 2022-03-04 | 中国航天空气动力技术研究院 | 一种一级模型可三自由度运动的风洞轨迹捕捉试验方法 |
CN114485574B (zh) * | 2021-12-21 | 2023-03-21 | 武汉大学 | 基于卡尔曼滤波模型的三线阵影像pos辅助对地定位方法 |
CN115855064B (zh) * | 2023-02-15 | 2023-05-30 | 成都理工大学工程技术学院 | 一种基于imu多传感器融合的室内行人定位融合方法 |
Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103323005A (zh) * | 2013-03-06 | 2013-09-25 | 郭雷 | 一种sins/gps/偏振光组合导航系统多目标优化抗干扰滤波方法 |
CN103712621A (zh) * | 2013-12-23 | 2014-04-09 | 大连理工大学 | 偏振光及红外传感器辅助惯导系统定姿方法 |
CN105021188A (zh) * | 2015-06-09 | 2015-11-04 | 北京航空航天大学 | 一种双模式仿生偏振/地磁辅助组合导航系统 |
CN108303081A (zh) * | 2017-12-29 | 2018-07-20 | 郭晓宇 | 一种仿生偏振/惯性/大气数据组合导航系统 |
CN108375381A (zh) * | 2018-02-08 | 2018-08-07 | 北方工业大学 | 一种基于扩展卡尔曼滤波的仿生偏振传感器多源误差标定方法 |
CN108387236A (zh) * | 2018-02-08 | 2018-08-10 | 北方工业大学 | 一种基于扩展卡尔曼滤波的偏振光slam方法 |
CN108759819A (zh) * | 2018-06-08 | 2018-11-06 | 北京航空航天大学 | 一种基于全天域偏振度信息的偏振导航实时定位方法 |
CN108981691A (zh) * | 2018-06-08 | 2018-12-11 | 北京航空航天大学 | 一种天空偏振光组合导航在线滤波与平滑方法 |
CN109459027A (zh) * | 2018-11-09 | 2019-03-12 | 北京航空航天大学 | 一种基于偏振-地磁矢量紧组合的导航方法 |
CN109556633A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于自适应ekf的仿生偏振传感器多源误差标定方法 |
CN109556632A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 |
CN110017837A (zh) * | 2019-04-26 | 2019-07-16 | 沈阳航空航天大学 | 一种姿态抗磁干扰的组合导航方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9562764B2 (en) * | 2012-07-23 | 2017-02-07 | Trimble Inc. | Use of a sky polarization sensor for absolute orientation determination in position determining systems |
-
2019
- 2019-10-28 CN CN201911030649.7A patent/CN110779514B/zh active Active
Patent Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103323005A (zh) * | 2013-03-06 | 2013-09-25 | 郭雷 | 一种sins/gps/偏振光组合导航系统多目标优化抗干扰滤波方法 |
CN103712621A (zh) * | 2013-12-23 | 2014-04-09 | 大连理工大学 | 偏振光及红外传感器辅助惯导系统定姿方法 |
CN105021188A (zh) * | 2015-06-09 | 2015-11-04 | 北京航空航天大学 | 一种双模式仿生偏振/地磁辅助组合导航系统 |
CN108303081A (zh) * | 2017-12-29 | 2018-07-20 | 郭晓宇 | 一种仿生偏振/惯性/大气数据组合导航系统 |
CN108375381A (zh) * | 2018-02-08 | 2018-08-07 | 北方工业大学 | 一种基于扩展卡尔曼滤波的仿生偏振传感器多源误差标定方法 |
CN108387236A (zh) * | 2018-02-08 | 2018-08-10 | 北方工业大学 | 一种基于扩展卡尔曼滤波的偏振光slam方法 |
CN108759819A (zh) * | 2018-06-08 | 2018-11-06 | 北京航空航天大学 | 一种基于全天域偏振度信息的偏振导航实时定位方法 |
CN108981691A (zh) * | 2018-06-08 | 2018-12-11 | 北京航空航天大学 | 一种天空偏振光组合导航在线滤波与平滑方法 |
CN109459027A (zh) * | 2018-11-09 | 2019-03-12 | 北京航空航天大学 | 一种基于偏振-地磁矢量紧组合的导航方法 |
CN109556633A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于自适应ekf的仿生偏振传感器多源误差标定方法 |
CN109556632A (zh) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 |
CN110017837A (zh) * | 2019-04-26 | 2019-07-16 | 沈阳航空航天大学 | 一种姿态抗磁干扰的组合导航方法 |
Non-Patent Citations (2)
Title |
---|
MIMU/偏振光互补滤波组合导航算法研究;金仁成等;《燕山大学学报》;20160531(第3期);第276-282页 * |
一种用于仿生导航无人机航姿求解的混合滤波方法;金仁成等;《导航定位与授时》;20190819;第6卷(第5期);第74-81页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110779514A (zh) | 2020-02-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Hua | Attitude estimation for accelerated vehicles using GPS/INS measurements | |
CN107270893B (zh) | 面向不动产测量的杆臂、时间不同步误差估计与补偿方法 | |
CN110779514B (zh) | 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置 | |
CN103323026B (zh) | 星敏感器和有效载荷的姿态基准偏差估计与修正方法 | |
CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
CN104698485A (zh) | 基于bd、gps及mems的组合导航系统及导航方法 | |
CN110057356B (zh) | 一种隧道内车辆定位方法及装置 | |
CN112835085B (zh) | 确定车辆位置的方法和装置 | |
CN111189442B (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
CN104697520A (zh) | 一体化无陀螺捷联惯导系统与gps系统组合导航方法 | |
Chen et al. | A novel fusion methodology to bridge GPS outages for land vehicle positioning | |
CN103712598A (zh) | 一种小型无人机姿态确定系统与确定方法 | |
CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
Liu et al. | Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter | |
Gao et al. | An integrated land vehicle navigation system based on context awareness | |
CN106595669B (zh) | 一种旋转体姿态解算方法 | |
Zhao et al. | Design of an attitude and heading reference system based on distributed filtering for small UAV | |
CN104634348B (zh) | 组合导航中的姿态角计算方法 | |
CN114459474B (zh) | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 | |
Yu et al. | SINS/CNS nonlinear integrated navigation algorithm for hypersonic vehicle | |
Qian et al. | Novel integration strategy for GNSS-aided inertial integrated navigation | |
Atia et al. | A novel systems integration approach for multi-sensor integrated navigation systems | |
CN115451955A (zh) | 基于分布鲁棒滤波的ins/gps紧耦合导航方法及系统 | |
CN114897942A (zh) | 点云地图的生成方法、设备及相关存储介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |