一种双轴旋转惯导系统转位机构非正交误差标定方法

1.本发明涉及惯性导航技术领域，具体涉及双轴旋转惯导系统转位机构非正交误差标定方法，适用于双轴旋转惯导系统转位机构的非正交误差标定和载体姿态提取。


背景技术：

2.惯性导航系统(inertial navigation system，ins)由惯性测量单元(inertial measurement unit，imu)、壳体、控制电路、导航计算机等构成。惯性导航系统可以利用其中的惯性测量单元测量运载体的角速度与加速度，再利用导航计算机对角速度和加速度进行解算以得到运载体的姿态、速度和位置。由于惯性导航系统是基于牛顿第二定律的积分推算系统，因此不可避免地误差会随时间积累。
3.捷联惯导系统是一种直接与运载体固联的惯导系统，因此捷联惯导系统计算出的惯性测量单元的姿态、速度和位置就是运载体的姿态、速度和位置。旋转惯导系统是一种利用转位机构使惯性测量单元周期性旋转，使惯性测量单元中的常值误差正负抵消，以达到高精度导航定位的惯性导航系统，其携带的旋转调制系统可以消除惯性器件常值误差、安装误差、刻度因数误差等引入的导航误差。旋转惯导系统不同于捷联惯导系统，由于惯性测量单元一直在相对运载体做周期性旋转，因此无法直接计算得到载体的姿态信息。
4.而在旋转惯导系统的某些应用场景中，对载体姿态要求较高，如高动态航空旋转惯导系统、长航时旋转惯导系统的角运动隔离等。因此，研究旋转惯导系统的高精度载体姿态解算技术具有重要的现实意义。如果需要提取旋转惯导系统的载体姿态信息，必须准确地与转位机构的姿态进行解耦。然而在实际的双轴旋转惯导系统中，惯性测量单元与转位机构间的安装不可避免存在非正交误差，双轴转位机构的内框和外框在安装时又存在非正交误差。因此如果不考虑非正交误差，直接进行载体姿态提取得到的姿态误差将很大。为了得到高精度的载体姿态信息，必须将转位机构的非正交误差进行标定并补偿。


技术实现要素：

