CN114910080B - 基于水下下行辐射光强和偏振光场的三维航姿确定方法 - Google Patents

基于水下下行辐射光强和偏振光场的三维航姿确定方法 Download PDF

Info

Publication number
CN114910080B
CN114910080B CN202210828890.XA CN202210828890A CN114910080B CN 114910080 B CN114910080 B CN 114910080B CN 202210828890 A CN202210828890 A CN 202210828890A CN 114910080 B CN114910080 B CN 114910080B
Authority
CN
China
Prior art keywords
light intensity
polarization
angle
underwater
inertial 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
Application number
CN202210828890.XA
Other languages
English (en)
Other versions
CN114910080A (zh
Inventor
杨健
胡鹏伟
郭雷
赵谦
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202210828890.XA priority Critical patent/CN114910080B/zh
Publication of CN114910080A publication Critical patent/CN114910080A/zh
Application granted granted Critical
Publication of CN114910080B publication Critical patent/CN114910080B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • 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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明涉及基于水下下行辐射光强和偏振光场的三维航姿确定方法。水下下行辐射光场包括光强场及偏振光场，通过图像式水下偏振传感器获取的不同检偏通道的偏振光强场图像，计算得光强场图像及偏振光场图像；从光强场图像中提取斯涅尔窗口中心像素，建立基于天顶矢量的量测方程；从偏振光场图像中计算天顶偏振方位角，建立基于偏振航向角的量测方程；根据捷联惯导系统误差建立状态方程，并结合两组量测方程，采用卡尔曼滤波器估计平台失准角，并对惯导进行反馈校正，确定三维航向与姿态信息。本发明仅利用水下下行辐射光场辅助惯导建立组合导航模型，用以修正惯导的三轴平台失准角，提高了水下自主导航信息的完备性。

Description

基于水下下行辐射光强和偏振光场的三维航姿确定方法
技术领域
本发明属水下自主导航领域，具体涉及一种基于水下下行辐射光强和偏振光场的三维航姿确定方法。
背景技术
现有研究表明，有些昆虫可感知大气偏振光场用于自身航向角的确定，从而发展出仿生偏振光导航技术，这种导航方式具有无源自主、误差不积累等优势。对于导航信息匮乏的水下环境而言，将仿生偏振光导航应用于水下是对水下导航的有力补充。
中国发明专利CN104833352B提出了一种基于视觉/惯性组合的水下导航方法，该方法需要水下标志点信息，但在陌生海域水下环境中，已知视觉标志点匮乏，无法有效提供导航信息；中国发明专利CN110887476B提出了一种偏振、天文及惯导的组合导航方法用以确定载体的航向角与姿态，但天文导航仅能应用于夜间环境，且该方法仅适用于大气环境中，水下环境对偏振光的折射等作用也影响了导航信息的获取精度。中国发明专利CN112129285A利用磁罗经/惯性组合导航实现姿态校正，但磁罗经易受外界磁环境的影响，这也使得该方法的应用受到了限制。而且上述方法需要天文、地磁等额外的辅助导航手段，增加了整体系统的复杂程度。中国发明专利CN202011307276.6通过辨识斯涅尔窗口边缘来确定载体水平姿态，但仅获取了水平姿态信息，无法估计航向信息。
针对上述问题，本发明提出了基于水下下行辐射光强和偏振光场的三维航姿确定方法。充分融合了水下下行辐射光场中的光强场与偏振场，综合获取水下运动体的姿态与航向信息，用于修正惯导的三轴平台失准角。实现了仅利用水下自然光场的三维航姿的实时确定，减少了对额外导航传感器的依赖。
发明内容
为解决水下自主导航技术中的航向角与姿态三维空间信息解算问题，本发明提出了基于水下下行辐射光强和偏振光场的三维航姿确定方法。该方法可从水下下行辐射光场的光强场图像与偏振图像中分别提取姿态信息与航向角信息，实现了仅利用水下自然光场对三维空间信息的解算。
本发明解决上述技术问题采用的技术方案为：基于水下下行辐射光强和偏振光场的三维航姿确定方法，其实现步骤如下：
（1）水下下行辐射光场包括光强场及偏振光场，通过图像式水下偏振传感器获取的不同检偏通道的偏振光强场图像为I α ，计算得水下下行辐射光场的光强场图像I p 及偏振光场图像P，其中α为检偏方向；
（2）根据水下下行辐射光强场中的斯涅尔窗口效应，提取步骤（1）计算的光强场图像I p 中的斯涅尔窗口中心像素(i z ,j z )，得到载体坐标系下的天顶矢量测量值
Figure 131220DEST_PATH_IMAGE001
，与惯导输出的姿态转换矩阵
Figure 970869DEST_PATH_IMAGE002
结合，并选择组合导航状态变量组成状态空间X，建立基于天顶矢量的量测方程，其中，所述b为载体坐标系，所述n’为惯导计算坐标系；
（3）利用步骤（1）计算的偏振光场图像P，结合步骤（2）得到的斯涅尔窗口中心像素(i z ,j z )，计算该像素点附近小区域内的偏振方位角均值，即得天顶偏振方位角p z ；
（4）根据步骤（3）中获取的天顶偏振方位角p z ，辅以光强场图像I p 分区对比消除歧义性得偏振航向角计算值
Figure 606250DEST_PATH_IMAGE003
，并与惯导航向角
Figure 965687DEST_PATH_IMAGE004
建立基于偏振航向角的量测方程；
（5）根据捷联惯导系统误差建立状态方程，并结合步骤（2）、步骤（4）得到两组量测方程，采用卡尔曼滤波器估计平台失准角ϕ，并对惯导进行反馈校正，确定三维航向与姿态信息。
进一步地，所述步骤（2）的具体步骤如下：
利用步骤（1）计算得到的光强场图像I p ，从中提取斯涅尔窗口中心，从而根据相机模型能够解算出载体坐标系，即b系下的天顶矢量测量值
Figure 231583DEST_PATH_IMAGE005
；其中，
Figure 43681DEST_PATH_IMAGE006
，z b 为b系下天顶矢量实际值，δz为天顶矢量测量噪声；
惯导的计算坐标系为n’系，在该坐标系下的天顶矢量为z n' ，则存在如下关系：
Figure 436748DEST_PATH_IMAGE007
其中，
Figure 713008DEST_PATH_IMAGE008
表示b系与n’系之间的转换的姿态转换矩阵。导航坐标系为n系，在该坐标系下的天顶矢量为z n =[0 0 1]T，则存在如下关系：
Figure 884226DEST_PATH_IMAGE009
其中，ϕ×表示平台失准角ϕ的反对称矩阵，由此可得
Figure 449200DEST_PATH_IMAGE010
；状态空间为
Figure 426383DEST_PATH_IMAGE011
，其中，
Figure 494833DEST_PATH_IMAGE012
为惯导的东、北、天向平台失准角，
Figure 899270DEST_PATH_IMAGE013
为陀螺东、北、天向常值漂移，
Figure 935228DEST_PATH_IMAGE014
为加计东、北、天向常值漂移；则建立天顶矢量量测方程如下：
Z z =H z X+V z
其中，
Figure 450523DEST_PATH_IMAGE015
，
Figure 904638DEST_PATH_IMAGE016
,
Figure 417659DEST_PATH_IMAGE017
。z n ×表示z n =[0 0 1]T的反对称矩阵。
进一步地，所述步骤（3）的具体步骤如下：
以斯涅尔窗口中心像素(i z ,j z )为中心，求其附近小范围以一定像素半径d区域D内的偏振方位角平均值，以此作为天顶偏振方位角p z ，即：
Figure 753962DEST_PATH_IMAGE018
其中
Figure 10631DEST_PATH_IMAGE019
,
Figure 115991DEST_PATH_IMAGE020
表示偏振角图像P的区域D内每个像素的偏振方位角。
进一步地，所述步骤（4）的具体步骤如下：
定义水平坐标系为h系，其z轴为天顶矢量方向，其x轴与n系x轴夹角为载体实际航向角
Figure 547716DEST_PATH_IMAGE021
。由太阳子午线与天顶偏振矢量垂直，则h系下具有歧义性的太阳方位角φ h '为：
φ h '=p z ±90°
在光强场图像I p 中，以斯涅尔窗口中心像素(i z ,j z )为圆心，分别在p z +90°和p z −90°两个方向上，以α为圆心角，以r为半径确定扇形区域S −和S +，则有：
Figure 105736DEST_PATH_IMAGE023
其中，(i,j)为光强场图像I p 的像素坐标。求两个区域的像素光强均值：
Figure 166096DEST_PATH_IMAGE024
其中，I +与I −分别表示扇形区域S +和S −的光强均值，I (i,j)表示像素(i,j)处的光强值，N +及N −分别表示扇形区域S +和S −内的像素个数。利用平均光强消除歧义性，得h系下的太阳方位角φ h ：
φ h =p z +sign(I +−I −)×90°
sign(I +−I −)表示的(I +−I −)正负号；
进而利用太阳年历，根据时间与经纬度信息能够得到地理坐标系，即g系下的太阳方位角φ g ，进而得偏振航向角计算值为：
Figure 125961DEST_PATH_IMAGE025
偏振航向角计算值
Figure 777523DEST_PATH_IMAGE026
存在误差角
Figure 26101DEST_PATH_IMAGE027
；n'系下由惯导得到的惯导俯仰角及惯导航向角分别为
Figure 890152DEST_PATH_IMAGE028
和
Figure 157054DEST_PATH_IMAGE029
，则有：
Figure 510675DEST_PATH_IMAGE030
令
Figure 980971DEST_PATH_IMAGE031
，
Figure 445450DEST_PATH_IMAGE032
,
Figure 52012DEST_PATH_IMAGE033
，则建立基于偏振航向角的量测方程：
Z p =H p X+V p
由此系统量测方程为：
Z=HX+V
其中，
Figure 576534DEST_PATH_IMAGE034
。
进一步地，所述步骤（5）的具体步骤如下：
系统误差状态方程为：
Figure 534126DEST_PATH_IMAGE035
其中，F为标准的惯导误差转移矩阵，W为系统噪声矢量。将系统状态方程与量测方程离散化处理得：
X k =Φ k,k-1 X k-1+W k-1
Z k =H k X k +V k
其中，X k 表示第k时刻系统状态；Φ k,k-1=e F∆t 为离散化后的状态一步转移矩阵；∆t=t k −t k-1表示两个相邻时刻t k-1与t k 之间的时间间隔；W k-1表示第k-1时刻状态噪声；Z k 表示第k时刻量测量；H k 表示第k时刻观测矩阵；V k 表示第k时刻量测噪声。
卡尔曼滤波时间更新为：
Figure 536717DEST_PATH_IMAGE036
Figure 14097DEST_PATH_IMAGE037
其中，
Figure 443941DEST_PATH_IMAGE038
和P k/k-1表示状态一步预测和一步预测均方误差，P k-1为k-1时刻估计均方误差，
Figure 685567DEST_PATH_IMAGE039
为k-1时刻状态噪声方差矩阵；
卡尔曼滤波量测更新为：
Figure 695111DEST_PATH_IMAGE040
其中，
Figure 338582DEST_PATH_IMAGE041
和P k 表示第k时刻状态估计及估计均方误差；K k 为第k时刻卡尔曼滤波增益；
Figure 877011DEST_PATH_IMAGE042
表示第k时刻量测噪声方差矩阵。
与现有的技术相比，本发明具有以下的优点：
本发明充分利用水下下行辐射光场中蕴含的导航信息，分别从水下光强场图像及偏振图像中提取姿态和航向角，仅利用水下光场解算三维空间信息，用以修正惯导的三轴平台失准角，提高了水下自主导航信息的完备性。本发明提取水下下行辐射光场中的光强场与偏振场包含的空间信息，与惯导系统组合估计载体三维航姿。
附图说明
图1为本发明的基于水下下行辐射光强和偏振光场的三维航姿确定方法的流程图；
图2为本发明的由水下下行辐射光场的偏振光强到光强场图像及偏振角图像转换关系图；
图3为本发明的消除太阳方位角歧义性的区域选取示意图；
图4为本发明的基于水下下行辐射光强和偏振光场的三维航姿确定方法信息结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅为本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域的普通技术人员在不付出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。
如图1所示，本发明的基于水下下行辐射光强和偏振光场的三维航姿确定方法具体实现步骤为：
步骤1、水下下行辐射光场包括光强场及偏振光场，通过图像式水下偏振传感器获取的不同检偏通道的偏振光强场图像为I α ，计算得水下下行辐射光场的光强场图像I p 及偏振光场图像P，其中α为检偏方向。具体步骤如下：
本发明适用于安装有鱼眼镜头的图像式水下偏振传感器。通过不同角度的检偏通道获取偏振光强，进而可计算水下下行辐射光场的光强场图像。如图2所示，此实施例中以0°,45°,90°,135°四个检偏通道的图像式水下偏振传感器为例，入射光线经过四个检偏通道后的偏振光强分别为I 0,I 45,I 90,I 135，水下下行辐射光场的光强场图像I p 为：
Figure 74774DEST_PATH_IMAGE043
如图2所示，由四检偏通道的偏振光强I 0,I 45,I 90,I 135可计算偏振角图像P为：
Figure 153589DEST_PATH_IMAGE044
步骤2、根据水下下行辐射光强场中的斯涅尔窗口效应，提取步骤1计算的光强场图像I p 中的斯涅尔窗口中心像素(i z ,j z )，得到载体坐标系下的天顶矢量测量值
Figure 369675DEST_PATH_IMAGE001
，与惯导输出的姿态转换矩阵
Figure 875743DEST_PATH_IMAGE002
结合，并选择组合导航状态变量组成状态空间X，建立基于天顶矢量的量测方程，其中，所述b为载体坐标系，所述n’为惯导计算坐标系。具体步骤如下：
从I p 中提取斯涅尔窗口中心，再根据相机模型能够解算出载体坐标系，即b系下的天顶矢量的测量值
Figure 357540DEST_PATH_IMAGE001
。那么有
Figure 443307DEST_PATH_IMAGE006
，其中z b 为b系下天顶矢量实际值，δz为天顶矢量量测噪声。
惯导的计算坐标系为n’系在该坐标系下的天顶矢量为z n' ，则存在如下关系：
Figure 61371DEST_PATH_IMAGE045
其中，
Figure 207181DEST_PATH_IMAGE008
表示b系与n’系之间的转换的姿态转换矩阵。导航坐标系为n系，在该坐标系下的天顶矢量为z n =[0 0 1]T，则存在如下关系：
Figure 910695DEST_PATH_IMAGE009
其中，ϕ×表示平台失准角ϕ的反对称矩阵，由此可得
Figure 534574DEST_PATH_IMAGE010
。状态空间为
Figure 272723DEST_PATH_IMAGE011
,其中，
Figure 77518DEST_PATH_IMAGE046
为惯导的东、北、天向平台失准角，
Figure 268328DEST_PATH_IMAGE013
为陀螺东、北、天向常值漂移，
Figure 695898DEST_PATH_IMAGE014
为加计东、北、天向常值漂移。则建立天顶矢量量测方程如下：
Z z =H z X+V z
其中，
Figure 288553DEST_PATH_IMAGE047
，
Figure 776167DEST_PATH_IMAGE016
,
Figure 454273DEST_PATH_IMAGE048
。z n ×表示z n =[0 0 1]T的反对称矩阵。则可得：
Figure 419955DEST_PATH_IMAGE049
由此可知，该测量方程在形式上虽表示可对三轴平台失准角进行估计，但由于H z 的最后一行元素全部为0，则基于天顶矢量的量测方程仅可估计ϕ E 与ϕ N 。
步骤3、利用步骤1计算的偏振光场图像P，结合步骤2得到的斯涅尔窗口中心像素(i z ,j z )，计算该像素点附近小区域内的偏振方位角均值，即得天顶偏振方位角p z 。具体步骤如下：
斯涅尔窗口中心像素(i z ,j z )为天顶点在图像中成像像素，以斯涅尔窗口中心像素(i z ,j z )为中心，求其附近小范围以一定像素半径d区域D内的偏振方位角平均值，以此作为天顶偏振方位角p z ，即：
Figure 601537DEST_PATH_IMAGE050
其中
Figure 322369DEST_PATH_IMAGE051
,
Figure 674721DEST_PATH_IMAGE020
表示偏振角图像P的区域D内每个像素的偏振方位角。
由于天顶处透射入水下的光线从水面垂直入射，则不会发生折射效应，所以天顶处偏振光的传播方向及偏振矢量不会发生改变。
步骤4、根据步骤3中获取的天顶偏振方位角p z ，辅以光强场图像I p 分区对比消除歧义性得偏振航向角计算值
Figure 506411DEST_PATH_IMAGE052
，并与惯导航向角
Figure 745763DEST_PATH_IMAGE004
建立基于偏振航向角的量测方程。具体实现如下：
定义水平坐标系为h系。其z轴为天顶矢量方向，其x轴与n系x轴夹角为载体实际航向角
Figure 637495DEST_PATH_IMAGE053
。由太阳子午线与天顶偏振矢量垂直，则h系下具有歧义性的太阳方位角φ h '为：
φ h '=p z ±90°
如图3所示，在光强场图像I p 中，以斯涅尔窗口中心像素(i z ,j z )为圆心，分别在p z +90°和p z −90°两个方向上，以α为圆心角，以r为半径确定扇形区域S −和S +，则有：
Figure 227876DEST_PATH_IMAGE055
其中，(i,j)为光强场图像I p 的像素坐标。
求两个区域的像素光强均值：
Figure 597678DEST_PATH_IMAGE056
其中，I +与I −分别表示扇形区域S +和S −的光强均值，I (i,j)表示像素(i,j)处的光强值，N +及N −分别表示扇形区域S +和S −内的像素个数。利用平均光强消除歧义性，得h系下的太阳方位角φ h ：
φ h =p z +sign(I +−I −)×90°
sign(I +−I −)表示的(I +−I −)正负号。进而利用太阳年历，根据时间与经纬度信息能够得到地理坐标系（g系）下的太阳方位角φ g ，进而得偏振航向角计算值为：
Figure 957115DEST_PATH_IMAGE057
惯导俯仰角、惯导横滚角及惯导航向角的误差角分别为
Figure 19749DEST_PATH_IMAGE058
，
Figure 582580DEST_PATH_IMAGE059
和
Figure 21651DEST_PATH_IMAGE060
。n'系下由惯导得到的惯导俯仰角、惯导横滚角及惯导航向角分别为
Figure 235595DEST_PATH_IMAGE061
，
Figure 469130DEST_PATH_IMAGE062
和
Figure 768524DEST_PATH_IMAGE063
，那么三轴误差角及三轴平台失准角之间关系表示为：
Figure 480129DEST_PATH_IMAGE064
由惯导平台失准角和姿态误差角之间关系得：
Figure 814158DEST_PATH_IMAGE065
在“东-北-天 312”欧拉角定义下有：
Figure 218594DEST_PATH_IMAGE066
其中，
Figure 802023DEST_PATH_IMAGE067
表示矩阵
Figure 504268DEST_PATH_IMAGE008
的第x行第y列元素。那么惯导航向角可以表示为：
Figure 755121DEST_PATH_IMAGE068
其中ψ为载体实际航向角，偏振航向角计算值ψ p 存在误差角δψ p ，则有：
ψ p =ψ+δψ p
将两个航向角作差得：
Figure 268142DEST_PATH_IMAGE069
令
Figure 604445DEST_PATH_IMAGE031
，
Figure 861114DEST_PATH_IMAGE032
,
Figure 966474DEST_PATH_IMAGE033
，则建立基于偏振航向角的量测方程：
Z p =H p X+V p
此量测方程可用于估计ϕ U 。由此系统量测方程为：
Z=HX+V
其中，
Figure 650396DEST_PATH_IMAGE034
步骤5、整体结构框图如图4所示。根据捷联惯导系统误差建立状态方程，并结合步骤2、步骤4得到两组量测方程，采用卡尔曼滤波器估计平台失准角ϕ，并对惯导进行反馈校正，确定三维航向与姿态信息。具体实现如下：
系统误差状态方程为：
Figure 208416DEST_PATH_IMAGE035
其中，F为标准的惯导误差转移矩阵，W为系统噪声矢量。将系统状态方程与量测方程离散化处理得：
X k =Φ k,k-1 X k-1+W k-1
Z k =H k X k +V k
其中，X k 表示第k时刻系统状态；Φ k,k-1=e F∆t 为离散化后的状态一步转移矩阵；∆t=t k −t k-1表示两个相邻时刻t k-1与t k 之间的时间间隔；W k-1表示第k-1时刻状态噪声；Z k 表示第k时刻量测量；H k 表示第k时刻观测矩阵；V k 表示第k时刻量测噪声。
卡尔曼滤波时间更新为：
Figure 16579DEST_PATH_IMAGE070
Figure 976444DEST_PATH_IMAGE071
其中，
Figure 831268DEST_PATH_IMAGE038
和P k/k-1表示状态一步预测和一步预测均方误差，P k-1为k-1时刻估计均方误差，
Figure 142164DEST_PATH_IMAGE072
为k-1时刻状态噪声方差矩阵；
卡尔曼滤波量测更新为：
Figure 740635DEST_PATH_IMAGE073
其中，
Figure 555007DEST_PATH_IMAGE074
和P k 表示第k时刻状态估计及估计均方误差；K k 为第k时刻卡尔曼滤波增益；
Figure 846311DEST_PATH_IMAGE042
表示第k时刻量测噪声方差矩阵。

Claims (2)

1.基于水下下行辐射光强和偏振光场的三维航姿确定方法，其特征在于，包括如下步骤：
（1）水下下行辐射光场包括光强场及偏振光场，通过图像式水下偏振传感器获取的不同检偏通道的偏振光强场图像为I α ，计算得水下下行辐射光场的光强场图像I p 及偏振光场图像P，其中α为检偏方向；
（2）根据水下下行辐射光强场中的斯涅尔窗口效应，提取步骤（1）计算的光强场图像I p 中的斯涅尔窗口中心像素(i z ,j z )，得到载体坐标系下的天顶矢量测量值
Figure DEST_PATH_IMAGE001
，与惯导输出的姿态转换矩阵
Figure 952156DEST_PATH_IMAGE002
结合，并选择组合导航状态变量组成状态空间X，建立基于天顶矢量的量测方程，其中，所述b为载体坐标系，所述n’为惯导计算坐标系，具体包括：
利用步骤（1）计算得到的光强场图像I p ，从中提取斯涅尔窗口中心，从而根据相机模型解算出载体坐标系下的天顶矢量测量值
Figure 528631DEST_PATH_IMAGE001
；其中，
Figure DEST_PATH_IMAGE003
，z b 为载体坐标系下天顶矢量实际值，δz为天顶矢量测量噪声，其中，所述载体坐标系为b系；
惯导的计算坐标系为n’系，在该坐标系下的天顶矢量为z n' ，则存在如下关系：
Figure 454998DEST_PATH_IMAGE004
其中，
Figure DEST_PATH_IMAGE005
表示b系与n’系之间的转换的姿态转换矩阵；导航坐标系为n系，在该坐标系下的天顶矢量为z n =[0 0 1]T，则存在如下关系：
Figure 800529DEST_PATH_IMAGE006
其中，ϕ×表示平台失准角ϕ的反对称矩阵，由此可得
Figure DEST_PATH_IMAGE007
；状态空间为
Figure 655615DEST_PATH_IMAGE008
，其中，
Figure DEST_PATH_IMAGE009
为惯导的东、北、天向平台失准角，
Figure 453806DEST_PATH_IMAGE010
为陀螺东、北、天向常值漂移，
Figure DEST_PATH_IMAGE011
为加计东、北、天向常值漂移；则建立天顶矢量量测方程如下：
Z z =H z X+V z
其中，
Figure 183865DEST_PATH_IMAGE012
，
Figure DEST_PATH_IMAGE013
,
Figure 383902DEST_PATH_IMAGE014
；z n ×表示z n =[0 0 1]T的反对称矩阵；
（3）利用步骤（1）计算的偏振光场图像P，结合步骤（2）得到的斯涅尔窗口中心像素(i z ,j z )，计算斯涅尔窗口中心像素(i z ,j z )附近小区域内的偏振方位角均值，即得天顶偏振方位角p z ，具体包括：
以斯涅尔窗口中心像素(i z ,j z )为中心，求其附近小范围以一定像素半径d区域D内的偏振方位角平均值，以此作为天顶偏振方位角p z ，即：
Figure DEST_PATH_IMAGE015
其中
Figure 908424DEST_PATH_IMAGE016
,
Figure DEST_PATH_IMAGE017
表示偏振角图像P的区域D内每个像素的偏振方位角；
（4）根据步骤（3）中获取的天顶偏振方位角p z ，辅以光强场图像I p 分区对比消除歧义性得偏振航向角计算值
Figure 928333DEST_PATH_IMAGE018
，并与惯导航向角
Figure DEST_PATH_IMAGE019
建立基于偏振航向角的量测方程，具体包括：
定义水平坐标系为h系，其z轴为天顶矢量方向，其x轴与n系x轴夹角为载体实际航向角
Figure 196503DEST_PATH_IMAGE020
；由太阳子午线与天顶偏振矢量垂直，则h系下具有歧义性的太阳方位角φ h '为：
φ h '=p z ±90°
在光强场图像I p 中，以斯涅尔窗口中心像素(i z ,j z )为圆心，分别在p z +90°和p z −90°两个方向上，以α为圆心角，以r为半径确定扇形区域S −和S +，则有：
Figure 516626DEST_PATH_IMAGE022
其中，(i,j)为光强场图像I p 的像素坐标；
求两个区域的像素光强均值：
Figure DEST_PATH_IMAGE023
其中，I +与I −分别表示扇形区域S +和S −的光强均值，I (i,j)表示像素(i,j)处的光强值，N +及N −分别表示扇形区域S +和S −内的像素个数；利用平均光强消除歧义性，得h系下的太阳方位角φ h ：
φ h =p z +sign(I +−I −)×90°
sign(I + −I −)表示的(I + −I −)的正负号；
进而利用太阳年历，根据时间与经纬度信息能够得到地理坐标系下的太阳方位角φ g ，进而得偏振航向角计算值为：
Figure 979094DEST_PATH_IMAGE024
其中，所述地理坐标系表示为g系；偏振航向角计算值
Figure DEST_PATH_IMAGE025
存在误差角
Figure 751878DEST_PATH_IMAGE026
；n'系下由惯导得到的惯导俯仰角及惯导航向角分别为
Figure DEST_PATH_IMAGE027
和
Figure 558160DEST_PATH_IMAGE028
，则有：
Figure DEST_PATH_IMAGE029
令
Figure 732789DEST_PATH_IMAGE030
，
Figure DEST_PATH_IMAGE031
,
Figure 599114DEST_PATH_IMAGE032
，则建立基于偏振航向角的量测方程：
Z p =H p X+V p
由此系统量测方程为：
Z=HX+V
其中，
Figure DEST_PATH_IMAGE033
；
（5）根据捷联惯导系统误差建立状态方程，并结合步骤（2）、步骤（4）得到两组量测方程，采用卡尔曼滤波器估计平台失准角ϕ，并对惯导进行反馈校正，确定三维航向与姿态信息。
2.根据权利要求1所述的基于水下下行辐射光强和偏振光场的三维航姿确定方法，其特征在于：所述步骤（5）的具体步骤如下：
系统误差状态方程为：
Figure 859194DEST_PATH_IMAGE034
其中，F为标准的惯导误差转移矩阵，W为系统噪声矢量；将系统状态方程与量测方程离散化处理得：
X k =Φ k,k-1 X k-1+W k-1
Z k =H k X k +V k
其中，X k 表示第k时刻系统状态；Φ k,k-1=e F∆t 为离散化后的状态一步转移矩阵；∆t=t k − t k-1表示两个相邻时刻t k-1与t k 之间的时间间隔；W k-1表示第k-1时刻状态噪声；Z k 表示第k时刻量测量；H k 表示第k时刻观测矩阵；V k 表示第k时刻量测噪声；
卡尔曼滤波时间更新为：
Figure DEST_PATH_IMAGE035
Figure 469167DEST_PATH_IMAGE036
其中，
Figure DEST_PATH_IMAGE037
和P k/k-1表示状态一步预测和一步预测均方误差，P k-1为k-1时刻估计均方误差，
Figure 498303DEST_PATH_IMAGE038
为k-1时刻状态噪声方差矩阵；
卡尔曼滤波量测更新为：
Figure DEST_PATH_IMAGE039
其中，
Figure 302573DEST_PATH_IMAGE040
和P k 表示第k时刻状态估计及估计均方误差；K k 为第k时刻卡尔曼滤波增益；
Figure DEST_PATH_IMAGE041
表示第k时刻量测噪声方差矩阵。
CN202210828890.XA 2022-07-15 2022-07-15 基于水下下行辐射光强和偏振光场的三维航姿确定方法 Active CN114910080B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210828890.XA CN114910080B (zh) 2022-07-15 2022-07-15 基于水下下行辐射光强和偏振光场的三维航姿确定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210828890.XA CN114910080B (zh) 2022-07-15 2022-07-15 基于水下下行辐射光强和偏振光场的三维航姿确定方法

Publications (2)

Publication Number Publication Date
CN114910080A CN114910080A (zh) 2022-08-16
CN114910080B true CN114910080B (zh) 2022-10-21

Family

ID=82771795

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210828890.XA Active CN114910080B (zh) 2022-07-15 2022-07-15 基于水下下行辐射光强和偏振光场的三维航姿确定方法

Country Status (1)

Country Link
CN (1) CN114910080B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222580B (zh) * 2023-05-09 2023-07-25 北京航空航天大学 一种基于跨介质折射干扰补偿修正的水下偏振定向方法
CN116222581B (zh) * 2023-05-10 2023-07-11 北京航空航天大学 一种基于水下光强拮抗差分的偏振抗干扰解算方法
CN117109594B (zh) * 2023-10-23 2024-01-05 北京航空航天大学 一种基于水下光强梯度的自主定向方法
CN118730095A (zh) * 2024-09-03 2024-10-01 北京航空航天大学杭州创新研究院 一种针对惯性/偏振组合定姿的复合非高斯滤波方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7630077B2 (en) * 2006-10-13 2009-12-08 University Of Massachusetts System and method for imaging through an irregular water surface
CN111220150B (zh) * 2019-12-09 2021-09-14 北京航空航天大学 一种基于水下偏振分布模式的太阳矢量解算方法
CN111027231B (zh) * 2019-12-29 2023-06-06 杭州科洛码光电科技有限公司 水下阵列相机成像方法
CN113739795B (zh) * 2021-06-03 2023-10-20 东北电力大学 一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法

Also Published As

Publication number Publication date
CN114910080A (zh) 2022-08-16

Similar Documents

Publication Publication Date Title
CN114910080B (zh) 基于水下下行辐射光强和偏振光场的三维航姿确定方法
CN111504312B (zh) 一种基于视觉惯性偏振光融合的无人机位姿估计方法
CN107806874B (zh) 一种视觉辅助的捷联惯导极区初始对准方法
CN104729506A (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN111412916B (zh) 一种基于大气偏振光场的天文航海船位计算方法
US9453731B2 (en) System and method for determining orientation relative to earth
WO2024109002A1 (zh) 仿生视觉多源信息智能感知无人平台
CN111121758B (zh) 室内磁地图的快速建模与可信定位方法
Wei et al. Intelligent vehicle localization in urban environments using ekf-based visual odometry and gps fusion
CN112129288B (zh) 一种基于偏振光/地磁航向约束的位姿估计方法及系统
CN111024091A (zh) 视觉辅助微型无人机室内飞行三维姿态算法
CN114877898B (zh) 基于水下偏振姿态与折射耦合反演的太阳动态跟踪方法
CN113532392A (zh) 一种基于表层漂流浮标的海流测量方法
CN117053797A (zh) 基于多目视觉下的大气偏振导航方法
Zhao et al. Direct georeferencing of oblique and vertical imagery in different coordinate systems
CN113819904B (zh) 一种基于天顶点矢量的偏振/vio三维姿态确定方法
Mounier et al. High-precision positioning in GNSS-challenged environments: a LiDAR-based multi-sensor fusion approach with 3D digital maps registration
Hu et al. Underwater downwelling radiance fields enable three-dimensional attitude and heading determination
CN110887477B (zh) 一种基于偏振北极点及偏振太阳矢量的自主定位方法
CN114937075B (zh) 基于三维太阳子午面拟合的水下偏振光场自主定向方法
CN117091601A (zh) 一种使用太阳方位区域的大气偏振光定向方法
CN114509071B (zh) 一种风洞试验模型姿态测量方法
CN116124079A (zh) 一种利用太阳计算载体真方位角的方法和装置
CN110887475B (zh) 一种基于偏振北极点及偏振太阳矢量的静基座粗对准方法
Ragab et al. Leveraging vision-based structure-from-motion for robust integrated land vehicle positioning systems in challenging GNSS environments

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