CN105823480A - 基于单信标的水下移动目标定位算法 - Google Patents

Abstract

基于单信标的水下移动目标定位算法，其特征在于：采用一个水上移动信标来进行水下移动目标的定位；所述移动信标配备有卫星定位接收器，水下移动目标配备有水平姿态传感器、航速传感器和垂直方位传感器。通过此技术可以获得水下目标与信标的直线距离，同时根据水下目标(自身携带的传感器获得水下目标的一些姿态、速度、深度测量值)。在距离与这些测量值的基础上通过建立系统模型、求解初始状态、状态可观性分析、状态方程离散化、卡尔曼滤波等步骤最终获得水下目标的位置估计值。

Description

基于单信标的水下移动目标定位算法

技术领域

本发明属于定位导航技术领域，涉及一种水下移动目标的定位技术，具体的说，涉及一种基于单信标的水下移动目标定位算法。

背景技术

水下定位导航技术是海洋科学考察、海洋资源勘探、海洋资源开发、深海空间站建设等工程的必备手段，而水下电磁波信号衰减快，陆地上的GPS信号无法直接应用于水下目标定位，所以水声定位技术是目前水下定位系统的主要研究方向。

传统上，依据水下定位信标之间的距离，我们将水下定位系统分3种类型：长基线定位系统(LBL)、短基线定位系统(SBL)以及超短基线定位系统(USBL)。传统方法放置的基元多，操作复杂，成本高昂。多个航行器可以接收导航信息速率随水中航行器数目增加而减少，而且导航区域限制在一定的范围内，因此传统方法实现有界误差的导航缺乏可扩展性。目前最先进、准确的惯性定位系统(InertialNavigationSystem,INS)也有大约每小时几公里的偏移率。在INS中增加一个多普勒声呐(Doppler-VelocitySonar,DVS)用于测量航行器相对于地球的速度，可以将该误差减少到航行距离的0.01％。但是只有潜艇和大型无人水下航行器(UUVs)配备INS和DVS技术，较小的航行器受空间和能源限制无法装备。大部分小型航行器使用的是最基本的导航推算技术。这种技术需要航行器定期到水面更新位置信息，这样就加大了航行器的工作负担。

发明内容

本发明的目的在于针对小型水下移动目标，提供一种导航效果好、定位准确的基于单信标的水下移动目标的定位算法。

本发明的技术方案为：基于单信标的水下移动目标定位算法，采用一个水上移动信标来进行水下移动目标的定位；移动信标配备有卫星定位接收器，水下移动目标配备有水平姿态传感器、航速传感器和垂直方位传感器，包括以下步骤：

A、建立水下坐标系，建立水下移动目标的运动数学模型：

其中，x，y，z为水下移动目标的位置，V为水下移动目标的运动速度，V_C为洋流速度；θ为水下移动目标的航向角，为水下移动目标的俯仰角(水下移动目标航行方向与水平方向的夹角，可通过水平姿态传感器得到)；

B、获得水下移动目标的初始位置；

C、建立水下移动目标的状态模型，并定义观测模型：

假定洋流的流速V_c为恒定值，为一个常值；航向角θ和航速V测量已经过滤波误差为零；距离r测量噪声为高斯白噪声；

x、y、z分别对应三维坐标系下x轴、y轴和z轴的状态分量；

其中，令状态量控制量为：

对X求导数，由于V_c为恒定值，均为0；

因此，所以，得到以下A和B，

定义移动信标的坐标为(x_b,y_b,0)，得到水下移动目标与移动信标之间的距离测量值：z为水下移动目标所处的水深；

定义观测模型为

得到状态方程

D、对系统模型进行可观性分析：

令U＝V,将B转换为：

用来代替r，h₂＝z；

将航向角作为测量值，h₃＝θ；

h₁的零阶李导数为：

其中z_c为Z方向洋流对航行器作用的位移。

h₁的一阶李导数为：

h₂的零阶和一阶李导数分别为：

h₃的零阶和一阶李导数分别为：

得到系统模型的观测矩阵O：

