一种基于地球坐标系的水下机器人组合导航方法

本发明涉及一种水下机器人组合导航方法，属于机器人定位导航。

背景技术：

1、水下机器人是极地海洋科学考察中的一种重要工具，但现有的极地导航技术仍面临诸多挑战。随着纬度升高，地理经线快速收敛，导致在中低纬度常用的地理坐标系失效。格网坐标系和横向地球坐标系下组合导航都基于圆球假设，因而在地球形状的数学描述上存在原理性误差。目前，为了实现全球任意纬度的导航，通常在中低纬度使用地理坐标系导航，而在极地则切换为格网坐标系或横向地球坐标系导航。然而，这种方法存在计算负担重、参数容易超调，存在奇点等缺陷，无法保证导航参数的唯一性。

技术实现思路

1、本发明为解决现有技术计算负担重、参数容易超调，存在奇点，无法保证导航参数的唯一性的问题，进而提出一种基于地球坐标系的水下机器人组合导航方法。

2、本发明为解决上述问题采取的技术方案是：本发明具体包括：

3、步骤100、获取惯性测量单元的测量数据，所述测量数据包括：姿态信息数据、速度信息数据和位置信息数据，并将所获得的所述姿态信息数据、速度信息数据和位置信息数据在地球坐标系下进行机械编排；

4、步骤200、获取水下应答器、多普勒计程仪、深度计的数据作为观测量，在地球坐标系下，使用误差状态卡尔曼滤波算法对导航参数误差进行估计和反馈校正；

5、步骤300、通过转换模块将地球坐标系下的导航参数转换到地理坐标系或格网坐标系。

6、进一步的，步骤100具体包括：

7、步骤110、将所述姿态信息数据带入对姿态微分方程进行求解，获得第一姿态信息数据；

8、步骤120、将所述速度信息数据带入对速度微分方程进行求解，获得第一速度信息数据；

9、步骤130、将所述位置信息数据带入对位置微分方程进行求解，获得第一位置信息数据。

10、进一步的，姿态微分方程为：

11、

12、公式(1)中，表示载体坐标系到地球坐标系的姿态矩阵，表示载体相对于地球的角速度在载体坐标系的投影,×表示反对称矩阵。

13、进一步的，速度微分方程为：

14、

15、公式(2)中，ve表示地速，表示载体坐标系到地球坐标系的姿态矩阵，fb表示比力，×表示叉乘，ge表示重力在地球坐标系的投影，表示地速的导数。

16、进一步的，位置微分方程为：

17、pe＝ve (3)，

18、公式(3)中，pe表示位置，ve表示地速。

19、进一步的，步骤200具体包括：构建过程模型、构建水下应答器观测模型、构建多普勒计程仪观测模型和构建深度计观测模型。

20、进一步的，构建过程模型的公式为：

21、

22、δx(t)＝[(φe)t(δve)t(δpe)t(δbg)t(δba)tδbc]t (5)，

23、

24、futp(t)＝[0]  (7)，

25、

26、公式(4)、(5)、(6)、(7)和(8)中，δx(t)表示状态向量；g(t)为过程噪声驱动矩阵，w(t)为过程噪声，f(t)表示状态转移矩阵；fins(t)表示与惯性导航系统相关的状态转移矩阵的子矩阵；fut(t)表示与水下应答器相关的状态转移矩阵的子矩阵；φ表示失准角；δve表示速度误差；δpe＝[δxδyδz]t为位置误差，δx、δy和δz表示地球坐标系的三轴位置误差；δbg表示陀螺仪零偏的残差；δba表示加速度计零偏的禅残差；δbc表示有效声速零偏的残差；表示载体坐标系到地球坐标系的姿态矩阵；×表示叉乘。

27、进一步的，构建水下应答器观测模型的公式为：

28、δzupt＝ρins-ρutp  (9)，

29、

30、ρins＝||pins-putp||+||p0+putp||    (11)，

31、

