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

一种引入径向速度的SINS/USBL紧组合导航定位方法，本发明专利技术涉及水下组合导航定位方法。本发明专利技术的目的是为了提高现有传统SINS/USBL紧组合的速度精度和大开角下紧组合的稳定性。过程为：一、将USBL基阵和SINS的陀螺组件和加速度计组件安装在水下航行器上；二、确定基元在基阵坐标系下的位置；三、得到USBL测量的各基元相对信标的径向运动速度；四、得到基元在计算导航坐标系下的位置；五、计算传播时延和传播时延差；六、将航行器运动速度沿径向分解；七、建立状态方程和观测方程；八、修正SINS输出，重置扩展卡尔曼滤波器状态，重新执行步骤三～步骤八，直至导航结束。本发明专利技术用于组合导航及水声定位技术领域。

【技术实现步骤摘要】
一种引入径向速度的SINS/USBL紧组合导航定位方法
本专利技术涉及SINS/USBL紧组合导航定位方法，属于组合导航及水声定位

技术介绍
传统的SINS/USBL紧组合建立在以传播时延(斜距)之差和时延差(斜距差)之差为观测量的基础上。径向速度是水下航行器与参考信标在视在方向的运动速度，描述了水下航行器与参考信标之间的距离变化率大小，径向速度与航行器的运动速度直接相关。USBL能够通过计算基元接收信号与发射信号间的多普勒频率，获得基元与信标之间的径向速度。通过引入径向速度既能够提高滤波系统对速度误差的估计精度，又能保证大开角下紧组合导航的稳定性，改善整体导航性能，而传统的SINS/USBL紧组合导航忽略了USBL计算的径向速度这一冗余测量，导致在传统的紧组合滤波系统中，速度误差的可观测性较弱，数据融合结果对SINS的速度误差修正效果相对较差，且降低了在大开角情况下，SINS/USBL紧组合的导航精度和稳定性。
技术实现思路
本专利技术的目的是为了提高现有传统SINS/USBL紧组合的速度误差估计精度和大开角情况下的SINS/USBL紧组合稳定性，而提出一种引入径向速度的SINS/USBL紧组合导航定位方法。一种引入径向速度的SINS/USBL紧组合导航定位方法具体过程为：步骤一、将USBL声学基阵倒置安装在水下航行器上，SINS的陀螺组件和加速度计组件固联在水下航行器上；所述USBL为超短基线定位系统；SINS为捷联惯性导航系统；水面布放一个同步信标，信标在导航坐标系n下的位置通过GPS获得；所述声学基阵是由多个基元均匀分布的一个圆柱形设备；所述多个为大于等于3个；所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；步骤二、建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；步骤三、USBL测量的声信号在各基元和信标间的传播时延为传播时延差为下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,I′，各基元在径向方向相对信标的运动速度为步骤四、根据SINS计算水下航行器的姿态、速度和位置；结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果，得到基元在计算导航坐标系下的位置；步骤五、结合声信号在航行器与信标之间传播的有效声速，根据SINS转换的基元在计算导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差；步骤六、将SINS解算的航行器运动速度沿径向分解；步骤七、建立基于传播时延、传播时延差和径向速度信息融合的扩展卡尔曼滤波器状态方程和观测方程；步骤八、计算SINS误差，修正SINS输出，重置扩展卡尔曼滤波器状态为零，重新执行步骤三～步骤八，直至导航结束。本专利技术的有益效果为：本专利技术将USBL定位系统计算的水下航行器相对声学信标在径向方向的运动速度引入传统的SINS/USBL紧组合中，增强滤波系统中速度误差的可观测性，提高速度误差的估计精度，最终实现组合导航速度和位置精度的提高；同时提高SINS/USBL紧组合在大开角情况下的导航精度和稳定性。附图说明图1为SINS/USBL紧组合导航定位数据融合原理图；图2为基阵坐标系、载体坐标系和导航坐标系的示意简图。具体实施方式具体实施方式一：本实施方式一种引入径向速度的SINS/USBL紧组合导航定位方法具体过程为：步骤一、将USBL声学基阵倒置安装在水下航行器上，SINS的陀螺组件和加速度计组件固联在水下航行器上；所述USBL为超短基线定位系统；SINS为捷联惯性导航系统；水面布放一个同步信标，信标在导航坐标系n下的位置通过GPS获得；所述声学基阵是由多个基元(声信号接收换能器)均匀分布的一个圆柱形设备；所述多个为大于等于3个；所述导航坐标系是指“东北天”(“ENU”)地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；步骤二、建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；步骤三、USBL测量的声信号在各基元和信标间的传播时延为传播时延差为下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,I′，各基元在径向方向相对信标的运动速度为步骤四、根据SINS计算水下航行器的姿态、速度和位置(在这里把SINS当成一个黑匣子，它输出水下航行器的姿态、速度和位置)；结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果(和)，得到基元在计算导航坐标系下的位置；步骤五、结合声信号在航行器与信标之间传播的有效声速(声速剖面仪(SVP)测量)，根据SINS转换的基元在计算导航坐标系中的位置信息(式5)，计算声信号在各基元与信标间的传播时延和传播时延差；步骤六、将SINS解算的航行器运动速度(SINS的输出)沿径向分解；步骤七、建立基于传播时延、传播时延差和径向速度信息(公式9、11、12)融合的扩展卡尔曼滤波器状态方程和观测方程；步骤八、计算SINS误差，修正SINS输出，重置扩展卡尔曼滤波器状态为零(所述重置滤波状态变量是指，在修正SINS输出后，理论上认为此时捷联惯导输出的导航信息不存在误差，因此滤波状态变量为零。)，重新执行步骤三～步骤八，直至导航结束。具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤二中建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；具体过程为：声学基阵坐标系是指以基阵中心为原点，y轴沿基阵平面指向水下航行器的艏向方向，z轴垂直于基阵平面向上，x轴与y轴、z轴构成右手坐标系；基元在声学基阵坐标系下的坐标为：式(1)中，i为基元序号，I′为基元总数，为基元i在声学基阵坐标系x轴下的坐标，r为基线长度，为基元i在声学基阵坐标系y轴下的坐标，为基元i在声学基阵坐标系z轴下的坐标。其它步骤及参数与具体实施方式一相同。具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤三中USBL测量的声信号在各基元和信标间的传播时延为传播时延差为下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,I′，各基元在径向方向相对信标的运动速度为具体过程为：将和表示为：式中，τi为第i个基元测量的传播时延真值，ni为第i个基元的测量噪声，τ1j为第j个基元测量的传播时延与1号基元测量的传播时延差真值，n1j为时延差测量噪声，vri为第i个基元测量的径向速度真值，nri为第i个基元的径向速度测量噪声。其它步骤及参数与具体实本文档来自技高网...