取出O中前三行构成矩阵O₁，如果O₁满秩说明系统中所有的位置量可观。

对O₁进行行变换和列变换，得到：

得到O₁不满秩的几种情况

根据可观性分析，得到如下结论：

(1)一般情况下，系统可观；

(2)航行器在过Z轴平面上运动时，系统不可观；

(3)当时，即航行器的航向过航行器与信标之间连线时，系统不可观；

E、对系统模型进行离散化处理：

①状态模型离散化：

系统方程的状态转移矩阵为

其泰勒展开为：

由于A的二阶以上项都为0，Φ(t,t₀)可以改写为：

X(t)可以表示为以下的形式：

定义离散化的周期为1，假定U前后的采样值相差不大，可得到离散化状态模型：

X_k＝AX_k-1+BU_k-1+w_k-1

离散化后，可得到新的A，B矩阵，其中A＝Φ(t)|_t＝T＝1,

即：

②观察模型离散化：

对进行泰勒展开，得到将其线性化，得到：

以1为间隔离散化变为：

测量值有距离r和深度z，由观测向量得到H_k矩阵为

其中：

得到系统的离散化观测模型为Z_k＝H_kX_k+v_k；

F、对系统进行卡尔曼滤波处理：

①先验估计：

②更新协方差P：

其中Q为w_k的协方差矩阵；

③计算先验估计对应的距离：

④计算H矩阵：

⑤计算卡尔曼增益，其中R是v_k的协方差矩阵：

⑥后验估计：

⑦更新协方差：

X_k即为估计的水下移动目标的移动轨迹。

优选的是：获得水下移动目标初始位置的方法为：

假定信标为固定，且以信标所在位置为原点，建立坐标系；

(1)假定水下移动目标的初始位置：记为P₀，此时水下移动目标与y轴件的夹角记为水下移动目标与信标之间的距离为r₁，时刻记为t₁；

(2)忽略洋流流速，根据水下移动目标的运动速度，获得t₂时刻水下移动目标的航位推算向量记为a，从而获得t₂时刻水下移动目标的理想位置，记为P_M；

(3)考虑洋流流速，获得t₂时刻水下移动目标的真实位置，记为P₁，此时水下移动目标与y轴间的夹角记为水下移动目标与信标之间的距离为r₂，时刻记为t₂；

(4)比较P₁与P_M之间的位置，获得洋流向量b；

(5)从P₁点开始，根据水下移动目标的运动速度，获得t₃时刻水下移动目标的航位推算向量记为c，加上(4)得到的洋流向量b，获得t₃时刻水下移动目标的理想位置，记为P₃，此时水下移动目标与信标之间的距离为r₄；

(6)由信标与航行器之间的测距信息，获得t₃时刻水下移动目标与信标之间的距离r₃，其中P₂为根据测距信息任意选的一个位置；

(7)计算r₃与r₄之间的差，记为r；

(8)将和分别由0～360°遍历，得到一系列的r值，在所有的r值中，找到最小的r值对应的位置P₀和洋流向量b，则此时P₀为最接近水下移动目标初始位置的初始状态。

优选的是：假定信标为固定，且以信标所在位置为原点，将水下移动目标所在的位置置于第一象限，建立坐标系，将和分别由0～90°遍历。

本发明的有益效果为：

本发明的算法是建立在单信标水声定位的基础上，该定位技术可应用于体型较小且成本不高的水下航行器，可以解决传统定位方法中布设回收困难、成本高昂，以及航位推算受到未知洋流影响导致误差发散的缺点。

在此定位技术的基础上提出了一种水下移动目标的定位算法，通过此技术可以获得水下目标与信标的直线距离(在未考虑到一些干扰的基础上)，同时根据水下目标(航行器)自身携带的传感器获得水下目标的一些姿态、速度、深度测量值。在距离与这些测量值的基础上通过建立系统模型、求解初始状态、状态可观性分析、状态方程离散化、卡尔曼滤波等步骤最终获得水下目标的位置估计值。

