申请/专利权人：北京自动化控制设备研究所

申请日：2015-09-01

公开（公告）日：2017-03-08

公开（公告）号：CN106483498A

主分类号：G01S5/02(2010.01)I

分类号：G01S5/02(2010.01)I;G01S5/12(2006.01)I;G01C21/16(2006.01)I

优先权：

专利状态码：失效-发明专利申请公布后的驳回

法律状态：2019.08.30#发明专利申请公布后的驳回;2017.04.05#实质审查的生效;2017.03.08#公开

摘要：本发明公开了一种SINSUSBL紧耦合算法。本发明属于紧耦合导航算法技术领域，具体涉及一种多应答器SINSUSBL组合导航算法。本发明技术方案设计了一种新型的多应答器SINSUSBL组合导航方案；并对多应答器SINSUSBL紧耦合导航算法进行误差建模与仿真分析。本发明能够避免USBL测角误差和USBL到SINS安装角误差等对定位精度带来的影响，且无需对USBL到SINS的安装角进行标定，使用更加方便。仿真结果表明，本发明能够获得比现有单应答器SINSUSBL组合导航算法更高的定位精度。

主权项：一种SINSUSBL紧耦合算法，适用于多应答器SINSUSBL组合导航，其特征在于，包括以下步骤：1确定超短基线定位系统USBL原理三应答器USBL的基本工作原理是以三个位置已知的应答器到基阵中心空间距离为半径的球面交会；i＝1,2,3表示三个应答器，其在地球直角坐标系中的位置为USBL测得的三个斜距值为Ri以及基阵位置为Xe,Ye,Ze，根据空间球面交会原理，得到如下的参数方程组：式1利用事先标定测量得到的应答器位置和USBL实时测量的斜距值，求解该方程组得到基阵的绝对位置；2构建紧耦合导航算法滤波模型选取惯导载体坐标系b为前上右坐标系，导航坐标系n为北天东坐标系；选取传统15维惯导系统误差状态量，速度误差δVn＝[δVN δVU δVE]T，姿态误差φn＝[φN φU φE]T，位置误差陀螺漂移ε＝[εx εy εz]T，加表零位▽＝[▽x ▽y ▽z]T；相应捷联惯导系统SINS误差状态量为：式2捷联惯导系统误差状态方程为：式3将USBL测距精度误差扩展为系统状态：XUSBL3＝[δKu1 δKu2 δKu3]T            式4USBL测距精度误差δKui视为随机常数，i＝1、2、3，USBL部分对应的卡尔曼滤波状态方程为：式5其中：FUSBL3＝[03×3]          式618维的三应答器SINSUSBL紧耦合导航算法误差状态量为：X＝[XSINS XUSBL3]T         式7SINSUSBL组合导航的卡尔曼滤波状态方程为：式8其中，G为系统噪声矩阵，W为系统噪声，F为状态转移矩阵：式9公式9中，FSINS为惯导部分系统矩阵；3构建紧耦合导航算法观测模型以SINS相对三个应答器的距离信息和USBL直接测量得到的载体相对于三个应答器的斜距信息之差为观测量；在地球直角坐标系中，利用惯导位置Xs,Ys,Zs和三个应答器位置构造相对距离为：式10对式10求偏微分：式11式12应答器i的位置为惯导解算的位置为利用地球定位中球面坐标系到直角坐标系的转换公式，二者在地球直角坐标系的相对位置为式13其中，RN表示地球卯酉圈曲率半径，将式13化简得：式14联立式12和式14得：式15其中：USBL实际测距表示USBL测量得到的惯导位置和第i个答器之间的距离，真实距离Ri表示实际的惯导位置和第i个答器之间的距离；USBL实际测距真实距离Ri和USBL测距精度误差δKui之间有如下关系：式164确定三应答器SINSUSBL紧耦合导航算法的观测量为：式17三应答器SINSUSBL紧耦合导航算法观测方程为：式18三应答器SINSUSBL紧耦合导航算法观测矩阵为：H＝[03×6 AsBs 03×6 diag[R1 R2 R3]]        式19三应答器SINSUSBL紧耦合导航算法量测方程为：Z＝HX+v              式205确定紧耦合导航算法滤波方程对式8和式20进行离散化处理，得到SINSUSBL组合导航的卡尔曼滤波模型为：式21其中k和k+1表示时刻。

