一种应用于无人船行进间的改进粗对准方法。提出一种变化存储窗口长度的改进惯性系粗对准方法用于无人船行进间对准，变化窗口长度构造不同坐标系下的对应矢量，针对传统行进间对准算法多利用多普勒计程仪等测量运动目标载体系速度(vb)及忽略项的问题，利用对准算法估算出的姿态矩阵将GPS提供的导航坐标系(n系)速度(vn)转换为载体坐标系(b系)速度(vb)，导航坐标系角速度转换为载体坐标系角速度用于迭代估计姿态矩阵。与以往基于惯性系的动基座行进间对准相比，该算法最大程度的接近了INS运动方程，提高了对测量数据的利用率，可以提高无人船行进间对准的精度和稳定性。
1.一种应用于无人船行进间的改进粗对准方法，其特征在于，包括以下步骤：S1，根据当前采样时间和GPS提供无人船的真实位置信息，进行从初始时刻惯性坐标系即i0系到导航坐标系即n系转换矩阵 的更新；
所述步骤S1中，利用当前采样时间和GPS提供无人船的真实位置信息，进行矩阵 的更新，更新方程为：
其中，Δλ是当前t时刻经度λt相对于初始t0时刻经度λ0的增量， 是地球自转角速度在导航坐标系即n系的投影；
S2，利用IMU采样间隔内，陀螺仪输出载体坐标系三轴角运动信息，进行载体坐标系即b系到初始时刻载体坐标系即ib0系转换矩阵 的更新；
所述步骤S2中推导了 的姿态更新算法，类似导航坐标系姿态微分方程，推导所述姿态更新算法为，
其中， 是当前时刻载体坐标系即b系到初始时刻载体坐标系即ib0系的变换矩阵；
是当前时刻载体坐标系即b系相对于初始时刻载体坐标系即ib0系的转动角速度，假定在采样间隔Δt＝tn+1‑tn内，角速度矢量 的方向不发生变化，可得微分方程的解为：其中，
S3，依据INS比力方程，利用已有信息，进行速度v 的更新；
所述步骤S3进一步包括：S31，推导比力方程，所述比力方程为：n
其中v 是无人船导航坐标系即n系速度， 是导航坐标系即n系相对于惯性坐标系即i系的转动角速度， 是地理坐标系即e系相对惯性坐标系即i系的角速度在导航坐标系即nn n
系投影，f是无人船加速度在导航坐标系即n系的投影，g是重力矢量在导航坐标系即n系的投影；
S32，推导初始时刻惯性坐标系即i0系与初始时刻载体坐标系即ib0系对应矢量的转换关系，所述矢量转换关系为：ib0
其中，v 是无人船速度在初始时刻载体坐标系即ib0系投影， 是初始时刻惯性坐标i0
系即i0系到初始时刻载体坐标系即ib0系转换矩阵，v 是无人船速度在初始时刻惯性坐标系即i0系投影；
S33，推导速度v 的更新方程，所述更新方程为：b
其中，v是无人船速度在载体坐标系即b系的投影， 是载体坐标系即b系相对于惯性坐标系即i系转动角速度的估计值， 是无人船载体坐标系即b系加速度的估计值；
S34，根据现有信息对v 进行更新并存储，其中， 和 可由陀螺仪和加速度计的输出得到，对于其他项，若当前采样时间，尚未得到矩阵 的估计值，忽略 项，利用b
无人船加速度 估算无人船的速度v 用于矩阵 的估计；若已经由前面步骤得到矩阵ib0
的估计值，利用 进行速度信息和角速度信息的转换应用于v 的估计更新；
S4，利用S1求取的矩阵 的转置矩阵和重力矢量在导航坐标系即n系的投影进行积分i0
S41，推导了v 的更新方程，所述更新方程为：其中， 是从估算导航坐标系即 系到初始时刻惯性坐标系即i0系的转换矩阵；
S42，利用估计矩阵 和重力矢量g 在导航坐标系即n系的投影积分更新速度v 并存储；
S5，根据采样次数，调整窗口的长度，选取不同的窗口位置构造初始时刻载体坐标系即b0系和初始时刻惯性坐标系即i0系的对应矢量；
所述步骤S5进一步包括：S51，根据采样次数调整存储数据窗口的长度，IMU采样时间间隔一定，采样次数与实时采样时间有关，更新存储数据窗口的长度，使其始终对应开始对准时间到实时采样时间；
ib0 i0 ib0 i0S52，选取窗口最后数据v (tk2)、v (tk2)和窗口中间数据v (tk1)、v (tk1)进行初ib0
始时刻载体坐标系即b0系和初始时刻惯性坐标系即i0系对应矢量的构造，对应矢量为vib0 ib0 ib0 ib0 ib0 i0 i0 i0(tk1)、v (tk1)×v (tk2)、v (tk1)×v (tk2)×v (tk1)和v (tk1)、v (tk1)×vi0 i0 i0
(tk2)、v (tk1)×v (tk2)×v (tk1)；
S6，根据构造矢量及其他坐标系转换矩阵，进行姿态变换矩阵 的估计，并进行矩阵正定化处理，完成解算得到三个姿态角；
所述步骤S6进一步包括：S61，推导矩阵 的估计公式，根据S3中矢量的转换关系，所述矢量的转换关系的计算公式为：
S62，对矩阵 进行拆分，根据链式相乘法则，拆分方法为：对矩阵 进行正定化处理：S63，对无人船的三个姿态角进行解算，解算公式为：其中，R为横滚角，P为纵摇角，H为航向角；
S7，重复步骤S1到S6，直至完成所有采样时间的姿态矩阵 估计。
一种应用于无人船行进间的改进粗对准方法
技术领域
[0001] 本发明涉及无人船测绘技术领域，尤其涉及一种应用于无人船行进间的改进粗对准方法。
背景技术
[0002] 捷联惯性导航系统的粗对准，就是在一定的时间内，得到一个近似的姿态矩阵为后续的精对准工作做准备。在海上工作时，无人船受风浪流等海况条件的影响，将做六自
由度运动：横摇、纵摇、艏摇、垂荡、纵荡、横荡等，无人船不再属于传统意义上的静基座条
件。同时，由于工作的需要，无人船可能从被母船投放开始，就要进行航行作业，除外界因素
造成的干扰线速度、干扰加速度外，还存在自身的线速度、加速度。综合考虑，不同于单纯的
静止或摇摆，无人船的运动可以被视为行进加摇摆的复合运动，对该运动状态下无人船的
粗对准进行研究具有一定的现实意义。
[0003] 国内外对于粗对准技术的研究已经经历了一段漫长的时间。最经典的解析式粗对准利用重力加速度和地球自转角速度在不同坐标系的投影估算姿态矩阵 当载体存在摇
摆时无法从陀螺输出提取到地球自转角速度，解析式粗对准不再适用。为解决此问题，惯性
系粗对准被提出，但惯性系粗对准假定加速度计的输出只有重力加速度、干扰加速度、加速
度计常值偏置，未考虑载体存在运动的情况。针对行进间动基座的对准，国内也提出了很多
方法，其中被广泛采用的是外速度参考辅助行进间对准，为保证对准精度，通过滤波器或罗
经水平对准对速度进行平滑，增加了粗对准的工作量。在此基础上，为解决无人船行进间对
准的问题，本文提出一种变化存储窗口长度的改进惯性系粗对准方法，提高了对准的精度
和稳定性。
发明内容
[0004] 发明目的：针对以上问题，本发明提出一种应用于无人船行进间的改进粗对准方法，适用于无人船受海况影响做六自由度运动，同时因航行需求存在线速度和加速度的情
况，可有效提高对准的精度和稳定性。
[0005] 技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种应用于无人船行进间的改进粗对准方法，包括以下步骤：
[0006] S1，根据当前采样时间和GPS提供无人船的真实位置信息，进行从初始时刻惯性坐标系即i0系到导航坐标系即n系转换矩阵 的更新；
[0007] S2，利用IMU采样间隔内，陀螺仪输出载体坐标系三轴角运动信息，进行载体坐标系即b系到初始时刻载体坐标系即ib0系转换矩阵 的更新；
[0008] S3，依据INS比力方程，利用已有信息，进行速度vib0的更新；
[0009] S4，利用S1求取的矩阵 的转置矩阵和重力矢量在导航坐标系(n系)的投影进行i0
积分运算，得到速度v 的更新；
[0010] S5，根据采样次数，调整窗口的长度，选取不同的窗口位置构造初始时刻载体坐标系即b0系和初始时刻惯性坐标系即i0系的对应矢量；
[0011] S6，根据构造矢量及其他坐标系转换矩阵，进行姿态变换矩阵 的估计，并进行矩阵正定化处理，完成解算得到三个姿态角；
[0012] S7，重复步骤S1到S6，直至完成所有采样时间的姿态矩阵 估计。
[0013] 作为本发明进一步改进，所述步骤S1中，利用当前采样时间和GPS提供无人船的真实位置信息，进行矩阵 的更新，更新方程为：
[0014]
[0015] 其中，Δλ是当前t时刻经度λt相对于初始t0时刻经度λ0的增量， 是地球自转角速度在导航坐标系即n系的投影。
[0016] 作为本发明进一步改进，所述步骤S2中推导了 的姿态更新算法，类似导航坐标系姿态微分方程，推导所述姿态更新算法为，
[0017]
[0018] 其中， 是当前时刻载体坐标系即b系到初始时刻载体坐标系即ib0系的变换矩阵； 是当前时刻载体坐标系即b系相对于初始时刻载体坐标系即ib0系的转动角速度，
假定在采样间隔Δt＝tn+1‑tn内，角速度矢量 的方向不发生变化，可得微分方程的解为：
[0019]
[0020] 其中，
[0021]
[0022] 作为本发明进一步改进，所述步骤S3进一步包括：
[0023] S31，推导比力方程，所述比力方程为：
[0024]n
[0025] 其中v是无人船导航坐标系即n系速度， 是导航坐标系即n系相对于惯性坐标系即i系的转动角速度， 是地理坐标系即e系相对惯性坐标系即i系的角速度在导航坐标系
n n
即n系投影，f是无人船加速度在导航坐标系即n系的投影，g 是重力矢量在导航坐标系即n
系的投影；
[0026] S32，推导初始时刻惯性坐标系即i0系与初始时刻载体坐标系即ib0系对应矢量的转换关系，所述矢量转换关系为：
[0027]
[0028] 其中，vib0是无人船速度在初始时刻载体坐标系即ib0系投影， 是初始时刻惯性i0
坐标系即i0系到初始时刻载体坐标系即ib0系转换矩阵，v 是无人船速度在初始时刻惯性
坐标系即i0系投影；
[0029] S33，推导速度vib0的更新方程，所述更新方程为：
[0030]b
[0031] 其中，v是无人船速度在载体坐标系即b系的投影， 是载体坐标系即b系相对于惯性坐标系即i系转动角速度的估计值， 是无人船载体坐标系即b系加速度的估计值；
ib0
[0032] S34，根据现有信息对v 进行更新并存储，其中， 和 可由陀螺仪和加速度计的输出得到，对于其他项，若当前采样时间，尚未得到矩阵 的估计值，忽略
b
项，利用无人船加速度 估算无人船的速度v用于矩阵 的估计；若已经由前面步骤得到
ib0
矩阵 的估计值，利用 进行速度信息和角速度信息的转换应用于v 的估计更新；
[0033]
[0034]
[0035] 作为本发明进一步改进，所述步骤S4进一步包括：
[0036] S41，推导了vi0的更新方程，所述更新方程为：
[0037]
[0038] 其中， 是从估算导航坐标系即 系到初始时刻惯性坐标系即i0系的转换矩阵；
[0039] S42，利用估计矩阵 和重力矢量gn在导航坐标系即n系的投影积分更新速度vi0并存储。
[0040] 作为本发明进一步改进，所述步骤S5进一步包括：
[0041] S51，根据采样次数调整存储数据窗口的长度，IMU采样时间间隔一定，采样次数与实时采样时间有关，更新存储数据窗口的长度，使其始终对应开始对准时间到实时采样时
间；
[0042] S52，选取窗口最后数据vib0(tk2)、vi0(tk2)和窗口中间数据vib0(tk1)、vi0(tk1)进行初始时刻载体坐标系即b0系和初始时刻惯性坐标系即i0系对应矢量的构造，对应矢量为
ib0 ib0 ib0 ib0 ib0 ib0 i0 i0
v (tk1)、v (tk1)×v (tk2)、v (tk1)×v (tk2)×v (tk1)和v (tk1)、v (tk1)×
i0 i0 i0 i0
v (tk2)、v (tk1)×v (tk2)×v (tk1)。
[0043] 作为本发明进一步改进，所述步骤S6进一步包括：
[0044] S61，推导矩阵 的估计公式，根据S3中矢量的转换关系，所述计算公式为：
[0045]
[0046] S62，对矩阵 进行拆分，根据链式相乘法则，拆分方法为：
[0047]
[0048] 对矩阵 进行正定化处理：
[0049]
[0050] S63，对无人船的三个姿态角进行解算，解算公式为：
[0051]
[0052]
[0053]
[0054] 其中，R为横滚角，P为纵摇角，H为航向角。
[0055] 与现有技术相比，本发明的有益效果为：
[0056] (1)区别于以往行进间动基座对准，只利用两个时间点的传感器输出，本算法利用改变存储数据窗口的长度，对所有测量数据进行了利用，用于 的估计。提高了对测量数
据的利用率，降低了对准的随机性。
[0057] (2)以往的外速度辅助行进间对准多采用里程计差分获得 这样必然会带来很大的噪声。本算法利用GPS能够提供精度较高的位置信息和速度信息，利用估算的矩阵
n b
将GPS提供的导航坐标系速度v转换为载体坐标系速度v ，迭代进行矩阵 的估算，减少因
测量带来的噪声干扰。
[0058] (3)在对速度vib0的更新中，以往的算法常因 无法获得，忽略 对速度ib0
更新的影响。本算法利用估算的矩阵 将角速度 转换为 减少对速度v 更新的误差，
提高对准精度。
附图说明
[0059] 图1为本发明的应用于无人船行进间的改进粗对准算法流程图；
[0060] 图2为利用不同信息更新速度vib0的示意图；
[0061] 图3为存储数据的窗口长度改变的示意图。
具体实施方式
[0062] 下面结合附图和实施例对本发明的技术方案作进一步的说明。
[0063] 本发明提出一种应用于无人船行进间的改进粗对准方法，适用于无人船受海况影响做六自由度运动，同时因航行需求存在线速度和加速度的情况，可有效提高对准的精度
和稳定性。
[0064] 如图1所示，本发明的应用于无人船行进间的改进粗对准算法，具体步骤如下：
[0065] S1，根据当前采样时间和GPS提供无人船的真实位置信息，进行从初始时刻惯性坐标系(i0系)到导航坐标系(n系)转换矩阵 的更新。
[0066]
[0067] S2，利用IMU采样间隔内，陀螺仪输出载体坐标系三轴角运动信息，进行载体坐标系(b系)到初始时刻载体坐标系(ib0系)转换矩阵 的更新。
[0068] 假定在采样间隔Δt＝tn+1‑tn内，角速度矢量 的方向不发生变化，可以得到矩阵 的近似值。
[0069]
[0070]
[0071]
[0072] S3，依据INS比力方程，利用已有信息，进行速度vib0的更新。
[0073]ib0
[0074] 根据现有信息对v 进行更新并存储。其中， 和 可由陀螺仪和加速度计的输出得到。对于其他项，若当前采样时间，尚未得到矩阵 的估计值，忽略 项，利用
b
无人船加速度 估算无人船的速度v 用于矩阵 的估计；若已经由前面步骤得到矩阵
ib0
的估计值，利用 进行速度信息和角速度信息的转换应用于v 的估计更新。
[0075]
[0076]
[0077] S4，利用S1求取的矩阵 的转置矩阵和重力矢量在导航坐标系(n系)的投影进行i0
积分运算，得到速度v 的更新。
[0078]
[0079] S5，根据采样次数，调整窗口的长度，选取不同的窗口位置构造初始时刻载体坐标系(b0系)和初始时刻惯性坐标系(i0系)的对应矢量。
[0080] IMU的采样时间间隔一定，采样次数与实时采样时间成正比，对存储数据的窗口长度进行更新，窗口始终对应开始对准时间到实时采样时间。
[0081] 构造初始时刻载体坐标系(b0系)和初始时刻惯性坐标系(i0系)的对应矢量：
[0082] vib0(tk1)vi0(tk1)
[0083] vib0(tk1)×vib0(tk2)vi0(tk1)×vi0(tk2)
[0084] vib0(tk1)×vib0(tk2)×vib0(tk1)vi0(tk1)×vi0(tk2)×vi0(tk1)
[0085] 根据构造矢量及其他坐标系转换矩阵，进行姿态变换矩阵 的估计，并进行矩阵正定化处理，完成解算得到三个姿态角。
[0086] 估计初始时刻载体坐标系(b0系)和初始时刻惯性坐标系(i0系)转换矩阵
[0087]
[0088] 估计矩阵 根据链式相乘法则得：
[0089]
[0090] 对矩阵 进行正定化处理：
[0091]
[0092] 利用矩阵 解算得到三个姿态角：
[0093]
[0094]
[0095]
[0096] 具体实例：
[0097] 为了验证本发明算法的有效性，进行相关仿真验证。设置360s的仿真时间，无人船的航速设置为4m/s，仿真过程中无人船的横摇角、纵摇角、航向角持续存在周期性的震荡，
同时存在垂荡、横荡、纵荡三个方向的干扰线速度。仿真结果表明，在设定仿真时间内，横摇
角对准精度可达6′，纵摇角对准精度可达5′，方向角对准精度可达30′，精度和稳定性满足
粗对准的要求。
[0098] 以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范
围。
