基于单信标的水下移动目标定位算法
【专利摘要】基于单信标的水下移动目标定位算法，其特征在于：采用一个水上移动信标来进行水下移动目标的定位；所述移动信标配备有卫星定位接收器，水下移动目标配备有水平姿态传感器、航速传感器和垂直方位传感器。通过此技术可以获得水下目标与信标的直线距离，同时根据水下目标(自身携带的传感器获得水下目标的一些姿态、速度、深度测量值)。在距离与这些测量值的基础上通过建立系统模型、求解初始状态、状态可观性分析、状态方程离散化、卡尔曼滤波等步骤最终获得水下目标的位置估计值。
【专利说明】
基于单信标的水下移动目标定位算法
技术领域
[0001] 本发明属于定位导航技术领域，设及一种水下移动目标的定位技术，具体的说，设 及一种基于单信标的水下移动目标定位算法。
【背景技术】
[0002] 水下定位导航技术是海洋科学考察、海洋资源勘探、海洋资源开发、深海空间站建 设等工程的必备手段，而水下电磁波信号衰减快，陆地上的GI^信号无法直接应用于水下目 标定位，所W水声定位技术是目前水下定位系统的主要研究方向。
[0003] 传统上，依据水下定位信标之间的距离，我们将水下定位系统分3种类型：长基线 定位系统化化）、短基线定位系统（S化）W及超短基线定位系统(USBL)。传统方法放置的基 元多，操作复杂，成本高昂。多个航行器可W接收导航信息速率随水中航行器数目增加而减 少，而且导航区域限制在一定的范围内，因此传统方法实现有界误差的导航缺乏可扩展性。 目前最先进、准确的惯性定位系统（1]1日的1日1化￥1肖日1:;[0]18731日1]1,^5)也有大约每小时几 公里的偏移率。在INS中增加一个多普勒声响(Doppler-Velocity Sonar，DVS)用于测量航 行器相对于地球的速度，可W将该误差减少到航行距离的0.01%。但是只有潜艇和大型无 人水下航行器(UUVs)配备INS和DVS技术，较小的航行器受空间和能源限制无法装备。大部 分小型航行器使用的是最基本的导航推算技术。运种技术需要航行器定期到水面更新位置 信息，运样就加大了航行器的工作负担。
【发明内容】
[0004] 本发明的目的在于针对小型水下移动目标，提供一种导航效果好、定位准确的基 于单信标的水下移动目标的定位算法。
[0005] 本发明的技术方案为:基于单信标的水下移动目标定位算法，采用一个水上移动 信标来进行水下移动目标的定位;移动信标配备有卫星定位接收器，水下移动目标配备有 水平姿态传感器、航速传感器和垂直方位传感器，包括W下步骤：
[0006] A、建立水下坐标系，建立水下移动目标的运动数学模型：
[0007]
其中，X，y，z为水下移动目标的位置，V为水下移动目标的 运动速度，Ve为洋流速度;Θ为水下移动目标的航向角，0为水下移动目标的俯仰角(水下移 动目标航行方向与水平方向的夹角，可通过水平姿态传感器得到）；
[000引B、获得水下移动目标的初始位置；
[0009] C、建立水下移动目标的状态模型，并定义观测模型：
[0010] 假定洋流的流速V。为恒定值，为一个常值;航向角Θ和航速V测量已经过滤波误差 为零;距离r测量噪声为高斯白噪声；
[0011] x、y、z分别对应Ξ维坐标系下X轴、y轴和z轴的状态分量；
[0016] 定义移动信标的坐标为(xb，yb，0)，得到水下移动目标与移动信标之间的距离测量 值：
Z为水下移动目标所处的水深；
[0017] 定义观测模型为
[0018] 得到状态方程
[0019] D、对系统模型进行可观性分析：
[0020] 令U = V,将B转换为:./|'=[sin6^cos?/?cos9cos口slru/?0 0 0]^
[0021 ]用
代替;r，h2 = z;
[0022] 将航向角作为测量值，h3 = 0;
[0023] hi的零阶李导数为：
[0024]
[0025] 其中zc为Z方向洋流对航行器作用的位移。
[0026] hi的一阶李导数为：
[0037]取出0中前Ξ行构成矩阵化，如果化满秩说明系统中所有的位置量可观。
[003引对化进行行变换和列变换，得到：
[0039]
[0040] 得到化不满秩的几种情况
[0041 ]根据可观性分析，得到如下结论：
[0042] (1)一般情况下，系统可观；
[0043] (2)航行器在过巧自平面上运动时，系统不可观；
[0044] (3)当
时，即航行器的航向过航行器与信标之间连线时，系统不可观；
[0045] E、对系统模型进行离散化处理：
[0046] ①状态模型离散化：
[0047] 系统方程的状态转移矩阵夫；：
[004引其泰勒展开为：
[0049]
[0050] 由于A的二阶W上项都为0,〇(*，切)可^改写为：
[0化1 ]
[0052] X(t)可W表示为W下的形式：
[0化3]
[0054]定义离散化的周期为1，假定U前后的采样值相差不大，可得到离散化状态模型： [0化5] Xk = AXk-i+BUk-i+wk-i
[0化6] 离散化后，可得到新的A，B矩阵，其中A=巫（t) |t=T=i [0057]即：
[0化引
[0化9]②观察模型离散化：
[0060] 系
将其线性化，得到：
[0061]
[0062] 为间隔离散化变为：
[0063]
[0064] 测量值有距离r和深度Z，由观测向量
，得到化矩阵为
[0069] 得到系统的离散化观测模型为Zk =化Xk+vk;
[0070] F、对系统进行卡尔曼滤波处理：
[0071] ①先验估计：
[0072]
[0073] ②更新协方差P:
[0074] 巧-='作I / + 0，其中Q为wk的协方差矩阵；
[0075] ③计算先验估计对应的距离：
[0076]
[0077] ④计算Η矩阵；
[007引
[0079]⑤计算卡尔曼增益，其中R是vk的协方差矩阵：
[0085] Xk即为估计的水下移动目标的移动轨迹。
[0086] 优选的是:获得水下移动目标初始位置的方法为：
[0087] 假定信标为固定，且W信标所在位置为原点，建立坐标系；
[008引（1)假定水下移动目标的初始位置:记为Po,此时水下移动目标与y轴件的夹角记 为巧:，水下移动目标与信标之间的距离为ri，时刻记为ti;
[0089] (2)忽略洋流流速，根据水下移动目标的运动速度，获得t2时刻水下移动目标的航 位推算向量记为曰，从而获得t2时刻水下移动目标的理想位置，记为Pm;
[0090] (3)考虑洋流流速，获得t2时刻水下移动目标的真实位置，记为Pi，此时水下移动目 标与y轴间的夹角记为锭，水下移动目标与信标之间的距离为η，时刻记为t2;
[0091 ] (4)比较Pi与Pm之间的位置，获得洋流向量b;
[0092] (5)从Pi点开始，根据水下移动目标的运动速度，获得t3时刻水下移动目标的航位 推算向量记为C，加上(4)得到的洋流向量b，获得t3时刻水下移动目标的理想位置，记为P3， 此时水下移动目标与信标之间的距离为Γ4;
[0093] (6)由信标与航行器之间的测距信息，获得t3时刻水下移动目标与信标之间的距 离η，其中P2为根据测距信息任意选的一个位置；
[0094] (7)计算η与Γ4之间的差，记为。
[00巧](8)将闽和巧分别由0~360°遍历，得到一系列的r值，在所有的r值中，找到最小的 r值对应的位置Po和洋流向量b，则此时Po为最接近水下移动目标初始位置的初始状态。
[0096] 优选的是:假定信标为固定，且W信标所在位置为原点，将水下移动目标所在的位 置置于第一象限，建立坐标系，将約和巧2分别由0~90°遍历。
[0097] 本发明的有益效果为：
[0098] 本发明的算法是建立在单信标水声定位的基础上，该定位技术可应用于体型较小 且成本不高的水下航行器，可W解决传统定位方法中布设回收困难、成本高昂，W及航位推 算受到未知洋流影响导致误差发散的缺点。
[0099] 在此定位技术的基础上提出了一种水下移动目标的定位算法，通过此技术可W获 得水下目标与信标的直线距离(在未考虑到一些干扰的基础上），同时根据水下目标(航行 器）自身携带的传感器获得水下目标的一些姿态、速度、深度测量值。在距离与运些测量值 的基础上通过建立系统模型、求解初始状态、状态可观性分析、状态方程离散化、卡尔曼滤 波等步骤最终获得水下目标的位置估计值。
[0100] 其中信标可移动使得本方法的导航区域扩大，Ξ维模型的建立也减少了深度值估 计的误差。该算法经过仿真验证，定位导航效果良好，能够比较准确的获得水下目标的位置 轨迹，完成导航。
[0101] 单信标定位技术只需要单个信标，用于标定信标支持的区域内航行器与已知信标 的相对距离，信标所做的工作只是广播水声信号，同时将其真实的地理位置编码到水声信 号中。各航行器可W计算自身的地理位置，而不需要信标系统进行计算，其支持的航行器的 数量不受限制。该技术的基础就是航位推算技术，通过测量的距离对位置误差进行纠正，弥 补了定期更新位置信息的缺点，提高了航行器的续航能力。单个信标布设和回收远远比多 个信标方便，而且不存在多信标相对位置的限制。
【附图说明】
[0102] 图1为本系统在坐标系下模型图。
[0103] 图2为水下移动目标初始位置遍历原理图。
[0104] 图3为水下移动目标初始位置遍历流程图。 图4为水下移动目标航向角和俯仰角示意图。
【具体实施方式】
[0105] W下结合附图对本发明的【具体实施方式】进行进一步的描述。
[0106] 基于单信标的水下移动目标定位算法，采用一个水上移动信标来进行水下移动目 标的定位;用于标定信标支持的区域内航行器与信标的相对距离。移动信标配备有卫星定 位接收器，水声换能器及同步时钟系统，通过全球定位系统获得信标的位置信息，其位置信 息与时钟信息编码到水声信号中，信标的工作是广播水声信号。
[0107] 水下移动目标配备有水平姿态传感器、航速传感器和垂直方位传感器，水平姿态 传感器可W得到水下移动目标航行的俯仰角信息，垂直方位传感器可W得到水下移动目标 的水深位置信息，航速传感器可W获得水下移动目标的航速信息。
[0108] 如图1所示，P(x，y)对应水下移动目标的位置，对水下移动目标定位的算法包括W 下步骤：
[0109] A、建立水下坐标系，信标为移动目标，建立水下移动目标的运动数学模型：
[0110]
其中，x，y，z为水下移动目标的位置，V为水下移动目标的 航速，Ve为洋流速度；Θ为水下移动目标的航向角，0为水下移动目标的俯仰角(水下移动目 标航行方向与水平方向的夹角，可通过水平姿态传感器得到）；
[0111] B、获得水下移动目标的初始位置。
[0112] 由水声测距方法可W获得信标与水下航行器的初始距离值，但是只有距离难W确 定水下航行器的初始位置，因此需要采用遍历的方法来求得水下航行器的真实初始位置。 由于Z方向的位置信息可W通过测量得到，因此可将Ξ维的遍历简化成二维的遍历W减少 计算量。遍历的基本原理是:假定一个具体的角度，在此角度下根据距离获得航行器的初始 位置，利用航位推算与洋流信息推算出下一时刻航行器在此角度下的位置信息，由此位置 信息求得与信标的距离，与真实时刻的距离信息作比较，在所有角度中寻找一个最接近的 值，那么运个角度就是所估计的角度，便可W得到航行器的初始位置信息。具体方法为：
[0113] 如图2和图3所示，假定信标为固定，且W信标所在位置为原点，建立坐标系；
[0114] (1)假定水下移动目标的初始位置:记为Po,此时水下移动目标与y轴件的夹角记 为約，水下移动目标与信标之间的距离为ri，时刻记为ti;
[0115] (2)忽略洋流流速，根据水下移动目标的运动速度，获得t2时刻水下移动目标的航 位推算向量记为曰，从而获得t2时刻水下移动目标的理想位置，记为Pm;
[0116] (3)考虑洋流流速，获得t2时刻水下移动目标的真实位置，记为Pi，此时水下移动目 标与y轴间的夹角记为巧，水下移动目标与信标之间的距离为?，时刻记为t2;
[0117] (4)比较Pi与Pm之间的位置，获得洋流向量b;
[011引（5)从Pi点开始，根据水下移动目标的运动速度，获得t3时刻水下移动目标的航位 推算向量记为C，加上(4)得到的洋流向量b，获得t3时刻水下移动目标的理想位置，记为P3， 此时水下移动目标与信标之间的距离为Γ4;
[0119] (6)由信标与航行器之间的测距信息，获得t3时刻水下移动目标与信标之间的距 离η，其中P2为根据测距信息任意选的一个位置；
[0120] (7)计算η与Γ4之间的差，记为r;
[0121] (8)将灼和媽分别由0~360°遍历，得到一系列的r值，r值的个数与所取的遍历分 度有关。在所有的r值中，找到最小的r值对应的位置Po和洋流向量b，则此时Po为最接近水下 移动目柄;初始位置的初始状态。
[0122] 在0~360°遍历的过程中，产生的所有的点集中在W坐标系原点为中屯、，Wri为半 径的圆周上，遍历过程中会产生两个最小值，受误差和精度的影响，查找到的最小值可能会 落在真实位置的对称点上，因此，为了节省计算量，假定信标为固定，且W信标所在位置为 原点，将水下移动目标所在的位置置于第一象限，建立坐标系，将約和钱分别由0~90°遍 历。
[0123] C、建立水下移动目标的状态模型，并定义观测模型：
[0124] 假定洋流的流速V。为恒定值，为一个常值;航向角Θ和航速V测量已经过滤波误差 为零;距离r测量噪声为高斯白噪声;在上述假设基础下，建立状态模型，其中需要估计的量 有两个，洋流的流速V。和航行器的当前位置P。
[0125] x、y、z分别对应Ξ维坐标系下X轴、y轴和Z轴的状态分量；
[0126] 其中，令状态量
[0127] 乂 =先1： + 及化；
[012引对X求导数
由于V。为恒定值，?ζ,，户。均为0;
[0129] 因此，
巧W，得到W下A和Β，
[0130] 定义移动信标的坐标为(xb，yb，0)，得到水下移动目标与移动信标之间的距离测量 值：
[0131]
;z为水下移动目标所处的水深；
[0132] 定义观测模型为
，
[0133] 得到状态方程
[0134] D、对系统模型进行可观性分析:为了得到真实的航行轨迹，必须进行卡尔曼滤波， 而进行卡尔曼滤波的条件便是状态可观。由于上述系统为一个非线性系统，对于一个非线 性系统，进行可观性分析的方法便是李导数法。
[013引令U=V,将B转换为：J^j=[sm如os婷 c;os''0cos0 細疗 0 0 of
[0136] 月
杉代替:r，h2 = z;
[0137] 将航向角作为测量值，h3 = Θ;
[013引hi的零阶李导数为：
[0139]
[0140] 其中zc为Z方向洋流对航行器作用的位移。
[0141] hi的一阶李导数为：
[0150] 得到系统模型的观测矩阵0:
[0151]
[0152] 取出0中前Ξ行构成矩阵化，如果化满秩说明系统中所有的位置量可观。
[0153] 对化进行行变换和列变换，得到：
[0154]
[0155] 得到化不满秩的几种情筑
[0156] 在运几种情况下航行器的位置不可观。
[0157] Ε、对系统模型进行离散化处理:为了进行卡尔曼滤波，获得相关参数，需要对状态 方程中的状态模型和观测模型就行离散化处理。
[0158] ①状态模型离散化：
[0159] 系统方程的状态转移矩阵为曲;
[0160] 其泰勒展开为：
[0161]
[0162] 由于A的二阶W上项都为0,〇(*，切)可^改写为：
[0163]
[0164] X(t)可W表示为W下的形式：
[01 化]
[0166] 定义离散化的周期为1，假定U前后的采样值相差不大，可得到离散化状态模型：
[0167] Xk = AXk-i+BUk-i+wk-i
[016引离散化时，可得到新的A，B矩阵，其中A=巫（t) |t=T=i
[0169] 良p:
[0170]
[0171] ②观察模型离散化：
[0172]对
将其线性化，得到：
[0176]巧慢值有距离r和深度Z，由观测向量f，得至帖矩阵为 Z
[0181] 得到系统的离散化观测模型为Zk =化Xk+vk;
[0182] F、对系统进行卡尔曼滤波处理:在对系统进行可观性分析W及离散化处理后，便 可W进行卡尔曼滤波，使得状态向量X中的各项值逐渐逼近真实值。具体的卡尔曼滤波步骤 如下：
[0183] ①先验估计：
[0184] 玄杞+公巧-
[01化]②更新协方差P:
[0186] 带二进1-^:' +谷，其中Q为wk的协方差矩阵；
[0187] ③计算先验估计对应的距离：
[0191] ⑤计算卡尔曼增益，其中R是Vk的协方差矩阵：
[0192]
[0193] ⑥后验估计：
[0194]
[01巧]⑦更新协方差：
[0196]
[0197] Xk即为估计的水下移动目标的移动轨迹(状态量的离散化形式），Xk中包括所要估 计的航行器的位置量(x，y，z)和要估计的洋流分量速度(Vex，Vw，Vcz)。
【主权项】
1.基于单信标的水下移动目标定位算法，其特征在于:采用一个水上移动信标来进行 水下移动目标的定位;所述移动信标配备有卫星定位接收器，水下移动目标配备有水平姿 态传感器、航速传感器和垂直方位传感器，包括以下步骤： A、 建立水下坐标系，建立水下移动目标的运动数学模型：，其中，X，y，Z为水下移动目标的位置，V为水下移动目标的运动速 度，Vc为洋流速度;Θ为水下移动目标的航向角，r为水下移动目标的俯仰角； B、 获得水下移动目标的初始位置； C、 建立水下移动目标的状态模型，并定义观测模型： 假定洋流的流速V。为恒定值，为一个常值;航向角Θ和航速V测量已经过滤波误差为零； 距离r测量噪声为高斯白噪声； X、y、z分别对应三维坐标系下X轴、y轴和z轴的状态分量；定义移动信标的坐标为(Xb，yb，〇)，得到水下移动目标与移动信标之间的距离测量值：#为水下移动目标所处的水深；D、 对系统模型进行可观性分析： 令U = V，将B转换为： 用来代替r，h2 = z，将航向角作为测量值，h3 = 9，求出hi，h2和h3 的各阶李导数及梯度，得到系统模型的观测矩阵〇，取出0中前三行构成矩阵0:，如果⑴满秩说明系统中所有的位置量可观； E、 对系统模型进行离散化处理： ①状态模型离散化： 系统方程的状态转移矩阵为其泰勒展开为：由于A的二阶以上项都为0,〇(1如)可以改写为：x(t)可以表示为以下的形式：定义离散化的周期为1，假定U前后的采样值相差不大，可得到离散化状态模型： Xk=AXk-ι+BUk-ι+wk-1 离散化后，可得到新的A，B矩阵，其中②观察模型离散化：进行泰勒展开：将其线性化，得到：以1为间隔离散化变为：测量值有距离r和深度z，由观测向量，得到Hk矩阵为得到系统的离散化观测模型为Zk = HkXk+vk; F.对系统进行卡尔曼滤波处理： ① 先验估计： ② 更新协方差P:if =+ β，其中Q为概的协方差矩阵； ③ 计算先验估计对应的距离：④ 计算Η矩阵：⑤ 计算卡尔曼增益，其中R是vk的协方差矩阵：⑥ 后验估计：⑦ 更新协方差：2.如权利要求1所述的基于单信标的水下移动目标定位算法，其特征在于:所述获得水 下移动目标初始位置的方法为： 假定信标为固定，且以信标所在位置为原点，建立坐标系； (1) 假定水下移动目标的初始位置:记为P〇，此时水下移动目标与y轴件的夹角记为約， 水下移动目标与信标之间的距离为η，时刻记为t; (2) 忽略洋流流速，根据水下移动目标的运动速度，获得t2时刻水下移动目标的航位推 算向量记为a，从而获得〖 2时刻水下移动目标的理想位置，记为Pm; (3) 考虑洋流流速，获得^时刻水下移动目标的真实位置，记为Pi，此时水下移动目标与 y轴间的夹角记为约，水下移动目标与信标之间的距离为r2,时刻记为t2; (4) 比较丹与?(《之间的位置，获得洋流向量b; (5) WPi点开始，根据水下移动目标的运动速度，获得t3时刻水下移动目标的航位推算 向量记为c，加上(4)得到的洋流向量b，获得t 3时刻水下移动目标的理想位置，记为P3,此时 水下移动目标与信标之间的距离为Γ4; (6) 由信标与航行器之间的测距信息，获得t3时刻水下移动目标与信标之间的距离r3， 其中P 2为根据测距信息任意选的一个位置； (7) 计算Γ3与Γ4之间的差，记为r; (8) 将的和愁分别由0~360°遍历，得到一系列的r值，在所有的r值中，找到最小的r值对 应的位置P〇和洋流向量b，则此时Ρο为最接近水下移动目标初始位置的初始状态。3.如权利要求2所述的基于单信标的水下移动目标定位算法，其特征在于:假定信标为 固定，且以信标所在位置为原点，将水下移动目标所在的位置置于第一象限，建立坐标系， 将約和_分别由0~90°遍历。
【文档编号】G01C21/12GK105823480SQ201610158157
【公开日】2016年8月3日
【申请日】2016年3月18日
【发明人】周琳, 赵庆胜, 鹿鑫, 迟书凯
【申请人】中国海洋大学