全文数据：一种SINSUSBL紧耦合算法技术领域[0001]本发明属于紧耦合导航算法技术领域，具体涉及一种多应答器SINSUSBL组合导航算法。背景技术[0002]捷联惯导系统（SINS能够为水下载体提供完备导航信息，但其误差随时间积累；超短基线（USBL定位系统操作简单、便携性强、成本较低，能够测量应答器相对于基阵的相对位置信息（包括角度信息和斜距信息），可作为辅助导航系统，用来抑制惯导系统误差的发散。SINSUSBL组合导航能够集成二者的优点，实现互补。[0003]现有USBL设备的普遍特点是测距精度高而测角精度相对较低。此外，常规的单应答器SINSUSBL组合导航系统使用前都要预先标定USBL到SINS的安装角，准备工作繁琐。国内外现有的单应答器SINSUSBL组合导航方案，其定位精度很大程度上受到USBL测角误差以及二者安装角误差的限制，不能满足部分水下高精度导航的需求。[0004]因此，亟需研制一种SINSUSBL紧耦合算法，以解决SINSUSBL组合导航时存在的不足。发明内容[0005]本发明要解决的技术问题是提供一种SINSUSBL紧耦合算法，以获得比现有单应答器SINSUSBL组合导航算法更高的定位精度。[0006]为了实现这一目的，本发明采取的技术方案是：[0007]-种SINSUSBL紧耦合算法，适用于多应答器SINSUSBL组合导航，包括以下步骤：[0008]1确定超短基线定位系统USBL原理[0009]三应答器USBL的基本工作原理是以三个位置已知的应答器到基阵中心空间距离为半径的球面交会；i=1，2,3表示三个应答器，其在地球直角坐标系中的位置为、USBL测得的三个斜距值为民以及基阵位置为（Xe，Γ，Ze，根据空间球面交会原理，得到如下的参数方程组：[0011]利用事先标定测量得到的应答器位置和USBL实时测量的斜距值，求解该方程组得到基阵的绝对位置；[0012]2构建紧耦合导航算法滤波模型[0013]选取惯导载体坐标系b为前上右坐标系，导航坐标系η为北天东坐标系；[0014]选取传统15维惯导系统误差状态量，速度误差δνη=[δνΝδν,δνΕ]τ，姿态误差，位置误差~^OSBLl·^-υ$ΒΙ'Λ+^VSBli^VSBli式5[0023]其中：[0024]FUSBL3=[03X3]式6[0025]18维的三应答器SINSUSBL紧耦合导航算法误差状态量为：[0026]X-[XsiNSX|JSBL3]式7[0027]SINSUSBL组合导航的卡尔曼滤波状态方程为：[0028]X^FX+GW式8[0029]其中，G为系统噪声矩阵，W为系统噪声，F为状态转移矩阵：式9[0031]公式（9中，FsinsS惯导部分系统矩阵；[0032]3构建紧耦合导航算法观测模型[0033]以SINS相对三个应答器的距离信息和USBL直接测量得到的载体相对于三个应答器的斜距信息之差为观测量；[0034]在地球直角坐标系中，利用惯导位置（xs，Ys，zs和三个应答器位置构造相对距离为：[0036]对式10求偏微分："式10式11[0039]应答器i的位置为」，惯导解算的位置为k利用地球定位中球面坐标系到直角坐标系的转换公式，二者在地球直角坐标系的相对位置为，：··[0041]其中，RN表示地球卯酉圈曲率半径，将式（13化简得：[0043]联立式12和式14得：[0047]USBL实际测距！,表示USBL测量得到的惯导位置和第i个答器之间的距离，真实距离Ri表示实际的惯导位置和第i个答器之间的距离；USBL实际测距々，真实距离民和USBL测距精度误差δΚ+之间有如下关系：式16[0049]4确定三应答器SINSUSBL紧耦合导航算法的观测量为：式17[0051]三应答器SINSUSBL紧耦合导航算法观测方程为：式18[0053]三应答器SINSUSBL紧耦合导航算法观测矩阵为：[0054]H=[03X6ASBS03X6diagtRiR2R3]]式19[0055]三应答器SINSUSBL紧耦合导航算法量测方程为：[0056]Z=HX+v式20[0057]5确定紧耦合导航算法滤波方程[0058]对式（8和式（20进行离散化处理，得到SINSUSBL组合导航的卡尔曼滤波模型为：[0059][0060]其中k和k+Ι表示时刻。[0061]进一步的，如上所述的一种SINSUSBL紧耦合算法，用于多应答器SINSUSBL组合导航，应答器数量大于3个时，i=1，2,3…η，η3,采用步骤（1~（5的方法确定SINSUSBL紧耦合算法。[0062]本发明技术方案的有益效果在于：通过采用标准卡尔曼滤波算法基本方程，选择合适的滤波初值（状态初始值，初始估计均方误差阵，系统噪声初始方差阵以及量测噪声方差阵），进行滤波估计，并实时修正速度、姿态和位置信息，；设计了一种新型的多应答器SINSUSBL组合导航方案；并对多应答器SINSUSBL紧耦合导航算法进行误差建模与仿真分析。本发明能够避免USBL测角误差和USBL到SINS安装角误差等对定位精度带来的影响，且无需对USBL到SINS的安装角进行标定，使用更加方便。仿真结果表明，本发明能够获得比现有单应答器SINSUSBL组合导航算法更高的定位精度。具体实施方式[0063]下面结合具体实施例对本发明技术方案进行详细说明。[0064]本发明一种SINSUSBL紧耦合算法，适用于多应答器SINSUSBL组合导航，包括以下步骤：[0065]1确定超短基线定位系统USBL原理[0066]三应答器USBL的基本工作原理是以三个位置已知的应答器到基阵中心空间距离为半径的球面交会；i=1，2,3表示三个应答器，其在地球直角坐标系中的位置为、USBL测得的三个斜距值为民以及基阵位置为（Xe，Γ，Γ，根据空间球面交会原理，得到如下的参数方程组：式⑴[0068]利用事先标定测量得到的应答器位置和USBL实时测量的斜距值，求解该方程组得到基阵的绝对位置；[0069]2构建紧耦合导航算法滤波模型[0070]选取惯导载体坐标系b为前上右坐标系，导航坐标系η为北天东坐标系；[0071]选取传统15维惯导系统误差状态量，速度误差δνη=[δνΝδν,δνΕ]τ，姿态误差Φη=[ΦΝΦυΦΕ]Τ，位置误差研,=[%袖叫、陀螺漂移ε=[εχεζ]τ，加表零位v=[vrvrν,]Γ；[0072]相应捷联惯导系统SINS误差状态量为：式⑵[0074]捷联惯导系统误差状态方程为：[0075]xsms=FSINSXS1NS+GsmWms式（3[0076]将USBL测距精度误差扩展为系统状态：[0077]XUSBL3=[δKu!δKu2δKu3]τ式（4[0078]USBL测距精度误差δKuJift为随机常数，i=1、2、3，USBL部分对应的卡尔曼滤波状态方程为：[0079]XvSBLi=+GlJSBLiWVSBL,式（5[0080]其中：[0081]FUSBL3=[03X3]式（6[0082]18维的三应答器SINSUSBL紧耦合导航算法误差状态量为：[0083]X-[XsiNSX|JSBL3]式（7[0084]SINSUSBL组合导航的卡尔曼滤波状态方程为：[0085].v=FX+GW式⑶[0086]其中，G为系统噪声矩阵，W为系统噪声，F为状态转移矩阵：式（9[0088]公式（9中，FsinsS惯导部分系统矩阵；[0089]3构建紧耦合导航算法观测模型[0090]以SINS相对三个应答器的距离信息和USBL直接测量得到的载体相对于三个应答器的斜距信息之差为观测量；[0091]在地球直角坐标系中，利用惯导位置（XS，YS，ZS和三个应答器位置构造相对距离为：式（10[0093]对式（10求偏微分：[0096]应答器i的位置为，惯导解算的位置为，利用地球定位中球面坐标系到直角坐标系的转换公式，二者在地球直角坐标系的相对位置为T：[0098]其中，RN表示地球卯酉圈曲率半径，将式（13化简得：[0100]联立式（12和式（14得：[0104]USBL实际测距元表示USBL测量得到的惯导位置和第i个答器之间的距离，真实距离民表示实际的惯导位置和第i个答器之间的距离；USBL实际测距々，真实距离民和USBL测距精度误差δΚ+之间有如下关系：[0105]R,=\+δΚίΐ;Η,式（16[0106]4确定三应答器SINSUSBL紧耦合导航算法的观测量为：[0108]三应答器SINSUSBL紧耦合导航算法观测方程为：[0110]三应答器SINSUSBL紧耦合导航算法观测矩阵为：[0111]H=[03X6ASBS03X6diag[RiR2R3]]式（19[0112]三应答器SINSUSBL紧耦合导航算法量测方程为：[0113]Z=HX+v式（20[0114]5确定紧耦合导航算法滤波方程[0115]对式（8和式（20进行离散化处理，得到SINSUSBL组合导航的卡尔曼滤波模型为：[0116][0117]其中k和k+Ι表示时刻。[0118]本实施例中所述的一种SINSUSBL紧耦合算法，应答器数量为3个时，当应答器数量大于3个时，i=1，2,3…n，n3,采用步骤（1~（5的方法确定SINSUSBL紧耦合算法同样适用。

