具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

步骤1：在SLAM研究算法中，对载体构建模型至关重要。选取水下潜航器的姿态、速度及位置作为系统状态，建立潜航器的运动学模型；

水下潜航器的运动规律十分复杂，需简化此运动，只考虑水平面和垂直面两个自由度的 平面运动。定义运动模型的增广状态如下：

上式，X_v为潜航器中当前姿态的状态向量矩阵，{x_i,i＝0,...,k-1}为状态向量在姿态图中 的节点，其中X_v定义为：

X_v＝[x y z y]^T

上式，x,y,z为位置坐标，y为潜航器在水平参考坐标系中的航向角。

由于相应的误差协方差矩阵得到扩充，故增广误差协方差矩阵可表示为：

上式，P_v,v为当前姿态的协方差，P_v,j和P_i,j分别表示第j个的当前状态和第i,j先前状态的协方差；

系统状态方程为：

上式，A为转移矩阵，x_v为v时刻的系统状态估计值，B为输入加权矩阵，u为控 制输入向量，w为零均值高斯分布的过程噪声向量，其协方差矩阵为Q_v。

建立当前姿态误差协方差与当前姿态状态向量的关系表达式，即

根据系统状态向量的定义，建立运动模型非线性方程：

上式，及分别为潜航器的位置横坐标、位置纵坐标、天顶处坐标及航向坐标，u为控制输入向量，w为零均值高斯分布的过程噪声向量；

步骤2：水下偏振模式E-矢量角中的水中模式对扰动(包括表面波)不太敏感，因此可以依赖太阳的位置，并以此确定太阳的航向角和俯仰角，从而准确估计观测者的位置。所以本步骤通过偏振传感器的定向算法，采用偏振态的最小二乘法计算太阳矢量；

利用偏振光的Mueller-Stokes形式，一束偏振光由波矢量(代表偏振光的波数 和传播方向)和Stokes矢量(代表偏振光的强度和偏振状态)表示，阳光(k_i,x_i,S_i) 在水中的折射、空气-水界面由地表法线n以及空气和水的实际折射率η_i,η_t定义，假设太 阳光是非偏振的，S_i＝(1,0,0,0)^T，因此根据斯奈尔折射定律，透射光(k_t,x_t,S_t)表示如下：

x_t＝x_i＝n×k_i/||n×k_i||

S_t＝M_RS_i

上式，S_i和S_t分别为入射角和出射角的Stokes矢量，其中，S_i＝(1 0 0 0)^T。利用上式，通过坐标变换矩阵M_R→S得到S_t，水下透射阳光模型中，散射矩阵表示为M_S，散射光的 波矢量k_s与k_t大小相同，指向探测器，具体表示如下：

S_s＝M_SM_R→SS_t

那么Stokes矢量为：

S_d＝M_S→DM_SM_R→SM_RS_i

利用Stokes矢量提取偏振角，计算太阳在天空角中位置的初始估计值为

上式的L₁范数作用是消除由测量噪声引起的异常值，在水下散射模型中， 表示潜航器上探测器方向的航向角，则：

表示元素角度差，τ表示偏振角度：

为消除模型误差的最小化，利用最小二乘法估计太阳位置：

由此根据估计值最终确定太阳位置：

上式，t为测量时间，arcdist(·)为弧长；

步骤3：通过散射模型的偏振模式确定出太阳高度角和方位角后，本步骤通过拓扑节点为载体，自主导航提供信息，进行定位解算，并且利用偏振信息识别场景和检测目 标，分析视觉场景环境特征地图构建问题；

拓扑节点特征识别示意图，如附图3所示，是目标样本训练前后对比图，在构建水下环境的导航拓扑图时，拓扑节点的识别是关键，在复杂的水下环境，有些相邻节点的 环境特征可能表现出高度的相似性，因此，若当前节点中的帧与前一个节点中的帧有太 多的相似性时，我们合并两个相邻的节点，这样可以节省map的内存；

具体做法是：根据识别图像的位置信息，确定目标样本(各节点中心对应区域的图像样本)，并标注标号，在导航拓扑图中，有n个节点共包含m个图像样本，各个节点的 对应图像目标记为：

上式，I_i为第i个图像样本的特征矢量，y_i∈[1,2,…,n]为节点的目标标号。

然后根据机器学习中大边界近邻算法，对相邻拓扑节点中目标样本进行训练，得到 新度量矩阵M，使相同标号的目标样本更加紧凑、不同标号的目标样本更加松散。定义任意两目标样本间的距离度量为：

d_M(I_i,I_i+j)＝(I_i-I_i+j)^TM(I_i-I_i+j)

上式，d_M为在重构后的拓扑节点空间中两节点的欧式距离，d_M(I_i,I_i+j)与度量矩阵呈线性关系，I_i和I_i+j为目标近邻样本；