32、公式(9)、(10)、(11)、(12)和(13)中，δzutp表示观测向量；ρutp表示水下应答器测量的斜距；ρins表示惯性导航系统预测的斜距；hutp表示观测矩阵；p表示无误差的水下机器人当前位置；pins＝[xinsyinszins]t表示惯性导航系统预测的水下机器人当前位置；t表示转置，0表示零元素，xinsyinszins分别代表惯性导航系统预测的水下机器人当前位置的x、y、z坐标，三者共同组成一组位置坐标即为pins，p0表示发射水下应答器查询信号时的位置；putp表示水下应答器的位置；δtutp表示水下应答器声学信号的双程传播时间；ηutp表示水下应答器测距噪声。

33、进一步的，构建多普勒计程仪观测模型的公式为：

34、

35、

36、公式(14)和(15)，δzdvl表示观测向量；表示惯性导航系统预测的波束坐标系速度；表示多普勒计程仪测量的波束坐标系速度；为惯性导航系统预测的地速在地球坐标系下的投影；hdvl表示观测矩阵；表示载体坐标系到波束坐标系的姿态矩阵；表示惯性导航系统计算的地球坐标系到载体坐标系的姿态矩阵；表示地球坐标系到载体坐标系的姿态矩阵；ηdvl表示多普勒计程仪测速噪声；φ表示失准角；δve表示速度误差；ve表示地速。

37、进一步的，构建深度计观测模型的公式为：

38、

39、公式(16)和(17)，δzdm表示观测向量；dins表示惯性导航系统预测的深度；ddm表示深度计测量的深度；ηdm表示深度计测深噪声；l表示纬度。

40、本发明的有益效果是：本发明通过直接在地球坐标系进行导航方案计算，避免了在地理坐标系、格网坐标系和横向地球坐标系下计算存在奇点的问题，保证了导航参数的唯一性；并且，本发明不需要进行无需根据纬度进行坐标系切换，可以有效地减轻计算负担，提高计算效率。

技术特征：

1.一种基于地球坐标系的水下机器人组合导航方法，其特征在于，具体包括：

2.根据权利要求1所述的一种基于地球坐标系的水下机器人组合导航方法，其特征在于，步骤100具体包括：

3.根据权利要求2所述的一种基于地球坐标系的水下机器人组合导航方法，其特征在于，姿态微分方程为：

4.根据权利要求2所述的一种基于地球坐标系的水下机器人组合导航方法，其特征在于，速度微分方程为：

5.根据权利要求2所述的一种基于地球坐标系的水下机器人组合导航方法，其特征在于，位置微分方程为：

6.根据权利要求1所述的一种基于地球坐标系的水下机器人组合导航方法，其特征在于，步骤200具体包括：构建过程模型、构建水下应答器观测模型、构建多普勒计程仪观测模型和构建深度计观测模型。

7.根据权利要求6所述的一种基于地球坐标系的水下机器人组合导航方法，其特征在于，构建过程模型的公式为：

8.根据权利要求6所述的一种基于地球坐标系的水下机器人组合导航方法，其特征在于，构建水下应答器观测模型的公式为：

9.根据权利要求6所述的一种基于地球坐标系的水下机器人组合导航方法，其特征在于，构建多普勒计程仪观测模型的公式为：

10.根据权利要求6所述的一种基于地球坐标系的水下机器人组合导航方法，其特征在于，构建深度计观测模型的公式为：

技术总结
一种基于地球坐标系的水下机器人组合导航方法，它涉及一种水下机器人组合导航方法。本发明为了解决现有技术计算负担重、参数容易超调，存在奇点，无法保证导航参数的唯一性的问题。本发明包括步骤100：获取惯性测量单元的测量数据，测量数据包括：姿态信息数据、速度信息数据和位置信息数据，并将所获得的姿态信息数据、速度信息数据和位置信息数据在地球坐标系下进行机械编排；步骤200：获取水下应答器、多普勒计程仪、深度计的数据作为观测量，在地球坐标系下，使用误差状态卡尔曼滤波算法对导航参数误差进行估计和反馈校正；步骤300：通过转换模块将地球坐标系下的导航参数转换到地理坐标系或格网坐标系。本发明属于机器人定位导航技术领域。

技术研发人员：秦洪德,鲍敬强,朱仲本,牟晓凯,白桂强,薛祎凡,曹小建,邓忠超
受保护的技术使用者：哈尔滨工程大学
技术研发日：
技术公布日：2024/12/2