一种紧耦合导航方法、系统、计算机设备及存储介质

本发明涉及水下自主航行器定位，尤其涉及一种紧耦合导航方法、系统、计算机设备及存储介质。

背景技术：

1、在水下自主航行器定位领域，应用最为广泛的为视觉惯性导航系统(vins)和长基线定位系统(lbl)。视觉惯性两类传感器优势互补，能实现不依赖于外界环境的较高精度的自主导航，但是无法提供全局位置信息，而在更加复杂的水下环境，传感器可用性降低。长基线定位系统(lbl)，利用在海面的浮标，通过水声传播给auv提供全局定位信息，相比于电磁波在水下传播快速衰减，声波在水下介质中传播更加稳定，提高定位精度及导航系统稳定性。lbl通常和惯性导航系统、dvl等进行结合，目前还没有将vins与lbl进行融合的有效方案。

2、传统的vins系统，由于没有全局定位信息，水下定位过程中十分依赖历史时刻的位姿，以及需要一个十分准确的初始位姿。并且，在水下大尺度导航场景下，由于无法形成闭环将导致累积误差无法消除。

技术实现思路

1、本发明提供了一种紧耦合导航方法、系统、计算机设备及可读存储介质，以解决现有的视觉惯性导航系统没有全局信息，水下定位精确性不高，缺少将vins与lbl进行融合有效方案的技术问题，实现提高水下自主航行器定位精度及导航系统稳定性。

2、为解决上述技术问题，第一方面，本发明实施例提供了一种紧耦合导航方法，应用于水下自主航行器，所述方法包括：

3、获取视觉惯性导航系统的惯性因子和视觉因子，并分别根据所述惯性因子和视觉因子计算惯性因子残差和视觉因子残差，所述惯性因子包括所述视觉惯性导航系统中imu的惯性状态信息，所述视觉因子包括图像特征点的逆深度；

4、获取长基线定位系统的全局因子，根据所述全局因子计算全局因子残差，所述全局因子包括所述水下自主航行器与所述长基线定位系统中应答器的斜距；

5、根据所述惯性因子、视觉因子和全局因子构建因子图，并采用基于滑动窗口的紧耦合方式将所述因子图内的惯性因子、视觉因子和全局因子进行融合，构建紧耦合模型；

6、根据所述惯性因子残差、视觉因子残差和全局因子残差对所述紧耦合模型进行优化，得到所述水下自主航行器的定位结果。

7、在进一步实施方式中，所述根据所述惯性因子、视觉因子和全局因子构建因子图，包括；

8、以所述水下自主航行器在世界坐标系中的位姿作为所述因子图的节点，所述位姿包括位置和方向；

9、以所述惯性因子作为所述因子图两个连续节点之间的第一局部约束；

10、以所述视觉因子作为所述因子图每个节点的第二局部约束，以所述全局因子作为所述因子图每个节点的全局约束。

11、在进一步实施方式中，所述采用紧耦合方式将所述因子图内的惯性因子、视觉因子和全局因子进行融合，构建紧耦合模型，包括：

12、获取所述视觉惯性导航系统中相机坐标系到载体坐标系的第一相对位姿；

13、通过所述视觉惯性导航系统和长基线定位系统分别获取所述水下自主航行器的局部定位轨迹和全局定位轨迹，并将所述局部定位轨迹和全局定位轨迹对齐，得到所述视觉惯性导航系统中载体坐标系到enu坐标系的旋转矩阵和平移向量，并根据所述旋转矩阵和平移向量计算所述载体坐标系到enu坐标系的第二相对位姿；

14、基于所述第一相对位姿和第二相对位姿，将所述滑动窗口内惯性因子、视觉因子和全局因子整合为一个状态向量；

15、确定所述滑动窗口时间周期，根据所述滑动窗口时间周期内的状态向量构建紧耦合模型。

16、在进一步实施方式中，所述将所述局部定位轨迹和全局定位轨迹对齐，得到所述视觉惯性导航系统中载体坐标系到enu坐标系的旋转矩阵和平移向量，包括：

17、选取所述全局定位轨迹的一个位置点作为参考点，计算所述参考点所对应的所述水下自主航行器的全局定位信息，将所述参考点作为enu坐标系的原点；

18、将所述全局定位轨迹所对应的全局定位信息从ecef坐标系转换为enu坐标系，获取所述水下自主航行器在enu坐标系下的全局定位信息；