【技术保护点】
1.一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述方法具体过程为：/n步骤一、将USBL声学基阵倒置安装在水下航行器上，SINS的陀螺组件和加速度计组件固联在水下航行器上；/n所述USBL为超短基线定位系统；SINS为捷联惯性导航系统；/n水面布放一个同步信标，信标在导航坐标系n下的位置通过GPS获得；/n所述声学基阵是由多个基元均匀分布的一个圆柱形设备；/n所述多个为大于等于3个；/n所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；/n步骤二、建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；/n步骤三、USBL测量的声信号在各基元和信标间的传播时延为

【技术特征摘要】
1.一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述方法具体过程为：
步骤一、将USBL声学基阵倒置安装在水下航行器上，SINS的陀螺组件和加速度计组件固联在水下航行器上；
所述USBL为超短基线定位系统；SINS为捷联惯性导航系统；
水面布放一个同步信标，信标在导航坐标系n下的位置通过GPS获得；
所述声学基阵是由多个基元均匀分布的一个圆柱形设备；
所述多个为大于等于3个；
所述导航坐标系是指“东北天”地理坐标系，该地理坐标系以水下航行器的质心为原点o，x轴指向地理东向，y轴指向地理北向，z轴垂直于xoy平面指向天向，x轴、y轴和z轴构成右手坐标系；
步骤二、建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；
步骤三、USBL测量的声信号在各基元和信标间的传播时延为传播时延差为下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,I′，各基元在径向方向相对信标的运动速度为
步骤四、根据SINS计算水下航行器的姿态、速度和位置；
结合声学基阵坐标系与水下航行器坐标系的安装偏差校准结果，得到基元在计算导航坐标系下的位置；
步骤五、结合声信号在航行器与信标之间传播的有效声速，根据SINS转换的基元在计算导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差；
步骤六、将SINS解算的航行器运动速度沿径向分解；
步骤七、建立基于传播时延、传播时延差和径向速度信息融合的扩展卡尔曼滤波器状态方程和观测方程；
步骤八、计算SINS误差，修正SINS输出，重置扩展卡尔曼滤波器状态为零，重新执行步骤三～步骤八，直至导航结束。


