一种基于姿态确定的USBL安装误差角标定方法技术

一种基于姿态确定的USBL安装误差角标定方法，超短基线定位系统在水下航行器定位中有着重要的应用。而USBL定位系统的安装误差角对USBL定位精度有重要影响。传统的标定方法对安装误差角的估计精度有限，且对路线要求较高。本申请SINS和USBL在应用过程中固联在一起时，USBL的安装误差角是固定不变的，然后利用姿态确定的思想来完成USBL安装误差角的标定。首先建立基于安装误差角矩阵的矢量观测模型，通过构造观测向量和参考向量，该方法可以实时标定SINS和USBL的安装误差角。该方法的优点在于：该方法能实时标定出USBL安装误差角，并且操作简单，对标定路线没有具体要求；USBL定位系统在水听器接收基阵坐标系下的定位精度越高，该方法的标定精度也就越高。

A Calibration Method of USBL Installation Error Angle Based on Attitude Determination

【技术实现步骤摘要】
一种基于姿态确定的USBL安装误差角标定方法
本专利技术涉及水下定位
，特别是涉及一种基于姿态确定的USBL安装误差角标定方法。
技术介绍
自主水下航行器(AUV)是人类进行海洋资源调查研究不可缺少的载体，水下定位技术是水下机器人水下作业的关键技术之一。海洋的介质环境决定了声音比光波和电磁波更适合作为水下定位技术的传播载体。在现有的水下声学定位技术中，USBL系统以其结构简单、操作方便、体积小等优点，在海洋资源调查和科研领域发挥着越来越重要的作用。影响USBL定位系统定位精度的主要因素是系统误差、海洋环境参数引起的测量误差和接收阵列的安装误差。安装误差角是指水听器阵列架与惯性导航体架不一致造成的误差。超短基线接收基阵的安装误差是超短基线定位系统的主要误差源，在实际使用前必须对其进行精确标定。因此，研究超短定位系统安装误差角的标定方法，对于提高超短基线系统定位精度具有重要的意义。虽然安装误差的标定对提高超短基线定位系统的定位精度起着重要作用，但安装误差角标定方法相关发表的文献却很少。2007年，哈尔滨工程大学郑翠娥提出了一种利用最小二乘法估计超短基线定位系统安装误差的标定方法，并进行了海上试验。然而，当该方法用最小二乘法求解安装误差角时，需要对矩阵进行求逆运算，这样会使得该算法存在奇异值的情况。2013年，ChenHsin-Hung设计了一种迭代标定方法，通过分析三个角度值分别对USBL定位的影响来估计安装误差角。但这种标定方法原理过于理想，标定过程中对路线要求过于复杂。因此，本文提出的标定方法可以实时的标定出超短基线定位系统的安装误差角，且对标定路线没有要求。
技术实现思路
为了解决现有标定方法技术中标定精度的不足以及标定路线要求高的缺点，本专利技术提供一种基于姿态确定的USBL安装误差角标定方法，通过具有RTK定位精度的GPS位置来修正SINS的姿态误差，在事先通过LBL计算得到应答器位置的条件下，构造基于USBL安装误差角矩阵的矢量观测模型，利用姿态确定方法求解出USBL安装误差角，为达此目的，本专利技术提供一种基于姿态确定的USBL安装误差角标定方法，其具体步骤如下：(1)超短基线系统由安装在AUV上的四个水听器组成的正方形基阵和布放在海底的应答器组成，换能器发送声波信号，应答器接收信号后经过一段延时发送应答信号，四个水听器接收到应答信号后，根据声波到达时间确定水听器与应答器间的斜距信息，以及声波与水听器基阵间的两个方位角信息；(2)利用具有RTK固定解定位精度的GPS位置信息与捷联惯性导航系统SINS进行松组合算法，以此来修正SINS的姿态角误差，从而为USBL标定方法提供更加精确的姿态矩阵(3)构造基于USBL安装误差角姿态矩阵的矢量观测模型，利用超短基线定位系统构造水声基阵坐标系下的位置矢量，利用SINS/RTK组合导航系统的位置和姿态构造载体系下的位置矢量；(4)根据步骤(3)构造的基于USBL安装误差角姿态矩阵的矢量观测模型，利用姿态确定方法求解出USBL安装误差角对应的姿态四元数，从而完成USBL安装误差角的实时标定。作为本专利技术进一步改进，所述步骤(2)中基于RTK固定解的SINS/GPS松组合导航方法的具体步骤为：(21)状态量X由以下15个变量组成；其中，φE，φN，φU是捷联惯导的三个失准角，δVE，δVN，δVU是东北天三个方向上的速度误差，δL，δλ，δh是捷联惯导的三个位置误差在导航系下的描述，εx，εy，εz是陀螺的三个轴向漂移，是加速度计三个轴向的零偏误差；(22)系统状态方程为；其中，X(t)为系统的状态向量，F(t)是惯导系统误差状态方程状态转移矩阵，W(t)是关于捷联惯导系统的噪声向量；(23)以RTK位置与SINS位置差作为观测量，建立系统观测方程为；Z(t)＝PSINS-PGPS＝H(t)X(t)+V(t)其中，Z(t)为观测方程的量测向量，PSINS为SINS得到的位置，PGPS为RTK位置，H(t)为观测方程的量测矩阵，V(t)为观测方程的量测噪声向量；其中量测矩阵的具体表达式为；H(t)＝[03×303×3I3×303×303×3]其中I3x3表示3阶单位矩阵。作为本专利技术进一步改进，所述步骤(3)中构造基于USBL安装误差角姿态矩阵的矢量观测模型的具体步骤为：(31)超短基线坐标系记为a系，捷联惯导载体坐标系记为b系，ra表示应答器在超短基线坐标系下的相对位置，可以通过超短基线定位系统的斜距及方位角信息求解得到，具体计算公式为，其中R表示应答器与水听器接收基阵之间的斜距信息，α，β分别为水听器与应答器之间的声波信号与超短基线坐标系x，y轴的方位角；(32)利用不同时刻计算得到的ra(t)作差，得到应答器在a系下相对位置的变化量具体计算公式为：(33)LBL计算得到的应答器在导航系下的位置为利用SINS位置和姿态，求解得到应答器在b系下的相对位置rb，具体计算方式为：其中，表示b系与n系之间的姿态矩阵，表示SINS在导航系下的位置。(34)建立rb与rb之间的关系表达式为：其中，表示b系与a系之间的姿态矩阵，Lb表示超短基线坐标系原点与载体系原点之间的杆臂值在载体系下的投影；(35)通过将不同时刻计算得到的rb(t)-Lb作差，得到应答器在b系下相对位置的变化量(36)通过步骤(32)构造的水声基阵坐标系下的位置矢量及步骤(35)构造的载体坐标系下的位置矢量建立基于USBL安装误差角的矢量观测模型如下：作为本专利技术进一步改进，所述步骤(4)中利用姿态确定方法求解USBL安装误差角对应的姿态四元数的具体步骤为：(41)通过步骤(36)建立的基于USBL安装误差角的矢量观测模型，建立对应的基于USBL安装误差姿态四元数的矢量观测模型：其中，为USBL安装误差角姿态矩阵对应的姿态四元数。表示四元数乘法运算。(42)将观测矢量和参考矢量简记为并构造两个矩阵如下式：(43)通过步骤(42)构造的两个矩阵，步骤(14)中的矢量观测模型转化为上式中构造的观测矢量模型可以通过最小二乘约束求得其中，则K矩阵最小特征值对应的特征向量即为USBL安装误差角姿态四元数与现有技术相比，本专利技术具有如下优点：(1)相比于传统标定方法的事后处理方式，本方法既可以通过事后处理的方式求解出安装误差角，也可以通过实时构造观测矢量及参考矢量来实时计算出USBL安装误差角。(2)相比于传统方法中要求标定路线为圆形或者直线型路线，本方法求解安装误差角所需的观测矢量及参考矢量在任意轨迹条件下都能构造得到，因此，本方法在任意路线条件下都能完成超短基线安装误差角的标定。(3)USBL定位系统计算得到的应答器在声学基阵坐标系下的定位精度越高，观测矢量构造的精度也就越高，则本方法标定的安装误差角精度越高。进一步地，USBL计算得到的载体在导航系下的定位精度也就越高。这是一个逐次递进的效果。附图说明图1为本专利技术的标定方法结构示意图；图2为本专利技术中标定方法整体流程图。具体实施方式下面结合附图与具体实施方式对本专利技术作进一步详细描述：本专利技术提供一种基于姿态确定的USBL安装误差角标定方法，通过具有RTK定位精度的GPS位置来修正SINS的姿态误差，在事先通过LBL计算得到应答器位置的条件下，构造基于USBL安装误本文档来自技高网...

【技术保护点】
1.一种基于姿态确定的USBL安装误差角标定方法，其特征在于，其具体步骤如下：(1)超短基线系统由安装在AUV上的四个水听器组成的正方形基阵和布放在海底的应答器组成，换能器发送声波信号，应答器接收信号后经过一段延时发送应答信号，四个水听器接收到应答信号后，根据声波到达时间确定水听器与应答器间的斜距信息，以及声波与水听器基阵间的两个方位角信息；(2)利用具有RTK固定解定位精度的GPS位置信息与捷联惯性导航系统SINS进行松组合算法，以此来修正SINS的姿态角误差，从而为USBL标定方法提供更加精确的姿态矩阵

【技术特征摘要】
1.一种基于姿态确定的USBL安装误差角标定方法，其特征在于，其具体步骤如下：(1)超短基线系统由安装在AUV上的四个水听器组成的正方形基阵和布放在海底的应答器组成，换能器发送声波信号，应答器接收信号后经过一段延时发送应答信号，四个水听器接收到应答信号后，根据声波到达时间确定水听器与应答器间的斜距信息，以及声波与水听器基阵间的两个方位角信息；(2)利用具有RTK固定解定位精度的GPS位置信息与捷联惯性导航系统SINS进行松组合算法，以此来修正SINS的姿态角误差，从而为USBL标定方法提供更加精确的姿态矩阵(3)构造基于USBL安装误差角姿态矩阵的矢量观测模型，利用超短基线定位系统构造水声基阵坐标系下的位置矢量，利用SINS/RTK组合导航系统的位置和姿态构造载体系下的位置矢量；(4)根据步骤(3)构造的基于USBL安装误差角姿态矩阵的矢量观测模型，利用姿态确定方法求解出USBL安装误差角对应的姿态四元数，从而完成USBL安装误差角的实时标定。2.根据权利要求1所述的一种基于姿态确定的USBL安装误差角标定方法，其特征在于：所述步骤(2)中基于RTK固定解的SINS/GPS松组合导航方法的具体步骤为：(21)状态量X由以下15个变量组成；其中，φE，φN，φU是捷联惯导的三个失准角，δVE，δVN，δVU是东北天三个方向上的速度误差，δL，δλ，δh是捷联惯导的三个位置误差在导航系下的描述，εx，εy，εz是陀螺的三个轴向漂移，是加速度计三个轴向的零偏误差；(22)系统状态方程为；其中，X(t)为系统的状态向量，F(t)是惯导系统误差状态方程状态转移矩阵，W(t)是关于捷联惯导系统的噪声向量；(23)以RTK位置与SINS位置差作为观测量，建立系统观测方程为；Z(t)＝PSINS-PGPS＝H(t)X(t)+V(t)其中，Z(t)为观测方程的量测向量，PSINS为SINS得到的位置，PGPS为RTK位置，H(t)为观测方程的量测矩阵，V(t)为观测方程的量测噪声向量；其中量测矩阵的具体表达式为；H(t)＝[03×303×3I3×303×3...

【专利技术属性】
技术研发人员：张涛，朱永云，张亮，金博楠，翁铖铖，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32