解析式粗对准中捷联姿态矩阵的正交化修正方法

摘要

本发明解析式粗对准中捷联姿态矩阵的正交化修正方法，包括：利用重力加速度和地球自转角速度确定三个三维向量；从三个三维向量确定测量得到包含不正交误差的捷联姿态矩阵，进一步将包含不正交误差的捷联姿态矩阵写成行向量的形式，进行正交化和归一化的处理，得到修正的捷联姿态矩阵，根据预设的条件判断修正的捷联姿态矩阵是否满足精度要求，满足则结束，不满足则重复迭代运算直到满足判断条件。本发明解决了捷联姿态矩阵不正交化对于解析式粗对准测量的影响，可提高载体姿态测量精度。

主权项

1.一种解析式粗对准中捷联姿态矩阵的正交化修正方法，其特征是：步骤1：解析式粗对准测量得到没有经过正交化的捷联姿态矩阵C₀，其中，捷联姿态矩阵的9个元素为$C_0=\left[\begin{array}{ccc}{C}_0\left(11\right)& C_0\left(12\right)& C_0\left(13\right)\\ C_0\left(21\right)& C_0\left(22\right)& C_0\left(23\right)\\ C_0\left(31\right)& C_0\left(32\right)& C_0\left(33\right)\end{array}\right]$步骤2：将矩阵C₀写成行向量的形式，并用表示，即${\bar{C}}_0\left(1\right)=\left[\begin{array}{ccc}{C}_0\left(11\right)& C_0\left(12\right)& C_0\left(13\right)\end{array}\right]$${\bar{C}}_0\left(2\right)=\left[\begin{array}{ccc}{C}_0\left(21\right)& C_0\left(22\right)& C_0\left(23\right)\end{array}\right]$${\bar{C}}_0\left(3\right)=\left[\begin{array}{ccc}{C}_0\left(31\right)& C_0\left(32\right)& C_0\left(33\right)\end{array}\right]$步骤3：利用步骤2中得到的得到中间测量值${\bar{T}}_0\left(1\right)=\frac{{\bar{C}}_0\left(2\right)\times {\bar{C}}_0\left(3\right)+{\bar{C}}_0\left(1\right)}{2}$${\bar{T}}_0\left(2\right)=\frac{{\bar{C}}_0\left(3\right)\times {\bar{C}}_0\left(1\right)+{\bar{C}}_0\left(2\right)}{2}$${\bar{T}}_0\left(3\right)=\frac{{\bar{C}}_0\left(1\right)\times {\bar{C}}_0\left(2\right)+{\bar{C}}_0\left(3\right)}{2}$步骤4：利用步骤3，得到的模为(i＝1，2，3)，即$\left|{\bar{T}}_0\left(1\right)\right|=\sqrt{{T_0\left(11\right)}^2+{T_0\left(12\right)}^2+{T_0\left(13\right)}^2}$$\left|{\bar{T}}_0\left(2\right)\right|=\sqrt{{T_0\left(21\right)}^2+{T_0\left(22\right)}^2+{T_0\left(23\right)}^2}$$\left|{\bar{T}}_0\left(3\right)\right|=\sqrt{{T_0\left(31\right)}^2+{T_0\left(32\right)}^2+{T_0\left(33\right)}^2}$其中${\bar{T}}_0\left(1\right)=\left[\begin{array}{ccc}{T}_0\left(11\right)& T_0\left(12\right)& T_0\left(13\right)\end{array}\right];$${\bar{T}}_0\left(2\right)=\left[\begin{array}{ccc}{T}_0\left(21\right)& T_0\left(22\right)& T_0\left(23\right)\end{array}\right];$${\bar{T}}_0\left(3\right)=\left[\begin{array}{ccc}{T}_0\left(31\right)& T_0\left(32\right)& T_0\left(33\right)\end{array}\right]$步骤5：利用步骤3和4，得到经过修正的三个行向量${\bar{C}}_1\left(1\right)=\frac{{\bar{T}}_0\left(1\right)}{\left|{\bar{T}}_0\left(1\right)\right|}$${\bar{C}}_1\left(2\right)=\frac{{\bar{T}}_0\left(2\right)}{\left|{\bar{T}}_0\left(2\right)\right|}$${\bar{C}}_1\left(3\right)=\frac{{\bar{T}}_0\left(3\right)}{\left|{\bar{T}}_0\left(3\right)\right|}$步骤6：进一步得到经过修正的捷联姿态矩阵C₁$C_1=\left[\begin{array}{c}{\bar{C}}_1\left(1\right)\\ {\bar{C}}_1\left(2\right)\\ {\bar{C}}_1\left(3\right)\end{array}\right]$其中，捷联姿态矩阵的9个元素为$C_1=\left[\begin{array}{ccc}{C}_1\left(11\right)& C_1\left(12\right)& C_1\left(13\right)\\ C_1\left(21\right)& C_1\left(22\right)& C_1\left(23\right)\\ C_1\left(31\right)& C_1\left(32\right)& C_1\left(33\right)\end{array}\right]$步骤7：利用步骤6中得到的第1次修正的捷联姿态矩阵C₁与没有经过正交化的捷联姿态矩阵C₀测量得到测距函数值$f_1=\underset{i=1}{\overset{3}{\Sigma }}\underset{j=1}{\overset{3}{\Sigma }}{(C_1\left(\mathrm{ij}\right)-C_0\left(\mathrm{ij}\right))}^2$其中C₀(ij)为C₀的第i行、第j列元素，C₁(ij)为C₁的第i行、第j列元素；步骤8：判断下述条件|f₁-f₀|≤0.00000003其中f₀为初始测距函数，并且有f₀＝0，如果满足该条件，执行步骤9，如果不满足该条件，用第1次修正的捷联姿态矩阵C₁替换没有经过正交化的捷联姿态矩阵C₀，再次执行步骤2至7，并执行步骤8中的判断条件，如此往复迭代测量，当进行了k次修正得到C_k，并有第k次修正后测距函数值满足|f_k-f_k-1|≤0.00000003时，执行步骤9；步骤9：由第k次修正后的捷联姿态矩阵C_k$C_k=\left[\begin{array}{ccc}{C}_k\left(11\right)& C_k\left(12\right)& C_k\left(13\right)\\ C_k\left(21\right)& C_k\left(22\right)& C_k\left(23\right)\\ C_k\left(31\right)& C_k\left(32\right)& C_k\left(33\right)\end{array}\right]$测量得到载体的航向角、纵摇角和横摇角，完成解析式粗对准测量任务。