其中信标可移动使得本方法的导航区域扩大，三维模型的建立也减少了深度值估计的误差。该算法经过仿真验证，定位导航效果良好，能够比较准确的获得水下目标的位置轨迹，完成导航。

单信标定位技术只需要单个信标，用于标定信标支持的区域内航行器与已知信标的相对距离，信标所做的工作只是广播水声信号，同时将其真实的地理位置编码到水声信号中。各航行器可以计算自身的地理位置，而不需要信标系统进行计算，其支持的航行器的数量不受限制。该技术的基础就是航位推算技术，通过测量的距离对位置误差进行纠正，弥补了定期更新位置信息的缺点，提高了航行器的续航能力。单个信标布设和回收远远比多个信标方便，而且不存在多信标相对位置的限制。

附图说明

图1为本系统在坐标系下模型图。

图2为水下移动目标初始位置遍历原理图。

图3为水下移动目标初始位置遍历流程图。

图4为水下移动目标航向角和俯仰角示意图。

具体实施方式

以下结合附图对本发明的具体实施方式进行进一步的描述。

基于单信标的水下移动目标定位算法，采用一个水上移动信标来进行水下移动目标的定位；用于标定信标支持的区域内航行器与信标的相对距离。移动信标配备有卫星定位接收器，水声换能器及同步时钟系统，通过全球定位系统获得信标的位置信息，其位置信息与时钟信息编码到水声信号中，信标的工作是广播水声信号。

水下移动目标配备有水平姿态传感器、航速传感器和垂直方位传感器，水平姿态传感器可以得到水下移动目标航行的俯仰角信息，垂直方位传感器可以得到水下移动目标的水深位置信息，航速传感器可以获得水下移动目标的航速信息。

如图1所示，P(x,y)对应水下移动目标的位置，对水下移动目标定位的算法包括以下步骤：

A、建立水下坐标系，信标为移动目标，建立水下移动目标的运动数学模型：

其中，x，y，z为水下移动目标的位置，V为水下移动目标的航速，V_C为洋流速度；θ为水下移动目标的航向角，为水下移动目标的俯仰角(水下移动目标航行方向与水平方向的夹角，可通过水平姿态传感器得到)；

B、获得水下移动目标的初始位置。

由水声测距方法可以获得信标与水下航行器的初始距离值，但是只有距离难以确定水下航行器的初始位置，因此需要采用遍历的方法来求得水下航行器的真实初始位置。由于Z方向的位置信息可以通过测量得到，因此可将三维的遍历简化成二维的遍历以减少计算量。遍历的基本原理是：假定一个具体的角度，在此角度下根据距离获得航行器的初始位置，利用航位推算与洋流信息推算出下一时刻航行器在此角度下的位置信息，由此位置信息求得与信标的距离，与真实时刻的距离信息作比较，在所有角度中寻找一个最接近的值，那么这个角度就是所估计的角度，便可以得到航行器的初始位置信息。具体方法为：

如图2和图3所示，假定信标为固定，且以信标所在位置为原点，建立坐标系；

(1)假定水下移动目标的初始位置：记为P₀，此时水下移动目标与y轴件的夹角记为水下移动目标与信标之间的距离为r₁，时刻记为t₁；

(2)忽略洋流流速，根据水下移动目标的运动速度，获得t₂时刻水下移动目标的航位推算向量记为a，从而获得t₂时刻水下移动目标的理想位置，记为P_M；

(3)考虑洋流流速，获得t₂时刻水下移动目标的真实位置，记为P₁，此时水下移动目标与y轴间的夹角记为水下移动目标与信标之间的距离为r₂，时刻记为t₂；

(4)比较P₁与P_M之间的位置，获得洋流向量b；

(5)从P₁点开始，根据水下移动目标的运动速度，获得t₃时刻水下移动目标的航位推算向量记为c，加上(4)得到的洋流向量b，获得t₃时刻水下移动目标的理想位置，记为P₃，此时水下移动目标与信标之间的距离为r₄；