5.本发明提出一种双轴旋转惯导系统转位机构非正交误差标定方法，可有效解决双轴旋转惯导系统的非正交误差标定问题，实现高精度的误差标定及载体姿态提取。
6.本发明采用的技术方案为，一种双轴旋转惯导系统转位机构非正交误差标定方法，所述方法包括：
7.建立双轴旋转惯导系统转位机构非正交误差模型；启动双轴旋转惯导系统，按规定的路径进行旋转，并采集数据；然后根据所述转位机构非正交误差模型，构建基于粒子群优化算法的双轴旋转惯导系统非正交误差标定方法；最后利用标定结果，将惯性测量单元的姿态与转位机构解耦，得到高精度的载体姿态信息。
8.具体步骤如下：
9.s1：建立双轴旋转惯导系统转位机构非正交误差模型
10.首先进行坐标系的定义：
11.imu坐标系，简称s系：imu坐标系的三轴与imu中正交安装的三个激光陀螺的敏感轴重合；
12.内框坐标系，简称in系：内框坐标系与双轴转位机构的内框架固联，内框坐标系的z
in
轴与双轴转位机构的内转轴重合，imu安装于内框架上，imu坐标系与内框坐标系的空间相对位置不变，imu坐标系与内框坐标系间存在固定的安装偏角；
13.外框坐标系，简称out系：外框坐标系与双轴转位机构的外框架固联，外框坐标系的x
out
轴与双轴转位机构的外转轴重合；
14.内框零位坐标系，简称in0系：定义双轴转位机构的内转轴转动角度为0时，内框坐标系所处的位置为内框零位坐标系，内框零位坐标系与外框架的空间相对位置不变，内框零位坐标系与外框坐标系间存在固定的安装偏角；
15.载体坐标系，简称b系：载体坐标系的x-y-z轴分别指向载体的右-前-上，载体与惯导系统固联，双轴转位机构的外转轴转动角度为0时，外框坐标系与载体坐标系重合；
16.导航坐标系，简称n系：定义导航坐标系的x-y-z轴分别指向地理东-北-天向；
17.s1.1构建imu坐标系与转位机构内框坐标系间的非正交误差模型
18.定义θ
x
,θy,θz为imu坐标系和内框坐标系间的非正交误差角，则imu坐标系到内框坐标系的姿态转移矩阵为：
[0019][0020]
由于非正交误差角通常很小(江一夫,李四海,严恭敏,谢波.基于双轴旋转调制惯性测量的载体导航信息提取方法[j].中国惯性技术学报,2022,30(03):304-308+315.)，基于小角假设，sinθ≈θ,cosθ≈1，同时忽略高阶项，imu坐标系到内框坐标系的姿态转移矩阵简化为：
[0021][0022]
s1.2构建转位机构内框零位坐标系与外框坐标系间的非正交误差模型
[0023]
定义η
x
,ηy,ηz为内框零位坐标系和外框坐标系间的非正交误差角，则内框零位坐标系到外框坐标系的姿态转移矩阵为：
[0024][0025]
由于非正交误差角通常很小，基于小角假设，sinη≈η,cosη≈1，同时忽略高阶项，内框零位坐标系到外框坐标系的姿态转移矩阵简化为：
[0026][0027]
s1.3构建双轴旋转惯导系统的载体姿态传递模型
[0028]
根据矩阵链乘法则，双轴旋转惯导系统的载体姿态传递模型如下：
[0029][0030]
式中为载体坐标系到导航坐标系的姿态转移矩阵，也就是载体姿态；为imu坐标系到导航坐标系的姿态转移矩阵，也就是imu的姿态，由imu测量并通过导航计算机解算直接得到；为内框坐标系到imu坐标系的姿态转移矩阵，其中表示的转置；表示外框坐标系到内框零位坐标系的姿态转移矩阵，表示外框坐标系到内框零位坐标系的姿态转移矩阵，为内框坐标系到内框零位坐标系的姿态转移矩阵，α为内框转动角度；为载体坐标系到外框坐标系的姿态转移矩阵，β为外框转动角度；和分别为：
[0031][0032]
s2：启动双轴旋转惯导系统，按规定的路径进行旋转，并采集数据
[0033]
s2.1启动双轴旋转惯导系统，导航计算机实时采集imu测量的数据和转位机构的测角速度，控制转位机构进行以下次序的旋转：
[0034]
1.开机，进行10分钟静基座对准，获得imu的初始姿态
[0035]
2.内框正转180度；
[0036]
3.外框正转180度；
[0037]
4.外框反转180度；
[0038]
5.内框反转180度；
[0039]
6.结束数据采集，关机；
[0040]
对准过程可参考《捷联惯导算法与组合导航原理》，严恭敏，翁浚编著，西北工业大学出版社，2019.08；
[0041]
s3：构建基于粒子群优化算法的双轴旋转惯导系统非正交误差标定方法
[0042]
s3.1定义粒子群算法的粒子格式
[0043]
定义粒子群中的第i个粒子的位置为pi，pi包含6个参数，为其中分别对应式(2)中imu坐标系到内框坐标系的姿态转移矩阵和式(4)中内框零位坐标系与外框坐标系的姿态转移矩阵的六个失准角误差值，由第i个粒子的位置pi的6个粒子参数编排构成以下非正交误差的形式：
[0044][0045]
[0046]
式中，表示括号中的矩阵由第i个粒子的位置参数编排而成；
[0047]
s3.2构建粒子群优化算法的适应度函数
[0048]
以由双轴旋转惯导系统提取出的载体姿态误差的均方根误差为适应度，构建粒子群优化算法的适应度函数，将标定问题转化为优化问题，利用粒子群优化算法进行求解；
[0049]
定义适应度函数为：
[0050][0051]
式(9)中，fit(pi)为第i个粒子在位置pi时的适应度函数；表示计算得到的载体在t时刻相对于初始0时刻的姿态转移矩阵，在t时刻相对于初始0时刻的姿态转移矩阵，表示t时刻提取到的载体相对于n系的姿态矩阵；表示初始0时刻的n系相对于载体的姿态矩阵；
×
表示矩阵叉乘；由于标定过程中惯导系统保持静止，所以理论上t时刻相对于0时刻惯导系统姿态保持不变，为单位矩阵，计算得到的即为由非正交误差引起的载体姿态误差矩阵；
[0052]
计算过程为：
[0053][0054]
计算过程为：
[0055][0056]
式(10)与(11)中，表示t时刻imu进行测量并解算出的姿态；和为根据式(7)和式(8)所示，由第i个粒子pi编排组成的非正交误差矩阵；表示t时刻内框转动角度为α
t
时，内框零位坐标系到内框坐标系的姿态转移矩阵，表示t时刻外框转动角度为β
t
时，载体坐标系到外框坐标系的姿态转移矩阵；
[0057]
式(9)中，sum(
·
)表示求和函数，euler(
·
)表示将括号中变量转换为欧拉角；姿态转移矩阵到欧拉角的转换及式(11)中捷联惯性解算的过程可参考《捷联惯导算法与组合导航原理》严恭敏，翁浚编著，西北工业大学出版社，2019.08。
[0058]
s3.3利用s3.2构建的适应度函数，采用粒子群优化算法进行寻优
[0059]
粒子群优化算法通过建立一个由多个粒子组成的种群，其中每个粒子的位置代表着一个潜在的解，通过粒子的简单行为和种群内部的信息交互来实现解决问题的智能求解；每个粒子搜索过程的总迭代次数为g，粒子的运动过程就是求解最优解的搜索过程，粒子的运动速度可以根据粒子历史最优位置和种群历史最优位置动态调整；搜索过程中第i个粒子具有最佳适应度的位置，称为个体最优位置种群中具有最佳适应度粒子的位置称为全局最优位置g
best
；
[0060]
第i个粒子在d次迭代时位置为第i个粒子在d次迭代时位置为d＝1,2,
…
,g；结合式(9)、(10)和(11)，相对应的适应度函数为：
[0061][0062][0063][0064]
式(12)和(13)中，下标d为当前粒子群的迭代次数，最大迭代次数为g；i＝1，2，...，n为粒子数序号，n为粒子群规模，也就是一个粒子群中，粒子的总个数；r1，r2是介于(0,1)之间的随机数；c1，c2为学习因子，分别表征粒子向自身和其他粒子学习的能力，通常取固定值2；ω为惯性权重常数，用来调整粒子的多样性，取值一般在[0.4,0.9]之间，ω取值越大，全局寻优能力越强，ω取值越小，局部寻优能力越强，为平衡全局寻优能力与局部寻优能力，通常取ω＝0.6；表示粒子的当前速度，v
gmin
,v
gmax
分别表示粒子当前的最小和最大速度，由非正交误差角的大小决定，非正交误差角通常小于1
°
，因此可定义v
gmax
＝1,v
gmin
＝-1；综合计算机运算能力以及工程经验，n＝50。
[0065]
利用式(12)计算第i个粒子在第d次迭代时位置的适应度，即可重新评估第i个粒子适应度最佳的个体最优位置计算粒子群中所有粒子在第d次迭代时的适应度，即可重新评估出当前适应度最佳的全局最优位置g
best
。
[0066]
当达到最大迭代次数(例如150次)或满足终止要求时(设定适应度小于0.01时即满足终止要求)，全局最优位置的适应度fit(g
best
)全局最小；将全局最优位置g
best
的六个参数代入式(7)和式(8)得到(15)和(16)，即为双轴旋转惯导转位机构准确的非正交误差：
[0067][0068][0069]
将式(15)和式(16)代入式(17)即可得到高精度的载体姿态
[0070][0071]
式中，为惯性解算得到的imu的姿态矩阵，和为粒子群优化算法标定得到的非正交误差矩阵(15)、(16)，为内框实时转动角度利用式(6)转换为的姿态矩阵，为外框实时转动角度利用式(6)转换为的姿态矩阵。为通过实时补偿后得到的高精度的载体姿态。
[0072]
本发明具有以下技术效果：
[0073]
1.本发明利用粒子群优化算法将标定问题转换为优化问题，可准确标定出双轴旋转惯导的非正交误差；
[0074]
2.相对于传统基于向量投影的误差标定方法，此发明的标定精度更高；
[0075]
3.本发明不需要利用任何外部设备进行辅助即可标定出非正交误差，节约了成本；
[0076]
4.本发明将双轴转位机构的非正交误差精确标定后，可以显著提升载体姿态提取的精度；进一步地可以提升双轴惯性导航系统的导航精度；
[0077]
5.本发明可以为双轴旋转惯导系统的载体角运动隔离技术提供技术支持。
附图说明
[0078]
图1：imu与转位机构内框坐标系的空间相对位置；
[0079]
图2：内框坐标系与外框坐标系的空间相对位置；
[0080]
图3：粒子群优化算法流程图；
[0081]
图4：不同标定方法的姿态误差对比。
具体实施方式
[0082]
为详细说明本发明公开的技术方案，下面结合具体实施例做进一步的阐述。
[0083]
本发明的可行性和有效性通过国防科技大学自主研制的双轴旋转惯导系统进行了验证：
[0084]
第一步：将双轴旋转惯导系统放置于平面，根据s2进行操作，启动双轴旋转惯导系统，按规定的路径进行旋转，并采集数据；
[0085]
启动双轴旋转惯导系统，导航计算机实时采集imu测量的数据和转位机构的测角速度，控制转位机构进行以下次序的旋转：
[0086]
1.开机，进行10分钟静基座对准，获得imu的初始姿态
[0087]
2.内框正转180度；
[0088]
3.外框正转180度；
[0089]
4.外框反转180度；
[0090]
5.内框反转180度；
[0091]
6.结束数据采集，关机。
[0092]
对准过程可参考《捷联惯导算法与组合导航原理》，严恭敏，翁浚编著，西北工业大学出版社，2019.08。
[0093]
第二步：根据s3进行操作，通过粒子群优化算法对双轴旋转惯导非正交误差进行标定；
[0094]
得到粒子的全局最优位置g
best
：
[0095]gbest
＝[-0.000279
ꢀ‑
0.000963
ꢀ‑
0.002212 0.000721
ꢀ‑
0.000806
ꢀ‑
0.009205]
°
[0096]
粒子群优化算法通过迭代得到的全局最优位置g
best
是适应度函数(12)的最优解，将全局最优位置g
best
的六个参数代入式(7)和式(8)得到(18)和(19)，即为双轴旋转惯导转位机构准确的非正交误差：
[0097]
[0098][0099]
第三步：将双轴旋转惯导系统放置于大理石工作平台，进行十六次序导航工作，实时补偿第二步标定得到的非正交误差，提取载体姿态信息。
[0100]
载体姿态的实时补偿公式为：
[0101][0102]
式中，为惯性解算得到的imu的姿态矩阵，和为非正交误差矩阵，通过第二步标定得到，为内框实时转动角度转换为的姿态矩阵，为外框实时转动角度转换为的姿态矩阵。为通过实时补偿后得到的高精度的载体姿态。计算双轴旋转惯导系统提取的载体姿态信息的误差，如图4所示。
[0103]
图4中传统方法为文献(江一夫,李四海,严恭敏等.基于双轴旋转调制惯性测量的载体导航信息提取方法[j].中国惯性技术学报,2022,30(03):304-308+315.doi:10.13695/j.cnki.12-1222/o3.2022.03.004.)的方法。通过对比发现，本发明的精度优势明显。

