CN110749861B - 一种基于多假设深度的水底固定目标的三维定位方法 - Google Patents

Abstract

本发明公开一种基于多假设深度的水底固定目标的三维定位方法。建立坐标系并存储当前时刻相关信息，通过门限条件筛选当前时刻信息和历史信息匹配成对并列出方程，解算方程得到结果，对同一深度的方程结果进行滑动平均，用滑动平均结果来剔除假设深度和对应定位结果不一致的数值，再检测是否有新的量测值，当有新的量测值时，重复进行上述过程，当没有新的量测值时，则定位结束。水声定位系统是通过对水下目标测距或者测向，从而在某一参照系下对目标进行定位的系统，在不同的目标环境中，用到的定位方法不同。

Description

一种基于多假设深度的水底固定目标的三维定位方法

技术领域

本发明属于水下目标三维定位的技术领域；具体涉及一种基于多假设深度的水底固定目标的三维定位方法。

背景技术

现有技术在无人水下航行器UUV开始运行的一段时间内不能定位，需要无人水下航行器UUV航行一段距离后才定位，且运用此算法时需要无人水下航行器UUV机动运行，不能直线行走。

发明内容

水声定位系统是通过对水下目标测距或者测向，从而在某一参照系下对目标进行定位的系统，在不同的目标环境中，用到的定位方法不同；对于被动长基线定位，如果目标是非合作的，用到的方法有基于相对时延的双曲线交会；如果目标是合作的，用到方法有基于绝对时延的距离交会，对于超短基线，目标是合作的，通过时延和测向可定位，此方法适用于对深度大致已知的水底固定目标定位。

本发明通过以下技术方案实现：

一种基于多假设深度的水底固定目标的三维定位方法，所述方法包括以下步骤：

步骤1：选取当地经纬深作为原点，建立坐标系；

步骤2：在坐标系内，存储当前时刻的平台信息及量测信息；

步骤3：当前时刻的信息和其他所有帧历史存储信息进行一一遍历匹配；

步骤4：匹配过程结束后，当无符合门限条件的匹配对存在时则回到步骤2，当有符合门限条件的匹配对存在时则进行到步骤5；

步骤5：收集符合条件的信息对，提取一对已匹配的平台信息及量测信息，根据几何关系列出方程；

步骤6：将步骤5中的方程内的深度未知量，根据具体测试区域水深情况提出多个假设值分别代替，对方程组进行解算，得到目标的横坐标和纵坐标；

步骤7：对同一深度的所有历史解算结果进行滑动平均，得到同一深度的平均的横坐标和纵坐标，对每一深度的结果均进行滑动平均；

步骤8：存储每个假设深度的滑动平均后的定位结果，从定位结果中得到对应深度的横坐标和纵坐标；

步骤9：根据步骤8得到的横坐标和纵坐标，来进行验证深度是否和相应假设一致，

当验证深度和相应假设一致时，保留此假设深度及对应定位结果，

当验证深度和相应假设不一致时，剔除此假设深度及对应定位结果；

步骤10：保留除步骤9之外的剩余多假设的深度及定位结果；

步骤11：判断是否有新的量测值，

当有新的量测值时，回到步骤2重复进行上述过程；当没有新的量测值时，则定位结束。

进一步的，所述步骤2中从探测到目标开始，存储的信息包括无人水下航行器UUV的探测空间方位角结果、坐标信息和方位姿态仪信息。

进一步的，所述步骤3中无人水下航行器UUV最新一帧坐标和历史坐标的距离，无人水下航行器UUV最新一帧航行方向和历史航行方向的夹角，需检验的物理量是：无人水下航行器UUV最新一帧的空间方位角和历史空间方位角，这两个方位角均须在设定的范围内。当和历史信息进行匹配和检验时，存在两种情况：第一，存在符合以上所有条件的一对或者多对信息；第二不存在符合以上所有条件的一对信息。当出现第一种情况时，进入下一步，即步骤4；当出现第二种情况时，继续等步骤2收集新的信息，然后步骤四进行匹配和检验，直到出现情况一，然后进入下一步。

进一步的，所述步骤4中的门限条件具体为距离、夹角和方位角，距离的门限设定为探测距离的5％，夹角的满足的范围设定为45°～135°；方位角的满足范围设定为30°～150°。

