基于虚拟位置观测的潜航器航行中初始对准方法与流程

本发明涉及水下探测，具体涉及基于虚拟位置观测的潜航器航行中初始对准方法。

背景技术：

1、随着科技和经济的发展，探测水下环境、开发和利用水下资源越来越受到重视，潜航器作为水下探测的先锋，由于能代替人类完成许多水下作业，现在备受工程人员和研究者的青睐，潜航器用于导航定位和环境感知的传感器主要有惯性传感器(imu)、gnss接收机、多普勒测速仪(dvl)、前视声纳和侧扫声纳等，无线电信号难以在水下进行远距离传播，因此卫星定位不能作为潜航器导航的核心方法，只能作为辅助手段，潜航器在布放初始时刻和上浮校正位置误差时，需要接收卫星定位信号，而在下潜执行任务期间，只能依靠惯性导航系统(ins)和基于水声信号的dvl、前视声纳等传感器进行导航定位；

2、较大的潜航器初始对准误差会对后续导航精度产生严重影响，因此，确定初始姿态角是初始对准的重点和难点，航行中对准所需的人为干预较少，是初始对准研究的热点，快速性和精度是初始对准的两大性能指标，当利用dvl辅助ins实现初始对准时，若在对准过程中能全程接收卫星定位信号，则快速性和精度都能得到较大提升，但要接收卫星信号就要求潜航器必须在水面航行，隐秘性不强。

3、目前工业上常用的另一种航行对准方法是仅在对准初时刻和末时刻接收卫星定位信号，而在中间过程潜航器下潜并正常行进，利用ins/dvl组合的方法解算姿态，整个过程通常持续10-15分钟，这种方法能减少潜航器在水面暴露的时间，进一步提高潜航器隐秘性，但由于缺少位置观测信息，所需对准时间更长，从而降低潜航器的工作效率。

4、因此，亟需一种基于虚拟位置观测的潜航器航行中初始对准方法解决上述问题。

技术实现思路

1、本发明的目的是提供基于虚拟位置观测的潜航器航行中初始对准方法，针对潜航器仅在对准初末时刻有卫星观测的情况，通过陀螺仪和dvl的航位推算，得到航行对准过程中潜航器相对运动轨迹，然后以对准初末时刻的卫星定位信息为基准，插值得到潜航器在对准过程中任一时刻的虚拟位置，而后利用dvl速度信息和虚拟位置观测，可建立起和全程gnss定位辅助时一样的卡尔曼滤波观测方程，经实际实验验证，基于虚拟位置观测的方法在5分钟内即可实现潜航器初始对准，对准精度和全程gnss定位辅助时精度非常接近，以解决背景技术中不足。

2、为了实现上述目的，本发明提供如下技术方案：基于虚拟位置观测的潜航器航行中初始对准方法，所述对准方法包括以下步骤：

3、s1：通过陀螺仪和dvl的航位推算，得到航行对准过程中潜航器相对运动轨迹；

4、s2：以对准初末时刻的卫星定位信息为基准，插值得到潜航器在对准过程中任一时刻的虚拟位置；

5、s3：通过dvl速度信息和虚拟位置观测，建立卡尔曼滤波观测方程；

6、s4：通过卡尔曼滤波系统方程以及与全程gnss定位辅助时等同的观测方程，经滤波实现潜航器初始对准。

7、在一个优选的实施方式中，步骤s4中，潜航器在对准过程中的滤波系统方程为：在ekf模型中，姿态失准角φn、速度误差和位置误差[δl δλ δh]t的相关定义为：

8、

9、其中，为姿态表达，即b系到n系的方向余弦矩阵，为速度，即载体相对地球的速度在n系的投影，位置表达通过纬度l、经度λ和高度h，表示变量x的计算值，∧表示矢量叉乘，将三维向量化为反对称矩阵；

10、则姿态失准角方程为：

11、

12、速度误差方程为：

13、

14、位置误差方程为：

15、

16、其中，φn＝[φnφeφd]t，δvn＝[δvnδveδvd]t，δfb分别为陀螺仪和加速度计的测量误差。

