CN104634348A - 组合导航中的姿态角计算方法 - Google Patents

Abstract

本发明提供了组合导航中的姿态角计算方法，具体设计导航领域。本发明提供的组合导航中的姿态角计算方法，采用随动角校正的方式，其通过先以预先获取的捷联惯导***误差角和角速度作为输入，随动角作为输出，根据随动角与捷联惯导***回路的增益、阻尼比和自然角频率的关联关系建立闭环反馈回路，进而获取具有收敛特性的随动角作为优化随动角，并进一步根据优化随动角对原始的捷联矩阵进行校正，以确定第一优化捷联矩阵，最终依据第一优化捷联矩阵获取姿态角，从而完成了较为精确的姿态角的计算，解决了现有技术中的不足。

Description

组合导航中的姿态角计算方法

技术领域

本发明涉及导航领域，具体而言，涉及组合导航中的姿态角计算方法。

背景技术

依靠地图进行行军作战，或者是旅游勘探已经是常规的方式了。使用地图的目的就是朝向目标的位置前进，利用地图进行导航也是最原始的导航方式。其中，进行导航的基础是获取两个要素，第一是目标的位置，第二是获取自身的位置。随着通讯技术与信息处理技术的进步，出现了两种新的导航方式，惯性导航技术和空间星导航技术，这两种方式也都是重在精确的获取自身的位置。

惯性导航技术是现代科学技术中一门尖端的技术学科，在航空、航天、航海的军事领域以及许多民用领域都得到了广泛应用。

惯性导航中的捷联惯导***结构简单，相对而言，体积小、重量轻、成本低，具有很高的可靠性。它是一种真正意义上的自主式导航***，输出信息连续，自主地提供加速度、速度、位置和角速度、姿态角等信息，完全依靠自身设备自主地完成导航任务，不和外界发生任何光电联系。因此，它具备隐蔽性好、不受外界干扰、不受时间地域和气候条件限制等诸多优点。但是捷联惯导***的导航误差随时间积累而增大。

具体而言，依据惯性进行导航，其主要利用惯性元件(加速度计)来测量运载体本身的加速度，经过积分和运算得到速度和位置，从而达到对运载体导航定位的目的。组成惯性导航***的设备都安装在运载体内，工作时不依赖外界信息，也不向外界辐射能量，不易受到干扰，是一种自主式导航***。惯性导航***通常由惯性测量装置、计算机、控制显示器等组成。惯性测量装置包括加速度计和陀螺仪，又称惯性测量单元。3个单自由度陀螺仪用来测量运载体的3个转动运动；3个加速度计用来测量运载体的3个平移运动的加速度。计算机根据测得的加速度信号计算出运载体的速度和位置数据。控制显示器显示各种导航参数。按照陀螺仪和加速度计在运载体上的安装方式，分为平台式惯性导航***(陀螺仪和加速度计安装在惯性平台的台体上)和捷联式惯性导航***(陀螺仪和加速度计直接安装在运载体上)。

另一种，卫星定位***的出现为空间导航定位技术带来革命性的发展。卫星接收机具有较高的导航定位精度且不随时间增长而降低，其不足之处在于数据刷新率低、易受干扰、动态环境下或受遮挡时容易丢星导致定位中断。以美国的GPS卫星导航为例对空间导航中的定位进行描述，美国的GPS卫星导航***共有24颗GPS卫星，且这24颗卫星均在离地面2万零2百公里的高空上，每颗卫星均以12小时的周期环绕地球运行，使得在任意时刻，在地面上的任意一点都可以同时观测到4颗以上的卫星。

由于卫星的位置精确可知，在GPS观测中，我们可得到卫星到接收机的距离，利用三维坐标中的距离公式，利用3颗卫星，就可以组成3个方程式，解出观测点的位置(X、Y和Z)。考虑到卫星的时钟与接收机时钟之间的误差，实际上有4个未知数，X、Y、Z和钟差，因而需要引入第4颗卫星，形成4个方程式进行求解，从而得到观测点的经纬度和高程。

接收机往往可以锁住4颗以上的卫星，这时，接收机可按卫星的星座分布分成若干组，每组4颗，然后通过算法挑选出误差最小的一组用作定位，从而提高精度。

由于卫星运行轨道、卫星时钟存在误差，大气对流层、电离层对信号的影响，以及人为的SA保护政策，使得民用GPS的定位精度较低。为提高定位精度，普遍采用差分GPS(DGPS)技术，建立基准站(差分台)进行GPS观测，利用已知的基准站精确坐标，与观测值进行比较，从而得出一修正数，并对外发布。接收机收到该修正数后，与自身的观测值进行比较，消去大部分误差，得到一个比较准确的位置。实验表明，利用差分GPS，定位精度可提高一个数量级。

由于前两种导航方式在确认自身位置的时候有均有一定的不足之处，因此产生了新的导航方式，也就是组合导航。组合导航算法将捷联惯导***和卫星接收机的信号以卡尔曼滤波方式最优融合，取长补短，弥补卫星接收机信号存在死区、惯导误差随时间而增长的缺点，使得组合后的***既能保证精度，又能提高可靠性和抗干扰能力。在这种融合方法中，将速度、位置作为观测量，根据***误差状态方程，利用卡尔曼滤波递推估计误差值。

传统算法中，当采用组合导航算法的运动载体(自身)俯仰角很大时，捷联惯导***解算的数学平台接近于失去一个自由度，导致航向角、俯仰角和滚动角这种导航角的计算误差很大，并直接导致速度误差很大。而组合导航算法中的卡尔曼滤波要求状态方程模型准确，航向角、滚动角和速度误差很大导致的模型误差使得卡尔曼滤波的递推状态估计和真实状态之间的偏差越来越大，从而导致组合导航***性能急剧恶化。