进一步的，所述步骤5得到两个方程，其中包含三个未知量，即目标在本坐标系下的横坐标，纵坐标，深度。

进一步的，所述步骤6中两个方程无法解算三个未知数，所以将三个未知数中的深度未知量根据具体测试区域水深情况提出多个假设值分别代替，使两个方程解决两个未知数，对方程组进行解算，得到目标的横坐标和纵坐标，对步骤6中的深度遍历，依次进行下一步。

进一步的，所述步骤7中所有假设深度的定位结果都进行滑动平均，每一时刻以平均结果为最终结果，因为有多个假设深度，所以最终每一时刻每一假设深度均会对应一个解算出的横、纵坐标。

本发明的有益效果是：

在对水域深度范围有大致估计的前提下，进行的水下固定目标定位。定位方法中运用多假设深度方法，可避免由于实际水深与估计水深存在些许差异导致的定位失败，容错率高，对于水底不平坦定位可提供参考；其次，对于解算前对于可利用信息的筛选，考虑了实际情况，保证了每一次解算结果的误差可接受；最后在解算中加入滑动平均，由于不同时刻的解算彼此独立，可看作不同事件，加入平均可从概率角度降低误差，而平均是滑动的原因是，考虑UUV与目标距离远时量测信息误差大，可能解算结果误差大，不希望前期的较大误差的解算结果影响所有时刻，所以计算滑动平均值时，方程的结算结果为n个，当n为偶数时，选取从n/2到最后的数据计算滑动平均值，当n为奇数时，选取从(n+1)/2到最后的数据计算滑动平均值。

附图说明

图1是本发明的流程图。

图2是本发明当前时刻信息和历史时刻信息匹配过程图。

图3是本发明所列方程对应的几何关系示意图。

图4是本发明仿真中无人水下航行器UUV与目标的相对态势图-俯视图。

图5是本发明仿真中无人水下航行器UUV与目标的相对态势图-三维图。

图6是本发明仿真中多个假设深度对应的解算x、y坐标结果。

图7是本发明真实深度对应x、y坐标滑动平均前后解算误差对比图。

图8是本发明海底平坦情况下，多假设深度解算目标位置相对示意图。

具体实施方式

下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

假定海深2000米，海底平坦；无人水下航行器UUV初始位置在(0，0，-1000)，航速1.5米/秒，航向变化率为1.5°/秒,无人水下航行器UUV航行保持在同一深度；目标位于(1000，-1200，-2000)处，固定不动。态势图见图4，图5。线列阵安装于无人水下航行器UUV上，与无人水下航行器UUV航向方向一致。本申请中方位角及航向角均为角度，定位实现主要步骤如图1，

多假设深度为多个假设深度数值。

一种基于多假设深度的水底固定目标的三维定位方法，所述方法包括以下步骤：

步骤1：选取当地经纬深作为原点，建立坐标系；

水下目标的信号被无人水下航行器UUV中的线列阵接受，并进行常规处理，可得到阵坐标下的方位角，由于目标与无人水下航行器UUV有深度差，此方位角是包含三维信息的方位角。存储此时刻的方位角，即量测值；同时也存储无人水下航行器UUV的坐标，航向角(指正北方向顺时针到航向方向的角度)，探测目标位于左舷还是右舷。

步骤2：在坐标系内，存储当前时刻的平台信息及量测信息；

步骤3：当前时刻的信息和其他所有帧历史存储信息进行一一遍历匹配；

步骤4：匹配过程结束后，当无符合门限条件的匹配对存在时则回到步骤2，当有符合门限条件的匹配对存在时则进行到步骤5；

是当前帧和历史所有帧进行一一遍历匹配，每一次匹配时，都会用门限去看是否符合要求，等所有帧信息匹配结束，看是否有符合要求的匹配对存在；

步骤5：收集符合条件的信息对，提取一对已匹配的平台信息及量测信息，根据几何关系列出方程；