(6)由信标与航行器之间的测距信息，获得t₃时刻水下移动目标与信标之间的距离r₃，其中P₂为根据测距信息任意选的一个位置；

(7)计算r₃与r₄之间的差，记为r；

(8)将和分别由0～360°遍历，得到一系列的r值，r值的个数与所取的遍历分度有关。在所有的r值中，找到最小的r值对应的位置P₀和洋流向量b，则此时P₀为最接近水下移动目标初始位置的初始状态。

在0～360°遍历的过程中，产生的所有的点集中在以坐标系原点为中心，以r₁为半径的圆周上，遍历过程中会产生两个最小值，受误差和精度的影响，查找到的最小值可能会落在真实位置的对称点上，因此，为了节省计算量，假定信标为固定，且以信标所在位置为原点，将水下移动目标所在的位置置于第一象限，建立坐标系，将和分别由0～90°遍历。

C、建立水下移动目标的状态模型，并定义观测模型：

假定洋流的流速V_c为恒定值，为一个常值；航向角θ和航速V测量已经过滤波误差为零；距离r测量噪声为高斯白噪声；在上述假设基础下，建立状态模型，其中需要估计的量有两个，洋流的流速V_c和航行器的当前位置P。

x、y、z分别对应三维坐标系下x轴、y轴和z轴的状态分量；

其中，令状态量控制量为：

对X求导数，由于V_c为恒定值，均为0；

因此，所以，得到以下A和B，

定义移动信标的坐标为(x_b,y_b,0)，得到水下移动目标与移动信标之间的距离测量值：

z为水下移动目标所处的水深；

定义观测模型为

得到状态方程

D、对系统模型进行可观性分析：为了得到真实的航行轨迹，必须进行卡尔曼滤波，而进行卡尔曼滤波的条件便是状态可观。由于上述系统为一个非线性系统，对于一个非线性系统，进行可观性分析的方法便是李导数法。

令U＝V,将B转换为：

用来代替r，h₂＝z；

将航向角作为测量值，h₃＝θ；

h₁的零阶李导数为：

其中z_c为Z方向洋流对航行器作用的位移。

h₁的一阶李导数为：

h₂的零阶和一阶李导数分别为：

h₃的零阶和一阶李导数分别为：

得到系统模型的观测矩阵O：

取出O中前三行构成矩阵O₁，如果O₁满秩说明系统中所有的位置量可观。

对O₁进行行变换和列变换，得到：

得到O₁不满秩的几种情况

在这几种情况下航行器的位置不可观。

E、对系统模型进行离散化处理：为了进行卡尔曼滤波，获得相关参数，需要对状态方程中的状态模型和观测模型就行离散化处理。

①状态模型离散化：

系统方程的状态转移矩阵为

其泰勒展开为：

由于A的二阶以上项都为0，Φ(t,t₀)可以改写为：

X(t)可以表示为以下的形式：

定义离散化的周期为1，假定U前后的采样值相差不大，可得到离散化状态模型：

X_k＝AX_k-1+BU_k-1+w_k-1

离散化时，可得到新的A，B矩阵，其中A＝Φ(t)|_t＝T＝1,

即：

②观察模型离散化：

对进行泰勒展开，得到将其线性化，得到：

以1为间隔离散化变为：

测量值有距离r和深度z，由观测向量得到H_k矩阵为

其中：

得到系统的离散化观测模型为Z_k＝H_kX_k+v_k；

F、对系统进行卡尔曼滤波处理：在对系统进行可观性分析以及离散化处理后，便可以进行卡尔曼滤波，使得状态向量X中的各项值逐渐逼近真实值。具体的卡尔曼滤波步骤如下：

①先验估计：

②更新协方差P：

其中Q为w_k的协方差矩阵；

③计算先验估计对应的距离：

④计算H矩阵：

⑤计算卡尔曼增益，其中R是v_k的协方差矩阵：

⑥后验估计：

⑦更新协方差：

X_k即为估计的水下移动目标的移动轨迹(状态量的离散化形式)，X_k中包括所要估计的航行器的位置量(x,y,z)和要估计的洋流分量速度(V_cx,V_cy,V_cz)。