2.根据权利要求1所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述步骤二中建立声学基阵坐标系a，并确定基元在声学基阵坐标系下的位置；具体过程为：
声学基阵坐标系是指以基阵中心为原点，y轴沿基阵平面指向水下航行器的艏向方向，z轴垂直于基阵平面向上，x轴与y轴、z轴构成右手坐标系；
基元在声学基阵坐标系下的坐标为：



式(1)中，i为基元序号，I′为基元总数，为基元i在声学基阵坐标系x轴下的坐标，r为基线长度，为基元i在声学基阵坐标系y轴下的坐标，为基元i在声学基阵坐标系z轴下的坐标。


3.根据权利要求1或2所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述步骤三中USBL测量的声信号在各基元和信标间的传播时延为传播时延差为下标表示第j个基元测量的传播时延与1号基元测量的传播时延之差，其中j＝2,...,I′，各基元在径向方向相对信标的运动速度为具体过程为：
将和表示为：



式中，τi为第i个基元测量的传播时延真值，ni为第i个基元的测量噪声，τ1j为第j个基元测量的传播时延与1号基元测量的传播时延差真值，n1j为时延差测量噪声，vri为第i个基元测量的径向速度真值，nri为第i个基元的径向速度测量噪声。


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






则第i个基元在计算导航坐标系下的位置表示为



式中，为第i个基元在声学基阵坐标系下的位置，为SINS计算的水下航行器位置；
由于SINS的计算导航坐标系n'与导航坐标系n的三个坐标轴之间存在角度误差φ＝[φxφyφz]T，称失准角误差，计算导航坐标系n'与导航坐标系n之间的转换矩阵近似为



其中



式中，φ＝[φxφyφz]T为失准角误差，I为三阶单位阵；φx、φy、φz为φ中的分量；
第i个基元在SINS计算导航坐标系下的位置与在真实导航坐标系下位置值之间的误差为



式中，ΔXi是与水下航行器在真实导航坐标系下位置的差；记



式中，是在计算导航坐标系中第i个基元相对水下航行器质心的位置。


5.根据权利要求4所述一种引入径向速度的SINS/USBL紧组合导航定位方法，其特征在于：所述步骤五中结合声信号在航行器与信标之间传播的有效声速，根据SINS转换的基元在计算导航坐标系中的位置信息，计算声信号在各基元与信标间的传播时延和传播时延差；具体过程为：
声信号在第i个基元与信标之间的传播时延预测值为：



式中，c是测量的有效声速，是信标在真实导航坐标系中的位置，||·||是矩阵二范数的表示；



其中，为信标在真实导航坐标系的xn轴方向位置，为信标在真实导航坐标系的yn轴方向位置，为信标在真实导航坐标系的zn轴方向位置，为第i个基元在计算导航坐标系的xn轴方向位置，为第i个基元在计算导航坐标系的yn轴方向位置，为第i个基元在计算导航坐标系的zn轴方向位置；
声信号的传播时延差为：...

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