【技术实现步骤摘要】
本专利技术属于惯性基导航初始对准，尤其涉及一种基于se2(3)流形的sins/dvl回溯式初始对准方法及装置。
技术介绍
1、自主水下航行器(autonomous underwater vehicle,auv)是探索和开发海洋的重要技术装备。auv在完成各种水下作业时，离不开高精度的水下导航技术。目前，捷联惯导/多普勒计程仪(sins/dvl)组合导航是auv最为常用的导航方案，可满足长航时高精度的水下导航要求。
2、但是，sins/dvl组合系统在载体行驶导航之前通常须先进行初始对准，以确定载体坐标系到导航坐标系的初始姿态矩阵，进而减小导航解算的姿态、速度和位置误差。
3、在水下环境中，sins/dvl的初始对准通常是在动基座下完成的，这将导致对准时间增加和对准精度降低。传统初始对准方法通常分为粗对准和精对准两个独立阶段。在某些特殊应用场合，受系统精度、对准时间和实际工作环境所限，粗对准环节无法满足后续精对准线性模型要求，此时线性误差模型和线性卡尔曼滤波算法无法适用，需要对大失准角下的初始对准问题和相关滤波算法进行研究，所以就有学者提出用非线性模型将对准来解决大失准角的问题。然而，非线性滤波方法需要足够多的传感器数据滤波结果才会收敛，对准时间比较长，难以解决缩短对准时间的同时保证对准精度这一主要矛盾。
技术实现思路
1、本专利技术的目的是提供一种基于se2(3)流形的sins/dvl回溯式初始对准方法及装置，将iekf与回溯式对准结合，可以在缩短对准时间的
2、本专利技术采用以下技术方案：一种基于se2(3)流形的sins/dvl回溯式初始对准方法，包括以下步骤：
3、获取惯导系统输出的第一载体姿态阵、速度矢量和位置矢量；
4、将第一载体姿态阵、速度矢量和位置矢量作为se2(3)的群元素，生成系统误差传递矩阵；其中，第一载体姿态阵为载体坐标系与导航惯性坐标系之间的载体姿态阵；
5、根据系统误差传递矩阵计算状态一步转移矩阵；
6、基于状态一步转移矩阵计算载体的状态误差估计，并基于状态误差估计校正载体状态量。
7、进一步地，系统误差传递矩阵具体为：
8、
9、其中，fr表示系统误差传递矩阵，表示载体坐标系和导航惯性坐标系之间的第一载体姿态阵，b表示载体坐标系，n0表示导航惯性坐标系，gn0表示导航惯性坐标系下的重力矢量，表示导惯导系统输出的导航惯性坐标系下的速度矢量，表示惯导系统输出的导航惯性坐标系下的位置矢量。
10、进一步地，根据系统误差传递矩阵计算状态一步转移矩阵包括：
11、φr,k/k-1＝i15×15+fr，k*δt*2，
12、其中，φr,k/k-1表示k时刻基于李群右不变误差的状态一步转移矩阵，i15×15表示十五维的单位矩阵，fr，k表示k时刻基于李群右不变误差的系统误差传递矩阵，δt表示传感器的采样时间间隔。
13、进一步地，基于状态估计校正载体状态量包括：
14、提取状态估计中的姿态误差；
15、基于姿态误差修正初始载体姿态阵，初始载体姿态阵为载体惯性坐标系与导航惯性坐标系之间的载体姿态阵；
16、利用修正后的初始载体姿态阵、第二载体姿态阵和第三载体姿态阵计算得到第四载体姿态阵；其中，第二载体姿态阵为导航坐标系和导航惯性坐标系之间的载体姿态阵，第三载体姿态阵为载体坐标系和载体惯性坐标系之间的载体姿态阵，第四载体姿态阵为载体坐标系和导航坐标系之间的载体姿态阵。
17、进一步地，基于姿态误差修正初始载体姿态阵包括：
18、
19、其中，表示修正后的初始载体姿态阵，表示k时刻状态误差估计中的姿态误差，表示初始载体姿态阵。
20、进一步地，利用修正后的初始载体姿态阵、第二载体姿态阵和第三载体姿态阵计算得到第四载体姿态阵包括：
21、
22、其中，表示第四载体姿态阵，表示第二载体姿态阵，表示第三载体姿态阵。
23、进一步地，基于状态估计校正载体状态量还包括：
24、
25、其中，表示k时刻载体在导航惯性坐标系下校正后的速度矢量，表示k时刻惯导系统输出的载体在导航惯性坐标系下的速度矢量，表示k时刻状态误差估计中的速度误差，表示k时刻状态误差估计中的姿态误差。
26、进一步地，基于状态估计校正载体状态量还包括：
27、
28、其中，表示k时刻载体在导航惯性坐标系下校正后的位置矢量，表示k时刻惯导系统输出的载体在导航惯性坐标系下的位置矢量，表示k时刻状态误差估计中的位置误差。
29、进一步地，基于状态估计校正载体状态量还包括：
30、通过第四载体姿态阵将校正后的位置矢量和校正后的速度矢量均转换至导航坐标系中。
31、本专利技术的另一种技术方案：一种基于se2(3)流形的sins/dvl回溯式初始对准装置，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述的方法。
32、本专利技术的有益效果是：本专利技术通过n0系下的载体状态姿态、速度和位置用李群状态表示，并基于李群状态误差表示构建n0系下惯导系统误差模型，最后以此惯导系统误差模型设计滤波模型，实现回溯对准过程，提高sins/dvl初始对准精度和对准速度。
本文档来自技高网...【技术保护点】
1.一种基于SE2(3)流形的SINS/DVL回溯式初始对准方法，其特征在于，包括以下步骤：
2.如权利要求1所述的一种基于SE2(3)流形的SINS/DVL回溯式初始对准方法，其特征在于，所述系统误差传递矩阵具体为：
3.如权利要求2所述的一种基于SE2(3)流形的SINS/DVL回溯式初始对准方法，其特征在于，根据所述系统误差传递矩阵计算状态一步转移矩阵包括：
4.如权利要求3所述的一种基于SE2(3)流形的SINS/DVL回溯式初始对准方法，其特征在于，基于所述状态估计校正载体状态量包括：
5.如权利要求4所述的一种基于SE2(3)流形的SINS/DVL回溯式初始对准方法，其特征在于，基于所述姿态误差修正初始载体姿态阵包括：
6.如权利要求5所述的一种基于SE2(3)流形的SINS/DVL回溯式初始对准方法，其特征在于，利用修正后的初始载体姿态阵、第二载体姿态阵和第三载体姿态阵计算得到第四载体姿态阵包括：
7.如权利要求6所述的一种基于SE2(3)流形的SINS/DVL回溯式初始对准方法，其特征在于，基
8.如权利要求7所述的一种基于SE2(3)流形的SINS/DVL回溯式初始对准方法，其特征在于，基于所述状态估计校正载体状态量还包括：
9.如权利要求8所述的一种基于SE2(3)流形的SINS/DVL回溯式初始对准方法，其特征在于，基于所述状态估计校正载体状态量还包括：
10.一种基于SE2(3)流形的SINS/DVL回溯式初始对准装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1-9任一项所述的方法。
...【技术特征摘要】
1.一种基于se2(3)流形的sins/dvl回溯式初始对准方法，其特征在于，包括以下步骤：
2.如权利要求1所述的一种基于se2(3)流形的sins/dvl回溯式初始对准方法，其特征在于，所述系统误差传递矩阵具体为：
3.如权利要求2所述的一种基于se2(3)流形的sins/dvl回溯式初始对准方法，其特征在于，根据所述系统误差传递矩阵计算状态一步转移矩阵包括：
4.如权利要求3所述的一种基于se2(3)流形的sins/dvl回溯式初始对准方法，其特征在于，基于所述状态估计校正载体状态量包括：
5.如权利要求4所述的一种基于se2(3)流形的sins/dvl回溯式初始对准方法，其特征在于，基于所述姿态误差修正初始载体姿态阵包括：
6.如权利要求5所述的一种基于se2(3)流形的sins/dvl回溯式初始对准方法...
【专利技术属性】
技术研发人员：胡高歌，向鹏，常路宾，高兵兵，耿佳乐，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。