综上，在运动载体俯仰角很大的时候，会导致姿态角准确度下降，进而导致现有导航方式的准确度下降。

发明内容

有鉴于此，本发明实施例的目的在于提供组合导航中的姿态角计算方法，以提高姿态角的准确度，进而提高组合导航的准确度。

第一方面，本发明实施例提供了组合导航中的姿态角计算方法，包括：

以预先获取的捷联惯导***误差角和角速度作为输入，随动角作为输出，根据随动角与捷联惯导***回路的增益、阻尼比和自然角频率的关联关系建立闭环反馈回路，所述误差角是俯仰角与预设的临界角的差值；

根据所述闭环反馈回路的传递函数式，计算预定状态下的优化随动角；

利用所述优化随动角对预先获取的原始捷联矩阵进行校正，以确定第一优化捷联矩阵；

根据第一优化捷联矩阵获取姿态角。

结合第一方面，本发明实施例提供了第一方面的第一种可能的实施方式，其中，还包括：

根据预先获取的航向角的误差补偿角和滚动角的误差补偿角，对当前捷联惯导***的捷联矩阵进行修正，以确定原始捷联矩阵。

结合第一方面，本发明实施例提供了第一方面的第二种可能的实施方式，其中，所述根据所述闭环反馈回路的传递函数式，计算预定状态下的优化随动角包括：

根据预先获取的采样周期，对所述闭环反馈回路的传递函数式进行离散化处理，以确定优化随动角的递推公式；

根据所述优化随动角的递推公式计算所述优化随动角。

