本发明属于导航领域，具体涉及一种两栖机器人的潜行爬行组合导航方法及导航系统。
背景技术：
1、随着科技的迅猛发展，机器人技术在各个领域的应用日益广泛。尤其是在复杂环境下的探索和作业任务中，传统的单一移动方式机器人往往难以应对多样化的地形需求。因此，具备多种移动方式的两栖机器人应运而生。两栖机器人能够在陆地和水中灵活行动，极大地拓展了机器人的应用范围。然而，在实际应用中，如何实现两栖机器人在不同环境间的高效定位和导航，成为了一项亟待解决的技术难题。
2、现有的两栖机器人导航技术主要集中在简单环境下的定位系统上，对于如何在多环境下实现精准的融合定位，研究仍相对薄弱。尤其是在机器人需要在潜行和爬行两种模式间频繁切换时，如何确保其定位的准确性和稳定性，更是一个技术挑战。此外，现有技术在复杂地形的定位精度、多传感器数据融合等方面的不足，限制了两栖机器人在实际任务中的应用效果。
技术实现思路
1、本发明的目的在于提供一种可潜行与爬行的水下机器人组合导航方法，实现了对水下机器人的不同航行状态(潜行/爬行)的位姿预测，提高了该类型水下机器人在水下作业时的定位精度。
2、实现本发明目的的技术解决方案为：一种两栖机器人的潜行爬行组合导航方法，包括如下步骤：
3、步骤(1)：导航系统初始对准：通过加速度计和陀螺仪进行载体惯性坐标系和导航坐标系之间转换关系的确定，得到初始姿态矩阵；
4、步骤(2)：建立状态转移方程：基于李群李代数方法将旋转矩阵映射到旋转向量并对惯性测量单元imu进行航迹推算，建立状态转移方程；
5、步骤(3)：建立观测方程：分别建立以多普勒速度计dvl为观测源的观测方程以及以里程计od为观测源的观测方程；
6、步骤(4)：数据融合：利用步骤(2)建立的状态转移方程进行误差状态卡尔曼滤波器eskf的预测过程，通过步骤(3)建立的观测方程进行误差状态卡尔曼滤波器eskf的更新过程，实现对不同工况下的融合定位。
7、进一步的，步骤(1)包括如下步骤：
8、步骤(11)：对待求解的初始姿态矩阵进行分解；
9、步骤(12)：逐步解算分解后的初始姿态矩阵的各个子矩阵，从而得到初始姿态矩阵，完成初始对准。
10、进一步的，步骤(11)具体为：
11、在对准初始时刻，定义载体惯性坐标系b0以及导航坐标系n0，载体惯性坐标系b0与对准开始时刻的载体惯性坐标系b系重合，随后相对惯性空间无转动，n0系与对准开始时刻的导航坐标系(n系)重合，后续相对于惯性空间无转动。初始姿态矩阵整体分解如下：
12、
13、进一步的，步骤(12)具体为：
14、对于子矩阵
15、
16、式中，为地球自转角速度矢量，即表示n系相对于n0系的定轴转动
17、
18、ωie表示地球自转角速率大小，l表示当地维度；
19、子矩阵在t时间段内的计算方式如下：
20、
21、其中，l表示当地纬度；
22、对于
23、
24、式中，表示陀螺仪测量值，姿态矩阵的初值设为单位阵i；
25、对子矩阵进行求解，由式(5)求得后，在b0系下的投影为
26、
27、由式(4)求得后，得重力矢量在n0系下的投影
28、
29、对式(6)和(7)进行积分，并记：
30、
31、式中，i＝1,2，取t2＝2t1，t2为初始对准结束时刻，给出惯导比力与重力矢量的关系：
32、
33、上标～表示实际量测值，表示在b0系下加速度计量测误差以及线加速度干扰；
34、对式(9)进行积分，并将(8)代入，得：
35、
36、忽略的干扰，并采用双矢量定姿法，得的最终结果
37、进一步的，步骤(2)具体包括如下步骤：
38、步骤(21)：旋转矩阵到旋转向量的映射关系：
39、根据bch近似：
40、
41、相反的：
42、exp((φ+△φ)∧)＝exp((jl△φ)∧)exp(φ∧)＝exp(φ∧)exp((jr△φ)∧) (13)
43、其中：
44、
45、
46、jr(φ)＝jl(-φ) (16)
47、步骤(22)：设置状态变量，并用流形上的方法求出误差状态变量的雅可比矩阵f：
48、设置状态变量x＝[p,v,r,bg,ba,g]t，其中p为平移，v为速度，r为旋转矩阵，bg,ba为陀螺仪和加表零偏，g为重力；
49、误差状态变量的离散时间运动方程：
50、
51、对应的雅可比矩阵f：
52、
53、进一步的，步骤(3)具体包括以下步骤：
54、步骤(31)：建立以dvl为观测源的观测方程：
55、将dvl对速度的观测作为载体系下的x，y，z三轴方向速度的观测，那么：
56、导航坐标系下矢量形式的速度观测写为：
57、vdvl＝[vx,vy,vz] (19)
58、其中vx,vy,vz代表导航坐标系下的三轴速度分量。
59、对应的观测模型为：
60、zdvl＝v+δv+ωdvl (20)
61、其中v代表名义状态变量，δv代表误差状态变量，ωdvl代表dvl观测噪声；观测模型zdvl对误差变量δx的雅可比矩阵
62、步骤(32)：建立以里程计为观测源的观测方程：
63、取前左上坐标系，那么矢量形式的轮速观测记为：
64、vod＝[vod,0,0]t (21)
65、对应的观测模型为：
66、vod＝rtv (22)
67、其中，r,v为车辆当前的状态，根据当前估计的r，将vod转到导航坐标系下，直接视为对v的观测：
68、rvod＝v (23)
69、对应的观测模型为：
70、zod＝v+δv+ωod (24)
71、其中v代表名义状态变量，δv代表误差状态变量，ωod代表里程计观测噪声；
72、观测模型zod对误差变量δx的雅可比矩阵
73、
74、进一步的，步骤(4)包括如下步骤：
75、步骤(41)：建立状态变量的预测方程，包括名义状态变量的预测和误差状
76、态变量的预测：
77、
78、
79、步骤(42)：根据爬行或潜行工况对应的观测方程进行eskf的更新：
80、潜行时：
81、
82、爬行时：
83、
84、步骤(43)将误差状态并于名义状态，求出最终融合定位数据：
85、经过预测和更新之后，修正误差状态的估计，把误差状态归入名义状态，然后重置eskf，最终的组合导航定位信息表示为：
86、pk+1＝pk+δpk
87、vk+1＝vk+δvk
88、rk+1＝rkexp(δθk)
89、bg,k+1＝bg,k+δbg,k (29)。
90、ba,k+1＝ba,k+δba,k
91、gk+1＝gk+δgk
92、一种两栖机器人的潜行爬行组合导航系统，采用上述的导航方法进行导航。
93、进一步的，系统包括多普勒声学多普勒速度计dvl，惯性测量单元imu，里程计od和误差状态卡尔曼滤波器；
94、误差状态卡尔曼滤波器具有以多普勒声学多普勒速度计dvl为观测源的观测模型zdvl以及以里程计od为观测源的观测模型zod；
95、在潜行模式下，使用多普勒声学多普勒速度计dvl获取速度信息，结合惯性测量单元imu数据，误差状态卡尔曼滤波器选用观测模型zdvl；在爬行模式下，使用里程计od获取运动信息，结合imu数据，误差状态卡尔曼滤波器选用观测模型zod。
96、本发明与现有技术相比，其显著优点在于：
97、本发明通过误差状态卡尔曼滤波eskf，利用李群李代数的方法将旋转矩阵映射到旋转向量中，从而让旋转的增量可以用三维变量表达，使得参数表达最小化，并用误差状态变量进行滤波器的更新过程，离奇异点较远，数值方面更加的稳定，以此实现多传感器融合定位，显著提高了机器人在不同水下运动模式下的定位精度和稳定性；
98、本发明在水下潜行模式下，结合多普勒声学多普勒速度计dvl和惯性测量单元imu数据，提供高精度的水下定位；在水下爬行模式下，则利用里程计与imu数据融合，实现稳定定位。
99、通过实时切换和融合传感器数据，本方法在复杂水下环境中提供了连续、可靠的导航解决方案，克服了现有技术在多环境、多模式下定位精度不足的问题，广泛适用于海洋探测、环境监测等领域，极大地提升了水下机器人的实际应用效果和任务执行能力。
1.一种两栖机器人的潜行爬行组合导航方法，其特征在于，包括如下步骤：
2.根据权利要求1所述的方法，其特征在于，步骤(1)包括如下步骤：
3.根据权利要求2所述的方法，其特征在于，步骤(11)具体为：
4.根据权利要求3所述的方法，其特征在于，步骤(12)具体为：
5.根据权利要求4所述的方法，其特征在于，步骤(2)具体包括如下步骤：
6.根据权利要求5所述方法，其特征在于，步骤(3)具体包括以下步骤：
7.根据权利要求6所述方法，其特征在于，步骤(4)包括如下步骤：
8.一种两栖机器人的潜行爬行组合导航系统，其特征在于，采用权利要求1-7任一项所述的导航方法进行导航。
9.根据权利要求8所述的系统，其特征在于，包括多普勒声学多普勒速度计dvl，惯性测量单元imu，里程计od和误差状态卡尔曼滤波器；
