技术领域
[0001] 本发明涉及一种基于因子图(Factor Graph，FG)的USV‑AUV编队协同探测方法，属于海洋机器人同步定位与建图(Simultaneous Localization and Mapping，SLAM)技术领域。
相关背景技术
[0002] 近年来，随着SLAM方法在单体水下机器人应用上的稳步推进，将现有的SLAM方法应用到多水下机器人系统的构思越来越受到关注。与单水下机器人系统相比，多水下机器人系统在执行效率、容错性、鲁棒性、可重构性和硬件成本等方面都更具优势。改进多水下机器人的协同SLAM方法对于提高多自主式水下机器人(Autonomous Underwater Vehicle，AUV)任务的准确性和效率至关重要，例如海洋学调查和海底地形探测等。
[0003] 多AUV协同SLAM是一个难题，包括协同导航与定位以及协同建图两个关键技术，在过去几十年里一直是大量研究的主题。一方面，可用于向AUV提供定位信息的技术包括：(1)使用自身搭载的感知传感器进行航位推算(DR)；(2)定期浮出水面进行GPS定位；(3)声学信标系统。航位推算是指利用AUV自身运动的测量来推断AUV的位置，主要分为两大类：(a)惯性导航系统(INS)与多普勒测速(DVL)声纳相结合；(b)磁罗盘/姿态航向参考系统。在不考虑传感器成本和质量的情况下，完全依赖自身搭载的感知传感器的问题是，随着AUV航行距离的增加，位置误差会无限制地增加。增长率将是洋流、AUV速度和航位推算传感器质量的函数。如果AUV可以浮出水面，则可以使用GPS进行定位，许多AUV已经拥有这种能力。然而，频繁浮出水面对于深水任务来说是不切实际、不可取的。在声学导航中，转发器充当信标来限制INS/DR误差的增长，主要采用两种类型的系统：长基线(LBL)和超短基线(USBL)，两种系统都采用外部换能器或换能器阵列作为导航辅助。
[0004] 因此，针对惯性导航系统产生的误差累积、多AUV水下通信受限等问题，引入专用无人水面船(Unmanned Surface Vessel，USV)(可访问GNSS)与AUV团队进行通信，使用从USV传输的声学范围测量值对水下AUV进行在线导航，以改善AUV团队的定位。此外，考虑到滤波算法无法利用历史时刻的多个量测信息对历史时刻的状态量进行批量估计，得到系统状态量的全局最优解，其架构兼容性差，精度较低等问题。另一方面，海底地形相似度高，在地图重叠区域较小的情况下，匹配效果较差导致不同AUV局部地图融合精度差，全局地图一致性较低的问题。
[0005] 因此，亟需提出一种基于因子图的USV‑AUV编队协同探测方法，解决上述技术问题。
具体实施方式
[0051] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0052] 在本发明的描述中，需要理解的是，术语“上”、“中”、“外”、“内”等指示方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的组件或元件必须具有特定的方位，以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0053] 下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
[0054] 如图1所示，本发明所述的一种基于因子图的USV‑AUV编队协同探测方法，包括以下步骤：
[0055] S1，通过USV‑AUV编队协同探测系统获取当前时刻的INS/GNSS组合导航系统测量值、INS/DVL组合导航系统测量值、USBL定位系统测量值及MBS探测系统测量值；
[0056] 首先，通过GNSS获得USV、AUV的初始位置，使AUV垂直下沉到指定深度，初始艏向角均为0°，然后通过USV、AUV搭载的传感器采集并处理数据信息。其中，USV的INS测量值、GNSS测量值是通过INS/GNSS组合导航系统测量得到，AUV的IMU测量值、DVL测量值是通过INS/DVL组合导航系统测量得到，USBL测量值是通过超短基线定位系统测量得到，MBS测量值是通过多波束声呐系统测量得到，其中USV搭载INS/GNSS组合导航系统和USBL基阵，AUV搭载INS/DVL组合导航系统、USBL声信标和MBS探测系统。
[0057] S2，基于INS/GNSS组合导航系统、INS/DVL组合导航系统、USBL定位系统及MBS探测系统的测量值，构建基于因子图的USV‑AUV编队协同探测概率图模型；
[0058] 其中，INS/GNSS组合导航系统具有良好的互补性，INS为USV提供预测信息，GNSS为USV提供量测更新信息，从而实现优势互补；
[0059] 同时，INS/DVL组合导航系统也具有良好的互补性，INS为AUV提供加速度和角速度信息，DVL为AUV提供速度信息，从而实现互补；
[0060] 惯性导航系统(INS)的性能在很大程度上取决于惯性测量元件(IMU)。在USV‑AUV进行编队协同探测时，惯导系统的输出频率高于系统的实际要求以及其他传感器的输出频率，对于整个编队协同探测系统，IMU测量数据有较多冗余信息，增加了计算量，不利于协同探测信息的实时输出。因此，在建立INS因子时，可以采用预积分因子的更新策略。将需要输出的导航状态作为变量因子加入，来处理相邻两个变量因子在相应时刻的INS测量值，等价于一个INS因子节点，降低了因子节点的更新频率，提高了算法的实时性。
[0061] INS因子节点的表达式为：
[0062]
[0063] 式中， 是通过INS解算获得的探测的状态变量估计值；
[0064] 对INS测量值进行预积分，得到INS预积分因子；
[0065] INS测量可以建模为平台真实运动与各种噪声和偏差源的组合。真实运动包括平台的线性加速度和角速度，而噪声和偏差可能包括测量误差、传感器漂移和其他不确定性来源。INS的公式忽略了科里奥利力和离心力，建模为：
[0066]
[0067] 其中包括加速度偏差ba、陀螺仪偏差bw和加性噪声na，原始陀螺仪测量 加速度计测量 我们假设加速度计和陀螺仪测量中的加性噪声是高斯白噪声，与加速度计和陀螺仪相关的缓慢变化的偏差被建模为随机游走。则时间间隔[tk,tk+1]处的预积分项可导出为：
[0068]
[0069] 因此，我们可以将惯性残差定义为：
[0070]
[0071] 其中， 表示预积分的INS测量项，2[·]xyz表示提取一个四元数的旋转矢量。
[0072] GNSS量测模型可以表示为：
[0073]GNSS
[0074] 式中，h 为描述GNSS量测值 与位置关系的量测函数， 为量测噪声；
[0075] 则描述GNSS的因子fGNSS表示为：
[0076]
[0077] GNSS测量结果 及其协方差 可以从GNSS接收器中获取，因此，GNSS因子的残差可表示为：
[0078]
[0079] DVL量测模型可表示为：
[0080]
[0081] 式中，hDVL为描述DVL量测值 与位置关系的量测函数， 为量测噪声；
[0082] 则描述DVL的因子fDVL表示为：
[0083]
[0084] DVL测量结果 及其协方差 可以从DVL中获取，多普勒坐标系下的速度 可以转换为世界坐标系(w系)下的速度 因此，DVL因子的残差值可表示为：
[0085]
[0086] USBL通过安装在USV上的换能器向安装在AUV上的水下应答器发射声学信号，由应答器将声学信号返回给水听器，可以得到换能器与水下应答器之间的斜距r和方位α，β；
[0087] USBL量测模型可表示为：
[0088]
[0089] 式中，hUSBL为描述USBL量测值 与位置关系的量测函数， 为量测噪声；
[0090] 则描述USBL的因子fUSBL表示为：
[0091]
[0092] 由USBL定位原理可知：
[0093]
[0094] 式中，PAUV表示AUV在USBL坐标系下的位置， 表示USV载体坐标系与USBL坐标系之间的转换矩阵， 表示导航坐标系与USV载体坐标系之间的转换矩阵，可通过惯导系统获得。 表示AUV在导航坐标系下的位置， 表示USV在导航坐标系下的位置。
[0095] USBL因子的残差值可由以下公式计算：
[0096]
[0097] 式中，Pw表示USV的绝对位置， 表示USV初始时刻的绝对位置；
[0098] MBS量测模型可表示为：
[0099]MBS
[0100] 式中，h 为描述MBS量测值 与位置关系的量测函数， 为量测噪声；
[0101] 则描述MBS的因子fMBS表示为：
[0102]
[0103] 对MBS探测系统测量得到的子地图进行匹配，根据匹配结果构建多个子地图间位姿约束的MBS因子，其测量原理为：
[0104]
[0105] 式中，PAUV1表示AUV1在AUV2坐标系下的位置， 表示导航坐标系与AUV2载体坐标系之间的转换矩阵，可通过INS获得。 表示AUV1在导航坐标系下的位置， 表示AUV2在导航坐标系下的位置。
[0106] MBS因子的残差值可由以下公式计算：
[0107]
[0108] 式中，Pw表示AUV2的绝对位置， 表示AUV2初始时刻的绝对位置。
[0109] GNSS动态权重函数表示为以下形式：
[0110]
[0111] λGNSS是动态权重函数，其取值范围为[0，1]。式中，rGNSS为GNSS因子的残差值，r1、r2可以根据实际情况进行选择，r1一般取值范围选择为[1，1.5]，r2一般取值范围选择为[3，8]；
[0112] 在传感器的新测量值被添加到因子图优化之前，动态权重函数会影响因子图的构建，添加了动态权重函数的GNSS因子节点将表示为：
[0113]
[0114] 对于INS、DVL、USBL、MBS，可以采用类似于GNSS的方法，通过获得的残差向量构造动态权重函数，最终因子节点将表示为：
[0115]
[0116] λIMU、λDVL、λUSBL、λMBS是动态权重函数，其取值范围均为[0，1]。
[0117] 根据USV、AUV的INS预积分因子、GNSS因子、DVL因子、USBL因子、MBS因子以及动态权重函数，构建得到基于USV‑AUV的多域协同导航因子图模型，图2为因子图模型示意图。
[0118] S3，利用预设的滑动窗口对编队协同探测因子图模型的所有因子进行优化，通过因子图优化的方法对测量数据信息进行融合，得到水下环境探测信息。
[0119] 步骤一，对编队协同探测因子图模型预设滑动窗口，将滑动窗口外历史时刻的姿态信息边缘化为先验信息；
[0120] 在因子图模型中添加滑动窗口后，当滑动窗口向前滑动时，移除滑动窗口末端边缘的因子节点；当边缘化移出滑动窗口的因子时，旧因子将作为新的先验因子参与滑动窗口内的节点的优化过程，滑动窗口示意图如图3所示。
[0121] 步骤二，根据滑动窗口内的姿态信息及其先验信息，利用编队协同探测因子图模型增量优化滑动窗口内变量节点和因子节点，更新编队协同探测因子图模型；
[0122] 首先，利用预设滑动窗口分别对协同探测因子图模型中的USV、AUV的状态进行处理。滑动窗口中的INS状态定义为X＝[x0,x1,...,xn]，导航状态变量xk可以表示为：
[0123]
[0124] 式中包括位置p、速度v、方向q、陀螺仪偏差bwk、加速度计偏差bak和滑动窗口大小n。
[0125] 根据INS/GNSS组合导航系统以及INS/DVL组合导航系统获得USV、AUV的初始位置，并通过初始时刻USV、AUV的INS因子节点构造先验因子，通过INS预积分更新USV、AUV的状态信息，同时检测是否有其他传感器的测量数据。传感器的测量数据可以同步或异步更新。
[0126] 在本实施例中，作为二分图模型，因子图可以表示为G＝(F,X,E)，它描述了随机变量的联合概率分布；因子分为函数因子fi∈F和变量因子xj∈X，当状态变量因子xj与函数因子fi相关时，它们之间有一条连接边，即为eij∈E；
[0127] 因此，因子图G的定义可以描述为f(X)的因式分解：
[0128] f(X)＝Πifi(xi)；
[0129] 因子fi(xi)可以用残差函数来描述：
[0130] fi(xi)＝r(zi,xi)；
[0131] 式中，r(·)是相应的残差函数(也称为成本函数)，zi是测量值。
[0132] 本实施例在USV‑AUV编队协同探测问题中引入因子图，将ti时刻的航行状态作为变量因子xi，在tk时刻定义包含过去所有时刻状态变量信息的集合 定义所有测量信息 zi表示在时间ti由不同导航传感器获得的实际测量信息。则所有状态变
量和测量的联合概率分布函数可以表示为：
[0133]
[0134] 式中，P(x0)是初始时刻所有状态变量的先验信息， 表示测量模型，表示变量因子的集合； 表示过程模型，其中测量信息来自INS。
[0135] 联合概率密度函数P(Xk|Zk)中，每个因子代表一个独立项，即：
[0136]
[0137] 利用所有历史时刻的有用测量信息来计算所有状态变量的最优估计，则可以利用最大后验概率(MAP)密度将多传感器信息融合问题转化为等效的非线性优化问题，可以表示为：
[0138]
[0139] 通过非线性优化找出 最小值对应的状态，完成优化。
[0140] 步骤三，根据AUV搭载的MBS探测系统获取海底环境信息，AUV对MBS图像数据进行预处理，选取关键帧将其划分为关键帧子地图，包括：
[0141] 在本实施例中，首先使用阈值分割和距离约束方法对MBS图像数据进行过滤将其转换为稀疏点云数据，保留重要的地形特征；
[0142] 然后选取点云数据的关键帧，将每个AUV的点云地图划分为关键帧子地图，保证点云数据质量，同时降低信息冗余度和计算负担，减少误差累积。
[0143] 步骤四，依次将不同AUV的所述关键帧子地图及相关导航信息传输给USV，USV对所述关键帧子地图进行特征提取并进行特征点匹配，从而获取不同AUV之间的位姿关联约束，进一步修正AUV导航位姿误差，包括：
[0144] 二维MBS发射声波，声呐接收到的回波携带了成像区域的特征，用极坐标表示为：
[0145] {(ri,bj),0≤i≤Nr,0≤j≤Nb}；
[0146] 本实施例假设每个波束对应的测量是独立的，并且每个波束都是单独处理的，给定测量波束强度公式：
[0147] zj＝{Iij＝I(ri,bj),0≤i≤Nr}；
[0148] 用强度代表识别海底返回的声音脉冲的距离。为了在各种噪声功率不同的水下环境中可靠地提取特征，本实施例采用一种改进的恒定误报率(CFAR)检测器进行特征提取，在精度和计算费用之间提供良好的平衡。该方法能够将极坐标 下检测到的特征映射到笛卡尔坐标，形成二维平面点云：
[0149] {pn＝(xn,yn)∈R2|0≤n≤N}；
[0150] 式中，
[0151] 由于本实施例假设AUV在固定深度进行探测任务，并且多波束声呐形成二维点云数据，因此在进行特征匹配时，设xt和xs为目标姿态和源姿态，令pt和ps分别为这些姿态处测量的目标点和源点，则两个姿态之间的相对变换xts可以通过相对平移矩阵tts和相对旋转矩阵Rts实现，
[0152] 如图4所示为两个AUV构建相对位姿约束的简单示例，f1和f2为相对位姿约束因子。每个AUV轨迹在其第一个位姿上均有先验因子，AUV之间的相对位姿约束因子是通过匹配检测到的对环境的共同观察，不仅可以产生诸如f1的同一时间的相对位姿约束，也可以产生诸如f2的连接在任意时间创建的相对位姿约束。
[0153] 最广泛使用的匹配算法是ICP算法，它最小化以下成本函数：
[0154]
[0155] 式中，Tp＝Rp+t为AUV运动引起的变换矩阵。上述代价函数中pt和ps的对应关系是通过最近邻搜索建立的。
[0156] 但是，由于ICP算法的非凸性而受到局部极小问题的困扰，并且最终结果在很大程度上取决于初始变换的质量，而初始变换通常是使用航位推算来估计的，由于多波束声呐扫描的点云数量相对稀疏且噪声较大，因此，ICP匹配算法结果在水下环境中受到的损害非常严重。
[0157] 因此，本实施例通过预先执行全局初始化来缓解ICP中的局部极小值问题，然后使用ICP来细化局部估计。令 为全局初始化的源位姿，可表示为：
[0158]
[0159] 式中，当条件为真时II＝1，否则II＝0，d定义为到最近目标点的距离，可表示为：
[0160]
[0161] 步骤五，USV根据关键帧及导航信息拼接同一个AUV的关键帧子地图，形成单个AUV的局部子地图，根据特征点匹配结果融合不同AUV的局部子地图，构建高精度的海底环境全局一致性地图，包括：
[0162] 首先构建同一个AUV的局部子地图，本实施例从SLAM过程中获取关键帧姿态，可表示为 利用上述步骤得到的多波束声呐数据以及提取的特征，构建位于关键帧处的子图，可表示为 其中 表示第k个关键帧中的二维网格单元。则整个
AUV的局部子地图可表示为
[0163] 然后根据步骤四匹配结果，USV将不同AUV的局部地图进行融合，构建海底环境全局地图，同时根据每个AUV获取的更新数据对全局地图进行更新。
[0164] 本发明实施例的实施原理为：
[0165] 通过USV‑AUV编队协同探测系统获取当前时刻的INS/GNSS组合导航系统测量值、INS/DVL组合导航系统测量值、USBL定位系统测量值及MBS探测系统测量值；基于INS/GNSS组合导航系统、INS/DVL组合导航系统、USBL定位系统及MBS探测系统的测量值，构建基于因子图的USV‑AUV编队协同探测概率图模型；利用预设的滑动窗口对编队协同探测因子图模型的所有因子进行优化，通过因子图优化的方法对测量数据信息进行融合，得到水下环境探测信息。将INS/GNSS、INS/DVL、USBL及MBS的信息在因子图框架下进行紧耦合，从而解决大范围水下探测AUV长时间航行，INS/DVL组合导航系产生的误差累积问题，提高了USV‑AUV编队协同探测精度和全局地图一致性。
[0166] 并且利用滑动窗口的方法控制因子的数量从而减小系统的运算量，提高整个协同探测系统的运算效率，满足实时性需求；
[0167] 同时利用残差设计了动态权重函数进行粗差甄别和加权，从而进一步增强USV‑AUV编队协同探测系统的鲁棒性。
[0168] 优选的，结合图1所示的实施例，本发明的一些实施例中，通过USV‑AUV编队协同探测系统获取当前时刻的INS/GNSS组合导航系统测量值、INS/DVL组合导航系统测量值、USBL定位系统测量值及MBS探测系统测量值之前，还包括：
[0169] 通过INS/GNSS组合导航系统测量得到USV的INS、GNSS初始测量值；
[0170] 通过INS/DVL组合导航系统测量得到AUV的INS、DVL初始测量值；
[0171] 通过USBL定位系统测量得到USBL初始测量值；
[0172] 通过MBS探测系统测量得到MBS初始测量值；
[0173] 对INS/GNSS组合导航系统、INS/DVL组合导航系统、USBL定位系统及MBS探测系统的相关状态量及残差值进行初始化，构建得到先验因子。
[0174] 本实施例中，在进行编队协同探测之前，需要先对USV‑AUV编队协同探测系统进行初始化，从而保证计算的精准度。
[0175] 优选的，结合图1‑4所示的实施例，本发明的一些实施例中，利用预设的滑动窗口对编队协同探测因子图模型的所有因子进行优化，通过因子图优化的方法对测量数据信息进行融合，得到水下环境探测信息，包括：
[0176] 对编队协同探测因子图模型预设滑动窗口，将滑动窗口外历史时刻的姿态信息边缘化为先验信息；
[0177] 根据滑动窗口内的姿态信息及其先验信息，利用编队协同探测因子图模型增量优化滑动窗口内变量节点和因子节点，更新编队协同探测因子图模型；
[0178] 根据AUV搭载的MBS探测系统获取海底环境信息，AUV对MBS图像数据进行预处理，选取关键帧将其划分为关键帧子地图；
[0179] 依次将不同AUV的关键帧子地图及相关导航信息传输给USV，USV对关键帧子地图进行特征提取并进行特征点匹配，从而获取不同AUV之间的位姿关联约束，进一步修正AUV导航位姿误差；
[0180] USV根据关键帧及导航信息拼接同一个AUV的关键帧子地图，形成单个AUV的局部子地图，然后根据特征点匹配结果融合不同AUV的所述局部子地图，构建高精度的海底环境全局一致性地图。
[0181] 本实施例中，在构建因子图模型过程中加入了滑动窗口，从而保证实时性。
[0182] 优选的，结合图1‑4所示的实施例，本发明的一些实施例中，利用预设的滑动窗口对编队协同探测因子图模型的所有因子进行优化，通过因子图优化的方法对测量数据信息进行融合，得到水下环境探测信息之后，还包括：
[0183] 当USV‑AUV编队协同探测系统根据水下环境探测信息执行水下编队协同探测时，判断水下编队协同探测是否结束；
[0184] 若结束，则关闭USV‑AUV编队协同探测系统；
[0185] 若未结束，则获取执行时刻的INS/GNSS组合导航系统、INS/DVL组合导航系统、USBL定位系统及MBS探测系统的测量值，执行步骤S2‑步骤S3，得到后续编队协同探测信息。
[0186] 在本说明书的描述中，参考术语“一个实施例”、“示例”、“具体示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0187] 以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。