结合第一方面，本发明实施例提供了第一方面的第三种可能的实施方式，其中，所述递推公式为Δθ(k)＝Δθ₁(k)+Δθ₂(k)，Δθ₁(k)＝a₁Δθ₁(k-1)+a₂Δθ₁(k-2)+b₀θ(k)+b₁θ(k-1)+b₂θ(k-2)，Δθ₂(k)＝c₁Δθ₂(k-1)+c₂Δθ₂(k-2)+c₃Δθ₂(k-3)+d₀ω_x(k)+d₁ω_x(k-1)+d₂ω_x(k-2)+d₃ω_x(k-3)，其中， $a_1=\frac{8+{2\mathrm{K\ω}}_n^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},a_2=\frac{-4+4{\mathrm{\ζ\ω}}_n-{\mathrm{K\ω}}_n^2}{4+4{\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},b_0=\frac{{\mathrm{\ω}}_n^2{T}_s^2-{4\mathrm{\θ}}_c}{4+4{\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},$ $b_1=\frac{{2\mathrm{\ω}}_n^2{T}_s^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},b_2=\frac{{\mathrm{\ω}}_n^2{T}_s^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},c_1=\frac{12+{4\mathrm{\ζ\ω}}_n+{6\mathrm{K\ω}}_n^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},$ $c_2=\frac{-12+{4\mathrm{\ζ\ω}}_n-{3\mathrm{K\ω}}_n^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},c_3=\frac{4-{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},d_0=\frac{{\mathrm{\ω}}_n^2{T}_s^3}{2(4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2)},$ $d_1=\frac{{3\mathrm{\ω}}_n^2{T}_s^3}{2(4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2)},d_2=\frac{{3\mathrm{\ω}}_n^2{T}_s^3}{2(4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2)},d_3=\frac{{\mathrm{\ω}}_n^2{T}_s^3}{2(4+{4\mathrm{\ζ\ω}}_n+K{\mathrm{\ω}}_n^2)},$ K为捷联惯导***回路的增益，ζ为捷联惯导***回路的阻尼比，ω_n为捷联惯导***回路的自然角频率。

结合第一方面，本发明实施例提供了第一方面的第四种可能的实施方式，其中，还包括：

根据如下公式计算参考俯仰角，θ＝αθ_s+(1-α)θ_a，其中，θ_s为通过四元数法计算得到的第一参考俯仰角，θ_a为根据加速度分解方法计算得到的第二参考俯仰角，α为预设的权值；

若所述参考俯仰角的数值大于预设的俯仰角参考值时，则执行步骤所述以预先获取的捷联惯导***误差角和角速度作为输入，随动角作为输出，根据随动角与捷联惯导***回路的增益、阻尼比和自然角频率的关联关系建立闭环反馈回路，所述俯仰角参考值为60-70度。

结合第一方面，本发明实施例提供了第一方面的第五种可能的实施方式，其中，还包括：

根据预设的航向角初始值和滚动角初始值，采用遗传算法进行搜索；

当搜索到航向角变化值与航向角实际值的差值小于预设的航向差值，且滚动角变化值与滚动角实际值的差值小于预设的滚动差值时，确定此时的航向角变化值与航向角实际值的差值为航向角的误差补偿角，且确定滚动角变化值与滚动角实际值的差值为滚动角的误差补偿角。

结合第一方面，本发明实施例提供了第一方面的第六种可能的实施方式，其中，所述根据第一优化捷联矩阵获取姿态角包括：

采用卡尔曼滤波的方式获取捷联惯导***的数学平台失准角；

根据所述数学平台失准角和优化随动角对所述第一优化捷联矩阵进行校正，以确定第二优化捷联矩阵；

从所述第二优化捷联矩阵中导出姿态角。

结合第一方面，本发明实施例提供了第一方面的第七种可能的实施方式，其中，所述采用卡尔曼滤波的方式获取捷联惯导***的数学平台失准角包括：

对预先获取的组合导航***状态方程和观测方程进行离散化处理，并通过卡尔曼滤波进行递推估计，以确定状态最优估值方程；

根据所述状态最优估值方程确定所述数学平台失准角。

结合第一方面，本发明实施例提供了第一方面的第八种可能的实施方式，其中，根据所述数学平台失准角和优化随动角对所述第一优化捷联矩阵进行校正，以确定第二优化捷联矩阵包括：

根据所述数学平台失准角对第一优化捷联矩阵进行校正，以确定不完全优化捷联矩阵；

根据所述优化随动角对所述不完全优化捷联矩阵再次进行校正，以确定所述第二优化捷联矩阵。

结合第一方面，本发明实施例提供了第一方面的第九种可能的实施方式，其中，还包括根据如下公式计算捷联惯导***的测算速度和测算位置， $\left\{\begin{array}{c}{\stackrel{\·}{x}}_e^n=v_e^n\\ {\stackrel{\·}{v}}_e^n={C_b}^n{f}^b-[{2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n]\×v_e^n+g^n\end{array}\right.,$ 其中是测算位置矢量，是测算速度矢量，f^b是比力信号，为捷联矩阵的逆矩阵，gⁿ＝[0,0,g]^T为地球重力矢量，为地球自转角速度Ω在导航坐标系中的投影，为导航坐标系相对地球坐标系的角速度；

根据所述测算速度矢量、测算位置矢量和预先通过卫星接收机获取的检测速度矢量和检测位置矢量分别计算速度误差和位置误差；

以所述速度误差、位置误差作为观测变量和以预先选取的数学平台失准角、陀螺仪零偏、陀螺仪随机漂移率和加速度计零偏作为组合***误差状态向量，分别得到***状态方程和观测方程。

本发明实施例提供的组合导航中的姿态角计算方法，采用随动角校正的方式，与现有技术中的在俯仰角过大的时候，导致组合导航***的姿态角难以精确的获得，进而导致导航误差过大相比，其通过先以预先获取的捷联惯导***误差角和角速度作为输入，随动角作为输出，根据随动角与捷联惯导***回路的增益、阻尼比和自然角频率的关联关系建立闭环反馈回路，进而获取具有收敛特性的随动角作为优化随动角，并进一步根据优化随动角对原始的捷联矩阵进行校正，以确定第一优化捷联矩阵，最终依据第一优化捷联矩阵获取姿态角，从而完成了较为精确的姿态角的计算，解决了现有技术中的不足。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单的介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1示出了本发明实施例所提供的组合导航中的姿态角计算方法的基本流程图；

图2示出了本发明实施例所提供的组合导航中的姿态角计算方

法的随动反馈回路示意图；

图3示出了本发明实施例所提供的组合导航中的姿态角计算方

法的使用遗传算法确定航向角的误差补偿角和滚动角的误差补

偿角的流程示意图；

图4示出了本发明实施例所提供的组合导航中的姿态角计算方

法的通过第一次优化后的捷联矩阵基础上，进一步获取更准确姿

态角的细化流程图；

图5示出了本发明实施例所提供的组合导航中的姿态角计算方

法的整体逻辑反馈控制示意图。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例提供了组合导航中的姿态角计算方法，如图1所示，包括如下步骤：

S101，以预先获取的捷联惯导***误差角和角速度作为输入，随动角作为输出，根据随动角与捷联惯导***回路的增益、阻尼比和自然角频率的关联关系建立闭环反馈回路，误差角是俯仰角与预设的临界角的差值；

S102，根据闭环反馈回路的传递函数式，计算预定状态下的优化随动角；

S103，利用优化随动角对预先获取的原始捷联矩阵进行校正，以确定第一优化捷联矩阵；

S104，根据第一优化捷联矩阵获取姿态角。

步骤S101中，为了使获取的随动角具有很好的收敛特性，需要通过设计***内部的以随动角作为输出的闭环反馈回路来确定。具体的，将随动反馈回路(闭环反馈回路)的动态响应特性设计成与增益K、阻尼比ζ、自然角频率ω_n有关。输入输出满足以下线性微分方程：

$\begin{array}{c}\mathrm{\Δ}\stackrel{\·\·}{{\mathrm{\θ}}_1}+2{\mathrm{\ζ\ω}}_n\mathrm{\Δ}\stackrel{\·}{{\mathrm{\θ}}_1}+K{\mathrm{\ω}}_n^2{\mathrm{\Δ\θ}}_1={\mathrm{\ω}}_n^2(\mathrm{\θ}-{\mathrm{\θ}}_c)\\ \mathrm{\Δ}\stackrel{\·\·\·}{{\mathrm{\θ}}_2}+2\mathrm{\ζ}{\mathrm{\ω}}_n\mathrm{\Δ}\stackrel{\·\·}{{\mathrm{\θ}}_2}+{\mathrm{K\ω}}_n^2\mathrm{\Δ}\stackrel{\·}{{\mathrm{\θ}}_2}={\mathrm{\ω}}_n^2{\mathrm{\ω}}_x\\ \mathrm{\Δ\θ}=\mathrm{\Δ}{\mathrm{\θ}}_1+\mathrm{\Δ}{\mathrm{\θ}}_2\end{array},$ 其中，θ-θ_c为误差角，ω_x为角速度，Δθ为随动角，Δθ₁和Δθ₂分别为随动角的两个分量。如图2所示，示出了该闭环反馈回路的原理结构图，从图中能够很明确的看到以θ-θ_c和ω_x作为输入，以随动角Δθ作为输出，并经过经典闭环负反馈调节方式来建立的负反馈调节***。

步骤S102，如图2中所示的***模型只有三个可调参数K、ζ、ω_n，结构简单、调试方便，可以很容易使随动反馈回路达到期望的动态响应特性。输入输出的三阶传递函数矩阵满足以下表达式：

$\mathrm{\Δ\θ}\left(s\right)=G\left(s\right)\left[\begin{array}{c}\mathrm{\θ}\left(s\right)-{\mathrm{\θ}}_c\\ {\mathrm{\ω}}_x\left(s\right)\end{array}\right]=\left[\begin{array}{cc}\frac{{\mathrm{\ω}}_n^2}{s^2+{2\mathrm{\ζ\ω}}_{n}s+K{\mathrm{\ω}}_n^2}& \frac{1}{s}\frac{{\mathrm{\ω}}_n^2}{s^2+{2\mathrm{\ζ\ω}}_{n}s+K{\mathrm{\ω}}_n^2}\end{array}\right]\left[\begin{array}{c}\mathrm{\θ}\left(s\right)-{\mathrm{\θ}}_c\\ {\mathrm{\ω}}_x\left(s\right)\end{array}\right];$ 其中，θ-θ_c为误差角，ω_x为角速度，Δθ为随动角。

其中根据闭环反馈回路的传递函数式，计算预定状态下的优化随动角包括：

根据预先获取的采样周期，对闭环反馈回路的传递函数式进行离散化处理，以确定优化随动角的递推公式；根据优化随动角的递推公式计算优化随动角。也就是在用组合导航计算机进行随动反馈回路计算时，若数据采样周期为T_s，用双线性变换公式进行离散化处理，于是得到随动角的递推计算公式：

Δθ(k)＝Δθ₁(k)+Δθ₂(k)；

Δθ₁(k)＝a₁Δθ₁(k-1)+a₂Δθ₁(k-2)+b₀θ(k)+b₁θ(k-1)+b₂θ(k-2)；

Δθ₂(k)＝c₁Δθ₂(k-1)+c₂Δθ₂(k-2)+c₃Δθ₂(k-3)+d₀ω_x(k)+d₁ω_x(k-1)+d₂ω_x(k-2)+d₃ω_x(k-3)；

式中 $a_1=\frac{8+{2\mathrm{K\ω}}_n^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},a_2=\frac{-4+4{\mathrm{\ζ\ω}}_n-{\mathrm{K\ω}}_n^2}{4+4{\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},b_0=\frac{{\mathrm{\ω}}_n^2{T}_s^2-{4\mathrm{\θ}}_c}{4+4{\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},$ $b_1=\frac{{2\mathrm{\ω}}_n^2{T}_s^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},b_2=\frac{{\mathrm{\ω}}_n^2{T}_s^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},c_1=\frac{12+{4\mathrm{\ζ\ω}}_n+{6\mathrm{K\ω}}_n^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},$ $c_2=\frac{-12+{4\mathrm{\ζ\ω}}_n-{3\mathrm{K\ω}}_n^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},c_3=\frac{4-{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2}{4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2},d_0=\frac{{\mathrm{\ω}}_n^2{T}_s^3}{2(4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2)},$ $d_1=\frac{{3\mathrm{\ω}}_n^2{T}_s^3}{2(4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2)},d_2=\frac{{3\mathrm{\ω}}_n^2{T}_s^3}{2(4+{4\mathrm{\ζ\ω}}_n+{\mathrm{K\ω}}_n^2)},d_3=\frac{{\mathrm{\ω}}_n^2{T}_s^3}{2(4+{4\mathrm{\ζ\ω}}_n+K{\mathrm{\ω}}_n^2)},$ 式中，K为捷联惯导***回路的增益，ζ为捷联惯导***回路的阻尼比，ω_n为捷联惯导***回路的自然角频率。由此，可以通过随动角的递推公式较为快速的获取收敛状态下的随动角，即优化随动角。

步骤S103，需要根据优化随动角对预先获取的捷联矩阵进行校正，以确定更为准确的捷联矩阵。具体的，在执行步骤S103之前，还可以包括步骤S105，根据预先获取的航向角的误差补偿角和滚动角的误差补偿角，对当前捷联惯导***的捷联矩阵进行修正，以确定原始捷联矩阵。

捷联惯导***的捷联矩阵是关于航向角、俯仰角、滚动角的三角函数矩阵。设当前的捷联矩阵为运动载体航向角ψ、滚动角γ的误差补偿角分别为Δψ₀、Δγ₀(以上参数Δψ₀和Δγ₀是可以通过现有的手段获得的，可以直接使用)，通过根据小角度下的坐标变换关系，利用航向角的误差补偿角Δψ₀和滚动角的误差补偿角Δγ₀对当前捷联惯导***的捷联矩阵进行修正，以得到原始捷联矩阵。具体的修正公式如下： $C_n^{b1}=\left[\begin{array}{ccc}1& {-\mathrm{\Δ\ψ}}_0& -{\mathrm{\Δ\γ}}_0\\ {\mathrm{\Δ\ψ}}_0& 1& -{\mathrm{\Δ\ψ}}_0{\mathrm{\Δ\γ}}_0\\ {\mathrm{\Δ\γ}}_0& 0& 1\end{array}\right]C_n^{b0},$ 式中，Δψ₀为航向角的误差补偿角，Δγ₀为滚动角的误差补偿角，为当前捷联惯导***的捷联矩阵，为原始捷联矩阵。

进一步，利用小角度下的坐标变换关系，利用步骤S102所获得的优化随动角Δθ对原始捷联矩阵再次进行修正，以确定第一优化捷联矩阵。具体利用随动角进行修正的公式如下： $C_n^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\Δ\θ}\right)& -\sin \left(\mathrm{\Δ\θ}\right)\\ 0& \sin \left(\mathrm{\Δ\θ}\right)& \cos \left(\mathrm{\Δ\θ}\right)\end{array}\right]C_n^{b1},$ 式中，Δθ为优化随动角，为原始捷联矩阵，为第一优化捷联矩阵。

第一优化捷联矩阵作为捷联惯导***更新后的新捷联矩阵，克服了大俯仰角的影响，以此进行捷联惯导***的速度、位置和姿态角解算是较为准确的，也就是可以执行步骤S104，根据第一优化捷联矩阵获取姿态角，从而完成了准确姿态角的获取，其中，所获取的姿态角指的是航向角、俯仰角、滚动角中的一种或多种。

需要说明的是，本发明所提供的组合导航中的姿态角计算方法优选应用在运动载体的俯仰角很大的情况下，当然也适用于俯仰角较低的情况下，在运动载体俯仰角较小的情况下，仍然能够起到对姿态角进行有效校正的功能。

为了降低***的计算量，在执行步骤S101之前，可以通过预先判断***当前俯仰角的方式来确定是否执行步骤S101-步骤S103，如此，在俯仰角达到一定要求的时候，可以根据具体情况来选择执行步骤S101以及后续的步骤。

也就是本发明所提供的方法还包括如下步骤，根据如下公式计算参考俯仰角，θ＝αθ_s+(1-α)θ_a，其中，θ_s为通过四元数法计算得到的第一参考俯仰角，θ_a为根据加速度分解方法计算得到的第二参考俯仰角，α为预设的权值；

若参考俯仰角的数值大于预设的俯仰角参考值时，则执行步骤以预先获取的捷联惯导***误差角和角速度作为输入，随动角作为输出，根据随动角与捷联惯导***回路的增益、阻尼比和自然角频率的关联关系建立闭环反馈回路，俯仰角参考值为60-70度。

其中，运动载体当前的加速度a(修正后)可以通过加速度计零偏▽＝[▽_x,▽_y,▽_z]^T对实际测量值进行修正，以得到更为精确的运动载体加速度。在三维坐标系中，加速度、角速度等矢量均可以分解为沿三维坐标系坐标轴的三个分量，也就是实际测量得到的a_m＝[a_mx,a_my,a_mz]^T，进而修正后的加速度 $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left[\begin{array}{c}{a}_{\mathrm{mx}}\\ a_{\mathrm{my}}\\ a_{\mathrm{mz}}\end{array}\right]-\left[\begin{array}{c}{\▿}_x\\ {\▿}_y\\ {\▿}_z\end{array}\right],$ 即a＝[a_x,a_y,a_z]^T，将修正后的加速度进行矢量运算，以得到修正后的加速度的数值(绝对加速度)为

在得到修正后的加速度a＝[a_x,a_y,a_z]^T后，根据加速度分解方法计算第二参考俯仰角其中，g为重力加速度，a_y为加速度沿竖直方向的分量。

在得到修正后的加速度的数值(绝对加速度)为后，根据捷联惯导***姿态角解算常用的四元数法可以计算第一参考俯仰角，θ_s＝arcsin[2(q₂q₃-q₀q₁)]，其中q₀、q₁、q₂和q₃分别为四元数中的元素。

为了能够使得到的参考俯仰角更为准确，可以通过分别对第一参考俯仰角θ_s和第二参考俯仰角θ_a赋予不同权值来调节最终的参考俯仰角θ的数值。具体如使用公式θ＝αθ_s+(1-α)θ_a来计算参考俯仰角θ，其中，θ_s为第一参考俯仰角，θ_a为第二参考俯仰角。当运动载体当前的绝对加速度a较小时，加权因子要取大一些，由此，当|a-g|<0.5时可取α＝0.8，否则α＝0.2。当然，通过对第一参考俯仰角θ_s和第二参考俯仰角θ_a进行加权的方式，来确定参考俯仰角θ的过程中，可以是第一参考俯仰角θ_s的权值和第二参考俯仰角θ_a权值不产生关联的方式，如θ＝α₁θ_s+α₂θ_a。

运动载体俯仰角大于60°时，组合导航性能恶化较为严重，可设定俯仰角临界值θ_c在(60°,70°)范围内取值，作为判断条件。修正后的俯仰角θ大于设定的临界值θ_c即θ>θ_c时，则启动随动反馈回路解算，也就是此时可以执行步骤S101。

步骤S105，根据预先获取的航向角的误差补偿角和滚动角的误差补偿角，对当前捷联惯导***的捷联矩阵进行修正，以确定原始捷联矩阵。可以包括如下步骤：根据预设的航向角初始值和滚动角初始值，采用遗传算法进行搜索；

当搜索到航向角变化值与航向角实际值的差值小于预设的航向差值，且滚动角变化值与滚动角实际值的差值小于预设的滚动差值时，确定此时的航向角变化值与航向角实际值的差值为航向角的误差补偿角，且确定滚动角变化值与滚动角实际值的差值为滚动角的误差补偿角。

也就是需要利用随动角修正原始捷联矩阵过程中，运动载体航向角、滚动角的误差补偿角Δψ₀、Δγ₀由离线计算的遗传算法搜索获得。遗传算法搜索的停止条件为：离线计算的运动载体航向角ψ、滚动角γ与对应真值(已知的滚动角实际值和航向角实际值)之差均在某个小角度范围内，即Δψ₀＝|ψ-ψ_真|<β、且Δγ₀＝|γ-γ_真|<β，β取值一般小于5度，则确定此时的航向角变化值与航向角实际值的差值为航向角的误差补偿角Δψ₀，且确定滚动角变化值与滚动角实际值的差值为滚动角的误差补偿角Δγ₀。需要说明的是，利用遗传算法进行搜索的时候需要先确定一个初始值，***在进行搜索的时候，根据这个初始值不停的变化数值(指滚动角和航向角的数值在不停的变化，也就是每次变化均生成一组滚动角变化值和航向角变化值)，也就是每次变化之后均要分别判断航向角变化值与航向角实际值的差值是否小于预设的航向差值，且滚动角变化值与滚动角实际值的差值是否小于预设的滚动差值，当这两个判断均成立的时候，则确定此时的航向角变化值与航向角实际值的差值为航向角的误差补偿角Δψ₀，且确定滚动角变化值与滚动角实际值的差值为滚动角的误差补偿角Δγ₀。

具体的，利用全过程采集的角速度、加速度信息和已知的运动载体航向角、滚动角真值，对误差补偿角进行离线计算的遗传算法搜索。

运动载体航向角、滚动角的误差补偿角的编码准则：均采用8位二进制编码，编码范围[0x00H，0xFFH]。0x00H对应-5度，0x80H对应0度，0xFFH对应5度。

运动载体航向角、滚动角的误差补偿角搜索的收敛准则：|ψ-ψ_真|<β、γ-γ_真|<β。

误差补偿角进行遗传算法搜索时，对二进制码采用选择、交叉、变异三种算子。搜索过程见图3。

搜索得到Δψ₀的编码数值为N_Δψ0，则也就计算出了航向角的误差补偿角。

搜索得到Δγ₀的编码数值为N_Δγ0，则也就计算出了滚动角的误差补偿角。

具体的，如图4所示，步骤S104，根据第一优化捷联矩阵获取姿态角包括如下步骤：

S1041，采用卡尔曼滤波的方式获取捷联惯导***的数学平台失准角；

S1042，根据数学平台失准角和优化随动角对第一优化捷联矩阵进行校正，以确定第二优化捷联矩阵；

S1043，从第二优化捷联矩阵中导出姿态角。

步骤S1041中，通过卡尔曼滤波的方式来获取捷联惯导***的数学平台失准角，具体可以分为两个步骤，也就是步骤S1041，采用卡尔曼滤波的方式获取捷联惯导***的数学平台失准角包括：

对预先获取的组合导航***状态方程和观测方程进行离散化处理，并通过卡尔曼滤波进行递推估计，以确定状态最优估值方程；

根据状态最优估值方程确定数学平台失准角。

其详细过程为先取数学平台失准角(φ_E、φ_N、φ_U)、速度误差(δV_E、δV_N、δV_U)、位置误差(δL、δλ、δh)、陀螺仪零偏(ε_bx、ε_by、ε_bz)、陀螺仪随机漂移率(ε_rx、ε_ry、ε_rz)、加速度计零偏(▽_x、▽_y、▽_z)作为组合导航***误差状态向量，位置、速度之差作为观测变量，可以得到***状态方程和观测方程： $Z=\left[\begin{array}{c}\mathrm{\&delta;R}\\ \mathrm{\&delta;V}\end{array}\right]=\mathrm{HX}+V_k,$ 其中，F(t)为捷联惯导***的状态转移矩阵，G(t)为捷联惯导***的噪声输入矩阵，w(t)是***噪声，δR＝[δL,δλ,δh]^T(位置误差)、δV＝[δV_E,δV_NδV_U]^T(速度误差)，V_k是量测噪声。

接着，分别将状态方程和观测方程离散化后得到：X(k+1)＝φ(k+1,k)X(k)+Γ(k+1,k)W(k)；Z(k)＝H(k)X(k)+V(k)。从而得到状态一步预测方程和状态最优估值方程： $\hat{X}(k+1/k+1)=\hat{X}(k+1/k)+K(k+1)[Z(k+1)-H(k+1)\hat{X}(k+1)/k];$ 其中，滤波增益方程为：

K(k+1)＝P(k+1/k)H^T(k+1)[H(k+1)P(k+1/k)H^T(k+1)+R_k+1]^-1；

一步预测均方误差方程为：

P(k+1/k)＝φ(k+1,k)P(k/k)φ^T(k+1,k)+Γ(k+1,k)Q_kΓ^T(k+1,k)；

估计均方误差方程为：

P(k+1/k+1)＝[I-K(k+1)H(k+1)]P(k+1/k)。利用离散化后的***模型进行卡尔曼滤波递推估计，得到误差状态向量的最优估计值，也就得到了数学平台失准角(数学平台失准角的最优估计值)。其中，步骤S1042，根据所述数学平台失准角和优化随动角对所述第一优化捷联矩阵进行校正，以确定第二优化捷联矩阵，可以分为如下两个步骤：

根据数学平台失准角对第一优化捷联矩阵进行校正，以确定不完全优化捷联矩阵；

根据优化随动角对不完全优化捷联矩阵再次进行校正，以确定第二优化捷联矩阵。

在使用数学平台失准角和优化随动角对第一优化捷联矩阵进行校正的时候，可以先使用随动角校正，后使用数学平台失准角进行校正，也可以先使用数学平台失准角进行校正，后使用随动角进行矫正；但优选的方案为先使用数学平台失准角进行校正，后使用随动角进行校正，使用随动角进行校正的具体公式如下： $C_n^{\mathrm{bt}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\&Delta;\&theta;}\right)& \sin \left(\mathrm{\&Delta;\&theta;}\right)\\ 0& -\sin \left(\mathrm{\&Delta;\&theta;}\right)& \cos \left(\mathrm{\&Delta;\&theta;}\right)\end{array}\right]C_n^b,$ 其中为之前已经求得的第一优化捷联矩阵，为所求的第二优化捷联矩阵，Δθ为随动角，在步骤S102中已经求得。

需要说明的是，卡尔曼滤波递推计算前陀螺仪零偏(ε_bx、ε_by、ε_bz)、陀螺仪随机漂移率(ε_rx、ε_ry、ε_rz)、加速度计零偏(▽_x、▽_y、▽_z)已预先获得，还需要计算速度误差(δV_E、δV_N、δV_U)、位置误差(δL、δλ、δh)。其中，速度误差是通过捷联惯导***根据基本方程计算得到的速度和卫星获得的速度做差得到的，位置误差是通过捷联惯导***根据基本方程计算得到的位置和卫星获得的位置做差得到的，通过捷联惯导***计算得到的速度和位置的算法如下： $\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_e^n=v_e^n\\ {\stackrel{\&CenterDot;}{v}}_e^n={C_b}^n{f}^b-[{2\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n]\&times;v_e^n+g^n\end{array}\right.,$ 其中，是位置矢量，是速度矢量，f^b是比力信号，为捷联矩阵的逆矩阵，gⁿ＝[0,0,g]^T为地球重力矢量，为地球自转角速度Ω在导航坐标系中的投影，为导航坐标系相对地球坐标系的角速度。再利用卫星获得的观测结果(速度和位置)就能够求得位置误差和速度误差，并进一步以取数学平台失准角(φ_E、φ_N、φ_U)、陀螺仪零偏(ε_bx、ε_by、ε_bz)、陀螺仪随机漂移率(ε_rx、ε_ry、ε_rz)、加速度计零偏(▽_x、▽_y、▽_z)作为组合导航***误差状态向量，速度误差(δV_E、δV_N、δV_U)、位置误差(δL、δλ、δh)、作为观测变量，可以得到***状态方程和观测方程。

整体来看本发明所提供的组合导航中的姿态角计算方法，如图5所示，惯性导航测量单元提供原始的角速度和加速度测量信息、卫星接收机提供参考的速度和位置信息(在计算速度误差和位置误差时使用)，组合导航算法主要包括捷联惯导***解算、卡尔曼滤波最优估计、随动反馈回路三大部分。算法的这三个部分相互作用、相互影响。从而通过随动反馈回路中获取收敛状态下的随动角，并进一步根据随动角来对捷联矩阵进行校正和使用数学平台失准角对捷联矩阵进行校正，从而实现了从优化后的捷联矩阵中提取准确的姿态角。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (10)

1.组合导航中的姿态角计算方法，其特征在于，包括：

以预先获取的捷联惯导***误差角和角速度作为输入，随动角作为输出，根据随动角与捷联惯导***回路的增益、阻尼比和自然角频率的关联关系建立闭环反馈回路，所述误差角是俯仰角与预设的临界角的差值；

根据所述闭环反馈回路的传递函数式，计算预定状态下的优化随动角；

利用所述优化随动角对预先获取的原始捷联矩阵进行校正，以确定第一优化捷联矩阵；

根据第一优化捷联矩阵获取姿态角。

2.根据权利要求1所述的组合导航中的姿态角计算方法，其特征在于，还包括：

根据预先获取的航向角的误差补偿角和滚动角的误差补偿角，对当前捷联惯导***的捷联矩阵进行修正，以确定原始捷联矩阵。

3.根据权利要求1所述的组合导航中的姿态角计算方法，其特征在于，所述根据所述闭环反馈回路的传递函数式，计算预定状态下的优化随动角包括：

根据预先获取的采样周期，对所述闭环反馈回路的传递函数式进行离散化处理，以确定优化随动角的递推公式；

根据所述优化随动角的递推公式计算所述优化随动角。

4.根据权利要求3所述的组合导航中的姿态角计算方法，其特征在于，所述递推公式为Δθ(k)＝Δθ₁(k)+Δθ₂(k)，Δθ₁(k)＝a₁Δθ₁(k-1)+a₂Δθ₁(k-2)+b₀θ(k)+b₁θ(k-1)+b₂θ(k-2)，Δθ₂(k)＝c₁Δθ₂(k-1)+c₂Δθ₂(k-2)+c₃Δθ₂(k-3)+d₀ω_x(k)+d₁ω_x(k-1)+d₂ω_x(k-2)+d₃ω_x(k-3)，其中， $a_1=\frac{8+2K{\mathrm{\&omega;}}_n^2}{4+4\mathrm{\&zeta;}{\mathrm{\&omega;}}_n+{\mathrm{K\&omega;}}_n^2},a_2=\frac{-4+4{\mathrm{\&zeta;\&omega;}}_n-{\mathrm{K\&omega;}}_n^2}{4+4{\mathrm{\&zeta;\&omega;}}_n+{\mathrm{K\&omega;}}_n^2},b_0=\frac{{\mathrm{\&omega;}}_n^2{T}_s^2-4{\mathrm{\&theta;}}_c}{4+4\mathrm{\&zeta;}{\mathrm{\&omega;}}_n+{\mathrm{K\&omega;}}_n^2},$ $b_1=\frac{2{\mathrm{\&omega;}}_n^2{T}_s^2}{4+{4\mathrm{\&zeta;\&omega;}}_n+{\mathrm{K\&omega;}}_n^2},b_2=\frac{{\mathrm{\&omega;}}_n^2{T}_s^2}{4+4\mathrm{\&zeta;}{\mathrm{\&omega;}}_n+{\mathrm{K\&omega;}}_n^2},c_1=\frac{12+4\mathrm{\&zeta;}{\mathrm{\&omega;}}_n+6{\mathrm{K\&omega;}}_n^2}{4+4{\mathrm{\&zeta;\&omega;}}_n+{\mathrm{K\&omega;}}_n^2},$ $c_2=\frac{-12+4{\mathrm{\&zeta;\&omega;}}_n-{3\mathrm{K\&omega;}}_n^2}{4+4{\mathrm{\&zeta;\&omega;}}_n+{\mathrm{K\&omega;}}_n^2},c_3=\frac{4-4{\mathrm{\&zeta;\&omega;}}_n+{\mathrm{K\&omega;}}_n^2}{4+4{\mathrm{\&zeta;\&omega;}}_n+{\mathrm{K\&omega;}}_n^2},d_0=\frac{{\mathrm{\&omega;}}_n^2{T}_s^3}{2(4+4{\mathrm{\&zeta;\&omega;}}_n+{\mathrm{K\&omega;}}_n^2)},$ $d_1=\frac{{3\mathrm{\&omega;}}_n^2{T}_s^3}{2(4+4{\mathrm{\&zeta;\&omega;}}_n+{\mathrm{K\&omega;}}_n^2)},d_2=\frac{{3\mathrm{\&omega;}}_n^2{T}_s^3}{2(4+4{\mathrm{\&zeta;\&omega;}}_n+{\mathrm{K\&omega;}}_n^2)},d_3=\frac{{\mathrm{\&omega;}}_n^2{T}_s^3}{2(4+4{\mathrm{\&zeta;\&omega;}}_n+{\mathrm{K\&omega;}}_n^2)},$ K为捷联惯导***回路的增益，ζ为捷联惯导***回路的阻尼比，ω_n为捷联惯导***回路的自然角频率。

5.根据权利要求1所述的组合导航中的姿态角计算方法，其特征在于，还包括：

根据如下公式计算参考俯仰角，θ＝αθ_s+(1-α)θ_a，其中，θ_s为通过四元数法计算得到的第一参考俯仰角，θ_a为根据加速度分解方法计算得到的第二参考俯仰角，α为预设的权值；

若所述参考俯仰角的数值大于预设的俯仰角参考值时，则执行步骤所述以预先获取的捷联惯导***误差角和角速度作为输入，随动角作为输出，根据随动角与捷联惯导***回路的增益、阻尼比和自然角频率的关联关系建立闭环反馈回路，所述俯仰角参考值为60-70度。

6.根据权利要求2所述的组合导航中的姿态角计算方法，其特征在于，还包括：

根据预设的航向角初始值和滚动角初始值，采用遗传算法进行搜索；

当搜索到航向角变化值与航向角实际值的差值小于预设的航向差值，且滚动角变化值与滚动角实际值的差值小于预设的滚动差值时，确定此时的航向角变化值与航向角实际值的差值为航向角的误差补偿角，且确定滚动角变化值与滚动角实际值的差值为滚动角的误差补偿角。

7.根据权利要求1-6任一项所述的组合导航中的姿态角计算方法，其特征在于，所述根据第一优化捷联矩阵获取姿态角包括：

采用卡尔曼滤波的方式获取捷联惯导***的数学平台失准角；

根据所述数学平台失准角和优化随动角对所述第一优化捷联矩阵进行校正，以确定第二优化捷联矩阵；

从所述第二优化捷联矩阵中导出姿态角。

8.根据权利要求7所述的组合导航中的姿态角计算方法，其特征在于，所述采用卡尔曼滤波的方式获取捷联惯导***的数学平台失准角包括：

对预先获取的组合导航***状态方程和观测方程进行离散化处理，并通过卡尔曼滤波进行递推估计，以确定状态最优估值方程；

根据所述状态最优估值方程确定所述数学平台失准角。

9.根据权利要求8所述的组合导航中的姿态角计算方法，其特征在于，根据所述数学平台失准角和优化随动角对所述第一优化捷联矩阵进行校正，以确定第二优化捷联矩阵包括：

根据所述数学平台失准角对第一优化捷联矩阵进行校正，以确定不完全优化捷联矩阵；

根据所述优化随动角对所述不完全优化捷联矩阵再次进行校正，以确定所述第二优化捷联矩阵。

10.根据权利要求9所述的组合导航中的姿态角计算方法，其特征在于，还包括根据如下公式计算捷联惯导***的测算速度和测算位置， $\left\{\begin{array}{c}{\stackrel{.}{x}}_e^n=v_e^n\\ {\stackrel{.}{v}}_e^n={C_b}^n{f}^b-[{2\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n]\&times;v_e^n+g^n\end{array}\right.,$ 其中是测算位置矢量，是测算速度矢量，f^b是比力信号，为捷联矩阵的逆矩阵，gⁿ＝[0,0,g]^T为地球重力矢量，为地球自转角速度Ω在导航坐标系中的投影，为导航坐标系相对地球坐标系的角速度；

根据所述测算速度矢量、测算位置矢量和预先通过卫星接收机获取的检测速度矢量和检测位置矢量分别计算速度误差和位置误差；

以所述速度误差、位置误差作为观测变量和以预先选取的数学平台失准角、陀螺仪零偏、陀螺仪随机漂移率和加速度计零偏作为组合***误差状态向量，分别得到***状态方程和观测方程。