存储当前时刻信息后和历史时刻信息匹配，遍历所有历史时刻。匹配过程如图2所示，匹配过程分为三部分：第一，两时刻无人水下航行器UUV的航向，考虑这个量的原因是在无人水下航行器UUV直行时，解算误差过大，所以考虑航向会影响解算。经过大量仿真验证，当航向差值为90°或270°时解算结果误差最小，同时仿真发现在90°左右一定范围内解算误差均可接受，大量仿真表明范围为45°～135°或者225°～315°；第二，两时刻无人水下航行器UUV的坐标的距离，考虑这个量的原因是，如果两点距离过近，从几何关系上，交汇处的坐标误差会比较大。经过仿真得到为最大探测距离的5％，本实验探测距离为3000米，距离门限可设置为150米；第三，两时刻方位角即量测值的范围，考虑这个量的原因是实际工程中，线性阵在对信号进行常规处理是会在0°，180°附近有较大的误差，即在0°，180°附近的角度不太可信。这个量的可取范围根据实际情况，本实验设置为30°～-150°。

会有多对匹配成功，只需取其中一对匹配成功的信息。

根据匹配成功的两点的信息列方程，几何关系如图3所示。在两点处分别对应一个方程，方程的物理意义是，此时刻无人水下航行器UUV坐标与目标坐标连线方向的向量与无人水下航行器UUV的航向方向的向量夹角为探测的方位角，即量测值，根据向量的数量积公式建立方程。如图3所示几何关系，已知t1时刻无人水下航行器UUV的坐标为(x1,y1,z1),航向角度为heading1，量测方位角为θ1，t2时刻无人水下航行器UUV的坐标为(x2,y2,z2),航向角度为heading2，量测方位角为θ2；目标坐标未知，可设为(x0,y0,z0)。

t1时刻方程建立过程：首先，求此时无人水下航行器UUV坐标与目标连线方向的向量，为(x0-x1,y0-y1,z0-z1)；其次，求航向方向的向量，为(sin(heading1*π/180)，cos(heading1*π/180)，0)；根据量测,上述两向量夹角为θ1，根据向量的数量积公式a·b＝|a|·|b|·cos(θ)建立方程：

