【技术实现步骤摘要】
一种引入径向速度的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为捷联惯性导航系统;水面布放一个同步信标, ...
【技术保护点】
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
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。