Claims (3)

1.基于单信标的水下移动目标定位算法，其特征在于：采用一个水上移动信标来进行水下移动目标的定位；所述移动信标配备有卫星定位接收器，水下移动目标配备有水平姿态传感器、航速传感器和垂直方位传感器，包括以下步骤：

A、建立水下坐标系，建立水下移动目标的运动数学模型：

其中，x，y，z为水下移动目标的位置，V为水下移动目标的运动速度，V_C为洋流速度；θ为水下移动目标的航向角，为水下移动目标的俯仰角；

B、获得水下移动目标的初始位置；

C、建立水下移动目标的状态模型，并定义观测模型：

假定洋流的流速V_c为恒定值，为一个常值；航向角θ和航速V测量已经过滤波误差为零；距离r测量噪声为高斯白噪声；

x、y、z分别对应三维坐标系下x轴、y轴和z轴的状态分量；

其中，令状态量控制量为：

对X求导数，由于V_c为恒定值，均为0；

因此， $\stackrel{\·}{X}=\left[\begin{array}{cccccc}0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ V_{cx}\\ V_{cy}\\ V_{cz}\end{array}\right]+\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{V}_x\\ V_y\\ V_z\end{array}\right],$ 所以，得到以下A和B，

$A=\left[\begin{array}{cccccc}0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right],B=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$

定义移动信标的坐标为(x_b,y_b,0)，得到水下移动目标与移动信标之间的距离测量值：

z为水下移动目标所处的水深；

定义观测模型为

得到状态方程

D、对系统模型进行可观性分析：

令U＝V,将B转换为：用来代替r，h₂＝z，将航向角作为测量值，h₃＝θ，求出h₁，h₂和h₃的各阶李导数及梯度，得到系统模型的观测矩阵O，

取出O中前三行构成矩阵O₁，如果O₁满秩说明系统中所有的位置量可观；

E、对系统模型进行离散化处理：

①状态模型离散化：

系统方程的状态转移矩阵为

其泰勒展开为：

$\mathrm{\Φ}(t,t_0)=I+A(t-t_0)+\frac{A^2{(t-t_0)}^2}{2!}+...$

由于A的二阶以上项都为0，Φ(t,t₀)可以改写为：

$\mathrm{\Φ}(t,t_0)=I+A(t-t_0)=\left[\begin{array}{cccccc}1& 0& 0& t-t_0& 0& 0\\ 0& 1& 0& 0& t-t_0& 0\\ 0& 0& 1& 0& 0& t-t_0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

X(t)可以表示为以下的形式：

$X\left(t\right)=\mathrm{\Φ}(t,t_0)X\left(t_0\right)+\underset{t_0}{\overset{t}{\∫}}\mathrm{\Φ}(t,\mathrm{\τ})LU\left(\mathrm{\τ}\right)d\mathrm{\τ}$

定义离散化的周期为1，假定U前后的采样值相差不大，可得到离散化状态模型：

X_k＝AX_k-1+BU_k-1+w_k-1

离散化后，可得到新的A，B矩阵，其中即：

$A=\left[\begin{array}{cccccc}1& 0& 0& 1& 0& 0\\ 0& 1& 0& 0& 1& 0\\ 0& 0& 1& 0& 0& 1\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],B=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right];$

②观察模型离散化：

对进行泰勒展开，得到

将其线性化，得到：

$r\left(X_t\right)=r\left(X_{t_0}\right)+\frac{\∂r}{\∂X_{t_0}}(X_t-X_{t_0})$

以1为间隔离散化变为：

$r_k-r_{k-1}=\frac{\∂r_{k-1}}{\∂X_{k-1}}(X_k-X_{k-1})$

测量值有距离r和深度z，由观测向量得到H_k矩阵为