其中，(sin(heading1*π/180)表示的含义是角度变弧度，求正弦值

同理，在t2时刻也建立同样的方程：

其中，(sin(heading2*π/180)表示的含义是角度变弧度，求正弦值，

两方程联立，并对方程变形，可组成方程组：

步骤6：将步骤5中的方程内的深度未知量，根据具体测试区域水深情况提出多个假设值分别代替，其中假设值以10米为间隔设置50个，对方程组进行解算，得到目标的横坐标和纵坐标；

在已知当地水域大致水深的条件下，由于并不知道水底是否平坦，在给定的水深一定范围内假设多个水深，使方程的z0变为已知，进行解方程。

本申请解方程用到的方法是拟牛顿迭代法，具体如下：

步骤6.1：初值的给定，在t1时刻，把量测的方位角看作平面角，根据左右舷信息，如果是右舷，则是量测方位角θ1加航向角度heading1，确定一个方向；如果是左舷，则是360°减去方位角θ1然后加上heading1，确定一个方向；t1时刻AUV坐标点沿此方向确定一条直线；同理，t2时刻亦可确定一条直线；两条直线交点x,y坐标即为初值。

步骤6.2：迭代求最优解，将方程组写成矩阵的形式：

初值矩阵:

迭代公式：

其中:

其中，Xⁱ表示的含义是第i次迭代的结果，F'(Xⁱ)表示的含义是F矩阵对第i次迭代结果的偏导矩阵，F(Xⁱ)表示的含义是第i次迭代结果对应的F矩阵，

A_i是偏导矩阵，每个元素均为相应偏导；其中f₁指的是F矩阵的第一个元素；f₂指的是F矩阵的第二个元素；xⁱ指的是第i次迭代结果矩阵的第一个元素；yⁱ指的是第i次迭代结果矩阵的第二个元素。

直到迭代稳定，即为方程的解；根据大量仿真，迭代一定次数即可稳定，文中设置迭代50次即得到稳定的解。

步骤7：对同一深度的所有历史解算结果进行滑动平均，得到同一深度的平均的横坐标和纵坐标，对每一深度的结果均进行滑动平均；

对于同一时刻而言，存在多个结果，即空间上多个结果；对于同一深度而言，存在多个结果，即时间上的多个结果。对于同一深度，不同时刻的解算彼此独立，可对不同时刻的解算结果进行滑动平均，得到最终结果。注意，滑动平均指的是时间上的滑动平均，每次平均只用到当前解算结果总个数的最新的1/2。如果解算结果个数为偶数a，用到的个数就是a/2；如果解算结果个数为奇数，用到的个数就是(a+1)/2。

对于多个假设的每个深度每一时刻均进行滑动平均操作，以平均结果为最终结果。对于滑动平均的效果，如图7所示，以真实深度解算结果进行对比，明显平均后比平均前解算结果更稳定，误差小。

步骤8：存储每个假设深度的滑动平均后的定位结果，从定位结果中得到对应深度的横坐标和纵坐标；

步骤9：根据步骤8得到的横坐标和纵坐标，验证此位置实际深度是否和相应假设一致，其中从时间上来看，解算出的横纵坐标起伏较大的假设深度不予考虑，

当验证深度和相应假设一致时，保留此假设深度及对应定位结果，

通过某深度解算的x,y处测深去验证此假设是否成立，参考图8。

举例说明，以平坦水底为例，深度1600米，解算出的x,y坐标为1000，1000，如果在x,y对应的位置测深发现为2000米，这说明此假设不成立，当删除此假设深度对应的结果；如果在x,y对应的位置测深发现为1600米左右，当认为假设当成立，保留此假设深度对应结果。

当验证深度和相应假设不一致时，剔除此假设深度及对应定位结果；

步骤10：保留除步骤9之外的剩余多假设的深度及定位结果；

步骤11：判断是否有新的量测值，

对应多个假设深度结果的验证，也可通过一定时间(设定10-100次)后观察解算结果的稳定性，如图6所示，本申请目标真实深度为2000米，可见真实深度处的解算结果比较稳定，而其他深度解算结果波动较大。

当有新的量测值时，回到步骤2重复进行上述过程；当没有新的量测值时，则定位结束。

综上，可得到真实的目标定位结果。

进一步的，所述步骤2中从探测到目标开始，存储的信息包括无人水下航行器UUV的探测空间方位角结果、坐标信息和方位姿态仪信息。

进一步的，所述步骤3中无人水下航行器UUV最新一帧坐标和历史坐标的距离，无人水下航行器UUV最新一帧航行方向和历史航行方向的夹角，需检验的物理量是：无人水下航行器UUV最新一帧的空间方位角和历史空间方位角，这两个方位角均须在设定的范围内。当和历史信息进行匹配和检验时，存在两种情况：第一，存在符合以上所有条件的一对或者多对信息；第二不存在符合以上所有条件的一对信息。当出现第一种情况时，进入下一步，即步骤4；当出现第二种情况时，继续等步骤2收集新的信息，然后步骤四进行匹配和检验，直到出现情况一，然后进入下一步。

进一步的，所述步骤4中的门限条件具体为距离、夹角和方位角，距离的门限设定为探测距离的5％，夹角的满足的范围设定为45°～135°，验证90度为最佳；方位角的满足范围设定为30°～150°，因为考虑到前端探测波束形成，由于阵的指向性导致0°和180°附近方位角结果误差过大，会影响定位，所以对可能存在较大误差的信息剔除。

进一步的，所述步骤5得到两个方程，其中包含三个未知量，即目标在本坐标系下的横坐标，纵坐标，深度。

进一步的，所述步骤6中两个方程无法解算三个未知数，所以将三个未知数中的深度未知量根据具体测试区域水深情况提出多个假设值分别代替，使两个方程解决两个未知数，对方程组进行解算，得到目标的横坐标和纵坐标，对步骤6中的深度遍历，依次进行下一步。

进一步的，所述步骤7中只平均最近的1/2；所有假设深度的定位结果都进行滑动平均，每一时刻以平均结果为最终结果，因为有多个假设深度，所以最终每一时刻每一假设深度均会对应一个解算出的横、纵坐标。

进一步的，所述步骤9对从时间上来看，解算出的横纵坐标起伏较大的假设深度不予考虑，原因是如果目标在此假设深度附近，解算结果一般不会起伏过大，详见图6。

Claims (6)

1.一种基于多假设深度的水底固定目标的三维定位方法，其特征在于，所述方法包括以下步骤：

步骤1：选取当地经纬深作为原点，建立坐标系；

步骤2：在坐标系内，存储当前时刻的平台信息及量测信息；

步骤3：当前时刻的信息和其他所有帧历史存储信息进行一一遍历匹配；

步骤4：匹配过程结束后，当无符合门限条件的匹配对存在时则回到步骤2，当有符合门限条件的匹配对存在时则进行到步骤5；

步骤5：收集符合条件的信息对，提取一对已匹配的平台信息及量测信息，根据几何关系列出方程；

步骤6：将步骤5中的方程内的深度未知量，根据具体测试区域水深情况提出多个假设值分别代替，对方程组进行解算，得到目标的横坐标和纵坐标；

步骤7：对同一深度的所有历史解算结果进行滑动平均，得到同一深度的平均的横坐标和纵坐标，对每一深度的结果均进行滑动平均；

步骤8：存储每个假设深度的滑动平均后的定位结果，从定位结果中得到对应深度的横坐标和纵坐标；

步骤9：根据步骤8得到的横坐标和纵坐标，来进行验证深度是否和相应假设一致，

当验证深度和相应假设一致时，保留此假设深度及对应定位结果，

当验证深度和相应假设不一致时，剔除此假设深度及对应定位结果；

步骤10：保留除步骤9之外的剩余多假设的深度及定位结果；

步骤11：判断是否有新的量测值，当有新的量测值时，回到步骤2重复进行上述过程；当没有新的量测值时，则定位结束；

所述步骤2中从探测到目标开始存储的信息包括无人水下航行器UUV的探测空间方位角结果、坐标信息和方位姿态仪信息；

根据匹配成功的两点的信息列方程，在两点处分别对应一个方程，方程的物理意义是，此时刻无人水下航行器UUV坐标与目标坐标连线方向的向量与无人水下航行器UUV的航向方向的向量夹角为探测的方位角，即量测值，根据向量的数量积公式建立方程；已知t1时刻无人水下航行器UUV的坐标为(x1,y1,z1),航向角度为heading1，量测方位角为θ1，t2时刻无人水下航行器UUV的坐标为(x2,y2,z2),航向角度为heading2，量测方位角为θ2；目标坐标未知，可设为(x0,y0,z0)；

t1时刻方程建立过程：首先，求此时无人水下航行器UUV坐标与目标连线方向的向量，为(x0-x1,y0-y1,z0-z1)；其次，求航向方向的向量，为(sin(heading1*π/180)，cos(heading1*π/180)，0)；根据量测,上述两向量夹角为θ1，根据向量的数量积公式a·b＝|a|·|b|·cos(θ)建立方程：

其中，sin(heading1*π/180)表示的含义是角度变弧度，求正弦值

同理，在t2时刻也建立同样的方程：

其中，sin(heading2*π/180)表示的含义是角度变弧度，求正弦值。

2.根据权利要求1所述方法，其特征在于，所述步骤3中无人水下航行器UUV最新一帧坐标和历史坐标的距离，无人水下航行器UUV最新一帧航行方向和历史航行方向的夹角，需检验的物理量是：无人水下航行器UUV最新一帧的空间方位角和历史空间方位角，这两个方位角均须在设定的范围内；当和历史信息进行匹配和检验时，存在两种情况：第一，存在符合以上所有条件的一对或者多对信息；第二不存在符合以上所有条件的一对信息；当出现第一种情况时，进入步骤4；当出现第二种情况时，继续等步骤2收集新的信息，然后步骤4进行匹配和检验，直到出现情况一，然后进入步骤5。

3.根据权利要求1所述方法，其特征在于，所述步骤4中的门限条件具体为距离、夹角和方位角，距离的门限设定为探测距离的5％，夹角的满足的范围设定为45°～135°；方位角的满足范围设定为30°～150°。

4.根据权利要求1所述方法，其特征在于，所述步骤5得到两个方程，其中包含三个未知量，即目标在本坐标系下的横坐标、纵坐标和深度。

5.根据权利要求1所述方法，其特征在于，所述步骤6中两个方程无法解算三个未知数，所以将三个未知数中的深度未知量根据具体测试区域水深情况提出多个假设值分别代替，使两个方程解决两个未知数，对方程组进行解算，得到目标的横坐标和纵坐标，对步骤6中的深度遍历，依次进行步骤7。

6.根据权利要求1所述方法，其特征在于，所述步骤7中所有假设深度的定位结果都进行滑动平均，每一时刻以平均结果为最终结果，因为有多个假设深度，所以最终每一时刻每一假设深度均会对应一个解算出的横、纵坐标。

Images