一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113739795A(43)申请公布日2021.12.03(21)申请号202110620366.9(22)申请日2021.06.03(71)申请人东北电力大学地址132012吉林省吉林市船营区长春路169号(72)发明人夏琳琳刘瑞敏蒙跃张晶晶(74)专利代理机构吉林省中玖专利代理有限公司22219代理人李泉宏(51)Int.Cl.G01C21/16(2006.01)G01C21/20(2006.01)G01C21/32(2006.01)权利要求书4页说明书11页附图4页(54)发明名称一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法(57)摘要本发明公开了一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法，属于水下导航领域本发明方法包括：首先利用水下偏振模式依赖太阳位置信息的原理，测量不同地点、深度和时间的水下偏振角，同时，利用偏振光传感器准确测得水下潜航器的速度、姿态、位置及航向角信息；其次利用场景的偏振信息，研究拓扑节点的识别方法和特征表达方法，以便更好地进行场景识别和目标探测；最后，考虑到水下滤波器的实用性，提出自适应无迹卡尔曼滤波(AUKF)数据融合策略，以改进的最大无偏后验噪声统计估计器在线估计未知系统噪声等参数，应用于水下潜航器的同时定位和地图构建(SLAM)，即实现AUKF‑SLAM。本发明可显著提高组合导航的定位定向精度。CN113739795ACN113739795A权利要求书1/4页1.一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法，其特征在于，该方法包括以下五个步骤：步骤1：选取水下潜航器的姿态、速度及位置作为系统状态，建立潜航器的运动学模型；步骤2：通过偏振传感器的定向算法，采用偏振态的最小二乘法计算太阳矢量，并确定太阳位置；步骤3：通过拓扑节点识别算法，利用图像偏振信息，分析视觉场景特征提取和匹配问题；步骤4：根据惯性导航系统的速度输出，建立速度惯性导航量测模型和偏振光非线性量测模型；步骤5：整合上述步骤，通过数据融合建立组合导航系统模型，构建环境地图。2.一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法，其特征在于，步骤1中所建立潜航器的运动学模型的非线性方程为：上式，及分别为潜航器的位置横坐标、位置纵坐标、天顶处坐标及航向坐标，u为控制输入向量，w为零均值高斯分布的过程噪声向量。3.一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法，其特征在于，步骤2中通过偏振传感器的定向算法，采用偏振态的最小二乘法计算太阳矢量，并确定太阳位置；上式，表示潜航器上探测器方向的航向角，表示元素角度差，为太阳位置的估计值；由此根据估计值确定太阳位置：上式，为太阳位置，t为测量时间，arcdist(·)为弧长。4.一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法，其特征在于，步骤3：构建视觉场景环境特征地图的步骤具体如下：1)根据识别图像的位置信息，确定目标样本，即各节点中心对应区域的图像样本，并标注标号，在导航拓扑图中，有n个节点共包含m个图像样本，各个节点的对应图像目标记为：上式，Ii为第i个图像样本的特征矢量，yi∈[1,2,…,n]为节点的目标标号；2)根据机器学习中大边界近邻算法，对相邻拓扑节点中目标样本进行训练，得到新度量矩阵M，使相同标号的目标样本更加紧凑、不同标号的目标样本更加松散；3)定义任意两目标样本间的距离度量为：2CN113739795A权利要求书2/4页TdM(Ii,Ii+j)＝(Ii‑Ii+j)M(Ii‑Ii+j)上式，dM为在重构后的拓扑节点空间中两节点的欧式距离，dM(Ii,Ii+j)与度量矩阵呈线性关系，Ii和Ii+j为目标近邻样本；4)利用度量矩阵M求解如下目标函数的最优解：令yil∈{0,1}表示样本的特征向量是否属于相同节点，若属于即为1，否则为0；λ表示权重参数，εijl为非目标样本总数5)由上述步骤得到拓扑图对运动环境的表达和度量，完成构建视觉场景环境特征地图。5.一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法，其特征在于，步骤4：建立速度惯性导航量测和非线性状态偏振光量测的组合模型如下：组合导航系统的状态模型为：上式，FIMU(t)、Fpolar(t)表示状态系数矩阵，GIMU(t)、Gpolar(t)表示系统噪声驱动矩阵，WIMU(t)、Wpolar(t)表示系统噪声矩阵；组合导航系统的量测方程为：上式，HIMU(t)、Hpolar(t)表示量测方程的非线性函数，UIMU(t)、Upolar(t)分别表示惯性传感器和偏振光传感器的量测噪声。6.一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法，其特征在于，步骤5：通