19、采用插值法将所述水下自主航行器在enu坐标系下的全局定位信息和所述局部定位轨迹的定位信息的时间戳对齐，得到所述视觉惯性导航系统中载体坐标系到enu坐标系的旋转矩阵和平移向量。

20、在进一步实施方式中，根据所述惯性因子残差、视觉因子残差和全局因子残差对所述紧耦合模型进行优化，得到所述水下自主航行器的定位结果，包括：

21、将惯性因子残差、视觉因子残差和全局因子残差合并到一个滑动窗口，得到所述紧耦合模型的代价函数；

22、计算所述滑动窗口时间周期内使所述代价函数最小的状态向量，并将所述计算得到的状态向量作为所述水下自主航行器的定位结果。

23、在进一步实施方式中，所述状态向量为：

24、

25、

26、

27、其中，xk表示第k时刻对应的imu的惯性状态信息；n表示滑动窗口时间周期内imu的惯性状态信息的总数；表示第一相对位姿；表示第二相对位姿；λ表示图像特征点的逆深度；m表示滑动窗口时间周期内特征点数量；ρ表示水下自主航行器与应答器的斜距；l表示滑动窗口时间周期内接收到的斜距信息数；分别表示第k时刻对应的视觉惯性导航系统的位置信息、速度信息和旋转信息；ba，bg代表零偏量；表示视觉惯性导航系统中相机坐标系到载体坐标系的旋转矩阵；表示视觉惯性导航系统中相机坐标系到载体坐标系的平移向量；表示视觉惯性导航系统中载体坐标系到enu坐标系的旋转矩阵；表示视觉惯性导航系统中载体坐标系到enu坐标系的平移向量；

28、所述代价函数为：

29、

30、其中，表示惯性因子残差；rr表示视觉因子残差；rl表示全局因子残差；表示k时刻惯性因子测量值；表示k时刻视觉因子测量值；表示k时刻全局因子测量值。

31、第二方面，本发明实施例提供了一种紧耦合导航系统，所述系统包括：

32、局部定位信息获取单元：用于获取视觉惯性导航系统的惯性因子和视觉因子，并分别根据所述惯性因子和视觉因子计算惯性因子残差和视觉因子残差，所述惯性因子包括所述视觉惯性导航系统中imu的惯性状态信息，所述视觉因子包括图像特征点的逆深度；

33、全局定位信息获取单元：用于获取长基线定位系统的全局因子，根据所述全局因子计算全局因子残差，所述全局因子包括所述水下自主航行器与所述长基线定位系统中应答器的斜距；

34、紧耦合模型构建单元：用于根据所述惯性因子、视觉因子和全局因子构建因子图，并采用基于滑动窗口的紧耦合方式将所述因子图内的惯性因子、视觉因子和全局因子进行融合，构建紧耦合模型；

35、计算优化单元：用于根据所述惯性因子残差、视觉因子残差和全局因子残差对所述紧耦合模型进行优化，得到所述水下自主航行器的定位结果。

36、第三方面，本发明实施例还提供一种计算机设备，其特征在于：包括存储器、处理器和收发器，它们之间通过总线连接；存储器用于存储一组计算机程序指令和数据，并可以将存储的数据传输给处理器，处理器可以执行存储器存储的程序指令，以执行以上权利要求所述的方法。

37、第四方面，本发明实施例还提供一种计算机可读存储介质，其特征在于：所述计算机可读存储介质中存储有计算机程序，当所述计算机程序被运行时，实现以上权利要求所述的方法。

38、本发明实施例提供了一种紧耦合导航方法、系统、计算机设备及存储介质。本技术根据从惯性导航系统获取的惯性因子和视觉因子，以及从长基线定位系统获取的全局因子计算惯性因子残差、视觉因子残差和全局因子残差，并构建因子图，采用基于滑动窗口的紧耦合方式将因子图内的惯性因子、视觉因子和全局因子进行融合，构建紧耦合模型；根据惯性因子残差、视觉因子残差和全局因子残差对所述紧耦合模型进行优化，得到所述水下自主航行器的定位结果。本技术为水下视觉惯性导航系统引入全局定位信息，将水下自主航行器的惯性因子、视觉因子和全局因子进行紧耦合，实现由局部定位轨迹到全局定位轨迹的校正，提升水下自主航行器定位系统的精度和鲁棒性。