$H_k=\left[\begin{array}{c}\frac{\∂r_{k-1}}{\∂X_{k-1}}\\ \frac{\∂z_{k-1}}{\∂X_{k-1}}\end{array}\right]=\left[\begin{array}{cccccc}a& b& c& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

其中：

$\begin{array}{cc}a=\frac{x_{k-1}-x_{b_{k-1}}}{\sqrt{{(x_{k-1}-x_{b_{k-1}})}^2+{(y_{k-1}-y_{b_{k-1}})}^2+{z^2}_{k-1}}}& b=\frac{y_{k-1}-y_{b_{k-1}}}{\sqrt{{(x_{k-1}-x_{b_{k-1}})}^2+{(y_{k-1}-y_{b_{k-1}})}^2+{z^2}_{k-1}}}\end{array}$

$c=\frac{z_{k-1}}{\sqrt{{(x_{k-1}-x_{b_{k-1}})}^2+{(y_{k-1}-y_{b_{k-1}})}^2+{z^2}_{k-1}}}$

得到系统的离散化观测模型为Z_k＝H_kX_k+v_k；

F.对系统进行卡尔曼滤波处理：

①先验估计：

${\hat{X}}_k^{-}=A{\hat{X}}_{k-1}^{-}+{\mathrm{BU}}_k$

②更新协方差P：

其中Q为w_k的协方差矩阵；

③计算先验估计对应的距离：

$l_k^{-}=\sqrt{{(x_k^{-}-x_{b_k}^{-})}^2+{(y_k^{-}-y_{b_k}^{-})}^2+{\left(z_k^{-}\right)}^2}$

④计算H矩阵：

$H_k=\left[\begin{array}{cccccc}a& b& c& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

⑤计算卡尔曼增益，其中R是v_k的协方差矩阵：

$Kg=\frac{{P_k}^{-}{H_k}^{{-}^{\′}}}{{H_k}^{-}{P_k}^{-}{H_k}^{{-}^{\′}}+R}$

⑥后验估计：

$X_k={X_k}^{-}+Kg(\left[\begin{array}{c}{r}_k\\ z_k\end{array}\right]-\left[\begin{array}{c}{l_k}^{-}\\ {z_k}^{-}\end{array}\right])$

⑦更新协方差：

$P_k=(I-{{\mathrm{KgH}}_k}^{-}){P_k}^{-}.$

2.如权利要求1所述的基于单信标的水下移动目标定位算法，其特征在于：所述获得水下移动目标初始位置的方法为：

假定信标为固定，且以信标所在位置为原点，建立坐标系；

(1)假定水下移动目标的初始位置：记为P₀，此时水下移动目标与y轴件的夹角记为水下移动目标与信标之间的距离为r₁，时刻记为t₁；

(2)忽略洋流流速，根据水下移动目标的运动速度，获得t₂时刻水下移动目标的航位推算向量记为a，从而获得t₂时刻水下移动目标的理想位置，记为P_M；

(3)考虑洋流流速，获得t₂时刻水下移动目标的真实位置，记为P₁，此时水下移动目标与y轴间的夹角记为水下移动目标与信标之间的距离为r₂，时刻记为t₂；

(4)比较P₁与P_M之间的位置，获得洋流向量b；

(5)从P₁点开始，根据水下移动目标的运动速度，获得t₃时刻水下移动目标的航位推算向量记为c，加上(4)得到的洋流向量b，获得t₃时刻水下移动目标的理想位置，记为P₃，此时水下移动目标与信标之间的距离为r₄；

(6)由信标与航行器之间的测距信息，获得t₃时刻水下移动目标与信标之间的距离r₃，其中P₂为根据测距信息任意选的一个位置；

(7)计算r₃与r₄之间的差，记为r；

(8)将和分别由0～360°遍历，得到一系列的r值，在所有的r值中，找到最小的r值对应的位置P₀和洋流向量b，则此时P₀为最接近水下移动目标初始位置的初始状态。

3.如权利要求2所述的基于单信标的水下移动目标定位算法，其特征在于：假定信标为固定，且以信标所在位置为原点，将水下移动目标所在的位置置于第一象限，建立坐标系，将和分别由0～90°遍历。