最后利用度量矩阵M求解如下目标函数的最优解：

令y_il∈{0,1}表示样本的特征向量是否属于相同节点，若属于即为1，否则为0；λ表示权重参数，ε_ijl为非目标样本总数。因此可获得最优解，得到拓扑图对运动环境的表达 和度量；

步骤4：建立速度惯性导航量测和非线性状态偏振光量测的组合模型；

组合导航中，取惯性导航系统中的姿态角误差、速度误差、位置误差以及陀螺随机漂移作为系统的状态变量，即：

上式，为惯性导航姿态角误差，δv_E,δv_N,δv_u为速度误差，δL,δλ,δh为位置误 差，ε_bx,ε_by,ε_bz为陀螺随机漂移的常值误差，ε_rx,ε_ry,ε_n为陀螺随机漂移的一阶马尔可夫过程， 为加速度计随机误差；

建立惯性导航和偏振光各子系统状态方程，分别为：

上式，F_IMU(t)、F_polar(t)表示状态系数矩阵，G_IMU(t)、G_polar(t)表示系统噪声驱动矩阵， W_IMU(t)、W_polar(t)表示系统噪声矩阵；

综上，组合导航系统的状态模型为：

建立系统的量测模型如下：

Y_IMU(t)＝H_IMU(t)+U_IMU(t)

Y_polar(t)＝H_polar(t)+U_polar(t)

上式，H_IMU(t)、H_polar(t)表示量测方程的非线性函数，U_IMU(t)、U_polar(t)分别表示惯性 传感器和偏振光传感器的量测噪声，其中：

综上，组合导航系统的量测方程为：

步骤5：将以上步骤整合，通过数据融合建立组合导航系统模型，构建环境地图。

本发明提出AUKF算法进行SLAM研究(AUKF-SLAM)，即整合以上步骤，通过 偏振光导航提供的航向信息、惯性导航提供的速度信息以及视觉导航提供的位置信息进 行水下自主潜航器的导航数据融合，该算法解决了传统UKF在未知且时变的情况下，精 度下降、出现发散的问题，并且引入自适应衰落因子修正预测误差协方差，调整滤波器 增益矩阵K，可以抑制滤波器发散，提高快速跟踪能力。AUKF-SLAM算法基本原理如 附图4所示，具体步骤如下：

在估计时变噪声统计时，应逐渐忘记旧的数据，但是UKF算法不具备噪声统计量的变化，所以改进该算法可以在未知和时变噪声统计参数的情况下，使用衰减记忆加权指 数法设计时变噪声统计估计器。选取加权系数：

β_i＝β_i-1b,0<b<1,

可以写成：

β_i＝d_kb^i-1,i＝1,…,k，

因此，衰减记忆时变噪声统计估计器的递推公式表述为：

上式，为系统噪声协方差，为观测噪声协方差，K为卡尔曼增益，W_i为均值和 协方差的权重，满足UKF序列；

AUKF算法通过引入衰减因子公式计算自适应因子系数λ_i，然后利用该系数校正可预测项的协方差P_k|k-1，从而抑制滤波器的发散，确保滤波器的收敛性。其收敛条件为：

对于衰减因子公式计算出自适应因子系数λ_i：

上式，λ₀为初始值，tr(·)表示矩阵跟踪，S为可调系数，为残差序列；

该算法中因子λ_i值越大，产生的信息比例就越小，导致残差向量效应v_k越显著。因此该 算法不仅抑制了滤波器发散、提高了滤波器的降噪能力，对状态突变情况也有很强的跟踪能 力；

降噪之后，采用最近邻数据关联算法，对获取的姿态、位置、速度及航向等进行数据关 联；若导航拓扑图中的节点(目标样本)相邻，则进行合并，即关联成功，此时潜航器将进 行一次时间更新和量测更新；否则，将新的度量作为新特征添加到当前状态向量，即进行状 态增广，然后继续执行相邻目标样本的合并(即数据关联)。操作如下：

上式，为新特征向量。

经过以上的改进，对滤波器进行时间更新和量测更新：

时间更新：

量测更新：

上式，及分别为系统先前的状态向量、协方差向量及k-1时刻状态的测量向量，x_i,k∣k-1、z_i,k∣k-1为Sigma点采样得到的值，W_i ^(m)和W_i ^(c)分别为系统均值和 协方差的权值，Q_k-1为系统噪声的协方差，和均为系统量测更新中获取的观测协 方差，R为观测噪声的协方差，K_k为滤波器增益矩阵，和分别为系统当前的状态向 量和协方差向量。

综上，AUKF-SLAM算法为在未知环境下进行水下同时定位和建图提供了一种可行且新颖的方法。