[发明专利]基于水下下行辐射光强和偏振光场的三维航姿确定方法

权利要求书

1.基于水下下行辐射光强和偏振光场的三维航姿确定方法，其特征在于，包括如下步骤：

（1）水下下行辐射光场包括光强场及偏振光场，通过图像式水下偏振传感器获取的不同检偏通道的偏振光强场图像为I_α，计算得水下下行辐射光场的光强场图像I_p及偏振光场图像P，其中α为检偏方向；

（2）根据水下下行辐射光强场中的斯涅尔窗口效应，提取步骤（1）计算的光强场图像I_p中的斯涅尔窗口中心像素(i_z,j_z)，得到载体坐标系下的天顶矢量测量值，与惯导输出的姿态转换矩阵结合，并选择组合导航状态变量组成状态空间X，建立基于天顶矢量的量测方程，其中，所述b为载体坐标系，所述n’为惯导计算坐标系，具体包括：

利用步骤（1）计算得到的光强场图像I_p，从中提取斯涅尔窗口中心，从而根据相机模型解算出载体坐标系下的天顶矢量测量值；其中，，z^b为载体坐标系下天顶矢量实际值，δz为天顶矢量测量噪声，其中，所述载体坐标系为b系；

惯导的计算坐标系为n’系，在该坐标系下的天顶矢量为z^n'，则存在如下关系：

其中，表示b系与n’系之间的转换的姿态转换矩阵；导航坐标系为n系，在该坐标系下的天顶矢量为zⁿ=[0 0 1]^T，则存在如下关系：

其中，ϕ×表示平台失准角ϕ的反对称矩阵，由此可得；状态空间为，其中，为惯导的东、北、天向平台失准角，为陀螺东、北、天向常值漂移，为加计东、北、天向常值漂移；则建立天顶矢量量测方程如下：

Z_z=H_zX+V_z

其中，，,；zⁿ×表示zⁿ=[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，即：

其中, 表示偏振角图像P的区域D内每个像素的偏振方位角；

（4）根据步骤（3）中获取的天顶偏振方位角p_z，辅以光强场图像I_p分区对比消除歧义性得偏振航向角计算值，并与惯导航向角建立基于偏振航向角的量测方程，具体包括：

定义水平坐标系为h系，其z轴为天顶矢量方向，其x轴与n系x轴夹角为载体实际航向角；由太阳子午线与天顶偏振矢量垂直，则h系下具有歧义性的太阳方位角φ_h^'为：

φ_h^'=p_z±90°

在光强场图像I_p中，以斯涅尔窗口中心像素(i_z,j_z)为圆心，分别在p_z+90°和p_z−90°两个方向上，以α为圆心角，以r为半径确定扇形区域S₋和S₊，则有：

其中，(i,j)为光强场图像I_p的像素坐标；

求两个区域的像素光强均值：

其中，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系；偏振航向角计算值存在误差角；n'系下由惯导得到的惯导俯仰角及惯导航向角分别为和，则有：

令，,，则建立基于偏振航向角的量测方程：

Z_p=H_pX+V_p

由此系统量测方程为：

Z=HX+V

其中，

；

（5）根据捷联惯导系统误差建立状态方程，并结合步骤（2）、步骤（4）得到两组量测方程，采用卡尔曼滤波器估计平台失准角ϕ，并对惯导进行反馈校正，确定三维航向与姿态信息。

2.根据权利要求1所述的基于水下下行辐射光强和偏振光场的三维航姿确定方法，其特征在于：所述步骤（5）的具体步骤如下：

系统误差状态方程为：

其中，F为标准的惯导误差转移矩阵，W为系统噪声矢量；将系统状态方程与量测方程离散化处理得：

X_k=Φ_k,k-1X_k-1+W_k-1

Z_k=H_kX_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时刻量测噪声；

卡尔曼滤波时间更新为：

其中，和P_k/k-1表示状态一步预测和一步预测均方误差，P_k-1为k-1时刻估计均方误差，为k-1时刻状态噪声方差矩阵；

卡尔曼滤波量测更新为：

其中，和P_k表示第k时刻状态估计及估计均方误差；K_k为第k时刻卡尔曼滤波增益；表示第k时刻量测噪声方差矩阵。