权利要求：1.一种SINSUSBL紧耦合算法，适用于多应答器SINSUSBL组合导航，其特征在于，包括以下步骤：1确定超短基线定位系统USBL原理三应答器USBL的基本工作原理是以三个位置已知的应答器到基阵中心空间距离为半径的球面交会；i=1，2,3表示三个应答器，其在地球直角坐标系中的位置为USBL测得的三个斜距值为民以及基阵位置为（Xe，Γ，Ze，根据空间球面交会原理，得到如下的参数方程组：利用事先标定测量得到的应答器位置和USBL实时测量的斜距值，求解该方程组得到基阵的绝对位置；2构建紧耦合导航算法滤波模型选取惯导载体坐标系b为前上右坐标系，导航坐标系η为北天东坐标系；选取传统15维惯导系统误差状态量，速度误差，姿态误差Φη，位置误差，陀螺漂移，加表零位▽相应捷联惯导系统SINS误差状态量为：捷联惯导系统误差状态方程为：将USBL测距精度误差扩展为系统状态：USBL测距精度误差δΚιι^Ι为随机常数，i=1、2、3，USBL部分对应的卡尔曼滤波状态方程为：18维的三应答器SINSUSBL紧耦合导航算法误差状态量为：SINSUSBL组合导航的卡尔曼滤波状态方程为：其中，G为系统噪声矩阵，W为系统噪声，F为状态转移矩阵：公式（9中，FsinsS惯导部分系统矩阵；3构建紧耦合导航算法观测模型以SINS相对三个应答器的距离信息和USBL直接测量得到的载体相对于三个应答器的斜距信息之差为观测量；在地球直角坐标系中，利用惯导位置（Xs，Ys，Zs和三个应答器位置构造相对距离为：应答器i的位置为，惯导解算的位置为，利用地球定位中球面坐标系到直角坐标系的转换公式，二者在地球直角坐标系的相对位置为:：：:联立式（12和式（14得：USBL实际测距f:i表示USBL测量得到的惯导位置和第i个答器之间的距离，真实距离民表示实际的惯导位置和第i个答器之间的距离；USBL实际测距，真实距离民和USBL测距精度误差SK+之间有如下关系：三应答器SINSUSBL紧耦合导航算法观测矩阵为：H=[03X6ASBS03X6diagt^R2R3]]式（19三应答器SINSUSBL紧耦合导航算法量测方程为：Z=HX+v式（205确定紧耦合导航算法滤波方程对式（8和式（20进行离散化处理，得到SINSUSBL组合导航的卡尔曼滤波模型为：其中k和k+Ι表示时刻。2.如权利要求1所述的一种SINSUSBL紧耦合算法，其特征在于，用于多应答器SINSUSBL组合导航，应答器数量大于3个时，i=1，2,3…η，η3,采用步骤（1~（5的方法确定SINSUSBL紧耦合算法。

百度查询： 北京自动化控制设备研究所 一种SINSUSBL紧耦合算法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD