一种引入径向速度的SINS/USBL松组合导航定位方法技术

一种引入径向速度的SINS/USBL松组合导航定位方法，它属于组合导航及水声定位技术领域。本发明专利技术解决了传统SINS/USBL松组合方法忽略基元与信标之间径向速度的测量导致SINS速度误差的估计精度受限，以及在USBL定位无效时，依靠SINS单独完成导航任务带来的导航误差累积的问题。本发明专利技术将USBL的径向速度测量信息引入SINS/USBL松组合中，能够在USBL定位无效时，使SINS保持组合工作方式，减小SINS的误差积累；通过将径向速度作为滤波观测量，能够提高SINS/USBL松组合滤波系统对SINS速度误差的估计精度，从而改善整体导航性能。本发明专利技术可以应用于组合导航及水声定位领域。

【技术实现步骤摘要】
一种引入径向速度的SINS/USBL松组合导航定位方法
本专利技术属于组合导航及水声定位
，具体涉及一种引入径向速度的SINS/USBL松组合导航定位方法。
技术介绍
传统的SINS(捷联惯导系统)/USBL(超短基线声学定位系统)松组合导航需要在USBL能够输出有效定位结果的条件下，以SINS和USBL计算的航行器位置之差作为组合导航滤波系统的观测量，该方法在USBL定位无效时，仅依靠SINS单独完成导航任务，容易使该段时间内的导航误差累积。径向速度与航行器的运动速度直接相关，描述了水下航行器与参考信标之间的距离变化率大小。USBL能够通过计算基元接收信号与发射信号间的多普勒频率，获得基元与信标之间的径向速度，而传统的SINS/USBL松组合导航忽略了USBL这一冗余测量，导致SINS/USBL松组合工作条件和SINS速度误差的估计精度受限。
技术实现思路
本专利技术的目的是为解决传统SINS/USBL松组合方法忽略基元与信标之间径向速度的测量导致SINS速度误差的估计精度受限，以及在USBL定位无效时，依靠SINS单独完成导航任务带来的导航误差累积的问题，而提出了一种引入径向速度的SINS/USBL松组合导航定位方法。本专利技术为解决上述技术问题采取的技术方案是：一种引入径向速度的SINS/USBL松组合导航定位方法，该方法包括以下步骤：步骤一、将USBL的声学基阵倒置安装在水下航行器上，将SINS的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标，信标在导航坐标系下的位置通过GPS获得；所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点on，xn轴指向地理东向，yn轴指向地理北向，zn轴垂直于xnonyn平面指向天向，xn轴、yn轴和zn轴构成右手坐标系；步骤二、建立基阵坐标系，并确定信标在基阵坐标系下的位置和各基元在基阵坐标系下的位置；所述基阵坐标系是指以声学基阵中心为原点oa，沿声学基阵平面指向水下航行器的艏向方向为ya轴，za轴垂直于基阵平面向上，xa轴与ya轴、za轴构成右手坐标系；步骤三、根据信标在基阵坐标系下的位置来判断USBL是否定位有效；若成立，则USBL定位有效；否则，USBL定位无效；其中，R是声信号在水下航行器和信标之间的传播距离；步骤四、根据信标在基阵坐标系下的位置，获得USBL在计算导航坐标系下对水下航行器的定位结果将称为水下航行器的位置测量值；SINS输出的水下航行器在计算导航坐标系下的位置为称为水下航行器的位置预测值，利用将各基元在基阵坐标系下的位置转换到计算导航坐标系下，获得各基元在计算导航坐标系下的位置；步骤五、根据SINS计算的水下航行器速度以及各基元在计算导航坐标系下的位置，获得各基元相对信标的径向运动速度预测值步骤六、USBL获得的各基元相对信标的径向运动速度测量值为若步骤三中判断USBL定位有效，则将与的差以及与的差作为观测量，建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；若步骤三中判断USBL定位无效，则将与的差作为观测量建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；步骤七、根据SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型原理，利用模型输出的状态估计值修正SINS的输出，并重置扩展卡尔曼滤波器状态；步骤八、重复步骤二至步骤七的过程，不断的对SINS的输出进行修正，获得修正后的导航输出结果。本专利技术的有益效果是：本专利技术提出了一种引入径向速度的SINS/USBL松组合导航定位方法，本专利技术将USBL的径向速度测量信息引入SINS/USBL松组合中，能够在USBL定位无效时，使SINS保持组合工作方式，减小SINS的误差积累；通过将径向速度作为滤波观测量，能够提高SINS/USBL松组合滤波系统对SINS速度误差的估计精度，从而改善整体导航性能。附图说明图1是引入径向速度的SINS/USBL松组合导航定位数据融合原理图；图2是基阵坐标系、载体坐标系和导航坐标系的示意图。具体实施方式具体实施方式一：如图1和图2所示，本实施方式所述的一种引入径向速度的SINS/USBL松组合导航定位方法，该方法包括以下步骤：步骤一、将USBL的声学基阵倒置安装在水下航行器上，将SINS的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标，信标在导航坐标系下的位置通过GPS获得；所述导航坐标系是指“东北天”(“ENU”)地理坐标系，该地理坐标系以水下航行器的质心为原点on，xn轴指向地理东向，yn轴指向地理北向，zn轴垂直于xnonyn平面指向天向，xn轴、yn轴和zn轴构成右手坐标系；步骤二、建立基阵坐标系，并确定信标在基阵坐标系下的位置和各基元在基阵坐标系下的位置；所述基阵坐标系是指以声学基阵中心为原点oa，沿声学基阵平面指向水下航行器的艏向方向为ya轴，za轴垂直于基阵平面向上，xa轴与ya轴、za轴构成右手坐标系；步骤三、根据信标在基阵坐标系下的位置来判断USBL是否定位有效；若成立，则USBL定位有效；否则，USBL定位无效；其中，R是声信号在水下航行器和信标之间的传播距离；步骤四、根据USBL计算的信标在基阵坐标系下的位置，获得USBL在计算导航坐标系下对水下航行器的定位结果将称为水下航行器的位置测量值；SINS输出的水下航行器在计算导航坐标系下的位置为称为水下航行器的位置预测值，利用将各基元在基阵坐标系下的位置转换到计算导航坐标系下，获得各基元在计算导航坐标系下的位置；根据SINS(捷联惯导系统)计算的水下航行器姿态而复现的导航坐标系称为计算导航坐标系n′；步骤五、根据SINS计算的水下航行器速度以及各基元在计算导航坐标系下的位置，获得各基元相对信标的径向运动速度预测值步骤六、USBL获得的各基元相对信标的径向运动速度测量值为若步骤三中判断USBL定位有效，则将与的差以及与的差作为观测量，建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；若步骤三中判断USBL定位无效，则将与的差作为观测量建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；步骤七、根据SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型原理，利用模型输出的状态估计值修正SINS的输出，并重置扩展卡尔曼滤波器状态；步骤八、重复步骤二至步骤七的过程，不断的对SINS的输出进行修正，获得修正后的导航输出结果。重置滤波状态变量是指，在修正捷联惯性导航输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。本实施方式中，超短基线声学基阵是由多个声信号接收换能器(基元)均匀分布的一个圆柱形设备。具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤二中，信标在基阵坐标本文档来自技高网...

【技术保护点】
1.一种引入径向速度的SINS/USBL松组合导航定位方法，其特征在于，该方法包括以下步骤：/n步骤一、将USBL的声学基阵倒置安装在水下航行器上，将SINS的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标，信标在导航坐标系下的位置通过GPS获得；/n所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点o

【技术特征摘要】
1.一种引入径向速度的SINS/USBL松组合导航定位方法，其特征在于，该方法包括以下步骤：
步骤一、将USBL的声学基阵倒置安装在水下航行器上，将SINS的陀螺组件和加速度计组件固联在水下航行器上，水面布放一个同步信标，信标在导航坐标系下的位置通过GPS获得；
所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点on，xn轴指向地理东向，yn轴指向地理北向，zn轴垂直于xnonyn平面指向天向，xn轴、yn轴和zn轴构成右手坐标系；
步骤二、建立基阵坐标系，并确定信标在基阵坐标系下的位置和各基元在基阵坐标系下的位置；
所述基阵坐标系是指以声学基阵中心为原点oa，沿声学基阵平面指向水下航行器的艏向方向为ya轴，za轴垂直于基阵平面向上，xa轴与ya轴、za轴构成右手坐标系；
步骤三、根据信标在基阵坐标系下的位置来判断USBL是否定位有效；
若成立，则USBL定位有效；否则，USBL定位无效；其中，R是声信号在水下航行器和信标之间的传播距离；
步骤四、根据信标在基阵坐标系下的位置，获得USBL在计算导航坐标系下对水下航行器的定位结果将称为水下航行器的位置测量值；
SINS输出的水下航行器在计算导航坐标系下的位置为称为水下航行器的位置预测值，利用将各基元在基阵坐标系下的位置转换到计算导航坐标系下，获得各基元在计算导航坐标系下的位置；
步骤五、根据SINS计算的水下航行器速度以及各基元在计算导航坐标系下的位置，获得各基元相对信标的径向运动速度预测值
步骤六、USBL获得的各基元相对信标的径向运动速度测量值为若步骤三中判断USBL定位有效，则将与的差以及与的差作为观测量，建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；
若步骤三中判断USBL定位无效，则将与的差作为观测量建立SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型；
步骤七、根据SINS/USBL组合导航数据融合扩展卡尔曼滤波器数学模型原理，利用模型输出的状态估计值修正SINS的输出，并重置扩展卡尔曼滤波器状态；
步骤八、重复步骤二至步骤七的过程，不断的对SINS的输出进行修正，获得修正后的导航输出结果。


2.根据权利要求1所述的一种引入径向速度的SINS/USBL松组合导航定位方法，其特征在于，所述步骤二中，信标在基阵坐标系下的位置为：



其中：R是声信号在水下航行器和信标之间的传播距离，X′a为信标在xa轴方向的位置，Y′a为信标在ya轴方向的位置，Z′a为信标在za轴方向的位置；cosθX是信标在xa轴方向的位置与R的比值，cosθY是信标在ya轴方向的位置与R的比值；



其中：c是水中声速，τX和τY分别是两两基元的传播时延差沿基阵坐标系xa轴和ya轴的分量，dX和dY是对应的两基元在xa轴和ya轴方向上的位置差；
各基元在基阵坐标系下的位置为：



其中：i为基元序号，i＝1,2,3,4，r为基元1与3的间距或基元2与4的间距；为各基元在基阵坐标系的xa轴方向上的位置，为各基元在基阵坐标系的ya轴方向上的位置，为各基元在基阵坐标系的za轴方向上的位置，上角标T代表转置。


3.根据权利要求2所述的一种引入径向速度的SINS/USBL松组合导航定位方法，其特征在于，所述步骤四的具体过程为：
SINS提供的水下航行器在计算导航坐标系下的姿态信息包括水下航行器的航向角A、俯仰角K和横滚角ψ；
建立载体坐标系obxbybzb，载体坐标系的坐标原点ob位于水下航行器的质心，坐标轴xb的正方向沿水下航行器的横轴指向右，坐标轴yb的正方向沿水下航行器的纵轴指向前，坐标轴zb的正方向沿水下航行器的立轴指向上，载体坐标系的定义满足右手定则；
基阵坐标系相对于载体坐标系的角度安装偏差分别为α、β和γ，基阵坐标系原点相对于载体坐标系原点的位置偏差为：ΔXb、ΔYb和ΔZb为中的分量，上角标T代表转置；
载体坐标系到计算导航坐标系的转换矩阵和基阵坐标系到载体坐标系的转换矩阵分别为：...

【专利技术属性】
技术研发人员：郑翠娥，刘云，孙大军，韩云峰，崔宏宇，张居成，张殿伦，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙;23