17、在一个优选的实施方式中，步骤s1中，潜航器进行航行对准时，dvl全程提供载体速度观测，gnss接收机在对准初末时刻潜航器露出水面时，接收到卫星定位信号。

18、在一个优选的实施方式中，在标定好dvl标度因数及imu安装角的前提下，dvl测得速度为是量测白噪声；

19、则通过dvl速度建立的观测方程为：

20、

21、利用位置信息建立的观测方程为：

22、

23、姿态失准角φn经滤波得到最优估计，在dvl速度和位置观测的辅助下，经不断滤波修正。

24、在一个优选的实施方式中，步骤s1和s2中，根据陀螺和dvl航位推算得到潜航器的相对位移以及对准初末时刻的gnss绝对位置，插值得到潜航器完整的运动轨迹，通过线性映射，得到潜航器各个时刻的虚拟位置；

25、再根据dvl速度和虚拟位置建立观测方程，滤波得到对准末时刻的姿态，进行对准；

26、对准初始时刻，ib0系和b系重合，因此有随时间变化的微分方程为：

27、

28、陀螺的输出正是描述载体相对于惯性系的角运动，由陀螺输出。

29、在一个优选的实施方式中，所述dvl输出载体速度通过航位推算得到载体相对于w系的位移在ib0系的表示为：

30、

31、设dvl输出相邻两个速度值的时间间隔为t，在t＝kt(k＝1,2,…,n)时刻，的计算值为：

32、

33、其中，的计算误差由陀螺测量误差、dvl测量误差和离散化影响。

34、在一个优选的实施方式中，步骤s4中，设gnss提供的载体在对准初时刻和末时刻的位置分别为和(和[lλh]t等效，其中载体在末时刻相对于w系的位移在iw0系表示为：

35、

36、在对准过程中，每个时刻的通过公式(3.3)得到，知道初始时刻和末时刻的值，和的对应关系已知，当知道每个时刻的时，通过公式(2.6)建立滤波观测方程，建立和的映射关系，和的换算关系为：

37、

38、是对准初始时刻载体姿态矩阵，根据对准初末时刻的值和值，解算出值，将每个时刻的换算为进而得到每个时刻的为潜航器的虚拟位置观测。

39、在一个优选的实施方式中，根据和建立方程，由于传感器测量误差，和的幅值不相等，计算刻度因子k，未知数求解方程为：

40、

41、其中，cφ0、cθ0和cψ0分别是初始时刻横滚角φ0、俯仰角θ0和航向角ψ0对应的姿态矩阵，刻度因子k的计算公式为：

42、

43、由方程(3.6)建立3个等式，求解3个未知量，利用幅值约束求解出刻度因子k，求解2个未知量，将方程(3.6)简化，减少未知量数目。

44、在一个优选的实施方式中，将横滚角φ0作为已知量，横滚角的值由ins/dvl回溯粗对准解算得到，或者设为0，方程(3.6)中的cφ0变成已知量后，解算俯仰角θ0和航向角ψ0，设方程(3.6)展开之后的形式如下：

45、

46、将方程(3.8)作进一步变形得：

47、

48、结合sin2ψ0+cos2ψ0＝1，sin2θ0+cos2θ0＝1，解算θ0和ψ0。

49、在一个优选的实施方式中，解出插值对准过程中每个时刻的插值计算过程为：

50、

51、

52、由转换得到[lλh]t虚拟位置观测，采用ekf算法，从对准初始时刻解算至对准末时刻，得到末时刻的姿态角，完成初始对准过程。

53、在上述技术方案中，本发明提供的技术效果和优点：

54、本发明通过陀螺仪和dvl的航位推算，得到航行对准过程中潜航器相对运动轨迹，然后以对准初末时刻的卫星定位信息为基准，插值得到潜航器在对准过程中任一时刻的虚拟位置，而后利用dvl速度信息和虚拟位置观测，可建立起和全程gnss定位辅助时一样的卡尔曼滤波观测方程。经实际实验验证，基于虚拟位置观测的方法在5分钟内即可实现潜航器初始对准，对准精度和全程gnss定位辅助时精度非常接近，对准时间短，有利于提高潜航器的工作效率。