[发明专利]基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法

说明书

本发明公开了一种基于多普勒计程仪(Doppler Velocity Log，DVL)辅助和向量截断化K矩阵的水下载体初始对准方法，首先获取各传感器数据，并对数据预处理；根据传感器数据计算观测矢量和参考矢量，建立K矩阵的状态矩阵方程和量测矩阵方程；利用K矩阵的零迹性和对称性，将状态矩阵方程和量测矩阵方程向量化后再截断；然后利用卡尔曼滤波估计最优K矩阵；从最优估计K矩阵中提取出最优四元数，将其转换成姿态矩阵方程；最后利用姿态矩阵链式方程，得到当前时刻的姿态矩阵，解算出失准角，完成初始对准过程。本发明打破了传统的初始对准框架，不再区分粗对准和精对准过程，也不再区分大失准角以及小失准角，改进了初始对准的收敛速度和精度。

技术领域

本发明涉及导航系统初对准技术领域中，基于Wahba问题的最优K矩阵姿态估计方法，尤其涉及一种基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法。

背景技术

海洋观测是认识海洋的基本手段，是海洋经济开发、环境保护和权益维护的基础。实施“透明海洋”战略，加强海洋观测技术装备开发，加快海洋观测体系建设，是目前海洋科技创新的一个重要方向。水下自主潜航器(Autonomous Underwater Vehicle,AUV)作为海洋观测技术的重要装备，以其隐蔽性好、可代替人类执行危险任务等优点，受到研究人员广泛关注。

目前，水下载体包含有水下自主潜航器(Autonomous Underwater Vehicle,AUV)、遥控无人潜水器ROV，以及混合水下无人载体HUV。其中，AUV作为水下载体在执行各种水下任务时，要求导航、制导与控制系统之间能够相互协调实现正常运转，导航技术是决定AUV技术水平的一个特别重要标志。显然，要达到定位的目的，需要已知运载体的初始速度和位置，初始对准精度的高低很大程度上决定了导航的精度。捷联惯性导航系统初始对准分为粗对准和精对准两部分，其中精对准需要在粗对准的基础上完成。

基于惯性系的姿态确定(Attitude Determination,AD)方法能够提高水下载体的初始对准性能，目前，采用姿态确定来进行初始对准时，有两种方法：一种是确定姿态矩阵(DCM)，然而，由于惯性测量单元(IMU)测量的观测矢量加速度包含随机噪声，该方法并没有获得良好的粗对准性能。另一种解决AD问题的方法是确定相应的姿态四元数。众多学者在此基础上提出了多种确定姿态四元数的方法，其中D.Choukroun等人研究的Optimal-REQUEST算法具有鲁棒性强，计算精度高等优点，是四元数的最优递归时变估计。但Optimal-REQUEST算法依赖于保守的估计性能指标和标量增益来估计K矩阵，而标量增益的设定很大程度上决定了算法的精度。

因此，如何通过具体的方法来对水下载体进行快速的初始对准，进而克服现有技术中的不足之处，成为亟需解决的技术问题。

发明内容

发明目的：在对水下载体进行初始对准时，为克服Optimal-REQUEST算法依赖于一个保守的估计性能指标和标量增益来估计K矩阵的缺点，本发明提供了一种基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法，将K矩阵的矩阵状态空间方程进行向量化，并利用K矩阵元素之间的线性相关性截断得到状态向量，在其基础上应用卡尔曼滤波来完成对水下载体的初始对准过程，解决了量测矢量存在噪声影响时初始对准问题。

技术方案：本发明基于DVL辅助和向量截断化K矩阵的水下载体初始对准方法包括以下步骤：

(1)定义解算所需的参考标准坐标系；

(2)获取惯性传感器实时数据以及DVL实时数据；

(3)根据载体的比力方程将比力方程两边同时积分，再忽略小量得到如下表达式：进而构造观测矢量ξ和参考矢量r；然后根据步骤(2)获取的惯性传感器实时数据和DVL实时数据，来解算当前时刻的观测矢量ξ和参考矢量r；

观测矢量ξ和参考矢量r的构造方法如式(1)至式(4)所示：

(3.1)构造观测矢量ξ：