本发明属于水下导航定位，具体涉及一种基于声学测距的单领航auv(autonomous underwater vehicle，水下航行器)水下动基座对准方法。
背景技术：
1、由于水下环境的复杂性和不确定性，单水下航行器难以满足需求，多水下航行器规模更大、精度更高、智能化更强、复杂任务完成度更好，因此多水下航行器协同作业模式越来越多的被应用于水下任务的执行中。
2、多水下航行器协同导航是指通过不同auv之间的信息共享，使auv的导航能力得到提升，该方法在海洋科学考察、海底资源勘探、海洋环境监测等领域具有广泛的应用前景。其按照编队形式可分为主从式、并行式和分层式，目前较为常用的是主从式结构中的单领航协同导航模式，将主auv配备高精度导航设备，从auv配备低精度导航设备，利用主auv的信息优势来提升从auv的导航精度，以此在保证较高整体导航能力的同时有效降低作业成本。
3、水下航行器通常搭载多种导航系统，惯性导航系统(inertial navigationsystem，ins，以下简称惯导)因其不易受到外界干扰、实时性好的优点，成为水下航行器最主要的导航系统之一。惯导系统的主要传感器为陀螺仪和加速度计，陀螺仪采集载体角速度信息，加速度计采集载体加速度信息，通过积分运算得到载体的速度、位置、姿态信息。
4、惯性导航系统虽然因其自主性和隐蔽性好的特点在水下环境中备受青睐，但受牛顿运动定律基本原理所限，该系统在工作前必须确定初始姿态信息以建立准确的导航基准，初始姿态信息的准确性直接影响到后续导航解算的精度和系统的整体性能。
5、初始对准是惯性导航前序的关键步骤，它利用惯导的误差规律以及外部信息，通过滤波算法估计并消除导航系统的初始姿态误差。单领航auv协同导航初始对准是指多auv水下作业时，利用主auv的信息优势对从auv进行在线对准，以提供准确的初始姿态信息。
6、公开号为cn114777812a的发明专利提出了“一种水下组合导航系统行进间对准与姿态估计方法”，该方法基于马氏距离，并利用变分贝叶斯鲁棒自适应卡尔曼滤波器来进行auv的sins/dvl组合导航系统行进间姿态估计。文献“vl辅助的无人水下航行器回溯式无纬度运动对准方法”提出水下航行器在dvl辅助下自由行驶，导航系统不需要任何初始位置信息即可在短时间内完成对准。文献“a robust foam initial alignment method for sinsassisted by dvl”提出了一种基于矢量误差分布独立性检验的鲁棒快速姿态矩阵初始对准方法，以适用于dvl速度信息出现异常值的情况。文献“initial alignment andposition aiding time delay compensation for sdins of deep sea underwatervehicles”针对深海对准中dvl提供的速度信息以及usbl提供的位置信息时延较长的情况，结合位置辅助时延补偿方法提出了一种简单有效的深海水下机器人初始对准方案。
7、综上所述，现有方法要么是在dvl辅助信息下利用不同的方法进行对准，要么是通过不同方法改善dvl辅助信息后再进行对准，但是当利用低刷新率的声学测距提供定位信息时，现有方法仍然无法实现水下动基座的对准。
技术实现思路
1、本发明的目的是为解决当利用声学测距提供定位信息时，现有方法无法实现水下动基座的对准的问题，而提出了一种基于声学测距的单领航auv水下动基座对准方法。
2、本发明为解决上述技术问题所采取的技术方案是：一种基于声学测距的单领航auv水下动基座对准方法，所述方法具体包括以下步骤：
3、步骤一、主auv和从auv按照设计的路径航行；
4、步骤二、当k＝1时，采集从auv惯性组件输出的数据，采集的数据具体包括加速度计输出的加速度信息以及mems陀螺仪输出的角速度信息；
5、将k时刻从auv的加速度信息记为上角标t表示转置，和分别表示k时刻从auv的加速度计测量比力在载体系三个坐标轴上的分量，单位均为m/s2；将k时刻从auv的角速度信息记为和分别表示k时刻mems陀螺仪测量的角速度在载体系三个坐标轴上的分量，单位均为rad/s；
6、步骤三、初始化时刻k＝2，迭代次数l＝1；
7、步骤四、采集从auv惯性组件在k时刻输出的数据，利用k-1时刻采集到的从auv惯性组件输出数据来更新k时刻从auv的惯导信息，即获得惯导系统输出的k时刻从auv的位置、速度和姿态信息；
8、步骤五、利用声学测距测量k时刻主auv的位置信息以及主auv与从auv的相对距离；
9、步骤六、判断是否满足k＝l·δt，δt为设定的阈值；
10、若满足，则将时刻(l-1)·δt+1至时刻l·δt的主auv与从auv的相对距离发送给从auv，并执行步骤七；
11、若不满足，则令k＝k+1，返回执行步骤四；
12、步骤七、将步骤五测得的主auv位置信息以及主auv与从auv的相对距离发送给从auv，利用接收到的信息和各时刻从auv的姿态信息来求解从auv的位置信息；
13、步骤八、对步骤七中获得的从auv位置信息进行筛选，获得筛选出的从auv位置信息；
14、步骤九、将步骤八中筛选出的从auv位置信息以及对应的时刻作为已知数据节点进行插值，得到插值后各个时刻的从auv位置信息；
15、步骤十、对于时刻(l-1)·δt+1至时刻l·δt之间的任意一个时刻，均获得当前时刻下惯导系统输出的从auv位置信息与插值后从auv位置信息的差值，将获得的从auv位置信息差值作为观测量；
16、将auv位置误差、速度误差、姿态误差、陀螺仪误差和加速度计误差组成状态量，根据状态量和观测量进行卡尔曼滤波，得到各时刻状态量估计结果；
17、再根据状态量估计结果更新各时刻从auv的姿态信息，并输出从auv的对准结果，即更新后的从auv姿态信息；
18、步骤十一、判断对准过程是否结束；
19、若未结束，则令l＝l+1，k＝(l-1)δt+1，并返回执行步骤四；
20、若结束，则完成整个对准过程，将从auv的姿态信息反馈回惯导系统。
21、本发明的有益效果是：
22、本发明主auv将自身位置以及主、从auv的相对距离发送给从auv，通过三次样条插值方法增加声学测距从auv有效位置信息数量，以惯导系统输出的从auv位置信息与插值后的从auv的位置信息差值为观测量，利用卡尔曼滤波在线进行对准，可进一步提高对准性能。而且相比于现有方法，本发明不需要单领航auv的外部信息作为基准。
1.一种基于声学测距的单领航auv水下动基座对准方法，其特征在于，所述方法具体包括以下步骤：
2.根据权利要求1所述的一种基于声学测距的单领航auv水下动基座对准方法，其特征在于，所述步骤一中，路径设计满足下述原则：
3.根据权利要求2所述的一种基于声学测距的单领航auv水下动基座对准方法，其特征在于，所述步骤四中，利用k-1时刻采集到的从auv惯性组件输出数据来更新k时刻从auv的惯导信息，具体为：
4.根据权利要求3所述的一种基于声学测距的单领航auv水下动基座对准方法，其特征在于，所述姿态转换矩阵的计算方法为：
5.根据权利要求4所述的一种基于声学测距的单领航auv水下动基座对准方法，其特征在于，所述步骤七的具体过程为：
6.根据权利要求5所述的一种基于声学测距的单领航auv水下动基座对准方法，其特征在于，所述步骤八的具体过程为：
7.根据权利要求6所述的一种基于声学测距的单领航auv水下动基座对准方法，其特征在于，所述步骤九中，将步骤八中筛选出的从auv位置信息以及对应的时刻作为已知数据节点进行插值，具体为：
8.根据权利要求7所述的一种基于声学测距的单领航auv水下动基座对准方法，其特征在于，所述插值函数同时满足下列条件(1)至条件(4)；
9.根据权利要求8所述的一种基于声学测距的单领航auv水下动基座对准方法，其特征在于，所述将auv位置误差、速度误差、姿态误差、陀螺仪误差和加速度计误差组成状态量，根据状态量和观测量进行卡尔曼滤波，得到各时刻状态量估计结果；
10.根据权利要求9所述的一种基于声学测距的单领航auv水下动基座对准方法，其特征在于，所述k时刻从auv观测量的获得方式为：