技术特征：
1.一种双轴旋转惯导系统转位机构非正交误差标定方法，其特征在于，所述方法包括：建立双轴旋转惯导系统转位机构非正交误差模型；启动双轴旋转惯导系统，按规定的路径进行旋转，并采集数据；然后根据所述转位机构非正交误差模型，构建基于粒子群优化算法的双轴旋转惯导系统非正交误差标定方法；最后利用标定结果，将惯性测量单元的姿态与转位机构解耦，得到高精度的载体姿态信息；具体步骤如下：s1：建立双轴旋转惯导系统转位机构非正交误差模型首先进行坐标系的定义：imu坐标系，简称s系：imu坐标系的三轴与imu中正交安装的三个激光陀螺的敏感轴重合；内框坐标系，简称in系：内框坐标系与双轴转位机构的内框架固联，内框坐标系的z
in
轴与双轴转位机构的内转轴重合，imu安装于内框架上，imu坐标系与内框坐标系的空间相对位置不变，imu坐标系与内框坐标系间存在固定的安装偏角；外框坐标系，简称out系：外框坐标系与双轴转位机构的外框架固联，外框坐标系的x
out
轴与双轴转位机构的外转轴重合；内框零位坐标系，简称in0系：定义双轴转位机构的内转轴转动角度为0时，内框坐标系所处的位置为内框零位坐标系，内框零位坐标系与外框架的空间相对位置不变，内框零位坐标系与外框坐标系间存在固定的安装偏角；载体坐标系，简称b系：载体坐标系的x-y-z轴分别指向载体的右-前-上，载体与惯导系统固联，双轴转位机构的外转轴转动角度为0时，外框坐标系与载体坐标系重合；导航坐标系，简称n系：定义导航坐标系的x-y-z轴分别指向地理东-北-天向；s1.1构建imu坐标系与转位机构内框坐标系间的非正交误差模型定义θ
x
,θ
y
,θ
z
为imu坐标系和内框坐标系间的非正交误差角，则imu坐标系到内框坐标系的姿态转移矩阵为：基于小角假设，sinθ≈θ,cosθ≈1，同时忽略高阶项，imu坐标系到内框坐标系的姿态转移矩阵简化为：s1.2构建转位机构内框零位坐标系与外框坐标系间的非正交误差模型定义η
x
,η
y
,η
z
为内框零位坐标系和外框坐标系间的非正交误差角，则内框零位坐标系到外框坐标系的姿态转移矩阵为：
基于小角假设，sinη≈η,cosη≈1，同时忽略高阶项，内框零位坐标系到外框坐标系的姿态转移矩阵简化为：s1.3构建双轴旋转惯导系统的载体姿态传递模型根据矩阵链乘法则，双轴旋转惯导系统的载体姿态传递模型如下：式中为载体坐标系到导航坐标系的姿态转移矩阵，也就是载体姿态；c
s
n为imu坐标系到导航坐标系的姿态转移矩阵，也就是imu的姿态，由imu测量并通过导航计算机解算直接得到；为内框坐标系到imu坐标系的姿态转移矩阵，其中表示的转置；表示外框坐标系到内框零位坐标系的姿态转移矩阵，为内框坐标系到内框零位坐标系的姿态转移矩阵，α为内框转动角度；为载体坐标系到外框坐标系的姿态转移矩阵，β为外框转动角度；和分别为：s2：启动双轴旋转惯导系统，按规定的路径进行旋转，并采集数据s2.1启动双轴旋转惯导系统，导航计算机实时采集imu测量的数据和转位机构的测角速度，控制转位机构进行以下次序的旋转：1.开机，进行10分钟静基座对准，获得imu的初始姿态2.内框正转180度；3.外框正转180度；4.外框反转180度；5.内框反转180度；6.结束数据采集，关机；s3：构建基于粒子群优化算法的双轴旋转惯导系统非正交误差标定方法s3.1定义粒子群算法的粒子格式定义粒子群中的第i个粒子的位置为p
i
，p
i
包含6个参数，为其中分别对应式(2)中imu坐标系到内框坐标系的姿态转移矩阵和式(4)中内框零位坐标系与外框坐标系的姿态转移矩阵的六个失准角误差值，由第i个粒子的位置p
i
的6个粒子参数编排构成以下非正交误差的形式：
式中，表示括号中的矩阵由第i个粒子的位置参数编排而成；s3.2构建粒子群优化算法的适应度函数以由双轴旋转惯导系统提取出的载体姿态误差的均方根误差为适应度，构建粒子群优化算法的适应度函数，将标定问题转化为优化问题，利用粒子群优化算法进行求解；定义适应度函数为：式(9)中，fit(p
i
)为第i个粒子在位置p
i
时的适应度函数；表示计算得到的载体在t时刻相对于初始0时刻的姿态转移矩阵，时刻相对于初始0时刻的姿态转移矩阵，表示t时刻提取到的载体相对于n系的姿态矩阵；表示初始0时刻的n系相对于载体的姿态矩阵；
×
表示矩阵叉乘；由于标定过程中惯导系统保持静止，所以理论上t时刻相对于0时刻惯导系统姿态保持不变，为单位矩阵，计算得到的即为由非正交误差引起的载体姿态误差矩阵；计算过程为：计算过程为：计算过程为：式(10)与(11)中，表示t时刻imu进行测量并解算出的姿态；和为根据式(7)和式(8)所示，由第i个粒子p
i
编排组成的非正交误差矩阵；表示t时刻内框转动角度为α
t
时，内框零位坐标系到内框坐标系的姿态转移矩阵，表示t时刻外框转动角度为β
t
时，载体坐标系到外框坐标系的姿态转移矩阵；式(9)中，sum(
·
)表示求和函数，euler(
·
)表示将括号中变量转换为欧拉角；s3.3利用s3.2构建的适应度函数，采用粒子群优化算法进行寻优粒子群优化算法通过建立一个由多个粒子组成的种群，其中每个粒子的位置代表着一个潜在的解，通过粒子的简单行为和种群内部的信息交互来实现解决问题的智能求解；每个粒子搜索过程的总迭代次数为g，粒子的运动过程就是求解最优解的搜索过程，粒子的运动速度可以根据粒子历史最优位置和种群历史最优位置动态调整；搜索过程中第i个粒子具有最佳适应度的位置，称为个体最优位置种群中具有最佳适应度粒子的位置称为全
局最优位置g
best
；第i个粒子在d次迭代时位置为d＝1,2,
…
,g；结合式(9)、(10)和(11)，相对应的适应度函数为：g；结合式(9)、(10)和(11)，相对应的适应度函数为：g；结合式(9)、(10)和(11)，相对应的适应度函数为：式(12)和(13)中，下标d为当前粒子群的迭代次数，最大迭代次数为g；i＝1，2，
…
，n为粒子数序号，n为粒子群规模，也就是一个粒子群中，粒子的总个数；r1，r2是介于(0,1)之间的随机数；c1，c2为学习因子，分别表征粒子向自身和其他粒子学习的能力；ω为惯性权重常数，用来调整粒子的多样性，ω取值越大，全局寻优能力越强，ω取值越小，局部寻优能力越强；表示粒子的当前速度，v
gmin
,v
gmax
分别表示粒子当前的最小和最大速度，由非正交误差角的大小决定；利用式(12)计算第i个粒子在第d次迭代时位置的适应度，即可重新评估第i个粒子适应度最佳的个体最优位置计算粒子群中所有粒子在第d次迭代时的适应度，即可重新评估出当前适应度最佳的全局最优位置g
best
；当达到最大迭代次数或满足终止要求时，全局最优位置的适应度fit(g
best
)全局最小；将全局最优位置g
best
的六个参数代入式(7)和式(8)得到(15)和(16)，即为双轴旋转惯导转位机构准确的非正交误差：转惯导转位机构准确的非正交误差：将式(15)和式(16)代入式(17)即可得到高精度的载体姿态将式(15)和式(16)代入式(17)即可得到高精度的载体姿态式中，为惯性解算得到的imu的姿态矩阵，和为粒子群优化算法标定得到的非正交误差矩阵(15)、(16)，为内框实时转动角度利用式(6)转换为的姿态矩阵，为外框实时转动角度利用式(6)转换为的姿态矩阵，为通过实时补偿后得到的高精度的载体姿态。2.一种双轴旋转惯导系统转位机构非正交误差标定方法，其特征在于：学习因子c1，:取
固定值2；惯性权重常数ω取值在[0.4,0.9]之间；粒子当前的最小和最大速度v
gmin
,v
gmax v
gmax
＝1,v
gmin
＝-1；粒子的总个数n＝50。3.一种双轴旋转惯导系统转位机构非正交误差标定方法，其特征在于：最大迭代次数为150次或设定适应度小于0.01时满足终止要求。

技术总结
本发明涉及惯性导航技术领域，具体涉及双轴旋转惯导系统转位机构非正交误差标定方法，适用于双轴旋转惯导系统转位机构的非正交误差标定和载体姿态提取；所述方法包括：建立双轴旋转惯导系统转位机构非正交误差模型；启动双轴旋转惯导系统，按规定的路径进行旋转，并采集数据；然后根据所述转位机构非正交误差模型，构建基于粒子群优化算法的双轴旋转惯导系统非正交误差标定方法；最后利用标定结果，将惯性测量单元的姿态与转位机构解耦，得到高精度的载体姿态信息。可有效解决双轴旋转惯导系统的非正交误差标定问题，实现高精度的误差标定及载体姿态提取。定及载体姿态提取。定及载体姿态提取。


技术研发人员：李鼎 于旭东 罗晖 魏国 高春峰
受保护的技术使用者：中国人民解放军国防科技大学
技术研发日：2023.03.20
技术公布日：2023/9/11